| | | 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 System.Runtime.CompilerServices; |
| | | 9 | | using FixedMathSharp; |
| | | 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) => |
| | 29863 | 21 | | tensor.M12 == Fixed64.Zero |
| | 29863 | 22 | | && tensor.M13 == Fixed64.Zero |
| | 29863 | 23 | | && tensor.M21 == Fixed64.Zero |
| | 29863 | 24 | | && tensor.M23 == Fixed64.Zero |
| | 29863 | 25 | | && tensor.M31 == Fixed64.Zero |
| | 29863 | 26 | | && tensor.M32 == Fixed64.Zero; |
| | | 27 | | |
| | | 28 | | public static Fixed3x3 InvertForSolver(Fixed3x3 tensor) |
| | | 29 | | { |
| | 86519 | 30 | | if (tensor == Fixed3x3.Zero) |
| | 56656 | 31 | | return Fixed3x3.Zero; |
| | | 32 | | |
| | 29863 | 33 | | if (IsDiagonal(tensor)) |
| | 29641 | 34 | | return InvertDiagonalForSolver(tensor); |
| | | 35 | | |
| | 222 | 36 | | if (!Fixed3x3.Invert(tensor, out Fixed3x3? inverse) || !inverse.HasValue) |
| | 25 | 37 | | return Fixed3x3.Zero; |
| | | 38 | | |
| | 197 | 39 | | return ClampNearZero(inverse.Value); |
| | | 40 | | } |
| | | 41 | | |
| | | 42 | | public static Fixed3x3 AddParallelAxisTensor(Fixed3x3 tensor, Fixed64 mass, Vector3d offset) |
| | | 43 | | { |
| | 17 | 44 | | if (mass <= Fixed64.Zero || offset == Vector3d.Zero) |
| | 5 | 45 | | return tensor; |
| | | 46 | | |
| | 12 | 47 | | Fixed64 xx = mass * ((offset.Y * offset.Y) + (offset.Z * offset.Z)); |
| | 12 | 48 | | Fixed64 yy = mass * ((offset.X * offset.X) + (offset.Z * offset.Z)); |
| | 12 | 49 | | Fixed64 zz = mass * ((offset.X * offset.X) + (offset.Y * offset.Y)); |
| | 12 | 50 | | Fixed64 xy = mass * offset.X * offset.Y; |
| | 12 | 51 | | Fixed64 xz = mass * offset.X * offset.Z; |
| | 12 | 52 | | Fixed64 yz = mass * offset.Y * offset.Z; |
| | | 53 | | |
| | 12 | 54 | | tensor.M11 += xx; |
| | 12 | 55 | | tensor.M22 += yy; |
| | 12 | 56 | | tensor.M33 += zz; |
| | 12 | 57 | | tensor.M12 -= xy; |
| | 12 | 58 | | tensor.M21 -= xy; |
| | 12 | 59 | | tensor.M13 -= xz; |
| | 12 | 60 | | tensor.M31 -= xz; |
| | 12 | 61 | | tensor.M23 -= yz; |
| | 12 | 62 | | tensor.M32 -= yz; |
| | 12 | 63 | | return ClampNearZero(tensor); |
| | | 64 | | } |
| | | 65 | | |
| | | 66 | | public static Fixed3x3 SubtractParallelAxisTensor(Fixed3x3 tensor, Fixed64 mass, Vector3d offset) |
| | | 67 | | { |
| | 4 | 68 | | if (mass <= Fixed64.Zero || offset == Vector3d.Zero) |
| | 3 | 69 | | return tensor; |
| | | 70 | | |
| | 1 | 71 | | Fixed64 xx = mass * ((offset.Y * offset.Y) + (offset.Z * offset.Z)); |
| | 1 | 72 | | Fixed64 yy = mass * ((offset.X * offset.X) + (offset.Z * offset.Z)); |
| | 1 | 73 | | Fixed64 zz = mass * ((offset.X * offset.X) + (offset.Y * offset.Y)); |
| | 1 | 74 | | Fixed64 xy = mass * offset.X * offset.Y; |
| | 1 | 75 | | Fixed64 xz = mass * offset.X * offset.Z; |
| | 1 | 76 | | Fixed64 yz = mass * offset.Y * offset.Z; |
| | | 77 | | |
| | 1 | 78 | | tensor.M11 -= xx; |
| | 1 | 79 | | tensor.M22 -= yy; |
| | 1 | 80 | | tensor.M33 -= zz; |
| | 1 | 81 | | tensor.M12 += xy; |
| | 1 | 82 | | tensor.M21 += xy; |
| | 1 | 83 | | tensor.M13 += xz; |
| | 1 | 84 | | tensor.M31 += xz; |
| | 1 | 85 | | tensor.M23 += yz; |
| | 1 | 86 | | tensor.M32 += yz; |
| | 1 | 87 | | return ClampNearZero(tensor); |
| | | 88 | | } |
| | | 89 | | |
| | | 90 | | public static Fixed3x3 RotateToFrame(Fixed3x3 tensor, FixedQuaternion rotation) |
| | | 91 | | { |
| | 5 | 92 | | if (rotation == FixedQuaternion.Identity) |
| | 2 | 93 | | return tensor; |
| | | 94 | | |
| | 3 | 95 | | Fixed3x3 rotationMatrix = rotation.ToMatrix3x3(); |
| | | 96 | | // FixedMathSharp matrices rotate row vectors; tensor transport uses |
| | | 97 | | // the corresponding active-column basis, consistently with geometry. |
| | 3 | 98 | | return ClampNearZero(rotationMatrix.Transpose() * tensor * rotationMatrix); |
| | | 99 | | } |
| | | 100 | | |
| | | 101 | | private static Fixed3x3 InvertDiagonalForSolver(Fixed3x3 tensor) => |
| | 29641 | 102 | | new( |
| | 29641 | 103 | | tensor.M11 > Fixed64.Zero ? Fixed64.One / tensor.M11 : Fixed64.Zero, |
| | 29641 | 104 | | Fixed64.Zero, |
| | 29641 | 105 | | Fixed64.Zero, |
| | 29641 | 106 | | Fixed64.Zero, |
| | 29641 | 107 | | tensor.M22 > Fixed64.Zero ? Fixed64.One / tensor.M22 : Fixed64.Zero, |
| | 29641 | 108 | | Fixed64.Zero, |
| | 29641 | 109 | | Fixed64.Zero, |
| | 29641 | 110 | | Fixed64.Zero, |
| | 29641 | 111 | | tensor.M33 > Fixed64.Zero ? Fixed64.One / tensor.M33 : Fixed64.Zero); |
| | | 112 | | |
| | | 113 | | private static Fixed3x3 ClampNearZero(Fixed3x3 tensor) |
| | | 114 | | { |
| | 213 | 115 | | tensor.M11 = ClampNearZero(tensor.M11); |
| | 213 | 116 | | tensor.M12 = ClampNearZero(tensor.M12); |
| | 213 | 117 | | tensor.M13 = ClampNearZero(tensor.M13); |
| | 213 | 118 | | tensor.M21 = ClampNearZero(tensor.M21); |
| | 213 | 119 | | tensor.M22 = ClampNearZero(tensor.M22); |
| | 213 | 120 | | tensor.M23 = ClampNearZero(tensor.M23); |
| | 213 | 121 | | tensor.M31 = ClampNearZero(tensor.M31); |
| | 213 | 122 | | tensor.M32 = ClampNearZero(tensor.M32); |
| | 213 | 123 | | tensor.M33 = ClampNearZero(tensor.M33); |
| | 213 | 124 | | return tensor; |
| | | 125 | | } |
| | | 126 | | |
| | | 127 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 128 | | private static Fixed64 ClampNearZero(Fixed64 value) => |
| | 1917 | 129 | | value.Abs() <= Fixed64.Epsilon ? Fixed64.Zero : value; |
| | | 130 | | } |