| | | 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 | | { |
| | 1 | 20 | | private static readonly Signed192 One = Signed192.One; |
| | 1 | 21 | | private static readonly Signed320 Point3dDenominator = |
| | 1 | 22 | | WideArithmetic.MultiplySigned192(One, One); |
| | 1 | 23 | | private static readonly Signed576 WeightMeasureDenominator = |
| | 1 | 24 | | PowerOfOne(3); |
| | 1 | 25 | | private static readonly Signed576 ParallelAxis2dDenominator = |
| | 1 | 26 | | PowerOfOne(4); |
| | 1 | 27 | | private static readonly Signed576 ParallelAxis3dDenominator = |
| | 1 | 28 | | PowerOfOne(6); |
| | | 29 | | |
| | | 30 | | internal static ExactMassWeight CreateWeight(Fixed64 measure) |
| | | 31 | | { |
| | 75 | 32 | | EnsureNonNegative(measure, nameof(measure)); |
| | 74 | 33 | | return CreateWeightCore( |
| | 74 | 34 | | measure, |
| | 74 | 35 | | Fixed64.One, |
| | 74 | 36 | | Fixed64.One, |
| | 74 | 37 | | Fixed64.One); |
| | | 38 | | } |
| | | 39 | | |
| | | 40 | | internal static ExactMassWeight CreateWeight( |
| | | 41 | | Fixed64 first, |
| | | 42 | | Fixed64 second) |
| | | 43 | | { |
| | 1 | 44 | | EnsureNonNegative(first, nameof(first)); |
| | 1 | 45 | | EnsureNonNegative(second, nameof(second)); |
| | 1 | 46 | | return CreateWeightCore( |
| | 1 | 47 | | first, |
| | 1 | 48 | | second, |
| | 1 | 49 | | Fixed64.One, |
| | 1 | 50 | | Fixed64.One); |
| | | 51 | | } |
| | | 52 | | |
| | | 53 | | internal static ExactMassWeight CreateWeight( |
| | | 54 | | Fixed64 first, |
| | | 55 | | Fixed64 second, |
| | | 56 | | Fixed64 third) |
| | | 57 | | { |
| | 4801 | 58 | | EnsureNonNegative(first, nameof(first)); |
| | 4801 | 59 | | EnsureNonNegative(second, nameof(second)); |
| | 4801 | 60 | | EnsureNonNegative(third, nameof(third)); |
| | 4801 | 61 | | return CreateWeightCore( |
| | 4801 | 62 | | first, |
| | 4801 | 63 | | second, |
| | 4801 | 64 | | third, |
| | 4801 | 65 | | Fixed64.One); |
| | | 66 | | } |
| | | 67 | | |
| | | 68 | | internal static ExactMassWeight CreateWeight( |
| | | 69 | | Fixed64 first, |
| | | 70 | | Fixed64 second, |
| | | 71 | | Fixed64 third, |
| | | 72 | | Fixed64 fourth) |
| | | 73 | | { |
| | 2559 | 74 | | EnsureNonNegative(first, nameof(first)); |
| | 2559 | 75 | | EnsureNonNegative(second, nameof(second)); |
| | 2559 | 76 | | EnsureNonNegative(third, nameof(third)); |
| | 2559 | 77 | | EnsureNonNegative(fourth, nameof(fourth)); |
| | 2559 | 78 | | return CreateWeightCore(first, second, third, fourth); |
| | | 79 | | } |
| | | 80 | | |
| | | 81 | | internal static ExactMassWeight CreateAreaWeight( |
| | | 82 | | Signed320 signedDoubleArea) |
| | | 83 | | { |
| | 5552 | 84 | | Signed320 absoluteArea = signedDoubleArea.Sign < 0 |
| | 5552 | 85 | | ? WideArithmetic.SubtractSigned320( |
| | 5552 | 86 | | default, |
| | 5552 | 87 | | signedDoubleArea) |
| | 5552 | 88 | | : signedDoubleArea; |
| | 5552 | 89 | | Signed320 halfScale = WideArithmetic.MultiplySigned192( |
| | 5552 | 90 | | Signed192.Raw(Fixed64.Half), |
| | 5552 | 91 | | One); |
| | 5552 | 92 | | return new ExactMassWeight( |
| | 5552 | 93 | | Signed320.NarrowValue( |
| | 5552 | 94 | | WideArithmetic.MultiplySigned320( |
| | 5552 | 95 | | absoluteArea, |
| | 5552 | 96 | | halfScale))); |
| | | 97 | | } |
| | | 98 | | |
| | | 99 | | internal static ExactMassWeight CreateTriangleAreaWeight( |
| | | 100 | | Signed320 squaredDoubleArea) |
| | | 101 | | { |
| | 161950 | 102 | | Signed320 scaledRoot = |
| | 161950 | 103 | | WideArithmetic.GetFloorSquareRootScaledByFixed64( |
| | 161950 | 104 | | Signed576.ExtendValue(squaredDoubleArea)); |
| | 161950 | 105 | | Signed576 numerator = WideArithmetic.MultiplySigned320( |
| | 161950 | 106 | | scaledRoot, |
| | 161950 | 107 | | Signed320.ExtendValue( |
| | 161950 | 108 | | Signed192.Raw(Fixed64.Half))); |
| | 161950 | 109 | | return new ExactMassWeight( |
| | 161950 | 110 | | Signed320.NarrowValue(numerator)); |
| | | 111 | | } |
| | | 112 | | |
| | | 113 | | internal static bool TryGetMeasure( |
| | | 114 | | ExactMassWeight weight, |
| | | 115 | | out Fixed64 measure) => |
| | 32 | 116 | | Fixed64.TryGetSignedRawRatio( |
| | 32 | 117 | | Signed576.ExtendValue(weight.Numerator), |
| | 32 | 118 | | WeightMeasureDenominator, |
| | 32 | 119 | | out measure); |
| | | 120 | | |
| | | 121 | | internal static bool TryAdd( |
| | | 122 | | ExactMassWeight first, |
| | | 123 | | ExactMassWeight second, |
| | | 124 | | out ExactMassWeight result) |
| | | 125 | | { |
| | 3714 | 126 | | Signed576 sum = WideArithmetic.AddSigned576( |
| | 3714 | 127 | | Signed576.ExtendValue(first.Numerator), |
| | 3714 | 128 | | Signed576.ExtendValue(second.Numerator)); |
| | 3714 | 129 | | if (!Signed320.TryNarrowSigned(sum, out Signed320 numerator) |
| | 3714 | 130 | | || numerator.Sign < 0) |
| | | 131 | | { |
| | 2 | 132 | | result = default; |
| | 2 | 133 | | return false; |
| | | 134 | | } |
| | | 135 | | |
| | 3712 | 136 | | result = new ExactMassWeight(numerator); |
| | 3712 | 137 | | return true; |
| | | 138 | | } |
| | | 139 | | |
| | | 140 | | internal static bool TryGetProportionalShare( |
| | | 141 | | ExactMassWeight weight, |
| | | 142 | | Fixed64 total, |
| | | 143 | | ExactMassWeight totalWeight, |
| | | 144 | | out Fixed64 share) |
| | | 145 | | { |
| | 1995 | 146 | | if (total < Fixed64.Zero |
| | 1995 | 147 | | || totalWeight.Numerator.Sign <= 0 |
| | 1995 | 148 | | || WideArithmetic.CompareNonNegative( |
| | 1995 | 149 | | Signed576.ExtendValue(weight.Numerator), |
| | 1995 | 150 | | Signed576.ExtendValue(totalWeight.Numerator)) > 0) |
| | | 151 | | { |
| | 3 | 152 | | share = default; |
| | 3 | 153 | | return false; |
| | | 154 | | } |
| | | 155 | | |
| | 1992 | 156 | | Signed576 numerator = WideArithmetic.MultiplySigned320( |
| | 1992 | 157 | | weight.Numerator, |
| | 1992 | 158 | | Signed320.ExtendValue(Signed192.Raw(total))); |
| | 1992 | 159 | | return Fixed64.TryGetSignedRawRatio( |
| | 1992 | 160 | | numerator, |
| | 1992 | 161 | | Signed576.ExtendValue(totalWeight.Numerator), |
| | 1992 | 162 | | out share); |
| | | 163 | | } |
| | | 164 | | |
| | | 165 | | internal static ExactMassPoint3D CreatePoint(Vector3d point) => |
| | 667 | 166 | | new( |
| | 667 | 167 | | Product(Signed192.Raw(point.X), One, One), |
| | 667 | 168 | | Product(Signed192.Raw(point.Y), One, One), |
| | 667 | 169 | | Product(Signed192.Raw(point.Z), One, One)); |
| | | 170 | | |
| | | 171 | | internal static ExactMassPoint3D CreatePoint( |
| | | 172 | | Vector3d outerPoint, |
| | | 173 | | Vector3d outerScale, |
| | | 174 | | Vector3d innerOffset, |
| | | 175 | | Vector3d innerScale, |
| | | 176 | | Vector3d innerDisplacement, |
| | | 177 | | FixedQuaternion innerRotation) |
| | | 178 | | { |
| | 66863 | 179 | | WideRationalBasis3d basis = new(innerRotation); |
| | 66863 | 180 | | Signed576 denominator = Signed576.ExtendValue( |
| | 66863 | 181 | | WideArithmetic.MultiplySigned192( |
| | 66863 | 182 | | basis.Denominator, |
| | 66863 | 183 | | Signed192.One)); |
| | 66863 | 184 | | Signed320 x = WideArithmetic.GetSignedRatioWith64FractionBits( |
| | 66863 | 185 | | WideRationalBasis3d.GetComposedScaledCoordinateNumerator( |
| | 66863 | 186 | | outerPoint.X, |
| | 66863 | 187 | | outerScale.X, |
| | 66863 | 188 | | basis.Xx, |
| | 66863 | 189 | | basis.Yx, |
| | 66863 | 190 | | basis.Zx, |
| | 66863 | 191 | | basis.Denominator, |
| | 66863 | 192 | | innerOffset.X, |
| | 66863 | 193 | | innerScale.X, |
| | 66863 | 194 | | innerDisplacement), |
| | 66863 | 195 | | denominator); |
| | 66863 | 196 | | Signed320 y = WideArithmetic.GetSignedRatioWith64FractionBits( |
| | 66863 | 197 | | WideRationalBasis3d.GetComposedScaledCoordinateNumerator( |
| | 66863 | 198 | | outerPoint.Y, |
| | 66863 | 199 | | outerScale.Y, |
| | 66863 | 200 | | basis.Xy, |
| | 66863 | 201 | | basis.Yy, |
| | 66863 | 202 | | basis.Zy, |
| | 66863 | 203 | | basis.Denominator, |
| | 66863 | 204 | | innerOffset.Y, |
| | 66863 | 205 | | innerScale.Y, |
| | 66863 | 206 | | innerDisplacement), |
| | 66863 | 207 | | denominator); |
| | 66863 | 208 | | Signed320 z = WideArithmetic.GetSignedRatioWith64FractionBits( |
| | 66863 | 209 | | WideRationalBasis3d.GetComposedScaledCoordinateNumerator( |
| | 66863 | 210 | | outerPoint.Z, |
| | 66863 | 211 | | outerScale.Z, |
| | 66863 | 212 | | basis.Xz, |
| | 66863 | 213 | | basis.Yz, |
| | 66863 | 214 | | basis.Zz, |
| | 66863 | 215 | | basis.Denominator, |
| | 66863 | 216 | | innerOffset.Z, |
| | 66863 | 217 | | innerScale.Z, |
| | 66863 | 218 | | innerDisplacement), |
| | 66863 | 219 | | denominator); |
| | 66863 | 220 | | return new ExactMassPoint3D(x, y, z); |
| | | 221 | | } |
| | | 222 | | |
| | | 223 | | internal static ExactMassPoint2D CreatePoint(Vector2d point) => |
| | 713 | 224 | | new( |
| | 713 | 225 | | WideArithmetic.MultiplySigned192(Signed192.Raw(point.X), One), |
| | 713 | 226 | | WideArithmetic.MultiplySigned192(Signed192.Raw(point.Y), One)); |
| | | 227 | | |
| | | 228 | | internal static ExactMassPoint2D CreatePoint( |
| | | 229 | | Vector2d outerPoint, |
| | | 230 | | Vector2d outerScale, |
| | | 231 | | Vector2d innerOffset, |
| | | 232 | | Vector2d innerScale, |
| | | 233 | | Vector2d innerDisplacement, |
| | | 234 | | Fixed64 innerRotation) |
| | | 235 | | { |
| | 48169 | 236 | | Fixed64 cosine = FixedMath.Cos(innerRotation); |
| | 48169 | 237 | | Fixed64 sine = FixedMath.Sin(innerRotation); |
| | 48169 | 238 | | Signed320 rotatedX = WideArithmetic.SubtractSigned320( |
| | 48169 | 239 | | WideArithmetic.MultiplySigned192( |
| | 48169 | 240 | | Signed192.Raw(innerDisplacement.X), |
| | 48169 | 241 | | Signed192.Raw(cosine)), |
| | 48169 | 242 | | WideArithmetic.MultiplySigned192( |
| | 48169 | 243 | | Signed192.Raw(innerDisplacement.Y), |
| | 48169 | 244 | | Signed192.Raw(sine))); |
| | 48169 | 245 | | Signed320 rotatedY = WideArithmetic.AddSigned320( |
| | 48169 | 246 | | WideArithmetic.MultiplySigned192( |
| | 48169 | 247 | | Signed192.Raw(innerDisplacement.X), |
| | 48169 | 248 | | Signed192.Raw(sine)), |
| | 48169 | 249 | | WideArithmetic.MultiplySigned192( |
| | 48169 | 250 | | Signed192.Raw(innerDisplacement.Y), |
| | 48169 | 251 | | Signed192.Raw(cosine))); |
| | 48169 | 252 | | return new ExactMassPoint2D( |
| | 48169 | 253 | | WideArithmetic.AddSigned320( |
| | 48169 | 254 | | WideArithmetic.AddSigned320( |
| | 48169 | 255 | | WideArithmetic.MultiplySigned192( |
| | 48169 | 256 | | Signed192.Raw(outerPoint.X), |
| | 48169 | 257 | | Signed192.Raw(outerScale.X)), |
| | 48169 | 258 | | WideArithmetic.MultiplySigned192( |
| | 48169 | 259 | | Signed192.Raw(innerOffset.X), |
| | 48169 | 260 | | Signed192.Raw(innerScale.X))), |
| | 48169 | 261 | | rotatedX), |
| | 48169 | 262 | | WideArithmetic.AddSigned320( |
| | 48169 | 263 | | WideArithmetic.AddSigned320( |
| | 48169 | 264 | | WideArithmetic.MultiplySigned192( |
| | 48169 | 265 | | Signed192.Raw(outerPoint.Y), |
| | 48169 | 266 | | Signed192.Raw(outerScale.Y)), |
| | 48169 | 267 | | WideArithmetic.MultiplySigned192( |
| | 48169 | 268 | | Signed192.Raw(innerOffset.Y), |
| | 48169 | 269 | | Signed192.Raw(innerScale.Y))), |
| | 48169 | 270 | | rotatedY)); |
| | | 271 | | } |
| | | 272 | | |
| | | 273 | | internal static bool TryGetPoint( |
| | | 274 | | ExactMassPoint3D point, |
| | | 275 | | out Vector3d result) |
| | | 276 | | { |
| | 55855 | 277 | | Signed576 denominator = |
| | 55855 | 278 | | Signed576.ExtendValue(Point3dDenominator); |
| | 55855 | 279 | | bool representable = Fixed64.TryGetSignedRawRatio( |
| | 55855 | 280 | | Signed576.ExtendValue(point.XNumerator), |
| | 55855 | 281 | | denominator, |
| | 55855 | 282 | | out Fixed64 x) |
| | 55855 | 283 | | & Fixed64.TryGetSignedRawRatio( |
| | 55855 | 284 | | Signed576.ExtendValue(point.YNumerator), |
| | 55855 | 285 | | denominator, |
| | 55855 | 286 | | out Fixed64 y) |
| | 55855 | 287 | | & Fixed64.TryGetSignedRawRatio( |
| | 55855 | 288 | | Signed576.ExtendValue(point.ZNumerator), |
| | 55855 | 289 | | denominator, |
| | 55855 | 290 | | out Fixed64 z); |
| | 55855 | 291 | | result = representable |
| | 55855 | 292 | | ? new Vector3d(x, y, z) |
| | 55855 | 293 | | : default; |
| | 55855 | 294 | | return representable; |
| | | 295 | | } |
| | | 296 | | |
| | | 297 | | internal static bool TryGetPoint( |
| | | 298 | | ExactMassPoint2D point, |
| | | 299 | | out Vector2d result) |
| | | 300 | | { |
| | 30001 | 301 | | Signed576 denominator = |
| | 30001 | 302 | | Signed576.ExtendValue(Signed320.ExtendValue(One)); |
| | 30001 | 303 | | bool representable = Fixed64.TryGetSignedRawRatio( |
| | 30001 | 304 | | Signed576.ExtendValue(point.XNumerator), |
| | 30001 | 305 | | denominator, |
| | 30001 | 306 | | out Fixed64 x) |
| | 30001 | 307 | | & Fixed64.TryGetSignedRawRatio( |
| | 30001 | 308 | | Signed576.ExtendValue(point.YNumerator), |
| | 30001 | 309 | | denominator, |
| | 30001 | 310 | | out Fixed64 y); |
| | 30001 | 311 | | result = representable |
| | 30001 | 312 | | ? new Vector2d(x, y) |
| | 30001 | 313 | | : default; |
| | 30001 | 314 | | return representable; |
| | | 315 | | } |
| | | 316 | | |
| | | 317 | | internal static bool TryGetWeightedAverage( |
| | | 318 | | ReadOnlySpan<ExactMassPoint3D> points, |
| | | 319 | | ReadOnlySpan<ExactMassWeight> weights, |
| | | 320 | | out Vector3d average) |
| | | 321 | | { |
| | 671 | 322 | | ValidateWeightedInputs(points.Length, weights.Length); |
| | 670 | 323 | | Signed576 totalWeight = default; |
| | 670 | 324 | | Signed576 x = default; |
| | 670 | 325 | | Signed576 y = default; |
| | 670 | 326 | | Signed576 z = default; |
| | 3628 | 327 | | for (int i = 0; i < points.Length; i++) |
| | | 328 | | { |
| | 1144 | 329 | | Signed320 weight = weights[i].Numerator; |
| | 1144 | 330 | | totalWeight = WideArithmetic.AddSigned576( |
| | 1144 | 331 | | totalWeight, |
| | 1144 | 332 | | Signed576.ExtendValue(weight)); |
| | 1144 | 333 | | x = WideArithmetic.AddSigned576( |
| | 1144 | 334 | | x, |
| | 1144 | 335 | | WideArithmetic.MultiplySigned320( |
| | 1144 | 336 | | points[i].XNumerator, |
| | 1144 | 337 | | weight)); |
| | 1144 | 338 | | y = WideArithmetic.AddSigned576( |
| | 1144 | 339 | | y, |
| | 1144 | 340 | | WideArithmetic.MultiplySigned320( |
| | 1144 | 341 | | points[i].YNumerator, |
| | 1144 | 342 | | weight)); |
| | 1144 | 343 | | z = WideArithmetic.AddSigned576( |
| | 1144 | 344 | | z, |
| | 1144 | 345 | | WideArithmetic.MultiplySigned320( |
| | 1144 | 346 | | points[i].ZNumerator, |
| | 1144 | 347 | | weight)); |
| | | 348 | | } |
| | | 349 | | |
| | 670 | 350 | | if (totalWeight.Sign <= 0) |
| | | 351 | | { |
| | 1 | 352 | | average = default; |
| | 1 | 353 | | return false; |
| | | 354 | | } |
| | | 355 | | |
| | 669 | 356 | | Signed576 denominator = WideArithmetic.MultiplySigned576( |
| | 669 | 357 | | totalWeight, |
| | 669 | 358 | | One, |
| | 669 | 359 | | One); |
| | 669 | 360 | | bool representable = Fixed64.TryGetSignedRawRatio( |
| | 669 | 361 | | x, |
| | 669 | 362 | | denominator, |
| | 669 | 363 | | out Fixed64 resultX) |
| | 669 | 364 | | & Fixed64.TryGetSignedRawRatio( |
| | 669 | 365 | | y, |
| | 669 | 366 | | denominator, |
| | 669 | 367 | | out Fixed64 resultY) |
| | 669 | 368 | | & Fixed64.TryGetSignedRawRatio( |
| | 669 | 369 | | z, |
| | 669 | 370 | | denominator, |
| | 669 | 371 | | out Fixed64 resultZ); |
| | 669 | 372 | | average = representable |
| | 669 | 373 | | ? new Vector3d(resultX, resultY, resultZ) |
| | 669 | 374 | | : default; |
| | 669 | 375 | | return representable; |
| | | 376 | | } |
| | | 377 | | |
| | | 378 | | internal static bool TryGetWeightedAverage( |
| | | 379 | | ReadOnlySpan<ExactMassPoint2D> points, |
| | | 380 | | ReadOnlySpan<ExactMassWeight> weights, |
| | | 381 | | out Vector2d average) |
| | | 382 | | { |
| | 716 | 383 | | ValidateWeightedInputs(points.Length, weights.Length); |
| | 716 | 384 | | Signed576 totalWeight = default; |
| | 716 | 385 | | Signed576 x = default; |
| | 716 | 386 | | Signed576 y = default; |
| | 4036 | 387 | | for (int i = 0; i < points.Length; i++) |
| | | 388 | | { |
| | 1302 | 389 | | Signed320 weight = weights[i].Numerator; |
| | 1302 | 390 | | totalWeight = WideArithmetic.AddSigned576( |
| | 1302 | 391 | | totalWeight, |
| | 1302 | 392 | | Signed576.ExtendValue(weight)); |
| | 1302 | 393 | | x = WideArithmetic.AddSigned576( |
| | 1302 | 394 | | x, |
| | 1302 | 395 | | WideArithmetic.MultiplySigned320( |
| | 1302 | 396 | | points[i].XNumerator, |
| | 1302 | 397 | | weight)); |
| | 1302 | 398 | | y = WideArithmetic.AddSigned576( |
| | 1302 | 399 | | y, |
| | 1302 | 400 | | WideArithmetic.MultiplySigned320( |
| | 1302 | 401 | | points[i].YNumerator, |
| | 1302 | 402 | | weight)); |
| | | 403 | | } |
| | | 404 | | |
| | 716 | 405 | | if (totalWeight.Sign <= 0) |
| | | 406 | | { |
| | 1 | 407 | | average = default; |
| | 1 | 408 | | return false; |
| | | 409 | | } |
| | | 410 | | |
| | 715 | 411 | | Signed576 denominator = WideArithmetic.MultiplySigned576( |
| | 715 | 412 | | totalWeight, |
| | 715 | 413 | | One); |
| | 715 | 414 | | bool representable = Fixed64.TryGetSignedRawRatio( |
| | 715 | 415 | | x, |
| | 715 | 416 | | denominator, |
| | 715 | 417 | | out Fixed64 resultX) |
| | 715 | 418 | | & Fixed64.TryGetSignedRawRatio( |
| | 715 | 419 | | y, |
| | 715 | 420 | | denominator, |
| | 715 | 421 | | out Fixed64 resultY); |
| | 715 | 422 | | average = representable |
| | 715 | 423 | | ? new Vector2d(resultX, resultY) |
| | 715 | 424 | | : default; |
| | 715 | 425 | | return representable; |
| | | 426 | | } |
| | | 427 | | |
| | | 428 | | internal static bool TryAddParallelAxisTensor( |
| | | 429 | | ExactMassPoint3D point, |
| | | 430 | | Fixed3x3 centerTensor, |
| | | 431 | | Fixed64 mass, |
| | | 432 | | Vector3d referencePoint, |
| | | 433 | | out Fixed3x3 tensor) |
| | | 434 | | { |
| | 10383 | 435 | | if (mass < Fixed64.Zero) |
| | | 436 | | { |
| | 1 | 437 | | tensor = default; |
| | 1 | 438 | | return false; |
| | | 439 | | } |
| | 10382 | 440 | | if (mass == Fixed64.Zero) |
| | | 441 | | { |
| | 1 | 442 | | tensor = centerTensor; |
| | 1 | 443 | | return true; |
| | | 444 | | } |
| | | 445 | | |
| | 10381 | 446 | | Signed320 dx = WideArithmetic.SubtractSigned320( |
| | 10381 | 447 | | point.XNumerator, |
| | 10381 | 448 | | Product(Signed192.Raw(referencePoint.X), One, One)); |
| | 10381 | 449 | | Signed320 dy = WideArithmetic.SubtractSigned320( |
| | 10381 | 450 | | point.YNumerator, |
| | 10381 | 451 | | Product(Signed192.Raw(referencePoint.Y), One, One)); |
| | 10381 | 452 | | Signed320 dz = WideArithmetic.SubtractSigned320( |
| | 10381 | 453 | | point.ZNumerator, |
| | 10381 | 454 | | Product(Signed192.Raw(referencePoint.Z), One, One)); |
| | 10381 | 455 | | Signed576 dxSquared = WideArithmetic.MultiplySigned320(dx, dx); |
| | 10381 | 456 | | Signed576 dySquared = WideArithmetic.MultiplySigned320(dy, dy); |
| | 10381 | 457 | | Signed576 dzSquared = WideArithmetic.MultiplySigned320(dz, dz); |
| | 10381 | 458 | | bool representable = TryGetParallelAxisValue( |
| | 10381 | 459 | | WideArithmetic.AddSigned576(dySquared, dzSquared), |
| | 10381 | 460 | | mass, |
| | 10381 | 461 | | ParallelAxis3dDenominator, |
| | 10381 | 462 | | out Fixed64 xx) |
| | 10381 | 463 | | & TryGetParallelAxisValue( |
| | 10381 | 464 | | WideArithmetic.AddSigned576(dxSquared, dzSquared), |
| | 10381 | 465 | | mass, |
| | 10381 | 466 | | ParallelAxis3dDenominator, |
| | 10381 | 467 | | out Fixed64 yy) |
| | 10381 | 468 | | & TryGetParallelAxisValue( |
| | 10381 | 469 | | WideArithmetic.AddSigned576(dxSquared, dySquared), |
| | 10381 | 470 | | mass, |
| | 10381 | 471 | | ParallelAxis3dDenominator, |
| | 10381 | 472 | | out Fixed64 zz) |
| | 10381 | 473 | | & TryGetParallelAxisValue( |
| | 10381 | 474 | | WideArithmetic.MultiplySigned320(dx, dy), |
| | 10381 | 475 | | mass, |
| | 10381 | 476 | | ParallelAxis3dDenominator, |
| | 10381 | 477 | | out Fixed64 xy) |
| | 10381 | 478 | | & TryGetParallelAxisValue( |
| | 10381 | 479 | | WideArithmetic.MultiplySigned320(dx, dz), |
| | 10381 | 480 | | mass, |
| | 10381 | 481 | | ParallelAxis3dDenominator, |
| | 10381 | 482 | | out Fixed64 xz) |
| | 10381 | 483 | | & TryGetParallelAxisValue( |
| | 10381 | 484 | | WideArithmetic.MultiplySigned320(dy, dz), |
| | 10381 | 485 | | mass, |
| | 10381 | 486 | | ParallelAxis3dDenominator, |
| | 10381 | 487 | | out Fixed64 yz); |
| | 10381 | 488 | | if (!representable) |
| | | 489 | | { |
| | 13 | 490 | | tensor = default; |
| | 13 | 491 | | return false; |
| | | 492 | | } |
| | | 493 | | |
| | 10368 | 494 | | representable = Fixed64.TryAdd(centerTensor.M11, xx, out Fixed64 m11) |
| | 10368 | 495 | | & Fixed64.TrySubtract(centerTensor.M12, xy, out Fixed64 m12) |
| | 10368 | 496 | | & Fixed64.TrySubtract(centerTensor.M13, xz, out Fixed64 m13) |
| | 10368 | 497 | | & Fixed64.TrySubtract(centerTensor.M21, xy, out Fixed64 m21) |
| | 10368 | 498 | | & Fixed64.TryAdd(centerTensor.M22, yy, out Fixed64 m22) |
| | 10368 | 499 | | & Fixed64.TrySubtract(centerTensor.M23, yz, out Fixed64 m23) |
| | 10368 | 500 | | & Fixed64.TrySubtract(centerTensor.M31, xz, out Fixed64 m31) |
| | 10368 | 501 | | & Fixed64.TrySubtract(centerTensor.M32, yz, out Fixed64 m32) |
| | 10368 | 502 | | & Fixed64.TryAdd(centerTensor.M33, zz, out Fixed64 m33); |
| | 10368 | 503 | | tensor = representable |
| | 10368 | 504 | | ? new Fixed3x3( |
| | 10368 | 505 | | m11, m12, m13, |
| | 10368 | 506 | | m21, m22, m23, |
| | 10368 | 507 | | m31, m32, m33) |
| | 10368 | 508 | | : default; |
| | 10368 | 509 | | return representable; |
| | | 510 | | } |
| | | 511 | | |
| | | 512 | | internal static bool TryAddParallelAxisMoment( |
| | | 513 | | ExactMassPoint2D point, |
| | | 514 | | Fixed64 centerMoment, |
| | | 515 | | Fixed64 mass, |
| | | 516 | | Vector2d referencePoint, |
| | | 517 | | out Fixed64 moment) |
| | | 518 | | { |
| | 17485 | 519 | | if (mass < Fixed64.Zero) |
| | | 520 | | { |
| | 1 | 521 | | moment = default; |
| | 1 | 522 | | return false; |
| | | 523 | | } |
| | 17484 | 524 | | if (mass == Fixed64.Zero) |
| | | 525 | | { |
| | 1 | 526 | | moment = centerMoment; |
| | 1 | 527 | | return true; |
| | | 528 | | } |
| | | 529 | | |
| | 17483 | 530 | | Signed320 dx = WideArithmetic.SubtractSigned320( |
| | 17483 | 531 | | point.XNumerator, |
| | 17483 | 532 | | WideArithmetic.MultiplySigned192( |
| | 17483 | 533 | | Signed192.Raw(referencePoint.X), |
| | 17483 | 534 | | One)); |
| | 17483 | 535 | | Signed320 dy = WideArithmetic.SubtractSigned320( |
| | 17483 | 536 | | point.YNumerator, |
| | 17483 | 537 | | WideArithmetic.MultiplySigned192( |
| | 17483 | 538 | | Signed192.Raw(referencePoint.Y), |
| | 17483 | 539 | | One)); |
| | 17483 | 540 | | Signed576 squaredDistance = WideArithmetic.AddSigned576( |
| | 17483 | 541 | | WideArithmetic.MultiplySigned320(dx, dx), |
| | 17483 | 542 | | WideArithmetic.MultiplySigned320(dy, dy)); |
| | 17483 | 543 | | if (!TryGetParallelAxisValue( |
| | 17483 | 544 | | squaredDistance, |
| | 17483 | 545 | | mass, |
| | 17483 | 546 | | ParallelAxis2dDenominator, |
| | 17483 | 547 | | out Fixed64 shift)) |
| | | 548 | | { |
| | 13 | 549 | | moment = default; |
| | 13 | 550 | | return false; |
| | | 551 | | } |
| | | 552 | | |
| | 17470 | 553 | | return Fixed64.TryAdd(centerMoment, shift, out moment); |
| | | 554 | | } |
| | | 555 | | |
| | | 556 | | private static ExactMassWeight CreateWeightCore( |
| | | 557 | | Fixed64 first, |
| | | 558 | | Fixed64 second, |
| | | 559 | | Fixed64 third, |
| | | 560 | | Fixed64 fourth) |
| | | 561 | | { |
| | 7435 | 562 | | Signed576 product = WideArithmetic.MultiplySigned576( |
| | 7435 | 563 | | Signed576.ExtendValue( |
| | 7435 | 564 | | Signed320.ExtendValue(Signed192.Raw(first))), |
| | 7435 | 565 | | Signed192.Raw(second), |
| | 7435 | 566 | | Signed192.Raw(third), |
| | 7435 | 567 | | Signed192.Raw(fourth)); |
| | 7435 | 568 | | return new ExactMassWeight(Signed320.NarrowValue(product)); |
| | | 569 | | } |
| | | 570 | | |
| | | 571 | | private static bool TryGetParallelAxisValue( |
| | | 572 | | Signed576 squaredValue, |
| | | 573 | | Fixed64 mass, |
| | | 574 | | Signed576 denominator, |
| | | 575 | | out Fixed64 value) => |
| | 79769 | 576 | | Fixed64.TryGetSignedRawRatio( |
| | 79769 | 577 | | WideArithmetic.MultiplySigned576( |
| | 79769 | 578 | | squaredValue, |
| | 79769 | 579 | | Signed192.Raw(mass)), |
| | 79769 | 580 | | denominator, |
| | 79769 | 581 | | out value); |
| | | 582 | | |
| | | 583 | | private static Signed320 Product( |
| | | 584 | | Signed192 first, |
| | | 585 | | Signed192 second, |
| | | 586 | | Signed192 third) => |
| | 33144 | 587 | | Signed320.NarrowValue( |
| | 33144 | 588 | | WideArithmetic.MultiplySigned576( |
| | 33144 | 589 | | Signed576.ExtendValue( |
| | 33144 | 590 | | Signed320.ExtendValue(first)), |
| | 33144 | 591 | | second, |
| | 33144 | 592 | | third)); |
| | | 593 | | |
| | | 594 | | private static Signed576 PowerOfOne(int exponent) |
| | | 595 | | { |
| | 3 | 596 | | Signed576 result = Signed576.ExtendValue( |
| | 3 | 597 | | Signed320.ExtendValue(Signed192.Signed(1))); |
| | 32 | 598 | | for (int i = 0; i < exponent; i++) |
| | 13 | 599 | | result = WideArithmetic.MultiplySigned576(result, One); |
| | 3 | 600 | | return result; |
| | | 601 | | } |
| | | 602 | | |
| | | 603 | | private static void EnsureNonNegative( |
| | | 604 | | Fixed64 value, |
| | | 605 | | string parameterName) |
| | | 606 | | { |
| | 24716 | 607 | | if (value < Fixed64.Zero) |
| | | 608 | | { |
| | 1 | 609 | | throw new ArgumentOutOfRangeException( |
| | 1 | 610 | | parameterName, |
| | 1 | 611 | | "Mass-property weight factors cannot be negative."); |
| | | 612 | | } |
| | 24715 | 613 | | } |
| | | 614 | | |
| | | 615 | | private static void ValidateWeightedInputs( |
| | | 616 | | int pointCount, |
| | | 617 | | int weightCount) |
| | | 618 | | { |
| | 1387 | 619 | | if (pointCount != weightCount) |
| | | 620 | | { |
| | 1 | 621 | | throw new ArgumentException( |
| | 1 | 622 | | "Mass points and weights must have the same length.", |
| | 1 | 623 | | "weights"); |
| | | 624 | | } |
| | 1386 | 625 | | } |
| | | 626 | | } |