< Summary

Information
Class: Gravitas.InertiaTensorMath
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/InertiaTensorMath.cs
Line coverage
100%
Covered lines: 74
Uncovered lines: 0
Coverable lines: 74
Total lines: 130
Line coverage: 100%
Branch coverage
100%
Covered branches: 36
Total branches: 36
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
IsDiagonal(...)100%1010100%
InvertForSolver(...)100%88100%
AddParallelAxisTensor(...)100%44100%
SubtractParallelAxisTensor(...)100%44100%
RotateToFrame(...)100%22100%
InvertDiagonalForSolver(...)100%66100%
ClampNearZero(...)100%11100%
ClampNearZero(...)100%22100%

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/InertiaTensorMath.cs

#LineLine coverage
 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
 8using System.Runtime.CompilerServices;
 9using FixedMathSharp;
 10
 11namespace Gravitas;
 12
 13/// <summary>
 14/// Deterministic helpers for 3D inertia tensors used by body mass properties
 15/// and solver effective mass calculations.
 16/// </summary>
 17internal static class InertiaTensorMath
 18{
 19    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 20    public static bool IsDiagonal(Fixed3x3 tensor) =>
 2986321        tensor.M12 == Fixed64.Zero
 2986322        && tensor.M13 == Fixed64.Zero
 2986323        && tensor.M21 == Fixed64.Zero
 2986324        && tensor.M23 == Fixed64.Zero
 2986325        && tensor.M31 == Fixed64.Zero
 2986326        && tensor.M32 == Fixed64.Zero;
 27
 28    public static Fixed3x3 InvertForSolver(Fixed3x3 tensor)
 29    {
 8651930        if (tensor == Fixed3x3.Zero)
 5665631            return Fixed3x3.Zero;
 32
 2986333        if (IsDiagonal(tensor))
 2964134            return InvertDiagonalForSolver(tensor);
 35
 22236        if (!Fixed3x3.Invert(tensor, out Fixed3x3? inverse) || !inverse.HasValue)
 2537            return Fixed3x3.Zero;
 38
 19739        return ClampNearZero(inverse.Value);
 40    }
 41
 42    public static Fixed3x3 AddParallelAxisTensor(Fixed3x3 tensor, Fixed64 mass, Vector3d offset)
 43    {
 1744        if (mass <= Fixed64.Zero || offset == Vector3d.Zero)
 545            return tensor;
 46
 1247        Fixed64 xx = mass * ((offset.Y * offset.Y) + (offset.Z * offset.Z));
 1248        Fixed64 yy = mass * ((offset.X * offset.X) + (offset.Z * offset.Z));
 1249        Fixed64 zz = mass * ((offset.X * offset.X) + (offset.Y * offset.Y));
 1250        Fixed64 xy = mass * offset.X * offset.Y;
 1251        Fixed64 xz = mass * offset.X * offset.Z;
 1252        Fixed64 yz = mass * offset.Y * offset.Z;
 53
 1254        tensor.M11 += xx;
 1255        tensor.M22 += yy;
 1256        tensor.M33 += zz;
 1257        tensor.M12 -= xy;
 1258        tensor.M21 -= xy;
 1259        tensor.M13 -= xz;
 1260        tensor.M31 -= xz;
 1261        tensor.M23 -= yz;
 1262        tensor.M32 -= yz;
 1263        return ClampNearZero(tensor);
 64    }
 65
 66    public static Fixed3x3 SubtractParallelAxisTensor(Fixed3x3 tensor, Fixed64 mass, Vector3d offset)
 67    {
 468        if (mass <= Fixed64.Zero || offset == Vector3d.Zero)
 369            return tensor;
 70
 171        Fixed64 xx = mass * ((offset.Y * offset.Y) + (offset.Z * offset.Z));
 172        Fixed64 yy = mass * ((offset.X * offset.X) + (offset.Z * offset.Z));
 173        Fixed64 zz = mass * ((offset.X * offset.X) + (offset.Y * offset.Y));
 174        Fixed64 xy = mass * offset.X * offset.Y;
 175        Fixed64 xz = mass * offset.X * offset.Z;
 176        Fixed64 yz = mass * offset.Y * offset.Z;
 77
 178        tensor.M11 -= xx;
 179        tensor.M22 -= yy;
 180        tensor.M33 -= zz;
 181        tensor.M12 += xy;
 182        tensor.M21 += xy;
 183        tensor.M13 += xz;
 184        tensor.M31 += xz;
 185        tensor.M23 += yz;
 186        tensor.M32 += yz;
 187        return ClampNearZero(tensor);
 88    }
 89
 90    public static Fixed3x3 RotateToFrame(Fixed3x3 tensor, FixedQuaternion rotation)
 91    {
 592        if (rotation == FixedQuaternion.Identity)
 293            return tensor;
 94
 395        Fixed3x3 rotationMatrix = rotation.ToMatrix3x3();
 96        // FixedMathSharp matrices rotate row vectors; tensor transport uses
 97        // the corresponding active-column basis, consistently with geometry.
 398        return ClampNearZero(rotationMatrix.Transpose() * tensor * rotationMatrix);
 99    }
 100
 101    private static Fixed3x3 InvertDiagonalForSolver(Fixed3x3 tensor) =>
 29641102        new(
 29641103            tensor.M11 > Fixed64.Zero ? Fixed64.One / tensor.M11 : Fixed64.Zero,
 29641104            Fixed64.Zero,
 29641105            Fixed64.Zero,
 29641106            Fixed64.Zero,
 29641107            tensor.M22 > Fixed64.Zero ? Fixed64.One / tensor.M22 : Fixed64.Zero,
 29641108            Fixed64.Zero,
 29641109            Fixed64.Zero,
 29641110            Fixed64.Zero,
 29641111            tensor.M33 > Fixed64.Zero ? Fixed64.One / tensor.M33 : Fixed64.Zero);
 112
 113    private static Fixed3x3 ClampNearZero(Fixed3x3 tensor)
 114    {
 213115        tensor.M11 = ClampNearZero(tensor.M11);
 213116        tensor.M12 = ClampNearZero(tensor.M12);
 213117        tensor.M13 = ClampNearZero(tensor.M13);
 213118        tensor.M21 = ClampNearZero(tensor.M21);
 213119        tensor.M22 = ClampNearZero(tensor.M22);
 213120        tensor.M23 = ClampNearZero(tensor.M23);
 213121        tensor.M31 = ClampNearZero(tensor.M31);
 213122        tensor.M32 = ClampNearZero(tensor.M32);
 213123        tensor.M33 = ClampNearZero(tensor.M33);
 213124        return tensor;
 125    }
 126
 127    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 128    private static Fixed64 ClampNearZero(Fixed64 value) =>
 1917129        value.Abs() <= Fixed64.Epsilon ? Fixed64.Zero : value;
 130}