| | | 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 | | using RotationFrame2d = FixedMathSharp.Geometry.WideConvex2dRelations.RotationFrame2d; |
| | | 10 | | |
| | | 11 | | namespace FixedMathSharp.Geometry; |
| | | 12 | | |
| | | 13 | | /// <summary> |
| | | 14 | | /// Owns rigid-frame full-domain relations for conceptual 2D capsules. |
| | | 15 | | /// </summary> |
| | | 16 | | internal static class WideCenteredCapsule2dRelations |
| | | 17 | | { |
| | | 18 | | /// <summary>Returns the exact world direction from an anchored point toward its closest finite core point.</summary |
| | | 19 | | internal static WideDirection3d GetPointToCoreDirection(Vector2d origin, Fixed64 rotation, |
| | | 20 | | Fixed64 coreLength, in FixedPointAnchor2d point) |
| | | 21 | | { |
| | 6 | 22 | | RationalPoint3d start = RationalPoint3d.FromAnchor(WideFiniteAxisIntersection.GetCenteredCapsuleSupportAnchor( |
| | 6 | 23 | | origin, rotation, Vector2d.Forward, coreLength, Fixed64.Zero, -Vector2d.Forward)); |
| | 6 | 24 | | RationalPoint3d end = RationalPoint3d.FromAnchor(WideFiniteAxisIntersection.GetCenteredCapsuleSupportAnchor( |
| | 6 | 25 | | origin, rotation, Vector2d.Forward, coreLength, Fixed64.Zero, Vector2d.Forward)); |
| | 6 | 26 | | Signed192 denominator = WideArithmetic.TruncateSigned320By64(start.Denominator); |
| | 6 | 27 | | WideDirection3d edge = WideTriangleRelations.GetScaledPointDifference(end, start, denominator), |
| | 6 | 28 | | offset = WideTriangleRelations.GetScaledPointDifference(RationalPoint3d.FromAnchor(point), start, denominato |
| | 6 | 29 | | Signed320 n = Signed320.NarrowValue(Signed576.NarrowValue(WideDirection3d.Dot(edge, offset))), |
| | 6 | 30 | | d = Signed320.NarrowValue(Signed576.NarrowValue(WideDirection3d.Dot(edge, edge))); |
| | 8 | 31 | | if (n.Sign <= 0) return -offset; |
| | 4 | 32 | | if (WideArithmetic.SubtractSigned320(n, d).Sign >= 0) |
| | 1 | 33 | | return new(WideArithmetic.SubtractSigned576(edge.X, offset.X), default(Signed576), |
| | 1 | 34 | | WideArithmetic.SubtractSigned576(edge.Z, offset.Z)); |
| | | 35 | | // All planar anchors share 2 Q²: differences need fewer than 131 bits, |
| | | 36 | | // dot products fewer than 264, and these cleared products fewer than 396. |
| | 3 | 37 | | return new(Signed576.NarrowValue(WideArithmetic.SubtractSigned832( |
| | 3 | 38 | | WideArithmetic.MultiplySigned576ToSigned832(edge.X, n), WideArithmetic.MultiplySigned576ToSigned832(offs |
| | 3 | 39 | | default(Signed576), Signed576.NarrowValue(WideArithmetic.SubtractSigned832( |
| | 3 | 40 | | WideArithmetic.MultiplySigned576ToSigned832(edge.Z, n), WideArithmetic.MultiplySigned576ToSigned832(offs |
| | | 41 | | } |
| | | 42 | | |
| | | 43 | | // Full-domain bounds (S = 2^32): relative point numerators are below 2^97, |
| | | 44 | | // exact axis components below 2^99, and signed overlaps below 2^199. |
| | | 45 | | // Squared-overlap/axis products stay below 2^597, fitting Signed832. |
| | | 46 | | // No world endpoint, unit axis, or radial square root is narrowed to Fixed64 |
| | | 47 | | // until classification and minimum-depth ordering are complete. |
| | 1 | 48 | | private static readonly Fixed64 SideAlignmentTolerance = |
| | 1 | 49 | | Fixed64.Epsilon * (Fixed64)16; |
| | | 50 | | |
| | | 51 | | private readonly struct ContactAxis |
| | | 52 | | { |
| | | 53 | | internal readonly Signed192 X; |
| | | 54 | | internal readonly Signed192 Y; |
| | | 55 | | internal readonly Signed320 SquaredLength; |
| | | 56 | | internal readonly Signed320 Overlap; |
| | | 57 | | internal readonly bool FromEdge; |
| | | 58 | | |
| | 6402 | 59 | | internal bool HasValue => !SquaredLength.IsZero; |
| | | 60 | | |
| | | 61 | | internal ContactAxis( |
| | | 62 | | Signed192 x, |
| | | 63 | | Signed192 y, |
| | | 64 | | Signed320 squaredLength, |
| | | 65 | | Signed320 overlap, |
| | | 66 | | bool fromEdge) |
| | | 67 | | { |
| | 7983 | 68 | | X = x; |
| | 7983 | 69 | | Y = y; |
| | 7983 | 70 | | SquaredLength = squaredLength; |
| | 7983 | 71 | | Overlap = overlap; |
| | 7983 | 72 | | FromEdge = fromEdge; |
| | 7983 | 73 | | } |
| | | 74 | | } |
| | | 75 | | |
| | | 76 | | /// <summary> |
| | | 77 | | /// Finds a contained or boundary point's minimum-separation normal toward a |
| | | 78 | | /// rotated nondegenerate closed convex polygon. Outside points return false |
| | | 79 | | /// and zero. |
| | | 80 | | /// </summary> |
| | | 81 | | /// <remarks> |
| | | 82 | | /// The caller supplies a validated convex boundary. Uses the same exact |
| | | 83 | | /// signed depth/axis ranking and feature ties as point contacts. Only the |
| | | 84 | | /// normalized direction is rounded to Q32.32; no depth |
| | | 85 | | /// or contact anchor is materialized. |
| | | 86 | | /// </remarks> |
| | | 87 | | internal static bool TryGetPointMinimumTranslationNormal( |
| | | 88 | | Vector2d point, |
| | | 89 | | Vector2d convexOrigin, |
| | | 90 | | Fixed64 convexRotation, |
| | | 91 | | ReadOnlySpan<Vector2d> convexOriginOffsets, |
| | | 92 | | out Vector2d normal) |
| | | 93 | | { |
| | 9 | 94 | | normal = default; |
| | 9 | 95 | | if (!TryGetContactAxis( |
| | 9 | 96 | | point, Vector2d.Right, Fixed64.Zero, Fixed64.Zero, |
| | 9 | 97 | | convexOrigin, convexRotation, convexOriginOffsets, |
| | 9 | 98 | | strict: false, out ContactAxis best, pointFaceAxesOnly: true)) |
| | 1 | 99 | | return false; |
| | | 100 | | |
| | 8 | 101 | | normal = WideNormalization.GetNormalized(best.X, best.Y); |
| | 8 | 102 | | return true; |
| | | 103 | | } |
| | | 104 | | |
| | | 105 | | internal static bool TryGetMinimumTranslation( |
| | | 106 | | Vector2d center, |
| | | 107 | | Vector2d capsuleAxis, |
| | | 108 | | Fixed64 axisLength, |
| | | 109 | | Fixed64 radius, |
| | | 110 | | Vector2d convexOrigin, |
| | | 111 | | ReadOnlySpan<Vector2d> convexOriginOffsets, |
| | | 112 | | out Vector2d normal, |
| | | 113 | | out Fixed64 depth) => |
| | 24 | 114 | | TryGetMinimumTranslation( |
| | 24 | 115 | | center, |
| | 24 | 116 | | capsuleAxis, |
| | 24 | 117 | | axisLength, |
| | 24 | 118 | | radius, |
| | 24 | 119 | | convexOrigin, |
| | 24 | 120 | | Fixed64.Zero, |
| | 24 | 121 | | convexOriginOffsets, |
| | 24 | 122 | | out normal, |
| | 24 | 123 | | out depth); |
| | | 124 | | |
| | | 125 | | internal static bool TryGetMinimumTranslation( |
| | | 126 | | Vector2d center, |
| | | 127 | | Vector2d capsuleAxis, |
| | | 128 | | Fixed64 axisLength, |
| | | 129 | | Fixed64 radius, |
| | | 130 | | Vector2d convexOrigin, |
| | | 131 | | Fixed64 convexRotation, |
| | | 132 | | ReadOnlySpan<Vector2d> convexOriginOffsets, |
| | | 133 | | out Vector2d normal, |
| | | 134 | | out Fixed64 depth) => |
| | 1314 | 135 | | TryGetMinimumTranslation( |
| | 1314 | 136 | | center, |
| | 1314 | 137 | | capsuleAxis, |
| | 1314 | 138 | | axisLength, |
| | 1314 | 139 | | radius, |
| | 1314 | 140 | | convexOrigin, |
| | 1314 | 141 | | convexRotation, |
| | 1314 | 142 | | convexOriginOffsets, |
| | 1314 | 143 | | out normal, |
| | 1314 | 144 | | out depth, |
| | 1314 | 145 | | out _, |
| | 1314 | 146 | | out _); |
| | | 147 | | |
| | | 148 | | /// <summary>Tests exact closed contact or strict penetration without materializing contact anchors or depth.</summa |
| | | 149 | | internal static bool DoesCapsuleOverlapConvex( |
| | | 150 | | Vector2d center, |
| | | 151 | | Vector2d capsuleAxis, |
| | | 152 | | Fixed64 axisLength, |
| | | 153 | | Fixed64 radius, |
| | | 154 | | Vector2d convexOrigin, |
| | | 155 | | Fixed64 convexRotation, |
| | | 156 | | ReadOnlySpan<Vector2d> convexOriginOffsets, |
| | | 157 | | bool strict = true) => |
| | 1320 | 158 | | TryGetContactAxis( |
| | 1320 | 159 | | center, capsuleAxis, axisLength, radius, |
| | 1320 | 160 | | convexOrigin, convexRotation, convexOriginOffsets, |
| | 1320 | 161 | | strict, out _); |
| | | 162 | | |
| | | 163 | | internal static bool TryGetContacts( |
| | | 164 | | Vector2d capsuleCenter, |
| | | 165 | | Fixed64 capsuleRotation, |
| | | 166 | | Vector2d localCapsuleAxis, |
| | | 167 | | Fixed64 axisLength, |
| | | 168 | | Fixed64 radius, |
| | | 169 | | Vector2d convexOrigin, |
| | | 170 | | Fixed64 convexRotation, |
| | | 171 | | ReadOnlySpan<Vector2d> convexVertexOffsets, |
| | | 172 | | Span<FixedPointAnchor2d> capsuleContacts, |
| | | 173 | | Span<FixedPointAnchor2d> convexContacts, |
| | | 174 | | out int contactCount, |
| | | 175 | | out Vector2d normal, |
| | | 176 | | out Fixed64 depth, |
| | | 177 | | out bool depthIsClamped) |
| | | 178 | | { |
| | 507 | 179 | | _ = Vector2d.TryRotate( |
| | 507 | 180 | | localCapsuleAxis, |
| | 507 | 181 | | capsuleRotation, |
| | 507 | 182 | | out Vector2d capsuleAxis); |
| | 507 | 183 | | if (!TryGetMinimumTranslation( |
| | 507 | 184 | | capsuleCenter, |
| | 507 | 185 | | capsuleAxis, |
| | 507 | 186 | | axisLength, |
| | 507 | 187 | | radius, |
| | 507 | 188 | | convexOrigin, |
| | 507 | 189 | | convexRotation, |
| | 507 | 190 | | convexVertexOffsets, |
| | 507 | 191 | | out normal, |
| | 507 | 192 | | out depth, |
| | 507 | 193 | | out depthIsClamped, |
| | 507 | 194 | | out ContactAxis contactAxis)) |
| | | 195 | | { |
| | 255 | 196 | | contactCount = default; |
| | 255 | 197 | | return false; |
| | | 198 | | } |
| | | 199 | | |
| | 252 | 200 | | _ = Vector2d.TryRotate( |
| | 252 | 201 | | normal, |
| | 252 | 202 | | -capsuleRotation, |
| | 252 | 203 | | out Vector2d localNormal); |
| | 252 | 204 | | localNormal = localNormal.Normalized; |
| | 252 | 205 | | GetSupportFeature( |
| | 252 | 206 | | capsuleCenter, |
| | 252 | 207 | | convexOrigin, |
| | 252 | 208 | | new RotationFrame2d(convexRotation), |
| | 252 | 209 | | convexVertexOffsets, |
| | 252 | 210 | | contactAxis, |
| | 252 | 211 | | out Vector2d featureStart, |
| | 252 | 212 | | out Vector2d featureEnd); |
| | 252 | 213 | | Vector2d featureDirection = WideNormalization.GetDirection(featureStart, featureEnd); |
| | 252 | 214 | | if (axisLength > Fixed64.Epsilon |
| | 252 | 215 | | && featureDirection != Vector2d.Zero |
| | 252 | 216 | | && Vector2d.Dot(capsuleAxis, normal).Abs() |
| | 252 | 217 | | <= SideAlignmentTolerance) |
| | | 218 | | { |
| | 7 | 219 | | FixedPointAnchor2d firstCapsuleAnchor = |
| | 7 | 220 | | GetCenteredCapsuleSideAnchor( |
| | 7 | 221 | | capsuleCenter, |
| | 7 | 222 | | capsuleRotation, |
| | 7 | 223 | | localCapsuleAxis, |
| | 7 | 224 | | -axisLength, |
| | 7 | 225 | | localNormal, |
| | 7 | 226 | | radius); |
| | 7 | 227 | | FixedPointAnchor2d secondCapsuleAnchor = |
| | 7 | 228 | | GetCenteredCapsuleSideAnchor( |
| | 7 | 229 | | capsuleCenter, |
| | 7 | 230 | | capsuleRotation, |
| | 7 | 231 | | localCapsuleAxis, |
| | 7 | 232 | | axisLength, |
| | 7 | 233 | | localNormal, |
| | 7 | 234 | | radius); |
| | 7 | 235 | | if (WideConvex2dRelations.TryProjectAnchorOntoFeature( |
| | 7 | 236 | | firstCapsuleAnchor, |
| | 7 | 237 | | convexOrigin, |
| | 7 | 238 | | convexRotation, |
| | 7 | 239 | | featureStart, |
| | 7 | 240 | | featureEnd, |
| | 7 | 241 | | requireInteriorProjection: true, |
| | 7 | 242 | | out Vector2d firstConvexOffset) |
| | 7 | 243 | | && WideConvex2dRelations.TryProjectAnchorOntoFeature( |
| | 7 | 244 | | secondCapsuleAnchor, |
| | 7 | 245 | | convexOrigin, |
| | 7 | 246 | | convexRotation, |
| | 7 | 247 | | featureStart, |
| | 7 | 248 | | featureEnd, |
| | 7 | 249 | | requireInteriorProjection: true, |
| | 7 | 250 | | out Vector2d secondConvexOffset)) |
| | | 251 | | { |
| | 4 | 252 | | capsuleContacts[0] = firstCapsuleAnchor; |
| | 4 | 253 | | capsuleContacts[1] = secondCapsuleAnchor; |
| | 4 | 254 | | convexContacts[0] = new FixedPointAnchor2d( |
| | 4 | 255 | | convexOrigin, |
| | 4 | 256 | | convexRotation, |
| | 4 | 257 | | firstConvexOffset); |
| | 4 | 258 | | convexContacts[1] = new FixedPointAnchor2d( |
| | 4 | 259 | | convexOrigin, |
| | 4 | 260 | | convexRotation, |
| | 4 | 261 | | secondConvexOffset); |
| | 4 | 262 | | contactCount = 2; |
| | 4 | 263 | | return true; |
| | | 264 | | } |
| | | 265 | | } |
| | | 266 | | |
| | 248 | 267 | | FixedPointAnchor2d capsuleContact = |
| | 248 | 268 | | WideFiniteAxisIntersection.GetCenteredCapsuleSupportAnchor( |
| | 248 | 269 | | capsuleCenter, |
| | 248 | 270 | | capsuleRotation, |
| | 248 | 271 | | localCapsuleAxis, |
| | 248 | 272 | | axisLength, |
| | 248 | 273 | | radius, |
| | 248 | 274 | | localNormal); |
| | 248 | 275 | | _ = WideConvex2dRelations.TryProjectAnchorOntoFeature( |
| | 248 | 276 | | capsuleContact, |
| | 248 | 277 | | convexOrigin, |
| | 248 | 278 | | convexRotation, |
| | 248 | 279 | | featureStart, |
| | 248 | 280 | | featureEnd, |
| | 248 | 281 | | requireInteriorProjection: false, |
| | 248 | 282 | | out Vector2d convexOffset); |
| | | 283 | | |
| | | 284 | | // A side support is an entire segment, not its arbitrary midpoint. |
| | | 285 | | // Resolve its axial witness from the opposing feature using the exact |
| | | 286 | | // selected axis; a rounded normal can turn a side tie into an end cap. |
| | 248 | 287 | | if (axisLength > Fixed64.Zero |
| | 248 | 288 | | && WideArithmetic.AddSigned320( |
| | 248 | 289 | | WideArithmetic.MultiplySigned192(contactAxis.X, Signed192.Raw(capsuleAxis.X)), |
| | 248 | 290 | | WideArithmetic.MultiplySigned192(contactAxis.Y, Signed192.Raw(capsuleAxis.Y))).IsZero) |
| | | 291 | | { |
| | 69 | 292 | | capsuleContact = GetProjectedSideAnchor( |
| | 69 | 293 | | capsuleCenter, capsuleRotation, localCapsuleAxis, axisLength, |
| | 69 | 294 | | localNormal, radius, convexOrigin, convexRotation, convexOffset); |
| | | 295 | | } |
| | | 296 | | |
| | 248 | 297 | | capsuleContacts[0] = capsuleContact; |
| | 248 | 298 | | convexContacts[0] = new FixedPointAnchor2d( |
| | 248 | 299 | | convexOrigin, |
| | 248 | 300 | | convexRotation, |
| | 248 | 301 | | convexOffset); |
| | 248 | 302 | | contactCount = 1; |
| | 248 | 303 | | return true; |
| | | 304 | | } |
| | | 305 | | |
| | | 306 | | /// <summary>Admits an original finite vertex normal against all exact incident sectors before normalization.</summa |
| | | 307 | | internal static bool TryGetBoundaryVertexContact(Vector2d center, Fixed64 rotation, Fixed64 axisLength, |
| | | 308 | | Fixed64 radius, in FixedPointAnchor2d vertex, ReadOnlySpan<FixedPointAnchor2d> incidentPoints, |
| | | 309 | | out FixedContactAnchors2d contact) |
| | | 310 | | { |
| | 9 | 311 | | _ = Vector2d.TryRotate(Vector2d.Forward, rotation, out Vector2d axis); |
| | 9 | 312 | | Span<Vector2d> point = stackalloc Vector2d[1] { vertex.LocalPoint }; |
| | 9 | 313 | | if (!TryGetContactAxis(center, axis, axisLength, radius, vertex.Origin, vertex.Rotation, point, |
| | 13 | 314 | | strict: false, out ContactAxis best)) { contact = default; return false; } |
| | 7 | 315 | | if (!WideConvex2dRelations.SupportsBoundaryFan(best.X, best.Y, vertex, incidentPoints)) |
| | | 316 | | { |
| | | 317 | | // A point on the core has tied opposite side directions. Do not |
| | | 318 | | // let the ordinary authored-direction tie hide an admissible side. |
| | 4 | 319 | | if (!best.Overlap.IsZero || !WideConvex2dRelations.SupportsBoundaryFan( |
| | 4 | 320 | | WideArithmetic.Negate(best.X), WideArithmetic.Negate(best.Y), vertex, incidentPoints)) |
| | 6 | 321 | | { contact = default; return false; } |
| | 1 | 322 | | best = new ContactAxis(WideArithmetic.Negate(best.X), WideArithmetic.Negate(best.Y), |
| | 1 | 323 | | best.SquaredLength, best.Overlap, best.FromEdge); |
| | | 324 | | } |
| | 4 | 325 | | Vector2d normal = WideNormalization.GetNormalized(best.X, best.Y); |
| | 4 | 326 | | _ = Vector2d.TryRotate(normal, -rotation, out Vector2d localNormal); |
| | 4 | 327 | | localNormal = localNormal.Normalized; |
| | 4 | 328 | | FixedPointAnchor2d anchor = WideFiniteAxisIntersection.GetCenteredCapsuleSupportAnchor( |
| | 4 | 329 | | center, rotation, Vector2d.Forward, axisLength, radius, localNormal); |
| | 4 | 330 | | if (axisLength > Fixed64.Zero && WideArithmetic.AddProducts(best.X, Signed192.Raw(axis.X), |
| | 4 | 331 | | best.Y, Signed192.Raw(axis.Y)).IsZero) |
| | 2 | 332 | | anchor = GetProjectedSideAnchor(center, rotation, Vector2d.Forward, axisLength, localNormal, |
| | 2 | 333 | | radius, vertex.Origin, vertex.Rotation, vertex.LocalPoint); |
| | 4 | 334 | | contact = new FixedContactAnchors2d(anchor, vertex, normal, GetDepth(best, radius, out bool clamped), clamped); |
| | 4 | 335 | | return true; |
| | | 336 | | } |
| | | 337 | | |
| | | 338 | | private static FixedPointAnchor2d GetProjectedSideAnchor( |
| | | 339 | | Vector2d center, |
| | | 340 | | Fixed64 rotation, |
| | | 341 | | Vector2d localAxis, |
| | | 342 | | Fixed64 axisLength, |
| | | 343 | | Vector2d localNormal, |
| | | 344 | | Fixed64 radius, |
| | | 345 | | Vector2d pointOrigin, |
| | | 346 | | Fixed64 pointRotation, |
| | | 347 | | Vector2d pointOffset) |
| | | 348 | | { |
| | 71 | 349 | | RotationFrame2d frame = new(rotation); |
| | 71 | 350 | | WideConvex2dRelations.GetRotatedOffset(frame, localAxis, |
| | 71 | 351 | | out Signed192 axisX, out Signed192 axisY); |
| | 71 | 352 | | WideConvex2dRelations.GetRotatedOffset(frame, localNormal, |
| | 71 | 353 | | out Signed192 radialX, out Signed192 radialY); |
| | 71 | 354 | | GetRelativePointNumerators(pointOrigin, new RotationFrame2d(pointRotation), |
| | 71 | 355 | | pointOffset, center, out Signed192 pointX, out Signed192 pointY); |
| | | 356 | | |
| | | 357 | | // Axis and point numerators have denominator S^2. Subtract the radial |
| | | 358 | | // support at denominator S^3 before projecting onto the conceptual side. |
| | | 359 | | // Twice the local axial distance in raw units is 2*dot(delta,axis)/|axis|^2. |
| | 71 | 360 | | Signed192 deltaX = Signed192.NarrowValue(WideArithmetic.SubtractSigned320( |
| | 71 | 361 | | WideArithmetic.MultiplySigned192(pointX, Signed192.One), |
| | 71 | 362 | | WideArithmetic.MultiplySigned192(radialX, Signed192.Raw(radius)))); |
| | 71 | 363 | | Signed192 deltaY = Signed192.NarrowValue(WideArithmetic.SubtractSigned320( |
| | 71 | 364 | | WideArithmetic.MultiplySigned192(pointY, Signed192.One), |
| | 71 | 365 | | WideArithmetic.MultiplySigned192(radialY, Signed192.Raw(radius)))); |
| | 71 | 366 | | Signed320 projection = WideArithmetic.AddSigned320( |
| | 71 | 367 | | WideArithmetic.MultiplySigned192(deltaX, axisX), |
| | 71 | 368 | | WideArithmetic.MultiplySigned192(deltaY, axisY)); |
| | 71 | 369 | | Signed320 numerator = WideArithmetic.AddSigned320(projection, projection); |
| | 71 | 370 | | Signed320 denominator = WideArithmetic.AddSigned320( |
| | 71 | 371 | | WideArithmetic.MultiplySigned192(axisX, axisX), |
| | 71 | 372 | | WideArithmetic.MultiplySigned192(axisY, axisY)); |
| | 71 | 373 | | Signed576 limit = WideArithmetic.MultiplySigned320(denominator, Signed192.Raw(axisLength)); |
| | 71 | 374 | | Signed576 signedNumerator = Signed576.ExtendValue(numerator); |
| | | 375 | | Fixed64 signedLength; |
| | 71 | 376 | | if (WideArithmetic.SubtractSigned576(signedNumerator, limit).Sign >= 0) |
| | 1 | 377 | | signedLength = axisLength; |
| | 70 | 378 | | else if (WideArithmetic.AddSigned576(signedNumerator, limit).Sign <= 0) |
| | 1 | 379 | | signedLength = -axisLength; |
| | | 380 | | else |
| | 69 | 381 | | _ = Fixed64.TryGetSignedRawRatio(signedNumerator, |
| | 69 | 382 | | Signed576.ExtendValue(denominator), out signedLength); |
| | | 383 | | |
| | 71 | 384 | | return GetCenteredCapsuleSideAnchor( |
| | 71 | 385 | | center, rotation, localAxis, signedLength, localNormal, radius); |
| | | 386 | | } |
| | | 387 | | |
| | | 388 | | private static FixedPointAnchor2d GetCenteredCapsuleSideAnchor( |
| | | 389 | | Vector2d center, |
| | | 390 | | Fixed64 rotation, |
| | | 391 | | Vector2d localAxis, |
| | | 392 | | Fixed64 signedAxisLength, |
| | | 393 | | Vector2d localRadialDirection, |
| | | 394 | | Fixed64 radius) |
| | | 395 | | { |
| | 85 | 396 | | Vector2d axialOffset = |
| | 85 | 397 | | localAxis * (signedAxisLength / Fixed64.Two); |
| | 85 | 398 | | Vector2d radialOffset = localRadialDirection * radius; |
| | 85 | 399 | | FixedPointAnchorTerm2d exactLocalTerm = |
| | 85 | 400 | | FixedPointAnchorTerm2d.CreateCenteredAxisSupport( |
| | 85 | 401 | | localAxis, |
| | 85 | 402 | | signedAxisLength, |
| | 85 | 403 | | localRadialDirection, |
| | 85 | 404 | | radius, |
| | 85 | 405 | | axialOffset, |
| | 85 | 406 | | radialOffset); |
| | 85 | 407 | | return new FixedPointAnchor2d( |
| | 85 | 408 | | center, |
| | 85 | 409 | | rotation, |
| | 85 | 410 | | axialOffset, |
| | 85 | 411 | | radialOffset, |
| | 85 | 412 | | exactLocalTerm); |
| | | 413 | | } |
| | | 414 | | |
| | | 415 | | private static bool TryGetMinimumTranslation( |
| | | 416 | | Vector2d center, |
| | | 417 | | Vector2d capsuleAxis, |
| | | 418 | | Fixed64 axisLength, |
| | | 419 | | Fixed64 radius, |
| | | 420 | | Vector2d convexOrigin, |
| | | 421 | | Fixed64 convexRotation, |
| | | 422 | | ReadOnlySpan<Vector2d> convexOriginOffsets, |
| | | 423 | | out Vector2d normal, |
| | | 424 | | out Fixed64 depth, |
| | | 425 | | out bool depthIsClamped, |
| | | 426 | | out ContactAxis best) |
| | | 427 | | { |
| | 1821 | 428 | | normal = default; |
| | 1821 | 429 | | depth = default; |
| | 1821 | 430 | | depthIsClamped = false; |
| | 1821 | 431 | | if (!TryGetContactAxis( |
| | 1821 | 432 | | center, capsuleAxis, axisLength, radius, |
| | 1821 | 433 | | convexOrigin, convexRotation, convexOriginOffsets, |
| | 1821 | 434 | | strict: false, out best)) |
| | | 435 | | { |
| | 1300 | 436 | | return false; |
| | | 437 | | } |
| | | 438 | | |
| | 521 | 439 | | normal = WideNormalization.GetNormalized(best.X, best.Y); |
| | 521 | 440 | | depth = GetDepth(best, radius, out depthIsClamped); |
| | 521 | 441 | | return true; |
| | | 442 | | } |
| | | 443 | | |
| | | 444 | | private static bool TryGetContactAxis( |
| | | 445 | | Vector2d center, |
| | | 446 | | Vector2d capsuleAxis, |
| | | 447 | | Fixed64 axisLength, |
| | | 448 | | Fixed64 radius, |
| | | 449 | | Vector2d convexOrigin, |
| | | 450 | | Fixed64 convexRotation, |
| | | 451 | | ReadOnlySpan<Vector2d> convexOriginOffsets, |
| | | 452 | | bool strict, |
| | | 453 | | out ContactAxis best, |
| | | 454 | | bool pointFaceAxesOnly = false) |
| | | 455 | | { |
| | 3159 | 456 | | RotationFrame2d frame = new(convexRotation); |
| | 3159 | 457 | | best = default; |
| | 15384 | 458 | | for (int i = 0; i < convexOriginOffsets.Length; i++) |
| | | 459 | | { |
| | 6692 | 460 | | WideConvex2dRelations.GetRotatedOffset( |
| | 6692 | 461 | | frame, convexOriginOffsets[i], |
| | 6692 | 462 | | out Signed192 startX, out Signed192 startY); |
| | 6692 | 463 | | WideConvex2dRelations.GetRotatedOffset( |
| | 6692 | 464 | | frame, convexOriginOffsets[(i + 1) % convexOriginOffsets.Length], |
| | 6692 | 465 | | out Signed192 endX, out Signed192 endY); |
| | 6692 | 466 | | Signed192 axisX = WideArithmetic.SubtractSigned192(startY, endY); |
| | 6692 | 467 | | Signed192 axisY = WideArithmetic.SubtractSigned192(endX, startX); |
| | 6692 | 468 | | if (axisX.IsZero && axisY.IsZero) |
| | | 469 | | continue; |
| | | 470 | | |
| | 6379 | 471 | | if (!TryKeepAxis( |
| | 6379 | 472 | | center, capsuleAxis, axisLength, radius, |
| | 6379 | 473 | | convexOrigin, frame, convexOriginOffsets, |
| | 6379 | 474 | | axisX, axisY, true, strict, ref best)) |
| | | 475 | | { |
| | 2159 | 476 | | return false; |
| | | 477 | | } |
| | | 478 | | } |
| | | 479 | | |
| | | 480 | | // The admitted nondegenerate point-normal path separates through a |
| | | 481 | | // face. General contacts retain vertex axes for degenerate boundaries. |
| | 1000 | 482 | | if (pointFaceAxesOnly) |
| | 8 | 483 | | return best.HasValue; |
| | | 484 | | |
| | | 485 | | // The segment contributes its own pair of sides to the polygon's |
| | | 486 | | // Minkowski expansion. A closest endpoint/vertex axis need not cover it. |
| | 992 | 487 | | if (axisLength > Fixed64.Zero |
| | 992 | 488 | | && !TryKeepAxis( |
| | 992 | 489 | | center, capsuleAxis, axisLength, radius, |
| | 992 | 490 | | convexOrigin, frame, convexOriginOffsets, |
| | 992 | 491 | | WideArithmetic.Negate(Signed192.Raw(capsuleAxis.Y)), |
| | 992 | 492 | | Signed192.Raw(capsuleAxis.X), false, strict, ref best)) |
| | | 493 | | { |
| | 87 | 494 | | return false; |
| | | 495 | | } |
| | | 496 | | |
| | 905 | 497 | | GetClosestVertexAxis( |
| | 905 | 498 | | center, capsuleAxis, axisLength, |
| | 905 | 499 | | convexOrigin, frame, convexOriginOffsets, |
| | 905 | 500 | | out Signed192 closestX, out Signed192 closestY); |
| | 905 | 501 | | if ((!closestX.IsZero || !closestY.IsZero) |
| | 905 | 502 | | && !TryKeepAxis( |
| | 905 | 503 | | center, capsuleAxis, axisLength, radius, |
| | 905 | 504 | | convexOrigin, frame, convexOriginOffsets, |
| | 905 | 505 | | closestX, closestY, false, strict, ref best)) |
| | | 506 | | { |
| | 123 | 507 | | return false; |
| | | 508 | | } |
| | | 509 | | |
| | | 510 | | // A coincident point and zero-length core have no geometric axis; |
| | | 511 | | // any cardinal axis gives their exact radial depth, including zero. |
| | 782 | 512 | | return best.HasValue || TryKeepAxis( |
| | 782 | 513 | | center, capsuleAxis, axisLength, radius, convexOrigin, frame, convexOriginOffsets, |
| | 782 | 514 | | Signed192.One, default, false, strict, ref best); |
| | | 515 | | } |
| | | 516 | | |
| | | 517 | | private static void GetClosestVertexAxis( |
| | | 518 | | Vector2d center, |
| | | 519 | | Vector2d capsuleAxis, |
| | | 520 | | Fixed64 axisLength, |
| | | 521 | | Vector2d convexOrigin, |
| | | 522 | | RotationFrame2d convexRotation, |
| | | 523 | | ReadOnlySpan<Vector2d> convexOriginOffsets, |
| | | 524 | | out Signed192 bestX, |
| | | 525 | | out Signed192 bestY) |
| | | 526 | | { |
| | 905 | 527 | | bool found = false; |
| | 905 | 528 | | bestX = default; |
| | 905 | 529 | | bestY = default; |
| | 905 | 530 | | Signed576 bestSquaredDistance = default; |
| | 905 | 531 | | Signed192 directionSquared = Signed192.NarrowValue( |
| | 905 | 532 | | WideArithmetic.AddSigned320( |
| | 905 | 533 | | WideArithmetic.MultiplySigned192( |
| | 905 | 534 | | Signed192.Raw(capsuleAxis.X), Signed192.Raw(capsuleAxis.X)), |
| | 905 | 535 | | WideArithmetic.MultiplySigned192( |
| | 905 | 536 | | Signed192.Raw(capsuleAxis.Y), Signed192.Raw(capsuleAxis.Y)))); |
| | 8758 | 537 | | for (int i = 0; i < convexOriginOffsets.Length; i++) |
| | | 538 | | { |
| | 3474 | 539 | | GetDirectionFromCenteredAxis( |
| | 3474 | 540 | | convexOrigin, convexRotation, convexOriginOffsets[i], |
| | 3474 | 541 | | center, capsuleAxis, axisLength, directionSquared, |
| | 3474 | 542 | | out Signed192 candidateX, out Signed192 candidateY, |
| | 3474 | 543 | | out Signed576 candidateSquaredDistance); |
| | 3474 | 544 | | if (candidateX.IsZero && candidateY.IsZero) |
| | | 545 | | continue; |
| | 3230 | 546 | | if (found && WideArithmetic.CompareNonNegative( |
| | 3230 | 547 | | candidateSquaredDistance, bestSquaredDistance) >= 0) |
| | | 548 | | { |
| | | 549 | | continue; |
| | | 550 | | } |
| | | 551 | | |
| | 1755 | 552 | | found = true; |
| | 1755 | 553 | | bestX = candidateX; |
| | 1755 | 554 | | bestY = candidateY; |
| | 1755 | 555 | | bestSquaredDistance = candidateSquaredDistance; |
| | | 556 | | } |
| | 905 | 557 | | } |
| | | 558 | | |
| | | 559 | | private static void GetDirectionFromCenteredAxis( |
| | | 560 | | Vector2d pointOrigin, |
| | | 561 | | RotationFrame2d pointRotation, |
| | | 562 | | Vector2d pointOriginOffset, |
| | | 563 | | Vector2d axisCenter, |
| | | 564 | | Vector2d axisDirection, |
| | | 565 | | Fixed64 axisLength, |
| | | 566 | | Signed192 directionSquared, |
| | | 567 | | out Signed192 directionX, |
| | | 568 | | out Signed192 directionY, |
| | | 569 | | out Signed576 squaredDistance) |
| | | 570 | | { |
| | 3474 | 571 | | GetRelativePointNumerators( |
| | 3474 | 572 | | pointOrigin, pointRotation, pointOriginOffset, axisCenter, |
| | 3474 | 573 | | out Signed192 relativeX, out Signed192 relativeY); |
| | 3474 | 574 | | Signed192 axisX = Signed192.Raw(axisDirection.X); |
| | 3474 | 575 | | Signed192 axisY = Signed192.Raw(axisDirection.Y); |
| | 3474 | 576 | | Signed320 projection = WideArithmetic.AddSigned320( |
| | 3474 | 577 | | WideArithmetic.MultiplySigned192(relativeX, axisX), |
| | 3474 | 578 | | WideArithmetic.MultiplySigned192(relativeY, axisY)); |
| | 3474 | 579 | | Signed320 doubleProjection = WideArithmetic.AddSigned320(projection, projection); |
| | 3474 | 580 | | Signed320 axialThreshold = WideArithmetic.MultiplySigned192( |
| | 3474 | 581 | | Signed192.Raw(axisLength), directionSquared); |
| | | 582 | | |
| | | 583 | | // A supplied normalized direction still has representable components. |
| | | 584 | | // Use its actual squared length when clamping the closest parameter. |
| | 3474 | 585 | | if (WideArithmetic.SubtractSigned320(doubleProjection, axialThreshold).Sign > 0 |
| | 3474 | 586 | | || WideArithmetic.AddSigned320(doubleProjection, axialThreshold).Sign < 0) |
| | | 587 | | { |
| | 2228 | 588 | | Signed192 signedLength = projection.Sign > 0 |
| | 2228 | 589 | | ? Signed192.Raw(axisLength) |
| | 2228 | 590 | | : WideArithmetic.Negate(Signed192.Raw(axisLength)); |
| | 2228 | 591 | | directionX = WideArithmetic.SubtractSigned192( |
| | 2228 | 592 | | WideArithmetic.AddSigned192(relativeX, relativeX), |
| | 2228 | 593 | | Signed192.NarrowValue(WideArithmetic.MultiplySigned192(axisX, signedLength))); |
| | 2228 | 594 | | directionY = WideArithmetic.SubtractSigned192( |
| | 2228 | 595 | | WideArithmetic.AddSigned192(relativeY, relativeY), |
| | 2228 | 596 | | Signed192.NarrowValue(WideArithmetic.MultiplySigned192(axisY, signedLength))); |
| | 2228 | 597 | | Signed320 endpointSquared = WideArithmetic.AddSigned320( |
| | 2228 | 598 | | WideArithmetic.MultiplySigned192(directionX, directionX), |
| | 2228 | 599 | | WideArithmetic.MultiplySigned192(directionY, directionY)); |
| | 2228 | 600 | | squaredDistance = WideArithmetic.MultiplySigned320(endpointSquared, directionSquared); |
| | 2228 | 601 | | return; |
| | | 602 | | } |
| | | 603 | | |
| | 1246 | 604 | | Signed320 cross = WideArithmetic.SubtractSigned320( |
| | 1246 | 605 | | WideArithmetic.MultiplySigned192(axisX, relativeY), |
| | 1246 | 606 | | WideArithmetic.MultiplySigned192(axisY, relativeX)); |
| | 1246 | 607 | | directionX = cross.IsZero ? default : WideArithmetic.Negate(axisY); |
| | 1246 | 608 | | directionY = cross.IsZero ? default : axisX; |
| | 1246 | 609 | | if (cross.Sign < 0) |
| | | 610 | | { |
| | 488 | 611 | | directionX = WideArithmetic.Negate(directionX); |
| | 488 | 612 | | directionY = WideArithmetic.Negate(directionY); |
| | | 613 | | } |
| | | 614 | | |
| | | 615 | | // Endpoint and interior distances share denominator 4 * |direction|^2. |
| | | 616 | | // The interior axis is exactly perpendicular, without a rounded projection. |
| | 1246 | 617 | | Signed320 twiceCross = WideArithmetic.AddSigned320(cross, cross); |
| | 1246 | 618 | | squaredDistance = WideArithmetic.MultiplySigned320(twiceCross, twiceCross); |
| | 1246 | 619 | | } |
| | | 620 | | |
| | | 621 | | private static void GetSupportFeature( |
| | | 622 | | Vector2d center, |
| | | 623 | | Vector2d convexOrigin, |
| | | 624 | | RotationFrame2d convexRotation, |
| | | 625 | | ReadOnlySpan<Vector2d> convexOriginOffsets, |
| | | 626 | | ContactAxis axis, |
| | | 627 | | out Vector2d featureStart, |
| | | 628 | | out Vector2d featureEnd) |
| | | 629 | | { |
| | 252 | 630 | | if (axis.FromEdge) |
| | | 631 | | { |
| | 44 | 632 | | WideConvex2dRelations.GetMinimumSupportFeature(convexOrigin, convexRotation, convexOriginOffsets, |
| | 44 | 633 | | axis.X, axis.Y, out featureStart, out featureEnd); |
| | 44 | 634 | | return; |
| | | 635 | | } |
| | | 636 | | |
| | 208 | 637 | | int minimumIndex = 0; |
| | 208 | 638 | | Signed320 minimum = GetRelativeProjection( |
| | 208 | 639 | | center, |
| | 208 | 640 | | convexOrigin, |
| | 208 | 641 | | convexRotation, |
| | 208 | 642 | | convexOriginOffsets[0], |
| | 208 | 643 | | axis.X, axis.Y); |
| | 1520 | 644 | | for (int i = 1; i < convexOriginOffsets.Length; i++) |
| | | 645 | | { |
| | 552 | 646 | | Signed320 projection = GetRelativeProjection( |
| | 552 | 647 | | center, |
| | 552 | 648 | | convexOrigin, |
| | 552 | 649 | | convexRotation, |
| | 552 | 650 | | convexOriginOffsets[i], |
| | 552 | 651 | | axis.X, axis.Y); |
| | 552 | 652 | | if (WideArithmetic.SubtractSigned320(projection, minimum).Sign < 0) |
| | | 653 | | { |
| | 232 | 654 | | minimum = projection; |
| | 232 | 655 | | minimumIndex = i; |
| | | 656 | | } |
| | | 657 | | } |
| | | 658 | | |
| | 208 | 659 | | featureStart = convexOriginOffsets[minimumIndex]; |
| | 208 | 660 | | featureEnd = featureStart; |
| | 208 | 661 | | } |
| | | 662 | | |
| | | 663 | | private static bool TryKeepAxis( |
| | | 664 | | Vector2d center, |
| | | 665 | | Vector2d capsuleAxis, |
| | | 666 | | Fixed64 axisLength, |
| | | 667 | | Fixed64 radius, |
| | | 668 | | Vector2d convexOrigin, |
| | | 669 | | RotationFrame2d convexRotation, |
| | | 670 | | ReadOnlySpan<Vector2d> convexOriginOffsets, |
| | | 671 | | Signed192 axisX, |
| | | 672 | | Signed192 axisY, |
| | | 673 | | bool axisFromEdge, |
| | | 674 | | bool strict, |
| | | 675 | | ref ContactAxis best) |
| | | 676 | | { |
| | 7982 | 677 | | GetRelativeProjectionRange( |
| | 7982 | 678 | | center, convexOrigin, convexRotation, convexOriginOffsets, |
| | 7982 | 679 | | axisX, axisY, out Signed320 minimum, out Signed320 maximum); |
| | 7982 | 680 | | Signed192 axialAlignment = Signed192.NarrowValue(WideArithmetic.AddSigned320( |
| | 7982 | 681 | | WideArithmetic.MultiplySigned192(Signed192.Raw(capsuleAxis.X), axisX), |
| | 7982 | 682 | | WideArithmetic.MultiplySigned192(Signed192.Raw(capsuleAxis.Y), axisY))); |
| | 7982 | 683 | | if (axialAlignment.Sign < 0) |
| | 3321 | 684 | | axialAlignment = WideArithmetic.Negate(axialAlignment); |
| | 7982 | 685 | | Signed320 axialExtent = WideArithmetic.MultiplySigned192( |
| | 7982 | 686 | | axialAlignment, Signed192.Raw(axisLength)); |
| | 7982 | 687 | | Signed320 positive = WideArithmetic.SubtractSigned320( |
| | 7982 | 688 | | axialExtent, WideArithmetic.AddSigned320(minimum, minimum)); |
| | 7982 | 689 | | Signed320 negative = WideArithmetic.AddSigned320( |
| | 7982 | 690 | | axialExtent, WideArithmetic.AddSigned320(maximum, maximum)); |
| | 7982 | 691 | | bool usePositive = WideArithmetic.SubtractSigned320(positive, negative).Sign <= 0; |
| | 7982 | 692 | | Signed320 overlap = usePositive ? positive : negative; |
| | 7982 | 693 | | Signed320 axisSquared = WideArithmetic.AddSigned320( |
| | 7982 | 694 | | WideArithmetic.MultiplySigned192(axisX, axisX), |
| | 7982 | 695 | | WideArithmetic.MultiplySigned192(axisY, axisY)); |
| | 7982 | 696 | | ContactAxis candidate = new( |
| | 7982 | 697 | | usePositive ? axisX : WideArithmetic.Negate(axisX), |
| | 7982 | 698 | | usePositive ? axisY : WideArithmetic.Negate(axisY), |
| | 7982 | 699 | | axisSquared, overlap, axisFromEdge); |
| | | 700 | | |
| | | 701 | | // With S = 2^32, the exact depth in raw units is |
| | | 702 | | // radius.Raw + overlap / (2*S*sqrt(axisSquared)). Test its sign |
| | | 703 | | // before rounding either the axis, radial support, or public depth. |
| | 7982 | 704 | | int depthSign = CompareDepthToTwiceRaw(candidate, radius, default); |
| | 7982 | 705 | | if (strict ? depthSign <= 0 : depthSign < 0) |
| | 2370 | 706 | | return false; |
| | | 707 | | // A boolean strict relation needs no minimum-depth ranking, normal, |
| | | 708 | | // square root, or rounded contact witness. |
| | 5612 | 709 | | if (!best.HasValue || (!strict && CompareDepths(candidate, best) < 0)) |
| | 2267 | 710 | | best = candidate; |
| | 5612 | 711 | | return true; |
| | | 712 | | } |
| | | 713 | | |
| | | 714 | | private static int CompareDepths(ContactAxis left, ContactAxis right) |
| | | 715 | | { |
| | | 716 | | // The radius is identical on every axis and cancels. Compare signed |
| | | 717 | | // normalized axial overlaps; squaring reverses order for negative ones. |
| | 2362 | 718 | | int leftSign = left.Overlap.Sign; |
| | 2362 | 719 | | int rightSign = right.Overlap.Sign; |
| | 2362 | 720 | | if (leftSign != rightSign) |
| | 456 | 721 | | return leftSign.CompareTo(rightSign); |
| | 1906 | 722 | | Signed576 leftSquared = WideArithmetic.MultiplySigned320(left.Overlap, left.Overlap); |
| | 1906 | 723 | | Signed576 rightSquared = WideArithmetic.MultiplySigned320(right.Overlap, right.Overlap); |
| | 1906 | 724 | | int comparison = WideArithmetic.SubtractSigned832( |
| | 1906 | 725 | | WideArithmetic.MultiplySigned576ToSigned832(leftSquared, right.SquaredLength), |
| | 1906 | 726 | | WideArithmetic.MultiplySigned576ToSigned832(rightSquared, left.SquaredLength)).Sign; |
| | 1906 | 727 | | return leftSign < 0 ? -comparison : comparison; |
| | | 728 | | } |
| | | 729 | | |
| | | 730 | | private static int CompareDepthToTwiceRaw( |
| | | 731 | | ContactAxis axis, |
| | | 732 | | Fixed64 radius, |
| | | 733 | | Signed192 twiceRaw) |
| | | 734 | | { |
| | 8515 | 735 | | Signed192 threshold = Signed192.NarrowValue(WideArithmetic.MultiplySigned192( |
| | 8515 | 736 | | WideArithmetic.SubtractSigned192( |
| | 8515 | 737 | | twiceRaw, |
| | 8515 | 738 | | WideArithmetic.AddSigned192(Signed192.Raw(radius), Signed192.Raw(radius))), |
| | 8515 | 739 | | Signed192.One)); |
| | 8515 | 740 | | int overlapSign = axis.Overlap.Sign; |
| | 8515 | 741 | | int thresholdSign = threshold.Sign; |
| | 8515 | 742 | | if (overlapSign != thresholdSign) |
| | 2694 | 743 | | return overlapSign.CompareTo(thresholdSign); |
| | 5821 | 744 | | Signed576 left = WideArithmetic.MultiplySigned320(axis.Overlap, axis.Overlap); |
| | 5821 | 745 | | Signed576 right = WideArithmetic.MultiplySigned320( |
| | 5821 | 746 | | WideArithmetic.MultiplySigned192(threshold, threshold), axis.SquaredLength); |
| | 5821 | 747 | | int comparison = WideArithmetic.SubtractSigned576(left, right).Sign; |
| | 5821 | 748 | | return overlapSign < 0 ? -comparison : comparison; |
| | | 749 | | } |
| | | 750 | | |
| | | 751 | | private static Fixed64 GetDepth(ContactAxis axis, Fixed64 radius, out bool isClamped) |
| | | 752 | | { |
| | 525 | 753 | | Signed192 twiceMaximum = WideArithmetic.AddSigned192( |
| | 525 | 754 | | Signed192.Signed(long.MaxValue), Signed192.Signed(long.MaxValue)); |
| | 525 | 755 | | isClamped = CompareDepthToTwiceRaw(axis, radius, twiceMaximum) > 0; |
| | 525 | 756 | | if (isClamped) |
| | 13 | 757 | | return Fixed64.MaxValue; |
| | | 758 | | |
| | 512 | 759 | | Signed320 root = WideArithmetic.GetFloorSquareRootScaledByFixed64( |
| | 512 | 760 | | Signed576.ExtendValue(axis.SquaredLength)); |
| | 512 | 761 | | long first = GetDepthBound(axis.Overlap, radius, root); |
| | 512 | 762 | | long second = GetDepthBound(axis.Overlap, radius, |
| | 512 | 763 | | WideArithmetic.AddSigned320(root, Signed320.ExtendValue(Signed192.Signed(1)))); |
| | 512 | 764 | | long low = Math.Min(first, second); |
| | 512 | 765 | | long high = Math.Max(first, second); |
| | | 766 | | // The exact root lies between these adjacent integer roots. Their |
| | | 767 | | // rounded depths bound the answer, even for sub-raw axes or cancellation |
| | | 768 | | // against the radius. Only a straddled midpoint needs exact refinement. |
| | 520 | 769 | | while (low < high) |
| | | 770 | | { |
| | 8 | 771 | | long candidate = (long)(((ulong)low + (ulong)high + 1UL) >> 1); |
| | 8 | 772 | | Signed192 midpoint = WideArithmetic.SubtractSigned192( |
| | 8 | 773 | | WideArithmetic.AddSigned192( |
| | 8 | 774 | | Signed192.Signed(candidate), Signed192.Signed(candidate)), |
| | 8 | 775 | | Signed192.Signed(1)); |
| | 8 | 776 | | int comparison = CompareDepthToTwiceRaw(axis, radius, midpoint); |
| | 8 | 777 | | if (comparison > 0 || (comparison == 0 && (candidate & 1L) == 0L)) |
| | 3 | 778 | | low = candidate; |
| | | 779 | | else |
| | 5 | 780 | | high = candidate - 1L; |
| | | 781 | | } |
| | 512 | 782 | | return Fixed64.FromRaw(low); |
| | | 783 | | } |
| | | 784 | | |
| | | 785 | | private static long GetDepthBound(Signed320 overlap, Fixed64 radius, Signed320 scaledRoot) |
| | | 786 | | { |
| | 1024 | 787 | | Signed320 denominator = WideArithmetic.AddSigned320(scaledRoot, scaledRoot); |
| | 1024 | 788 | | Signed576 numerator = WideArithmetic.AddSigned576( |
| | 1024 | 789 | | Signed576.ExtendValue(overlap), |
| | 1024 | 790 | | WideArithmetic.MultiplySigned320(denominator, Signed192.Raw(radius))); |
| | 1024 | 791 | | if (numerator.Sign <= 0) |
| | 113 | 792 | | return 0L; |
| | 911 | 793 | | Signed576 maximum = WideArithmetic.MultiplySigned320( |
| | 911 | 794 | | denominator, Signed192.Signed(long.MaxValue)); |
| | 911 | 795 | | if (WideArithmetic.SubtractSigned576(numerator, maximum).Sign >= 0) |
| | 6 | 796 | | return long.MaxValue; |
| | | 797 | | |
| | | 798 | | // Clip the root bound before conversion, so nearest-even narrowing is |
| | | 799 | | // representable even when an approximation lies outside the raw domain. |
| | 905 | 800 | | _ = Fixed64.TryGetSignedRawRatio( |
| | 905 | 801 | | numerator, Signed576.ExtendValue(denominator), out Fixed64 bound); |
| | 905 | 802 | | return bound.m_rawValue; |
| | | 803 | | } |
| | | 804 | | |
| | | 805 | | private static void GetRelativeProjectionRange( |
| | | 806 | | Vector2d center, |
| | | 807 | | Vector2d convexOrigin, |
| | | 808 | | RotationFrame2d convexRotation, |
| | | 809 | | ReadOnlySpan<Vector2d> convexOriginOffsets, |
| | | 810 | | Signed192 axisX, |
| | | 811 | | Signed192 axisY, |
| | | 812 | | out Signed320 minimum, |
| | | 813 | | out Signed320 maximum) |
| | | 814 | | { |
| | 7982 | 815 | | minimum = GetRelativeProjection( |
| | 7982 | 816 | | center, convexOrigin, convexRotation, convexOriginOffsets[0], axisX, axisY); |
| | 7982 | 817 | | maximum = minimum; |
| | 62906 | 818 | | for (int i = 1; i < convexOriginOffsets.Length; i++) |
| | | 819 | | { |
| | 23471 | 820 | | Signed320 projection = GetRelativeProjection( |
| | 23471 | 821 | | center, convexOrigin, convexRotation, convexOriginOffsets[i], axisX, axisY); |
| | 23471 | 822 | | if (WideArithmetic.SubtractSigned320(projection, minimum).Sign < 0) |
| | 4159 | 823 | | minimum = projection; |
| | 19312 | 824 | | else if (WideArithmetic.SubtractSigned320(projection, maximum).Sign > 0) |
| | 5253 | 825 | | maximum = projection; |
| | | 826 | | } |
| | 7982 | 827 | | } |
| | | 828 | | |
| | | 829 | | private static Signed320 GetRelativeProjection( |
| | | 830 | | Vector2d center, |
| | | 831 | | Vector2d pointOrigin, |
| | | 832 | | RotationFrame2d pointRotation, |
| | | 833 | | Vector2d pointOriginOffset, |
| | | 834 | | Signed192 axisX, |
| | | 835 | | Signed192 axisY) |
| | | 836 | | { |
| | 32213 | 837 | | GetRelativePointNumerators( |
| | 32213 | 838 | | pointOrigin, pointRotation, pointOriginOffset, center, |
| | 32213 | 839 | | out Signed192 differenceX, out Signed192 differenceY); |
| | 32213 | 840 | | return WideArithmetic.AddSigned320( |
| | 32213 | 841 | | WideArithmetic.MultiplySigned192(differenceX, axisX), |
| | 32213 | 842 | | WideArithmetic.MultiplySigned192(differenceY, axisY)); |
| | | 843 | | } |
| | | 844 | | private static void GetRelativePointNumerators( |
| | | 845 | | Vector2d origin, |
| | | 846 | | RotationFrame2d rotation, |
| | | 847 | | Vector2d offset, |
| | | 848 | | Vector2d center, |
| | | 849 | | out Signed192 x, |
| | | 850 | | out Signed192 y) |
| | | 851 | | { |
| | 35758 | 852 | | WideConvex2dRelations.GetRotatedOffset(rotation, offset, out x, out y); |
| | 35758 | 853 | | x = WideArithmetic.AddSigned192(x, WideArithmetic.SubtractSigned192( |
| | 35758 | 854 | | WideArithmetic.Scale(origin.X), WideArithmetic.Scale(center.X))); |
| | 35758 | 855 | | y = WideArithmetic.AddSigned192(y, WideArithmetic.SubtractSigned192( |
| | 35758 | 856 | | WideArithmetic.Scale(origin.Y), WideArithmetic.Scale(center.Y))); |
| | 35758 | 857 | | } |
| | | 858 | | |
| | | 859 | | } |