Skip to content

Commit 4dd6bcb

Browse files
mrdav30mrdav30
committed
perf: specialize exact point anchors for aligned frames
1 parent b53397c commit 4dd6bcb

5 files changed

Lines changed: 303 additions & 12 deletions

File tree

‎src/FixedMathSharp/Geometry/Anchors/FixedPointAnchor.cs‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -200,6 +200,11 @@ public bool TryGetScaledOffsetFrom(
200200
offset = default;
201201
return false;
202202
}
203+
if (scale == Fixed64.Zero)
204+
{
205+
offset = Vector3d.Zero;
206+
return true;
207+
}
203208

204209
return WidePointAnchor3d.TryGetRelativeOffset(
205210
Origin,

‎src/FixedMathSharp/Geometry/Anchors/Wide/WidePointAnchor3d.Exact.cs‎

Lines changed: 0 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -690,12 +690,6 @@ private static bool TryGetScaledExactCoordinate(
690690
denominator,
691691
out coordinate);
692692
}
693-
if (scale == Fixed64.Zero)
694-
{
695-
coordinate = Fixed64.Zero;
696-
return true;
697-
}
698-
699693
return Fixed64.TryGetSignedRawRatio(
700694
WideArithmetic.MultiplySigned576ToSigned704(
701695
numerator,

‎src/FixedMathSharp/Geometry/Anchors/Wide/WidePointAnchor3d.cs‎

Lines changed: 88 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,64 @@ internal static bool TryGetRelativeOffset(
7676
Fixed64 scale,
7777
out Vector3d offset)
7878
{
79+
if (firstOrigin == secondOrigin
80+
&& firstRotation == secondRotation)
81+
{
82+
bool localRepresentable;
83+
Vector3d localOffset;
84+
if (firstLocalDisplacement == Vector3d.Zero
85+
&& secondLocalDisplacement == Vector3d.Zero)
86+
{
87+
localRepresentable = Vector3d.TrySubtract(
88+
firstLocalPoint,
89+
secondLocalPoint,
90+
out localOffset);
91+
}
92+
else
93+
{
94+
localRepresentable = Vector3d.TrySubtractSums(
95+
firstLocalPoint,
96+
firstLocalDisplacement,
97+
secondLocalPoint,
98+
secondLocalDisplacement,
99+
out localOffset);
100+
}
101+
102+
if (localRepresentable)
103+
{
104+
if (firstRotation != FixedQuaternion.Identity)
105+
{
106+
if (scale == Fixed64.One)
107+
return firstRotation.TryRotate(localOffset, out offset);
108+
}
109+
else
110+
{
111+
return TryScaleOffset(localOffset, scale, out offset);
112+
}
113+
}
114+
}
115+
116+
if (firstRotation == FixedQuaternion.Identity
117+
&& secondRotation == FixedQuaternion.Identity)
118+
{
119+
bool compactRepresentable = Vector3d.TryAddSubtract(
120+
firstOrigin,
121+
firstLocalPoint,
122+
secondOrigin,
123+
out Vector3d firstStep);
124+
compactRepresentable &= Vector3d.TryAddSubtract(
125+
firstStep,
126+
firstLocalDisplacement,
127+
secondLocalPoint,
128+
out Vector3d secondStep);
129+
compactRepresentable &= Vector3d.TrySubtract(
130+
secondStep,
131+
secondLocalDisplacement,
132+
out Vector3d compactOffset);
133+
if (compactRepresentable)
134+
return TryScaleOffset(compactOffset, scale, out offset);
135+
}
136+
79137
WideRationalBasis3d firstBasis = new(firstRotation);
80138
WideRationalBasis3d secondBasis = new(secondRotation);
81139
Signed320 denominator = WideArithmetic.MultiplySigned192(
@@ -149,6 +207,36 @@ internal static bool TryGetRelativeOffset(
149207
return true;
150208
}
151209

210+
private static bool TryScaleOffset(
211+
Vector3d value,
212+
Fixed64 scale,
213+
out Vector3d result)
214+
{
215+
if (scale == Fixed64.One)
216+
{
217+
result = value;
218+
return true;
219+
}
220+
221+
bool representable = Fixed64.TryMultiplyDivide(
222+
value.X,
223+
scale,
224+
Fixed64.One,
225+
out Fixed64 x)
226+
& Fixed64.TryMultiplyDivide(
227+
value.Y,
228+
scale,
229+
Fixed64.One,
230+
out Fixed64 y)
231+
& Fixed64.TryMultiplyDivide(
232+
value.Z,
233+
scale,
234+
Fixed64.One,
235+
out Fixed64 z);
236+
result = representable ? new Vector3d(x, y, z) : default;
237+
return representable;
238+
}
239+
152240
internal static bool TryGetLocalPointIn(
153241
Vector3d pointOrigin,
154242
FixedQuaternion pointRotation,
@@ -534,12 +622,6 @@ private static bool TryGetRelativeOffsetCoordinate(
534622
denominatorWide,
535623
out coordinate);
536624
}
537-
if (scale == Fixed64.Zero)
538-
{
539-
coordinate = Fixed64.Zero;
540-
return true;
541-
}
542-
543625
Signed320 scaleRaw = Signed320.ExtendValue(Signed192.Raw(scale));
544626
Signed320 oneRaw = Signed320.ExtendValue(Signed192.One);
545627
return Fixed64.TryGetSignedRawRatio(

‎tests/FixedMathSharp.Benchmarks/PointAnchorBenchmarks.cs‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,28 @@ namespace FixedMathSharp.Benchmarks;
1010
[MemoryDiagnoser]
1111
public class PointAnchorBenchmarks
1212
{
13+
private readonly FixedPointAnchor _sameFrameIdentity = new(
14+
new Vector3d(5, -3, 7),
15+
FixedQuaternion.Identity,
16+
new Vector3d(2, 1, -4),
17+
new Vector3d(Fixed64.Half, -Fixed64.Half, Fixed64.One));
18+
private readonly FixedPointAnchor _sameFrameIdentityOther = new(
19+
new Vector3d(5, -3, 7),
20+
FixedQuaternion.Identity,
21+
new Vector3d(-2, 4, 1));
22+
private readonly FixedPointAnchor _identityFrameOtherOrigin = new(
23+
new Vector3d(-1, 4, 3),
24+
FixedQuaternion.Identity,
25+
new Vector3d(-2, 4, 1));
26+
private readonly FixedPointAnchor _sameFrameRotated = new(
27+
new Vector3d(5, -3, 7),
28+
FixedQuaternion.FromAxisAngle(Vector3d.Up, Fixed64.PiOver4),
29+
new Vector3d(2, 1, -4),
30+
new Vector3d(Fixed64.Half, -Fixed64.Half, Fixed64.One));
31+
private readonly FixedPointAnchor _sameFrameRotatedOther = new(
32+
new Vector3d(5, -3, 7),
33+
FixedQuaternion.FromAxisAngle(Vector3d.Up, Fixed64.PiOver4),
34+
new Vector3d(-2, 4, 1));
1335
private readonly FixedPointAnchor _anchor = new(
1436
new Vector3d(5, -3, 7),
1537
FixedQuaternion.FromAxisAngle(Vector3d.Up, Fixed64.PiOver4),
@@ -62,6 +84,24 @@ public bool ProjectedRelativeOffset() =>
6284
public bool RelativeOffset2d() =>
6385
_anchor2d.TryGetOffsetFrom(_other2d, out _);
6486

87+
[Benchmark]
88+
public bool SameFrameIdentityOffset() =>
89+
_sameFrameIdentity.TryGetOffsetFrom(
90+
_sameFrameIdentityOther,
91+
out _);
92+
93+
[Benchmark]
94+
public bool IdentityFrameOffset() =>
95+
_sameFrameIdentity.TryGetOffsetFrom(
96+
_identityFrameOtherOrigin,
97+
out _);
98+
99+
[Benchmark]
100+
public bool SameFrameRotatedOffset() =>
101+
_sameFrameRotated.TryGetOffsetFrom(
102+
_sameFrameRotatedOther,
103+
out _);
104+
65105
[Benchmark]
66106
public bool ReexpressInRotatedFrame2d() =>
67107
_anchor2d.TryGetLocalPointIn(

‎tests/FixedMathSharp.Tests/Geometry/Primitives/FixedPointAnchor.Tests.cs‎

Lines changed: 170 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -236,6 +236,176 @@ public void TryGetOffsetFrom_FusesBothRigidFramesAcrossScalarFaces()
236236
Assert.Equal(Vector3d.Zero, offset);
237237
}
238238

239+
[Fact]
240+
public void TryGetOffsetFrom_SameFramePreservesCompositeLocalDifference()
241+
{
242+
Vector3d origin = new(7, -3, 11);
243+
FixedPointAnchor first = new(
244+
origin,
245+
FixedQuaternion.Identity,
246+
new Vector3d(5, 2, -4),
247+
new Vector3d(-2, 3, 1));
248+
FixedPointAnchor second = new(
249+
origin,
250+
FixedQuaternion.Identity,
251+
new Vector3d(1, -1, 2),
252+
new Vector3d(3, 1, -2));
253+
254+
Assert.True(first.TryGetOffsetFrom(second, out Vector3d offset));
255+
Assert.Equal(new Vector3d(-1, 5, -3), offset);
256+
Assert.True(first.TryGetScaledOffsetFrom(
257+
second,
258+
-Fixed64.Half,
259+
out Vector3d scaled));
260+
Assert.Equal(new Vector3d(
261+
Fixed64.Half,
262+
-Fixed64.FromFraction(5, 2),
263+
Fixed64.FromFraction(3, 2)), scaled);
264+
}
265+
266+
[Fact]
267+
public void TryGetOffsetFrom_IdentityFramesPreserveCompositeWorldDifference()
268+
{
269+
FixedPointAnchor first = new(
270+
new Vector3d(7, -3, 11),
271+
FixedQuaternion.Identity,
272+
new Vector3d(5, 2, -4),
273+
new Vector3d(-2, 3, 1));
274+
FixedPointAnchor second = new(
275+
new Vector3d(-1, 4, 3),
276+
FixedQuaternion.Identity,
277+
new Vector3d(1, -1, 2),
278+
new Vector3d(3, 1, -2));
279+
280+
Assert.True(first.TryGetOffsetFrom(second, out Vector3d offset));
281+
Assert.Equal(new Vector3d(7, -2, 5), offset);
282+
Assert.True(first.TryGetScaledOffsetFrom(
283+
second,
284+
Fixed64.Half,
285+
out Vector3d scaled));
286+
Assert.Equal(new Vector3d(
287+
Fixed64.FromFraction(7, 2),
288+
-Fixed64.One,
289+
Fixed64.FromFraction(5, 2)), scaled);
290+
}
291+
292+
[Fact]
293+
public void TryGetScaledOffsetFrom_SameIdentityFrameRoundsRawTiesToEven()
294+
{
295+
Fixed64 oneRaw = Fixed64.FromRaw(1);
296+
Fixed64 threeRaw = Fixed64.FromRaw(3);
297+
FixedPointAnchor first = new(
298+
Vector3d.Zero,
299+
FixedQuaternion.Identity,
300+
new Vector3d(oneRaw, threeRaw, -threeRaw));
301+
FixedPointAnchor second = new(
302+
Vector3d.Zero,
303+
FixedQuaternion.Identity,
304+
Vector3d.Zero);
305+
306+
Assert.True(first.TryGetScaledOffsetFrom(
307+
second,
308+
Fixed64.Half,
309+
out Vector3d offset));
310+
Assert.Equal(new Vector3d(
311+
Fixed64.Zero,
312+
Fixed64.FromRaw(2),
313+
Fixed64.FromRaw(-2)), offset);
314+
}
315+
316+
[Fact]
317+
public void TryGetOffsetFrom_IdentityFramesRetainIntermediateOverflowFallback()
318+
{
319+
FixedPointAnchor first = new(
320+
new Vector3d(Fixed64.MaxValue, Fixed64.Zero, Fixed64.Zero),
321+
FixedQuaternion.Identity,
322+
Vector3d.Right,
323+
Vector3d.Left);
324+
FixedPointAnchor second = new(
325+
Vector3d.Zero,
326+
FixedQuaternion.Identity,
327+
Vector3d.Zero);
328+
329+
Assert.True(first.TryGetOffsetFrom(second, out Vector3d offset));
330+
Assert.Equal(
331+
new Vector3d(Fixed64.MaxValue, Fixed64.Zero, Fixed64.Zero),
332+
offset);
333+
}
334+
335+
[Fact]
336+
public void TryGetOffsetFrom_SameRotatedFrameUsesCompleteLocalDifference()
337+
{
338+
Vector3d origin = new(7, -3, 11);
339+
FixedQuaternion rotation =
340+
FixedQuaternion.FromAxisAngle(Vector3d.Up, Fixed64.HalfPi);
341+
FixedPointAnchor first = new(
342+
origin,
343+
rotation,
344+
new Vector3d(5, 2, -4),
345+
new Vector3d(-2, 3, 1));
346+
FixedPointAnchor second = new(
347+
origin,
348+
rotation,
349+
new Vector3d(1, -1, 2),
350+
new Vector3d(3, 1, -2));
351+
352+
Assert.True(first.TryGetOffsetFrom(second, out Vector3d offset));
353+
Assert.Equal(new Vector3d(-3, 5, 1), offset);
354+
Assert.True(first.TryGetScaledOffsetFrom(
355+
second,
356+
Fixed64.Half,
357+
out Vector3d scaled));
358+
Assert.Equal(new Vector3d(
359+
Fixed64.FromFraction(-3, 2),
360+
Fixed64.FromFraction(5, 2),
361+
Fixed64.Half), scaled);
362+
}
363+
364+
[Fact]
365+
public void TryGetOffsetFrom_SameRotatedFrameRetainsWideLocalFallback()
366+
{
367+
Fixed64 large = Fixed64.FromRaw((long.MaxValue / 5L) * 3L);
368+
FixedQuaternion rotation =
369+
FixedQuaternion.FromAxisAngle(Vector3d.Up, Fixed64.PiOver4);
370+
FixedPointAnchor first = new(
371+
Vector3d.Zero,
372+
rotation,
373+
new Vector3d(large, Fixed64.Zero, Fixed64.Zero));
374+
FixedPointAnchor second = new(
375+
Vector3d.Zero,
376+
rotation,
377+
new Vector3d(-large, Fixed64.Zero, Fixed64.Zero));
378+
379+
Assert.False(Vector3d.TrySubtract(
380+
first.LocalPoint,
381+
second.LocalPoint,
382+
out _));
383+
Assert.True(first.TryGetOffsetFrom(second, out Vector3d offset));
384+
Assert.Equal(new Vector3d(
385+
Fixed64.FromRaw(7826290728543493829L),
386+
Fixed64.Zero,
387+
Fixed64.FromRaw(-7826290661855844679L)), offset);
388+
}
389+
390+
[Fact]
391+
public void TryGetScaledOffsetFrom_SameIdentityFrameRejectsFinalOverflow()
392+
{
393+
FixedPointAnchor first = new(
394+
Vector3d.Zero,
395+
FixedQuaternion.Identity,
396+
new Vector3d(Fixed64.MaxValue, Fixed64.Zero, Fixed64.Zero));
397+
FixedPointAnchor second = new(
398+
Vector3d.Zero,
399+
FixedQuaternion.Identity,
400+
Vector3d.Zero);
401+
402+
Assert.False(first.TryGetScaledOffsetFrom(
403+
second,
404+
Fixed64.Two,
405+
out Vector3d offset));
406+
Assert.Equal(default, offset);
407+
}
408+
239409
[Fact]
240410
public void TryGetPoint_RejectsUnrepresentableWorldCoordinate()
241411
{

0 commit comments

Comments
 (0)