| | | 1 | | //======================================================================= |
| | | 2 | | // CollisionDetectionMixed.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 System.Runtime.CompilerServices; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | using FixedMathSharp.Geometry; |
| | | 11 | | using Gravitas.Colliders; |
| | | 12 | | |
| | | 13 | | namespace Gravitas.CollisionHandling; |
| | | 14 | | |
| | | 15 | | /// <summary> |
| | | 16 | | /// Deterministic mixed 2D/3D narrow-phase collision checks. |
| | | 17 | | /// </summary> |
| | | 18 | | /// <content>Builds caller-owned mixed manifolds and provides representative contact inspection.</content> |
| | | 19 | | public static partial class CollisionDetectionMixed |
| | | 20 | | { |
| | | 21 | | /// <summary>Tests a 3D collider against an embedded 2D collider and returns a representative contact.</summary> |
| | | 22 | | /// <remarks>The representative is the deepest retained row, not a whole-owner escape. Use <see cref="TryBuildManifo |
| | | 23 | | public static bool TryCollide(LSCollider collider3D, LSCollider2D collider2D, out MixedContact contact) |
| | | 24 | | { |
| | 4308 | 25 | | ChronicleGuard.ThrowIfNull(collider3D, nameof(collider3D)); |
| | 4308 | 26 | | ChronicleGuard.ThrowIfNull(collider2D, nameof(collider2D)); |
| | 4308 | 27 | | ContactManifold manifold = collider3D.Context.CollisionScratch.MixedManifold; |
| | 4308 | 28 | | bool found = TryBuildManifold(collider3D, collider2D, manifold); |
| | 4308 | 29 | | contact = found ? MixedContact.FromManifoldContact(manifold.PrimaryContact) : default; |
| | 4308 | 30 | | return found; |
| | | 31 | | } |
| | | 32 | | |
| | | 33 | | /// <summary>Rebuilds all mixed contacts in a caller-owned manifold, with A identifying the 3D participant and B the |
| | | 34 | | /// <remarks>Compound leaves retain separate provenance and materials. The buffer is cleared even when the pair is s |
| | | 35 | | public static bool TryBuildManifold(LSCollider collider3D, LSCollider2D collider2D, ContactManifold manifold) |
| | | 36 | | { |
| | 17426 | 37 | | ChronicleGuard.ThrowIfNull(collider3D, nameof(collider3D)); |
| | 17425 | 38 | | ChronicleGuard.ThrowIfNull(collider2D, nameof(collider2D)); |
| | 17424 | 39 | | ChronicleGuard.ThrowIfNull(manifold, nameof(manifold)); |
| | 17423 | 40 | | manifold.BeginUpdate(collider3D.Context.FrameCount); |
| | 17423 | 41 | | AppendManifold(collider3D, collider2D, manifold, 0, 0, false); |
| | 17423 | 42 | | return manifold.HasContact; |
| | | 43 | | } |
| | | 44 | | |
| | | 45 | | internal static bool TryBuildDiscreteManifold(LSCollider collider3D, LSCollider2D collider2D, ContactManifold manifo |
| | | 46 | | { |
| | 17769 | 47 | | manifold.BeginUpdate(collider3D.Context.FrameCount); |
| | 17769 | 48 | | return AppendManifold(collider3D, collider2D, manifold, 0, 0, true); |
| | | 49 | | } |
| | | 50 | | |
| | | 51 | | internal static bool AppendManifold(LSCollider collider3D, LSCollider2D collider2D, |
| | | 52 | | ContactManifold manifold, int namespace3D, int namespace2D, bool surfaceResponse) |
| | | 53 | | { |
| | 37512 | 54 | | if (!collider3D.Bounds.Intersects(collider2D.MixedBounds3D)) |
| | 9970 | 55 | | return false; |
| | | 56 | | |
| | 27542 | 57 | | if (collider3D is LSCompoundCollider compound3D) |
| | | 58 | | { |
| | 191 | 59 | | bool overlaps = false; |
| | 1098 | 60 | | for (int i = 0; i < compound3D.PartCount; i++) |
| | 358 | 61 | | overlaps |= AppendManifold(compound3D.GetPartCollider(i), collider2D, manifold, i + 1, namespace2D, surf |
| | 191 | 62 | | return overlaps; |
| | | 63 | | } |
| | 27351 | 64 | | if (collider2D is LSCompoundCollider2D compound2D) |
| | | 65 | | { |
| | 187 | 66 | | bool overlaps = false; |
| | 1118 | 67 | | for (int i = 0; i < compound2D.PartCount; i++) |
| | 372 | 68 | | overlaps |= AppendManifold(collider3D, compound2D.GetPartCollider(i), manifold, namespace3D, -(i + 1), s |
| | 187 | 69 | | return overlaps; |
| | | 70 | | } |
| | 27164 | 71 | | if (surfaceResponse && collider2D.StaticSurface is { } solidSurface |
| | 27164 | 72 | | && !collider3D.IsTrigger && !collider2D.IsTrigger |
| | 27164 | 73 | | && (collider3D is LSCuboidCollider or LSSphereCollider or LSCylinderCollider or LSConeCollider |
| | 27164 | 74 | | || collider3D is LSCapsuleCollider |
| | 27164 | 75 | | || collider3D is LSMeshCollider)) |
| | 7424 | 76 | | return solidSurface.AppendMixed(collider3D, collider2D, manifold, namespace3D, namespace2D); |
| | 19740 | 77 | | if (collider3D is LSMeshCollider mesh) |
| | | 78 | | { |
| | 8312 | 79 | | if (surfaceResponse && mesh.StaticSurface is { } surface && !mesh.IsTrigger && !collider2D.IsTrigger) |
| | 2691 | 80 | | return surface.AppendMixed(mesh, collider2D, manifold, namespace3D, namespace2D); |
| | 5621 | 81 | | int previous = manifold.Count; |
| | 5621 | 82 | | AppendMeshEmbedded2D(mesh, collider2D, manifold, namespace3D, namespace2D); |
| | 5621 | 83 | | return manifold.Count != previous; |
| | | 84 | | } |
| | 11428 | 85 | | if (!TryPrimitiveContact(collider3D, collider2D, out MixedContact contact)) |
| | 1826 | 86 | | return false; |
| | | 87 | | |
| | | 88 | | // Declared-domain CCD can aggregate several ordinary member contacts. |
| | | 89 | | // Preserve each supplier before its rows share a manifold and response owner. |
| | 9602 | 90 | | bool declaredSupplier = collider2D.StaticSurface != null; |
| | 9602 | 91 | | if (namespace3D != 0 || namespace2D != 0 || declaredSupplier) |
| | 1795 | 92 | | contact = contact.WithFallbackMaterials(collider3D.Material, collider2D.Material); |
| | 9602 | 93 | | ManifoldContact row = contact.ToManifoldContact(namespace3D, namespace2D); |
| | 9602 | 94 | | manifold.AddContact(new ContactGroupKey(namespace3D, namespace2D, |
| | 9602 | 95 | | sourceIdB: declaredSupplier ? collider2D.Id + 1 : 0, |
| | 9602 | 96 | | sourceOrderB: declaredSupplier ? ((IPhysicsColliderRegistryItem)collider2D).ReplayOrder + 1 : 0), row); |
| | 9602 | 97 | | return true; |
| | | 98 | | } |
| | | 99 | | |
| | | 100 | | private static bool TryPrimitiveContact(LSCollider collider3D, LSCollider2D collider2D, out MixedContact contact) => |
| | 11428 | 101 | | collider3D.Shape switch |
| | 11428 | 102 | | { |
| | 2594 | 103 | | ColliderType.Sphere => TrySphereEmbedded2D((LSSphereCollider)collider3D, collider2D, out contact), |
| | 4248 | 104 | | ColliderType.AABox or ColliderType.OBBox => TryCuboidEmbedded2D((LSCuboidCollider)collider3D, collider2D, ou |
| | 3217 | 105 | | ColliderType.Capsule => TryCapsuleEmbedded2D((LSCapsuleCollider)collider3D, collider2D, out contact), |
| | 768 | 106 | | ColliderType.Cylinder => TryCylinderEmbedded2D((LSCylinderCollider)collider3D, collider2D, out contact), |
| | 554 | 107 | | ColliderType.Cone => TryConeEmbedded2D((LSConeCollider)collider3D, collider2D, out contact), |
| | 47 | 108 | | _ => NoContact(out contact) |
| | 11428 | 109 | | }; |
| | | 110 | | |
| | | 111 | | private static bool TrySphereEmbedded2D(LSSphereCollider sphere, LSCollider2D embedded, out MixedContact contact) |
| | | 112 | | { |
| | 2594 | 113 | | if (embedded is LSAABBoxCollider2D or LSPolygonCollider2D) |
| | 2129 | 114 | | return TryGetSphereConvexPrismContact(sphere, embedded, out contact); |
| | | 115 | | |
| | 465 | 116 | | Vector3d sphereCenter = sphere.Center; |
| | 465 | 117 | | Vector2d planarCenter = new(sphereCenter.X, sphereCenter.Z); |
| | 465 | 118 | | bool planarInside = embedded.ContainsPoint(planarCenter); |
| | 465 | 119 | | bool inside = planarInside |
| | 465 | 120 | | && MixedEmbedded2DGeometry.ContainsPointInSlab( |
| | 465 | 121 | | embedded, |
| | 465 | 122 | | sphereCenter.Y); |
| | 465 | 123 | | ContactAnchor embeddedAnchor = |
| | 465 | 124 | | MixedEmbedded2DGeometry.GetClosestAnchorOnEmbeddedVolume( |
| | 465 | 125 | | embedded, |
| | 465 | 126 | | sphereCenter); |
| | | 127 | | // Broad-phase overlap plus a closest-volume anchor bounds every |
| | | 128 | | // component of this relative offset to the Fixed64 domain. |
| | 465 | 129 | | _ = embeddedAnchor.TryGetOffsetFrom( |
| | 465 | 130 | | sphereCenter, |
| | 465 | 131 | | out Vector3d delta); |
| | | 132 | | // Outside overlap bounds the closest witness by the sphere's local |
| | | 133 | | // reach; inside, the nearer planar boundary bounds it independently of |
| | | 134 | | // slab height. Its magnitude and final penetration therefore fit. |
| | 465 | 135 | | _ = Vector3d.TryGetMagnitude(delta, out Fixed64 distance); |
| | 465 | 136 | | if (!inside && distance > sphere.ScaledRadius) |
| | | 137 | | { |
| | 9 | 138 | | contact = default; |
| | 9 | 139 | | return false; |
| | | 140 | | } |
| | | 141 | | |
| | 456 | 142 | | Vector3d normal = distance > Fixed64.Zero |
| | 456 | 143 | | ? WideNormalization.GetNormalized(inside ? -delta : delta) |
| | 456 | 144 | | : ResolveFallbackNormal(sphereCenter, embedded); |
| | 456 | 145 | | Fixed64 depth = inside ? sphere.ScaledRadius + distance : sphere.ScaledRadius - distance; |
| | 456 | 146 | | contact = new MixedContact( |
| | 456 | 147 | | new ContactAnchor( |
| | 456 | 148 | | sphereCenter, |
| | 456 | 149 | | normal * sphere.ScaledRadius), |
| | 456 | 150 | | embeddedAnchor, |
| | 456 | 151 | | normal, |
| | 456 | 152 | | depth, |
| | 456 | 153 | | depthIsClamped: false); |
| | 456 | 154 | | return true; |
| | | 155 | | } |
| | | 156 | | |
| | | 157 | | private static bool TryCapsuleEmbedded2D(LSCapsuleCollider capsule, LSCollider2D embedded, out MixedContact contact) |
| | | 158 | | { |
| | 3217 | 159 | | if (embedded is LSAABBoxCollider2D or LSPolygonCollider2D) |
| | 2333 | 160 | | return TryGetCapsuleConvexPrismContact(capsule, embedded, out contact); |
| | | 161 | | |
| | 884 | 162 | | if (embedded is LSCircleCollider2D circle) |
| | 314 | 163 | | return TryGetCapsuleCircleSlabContact(capsule, circle, circle.ScaledRadius, out contact); |
| | | 164 | | |
| | 570 | 165 | | var prism = (LSCapsuleCollider2D)embedded; |
| | 570 | 166 | | if (!WideConvexPrismRelations.TryGetCenteredCapsuleSlabCapsulePenetration( |
| | 570 | 167 | | GetEmbeddedCenter3D(prism), prism.Rotation, prism.AxisLength, prism.ScaledRadius, prism.MixedHalfThickne |
| | 570 | 168 | | capsule.Center, capsule.Rotation, capsule.AxisLength, capsule.ScaledRadius, |
| | 570 | 169 | | out Vector3d normal, out Fixed64 depth, out bool depthIsClamped)) |
| | 24 | 170 | | return NoContact(out contact); |
| | | 171 | | |
| | 546 | 172 | | contact = BuildCanonicalSupportContact(capsule, prism, -normal, depth, depthIsClamped); |
| | 546 | 173 | | return true; |
| | | 174 | | } |
| | | 175 | | |
| | | 176 | | private static bool TryCylinderEmbedded2D(LSCylinderCollider cylinder, LSCollider2D embedded, out MixedContact conta |
| | | 177 | | { |
| | 768 | 178 | | if (embedded is LSAABBoxCollider2D or LSPolygonCollider2D) |
| | 376 | 179 | | return TryGetCylinderConvexPrismContact(cylinder, embedded, out contact); |
| | | 180 | | |
| | 392 | 181 | | bool collided = embedded.Shape == ColliderType2D.Circle |
| | 392 | 182 | | ? TryTestCylinderCircleSlab(cylinder, (LSCircleCollider2D)embedded, out AxisPenetration penetration) |
| | 392 | 183 | | : TryTestCylinderCapsuleSlab(cylinder, (LSCapsuleCollider2D)embedded, out penetration); |
| | | 184 | | |
| | 392 | 185 | | return collided |
| | 392 | 186 | | ? BuildCylinderContact(cylinder, embedded, penetration, out contact) |
| | 392 | 187 | | : NoContact(out contact); |
| | | 188 | | } |
| | | 189 | | |
| | | 190 | | private static bool TryConeEmbedded2D(LSConeCollider cone, LSCollider2D embedded, out MixedContact contact) |
| | | 191 | | { |
| | 554 | 192 | | if (embedded is LSAABBoxCollider2D or LSPolygonCollider2D) |
| | 385 | 193 | | return TryGetConeConvexPrismContact(cone, embedded, out contact); |
| | | 194 | | |
| | 169 | 195 | | bool collided = embedded.Shape == ColliderType2D.Circle |
| | 169 | 196 | | ? TryTestConeCircleSlab(cone, (LSCircleCollider2D)embedded, out AxisPenetration penetration) |
| | 169 | 197 | | : TryTestConeCapsuleSlab(cone, (LSCapsuleCollider2D)embedded, out penetration); |
| | | 198 | | |
| | 169 | 199 | | return collided |
| | 169 | 200 | | ? BuildConeContact(cone, embedded, penetration, out contact) |
| | 169 | 201 | | : NoContact(out contact); |
| | | 202 | | } |
| | | 203 | | |
| | | 204 | | private static bool TryGetCapsuleCircleSlabContact( |
| | | 205 | | LSCapsuleCollider capsule, |
| | | 206 | | LSCollider2D slab, |
| | | 207 | | Fixed64 radius, |
| | | 208 | | out MixedContact contact) |
| | | 209 | | { |
| | | 210 | | // Keep the admitted half-thickness even when full height is wider |
| | | 211 | | // than Fixed64. The complete query owns rim and interior-core minima. |
| | 314 | 212 | | Signed192 length = WideArithmetic.AddSigned192( |
| | 314 | 213 | | Signed192.Raw(slab.MixedHalfThickness), Signed192.Raw(slab.MixedHalfThickness)); |
| | 314 | 214 | | if (!WideConvexPrismRelations.TryGetCenteredFiniteCylinderCapsulePenetration( |
| | 314 | 215 | | GetEmbeddedCenter3D(slab), FixedQuaternion.Identity, Vector3d.Up, length, radius, |
| | 314 | 216 | | capsule.Center, capsule.Rotation, Vector3d.Up, capsule.AxisLength, capsule.ScaledRadius, |
| | 314 | 217 | | out Vector3d normal, out Fixed64 depth, out bool depthIsClamped)) |
| | 6 | 218 | | return NoContact(out contact); |
| | | 219 | | |
| | | 220 | | // The geometry query is cylinder-first; mixed contacts are 3D-to-2D. |
| | 308 | 221 | | contact = BuildCanonicalSupportContact(capsule, slab, -normal, depth, depthIsClamped); |
| | 308 | 222 | | return true; |
| | | 223 | | } |
| | | 224 | | |
| | | 225 | | private static bool TryTestCylinderCircleSlab( |
| | | 226 | | LSCylinderCollider cylinder, |
| | | 227 | | LSCircleCollider2D circle, |
| | | 228 | | out AxisPenetration penetration) |
| | | 229 | | { |
| | 288 | 230 | | penetration = default; |
| | | 231 | | // Keep the full admitted half-thickness: doubling in Fixed64 would |
| | | 232 | | // shorten slabs whose physical full height exceeds the scalar domain. |
| | 288 | 233 | | Signed192 slabLength = WideArithmetic.AddSigned192( |
| | 288 | 234 | | Signed192.Raw(circle.MixedHalfThickness), Signed192.Raw(circle.MixedHalfThickness)); |
| | 288 | 235 | | if (!WideConvexPrismRelations.TryGetPositiveRadiusCylinderPairPenetration( |
| | 288 | 236 | | cylinder.Center, cylinder.Rotation, Vector3d.Up, |
| | 288 | 237 | | Signed192.Raw(cylinder.Height), cylinder.ScaledRadius, |
| | 288 | 238 | | GetEmbeddedCenter3D(circle), FixedQuaternion.Identity, Vector3d.Up, |
| | 288 | 239 | | slabLength, circle.ScaledRadius, |
| | 288 | 240 | | out Vector3d normal, out Fixed64 depth, out bool depthIsClamped)) |
| | 7 | 241 | | return false; |
| | | 242 | | |
| | 281 | 243 | | penetration = new AxisPenetration(normal, depth, depthIsClamped); |
| | 281 | 244 | | return true; |
| | | 245 | | } |
| | | 246 | | |
| | | 247 | | private static bool TryTestConeCircleSlab( |
| | | 248 | | LSConeCollider cone, |
| | | 249 | | LSCircleCollider2D circle, |
| | | 250 | | out AxisPenetration penetration) |
| | | 251 | | { |
| | 68 | 252 | | penetration = default; |
| | 68 | 253 | | Vector3d coneAxis = GetRigidUpAxis(cone.Rotation); |
| | | 254 | | |
| | 68 | 255 | | if (!CheckConeCircleSlabAxis(cone, circle, coneAxis, ref penetration)) |
| | 1 | 256 | | return false; |
| | | 257 | | |
| | 67 | 258 | | CheckConeCircleSlabAxis(cone, circle, Vector3d.Up, ref penetration); |
| | | 259 | | |
| | 67 | 260 | | if (!CheckConeCircleSlabAxis( |
| | 67 | 261 | | cone, |
| | 67 | 262 | | circle, |
| | 67 | 263 | | Vector3d.Cross(coneAxis, Vector3d.Up), |
| | 67 | 264 | | ref penetration)) |
| | 1 | 265 | | return false; |
| | | 266 | | |
| | 66 | 267 | | Vector3d circleCenter = GetEmbeddedCenter3D(circle); |
| | | 268 | | // The admitted broad-phase bounds and separating axes guarantee that |
| | | 269 | | // this closest-feature offset is representable in either rigid frame. |
| | 66 | 270 | | _ = FixedSegment.TryGetClosestCenteredFiniteConeSurfaceAnchor( |
| | 66 | 271 | | circleCenter, |
| | 66 | 272 | | cone.Center, |
| | 66 | 273 | | cone.Rotation, |
| | 66 | 274 | | Vector3d.Up, |
| | 66 | 275 | | cone.Height, |
| | 66 | 276 | | cone.ScaledRadius, |
| | 66 | 277 | | Vector3d.Right, |
| | 66 | 278 | | out FixedPointAnchor conePoint, |
| | 66 | 279 | | out _, |
| | 66 | 280 | | out _); |
| | 66 | 281 | | _ = conePoint.TryGetOffsetFrom( |
| | 66 | 282 | | new FixedPointAnchor( |
| | 66 | 283 | | circleCenter, |
| | 66 | 284 | | FixedQuaternion.Identity, |
| | 66 | 285 | | Vector3d.Zero), |
| | 66 | 286 | | out Vector3d conePointFromCircle); |
| | | 287 | | |
| | 66 | 288 | | Fixed64 closestCircleY = FixedMath.Clamp( |
| | 66 | 289 | | conePointFromCircle.Y, |
| | 66 | 290 | | -circle.MixedHalfThickness, |
| | 66 | 291 | | circle.MixedHalfThickness); |
| | 66 | 292 | | _ = Vector3d.TrySubtract( |
| | 66 | 293 | | new Vector3d(Fixed64.Zero, closestCircleY, Fixed64.Zero), |
| | 66 | 294 | | conePointFromCircle, |
| | 66 | 295 | | out Vector3d closestFeatureAxis); |
| | 66 | 296 | | if (!CheckConeCircleSlabAxis( |
| | 66 | 297 | | cone, |
| | 66 | 298 | | circle, |
| | 66 | 299 | | closestFeatureAxis, |
| | 66 | 300 | | ref penetration)) |
| | 1 | 301 | | return false; |
| | | 302 | | |
| | 65 | 303 | | return penetration.HasValue; |
| | | 304 | | } |
| | | 305 | | |
| | | 306 | | private static bool TryTestCylinderCapsuleSlab( |
| | | 307 | | LSCylinderCollider cylinder, |
| | | 308 | | LSCapsuleCollider2D prism, |
| | | 309 | | out AxisPenetration penetration) |
| | | 310 | | { |
| | 104 | 311 | | penetration = default; |
| | 104 | 312 | | Vector3d cylinderAxis = GetRigidUpAxis(cylinder.Rotation); |
| | 104 | 313 | | GetEmbeddedCapsuleAxes( |
| | 104 | 314 | | prism, |
| | 104 | 315 | | out Vector2d prismPlanarAxis, |
| | 104 | 316 | | out Vector3d prismAxis, |
| | 104 | 317 | | out Vector3d prismNormal); |
| | | 318 | | |
| | 104 | 319 | | if (!CheckCylinderPrismAxis( |
| | 104 | 320 | | cylinder, |
| | 104 | 321 | | prism, |
| | 104 | 322 | | cylinderAxis, |
| | 104 | 323 | | prismPlanarAxis, |
| | 104 | 324 | | cylinderAxis, |
| | 104 | 325 | | ref penetration)) |
| | 1 | 326 | | return false; |
| | | 327 | | |
| | 103 | 328 | | CheckCylinderPrismAxis( |
| | 103 | 329 | | cylinder, |
| | 103 | 330 | | prism, |
| | 103 | 331 | | cylinderAxis, |
| | 103 | 332 | | prismPlanarAxis, |
| | 103 | 333 | | Vector3d.Up, |
| | 103 | 334 | | ref penetration); |
| | | 335 | | |
| | 103 | 336 | | if (!CheckCylinderPrismAxis( |
| | 103 | 337 | | cylinder, |
| | 103 | 338 | | prism, |
| | 103 | 339 | | cylinderAxis, |
| | 103 | 340 | | prismPlanarAxis, |
| | 103 | 341 | | Vector3d.Cross(cylinderAxis, Vector3d.Up), |
| | 103 | 342 | | ref penetration)) |
| | 1 | 343 | | return false; |
| | | 344 | | |
| | 102 | 345 | | if (!CheckEmbeddedCapsuleAxes( |
| | 102 | 346 | | cylinder, |
| | 102 | 347 | | cylinderAxis, |
| | 102 | 348 | | prism, |
| | 102 | 349 | | prismPlanarAxis, |
| | 102 | 350 | | prismAxis, |
| | 102 | 351 | | prismNormal, |
| | 102 | 352 | | ref penetration) |
| | 102 | 353 | | || !CheckCylinderEmbeddedCapsuleEdgeAxis( |
| | 102 | 354 | | cylinder, |
| | 102 | 355 | | cylinderAxis, |
| | 102 | 356 | | prism, |
| | 102 | 357 | | prismPlanarAxis, |
| | 102 | 358 | | prismAxis, |
| | 102 | 359 | | ref penetration)) |
| | | 360 | | { |
| | 8 | 361 | | return false; |
| | | 362 | | } |
| | | 363 | | |
| | 94 | 364 | | Vector3d closestFeatureAxis = |
| | 94 | 365 | | FixedSegment.GetClosestDirectionBetweenCenteredAxes( |
| | 94 | 366 | | cylinder.Center, |
| | 94 | 367 | | cylinderAxis, |
| | 94 | 368 | | cylinder.Height, |
| | 94 | 369 | | GetEmbeddedCenter3D(prism), |
| | 94 | 370 | | prism.AxisLength <= Fixed64.Epsilon |
| | 94 | 371 | | ? Vector3d.Up |
| | 94 | 372 | | : prismAxis, |
| | 94 | 373 | | prism.AxisLength); |
| | 94 | 374 | | if (!CheckCylinderPrismAxis( |
| | 94 | 375 | | cylinder, |
| | 94 | 376 | | prism, |
| | 94 | 377 | | cylinderAxis, |
| | 94 | 378 | | prismPlanarAxis, |
| | 94 | 379 | | closestFeatureAxis, |
| | 94 | 380 | | ref penetration)) |
| | 1 | 381 | | return false; |
| | | 382 | | |
| | 93 | 383 | | return penetration.HasValue; |
| | | 384 | | } |
| | | 385 | | |
| | | 386 | | private static bool TryTestConeCapsuleSlab( |
| | | 387 | | LSConeCollider cone, |
| | | 388 | | LSCapsuleCollider2D prism, |
| | | 389 | | out AxisPenetration penetration) |
| | | 390 | | { |
| | 101 | 391 | | penetration = default; |
| | 101 | 392 | | Vector3d coneAxis = GetRigidUpAxis(cone.Rotation); |
| | 101 | 393 | | GetEmbeddedCapsuleAxes( |
| | 101 | 394 | | prism, |
| | 101 | 395 | | out Vector2d prismPlanarAxis, |
| | 101 | 396 | | out Vector3d prismAxis, |
| | 101 | 397 | | out Vector3d prismNormal); |
| | | 398 | | |
| | 101 | 399 | | if (!CheckConePrismAxis( |
| | 101 | 400 | | cone, |
| | 101 | 401 | | prism, |
| | 101 | 402 | | prismPlanarAxis, |
| | 101 | 403 | | coneAxis, |
| | 101 | 404 | | ref penetration)) |
| | 1 | 405 | | return false; |
| | | 406 | | |
| | 100 | 407 | | CheckConePrismAxis( |
| | 100 | 408 | | cone, |
| | 100 | 409 | | prism, |
| | 100 | 410 | | prismPlanarAxis, |
| | 100 | 411 | | Vector3d.Up, |
| | 100 | 412 | | ref penetration); |
| | | 413 | | |
| | 100 | 414 | | if (!CheckConePrismAxis( |
| | 100 | 415 | | cone, |
| | 100 | 416 | | prism, |
| | 100 | 417 | | prismPlanarAxis, |
| | 100 | 418 | | Vector3d.Cross(coneAxis, Vector3d.Up), |
| | 100 | 419 | | ref penetration)) |
| | 1 | 420 | | return false; |
| | | 421 | | |
| | 99 | 422 | | if (!CheckEmbeddedCapsuleAxes( |
| | 99 | 423 | | cone, |
| | 99 | 424 | | prism, |
| | 99 | 425 | | prismPlanarAxis, |
| | 99 | 426 | | prismAxis, |
| | 99 | 427 | | prismNormal, |
| | 99 | 428 | | ref penetration) |
| | 99 | 429 | | || !CheckConeEmbeddedCapsuleEdgeAxis( |
| | 99 | 430 | | cone, |
| | 99 | 431 | | coneAxis, |
| | 99 | 432 | | prism, |
| | 99 | 433 | | prismPlanarAxis, |
| | 99 | 434 | | prismAxis, |
| | 99 | 435 | | ref penetration)) |
| | | 436 | | { |
| | 3 | 437 | | return false; |
| | | 438 | | } |
| | | 439 | | |
| | 96 | 440 | | ContactAnchor embeddedPoint = |
| | 96 | 441 | | MixedEmbedded2DGeometry.GetClosestAnchorOnEmbeddedVolume( |
| | 96 | 442 | | prism, |
| | 96 | 443 | | cone.Center); |
| | | 444 | | // Earlier exact axes admit a shared finite feature neighborhood, so |
| | | 445 | | // each relative offset remains representable through the cone frame. |
| | 96 | 446 | | _ = embeddedPoint.TryGetOffsetFrom( |
| | 96 | 447 | | cone.Center, |
| | 96 | 448 | | out Vector3d centerToEmbedded); |
| | 96 | 449 | | _ = cone.Rotation.Inverse().TryRotate( |
| | 96 | 450 | | centerToEmbedded, |
| | 96 | 451 | | out Vector3d localEmbeddedPoint); |
| | 96 | 452 | | _ = FixedSegment.TryGetClosestCenteredFiniteConeSurfaceOffset( |
| | 96 | 453 | | localEmbeddedPoint, |
| | 96 | 454 | | Vector3d.Zero, |
| | 96 | 455 | | Vector3d.Up, |
| | 96 | 456 | | cone.Height, |
| | 96 | 457 | | cone.ScaledRadius, |
| | 96 | 458 | | Vector3d.Right, |
| | 96 | 459 | | out Vector3d coneSurfaceOffset, |
| | 96 | 460 | | out _, |
| | 96 | 461 | | out _); |
| | 96 | 462 | | _ = embeddedPoint.TryGetOffsetFrom( |
| | 96 | 463 | | new ContactAnchor( |
| | 96 | 464 | | cone.Center, |
| | 96 | 465 | | cone.Rotation, |
| | 96 | 466 | | coneSurfaceOffset), |
| | 96 | 467 | | out Vector3d closestFeatureAxis); |
| | 96 | 468 | | if (!CheckConePrismAxis( |
| | 96 | 469 | | cone, |
| | 96 | 470 | | prism, |
| | 96 | 471 | | prismPlanarAxis, |
| | 96 | 472 | | closestFeatureAxis, |
| | 96 | 473 | | ref penetration)) |
| | 8 | 474 | | return false; |
| | | 475 | | |
| | 88 | 476 | | return penetration.HasValue; |
| | | 477 | | } |
| | | 478 | | |
| | | 479 | | private static bool CheckConeCircleSlabAxis( |
| | | 480 | | LSConeCollider cone, |
| | | 481 | | LSCircleCollider2D circle, |
| | | 482 | | Vector3d axis, |
| | | 483 | | ref AxisPenetration penetration) |
| | | 484 | | { |
| | 268 | 485 | | if (!TryNormalizeAxis(axis, out Vector3d normalizedAxis)) |
| | 23 | 486 | | return true; |
| | | 487 | | |
| | 245 | 488 | | if (!FixedSegment.TryGetCenteredFiniteConeCapsuleSlabAxisPenetration( |
| | 245 | 489 | | normalizedAxis, |
| | 245 | 490 | | cone.Center, |
| | 245 | 491 | | GetRigidUpAxis(cone.Rotation), |
| | 245 | 492 | | cone.Height, |
| | 245 | 493 | | cone.ScaledRadius, |
| | 245 | 494 | | GetEmbeddedCenter3D(circle), |
| | 245 | 495 | | Vector2d.Forward, |
| | 245 | 496 | | Fixed64.Zero, |
| | 245 | 497 | | circle.ScaledRadius, |
| | 245 | 498 | | circle.MixedHalfThickness, |
| | 245 | 499 | | out Vector3d orientedAxis, |
| | 245 | 500 | | out Fixed64 depth, |
| | 245 | 501 | | out bool depthIsClamped)) |
| | | 502 | | { |
| | 3 | 503 | | return false; |
| | | 504 | | } |
| | | 505 | | |
| | 242 | 506 | | return KeepAxisPenetration( |
| | 242 | 507 | | orientedAxis, |
| | 242 | 508 | | depth, |
| | 242 | 509 | | depthIsClamped, |
| | 242 | 510 | | ref penetration); |
| | | 511 | | } |
| | | 512 | | |
| | | 513 | | private static bool CheckCylinderPrismAxis( |
| | | 514 | | LSCylinderCollider cylinder, |
| | | 515 | | LSCapsuleCollider2D prism, |
| | | 516 | | Vector3d cylinderAxis, |
| | | 517 | | Vector2d prismAxis, |
| | | 518 | | Vector3d axis, |
| | | 519 | | ref AxisPenetration penetration) |
| | | 520 | | { |
| | 701 | 521 | | if (!TryNormalizeAxis(axis, out Vector3d normalizedAxis)) |
| | 43 | 522 | | return true; |
| | | 523 | | |
| | 658 | 524 | | if (!FixedSegment.TryGetCenteredFiniteCylinderCapsuleSlabAxisPenetration( |
| | 658 | 525 | | normalizedAxis, |
| | 658 | 526 | | cylinder.Center, |
| | 658 | 527 | | cylinderAxis, |
| | 658 | 528 | | cylinder.Height, |
| | 658 | 529 | | cylinder.ScaledRadius, |
| | 658 | 530 | | GetEmbeddedCenter3D(prism), |
| | 658 | 531 | | prismAxis, |
| | 658 | 532 | | prism.AxisLength, |
| | 658 | 533 | | prism.ScaledRadius, |
| | 658 | 534 | | prism.MixedHalfThickness, |
| | 658 | 535 | | out Vector3d orientedAxis, |
| | 658 | 536 | | out Fixed64 depth, |
| | 658 | 537 | | out bool depthIsClamped)) |
| | 11 | 538 | | return false; |
| | | 539 | | |
| | 647 | 540 | | return KeepAxisPenetration( |
| | 647 | 541 | | orientedAxis, |
| | 647 | 542 | | depth, |
| | 647 | 543 | | depthIsClamped, |
| | 647 | 544 | | ref penetration); |
| | | 545 | | } |
| | | 546 | | |
| | | 547 | | private static bool CheckConePrismAxis( |
| | | 548 | | LSConeCollider cone, |
| | | 549 | | LSCapsuleCollider2D prism, |
| | | 550 | | Vector2d prismAxis, |
| | | 551 | | Vector3d axis, |
| | | 552 | | ref AxisPenetration penetration) |
| | | 553 | | { |
| | 690 | 554 | | if (!TryNormalizeAxis(axis, out Vector3d normalizedAxis)) |
| | 46 | 555 | | return true; |
| | | 556 | | |
| | 644 | 557 | | if (!FixedSegment.TryGetCenteredFiniteConeCapsuleSlabAxisPenetration( |
| | 644 | 558 | | normalizedAxis, |
| | 644 | 559 | | cone.Center, |
| | 644 | 560 | | GetRigidUpAxis(cone.Rotation), |
| | 644 | 561 | | cone.Height, |
| | 644 | 562 | | cone.ScaledRadius, |
| | 644 | 563 | | GetEmbeddedCenter3D(prism), |
| | 644 | 564 | | prismAxis, |
| | 644 | 565 | | prism.AxisLength, |
| | 644 | 566 | | prism.ScaledRadius, |
| | 644 | 567 | | prism.MixedHalfThickness, |
| | 644 | 568 | | out Vector3d orientedAxis, |
| | 644 | 569 | | out Fixed64 depth, |
| | 644 | 570 | | out bool depthIsClamped)) |
| | 13 | 571 | | return false; |
| | | 572 | | |
| | 631 | 573 | | return KeepAxisPenetration( |
| | 631 | 574 | | orientedAxis, |
| | 631 | 575 | | depth, |
| | 631 | 576 | | depthIsClamped, |
| | 631 | 577 | | ref penetration); |
| | | 578 | | } |
| | | 579 | | |
| | | 580 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 581 | | private static bool KeepAxisPenetration( |
| | | 582 | | Vector3d axis, |
| | | 583 | | Fixed64 depth, |
| | | 584 | | bool depthIsClamped, |
| | | 585 | | ref AxisPenetration penetration) |
| | | 586 | | { |
| | 1520 | 587 | | if (!penetration.HasValue || depth < penetration.Depth) |
| | | 588 | | { |
| | 624 | 589 | | penetration = new AxisPenetration( |
| | 624 | 590 | | axis, |
| | 624 | 591 | | depth, |
| | 624 | 592 | | depthIsClamped); |
| | | 593 | | } |
| | | 594 | | |
| | 1520 | 595 | | return true; |
| | | 596 | | } |
| | | 597 | | |
| | | 598 | | private static bool CheckEmbeddedCapsuleAxes( |
| | | 599 | | LSCylinderCollider cylinder, |
| | | 600 | | Vector3d cylinderAxis, |
| | | 601 | | LSCapsuleCollider2D capsule, |
| | | 602 | | Vector2d capsulePlanarAxis, |
| | | 603 | | Vector3d capsuleAxis, |
| | | 604 | | Vector3d capsuleNormal, |
| | | 605 | | ref AxisPenetration penetration) |
| | | 606 | | { |
| | 102 | 607 | | return CheckCylinderPrismAxis( |
| | 102 | 608 | | cylinder, |
| | 102 | 609 | | capsule, |
| | 102 | 610 | | cylinderAxis, |
| | 102 | 611 | | capsulePlanarAxis, |
| | 102 | 612 | | capsuleAxis, |
| | 102 | 613 | | ref penetration) |
| | 102 | 614 | | && CheckCylinderPrismAxis( |
| | 102 | 615 | | cylinder, |
| | 102 | 616 | | capsule, |
| | 102 | 617 | | cylinderAxis, |
| | 102 | 618 | | capsulePlanarAxis, |
| | 102 | 619 | | capsuleNormal, |
| | 102 | 620 | | ref penetration); |
| | | 621 | | } |
| | | 622 | | |
| | | 623 | | private static bool CheckEmbeddedCapsuleAxes( |
| | | 624 | | LSConeCollider cone, |
| | | 625 | | LSCapsuleCollider2D capsule, |
| | | 626 | | Vector2d capsulePlanarAxis, |
| | | 627 | | Vector3d capsuleAxis, |
| | | 628 | | Vector3d capsuleNormal, |
| | | 629 | | ref AxisPenetration penetration) |
| | | 630 | | { |
| | 99 | 631 | | return CheckConePrismAxis( |
| | 99 | 632 | | cone, |
| | 99 | 633 | | capsule, |
| | 99 | 634 | | capsulePlanarAxis, |
| | 99 | 635 | | capsuleAxis, |
| | 99 | 636 | | ref penetration) |
| | 99 | 637 | | && CheckConePrismAxis( |
| | 99 | 638 | | cone, |
| | 99 | 639 | | capsule, |
| | 99 | 640 | | capsulePlanarAxis, |
| | 99 | 641 | | capsuleNormal, |
| | 99 | 642 | | ref penetration); |
| | | 643 | | } |
| | | 644 | | |
| | | 645 | | private static bool CheckCylinderEmbeddedCapsuleEdgeAxis( |
| | | 646 | | LSCylinderCollider cylinder, |
| | | 647 | | Vector3d cylinderAxis, |
| | | 648 | | LSCapsuleCollider2D capsule, |
| | | 649 | | Vector2d capsulePlanarAxis, |
| | | 650 | | Vector3d capsuleAxis, |
| | | 651 | | ref AxisPenetration penetration) |
| | | 652 | | { |
| | 94 | 653 | | return CheckCylinderPrismAxis( |
| | 94 | 654 | | cylinder, |
| | 94 | 655 | | capsule, |
| | 94 | 656 | | cylinderAxis, |
| | 94 | 657 | | capsulePlanarAxis, |
| | 94 | 658 | | Vector3d.Cross(cylinderAxis, capsuleAxis), |
| | 94 | 659 | | ref penetration); |
| | | 660 | | } |
| | | 661 | | |
| | | 662 | | private static bool CheckConeEmbeddedCapsuleEdgeAxis( |
| | | 663 | | LSConeCollider cone, |
| | | 664 | | Vector3d coneAxis, |
| | | 665 | | LSCapsuleCollider2D capsule, |
| | | 666 | | Vector2d capsulePlanarAxis, |
| | | 667 | | Vector3d capsuleAxis, |
| | | 668 | | ref AxisPenetration penetration) |
| | | 669 | | { |
| | 96 | 670 | | return CheckConePrismAxis( |
| | 96 | 671 | | cone, |
| | 96 | 672 | | capsule, |
| | 96 | 673 | | capsulePlanarAxis, |
| | 96 | 674 | | Vector3d.Cross(coneAxis, capsuleAxis), |
| | 96 | 675 | | ref penetration); |
| | | 676 | | } |
| | | 677 | | |
| | | 678 | | private static bool BuildCylinderContact( |
| | | 679 | | LSCylinderCollider cylinder, |
| | | 680 | | LSCollider2D embedded, |
| | | 681 | | AxisPenetration penetration, |
| | | 682 | | out MixedContact contact) |
| | | 683 | | { |
| | 374 | 684 | | contact = BuildCanonicalSupportContact( |
| | 374 | 685 | | cylinder, |
| | 374 | 686 | | embedded, |
| | 374 | 687 | | penetration.Axis, |
| | 374 | 688 | | penetration.Depth, |
| | 374 | 689 | | penetration.DepthIsClamped); |
| | 374 | 690 | | return true; |
| | | 691 | | } |
| | | 692 | | |
| | | 693 | | private static bool BuildConeContact( |
| | | 694 | | LSConeCollider cone, |
| | | 695 | | LSCollider2D embedded, |
| | | 696 | | AxisPenetration penetration, |
| | | 697 | | out MixedContact contact) |
| | | 698 | | { |
| | 153 | 699 | | contact = BuildCanonicalSupportContact( |
| | 153 | 700 | | cone, |
| | 153 | 701 | | embedded, |
| | 153 | 702 | | penetration.Axis, |
| | 153 | 703 | | penetration.Depth, |
| | 153 | 704 | | penetration.DepthIsClamped); |
| | 153 | 705 | | return true; |
| | | 706 | | } |
| | | 707 | | |
| | | 708 | | private static MixedContact BuildCanonicalSupportContact( |
| | | 709 | | LSCollider collider3D, |
| | | 710 | | LSCollider2D embedded, |
| | | 711 | | Vector3d normal3DTo2D, |
| | | 712 | | Fixed64 depth, |
| | | 713 | | bool depthIsClamped) |
| | | 714 | | { |
| | 1381 | 715 | | return new MixedContact( |
| | 1381 | 716 | | new ContactAnchor( |
| | 1381 | 717 | | ConvexColliderSupport.GetSupportAnchor( |
| | 1381 | 718 | | collider3D, |
| | 1381 | 719 | | normal3DTo2D, |
| | 1381 | 720 | | Vector3d.Zero)), |
| | 1381 | 721 | | MixedEmbedded2DGeometry.GetSupportAnchor(embedded, -normal3DTo2D), |
| | 1381 | 722 | | normal3DTo2D, |
| | 1381 | 723 | | depth, |
| | 1381 | 724 | | depthIsClamped); |
| | | 725 | | } |
| | | 726 | | |
| | | 727 | | } |