| | | 1 | | //======================================================================= |
| | | 2 | | // TriangleShellMassProperties.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2024-present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using System; |
| | | 9 | | |
| | | 10 | | using FixedMathSharp; |
| | | 11 | | using FixedMathSharp.Geometry; |
| | | 12 | | |
| | | 13 | | namespace Gravitas.Colliders; |
| | | 14 | | |
| | | 15 | | /// <summary> |
| | | 16 | | /// Owns wide uniform thin-shell integration for indexed triangle surfaces. |
| | | 17 | | /// </summary> |
| | | 18 | | internal static partial class TriangleShellMassProperties |
| | | 19 | | { |
| | 2 | 20 | | private static readonly Signed192 Three = Signed192.Signed(3); |
| | 2 | 21 | | private static readonly Signed192 Six = Signed192.Signed(6); |
| | 2 | 22 | | private static readonly Signed192 Twelve = Signed192.Signed(12); |
| | | 23 | | |
| | | 24 | | internal static bool TryCreateUniformShell( |
| | | 25 | | ReadOnlySpan<Vector3d> vertices, |
| | | 26 | | ReadOnlySpan<int> triangleIndices, |
| | | 27 | | out ExactMassWeight surfaceWeight, |
| | | 28 | | out Vector3d centerOfMass, |
| | | 29 | | out Fixed3x3 unitMassInertiaTensor, |
| | | 30 | | out MeshMassIntegrals integrals, |
| | | 31 | | in ScaledRigidFrame3d sourceFrame = default, |
| | | 32 | | bool exactScale = false) |
| | | 33 | | { |
| | 4783 | 34 | | integrals = default; |
| | 4783 | 35 | | if (triangleIndices.Length % FixedTriangle.VertexCount != 0) |
| | | 36 | | { |
| | 1 | 37 | | throw new ArgumentException( |
| | 1 | 38 | | "Triangle indices must contain complete index triplets.", |
| | 1 | 39 | | nameof(triangleIndices)); |
| | | 40 | | } |
| | | 41 | | |
| | 4782 | 42 | | Signed576 totalWeight = default; |
| | 4782 | 43 | | Signed576 firstMomentX = default; |
| | 4782 | 44 | | Signed576 firstMomentY = default; |
| | 4782 | 45 | | Signed576 firstMomentZ = default; |
| | 4782 | 46 | | Signed576 secondMomentX = default; |
| | 4782 | 47 | | Signed576 secondMomentY = default; |
| | 4782 | 48 | | Signed576 secondMomentZ = default; |
| | 4782 | 49 | | Signed576 productMomentXY = default; |
| | 4782 | 50 | | Signed576 productMomentXZ = default; |
| | 4782 | 51 | | Signed576 productMomentYZ = default; |
| | 307820 | 52 | | for (int i = 0; i < triangleIndices.Length; i += FixedTriangle.VertexCount) |
| | | 53 | | { |
| | 149131 | 54 | | GetTriangle( |
| | 149131 | 55 | | vertices, |
| | 149131 | 56 | | triangleIndices, |
| | 149131 | 57 | | i, |
| | 149131 | 58 | | out FixedTriangle triangle); |
| | 149128 | 59 | | var coordinates = new TriangleCoordinates(triangle, sourceFrame, exactScale); |
| | | 60 | | ExactMassWeight weight; |
| | 149128 | 61 | | if (exactScale) |
| | | 62 | | { |
| | 585 | 63 | | WideDirection3d normal = WideTriangleRelations.GetScaledTriangleLocalNormal(triangle, sourceFrame); |
| | | 64 | | // Q128 area weights retain the exact scaled surface even when |
| | | 65 | | // its Q32.32 inspection vertices would collapse to a line. |
| | 585 | 66 | | Signed576 root = WideArithmetic.GetFloorSquareRootOfProduct(normal.SquaredLength, Signed192.Signed(9)); |
| | 585 | 67 | | Signed192 numerator = WideArithmetic.DivideSigned576ToSigned192(root, new Signed320(0, 0, 0, 1, 0)); |
| | 585 | 68 | | weight = new ExactMassWeight(Signed320.ExtendValue(numerator)); |
| | | 69 | | } |
| | | 70 | | else |
| | | 71 | | { |
| | 148543 | 72 | | triangle.GetExactNormal(out _, out _, out _, out Signed320 squaredDoubleArea); |
| | 148543 | 73 | | weight = ExactMassProperties.CreateTriangleAreaWeight(squaredDoubleArea); |
| | | 74 | | } |
| | 149128 | 75 | | Signed320 weightNumerator = weight.Numerator; |
| | 149128 | 76 | | totalWeight = WideArithmetic.AddSigned576( |
| | 149128 | 77 | | totalWeight, |
| | 149128 | 78 | | Signed576.ExtendValue(weightNumerator)); |
| | 149128 | 79 | | firstMomentX = AddWeightedVertexSum( |
| | 149128 | 80 | | firstMomentX, |
| | 149128 | 81 | | coordinates.AX, |
| | 149128 | 82 | | coordinates.BX, |
| | 149128 | 83 | | coordinates.CX, |
| | 149128 | 84 | | weightNumerator); |
| | 149128 | 85 | | firstMomentY = AddWeightedVertexSum( |
| | 149128 | 86 | | firstMomentY, |
| | 149128 | 87 | | coordinates.AY, |
| | 149128 | 88 | | coordinates.BY, |
| | 149128 | 89 | | coordinates.CY, |
| | 149128 | 90 | | weightNumerator); |
| | 149128 | 91 | | firstMomentZ = AddWeightedVertexSum( |
| | 149128 | 92 | | firstMomentZ, |
| | 149128 | 93 | | coordinates.AZ, |
| | 149128 | 94 | | coordinates.BZ, |
| | 149128 | 95 | | coordinates.CZ, |
| | 149128 | 96 | | weightNumerator); |
| | 149128 | 97 | | GetProductSums( |
| | 149128 | 98 | | coordinates, |
| | 149128 | 99 | | out Signed320 x2, |
| | 149128 | 100 | | out Signed320 y2, |
| | 149128 | 101 | | out Signed320 z2, |
| | 149128 | 102 | | out Signed320 xy, |
| | 149128 | 103 | | out Signed320 xz, |
| | 149128 | 104 | | out Signed320 yz); |
| | 149128 | 105 | | secondMomentX = AddWeighted( |
| | 149128 | 106 | | secondMomentX, |
| | 149128 | 107 | | x2, |
| | 149128 | 108 | | weightNumerator); |
| | 149128 | 109 | | secondMomentY = AddWeighted( |
| | 149128 | 110 | | secondMomentY, |
| | 149128 | 111 | | y2, |
| | 149128 | 112 | | weightNumerator); |
| | 149128 | 113 | | secondMomentZ = AddWeighted( |
| | 149128 | 114 | | secondMomentZ, |
| | 149128 | 115 | | z2, |
| | 149128 | 116 | | weightNumerator); |
| | 149128 | 117 | | productMomentXY = AddWeighted( |
| | 149128 | 118 | | productMomentXY, |
| | 149128 | 119 | | xy, |
| | 149128 | 120 | | weightNumerator); |
| | 149128 | 121 | | productMomentXZ = AddWeighted( |
| | 149128 | 122 | | productMomentXZ, |
| | 149128 | 123 | | xz, |
| | 149128 | 124 | | weightNumerator); |
| | 149128 | 125 | | productMomentYZ = AddWeighted( |
| | 149128 | 126 | | productMomentYZ, |
| | 149128 | 127 | | yz, |
| | 149128 | 128 | | weightNumerator); |
| | | 129 | | } |
| | | 130 | | |
| | 4779 | 131 | | if (totalWeight.IsZero) |
| | | 132 | | { |
| | 1 | 133 | | surfaceWeight = default; |
| | 1 | 134 | | centerOfMass = default; |
| | 1 | 135 | | unitMassInertiaTensor = default; |
| | 1 | 136 | | integrals = default; |
| | 1 | 137 | | return false; |
| | | 138 | | } |
| | | 139 | | |
| | 4778 | 140 | | surfaceWeight = new ExactMassWeight( |
| | 4778 | 141 | | Signed320.NarrowValue(totalWeight)); |
| | 4778 | 142 | | integrals = MeshMassIntegrals.FromShell(surfaceWeight, |
| | 4778 | 143 | | firstMomentX, firstMomentY, firstMomentZ, |
| | 4778 | 144 | | secondMomentX, secondMomentY, secondMomentZ, |
| | 4778 | 145 | | productMomentXY, productMomentXZ, productMomentYZ, exactScale ? 64 : 0); |
| | 4778 | 146 | | Signed576 centerDenominator = |
| | 4778 | 147 | | WideArithmetic.MultiplySigned576( |
| | 4778 | 148 | | totalWeight, |
| | 4778 | 149 | | exactScale ? Signed192.NarrowValue(WideArithmetic.MultiplySigned192(Three, new Signed192(0, 1, 0))) : Th |
| | 4778 | 150 | | _ = Fixed64.TryGetSignedRawRatio( |
| | 4778 | 151 | | firstMomentX, |
| | 4778 | 152 | | centerDenominator, |
| | 4778 | 153 | | out Fixed64 centerX); |
| | 4778 | 154 | | _ = Fixed64.TryGetSignedRawRatio( |
| | 4778 | 155 | | firstMomentY, |
| | 4778 | 156 | | centerDenominator, |
| | 4778 | 157 | | out Fixed64 centerY); |
| | 4778 | 158 | | _ = Fixed64.TryGetSignedRawRatio( |
| | 4778 | 159 | | firstMomentZ, |
| | 4778 | 160 | | centerDenominator, |
| | 4778 | 161 | | out Fixed64 centerZ); |
| | 4778 | 162 | | centerOfMass = new Vector3d(centerX, centerY, centerZ); |
| | | 163 | | |
| | 4778 | 164 | | Signed576 centeredX = TranslateSquaredMoment( |
| | 4778 | 165 | | secondMomentX, |
| | 4778 | 166 | | firstMomentX, |
| | 4778 | 167 | | totalWeight, |
| | 4778 | 168 | | exactScale ? ScaleCoordinate(centerX) : Signed192.Raw(centerX)); |
| | 4778 | 169 | | Signed576 centeredY = TranslateSquaredMoment( |
| | 4778 | 170 | | secondMomentY, |
| | 4778 | 171 | | firstMomentY, |
| | 4778 | 172 | | totalWeight, |
| | 4778 | 173 | | exactScale ? ScaleCoordinate(centerY) : Signed192.Raw(centerY)); |
| | 4778 | 174 | | Signed576 centeredZ = TranslateSquaredMoment( |
| | 4778 | 175 | | secondMomentZ, |
| | 4778 | 176 | | firstMomentZ, |
| | 4778 | 177 | | totalWeight, |
| | 4778 | 178 | | exactScale ? ScaleCoordinate(centerZ) : Signed192.Raw(centerZ)); |
| | 4778 | 179 | | Signed576 centeredXY = TranslateProductMoment( |
| | 4778 | 180 | | productMomentXY, |
| | 4778 | 181 | | firstMomentX, |
| | 4778 | 182 | | firstMomentY, |
| | 4778 | 183 | | totalWeight, |
| | 4778 | 184 | | exactScale ? ScaleCoordinate(centerX) : Signed192.Raw(centerX), |
| | 4778 | 185 | | exactScale ? ScaleCoordinate(centerY) : Signed192.Raw(centerY)); |
| | 4778 | 186 | | Signed576 centeredXZ = TranslateProductMoment( |
| | 4778 | 187 | | productMomentXZ, |
| | 4778 | 188 | | firstMomentX, |
| | 4778 | 189 | | firstMomentZ, |
| | 4778 | 190 | | totalWeight, |
| | 4778 | 191 | | exactScale ? ScaleCoordinate(centerX) : Signed192.Raw(centerX), |
| | 4778 | 192 | | exactScale ? ScaleCoordinate(centerZ) : Signed192.Raw(centerZ)); |
| | 4778 | 193 | | Signed576 centeredYZ = TranslateProductMoment( |
| | 4778 | 194 | | productMomentYZ, |
| | 4778 | 195 | | firstMomentY, |
| | 4778 | 196 | | firstMomentZ, |
| | 4778 | 197 | | totalWeight, |
| | 4778 | 198 | | exactScale ? ScaleCoordinate(centerY) : Signed192.Raw(centerY), |
| | 4778 | 199 | | exactScale ? ScaleCoordinate(centerZ) : Signed192.Raw(centerZ)); |
| | 4778 | 200 | | Signed576 inertiaXX = |
| | 4778 | 201 | | WideArithmetic.AddSigned576(centeredY, centeredZ); |
| | 4778 | 202 | | Signed576 inertiaYY = |
| | 4778 | 203 | | WideArithmetic.AddSigned576(centeredX, centeredZ); |
| | 4778 | 204 | | Signed576 inertiaZZ = |
| | 4778 | 205 | | WideArithmetic.AddSigned576(centeredX, centeredY); |
| | | 206 | | |
| | 4778 | 207 | | Signed576 diagonalDenominator = |
| | 4778 | 208 | | WideArithmetic.MultiplySigned576( |
| | 4778 | 209 | | totalWeight, |
| | 4778 | 210 | | exactScale ? new Signed192(1UL << 32, 0, 0) : Signed192.One, |
| | 4778 | 211 | | Six); |
| | 4778 | 212 | | Signed576 productDenominator = |
| | 4778 | 213 | | WideArithmetic.MultiplySigned576( |
| | 4778 | 214 | | totalWeight, |
| | 4778 | 215 | | exactScale ? new Signed192(1UL << 32, 0, 0) : Signed192.One, |
| | 4778 | 216 | | Twelve); |
| | 4778 | 217 | | bool representable = Fixed64.TryGetSignedRawRatio( |
| | 4778 | 218 | | inertiaXX, |
| | 4778 | 219 | | diagonalDenominator, |
| | 4778 | 220 | | out Fixed64 tensorXX) |
| | 4778 | 221 | | & Fixed64.TryGetSignedRawRatio( |
| | 4778 | 222 | | inertiaYY, |
| | 4778 | 223 | | diagonalDenominator, |
| | 4778 | 224 | | out Fixed64 tensorYY) |
| | 4778 | 225 | | & Fixed64.TryGetSignedRawRatio( |
| | 4778 | 226 | | inertiaZZ, |
| | 4778 | 227 | | diagonalDenominator, |
| | 4778 | 228 | | out Fixed64 tensorZZ) |
| | 4778 | 229 | | & Fixed64.TryGetSignedRawRatio( |
| | 4778 | 230 | | WideArithmetic.SubtractSigned576(default, centeredXY), |
| | 4778 | 231 | | productDenominator, |
| | 4778 | 232 | | out Fixed64 tensorXY) |
| | 4778 | 233 | | & Fixed64.TryGetSignedRawRatio( |
| | 4778 | 234 | | WideArithmetic.SubtractSigned576(default, centeredXZ), |
| | 4778 | 235 | | productDenominator, |
| | 4778 | 236 | | out Fixed64 tensorXZ) |
| | 4778 | 237 | | & Fixed64.TryGetSignedRawRatio( |
| | 4778 | 238 | | WideArithmetic.SubtractSigned576(default, centeredYZ), |
| | 4778 | 239 | | productDenominator, |
| | 4778 | 240 | | out Fixed64 tensorYZ); |
| | 4778 | 241 | | if (!representable) |
| | | 242 | | { |
| | 1 | 243 | | surfaceWeight = default; |
| | 1 | 244 | | centerOfMass = default; |
| | 1 | 245 | | unitMassInertiaTensor = default; |
| | 1 | 246 | | integrals = default; |
| | 1 | 247 | | return false; |
| | | 248 | | } |
| | | 249 | | |
| | 4777 | 250 | | unitMassInertiaTensor = new Fixed3x3( |
| | 4777 | 251 | | tensorXX, tensorXY, tensorXZ, |
| | 4777 | 252 | | tensorXY, tensorYY, tensorYZ, |
| | 4777 | 253 | | tensorXZ, tensorYZ, tensorZZ); |
| | 4777 | 254 | | return true; |
| | | 255 | | } |
| | | 256 | | |
| | | 257 | | internal static Signed576 AddWeightedVertexSum(Signed576 total, Signed192 first, Signed192 second, Signed192 third, |
| | | 258 | | { |
| | 447384 | 259 | | Signed320 sum = WideArithmetic.AddSigned320(WideArithmetic.AddSigned320( |
| | 447384 | 260 | | Signed320.ExtendValue(first), Signed320.ExtendValue(second)), Signed320.ExtendValue(third)); |
| | 447384 | 261 | | return AddWeighted(total, sum, weight); |
| | | 262 | | } |
| | | 263 | | |
| | | 264 | | private static Signed576 AddWeighted( |
| | | 265 | | Signed576 total, |
| | | 266 | | Signed320 value, |
| | | 267 | | Signed320 weight) => |
| | 1342152 | 268 | | WideArithmetic.AddSigned576( |
| | 1342152 | 269 | | total, |
| | 1342152 | 270 | | WideArithmetic.MultiplySigned320( |
| | 1342152 | 271 | | value, |
| | 1342152 | 272 | | weight)); |
| | | 273 | | |
| | | 274 | | internal static void GetProductSums(in TriangleCoordinates triangle, |
| | | 275 | | out Signed320 x2, out Signed320 y2, out Signed320 z2, out Signed320 xy, out Signed320 xz, out Signed320 yz) |
| | | 276 | | { |
| | 527976 | 277 | | Signed192 ax = triangle.AX, ay = triangle.AY, az = triangle.AZ; |
| | 527976 | 278 | | Signed192 bx = triangle.BX, by = triangle.BY, bz = triangle.BZ; |
| | 527976 | 279 | | Signed192 cx = triangle.CX, cy = triangle.CY, cz = triangle.CZ; |
| | 175992 | 280 | | x2 = SquaredBarycentricSum(ax, bx, cx); |
| | 175992 | 281 | | y2 = SquaredBarycentricSum(ay, by, cy); |
| | 175992 | 282 | | z2 = SquaredBarycentricSum(az, bz, cz); |
| | 175992 | 283 | | xy = BarycentricCrossSum(ax, bx, cx, ay, by, cy); |
| | 175992 | 284 | | xz = BarycentricCrossSum(ax, bx, cx, az, bz, cz); |
| | 175992 | 285 | | yz = BarycentricCrossSum(ay, by, cy, az, bz, cz); |
| | 175992 | 286 | | } |
| | | 287 | | |
| | | 288 | | private static Signed320 SquaredBarycentricSum( |
| | | 289 | | Signed192 first, |
| | | 290 | | Signed192 second, |
| | | 291 | | Signed192 third) => |
| | 527976 | 292 | | WideArithmetic.AddSigned320( |
| | 527976 | 293 | | WideArithmetic.AddSigned320( |
| | 527976 | 294 | | WideArithmetic.AddSigned320( |
| | 527976 | 295 | | WideArithmetic.MultiplySigned192(first, first), |
| | 527976 | 296 | | WideArithmetic.MultiplySigned192(second, second)), |
| | 527976 | 297 | | WideArithmetic.AddSigned320( |
| | 527976 | 298 | | WideArithmetic.MultiplySigned192(third, third), |
| | 527976 | 299 | | WideArithmetic.MultiplySigned192(first, second))), |
| | 527976 | 300 | | WideArithmetic.AddSigned320( |
| | 527976 | 301 | | WideArithmetic.MultiplySigned192(first, third), |
| | 527976 | 302 | | WideArithmetic.MultiplySigned192(second, third))); |
| | | 303 | | |
| | | 304 | | private static Signed320 BarycentricCrossSum( |
| | | 305 | | Signed192 firstA, |
| | | 306 | | Signed192 firstB, |
| | | 307 | | Signed192 firstC, |
| | | 308 | | Signed192 secondA, |
| | | 309 | | Signed192 secondB, |
| | | 310 | | Signed192 secondC) |
| | | 311 | | { |
| | 527976 | 312 | | Signed192 firstSum = Signed192.NarrowProven( |
| | 527976 | 313 | | WideArithmetic.AddSigned320( |
| | 527976 | 314 | | WideArithmetic.AddSigned320( |
| | 527976 | 315 | | Signed320.ExtendValue(firstA), |
| | 527976 | 316 | | Signed320.ExtendValue(firstB)), |
| | 527976 | 317 | | Signed320.ExtendValue(firstC))); |
| | 527976 | 318 | | Signed192 secondSum = Signed192.NarrowProven( |
| | 527976 | 319 | | WideArithmetic.AddSigned320( |
| | 527976 | 320 | | WideArithmetic.AddSigned320( |
| | 527976 | 321 | | Signed320.ExtendValue(secondA), |
| | 527976 | 322 | | Signed320.ExtendValue(secondB)), |
| | 527976 | 323 | | Signed320.ExtendValue(secondC))); |
| | 527976 | 324 | | Signed320 matching = WideArithmetic.AddSigned320( |
| | 527976 | 325 | | WideArithmetic.AddSigned320( |
| | 527976 | 326 | | WideArithmetic.MultiplySigned192(firstA, secondA), |
| | 527976 | 327 | | WideArithmetic.MultiplySigned192(firstB, secondB)), |
| | 527976 | 328 | | WideArithmetic.MultiplySigned192(firstC, secondC)); |
| | 527976 | 329 | | return WideArithmetic.AddSigned320( |
| | 527976 | 330 | | WideArithmetic.MultiplySigned192( |
| | 527976 | 331 | | firstSum, |
| | 527976 | 332 | | secondSum), |
| | 527976 | 333 | | matching); |
| | | 334 | | } |
| | | 335 | | |
| | | 336 | | private static Signed576 TranslateSquaredMoment( |
| | | 337 | | Signed576 secondMoment, |
| | | 338 | | Signed576 firstMoment, |
| | | 339 | | Signed576 totalWeight, |
| | | 340 | | Signed192 centerRaw) |
| | | 341 | | { |
| | 14334 | 342 | | Signed576 shifted = WideArithmetic.SubtractSigned576( |
| | 14334 | 343 | | secondMoment, |
| | 14334 | 344 | | WideArithmetic.MultiplySigned576( |
| | 14334 | 345 | | firstMoment, |
| | 14334 | 346 | | centerRaw, |
| | 14334 | 347 | | Signed192.Signed(4))); |
| | 14334 | 348 | | return WideArithmetic.AddSigned576( |
| | 14334 | 349 | | shifted, |
| | 14334 | 350 | | WideArithmetic.MultiplySigned576( |
| | 14334 | 351 | | totalWeight, |
| | 14334 | 352 | | centerRaw, |
| | 14334 | 353 | | centerRaw, |
| | 14334 | 354 | | Six)); |
| | | 355 | | } |
| | | 356 | | |
| | | 357 | | private static Signed576 TranslateProductMoment( |
| | | 358 | | Signed576 productMoment, |
| | | 359 | | Signed576 firstMoment, |
| | | 360 | | Signed576 secondMoment, |
| | | 361 | | Signed576 totalWeight, |
| | | 362 | | Signed192 firstCenterRaw, |
| | | 363 | | Signed192 secondCenterRaw) |
| | | 364 | | { |
| | 14334 | 365 | | Signed576 shifted = WideArithmetic.SubtractSigned576( |
| | 14334 | 366 | | productMoment, |
| | 14334 | 367 | | WideArithmetic.MultiplySigned576( |
| | 14334 | 368 | | secondMoment, |
| | 14334 | 369 | | firstCenterRaw, |
| | 14334 | 370 | | Signed192.Signed(4))); |
| | 14334 | 371 | | shifted = WideArithmetic.SubtractSigned576( |
| | 14334 | 372 | | shifted, |
| | 14334 | 373 | | WideArithmetic.MultiplySigned576( |
| | 14334 | 374 | | firstMoment, |
| | 14334 | 375 | | secondCenterRaw, |
| | 14334 | 376 | | Signed192.Signed(4))); |
| | 14334 | 377 | | return WideArithmetic.AddSigned576( |
| | 14334 | 378 | | shifted, |
| | 14334 | 379 | | WideArithmetic.MultiplySigned576( |
| | 14334 | 380 | | totalWeight, |
| | 14334 | 381 | | firstCenterRaw, |
| | 14334 | 382 | | secondCenterRaw, |
| | 14334 | 383 | | Twelve)); |
| | | 384 | | } |
| | | 385 | | |
| | 819 | 386 | | private static Signed192 ScaleCoordinate(Fixed64 value) => Signed192.NarrowValue( |
| | 819 | 387 | | WideArithmetic.MultiplySigned192(Signed192.Raw(value), new Signed192(0, 1, 0))); |
| | | 388 | | |
| | | 389 | | |
| | | 390 | | |
| | | 391 | | private static void GetTriangle( |
| | | 392 | | ReadOnlySpan<Vector3d> vertices, |
| | | 393 | | ReadOnlySpan<int> triangleIndices, |
| | | 394 | | int index, |
| | | 395 | | out FixedTriangle triangle) |
| | | 396 | | { |
| | 149131 | 397 | | int first = triangleIndices[index]; |
| | 149131 | 398 | | int second = triangleIndices[index + 1]; |
| | 149131 | 399 | | int third = triangleIndices[index + 2]; |
| | 149131 | 400 | | if ((uint)first >= (uint)vertices.Length |
| | 149131 | 401 | | || (uint)second >= (uint)vertices.Length |
| | 149131 | 402 | | || (uint)third >= (uint)vertices.Length) |
| | | 403 | | { |
| | 3 | 404 | | throw new ArgumentOutOfRangeException( |
| | 3 | 405 | | nameof(triangleIndices), |
| | 3 | 406 | | "Triangle indices must reference supplied vertices."); |
| | | 407 | | } |
| | | 408 | | |
| | 149128 | 409 | | triangle = new FixedTriangle( |
| | 149128 | 410 | | vertices[first], |
| | 149128 | 411 | | vertices[second], |
| | 149128 | 412 | | vertices[third]); |
| | 149128 | 413 | | } |
| | | 414 | | } |