Skip to content

Commit 9594ba5

Browse files
mrdav30mrdav30
committed
perf: accelerate exact OBB triangle and hull contacts
1 parent 120cafc commit 9594ba5

13 files changed

Lines changed: 657 additions & 367 deletions

‎src/FixedMathSharp/Geometry/Wide/Common/WideAxis3.cs‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@ internal WideAxis3(Signed320 x, Signed320 y, Signed320 z)
3232
WideArithmetic.MultiplySigned320(Y, Y)),
3333
WideArithmetic.MultiplySigned320(Z, Z));
3434

35-
internal static WideAxis3 Cross(WideAxis3 left, WideAxis3 right) =>
35+
internal static WideAxis3 Cross(in WideAxis3 left, in WideAxis3 right) =>
3636
new(
3737
Signed320.NarrowValue(WideArithmetic.SubtractSigned576(
3838
WideArithmetic.MultiplySigned320(left.Y, right.Z),
@@ -44,6 +44,13 @@ internal static WideAxis3 Cross(WideAxis3 left, WideAxis3 right) =>
4444
WideArithmetic.MultiplySigned320(left.X, right.Y),
4545
WideArithmetic.MultiplySigned320(left.Y, right.X))));
4646

47+
internal static Signed576 Dot(in WideAxis3 left, in WideAxis3 right) =>
48+
WideArithmetic.AddSigned576(
49+
WideArithmetic.AddSigned576(
50+
WideArithmetic.MultiplySigned320(left.X, right.X),
51+
WideArithmetic.MultiplySigned320(left.Y, right.Y)),
52+
WideArithmetic.MultiplySigned320(left.Z, right.Z));
53+
4754
public static WideAxis3 operator -(WideAxis3 value) =>
4855
new(
4956
WideArithmetic.SubtractSigned320(default, value.X),

‎src/FixedMathSharp/Geometry/Wide/Common/WideRigidProjection.cs‎

Lines changed: 80 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@ namespace FixedMathSharp.Geometry;
1414
internal static class WideRigidProjection
1515
{
1616
internal static WideAxis3 TransformLocalAxis(
17-
WideRationalBasis3d basis,
17+
in WideRationalBasis3d basis,
1818
Signed192 localX,
1919
Signed192 localY,
2020
Signed192 localZ) =>
@@ -35,7 +35,7 @@ internal static WideAxis3 TransformLocalAxis(
3535
/// keeps each transformed component within <see cref="Signed320"/>.
3636
/// </summary>
3737
internal static WideAxis3 TransformLocalTriangleNormalCrossEdgeAxis(
38-
WideRationalBasis3d basis,
38+
in WideRationalBasis3d basis,
3939
Signed320 localX,
4040
Signed320 localY,
4141
Signed320 localZ) =>
@@ -50,29 +50,75 @@ internal static WideAxis3 TransformLocalTriangleNormalCrossEdgeAxis(
5050
localX, localY, localZ,
5151
basis.Xz, basis.Yz, basis.Zz));
5252

53-
internal static Signed576 GetTransformedLocalOffsetProjection(
53+
internal static void GetLocalAxisProjections(
54+
in WideAxis3 axis,
55+
in WideRationalBasis3d basis,
56+
out Signed576 x,
57+
out Signed576 y,
58+
out Signed576 z)
59+
{
60+
x = GetBasisAxisProjection(
61+
axis,
62+
basis.Xx,
63+
basis.Xy,
64+
basis.Xz);
65+
y = GetBasisAxisProjection(
66+
axis,
67+
basis.Yx,
68+
basis.Yy,
69+
basis.Yz);
70+
z = GetBasisAxisProjection(
71+
axis,
72+
basis.Zx,
73+
basis.Zy,
74+
basis.Zz);
75+
}
76+
77+
internal static Signed576 GetLocalOffsetProjection(
5478
Vector3d localOffset,
55-
WideRationalBasis3d basis,
56-
WideAxis3 axis) =>
79+
in Signed576 axisX,
80+
in Signed576 axisY,
81+
in Signed576 axisZ) =>
5782
WideArithmetic.AddSigned576(
5883
WideArithmetic.AddSigned576(
5984
WideArithmetic.MultiplySigned576(
60-
GetBasisAxisProjection(
61-
axis, basis.Xx, basis.Xy, basis.Xz),
62-
Signed192.Raw(localOffset.X)),
85+
axisX,
86+
localOffset.X.m_rawValue),
6387
WideArithmetic.MultiplySigned576(
64-
GetBasisAxisProjection(
65-
axis, basis.Yx, basis.Yy, basis.Yz),
66-
Signed192.Raw(localOffset.Y))),
88+
axisY,
89+
localOffset.Y.m_rawValue)),
6790
WideArithmetic.MultiplySigned576(
68-
GetBasisAxisProjection(
69-
axis, basis.Zx, basis.Zy, basis.Zz),
70-
Signed192.Raw(localOffset.Z)));
91+
axisZ,
92+
localOffset.Z.m_rawValue));
93+
94+
internal static void TransformLocalAxis(
95+
in WideRationalBasis3d basis,
96+
in WideAxis3 localAxis,
97+
out Signed576 x,
98+
out Signed576 y,
99+
out Signed576 z)
100+
{
101+
x = TransformLocalAxisComponent(
102+
localAxis,
103+
basis.Xx,
104+
basis.Yx,
105+
basis.Zx);
106+
y = TransformLocalAxisComponent(
107+
localAxis,
108+
basis.Xy,
109+
basis.Yy,
110+
basis.Zy);
111+
z = TransformLocalAxisComponent(
112+
localAxis,
113+
basis.Xz,
114+
basis.Yz,
115+
basis.Zz);
116+
}
71117

