| | | 1 | | //======================================================================= |
| | | 2 | | // CollisionDetection.Cuboid.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2026–present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using FixedMathSharp; |
| | | 9 | | using FixedMathSharp.Geometry; |
| | | 10 | | using Gravitas.Colliders; |
| | | 11 | | |
| | | 12 | | namespace Gravitas.CollisionHandling; |
| | | 13 | | |
| | | 14 | | public static partial class CollisionDetection |
| | | 15 | | { |
| | | 16 | | #region Cuboid |
| | | 17 | | |
| | | 18 | | private static bool DoCuboidSphereCheck(CollisionWorkItem pair) |
| | | 19 | | { |
| | 4690 | 20 | | var cuboid = (LSCuboidCollider)pair.ColliderA; |
| | 4690 | 21 | | if (!cuboid.OrientedBox.TryGetSphereContact( |
| | 4690 | 22 | | pair.ColliderB.Center, |
| | 4690 | 23 | | pair.ColliderB.Rotation, |
| | 4690 | 24 | | pair.ColliderB.ScaledRadius, |
| | 4690 | 25 | | out FixedContactAnchors contact)) |
| | | 26 | | { |
| | 2007 | 27 | | return false; |
| | | 28 | | } |
| | | 29 | | |
| | 2683 | 30 | | pair.Manifold.SetContact( |
| | 2683 | 31 | | new ContactAnchor(contact.FirstAnchor), |
| | 2683 | 32 | | new ContactAnchor(contact.SecondAnchor), |
| | 2683 | 33 | | contact.Depth, |
| | 2683 | 34 | | contact.Normal, |
| | 2683 | 35 | | contact.DepthIsClamped); |
| | 2683 | 36 | | return true; |
| | | 37 | | } |
| | | 38 | | |
| | | 39 | | private static bool DoCuboidCapsuleCheck(CollisionWorkItem pair) |
| | | 40 | | { |
| | 445 | 41 | | var cuboid = (LSCuboidCollider)pair.ColliderA; |
| | 445 | 42 | | var capsule = (LSCapsuleCollider)pair.ColliderB; |
| | | 43 | | |
| | 445 | 44 | | if (!cuboid.OrientedBox.TryGetCenteredCapsuleContact( |
| | 445 | 45 | | capsule.Center, |
| | 445 | 46 | | capsule.Rotation, |
| | 445 | 47 | | Vector3d.Up, |
| | 445 | 48 | | capsule.AxisLength, |
| | 445 | 49 | | capsule.ScaledRadius, |
| | 445 | 50 | | out FixedContactAnchors contact)) |
| | | 51 | | { |
| | 9 | 52 | | return false; |
| | | 53 | | } |
| | | 54 | | |
| | 436 | 55 | | pair.Manifold.SetContact( |
| | 436 | 56 | | new ContactAnchor(contact.FirstAnchor), |
| | 436 | 57 | | new ContactAnchor(contact.SecondAnchor), |
| | 436 | 58 | | contact.Depth, |
| | 436 | 59 | | contact.Normal, |
| | 436 | 60 | | contact.DepthIsClamped); |
| | 436 | 61 | | return true; |
| | | 62 | | } |
| | | 63 | | |
| | | 64 | | /// <summary> |
| | | 65 | | /// Checks for collisions between two poly-poly colliders. |
| | | 66 | | /// </summary> |
| | | 67 | | /// <returns>true if a collision is detected, false otherwise.</returns> |
| | | 68 | | private static bool DoCuboidsCheck(CollisionWorkItem pair) |
| | | 69 | | { |
| | 708 | 70 | | var cuboidA = (LSCuboidCollider)pair.ColliderA; |
| | 708 | 71 | | var cuboidB = (LSCuboidCollider)pair.ColliderB; |
| | | 72 | | |
| | 708 | 73 | | if (cuboidA.Shape == ColliderType.AABox |
| | 708 | 74 | | && cuboidB.Shape == ColliderType.AABox |
| | 708 | 75 | | && CanBuildAxisAlignedManifold(cuboidA) |
| | 708 | 76 | | && CanBuildAxisAlignedManifold(cuboidB)) |
| | | 77 | | { |
| | 241 | 78 | | return TryBuildAxisAlignedCuboidManifold(pair, cuboidA, cuboidB); |
| | | 79 | | } |
| | | 80 | | |
| | 467 | 81 | | if (!cuboidA.OrientedBox.TryGetContact( |
| | 467 | 82 | | cuboidB.OrientedBox, |
| | 467 | 83 | | out FixedContactAnchors contact)) |
| | | 84 | | { |
| | 5 | 85 | | return false; |
| | | 86 | | } |
| | | 87 | | |
| | 462 | 88 | | pair.Manifold.SetContact( |
| | 462 | 89 | | new ContactAnchor(contact.FirstAnchor), |
| | 462 | 90 | | new ContactAnchor(contact.SecondAnchor), |
| | 462 | 91 | | contact.Depth, |
| | 462 | 92 | | contact.Normal, |
| | 462 | 93 | | contact.DepthIsClamped); |
| | 462 | 94 | | return true; |
| | | 95 | | } |
| | | 96 | | |
| | | 97 | | private static bool CanBuildAxisAlignedManifold(LSCuboidCollider cuboid) |
| | | 98 | | { |
| | 483 | 99 | | Vector3d center = cuboid.Center; |
| | 483 | 100 | | Vector3d halfExtents = cuboid.OrientedBox.HalfExtents; |
| | | 101 | | // The four-point fast path materializes both bounds and subtracts their |
| | | 102 | | // overlap widths. Wider conceptual boxes use the exact relative path. |
| | 483 | 103 | | return Vector3d.TryAdd(center, halfExtents, out _) |
| | 483 | 104 | | && Vector3d.TrySubtract(center, halfExtents, out _) |
| | 483 | 105 | | && Vector3d.TryAdd(halfExtents, halfExtents, out _); |
| | | 106 | | } |
| | | 107 | | |
| | | 108 | | private static bool TryBuildAxisAlignedCuboidManifold( |
| | | 109 | | CollisionWorkItem pair, |
| | | 110 | | LSCuboidCollider cuboidA, |
| | | 111 | | LSCuboidCollider cuboidB) |
| | | 112 | | { |
| | 241 | 113 | | Fixed64 overlapX = FixedMath.Min(cuboidA.BoundsMax.X, cuboidB.BoundsMax.X) - FixedMath.Max(cuboidA.BoundsMin.X, |
| | 241 | 114 | | Fixed64 overlapY = FixedMath.Min(cuboidA.BoundsMax.Y, cuboidB.BoundsMax.Y) - FixedMath.Max(cuboidA.BoundsMin.Y, |
| | 241 | 115 | | Fixed64 overlapZ = FixedMath.Min(cuboidA.BoundsMax.Z, cuboidB.BoundsMax.Z) - FixedMath.Max(cuboidA.BoundsMin.Z, |
| | | 116 | | |
| | 241 | 117 | | if (overlapX < Fixed64.Zero || overlapY < Fixed64.Zero || overlapZ < Fixed64.Zero) |
| | 2 | 118 | | return false; |
| | | 119 | | |
| | 239 | 120 | | Vector3d centerDelta = cuboidB.Center - cuboidA.Center; |
| | 239 | 121 | | int axis = 0; |
| | 239 | 122 | | Fixed64 depth = overlapX; |
| | 239 | 123 | | if (overlapY < depth) |
| | | 124 | | { |
| | 21 | 125 | | axis = 1; |
| | 21 | 126 | | depth = overlapY; |
| | | 127 | | } |
| | | 128 | | |
| | 239 | 129 | | if (overlapZ < depth) |
| | | 130 | | { |
| | 2 | 131 | | axis = 2; |
| | 2 | 132 | | depth = overlapZ; |
| | | 133 | | } |
| | | 134 | | |
| | 239 | 135 | | Vector3d normal = axis switch |
| | 239 | 136 | | { |
| | 216 | 137 | | 0 => new Vector3d(centerDelta.X < Fixed64.Zero ? -Fixed64.One : Fixed64.One, Fixed64.Zero, Fixed64.Zero), |
| | 21 | 138 | | 1 => new Vector3d(Fixed64.Zero, centerDelta.Y < Fixed64.Zero ? -Fixed64.One : Fixed64.One, Fixed64.Zero), |
| | 2 | 139 | | _ => new Vector3d(Fixed64.Zero, Fixed64.Zero, centerDelta.Z < Fixed64.Zero ? -Fixed64.One : Fixed64.One) |
| | 239 | 140 | | }; |
| | | 141 | | |
| | 239 | 142 | | AddAxisAlignedCuboidContacts(pair.Manifold, cuboidA, cuboidB, axis, depth, normal); |
| | 239 | 143 | | return pair.Manifold.HasContact; |
| | | 144 | | } |
| | | 145 | | |
| | | 146 | | private static void AddAxisAlignedCuboidContacts( |
| | | 147 | | ContactManifold manifold, |
| | | 148 | | LSCuboidCollider cuboidA, |
| | | 149 | | LSCuboidCollider cuboidB, |
| | | 150 | | int axis, |
| | | 151 | | Fixed64 depth, |
| | | 152 | | Vector3d normal) |
| | | 153 | | { |
| | 239 | 154 | | Fixed64 minX = FixedMath.Max(cuboidA.BoundsMin.X, cuboidB.BoundsMin.X); |
| | 239 | 155 | | Fixed64 maxX = FixedMath.Min(cuboidA.BoundsMax.X, cuboidB.BoundsMax.X); |
| | 239 | 156 | | Fixed64 minY = FixedMath.Max(cuboidA.BoundsMin.Y, cuboidB.BoundsMin.Y); |
| | 239 | 157 | | Fixed64 maxY = FixedMath.Min(cuboidA.BoundsMax.Y, cuboidB.BoundsMax.Y); |
| | 239 | 158 | | Fixed64 minZ = FixedMath.Max(cuboidA.BoundsMin.Z, cuboidB.BoundsMin.Z); |
| | 239 | 159 | | Fixed64 maxZ = FixedMath.Min(cuboidA.BoundsMax.Z, cuboidB.BoundsMax.Z); |
| | | 160 | | |
| | | 161 | | switch (axis) |
| | | 162 | | { |
| | | 163 | | case 0: |
| | | 164 | | { |
| | 216 | 165 | | Fixed64 x = normal.X > Fixed64.Zero ? cuboidA.BoundsMax.X : cuboidA.BoundsMin.X; |
| | 216 | 166 | | AddCuboidContact(manifold, new Vector3d(x, minY, minZ), normal, depth); |
| | 216 | 167 | | AddCuboidContact(manifold, new Vector3d(x, minY, maxZ), normal, depth); |
| | 216 | 168 | | AddCuboidContact(manifold, new Vector3d(x, maxY, minZ), normal, depth); |
| | 216 | 169 | | AddCuboidContact(manifold, new Vector3d(x, maxY, maxZ), normal, depth); |
| | 216 | 170 | | break; |
| | | 171 | | } |
| | | 172 | | case 1: |
| | | 173 | | { |
| | 21 | 174 | | Fixed64 y = normal.Y > Fixed64.Zero ? cuboidA.BoundsMax.Y : cuboidA.BoundsMin.Y; |
| | 21 | 175 | | AddCuboidContact(manifold, new Vector3d(minX, y, minZ), normal, depth); |
| | 21 | 176 | | AddCuboidContact(manifold, new Vector3d(minX, y, maxZ), normal, depth); |
| | 21 | 177 | | AddCuboidContact(manifold, new Vector3d(maxX, y, minZ), normal, depth); |
| | 21 | 178 | | AddCuboidContact(manifold, new Vector3d(maxX, y, maxZ), normal, depth); |
| | 21 | 179 | | break; |
| | | 180 | | } |
| | | 181 | | default: |
| | | 182 | | { |
| | 2 | 183 | | Fixed64 z = normal.Z > Fixed64.Zero ? cuboidA.BoundsMax.Z : cuboidA.BoundsMin.Z; |
| | 2 | 184 | | AddCuboidContact(manifold, new Vector3d(minX, minY, z), normal, depth); |
| | 2 | 185 | | AddCuboidContact(manifold, new Vector3d(minX, maxY, z), normal, depth); |
| | 2 | 186 | | AddCuboidContact(manifold, new Vector3d(maxX, minY, z), normal, depth); |
| | 2 | 187 | | AddCuboidContact(manifold, new Vector3d(maxX, maxY, z), normal, depth); |
| | | 188 | | break; |
| | | 189 | | } |
| | | 190 | | } |
| | 2 | 191 | | } |
| | | 192 | | |
| | | 193 | | private static void AddCuboidContact(ContactManifold manifold, Vector3d pointA, Vector3d normal, Fixed64 depth) |
| | | 194 | | { |
| | 956 | 195 | | Vector3d pointB = pointA - normal * depth; |
| | 956 | 196 | | manifold.AddContact(pointA, pointB, depth, normal); |
| | 956 | 197 | | } |
| | | 198 | | |
| | | 199 | | #endregion |
| | | 200 | | |
| | | 201 | | } |