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