| | | 1 | | //======================================================================= |
| | | 2 | | // ExactMassProperties.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 exact bounded arithmetic for semantic mass-property values. |
| | | 17 | | /// </summary> |
| | | 18 | | internal static class ExactMassProperties |
| | | 19 | | { |
| | 2 | 20 | | private static readonly Signed192 One = Signed192.One; |
| | 2 | 21 | | private static readonly Signed320 Point3dDenominator = |
| | 2 | 22 | | WideArithmetic.MultiplySigned192(One, One); |
| | 2 | 23 | | private static readonly Signed576 WeightMeasureDenominator = |
| | 2 | 24 | | PowerOfOne(3); |
| | | 25 | | // A common factor of six preserves tetrahedron volumes without dividing |
| | | 26 | | // their determinant before compound moments have been normalized. |
| | 2 | 27 | | private static readonly Signed192 WeightFactor = Signed192.Signed(6); |
| | 2 | 28 | | private static readonly Signed576 ParallelAxis2dDenominator = |
| | 2 | 29 | | PowerOfOne(4); |
| | 2 | 30 | | private static readonly Signed576 ParallelAxis3dDenominator = |
| | 2 | 31 | | PowerOfOne(6); |
| | 2 | 32 | | private static readonly Signed192 BoxMomentDenominator = Signed192.NarrowValue( |
| | 2 | 33 | | WideArithmetic.MultiplySigned192(Fixed64.GetExactRawProduct(Fixed64.One, Fixed64.One), Signed192.Signed(3))); |
| | | 34 | | |
| | | 35 | | // Geometry retains Q96 products until mass is applied. Three positive raw |
| | | 36 | | // factors fit 192 bits; applying a full-domain mass fits 320 bits. |
| | | 37 | | internal static Signed192 InertiaGeometryTerm( |
| | | 38 | | Fixed64 coefficient, Fixed64 first, Fixed64 second) => |
| | 55019 | 39 | | Signed192.NarrowValue(WideArithmetic.MultiplySigned192( |
| | 55019 | 40 | | Signed192.Raw(coefficient), Fixed64.GetExactRawProduct(first, second))); |
| | | 41 | | |
| | | 42 | | internal static Fixed64 InertiaMoment(Fixed64 mass, Signed192 geometry) => |
| | 22274 | 43 | | MaterializeInertia(WideArithmetic.MultiplySigned192(Signed192.Raw(mass), geometry)); |
| | | 44 | | |
| | | 45 | | // A box diagonal contains both squared extents. Rounding each /3 term |
| | | 46 | | // separately loses half-even parity even when the complete moment fits. |
| | | 47 | | internal static Fixed64 InertiaBoxMoment(Fixed64 mass, Signed192 firstSquare, Signed192 secondSquare) |
| | | 48 | | { |
| | 14141 | 49 | | Signed192 squares = WideArithmetic.AddSigned192(firstSquare, secondSquare); |
| | | 50 | | // Two full-domain squared coordinates and one mass need at most 191 bits. |
| | 14141 | 51 | | Signed192 numerator = Signed192.NarrowValue(WideArithmetic.MultiplySigned192(Signed192.Raw(mass), squares)); |
| | 14141 | 52 | | return Fixed64.TryGetSignedRawRatio(numerator, BoxMomentDenominator, out Fixed64 moment) |
| | 14141 | 53 | | ? moment : Fixed64.MaxValue; |
| | | 54 | | } |
| | | 55 | | |
| | | 56 | | // Cancelled common radius factors leave linear weights. Keeping those |
| | | 57 | | // weights exact avoids quantizing a whole shape's internal component mass. |
| | | 58 | | internal static Fixed64 InertiaWeightedMoment( |
| | | 59 | | Fixed64 mass, Signed192 firstGeometry, Signed192 firstWeight, |
| | | 60 | | Signed192 secondGeometry, Signed192 secondWeight, uint divisor = 1) |
| | | 61 | | { |
| | 7954 | 62 | | Signed320 weightedGeometry = WideArithmetic.AddSigned320( |
| | 7954 | 63 | | WideArithmetic.MultiplySigned192(firstGeometry, firstWeight), |
| | 7954 | 64 | | WideArithmetic.MultiplySigned192(secondGeometry, secondWeight)); |
| | 7954 | 65 | | Signed576 numerator = WideArithmetic.MultiplySigned576( |
| | 7954 | 66 | | Signed576.ExtendValue(weightedGeometry), Signed192.Raw(mass)); |
| | 7954 | 67 | | Signed192 weight = WideArithmetic.AddSigned192(firstWeight, secondWeight); |
| | 7954 | 68 | | if (divisor != 1) |
| | 3240 | 69 | | weight = Signed192.NarrowValue(WideArithmetic.MultiplySigned192(weight, Signed192.Signed(divisor))); |
| | 7954 | 70 | | Signed320 denominator = WideArithmetic.MultiplySigned192( |
| | 7954 | 71 | | weight, Signed192.NarrowValue(WeightMeasureDenominator)); |
| | | 72 | | // Geometry is Q96 and linear weights are Q64: numerator stays below |
| | | 73 | | // 330 bits, denominator below 200 bits, and only final inertia saturates. |
| | 7954 | 74 | | return Fixed64.TryGetSignedRawRatio(numerator, denominator, out Fixed64 moment) |
| | 7954 | 75 | | ? moment : Fixed64.MaxValue; |
| | | 76 | | } |
| | | 77 | | |
| | | 78 | | private static Fixed64 MaterializeInertia(Signed320 numerator) => |
| | 22274 | 79 | | Fixed64.TryRoundNonnegativeQ128(numerator, out Fixed64 moment) |
| | 22274 | 80 | | ? moment : Fixed64.MaxValue; |
| | | 81 | | |
| | | 82 | | internal static ExactMassWeight CreateWeight(Fixed64 measure) |
| | | 83 | | { |
| | 53 | 84 | | EnsureNonNegative(measure, nameof(measure)); |
| | 52 | 85 | | return CreateWeightCore( |
| | 52 | 86 | | measure, |
| | 52 | 87 | | Fixed64.One, |
| | 52 | 88 | | Fixed64.One, |
| | 52 | 89 | | Fixed64.One); |
| | | 90 | | } |
| | | 91 | | |
| | | 92 | | internal static ExactMassWeight CreateWeight( |
| | | 93 | | Fixed64 first, |
| | | 94 | | Fixed64 second) |
| | | 95 | | { |
| | 1 | 96 | | EnsureNonNegative(first, nameof(first)); |
| | 1 | 97 | | EnsureNonNegative(second, nameof(second)); |
| | 1 | 98 | | return CreateWeightCore( |
| | 1 | 99 | | first, |
| | 1 | 100 | | second, |
| | 1 | 101 | | Fixed64.One, |
| | 1 | 102 | | Fixed64.One); |
| | | 103 | | } |
| | | 104 | | |
| | | 105 | | internal static ExactMassWeight CreateWeight( |
| | | 106 | | Fixed64 first, |
| | | 107 | | Fixed64 second, |
| | | 108 | | Fixed64 third) |
| | | 109 | | { |
| | 2809 | 110 | | EnsureNonNegative(first, nameof(first)); |
| | 2809 | 111 | | EnsureNonNegative(second, nameof(second)); |
| | 2809 | 112 | | EnsureNonNegative(third, nameof(third)); |
| | 2809 | 113 | | return CreateWeightCore( |
| | 2809 | 114 | | first, |
| | 2809 | 115 | | second, |
| | 2809 | 116 | | third, |
| | 2809 | 117 | | Fixed64.One); |
| | | 118 | | } |
| | | 119 | | |
| | | 120 | | internal static ExactMassWeight CreateWeight( |
| | | 121 | | Fixed64 first, |
| | | 122 | | Fixed64 second, |
| | | 123 | | Fixed64 third, |
| | | 124 | | Fixed64 fourth) |
| | | 125 | | { |
| | 45476 | 126 | | EnsureNonNegative(first, nameof(first)); |
| | 45476 | 127 | | EnsureNonNegative(second, nameof(second)); |
| | 45476 | 128 | | EnsureNonNegative(third, nameof(third)); |
| | 45476 | 129 | | EnsureNonNegative(fourth, nameof(fourth)); |
| | 45476 | 130 | | return CreateWeightCore(first, second, third, fourth); |
| | | 131 | | } |
| | | 132 | | |
| | | 133 | | internal static ExactMassWeight CreateAreaWeight( |
| | | 134 | | Signed320 signedDoubleArea) |
| | | 135 | | { |
| | 1453 | 136 | | Signed320 absoluteArea = signedDoubleArea.Sign < 0 |
| | 1453 | 137 | | ? WideArithmetic.SubtractSigned320( |
| | 1453 | 138 | | default, |
| | 1453 | 139 | | signedDoubleArea) |
| | 1453 | 140 | | : signedDoubleArea; |
| | 1453 | 141 | | Signed320 halfScale = WideArithmetic.MultiplySigned192( |
| | 1453 | 142 | | Signed192.Raw(Fixed64.Half), |
| | 1453 | 143 | | Signed192.NarrowValue(WideArithmetic.MultiplySigned192(One, WeightFactor))); |
| | 1453 | 144 | | return new ExactMassWeight( |
| | 1453 | 145 | | Signed320.NarrowValue( |
| | 1453 | 146 | | WideArithmetic.MultiplySigned320( |
| | 1453 | 147 | | absoluteArea, |
| | 1453 | 148 | | halfScale))); |
| | | 149 | | } |
| | | 150 | | |
| | | 151 | | internal static ExactMassWeight CreateTriangleAreaWeight( |
| | | 152 | | Signed320 squaredDoubleArea) |
| | | 153 | | { |
| | 148543 | 154 | | Signed320 scaledRoot = |
| | 148543 | 155 | | WideArithmetic.GetFloorSquareRootScaledByFixed64( |
| | 148543 | 156 | | Signed576.ExtendValue(squaredDoubleArea)); |
| | 148543 | 157 | | Signed576 numerator = WideArithmetic.MultiplySigned320( |
| | 148543 | 158 | | scaledRoot, |
| | 148543 | 159 | | Signed320.ExtendValue( |
| | 148543 | 160 | | Signed192.Raw(Fixed64.Half))); |
| | 148543 | 161 | | numerator = WideArithmetic.MultiplySigned576(numerator, WeightFactor); |
| | 148543 | 162 | | return new ExactMassWeight( |
| | 148543 | 163 | | Signed320.NarrowValue(numerator)); |
| | | 164 | | } |
| | | 165 | | |
| | | 166 | | internal static bool TryGetMeasure( |
| | | 167 | | ExactMassWeight weight, |
| | | 168 | | out Fixed64 measure) => |
| | 35 | 169 | | Fixed64.TryGetSignedRawRatio( |
| | 35 | 170 | | Signed576.ExtendValue(weight.Numerator), |
| | 35 | 171 | | WideArithmetic.MultiplySigned576(WeightMeasureDenominator, WeightFactor), |
| | 35 | 172 | | out measure); |
| | | 173 | | |
| | | 174 | | internal static bool TryAdd( |
| | | 175 | | ExactMassWeight first, |
| | | 176 | | ExactMassWeight second, |
| | | 177 | | out ExactMassWeight result) |
| | | 178 | | { |
| | 32711 | 179 | | Signed576 sum = WideArithmetic.AddSigned576( |
| | 32711 | 180 | | Signed576.ExtendValue(first.Numerator), |
| | 32711 | 181 | | Signed576.ExtendValue(second.Numerator)); |
| | 32711 | 182 | | if (!Signed320.TryNarrowSigned(sum, out Signed320 numerator) |
| | 32711 | 183 | | || numerator.Sign < 0) |
| | | 184 | | { |
| | 2 | 185 | | result = default; |
| | 2 | 186 | | return false; |
| | | 187 | | } |
| | | 188 | | |
| | 32709 | 189 | | result = new ExactMassWeight(numerator); |
| | 32709 | 190 | | return true; |
| | | 191 | | } |
| | | 192 | | |
| | | 193 | | internal static bool TryGetProportionalShare( |
| | | 194 | | ExactMassWeight weight, |
| | | 195 | | Fixed64 total, |
| | | 196 | | ExactMassWeight totalWeight, |
| | | 197 | | out Fixed64 share) |
| | | 198 | | { |
| | 7 | 199 | | if (total < Fixed64.Zero |
| | 7 | 200 | | || totalWeight.Numerator.Sign <= 0 |
| | 7 | 201 | | || WideArithmetic.CompareNonNegative( |
| | 7 | 202 | | Signed576.ExtendValue(weight.Numerator), |
| | 7 | 203 | | Signed576.ExtendValue(totalWeight.Numerator)) > 0) |
| | | 204 | | { |
| | 3 | 205 | | share = default; |
| | 3 | 206 | | return false; |
| | | 207 | | } |
| | | 208 | | |
| | 4 | 209 | | Signed576 numerator = WideArithmetic.MultiplySigned320( |
| | 4 | 210 | | weight.Numerator, |
| | 4 | 211 | | Signed320.ExtendValue(Signed192.Raw(total))); |
| | 4 | 212 | | return Fixed64.TryGetSignedRawRatio( |
| | 4 | 213 | | numerator, |
| | 4 | 214 | | Signed576.ExtendValue(totalWeight.Numerator), |
| | 4 | 215 | | out share); |
| | | 216 | | } |
| | | 217 | | |
| | | 218 | | internal static ExactMassPoint3D CreatePoint(Vector3d point) => |
| | 30711 | 219 | | new( |
| | 30711 | 220 | | Product(Signed192.Raw(point.X), One, One), |
| | 30711 | 221 | | Product(Signed192.Raw(point.Y), One, One), |
| | 30711 | 222 | | Product(Signed192.Raw(point.Z), One, One)); |
| | | 223 | | |
| | | 224 | | internal static ExactMassPoint3D CreatePoint( |
| | | 225 | | Vector3d outerPoint, |
| | | 226 | | Vector3d outerScale, |
| | | 227 | | Vector3d innerOffset, |
| | | 228 | | Vector3d innerScale, |
| | | 229 | | Vector3d innerDisplacement, |
| | | 230 | | FixedQuaternion innerRotation) |
| | | 231 | | { |
| | 141414 | 232 | | WideRationalBasis3d basis = new(innerRotation); |
| | 141414 | 233 | | Signed576 denominator = Signed576.ExtendValue( |
| | 141414 | 234 | | WideArithmetic.MultiplySigned192( |
| | 141414 | 235 | | basis.Denominator, |
| | 141414 | 236 | | Signed192.One)); |
| | 141414 | 237 | | Signed320 x = WideArithmetic.GetSignedRatioWith64FractionBits( |
| | 141414 | 238 | | WideRationalBasis3d.GetComposedScaledCoordinateNumerator( |
| | 141414 | 239 | | outerPoint.X, |
| | 141414 | 240 | | outerScale.X, |
| | 141414 | 241 | | basis.Xx, |
| | 141414 | 242 | | basis.Yx, |
| | 141414 | 243 | | basis.Zx, |
| | 141414 | 244 | | basis.Denominator, |
| | 141414 | 245 | | innerOffset.X, |
| | 141414 | 246 | | innerScale.X, |
| | 141414 | 247 | | innerDisplacement), |
| | 141414 | 248 | | denominator); |
| | 141414 | 249 | | Signed320 y = WideArithmetic.GetSignedRatioWith64FractionBits( |
| | 141414 | 250 | | WideRationalBasis3d.GetComposedScaledCoordinateNumerator( |
| | 141414 | 251 | | outerPoint.Y, |
| | 141414 | 252 | | outerScale.Y, |
| | 141414 | 253 | | basis.Xy, |
| | 141414 | 254 | | basis.Yy, |
| | 141414 | 255 | | basis.Zy, |
| | 141414 | 256 | | basis.Denominator, |
| | 141414 | 257 | | innerOffset.Y, |
| | 141414 | 258 | | innerScale.Y, |
| | 141414 | 259 | | innerDisplacement), |
| | 141414 | 260 | | denominator); |
| | 141414 | 261 | | Signed320 z = WideArithmetic.GetSignedRatioWith64FractionBits( |
| | 141414 | 262 | | WideRationalBasis3d.GetComposedScaledCoordinateNumerator( |
| | 141414 | 263 | | outerPoint.Z, |
| | 141414 | 264 | | outerScale.Z, |
| | 141414 | 265 | | basis.Xz, |
| | 141414 | 266 | | basis.Yz, |
| | 141414 | 267 | | basis.Zz, |
| | 141414 | 268 | | basis.Denominator, |
| | 141414 | 269 | | innerOffset.Z, |
| | 141414 | 270 | | innerScale.Z, |
| | 141414 | 271 | | innerDisplacement), |
| | 141414 | 272 | | denominator); |
| | 141414 | 273 | | return new ExactMassPoint3D(x, y, z); |
| | | 274 | | } |
| | | 275 | | |
| | | 276 | | internal static ExactMassPoint2D CreatePoint(Vector2d point) => |
| | 16313 | 277 | | new( |
| | 16313 | 278 | | WideArithmetic.MultiplySigned192(Signed192.Raw(point.X), One), |
| | 16313 | 279 | | WideArithmetic.MultiplySigned192(Signed192.Raw(point.Y), One)); |
| | | 280 | | |
| | | 281 | | internal static ExactMassPoint2D CreatePoint( |
| | | 282 | | Vector2d outerPoint, |
| | | 283 | | Vector2d outerScale, |
| | | 284 | | Vector2d innerOffset, |
| | | 285 | | Vector2d innerScale, |
| | | 286 | | Vector2d innerDisplacement, |
| | | 287 | | Fixed64 innerRotation) |
| | | 288 | | { |
| | 90578 | 289 | | Fixed64 cosine = FixedMath.Cos(innerRotation); |
| | 90578 | 290 | | Fixed64 sine = FixedMath.Sin(innerRotation); |
| | 90578 | 291 | | Signed320 rotatedX = WideArithmetic.SubtractSigned320( |
| | 90578 | 292 | | WideArithmetic.MultiplySigned192( |
| | 90578 | 293 | | Signed192.Raw(innerDisplacement.X), |
| | 90578 | 294 | | Signed192.Raw(cosine)), |
| | 90578 | 295 | | WideArithmetic.MultiplySigned192( |
| | 90578 | 296 | | Signed192.Raw(innerDisplacement.Y), |
| | 90578 | 297 | | Signed192.Raw(sine))); |
| | 90578 | 298 | | Signed320 rotatedY = WideArithmetic.AddSigned320( |
| | 90578 | 299 | | WideArithmetic.MultiplySigned192( |
| | 90578 | 300 | | Signed192.Raw(innerDisplacement.X), |
| | 90578 | 301 | | Signed192.Raw(sine)), |
| | 90578 | 302 | | WideArithmetic.MultiplySigned192( |
| | 90578 | 303 | | Signed192.Raw(innerDisplacement.Y), |
| | 90578 | 304 | | Signed192.Raw(cosine))); |
| | 90578 | 305 | | return new ExactMassPoint2D( |
| | 90578 | 306 | | WideArithmetic.AddSigned320( |
| | 90578 | 307 | | WideArithmetic.AddSigned320( |
| | 90578 | 308 | | WideArithmetic.MultiplySigned192( |
| | 90578 | 309 | | Signed192.Raw(outerPoint.X), |
| | 90578 | 310 | | Signed192.Raw(outerScale.X)), |
| | 90578 | 311 | | WideArithmetic.MultiplySigned192( |
| | 90578 | 312 | | Signed192.Raw(innerOffset.X), |
| | 90578 | 313 | | Signed192.Raw(innerScale.X))), |
| | 90578 | 314 | | rotatedX), |
| | 90578 | 315 | | WideArithmetic.AddSigned320( |
| | 90578 | 316 | | WideArithmetic.AddSigned320( |
| | 90578 | 317 | | WideArithmetic.MultiplySigned192( |
| | 90578 | 318 | | Signed192.Raw(outerPoint.Y), |
| | 90578 | 319 | | Signed192.Raw(outerScale.Y)), |
| | 90578 | 320 | | WideArithmetic.MultiplySigned192( |
| | 90578 | 321 | | Signed192.Raw(innerOffset.Y), |
| | 90578 | 322 | | Signed192.Raw(innerScale.Y))), |
| | 90578 | 323 | | rotatedY)); |
| | | 324 | | } |
| | | 325 | | |
| | | 326 | | internal static bool TryGetPoint( |
| | | 327 | | ExactMassPoint3D point, |
| | | 328 | | out Vector3d result) |
| | | 329 | | { |
| | 104021 | 330 | | Signed576 denominator = |
| | 104021 | 331 | | Signed576.ExtendValue(Point3dDenominator); |
| | 104021 | 332 | | bool representable = Fixed64.TryGetSignedRawRatio( |
| | 104021 | 333 | | Signed576.ExtendValue(point.XNumerator), |
| | 104021 | 334 | | denominator, |
| | 104021 | 335 | | out Fixed64 x) |
| | 104021 | 336 | | & Fixed64.TryGetSignedRawRatio( |
| | 104021 | 337 | | Signed576.ExtendValue(point.YNumerator), |
| | 104021 | 338 | | denominator, |
| | 104021 | 339 | | out Fixed64 y) |
| | 104021 | 340 | | & Fixed64.TryGetSignedRawRatio( |
| | 104021 | 341 | | Signed576.ExtendValue(point.ZNumerator), |
| | 104021 | 342 | | denominator, |
| | 104021 | 343 | | out Fixed64 z); |
| | 104021 | 344 | | result = representable |
| | 104021 | 345 | | ? new Vector3d(x, y, z) |
| | 104021 | 346 | | : default; |
| | 104021 | 347 | | return representable; |
| | | 348 | | } |
| | | 349 | | |
| | | 350 | | internal static bool TryGetPoint( |
| | | 351 | | ExactMassPoint2D point, |
| | | 352 | | out Vector2d result) |
| | | 353 | | { |
| | 59649 | 354 | | Signed576 denominator = |
| | 59649 | 355 | | Signed576.ExtendValue(Signed320.ExtendValue(One)); |
| | 59649 | 356 | | bool representable = Fixed64.TryGetSignedRawRatio( |
| | 59649 | 357 | | Signed576.ExtendValue(point.XNumerator), |
| | 59649 | 358 | | denominator, |
| | 59649 | 359 | | out Fixed64 x) |
| | 59649 | 360 | | & Fixed64.TryGetSignedRawRatio( |
| | 59649 | 361 | | Signed576.ExtendValue(point.YNumerator), |
| | 59649 | 362 | | denominator, |
| | 59649 | 363 | | out Fixed64 y); |
| | 59649 | 364 | | result = representable |
| | 59649 | 365 | | ? new Vector2d(x, y) |
| | 59649 | 366 | | : default; |
| | 59649 | 367 | | return representable; |
| | | 368 | | } |
| | | 369 | | |
| | | 370 | | internal static bool TryGetWeightedAverage( |
| | | 371 | | ReadOnlySpan<ExactMassPoint3D> points, |
| | | 372 | | ReadOnlySpan<ExactMassWeight> weights, |
| | | 373 | | out Vector3d average) |
| | | 374 | | { |
| | 20413 | 375 | | ValidateWeightedInputs(points.Length, weights.Length); |
| | 20412 | 376 | | Signed576 totalWeight = default; |
| | 20412 | 377 | | Signed576 x = default; |
| | 20412 | 378 | | Signed576 y = default; |
| | 20412 | 379 | | Signed576 z = default; |
| | 83166 | 380 | | for (int i = 0; i < points.Length; i++) |
| | | 381 | | { |
| | 21171 | 382 | | Signed320 weight = weights[i].Numerator; |
| | 21171 | 383 | | totalWeight = WideArithmetic.AddSigned576( |
| | 21171 | 384 | | totalWeight, |
| | 21171 | 385 | | Signed576.ExtendValue(weight)); |
| | 21171 | 386 | | x = WideArithmetic.AddSigned576( |
| | 21171 | 387 | | x, |
| | 21171 | 388 | | WideArithmetic.MultiplySigned320( |
| | 21171 | 389 | | points[i].XNumerator, |
| | 21171 | 390 | | weight)); |
| | 21171 | 391 | | y = WideArithmetic.AddSigned576( |
| | 21171 | 392 | | y, |
| | 21171 | 393 | | WideArithmetic.MultiplySigned320( |
| | 21171 | 394 | | points[i].YNumerator, |
| | 21171 | 395 | | weight)); |
| | 21171 | 396 | | z = WideArithmetic.AddSigned576( |
| | 21171 | 397 | | z, |
| | 21171 | 398 | | WideArithmetic.MultiplySigned320( |
| | 21171 | 399 | | points[i].ZNumerator, |
| | 21171 | 400 | | weight)); |
| | | 401 | | } |
| | | 402 | | |
| | 20412 | 403 | | if (totalWeight.Sign <= 0) |
| | | 404 | | { |
| | 1 | 405 | | average = default; |
| | 1 | 406 | | return false; |
| | | 407 | | } |
| | | 408 | | |
| | 20411 | 409 | | Signed576 denominator = WideArithmetic.MultiplySigned576( |
| | 20411 | 410 | | totalWeight, |
| | 20411 | 411 | | One, |
| | 20411 | 412 | | One); |
| | 20411 | 413 | | bool representable = Fixed64.TryGetSignedRawRatio( |
| | 20411 | 414 | | x, |
| | 20411 | 415 | | denominator, |
| | 20411 | 416 | | out Fixed64 resultX) |
| | 20411 | 417 | | & Fixed64.TryGetSignedRawRatio( |
| | 20411 | 418 | | y, |
| | 20411 | 419 | | denominator, |
| | 20411 | 420 | | out Fixed64 resultY) |
| | 20411 | 421 | | & Fixed64.TryGetSignedRawRatio( |
| | 20411 | 422 | | z, |
| | 20411 | 423 | | denominator, |
| | 20411 | 424 | | out Fixed64 resultZ); |
| | 20411 | 425 | | average = representable |
| | 20411 | 426 | | ? new Vector3d(resultX, resultY, resultZ) |
| | 20411 | 427 | | : default; |
| | 20411 | 428 | | return representable; |
| | | 429 | | } |
| | | 430 | | |
| | | 431 | | internal static bool TryGetWeightedAverage( |
| | | 432 | | ReadOnlySpan<ExactMassPoint2D> points, |
| | | 433 | | ReadOnlySpan<ExactMassWeight> weights, |
| | | 434 | | out Vector2d average) |
| | | 435 | | { |
| | 1770 | 436 | | ValidateWeightedInputs(points.Length, weights.Length); |
| | 1770 | 437 | | Signed576 totalWeight = default; |
| | 1770 | 438 | | Signed576 x = default; |
| | 1770 | 439 | | Signed576 y = default; |
| | 8248 | 440 | | for (int i = 0; i < points.Length; i++) |
| | | 441 | | { |
| | 2354 | 442 | | Signed320 weight = weights[i].Numerator; |
| | 2354 | 443 | | totalWeight = WideArithmetic.AddSigned576( |
| | 2354 | 444 | | totalWeight, |
| | 2354 | 445 | | Signed576.ExtendValue(weight)); |
| | 2354 | 446 | | x = WideArithmetic.AddSigned576( |
| | 2354 | 447 | | x, |
| | 2354 | 448 | | WideArithmetic.MultiplySigned320( |
| | 2354 | 449 | | points[i].XNumerator, |
| | 2354 | 450 | | weight)); |
| | 2354 | 451 | | y = WideArithmetic.AddSigned576( |
| | 2354 | 452 | | y, |
| | 2354 | 453 | | WideArithmetic.MultiplySigned320( |
| | 2354 | 454 | | points[i].YNumerator, |
| | 2354 | 455 | | weight)); |
| | | 456 | | } |
| | | 457 | | |
| | 1770 | 458 | | if (totalWeight.Sign <= 0) |
| | | 459 | | { |
| | 1 | 460 | | average = default; |
| | 1 | 461 | | return false; |
| | | 462 | | } |
| | | 463 | | |
| | 1769 | 464 | | Signed576 denominator = WideArithmetic.MultiplySigned576( |
| | 1769 | 465 | | totalWeight, |
| | 1769 | 466 | | One); |
| | 1769 | 467 | | bool representable = Fixed64.TryGetSignedRawRatio( |
| | 1769 | 468 | | x, |
| | 1769 | 469 | | denominator, |
| | 1769 | 470 | | out Fixed64 resultX) |
| | 1769 | 471 | | & Fixed64.TryGetSignedRawRatio( |
| | 1769 | 472 | | y, |
| | 1769 | 473 | | denominator, |
| | 1769 | 474 | | out Fixed64 resultY); |
| | 1769 | 475 | | average = representable |
| | 1769 | 476 | | ? new Vector2d(resultX, resultY) |
| | 1769 | 477 | | : default; |
| | 1769 | 478 | | return representable; |
| | | 479 | | } |
| | | 480 | | |
| | | 481 | | internal static bool TryAddParallelAxisTensor( |
| | | 482 | | ExactMassPoint3D point, |
| | | 483 | | Fixed3x3 centerTensor, |
| | | 484 | | Fixed64 mass, |
| | | 485 | | Vector3d referencePoint, |
| | | 486 | | out Fixed3x3 tensor) |
| | | 487 | | { |
| | 25884 | 488 | | if (mass < Fixed64.Zero) |
| | | 489 | | { |
| | 1 | 490 | | tensor = default; |
| | 1 | 491 | | return false; |
| | | 492 | | } |
| | 25883 | 493 | | if (mass == Fixed64.Zero) |
| | | 494 | | { |
| | 1 | 495 | | tensor = centerTensor; |
| | 1 | 496 | | return true; |
| | | 497 | | } |
| | | 498 | | |
| | 25882 | 499 | | Signed320 dx = WideArithmetic.SubtractSigned320( |
| | 25882 | 500 | | point.XNumerator, |
| | 25882 | 501 | | Product(Signed192.Raw(referencePoint.X), One, One)); |
| | 25882 | 502 | | Signed320 dy = WideArithmetic.SubtractSigned320( |
| | 25882 | 503 | | point.YNumerator, |
| | 25882 | 504 | | Product(Signed192.Raw(referencePoint.Y), One, One)); |
| | 25882 | 505 | | Signed320 dz = WideArithmetic.SubtractSigned320( |
| | 25882 | 506 | | point.ZNumerator, |
| | 25882 | 507 | | Product(Signed192.Raw(referencePoint.Z), One, One)); |
| | 25882 | 508 | | if (dx.IsZero && dy.IsZero && dz.IsZero) |
| | | 509 | | { |
| | 25853 | 510 | | tensor = centerTensor; |
| | 25853 | 511 | | return true; |
| | | 512 | | } |
| | 29 | 513 | | Signed576 dxSquared = WideArithmetic.MultiplySigned320(dx, dx); |
| | 29 | 514 | | Signed576 dySquared = WideArithmetic.MultiplySigned320(dy, dy); |
| | 29 | 515 | | Signed576 dzSquared = WideArithmetic.MultiplySigned320(dz, dz); |
| | 29 | 516 | | bool representable = TryGetParallelAxisValue( |
| | 29 | 517 | | WideArithmetic.AddSigned576(dySquared, dzSquared), |
| | 29 | 518 | | mass, |
| | 29 | 519 | | ParallelAxis3dDenominator, |
| | 29 | 520 | | out Fixed64 xx) |
| | 29 | 521 | | & TryGetParallelAxisValue( |
| | 29 | 522 | | WideArithmetic.AddSigned576(dxSquared, dzSquared), |
| | 29 | 523 | | mass, |
| | 29 | 524 | | ParallelAxis3dDenominator, |
| | 29 | 525 | | out Fixed64 yy) |
| | 29 | 526 | | & TryGetParallelAxisValue( |
| | 29 | 527 | | WideArithmetic.AddSigned576(dxSquared, dySquared), |
| | 29 | 528 | | mass, |
| | 29 | 529 | | ParallelAxis3dDenominator, |
| | 29 | 530 | | out Fixed64 zz) |
| | 29 | 531 | | & TryGetParallelAxisValue( |
| | 29 | 532 | | WideArithmetic.MultiplySigned320(dx, dy), |
| | 29 | 533 | | mass, |
| | 29 | 534 | | ParallelAxis3dDenominator, |
| | 29 | 535 | | out Fixed64 xy) |
| | 29 | 536 | | & TryGetParallelAxisValue( |
| | 29 | 537 | | WideArithmetic.MultiplySigned320(dx, dz), |
| | 29 | 538 | | mass, |
| | 29 | 539 | | ParallelAxis3dDenominator, |
| | 29 | 540 | | out Fixed64 xz) |
| | 29 | 541 | | & TryGetParallelAxisValue( |
| | 29 | 542 | | WideArithmetic.MultiplySigned320(dy, dz), |
| | 29 | 543 | | mass, |
| | 29 | 544 | | ParallelAxis3dDenominator, |
| | 29 | 545 | | out Fixed64 yz); |
| | 29 | 546 | | if (!representable) |
| | | 547 | | { |
| | 12 | 548 | | tensor = default; |
| | 12 | 549 | | return false; |
| | | 550 | | } |
| | | 551 | | |
| | 17 | 552 | | representable = Fixed64.TryAdd(centerTensor.M11, xx, out Fixed64 m11) |
| | 17 | 553 | | & Fixed64.TrySubtract(centerTensor.M12, xy, out Fixed64 m12) |
| | 17 | 554 | | & Fixed64.TrySubtract(centerTensor.M13, xz, out Fixed64 m13) |
| | 17 | 555 | | & Fixed64.TrySubtract(centerTensor.M21, xy, out Fixed64 m21) |
| | 17 | 556 | | & Fixed64.TryAdd(centerTensor.M22, yy, out Fixed64 m22) |
| | 17 | 557 | | & Fixed64.TrySubtract(centerTensor.M23, yz, out Fixed64 m23) |
| | 17 | 558 | | & Fixed64.TrySubtract(centerTensor.M31, xz, out Fixed64 m31) |
| | 17 | 559 | | & Fixed64.TrySubtract(centerTensor.M32, yz, out Fixed64 m32) |
| | 17 | 560 | | & Fixed64.TryAdd(centerTensor.M33, zz, out Fixed64 m33); |
| | 17 | 561 | | tensor = representable |
| | 17 | 562 | | ? new Fixed3x3( |
| | 17 | 563 | | m11, m12, m13, |
| | 17 | 564 | | m21, m22, m23, |
| | 17 | 565 | | m31, m32, m33) |
| | 17 | 566 | | : default; |
| | 17 | 567 | | return representable; |
| | | 568 | | } |
| | | 569 | | |
| | | 570 | | internal static bool TryAddParallelAxisMoment( |
| | | 571 | | ExactMassPoint2D point, |
| | | 572 | | Fixed64 centerMoment, |
| | | 573 | | Fixed64 mass, |
| | | 574 | | Vector2d referencePoint, |
| | | 575 | | out Fixed64 moment) |
| | | 576 | | { |
| | 16322 | 577 | | if (mass < Fixed64.Zero) |
| | | 578 | | { |
| | 1 | 579 | | moment = default; |
| | 1 | 580 | | return false; |
| | | 581 | | } |
| | 16321 | 582 | | if (mass == Fixed64.Zero) |
| | | 583 | | { |
| | 1 | 584 | | moment = centerMoment; |
| | 1 | 585 | | return true; |
| | | 586 | | } |
| | | 587 | | |
| | 16320 | 588 | | Signed320 dx = WideArithmetic.SubtractSigned320( |
| | 16320 | 589 | | point.XNumerator, |
| | 16320 | 590 | | WideArithmetic.MultiplySigned192( |
| | 16320 | 591 | | Signed192.Raw(referencePoint.X), |
| | 16320 | 592 | | One)); |
| | 16320 | 593 | | Signed320 dy = WideArithmetic.SubtractSigned320( |
| | 16320 | 594 | | point.YNumerator, |
| | 16320 | 595 | | WideArithmetic.MultiplySigned192( |
| | 16320 | 596 | | Signed192.Raw(referencePoint.Y), |
| | 16320 | 597 | | One)); |
| | 16320 | 598 | | if (dx.IsZero && dy.IsZero) |
| | | 599 | | { |
| | 16279 | 600 | | moment = centerMoment; |
| | 16279 | 601 | | return true; |
| | | 602 | | } |
| | 41 | 603 | | Signed576 squaredDistance = WideArithmetic.AddSigned576( |
| | 41 | 604 | | WideArithmetic.MultiplySigned320(dx, dx), |
| | 41 | 605 | | WideArithmetic.MultiplySigned320(dy, dy)); |
| | 41 | 606 | | if (!TryGetParallelAxisValue( |
| | 41 | 607 | | squaredDistance, |
| | 41 | 608 | | mass, |
| | 41 | 609 | | ParallelAxis2dDenominator, |
| | 41 | 610 | | out Fixed64 shift)) |
| | | 611 | | { |
| | 12 | 612 | | moment = default; |
| | 12 | 613 | | return false; |
| | | 614 | | } |
| | | 615 | | |
| | 29 | 616 | | return Fixed64.TryAdd(centerMoment, shift, out moment); |
| | | 617 | | } |
| | | 618 | | |
| | | 619 | | private static ExactMassWeight CreateWeightCore( |
| | | 620 | | Fixed64 first, |
| | | 621 | | Fixed64 second, |
| | | 622 | | Fixed64 third, |
| | | 623 | | Fixed64 fourth) |
| | | 624 | | { |
| | 48338 | 625 | | Signed576 product = WideArithmetic.MultiplySigned576( |
| | 48338 | 626 | | Signed576.ExtendValue( |
| | 48338 | 627 | | Signed320.ExtendValue(Signed192.Raw(first))), |
| | 48338 | 628 | | Signed192.Raw(second), |
| | 48338 | 629 | | Signed192.Raw(third), |
| | 48338 | 630 | | Signed192.Raw(fourth)); |
| | 48338 | 631 | | return new ExactMassWeight(Signed320.NarrowValue( |
| | 48338 | 632 | | WideArithmetic.MultiplySigned576(product, WeightFactor))); |
| | | 633 | | } |
| | | 634 | | |
| | | 635 | | private static bool TryGetParallelAxisValue( |
| | | 636 | | Signed576 squaredValue, |
| | | 637 | | Fixed64 mass, |
| | | 638 | | Signed576 denominator, |
| | | 639 | | out Fixed64 value) => |
| | 215 | 640 | | Fixed64.TryGetSignedRawRatio( |
| | 215 | 641 | | WideArithmetic.MultiplySigned576( |
| | 215 | 642 | | squaredValue, |
| | 215 | 643 | | Signed192.Raw(mass)), |
| | 215 | 644 | | denominator, |
| | 215 | 645 | | out value); |
| | | 646 | | |
| | | 647 | | private static Signed320 Product( |
| | | 648 | | Signed192 first, |
| | | 649 | | Signed192 second, |
| | | 650 | | Signed192 third) => |
| | 169779 | 651 | | Signed320.NarrowValue( |
| | 169779 | 652 | | WideArithmetic.MultiplySigned576( |
| | 169779 | 653 | | Signed576.ExtendValue( |
| | 169779 | 654 | | Signed320.ExtendValue(first)), |
| | 169779 | 655 | | second, |
| | 169779 | 656 | | third)); |
| | | 657 | | |
| | | 658 | | private static Signed576 PowerOfOne(int exponent) |
| | | 659 | | { |
| | 6 | 660 | | Signed576 result = Signed576.ExtendValue( |
| | 6 | 661 | | Signed320.ExtendValue(Signed192.Signed(1))); |
| | 64 | 662 | | for (int i = 0; i < exponent; i++) |
| | 26 | 663 | | result = WideArithmetic.MultiplySigned576(result, One); |
| | 6 | 664 | | return result; |
| | | 665 | | } |
| | | 666 | | |
| | | 667 | | private static void EnsureNonNegative( |
| | | 668 | | Fixed64 value, |
| | | 669 | | string parameterName) |
| | | 670 | | { |
| | 190386 | 671 | | if (value < Fixed64.Zero) |
| | | 672 | | { |
| | 1 | 673 | | throw new ArgumentOutOfRangeException( |
| | 1 | 674 | | parameterName, |
| | 1 | 675 | | "Mass-property weight factors cannot be negative."); |
| | | 676 | | } |
| | 190385 | 677 | | } |
| | | 678 | | |
| | | 679 | | private static void ValidateWeightedInputs( |
| | | 680 | | int pointCount, |
| | | 681 | | int weightCount) |
| | | 682 | | { |
| | 22183 | 683 | | if (pointCount != weightCount) |
| | | 684 | | { |
| | 1 | 685 | | throw new ArgumentException( |
| | 1 | 686 | | "Mass points and weights must have the same length.", |
| | 1 | 687 | | "weights"); |
| | | 688 | | } |
| | 22182 | 689 | | } |
| | | 690 | | } |