| | | 1 | | //======================================================================= |
| | | 2 | | // InertiaTensorMath.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 FixedMathSharp; |
| | | 9 | | using System.Runtime.CompilerServices; |
| | | 10 | | |
| | | 11 | | namespace Gravitas; |
| | | 12 | | |
| | | 13 | | /// <summary> |
| | | 14 | | /// Deterministic helpers for 3D inertia tensors used by body mass properties |
| | | 15 | | /// and solver effective mass calculations. |
| | | 16 | | /// </summary> |
| | | 17 | | internal static class InertiaTensorMath |
| | | 18 | | { |
| | | 19 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 20 | | public static bool IsDiagonal(Fixed3x3 tensor) => |
| | 10185 | 21 | | tensor.M12 == Fixed64.Zero |
| | 10185 | 22 | | && tensor.M13 == Fixed64.Zero |
| | 10185 | 23 | | && tensor.M21 == Fixed64.Zero |
| | 10185 | 24 | | && tensor.M23 == Fixed64.Zero |
| | 10185 | 25 | | && tensor.M31 == Fixed64.Zero |
| | 10185 | 26 | | && tensor.M32 == Fixed64.Zero; |
| | | 27 | | |
| | | 28 | | public static Fixed3x3 InvertForSolver(Fixed3x3 tensor) |
| | | 29 | | { |
| | 10216 | 30 | | if (tensor == Fixed3x3.Zero) |
| | 31 | 31 | | return Fixed3x3.Zero; |
| | | 32 | | |
| | 10185 | 33 | | if (IsDiagonal(tensor)) |
| | 10130 | 34 | | return InvertDiagonalForSolver(tensor); |
| | | 35 | | |
| | 55 | 36 | | if (!Fixed3x3.Invert(tensor, out Fixed3x3? inverse) || !inverse.HasValue) |
| | 1 | 37 | | return Fixed3x3.Zero; |
| | | 38 | | |
| | 54 | 39 | | return ClampNearZero(inverse.Value); |
| | | 40 | | } |
| | | 41 | | |
| | | 42 | | public static Fixed3x3 AddParallelAxisTensor(Fixed3x3 tensor, Fixed64 mass, Vector3d offset) |
| | | 43 | | { |
| | 180 | 44 | | if (mass <= Fixed64.Zero || offset == Vector3d.Zero) |
| | 167 | 45 | | return tensor; |
| | | 46 | | |
| | 13 | 47 | | Fixed64 xx = mass * ((offset.Y * offset.Y) + (offset.Z * offset.Z)); |
| | 13 | 48 | | Fixed64 yy = mass * ((offset.X * offset.X) + (offset.Z * offset.Z)); |
| | 13 | 49 | | Fixed64 zz = mass * ((offset.X * offset.X) + (offset.Y * offset.Y)); |
| | 13 | 50 | | Fixed64 xy = mass * offset.X * offset.Y; |
| | 13 | 51 | | Fixed64 xz = mass * offset.X * offset.Z; |
| | 13 | 52 | | Fixed64 yz = mass * offset.Y * offset.Z; |
| | | 53 | | |
| | 13 | 54 | | tensor.M11 += xx; |
| | 13 | 55 | | tensor.M22 += yy; |
| | 13 | 56 | | tensor.M33 += zz; |
| | 13 | 57 | | tensor.M12 -= xy; |
| | 13 | 58 | | tensor.M21 -= xy; |
| | 13 | 59 | | tensor.M13 -= xz; |
| | 13 | 60 | | tensor.M31 -= xz; |
| | 13 | 61 | | tensor.M23 -= yz; |
| | 13 | 62 | | tensor.M32 -= yz; |
| | 13 | 63 | | return ClampNearZero(tensor); |
| | | 64 | | } |
| | | 65 | | |
| | | 66 | | public static Fixed3x3 SubtractParallelAxisTensor(Fixed3x3 tensor, Fixed64 mass, Vector3d offset) |
| | | 67 | | { |
| | 167 | 68 | | if (mass <= Fixed64.Zero || offset == Vector3d.Zero) |
| | 161 | 69 | | return tensor; |
| | | 70 | | |
| | 6 | 71 | | Fixed64 xx = mass * ((offset.Y * offset.Y) + (offset.Z * offset.Z)); |
| | 6 | 72 | | Fixed64 yy = mass * ((offset.X * offset.X) + (offset.Z * offset.Z)); |
| | 6 | 73 | | Fixed64 zz = mass * ((offset.X * offset.X) + (offset.Y * offset.Y)); |
| | 6 | 74 | | Fixed64 xy = mass * offset.X * offset.Y; |
| | 6 | 75 | | Fixed64 xz = mass * offset.X * offset.Z; |
| | 6 | 76 | | Fixed64 yz = mass * offset.Y * offset.Z; |
| | | 77 | | |
| | 6 | 78 | | tensor.M11 -= xx; |
| | 6 | 79 | | tensor.M22 -= yy; |
| | 6 | 80 | | tensor.M33 -= zz; |
| | 6 | 81 | | tensor.M12 += xy; |
| | 6 | 82 | | tensor.M21 += xy; |
| | 6 | 83 | | tensor.M13 += xz; |
| | 6 | 84 | | tensor.M31 += xz; |
| | 6 | 85 | | tensor.M23 += yz; |
| | 6 | 86 | | tensor.M32 += yz; |
| | 6 | 87 | | return ClampNearZero(tensor); |
| | | 88 | | } |
| | | 89 | | |
| | | 90 | | public static Fixed3x3 RotateToFrame(Fixed3x3 tensor, FixedQuaternion rotation) |
| | | 91 | | { |
| | 182 | 92 | | if (rotation == FixedQuaternion.Identity) |
| | 153 | 93 | | return tensor; |
| | | 94 | | |
| | 29 | 95 | | Fixed3x3 rotationMatrix = rotation.ToMatrix3x3(); |
| | 29 | 96 | | return ClampNearZero(rotationMatrix * tensor * rotationMatrix.Transpose()); |
| | | 97 | | } |
| | | 98 | | |
| | | 99 | | private static Fixed3x3 InvertDiagonalForSolver(Fixed3x3 tensor) => |
| | 10130 | 100 | | new( |
| | 10130 | 101 | | tensor.M11 > Fixed64.Zero ? Fixed64.One / tensor.M11 : Fixed64.Zero, |
| | 10130 | 102 | | Fixed64.Zero, |
| | 10130 | 103 | | Fixed64.Zero, |
| | 10130 | 104 | | Fixed64.Zero, |
| | 10130 | 105 | | tensor.M22 > Fixed64.Zero ? Fixed64.One / tensor.M22 : Fixed64.Zero, |
| | 10130 | 106 | | Fixed64.Zero, |
| | 10130 | 107 | | Fixed64.Zero, |
| | 10130 | 108 | | Fixed64.Zero, |
| | 10130 | 109 | | tensor.M33 > Fixed64.Zero ? Fixed64.One / tensor.M33 : Fixed64.Zero); |
| | | 110 | | |
| | | 111 | | private static Fixed3x3 ClampNearZero(Fixed3x3 tensor) |
| | | 112 | | { |
| | 102 | 113 | | tensor.M11 = ClampNearZero(tensor.M11); |
| | 102 | 114 | | tensor.M12 = ClampNearZero(tensor.M12); |
| | 102 | 115 | | tensor.M13 = ClampNearZero(tensor.M13); |
| | 102 | 116 | | tensor.M21 = ClampNearZero(tensor.M21); |
| | 102 | 117 | | tensor.M22 = ClampNearZero(tensor.M22); |
| | 102 | 118 | | tensor.M23 = ClampNearZero(tensor.M23); |
| | 102 | 119 | | tensor.M31 = ClampNearZero(tensor.M31); |
| | 102 | 120 | | tensor.M32 = ClampNearZero(tensor.M32); |
| | 102 | 121 | | tensor.M33 = ClampNearZero(tensor.M33); |
| | 102 | 122 | | return tensor; |
| | | 123 | | } |
| | | 124 | | |
| | | 125 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 126 | | private static Fixed64 ClampNearZero(Fixed64 value) => |
| | 918 | 127 | | value.Abs() <= Fixed64.Epsilon ? Fixed64.Zero : value; |
| | | 128 | | } |