| | | 1 | | //======================================================================= |
| | | 2 | | // MeshCheckedMath.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 | | |
| | | 10 | | namespace Gravitas.Colliders; |
| | | 11 | | |
| | | 12 | | /// <summary> |
| | | 13 | | /// Provides checked fixed-point arithmetic for mesh mass properties and inertia shifts. |
| | | 14 | | /// </summary> |
| | | 15 | | internal static class MeshCheckedMath |
| | | 16 | | { |
| | | 17 | | internal static bool TryCreateParallelAxisTensor(Vector3d offset, out Fixed3x3 tensor) |
| | | 18 | | { |
| | 135 | 19 | | tensor = default; |
| | 135 | 20 | | bool valid = TryMultiply(offset.X, offset.X, out Fixed64 xx); |
| | 135 | 21 | | valid &= TryMultiply(offset.Y, offset.Y, out Fixed64 yy); |
| | 135 | 22 | | valid &= TryMultiply(offset.Z, offset.Z, out Fixed64 zz); |
| | 135 | 23 | | valid &= TryMultiply(offset.X, offset.Y, out Fixed64 xyProduct); |
| | 135 | 24 | | valid &= TryMultiply(offset.X, offset.Z, out Fixed64 xzProduct); |
| | 135 | 25 | | valid &= TryMultiply(offset.Y, offset.Z, out Fixed64 yzProduct); |
| | 135 | 26 | | valid &= TryAdd(yy, zz, out Fixed64 diagonalX); |
| | 135 | 27 | | valid &= TryAdd(xx, zz, out Fixed64 diagonalY); |
| | 135 | 28 | | valid &= TryAdd(xx, yy, out Fixed64 diagonalZ); |
| | 135 | 29 | | valid &= TryNegate(xyProduct, out Fixed64 xy); |
| | 135 | 30 | | valid &= TryNegate(xzProduct, out Fixed64 xz); |
| | 135 | 31 | | valid &= TryNegate(yzProduct, out Fixed64 yz); |
| | 135 | 32 | | if (!valid) |
| | 3 | 33 | | return false; |
| | | 34 | | |
| | 132 | 35 | | tensor = new Fixed3x3(diagonalX, xy, xz, xy, diagonalY, yz, xz, yz, diagonalZ); |
| | 132 | 36 | | return true; |
| | | 37 | | } |
| | | 38 | | |
| | | 39 | | internal static bool TryAdd(Vector3d first, Vector3d second, out Vector3d result) => |
| | 1 | 40 | | Vector3d.TryAdd(first, second, out result); |
| | | 41 | | |
| | | 42 | | internal static bool TrySubtract(Vector3d first, Vector3d second, out Vector3d result) => |
| | 135 | 43 | | Vector3d.TrySubtract(first, second, out result); |
| | | 44 | | |
| | | 45 | | internal static bool TryMultiply(Vector3d first, Vector3d second, out Vector3d result) |
| | | 46 | | { |
| | 135 | 47 | | result = default; |
| | 135 | 48 | | bool valid = Fixed64.TryMultiplyDivide( |
| | 135 | 49 | | first.X, |
| | 135 | 50 | | second.X, |
| | 135 | 51 | | Fixed64.One, |
| | 135 | 52 | | out Fixed64 x); |
| | 135 | 53 | | valid &= Fixed64.TryMultiplyDivide( |
| | 135 | 54 | | first.Y, |
| | 135 | 55 | | second.Y, |
| | 135 | 56 | | Fixed64.One, |
| | 135 | 57 | | out Fixed64 y); |
| | 135 | 58 | | valid &= Fixed64.TryMultiplyDivide( |
| | 135 | 59 | | first.Z, |
| | 135 | 60 | | second.Z, |
| | 135 | 61 | | Fixed64.One, |
| | 135 | 62 | | out Fixed64 z); |
| | 135 | 63 | | if (!valid) |
| | 1 | 64 | | return false; |
| | | 65 | | |
| | 134 | 66 | | result = new Vector3d(x, y, z); |
| | 134 | 67 | | return true; |
| | | 68 | | } |
| | | 69 | | |
| | | 70 | | internal static bool TryAdd(Fixed3x3 first, Fixed3x3 second, out Fixed3x3 result) => |
| | 68 | 71 | | TryCreateMatrix(first, second, add: true, out result); |
| | | 72 | | |
| | | 73 | | internal static bool TrySubtract(Fixed3x3 first, Fixed3x3 second, out Fixed3x3 result) => |
| | 67 | 74 | | TryCreateMatrix(first, second, add: false, out result); |
| | | 75 | | |
| | | 76 | | internal static bool TryAdd( |
| | | 77 | | Fixed64 first, |
| | | 78 | | Fixed64 second, |
| | | 79 | | out Fixed64 result) => |
| | 1419 | 80 | | Fixed64.TryAdd(first, second, out result); |
| | | 81 | | |
| | | 82 | | internal static bool TrySubtract( |
| | | 83 | | Fixed64 first, |
| | | 84 | | Fixed64 second, |
| | | 85 | | out Fixed64 result) => |
| | 804 | 86 | | Fixed64.TrySubtract(first, second, out result); |
| | | 87 | | |
| | | 88 | | internal static bool TryMultiply( |
| | | 89 | | Fixed64 first, |
| | | 90 | | Fixed64 second, |
| | | 91 | | out Fixed64 result) => |
| | 2028 | 92 | | Fixed64.TryMultiplyDivide( |
| | 2028 | 93 | | first, |
| | 2028 | 94 | | second, |
| | 2028 | 95 | | Fixed64.One, |
| | 2028 | 96 | | out result); |
| | | 97 | | |
| | | 98 | | internal static bool TryNegate( |
| | | 99 | | Fixed64 value, |
| | | 100 | | out Fixed64 result) => |
| | 807 | 101 | | Fixed64.TrySubtract(Fixed64.Zero, value, out result); |
| | | 102 | | |
| | | 103 | | private static bool TryCreateMatrix(Fixed3x3 first, Fixed3x3 second, bool add, out Fixed3x3 result) |
| | | 104 | | { |
| | 135 | 105 | | result = default; |
| | 135 | 106 | | bool valid = TryCombine(first.M11, second.M11, add, out Fixed64 m11); |
| | 135 | 107 | | valid &= TryCombine(first.M12, second.M12, add, out Fixed64 m12); |
| | 135 | 108 | | valid &= TryCombine(first.M13, second.M13, add, out Fixed64 m13); |
| | 135 | 109 | | valid &= TryCombine(first.M21, second.M21, add, out Fixed64 m21); |
| | 135 | 110 | | valid &= TryCombine(first.M22, second.M22, add, out Fixed64 m22); |
| | 135 | 111 | | valid &= TryCombine(first.M23, second.M23, add, out Fixed64 m23); |
| | 135 | 112 | | valid &= TryCombine(first.M31, second.M31, add, out Fixed64 m31); |
| | 135 | 113 | | valid &= TryCombine(first.M32, second.M32, add, out Fixed64 m32); |
| | 135 | 114 | | valid &= TryCombine(first.M33, second.M33, add, out Fixed64 m33); |
| | 135 | 115 | | if (!valid) |
| | 1 | 116 | | return false; |
| | | 117 | | |
| | 134 | 118 | | result = new Fixed3x3(m11, m12, m13, m21, m22, m23, m31, m32, m33); |
| | 134 | 119 | | return true; |
| | | 120 | | } |
| | | 121 | | |
| | | 122 | | private static bool TryCombine(Fixed64 first, Fixed64 second, bool add, out Fixed64 result) => |
| | 1215 | 123 | | add ? TryAdd(first, second, out result) : TrySubtract(first, second, out result); |
| | | 124 | | } |