| | | 1 | | //======================================================================= |
| | | 2 | | // FixedSegment2d.CenteredAxis.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 | | /// <content> |
| | | 13 | | /// Contains centered-axis and rotated-capsule geometry that avoids |
| | | 14 | | /// materializing potentially unrepresentable segment endpoints. |
| | | 15 | | /// </content> |
| | | 16 | | public partial struct FixedSegment2d |
| | | 17 | | { |
| | | 18 | | /// <summary> |
| | | 19 | | /// Attempts to return the rounded distance from a point to a conceptual |
| | | 20 | | /// centered finite axis without materializing its endpoints. |
| | | 21 | | /// </summary> |
| | | 22 | | /// <returns> |
| | | 23 | | /// True when the final distance is representable; otherwise false and |
| | | 24 | | /// <paramref name="distance"/> is <see cref="Fixed64.MaxValue"/>. |
| | | 25 | | /// </returns> |
| | | 26 | | public static bool TryGetDistanceToCenteredAxis( |
| | | 27 | | Vector2d point, |
| | | 28 | | Vector2d center, |
| | | 29 | | Vector2d axisDirection, |
| | | 30 | | Fixed64 axisLength, |
| | | 31 | | out Fixed64 distance) |
| | | 32 | | { |
| | 2 | 33 | | ValidateCenteredAxis(axisDirection, axisLength); |
| | 2 | 34 | | return WideFiniteAxisIntersection.TryGetDistanceToCenteredAxis( |
| | 2 | 35 | | point, |
| | 2 | 36 | | center, |
| | 2 | 37 | | axisDirection, |
| | 2 | 38 | | axisLength, |
| | 2 | 39 | | out distance); |
| | | 40 | | } |
| | | 41 | | |
| | | 42 | | /// <summary> |
| | | 43 | | /// Attempts to return the rounded distance between two conceptual |
| | | 44 | | /// centered finite axes without materializing either pair of endpoints. |
| | | 45 | | /// </summary> |
| | | 46 | | /// <returns> |
| | | 47 | | /// True when the final distance is representable; otherwise false and |
| | | 48 | | /// <paramref name="distance"/> is <see cref="Fixed64.MaxValue"/>. |
| | | 49 | | /// </returns> |
| | | 50 | | public static bool TryGetDistanceBetweenCenteredAxes( |
| | | 51 | | Vector2d firstCenter, |
| | | 52 | | Vector2d firstAxisDirection, |
| | | 53 | | Fixed64 firstAxisLength, |
| | | 54 | | Vector2d secondCenter, |
| | | 55 | | Vector2d secondAxisDirection, |
| | | 56 | | Fixed64 secondAxisLength, |
| | | 57 | | out Fixed64 distance) |
| | | 58 | | { |
| | 4612 | 59 | | ValidateCenteredAxis( |
| | 4612 | 60 | | firstAxisDirection, |
| | 4612 | 61 | | firstAxisLength, |
| | 4612 | 62 | | nameof(firstAxisDirection), |
| | 4612 | 63 | | nameof(firstAxisLength)); |
| | 4611 | 64 | | ValidateCenteredAxis( |
| | 4611 | 65 | | secondAxisDirection, |
| | 4611 | 66 | | secondAxisLength, |
| | 4611 | 67 | | nameof(secondAxisDirection), |
| | 4611 | 68 | | nameof(secondAxisLength)); |
| | 4610 | 69 | | return WideFiniteAxisIntersection.TryGetDistanceBetweenCenteredAxes( |
| | 4610 | 70 | | firstCenter, |
| | 4610 | 71 | | firstAxisDirection, |
| | 4610 | 72 | | firstAxisLength, |
| | 4610 | 73 | | secondCenter, |
| | 4610 | 74 | | secondAxisDirection, |
| | 4610 | 75 | | secondAxisLength, |
| | 4610 | 76 | | out distance); |
| | | 77 | | } |
| | | 78 | | |
| | | 79 | | /// <summary> |
| | | 80 | | /// Returns whether two conceptual centered capsules overlap, including |
| | | 81 | | /// exact tangency, without narrowing their axis distance or radius sum. |
| | | 82 | | /// </summary> |
| | | 83 | | public static bool DoCenteredCapsulesOverlap( |
| | | 84 | | Vector2d firstCenter, |
| | | 85 | | Vector2d firstAxisDirection, |
| | | 86 | | Fixed64 firstAxisLength, |
| | | 87 | | Fixed64 firstRadius, |
| | | 88 | | Vector2d secondCenter, |
| | | 89 | | Vector2d secondAxisDirection, |
| | | 90 | | Fixed64 secondAxisLength, |
| | | 91 | | Fixed64 secondRadius) |
| | | 92 | | { |
| | 4613 | 93 | | ValidateCenteredAxis( |
| | 4613 | 94 | | firstAxisDirection, |
| | 4613 | 95 | | firstAxisLength, |
| | 4613 | 96 | | nameof(firstAxisDirection), |
| | 4613 | 97 | | nameof(firstAxisLength)); |
| | 4613 | 98 | | if (firstRadius < Fixed64.Zero) |
| | 1 | 99 | | throw new ArgumentOutOfRangeException(nameof(firstRadius)); |
| | 4612 | 100 | | ValidateCenteredAxis( |
| | 4612 | 101 | | secondAxisDirection, |
| | 4612 | 102 | | secondAxisLength, |
| | 4612 | 103 | | nameof(secondAxisDirection), |
| | 4612 | 104 | | nameof(secondAxisLength)); |
| | 4612 | 105 | | if (secondRadius < Fixed64.Zero) |
| | 1 | 106 | | throw new ArgumentOutOfRangeException(nameof(secondRadius)); |
| | | 107 | | |
| | 4611 | 108 | | return WideFiniteAxisIntersection.DoCenteredCapsulesOverlap( |
| | 4611 | 109 | | firstCenter, |
| | 4611 | 110 | | firstAxisDirection, |
| | 4611 | 111 | | firstAxisLength, |
| | 4611 | 112 | | firstRadius, |
| | 4611 | 113 | | secondCenter, |
| | 4611 | 114 | | secondAxisDirection, |
| | 4611 | 115 | | secondAxisLength, |
| | 4611 | 116 | | secondRadius); |
| | | 117 | | } |
| | | 118 | | |
| | | 119 | | /// <summary> |
| | | 120 | | /// Attempts to build an exact contact between two conceptual centered |
| | | 121 | | /// capsules. |
| | | 122 | | /// </summary> |
| | | 123 | | /// <remarks> |
| | | 124 | | /// Each surface witness retains its axial and radial contributions |
| | | 125 | | /// separately. Classification is independent of absolute world-point |
| | | 126 | | /// materialization. |
| | | 127 | | /// </remarks> |
| | | 128 | | public static bool TryGetCenteredCapsulesContact( |
| | | 129 | | Vector2d firstCenter, |
| | | 130 | | Fixed64 firstAnchorRotation, |
| | | 131 | | Vector2d firstAxisDirection, |
| | | 132 | | Fixed64 firstAxisLength, |
| | | 133 | | Fixed64 firstRadius, |
| | | 134 | | Vector2d secondCenter, |
| | | 135 | | Fixed64 secondAnchorRotation, |
| | | 136 | | Vector2d secondAxisDirection, |
| | | 137 | | Fixed64 secondAxisLength, |
| | | 138 | | Fixed64 secondRadius, |
| | | 139 | | Vector2d fallbackNormal, |
| | | 140 | | out FixedContactAnchors2d contact) |
| | | 141 | | { |
| | 10 | 142 | | ValidateCenteredAxis( |
| | 10 | 143 | | firstAxisDirection, |
| | 10 | 144 | | firstAxisLength, |
| | 10 | 145 | | nameof(firstAxisDirection), |
| | 10 | 146 | | nameof(firstAxisLength)); |
| | 10 | 147 | | if (firstRadius < Fixed64.Zero) |
| | 1 | 148 | | throw new ArgumentOutOfRangeException(nameof(firstRadius)); |
| | 9 | 149 | | ValidateCenteredAxis( |
| | 9 | 150 | | secondAxisDirection, |
| | 9 | 151 | | secondAxisLength, |
| | 9 | 152 | | nameof(secondAxisDirection), |
| | 9 | 153 | | nameof(secondAxisLength)); |
| | 9 | 154 | | if (secondRadius < Fixed64.Zero) |
| | 1 | 155 | | throw new ArgumentOutOfRangeException(nameof(secondRadius)); |
| | 8 | 156 | | if (!fallbackNormal.IsNormalized()) |
| | 1 | 157 | | throw new ArgumentException("Fallback normal must be normalized.", nameof(fallbackNormal)); |
| | | 158 | | |
| | 7 | 159 | | return WideFiniteAxisIntersection.TryGetCenteredCapsulesContact( |
| | 7 | 160 | | firstCenter, |
| | 7 | 161 | | firstAnchorRotation, |
| | 7 | 162 | | firstAxisDirection, |
| | 7 | 163 | | firstAxisLength, |
| | 7 | 164 | | firstRadius, |
| | 7 | 165 | | secondCenter, |
| | 7 | 166 | | secondAnchorRotation, |
| | 7 | 167 | | secondAxisDirection, |
| | 7 | 168 | | secondAxisLength, |
| | 7 | 169 | | secondRadius, |
| | 7 | 170 | | fallbackNormal, |
| | 7 | 171 | | out contact); |
| | | 172 | | } |
| | | 173 | | |
| | | 174 | | /// <summary> |
| | | 175 | | /// Attempts to materialize one endpoint of a conceptual centered finite |
| | | 176 | | /// axis from its normalized direction and full length. |
| | | 177 | | /// </summary> |
| | | 178 | | /// <remarks> |
| | | 179 | | /// The endpoint is rounded only after combining its center and half-length |
| | | 180 | | /// contribution. Failure is atomic. |
| | | 181 | | /// </remarks> |
| | | 182 | | public static bool TryGetCenteredAxisEndpoint( |
| | | 183 | | Vector2d center, |
| | | 184 | | Vector2d axisDirection, |
| | | 185 | | Fixed64 axisLength, |
| | | 186 | | bool positive, |
| | | 187 | | out Vector2d endpoint) |
| | | 188 | | { |
| | 3 | 189 | | ValidateCenteredAxis(axisDirection, axisLength); |
| | 2 | 190 | | return WideFiniteAxisIntersection.TryGetCenteredAxisEndpoint( |
| | 2 | 191 | | center, |
| | 2 | 192 | | axisDirection, |
| | 2 | 193 | | axisLength, |
| | 2 | 194 | | positive, |
| | 2 | 195 | | out endpoint); |
| | | 196 | | } |
| | | 197 | | |
| | | 198 | | /// <summary> |
| | | 199 | | /// Attempts to materialize the support point of a conceptual centered |
| | | 200 | | /// capsule in a world-space direction. |
| | | 201 | | /// </summary> |
| | | 202 | | /// <remarks> |
| | | 203 | | /// The direction need not be normalized. A zero direction and an exact |
| | | 204 | | /// axial tie select the capsule center along the tied axis. |
| | | 205 | | /// </remarks> |
| | | 206 | | public static bool TryGetCenteredCapsuleSupport( |
| | | 207 | | Vector2d center, |
| | | 208 | | Vector2d axisDirection, |
| | | 209 | | Fixed64 axisLength, |
| | | 210 | | Fixed64 radius, |
| | | 211 | | Vector2d direction, |
| | | 212 | | out Vector2d support) |
| | | 213 | | { |
| | 4 | 214 | | ValidateCenteredAxis(axisDirection, axisLength); |
| | 4 | 215 | | if (radius < Fixed64.Zero) |
| | 1 | 216 | | throw new ArgumentOutOfRangeException(nameof(radius)); |
| | | 217 | | |
| | 3 | 218 | | return WideFiniteAxisIntersection.TryGetCenteredCapsuleSupport( |
| | 3 | 219 | | center, |
| | 3 | 220 | | axisDirection, |
| | 3 | 221 | | axisLength, |
| | 3 | 222 | | radius, |
| | 3 | 223 | | direction, |
| | 3 | 224 | | out support); |
| | | 225 | | } |
| | | 226 | | |
| | | 227 | | /// <summary> |
| | | 228 | | /// Returns the support anchor of a conceptual centered capsule without |
| | | 229 | | /// combining its axial and radial feature components. |
| | | 230 | | /// </summary> |
| | | 231 | | /// <remarks> |
| | | 232 | | /// The direction need not be normalized. A zero direction and an exact |
| | | 233 | | /// axial tie select the axis center. |
| | | 234 | | /// </remarks> |
| | | 235 | | public static FixedPointAnchor2d GetCenteredCapsuleSupportAnchor( |
| | | 236 | | Vector2d center, |
| | | 237 | | Fixed64 frameRotation, |
| | | 238 | | Vector2d localAxisDirection, |
| | | 239 | | Fixed64 axisLength, |
| | | 240 | | Fixed64 radius, |
| | | 241 | | Vector2d localDirection) |
| | | 242 | | { |
| | 156 | 243 | | ValidateCenteredAxis(localAxisDirection, axisLength); |
| | 155 | 244 | | if (radius < Fixed64.Zero) |
| | 1 | 245 | | throw new ArgumentOutOfRangeException(nameof(radius)); |
| | | 246 | | |
| | 154 | 247 | | return WideFiniteAxisIntersection.GetCenteredCapsuleSupportAnchor( |
| | 154 | 248 | | center, |
| | 154 | 249 | | frameRotation, |
| | 154 | 250 | | localAxisDirection, |
| | 154 | 251 | | axisLength, |
| | 154 | 252 | | radius, |
| | 154 | 253 | | localDirection); |
| | | 254 | | } |
| | | 255 | | |
| | | 256 | | /// <summary> |
| | | 257 | | /// Returns the rounded nonnegative distance from a point to a conceptual |
| | | 258 | | /// centered capsule surface, saturating to <see cref="Fixed64.MaxValue"/> |
| | | 259 | | /// when the distance is not representable. |
| | | 260 | | /// </summary> |
| | | 261 | | /// <remarks> |
| | | 262 | | /// The final value uses round-half-to-even. Points inside or on the capsule |
| | | 263 | | /// return zero. |
| | | 264 | | /// </remarks> |
| | | 265 | | /// <exception cref="ArgumentException"> |
| | | 266 | | /// Thrown when <paramref name="axisDirection"/> is zero or not normalized. |
| | | 267 | | /// </exception> |
| | | 268 | | /// <exception cref="ArgumentOutOfRangeException"> |
| | | 269 | | /// Thrown when <paramref name="axisLength"/> or <paramref name="radius"/> |
| | | 270 | | /// is negative. |
| | | 271 | | /// </exception> |
| | | 272 | | public static Fixed64 GetDistanceToCenteredCapsule( |
| | | 273 | | Vector2d point, |
| | | 274 | | Vector2d center, |
| | | 275 | | Vector2d axisDirection, |
| | | 276 | | Fixed64 axisLength, |
| | | 277 | | Fixed64 radius) |
| | | 278 | | { |
| | 2 | 279 | | TryGetDistanceToCenteredCapsule( |
| | 2 | 280 | | point, |
| | 2 | 281 | | center, |
| | 2 | 282 | | axisDirection, |
| | 2 | 283 | | axisLength, |
| | 2 | 284 | | radius, |
| | 2 | 285 | | out Fixed64 distance); |
| | 2 | 286 | | return distance; |
| | | 287 | | } |
| | | 288 | | |
| | | 289 | | /// <summary> |
| | | 290 | | /// Attempts to return the rounded nonnegative distance from a point to a |
| | | 291 | | /// conceptual centered capsule surface. |
| | | 292 | | /// </summary> |
| | | 293 | | /// <remarks>The final value uses round-half-to-even.</remarks> |
| | | 294 | | /// <returns> |
| | | 295 | | /// True when the final distance is representable; otherwise false and |
| | | 296 | | /// <paramref name="distance"/> is <see cref="Fixed64.MaxValue"/>. |
| | | 297 | | /// Points inside or on the capsule return zero. |
| | | 298 | | /// </returns> |
| | | 299 | | /// <exception cref="ArgumentException"> |
| | | 300 | | /// Thrown when <paramref name="axisDirection"/> is zero or not normalized. |
| | | 301 | | /// </exception> |
| | | 302 | | /// <exception cref="ArgumentOutOfRangeException"> |
| | | 303 | | /// Thrown when <paramref name="axisLength"/> or <paramref name="radius"/> |
| | | 304 | | /// is negative. |
| | | 305 | | /// </exception> |
| | | 306 | | public static bool TryGetDistanceToCenteredCapsule( |
| | | 307 | | Vector2d point, |
| | | 308 | | Vector2d center, |
| | | 309 | | Vector2d axisDirection, |
| | | 310 | | Fixed64 axisLength, |
| | | 311 | | Fixed64 radius, |
| | | 312 | | out Fixed64 distance) |
| | | 313 | | { |
| | 8 | 314 | | ValidateCenteredAxis(axisDirection, axisLength); |
| | 8 | 315 | | if (radius < Fixed64.Zero) |
| | 1 | 316 | | throw new ArgumentOutOfRangeException(nameof(radius)); |
| | | 317 | | |
| | 7 | 318 | | return WideFiniteAxisIntersection.TryGetDistanceToCenteredCapsule( |
| | 7 | 319 | | point, |
| | 7 | 320 | | center, |
| | 7 | 321 | | axisDirection, |
| | 7 | 322 | | axisLength, |
| | 7 | 323 | | radius, |
| | 7 | 324 | | out distance); |
| | | 325 | | } |
| | | 326 | | |
| | | 327 | | /// <summary> |
| | | 328 | | /// Returns whether a point lies inside or on a conceptual centered capsule. |
| | | 329 | | /// </summary> |
| | | 330 | | /// <exception cref="ArgumentException"> |
| | | 331 | | /// Thrown when <paramref name="axisDirection"/> is zero or not normalized. |
| | | 332 | | /// </exception> |
| | | 333 | | /// <exception cref="ArgumentOutOfRangeException"> |
| | | 334 | | /// Thrown when <paramref name="axisLength"/> or <paramref name="radius"/> |
| | | 335 | | /// is negative. |
| | | 336 | | /// </exception> |
| | | 337 | | public static bool ContainsPointInCenteredCapsule( |
| | | 338 | | Vector2d point, |
| | | 339 | | Vector2d center, |
| | | 340 | | Vector2d axisDirection, |
| | | 341 | | Fixed64 axisLength, |
| | | 342 | | Fixed64 radius) => |
| | 7 | 343 | | ContainsPointInCenteredCapsule( |
| | 7 | 344 | | point, |
| | 7 | 345 | | center, |
| | 7 | 346 | | axisDirection, |
| | 7 | 347 | | axisLength, |
| | 7 | 348 | | radius, |
| | 7 | 349 | | Fixed64.Zero, |
| | 7 | 350 | | strict: false); |
| | | 351 | | |
| | | 352 | | /// <summary> |
| | | 353 | | /// Returns whether a point lies strictly inside or inclusively within a |
| | | 354 | | /// conceptual centered capsule. |
| | | 355 | | /// </summary> |
| | | 356 | | public static bool ContainsPointInCenteredCapsule( |
| | | 357 | | Vector2d point, |
| | | 358 | | Vector2d center, |
| | | 359 | | Vector2d axisDirection, |
| | | 360 | | Fixed64 axisLength, |
| | | 361 | | Fixed64 radius, |
| | | 362 | | bool strict) => |
| | 3 | 363 | | ContainsPointInCenteredCapsule( |
| | 3 | 364 | | point, |
| | 3 | 365 | | center, |
| | 3 | 366 | | axisDirection, |
| | 3 | 367 | | axisLength, |
| | 3 | 368 | | radius, |
| | 3 | 369 | | Fixed64.Zero, |
| | 3 | 370 | | strict); |
| | | 371 | | |
| | | 372 | | /// <summary> |
| | | 373 | | /// Returns whether a point lies inside or on a conceptual centered capsule, |
| | | 374 | | /// including a nonnegative radial expansion. |
| | | 375 | | /// </summary> |
| | | 376 | | /// <exception cref="ArgumentException"> |
| | | 377 | | /// Thrown when <paramref name="axisDirection"/> is zero or not normalized. |
| | | 378 | | /// </exception> |
| | | 379 | | /// <exception cref="ArgumentOutOfRangeException"> |
| | | 380 | | /// Thrown when <paramref name="axisLength"/>, <paramref name="radius"/>, |
| | | 381 | | /// or <paramref name="radiusExpansion"/> is negative. |
| | | 382 | | /// </exception> |
| | | 383 | | public static bool ContainsPointInCenteredCapsule( |
| | | 384 | | Vector2d point, |
| | | 385 | | Vector2d center, |
| | | 386 | | Vector2d axisDirection, |
| | | 387 | | Fixed64 axisLength, |
| | | 388 | | Fixed64 radius, |
| | | 389 | | Fixed64 radiusExpansion) => |
| | 5 | 390 | | ContainsPointInCenteredCapsule( |
| | 5 | 391 | | point, |
| | 5 | 392 | | center, |
| | 5 | 393 | | axisDirection, |
| | 5 | 394 | | axisLength, |
| | 5 | 395 | | radius, |
| | 5 | 396 | | radiusExpansion, |
| | 5 | 397 | | strict: false); |
| | | 398 | | |
| | | 399 | | /// <summary> |
| | | 400 | | /// Returns whether a point lies strictly inside or inclusively within a |
| | | 401 | | /// conceptual centered capsule with nonnegative radial expansion. |
| | | 402 | | /// </summary> |
| | | 403 | | public static bool ContainsPointInCenteredCapsule( |
| | | 404 | | Vector2d point, |
| | | 405 | | Vector2d center, |
| | | 406 | | Vector2d axisDirection, |
| | | 407 | | Fixed64 axisLength, |
| | | 408 | | Fixed64 radius, |
| | | 409 | | Fixed64 radiusExpansion, |
| | | 410 | | bool strict) |
| | | 411 | | { |
| | 17 | 412 | | ValidateCenteredAxis(axisDirection, axisLength); |
| | 17 | 413 | | if (radius < Fixed64.Zero) |
| | 1 | 414 | | throw new ArgumentOutOfRangeException(nameof(radius)); |
| | 16 | 415 | | if (radiusExpansion < Fixed64.Zero) |
| | 1 | 416 | | throw new ArgumentOutOfRangeException(nameof(radiusExpansion)); |
| | | 417 | | |
| | 15 | 418 | | return WideFiniteAxisIntersection.ContainsPointInCenteredCapsule( |
| | 15 | 419 | | point, |
| | 15 | 420 | | center, |
| | 15 | 421 | | axisDirection, |
| | 15 | 422 | | axisLength, |
| | 15 | 423 | | radius, |
| | 15 | 424 | | radiusExpansion, |
| | 15 | 425 | | strict); |
| | | 426 | | } |
| | | 427 | | |
| | | 428 | | /// <summary> |
| | | 429 | | /// Attempts to return the selected point on a conceptual centered capsule |
| | | 430 | | /// surface without narrowing its axis point before applying the radial offset. |
| | | 431 | | /// </summary> |
| | | 432 | | /// <remarks> |
| | | 433 | | /// <paramref name="surfaceDirection"/> must be the nonzero result of |
| | | 434 | | /// <see cref="GetDirectionFromCenteredAxis(Vector2d, Vector2d, Vector2d, Fixed64)"/>, |
| | | 435 | | /// or a caller-selected normalized |
| | | 436 | | /// direction perpendicular to the axis when that method returns zero. |
| | | 437 | | /// </remarks> |
| | | 438 | | /// <returns> |
| | | 439 | | /// True when every final surface coordinate is representable; otherwise |
| | | 440 | | /// false and <paramref name="surfacePoint"/> is zero. |
| | | 441 | | /// </returns> |
| | | 442 | | /// <exception cref="ArgumentException"> |
| | | 443 | | /// Thrown when <paramref name="axisDirection"/> is zero or not normalized, |
| | | 444 | | /// or when <paramref name="surfaceDirection"/> is not normalized. |
| | | 445 | | /// </exception> |
| | | 446 | | /// <exception cref="ArgumentOutOfRangeException"> |
| | | 447 | | /// Thrown when <paramref name="axisLength"/> or <paramref name="radius"/> |
| | | 448 | | /// is negative. |
| | | 449 | | /// </exception> |
| | | 450 | | public static bool TryGetSurfacePointOnCenteredCapsule( |
| | | 451 | | Vector2d point, |
| | | 452 | | Vector2d center, |
| | | 453 | | Vector2d axisDirection, |
| | | 454 | | Fixed64 axisLength, |
| | | 455 | | Fixed64 radius, |
| | | 456 | | Vector2d surfaceDirection, |
| | | 457 | | out Vector2d surfacePoint) |
| | | 458 | | { |
| | 9 | 459 | | ValidateCenteredAxis(axisDirection, axisLength); |
| | 9 | 460 | | if (radius < Fixed64.Zero) |
| | 1 | 461 | | throw new ArgumentOutOfRangeException(nameof(radius)); |
| | 8 | 462 | | if (!surfaceDirection.IsNormalized()) |
| | 1 | 463 | | throw new ArgumentException("Surface direction must be normalized.", nameof(surfaceDirection)); |
| | | 464 | | |
| | 7 | 465 | | return WideFiniteAxisIntersection.TryGetSurfacePointOnCenteredCapsule( |
| | 7 | 466 | | point, |
| | 7 | 467 | | center, |
| | 7 | 468 | | axisDirection, |
| | 7 | 469 | | axisLength, |
| | 7 | 470 | | radius, |
| | 7 | 471 | | surfaceDirection, |
| | 7 | 472 | | out surfacePoint); |
| | | 473 | | } |
| | | 474 | | |
| | | 475 | | /// <summary> |
| | | 476 | | /// Attempts to return the center-relative surface offset on a conceptual |
| | | 477 | | /// centered capsule nearest to a world-space point in the supplied surface |
| | | 478 | | /// direction. |
| | | 479 | | /// </summary> |
| | | 480 | | public static bool TryGetSurfaceOffsetOnCenteredCapsule( |
| | | 481 | | Vector2d point, |
| | | 482 | | Vector2d center, |
| | | 483 | | Vector2d axisDirection, |
| | | 484 | | Fixed64 axisLength, |
| | | 485 | | Fixed64 radius, |
| | | 486 | | Vector2d surfaceDirection, |
| | | 487 | | out Vector2d surfaceOffset) |
| | | 488 | | { |
| | 5 | 489 | | ValidateCenteredAxis(axisDirection, axisLength); |
| | 5 | 490 | | if (radius < Fixed64.Zero) |
| | 1 | 491 | | throw new ArgumentOutOfRangeException(nameof(radius)); |
| | 4 | 492 | | if (!surfaceDirection.IsNormalized()) |
| | | 493 | | { |
| | 1 | 494 | | throw new ArgumentException( |
| | 1 | 495 | | "Surface direction must be normalized.", |
| | 1 | 496 | | nameof(surfaceDirection)); |
| | | 497 | | } |
| | | 498 | | |
| | 3 | 499 | | return WideFiniteAxisIntersection.TryGetSurfaceOffsetOnCenteredCapsule( |
| | 3 | 500 | | point, |
| | 3 | 501 | | center, |
| | 3 | 502 | | axisDirection, |
| | 3 | 503 | | axisLength, |
| | 3 | 504 | | radius, |
| | 3 | 505 | | surfaceDirection, |
| | 3 | 506 | | out surfaceOffset); |
| | | 507 | | } |
| | | 508 | | |
| | | 509 | | /// <summary> |
| | | 510 | | /// Returns the centered-capsule surface anchor nearest to a world-space |
| | | 511 | | /// point in the supplied normalized surface direction. |
| | | 512 | | /// </summary> |
| | | 513 | | public static FixedPointAnchor2d GetSurfaceAnchorOnCenteredCapsule( |
| | | 514 | | Vector2d point, |
| | | 515 | | Vector2d center, |
| | | 516 | | Fixed64 frameRotation, |
| | | 517 | | Vector2d localAxisDirection, |
| | | 518 | | Fixed64 axisLength, |
| | | 519 | | Fixed64 radius, |
| | | 520 | | Vector2d localSurfaceDirection) |
| | | 521 | | { |
| | 5 | 522 | | ValidateCenteredAxis(localAxisDirection, axisLength); |
| | 5 | 523 | | if (radius < Fixed64.Zero) |
| | 1 | 524 | | throw new ArgumentOutOfRangeException(nameof(radius)); |
| | 4 | 525 | | if (!localSurfaceDirection.IsNormalized()) |
| | | 526 | | { |
| | 1 | 527 | | throw new ArgumentException( |
| | 1 | 528 | | "Surface direction must be normalized.", |
| | 1 | 529 | | nameof(localSurfaceDirection)); |
| | | 530 | | } |
| | | 531 | | |
| | 3 | 532 | | return WideFiniteAxisIntersection.GetSurfaceAnchorOnCenteredCapsule( |
| | 3 | 533 | | point, |
| | 3 | 534 | | center, |
| | 3 | 535 | | frameRotation, |
| | 3 | 536 | | localAxisDirection, |
| | 3 | 537 | | axisLength, |
| | 3 | 538 | | radius, |
| | 3 | 539 | | localSurfaceDirection); |
| | | 540 | | } |
| | | 541 | | |
| | | 542 | | /// <summary> |
| | | 543 | | /// Returns the normalized direction from the closest point on a conceptual |
| | | 544 | | /// centered finite axis toward <paramref name="point"/>, or zero when the |
| | | 545 | | /// point lies on that axis. |
| | | 546 | | /// </summary> |
| | | 547 | | /// <remarks> |
| | | 548 | | /// The conceptual endpoints are |
| | | 549 | | /// <c>center +/- axisDirection * (axisLength / 2)</c>. They are never |
| | | 550 | | /// constructed or narrowed before the closest-feature decision. |
| | | 551 | | /// </remarks> |
| | | 552 | | /// <exception cref="ArgumentException"> |
| | | 553 | | /// Thrown when <paramref name="axisDirection"/> is zero or not normalized. |
| | | 554 | | /// </exception> |
| | | 555 | | /// <exception cref="ArgumentOutOfRangeException"> |
| | | 556 | | /// Thrown when <paramref name="axisLength"/> is negative. |
| | | 557 | | /// </exception> |
| | | 558 | | public static Vector2d GetDirectionFromCenteredAxis( |
| | | 559 | | Vector2d point, |
| | | 560 | | Vector2d center, |
| | | 561 | | Vector2d axisDirection, |
| | | 562 | | Fixed64 axisLength) |
| | | 563 | | { |
| | 7 | 564 | | ValidateCenteredAxis(axisDirection, axisLength); |
| | | 565 | | |
| | 5 | 566 | | return WideFiniteAxisIntersection.GetDirectionFromCenteredAxis( |
| | 5 | 567 | | point, |
| | 5 | 568 | | center, |
| | 5 | 569 | | axisDirection, |
| | 5 | 570 | | axisLength); |
| | | 571 | | } |
| | | 572 | | |
| | | 573 | | /// <summary> |
| | | 574 | | /// Attempts to return the closest points on two conceptual centered finite |
| | | 575 | | /// axes described by normalized directions and full lengths. |
| | | 576 | | /// </summary> |
| | | 577 | | /// <returns> |
| | | 578 | | /// <see langword="true"/> when both final world points are representable; |
| | | 579 | | /// otherwise <see langword="false"/> and both outputs are zero. |
| | | 580 | | /// </returns> |
| | | 581 | | public static bool TryGetClosestPointsBetweenCenteredAxes( |
| | | 582 | | Vector2d firstCenter, |
| | | 583 | | Vector2d firstAxisDirection, |
| | | 584 | | Fixed64 firstAxisLength, |
| | | 585 | | Vector2d secondCenter, |
| | | 586 | | Vector2d secondAxisDirection, |
| | | 587 | | Fixed64 secondAxisLength, |
| | | 588 | | out Vector2d firstPoint, |
| | | 589 | | out Vector2d secondPoint) |
| | | 590 | | { |
| | 4 | 591 | | ValidateCenteredAxis( |
| | 4 | 592 | | firstAxisDirection, |
| | 4 | 593 | | firstAxisLength, |
| | 4 | 594 | | nameof(firstAxisDirection), |
| | 4 | 595 | | nameof(firstAxisLength)); |
| | 4 | 596 | | ValidateCenteredAxis( |
| | 4 | 597 | | secondAxisDirection, |
| | 4 | 598 | | secondAxisLength, |
| | 4 | 599 | | nameof(secondAxisDirection), |
| | 4 | 600 | | nameof(secondAxisLength)); |
| | 4 | 601 | | return WideFiniteAxisIntersection.TryGetClosestPointsBetweenCenteredAxes( |
| | 4 | 602 | | firstCenter, |
| | 4 | 603 | | firstAxisDirection, |
| | 4 | 604 | | firstAxisLength, |
| | 4 | 605 | | secondCenter, |
| | 4 | 606 | | secondAxisDirection, |
| | 4 | 607 | | secondAxisLength, |
| | 4 | 608 | | out firstPoint, |
| | 4 | 609 | | out secondPoint); |
| | | 610 | | } |
| | | 611 | | |
| | | 612 | | private static void ValidateCenteredAxis(Vector2d axisDirection, Fixed64 axisLength) |
| | 305 | 613 | | => ValidateCenteredAxis( |
| | 305 | 614 | | axisDirection, |
| | 305 | 615 | | axisLength, |
| | 305 | 616 | | nameof(axisDirection), |
| | 305 | 617 | | nameof(axisLength)); |
| | | 618 | | |
| | | 619 | | private static void ValidateCenteredAxis( |
| | | 620 | | Vector2d axisDirection, |
| | | 621 | | Fixed64 axisLength, |
| | | 622 | | string directionParameterName, |
| | | 623 | | string lengthParameterName) |
| | | 624 | | { |
| | 18790 | 625 | | if (!axisDirection.IsNormalized()) |
| | 6 | 626 | | throw new ArgumentException("Axis direction must be normalized.", directionParameterName); |
| | 18784 | 627 | | if (axisLength < Fixed64.Zero) |
| | 3 | 628 | | throw new ArgumentOutOfRangeException(lengthParameterName); |
| | 18781 | 629 | | } |
| | | 630 | | |
| | | 631 | | /// <summary> |
| | | 632 | | /// Returns whether a point lies inside or on a centered rotated capsule. |
| | | 633 | | /// </summary> |
| | | 634 | | public static bool ContainsPointInCenteredCapsule( |
| | | 635 | | Vector2d point, |
| | | 636 | | Vector2d center, |
| | | 637 | | Fixed64 frameRotation, |
| | | 638 | | Fixed64 axisLength, |
| | | 639 | | Fixed64 radius) => |
| | 1 | 640 | | ContainsPointInCenteredCapsule( |
| | 1 | 641 | | point, |
| | 1 | 642 | | center, |
| | 1 | 643 | | frameRotation, |
| | 1 | 644 | | axisLength, |
| | 1 | 645 | | radius, |
| | 1 | 646 | | Fixed64.Zero, |
| | 1 | 647 | | strict: false); |
| | | 648 | | |
| | | 649 | | /// <summary> |
| | | 650 | | /// Returns whether a point lies inside or on a radially expanded centered |
| | | 651 | | /// rotated capsule. |
| | | 652 | | /// </summary> |
| | | 653 | | public static bool ContainsPointInCenteredCapsule( |
| | | 654 | | Vector2d point, |
| | | 655 | | Vector2d center, |
| | | 656 | | Fixed64 frameRotation, |
| | | 657 | | Fixed64 axisLength, |
| | | 658 | | Fixed64 radius, |
| | | 659 | | Fixed64 radiusExpansion) => |
| | 1 | 660 | | ContainsPointInCenteredCapsule( |
| | 1 | 661 | | point, |
| | 1 | 662 | | center, |
| | 1 | 663 | | frameRotation, |
| | 1 | 664 | | axisLength, |
| | 1 | 665 | | radius, |
| | 1 | 666 | | radiusExpansion, |
| | 1 | 667 | | strict: false); |
| | | 668 | | |
| | | 669 | | /// <summary> |
| | | 670 | | /// Returns whether a point lies within a centered capsule whose local |
| | | 671 | | /// positive Y axis is transformed by a scalar rigid-frame rotation. |
| | | 672 | | /// </summary> |
| | | 673 | | public static bool ContainsPointInCenteredCapsule( |
| | | 674 | | Vector2d point, |
| | | 675 | | Vector2d center, |
| | | 676 | | Fixed64 frameRotation, |
| | | 677 | | Fixed64 axisLength, |
| | | 678 | | Fixed64 radius, |
| | | 679 | | Fixed64 radiusExpansion, |
| | | 680 | | bool strict) |
| | | 681 | | { |
| | 4 | 682 | | ValidateRotatedCapsule(axisLength, radius); |
| | 4 | 683 | | if (radiusExpansion < Fixed64.Zero) |
| | 1 | 684 | | throw new ArgumentOutOfRangeException(nameof(radiusExpansion)); |
| | | 685 | | |
| | 3 | 686 | | return WideFiniteAxisIntersection.ContainsPointInCenteredCapsule( |
| | 3 | 687 | | point, |
| | 3 | 688 | | center, |
| | 3 | 689 | | GetRotatedPositiveYAxis(frameRotation), |
| | 3 | 690 | | axisLength, |
| | 3 | 691 | | radius, |
| | 3 | 692 | | radiusExpansion, |
| | 3 | 693 | | strict); |
| | | 694 | | } |
| | | 695 | | |
| | | 696 | | /// <summary> |
| | | 697 | | /// Returns the normalized direction from the closest point on a centered |
| | | 698 | | /// capsule axis expressed by a scalar rigid-frame rotation. |
| | | 699 | | /// </summary> |
| | | 700 | | public static Vector2d GetDirectionFromCenteredAxis( |
| | | 701 | | Vector2d point, |
| | | 702 | | Vector2d center, |
| | | 703 | | Fixed64 frameRotation, |
| | | 704 | | Fixed64 axisLength) |
| | | 705 | | { |
| | 1 | 706 | | ValidateCenteredAxis(Vector2d.Forward, axisLength); |
| | 1 | 707 | | return WideFiniteAxisIntersection.GetDirectionFromCenteredAxis( |
| | 1 | 708 | | point, |
| | 1 | 709 | | center, |
| | 1 | 710 | | GetRotatedPositiveYAxis(frameRotation), |
| | 1 | 711 | | axisLength); |
| | | 712 | | } |
| | | 713 | | |
| | | 714 | | /// <summary> |
| | | 715 | | /// Returns the rounded distance from a point to a centered rotated capsule. |
| | | 716 | | /// </summary> |
| | | 717 | | public static Fixed64 GetDistanceToCenteredCapsule( |
| | | 718 | | Vector2d point, |
| | | 719 | | Vector2d center, |
| | | 720 | | Fixed64 frameRotation, |
| | | 721 | | Fixed64 axisLength, |
| | | 722 | | Fixed64 radius) |
| | | 723 | | { |
| | 1 | 724 | | _ = TryGetDistanceToCenteredCapsule( |
| | 1 | 725 | | point, |
| | 1 | 726 | | center, |
| | 1 | 727 | | frameRotation, |
| | 1 | 728 | | axisLength, |
| | 1 | 729 | | radius, |
| | 1 | 730 | | out Fixed64 distance); |
| | 1 | 731 | | return distance; |
| | | 732 | | } |
| | | 733 | | |
| | | 734 | | /// <summary> |
| | | 735 | | /// Attempts to return the rounded distance from a point to a centered |
| | | 736 | | /// rotated capsule. |
| | | 737 | | /// </summary> |
| | | 738 | | public static bool TryGetDistanceToCenteredCapsule( |
| | | 739 | | Vector2d point, |
| | | 740 | | Vector2d center, |
| | | 741 | | Fixed64 frameRotation, |
| | | 742 | | Fixed64 axisLength, |
| | | 743 | | Fixed64 radius, |
| | | 744 | | out Fixed64 distance) |
| | | 745 | | { |
| | 2 | 746 | | ValidateRotatedCapsule(axisLength, radius); |
| | 2 | 747 | | return WideFiniteAxisIntersection.TryGetDistanceToCenteredCapsule( |
| | 2 | 748 | | point, |
| | 2 | 749 | | center, |
| | 2 | 750 | | GetRotatedPositiveYAxis(frameRotation), |
| | 2 | 751 | | axisLength, |
| | 2 | 752 | | radius, |
| | 2 | 753 | | out distance); |
| | | 754 | | } |
| | | 755 | | |
| | | 756 | | /// <summary> |
| | | 757 | | /// Attempts to materialize support for a centered rotated capsule. |
| | | 758 | | /// </summary> |
| | | 759 | | public static bool TryGetCenteredCapsuleSupport( |
| | | 760 | | Vector2d center, |
| | | 761 | | Fixed64 frameRotation, |
| | | 762 | | Fixed64 axisLength, |
| | | 763 | | Fixed64 radius, |
| | | 764 | | Vector2d direction, |
| | | 765 | | out Vector2d support) |
| | | 766 | | { |
| | 2 | 767 | | ValidateRotatedCapsule(axisLength, radius); |
| | 1 | 768 | | return WideFiniteAxisIntersection.TryGetCenteredCapsuleSupport( |
| | 1 | 769 | | center, |
| | 1 | 770 | | GetRotatedPositiveYAxis(frameRotation), |
| | 1 | 771 | | axisLength, |
| | 1 | 772 | | radius, |
| | 1 | 773 | | direction, |
| | 1 | 774 | | out support); |
| | | 775 | | } |
| | | 776 | | |
| | | 777 | | /// <summary> |
| | | 778 | | /// Attempts to materialize the selected surface point of a centered |
| | | 779 | | /// rotated capsule. |
| | | 780 | | /// </summary> |
| | | 781 | | public static bool TryGetSurfacePointOnCenteredCapsule( |
| | | 782 | | Vector2d point, |
| | | 783 | | Vector2d center, |
| | | 784 | | Fixed64 frameRotation, |
| | | 785 | | Fixed64 axisLength, |
| | | 786 | | Fixed64 radius, |
| | | 787 | | Vector2d surfaceDirection, |
| | | 788 | | out Vector2d surfacePoint) |
| | | 789 | | { |
| | 2 | 790 | | ValidateRotatedCapsule(axisLength, radius); |
| | 2 | 791 | | if (!surfaceDirection.IsNormalized()) |
| | | 792 | | { |
| | 1 | 793 | | throw new ArgumentException( |
| | 1 | 794 | | "Surface direction must be normalized.", |
| | 1 | 795 | | nameof(surfaceDirection)); |
| | | 796 | | } |
| | | 797 | | |
| | 1 | 798 | | return WideFiniteAxisIntersection.TryGetSurfacePointOnCenteredCapsule( |
| | 1 | 799 | | point, |
| | 1 | 800 | | center, |
| | 1 | 801 | | GetRotatedPositiveYAxis(frameRotation), |
| | 1 | 802 | | axisLength, |
| | 1 | 803 | | radius, |
| | 1 | 804 | | surfaceDirection, |
| | 1 | 805 | | out surfacePoint); |
| | | 806 | | } |
| | | 807 | | |
| | | 808 | | /// <summary> |
| | | 809 | | /// Attempts to materialize the center-relative selected surface offset of |
| | | 810 | | /// a centered rotated capsule. |
| | | 811 | | /// </summary> |
| | | 812 | | public static bool TryGetSurfaceOffsetOnCenteredCapsule( |
| | | 813 | | Vector2d point, |
| | | 814 | | Vector2d center, |
| | | 815 | | Fixed64 frameRotation, |
| | | 816 | | Fixed64 axisLength, |
| | | 817 | | Fixed64 radius, |
| | | 818 | | Vector2d surfaceDirection, |
| | | 819 | | out Vector2d surfaceOffset) |
| | | 820 | | { |
| | 2 | 821 | | ValidateRotatedCapsule(axisLength, radius); |
| | 2 | 822 | | if (!surfaceDirection.IsNormalized()) |
| | | 823 | | { |
| | 1 | 824 | | throw new ArgumentException( |
| | 1 | 825 | | "Surface direction must be normalized.", |
| | 1 | 826 | | nameof(surfaceDirection)); |
| | | 827 | | } |
| | | 828 | | |
| | 1 | 829 | | return WideFiniteAxisIntersection.TryGetSurfaceOffsetOnCenteredCapsule( |
| | 1 | 830 | | point, |
| | 1 | 831 | | center, |
| | 1 | 832 | | GetRotatedPositiveYAxis(frameRotation), |
| | 1 | 833 | | axisLength, |
| | 1 | 834 | | radius, |
| | 1 | 835 | | surfaceDirection, |
| | 1 | 836 | | out surfaceOffset); |
| | | 837 | | } |
| | | 838 | | |
| | | 839 | | /// <summary> |
| | | 840 | | /// Returns whether two centered capsules expressed by scalar rigid-frame |
| | | 841 | | /// rotations overlap, including exact tangency. |
| | | 842 | | /// </summary> |
| | | 843 | | public static bool DoCenteredCapsulesOverlap( |
| | | 844 | | Vector2d firstCenter, |
| | | 845 | | Fixed64 firstRotation, |
| | | 846 | | Fixed64 firstAxisLength, |
| | | 847 | | Fixed64 firstRadius, |
| | | 848 | | Vector2d secondCenter, |
| | | 849 | | Fixed64 secondRotation, |
| | | 850 | | Fixed64 secondAxisLength, |
| | | 851 | | Fixed64 secondRadius) |
| | | 852 | | { |
| | 1 | 853 | | ValidateRotatedCapsule(firstAxisLength, firstRadius); |
| | 1 | 854 | | ValidateRotatedCapsule(secondAxisLength, secondRadius); |
| | 1 | 855 | | return WideFiniteAxisIntersection.DoCenteredCapsulesOverlap( |
| | 1 | 856 | | firstCenter, |
| | 1 | 857 | | GetRotatedPositiveYAxis(firstRotation), |
| | 1 | 858 | | firstAxisLength, |
| | 1 | 859 | | firstRadius, |
| | 1 | 860 | | secondCenter, |
| | 1 | 861 | | GetRotatedPositiveYAxis(secondRotation), |
| | 1 | 862 | | secondAxisLength, |
| | 1 | 863 | | secondRadius); |
| | | 864 | | } |
| | | 865 | | |
| | | 866 | | /// <summary> |
| | | 867 | | /// Attempts to build exact anchors between two centered capsules expressed |
| | | 868 | | /// by scalar rigid-frame rotations. |
| | | 869 | | /// </summary> |
| | | 870 | | public static bool TryGetCenteredCapsulesContact( |
| | | 871 | | Vector2d firstCenter, |
| | | 872 | | Fixed64 firstRotation, |
| | | 873 | | Fixed64 firstAxisLength, |
| | | 874 | | Fixed64 firstRadius, |
| | | 875 | | Vector2d secondCenter, |
| | | 876 | | Fixed64 secondRotation, |
| | | 877 | | Fixed64 secondAxisLength, |
| | | 878 | | Fixed64 secondRadius, |
| | | 879 | | Vector2d fallbackNormal, |
| | | 880 | | out FixedContactAnchors2d contact) |
| | | 881 | | { |
| | 2 | 882 | | ValidateRotatedCapsule(firstAxisLength, firstRadius); |
| | 2 | 883 | | ValidateRotatedCapsule(secondAxisLength, secondRadius); |
| | 2 | 884 | | if (!fallbackNormal.IsNormalized()) |
| | | 885 | | { |
| | 1 | 886 | | throw new ArgumentException( |
| | 1 | 887 | | "Fallback normal must be normalized.", |
| | 1 | 888 | | nameof(fallbackNormal)); |
| | | 889 | | } |
| | | 890 | | |
| | 1 | 891 | | return WideFiniteAxisIntersection.TryGetCenteredCapsulesContact( |
| | 1 | 892 | | firstCenter, |
| | 1 | 893 | | firstRotation, |
| | 1 | 894 | | GetRotatedPositiveYAxis(firstRotation), |
| | 1 | 895 | | firstAxisLength, |
| | 1 | 896 | | firstRadius, |
| | 1 | 897 | | secondCenter, |
| | 1 | 898 | | secondRotation, |
| | 1 | 899 | | GetRotatedPositiveYAxis(secondRotation), |
| | 1 | 900 | | secondAxisLength, |
| | 1 | 901 | | secondRadius, |
| | 1 | 902 | | fallbackNormal, |
| | 1 | 903 | | out contact); |
| | | 904 | | } |
| | | 905 | | |
| | | 906 | | /// <summary> |
| | | 907 | | /// Finds the minimum translation that separates a centered rotated capsule |
| | | 908 | | /// from a closed convex polygon represented by origin-relative offsets. |
| | | 909 | | /// </summary> |
| | | 910 | | public static bool TryGetCenteredCapsuleConvexMinimumTranslation( |
| | | 911 | | Vector2d center, |
| | | 912 | | Fixed64 rotation, |
| | | 913 | | Fixed64 axisLength, |
| | | 914 | | Fixed64 radius, |
| | | 915 | | Vector2d convexOrigin, |
| | | 916 | | ReadOnlySpan<Vector2d> convexVertexOffsets, |
| | | 917 | | out Vector2d normal, |
| | | 918 | | out Fixed64 depth) |
| | | 919 | | { |
| | 2 | 920 | | ValidateRotatedCapsule(axisLength, radius); |
| | 2 | 921 | | if (convexVertexOffsets.Length < 3) |
| | | 922 | | { |
| | 1 | 923 | | throw new ArgumentException( |
| | 1 | 924 | | "A convex polygon requires at least three vertex offsets.", |
| | 1 | 925 | | nameof(convexVertexOffsets)); |
| | | 926 | | } |
| | | 927 | | |
| | 1 | 928 | | return WideCenteredCapsule2dRelations.TryGetMinimumTranslation( |
| | 1 | 929 | | center, |
| | 1 | 930 | | GetRotatedPositiveYAxis(rotation), |
| | 1 | 931 | | axisLength, |
| | 1 | 932 | | radius, |
| | 1 | 933 | | convexOrigin, |
| | 1 | 934 | | convexVertexOffsets, |
| | 1 | 935 | | out normal, |
| | 1 | 936 | | out depth); |
| | | 937 | | } |
| | | 938 | | |
| | | 939 | | /// <summary> |
| | | 940 | | /// Finds the first distance where a translated centered rotated capsule |
| | | 941 | | /// intersects an endpoint-authored capsule axis. |
| | | 942 | | /// </summary> |
| | | 943 | | public static bool TryGetSweptCenteredCapsuleSegmentFirstDistance( |
| | | 944 | | Vector2d center, |
| | | 945 | | Fixed64 rotation, |
| | | 946 | | Fixed64 axisLength, |
| | | 947 | | Fixed64 radius, |
| | | 948 | | Vector2d direction, |
| | | 949 | | Fixed64 maximumDistance, |
| | | 950 | | FixedSegment2d targetAxis, |
| | | 951 | | Fixed64 targetRadius, |
| | | 952 | | out Fixed64 distance, |
| | | 953 | | out Vector2d normal) |
| | | 954 | | { |
| | 1 | 955 | | ValidateSweep( |
| | 1 | 956 | | Vector2d.Forward, |
| | 1 | 957 | | axisLength, |
| | 1 | 958 | | radius, |
| | 1 | 959 | | direction, |
| | 1 | 960 | | maximumDistance, |
| | 1 | 961 | | targetRadius); |
| | 1 | 962 | | return WideFiniteAxisIntersection.TryGetSweptCenteredCapsuleSegmentFirstDistance( |
| | 1 | 963 | | center, |
| | 1 | 964 | | GetRotatedPositiveYAxis(rotation), |
| | 1 | 965 | | axisLength, |
| | 1 | 966 | | radius, |
| | 1 | 967 | | direction, |
| | 1 | 968 | | maximumDistance, |
| | 1 | 969 | | targetAxis, |
| | 1 | 970 | | targetRadius, |
| | 1 | 971 | | out distance, |
| | 1 | 972 | | out normal); |
| | | 973 | | } |
| | | 974 | | |
| | | 975 | | /// <summary> |
| | | 976 | | /// Finds the first distance where a translated centered rotated capsule |
| | | 977 | | /// intersects another centered rotated capsule. |
| | | 978 | | /// </summary> |
| | | 979 | | public static bool TryGetSweptCenteredCapsulesFirstDistance( |
| | | 980 | | Vector2d center, |
| | | 981 | | Fixed64 rotation, |
| | | 982 | | Fixed64 axisLength, |
| | | 983 | | Fixed64 radius, |
| | | 984 | | Vector2d direction, |
| | | 985 | | Fixed64 maximumDistance, |
| | | 986 | | Vector2d targetCenter, |
| | | 987 | | Fixed64 targetRotation, |
| | | 988 | | Fixed64 targetAxisLength, |
| | | 989 | | Fixed64 targetRadius, |
| | | 990 | | out Fixed64 distance, |
| | | 991 | | out Vector2d normal) |
| | | 992 | | { |
| | 1 | 993 | | ValidateSweep( |
| | 1 | 994 | | Vector2d.Forward, |
| | 1 | 995 | | axisLength, |
| | 1 | 996 | | radius, |
| | 1 | 997 | | direction, |
| | 1 | 998 | | maximumDistance, |
| | 1 | 999 | | targetRadius); |
| | 1 | 1000 | | ValidateCenteredAxis(Vector2d.Forward, targetAxisLength); |
| | 1 | 1001 | | return WideFiniteAxisIntersection.TryGetSweptCenteredCapsulesFirstDistance( |
| | 1 | 1002 | | center, |
| | 1 | 1003 | | GetRotatedPositiveYAxis(rotation), |
| | 1 | 1004 | | axisLength, |
| | 1 | 1005 | | radius, |
| | 1 | 1006 | | direction, |
| | 1 | 1007 | | maximumDistance, |
| | 1 | 1008 | | targetCenter, |
| | 1 | 1009 | | GetRotatedPositiveYAxis(targetRotation), |
| | 1 | 1010 | | targetAxisLength, |
| | 1 | 1011 | | targetRadius, |
| | 1 | 1012 | | out distance, |
| | 1 | 1013 | | out normal); |
| | | 1014 | | } |
| | | 1015 | | |
| | | 1016 | | /// <summary> |
| | | 1017 | | /// Finds the physical-distance interval where this segment intersects a |
| | | 1018 | | /// centered rotated capsule. |
| | | 1019 | | /// </summary> |
| | | 1020 | | public readonly bool TryGetCapsuleIntersectionDistanceInterval( |
| | | 1021 | | Vector2d center, |
| | | 1022 | | Fixed64 rotation, |
| | | 1023 | | Fixed64 axisLength, |
| | | 1024 | | Fixed64 radius, |
| | | 1025 | | Fixed64 radiusExpansion, |
| | | 1026 | | Fixed64 totalDistance, |
| | | 1027 | | out Fixed64 entryDistance, |
| | | 1028 | | out Fixed64 exitDistance, |
| | | 1029 | | out bool startContained, |
| | | 1030 | | out bool endContainedStrict) |
| | | 1031 | | { |
| | 2 | 1032 | | ValidateTotalDistance(totalDistance); |
| | 2 | 1033 | | ValidateRotatedCapsule(axisLength, radius); |
| | 2 | 1034 | | if (radiusExpansion < Fixed64.Zero) |
| | 1 | 1035 | | throw new ArgumentOutOfRangeException(nameof(radiusExpansion)); |
| | | 1036 | | |
| | 1 | 1037 | | return WideFiniteAxisIntersection.TryGetCapsuleDistanceInterval( |
| | 1 | 1038 | | this, |
| | 1 | 1039 | | center, |
| | 1 | 1040 | | GetRotatedPositiveYAxis(rotation), |
| | 1 | 1041 | | axisLength, |
| | 1 | 1042 | | radius, |
| | 1 | 1043 | | radiusExpansion, |
| | 1 | 1044 | | totalDistance, |
| | 1 | 1045 | | out entryDistance, |
| | 1 | 1046 | | out exitDistance, |
| | 1 | 1047 | | out startContained, |
| | 1 | 1048 | | out endContainedStrict); |
| | | 1049 | | } |
| | | 1050 | | |
| | | 1051 | | private static Vector2d GetRotatedPositiveYAxis(Fixed64 rotation) => |
| | 18 | 1052 | | new(-FixedMath.Sin(rotation), FixedMath.Cos(rotation)); |
| | | 1053 | | |
| | | 1054 | | private static void ValidateRotatedCapsule( |
| | | 1055 | | Fixed64 axisLength, |
| | | 1056 | | Fixed64 radius) |
| | | 1057 | | { |
| | 22 | 1058 | | ValidateCenteredAxis(Vector2d.Forward, axisLength); |
| | 22 | 1059 | | if (radius < Fixed64.Zero) |
| | 1 | 1060 | | throw new ArgumentOutOfRangeException(nameof(radius)); |
| | 21 | 1061 | | } |
| | | 1062 | | } |