| | | 1 | | //======================================================================= |
| | | 2 | | // ExactLever3D.Arithmetic.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 | | using FixedMathSharp.Geometry; |
| | | 10 | | |
| | | 11 | | namespace Gravitas.CollisionHandling; |
| | | 12 | | |
| | | 13 | | /// <content> |
| | | 14 | | /// Contains exact point-velocity, quadratic-form, and transformed-cross |
| | | 15 | | /// arithmetic for rigid-body contact levers. |
| | | 16 | | /// </content> |
| | | 17 | | internal readonly partial struct ExactLever3D |
| | | 18 | | { |
| | | 19 | | internal static void GetRelativePointVelocityRatio( |
| | | 20 | | Vector3d firstLinearVelocity, |
| | | 21 | | Vector3d firstAngularVelocity, |
| | | 22 | | in ExactLever3D firstLever, |
| | | 23 | | Vector3d secondLinearVelocity, |
| | | 24 | | Vector3d secondAngularVelocity, |
| | | 25 | | in ExactLever3D secondLever, |
| | | 26 | | Vector3d projectionAxis, |
| | | 27 | | out Signed832 numerator, |
| | | 28 | | out Signed832 denominator, |
| | | 29 | | Fixed64? firstPlanarAngularVelocity = null, |
| | | 30 | | Fixed64? secondPlanarAngularVelocity = null) |
| | | 31 | | { |
| | | 32 | | // Normalized anchors bound each lever denominator below the Signed320 width. |
| | 24614 | 33 | | Signed320 firstDenominator = |
| | 24614 | 34 | | Signed320.NarrowValue(firstLever.Denominator); |
| | 24614 | 35 | | Signed320 secondDenominator = |
| | 24614 | 36 | | Signed320.NarrowValue(secondLever.Denominator); |
| | 24614 | 37 | | Signed704 firstAngular = |
| | 24614 | 38 | | GetCrossProductProjectionNumerator( |
| | 24614 | 39 | | firstLever, |
| | 24614 | 40 | | projectionAxis, |
| | 24614 | 41 | | firstAngularVelocity, firstPlanarAngularVelocity); |
| | 24614 | 42 | | Signed704 secondAngular = |
| | 24614 | 43 | | GetCrossProductProjectionNumerator( |
| | 24614 | 44 | | secondLever, |
| | 24614 | 45 | | projectionAxis, |
| | 24614 | 46 | | secondAngularVelocity, secondPlanarAngularVelocity); |
| | 24614 | 47 | | Signed192 linear = WideGeometry.GetDifferenceDotProduct3D( |
| | 24614 | 48 | | secondLinearVelocity.X, |
| | 24614 | 49 | | firstLinearVelocity.X, |
| | 24614 | 50 | | secondLinearVelocity.Y, |
| | 24614 | 51 | | firstLinearVelocity.Y, |
| | 24614 | 52 | | secondLinearVelocity.Z, |
| | 24614 | 53 | | firstLinearVelocity.Z, |
| | 24614 | 54 | | projectionAxis.X, |
| | 24614 | 55 | | Fixed64.Zero, |
| | 24614 | 56 | | projectionAxis.Y, |
| | 24614 | 57 | | Fixed64.Zero, |
| | 24614 | 58 | | projectionAxis.Z, |
| | 24614 | 59 | | Fixed64.Zero); |
| | 24614 | 60 | | Signed576 commonLeverDenominator = |
| | 24614 | 61 | | WideArithmetic.MultiplySigned320( |
| | 24614 | 62 | | firstDenominator, |
| | 24614 | 63 | | secondDenominator); |
| | 24614 | 64 | | Signed576 linearAtCommonDenominator = |
| | 24614 | 65 | | WideArithmetic.MultiplySigned576( |
| | 24614 | 66 | | commonLeverDenominator, |
| | 24614 | 67 | | linear, |
| | 24614 | 68 | | Signed192.One); |
| | 24614 | 69 | | numerator = WideArithmetic.AddSigned832( |
| | 24614 | 70 | | Signed832.ExtendValue(linearAtCommonDenominator), |
| | 24614 | 71 | | WideArithmetic.MultiplySigned704ToSigned832( |
| | 24614 | 72 | | secondAngular, |
| | 24614 | 73 | | firstDenominator)); |
| | 24614 | 74 | | numerator = WideArithmetic.SubtractSigned832( |
| | 24614 | 75 | | numerator, |
| | 24614 | 76 | | WideArithmetic.MultiplySigned704ToSigned832( |
| | 24614 | 77 | | firstAngular, |
| | 24614 | 78 | | secondDenominator)); |
| | 24614 | 79 | | Signed576 narrowDenominator = WideArithmetic.MultiplySigned576( |
| | 24614 | 80 | | commonLeverDenominator, |
| | 24614 | 81 | | Signed192.One, |
| | 24614 | 82 | | Signed192.One); |
| | 24614 | 83 | | denominator = Signed832.ExtendValue(narrowDenominator); |
| | 24614 | 84 | | } |
| | | 85 | | |
| | | 86 | | internal static void GetCrossProductQuadraticFormRatio( |
| | | 87 | | in ExactLever3D lever, |
| | | 88 | | Vector3d crossVector, |
| | | 89 | | Fixed3x3 transform, |
| | | 90 | | out Signed832 numerator, |
| | | 91 | | out Signed832 denominator) |
| | | 92 | | { |
| | 35032 | 93 | | if (transform == Fixed3x3.Zero) |
| | | 94 | | { |
| | 16422 | 95 | | numerator = default; |
| | 16422 | 96 | | denominator = Signed832.ExtendValue(Signed192.One); |
| | 16422 | 97 | | return; |
| | | 98 | | } |
| | | 99 | | |
| | 18610 | 100 | | GetCrossProductNumerators( |
| | 18610 | 101 | | lever.XNumerator, |
| | 18610 | 102 | | lever.YNumerator, |
| | 18610 | 103 | | lever.ZNumerator, |
| | 18610 | 104 | | crossVector, |
| | 18610 | 105 | | out Signed704 crossX, |
| | 18610 | 106 | | out Signed704 crossY, |
| | 18610 | 107 | | out Signed704 crossZ); |
| | | 108 | | |
| | 18610 | 109 | | numerator = GetQuadraticTerm(crossX, crossX, Signed192.Raw(transform.M11)); |
| | 18610 | 110 | | numerator = WideArithmetic.AddSigned832(numerator, |
| | 18610 | 111 | | GetQuadraticTerm(crossY, crossY, Signed192.Raw(transform.M22))); |
| | 18610 | 112 | | numerator = WideArithmetic.AddSigned832(numerator, |
| | 18610 | 113 | | GetQuadraticTerm(crossZ, crossZ, Signed192.Raw(transform.M33))); |
| | | 114 | | // Pair the off-diagonal coefficients at full width before multiplying. |
| | 18610 | 115 | | numerator = WideArithmetic.AddSigned832(numerator, GetQuadraticTerm(crossX, crossY, |
| | 18610 | 116 | | WideArithmetic.AddSigned192(Signed192.Raw(transform.M12), Signed192.Raw(transform.M21)))); |
| | 18610 | 117 | | numerator = WideArithmetic.AddSigned832(numerator, GetQuadraticTerm(crossX, crossZ, |
| | 18610 | 118 | | WideArithmetic.AddSigned192(Signed192.Raw(transform.M13), Signed192.Raw(transform.M31)))); |
| | 18610 | 119 | | numerator = WideArithmetic.AddSigned832(numerator, GetQuadraticTerm(crossY, crossZ, |
| | 18610 | 120 | | WideArithmetic.AddSigned192(Signed192.Raw(transform.M23), Signed192.Raw(transform.M32)))); |
| | | 121 | | |
| | 18610 | 122 | | denominator = WideArithmetic.MultiplySigned576ToSigned832( |
| | 18610 | 123 | | lever.Denominator, |
| | 18610 | 124 | | lever.Denominator); |
| | | 125 | | // Four Q32.32 scale factors form the single integer factor 2^128. |
| | 18610 | 126 | | denominator = WideArithmetic.MultiplySigned832(denominator, new Signed192(1UL, 0UL, 0UL)); |
| | 18610 | 127 | | } |
| | | 128 | | |
| | | 129 | | internal static bool TryGetTransformedScaledCrossProduct( |
| | | 130 | | in ExactLever3D lever, |
| | | 131 | | Vector3d crossVector, |
| | | 132 | | Fixed3x3 transform, |
| | | 133 | | Fixed64 firstMultiplier, |
| | | 134 | | Fixed64 secondMultiplier, |
| | | 135 | | Fixed64 divisor, |
| | | 136 | | out Vector3d result) |
| | | 137 | | { |
| | 12 | 138 | | GetTransformedCrossProduct( |
| | 12 | 139 | | lever, |
| | 12 | 140 | | crossVector, |
| | 12 | 141 | | transform, |
| | 12 | 142 | | out Signed832 transformedX, |
| | 12 | 143 | | out Signed832 transformedY, |
| | 12 | 144 | | out Signed832 transformedZ); |
| | 12 | 145 | | Signed832 denominator = GetTransformedCrossProductDenominator( |
| | 12 | 146 | | lever, |
| | 12 | 147 | | Signed192.Raw(divisor)); |
| | 12 | 148 | | return TryGetScaledVector( |
| | 12 | 149 | | transformedX, |
| | 12 | 150 | | transformedY, |
| | 12 | 151 | | transformedZ, |
| | 12 | 152 | | firstMultiplier, |
| | 12 | 153 | | secondMultiplier, |
| | 12 | 154 | | denominator, |
| | 12 | 155 | | out result); |
| | | 156 | | } |
| | | 157 | | |
| | | 158 | | internal static bool TryGetTransformedWeightedCrossProduct( |
| | | 159 | | in ExactLever3D lever, |
| | | 160 | | Vector3d first, |
| | | 161 | | Fixed64 firstScale, |
| | | 162 | | Vector3d second, |
| | | 163 | | Fixed64 secondScale, |
| | | 164 | | Vector3d third, |
| | | 165 | | Fixed64 thirdScale, |
| | | 166 | | Fixed3x3 transform, |
| | | 167 | | out Vector3d result) |
| | | 168 | | { |
| | 10 | 169 | | GetTransformedCrossProduct( |
| | 10 | 170 | | lever, |
| | 10 | 171 | | first, |
| | 10 | 172 | | transform, |
| | 10 | 173 | | out Signed832 firstX, |
| | 10 | 174 | | out Signed832 firstY, |
| | 10 | 175 | | out Signed832 firstZ); |
| | 10 | 176 | | GetTransformedCrossProduct( |
| | 10 | 177 | | lever, |
| | 10 | 178 | | second, |
| | 10 | 179 | | transform, |
| | 10 | 180 | | out Signed832 secondX, |
| | 10 | 181 | | out Signed832 secondY, |
| | 10 | 182 | | out Signed832 secondZ); |
| | 10 | 183 | | GetTransformedCrossProduct( |
| | 10 | 184 | | lever, |
| | 10 | 185 | | third, |
| | 10 | 186 | | transform, |
| | 10 | 187 | | out Signed832 thirdX, |
| | 10 | 188 | | out Signed832 thirdY, |
| | 10 | 189 | | out Signed832 thirdZ); |
| | 10 | 190 | | Signed832 denominator = GetTransformedCrossProductDenominator( |
| | 10 | 191 | | lever, |
| | 10 | 192 | | Signed192.One); |
| | 10 | 193 | | bool representable = TryGetWeightedComponent( |
| | 10 | 194 | | firstX, |
| | 10 | 195 | | firstScale, |
| | 10 | 196 | | secondX, |
| | 10 | 197 | | secondScale, |
| | 10 | 198 | | thirdX, |
| | 10 | 199 | | thirdScale, |
| | 10 | 200 | | denominator, |
| | 10 | 201 | | out Fixed64 resultX) |
| | 10 | 202 | | & TryGetWeightedComponent( |
| | 10 | 203 | | firstY, |
| | 10 | 204 | | firstScale, |
| | 10 | 205 | | secondY, |
| | 10 | 206 | | secondScale, |
| | 10 | 207 | | thirdY, |
| | 10 | 208 | | thirdScale, |
| | 10 | 209 | | denominator, |
| | 10 | 210 | | out Fixed64 resultY) |
| | 10 | 211 | | & TryGetWeightedComponent( |
| | 10 | 212 | | firstZ, |
| | 10 | 213 | | firstScale, |
| | 10 | 214 | | secondZ, |
| | 10 | 215 | | secondScale, |
| | 10 | 216 | | thirdZ, |
| | 10 | 217 | | thirdScale, |
| | 10 | 218 | | denominator, |
| | 10 | 219 | | out Fixed64 resultZ); |
| | 10 | 220 | | result = representable |
| | 10 | 221 | | ? new Vector3d(resultX, resultY, resultZ) |
| | 10 | 222 | | : default; |
| | 10 | 223 | | return representable; |
| | | 224 | | } |
| | | 225 | | |
| | | 226 | | internal static void GetTransformedCrossProduct( |
| | | 227 | | in ExactLever3D lever, |
| | | 228 | | Vector3d crossVector, |
| | | 229 | | Fixed3x3 transform, |
| | | 230 | | out Signed832 x, |
| | | 231 | | out Signed832 y, |
| | | 232 | | out Signed832 z) |
| | | 233 | | { |
| | 19137 | 234 | | GetCrossProductNumerators( |
| | 19137 | 235 | | lever.XNumerator, |
| | 19137 | 236 | | lever.YNumerator, |
| | 19137 | 237 | | lever.ZNumerator, |
| | 19137 | 238 | | crossVector, |
| | 19137 | 239 | | out Signed704 crossX, |
| | 19137 | 240 | | out Signed704 crossY, |
| | 19137 | 241 | | out Signed704 crossZ); |
| | 19137 | 242 | | x = TransformCrossProduct( |
| | 19137 | 243 | | crossX, |
| | 19137 | 244 | | crossY, |
| | 19137 | 245 | | crossZ, |
| | 19137 | 246 | | transform.M11, |
| | 19137 | 247 | | transform.M21, |
| | 19137 | 248 | | transform.M31); |
| | 19137 | 249 | | y = TransformCrossProduct( |
| | 19137 | 250 | | crossX, |
| | 19137 | 251 | | crossY, |
| | 19137 | 252 | | crossZ, |
| | 19137 | 253 | | transform.M12, |
| | 19137 | 254 | | transform.M22, |
| | 19137 | 255 | | transform.M32); |
| | 19137 | 256 | | z = TransformCrossProduct( |
| | 19137 | 257 | | crossX, |
| | 19137 | 258 | | crossY, |
| | 19137 | 259 | | crossZ, |
| | 19137 | 260 | | transform.M13, |
| | 19137 | 261 | | transform.M23, |
| | 19137 | 262 | | transform.M33); |
| | 19137 | 263 | | } |
| | | 264 | | |
| | | 265 | | private static bool TryGetScaledVector( |
| | | 266 | | Signed832 transformedX, |
| | | 267 | | Signed832 transformedY, |
| | | 268 | | Signed832 transformedZ, |
| | | 269 | | Fixed64 firstMultiplier, |
| | | 270 | | Fixed64 secondMultiplier, |
| | | 271 | | Signed832 denominator, |
| | | 272 | | out Vector3d result) |
| | | 273 | | { |
| | 12 | 274 | | bool representable = TryGetScaledComponent( |
| | 12 | 275 | | transformedX, |
| | 12 | 276 | | firstMultiplier, |
| | 12 | 277 | | secondMultiplier, |
| | 12 | 278 | | denominator, |
| | 12 | 279 | | out Fixed64 resultX) |
| | 12 | 280 | | & TryGetScaledComponent( |
| | 12 | 281 | | transformedY, |
| | 12 | 282 | | firstMultiplier, |
| | 12 | 283 | | secondMultiplier, |
| | 12 | 284 | | denominator, |
| | 12 | 285 | | out Fixed64 resultY) |
| | 12 | 286 | | & TryGetScaledComponent( |
| | 12 | 287 | | transformedZ, |
| | 12 | 288 | | firstMultiplier, |
| | 12 | 289 | | secondMultiplier, |
| | 12 | 290 | | denominator, |
| | 12 | 291 | | out Fixed64 resultZ); |
| | 12 | 292 | | result = representable |
| | 12 | 293 | | ? new Vector3d(resultX, resultY, resultZ) |
| | 12 | 294 | | : default; |
| | 12 | 295 | | return representable; |
| | | 296 | | } |
| | | 297 | | |
| | | 298 | | private static void GetRawCrossProduct( |
| | | 299 | | Vector3d first, |
| | | 300 | | Vector3d second, |
| | | 301 | | out Signed320 x, |
| | | 302 | | out Signed320 y, |
| | | 303 | | out Signed320 z) |
| | | 304 | | { |
| | 40310 | 305 | | x = WideArithmetic.SubtractSigned320( |
| | 40310 | 306 | | WideArithmetic.MultiplySigned192( |
| | 40310 | 307 | | Signed192.Raw(first.Y), |
| | 40310 | 308 | | Signed192.Raw(second.Z)), |
| | 40310 | 309 | | WideArithmetic.MultiplySigned192( |
| | 40310 | 310 | | Signed192.Raw(first.Z), |
| | 40310 | 311 | | Signed192.Raw(second.Y))); |
| | 40310 | 312 | | y = WideArithmetic.SubtractSigned320( |
| | 40310 | 313 | | WideArithmetic.MultiplySigned192( |
| | 40310 | 314 | | Signed192.Raw(first.Z), |
| | 40310 | 315 | | Signed192.Raw(second.X)), |
| | 40310 | 316 | | WideArithmetic.MultiplySigned192( |
| | 40310 | 317 | | Signed192.Raw(first.X), |
| | 40310 | 318 | | Signed192.Raw(second.Z))); |
| | 40310 | 319 | | z = WideArithmetic.SubtractSigned320( |
| | 40310 | 320 | | WideArithmetic.MultiplySigned192( |
| | 40310 | 321 | | Signed192.Raw(first.X), |
| | 40310 | 322 | | Signed192.Raw(second.Y)), |
| | 40310 | 323 | | WideArithmetic.MultiplySigned192( |
| | 40310 | 324 | | Signed192.Raw(first.Y), |
| | 40310 | 325 | | Signed192.Raw(second.X))); |
| | 40310 | 326 | | } |
| | | 327 | | |
| | | 328 | | private static Signed704 GetCrossProductProjectionNumerator( |
| | | 329 | | in ExactLever3D lever, |
| | | 330 | | Vector3d crossVector, |
| | | 331 | | Vector3d projectionVector, |
| | | 332 | | Fixed64? planarAngularVelocity) |
| | | 333 | | { |
| | | 334 | | Signed320 coefficientX, coefficientY, coefficientZ; |
| | 49228 | 335 | | if (planarAngularVelocity.HasValue) |
| | | 336 | | { |
| | | 337 | | // Project signed yaw before narrowing its negated spatial Y value. |
| | 8918 | 338 | | Signed192 yaw = Signed192.Raw(planarAngularVelocity.Value); |
| | 8918 | 339 | | coefficientX = WideArithmetic.MultiplySigned192(Signed192.Raw(crossVector.Z), yaw); |
| | 8918 | 340 | | coefficientY = default; |
| | 8918 | 341 | | coefficientZ = WideArithmetic.MultiplySigned192(Signed192.Raw(crossVector.X), |
| | 8918 | 342 | | WideArithmetic.SubtractSigned192(default, yaw)); |
| | | 343 | | } |
| | 40310 | 344 | | else GetRawCrossProduct(crossVector, projectionVector, |
| | 40310 | 345 | | out coefficientX, out coefficientY, out coefficientZ); |
| | 49228 | 346 | | return WideArithmetic.AddSigned704( |
| | 49228 | 347 | | WideArithmetic.AddSigned704( |
| | 49228 | 348 | | WideArithmetic.MultiplySigned576ToSigned704( |
| | 49228 | 349 | | lever.XNumerator, |
| | 49228 | 350 | | coefficientX), |
| | 49228 | 351 | | WideArithmetic.MultiplySigned576ToSigned704( |
| | 49228 | 352 | | lever.YNumerator, |
| | 49228 | 353 | | coefficientY)), |
| | 49228 | 354 | | WideArithmetic.MultiplySigned576ToSigned704( |
| | 49228 | 355 | | lever.ZNumerator, |
| | 49228 | 356 | | coefficientZ)); |
| | | 357 | | } |
| | | 358 | | |
| | | 359 | | private static void GetCrossProductNumerators( |
| | | 360 | | Signed576 x, |
| | | 361 | | Signed576 y, |
| | | 362 | | Signed576 z, |
| | | 363 | | Vector3d vector, |
| | | 364 | | out Signed704 crossX, |
| | | 365 | | out Signed704 crossY, |
| | | 366 | | out Signed704 crossZ) |
| | | 367 | | { |
| | 37747 | 368 | | Signed320 vectorX = |
| | 37747 | 369 | | Signed320.ExtendValue(Signed192.Raw(vector.X)); |
| | 37747 | 370 | | Signed320 vectorY = |
| | 37747 | 371 | | Signed320.ExtendValue(Signed192.Raw(vector.Y)); |
| | 37747 | 372 | | Signed320 vectorZ = |
| | 37747 | 373 | | Signed320.ExtendValue(Signed192.Raw(vector.Z)); |
| | 37747 | 374 | | crossX = WideArithmetic.SubtractSigned704( |
| | 37747 | 375 | | WideArithmetic.MultiplySigned576ToSigned704(y, vectorZ), |
| | 37747 | 376 | | WideArithmetic.MultiplySigned576ToSigned704(z, vectorY)); |
| | 37747 | 377 | | crossY = WideArithmetic.SubtractSigned704( |
| | 37747 | 378 | | WideArithmetic.MultiplySigned576ToSigned704(z, vectorX), |
| | 37747 | 379 | | WideArithmetic.MultiplySigned576ToSigned704(x, vectorZ)); |
| | 37747 | 380 | | crossZ = WideArithmetic.SubtractSigned704( |
| | 37747 | 381 | | WideArithmetic.MultiplySigned576ToSigned704(x, vectorY), |
| | 37747 | 382 | | WideArithmetic.MultiplySigned576ToSigned704(y, vectorX)); |
| | 37747 | 383 | | } |
| | | 384 | | |
| | | 385 | | private static Signed832 TransformCrossProduct( |
| | | 386 | | Signed704 x, |
| | | 387 | | Signed704 y, |
| | | 388 | | Signed704 z, |
| | | 389 | | Fixed64 coefficientX, |
| | | 390 | | Fixed64 coefficientY, |
| | | 391 | | Fixed64 coefficientZ) => |
| | 57411 | 392 | | WideArithmetic.AddSigned832( |
| | 57411 | 393 | | WideArithmetic.AddSigned832( |
| | 57411 | 394 | | WideArithmetic.MultiplySigned704ToSigned832( |
| | 57411 | 395 | | x, |
| | 57411 | 396 | | Signed192.Raw(coefficientX)), |
| | 57411 | 397 | | WideArithmetic.MultiplySigned704ToSigned832( |
| | 57411 | 398 | | y, |
| | 57411 | 399 | | Signed192.Raw(coefficientY))), |
| | 57411 | 400 | | WideArithmetic.MultiplySigned704ToSigned832( |
| | 57411 | 401 | | z, |
| | 57411 | 402 | | Signed192.Raw(coefficientZ))); |
| | | 403 | | |
| | | 404 | | private static Signed832 GetTransformedCrossProductDenominator( |
| | | 405 | | in ExactLever3D lever, |
| | | 406 | | Signed192 divisor) |
| | | 407 | | { |
| | 22 | 408 | | Signed320 fixedScaleSquared = WideArithmetic.MultiplySigned192( |
| | 22 | 409 | | Signed192.One, |
| | 22 | 410 | | Signed192.One); |
| | 22 | 411 | | Signed704 transformedDenominator = |
| | 22 | 412 | | WideArithmetic.MultiplySigned576ToSigned704( |
| | 22 | 413 | | lever.Denominator, |
| | 22 | 414 | | fixedScaleSquared); |
| | 22 | 415 | | Signed320 scaleAndDivisor = WideArithmetic.MultiplySigned192( |
| | 22 | 416 | | Signed192.One, |
| | 22 | 417 | | divisor); |
| | 22 | 418 | | return WideArithmetic.MultiplySigned704ToSigned832( |
| | 22 | 419 | | transformedDenominator, |
| | 22 | 420 | | scaleAndDivisor); |
| | | 421 | | } |
| | | 422 | | |
| | | 423 | | private static bool TryGetScaledComponent( |
| | | 424 | | Signed832 value, |
| | | 425 | | Fixed64 firstMultiplier, |
| | | 426 | | Fixed64 secondMultiplier, |
| | | 427 | | Signed832 denominator, |
| | | 428 | | out Fixed64 result) => |
| | 36 | 429 | | Fixed64.TryGetSignedRawRatio( |
| | 36 | 430 | | WideArithmetic.MultiplySigned832( |
| | 36 | 431 | | WideArithmetic.MultiplySigned832( |
| | 36 | 432 | | value, |
| | 36 | 433 | | Signed192.Raw(firstMultiplier)), |
| | 36 | 434 | | Signed192.Raw(secondMultiplier)), |
| | 36 | 435 | | denominator, |
| | 36 | 436 | | 0, |
| | 36 | 437 | | out result); |
| | | 438 | | |
| | | 439 | | private static bool TryGetWeightedComponent( |
| | | 440 | | Signed832 first, |
| | | 441 | | Fixed64 firstScale, |
| | | 442 | | Signed832 second, |
| | | 443 | | Fixed64 secondScale, |
| | | 444 | | Signed832 third, |
| | | 445 | | Fixed64 thirdScale, |
| | | 446 | | Signed832 denominator, |
| | | 447 | | out Fixed64 result) |
| | | 448 | | { |
| | 30 | 449 | | Signed832 numerator = WideArithmetic.AddSigned832( |
| | 30 | 450 | | WideArithmetic.AddSigned832( |
| | 30 | 451 | | WideArithmetic.MultiplySigned832( |
| | 30 | 452 | | first, |
| | 30 | 453 | | Signed192.Raw(firstScale)), |
| | 30 | 454 | | WideArithmetic.MultiplySigned832( |
| | 30 | 455 | | second, |
| | 30 | 456 | | Signed192.Raw(secondScale))), |
| | 30 | 457 | | WideArithmetic.MultiplySigned832( |
| | 30 | 458 | | third, |
| | 30 | 459 | | Signed192.Raw(thirdScale))); |
| | 30 | 460 | | return Fixed64.TryGetSignedRawRatio( |
| | 30 | 461 | | WideArithmetic.MultiplySigned832( |
| | 30 | 462 | | numerator, |
| | 30 | 463 | | Signed192.One), |
| | 30 | 464 | | denominator, |
| | 30 | 465 | | 0, |
| | 30 | 466 | | out result); |
| | | 467 | | } |
| | | 468 | | |
| | | 469 | | private static Signed832 GetQuadraticTerm( |
| | | 470 | | Signed704 left, |
| | | 471 | | Signed704 right, |
| | | 472 | | Signed192 coefficient) |
| | | 473 | | { |
| | 111660 | 474 | | if (left.IsZero | right.IsZero | coefficient.IsZero) |
| | 68817 | 475 | | return default; |
| | | 476 | | |
| | 42843 | 477 | | return WideArithmetic.MultiplySigned704ToSigned832( |
| | 42843 | 478 | | left, |
| | 42843 | 479 | | right, |
| | 42843 | 480 | | coefficient); |
| | | 481 | | } |
| | | 482 | | } |