| | | 1 | | //======================================================================= |
| | | 2 | | // FixedConvex2dRelations.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 | | /// Provides exact relations for convex 2D shapes represented by canonical |
| | | 14 | | /// origins, scalar rotations, and local vertex offsets. |
| | | 15 | | /// </summary> |
| | | 16 | | /// <remarks> |
| | | 17 | | /// Boundary-ordered offsets may repeat or be collinear. Closed containment and |
| | | 18 | | /// contact queries retain their finite convex hull: a segment for collinear |
| | | 19 | | /// offsets, or a point when all offsets coincide. At least three offsets are required. |
| | | 20 | | /// </remarks> |
| | | 21 | | /// <content>Owns exact planar convex contacts, queries and translated shape intersections.</content> |
| | | 22 | | public static partial class FixedConvex2dRelations |
| | | 23 | | { |
| | | 24 | | /// <summary> |
| | | 25 | | /// Returns whether boundary-ordered vertices form a strictly convex |
| | | 26 | | /// polygon in either winding direction. |
| | | 27 | | /// </summary> |
| | | 28 | | public static bool IsStrictlyConvex( |
| | | 29 | | ReadOnlySpan<Vector2d> convexVertexOffsets) |
| | | 30 | | { |
| | 5 | 31 | | ValidateVertexOffsets(convexVertexOffsets); |
| | 5 | 32 | | return WideConvex2dRelations.IsStrictlyConvex(convexVertexOffsets); |
| | | 33 | | } |
| | | 34 | | |
| | | 35 | | /// <summary> |
| | | 36 | | /// Returns whether a world-space point lies inside or on a closed convex |
| | | 37 | | /// polygon represented by an origin and origin-relative vertices. |
| | | 38 | | /// </summary> |
| | | 39 | | public static bool ContainsPoint( |
| | | 40 | | Vector2d point, |
| | | 41 | | Vector2d convexOrigin, |
| | | 42 | | ReadOnlySpan<Vector2d> convexVertexOffsets) |
| | | 43 | | { |
| | 92 | 44 | | ValidateVertexOffsets(convexVertexOffsets); |
| | 92 | 45 | | return WideConvex2dRelations.ContainsPoint( |
| | 92 | 46 | | point, |
| | 92 | 47 | | convexOrigin, |
| | 92 | 48 | | Fixed64.Zero, |
| | 92 | 49 | | convexVertexOffsets); |
| | | 50 | | } |
| | | 51 | | |
| | | 52 | | /// <summary> |
| | | 53 | | /// Returns whether a world-space point lies inside or on a rotated closed |
| | | 54 | | /// convex polygon represented by a frame and local vertex offsets. |
| | | 55 | | /// </summary> |
| | | 56 | | public static bool ContainsPoint( |
| | | 57 | | Vector2d point, |
| | | 58 | | Vector2d convexOrigin, |
| | | 59 | | Fixed64 convexRotation, |
| | | 60 | | ReadOnlySpan<Vector2d> convexVertexOffsets) |
| | | 61 | | { |
| | 411 | 62 | | ValidateVertexOffsets(convexVertexOffsets); |
| | 411 | 63 | | return WideConvex2dRelations.ContainsPoint( |
| | 411 | 64 | | point, |
| | 411 | 65 | | convexOrigin, |
| | 411 | 66 | | convexRotation, |
| | 411 | 67 | | convexVertexOffsets); |
| | | 68 | | } |
| | | 69 | | |
| | | 70 | | /// <summary> |
| | | 71 | | /// Finds the polygon-relative closest point to a world-space point without |
| | | 72 | | /// materializing polygon vertices. |
| | | 73 | | /// </summary> |
| | | 74 | | public static Vector2d GetClosestPointOffset( |
| | | 75 | | Vector2d point, |
| | | 76 | | Vector2d convexOrigin, |
| | | 77 | | ReadOnlySpan<Vector2d> convexVertexOffsets) |
| | | 78 | | { |
| | 1 | 79 | | ValidateVertexOffsets(convexVertexOffsets); |
| | 1 | 80 | | return WideConvex2dRelations.GetClosestPointOffset( |
| | 1 | 81 | | point, |
| | 1 | 82 | | Vector2d.Zero, |
| | 1 | 83 | | convexOrigin, |
| | 1 | 84 | | convexVertexOffsets); |
| | | 85 | | } |
| | | 86 | | |
| | | 87 | | /// <summary> |
| | | 88 | | /// Finds the closest point on a rotated polygon while retaining that point |
| | | 89 | | /// in the polygon's supplied local frame. |
| | | 90 | | /// </summary> |
| | | 91 | | public static FixedPointAnchor2d GetClosestPointAnchor( |
| | | 92 | | Vector2d point, |
| | | 93 | | Vector2d convexOrigin, |
| | | 94 | | Fixed64 convexRotation, |
| | | 95 | | ReadOnlySpan<Vector2d> convexVertexOffsets) |
| | | 96 | | { |
| | 1 | 97 | | ValidateVertexOffsets(convexVertexOffsets); |
| | 1 | 98 | | return new FixedPointAnchor2d( |
| | 1 | 99 | | convexOrigin, |
| | 1 | 100 | | convexRotation, |
| | 1 | 101 | | WideConvex2dRelations.GetClosestPointOffset( |
| | 1 | 102 | | point, |
| | 1 | 103 | | Vector2d.Zero, |
| | 1 | 104 | | convexOrigin, |
| | 1 | 105 | | convexRotation, |
| | 1 | 106 | | convexVertexOffsets)); |
| | | 107 | | } |
| | | 108 | | |
| | | 109 | | /// <summary> |
| | | 110 | | /// Returns the first authored polygon-relative vertex with the greatest |
| | | 111 | | /// exact projection along a direction. |
| | | 112 | | /// </summary> |
| | | 113 | | public static Vector2d GetSupportOffset( |
| | | 114 | | ReadOnlySpan<Vector2d> convexVertexOffsets, |
| | | 115 | | Vector2d direction) |
| | | 116 | | { |
| | 2 | 117 | | ValidateVertexOffsets(convexVertexOffsets); |
| | 2 | 118 | | return WideConvex2dRelations.GetSupportOffset( |
| | 2 | 119 | | convexVertexOffsets, |
| | 2 | 120 | | direction); |
| | | 121 | | } |
| | | 122 | | |
| | | 123 | | /// <summary> |
| | | 124 | | /// Returns the first local vertex with the greatest exact world-space |
| | | 125 | | /// projection along a direction. |
| | | 126 | | /// </summary> |
| | | 127 | | public static FixedPointAnchor2d GetSupportAnchor( |
| | | 128 | | Vector2d convexOrigin, |
| | | 129 | | Fixed64 convexRotation, |
| | | 130 | | ReadOnlySpan<Vector2d> convexVertexOffsets, |
| | | 131 | | Vector2d direction) |
| | | 132 | | { |
| | 1 | 133 | | ValidateVertexOffsets(convexVertexOffsets); |
| | 1 | 134 | | return new FixedPointAnchor2d( |
| | 1 | 135 | | convexOrigin, |
| | 1 | 136 | | convexRotation, |
| | 1 | 137 | | WideConvex2dRelations.GetSupportOffset( |
| | 1 | 138 | | convexRotation, |
| | 1 | 139 | | convexVertexOffsets, |
| | 1 | 140 | | direction)); |
| | | 141 | | } |
| | | 142 | | |
| | | 143 | | /// <summary> |
| | | 144 | | /// Attempts to derive the area and centroid of a boundary-ordered convex |
| | | 145 | | /// polygon without narrowing cross products or weighted-coordinate sums. |
| | | 146 | | /// </summary> |
| | | 147 | | /// <remarks> |
| | | 148 | | /// Area is nonnegative and clamps to <see cref="Fixed64.MaxValue"/> when |
| | | 149 | | /// the conceptual value exceeds the scalar domain. The centroid uses one |
| | | 150 | | /// final round-half-to-even conversion per component. |
| | | 151 | | /// </remarks> |
| | | 152 | | /// <returns> |
| | | 153 | | /// <see langword="false"/> only when the polygon has exact zero signed |
| | | 154 | | /// area or its final centroid is outside the scalar domain. |
| | | 155 | | /// </returns> |
| | | 156 | | public static bool TryGetAreaAndCentroid( |
| | | 157 | | ReadOnlySpan<Vector2d> convexVertexOffsets, |
| | | 158 | | out Fixed64 area, |
| | | 159 | | out Vector2d centroid) |
| | | 160 | | { |
| | 133 | 161 | | ValidateVertexOffsets(convexVertexOffsets); |
| | 133 | 162 | | return WideConvex2dRelations.TryGetAreaAndCentroid( |
| | 133 | 163 | | convexVertexOffsets, |
| | 133 | 164 | | out area, |
| | 133 | 165 | | out centroid); |
| | | 166 | | } |
| | | 167 | | |
| | | 168 | | /// <summary> |
| | | 169 | | /// Attempts to build up to two local-frame contact-anchor pairs between |
| | | 170 | | /// two rotated closed convex polygons. |
| | | 171 | | /// </summary> |
| | | 172 | | public static bool TryGetConvexContacts( |
| | | 173 | | Vector2d firstOrigin, |
| | | 174 | | Fixed64 firstRotation, |
| | | 175 | | ReadOnlySpan<Vector2d> firstVertexOffsets, |
| | | 176 | | Vector2d secondOrigin, |
| | | 177 | | Fixed64 secondRotation, |
| | | 178 | | ReadOnlySpan<Vector2d> secondVertexOffsets, |
| | | 179 | | Span<FixedPointAnchor2d> firstContacts, |
| | | 180 | | Span<FixedPointAnchor2d> secondContacts, |
| | | 181 | | out int contactCount, |
| | | 182 | | out Vector2d normal, |
| | | 183 | | out Fixed64 depth, |
| | | 184 | | out bool depthIsClamped) |
| | | 185 | | { |
| | 189 | 186 | | ValidateVertexOffsets(firstVertexOffsets); |
| | 189 | 187 | | ValidateVertexOffsets(secondVertexOffsets); |
| | 189 | 188 | | if (firstContacts.Length < 2) |
| | | 189 | | { |
| | 1 | 190 | | throw new ArgumentException( |
| | 1 | 191 | | "Contact output requires capacity for two anchors.", |
| | 1 | 192 | | nameof(firstContacts)); |
| | | 193 | | } |
| | 188 | 194 | | if (secondContacts.Length < 2) |
| | | 195 | | { |
| | 1 | 196 | | throw new ArgumentException( |
| | 1 | 197 | | "Contact output requires capacity for two anchors.", |
| | 1 | 198 | | nameof(secondContacts)); |
| | | 199 | | } |
| | | 200 | | |
| | 187 | 201 | | Span<Vector2d> firstOffsets = stackalloc Vector2d[2]; |
| | 187 | 202 | | Span<Vector2d> secondOffsets = stackalloc Vector2d[2]; |
| | 187 | 203 | | if (!WideConvex2dRelations.TryGetContactOffsets( |
| | 187 | 204 | | firstOrigin, |
| | 187 | 205 | | firstRotation, |
| | 187 | 206 | | firstVertexOffsets, |
| | 187 | 207 | | secondOrigin, |
| | 187 | 208 | | secondRotation, |
| | 187 | 209 | | secondVertexOffsets, |
| | 187 | 210 | | firstOffsets, |
| | 187 | 211 | | secondOffsets, |
| | 187 | 212 | | out contactCount, |
| | 187 | 213 | | out normal, |
| | 187 | 214 | | out depth, |
| | 187 | 215 | | out depthIsClamped)) |
| | | 216 | | { |
| | 22 | 217 | | return false; |
| | | 218 | | } |
| | | 219 | | |
| | 690 | 220 | | for (int i = 0; i < contactCount; i++) |
| | | 221 | | { |
| | 180 | 222 | | firstContacts[i] = new FixedPointAnchor2d( |
| | 180 | 223 | | firstOrigin, |
| | 180 | 224 | | firstRotation, |
| | 180 | 225 | | firstOffsets[i]); |
| | 180 | 226 | | secondContacts[i] = new FixedPointAnchor2d( |
| | 180 | 227 | | secondOrigin, |
| | 180 | 228 | | secondRotation, |
| | 180 | 229 | | secondOffsets[i]); |
| | | 230 | | } |
| | 165 | 231 | | return true; |
| | | 232 | | } |
| | | 233 | | |
| | | 234 | | /// <summary> |
| | | 235 | | /// Attempts to build a contact-anchor pair between a conceptual circle and |
| | | 236 | | /// a rotated closed convex polygon. |
| | | 237 | | /// </summary> |
| | | 238 | | /// <remarks> |
| | | 239 | | /// A circle's rotation does not change its geometry, but it defines the |
| | | 240 | | /// rigid local frame of the returned circle anchor. |
| | | 241 | | /// </remarks> |
| | | 242 | | public static bool TryGetCircleContact( |
| | | 243 | | Vector2d circleCenter, |
| | | 244 | | Fixed64 circleRotation, |
| | | 245 | | Fixed64 circleRadius, |
| | | 246 | | Vector2d convexOrigin, |
| | | 247 | | Fixed64 convexRotation, |
| | | 248 | | ReadOnlySpan<Vector2d> convexVertexOffsets, |
| | | 249 | | out FixedPointAnchor2d circleContact, |
| | | 250 | | out FixedPointAnchor2d convexContact, |
| | | 251 | | out Vector2d normal, |
| | | 252 | | out Fixed64 depth, |
| | | 253 | | out bool depthIsClamped) |
| | | 254 | | { |
| | 210 | 255 | | if (circleRadius < Fixed64.Zero) |
| | 1 | 256 | | throw new ArgumentOutOfRangeException(nameof(circleRadius)); |
| | 209 | 257 | | ValidateVertexOffsets(convexVertexOffsets); |
| | | 258 | | |
| | 208 | 259 | | Span<FixedPointAnchor2d> circleContacts = |
| | 208 | 260 | | stackalloc FixedPointAnchor2d[2]; |
| | 208 | 261 | | Span<FixedPointAnchor2d> convexContacts = |
| | 208 | 262 | | stackalloc FixedPointAnchor2d[2]; |
| | 208 | 263 | | if (!WideCenteredCapsule2dRelations.TryGetContacts( |
| | 208 | 264 | | circleCenter, |
| | 208 | 265 | | circleRotation, |
| | 208 | 266 | | Vector2d.Right, |
| | 208 | 267 | | Fixed64.Zero, |
| | 208 | 268 | | circleRadius, |
| | 208 | 269 | | convexOrigin, |
| | 208 | 270 | | convexRotation, |
| | 208 | 271 | | convexVertexOffsets, |
| | 208 | 272 | | circleContacts, |
| | 208 | 273 | | convexContacts, |
| | 208 | 274 | | out _, |
| | 208 | 275 | | out normal, |
| | 208 | 276 | | out depth, |
| | 208 | 277 | | out depthIsClamped)) |
| | | 278 | | { |
| | 113 | 279 | | circleContact = default; |
| | 113 | 280 | | convexContact = default; |
| | 113 | 281 | | return false; |
| | | 282 | | } |
| | | 283 | | |
| | 95 | 284 | | circleContact = circleContacts[0]; |
| | 95 | 285 | | convexContact = convexContacts[0]; |
| | 95 | 286 | | return true; |
| | | 287 | | } |
| | | 288 | | |
| | | 289 | | private static void ValidateVertexOffsets( |
| | | 290 | | ReadOnlySpan<Vector2d> convexVertexOffsets) |
| | | 291 | | { |
| | 3898 | 292 | | if (convexVertexOffsets.Length < 3) |
| | | 293 | | { |
| | 2 | 294 | | throw new ArgumentException( |
| | 2 | 295 | | "A convex polygon requires at least three vertex offsets.", |
| | 2 | 296 | | nameof(convexVertexOffsets)); |
| | | 297 | | } |
| | 3896 | 298 | | } |
| | | 299 | | |
| | | 300 | | /// <summary> |
| | | 301 | | /// Finds the first distance where a finite directed segment reaches a |
| | | 302 | | /// closed convex polygon. |
| | | 303 | | /// </summary> |
| | | 304 | | public static bool TryGetSegmentFirstIntersectionDistance( |
| | | 305 | | Vector2d start, |
| | | 306 | | Vector2d direction, |
| | | 307 | | Fixed64 maximumDistance, |
| | | 308 | | Vector2d convexOrigin, |
| | | 309 | | ReadOnlySpan<Vector2d> convexVertexOffsets, |
| | | 310 | | out Fixed64 distance, |
| | | 311 | | out Vector2d normal, |
| | | 312 | | out Vector2d convexContactOffset) |
| | | 313 | | { |
| | 174 | 314 | | ValidateSweep(direction, maximumDistance); |
| | 174 | 315 | | ValidateVertexOffsets(convexVertexOffsets); |
| | 174 | 316 | | if (WideConvex2dRelations.ContainsPoint( |
| | 174 | 317 | | start, |
| | 174 | 318 | | convexOrigin, |
| | 174 | 319 | | Fixed64.Zero, |
| | 174 | 320 | | convexVertexOffsets)) |
| | | 321 | | { |
| | 1 | 322 | | distance = Fixed64.Zero; |
| | 1 | 323 | | normal = -direction; |
| | 1 | 324 | | convexContactOffset = |
| | 1 | 325 | | WideConvex2dRelations.GetSweptPointTargetContactOffset( |
| | 1 | 326 | | start, |
| | 1 | 327 | | Vector2d.Zero, |
| | 1 | 328 | | Vector2d.Zero, |
| | 1 | 329 | | convexOrigin, |
| | 1 | 330 | | convexVertexOffsets, |
| | 1 | 331 | | normal); |
| | 1 | 332 | | return true; |
| | | 333 | | } |
| | | 334 | | |
| | 173 | 335 | | return TryGetSweptCenteredCapsuleFirstDistanceCore( |
| | 173 | 336 | | start, |
| | 173 | 337 | | Vector2d.Right, |
| | 173 | 338 | | Fixed64.Zero, |
| | 173 | 339 | | Fixed64.Zero, |
| | 173 | 340 | | direction, |
| | 173 | 341 | | maximumDistance, |
| | 173 | 342 | | convexOrigin, |
| | 173 | 343 | | convexVertexOffsets, |
| | 173 | 344 | | out distance, |
| | 173 | 345 | | out normal, |
| | 173 | 346 | | out convexContactOffset); |
| | | 347 | | } |
| | | 348 | | |
| | | 349 | | /// <summary> |
| | | 350 | | /// Finds the first distance where a finite directed segment reaches a |
| | | 351 | | /// rotated closed convex polygon. |
| | | 352 | | /// </summary> |
| | | 353 | | public static bool TryGetSegmentFirstIntersectionDistance( |
| | | 354 | | Vector2d start, |
| | | 355 | | Vector2d direction, |
| | | 356 | | Fixed64 maximumDistance, |
| | | 357 | | Vector2d convexOrigin, |
| | | 358 | | Fixed64 convexRotation, |
| | | 359 | | ReadOnlySpan<Vector2d> convexVertexOffsets, |
| | | 360 | | out Fixed64 distance, |
| | | 361 | | out Vector2d normal, |
| | | 362 | | out FixedPointAnchor2d convexContact) |
| | | 363 | | { |
| | 5 | 364 | | ValidateSweep(direction, maximumDistance); |
| | 5 | 365 | | ValidateVertexOffsets(convexVertexOffsets); |
| | | 366 | | Vector2d contactOffset; |
| | 5 | 367 | | if (WideConvex2dRelations.ContainsPoint( |
| | 5 | 368 | | start, |
| | 5 | 369 | | convexOrigin, |
| | 5 | 370 | | convexRotation, |
| | 5 | 371 | | convexVertexOffsets)) |
| | | 372 | | { |
| | 1 | 373 | | distance = Fixed64.Zero; |
| | 1 | 374 | | normal = -direction; |
| | 1 | 375 | | contactOffset = |
| | 1 | 376 | | WideConvex2dRelations.GetSweptPointTargetContactOffset( |
| | 1 | 377 | | start, |
| | 1 | 378 | | Vector2d.Zero, |
| | 1 | 379 | | Vector2d.Zero, |
| | 1 | 380 | | convexOrigin, |
| | 1 | 381 | | convexRotation, |
| | 1 | 382 | | convexVertexOffsets, |
| | 1 | 383 | | normal); |
| | | 384 | | } |
| | 4 | 385 | | else if (!TryGetSweptCenteredCapsuleFirstDistanceCore( |
| | 4 | 386 | | start, |
| | 4 | 387 | | Vector2d.Right, |
| | 4 | 388 | | Fixed64.Zero, |
| | 4 | 389 | | Fixed64.Zero, |
| | 4 | 390 | | direction, |
| | 4 | 391 | | maximumDistance, |
| | 4 | 392 | | convexOrigin, |
| | 4 | 393 | | convexRotation, |
| | 4 | 394 | | convexVertexOffsets, |
| | 4 | 395 | | out distance, |
| | 4 | 396 | | out normal, |
| | 4 | 397 | | out contactOffset)) |
| | | 398 | | { |
| | 1 | 399 | | convexContact = default; |
| | 1 | 400 | | return false; |
| | | 401 | | } |
| | | 402 | | |
| | 4 | 403 | | convexContact = new FixedPointAnchor2d( |
| | 4 | 404 | | convexOrigin, |
| | 4 | 405 | | convexRotation, |
| | 4 | 406 | | contactOffset); |
| | 4 | 407 | | return true; |
| | | 408 | | } |
| | | 409 | | |
| | | 410 | | /// <summary> |
| | | 411 | | /// Finds the first distance where a translated conceptual circle reaches a |
| | | 412 | | /// closed convex polygon. |
| | | 413 | | /// </summary> |
| | | 414 | | public static bool TryGetSweptCircleFirstDistance( |
| | | 415 | | Vector2d circleCenter, |
| | | 416 | | Fixed64 circleRadius, |
| | | 417 | | Vector2d direction, |
| | | 418 | | Fixed64 maximumDistance, |
| | | 419 | | Vector2d convexOrigin, |
| | | 420 | | ReadOnlySpan<Vector2d> convexVertexOffsets, |
| | | 421 | | out Fixed64 distance, |
| | | 422 | | out Vector2d normal, |
| | | 423 | | out Vector2d convexContactOffset) |
| | | 424 | | { |
| | 86 | 425 | | if (circleRadius < Fixed64.Zero) |
| | 1 | 426 | | throw new ArgumentOutOfRangeException(nameof(circleRadius)); |
| | 85 | 427 | | ValidateSweep(direction, maximumDistance); |
| | 85 | 428 | | ValidateVertexOffsets(convexVertexOffsets); |
| | 85 | 429 | | if (TryGetCircleContact( |
| | 85 | 430 | | circleCenter, |
| | 85 | 431 | | Fixed64.Zero, |
| | 85 | 432 | | circleRadius, |
| | 85 | 433 | | convexOrigin, |
| | 85 | 434 | | Fixed64.Zero, |
| | 85 | 435 | | convexVertexOffsets, |
| | 85 | 436 | | out _, |
| | 85 | 437 | | out FixedPointAnchor2d initialContact, |
| | 85 | 438 | | out normal, |
| | 85 | 439 | | out _, |
| | 85 | 440 | | out _)) |
| | | 441 | | { |
| | 2 | 442 | | distance = Fixed64.Zero; |
| | 2 | 443 | | convexContactOffset = initialContact.LocalPoint; |
| | 2 | 444 | | return true; |
| | | 445 | | } |
| | | 446 | | |
| | 83 | 447 | | return TryGetSweptCenteredCapsuleFirstDistanceCore( |
| | 83 | 448 | | circleCenter, |
| | 83 | 449 | | Vector2d.Right, |
| | 83 | 450 | | Fixed64.Zero, |
| | 83 | 451 | | circleRadius, |
| | 83 | 452 | | direction, |
| | 83 | 453 | | maximumDistance, |
| | 83 | 454 | | convexOrigin, |
| | 83 | 455 | | convexVertexOffsets, |
| | 83 | 456 | | out distance, |
| | 83 | 457 | | out normal, |
| | 83 | 458 | | out convexContactOffset); |
| | | 459 | | } |
| | | 460 | | |
| | | 461 | | /// <summary> |
| | | 462 | | /// Finds the first distance where a translated conceptual circle reaches |
| | | 463 | | /// a rotated closed convex polygon. |
| | | 464 | | /// </summary> |
| | | 465 | | public static bool TryGetSweptCircleFirstDistance( |
| | | 466 | | Vector2d circleCenter, |
| | | 467 | | Fixed64 circleRadius, |
| | | 468 | | Vector2d direction, |
| | | 469 | | Fixed64 maximumDistance, |
| | | 470 | | Vector2d convexOrigin, |
| | | 471 | | Fixed64 convexRotation, |
| | | 472 | | ReadOnlySpan<Vector2d> convexVertexOffsets, |
| | | 473 | | out Fixed64 distance, |
| | | 474 | | out Vector2d normal, |
| | | 475 | | out FixedPointAnchor2d convexContact) |
| | | 476 | | { |
| | 20 | 477 | | if (circleRadius < Fixed64.Zero) |
| | 1 | 478 | | throw new ArgumentOutOfRangeException(nameof(circleRadius)); |
| | 19 | 479 | | ValidateSweep(direction, maximumDistance); |
| | 19 | 480 | | ValidateVertexOffsets(convexVertexOffsets); |
| | | 481 | | Vector2d contactOffset; |
| | 19 | 482 | | if (TryGetCircleContact( |
| | 19 | 483 | | circleCenter, |
| | 19 | 484 | | Fixed64.Zero, |
| | 19 | 485 | | circleRadius, |
| | 19 | 486 | | convexOrigin, |
| | 19 | 487 | | convexRotation, |
| | 19 | 488 | | convexVertexOffsets, |
| | 19 | 489 | | out _, |
| | 19 | 490 | | out FixedPointAnchor2d initialContact, |
| | 19 | 491 | | out normal, |
| | 19 | 492 | | out _, |
| | 19 | 493 | | out _)) |
| | | 494 | | { |
| | 1 | 495 | | distance = Fixed64.Zero; |
| | 1 | 496 | | convexContact = initialContact; |
| | 1 | 497 | | return true; |
| | | 498 | | } |
| | 18 | 499 | | if (!TryGetSweptCenteredCapsuleFirstDistanceCore( |
| | 18 | 500 | | circleCenter, |
| | 18 | 501 | | Vector2d.Right, |
| | 18 | 502 | | Fixed64.Zero, |
| | 18 | 503 | | circleRadius, |
| | 18 | 504 | | direction, |
| | 18 | 505 | | maximumDistance, |
| | 18 | 506 | | convexOrigin, |
| | 18 | 507 | | convexRotation, |
| | 18 | 508 | | convexVertexOffsets, |
| | 18 | 509 | | out distance, |
| | 18 | 510 | | out normal, |
| | 18 | 511 | | out contactOffset)) |
| | | 512 | | { |
| | 1 | 513 | | convexContact = default; |
| | 1 | 514 | | return false; |
| | | 515 | | } |
| | | 516 | | |
| | 17 | 517 | | convexContact = new FixedPointAnchor2d( |
| | 17 | 518 | | convexOrigin, |
| | 17 | 519 | | convexRotation, |
| | 17 | 520 | | contactOffset); |
| | 17 | 521 | | return true; |
| | | 522 | | } |
| | | 523 | | |
| | | 524 | | /// <summary> |
| | | 525 | | /// Finds the first distance where a translated centered rotated capsule |
| | | 526 | | /// reaches a rotated closed convex polygon. |
| | | 527 | | /// </summary> |
| | | 528 | | public static bool TryGetSweptCenteredCapsuleFirstDistance( |
| | | 529 | | Vector2d capsuleCenter, |
| | | 530 | | Fixed64 capsuleRotation, |
| | | 531 | | Fixed64 capsuleAxisLength, |
| | | 532 | | Fixed64 capsuleRadius, |
| | | 533 | | Vector2d direction, |
| | | 534 | | Fixed64 maximumDistance, |
| | | 535 | | Vector2d convexOrigin, |
| | | 536 | | Fixed64 convexRotation, |
| | | 537 | | ReadOnlySpan<Vector2d> convexVertexOffsets, |
| | | 538 | | out Fixed64 distance, |
| | | 539 | | out Vector2d normal, |
| | | 540 | | out FixedPointAnchor2d convexContact) |
| | | 541 | | { |
| | 5 | 542 | | if (capsuleAxisLength < Fixed64.Zero) |
| | 1 | 543 | | throw new ArgumentOutOfRangeException(nameof(capsuleAxisLength)); |
| | 4 | 544 | | if (capsuleRadius < Fixed64.Zero) |
| | 1 | 545 | | throw new ArgumentOutOfRangeException(nameof(capsuleRadius)); |
| | 3 | 546 | | ValidateSweep(direction, maximumDistance); |
| | 3 | 547 | | ValidateVertexOffsets(convexVertexOffsets); |
| | | 548 | | |
| | 3 | 549 | | Span<FixedPointAnchor2d> capsuleContacts = |
| | 3 | 550 | | stackalloc FixedPointAnchor2d[2]; |
| | 3 | 551 | | Span<FixedPointAnchor2d> convexContacts = |
| | 3 | 552 | | stackalloc FixedPointAnchor2d[2]; |
| | 3 | 553 | | if (FixedSegment2d.TryGetCenteredCapsuleConvexContacts( |
| | 3 | 554 | | capsuleCenter, |
| | 3 | 555 | | capsuleRotation, |
| | 3 | 556 | | Vector2d.Forward, |
| | 3 | 557 | | capsuleAxisLength, |
| | 3 | 558 | | capsuleRadius, |
| | 3 | 559 | | convexOrigin, |
| | 3 | 560 | | convexRotation, |
| | 3 | 561 | | convexVertexOffsets, |
| | 3 | 562 | | capsuleContacts, |
| | 3 | 563 | | convexContacts, |
| | 3 | 564 | | out int contactCount, |
| | 3 | 565 | | out normal, |
| | 3 | 566 | | out _, |
| | 3 | 567 | | out _)) |
| | | 568 | | { |
| | 1 | 569 | | distance = Fixed64.Zero; |
| | 1 | 570 | | convexContact = convexContacts[contactCount - 1]; |
| | 1 | 571 | | return true; |
| | | 572 | | } |
| | | 573 | | |
| | 2 | 574 | | Vector2d capsuleAxis = new( |
| | 2 | 575 | | -FixedMath.Sin(capsuleRotation), |
| | 2 | 576 | | FixedMath.Cos(capsuleRotation)); |
| | 2 | 577 | | if (!TryGetSweptCenteredCapsuleFirstDistanceCore( |
| | 2 | 578 | | capsuleCenter, |
| | 2 | 579 | | capsuleAxis, |
| | 2 | 580 | | capsuleAxisLength, |
| | 2 | 581 | | capsuleRadius, |
| | 2 | 582 | | direction, |
| | 2 | 583 | | maximumDistance, |
| | 2 | 584 | | convexOrigin, |
| | 2 | 585 | | convexRotation, |
| | 2 | 586 | | convexVertexOffsets, |
| | 2 | 587 | | out distance, |
| | 2 | 588 | | out normal, |
| | 2 | 589 | | out Vector2d contactOffset)) |
| | | 590 | | { |
| | 1 | 591 | | convexContact = default; |
| | 1 | 592 | | return false; |
| | | 593 | | } |
| | | 594 | | |
| | 1 | 595 | | convexContact = new FixedPointAnchor2d( |
| | 1 | 596 | | convexOrigin, |
| | 1 | 597 | | convexRotation, |
| | 1 | 598 | | contactOffset); |
| | 1 | 599 | | return true; |
| | | 600 | | } |
| | | 601 | | |
| | | 602 | | /// <summary> |
| | | 603 | | /// Finds the first distance where a translated conceptual centered |
| | | 604 | | /// capsule reaches a closed convex polygon. |
| | | 605 | | /// </summary> |
| | | 606 | | public static bool TryGetSweptCenteredCapsuleFirstDistance( |
| | | 607 | | Vector2d capsuleCenter, |
| | | 608 | | Vector2d capsuleAxisDirection, |
| | | 609 | | Fixed64 capsuleAxisLength, |
| | | 610 | | Fixed64 capsuleRadius, |
| | | 611 | | Vector2d direction, |
| | | 612 | | Fixed64 maximumDistance, |
| | | 613 | | Vector2d convexOrigin, |
| | | 614 | | ReadOnlySpan<Vector2d> convexVertexOffsets, |
| | | 615 | | out Fixed64 distance, |
| | | 616 | | out Vector2d normal, |
| | | 617 | | out Vector2d convexContactOffset) |
| | | 618 | | { |
| | 122 | 619 | | ValidateCenteredAxis( |
| | 122 | 620 | | capsuleAxisDirection, |
| | 122 | 621 | | capsuleAxisLength); |
| | 120 | 622 | | if (capsuleRadius < Fixed64.Zero) |
| | 1 | 623 | | throw new ArgumentOutOfRangeException(nameof(capsuleRadius)); |
| | 119 | 624 | | ValidateSweep(direction, maximumDistance); |
| | 119 | 625 | | ValidateVertexOffsets(convexVertexOffsets); |
| | 119 | 626 | | Span<FixedPointAnchor2d> capsuleContacts = |
| | 119 | 627 | | stackalloc FixedPointAnchor2d[2]; |
| | 119 | 628 | | Span<FixedPointAnchor2d> convexContacts = |
| | 119 | 629 | | stackalloc FixedPointAnchor2d[2]; |
| | 119 | 630 | | if (WideCenteredCapsule2dRelations.TryGetContacts( |
| | 119 | 631 | | capsuleCenter, |
| | 119 | 632 | | Fixed64.Zero, |
| | 119 | 633 | | capsuleAxisDirection, |
| | 119 | 634 | | capsuleAxisLength, |
| | 119 | 635 | | capsuleRadius, |
| | 119 | 636 | | convexOrigin, |
| | 119 | 637 | | Fixed64.Zero, |
| | 119 | 638 | | convexVertexOffsets, |
| | 119 | 639 | | capsuleContacts, |
| | 119 | 640 | | convexContacts, |
| | 119 | 641 | | out int contactCount, |
| | 119 | 642 | | out normal, |
| | 119 | 643 | | out _, |
| | 119 | 644 | | out _)) |
| | | 645 | | { |
| | 2 | 646 | | distance = Fixed64.Zero; |
| | 2 | 647 | | convexContactOffset = |
| | 2 | 648 | | convexContacts[contactCount - 1].LocalPoint; |
| | 2 | 649 | | return true; |
| | | 650 | | } |
| | | 651 | | |
| | 117 | 652 | | return TryGetSweptCenteredCapsuleFirstDistanceCore( |
| | 117 | 653 | | capsuleCenter, |
| | 117 | 654 | | capsuleAxisDirection, |
| | 117 | 655 | | capsuleAxisLength, |
| | 117 | 656 | | capsuleRadius, |
| | 117 | 657 | | direction, |
| | 117 | 658 | | maximumDistance, |
| | 117 | 659 | | convexOrigin, |
| | 117 | 660 | | convexVertexOffsets, |
| | 117 | 661 | | out distance, |
| | 117 | 662 | | out normal, |
| | 117 | 663 | | out convexContactOffset); |
| | | 664 | | } |
| | | 665 | | |
| | | 666 | | /// <summary> |
| | | 667 | | /// Finds the first distance where a translated conceptual centered capsule |
| | | 668 | | /// reaches a rotated closed convex polygon. |
| | | 669 | | /// </summary> |
| | | 670 | | public static bool TryGetSweptCenteredCapsuleFirstDistance( |
| | | 671 | | Vector2d capsuleCenter, |
| | | 672 | | Vector2d capsuleAxisDirection, |
| | | 673 | | Fixed64 capsuleAxisLength, |
| | | 674 | | Fixed64 capsuleRadius, |
| | | 675 | | Vector2d direction, |
| | | 676 | | Fixed64 maximumDistance, |
| | | 677 | | Vector2d convexOrigin, |
| | | 678 | | Fixed64 convexRotation, |
| | | 679 | | ReadOnlySpan<Vector2d> convexVertexOffsets, |
| | | 680 | | out Fixed64 distance, |
| | | 681 | | out Vector2d normal, |
| | | 682 | | out FixedPointAnchor2d convexContact) |
| | | 683 | | { |
| | 24 | 684 | | ValidateCenteredAxis(capsuleAxisDirection, capsuleAxisLength); |
| | 24 | 685 | | if (capsuleRadius < Fixed64.Zero) |
| | 1 | 686 | | throw new ArgumentOutOfRangeException(nameof(capsuleRadius)); |
| | 23 | 687 | | ValidateSweep(direction, maximumDistance); |
| | 23 | 688 | | ValidateVertexOffsets(convexVertexOffsets); |
| | 23 | 689 | | Span<FixedPointAnchor2d> capsuleContacts = |
| | 23 | 690 | | stackalloc FixedPointAnchor2d[2]; |
| | 23 | 691 | | Span<FixedPointAnchor2d> convexContacts = |
| | 23 | 692 | | stackalloc FixedPointAnchor2d[2]; |
| | 23 | 693 | | if (WideCenteredCapsule2dRelations.TryGetContacts( |
| | 23 | 694 | | capsuleCenter, |
| | 23 | 695 | | Fixed64.Zero, |
| | 23 | 696 | | capsuleAxisDirection, |
| | 23 | 697 | | capsuleAxisLength, |
| | 23 | 698 | | capsuleRadius, |
| | 23 | 699 | | convexOrigin, |
| | 23 | 700 | | convexRotation, |
| | 23 | 701 | | convexVertexOffsets, |
| | 23 | 702 | | capsuleContacts, |
| | 23 | 703 | | convexContacts, |
| | 23 | 704 | | out int contactCount, |
| | 23 | 705 | | out normal, |
| | 23 | 706 | | out _, |
| | 23 | 707 | | out _)) |
| | | 708 | | { |
| | 3 | 709 | | distance = Fixed64.Zero; |
| | 3 | 710 | | convexContact = convexContacts[contactCount - 1]; |
| | 3 | 711 | | return true; |
| | | 712 | | } |
| | | 713 | | |
| | 20 | 714 | | if (!TryGetSweptCenteredCapsuleFirstDistanceCore( |
| | 20 | 715 | | capsuleCenter, |
| | 20 | 716 | | capsuleAxisDirection, |
| | 20 | 717 | | capsuleAxisLength, |
| | 20 | 718 | | capsuleRadius, |
| | 20 | 719 | | direction, |
| | 20 | 720 | | maximumDistance, |
| | 20 | 721 | | convexOrigin, |
| | 20 | 722 | | convexRotation, |
| | 20 | 723 | | convexVertexOffsets, |
| | 20 | 724 | | out distance, |
| | 20 | 725 | | out normal, |
| | 20 | 726 | | out Vector2d contactOffset)) |
| | | 727 | | { |
| | 2 | 728 | | convexContact = default; |
| | 2 | 729 | | return false; |
| | | 730 | | } |
| | | 731 | | |
| | 18 | 732 | | convexContact = new FixedPointAnchor2d( |
| | 18 | 733 | | convexOrigin, |
| | 18 | 734 | | convexRotation, |
| | 18 | 735 | | contactOffset); |
| | 18 | 736 | | return true; |
| | | 737 | | } |
| | | 738 | | |
| | | 739 | | /// <summary> |
| | | 740 | | /// Finds the first distance where a translated closed convex polygon |
| | | 741 | | /// reaches another closed convex polygon. |
| | | 742 | | /// </summary> |
| | | 743 | | public static bool TryGetSweptConvexFirstDistance( |
| | | 744 | | Vector2d moverOrigin, |
| | | 745 | | ReadOnlySpan<Vector2d> moverVertexOffsets, |
| | | 746 | | Vector2d direction, |
| | | 747 | | Fixed64 maximumDistance, |
| | | 748 | | Vector2d targetOrigin, |
| | | 749 | | ReadOnlySpan<Vector2d> targetVertexOffsets, |
| | | 750 | | out Fixed64 distance, |
| | | 751 | | out Vector2d normal, |
| | | 752 | | out Vector2d targetContactOffset) => |
| | 106 | 753 | | TryGetSweptConvexFirstDistanceCore( |
| | 106 | 754 | | moverOrigin, |
| | 106 | 755 | | Fixed64.Zero, |
| | 106 | 756 | | moverVertexOffsets, |
| | 106 | 757 | | direction, |
| | 106 | 758 | | maximumDistance, |
| | 106 | 759 | | targetOrigin, |
| | 106 | 760 | | Fixed64.Zero, |
| | 106 | 761 | | targetVertexOffsets, |
| | 106 | 762 | | out distance, |
| | 106 | 763 | | out normal, |
| | 106 | 764 | | out targetContactOffset); |
| | | 765 | | |
| | | 766 | | /// <summary> |
| | | 767 | | /// Finds the first distance where a translated rotated convex polygon |
| | | 768 | | /// reaches another rotated closed convex polygon. |
| | | 769 | | /// </summary> |
| | | 770 | | public static bool TryGetSweptConvexFirstDistance( |
| | | 771 | | Vector2d moverOrigin, |
| | | 772 | | Fixed64 moverRotation, |
| | | 773 | | ReadOnlySpan<Vector2d> moverVertexOffsets, |
| | | 774 | | Vector2d direction, |
| | | 775 | | Fixed64 maximumDistance, |
| | | 776 | | Vector2d targetOrigin, |
| | | 777 | | Fixed64 targetRotation, |
| | | 778 | | ReadOnlySpan<Vector2d> targetVertexOffsets, |
| | | 779 | | out Fixed64 distance, |
| | | 780 | | out Vector2d normal, |
| | | 781 | | out FixedPointAnchor2d targetContact) |
| | | 782 | | { |
| | 25 | 783 | | if (!TryGetSweptConvexFirstDistanceCore( |
| | 25 | 784 | | moverOrigin, |
| | 25 | 785 | | moverRotation, |
| | 25 | 786 | | moverVertexOffsets, |
| | 25 | 787 | | direction, |
| | 25 | 788 | | maximumDistance, |
| | 25 | 789 | | targetOrigin, |
| | 25 | 790 | | targetRotation, |
| | 25 | 791 | | targetVertexOffsets, |
| | 25 | 792 | | out distance, |
| | 25 | 793 | | out normal, |
| | 25 | 794 | | out Vector2d targetContactOffset)) |
| | | 795 | | { |
| | 1 | 796 | | targetContact = default; |
| | 1 | 797 | | return false; |
| | | 798 | | } |
| | | 799 | | |
| | 24 | 800 | | targetContact = new FixedPointAnchor2d( |
| | 24 | 801 | | targetOrigin, |
| | 24 | 802 | | targetRotation, |
| | 24 | 803 | | targetContactOffset); |
| | 24 | 804 | | return true; |
| | | 805 | | } |
| | | 806 | | |
| | | 807 | | // Authored mode consumes an already admitted displacement and its positive |
| | | 808 | | // representable length; public direction/distance entry points keep validation. |
| | | 809 | | internal static bool TryGetSweptConvexFirstDistanceCore( |
| | | 810 | | Vector2d moverOrigin, |
| | | 811 | | Fixed64 moverRotation, |
| | | 812 | | ReadOnlySpan<Vector2d> moverVertexOffsets, |
| | | 813 | | Vector2d direction, |
| | | 814 | | Fixed64 maximumDistance, |
| | | 815 | | Vector2d targetOrigin, |
| | | 816 | | Fixed64 targetRotation, |
| | | 817 | | ReadOnlySpan<Vector2d> targetVertexOffsets, |
| | | 818 | | out Fixed64 distance, |
| | | 819 | | out Vector2d normal, |
| | | 820 | | out Vector2d targetContactOffset, |
| | | 821 | | bool authoredDisplacement = false) |
| | | 822 | | { |
| | 132 | 823 | | ValidateVertexOffsets(moverVertexOffsets); |
| | 132 | 824 | | ValidateVertexOffsets(targetVertexOffsets); |
| | 132 | 825 | | if (!authoredDisplacement) |
| | 131 | 826 | | ValidateSweep(direction, maximumDistance); |
| | 130 | 827 | | Span<Vector2d> moverContacts = stackalloc Vector2d[2]; |
| | 130 | 828 | | Span<Vector2d> targetContacts = stackalloc Vector2d[2]; |
| | 130 | 829 | | if (WideConvex2dRelations.TryGetContactOffsets( |
| | 130 | 830 | | moverOrigin, |
| | 130 | 831 | | moverRotation, |
| | 130 | 832 | | moverVertexOffsets, |
| | 130 | 833 | | targetOrigin, |
| | 130 | 834 | | targetRotation, |
| | 130 | 835 | | targetVertexOffsets, |
| | 130 | 836 | | moverContacts, |
| | 130 | 837 | | targetContacts, |
| | 130 | 838 | | out int contactCount, |
| | 130 | 839 | | out normal, |
| | 130 | 840 | | out _, |
| | 130 | 841 | | out _)) |
| | | 842 | | { |
| | 3 | 843 | | distance = Fixed64.Zero; |
| | 3 | 844 | | targetContactOffset = targetContacts[contactCount - 1]; |
| | 3 | 845 | | return true; |
| | | 846 | | } |
| | | 847 | | |
| | 127 | 848 | | bool found = false; |
| | 127 | 849 | | distance = default; |
| | 127 | 850 | | normal = default; |
| | 127 | 851 | | targetContactOffset = default; |
| | 127 | 852 | | for (int moverIndex = 0; |
| | 629 | 853 | | moverIndex < moverVertexOffsets.Length; |
| | 502 | 854 | | moverIndex++) |
| | | 855 | | { |
| | 502 | 856 | | Vector2d moverStart = moverVertexOffsets[moverIndex]; |
| | 502 | 857 | | Vector2d moverEnd = moverVertexOffsets[ |
| | 502 | 858 | | moverIndex + 1 == moverVertexOffsets.Length |
| | 502 | 859 | | ? 0 |
| | 502 | 860 | | : moverIndex + 1]; |
| | 502 | 861 | | for (int targetIndex = 0; |
| | 2092 | 862 | | targetIndex < targetVertexOffsets.Length; |
| | 1590 | 863 | | targetIndex++) |
| | | 864 | | { |
| | 1590 | 865 | | Vector2d targetStart = targetVertexOffsets[targetIndex]; |
| | 1590 | 866 | | Vector2d targetEnd = targetVertexOffsets[ |
| | 1590 | 867 | | targetIndex + 1 == targetVertexOffsets.Length |
| | 1590 | 868 | | ? 0 |
| | 1590 | 869 | | : targetIndex + 1]; |
| | 1590 | 870 | | if (WideFiniteAxisIntersection |
| | 1590 | 871 | | .TryGetSweptOriginOffsetSegmentsFirstDistance( |
| | 1590 | 872 | | moverOrigin, |
| | 1590 | 873 | | moverRotation, |
| | 1590 | 874 | | moverStart, |
| | 1590 | 875 | | moverEnd, |
| | 1590 | 876 | | Fixed64.Zero, |
| | 1590 | 877 | | direction, |
| | 1590 | 878 | | maximumDistance, |
| | 1590 | 879 | | targetOrigin, |
| | 1590 | 880 | | targetRotation, |
| | 1590 | 881 | | targetStart, |
| | 1590 | 882 | | targetEnd, |
| | 1590 | 883 | | Fixed64.Zero, |
| | 1590 | 884 | | out Fixed64 candidateDistance, |
| | 1590 | 885 | | out _, |
| | 1590 | 886 | | authoredDisplacement)) |
| | | 887 | | { |
| | 950 | 888 | | if (!found || candidateDistance < distance) |
| | | 889 | | { |
| | 137 | 890 | | found = true; |
| | 137 | 891 | | distance = candidateDistance; |
| | | 892 | | } |
| | | 893 | | } |
| | | 894 | | } |
| | | 895 | | } |
| | | 896 | | |
| | 127 | 897 | | if (!found) |
| | 1 | 898 | | return false; |
| | | 899 | | |
| | 126 | 900 | | WideConvex2dRelations.GetSweptConvexContact( |
| | 126 | 901 | | moverOrigin, moverRotation, moverVertexOffsets, targetOrigin, targetRotation, targetVertexOffsets, |
| | 126 | 902 | | direction, out normal, out targetContactOffset); |
| | 126 | 903 | | return true; |
| | | 904 | | } |
| | | 905 | | |
| | | 906 | | private static bool TryGetSweptCenteredCapsuleFirstDistanceCore( |
| | | 907 | | Vector2d center, |
| | | 908 | | Vector2d axisDirection, |
| | | 909 | | Fixed64 axisLength, |
| | | 910 | | Fixed64 radius, |
| | | 911 | | Vector2d direction, |
| | | 912 | | Fixed64 maximumDistance, |
| | | 913 | | Vector2d convexOrigin, |
| | | 914 | | ReadOnlySpan<Vector2d> convexVertexOffsets, |
| | | 915 | | out Fixed64 distance, |
| | | 916 | | out Vector2d normal, |
| | | 917 | | out Vector2d convexContactOffset) => |
| | 373 | 918 | | TryGetSweptCenteredCapsuleFirstDistanceCore( |
| | 373 | 919 | | center, |
| | 373 | 920 | | axisDirection, |
| | 373 | 921 | | axisLength, |
| | 373 | 922 | | radius, |
| | 373 | 923 | | direction, |
| | 373 | 924 | | maximumDistance, |
| | 373 | 925 | | convexOrigin, |
| | 373 | 926 | | Fixed64.Zero, |
| | 373 | 927 | | convexVertexOffsets, |
| | 373 | 928 | | out distance, |
| | 373 | 929 | | out normal, |
| | 373 | 930 | | out convexContactOffset); |
| | | 931 | | |
| | | 932 | | // Chord mode retains a physical-distance numerator over the admitted length, |
| | | 933 | | // so anchor reconstruction does not quantize time to a second Fixed64 fraction. |
| | | 934 | | internal static bool TryGetSweptCenteredCapsuleFirstDistanceCore( |
| | | 935 | | Vector2d center, |
| | | 936 | | Vector2d axisDirection, |
| | | 937 | | Fixed64 axisLength, |
| | | 938 | | Fixed64 radius, |
| | | 939 | | Vector2d direction, |
| | | 940 | | Fixed64 maximumDistance, |
| | | 941 | | Vector2d convexOrigin, |
| | | 942 | | Fixed64 convexRotation, |
| | | 943 | | ReadOnlySpan<Vector2d> convexVertexOffsets, |
| | | 944 | | out Fixed64 distance, |
| | | 945 | | out Vector2d normal, |
| | | 946 | | out Vector2d convexContactOffset, |
| | | 947 | | bool authoredDisplacement = false) |
| | | 948 | | { |
| | 433 | 949 | | bool found = false; |
| | 433 | 950 | | distance = default; |
| | 433 | 951 | | normal = default; |
| | 433 | 952 | | convexContactOffset = default; |
| | 3750 | 953 | | for (int i = 0; i < convexVertexOffsets.Length; i++) |
| | | 954 | | { |
| | 1442 | 955 | | Vector2d start = convexVertexOffsets[i]; |
| | 1442 | 956 | | Vector2d end = convexVertexOffsets[ |
| | 1442 | 957 | | i + 1 == convexVertexOffsets.Length ? 0 : i + 1]; |
| | 1442 | 958 | | if (WideFiniteAxisIntersection |
| | 1442 | 959 | | .TryGetSweptCenteredCapsuleOriginOffsetSegmentFirstDistance( |
| | 1442 | 960 | | center, |
| | 1442 | 961 | | axisDirection, |
| | 1442 | 962 | | axisLength, |
| | 1442 | 963 | | radius, |
| | 1442 | 964 | | direction, |
| | 1442 | 965 | | maximumDistance, |
| | 1442 | 966 | | convexOrigin, |
| | 1442 | 967 | | convexRotation, |
| | 1442 | 968 | | start, |
| | 1442 | 969 | | end, |
| | 1442 | 970 | | Fixed64.Zero, |
| | 1442 | 971 | | out Fixed64 candidateDistance, |
| | 1442 | 972 | | out Vector2d candidateNormal, |
| | 1442 | 973 | | authoredDisplacement)) |
| | | 974 | | { |
| | | 975 | | // Positive radius preserves the geometric side; a rounded tangent normal's motion dot need not be zero. |
| | 1045 | 976 | | candidateNormal = radius == Fixed64.Zero |
| | 1045 | 977 | | ? OrientZeroRadiusSweepNormal(candidateNormal, direction) |
| | 1045 | 978 | | : -candidateNormal; |
| | 1045 | 979 | | if (!ShouldReplaceSweepCandidate( |
| | 1045 | 980 | | candidateDistance, |
| | 1045 | 981 | | candidateNormal, |
| | 1045 | 982 | | direction, |
| | 1045 | 983 | | found, |
| | 1045 | 984 | | distance, |
| | 1045 | 985 | | normal)) |
| | | 986 | | { |
| | | 987 | | continue; |
| | | 988 | | } |
| | 449 | 989 | | found = true; |
| | 449 | 990 | | distance = candidateDistance; |
| | 449 | 991 | | normal = candidateNormal; |
| | | 992 | | } |
| | | 993 | | } |
| | | 994 | | |
| | 433 | 995 | | if (!found) |
| | 5 | 996 | | return false; |
| | | 997 | | |
| | 428 | 998 | | if (radius == Fixed64.Zero && WideConvex2dRelations.TryGetSweptCenteredAxisContact( |
| | 428 | 999 | | center, axisDirection, axisLength, convexOrigin, convexRotation, convexVertexOffsets, |
| | 428 | 1000 | | direction, out Vector2d entryNormal, out convexContactOffset)) |
| | | 1001 | | { |
| | 284 | 1002 | | normal = entryNormal; |
| | 284 | 1003 | | return true; |
| | | 1004 | | } |
| | 144 | 1005 | | Vector2d translation = authoredDisplacement |
| | 144 | 1006 | | ? new FixedSegment2d(Vector2d.Zero, direction).GetPointAtDistance(distance, maximumDistance) |
| | 144 | 1007 | | : direction * distance; |
| | 144 | 1008 | | FixedPointAnchor2d sourceSupport = |
| | 144 | 1009 | | WideFiniteAxisIntersection.GetCenteredCapsuleSupportAnchor( |
| | 144 | 1010 | | center, |
| | 144 | 1011 | | Fixed64.Zero, |
| | 144 | 1012 | | axisDirection, |
| | 144 | 1013 | | axisLength, |
| | 144 | 1014 | | radius, |
| | 144 | 1015 | | -normal); |
| | 144 | 1016 | | convexContactOffset = |
| | 144 | 1017 | | WideConvex2dRelations.GetSweptAnchorTargetContactOffset( |
| | 144 | 1018 | | sourceSupport, |
| | 144 | 1019 | | translation, |
| | 144 | 1020 | | convexOrigin, |
| | 144 | 1021 | | convexRotation, |
| | 144 | 1022 | | convexVertexOffsets, |
| | 144 | 1023 | | normal); |
| | 144 | 1024 | | return true; |
| | | 1025 | | } |
| | | 1026 | | |
| | | 1027 | | private static bool ShouldReplaceSweepCandidate( |
| | | 1028 | | Fixed64 candidateDistance, |
| | | 1029 | | Vector2d candidateNormal, |
| | | 1030 | | Vector2d direction, |
| | | 1031 | | bool found, |
| | | 1032 | | Fixed64 bestDistance, |
| | | 1033 | | Vector2d bestNormal) => |
| | 1045 | 1034 | | !found |
| | 1045 | 1035 | | || candidateDistance < bestDistance |
| | 1045 | 1036 | | || (candidateDistance == bestDistance |
| | 1045 | 1037 | | && Vector2d.Dot(candidateNormal, direction) |
| | 1045 | 1038 | | < Vector2d.Dot(bestNormal, direction)); |
| | | 1039 | | |
| | | 1040 | | // A zero-radius crossing can round beyond the face, so its reconstructed normal needs motion orientation. |
| | | 1041 | | private static Vector2d OrientZeroRadiusSweepNormal(Vector2d normal, Vector2d direction) => |
| | 719 | 1042 | | Vector2d.Dot(normal, direction) > Fixed64.Zero ? -normal : normal; |
| | | 1043 | | |
| | | 1044 | | private static void ValidateSweep( |
| | | 1045 | | Vector2d direction, |
| | | 1046 | | Fixed64 maximumDistance) |
| | | 1047 | | { |
| | 559 | 1048 | | if (!direction.IsNormalized()) |
| | 1 | 1049 | | throw new ArgumentException( |
| | 1 | 1050 | | "Sweep direction must be normalized.", |
| | 1 | 1051 | | nameof(direction)); |
| | 558 | 1052 | | if (maximumDistance < Fixed64.Zero) |
| | | 1053 | | { |
| | 1 | 1054 | | throw new ArgumentOutOfRangeException( |
| | 1 | 1055 | | nameof(maximumDistance)); |
| | | 1056 | | } |
| | 557 | 1057 | | } |
| | | 1058 | | |
| | | 1059 | | private static void ValidateCenteredAxis( |
| | | 1060 | | Vector2d axisDirection, |
| | | 1061 | | Fixed64 axisLength) |
| | | 1062 | | { |
| | 146 | 1063 | | if (!axisDirection.IsNormalized()) |
| | | 1064 | | { |
| | 1 | 1065 | | throw new ArgumentException( |
| | 1 | 1066 | | "Axis direction must be normalized.", |
| | 1 | 1067 | | nameof(axisDirection)); |
| | | 1068 | | } |
| | 145 | 1069 | | if (axisLength < Fixed64.Zero) |
| | 1 | 1070 | | throw new ArgumentOutOfRangeException(nameof(axisLength)); |
| | 144 | 1071 | | } |
| | | 1072 | | } |