72118
internal static Signed576 GetWorldOriginDifferenceProjection(
73119
Vector3d end,
74120
Vector3d start,
75-
WideAxis3 axis) =>
121+
in WideAxis3 axis) =>
76122
WideArithmetic.AddSigned576(
77123
WideArithmetic.AddSigned576(
78124
WideArithmetic.MultiplySigned320(
@@ -92,7 +138,7 @@ internal static Signed576 GetWorldOriginDifferenceProjection(
92138
axis.Z));
93139

94140
internal static Signed576 GetBasisAxisProjection(
95-
WideAxis3 axis,
141+
in WideAxis3 axis,
96142
Signed192 basisX,
97143
Signed192 basisY,
98144
Signed192 basisZ) =>
@@ -109,7 +155,7 @@ internal static Signed576 GetBasisAxisProjection(
109155
basisZ));
110156

111157
internal static void IncludeProjection(
112-
Signed576 candidate,
158+
in Signed576 candidate,
113159
ref Signed576 minimum,
114160
ref Signed576 maximum)
115161
{
@@ -153,4 +199,21 @@ private static Signed320 TransformLocalTriangleNormalCrossEdgeComponent(
153199
basisZ));
154200
return Signed320.NarrowValue(transformed);
155201
}
202+
203+
private static Signed576 TransformLocalAxisComponent(
204+
in WideAxis3 localAxis,
205+
Signed192 basisX,
206+
Signed192 basisY,
207+
Signed192 basisZ) =>
208+
WideArithmetic.AddSigned576(
209+
WideArithmetic.AddSigned576(
210+
WideArithmetic.MultiplySigned576(
211+
Signed576.ExtendValue(localAxis.X),
212+
basisX),
213+
WideArithmetic.MultiplySigned576(
214+
Signed576.ExtendValue(localAxis.Y),
215+
basisY)),
216+
WideArithmetic.MultiplySigned576(
217+
Signed576.ExtendValue(localAxis.Z),
218+
basisZ));
156219
}

‎src/FixedMathSharp/Geometry/Wide/OrientedBox/WideOrientedBox.AxisProjection.cs‎

Lines changed: 18 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -15,9 +15,9 @@ namespace FixedMathSharp.Geometry;
1515
internal static partial class WideOrientedBox
1616
{
1717
private static Signed576 GetBoxProjectionRadiusNumerator(
18-
WideAxis3 axis,
18+
in WideAxis3 axis,
1919
Vector3d halfExtents,
20-
WideRationalBasis3d basis) =>
20+
in WideRationalBasis3d basis) =>
2121
WideArithmetic.AddSigned576(
2222
WideArithmetic.AddSigned576(
2323
WideArithmetic.MultiplySigned576(
@@ -42,6 +42,21 @@ private static Signed576 GetBoxProjectionRadiusNumerator(
4242
basis.Zz)),
4343
Signed192.Raw(halfExtents.Z)));
4444

45+
private static Signed576 GetLocalBoxProjectionRadiusNumerator(
46+
in WideAxis3 axis,
47+
Vector3d halfExtents) =>
48+
WideArithmetic.AddSigned576(
49+
WideArithmetic.AddSigned576(
50+
WideArithmetic.MultiplySigned576(
51+
Signed576.ExtendValue(GetMagnitude(axis.X)),
52+
halfExtents.X.m_rawValue),
53+
WideArithmetic.MultiplySigned576(
54+
Signed576.ExtendValue(GetMagnitude(axis.Y)),
55+
halfExtents.Y.m_rawValue)),
56+
WideArithmetic.MultiplySigned576(
57+
Signed576.ExtendValue(GetMagnitude(axis.Z)),
58+
halfExtents.Z.m_rawValue));
59+
4560
internal static int GetRotatedLocalProjectionSign(
4661
FixedQuaternion rotation,
4762
Vector3d localDirection,
@@ -114,7 +129,7 @@ internal static void GetRelativeLocalPointNumerators(
114129
internal static void GetRelativeLocalPointNumerators(
115130
Vector3d point,
116131
Vector3d origin,
117-
WideRationalBasis3d basis,
132+
in WideRationalBasis3d basis,
118133
out Signed192 x,
119134
out Signed192 y,
120135
out Signed192 z)

‎src/FixedMathSharp/Geometry/Wide/OrientedBox/WideOrientedBox.CapsuleEdges.cs‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ private static CapsuleAxis3 GetVertexToCapsuleAxis(
5656
basis,
5757
localVertex,
5858
capsuleAxisDenominator);
59-
Signed576 projection = GetAxisProjection(
59+
Signed576 projection = WideAxis3.Dot(
6060
centerAxis,
6161
capsuleAxis);
6262
Signed576 capsuleSquared = capsuleAxis.SquaredLength;
@@ -119,7 +119,7 @@ private static CapsuleAxis3 GetCapsuleEndpointToBoxEdgeAxis(
119119
capsuleAxisLength,
120120
endpointSign);
121121
WideAxis3 edgeAxis = basis.GetAxis(edgeAxisIndex);
122-
Signed576 projection = GetAxisProjection(endpointAxis, edgeAxis);
122+
Signed576 projection = WideAxis3.Dot(endpointAxis, edgeAxis);
123123
Signed576 edgeSquared = edgeAxis.SquaredLength;
124124
Fixed64 edgeExtent = GetEdgeAxisExtent(
125125
halfExtents,

0 commit comments

Comments
 (0)