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