| | | 1 | | //======================================================================= |
| | | 2 | | // FixedOrientedBox.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2024–present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using System; |
| | | 9 | | using System.Runtime.CompilerServices; |
| | | 10 | | using System.Text.Json.Serialization; |
| | | 11 | | |
| | | 12 | | namespace FixedMathSharp.Geometry; |
| | | 13 | | |
| | | 14 | | /// <summary> |
| | | 15 | | /// Represents an immutable oriented box through canonical center, orientation, |
| | | 16 | | /// and positive local half-extents. |
| | | 17 | | /// </summary> |
| | | 18 | | /// <remarks> |
| | | 19 | | /// World corners are intentionally not stored or exposed. Use local feature |
| | | 20 | | /// selection and <see cref="TryMaterializeLocalPoint"/> when a representable |
| | | 21 | | /// world-space witness is required. Geometry is evaluated against the exact |
| | | 22 | | /// scale-invariant rational basis of the stored quaternion; rounded axes are |
| | | 23 | | /// presentation values rather than query inputs. |
| | | 24 | | /// </remarks> |
| | | 25 | | public readonly struct FixedOrientedBox : IEquatable<FixedOrientedBox> |
| | | 26 | | { |
| | | 27 | | /// <summary> |
| | | 28 | | /// The number of stable local corners exposed by <see cref="GetLocalCorner"/>. |
| | | 29 | | /// </summary> |
| | | 30 | | public const int CornerCount = 8; |
| | | 31 | | |
| | | 32 | | /// <summary> |
| | | 33 | | /// Initializes an oriented box from canonical geometry. |
| | | 34 | | /// </summary> |
| | | 35 | | /// <exception cref="ArgumentException"> |
| | | 36 | | /// <paramref name="orientation"/> is not normalized. |
| | | 37 | | /// </exception> |
| | | 38 | | /// <exception cref="ArgumentOutOfRangeException"> |
| | | 39 | | /// At least one half-extent is not positive. |
| | | 40 | | /// </exception> |
| | | 41 | | [JsonConstructor] |
| | | 42 | | public FixedOrientedBox( |
| | | 43 | | Vector3d center, |
| | | 44 | | FixedQuaternion orientation, |
| | | 45 | | Vector3d halfExtents) |
| | | 46 | | { |
| | 758 | 47 | | if (!orientation.IsNormalized()) |
| | 3 | 48 | | throw new ArgumentException("The oriented-box rotation must be normalized.", nameof(orientation)); |
| | 755 | 49 | | if (!HasPositiveHalfExtents(halfExtents)) |
| | | 50 | | { |
| | 3 | 51 | | throw new ArgumentOutOfRangeException( |
| | 3 | 52 | | nameof(halfExtents), |
| | 3 | 53 | | "Every oriented-box half-extent must be positive."); |
| | | 54 | | } |
| | | 55 | | |
| | 752 | 56 | | Center = center; |
| | 752 | 57 | | Orientation = orientation; |
| | 752 | 58 | | HalfExtents = halfExtents; |
| | 752 | 59 | | } |
| | | 60 | | |
| | | 61 | | /// <summary> |
| | | 62 | | /// The world-space center. |
| | | 63 | | /// </summary> |
| | | 64 | | [JsonInclude] |
| | | 65 | | public Vector3d Center { get; } |
| | | 66 | | |
| | | 67 | | /// <summary> |
| | | 68 | | /// The normalized local-to-world orientation. |
| | | 69 | | /// </summary> |
| | | 70 | | [JsonInclude] |
| | | 71 | | public FixedQuaternion Orientation { get; } |
| | | 72 | | |
| | | 73 | | /// <summary> |
| | | 74 | | /// The positive local face distances. |
| | | 75 | | /// </summary> |
| | | 76 | | [JsonInclude] |
| | | 77 | | public Vector3d HalfExtents { get; } |
| | | 78 | | |
| | | 79 | | /// <summary> |
| | | 80 | | /// Gets the nearest round-half-to-even <see cref="Fixed64"/> views of all |
| | | 81 | | /// three exact local-to-world axes. |
| | | 82 | | /// </summary> |
| | | 83 | | public void GetAxes( |
| | | 84 | | out Vector3d axisX, |
| | | 85 | | out Vector3d axisY, |
| | | 86 | | out Vector3d axisZ) |
| | | 87 | | { |
| | 529 | 88 | | EnsureValid(); |
| | 528 | 89 | | GetAxesUnchecked(out axisX, out axisY, out axisZ); |
| | 528 | 90 | | } |
| | | 91 | | |
| | | 92 | | /// <summary> |
| | | 93 | | /// Gets a stable center-relative local corner. |
| | | 94 | | /// </summary> |
| | | 95 | | /// <remarks> |
| | | 96 | | /// Index bits select positive X, Y, and Z respectively. The order is |
| | | 97 | | /// <c>---</c>, <c>+--</c>, <c>-+-</c>, <c>++-</c>, <c>--+</c>, |
| | | 98 | | /// <c>+-+</c>, <c>-++</c>, <c>+++</c>. |
| | | 99 | | /// </remarks> |
| | | 100 | | public Vector3d GetLocalCorner(int index) |
| | | 101 | | { |
| | 522 | 102 | | EnsureValid(); |
| | 521 | 103 | | if ((uint)index >= CornerCount) |
| | | 104 | | { |
| | 2 | 105 | | throw new ArgumentOutOfRangeException( |
| | 2 | 106 | | nameof(index), |
| | 2 | 107 | | $"Corner index must be between 0 and {CornerCount - 1}."); |
| | | 108 | | } |
| | | 109 | | |
| | 519 | 110 | | return new Vector3d( |
| | 519 | 111 | | (index & 1) == 0 ? -HalfExtents.X : HalfExtents.X, |
| | 519 | 112 | | (index & 2) == 0 ? -HalfExtents.Y : HalfExtents.Y, |
| | 519 | 113 | | (index & 4) == 0 ? -HalfExtents.Z : HalfExtents.Z); |
| | | 114 | | } |
| | | 115 | | |
| | | 116 | | /// <summary> |
| | | 117 | | /// Gets the center-relative local support point for a world-space direction. |
| | | 118 | | /// </summary> |
| | | 119 | | /// <remarks> |
| | | 120 | | /// An exact zero projection selects the negative extent so ties retain the |
| | | 121 | | /// lower local corner index. |
| | | 122 | | /// </remarks> |
| | | 123 | | public Vector3d GetLocalSupportPoint(Vector3d worldDirection) |
| | | 124 | | { |
| | 516 | 125 | | EnsureValid(); |
| | 515 | 126 | | return WideOrientedBox.GetLocalSupportPoint( |
| | 515 | 127 | | worldDirection, |
| | 515 | 128 | | Orientation, |
| | 515 | 129 | | HalfExtents); |
| | | 130 | | } |
| | | 131 | | |
| | | 132 | | /// <summary> |
| | | 133 | | /// Gets the least-outward analytical axis-aligned bounds, clipped only at |
| | | 134 | | /// the final representable scalar endpoints. |
| | | 135 | | /// </summary> |
| | | 136 | | public FixedBoundBox GetBoundsClippedToDomain() |
| | | 137 | | { |
| | 517 | 138 | | EnsureValid(); |
| | 516 | 139 | | return WideOrientedBox.GetBoundsClippedToDomain( |
| | 516 | 140 | | Center, |
| | 516 | 141 | | Orientation, |
| | 516 | 142 | | HalfExtents); |
| | | 143 | | } |
| | | 144 | | |
| | | 145 | | /// <summary> |
| | | 146 | | /// Returns whether a world-space point lies inside or on every conceptual |
| | | 147 | | /// box face. |
| | | 148 | | /// </summary> |
| | | 149 | | public bool Contains(Vector3d point) |
| | | 150 | | { |
| | 2703 | 151 | | EnsureValid(); |
| | 2702 | 152 | | return WideOrientedBox.Contains( |
| | 2702 | 153 | | point, |
| | 2702 | 154 | | Center, |
| | 2702 | 155 | | Orientation, |
| | 2702 | 156 | | HalfExtents); |
| | | 157 | | } |
| | | 158 | | |
| | | 159 | | /// <summary> |
| | | 160 | | /// Gets the conceptual closest surface point in this box's rigid frame. |
| | | 161 | | /// </summary> |
| | | 162 | | /// <remarks> |
| | | 163 | | /// Outside points clamp independently to the local extents. Inside points |
| | | 164 | | /// select the nearest face with stable X, then Y, then Z ties. The returned |
| | | 165 | | /// anchor remains valid when the selected absolute world point is outside |
| | | 166 | | /// the representable scalar domain. |
| | | 167 | | /// </remarks> |
| | | 168 | | public FixedPointAnchor GetClosestPointAnchor(Vector3d point) |
| | | 169 | | { |
| | 524 | 170 | | EnsureValid(); |
| | 523 | 171 | | return WideOrientedBox.GetClosestPointAnchor( |
| | 523 | 172 | | point, |
| | 523 | 173 | | Center, |
| | 523 | 174 | | Orientation, |
| | 523 | 175 | | HalfExtents); |
| | | 176 | | } |
| | | 177 | | |
| | | 178 | | /// <summary> |
| | | 179 | | /// Attempts to materialize the nearest lattice representation of the |
| | | 180 | | /// conceptual closest surface point. |
| | | 181 | | /// </summary> |
| | | 182 | | /// <remarks> |
| | | 183 | | /// Outside points clamp independently to the local extents. Inside points |
| | | 184 | | /// select the nearest face with stable X, then Y, then Z ties. The method |
| | | 185 | | /// returns <see langword="false"/> only when the selected final world point |
| | | 186 | | /// is not representable. |
| | | 187 | | /// </remarks> |
| | | 188 | | public bool TryGetClosestPointOnSurface( |
| | | 189 | | Vector3d point, |
| | | 190 | | out Vector3d closestPoint) |
| | 523 | 191 | | => GetClosestPointAnchor(point).TryGetPoint(out closestPoint); |
| | | 192 | | |
| | | 193 | | /// <summary> |
| | | 194 | | /// Gets the nearest representable normal of the nearest conceptual face. |
| | | 195 | | /// </summary> |
| | | 196 | | /// <remarks> |
| | | 197 | | /// Outside points select the first violated axis in X, then Y, then Z |
| | | 198 | | /// order, matching their nearest clamped surface feature. For contained |
| | | 199 | | /// points, equal inward face distances use the same axis order. A zero |
| | | 200 | | /// local projection selects the positive face. |
| | | 201 | | /// </remarks> |
| | | 202 | | public Vector3d GetNearestFaceNormal(Vector3d point) |
| | | 203 | | { |
| | 527 | 204 | | EnsureValid(); |
| | 526 | 205 | | return WideOrientedBox.GetNearestFaceNormal( |
| | 526 | 206 | | point, |
| | 526 | 207 | | Center, |
| | 526 | 208 | | Orientation, |
| | 526 | 209 | | HalfExtents); |
| | | 210 | | } |
| | | 211 | | |
| | | 212 | | /// <summary> |
| | | 213 | | /// Attempts to materialize a center-relative local point in world space. |
| | | 214 | | /// </summary> |
| | | 215 | | /// <remarks> |
| | | 216 | | /// Each coordinate retains the exact rational quaternion basis, all three |
| | | 217 | | /// products, and the center contribution until one final round-half-to-even |
| | | 218 | | /// conversion. The result is the nearest lattice representation of the |
| | | 219 | | /// conceptual point; a conceptual boundary point need not remain exactly on |
| | | 220 | | /// that boundary after quantization. Failure is atomic. |
| | | 221 | | /// </remarks> |
| | | 222 | | public bool TryMaterializeLocalPoint( |
| | | 223 | | Vector3d localPoint, |
| | | 224 | | out Vector3d worldPoint) |
| | | 225 | | { |
| | 528 | 226 | | EnsureValid(); |
| | 527 | 227 | | return WideOrientedBox.TryMaterializeLocalPoint( |
| | 527 | 228 | | Center, |
| | 527 | 229 | | Orientation, |
| | 527 | 230 | | localPoint, |
| | 527 | 231 | | out worldPoint); |
| | | 232 | | } |
| | | 233 | | |
| | | 234 | | /// <summary> |
| | | 235 | | /// Attempts to rotate a center-relative local offset into world-space |
| | | 236 | | /// coordinates without adding <see cref="Center"/>. |
| | | 237 | | /// </summary> |
| | | 238 | | /// <remarks> |
| | | 239 | | /// All three quaternion-basis products are retained until one final |
| | | 240 | | /// round-half-to-even conversion per component. This is the canonical |
| | | 241 | | /// admission path for relative box features whose absolute world points |
| | | 242 | | /// may lie outside the scalar coordinate domain. |
| | | 243 | | /// </remarks> |
| | | 244 | | public bool TryTransformLocalOffset( |
| | | 245 | | Vector3d localOffset, |
| | | 246 | | out Vector3d worldOffset) |
| | | 247 | | { |
| | 2 | 248 | | EnsureValid(); |
| | 2 | 249 | | return WideOrientedBox.TryTransformLocalOffset( |
| | 2 | 250 | | Orientation, |
| | 2 | 251 | | localOffset, |
| | 2 | 252 | | out worldOffset); |
| | | 253 | | } |
| | | 254 | | |
| | | 255 | | /// <summary> |
| | | 256 | | /// Attempts to return the center-relative world-space support offset for a |
| | | 257 | | /// direction. |
| | | 258 | | /// </summary> |
| | | 259 | | /// <remarks> |
| | | 260 | | /// Exact zero projections select the negative local extent so ties retain |
| | | 261 | | /// the stable lower corner index. |
| | | 262 | | /// </remarks> |
| | | 263 | | public bool TryGetSupportOffset( |
| | | 264 | | Vector3d worldDirection, |
| | | 265 | | out Vector3d centerOffset) |
| | | 266 | | { |
| | 4 | 267 | | EnsureValid(); |
| | 4 | 268 | | return WideOrientedBox.TryGetSupportOffset( |
| | 4 | 269 | | worldDirection, |
| | 4 | 270 | | Orientation, |
| | 4 | 271 | | HalfExtents, |
| | 4 | 272 | | out centerOffset); |
| | | 273 | | } |
| | | 274 | | |
| | | 275 | | /// <summary> |
| | | 276 | | /// Attempts to construct one exact-final-narrowing Minkowski support |
| | | 277 | | /// difference between this box and another origin-relative support. |
| | | 278 | | /// </summary> |
| | | 279 | | /// <remarks> |
| | | 280 | | /// The result is |
| | | 281 | | /// <c>(Center + boxSupport) - (otherOrigin + otherOriginSupportOffset)</c>. |
| | | 282 | | /// Neither absolute support point is materialized independently. |
| | | 283 | | /// </remarks> |
| | | 284 | | public bool TryGetSupportDifference( |
| | | 285 | | Vector3d otherOrigin, |
| | | 286 | | Vector3d otherOriginSupportOffset, |
| | | 287 | | Vector3d worldDirection, |
| | | 288 | | out Vector3d difference) |
| | | 289 | | { |
| | 3 | 290 | | EnsureValid(); |
| | 3 | 291 | | return WideOrientedBox.TryGetSupportDifference( |
| | 3 | 292 | | Center, |
| | 3 | 293 | | Orientation, |
| | 3 | 294 | | HalfExtents, |
| | 3 | 295 | | otherOrigin, |
| | 3 | 296 | | otherOriginSupportOffset, |
| | 3 | 297 | | worldDirection, |
| | 3 | 298 | | out difference); |
| | | 299 | | } |
| | | 300 | | |
| | | 301 | | /// <summary> |
| | | 302 | | /// Attempts to construct the exact Minkowski support difference between |
| | | 303 | | /// this box and <paramref name="other"/>. |
| | | 304 | | /// </summary> |
| | | 305 | | /// <remarks> |
| | | 306 | | /// Both rotated support offsets and the center difference are retained as |
| | | 307 | | /// rational wide values until one final narrowing per component. |
| | | 308 | | /// </remarks> |
| | | 309 | | public bool TryGetSupportDifference( |
| | | 310 | | FixedOrientedBox other, |
| | | 311 | | Vector3d worldDirection, |
| | | 312 | | out Vector3d difference) |
| | | 313 | | { |
| | 2 | 314 | | EnsureValid(); |
| | 2 | 315 | | other.EnsureValid(); |
| | 2 | 316 | | return WideOrientedBox.TryGetSupportDifference( |
| | 2 | 317 | | Center, |
| | 2 | 318 | | Orientation, |
| | 2 | 319 | | HalfExtents, |
| | 2 | 320 | | other.Center, |
| | 2 | 321 | | other.Orientation, |
| | 2 | 322 | | other.HalfExtents, |
| | 2 | 323 | | worldDirection, |
| | 2 | 324 | | out difference); |
| | | 325 | | } |
| | | 326 | | |
| | | 327 | | /// <summary> |
| | | 328 | | /// Finds the closed parameter interval where a bounded ray overlaps this |
| | | 329 | | /// oriented box. |
| | | 330 | | /// </summary> |
| | | 331 | | /// <remarks> |
| | | 332 | | /// World-to-local projections and slab clipping retain the exact rational |
| | | 333 | | /// quaternion basis. The direction need not be normalized. |
| | | 334 | | /// </remarks> |
| | | 335 | | public bool TryGetRayIntersectionInterval( |
| | | 336 | | FixedRay ray, |
| | | 337 | | Fixed64 maxParameter, |
| | | 338 | | out Fixed64 entry, |
| | | 339 | | out Fixed64 exit) |
| | | 340 | | { |
| | 12 | 341 | | EnsureValid(); |
| | 12 | 342 | | if (maxParameter < Fixed64.Zero) |
| | 1 | 343 | | throw new ArgumentOutOfRangeException(nameof(maxParameter)); |
| | | 344 | | |
| | 11 | 345 | | return WideOrientedBox.TryGetRayIntersectionInterval( |
| | 11 | 346 | | Center, |
| | 11 | 347 | | Orientation, |
| | 11 | 348 | | HalfExtents, |
| | 11 | 349 | | ray.Position, |
| | 11 | 350 | | ray.Direction, |
| | 11 | 351 | | maxParameter, |
| | 11 | 352 | | out entry, |
| | 11 | 353 | | out exit); |
| | | 354 | | } |
| | | 355 | | |
| | | 356 | | /// <summary> |
| | | 357 | | /// Determines whether two boxes have identical canonical state. |
| | | 358 | | /// </summary> |
| | | 359 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 360 | | public static bool operator ==(FixedOrientedBox left, FixedOrientedBox right) => |
| | 2 | 361 | | left.Equals(right); |
| | | 362 | | |
| | | 363 | | /// <summary> |
| | | 364 | | /// Determines whether two boxes have different canonical state. |
| | | 365 | | /// </summary> |
| | | 366 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 367 | | public static bool operator !=(FixedOrientedBox left, FixedOrientedBox right) => |
| | 2 | 368 | | !left.Equals(right); |
| | | 369 | | |
| | | 370 | | /// <inheritdoc/> |
| | | 371 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 372 | | public override bool Equals(object? obj) => |
| | 2 | 373 | | obj is FixedOrientedBox other && Equals(other); |
| | | 374 | | |
| | | 375 | | /// <inheritdoc/> |
| | | 376 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 377 | | public bool Equals(FixedOrientedBox other) => |
| | 7 | 378 | | Center.Equals(other.Center) |
| | 7 | 379 | | && Orientation.Equals(other.Orientation) |
| | 7 | 380 | | && HalfExtents.Equals(other.HalfExtents); |
| | | 381 | | |
| | | 382 | | /// <inheritdoc/> |
| | | 383 | | public override int GetHashCode() |
| | | 384 | | { |
| | | 385 | | unchecked |
| | | 386 | | { |
| | 2 | 387 | | int hash = 17; |
| | 2 | 388 | | hash = (hash * 31) + Center.GetHashCode(); |
| | 2 | 389 | | hash = (hash * 31) + Orientation.GetHashCode(); |
| | 2 | 390 | | hash = (hash * 31) + HalfExtents.GetHashCode(); |
| | 2 | 391 | | return hash; |
| | | 392 | | } |
| | | 393 | | } |
| | | 394 | | |
| | | 395 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 396 | | private void GetAxesUnchecked( |
| | | 397 | | out Vector3d axisX, |
| | | 398 | | out Vector3d axisY, |
| | | 399 | | out Vector3d axisZ) => |
| | 528 | 400 | | WideOrientedBox.GetAxes(Orientation, out axisX, out axisY, out axisZ); |
| | | 401 | | |
| | | 402 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 403 | | private void EnsureValid() |
| | | 404 | | { |
| | 8003 | 405 | | if (!HasPositiveHalfExtents(HalfExtents)) |
| | | 406 | | { |
| | 9 | 407 | | throw new InvalidOperationException("The oriented box does not contain valid canonical geometry."); |
| | | 408 | | } |
| | 7994 | 409 | | } |
| | | 410 | | |
| | | 411 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 412 | | private static bool HasPositiveHalfExtents(Vector3d halfExtents) => |
| | 8758 | 413 | | halfExtents.X > Fixed64.Zero |
| | 8758 | 414 | | && halfExtents.Y > Fixed64.Zero |
| | 8758 | 415 | | && halfExtents.Z > Fixed64.Zero; |
| | | 416 | | |
| | | 417 | | /// <summary> |
| | | 418 | | /// Attempts to construct an exact contact against another oriented box. |
| | | 419 | | /// </summary> |
| | | 420 | | public bool TryGetContact( |
| | | 421 | | FixedOrientedBox other, |
| | | 422 | | out FixedContactAnchors contact) |
| | | 423 | | { |
| | 15 | 424 | | EnsureValid(); |
| | 15 | 425 | | other.EnsureValid(); |
| | 14 | 426 | | return WideOrientedBox.TryGetContact( |
| | 14 | 427 | | Center, |
| | 14 | 428 | | Orientation, |
| | 14 | 429 | | HalfExtents, |
| | 14 | 430 | | other.Center, |
| | 14 | 431 | | other.Orientation, |
| | 14 | 432 | | other.HalfExtents, |
| | 14 | 433 | | out contact); |
| | | 434 | | } |
| | | 435 | | |
| | | 436 | | /// <summary> |
| | | 437 | | /// Attempts to construct an exact contact against a nondegenerate |
| | | 438 | | /// triangle. |
| | | 439 | | /// </summary> |
| | | 440 | | /// <param name="triangleOrigin"> |
| | | 441 | | /// The triangle's rigid-frame origin. |
| | | 442 | | /// </param> |
| | | 443 | | /// <param name="triangleRotation">The triangle's local-to-world rotation.</param> |
| | | 444 | | /// <param name="triangle">The triangle in its rigid-frame coordinates.</param> |
| | | 445 | | /// <param name="contact"> |
| | | 446 | | /// The box-center-relative and triangle-origin-relative contact relation. |
| | | 447 | | /// </param> |
| | | 448 | | /// <returns> |
| | | 449 | | /// <see langword="true"/> when the shapes overlap and both relative |
| | | 450 | | /// contact anchors are representable; otherwise <see langword="false"/>. |
| | | 451 | | /// </returns> |
| | | 452 | | public bool TryGetTriangleContact( |
| | | 453 | | Vector3d triangleOrigin, |
| | | 454 | | FixedQuaternion triangleRotation, |
| | | 455 | | FixedTriangle triangle, |
| | | 456 | | out FixedContactAnchors contact) |
| | | 457 | | { |
| | 142 | 458 | | EnsureValid(); |
| | 142 | 459 | | if (!triangleRotation.IsNormalized()) |
| | | 460 | | { |
| | 1 | 461 | | throw new ArgumentException( |
| | 1 | 462 | | "The triangle rotation must be normalized.", |
| | 1 | 463 | | nameof(triangleRotation)); |
| | | 464 | | } |
| | 141 | 465 | | return WideOrientedBox.TryGetTriangleContact( |
| | 141 | 466 | | Center, |
| | 141 | 467 | | Orientation, |
| | 141 | 468 | | HalfExtents, |
| | 141 | 469 | | triangleOrigin, |
| | 141 | 470 | | triangleRotation, |
| | 141 | 471 | | triangle, |
| | 141 | 472 | | out contact); |
| | | 473 | | } |
| | | 474 | | |
| | | 475 | | /// <summary> |
| | | 476 | | /// Attempts to construct an exact triangle contact and, for a parallel |
| | | 477 | | /// box-face/triangle-face feature, clips stable box corners to the |
| | | 478 | | /// triangle. |
| | | 479 | | /// </summary> |
| | | 480 | | /// <remarks> |
| | | 481 | | /// <paramref name="faceContacts"/> must provide capacity for four |
| | | 482 | | /// contacts. <paramref name="contact"/> always contains the primary exact |
| | | 483 | | /// contact relation when the shapes overlap. <paramref name="faceContactCount"/> |
| | | 484 | | /// is zero for non-face features. |
| | | 485 | | /// </remarks> |
| | | 486 | | public bool TryGetTriangleContact( |
| | | 487 | | Vector3d triangleOrigin, |
| | | 488 | | FixedQuaternion triangleRotation, |
| | | 489 | | FixedTriangle triangle, |
| | | 490 | | Span<FixedContactLocalPoints> faceContacts, |
| | | 491 | | out FixedContactAnchors contact, |
| | | 492 | | out int faceContactCount) |
| | | 493 | | { |
| | 11 | 494 | | EnsureValid(); |
| | 11 | 495 | | if (faceContacts.Length < 4) |
| | | 496 | | { |
| | 1 | 497 | | throw new ArgumentException( |
| | 1 | 498 | | "Triangle face-contact output requires capacity for four contacts.", |
| | 1 | 499 | | nameof(faceContacts)); |
| | | 500 | | } |
| | 10 | 501 | | if (!triangleRotation.IsNormalized()) |
| | | 502 | | { |
| | 1 | 503 | | throw new ArgumentException( |
| | 1 | 504 | | "The triangle rotation must be normalized.", |
| | 1 | 505 | | nameof(triangleRotation)); |
| | | 506 | | } |
| | 9 | 507 | | if (!WideOrientedBox.TryGetTriangleContact( |
| | 9 | 508 | | Center, |
| | 9 | 509 | | Orientation, |
| | 9 | 510 | | HalfExtents, |
| | 9 | 511 | | triangleOrigin, |
| | 9 | 512 | | triangleRotation, |
| | 9 | 513 | | triangle, |
| | 9 | 514 | | out contact)) |
| | | 515 | | { |
| | 1 | 516 | | faceContactCount = 0; |
| | 1 | 517 | | return false; |
| | | 518 | | } |
| | | 519 | | |
| | 8 | 520 | | WideOrientedBox.GetTriangleFaceContacts( |
| | 8 | 521 | | Center, |
| | 8 | 522 | | Orientation, |
| | 8 | 523 | | HalfExtents, |
| | 8 | 524 | | triangleOrigin, |
| | 8 | 525 | | triangleRotation, |
| | 8 | 526 | | triangle, |
| | 8 | 527 | | contact, |
| | 8 | 528 | | faceContacts, |
| | 8 | 529 | | out faceContactCount); |
| | 8 | 530 | | return true; |
| | | 531 | | } |
| | | 532 | | |
| | | 533 | | /// <summary> |
| | | 534 | | /// Attempts to construct an exact contact against a convex point span. |
| | | 535 | | /// </summary> |
| | | 536 | | /// <remarks> |
| | | 537 | | /// Hull points are origin-relative offsets expressed in the hull's local |
| | | 538 | | /// orientation. Triangle and edge indices provide the complete convex SAT |
| | | 539 | | /// topology. The method performs no absolute world-point materialization. |
| | | 540 | | /// </remarks> |
| | | 541 | | public bool TryGetConvexHullContact( |
| | | 542 | | Vector3d hullOrigin, |
| | | 543 | | FixedQuaternion hullOrientation, |
| | | 544 | | ReadOnlySpan<Vector3d> hullLocalOffsets, |
| | | 545 | | ReadOnlySpan<int> triangleVertexIndices, |
| | | 546 | | ReadOnlySpan<int> edgeVertexPairs, |
| | | 547 | | out FixedContactAnchors contact) |
| | | 548 | | { |
| | 84 | 549 | | EnsureValid(); |
| | 84 | 550 | | if (!hullOrientation.IsNormalized()) |
| | | 551 | | { |
| | 1 | 552 | | throw new ArgumentException( |
| | 1 | 553 | | "The convex-hull rotation must be normalized.", |
| | 1 | 554 | | nameof(hullOrientation)); |
| | | 555 | | } |
| | 83 | 556 | | if (hullLocalOffsets.Length < 4) |
| | | 557 | | { |
| | 1 | 558 | | throw new ArgumentException( |
| | 1 | 559 | | "A convex hull requires at least four origin-relative points.", |
| | 1 | 560 | | nameof(hullLocalOffsets)); |
| | | 561 | | } |
| | 82 | 562 | | if (triangleVertexIndices.Length == 0 |
| | 82 | 563 | | || triangleVertexIndices.Length % FixedTriangle.VertexCount != 0) |
| | | 564 | | { |
| | 2 | 565 | | throw new ArgumentException( |
| | 2 | 566 | | "Convex-hull triangle topology must be a nonempty multiple of three.", |
| | 2 | 567 | | nameof(triangleVertexIndices)); |
| | | 568 | | } |
| | 80 | 569 | | if (edgeVertexPairs.Length == 0 |
| | 80 | 570 | | || (edgeVertexPairs.Length & 1) != 0) |
| | | 571 | | { |
| | 2 | 572 | | throw new ArgumentException( |
| | 2 | 573 | | "Convex-hull edge topology must contain complete vertex-index pairs.", |
| | 2 | 574 | | nameof(edgeVertexPairs)); |
| | | 575 | | } |
| | 78 | 576 | | ValidateConvexHullIndices( |
| | 78 | 577 | | hullLocalOffsets.Length, |
| | 78 | 578 | | triangleVertexIndices, |
| | 78 | 579 | | nameof(triangleVertexIndices)); |
| | 77 | 580 | | ValidateConvexHullIndices( |
| | 77 | 581 | | hullLocalOffsets.Length, |
| | 77 | 582 | | edgeVertexPairs, |
| | 77 | 583 | | nameof(edgeVertexPairs)); |
| | 76 | 584 | | return WideOrientedBox.TryGetConvexHullContact( |
| | 76 | 585 | | Center, |
| | 76 | 586 | | Orientation, |
| | 76 | 587 | | HalfExtents, |
| | 76 | 588 | | hullOrigin, |
| | 76 | 589 | | hullOrientation, |
| | 76 | 590 | | hullLocalOffsets, |
| | 76 | 591 | | triangleVertexIndices, |
| | 76 | 592 | | edgeVertexPairs, |
| | 76 | 593 | | out contact); |
| | | 594 | | } |
| | | 595 | | |
| | | 596 | | private static void ValidateConvexHullIndices( |
| | | 597 | | int vertexCount, |
| | | 598 | | ReadOnlySpan<int> indices, |
| | | 599 | | string parameterName) |
| | | 600 | | { |
| | 9508 | 601 | | for (int index = 0; index < indices.Length; index++) |
| | | 602 | | { |
| | 4601 | 603 | | if ((uint)indices[index] >= (uint)vertexCount) |
| | | 604 | | { |
| | 2 | 605 | | throw new ArgumentOutOfRangeException( |
| | 2 | 606 | | parameterName, |
| | 2 | 607 | | "Convex-hull topology contains an invalid vertex index."); |
| | | 608 | | } |
| | | 609 | | } |
| | 153 | 610 | | } |
| | | 611 | | |
| | | 612 | | /// <summary> |
| | | 613 | | /// Attempts to construct an exact contact against a centered 3D capsule. |
| | | 614 | | /// </summary> |
| | | 615 | | public bool TryGetCenteredCapsuleContact( |
| | | 616 | | Vector3d capsuleCenter, |
| | | 617 | | FixedQuaternion capsuleRotation, |
| | | 618 | | Vector3d localCapsuleAxisDirection, |
| | | 619 | | Fixed64 capsuleAxisLength, |
| | | 620 | | Fixed64 capsuleRadius, |
| | | 621 | | out FixedContactAnchors contact) |
| | | 622 | | { |
| | 72 | 623 | | EnsureValid(); |
| | 72 | 624 | | if (!capsuleRotation.IsNormalized()) |
| | | 625 | | { |
| | 1 | 626 | | throw new ArgumentException( |
| | 1 | 627 | | "Capsule rotation must be normalized.", |
| | 1 | 628 | | nameof(capsuleRotation)); |
| | | 629 | | } |
| | 71 | 630 | | if (!localCapsuleAxisDirection.IsNormalized()) |
| | | 631 | | { |
| | 1 | 632 | | throw new ArgumentException( |
| | 1 | 633 | | "Local capsule axis direction must be normalized.", |
| | 1 | 634 | | nameof(localCapsuleAxisDirection)); |
| | | 635 | | } |
| | 70 | 636 | | if (capsuleAxisLength < Fixed64.Zero) |
| | 1 | 637 | | throw new ArgumentOutOfRangeException(nameof(capsuleAxisLength)); |
| | 69 | 638 | | if (capsuleRadius < Fixed64.Zero) |
| | 1 | 639 | | throw new ArgumentOutOfRangeException(nameof(capsuleRadius)); |
| | 68 | 640 | | return WideOrientedBox.TryGetCenteredCapsuleContact( |
| | 68 | 641 | | Center, |
| | 68 | 642 | | Orientation, |
| | 68 | 643 | | HalfExtents, |
| | 68 | 644 | | capsuleCenter, |
| | 68 | 645 | | capsuleRotation, |
| | 68 | 646 | | localCapsuleAxisDirection, |
| | 68 | 647 | | capsuleAxisLength, |
| | 68 | 648 | | capsuleRadius, |
| | 68 | 649 | | out contact); |
| | | 650 | | } |
| | | 651 | | |
| | | 652 | | /// <summary> |
| | | 653 | | /// Attempts to construct an exact contact against a centered finite |
| | | 654 | | /// cylinder. |
| | | 655 | | /// </summary> |
| | | 656 | | public bool TryGetCenteredCylinderContact( |
| | | 657 | | Vector3d cylinderCenter, |
| | | 658 | | FixedQuaternion cylinderRotation, |
| | | 659 | | Vector3d localCylinderAxisDirection, |
| | | 660 | | Fixed64 cylinderAxisLength, |
| | | 661 | | Fixed64 cylinderRadius, |
| | | 662 | | out FixedContactAnchors contact) |
| | | 663 | | { |
| | 14 | 664 | | EnsureValid(); |
| | 14 | 665 | | ValidateCenteredCylinder( |
| | 14 | 666 | | cylinderRotation, |
| | 14 | 667 | | localCylinderAxisDirection, |
| | 14 | 668 | | cylinderAxisLength, |
| | 14 | 669 | | cylinderRadius); |
| | 10 | 670 | | return WideOrientedBox.TryGetCenteredCylinderContact( |
| | 10 | 671 | | Center, |
| | 10 | 672 | | Orientation, |
| | 10 | 673 | | HalfExtents, |
| | 10 | 674 | | cylinderCenter, |
| | 10 | 675 | | cylinderRotation, |
| | 10 | 676 | | localCylinderAxisDirection, |
| | 10 | 677 | | cylinderAxisLength, |
| | 10 | 678 | | cylinderRadius, |
| | 10 | 679 | | out contact); |
| | | 680 | | } |
| | | 681 | | |
| | | 682 | | /// <summary> |
| | | 683 | | /// Attempts to construct an exact contact against a centered finite |
| | | 684 | | /// cylinder and, for a parallel cap-to-face feature, clips a stable |
| | | 685 | | /// allocation-free contact set from the disk/rectangle intersection. |
| | | 686 | | /// </summary> |
| | | 687 | | /// <remarks> |
| | | 688 | | /// <paramref name="capFaceContacts"/> must provide capacity for four |
| | | 689 | | /// contacts. <paramref name="contact"/> always contains the primary exact |
| | | 690 | | /// contact relation when the shapes overlap. <paramref name="capFaceContactCount"/> |
| | | 691 | | /// is zero for non-cap-face features, so callers should use the primary |
| | | 692 | | /// contact in that case. |
| | | 693 | | /// </remarks> |
| | | 694 | | public bool TryGetCenteredCylinderContact( |
| | | 695 | | Vector3d cylinderCenter, |
| | | 696 | | FixedQuaternion cylinderRotation, |
| | | 697 | | Vector3d localCylinderAxisDirection, |
| | | 698 | | Fixed64 cylinderAxisLength, |
| | | 699 | | Fixed64 cylinderRadius, |
| | | 700 | | Span<FixedContactLocalPoints> capFaceContacts, |
| | | 701 | | out FixedContactAnchors contact, |
| | | 702 | | out int capFaceContactCount) |
| | | 703 | | { |
| | 1017 | 704 | | EnsureValid(); |
| | 1017 | 705 | | ValidateCenteredCylinder( |
| | 1017 | 706 | | cylinderRotation, |
| | 1017 | 707 | | localCylinderAxisDirection, |
| | 1017 | 708 | | cylinderAxisLength, |
| | 1017 | 709 | | cylinderRadius); |
| | 1017 | 710 | | if (capFaceContacts.Length < 4) |
| | | 711 | | { |
| | 1 | 712 | | throw new ArgumentException( |
| | 1 | 713 | | "Cylinder cap-face contact output requires capacity for four contacts.", |
| | 1 | 714 | | nameof(capFaceContacts)); |
| | | 715 | | } |
| | | 716 | | |
| | 1016 | 717 | | if (!WideOrientedBox.TryGetCenteredCylinderContact( |
| | 1016 | 718 | | Center, |
| | 1016 | 719 | | Orientation, |
| | 1016 | 720 | | HalfExtents, |
| | 1016 | 721 | | cylinderCenter, |
| | 1016 | 722 | | cylinderRotation, |
| | 1016 | 723 | | localCylinderAxisDirection, |
| | 1016 | 724 | | cylinderAxisLength, |
| | 1016 | 725 | | cylinderRadius, |
| | 1016 | 726 | | out contact, |
| | 1016 | 727 | | out WideOrientedBox.CenteredCylinderContactFeature feature)) |
| | | 728 | | { |
| | 1 | 729 | | capFaceContactCount = 0; |
| | 1 | 730 | | return false; |
| | | 731 | | } |
| | | 732 | | |
| | 1015 | 733 | | WideOrientedBox.GetCenteredCylinderCapFaceContacts( |
| | 1015 | 734 | | Center, |
| | 1015 | 735 | | Orientation, |
| | 1015 | 736 | | HalfExtents, |
| | 1015 | 737 | | cylinderCenter, |
| | 1015 | 738 | | cylinderRotation, |
| | 1015 | 739 | | localCylinderAxisDirection, |
| | 1015 | 740 | | cylinderAxisLength, |
| | 1015 | 741 | | cylinderRadius, |
| | 1015 | 742 | | contact, |
| | 1015 | 743 | | feature, |
| | 1015 | 744 | | capFaceContacts, |
| | 1015 | 745 | | out capFaceContactCount); |
| | 1015 | 746 | | return true; |
| | | 747 | | } |
| | | 748 | | |
| | | 749 | | private static void ValidateCenteredCylinder( |
| | | 750 | | FixedQuaternion cylinderRotation, |
| | | 751 | | Vector3d localCylinderAxisDirection, |
| | | 752 | | Fixed64 cylinderAxisLength, |
| | | 753 | | Fixed64 cylinderRadius) |
| | | 754 | | { |
| | 1031 | 755 | | if (!cylinderRotation.IsNormalized()) |
| | | 756 | | { |
| | 1 | 757 | | throw new ArgumentException( |
| | 1 | 758 | | "Cylinder rotation must be normalized.", |
| | 1 | 759 | | nameof(cylinderRotation)); |
| | | 760 | | } |
| | 1030 | 761 | | if (!localCylinderAxisDirection.IsNormalized()) |
| | | 762 | | { |
| | 1 | 763 | | throw new ArgumentException( |
| | 1 | 764 | | "Local cylinder axis direction must be normalized.", |
| | 1 | 765 | | nameof(localCylinderAxisDirection)); |
| | | 766 | | } |
| | 1029 | 767 | | if (cylinderAxisLength <= Fixed64.Zero) |
| | 1 | 768 | | throw new ArgumentOutOfRangeException(nameof(cylinderAxisLength)); |
| | 1028 | 769 | | if (cylinderRadius < Fixed64.Zero) |
| | 1 | 770 | | throw new ArgumentOutOfRangeException(nameof(cylinderRadius)); |
| | 1027 | 771 | | } |
| | | 772 | | |
| | | 773 | | /// <summary> |
| | | 774 | | /// Attempts to construct an exact box-to-sphere contact using only |
| | | 775 | | /// center-relative contact offsets. |
| | | 776 | | /// </summary> |
| | | 777 | | /// <param name="sphereCenter">The sphere center.</param> |
| | | 778 | | /// <param name="sphereRotation"> |
| | | 779 | | /// The normalized rigid frame used to retain stable sphere-local contact |
| | | 780 | | /// feature identity. |
| | | 781 | | /// </param> |
| | | 782 | | /// <param name="sphereRadius">The nonnegative sphere radius.</param> |
| | | 783 | | /// <param name="contact">The relative contact relation when overlapping.</param> |
| | | 784 | | /// <returns> |
| | | 785 | | /// <see langword="true"/> when the conceptual shapes overlap and both |
| | | 786 | | /// required relative offsets are representable. |
| | | 787 | | /// </returns> |
| | | 788 | | public bool TryGetSphereContact( |
| | | 789 | | Vector3d sphereCenter, |
| | | 790 | | FixedQuaternion sphereRotation, |
| | | 791 | | Fixed64 sphereRadius, |
| | | 792 | | out FixedContactAnchors contact) |
| | | 793 | | { |
| | 21 | 794 | | EnsureValid(); |
| | 21 | 795 | | if (!sphereRotation.IsNormalized()) |
| | | 796 | | { |
| | 1 | 797 | | throw new ArgumentException( |
| | 1 | 798 | | "Sphere rotation must be normalized.", |
| | 1 | 799 | | nameof(sphereRotation)); |
| | | 800 | | } |
| | 20 | 801 | | if (sphereRadius < Fixed64.Zero) |
| | 1 | 802 | | throw new ArgumentOutOfRangeException(nameof(sphereRadius)); |
| | 19 | 803 | | return WideOrientedBox.TryGetSphereContact( |
| | 19 | 804 | | Center, |
| | 19 | 805 | | Orientation, |
| | 19 | 806 | | HalfExtents, |
| | 19 | 807 | | sphereCenter, |
| | 19 | 808 | | sphereRotation, |
| | 19 | 809 | | sphereRadius, |
| | 19 | 810 | | out contact); |
| | | 811 | | } |
| | | 812 | | |
| | | 813 | | /// <summary> |
| | | 814 | | /// Attempts to construct an exact contact against a rotated convex polygon |
| | | 815 | | /// extruded through a closed world-Y slab. |
| | | 816 | | /// </summary> |
| | | 817 | | /// <param name="prismOrigin">The representable prism origin.</param> |
| | | 818 | | /// <param name="prismRotation"> |
| | | 819 | | /// The prism's counterclockwise X/Z rotation in radians. |
| | | 820 | | /// </param> |
| | | 821 | | /// <param name="prismLocalOffsets"> |
| | | 822 | | /// Ordered clockwise or counterclockwise X/Z boundary offsets in the |
| | | 823 | | /// prism's local frame. |
| | | 824 | | /// </param> |
| | | 825 | | /// <param name="prismHalfThickness">The positive world-Y half-thickness.</param> |
| | | 826 | | /// <param name="contact">The frame-relative contact anchors when overlapping.</param> |
| | | 827 | | public bool TryGetConvexPrismContact( |
| | | 828 | | Vector3d prismOrigin, |
| | | 829 | | Fixed64 prismRotation, |
| | | 830 | | ReadOnlySpan<Vector2d> prismLocalOffsets, |
| | | 831 | | Fixed64 prismHalfThickness, |
| | | 832 | | out FixedContactAnchors contact) |
| | | 833 | | { |
| | 27 | 834 | | EnsureValid(); |
| | 27 | 835 | | if (prismLocalOffsets.Length < 3) |
| | | 836 | | { |
| | 1 | 837 | | throw new ArgumentException( |
| | 1 | 838 | | "A convex prism requires at least three ordered boundary offsets.", |
| | 1 | 839 | | nameof(prismLocalOffsets)); |
| | | 840 | | } |
| | 26 | 841 | | if (prismHalfThickness <= Fixed64.Zero) |
| | 2 | 842 | | throw new ArgumentOutOfRangeException(nameof(prismHalfThickness)); |
| | 24 | 843 | | return WideOrientedBox.TryGetConvexPrismContact( |
| | 24 | 844 | | Center, |
| | 24 | 845 | | Orientation, |
| | 24 | 846 | | HalfExtents, |
| | 24 | 847 | | prismOrigin, |
| | 24 | 848 | | prismRotation, |
| | 24 | 849 | | prismLocalOffsets, |
| | 24 | 850 | | prismHalfThickness, |
| | 24 | 851 | | out contact); |
| | | 852 | | } |
| | | 853 | | |
| | | 854 | | /// <summary> |
| | | 855 | | /// Attempts to construct an exact contact against a vertical circular |
| | | 856 | | /// prism centered in X/Z and extruded through a closed world-Y slab. |
| | | 857 | | /// </summary> |
| | | 858 | | public bool TryGetCircleSlabContact( |
| | | 859 | | Vector3d slabCenter, |
| | | 860 | | Fixed64 circleFrameRotation, |
| | | 861 | | Fixed64 slabHalfThickness, |
| | | 862 | | Fixed64 radius, |
| | | 863 | | out FixedContactAnchors contact) |
| | | 864 | | { |
| | 21 | 865 | | EnsureValid(); |
| | 21 | 866 | | if (slabHalfThickness <= Fixed64.Zero) |
| | 1 | 867 | | throw new ArgumentOutOfRangeException(nameof(slabHalfThickness)); |
| | 20 | 868 | | if (radius < Fixed64.Zero) |
| | 1 | 869 | | throw new ArgumentOutOfRangeException(nameof(radius)); |
| | 19 | 870 | | return WideOrientedBox.TryGetCircleSlabContact( |
| | 19 | 871 | | Center, |
| | 19 | 872 | | Orientation, |
| | 19 | 873 | | HalfExtents, |
| | 19 | 874 | | slabCenter, |
| | 19 | 875 | | circleFrameRotation, |
| | 19 | 876 | | slabHalfThickness, |
| | 19 | 877 | | radius, |
| | 19 | 878 | | out contact); |
| | | 879 | | } |
| | | 880 | | |
| | | 881 | | /// <summary> |
| | | 882 | | /// Attempts to construct an exact contact against a centered planar |
| | | 883 | | /// capsule extruded through a closed world-Y slab. |
| | | 884 | | /// </summary> |
| | | 885 | | public bool TryGetCenteredCapsuleSlabContact( |
| | | 886 | | Vector3d slabCenter, |
| | | 887 | | Fixed64 capsuleFrameRotation, |
| | | 888 | | Vector2d localCapsuleAxisDirection, |
| | | 889 | | Fixed64 capsuleAxisLength, |
| | | 890 | | Fixed64 capsuleRadius, |
| | | 891 | | Fixed64 slabHalfThickness, |
| | | 892 | | out FixedContactAnchors contact) |
| | | 893 | | { |
| | 16 | 894 | | EnsureValid(); |
| | 16 | 895 | | if (!localCapsuleAxisDirection.IsNormalized()) |
| | | 896 | | { |
| | 1 | 897 | | throw new ArgumentException( |
| | 1 | 898 | | "Local capsule axis direction must be normalized.", |
| | 1 | 899 | | nameof(localCapsuleAxisDirection)); |
| | | 900 | | } |
| | 15 | 901 | | if (capsuleAxisLength < Fixed64.Zero) |
| | 1 | 902 | | throw new ArgumentOutOfRangeException(nameof(capsuleAxisLength)); |
| | 14 | 903 | | if (capsuleRadius < Fixed64.Zero) |
| | 1 | 904 | | throw new ArgumentOutOfRangeException(nameof(capsuleRadius)); |
| | 13 | 905 | | if (slabHalfThickness <= Fixed64.Zero) |
| | 1 | 906 | | throw new ArgumentOutOfRangeException(nameof(slabHalfThickness)); |
| | 12 | 907 | | return WideOrientedBox.TryGetCenteredCapsuleSlabContact( |
| | 12 | 908 | | Center, |
| | 12 | 909 | | Orientation, |
| | 12 | 910 | | HalfExtents, |
| | 12 | 911 | | slabCenter, |
| | 12 | 912 | | capsuleFrameRotation, |
| | 12 | 913 | | localCapsuleAxisDirection, |
| | 12 | 914 | | capsuleAxisLength, |
| | 12 | 915 | | capsuleRadius, |
| | 12 | 916 | | slabHalfThickness, |
| | 12 | 917 | | out contact); |
| | | 918 | | } |
| | | 919 | | |
| | | 920 | | /// <summary> |
| | | 921 | | /// Finds the first X/Z travel distance at which a circle extruded through |
| | | 922 | | /// a fixed world-Y slab intersects this oriented box. |
| | | 923 | | /// </summary> |
| | | 924 | | /// <remarks> |
| | | 925 | | /// The slab-clipped box projection remains an internal wide rational |
| | | 926 | | /// relation. No box corner, slab intersection, or projected polygon must |
| | | 927 | | /// be representable. Tangency and start overlap are inclusive, and the |
| | | 928 | | /// returned distance uses round-half-to-even. |
| | | 929 | | /// </remarks> |
| | | 930 | | public bool TryGetCircleSlabSweepDistance( |
| | | 931 | | Vector3d slabStartCenter, |
| | | 932 | | Vector2d normalizedDirection, |
| | | 933 | | Fixed64 maxDistance, |
| | | 934 | | Fixed64 slabHalfThickness, |
| | | 935 | | Fixed64 radius, |
| | | 936 | | out Fixed64 distance) |
| | | 937 | | { |
| | 146 | 938 | | EnsureValid(); |
| | 146 | 939 | | if (!normalizedDirection.IsNormalized()) |
| | | 940 | | { |
| | 1 | 941 | | throw new ArgumentException( |
| | 1 | 942 | | "Sweep direction must be normalized.", |
| | 1 | 943 | | nameof(normalizedDirection)); |
| | | 944 | | } |
| | 145 | 945 | | if (maxDistance < Fixed64.Zero) |
| | 1 | 946 | | throw new ArgumentOutOfRangeException(nameof(maxDistance)); |
| | 144 | 947 | | if (slabHalfThickness <= Fixed64.Zero) |
| | 1 | 948 | | throw new ArgumentOutOfRangeException(nameof(slabHalfThickness)); |
| | 143 | 949 | | if (radius < Fixed64.Zero) |
| | 1 | 950 | | throw new ArgumentOutOfRangeException(nameof(radius)); |
| | 142 | 951 | | return WideOrientedBox.TryGetCircleSlabSweepDistance( |
| | 142 | 952 | | Center, |
| | 142 | 953 | | Orientation, |
| | 142 | 954 | | HalfExtents, |
| | 142 | 955 | | slabStartCenter, |
| | 142 | 956 | | normalizedDirection, |
| | 142 | 957 | | maxDistance, |
| | 142 | 958 | | slabHalfThickness, |
| | 142 | 959 | | radius, |
| | 142 | 960 | | out distance); |
| | | 961 | | } |
| | | 962 | | |
| | | 963 | | /// <summary> |
| | | 964 | | /// Returns a conservative X/Z separation lower bound between this box and |
| | | 965 | | /// a circle extruded through a fixed world-Y slab. |
| | | 966 | | /// </summary> |
| | | 967 | | /// <remarks> |
| | | 968 | | /// Projected edge witnesses are certified against every rational |
| | | 969 | | /// half-plane. When the closest feature is a projected vertex, the result |
| | | 970 | | /// uses the full-domain Euclidean distance floor before subtracting the |
| | | 971 | | /// circle radius. The result never overestimates true projected |
| | | 972 | | /// separation. When the Y intervals are disjoint, it returns the |
| | | 973 | | /// conservative vertical interval gap instead. |
| | | 974 | | /// </remarks> |
| | | 975 | | public Fixed64 GetCircleSlabSeparationLowerBound( |
| | | 976 | | Vector3d slabCenter, |
| | | 977 | | Fixed64 slabHalfThickness, |
| | | 978 | | Fixed64 radius) |
| | | 979 | | { |
| | 11 | 980 | | EnsureValid(); |
| | 11 | 981 | | if (slabHalfThickness <= Fixed64.Zero) |
| | 1 | 982 | | throw new ArgumentOutOfRangeException(nameof(slabHalfThickness)); |
| | 10 | 983 | | if (radius < Fixed64.Zero) |
| | 1 | 984 | | throw new ArgumentOutOfRangeException(nameof(radius)); |
| | 9 | 985 | | return WideOrientedBox.GetCircleSlabSeparationLowerBound( |
| | 9 | 986 | | Center, |
| | 9 | 987 | | Orientation, |
| | 9 | 988 | | HalfExtents, |
| | 9 | 989 | | slabCenter, |
| | 9 | 990 | | slabHalfThickness, |
| | 9 | 991 | | radius); |
| | | 992 | | } |
| | | 993 | | } |