| | | 1 | | //======================================================================= |
| | | 2 | | // MeshTriangleContactGenerator.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 SwiftCollections; |
| | | 12 | | using SwiftCollections.Query; |
| | | 13 | | using System; |
| | | 14 | | using System.Runtime.CompilerServices; |
| | | 15 | | |
| | | 16 | | namespace Gravitas.CollisionHandling; |
| | | 17 | | |
| | | 18 | | /// <summary> |
| | | 19 | | /// Builds deterministic triangle-level manifolds for mesh paths that cannot use |
| | | 20 | | /// whole-shape convex assumptions. |
| | | 21 | | /// </summary> |
| | | 22 | | internal static class MeshTriangleContactGenerator |
| | | 23 | | { |
| | | 24 | | public static bool TryBuildMeshSphereManifold( |
| | | 25 | | CollisionWorkItem pair, |
| | | 26 | | LSMeshCollider mesh, |
| | | 27 | | LSSphereCollider sphere, |
| | | 28 | | SwiftList<int> triangleBuffer) |
| | | 29 | | { |
| | 12 | 30 | | mesh.GetTrianglesInBounds(CreateBounds(sphere.BoundsMin, sphere.BoundsMax), triangleBuffer); |
| | 90 | 31 | | for (int i = 0; i < triangleBuffer.Count; i++) |
| | 33 | 32 | | TryAddSphereTriangleContact(pair, mesh, triangleBuffer[i], sphere); |
| | | 33 | | |
| | 12 | 34 | | return pair.Manifold.HasContact; |
| | | 35 | | } |
| | | 36 | | |
| | | 37 | | public static bool TryBuildMeshCapsuleManifold( |
| | | 38 | | CollisionWorkItem pair, |
| | | 39 | | LSMeshCollider mesh, |
| | | 40 | | LSCapsuleCollider capsule, |
| | | 41 | | SwiftList<int> triangleBuffer) |
| | | 42 | | { |
| | 5 | 43 | | mesh.GetTrianglesInBounds(CreateBounds(capsule.BoundsMin, capsule.BoundsMax), triangleBuffer); |
| | | 44 | | |
| | 5 | 45 | | bool overlaps = false; |
| | 32 | 46 | | for (int i = 0; i < triangleBuffer.Count; i++) |
| | 11 | 47 | | overlaps |= TryAddCapsuleTriangleContact(pair, mesh, triangleBuffer[i], capsule); |
| | | 48 | | |
| | 5 | 49 | | return overlaps; |
| | | 50 | | } |
| | | 51 | | |
| | | 52 | | public static bool TryBuildMeshCuboidManifold( |
| | | 53 | | CollisionWorkItem pair, |
| | | 54 | | LSMeshCollider mesh, |
| | | 55 | | LSCuboidCollider cuboid, |
| | | 56 | | SwiftList<int> triangleBuffer) |
| | | 57 | | { |
| | 689 | 58 | | mesh.GetTrianglesInBounds(CreateBounds(cuboid.BoundsMin, cuboid.BoundsMax), triangleBuffer); |
| | 3640 | 59 | | for (int i = 0; i < triangleBuffer.Count; i++) |
| | 1131 | 60 | | TryAddCuboidTriangleContact(pair, mesh, triangleBuffer[i], cuboid); |
| | | 61 | | |
| | 689 | 62 | | return pair.Manifold.HasContact; |
| | | 63 | | } |
| | | 64 | | |
| | | 65 | | public static bool TryBuildMeshCylinderManifold( |
| | | 66 | | CollisionWorkItem pair, |
| | | 67 | | LSMeshCollider mesh, |
| | | 68 | | LSCylinderCollider cylinder, |
| | | 69 | | SwiftList<int> triangleBuffer) |
| | | 70 | | { |
| | 428 | 71 | | mesh.GetTrianglesInBounds(CreateBounds(cylinder.BoundsMin, cylinder.BoundsMax), triangleBuffer); |
| | 428 | 72 | | bool overlaps = false; |
| | 2564 | 73 | | for (int i = 0; i < triangleBuffer.Count; i++) |
| | 854 | 74 | | overlaps |= TryAddCylinderTriangleContact(pair, mesh, triangleBuffer[i], cylinder); |
| | | 75 | | |
| | 428 | 76 | | return overlaps; |
| | | 77 | | } |
| | | 78 | | |
| | | 79 | | public static bool TryBuildMeshMeshManifold( |
| | | 80 | | CollisionWorkItem pair, |
| | | 81 | | LSMeshCollider meshA, |
| | | 82 | | LSMeshCollider meshB, |
| | | 83 | | SwiftList<int> triangleBufferA, |
| | | 84 | | SwiftList<int> triangleBufferB) |
| | | 85 | | { |
| | 645 | 86 | | bool reverseContact = meshA.Id > meshB.Id; |
| | 645 | 87 | | LSMeshCollider firstMesh = reverseContact ? meshB : meshA; |
| | 645 | 88 | | LSMeshCollider secondMesh = reverseContact ? meshA : meshB; |
| | 645 | 89 | | firstMesh.GetTrianglesInBounds( |
| | 645 | 90 | | CreateBounds(secondMesh.BoundsMin, secondMesh.BoundsMax), |
| | 645 | 91 | | triangleBufferA); |
| | 16582 | 92 | | for (int i = 0; i < triangleBufferA.Count; i++) |
| | | 93 | | { |
| | 7646 | 94 | | int triangleA = triangleBufferA[i]; |
| | 7646 | 95 | | firstMesh.Mesh.GetLocalTriangleVertices( |
| | 7646 | 96 | | triangleA, |
| | 7646 | 97 | | out Vector3d firstVertex, |
| | 7646 | 98 | | out Vector3d secondVertex, |
| | 7646 | 99 | | out Vector3d thirdVertex); |
| | 7646 | 100 | | var firstTriangle = |
| | 7646 | 101 | | new FixedTriangle(firstVertex, secondVertex, thirdVertex); |
| | 7646 | 102 | | GetTriangleBoundsInFrame( |
| | 7646 | 103 | | firstTriangle.Bounds, |
| | 7646 | 104 | | firstMesh.Mesh.Origin, |
| | 7646 | 105 | | firstMesh.Mesh.Rotation, |
| | 7646 | 106 | | secondMesh.Mesh.Origin, |
| | 7646 | 107 | | secondMesh.Mesh.Rotation, |
| | 7646 | 108 | | out FixedBoundVolume firstInSecondFrameBounds); |
| | 7646 | 109 | | secondMesh.Mesh.GetTrianglesInLocalBounds( |
| | 7646 | 110 | | firstInSecondFrameBounds, |
| | 7646 | 111 | | triangleBufferB); |
| | | 112 | | |
| | 76588 | 113 | | for (int j = 0; j < triangleBufferB.Count; j++) |
| | | 114 | | { |
| | 30648 | 115 | | int triangleB = triangleBufferB[j]; |
| | 30648 | 116 | | secondMesh.Mesh.GetLocalTriangleVertices( |
| | 30648 | 117 | | triangleB, |
| | 30648 | 118 | | out Vector3d secondFirstVertex, |
| | 30648 | 119 | | out Vector3d secondSecondVertex, |
| | 30648 | 120 | | out Vector3d secondThirdVertex); |
| | 30648 | 121 | | var secondTriangle = new FixedTriangle( |
| | 30648 | 122 | | secondFirstVertex, |
| | 30648 | 123 | | secondSecondVertex, |
| | 30648 | 124 | | secondThirdVertex); |
| | 30648 | 125 | | if (!firstTriangle.TryGetContact( |
| | 30648 | 126 | | firstMesh.Mesh.Origin, |
| | 30648 | 127 | | firstMesh.Mesh.Rotation, |
| | 30648 | 128 | | secondMesh.Mesh.Origin, |
| | 30648 | 129 | | secondMesh.Mesh.Rotation, |
| | 30648 | 130 | | secondTriangle, |
| | 30648 | 131 | | out FixedContactAnchors contact)) |
| | | 132 | | { |
| | | 133 | | continue; |
| | | 134 | | } |
| | | 135 | | |
| | 19583 | 136 | | if (reverseContact) |
| | | 137 | | { |
| | 85 | 138 | | AddContact( |
| | 85 | 139 | | pair, |
| | 85 | 140 | | new ContactAnchor(contact.SecondAnchor), |
| | 85 | 141 | | new ContactAnchor(contact.FirstAnchor), |
| | 85 | 142 | | contact.Depth, |
| | 85 | 143 | | -contact.Normal, |
| | 85 | 144 | | contact.DepthIsClamped); |
| | | 145 | | } |
| | | 146 | | else |
| | | 147 | | { |
| | 19498 | 148 | | AddContact( |
| | 19498 | 149 | | pair, |
| | 19498 | 150 | | new ContactAnchor(contact.FirstAnchor), |
| | 19498 | 151 | | new ContactAnchor(contact.SecondAnchor), |
| | 19498 | 152 | | contact.Depth, |
| | 19498 | 153 | | contact.Normal, |
| | 19498 | 154 | | contact.DepthIsClamped); |
| | | 155 | | } |
| | | 156 | | } |
| | | 157 | | } |
| | | 158 | | |
| | 645 | 159 | | return pair.Manifold.HasContact; |
| | | 160 | | } |
| | | 161 | | |
| | | 162 | | private static void TryAddSphereTriangleContact( |
| | | 163 | | CollisionWorkItem pair, |
| | | 164 | | LSMeshCollider mesh, |
| | | 165 | | int triangleIndex, |
| | | 166 | | LSSphereCollider sphere) |
| | | 167 | | { |
| | 33 | 168 | | mesh.Mesh.GetLocalTriangleVertices( |
| | 33 | 169 | | triangleIndex, |
| | 33 | 170 | | out Vector3d first, |
| | 33 | 171 | | out Vector3d second, |
| | 33 | 172 | | out Vector3d third); |
| | 33 | 173 | | if (!new FixedTriangle(first, second, third).TryGetSphereContact( |
| | 33 | 174 | | mesh.Mesh.Origin, |
| | 33 | 175 | | mesh.Mesh.Rotation, |
| | 33 | 176 | | sphere.Center, |
| | 33 | 177 | | sphere.Rotation, |
| | 33 | 178 | | sphere.ScaledRadius, |
| | 33 | 179 | | out FixedContactAnchors contact)) |
| | | 180 | | { |
| | 19 | 181 | | return; |
| | | 182 | | } |
| | | 183 | | |
| | 14 | 184 | | AddContact( |
| | 14 | 185 | | pair, |
| | 14 | 186 | | new ContactAnchor(contact.FirstAnchor), |
| | 14 | 187 | | new ContactAnchor(contact.SecondAnchor), |
| | 14 | 188 | | contact.Depth, |
| | 14 | 189 | | contact.Normal, |
| | 14 | 190 | | contact.DepthIsClamped); |
| | 14 | 191 | | } |
| | | 192 | | |
| | | 193 | | private static bool TryAddCapsuleTriangleContact( |
| | | 194 | | CollisionWorkItem pair, |
| | | 195 | | LSMeshCollider mesh, |
| | | 196 | | int triangleIndex, |
| | | 197 | | LSCapsuleCollider capsule) |
| | | 198 | | { |
| | 11 | 199 | | mesh.Mesh.GetLocalTriangleVertices( |
| | 11 | 200 | | triangleIndex, |
| | 11 | 201 | | out Vector3d first, |
| | 11 | 202 | | out Vector3d second, |
| | 11 | 203 | | out Vector3d third); |
| | 11 | 204 | | var triangle = new FixedTriangle(first, second, third); |
| | 11 | 205 | | Vector3d fallbackNormal = OrientNormal( |
| | 11 | 206 | | mesh.Mesh.CreatePointAnchor(triangle.Centroid), |
| | 11 | 207 | | new FixedPointAnchor( |
| | 11 | 208 | | capsule.Center, |
| | 11 | 209 | | FixedQuaternion.Identity, |
| | 11 | 210 | | Vector3d.Zero), |
| | 11 | 211 | | mesh.Mesh.GetFaceNormalWorld(triangleIndex)); |
| | 11 | 212 | | if (!triangle.TryGetCenteredCapsuleContact( |
| | 11 | 213 | | mesh.Mesh.Origin, |
| | 11 | 214 | | mesh.Mesh.Rotation, |
| | 11 | 215 | | capsule.Center, |
| | 11 | 216 | | capsule.Rotation, |
| | 11 | 217 | | capsule.AxisLength, |
| | 11 | 218 | | capsule.ScaledRadius, |
| | 11 | 219 | | fallbackNormal, |
| | 11 | 220 | | out FixedContactAnchors contact)) |
| | | 221 | | { |
| | 4 | 222 | | return false; |
| | | 223 | | } |
| | | 224 | | |
| | 7 | 225 | | AddContact( |
| | 7 | 226 | | pair, |
| | 7 | 227 | | new ContactAnchor(contact.FirstAnchor), |
| | 7 | 228 | | new ContactAnchor(contact.SecondAnchor), |
| | 7 | 229 | | contact.Depth, |
| | 7 | 230 | | contact.Normal, |
| | 7 | 231 | | contact.DepthIsClamped); |
| | 7 | 232 | | return true; |
| | | 233 | | } |
| | | 234 | | |
| | | 235 | | private static void TryAddCuboidTriangleContact( |
| | | 236 | | CollisionWorkItem pair, |
| | | 237 | | LSMeshCollider mesh, |
| | | 238 | | int triangleIndex, |
| | | 239 | | LSCuboidCollider cuboid) |
| | | 240 | | { |
| | 1131 | 241 | | mesh.Mesh.GetLocalTriangleVertices( |
| | 1131 | 242 | | triangleIndex, |
| | 1131 | 243 | | out Vector3d first, |
| | 1131 | 244 | | out Vector3d second, |
| | 1131 | 245 | | out Vector3d third); |
| | 1131 | 246 | | var triangle = new FixedTriangle(first, second, third); |
| | 1131 | 247 | | Span<FixedContactLocalPoints> faceContacts = |
| | 1131 | 248 | | stackalloc FixedContactLocalPoints[4]; |
| | 1131 | 249 | | if (!cuboid.OrientedBox.TryGetTriangleContact( |
| | 1131 | 250 | | mesh.Mesh.Origin, |
| | 1131 | 251 | | mesh.Mesh.Rotation, |
| | 1131 | 252 | | triangle, |
| | 1131 | 253 | | faceContacts, |
| | 1131 | 254 | | out FixedContactAnchors contact, |
| | 1131 | 255 | | out int faceContactCount)) |
| | | 256 | | { |
| | 237 | 257 | | return; |
| | | 258 | | } |
| | | 259 | | |
| | 894 | 260 | | if (faceContactCount > 0) |
| | | 261 | | { |
| | 5962 | 262 | | for (int index = 0; index < faceContactCount; index++) |
| | | 263 | | { |
| | 2130 | 264 | | AddReversedBoxTriangleContact( |
| | 2130 | 265 | | pair, |
| | 2130 | 266 | | contact, |
| | 2130 | 267 | | faceContacts[index]); |
| | | 268 | | } |
| | 851 | 269 | | return; |
| | | 270 | | } |
| | | 271 | | |
| | 43 | 272 | | AddReversedBoxTriangleContact(pair, contact); |
| | 43 | 273 | | } |
| | | 274 | | |
| | | 275 | | private static void AddReversedBoxTriangleContact( |
| | | 276 | | CollisionWorkItem pair, |
| | | 277 | | FixedContactAnchors contact) |
| | | 278 | | { |
| | 43 | 279 | | AddContact( |
| | 43 | 280 | | pair, |
| | 43 | 281 | | new ContactAnchor(contact.SecondAnchor), |
| | 43 | 282 | | new ContactAnchor(contact.FirstAnchor), |
| | 43 | 283 | | contact.Depth, |
| | 43 | 284 | | -contact.Normal, |
| | 43 | 285 | | contact.DepthIsClamped); |
| | 43 | 286 | | } |
| | | 287 | | |
| | | 288 | | private static void AddReversedBoxTriangleContact( |
| | | 289 | | CollisionWorkItem pair, |
| | | 290 | | FixedContactAnchors primary, |
| | | 291 | | FixedContactLocalPoints contact) |
| | | 292 | | { |
| | 2130 | 293 | | AddContact( |
| | 2130 | 294 | | pair, |
| | 2130 | 295 | | new ContactAnchor( |
| | 2130 | 296 | | primary.SecondAnchor.Origin, |
| | 2130 | 297 | | primary.SecondAnchor.Rotation, |
| | 2130 | 298 | | contact.SecondLocalPoint), |
| | 2130 | 299 | | new ContactAnchor( |
| | 2130 | 300 | | primary.FirstAnchor.Origin, |
| | 2130 | 301 | | primary.FirstAnchor.Rotation, |
| | 2130 | 302 | | contact.FirstLocalPoint), |
| | 2130 | 303 | | primary.Depth, |
| | 2130 | 304 | | -primary.Normal, |
| | 2130 | 305 | | primary.DepthIsClamped); |
| | 2130 | 306 | | } |
| | | 307 | | |
| | | 308 | | private static bool TryAddCylinderTriangleContact( |
| | | 309 | | CollisionWorkItem pair, |
| | | 310 | | LSMeshCollider mesh, |
| | | 311 | | int triangleIndex, |
| | | 312 | | LSCylinderCollider cylinder) |
| | | 313 | | { |
| | 854 | 314 | | mesh.Mesh.GetLocalTriangleVertices( |
| | 854 | 315 | | triangleIndex, |
| | 854 | 316 | | out Vector3d first, |
| | 854 | 317 | | out Vector3d second, |
| | 854 | 318 | | out Vector3d third); |
| | 854 | 319 | | var triangle = new FixedTriangle(first, second, third); |
| | 854 | 320 | | var cylinderCenter = new FixedPointAnchor( |
| | 854 | 321 | | cylinder.Center, |
| | 854 | 322 | | FixedQuaternion.Identity, |
| | 854 | 323 | | Vector3d.Zero); |
| | 854 | 324 | | Vector3d normal = OrientNormal( |
| | 854 | 325 | | mesh.Mesh.CreatePointAnchor(triangle.Centroid), |
| | 854 | 326 | | cylinderCenter, |
| | 854 | 327 | | mesh.Mesh.GetFaceNormalWorld(triangleIndex)); |
| | 854 | 328 | | if (TryAddCylinderCapTriangleContacts( |
| | 854 | 329 | | pair, |
| | 854 | 330 | | mesh, |
| | 854 | 331 | | triangle, |
| | 854 | 332 | | cylinder, |
| | 854 | 333 | | normal)) |
| | | 334 | | { |
| | 428 | 335 | | return true; |
| | | 336 | | } |
| | | 337 | | |
| | 426 | 338 | | if (!cylinderCenter.TryGetLocalPointIn( |
| | 426 | 339 | | mesh.Mesh.Origin, |
| | 426 | 340 | | mesh.Mesh.Rotation, |
| | 426 | 341 | | out Vector3d localCylinderCenter)) |
| | | 342 | | { |
| | 1 | 343 | | return false; |
| | | 344 | | } |
| | | 345 | | |
| | 425 | 346 | | Vector3d localPointOnMesh = |
| | 425 | 347 | | triangle.ClosestPoint(localCylinderCenter); |
| | 425 | 348 | | FixedPointAnchor meshAnchor = |
| | 425 | 349 | | mesh.Mesh.CreatePointAnchor(localPointOnMesh); |
| | | 350 | | // The closest point belongs to the admitted triangle candidate, so its |
| | | 351 | | // cylinder-frame offset is finite once the center entered the mesh frame. |
| | 425 | 352 | | _ = meshAnchor.TryGetLocalPointIn( |
| | 425 | 353 | | cylinder.Center, |
| | 425 | 354 | | cylinder.Rotation, |
| | 425 | 355 | | out Vector3d localPointInCylinder); |
| | 425 | 356 | | if (!FixedSegment.ContainsPointInCenteredFiniteCylinder( |
| | 425 | 357 | | localPointInCylinder, |
| | 425 | 358 | | Vector3d.Zero, |
| | 425 | 359 | | Vector3d.Up, |
| | 425 | 360 | | cylinder.Height, |
| | 425 | 361 | | cylinder.ScaledRadius)) |
| | | 362 | | { |
| | 5 | 363 | | return false; |
| | | 364 | | } |
| | | 365 | | // Exact containment proves the centered canonical surface offset |
| | | 366 | | // remains inside the admitted radius and height. |
| | 420 | 367 | | _ = FixedSegment.TryGetClosestCenteredFiniteCylinderSurfaceOffset( |
| | 420 | 368 | | localPointInCylinder, |
| | 420 | 369 | | Vector3d.Zero, |
| | 420 | 370 | | Vector3d.Up, |
| | 420 | 371 | | cylinder.Height, |
| | 420 | 372 | | cylinder.ScaledRadius, |
| | 420 | 373 | | Vector3d.Right, |
| | 420 | 374 | | out Vector3d localCylinderPoint, |
| | 420 | 375 | | out _, |
| | 420 | 376 | | out Fixed64 signedDistance); |
| | | 377 | | |
| | 420 | 378 | | Fixed64 depth = -signedDistance; |
| | 420 | 379 | | AddContact( |
| | 420 | 380 | | pair, |
| | 420 | 381 | | new ContactAnchor(meshAnchor), |
| | 420 | 382 | | new ContactAnchor( |
| | 420 | 383 | | cylinder.Center, |
| | 420 | 384 | | cylinder.Rotation, |
| | 420 | 385 | | localCylinderPoint), |
| | 420 | 386 | | depth, |
| | 420 | 387 | | normal, |
| | 420 | 388 | | depthIsClamped: false); |
| | 420 | 389 | | return true; |
| | | 390 | | } |
| | | 391 | | |
| | | 392 | | private static bool TryAddCylinderCapTriangleContacts( |
| | | 393 | | CollisionWorkItem pair, |
| | | 394 | | LSMeshCollider mesh, |
| | | 395 | | FixedTriangle triangle, |
| | | 396 | | LSCylinderCollider cylinder, |
| | | 397 | | Vector3d normal) |
| | | 398 | | { |
| | 854 | 399 | | if (!CylinderContactGeometry.IsAxisAligned( |
| | 854 | 400 | | cylinder.Rotation, |
| | 854 | 401 | | Vector3d.Up, |
| | 854 | 402 | | normal)) |
| | 423 | 403 | | return false; |
| | | 404 | | |
| | 431 | 405 | | int initialCount = pair.Manifold.Count; |
| | 431 | 406 | | CylinderContactGeometry.GetCapBasis(cylinder, out Vector3d tangentA, out Vector3d tangentB); |
| | 431 | 407 | | TryAddCylinderCapTriangleContact(pair, mesh, triangle, cylinder, -normal + tangentA, normal); |
| | 431 | 408 | | TryAddCylinderCapTriangleContact(pair, mesh, triangle, cylinder, -normal - tangentA, normal); |
| | 431 | 409 | | TryAddCylinderCapTriangleContact(pair, mesh, triangle, cylinder, -normal + tangentB, normal); |
| | 431 | 410 | | TryAddCylinderCapTriangleContact(pair, mesh, triangle, cylinder, -normal - tangentB, normal); |
| | | 411 | | |
| | 431 | 412 | | if (pair.Manifold.Count > initialCount) |
| | 428 | 413 | | return true; |
| | | 414 | | |
| | 3 | 415 | | TryAddCylinderCapTriangleContact(pair, mesh, triangle, cylinder, -normal, normal); |
| | 3 | 416 | | return pair.Manifold.Count > initialCount; |
| | | 417 | | } |
| | | 418 | | |
| | | 419 | | private static void TryAddCylinderCapTriangleContact( |
| | | 420 | | CollisionWorkItem pair, |
| | | 421 | | LSMeshCollider mesh, |
| | | 422 | | FixedTriangle triangle, |
| | | 423 | | LSCylinderCollider cylinder, |
| | | 424 | | Vector3d supportDirection, |
| | | 425 | | Vector3d normal) |
| | | 426 | | { |
| | 1727 | 427 | | if (!triangle.TryGetCenteredFiniteCylinderSupportContact( |
| | 1727 | 428 | | mesh.Mesh.Origin, |
| | 1727 | 429 | | mesh.Mesh.Rotation, |
| | 1727 | 430 | | cylinder.Center, |
| | 1727 | 431 | | cylinder.Rotation, |
| | 1727 | 432 | | cylinder.Height, |
| | 1727 | 433 | | cylinder.ScaledRadius, |
| | 1727 | 434 | | supportDirection, |
| | 1727 | 435 | | normal, |
| | 1727 | 436 | | out FixedContactAnchors contact)) |
| | | 437 | | { |
| | 868 | 438 | | return; |
| | | 439 | | } |
| | | 440 | | |
| | 859 | 441 | | AddContact( |
| | 859 | 442 | | pair, |
| | 859 | 443 | | new ContactAnchor(contact.FirstAnchor), |
| | 859 | 444 | | new ContactAnchor(contact.SecondAnchor), |
| | 859 | 445 | | contact.Depth, |
| | 859 | 446 | | contact.Normal, |
| | 859 | 447 | | contact.DepthIsClamped); |
| | 859 | 448 | | } |
| | | 449 | | |
| | | 450 | | private static void GetTriangleBoundsInFrame( |
| | | 451 | | FixedBoundBox triangleBounds, |
| | | 452 | | Vector3d sourceOrigin, |
| | | 453 | | FixedQuaternion sourceRotation, |
| | | 454 | | Vector3d frameOrigin, |
| | | 455 | | FixedQuaternion frameRotation, |
| | | 456 | | out FixedBoundVolume bounds) |
| | | 457 | | { |
| | 7646 | 458 | | FixedBoundBox reframedBounds = |
| | 7646 | 459 | | FixedBoundBox.FromRelativeRotatedBoundsClippedToDomain( |
| | 7646 | 460 | | sourceOrigin, |
| | 7646 | 461 | | sourceRotation, |
| | 7646 | 462 | | triangleBounds.Min, |
| | 7646 | 463 | | triangleBounds.Max, |
| | 7646 | 464 | | frameOrigin, |
| | 7646 | 465 | | frameRotation); |
| | 7646 | 466 | | bounds = CreateBounds(reframedBounds.Min, reframedBounds.Max); |
| | 7646 | 467 | | } |
| | | 468 | | |
| | | 469 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 470 | | private static FixedBoundVolume CreateBounds(Vector3d min, Vector3d max) => |
| | 9425 | 471 | | new(min, max); |
| | | 472 | | |
| | | 473 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 474 | | private static Vector3d OrientNormal( |
| | | 475 | | in FixedPointAnchor source, |
| | | 476 | | in FixedPointAnchor target, |
| | | 477 | | Vector3d normal) |
| | | 478 | | { |
| | 865 | 479 | | Vector3d resolved = normal.Normalized; |
| | 865 | 480 | | return source.ProjectNonNegativeOffsetFrom(target, resolved) |
| | 865 | 481 | | > Fixed64.Zero |
| | 865 | 482 | | ? -resolved |
| | 865 | 483 | | : resolved; |
| | | 484 | | } |
| | | 485 | | |
| | | 486 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 487 | | private static void AddContact( |
| | | 488 | | CollisionWorkItem pair, |
| | | 489 | | ContactAnchor anchorOnFirst, |
| | | 490 | | ContactAnchor anchorOnSecond, |
| | | 491 | | Fixed64 depth, |
| | | 492 | | Vector3d normalFirstToSecond, |
| | | 493 | | bool depthIsClamped) |
| | | 494 | | { |
| | 23056 | 495 | | pair.Manifold.AddContact( |
| | 23056 | 496 | | anchorOnFirst, |
| | 23056 | 497 | | anchorOnSecond, |
| | 23056 | 498 | | depth, |
| | 23056 | 499 | | normalFirstToSecond, |
| | 23056 | 500 | | depthIsClamped); |
| | 23056 | 501 | | } |
| | | 502 | | |
| | | 503 | | } |