| | | 1 | | //======================================================================= |
| | | 2 | | // CompoundInertiaMath3D.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2026-present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using System; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | |
| | | 11 | | namespace Gravitas.Colliders; |
| | | 12 | | |
| | | 13 | | |
| | | 14 | | |
| | | 15 | | /// <summary>Accumulates homogeneous weighted moments without child mass or tensor rounding.</summary> |
| | | 16 | | internal static class CompoundInertiaMath3D |
| | | 17 | | { |
| | 1 | 18 | | internal static readonly Signed320 Q192 = Signed320.NarrowValue( |
| | 1 | 19 | | WideArithmetic.MultiplySigned576(Signed576.ExtendValue( |
| | 1 | 20 | | WideArithmetic.MultiplySigned192(Signed192.One, Signed192.One)), |
| | 1 | 21 | | Signed192.One, Signed192.One, Signed192.One, Signed192.One)); |
| | | 22 | | |
| | | 23 | | internal static Signed320 ScaleWeight(ExactMassWeight weight) => |
| | 3405 | 24 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320(weight.Numerator, Signed192.Signed(60))); |
| | | 25 | | |
| | | 26 | | internal static void Add(ref CompoundInertiaTensor tensor, LSCollider part, |
| | | 27 | | ExactMassWeight weight, Vector3d reference) |
| | | 28 | | { |
| | 285 | 29 | | if (part is LSMeshCollider meshCollider) |
| | | 30 | | { |
| | 32 | 31 | | MeshMassIntegrals mesh = meshCollider.Mesh.GetCompoundMassIntegrals(usePrepared: false, meshCollider.Inertia |
| | 32 | 32 | | AddMesh(ref tensor, mesh, part.CompoundLocalRotation, |
| | 32 | 33 | | part.TransformRelativeMassPropertyPointExact(mesh.Reference), reference); |
| | 32 | 34 | | return; |
| | | 35 | | } |
| | 253 | 36 | | Span<Signed576> intrinsic = stackalloc Signed576[6]; |
| | 253 | 37 | | intrinsic.Clear(); |
| | 253 | 38 | | GetIntrinsic(part, weight, intrinsic); |
| | 253 | 39 | | ExactMassPoint3D point = part.CalculateLocalMassPoint(); |
| | | 40 | | |
| | | 41 | | // Q224 intrinsic moments acquire Q64 rotation products and Q32 padding. |
| | | 42 | | // Q96 semantic displacements square directly into the same Q320 frame. |
| | 253 | 43 | | AddIntrinsic(ref tensor, intrinsic, part.CompoundLocalRotation); |
| | 253 | 44 | | Signed320 dx = Difference(point.XNumerator, reference.X); |
| | 253 | 45 | | Signed320 dy = Difference(point.YNumerator, reference.Y); |
| | 253 | 46 | | Signed320 dz = Difference(point.ZNumerator, reference.Z); |
| | 253 | 47 | | AddParallelAxis(ref tensor, ScaleWeight(weight), dx, dy, dz); |
| | 253 | 48 | | } |
| | | 49 | | |
| | | 50 | | internal static void AddMesh(ref CompoundInertiaTensor tensor, in MeshMassIntegrals mesh, |
| | | 51 | | FixedQuaternion rotation, ExactMassPoint3D point, Vector3d reference) |
| | | 52 | | { |
| | 80 | 53 | | Span<Signed576> intrinsic = stackalloc Signed576[6]; |
| | 240 | 54 | | intrinsic[0] = mesh.Ixx; intrinsic[1] = mesh.Iyy; intrinsic[2] = mesh.Izz; |
| | 240 | 55 | | intrinsic[3] = mesh.Ixy; intrinsic[4] = mesh.Ixz; intrinsic[5] = mesh.Iyz; |
| | 80 | 56 | | AddIntrinsic(ref tensor, intrinsic, rotation); |
| | 80 | 57 | | Signed320 dx = Difference(point.XNumerator, reference.X); |
| | 80 | 58 | | Signed320 dy = Difference(point.YNumerator, reference.Y); |
| | 80 | 59 | | Signed320 dz = Difference(point.ZNumerator, reference.Z); |
| | 80 | 60 | | AddParallelAxis(ref tensor, ScaleWeight(mesh.Weight), dx, dy, dz); |
| | 80 | 61 | | AddFirstMoments(ref tensor, mesh, rotation, dx, dy, dz); |
| | 80 | 62 | | } |
| | | 63 | | |
| | | 64 | | private static Signed320 Difference(Signed320 point, Fixed64 reference) => |
| | 999 | 65 | | WideArithmetic.SubtractSigned320(point, Signed320.NarrowValue( |
| | 999 | 66 | | WideArithmetic.MultiplySigned320( |
| | 999 | 67 | | WideArithmetic.MultiplySigned192(Signed192.Raw(reference), Signed192.One), Signed192.One))); |
| | | 68 | | |
| | | 69 | | private static Signed576 Weighted(ExactMassWeight weight, Signed192 geometry, int factor = 60) => |
| | 476 | 70 | | WideArithmetic.MultiplySigned576( |
| | 476 | 71 | | WideArithmetic.MultiplySigned320(weight.Numerator, geometry), Signed192.Signed(factor)); |
| | | 72 | | |
| | | 73 | | private static void GetIntrinsic(LSCollider part, ExactMassWeight weight, Span<Signed576> values) |
| | | 74 | | { |
| | | 75 | | switch (part) |
| | | 76 | | { |
| | | 77 | | case LSSphereCollider sphere: |
| | 134 | 78 | | Fixed64 radius = sphere.GetMassPropertyRadius(); |
| | 134 | 79 | | Signed576 spherical = Weighted(weight, |
| | 134 | 80 | | ExactMassProperties.InertiaGeometryTerm(Fixed64.FromFraction(2, 5), radius, radius)); |
| | 134 | 81 | | values[0] = values[1] = values[2] = spherical; |
| | 134 | 82 | | break; |
| | | 83 | | case LSCuboidCollider box: |
| | 65 | 84 | | Vector3d extents = box.GetMassPropertyHalfExtents(); |
| | 65 | 85 | | Signed576 x = Weighted(weight, ExactMassProperties.InertiaGeometryTerm(Fixed64.One, extents.X, extents.X |
| | 65 | 86 | | Signed576 y = Weighted(weight, ExactMassProperties.InertiaGeometryTerm(Fixed64.One, extents.Y, extents.Y |
| | 65 | 87 | | Signed576 z = Weighted(weight, ExactMassProperties.InertiaGeometryTerm(Fixed64.One, extents.Z, extents.Z |
| | 65 | 88 | | values[0] = WideArithmetic.AddSigned576(y, z); |
| | 65 | 89 | | values[1] = WideArithmetic.AddSigned576(x, z); |
| | 65 | 90 | | values[2] = WideArithmetic.AddSigned576(x, y); |
| | 65 | 91 | | break; |
| | | 92 | | case LSCylinderCollider cylinder: |
| | 15 | 93 | | cylinder.GetInertiaGeometry(out Signed192 cylinderTransverse, out Signed192 cylinderAxial); |
| | 15 | 94 | | values[0] = values[2] = Weighted(weight, cylinderTransverse); |
| | 15 | 95 | | values[1] = Weighted(weight, cylinderAxial); |
| | 15 | 96 | | break; |
| | | 97 | | case LSConeCollider cone: |
| | 18 | 98 | | cone.GetInertiaGeometry(out Signed192 coneTransverse, out Signed192 coneAxial); |
| | 18 | 99 | | values[0] = values[2] = Weighted(weight, coneTransverse); |
| | 18 | 100 | | values[1] = Weighted(weight, coneAxial); |
| | 18 | 101 | | break; |
| | | 102 | | default: |
| | | 103 | | // Authored compound definitions admit only this closed family. |
| | 21 | 104 | | GetCapsuleIntrinsic((LSCapsuleCollider)part, weight, values); |
| | | 105 | | break; |
| | | 106 | | } |
| | 21 | 107 | | } |
| | | 108 | | |
| | | 109 | | private static void GetCapsuleIntrinsic(LSCapsuleCollider capsule, ExactMassWeight weight, Span<Signed576> values) |
| | | 110 | | { |
| | 21 | 111 | | capsule.GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 axis); |
| | 21 | 112 | | Signed192 sphereAxial = ExactMassProperties.InertiaGeometryTerm(Fixed64.FromFraction(2, 5), radius, radius); |
| | 21 | 113 | | if (axis <= Fixed64.Epsilon) |
| | | 114 | | { |
| | 1 | 115 | | values[0] = values[1] = values[2] = Weighted(weight, sphereAxial); |
| | 1 | 116 | | return; |
| | | 117 | | } |
| | 20 | 118 | | LSCapsuleCollider.GetComponentInertiaGeometry(radius, axis, sphereAxial, |
| | 20 | 119 | | out Signed192 cylinderTransverse, out Signed192 sphereTransverse); |
| | 20 | 120 | | ExactMassWeight cylinderWeight = LSCapsuleCollider.GetCylinderWeight(radius, axis); |
| | 20 | 121 | | ExactMassWeight capWeight = LSCapsuleCollider.GetCapWeight(radius); |
| | 20 | 122 | | values[0] = values[2] = WideArithmetic.AddSigned576( |
| | 20 | 123 | | Weighted(cylinderWeight, cylinderTransverse), Weighted(capWeight, sphereTransverse)); |
| | 20 | 124 | | values[1] = WideArithmetic.AddSigned576( |
| | 20 | 125 | | Weighted(cylinderWeight, ExactMassProperties.InertiaGeometryTerm(Fixed64.Half, radius, radius)), |
| | 20 | 126 | | Weighted(capWeight, sphereAxial)); |
| | 20 | 127 | | } |
| | | 128 | | |
| | | 129 | | private static void AddIntrinsic(ref CompoundInertiaTensor tensor, ReadOnlySpan<Signed576> values, FixedQuaternion r |
| | | 130 | | { |
| | 333 | 131 | | if (rotation == FixedQuaternion.Identity) |
| | | 132 | | { |
| | 290 | 133 | | tensor.Xx = AddPadded(tensor.Xx, values[0]); |
| | 290 | 134 | | tensor.Yy = AddPadded(tensor.Yy, values[1]); |
| | 290 | 135 | | tensor.Zz = AddPadded(tensor.Zz, values[2]); |
| | 290 | 136 | | tensor.Xy = AddPadded(tensor.Xy, values[3]); |
| | 290 | 137 | | tensor.Xz = AddPadded(tensor.Xz, values[4]); |
| | 290 | 138 | | tensor.Yz = AddPadded(tensor.Yz, values[5]); |
| | 290 | 139 | | return; |
| | | 140 | | } |
| | 43 | 141 | | Fixed3x3 matrix = rotation.ToMatrix3x3().Transpose(); |
| | 43 | 142 | | Vector3d x = new(matrix.M11, matrix.M12, matrix.M13); |
| | 43 | 143 | | Vector3d y = new(matrix.M21, matrix.M22, matrix.M23); |
| | 43 | 144 | | Vector3d z = new(matrix.M31, matrix.M32, matrix.M33); |
| | 43 | 145 | | tensor.Xx = AddRotated(tensor.Xx, values, x, x); |
| | 43 | 146 | | tensor.Yy = AddRotated(tensor.Yy, values, y, y); |
| | 43 | 147 | | tensor.Zz = AddRotated(tensor.Zz, values, z, z); |
| | 43 | 148 | | tensor.Xy = AddRotated(tensor.Xy, values, x, y); |
| | 43 | 149 | | tensor.Xz = AddRotated(tensor.Xz, values, x, z); |
| | 43 | 150 | | tensor.Yz = AddRotated(tensor.Yz, values, y, z); |
| | 43 | 151 | | } |
| | | 152 | | |
| | | 153 | | private static Signed704 AddPadded(Signed704 total, Signed576 value) => |
| | 1740 | 154 | | WideArithmetic.AddSigned704(total, Signed704.ExtendValue( |
| | 1740 | 155 | | WideArithmetic.MultiplySigned576(value, Signed192.One, Signed192.One, Signed192.One))); |
| | | 156 | | |
| | | 157 | | private static Signed704 AddRotated(Signed704 total, ReadOnlySpan<Signed576> values, Vector3d row, Vector3d column) |
| | | 158 | | { |
| | 258 | 159 | | Signed576 value = WideArithmetic.AddSigned576( |
| | 258 | 160 | | WideArithmetic.AddSigned576(RotationTerm(values[0], row.X, column.X), RotationTerm(values[1], row.Y, column. |
| | 258 | 161 | | RotationTerm(values[2], row.Z, column.Z)); |
| | 258 | 162 | | value = WideArithmetic.AddSigned576(value, RotationProduct(values[3], row.X, column.Y, row.Y, column.X)); |
| | 258 | 163 | | value = WideArithmetic.AddSigned576(value, RotationProduct(values[4], row.X, column.Z, row.Z, column.X)); |
| | 258 | 164 | | value = WideArithmetic.AddSigned576(value, RotationProduct(values[5], row.Y, column.Z, row.Z, column.Y)); |
| | 258 | 165 | | return WideArithmetic.AddSigned704(total, Signed704.ExtendValue( |
| | 258 | 166 | | WideArithmetic.MultiplySigned576(value, Signed192.One))); |
| | | 167 | | } |
| | | 168 | | |
| | | 169 | | private static Signed576 RotationTerm(Signed576 value, Fixed64 first, Fixed64 second) => |
| | 774 | 170 | | WideArithmetic.MultiplySigned576(value, Fixed64.GetExactRawProduct(first, second)); |
| | | 171 | | |
| | | 172 | | private static Signed576 RotationProduct(Signed576 value, |
| | | 173 | | Fixed64 first, Fixed64 second, Fixed64 third, Fixed64 fourth) => |
| | 774 | 174 | | WideArithmetic.MultiplySigned576(value, WideArithmetic.AddSigned192( |
| | 774 | 175 | | Fixed64.GetExactRawProduct(first, second), Fixed64.GetExactRawProduct(third, fourth))); |
| | | 176 | | |
| | | 177 | | private static void AddParallelAxis(ref CompoundInertiaTensor tensor, Signed320 weight, |
| | | 178 | | Signed320 dx, Signed320 dy, Signed320 dz) |
| | | 179 | | { |
| | 333 | 180 | | Signed704 x = WideArithmetic.MultiplySigned320(dx, dx, weight); |
| | 333 | 181 | | Signed704 y = WideArithmetic.MultiplySigned320(dy, dy, weight); |
| | 333 | 182 | | Signed704 z = WideArithmetic.MultiplySigned320(dz, dz, weight); |
| | 333 | 183 | | tensor.Xx = WideArithmetic.AddSigned704(tensor.Xx, WideArithmetic.AddSigned704(y, z)); |
| | 333 | 184 | | tensor.Yy = WideArithmetic.AddSigned704(tensor.Yy, WideArithmetic.AddSigned704(x, z)); |
| | 333 | 185 | | tensor.Zz = WideArithmetic.AddSigned704(tensor.Zz, WideArithmetic.AddSigned704(x, y)); |
| | 333 | 186 | | tensor.Xy = WideArithmetic.SubtractSigned704(tensor.Xy, WideArithmetic.MultiplySigned320(dx, dy, weight)); |
| | 333 | 187 | | tensor.Xz = WideArithmetic.SubtractSigned704(tensor.Xz, WideArithmetic.MultiplySigned320(dx, dz, weight)); |
| | 333 | 188 | | tensor.Yz = WideArithmetic.SubtractSigned704(tensor.Yz, WideArithmetic.MultiplySigned320(dy, dz, weight)); |
| | 333 | 189 | | } |
| | | 190 | | |
| | | 191 | | private static void AddFirstMoments(ref CompoundInertiaTensor tensor, MeshMassIntegrals mesh, |
| | | 192 | | FixedQuaternion rotation, Signed320 dx, Signed320 dy, Signed320 dz) |
| | | 193 | | { |
| | 80 | 194 | | if (mesh.FirstX.IsZero && mesh.FirstY.IsZero && mesh.FirstZ.IsZero) |
| | 60 | 195 | | return; |
| | 20 | 196 | | Fixed3x3 matrix = rotation.ToMatrix3x3().Transpose(); |
| | 20 | 197 | | Signed576 fx = RotateFirst(mesh, matrix.M11, matrix.M12, matrix.M13); |
| | 20 | 198 | | Signed576 fy = RotateFirst(mesh, matrix.M21, matrix.M22, matrix.M23); |
| | 20 | 199 | | Signed576 fz = RotateFirst(mesh, matrix.M31, matrix.M32, matrix.M33); |
| | | 200 | | // First moments retain the centroid's fractional residual. Omitting |
| | | 201 | | // these cross terms would make translation depend on a rounded centroid. |
| | 20 | 202 | | tensor.Xx = AddFirstDiagonal(tensor.Xx, fy, dy, fz, dz); |
| | 20 | 203 | | tensor.Yy = AddFirstDiagonal(tensor.Yy, fx, dx, fz, dz); |
| | 20 | 204 | | tensor.Zz = AddFirstDiagonal(tensor.Zz, fx, dx, fy, dy); |
| | 20 | 205 | | tensor.Xy = SubtractFirstProduct(tensor.Xy, fx, dy, fy, dx); |
| | 20 | 206 | | tensor.Xz = SubtractFirstProduct(tensor.Xz, fx, dz, fz, dx); |
| | 20 | 207 | | tensor.Yz = SubtractFirstProduct(tensor.Yz, fy, dz, fz, dy); |
| | 20 | 208 | | } |
| | | 209 | | |
| | | 210 | | private static Signed576 RotateFirst(MeshMassIntegrals mesh, Fixed64 x, Fixed64 y, Fixed64 z) => |
| | 60 | 211 | | WideArithmetic.AddSigned576(WideArithmetic.AddSigned576( |
| | 60 | 212 | | WideArithmetic.MultiplySigned576(mesh.FirstX, Signed192.Raw(x)), |
| | 60 | 213 | | WideArithmetic.MultiplySigned576(mesh.FirstY, Signed192.Raw(y))), |
| | 60 | 214 | | WideArithmetic.MultiplySigned576(mesh.FirstZ, Signed192.Raw(z))); |
| | | 215 | | |
| | | 216 | | private static Signed576 FirstProduct(Signed576 first, Signed320 displacement) => |
| | 240 | 217 | | WideArithmetic.MultiplySigned576(first, Signed192.NarrowValue(displacement)); |
| | | 218 | | |
| | | 219 | | private static Signed704 AddFirstDiagonal(Signed704 total, |
| | | 220 | | Signed576 first, Signed320 firstDisplacement, Signed576 second, Signed320 secondDisplacement) |
| | | 221 | | { |
| | 60 | 222 | | Signed576 shift = WideArithmetic.AddSigned576( |
| | 60 | 223 | | FirstProduct(first, firstDisplacement), FirstProduct(second, secondDisplacement)); |
| | 60 | 224 | | return WideArithmetic.AddSigned704(total, Signed704.ExtendValue(WideArithmetic.AddSigned576(shift, shift))); |
| | | 225 | | } |
| | | 226 | | |
| | | 227 | | private static Signed704 SubtractFirstProduct(Signed704 total, |
| | | 228 | | Signed576 first, Signed320 firstDisplacement, Signed576 second, Signed320 secondDisplacement) => |
| | 60 | 229 | | WideArithmetic.SubtractSigned704(total, Signed704.ExtendValue(WideArithmetic.AddSigned576( |
| | 60 | 230 | | FirstProduct(first, firstDisplacement), FirstProduct(second, secondDisplacement)))); |
| | | 231 | | } |