| | | 1 | | //======================================================================= |
| | | 2 | | // WideCenteredCapsule2dRelations.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 | | namespace FixedMathSharp.Geometry; |
| | | 11 | | |
| | | 12 | | /// <summary> |
| | | 13 | | /// Owns rigid-frame full-domain relations for conceptual 2D capsules. |
| | | 14 | | /// </summary> |
| | | 15 | | internal static class WideCenteredCapsule2dRelations |
| | | 16 | | { |
| | 1 | 17 | | private static readonly Fixed64 SideAlignmentTolerance = |
| | 1 | 18 | | Fixed64.Epsilon * (Fixed64)16; |
| | 1 | 19 | | private static readonly Signed192 DoubleScaleSquared = |
| | 1 | 20 | | new(0UL, 2UL, 0UL); |
| | 1 | 21 | | private static readonly Signed192 ScaleSquared = |
| | 1 | 22 | | new(0UL, 1UL, 0UL); |
| | | 23 | | |
| | | 24 | | internal static bool TryGetMinimumTranslation( |
| | | 25 | | Vector2d center, |
| | | 26 | | Vector2d capsuleAxis, |
| | | 27 | | Fixed64 axisLength, |
| | | 28 | | Fixed64 radius, |
| | | 29 | | Vector2d convexOrigin, |
| | | 30 | | ReadOnlySpan<Vector2d> convexOriginOffsets, |
| | | 31 | | out Vector2d normal, |
| | | 32 | | out Fixed64 depth) => |
| | 7 | 33 | | TryGetMinimumTranslation( |
| | 7 | 34 | | center, |
| | 7 | 35 | | capsuleAxis, |
| | 7 | 36 | | axisLength, |
| | 7 | 37 | | radius, |
| | 7 | 38 | | convexOrigin, |
| | 7 | 39 | | Fixed64.Zero, |
| | 7 | 40 | | convexOriginOffsets, |
| | 7 | 41 | | out normal, |
| | 7 | 42 | | out depth); |
| | | 43 | | |
| | | 44 | | internal static bool TryGetMinimumTranslation( |
| | | 45 | | Vector2d center, |
| | | 46 | | Vector2d capsuleAxis, |
| | | 47 | | Fixed64 axisLength, |
| | | 48 | | Fixed64 radius, |
| | | 49 | | Vector2d convexOrigin, |
| | | 50 | | Fixed64 convexRotation, |
| | | 51 | | ReadOnlySpan<Vector2d> convexOriginOffsets, |
| | | 52 | | out Vector2d normal, |
| | | 53 | | out Fixed64 depth) => |
| | 8 | 54 | | TryGetMinimumTranslation( |
| | 8 | 55 | | center, |
| | 8 | 56 | | capsuleAxis, |
| | 8 | 57 | | axisLength, |
| | 8 | 58 | | radius, |
| | 8 | 59 | | convexOrigin, |
| | 8 | 60 | | convexRotation, |
| | 8 | 61 | | convexOriginOffsets, |
| | 8 | 62 | | out normal, |
| | 8 | 63 | | out depth, |
| | 8 | 64 | | out _, |
| | 8 | 65 | | out _); |
| | | 66 | | |
| | | 67 | | internal static bool TryGetContacts( |
| | | 68 | | Vector2d capsuleCenter, |
| | | 69 | | Fixed64 capsuleRotation, |
| | | 70 | | Vector2d localCapsuleAxis, |
| | | 71 | | Fixed64 axisLength, |
| | | 72 | | Fixed64 radius, |
| | | 73 | | Vector2d convexOrigin, |
| | | 74 | | Fixed64 convexRotation, |
| | | 75 | | ReadOnlySpan<Vector2d> convexVertexOffsets, |
| | | 76 | | Span<FixedPointAnchor2d> capsuleContacts, |
| | | 77 | | Span<FixedPointAnchor2d> convexContacts, |
| | | 78 | | out int contactCount, |
| | | 79 | | out Vector2d normal, |
| | | 80 | | out Fixed64 depth, |
| | | 81 | | out bool depthIsClamped) |
| | | 82 | | { |
| | 39 | 83 | | _ = Vector2d.TryRotate( |
| | 39 | 84 | | localCapsuleAxis, |
| | 39 | 85 | | capsuleRotation, |
| | 39 | 86 | | out Vector2d capsuleAxis); |
| | 39 | 87 | | if (!TryGetMinimumTranslation( |
| | 39 | 88 | | capsuleCenter, |
| | 39 | 89 | | capsuleAxis, |
| | 39 | 90 | | axisLength, |
| | 39 | 91 | | radius, |
| | 39 | 92 | | convexOrigin, |
| | 39 | 93 | | convexRotation, |
| | 39 | 94 | | convexVertexOffsets, |
| | 39 | 95 | | out normal, |
| | 39 | 96 | | out depth, |
| | 39 | 97 | | out depthIsClamped, |
| | 39 | 98 | | out bool axisFromEdge)) |
| | | 99 | | { |
| | 15 | 100 | | contactCount = default; |
| | 15 | 101 | | return false; |
| | | 102 | | } |
| | | 103 | | |
| | 24 | 104 | | _ = Vector2d.TryRotate( |
| | 24 | 105 | | normal, |
| | 24 | 106 | | -capsuleRotation, |
| | 24 | 107 | | out Vector2d localNormal); |
| | 24 | 108 | | localNormal = localNormal.Normalized; |
| | 24 | 109 | | GetSupportFeature( |
| | 24 | 110 | | capsuleCenter, |
| | 24 | 111 | | convexOrigin, |
| | 24 | 112 | | convexRotation, |
| | 24 | 113 | | convexVertexOffsets, |
| | 24 | 114 | | normal, |
| | 24 | 115 | | axisFromEdge, |
| | 24 | 116 | | out Vector2d featureStart, |
| | 24 | 117 | | out Vector2d featureEnd); |
| | 24 | 118 | | Vector2d featureDirection = WideNormalization.GetDirection(featureStart, featureEnd); |
| | 24 | 119 | | if (axisLength > Fixed64.Epsilon |
| | 24 | 120 | | && featureDirection != Vector2d.Zero |
| | 24 | 121 | | && Vector2d.Dot(capsuleAxis, normal).Abs() |
| | 24 | 122 | | <= SideAlignmentTolerance) |
| | | 123 | | { |
| | 7 | 124 | | FixedPointAnchor2d firstCapsuleAnchor = |
| | 7 | 125 | | GetCenteredCapsuleSideAnchor( |
| | 7 | 126 | | capsuleCenter, |
| | 7 | 127 | | capsuleRotation, |
| | 7 | 128 | | localCapsuleAxis, |
| | 7 | 129 | | -axisLength, |
| | 7 | 130 | | localNormal, |
| | 7 | 131 | | radius); |
| | 7 | 132 | | FixedPointAnchor2d secondCapsuleAnchor = |
| | 7 | 133 | | GetCenteredCapsuleSideAnchor( |
| | 7 | 134 | | capsuleCenter, |
| | 7 | 135 | | capsuleRotation, |
| | 7 | 136 | | localCapsuleAxis, |
| | 7 | 137 | | axisLength, |
| | 7 | 138 | | localNormal, |
| | 7 | 139 | | radius); |
| | 7 | 140 | | if (WideConvex2dRelations.TryProjectAnchorOntoFeature( |
| | 7 | 141 | | firstCapsuleAnchor, |
| | 7 | 142 | | convexOrigin, |
| | 7 | 143 | | convexRotation, |
| | 7 | 144 | | featureStart, |
| | 7 | 145 | | featureEnd, |
| | 7 | 146 | | requireInteriorProjection: true, |
| | 7 | 147 | | out Vector2d firstConvexOffset) |
| | 7 | 148 | | && WideConvex2dRelations.TryProjectAnchorOntoFeature( |
| | 7 | 149 | | secondCapsuleAnchor, |
| | 7 | 150 | | convexOrigin, |
| | 7 | 151 | | convexRotation, |
| | 7 | 152 | | featureStart, |
| | 7 | 153 | | featureEnd, |
| | 7 | 154 | | requireInteriorProjection: true, |
| | 7 | 155 | | out Vector2d secondConvexOffset)) |
| | | 156 | | { |
| | 4 | 157 | | capsuleContacts[0] = firstCapsuleAnchor; |
| | 4 | 158 | | capsuleContacts[1] = secondCapsuleAnchor; |
| | 4 | 159 | | convexContacts[0] = new FixedPointAnchor2d( |
| | 4 | 160 | | convexOrigin, |
| | 4 | 161 | | convexRotation, |
| | 4 | 162 | | firstConvexOffset); |
| | 4 | 163 | | convexContacts[1] = new FixedPointAnchor2d( |
| | 4 | 164 | | convexOrigin, |
| | 4 | 165 | | convexRotation, |
| | 4 | 166 | | secondConvexOffset); |
| | 4 | 167 | | contactCount = 2; |
| | 4 | 168 | | return true; |
| | | 169 | | } |
| | | 170 | | } |
| | | 171 | | |
| | 20 | 172 | | FixedPointAnchor2d capsuleContact = |
| | 20 | 173 | | WideFiniteAxisIntersection.GetCenteredCapsuleSupportAnchor( |
| | 20 | 174 | | capsuleCenter, |
| | 20 | 175 | | capsuleRotation, |
| | 20 | 176 | | localCapsuleAxis, |
| | 20 | 177 | | axisLength, |
| | 20 | 178 | | radius, |
| | 20 | 179 | | localNormal); |
| | 20 | 180 | | _ = WideConvex2dRelations.TryProjectAnchorOntoFeature( |
| | 20 | 181 | | capsuleContact, |
| | 20 | 182 | | convexOrigin, |
| | 20 | 183 | | convexRotation, |
| | 20 | 184 | | featureStart, |
| | 20 | 185 | | featureEnd, |
| | 20 | 186 | | requireInteriorProjection: false, |
| | 20 | 187 | | out Vector2d convexOffset); |
| | | 188 | | |
| | 20 | 189 | | capsuleContacts[0] = capsuleContact; |
| | 20 | 190 | | convexContacts[0] = new FixedPointAnchor2d( |
| | 20 | 191 | | convexOrigin, |
| | 20 | 192 | | convexRotation, |
| | 20 | 193 | | convexOffset); |
| | 20 | 194 | | contactCount = 1; |
| | 20 | 195 | | return true; |
| | | 196 | | } |
| | | 197 | | |
| | | 198 | | private static FixedPointAnchor2d GetCenteredCapsuleSideAnchor( |
| | | 199 | | Vector2d center, |
| | | 200 | | Fixed64 rotation, |
| | | 201 | | Vector2d localAxis, |
| | | 202 | | Fixed64 signedAxisLength, |
| | | 203 | | Vector2d localRadialDirection, |
| | | 204 | | Fixed64 radius) |
| | | 205 | | { |
| | 14 | 206 | | Vector2d axialOffset = |
| | 14 | 207 | | localAxis * (signedAxisLength / Fixed64.Two); |
| | 14 | 208 | | Vector2d radialOffset = localRadialDirection * radius; |
| | 14 | 209 | | FixedPointAnchorTerm2d exactLocalTerm = |
| | 14 | 210 | | FixedPointAnchorTerm2d.CreateCenteredAxisSupport( |
| | 14 | 211 | | localAxis, |
| | 14 | 212 | | signedAxisLength, |
| | 14 | 213 | | localRadialDirection, |
| | 14 | 214 | | radius, |
| | 14 | 215 | | axialOffset, |
| | 14 | 216 | | radialOffset); |
| | 14 | 217 | | return new FixedPointAnchor2d( |
| | 14 | 218 | | center, |
| | 14 | 219 | | rotation, |
| | 14 | 220 | | axialOffset, |
| | 14 | 221 | | radialOffset, |
| | 14 | 222 | | exactLocalTerm); |
| | | 223 | | } |
| | | 224 | | |
| | | 225 | | private static bool TryGetMinimumTranslation( |
| | | 226 | | Vector2d center, |
| | | 227 | | Vector2d capsuleAxis, |
| | | 228 | | Fixed64 axisLength, |
| | | 229 | | Fixed64 radius, |
| | | 230 | | Vector2d convexOrigin, |
| | | 231 | | Fixed64 convexRotation, |
| | | 232 | | ReadOnlySpan<Vector2d> convexOriginOffsets, |
| | | 233 | | out Vector2d normal, |
| | | 234 | | out Fixed64 depth, |
| | | 235 | | out bool depthIsClamped, |
| | | 236 | | out bool bestAxisFromEdge) |
| | | 237 | | { |
| | 47 | 238 | | bool found = false; |
| | 47 | 239 | | Signed576 bestDepth = default; |
| | 47 | 240 | | Vector2d bestNormal = default; |
| | 47 | 241 | | bestAxisFromEdge = false; |
| | | 242 | | |
| | 378 | 243 | | for (int i = 0; i < convexOriginOffsets.Length; i++) |
| | | 244 | | { |
| | 155 | 245 | | Vector2d edgeDirection = WideConvex2dRelations.GetTransformedEdgeDirection( |
| | 155 | 246 | | convexRotation, |
| | 155 | 247 | | convexOriginOffsets[i], |
| | 155 | 248 | | convexOriginOffsets[(i + 1) % convexOriginOffsets.Length]); |
| | 155 | 249 | | if (edgeDirection == Vector2d.Zero) |
| | | 250 | | continue; |
| | | 251 | | |
| | 150 | 252 | | Vector2d edgeNormal = new(-edgeDirection.Y, edgeDirection.X); |
| | 150 | 253 | | if (!TryKeepAxis( |
| | 150 | 254 | | center, |
| | 150 | 255 | | capsuleAxis, |
| | 150 | 256 | | axisLength, |
| | 150 | 257 | | radius, |
| | 150 | 258 | | convexOrigin, |
| | 150 | 259 | | convexRotation, |
| | 150 | 260 | | convexOriginOffsets, |
| | 150 | 261 | | edgeNormal, |
| | 150 | 262 | | true, |
| | 150 | 263 | | ref found, |
| | 150 | 264 | | ref bestDepth, |
| | 150 | 265 | | ref bestNormal, |
| | 150 | 266 | | ref bestAxisFromEdge)) |
| | | 267 | | { |
| | 13 | 268 | | normal = default; |
| | 13 | 269 | | depth = default; |
| | 13 | 270 | | depthIsClamped = default; |
| | 13 | 271 | | return false; |
| | | 272 | | } |
| | | 273 | | } |
| | | 274 | | |
| | 34 | 275 | | Vector2d closestAxis = GetClosestVertexAxis( |
| | 34 | 276 | | center, |
| | 34 | 277 | | capsuleAxis, |
| | 34 | 278 | | axisLength, |
| | 34 | 279 | | convexOrigin, |
| | 34 | 280 | | convexRotation, |
| | 34 | 281 | | convexOriginOffsets); |
| | 34 | 282 | | if (closestAxis != Vector2d.Zero |
| | 34 | 283 | | && !TryKeepAxis( |
| | 34 | 284 | | center, |
| | 34 | 285 | | capsuleAxis, |
| | 34 | 286 | | axisLength, |
| | 34 | 287 | | radius, |
| | 34 | 288 | | convexOrigin, |
| | 34 | 289 | | convexRotation, |
| | 34 | 290 | | convexOriginOffsets, |
| | 34 | 291 | | closestAxis, |
| | 34 | 292 | | false, |
| | 34 | 293 | | ref found, |
| | 34 | 294 | | ref bestDepth, |
| | 34 | 295 | | ref bestNormal, |
| | 34 | 296 | | ref bestAxisFromEdge)) |
| | | 297 | | { |
| | 2 | 298 | | normal = default; |
| | 2 | 299 | | depth = default; |
| | 2 | 300 | | depthIsClamped = default; |
| | 2 | 301 | | return false; |
| | | 302 | | } |
| | | 303 | | |
| | 32 | 304 | | if (!found) |
| | | 305 | | { |
| | 1 | 306 | | normal = default; |
| | 1 | 307 | | depth = default; |
| | 1 | 308 | | depthIsClamped = default; |
| | 1 | 309 | | return false; |
| | | 310 | | } |
| | | 311 | | |
| | 31 | 312 | | normal = bestNormal; |
| | 31 | 313 | | if (!Fixed64.TryGetSignedRawRatio( |
| | 31 | 314 | | bestDepth, |
| | 31 | 315 | | Signed576.ExtendValue( |
| | 31 | 316 | | Signed320.ExtendValue( |
| | 31 | 317 | | DoubleScaleSquared)), |
| | 31 | 318 | | out depth)) |
| | | 319 | | { |
| | 2 | 320 | | depth = Fixed64.MaxValue; |
| | 2 | 321 | | depthIsClamped = true; |
| | 2 | 322 | | return true; |
| | | 323 | | } |
| | | 324 | | |
| | 29 | 325 | | depthIsClamped = false; |
| | 29 | 326 | | return true; |
| | | 327 | | } |
| | | 328 | | |
| | | 329 | | private static Vector2d GetClosestVertexAxis( |
| | | 330 | | Vector2d center, |
| | | 331 | | Vector2d capsuleAxis, |
| | | 332 | | Fixed64 axisLength, |
| | | 333 | | Vector2d convexOrigin, |
| | | 334 | | Fixed64 convexRotation, |
| | | 335 | | ReadOnlySpan<Vector2d> convexOriginOffsets) |
| | | 336 | | { |
| | 34 | 337 | | bool found = false; |
| | 34 | 338 | | Vector2d bestAxis = default; |
| | 34 | 339 | | Signed576 bestSquaredDistance = default; |
| | 342 | 340 | | for (int i = 0; i < convexOriginOffsets.Length; i++) |
| | | 341 | | { |
| | 137 | 342 | | Vector2d candidate = GetDirectionFromCenteredAxis( |
| | 137 | 343 | | convexOrigin, |
| | 137 | 344 | | convexRotation, |
| | 137 | 345 | | convexOriginOffsets[i], |
| | 137 | 346 | | center, |
| | 137 | 347 | | capsuleAxis, |
| | 137 | 348 | | axisLength, |
| | 137 | 349 | | out Signed576 candidateSquaredDistance); |
| | 137 | 350 | | if (candidate == Vector2d.Zero) |
| | | 351 | | continue; |
| | 134 | 352 | | if (found |
| | 134 | 353 | | && WideArithmetic.SubtractSigned576( |
| | 134 | 354 | | candidateSquaredDistance, |
| | 134 | 355 | | bestSquaredDistance).Sign >= 0) |
| | | 356 | | { |
| | | 357 | | continue; |
| | | 358 | | } |
| | | 359 | | |
| | 44 | 360 | | found = true; |
| | 44 | 361 | | bestAxis = candidate; |
| | 44 | 362 | | bestSquaredDistance = candidateSquaredDistance; |
| | | 363 | | } |
| | | 364 | | |
| | 34 | 365 | | return bestAxis; |
| | | 366 | | } |
| | | 367 | | |
| | | 368 | | private static Vector2d GetDirectionFromCenteredAxis( |
| | | 369 | | Vector2d pointOrigin, |
| | | 370 | | Fixed64 pointRotation, |
| | | 371 | | Vector2d pointOriginOffset, |
| | | 372 | | Vector2d axisCenter, |
| | | 373 | | Vector2d axisDirection, |
| | | 374 | | Fixed64 axisLength, |
| | | 375 | | out Signed576 squaredDistance) |
| | | 376 | | { |
| | 137 | 377 | | WideConvex2dRelations.GetRelativePointNumerators( |
| | 137 | 378 | | pointOrigin, |
| | 137 | 379 | | pointRotation, |
| | 137 | 380 | | pointOriginOffset, |
| | 137 | 381 | | axisCenter, |
| | 137 | 382 | | out Signed192 relativeX, |
| | 137 | 383 | | out Signed192 relativeY); |
| | 137 | 384 | | Signed320 projection = WideArithmetic.AddSigned320( |
| | 137 | 385 | | WideArithmetic.MultiplySigned192( |
| | 137 | 386 | | relativeX, |
| | 137 | 387 | | Signed192.Signed(axisDirection.X.m_rawValue)), |
| | 137 | 388 | | WideArithmetic.MultiplySigned192( |
| | 137 | 389 | | relativeY, |
| | 137 | 390 | | Signed192.Signed(axisDirection.Y.m_rawValue))); |
| | 137 | 391 | | Signed320 doubleProjection = |
| | 137 | 392 | | WideArithmetic.AddSigned320(projection, projection); |
| | 137 | 393 | | Signed320 axialThreshold = WideArithmetic.MultiplySigned192( |
| | 137 | 394 | | Signed192.Signed(axisLength.m_rawValue), |
| | 137 | 395 | | ScaleSquared); |
| | | 396 | | |
| | | 397 | | Signed320 directionX; |
| | | 398 | | Signed320 directionY; |
| | 137 | 399 | | if (WideArithmetic.SubtractSigned320( |
| | 137 | 400 | | doubleProjection, |
| | 137 | 401 | | axialThreshold).Sign > 0) |
| | | 402 | | { |
| | 58 | 403 | | Signed192 twiceRelativeX = |
| | 58 | 404 | | WideArithmetic.AddSigned192(relativeX, relativeX); |
| | 58 | 405 | | Signed192 twiceRelativeY = |
| | 58 | 406 | | WideArithmetic.AddSigned192(relativeY, relativeY); |
| | 58 | 407 | | Signed192 endpointX = NarrowProven( |
| | 58 | 408 | | WideArithmetic.MultiplySigned192( |
| | 58 | 409 | | Signed192.Signed(axisDirection.X.m_rawValue), |
| | 58 | 410 | | Signed192.Signed(axisLength.m_rawValue))); |
| | 58 | 411 | | Signed192 endpointY = NarrowProven( |
| | 58 | 412 | | WideArithmetic.MultiplySigned192( |
| | 58 | 413 | | Signed192.Signed(axisDirection.Y.m_rawValue), |
| | 58 | 414 | | Signed192.Signed(axisLength.m_rawValue))); |
| | 58 | 415 | | directionX = WideArithmetic.MultiplySigned192( |
| | 58 | 416 | | WideArithmetic.SubtractSigned192( |
| | 58 | 417 | | twiceRelativeX, |
| | 58 | 418 | | endpointX), |
| | 58 | 419 | | ScaleSquared); |
| | 58 | 420 | | directionY = WideArithmetic.MultiplySigned192( |
| | 58 | 421 | | WideArithmetic.SubtractSigned192( |
| | 58 | 422 | | twiceRelativeY, |
| | 58 | 423 | | endpointY), |
| | 58 | 424 | | ScaleSquared); |
| | | 425 | | } |
| | 79 | 426 | | else if (WideArithmetic.AddSigned320( |
| | 79 | 427 | | doubleProjection, |
| | 79 | 428 | | axialThreshold).Sign < 0) |
| | | 429 | | { |
| | 42 | 430 | | Signed192 twiceRelativeX = |
| | 42 | 431 | | WideArithmetic.AddSigned192(relativeX, relativeX); |
| | 42 | 432 | | Signed192 twiceRelativeY = |
| | 42 | 433 | | WideArithmetic.AddSigned192(relativeY, relativeY); |
| | 42 | 434 | | Signed192 endpointX = NarrowProven( |
| | 42 | 435 | | WideArithmetic.MultiplySigned192( |
| | 42 | 436 | | Signed192.Signed(axisDirection.X.m_rawValue), |
| | 42 | 437 | | Signed192.Signed(axisLength.m_rawValue))); |
| | 42 | 438 | | Signed192 endpointY = NarrowProven( |
| | 42 | 439 | | WideArithmetic.MultiplySigned192( |
| | 42 | 440 | | Signed192.Signed(axisDirection.Y.m_rawValue), |
| | 42 | 441 | | Signed192.Signed(axisLength.m_rawValue))); |
| | 42 | 442 | | directionX = WideArithmetic.MultiplySigned192( |
| | 42 | 443 | | WideArithmetic.AddSigned192( |
| | 42 | 444 | | twiceRelativeX, |
| | 42 | 445 | | endpointX), |
| | 42 | 446 | | ScaleSquared); |
| | 42 | 447 | | directionY = WideArithmetic.MultiplySigned192( |
| | 42 | 448 | | WideArithmetic.AddSigned192( |
| | 42 | 449 | | twiceRelativeY, |
| | 42 | 450 | | endpointY), |
| | 42 | 451 | | ScaleSquared); |
| | | 452 | | } |
| | | 453 | | else |
| | | 454 | | { |
| | 37 | 455 | | Signed192 narrowProjection = NarrowProven(projection); |
| | 37 | 456 | | Signed320 residualX = WideArithmetic.SubtractSigned320( |
| | 37 | 457 | | WideArithmetic.MultiplySigned192( |
| | 37 | 458 | | relativeX, |
| | 37 | 459 | | ScaleSquared), |
| | 37 | 460 | | WideArithmetic.MultiplySigned192( |
| | 37 | 461 | | Signed192.Signed( |
| | 37 | 462 | | axisDirection.X.m_rawValue), |
| | 37 | 463 | | narrowProjection)); |
| | 37 | 464 | | Signed320 residualY = WideArithmetic.SubtractSigned320( |
| | 37 | 465 | | WideArithmetic.MultiplySigned192( |
| | 37 | 466 | | relativeY, |
| | 37 | 467 | | ScaleSquared), |
| | 37 | 468 | | WideArithmetic.MultiplySigned192( |
| | 37 | 469 | | Signed192.Signed( |
| | 37 | 470 | | axisDirection.Y.m_rawValue), |
| | 37 | 471 | | narrowProjection)); |
| | 37 | 472 | | directionX = WideArithmetic.AddSigned320( |
| | 37 | 473 | | residualX, |
| | 37 | 474 | | residualX); |
| | 37 | 475 | | directionY = WideArithmetic.AddSigned320( |
| | 37 | 476 | | residualY, |
| | 37 | 477 | | residualY); |
| | | 478 | | } |
| | | 479 | | |
| | 137 | 480 | | squaredDistance = WideArithmetic.AddSigned576( |
| | 137 | 481 | | WideArithmetic.MultiplySigned320(directionX, directionX), |
| | 137 | 482 | | WideArithmetic.MultiplySigned320(directionY, directionY)); |
| | 137 | 483 | | return WideNormalization.GetNormalized(directionX, directionY); |
| | | 484 | | } |
| | | 485 | | |
| | | 486 | | private static Signed192 NarrowProven(Signed320 value) |
| | | 487 | | { |
| | 1077 | 488 | | _ = Signed192.TryNarrowSigned(value, out Signed192 result); |
| | 1077 | 489 | | return result; |
| | | 490 | | } |
| | | 491 | | |
| | | 492 | | private static void GetSupportFeature( |
| | | 493 | | Vector2d center, |
| | | 494 | | Vector2d convexOrigin, |
| | | 495 | | Fixed64 convexRotation, |
| | | 496 | | ReadOnlySpan<Vector2d> convexOriginOffsets, |
| | | 497 | | Vector2d normal, |
| | | 498 | | bool axisFromEdge, |
| | | 499 | | out Vector2d featureStart, |
| | | 500 | | out Vector2d featureEnd) |
| | | 501 | | { |
| | 24 | 502 | | int minimumIndex = 0; |
| | 24 | 503 | | Signed192 minimum = GetRelativeProjection( |
| | 24 | 504 | | center, |
| | 24 | 505 | | convexOrigin, |
| | 24 | 506 | | convexRotation, |
| | 24 | 507 | | convexOriginOffsets[0], |
| | 24 | 508 | | normal); |
| | 196 | 509 | | for (int i = 1; i < convexOriginOffsets.Length; i++) |
| | | 510 | | { |
| | 74 | 511 | | Signed192 projection = GetRelativeProjection( |
| | 74 | 512 | | center, |
| | 74 | 513 | | convexOrigin, |
| | 74 | 514 | | convexRotation, |
| | 74 | 515 | | convexOriginOffsets[i], |
| | 74 | 516 | | normal); |
| | 74 | 517 | | if (WideArithmetic.SubtractSigned192(projection, minimum).Sign < 0) |
| | | 518 | | { |
| | 9 | 519 | | minimum = projection; |
| | 9 | 520 | | minimumIndex = i; |
| | | 521 | | } |
| | | 522 | | } |
| | | 523 | | |
| | 24 | 524 | | featureStart = convexOriginOffsets[minimumIndex]; |
| | 24 | 525 | | featureEnd = featureStart; |
| | 24 | 526 | | if (!axisFromEdge) |
| | 1 | 527 | | return; |
| | | 528 | | |
| | 23 | 529 | | int previousIndex = minimumIndex; |
| | | 530 | | do |
| | | 531 | | { |
| | 24 | 532 | | previousIndex = |
| | 24 | 533 | | (previousIndex + convexOriginOffsets.Length - 1) |
| | 24 | 534 | | % convexOriginOffsets.Length; |
| | | 535 | | } |
| | 24 | 536 | | while (convexOriginOffsets[previousIndex] == featureStart); |
| | | 537 | | |
| | 23 | 538 | | int nextIndex = minimumIndex; |
| | | 539 | | do |
| | | 540 | | { |
| | 24 | 541 | | nextIndex = (nextIndex + 1) % convexOriginOffsets.Length; |
| | | 542 | | } |
| | 24 | 543 | | while (convexOriginOffsets[nextIndex] == featureStart); |
| | | 544 | | |
| | 23 | 545 | | Vector2d previous = convexOriginOffsets[previousIndex]; |
| | 23 | 546 | | Vector2d next = convexOriginOffsets[nextIndex]; |
| | 23 | 547 | | Vector2d previousDirection = WideConvex2dRelations.GetTransformedEdgeDirection( |
| | 23 | 548 | | convexRotation, |
| | 23 | 549 | | previous, |
| | 23 | 550 | | featureStart); |
| | 23 | 551 | | Vector2d nextDirection = WideConvex2dRelations.GetTransformedEdgeDirection( |
| | 23 | 552 | | convexRotation, |
| | 23 | 553 | | featureStart, |
| | 23 | 554 | | next); |
| | | 555 | | |
| | 23 | 556 | | Fixed64 previousAlignment = Vector2d.Dot(previousDirection, normal).Abs(); |
| | 23 | 557 | | Fixed64 nextAlignment = Vector2d.Dot(nextDirection, normal).Abs(); |
| | 23 | 558 | | if (previousAlignment <= nextAlignment) |
| | 8 | 559 | | featureStart = previous; |
| | | 560 | | else |
| | 15 | 561 | | featureEnd = next; |
| | 15 | 562 | | } |
| | | 563 | | |
| | | 564 | | private static bool TryKeepAxis( |
| | | 565 | | Vector2d center, |
| | | 566 | | Vector2d capsuleAxis, |
| | | 567 | | Fixed64 axisLength, |
| | | 568 | | Fixed64 radius, |
| | | 569 | | Vector2d convexOrigin, |
| | | 570 | | Fixed64 convexRotation, |
| | | 571 | | ReadOnlySpan<Vector2d> convexOriginOffsets, |
| | | 572 | | Vector2d axis, |
| | | 573 | | bool axisFromEdge, |
| | | 574 | | ref bool found, |
| | | 575 | | ref Signed576 bestDepth, |
| | | 576 | | ref Vector2d bestNormal, |
| | | 577 | | ref bool bestAxisFromEdge) |
| | | 578 | | { |
| | 183 | 579 | | GetRelativeProjectionRange( |
| | 183 | 580 | | center, |
| | 183 | 581 | | convexOrigin, |
| | 183 | 582 | | convexRotation, |
| | 183 | 583 | | convexOriginOffsets, |
| | 183 | 584 | | axis, |
| | 183 | 585 | | out Signed192 minimum, |
| | 183 | 586 | | out Signed192 maximum); |
| | 183 | 587 | | Signed192 axialAlignment = WideGeometry.GetDifferenceDotProduct2D( |
| | 183 | 588 | | capsuleAxis.X, |
| | 183 | 589 | | Fixed64.Zero, |
| | 183 | 590 | | capsuleAxis.Y, |
| | 183 | 591 | | Fixed64.Zero, |
| | 183 | 592 | | axis.X, |
| | 183 | 593 | | Fixed64.Zero, |
| | 183 | 594 | | axis.Y, |
| | 183 | 595 | | Fixed64.Zero); |
| | 183 | 596 | | if (axialAlignment.Sign < 0) |
| | 59 | 597 | | axialAlignment = WideArithmetic.SubtractSigned192(default, axialAlignment); |
| | | 598 | | |
| | 183 | 599 | | Signed320 extentNumerator = WideArithmetic.AddSigned320( |
| | 183 | 600 | | WideArithmetic.MultiplySigned192( |
| | 183 | 601 | | axialAlignment, |
| | 183 | 602 | | Signed192.Signed(axisLength.m_rawValue)), |
| | 183 | 603 | | WideArithmetic.MultiplySigned192( |
| | 183 | 604 | | Signed192.Signed(radius.m_rawValue), |
| | 183 | 605 | | DoubleScaleSquared)); |
| | 183 | 606 | | Signed320 minimumProjection = WideArithmetic.AddSigned320( |
| | 183 | 607 | | Signed320.ExtendValue(minimum), |
| | 183 | 608 | | Signed320.ExtendValue(minimum)); |
| | 183 | 609 | | Signed320 maximumProjection = WideArithmetic.AddSigned320( |
| | 183 | 610 | | Signed320.ExtendValue(maximum), |
| | 183 | 611 | | Signed320.ExtendValue(maximum)); |
| | 183 | 612 | | Signed576 positive = Signed576.ExtendValue( |
| | 183 | 613 | | WideArithmetic.SubtractSigned320( |
| | 183 | 614 | | extentNumerator, |
| | 183 | 615 | | minimumProjection)); |
| | 183 | 616 | | Signed576 negative = Signed576.ExtendValue( |
| | 183 | 617 | | WideArithmetic.AddSigned320( |
| | 183 | 618 | | maximumProjection, |
| | 183 | 619 | | extentNumerator)); |
| | 183 | 620 | | if (positive.Sign < 0 || negative.Sign < 0) |
| | 15 | 621 | | return false; |
| | | 622 | | |
| | 168 | 623 | | bool usePositive = WideArithmetic.CompareNonNegative(positive, negative) <= 0; |
| | 168 | 624 | | Signed576 candidateDepth = usePositive ? positive : negative; |
| | 168 | 625 | | if (!found |
| | 168 | 626 | | || WideArithmetic.CompareNonNegative(candidateDepth, bestDepth) < 0) |
| | | 627 | | { |
| | 58 | 628 | | found = true; |
| | 58 | 629 | | bestDepth = candidateDepth; |
| | 58 | 630 | | bestNormal = usePositive ? axis : -axis; |
| | 58 | 631 | | bestAxisFromEdge = axisFromEdge; |
| | | 632 | | } |
| | | 633 | | |
| | 168 | 634 | | return true; |
| | | 635 | | } |
| | | 636 | | |
| | | 637 | | private static void GetRelativeProjectionRange( |
| | | 638 | | Vector2d center, |
| | | 639 | | Vector2d convexOrigin, |
| | | 640 | | Fixed64 convexRotation, |
| | | 641 | | ReadOnlySpan<Vector2d> convexOriginOffsets, |
| | | 642 | | Vector2d axis, |
| | | 643 | | out Signed192 minimum, |
| | | 644 | | out Signed192 maximum) |
| | | 645 | | { |
| | 183 | 646 | | minimum = GetRelativeProjection( |
| | 183 | 647 | | center, |
| | 183 | 648 | | convexOrigin, |
| | 183 | 649 | | convexRotation, |
| | 183 | 650 | | convexOriginOffsets[0], |
| | 183 | 651 | | axis); |
| | 183 | 652 | | maximum = minimum; |
| | 1484 | 653 | | for (int i = 1; i < convexOriginOffsets.Length; i++) |
| | | 654 | | { |
| | 559 | 655 | | Signed192 projection = GetRelativeProjection( |
| | 559 | 656 | | center, |
| | 559 | 657 | | convexOrigin, |
| | 559 | 658 | | convexRotation, |
| | 559 | 659 | | convexOriginOffsets[i], |
| | 559 | 660 | | axis); |
| | 559 | 661 | | if (WideArithmetic.SubtractSigned192(projection, minimum).Sign < 0) |
| | 119 | 662 | | minimum = projection; |
| | 440 | 663 | | else if (WideArithmetic.SubtractSigned192(projection, maximum).Sign > 0) |
| | 97 | 664 | | maximum = projection; |
| | | 665 | | } |
| | 183 | 666 | | } |
| | | 667 | | |
| | | 668 | | private static Signed192 GetRelativeProjection( |
| | | 669 | | Vector2d center, |
| | | 670 | | Vector2d pointOrigin, |
| | | 671 | | Fixed64 pointRotation, |
| | | 672 | | Vector2d pointOriginOffset, |
| | | 673 | | Vector2d axis) |
| | | 674 | | { |
| | 840 | 675 | | WideConvex2dRelations.GetRelativePointNumerators( |
| | 840 | 676 | | pointOrigin, |
| | 840 | 677 | | pointRotation, |
| | 840 | 678 | | pointOriginOffset, |
| | 840 | 679 | | center, |
| | 840 | 680 | | out Signed192 differenceX, |
| | 840 | 681 | | out Signed192 differenceY); |
| | 840 | 682 | | Signed320 projection = WideArithmetic.AddSigned320( |
| | 840 | 683 | | WideArithmetic.MultiplySigned192( |
| | 840 | 684 | | differenceX, |
| | 840 | 685 | | Signed192.Signed(axis.X.m_rawValue)), |
| | 840 | 686 | | WideArithmetic.MultiplySigned192( |
| | 840 | 687 | | differenceY, |
| | 840 | 688 | | Signed192.Signed(axis.Y.m_rawValue))); |
| | 840 | 689 | | return NarrowProven(projection); |
| | | 690 | | } |
| | | 691 | | |
| | | 692 | | } |