| | | 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 FixedMathSharp; |
| | | 9 | | using FixedMathSharp.Geometry; |
| | | 10 | | using Gravitas.Colliders; |
| | | 11 | | using System; |
| | | 12 | | using System.Runtime.CompilerServices; |
| | | 13 | | |
| | | 14 | | namespace Gravitas.CollisionHandling; |
| | | 15 | | |
| | | 16 | | /// <summary> |
| | | 17 | | /// Deterministic mixed 2D/3D narrow-phase collision checks. |
| | | 18 | | /// </summary> |
| | | 19 | | public static partial class CollisionDetectionMixed |
| | | 20 | | { |
| | | 21 | | /// <summary>Tests a 3D collider against an embedded 2D collider.</summary> |
| | | 22 | | public static bool TryCollide(LSCollider collider3D, LSCollider2D collider2D, out MixedContact contact) |
| | | 23 | | { |
| | 8040 | 24 | | SwiftThrowHelper.ThrowIfNull(collider3D, nameof(collider3D)); |
| | 8040 | 25 | | SwiftThrowHelper.ThrowIfNull(collider2D, nameof(collider2D)); |
| | | 26 | | |
| | 8040 | 27 | | if (!collider3D.Bounds.Intersects(collider2D.MixedBounds3D)) |
| | | 28 | | { |
| | 3152 | 29 | | contact = default; |
| | 3152 | 30 | | return false; |
| | | 31 | | } |
| | | 32 | | |
| | 4888 | 33 | | if (collider2D is LSCompoundCollider2D compound2D) |
| | 6 | 34 | | return TryEmbeddedCompound2D(collider3D, compound2D, out contact); |
| | | 35 | | |
| | 4882 | 36 | | return collider3D.Shape switch |
| | 4882 | 37 | | { |
| | 1466 | 38 | | ColliderType.Sphere => TrySphereEmbedded2D((LSSphereCollider)collider3D, collider2D, out contact), |
| | 2003 | 39 | | ColliderType.AABox or ColliderType.OBBox => TryCuboidEmbedded2D((LSCuboidCollider)collider3D, collider2D, ou |
| | 538 | 40 | | ColliderType.Capsule => TryCapsuleEmbedded2D((LSCapsuleCollider)collider3D, collider2D, out contact), |
| | 410 | 41 | | ColliderType.Cylinder => TryCylinderEmbedded2D((LSCylinderCollider)collider3D, collider2D, out contact), |
| | 396 | 42 | | ColliderType.Cone => TryConeEmbedded2D((LSConeCollider)collider3D, collider2D, out contact), |
| | 20 | 43 | | ColliderType.Mesh => TryMeshEmbedded2D((LSMeshCollider)collider3D, collider2D, out contact), |
| | 6 | 44 | | ColliderType.Compound => TryCompoundEmbedded2D((LSCompoundCollider)collider3D, collider2D, out contact), |
| | 43 | 45 | | _ => NoContact(out contact) |
| | 4882 | 46 | | }; |
| | | 47 | | } |
| | | 48 | | |
| | | 49 | | private static bool TryEmbeddedCompound2D( |
| | | 50 | | LSCollider collider3D, |
| | | 51 | | LSCompoundCollider2D compound2D, |
| | | 52 | | out MixedContact contact) |
| | | 53 | | { |
| | 6 | 54 | | bool found = false; |
| | 6 | 55 | | MixedContact best = default; |
| | | 56 | | |
| | 32 | 57 | | for (int i = 0; i < compound2D.PartCount; i++) |
| | | 58 | | { |
| | 10 | 59 | | LSCollider2D part = compound2D.GetPartCollider(i); |
| | 10 | 60 | | if (!collider3D.Bounds.Intersects(part.MixedBounds3D) |
| | 10 | 61 | | || !TryCollide(collider3D, part, out MixedContact candidate)) |
| | | 62 | | { |
| | | 63 | | continue; |
| | | 64 | | } |
| | | 65 | | |
| | 7 | 66 | | candidate = candidate.WithFallbackMaterials(collider3D.Material, part.Material); |
| | | 67 | | |
| | 7 | 68 | | if (ContactSelectionPolicy.ShouldReplaceWithShallower(candidate, found, best)) |
| | | 69 | | { |
| | 6 | 70 | | best = candidate; |
| | 6 | 71 | | found = true; |
| | | 72 | | } |
| | | 73 | | } |
| | | 74 | | |
| | 6 | 75 | | if (!found) |
| | 1 | 76 | | return NoContact(out contact); |
| | | 77 | | |
| | 5 | 78 | | contact = best; |
| | 5 | 79 | | return true; |
| | | 80 | | } |
| | | 81 | | |
| | | 82 | | private static bool TrySphereEmbedded2D(LSSphereCollider sphere, LSCollider2D embedded, out MixedContact contact) |
| | | 83 | | { |
| | 1466 | 84 | | if (embedded is LSAABBoxCollider2D or LSPolygonCollider2D) |
| | 1188 | 85 | | return TryGetSphereConvexPrismContact(sphere, embedded, out contact); |
| | | 86 | | |
| | 278 | 87 | | Vector3d sphereCenter = sphere.Center; |
| | 278 | 88 | | Vector2d planarCenter = new(sphereCenter.X, sphereCenter.Z); |
| | 278 | 89 | | bool planarInside = embedded.ContainsPoint(planarCenter); |
| | 278 | 90 | | bool inside = planarInside |
| | 278 | 91 | | && MixedEmbedded2DGeometry.ContainsPointInSlab( |
| | 278 | 92 | | embedded, |
| | 278 | 93 | | sphereCenter.Y); |
| | 278 | 94 | | ContactAnchor embeddedAnchor = |
| | 278 | 95 | | MixedEmbedded2DGeometry.GetClosestAnchorOnEmbeddedVolume( |
| | 278 | 96 | | embedded, |
| | 278 | 97 | | sphereCenter); |
| | | 98 | | // Broad-phase overlap plus a closest-volume anchor bounds every |
| | | 99 | | // component of this relative offset to the Fixed64 domain. |
| | 278 | 100 | | _ = embeddedAnchor.TryGetOffsetFrom( |
| | 278 | 101 | | sphereCenter, |
| | 278 | 102 | | out Vector3d delta); |
| | 278 | 103 | | if (!Vector3d.TryGetMagnitude(delta, out Fixed64 distance) |
| | 278 | 104 | | || (!inside && distance > sphere.ScaledRadius)) |
| | | 105 | | { |
| | 8 | 106 | | contact = default; |
| | 8 | 107 | | return false; |
| | | 108 | | } |
| | | 109 | | |
| | 270 | 110 | | Vector3d normal = distance > Fixed64.Zero |
| | 270 | 111 | | ? delta / distance |
| | 270 | 112 | | : ResolveFallbackNormal(sphereCenter, embedded); |
| | 270 | 113 | | bool exactDepth = inside |
| | 270 | 114 | | ? Fixed64.TryAdd( |
| | 270 | 115 | | sphere.ScaledRadius, |
| | 270 | 116 | | distance, |
| | 270 | 117 | | out Fixed64 depth) |
| | 270 | 118 | | : Fixed64.TrySubtract( |
| | 270 | 119 | | sphere.ScaledRadius, |
| | 270 | 120 | | distance, |
| | 270 | 121 | | out depth); |
| | 270 | 122 | | if (!exactDepth) |
| | 1 | 123 | | depth = Fixed64.MaxValue; |
| | 270 | 124 | | contact = new MixedContact( |
| | 270 | 125 | | new ContactAnchor( |
| | 270 | 126 | | sphereCenter, |
| | 270 | 127 | | normal * sphere.ScaledRadius), |
| | 270 | 128 | | embeddedAnchor, |
| | 270 | 129 | | normal, |
| | 270 | 130 | | depth, |
| | 270 | 131 | | depthIsClamped: !exactDepth); |
| | 270 | 132 | | return true; |
| | | 133 | | } |
| | | 134 | | |
| | | 135 | | private static bool TryCapsuleEmbedded2D(LSCapsuleCollider capsule, LSCollider2D embedded, out MixedContact contact) |
| | | 136 | | { |
| | 538 | 137 | | if (embedded is LSAABBoxCollider2D or LSPolygonCollider2D) |
| | 301 | 138 | | return TryGetCapsuleConvexPrismContact(capsule, embedded, out contact); |
| | | 139 | | |
| | 237 | 140 | | bool collided = embedded.Shape == ColliderType2D.Circle |
| | 237 | 141 | | ? TryTestCapsuleCircleSlab(capsule, (LSCircleCollider2D)embedded, out AxisPenetration penetration) |
| | 237 | 142 | | : TryTestCapsuleCapsuleSlab(capsule, (LSCapsuleCollider2D)embedded, out penetration); |
| | | 143 | | |
| | 237 | 144 | | return collided |
| | 237 | 145 | | ? BuildCapsuleContact(capsule, embedded, penetration, out contact) |
| | 237 | 146 | | : NoContact(out contact); |
| | | 147 | | } |
| | | 148 | | |
| | | 149 | | private static bool TryCylinderEmbedded2D(LSCylinderCollider cylinder, LSCollider2D embedded, out MixedContact conta |
| | | 150 | | { |
| | 410 | 151 | | if (embedded is LSAABBoxCollider2D or LSPolygonCollider2D) |
| | 239 | 152 | | return TryGetCylinderConvexPrismContact(cylinder, embedded, out contact); |
| | | 153 | | |
| | 171 | 154 | | bool collided = embedded.Shape == ColliderType2D.Circle |
| | 171 | 155 | | ? TryTestCylinderCircleSlab(cylinder, (LSCircleCollider2D)embedded, out AxisPenetration penetration) |
| | 171 | 156 | | : TryTestCylinderCapsuleSlab(cylinder, (LSCapsuleCollider2D)embedded, out penetration); |
| | | 157 | | |
| | 171 | 158 | | return collided |
| | 171 | 159 | | ? BuildCylinderContact(cylinder, embedded, penetration, out contact) |
| | 171 | 160 | | : NoContact(out contact); |
| | | 161 | | } |
| | | 162 | | |
| | | 163 | | private static bool TryConeEmbedded2D(LSConeCollider cone, LSCollider2D embedded, out MixedContact contact) |
| | | 164 | | { |
| | 396 | 165 | | if (embedded is LSAABBoxCollider2D or LSPolygonCollider2D) |
| | 230 | 166 | | return TryGetConeConvexPrismContact(cone, embedded, out contact); |
| | | 167 | | |
| | 166 | 168 | | bool collided = embedded.Shape == ColliderType2D.Circle |
| | 166 | 169 | | ? TryTestConeCircleSlab(cone, (LSCircleCollider2D)embedded, out AxisPenetration penetration) |
| | 166 | 170 | | : TryTestConeCapsuleSlab(cone, (LSCapsuleCollider2D)embedded, out penetration); |
| | | 171 | | |
| | 166 | 172 | | return collided |
| | 166 | 173 | | ? BuildConeContact(cone, embedded, penetration, out contact) |
| | 166 | 174 | | : NoContact(out contact); |
| | | 175 | | } |
| | | 176 | | |
| | | 177 | | private static bool TryTestCapsuleCircleSlab( |
| | | 178 | | LSCapsuleCollider capsule, |
| | | 179 | | LSCircleCollider2D circle, |
| | | 180 | | out AxisPenetration penetration) |
| | | 181 | | { |
| | 96 | 182 | | penetration = default; |
| | 96 | 183 | | Vector3d capsuleAxis = GetRigidUpAxis(capsule.Rotation); |
| | 96 | 184 | | CheckCapsuleCircleSlabAxis( |
| | 96 | 185 | | capsule, |
| | 96 | 186 | | circle, |
| | 96 | 187 | | capsuleAxis, |
| | 96 | 188 | | Vector3d.Up, |
| | 96 | 189 | | ref penetration); |
| | | 190 | | |
| | 96 | 191 | | if (!CheckCapsuleCircleSlabAxis( |
| | 96 | 192 | | capsule, |
| | 96 | 193 | | circle, |
| | 96 | 194 | | capsuleAxis, |
| | 96 | 195 | | capsuleAxis, |
| | 96 | 196 | | ref penetration)) |
| | 1 | 197 | | return false; |
| | | 198 | | |
| | 95 | 199 | | if (!CheckCapsuleCircleSlabAxis( |
| | 95 | 200 | | capsule, |
| | 95 | 201 | | circle, |
| | 95 | 202 | | capsuleAxis, |
| | 95 | 203 | | Vector3d.Cross(capsuleAxis, Vector3d.Up), |
| | 95 | 204 | | ref penetration)) |
| | 1 | 205 | | return false; |
| | | 206 | | |
| | 94 | 207 | | Vector3d closestFeatureAxis = |
| | 94 | 208 | | FixedSegment.GetClosestDirectionBetweenCenteredAxes( |
| | 94 | 209 | | capsule.Center, |
| | 94 | 210 | | capsuleAxis, |
| | 94 | 211 | | capsule.AxisLength, |
| | 94 | 212 | | GetEmbeddedCenter3D(circle), |
| | 94 | 213 | | Vector3d.Up, |
| | 94 | 214 | | circle.MixedHalfThickness * Fixed64.Two); |
| | 94 | 215 | | if (!CheckCapsuleCircleSlabAxis( |
| | 94 | 216 | | capsule, |
| | 94 | 217 | | circle, |
| | 94 | 218 | | capsuleAxis, |
| | 94 | 219 | | closestFeatureAxis, |
| | 94 | 220 | | ref penetration)) |
| | 1 | 221 | | return false; |
| | | 222 | | |
| | 93 | 223 | | return penetration.HasValue; |
| | | 224 | | } |
| | | 225 | | |
| | | 226 | | private static bool TryTestCylinderCircleSlab( |
| | | 227 | | LSCylinderCollider cylinder, |
| | | 228 | | LSCircleCollider2D circle, |
| | | 229 | | out AxisPenetration penetration) |
| | | 230 | | { |
| | 67 | 231 | | penetration = default; |
| | 67 | 232 | | Vector3d cylinderAxis = GetRigidUpAxis(cylinder.Rotation); |
| | 67 | 233 | | if (!CheckCylinderCircleSlabAxis( |
| | 67 | 234 | | cylinder, |
| | 67 | 235 | | circle, |
| | 67 | 236 | | cylinderAxis, |
| | 67 | 237 | | cylinderAxis, |
| | 67 | 238 | | ref penetration)) |
| | 1 | 239 | | return false; |
| | | 240 | | |
| | 66 | 241 | | CheckCylinderCircleSlabAxis( |
| | 66 | 242 | | cylinder, |
| | 66 | 243 | | circle, |
| | 66 | 244 | | cylinderAxis, |
| | 66 | 245 | | Vector3d.Up, |
| | 66 | 246 | | ref penetration); |
| | | 247 | | |
| | 66 | 248 | | if (!CheckCylinderCircleSlabAxis( |
| | 66 | 249 | | cylinder, |
| | 66 | 250 | | circle, |
| | 66 | 251 | | cylinderAxis, |
| | 66 | 252 | | Vector3d.Cross(cylinderAxis, Vector3d.Up), |
| | 66 | 253 | | ref penetration)) |
| | 1 | 254 | | return false; |
| | | 255 | | |
| | 65 | 256 | | Vector3d closestFeatureAxis = |
| | 65 | 257 | | FixedSegment.GetClosestDirectionBetweenCenteredAxes( |
| | 65 | 258 | | cylinder.Center, |
| | 65 | 259 | | cylinderAxis, |
| | 65 | 260 | | cylinder.Height, |
| | 65 | 261 | | GetEmbeddedCenter3D(circle), |
| | 65 | 262 | | Vector3d.Up, |
| | 65 | 263 | | circle.MixedHalfThickness * Fixed64.Two); |
| | 65 | 264 | | if (!CheckCylinderCircleSlabAxis( |
| | 65 | 265 | | cylinder, |
| | 65 | 266 | | circle, |
| | 65 | 267 | | cylinderAxis, |
| | 65 | 268 | | closestFeatureAxis, |
| | 65 | 269 | | ref penetration)) |
| | 1 | 270 | | return false; |
| | | 271 | | |
| | 64 | 272 | | return penetration.HasValue; |
| | | 273 | | } |
| | | 274 | | |
| | | 275 | | private static bool TryTestConeCircleSlab( |
| | | 276 | | LSConeCollider cone, |
| | | 277 | | LSCircleCollider2D circle, |
| | | 278 | | out AxisPenetration penetration) |
| | | 279 | | { |
| | 65 | 280 | | penetration = default; |
| | 65 | 281 | | Vector3d coneAxis = GetRigidUpAxis(cone.Rotation); |
| | | 282 | | |
| | 65 | 283 | | if (!CheckConeCircleSlabAxis(cone, circle, coneAxis, ref penetration)) |
| | 1 | 284 | | return false; |
| | | 285 | | |
| | 64 | 286 | | CheckConeCircleSlabAxis(cone, circle, Vector3d.Up, ref penetration); |
| | | 287 | | |
| | 64 | 288 | | if (!CheckConeCircleSlabAxis( |
| | 64 | 289 | | cone, |
| | 64 | 290 | | circle, |
| | 64 | 291 | | Vector3d.Cross(coneAxis, Vector3d.Up), |
| | 64 | 292 | | ref penetration)) |
| | 1 | 293 | | return false; |
| | | 294 | | |
| | 63 | 295 | | Vector3d circleCenter = GetEmbeddedCenter3D(circle); |
| | | 296 | | // The admitted broad-phase bounds and separating axes guarantee that |
| | | 297 | | // this closest-feature offset is representable in either rigid frame. |
| | 63 | 298 | | _ = FixedSegment.TryGetClosestCenteredFiniteConeSurfaceAnchor( |
| | 63 | 299 | | circleCenter, |
| | 63 | 300 | | cone.Center, |
| | 63 | 301 | | cone.Rotation, |
| | 63 | 302 | | Vector3d.Up, |
| | 63 | 303 | | cone.Height, |
| | 63 | 304 | | cone.ScaledRadius, |
| | 63 | 305 | | Vector3d.Right, |
| | 63 | 306 | | out FixedPointAnchor conePoint, |
| | 63 | 307 | | out _, |
| | 63 | 308 | | out _); |
| | 63 | 309 | | _ = conePoint.TryGetOffsetFrom( |
| | 63 | 310 | | new FixedPointAnchor( |
| | 63 | 311 | | circleCenter, |
| | 63 | 312 | | FixedQuaternion.Identity, |
| | 63 | 313 | | Vector3d.Zero), |
| | 63 | 314 | | out Vector3d conePointFromCircle); |
| | | 315 | | |
| | 63 | 316 | | Fixed64 closestCircleY = FixedMath.Clamp( |
| | 63 | 317 | | conePointFromCircle.Y, |
| | 63 | 318 | | -circle.MixedHalfThickness, |
| | 63 | 319 | | circle.MixedHalfThickness); |
| | 63 | 320 | | _ = Vector3d.TrySubtract( |
| | 63 | 321 | | new Vector3d(Fixed64.Zero, closestCircleY, Fixed64.Zero), |
| | 63 | 322 | | conePointFromCircle, |
| | 63 | 323 | | out Vector3d closestFeatureAxis); |
| | 63 | 324 | | if (!CheckConeCircleSlabAxis( |
| | 63 | 325 | | cone, |
| | 63 | 326 | | circle, |
| | 63 | 327 | | closestFeatureAxis, |
| | 63 | 328 | | ref penetration)) |
| | 1 | 329 | | return false; |
| | | 330 | | |
| | 62 | 331 | | return penetration.HasValue; |
| | | 332 | | } |
| | | 333 | | |
| | | 334 | | private static bool TryTestCapsuleCapsuleSlab( |
| | | 335 | | LSCapsuleCollider capsule, |
| | | 336 | | LSCapsuleCollider2D prism, |
| | | 337 | | out AxisPenetration penetration) |
| | | 338 | | { |
| | 141 | 339 | | penetration = default; |
| | 141 | 340 | | Vector3d capsuleAxis = GetRigidUpAxis(capsule.Rotation); |
| | 141 | 341 | | GetEmbeddedCapsuleAxes( |
| | 141 | 342 | | prism, |
| | 141 | 343 | | out Vector2d prismPlanarAxis, |
| | 141 | 344 | | out Vector3d prismAxis, |
| | 141 | 345 | | out Vector3d prismNormal); |
| | | 346 | | |
| | 141 | 347 | | if (!CheckCapsulePrismAxis( |
| | 141 | 348 | | capsule, |
| | 141 | 349 | | prism, |
| | 141 | 350 | | capsuleAxis, |
| | 141 | 351 | | prismPlanarAxis, |
| | 141 | 352 | | capsuleAxis, |
| | 141 | 353 | | ref penetration)) |
| | 1 | 354 | | return false; |
| | | 355 | | |
| | 140 | 356 | | CheckCapsulePrismAxis( |
| | 140 | 357 | | capsule, |
| | 140 | 358 | | prism, |
| | 140 | 359 | | capsuleAxis, |
| | 140 | 360 | | prismPlanarAxis, |
| | 140 | 361 | | Vector3d.Up, |
| | 140 | 362 | | ref penetration); |
| | | 363 | | |
| | 140 | 364 | | if (!CheckCapsulePrismAxis( |
| | 140 | 365 | | capsule, |
| | 140 | 366 | | prism, |
| | 140 | 367 | | capsuleAxis, |
| | 140 | 368 | | prismPlanarAxis, |
| | 140 | 369 | | Vector3d.Cross(capsuleAxis, Vector3d.Up), |
| | 140 | 370 | | ref penetration)) |
| | 8 | 371 | | return false; |
| | | 372 | | |
| | 132 | 373 | | if (!CheckEmbeddedCapsuleAxes( |
| | 132 | 374 | | capsule, |
| | 132 | 375 | | capsuleAxis, |
| | 132 | 376 | | prism, |
| | 132 | 377 | | prismPlanarAxis, |
| | 132 | 378 | | prismAxis, |
| | 132 | 379 | | prismNormal, |
| | 132 | 380 | | ref penetration) |
| | 132 | 381 | | || !CheckCapsuleEmbeddedCapsuleEdgeAxis( |
| | 132 | 382 | | capsule, |
| | 132 | 383 | | capsuleAxis, |
| | 132 | 384 | | prism, |
| | 132 | 385 | | prismPlanarAxis, |
| | 132 | 386 | | prismAxis, |
| | 132 | 387 | | ref penetration)) |
| | | 388 | | { |
| | 8 | 389 | | return false; |
| | | 390 | | } |
| | | 391 | | |
| | 124 | 392 | | Vector3d closestFeatureAxis = |
| | 124 | 393 | | FixedSegment.GetClosestDirectionBetweenCenteredAxes( |
| | 124 | 394 | | capsule.Center, |
| | 124 | 395 | | capsuleAxis, |
| | 124 | 396 | | capsule.AxisLength, |
| | 124 | 397 | | GetEmbeddedCenter3D(prism), |
| | 124 | 398 | | prism.AxisLength <= Fixed64.Epsilon |
| | 124 | 399 | | ? Vector3d.Up |
| | 124 | 400 | | : prismAxis, |
| | 124 | 401 | | prism.AxisLength); |
| | 124 | 402 | | if (!CheckCapsulePrismAxis( |
| | 124 | 403 | | capsule, |
| | 124 | 404 | | prism, |
| | 124 | 405 | | capsuleAxis, |
| | 124 | 406 | | prismPlanarAxis, |
| | 124 | 407 | | closestFeatureAxis, |
| | 124 | 408 | | ref penetration)) |
| | 1 | 409 | | return false; |
| | | 410 | | |
| | 123 | 411 | | return penetration.HasValue; |
| | | 412 | | } |
| | | 413 | | |
| | | 414 | | private static bool TryTestCylinderCapsuleSlab( |
| | | 415 | | LSCylinderCollider cylinder, |
| | | 416 | | LSCapsuleCollider2D prism, |
| | | 417 | | out AxisPenetration penetration) |
| | | 418 | | { |
| | 104 | 419 | | penetration = default; |
| | 104 | 420 | | Vector3d cylinderAxis = GetRigidUpAxis(cylinder.Rotation); |
| | 104 | 421 | | GetEmbeddedCapsuleAxes( |
| | 104 | 422 | | prism, |
| | 104 | 423 | | out Vector2d prismPlanarAxis, |
| | 104 | 424 | | out Vector3d prismAxis, |
| | 104 | 425 | | out Vector3d prismNormal); |
| | | 426 | | |
| | 104 | 427 | | if (!CheckCylinderPrismAxis( |
| | 104 | 428 | | cylinder, |
| | 104 | 429 | | prism, |
| | 104 | 430 | | cylinderAxis, |
| | 104 | 431 | | prismPlanarAxis, |
| | 104 | 432 | | cylinderAxis, |
| | 104 | 433 | | ref penetration)) |
| | 1 | 434 | | return false; |
| | | 435 | | |
| | 103 | 436 | | CheckCylinderPrismAxis( |
| | 103 | 437 | | cylinder, |
| | 103 | 438 | | prism, |
| | 103 | 439 | | cylinderAxis, |
| | 103 | 440 | | prismPlanarAxis, |
| | 103 | 441 | | Vector3d.Up, |
| | 103 | 442 | | ref penetration); |
| | | 443 | | |
| | 103 | 444 | | if (!CheckCylinderPrismAxis( |
| | 103 | 445 | | cylinder, |
| | 103 | 446 | | prism, |
| | 103 | 447 | | cylinderAxis, |
| | 103 | 448 | | prismPlanarAxis, |
| | 103 | 449 | | Vector3d.Cross(cylinderAxis, Vector3d.Up), |
| | 103 | 450 | | ref penetration)) |
| | 1 | 451 | | return false; |
| | | 452 | | |
| | 102 | 453 | | if (!CheckEmbeddedCapsuleAxes( |
| | 102 | 454 | | cylinder, |
| | 102 | 455 | | cylinderAxis, |
| | 102 | 456 | | prism, |
| | 102 | 457 | | prismPlanarAxis, |
| | 102 | 458 | | prismAxis, |
| | 102 | 459 | | prismNormal, |
| | 102 | 460 | | ref penetration) |
| | 102 | 461 | | || !CheckCylinderEmbeddedCapsuleEdgeAxis( |
| | 102 | 462 | | cylinder, |
| | 102 | 463 | | cylinderAxis, |
| | 102 | 464 | | prism, |
| | 102 | 465 | | prismPlanarAxis, |
| | 102 | 466 | | prismAxis, |
| | 102 | 467 | | ref penetration)) |
| | | 468 | | { |
| | 8 | 469 | | return false; |
| | | 470 | | } |
| | | 471 | | |
| | 94 | 472 | | Vector3d closestFeatureAxis = |
| | 94 | 473 | | FixedSegment.GetClosestDirectionBetweenCenteredAxes( |
| | 94 | 474 | | cylinder.Center, |
| | 94 | 475 | | cylinderAxis, |
| | 94 | 476 | | cylinder.Height, |
| | 94 | 477 | | GetEmbeddedCenter3D(prism), |
| | 94 | 478 | | prism.AxisLength <= Fixed64.Epsilon |
| | 94 | 479 | | ? Vector3d.Up |
| | 94 | 480 | | : prismAxis, |
| | 94 | 481 | | prism.AxisLength); |
| | 94 | 482 | | if (!CheckCylinderPrismAxis( |
| | 94 | 483 | | cylinder, |
| | 94 | 484 | | prism, |
| | 94 | 485 | | cylinderAxis, |
| | 94 | 486 | | prismPlanarAxis, |
| | 94 | 487 | | closestFeatureAxis, |
| | 94 | 488 | | ref penetration)) |
| | 1 | 489 | | return false; |
| | | 490 | | |
| | 93 | 491 | | return penetration.HasValue; |
| | | 492 | | } |
| | | 493 | | |
| | | 494 | | private static bool TryTestConeCapsuleSlab( |
| | | 495 | | LSConeCollider cone, |
| | | 496 | | LSCapsuleCollider2D prism, |
| | | 497 | | out AxisPenetration penetration) |
| | | 498 | | { |
| | 101 | 499 | | penetration = default; |
| | 101 | 500 | | Vector3d coneAxis = GetRigidUpAxis(cone.Rotation); |
| | 101 | 501 | | GetEmbeddedCapsuleAxes( |
| | 101 | 502 | | prism, |
| | 101 | 503 | | out Vector2d prismPlanarAxis, |
| | 101 | 504 | | out Vector3d prismAxis, |
| | 101 | 505 | | out Vector3d prismNormal); |
| | | 506 | | |
| | 101 | 507 | | if (!CheckConePrismAxis( |
| | 101 | 508 | | cone, |
| | 101 | 509 | | prism, |
| | 101 | 510 | | prismPlanarAxis, |
| | 101 | 511 | | coneAxis, |
| | 101 | 512 | | ref penetration)) |
| | 1 | 513 | | return false; |
| | | 514 | | |
| | 100 | 515 | | CheckConePrismAxis( |
| | 100 | 516 | | cone, |
| | 100 | 517 | | prism, |
| | 100 | 518 | | prismPlanarAxis, |
| | 100 | 519 | | Vector3d.Up, |
| | 100 | 520 | | ref penetration); |
| | | 521 | | |
| | 100 | 522 | | if (!CheckConePrismAxis( |
| | 100 | 523 | | cone, |
| | 100 | 524 | | prism, |
| | 100 | 525 | | prismPlanarAxis, |
| | 100 | 526 | | Vector3d.Cross(coneAxis, Vector3d.Up), |
| | 100 | 527 | | ref penetration)) |
| | 1 | 528 | | return false; |
| | | 529 | | |
| | 99 | 530 | | if (!CheckEmbeddedCapsuleAxes( |
| | 99 | 531 | | cone, |
| | 99 | 532 | | prism, |
| | 99 | 533 | | prismPlanarAxis, |
| | 99 | 534 | | prismAxis, |
| | 99 | 535 | | prismNormal, |
| | 99 | 536 | | ref penetration) |
| | 99 | 537 | | || !CheckConeEmbeddedCapsuleEdgeAxis( |
| | 99 | 538 | | cone, |
| | 99 | 539 | | coneAxis, |
| | 99 | 540 | | prism, |
| | 99 | 541 | | prismPlanarAxis, |
| | 99 | 542 | | prismAxis, |
| | 99 | 543 | | ref penetration)) |
| | | 544 | | { |
| | 3 | 545 | | return false; |
| | | 546 | | } |
| | | 547 | | |
| | 96 | 548 | | ContactAnchor embeddedPoint = |
| | 96 | 549 | | MixedEmbedded2DGeometry.GetClosestAnchorOnEmbeddedVolume( |
| | 96 | 550 | | prism, |
| | 96 | 551 | | cone.Center); |
| | | 552 | | // Earlier exact axes admit a shared finite feature neighborhood, so |
| | | 553 | | // each relative offset remains representable through the cone frame. |
| | 96 | 554 | | _ = embeddedPoint.TryGetOffsetFrom( |
| | 96 | 555 | | cone.Center, |
| | 96 | 556 | | out Vector3d centerToEmbedded); |
| | 96 | 557 | | _ = cone.Rotation.Inverse().TryRotate( |
| | 96 | 558 | | centerToEmbedded, |
| | 96 | 559 | | out Vector3d localEmbeddedPoint); |
| | 96 | 560 | | _ = FixedSegment.TryGetClosestCenteredFiniteConeSurfaceOffset( |
| | 96 | 561 | | localEmbeddedPoint, |
| | 96 | 562 | | Vector3d.Zero, |
| | 96 | 563 | | Vector3d.Up, |
| | 96 | 564 | | cone.Height, |
| | 96 | 565 | | cone.ScaledRadius, |
| | 96 | 566 | | Vector3d.Right, |
| | 96 | 567 | | out Vector3d coneSurfaceOffset, |
| | 96 | 568 | | out _, |
| | 96 | 569 | | out _); |
| | 96 | 570 | | _ = embeddedPoint.TryGetOffsetFrom( |
| | 96 | 571 | | new ContactAnchor( |
| | 96 | 572 | | cone.Center, |
| | 96 | 573 | | cone.Rotation, |
| | 96 | 574 | | coneSurfaceOffset), |
| | 96 | 575 | | out Vector3d closestFeatureAxis); |
| | 96 | 576 | | if (!CheckConePrismAxis( |
| | 96 | 577 | | cone, |
| | 96 | 578 | | prism, |
| | 96 | 579 | | prismPlanarAxis, |
| | 96 | 580 | | closestFeatureAxis, |
| | 96 | 581 | | ref penetration)) |
| | 8 | 582 | | return false; |
| | | 583 | | |
| | 88 | 584 | | return penetration.HasValue; |
| | | 585 | | } |
| | | 586 | | |
| | | 587 | | private static bool CheckCapsuleCircleSlabAxis( |
| | | 588 | | LSCapsuleCollider capsule, |
| | | 589 | | LSCircleCollider2D circle, |
| | | 590 | | Vector3d capsuleAxis, |
| | | 591 | | Vector3d axis, |
| | | 592 | | ref AxisPenetration penetration) |
| | | 593 | | { |
| | 381 | 594 | | if (!TryNormalizeAxis(axis, out Vector3d normalizedAxis)) |
| | 45 | 595 | | return true; |
| | | 596 | | |
| | 336 | 597 | | if (!FixedSegment.TryGetCenteredFiniteCylinderCapsuleAxisPenetration( |
| | 336 | 598 | | normalizedAxis, |
| | 336 | 599 | | GetEmbeddedCenter3D(circle), |
| | 336 | 600 | | Vector3d.Up, |
| | 336 | 601 | | circle.MixedHalfThickness * Fixed64.Two, |
| | 336 | 602 | | circle.ScaledRadius, |
| | 336 | 603 | | capsule.Center, |
| | 336 | 604 | | capsuleAxis, |
| | 336 | 605 | | capsule.AxisLength, |
| | 336 | 606 | | capsule.ScaledRadius, |
| | 336 | 607 | | out Vector3d circleToCapsuleAxis, |
| | 336 | 608 | | out Fixed64 depth, |
| | 336 | 609 | | out bool depthIsClamped)) |
| | | 610 | | { |
| | 3 | 611 | | return false; |
| | | 612 | | } |
| | | 613 | | |
| | 333 | 614 | | if (!penetration.HasValue || depth < penetration.Depth) |
| | | 615 | | { |
| | 225 | 616 | | penetration = new AxisPenetration( |
| | 225 | 617 | | -circleToCapsuleAxis, |
| | 225 | 618 | | depth, |
| | 225 | 619 | | depthIsClamped); |
| | | 620 | | } |
| | | 621 | | |
| | 333 | 622 | | return true; |
| | | 623 | | } |
| | | 624 | | |
| | | 625 | | private static bool CheckCylinderCircleSlabAxis( |
| | | 626 | | LSCylinderCollider cylinder, |
| | | 627 | | LSCircleCollider2D circle, |
| | | 628 | | Vector3d cylinderAxis, |
| | | 629 | | Vector3d axis, |
| | | 630 | | ref AxisPenetration penetration) |
| | | 631 | | { |
| | 264 | 632 | | if (!TryNormalizeAxis(axis, out Vector3d normalizedAxis)) |
| | 28 | 633 | | return true; |
| | | 634 | | |
| | 236 | 635 | | if (!FixedSegment.TryGetCenteredFiniteCylinderCapsuleSlabAxisPenetration( |
| | 236 | 636 | | normalizedAxis, |
| | 236 | 637 | | cylinder.Center, |
| | 236 | 638 | | cylinderAxis, |
| | 236 | 639 | | cylinder.Height, |
| | 236 | 640 | | cylinder.ScaledRadius, |
| | 236 | 641 | | GetEmbeddedCenter3D(circle), |
| | 236 | 642 | | Vector2d.Forward, |
| | 236 | 643 | | Fixed64.Zero, |
| | 236 | 644 | | circle.ScaledRadius, |
| | 236 | 645 | | circle.MixedHalfThickness, |
| | 236 | 646 | | out Vector3d orientedAxis, |
| | 236 | 647 | | out Fixed64 depth, |
| | 236 | 648 | | out bool depthIsClamped)) |
| | | 649 | | { |
| | 3 | 650 | | return false; |
| | | 651 | | } |
| | | 652 | | |
| | 233 | 653 | | return KeepAxisPenetration( |
| | 233 | 654 | | orientedAxis, |
| | 233 | 655 | | depth, |
| | 233 | 656 | | depthIsClamped, |
| | 233 | 657 | | ref penetration); |
| | | 658 | | } |
| | | 659 | | |
| | | 660 | | private static bool CheckConeCircleSlabAxis( |
| | | 661 | | LSConeCollider cone, |
| | | 662 | | LSCircleCollider2D circle, |
| | | 663 | | Vector3d axis, |
| | | 664 | | ref AxisPenetration penetration) |
| | | 665 | | { |
| | 256 | 666 | | if (!TryNormalizeAxis(axis, out Vector3d normalizedAxis)) |
| | 22 | 667 | | return true; |
| | | 668 | | |
| | 234 | 669 | | if (!FixedSegment.TryGetCenteredFiniteConeCapsuleSlabAxisPenetration( |
| | 234 | 670 | | normalizedAxis, |
| | 234 | 671 | | cone.Center, |
| | 234 | 672 | | GetRigidUpAxis(cone.Rotation), |
| | 234 | 673 | | cone.Height, |
| | 234 | 674 | | cone.ScaledRadius, |
| | 234 | 675 | | GetEmbeddedCenter3D(circle), |
| | 234 | 676 | | Vector2d.Forward, |
| | 234 | 677 | | Fixed64.Zero, |
| | 234 | 678 | | circle.ScaledRadius, |
| | 234 | 679 | | circle.MixedHalfThickness, |
| | 234 | 680 | | out Vector3d orientedAxis, |
| | 234 | 681 | | out Fixed64 depth, |
| | 234 | 682 | | out bool depthIsClamped)) |
| | | 683 | | { |
| | 3 | 684 | | return false; |
| | | 685 | | } |
| | | 686 | | |
| | 231 | 687 | | return KeepAxisPenetration( |
| | 231 | 688 | | orientedAxis, |
| | 231 | 689 | | depth, |
| | 231 | 690 | | depthIsClamped, |
| | 231 | 691 | | ref penetration); |
| | | 692 | | } |
| | | 693 | | |
| | | 694 | | private static bool CheckCapsulePrismAxis( |
| | | 695 | | LSCapsuleCollider capsule, |
| | | 696 | | LSCapsuleCollider2D prism, |
| | | 697 | | Vector3d capsuleAxis, |
| | | 698 | | Vector2d prismAxis, |
| | | 699 | | Vector3d axis, |
| | | 700 | | ref AxisPenetration penetration) |
| | | 701 | | { |
| | 932 | 702 | | if (!TryNormalizeAxis(axis, out Vector3d normalizedAxis)) |
| | 51 | 703 | | return true; |
| | | 704 | | |
| | 881 | 705 | | if (!FixedSegment.TryGetCenteredCapsuleCapsuleSlabAxisPenetration( |
| | 881 | 706 | | normalizedAxis, |
| | 881 | 707 | | capsule.Center, |
| | 881 | 708 | | capsuleAxis, |
| | 881 | 709 | | capsule.AxisLength, |
| | 881 | 710 | | capsule.ScaledRadius, |
| | 881 | 711 | | GetEmbeddedCenter3D(prism), |
| | 881 | 712 | | prismAxis, |
| | 881 | 713 | | prism.AxisLength, |
| | 881 | 714 | | prism.ScaledRadius, |
| | 881 | 715 | | prism.MixedHalfThickness, |
| | 881 | 716 | | out Vector3d orientedAxis, |
| | 881 | 717 | | out Fixed64 depth, |
| | 881 | 718 | | out bool depthIsClamped)) |
| | 18 | 719 | | return false; |
| | | 720 | | |
| | 863 | 721 | | return KeepAxisPenetration( |
| | 863 | 722 | | orientedAxis, |
| | 863 | 723 | | depth, |
| | 863 | 724 | | depthIsClamped, |
| | 863 | 725 | | ref penetration); |
| | | 726 | | } |
| | | 727 | | |
| | | 728 | | private static bool CheckCylinderPrismAxis( |
| | | 729 | | LSCylinderCollider cylinder, |
| | | 730 | | LSCapsuleCollider2D prism, |
| | | 731 | | Vector3d cylinderAxis, |
| | | 732 | | Vector2d prismAxis, |
| | | 733 | | Vector3d axis, |
| | | 734 | | ref AxisPenetration penetration) |
| | | 735 | | { |
| | 701 | 736 | | if (!TryNormalizeAxis(axis, out Vector3d normalizedAxis)) |
| | 43 | 737 | | return true; |
| | | 738 | | |
| | 658 | 739 | | if (!FixedSegment.TryGetCenteredFiniteCylinderCapsuleSlabAxisPenetration( |
| | 658 | 740 | | normalizedAxis, |
| | 658 | 741 | | cylinder.Center, |
| | 658 | 742 | | cylinderAxis, |
| | 658 | 743 | | cylinder.Height, |
| | 658 | 744 | | cylinder.ScaledRadius, |
| | 658 | 745 | | GetEmbeddedCenter3D(prism), |
| | 658 | 746 | | prismAxis, |
| | 658 | 747 | | prism.AxisLength, |
| | 658 | 748 | | prism.ScaledRadius, |
| | 658 | 749 | | prism.MixedHalfThickness, |
| | 658 | 750 | | out Vector3d orientedAxis, |
| | 658 | 751 | | out Fixed64 depth, |
| | 658 | 752 | | out bool depthIsClamped)) |
| | 11 | 753 | | return false; |
| | | 754 | | |
| | 647 | 755 | | return KeepAxisPenetration( |
| | 647 | 756 | | orientedAxis, |
| | 647 | 757 | | depth, |
| | 647 | 758 | | depthIsClamped, |
| | 647 | 759 | | ref penetration); |
| | | 760 | | } |
| | | 761 | | |
| | | 762 | | private static bool CheckConePrismAxis( |
| | | 763 | | LSConeCollider cone, |
| | | 764 | | LSCapsuleCollider2D prism, |
| | | 765 | | Vector2d prismAxis, |
| | | 766 | | Vector3d axis, |
| | | 767 | | ref AxisPenetration penetration) |
| | | 768 | | { |
| | 690 | 769 | | if (!TryNormalizeAxis(axis, out Vector3d normalizedAxis)) |
| | 46 | 770 | | return true; |
| | | 771 | | |
| | 644 | 772 | | if (!FixedSegment.TryGetCenteredFiniteConeCapsuleSlabAxisPenetration( |
| | 644 | 773 | | normalizedAxis, |
| | 644 | 774 | | cone.Center, |
| | 644 | 775 | | GetRigidUpAxis(cone.Rotation), |
| | 644 | 776 | | cone.Height, |
| | 644 | 777 | | cone.ScaledRadius, |
| | 644 | 778 | | GetEmbeddedCenter3D(prism), |
| | 644 | 779 | | prismAxis, |
| | 644 | 780 | | prism.AxisLength, |
| | 644 | 781 | | prism.ScaledRadius, |
| | 644 | 782 | | prism.MixedHalfThickness, |
| | 644 | 783 | | out Vector3d orientedAxis, |
| | 644 | 784 | | out Fixed64 depth, |
| | 644 | 785 | | out bool depthIsClamped)) |
| | 13 | 786 | | return false; |
| | | 787 | | |
| | 631 | 788 | | return KeepAxisPenetration( |
| | 631 | 789 | | orientedAxis, |
| | 631 | 790 | | depth, |
| | 631 | 791 | | depthIsClamped, |
| | 631 | 792 | | ref penetration); |
| | | 793 | | } |
| | | 794 | | |
| | | 795 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 796 | | private static bool KeepAxisPenetration( |
| | | 797 | | Vector3d axis, |
| | | 798 | | Fixed64 depth, |
| | | 799 | | bool depthIsClamped, |
| | | 800 | | ref AxisPenetration penetration) |
| | | 801 | | { |
| | 2605 | 802 | | if (!penetration.HasValue || depth < penetration.Depth) |
| | | 803 | | { |
| | 1184 | 804 | | penetration = new AxisPenetration( |
| | 1184 | 805 | | axis, |
| | 1184 | 806 | | depth, |
| | 1184 | 807 | | depthIsClamped); |
| | | 808 | | } |
| | | 809 | | |
| | 2605 | 810 | | return true; |
| | | 811 | | } |
| | | 812 | | |
| | | 813 | | private static bool CheckEmbeddedCapsuleAxes( |
| | | 814 | | LSCapsuleCollider capsule3D, |
| | | 815 | | Vector3d capsule3DAxis, |
| | | 816 | | LSCapsuleCollider2D capsule2D, |
| | | 817 | | Vector2d capsule2DPlanarAxis, |
| | | 818 | | Vector3d capsule2DAxis, |
| | | 819 | | Vector3d capsule2DNormal, |
| | | 820 | | ref AxisPenetration penetration) |
| | | 821 | | { |
| | 132 | 822 | | return CheckCapsulePrismAxis( |
| | 132 | 823 | | capsule3D, |
| | 132 | 824 | | capsule2D, |
| | 132 | 825 | | capsule3DAxis, |
| | 132 | 826 | | capsule2DPlanarAxis, |
| | 132 | 827 | | capsule2DAxis, |
| | 132 | 828 | | ref penetration) |
| | 132 | 829 | | && CheckCapsulePrismAxis( |
| | 132 | 830 | | capsule3D, |
| | 132 | 831 | | capsule2D, |
| | 132 | 832 | | capsule3DAxis, |
| | 132 | 833 | | capsule2DPlanarAxis, |
| | 132 | 834 | | capsule2DNormal, |
| | 132 | 835 | | ref penetration); |
| | | 836 | | } |
| | | 837 | | |
| | | 838 | | private static bool CheckEmbeddedCapsuleAxes( |
| | | 839 | | LSCylinderCollider cylinder, |
| | | 840 | | Vector3d cylinderAxis, |
| | | 841 | | LSCapsuleCollider2D capsule, |
| | | 842 | | Vector2d capsulePlanarAxis, |
| | | 843 | | Vector3d capsuleAxis, |
| | | 844 | | Vector3d capsuleNormal, |
| | | 845 | | ref AxisPenetration penetration) |
| | | 846 | | { |
| | 102 | 847 | | return CheckCylinderPrismAxis( |
| | 102 | 848 | | cylinder, |
| | 102 | 849 | | capsule, |
| | 102 | 850 | | cylinderAxis, |
| | 102 | 851 | | capsulePlanarAxis, |
| | 102 | 852 | | capsuleAxis, |
| | 102 | 853 | | ref penetration) |
| | 102 | 854 | | && CheckCylinderPrismAxis( |
| | 102 | 855 | | cylinder, |
| | 102 | 856 | | capsule, |
| | 102 | 857 | | cylinderAxis, |
| | 102 | 858 | | capsulePlanarAxis, |
| | 102 | 859 | | capsuleNormal, |
| | 102 | 860 | | ref penetration); |
| | | 861 | | } |
| | | 862 | | |
| | | 863 | | private static bool CheckEmbeddedCapsuleAxes( |
| | | 864 | | LSConeCollider cone, |
| | | 865 | | LSCapsuleCollider2D capsule, |
| | | 866 | | Vector2d capsulePlanarAxis, |
| | | 867 | | Vector3d capsuleAxis, |
| | | 868 | | Vector3d capsuleNormal, |
| | | 869 | | ref AxisPenetration penetration) |
| | | 870 | | { |
| | 99 | 871 | | return CheckConePrismAxis( |
| | 99 | 872 | | cone, |
| | 99 | 873 | | capsule, |
| | 99 | 874 | | capsulePlanarAxis, |
| | 99 | 875 | | capsuleAxis, |
| | 99 | 876 | | ref penetration) |
| | 99 | 877 | | && CheckConePrismAxis( |
| | 99 | 878 | | cone, |
| | 99 | 879 | | capsule, |
| | 99 | 880 | | capsulePlanarAxis, |
| | 99 | 881 | | capsuleNormal, |
| | 99 | 882 | | ref penetration); |
| | | 883 | | } |
| | | 884 | | |
| | | 885 | | private static bool CheckCapsuleEmbeddedCapsuleEdgeAxis( |
| | | 886 | | LSCapsuleCollider capsule3D, |
| | | 887 | | Vector3d capsule3DAxis, |
| | | 888 | | LSCapsuleCollider2D capsule2D, |
| | | 889 | | Vector2d capsule2DPlanarAxis, |
| | | 890 | | Vector3d capsule2DAxis, |
| | | 891 | | ref AxisPenetration penetration) |
| | | 892 | | { |
| | 124 | 893 | | return CheckCapsulePrismAxis( |
| | 124 | 894 | | capsule3D, |
| | 124 | 895 | | capsule2D, |
| | 124 | 896 | | capsule3DAxis, |
| | 124 | 897 | | capsule2DPlanarAxis, |
| | 124 | 898 | | Vector3d.Cross(capsule3DAxis, capsule2DAxis), |
| | 124 | 899 | | ref penetration); |
| | | 900 | | } |
| | | 901 | | |
| | | 902 | | private static bool CheckCylinderEmbeddedCapsuleEdgeAxis( |
| | | 903 | | LSCylinderCollider cylinder, |
| | | 904 | | Vector3d cylinderAxis, |
| | | 905 | | LSCapsuleCollider2D capsule, |
| | | 906 | | Vector2d capsulePlanarAxis, |
| | | 907 | | Vector3d capsuleAxis, |
| | | 908 | | ref AxisPenetration penetration) |
| | | 909 | | { |
| | 94 | 910 | | return CheckCylinderPrismAxis( |
| | 94 | 911 | | cylinder, |
| | 94 | 912 | | capsule, |
| | 94 | 913 | | cylinderAxis, |
| | 94 | 914 | | capsulePlanarAxis, |
| | 94 | 915 | | Vector3d.Cross(cylinderAxis, capsuleAxis), |
| | 94 | 916 | | ref penetration); |
| | | 917 | | } |
| | | 918 | | |
| | | 919 | | private static bool CheckConeEmbeddedCapsuleEdgeAxis( |
| | | 920 | | LSConeCollider cone, |
| | | 921 | | Vector3d coneAxis, |
| | | 922 | | LSCapsuleCollider2D capsule, |
| | | 923 | | Vector2d capsulePlanarAxis, |
| | | 924 | | Vector3d capsuleAxis, |
| | | 925 | | ref AxisPenetration penetration) |
| | | 926 | | { |
| | 96 | 927 | | return CheckConePrismAxis( |
| | 96 | 928 | | cone, |
| | 96 | 929 | | capsule, |
| | 96 | 930 | | capsulePlanarAxis, |
| | 96 | 931 | | Vector3d.Cross(coneAxis, capsuleAxis), |
| | 96 | 932 | | ref penetration); |
| | | 933 | | } |
| | | 934 | | |
| | | 935 | | private static bool BuildCapsuleContact( |
| | | 936 | | LSCapsuleCollider capsule, |
| | | 937 | | LSCollider2D embedded, |
| | | 938 | | AxisPenetration penetration, |
| | | 939 | | out MixedContact contact) |
| | | 940 | | { |
| | 216 | 941 | | contact = BuildCanonicalSupportContact( |
| | 216 | 942 | | capsule, |
| | 216 | 943 | | embedded, |
| | 216 | 944 | | penetration.Axis, |
| | 216 | 945 | | penetration.Depth, |
| | 216 | 946 | | penetration.DepthIsClamped); |
| | 216 | 947 | | return true; |
| | | 948 | | } |
| | | 949 | | |
| | | 950 | | private static bool BuildCylinderContact( |
| | | 951 | | LSCylinderCollider cylinder, |
| | | 952 | | LSCollider2D embedded, |
| | | 953 | | AxisPenetration penetration, |
| | | 954 | | out MixedContact contact) |
| | | 955 | | { |
| | 157 | 956 | | contact = BuildCanonicalSupportContact( |
| | 157 | 957 | | cylinder, |
| | 157 | 958 | | embedded, |
| | 157 | 959 | | penetration.Axis, |
| | 157 | 960 | | penetration.Depth, |
| | 157 | 961 | | penetration.DepthIsClamped); |
| | 157 | 962 | | return true; |
| | | 963 | | } |
| | | 964 | | |
| | | 965 | | private static bool BuildConeContact( |
| | | 966 | | LSConeCollider cone, |
| | | 967 | | LSCollider2D embedded, |
| | | 968 | | AxisPenetration penetration, |
| | | 969 | | out MixedContact contact) |
| | | 970 | | { |
| | 150 | 971 | | contact = BuildCanonicalSupportContact( |
| | 150 | 972 | | cone, |
| | 150 | 973 | | embedded, |
| | 150 | 974 | | penetration.Axis, |
| | 150 | 975 | | penetration.Depth, |
| | 150 | 976 | | penetration.DepthIsClamped); |
| | 150 | 977 | | return true; |
| | | 978 | | } |
| | | 979 | | |
| | | 980 | | private static MixedContact BuildCanonicalSupportContact( |
| | | 981 | | LSCollider collider3D, |
| | | 982 | | LSCollider2D embedded, |
| | | 983 | | Vector3d normal3DTo2D, |
| | | 984 | | Fixed64 depth, |
| | | 985 | | bool depthIsClamped) |
| | | 986 | | { |
| | 523 | 987 | | return new MixedContact( |
| | 523 | 988 | | new ContactAnchor( |
| | 523 | 989 | | ConvexColliderSupport.GetSupportAnchor( |
| | 523 | 990 | | collider3D, |
| | 523 | 991 | | normal3DTo2D, |
| | 523 | 992 | | Vector3d.Zero)), |
| | 523 | 993 | | MixedEmbedded2DGeometry.GetSupportAnchor(embedded, -normal3DTo2D), |
| | 523 | 994 | | normal3DTo2D, |
| | 523 | 995 | | depth, |
| | 523 | 996 | | depthIsClamped); |
| | | 997 | | } |
| | | 998 | | |
| | | 999 | | } |