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