3 A collision detection method in iMSTK is defined through the `CollisionDetectionAlgorithm` base class. This class assumes two input geometries and produces a CollisionData output. `CollisionDetectionAlgorithm` accepts geometries in either order, the output will be flipped if inputs are flipped. For example:
6 SurfaceMeshToSphereCD myCollisionDetection;
7 myCollisionDetection.setInput(myGeomA, 0);
8 myCollisionDetection.setInput(myGeomB, 1);
9 myCollisionDetection.update();
11 std::shared_ptr<CollisionData> cdData = myCollisionDetection.getCollisionData();
12 cdData->elementsA; // Refers to input 0 geometry/myGeomA
13 cdData->elementsB; // Refers to input 1 geometry/myGeomB
16 ## Supported Collision Detection
21 - `-`: Not collidable, unimplemented.
22 - X: Collidable, implemented.
23 - N/A: Not collidable, not applicable.
27 | | Capsule | Cylinder | ImplicitGeometry | LineMesh | OrientedBox | Plane | PointSet | Sphere | SurfaceMesh |
28 | :--- | :----: | :----: | :----: | :----: | :----: | :----: | :----: | :----: | :----: |
29 | Capsule | X | | | | | | | | |
30 | Cylinder | `-` | `-` | | | | | | | |
31 | ImplicitGeometry | `-` | `-` | `-` | | | | | | |
32 | LineMesh | X | `-` | `-` | X | | | | | |
33 | OrientedBox | `-` | `-` | `-` | `-` | `-` | | | | |
34 | Plane | X | `-` | `-` | `-` | `-` | N/A | | | |
35 | PointSet | X | X | X | N/A | X | X | N/A | | |
36 | Sphere | X | X | `-` | X | `-` | X | X | X | |
37 | SurfaceMesh | X | `-` | `-` | X | `-` | X | X | X | X |
41 - Fluid collision is available but not through a generalized PointSetvsPointSetCD.
42 - SurfaceMesh & LineMesh extend PointSet. So PointSet to X may give sufficient behaviour.
43 - SurfaceMeshToPlaneCD covered by PointSetPlaneCD
45 There are two approaches to collision detection.
47 ## Static Collision Detection
49 Given a snapshot of two already overlapping geometries, find the best solution to separate them. This normally involves resolving the geometry along directions that produce minimal movement. Often objects in physics simulations move small amounts so one can reliably resolve along this direction to get believable behavior.
51 - Objects moving too fast may produce strange results. Such as objects tunneling through each other without ever observing a collision or geometry resolving to the wrong feature of another. This also effects the maximum allowed velocity or force of a simulation. Which can make a simulation harder to tune (smaller stable parameter space).
53 ## Dynamic Collision Detection
55 Given two overlapping geometries and their dynamics (or two cached snapshots of them over time). Find the intersections a-priori and deal with them. Almost 100% of the time dynamic CD methods are paired with static CD methods as a fallback. This allows solvers to not guarantee non-intersecting states at the end of a timestep which may be difficult (for example, in an over constrained system).
57 ## Continuous Collision Detection
59 Continuous collision detection is a form of dynamic CD. It uses the dynamics of the object to compute exact time of impact. The usual solution is to linear interpolate elements of a geometry over time and find the time at which the elements are coplanar. For example, a vertex and a triangle moving. When the vertex and triangle are coplanar they intersect so long as the vertex still lies in the triangle. CCD can often be hard to tune and deal with floating point error. This can be alleviated with static CD as a fallback.
61 ## Collision Manifolds
63 Collision manifolds define the areas/points/patches of contact should two bodies be separated and *just* touch. From contacts or whatever is needed for the solver to separate may be computed. These manifolds are made up of N or more types of contacts. With various types of contacts for shapes.
66 <img src="Collision_Detection/contactManifolds.png" alt="various contact manifolds"/>
70 * For triangle meshes, popularly just called vertex-triangle or VT/TV contact.
73 * Ignored, covered with Face-Vertex
76 * Ignored, covered with Face-Vertex.
79 * Required for line mesh vs line mesh (polylines).
82 * Ignored, covered with Face-Vertex.
83 * Required when using curved surface vs meshes.
86 * Ignored, covered with Face-Vertex.
87 * Required when using curved surface vs meshes.
89 How a collision manifold is specified varies a lot among collision systems. The solution can get ambigious when only looking at a snapshot of overlapping geometries. It may not be clear what the **best** manifold would be.
92 <img src="Collision_Detection/edgeContactOverlap.png" alt="manifold when overlapping" width="200"/>
96 <img src="Collision_Detection/edgeContact.png" alt="manifold when separated" width="200"/>
99 It becomes of a problem of specifying this to the solver. Some collision systems report intersecting elements (ie: This triangle touched this edge, or this edge touched this point). Others report per contact points where N point-based contacts need to be used to support a face.
102 <img src="Collision_Detection/edgeContactResolve.png" alt="Two point,direction,depth contacts required to support the box" width="300"/>
105 Instead of subbing points for faces though, a constraint can be formulated between two elements in contact. For example, Vertex-Triangle as mentioned earlier, shown below:
108 <img src="Collision_Detection/vertexTriangle.png" alt="vertex triangle manifold" width="300"/>
114 With this approach it is required to store contact pairs of elements. Vertex-triangle, edge-edge. Whereas in the previous approach the system can only store point-based contacts. iMSTK supports both methods providing the following element types.
116 * **PointDirectionElement**: Gives point, normal, and depth contact
117 * **CellIndexElement**: Gives the id of a cell or the id of a cells vertices. Check count to tell which.
119 * If idCount==1. The id refers to a cell given by type.
126 * If idCount > 1. The id refers to the vertex indices of the cell.
128 * ex: idCount==3, means 3 vertex ids of the triangle.
129 * The ability to give cells via vertex ids is useful to avoid assigning ids to cells of cells. ie: edges of triangles, triangle faces of a tetrahedron, edges of a tetrahedron.
131 * parentId is provided and optional. When possible it will refer to the highest dimensional cell containing the vertex. For a SurfaceMesh this would be a triangle id, for a TetrahedralMesh this would be a tetrahedron id. For a LineMesh this would be a line id.
133 * **CellVertexElement**: Same as a CellIndexElement but gives the vertices by value instead.
135 * Useful when the other geometry doesn't contain vertices with ids (implicit geometry).
137 * **TransientCellIndexElement**: Encapsulates one CellVertexElement and one CellIndexElement to store a transient (moving in time) cell required by CCD algorithms.
139 * The CellVertexElement stores the previous time state of the cell directly in terms of 3D points.
140 * The CellIndexElement stores the current time state of the cell using cell ids from the colliding geometries.
142 iMSTK collision methods prefer to produce contact element pairs over point-based contacts. This is because point-based contacts can be computed from element pairs when needed. But element pairs cannot so easily be computed from point-based contacts.
144 ## Collision Resolution
146 Resolving collision can be classified into two categories.
148 * Matrix-Free: Normally resolve collisions at the moment of finding or in later iterations over all contacts found during collision.
150 * Ex1: A point lies under a plane 50 units. At the moment of noticing, move it up 50 units.
151 * Ex2: A point lies under a plane 50 units. Add a contact that informs to move it up 50 units. Later resolve all contacts.
152 * If all contacts resolved, later another could be created. For example, stacked cubes A, B, & C. Resolving A-B might move B into C. This would normally require another collision detection pass (likely next step of the simulation). Ex1 may not require another CD iteration as it does CD while resolving. Given the correct order of CD testing, they would actually resolve.
154 * Matrix: These approaches assemble matrices to resolve them all in a semi-implicit or implicit manner.
156 * Non-penetration equations are solved in iterative manners
157 * Often "constraint" based
159 ## Collision Constraints
161 The matrix ones are often "constraint based". The constraints giving a single scalar and gradient for which to solve. Often represented as a single row in a system. For a non-penetration constraint the scalar should be 0 when separated. The gradient then gives the direction to change the inputs such that the scalar becomes 0 (the root). This gives a bit better generalization to apply to a lot of things, perhaps not even non-penetration constraints (springs, joints, etc).
163 * PBDCollisionConstraints: Given positions of geometry, computes a gradient and scalar to reach zero/non-penetration.
164 * RBDConstraint: Given position and orientation of body, computes a jacobian (linear and angular gradient) and scalar to reach zero/non-penetration.
166 A pbd constraint to keep a point to a plane:
168 * Scalar = distance between the plane and point
169 * Gradient = the plane normal (direction to resolve, direction to get to a scalar of 0)
171 .. image:: Collision_Detection/constraintEx1.png
173 :alt: Alternative text
176 A rbd constraint to keep a box above a plane by adding a constraint per vertex corner.
178 * Scalar = distance between plane and vertex/corner of box.
181 * Linear Gradient = plane normal (direction to resolve linearly)
182 * Angular Gradient = plane normal crossed with the distance between contact point and center of body mass (direction to resolve angularly)
184 A useful function of these constraints is reprojection. In the PbdConstraint example, as the vertex resolves closer to the triangle the distance to the triangle is recomputed. While this isn't as foolproof as recomputing collision, it does allow better solutions at a cheaper cost (especially on overconstrained systems).
186 Lastly the signs of the constraints matter. Unsigned constraints gradients flip.
188 ## Persistent Contacts
191 Persistent contacts are those that persist over time. Often these are brought up with resting contacts keeping track of the same contacts frame-to-frame. The schemes for persistent contacts vary.
193 * A common implementation is to build up contacts over time. Instead of computing a full new set of contacts every frame. Keep around contacts from previous frames according to a heurisitc. Some implementations afford only reporting few contacts in one frame while ending up with N contacts to support whatever minimum number of contacts is required.
194 * Another such implementation is in ImplicitGeoetryToPointSetCCD. This CD will remember that last point outside of the implicit geometry before entering and recycle it to update the existing contact should the corresponding vertex still be inside the implicit geometry.
196 ## Collision Detection Types
198 ### BidirectionalPlaneToSphereCD
201 * Static Collision Method
206 <img src="Collision_Detection/sphereToPlane.png" width="300"/>
209 * Projects the sphere center onto the plane to compute distance to the plane. If the distance exceeds radius then it is not touching the plane. The direction to resolve is then computed from the difference between the nearest point on the plane and the sphere center.
213 * If the sphere crosses the center of the plane it will resolve to the opposite side. Thus not working bidirectionally but suffers from easy tunneling depending on the sphere size and displacement.
214 * This method produces 1 PointDirectionElement for the sphere.
215 * This method produces 1 PointDirectionElement for the plane.
216 * Only on contact is required for sphere on a plane.
218 ### UnidirectionalPlaneToSphereCD
221 * Static Collision Method
226 <img src="Collision_Detection/sphereToPlane.png" width="300"/>
229 * Projects the sphere center onto the plane to compute distance to the plane. If the distance exceeds the radius then it is not touching the plane. The direction to resolve is always the normal of the plane.
233 * One side of the plane is considered "in" the other "out".
234 * Only requires one contact point.
236 ### ImplicitGeometryToPointSetCCD
239 * Dynamic Collision Method
244 <img src="Collision_Detection/pointCCD.png" width="300"/>
247 * This method traces the displacement of a point from previous to current position sampling the implicit geometry looking for the first sign change/root. This point is recorded as the contact point.
248 * It then computes the normalized gradient at the contact point from the implicit geometry. This is used as the contact normal.
249 * Lastly it projects the (current position - contact position).dot(contact normal) to produce the depth to resolve it along the contact normal for the point to arrive on the plane of the normal which should minimize.
253 * This method is very unique it that it saves the last contact point outside the shape. Should a point not exit/resolve within a frame it will retrace the displacement, find the root, contact point, contact normal, and reproject to produce an updated contact using the old one.
254 * This method is important as it avoids sampling the interior of the implicit geometry which is useful for levelsets and non-SDF implicit geometries.
256 ### ImplicitGeometryToPointSetCD
259 * Static Collision Method
264 <img src="Collision_Detection/pointToImplicit.png" width="300"/>
267 * This method samples the implicit geometry for distance and then computes the normalized gradient for the direction to resolve a point (see gradient computation in diagram via central finite difference).
271 * This is traditional point vs SDF collision resolution.
272 * This method produces N PointIndexDirectionElements for every point.
274 ### ClosedSurfaceMeshToMeshCD
277 * Static Collision Method
281 * This method computes collision for SurfaceMesh vs PointSet/LineMesh/SurfaceMesh. It works best for closed surfaces with an inside/outside. It also works for non-closed geometry but will still assume sides. For example, it would still work for a plane mesh.
282 * This method works with two brute force expensive passes:
285 <img src="Collision_Detection/pointInPolygon.png" width="300"/>
288 For a curved surface this point in polygon strategy works well.
290 * Vertex Pass: For every point compute the nearest point on the other mesh. That point may be on a vertex, edge, or triangle. Then the angled-weighted pseudonormal is computed on that element. This normal is used to compute the sign in order to tell if inside/outside the SurfaceMesh. Finally mark which vertices lie inside/outside during this pass for later.
293 <img src="Collision_Detection/psuedonormalProblem.png" width="300"/>
296 For meshes using the normal of the element will produce incorrect results (see above where point is inside according to one face, outside according to the other). This is where the angle-weighted pseudonormal comes in.
298 * Edge Pass: For every edge that still lies outside the SurfaceMesh (with previously computed inside/outside mask). Compute the closest point on that edge with every edge in the SurfaceMesh. Then compute if that closest point lies inside the SurfaceMesh via another point in polygon test.
301 <img src="Collision_Detection/edgeToEdge.png" width="300"/>
306 * This method may produce vertex-triangle, vertex-edge, or vertex-vertex collision data.
307 * It can resolve completely deep penetrations of meshes similar to SDF collision.
308 * It's tougher to spatially accelerate as it requires global queries rather than bounded ones. Specify a bound/maximum radius to search. This would establish a maximum penetration depth. This fact heavily influences what type of spatial acceleration possible. Kdtree's, for instance, may be a bad idea.
310 ### PointSetToCapsuleCD
313 * Static Collision Method
318 <img src="Collision_Detection/capsuleToPoint.png" width="300"/>
321 * Given the line segment that forms the center/medial of the capsule, compute the closest poitn on it via projection. Computing both orthogonal (a) and parallel distance (b). Should the distance to this closest point exceed radius then it is outside the capsule.
325 * This method may also be used to compute signed distances of a capsule.
326 * This method produces N PointIndexDirectionElements for each point.
327 * This method produces N PointDirectionElements for the capsule given each point.
328 * Only one contact point is required for a single point on a capsule.
330 ### PointSetToOrientedBoxCD
333 * Static Collision Method
338 <img src="Collision_Detection/obbToPoint.png" width="300"/>
341 * To compute the resolution vectors (shown in pink). First project the point along each axes of the oriented box. These axes can be acquired from the inverse (transpose) of the oriented box rotation matrix (each column).
342 * If within these bounds, finding if inside the box is trivial.
343 * To then find the direction and amount to minimally resolve, find the nearest point on the box.
345 * If the point is inside the box:
347 * Compute the distance to each face of the box by subtracting the distance along the orienetation axes from the extent (half length, width, height). If width is 5 and is 6 units along the axes. Then it is 1 unit in front of the face.
348 * Then compute point on that face.
350 * If the point outside the box (not required for contact put part of the standard computation to find closest point):
352 * Sum the vectors from each face to the point so long as they lie outside the plane of the face. The resulting vector will give distance to point on nearest face, edge, or vertex of the box.
356 * This method may also be used to compute signed distance of an oriented box.
357 * This method produces N PointIndexDirectionElements for each point.
358 * This method produces N PointDirectionElements for the box.
359 * Only one contact is required for a single point on a box.
361 ### PointSetToPlaneCD
364 * Static Collision Method
369 <img src="Collision_Detection/pointPlaneProj.png" width="300"/>
372 * Projects the point (in green) to the plane given the plane normal and origin (red). Resolution vector shown in pink.
376 * This method produces N PointIndexDirectionElements for each point.
377 * This method produces N PointDirectionElements for the plane given each point.
378 * This method may also be used to compute signed distances to plane.
379 * Only one contact is required for a single point on plane.
381 ### PointSetToSphereCD
384 * Static Collision Method
389 <img src="Collision_Detection/pointToSphere.png" width="300"/>
392 * Compute the squared distance to the center of the sphere from the point. Check it against radius.
393 * Penetration vector direction given by the normalized difference between the sphere center and point.
394 * Penetration vector magnitude given by the difference between the radius and distance (distance required to separate along that direction).
398 * This method may also be used to compute signed distances to sphere.
399 * Only one contact required for a single point on a sphere.
401 ### SphereToCylinderCD
404 * Static Collision Method
408 * There are 3 cases. Proceed in a point vs cylinder like fashion.
409 * Case 1: Nearest point on the wall of the cylinder.
412 <img src="Collision_Detection/cylinderToPoint.png" width="300"/>
415 * Project the sphere center along the cylinder axes to get distance along the axes. If the center lies outside of the half heights of the cylinder, proceed to the other 2 cases. If it lies within the range of the cylinder. The shortest way out is the wall of the cylinder.
417 * For the other two cases, look at the projected distance along the orthogonal axes. If the orthogonal distance is larger than the radius of the cylinder then the nearest point must be on the edge/rim of the cylinder. If smaller then the nearest point must be on the cap.
419 * Case 2: Nearest point on the face of the cap.
420 * The normal of the cap is used to resolve.
421 * Case 3: Nearest point on the edge/rim of the cap.
422 * The difference between the nearest point and sphere center is used as the normal.
426 * One of the more expensive primitives to use.
427 * This primitive has sharp edges & curved surfaces. However only one contact point is needed for a sphere vs convex shape.
432 * Static Collision Method
437 <img src="Collision_Detection/sphereToSphere.png" width="300"/>
440 * To compute intersections between spheres, compute the distance between them and then check if it's less than the sum of the radii. The direction to resolve is given by the normalized difference between the centers.
444 * Only one contact is needed.
446 ### CapsuleToCapsuleCD
449 * Static Collision Method
453 * To compute intersection between two capsules, find the nearest point on the edges/centerlines of the capsules. Then perform sphere to sphere intersection between two spheres of capsules radius at these two points.
455 ### BidirectionalSurfaceMeshToSphereCD
458 * Static Collision Method
463 <img src="Collision_Detection/sphereToTriangle.png" width="300"/>
466 * For every triangle compute the closest point on the triangle to the sphere center. There are then 3 cases:
468 * Case 1: Closest point on triangle lies on an edge. Just an edge is touching the sphere.
469 * Case 2: Closest point on triangle lies on a vertex. Just a vertex is touching the sphere.
470 * Case 3: Closest point on triangle lies no its face. The face is touching the sphere
474 * A unidirectionl one for closed surfaces could be done via pseudonormal calculation on the sphere center.
475 * Only one contact required per triangle touching.
477 ### TetraToLineMeshCD
480 * Static Collision Method
484 * For every line segment, check intersection with every triangle face of every tetrahedron.
488 * This method misses the case of the line segment being entirely inside of the tetrahedron. It avoids doing a line vs tet SAT like solution.
490 ### TetraToPointSetCD
493 * Static Collision Method
497 * For every point compute the barycentric coordinates (u,v,w,y) of it to test if inside or out of the tetrahedron.
501 * This CD, at the moment, uses a built in spatial hashing for intersection tests.
503 ### SurfaceMeshToCapsuleCD
506 * Static Collision Method
510 * Works just like SurfaceMeshToSphereCD but computes the nearest point on the axes of the capsule to the triangle and creates a "virtual sphere" to then do CD with.
513 <img src="Collision_Detection/capsuleToTriangle.png" width="300"/>
516 * It does this by first computing the nearest points on the triangle to the two vertices of the segment (purple dotted lines). Then computing the nearest point on the segment to the nearest point on the triangle (orange dotted line). Choosing the closest gives us the closest point on edge to triangle. If this point is within the bounds of the capsule height, treat the CD as sphere-triangle at that point with the capsule's radius.
520 * Completely embedded triangles aren't handle well.
521 * Only one PointDirection contact is currently generated at the center of the triangle edge, when that edge is parallel with the capsule.
523 ### LineMeshToLineMeshCCD
526 * Continuous collision detection for LineMesh vs LineMesh including self-collision. Self collision is indicated by `input0 == input1` to the algorithm.
528 * The implementation uses Algorithm 1 described by Qi et al [[lfs]](#lfs) in Section 4.
530 * This has a thickness parameter found in [imstkEdgeEdgeCCDState.h](https://gitlab.kitware.com/iMSTK/iMSTK/-/blob/master/Source/CollisionDetection/CollisionDetection/imstkEdgeEdgeCCDState.h#L128)
532 # References & Resources
534 Much of the math for geometric intersections can be derived from SAT and GJK.
536 * [SAT](https://en.wikipedia.org/wiki/Hyperplane_separation_theorem) (Seperating Axis Theorem): Projects geometry along numerous axes in an aim to find a axes fo separation between the two. It also works for certain curved surfaces such as spheres.
538 * May also be used to find the axes of minimal separation. Useful for contact generation.
540 * [GJK](https://en.wikipedia.org/wiki/Gilbert%E2%80%93Johnson%E2%80%93Keerthi_distance_algorithm): Aims to find the closest points and distance between two convex geometries by minkowski summing one geometry with the other and checking if the origin lies in the summed geometry.
542 * Works with any convex shape so long `minkowski sum` and `point-in-convex-polygon check` are implemented.
544 * Often combined with EPA (expanding polytope algorithm) to find minimal separation.
546 <a id="gpp">[gpp]</a>
547 den, B. G. van. (2010). Game physics pearls. A.K. Peters.
549 <a id="rcd">[rcd]</a>
550 Ericson, C. (n.d.). Real-time collision detection. Elsevier.
552 <a id="lfs">[lfs]</a>
553 Qi, Di, Karthikeyan Panneerselvam, Woojin Ahn, Venkata Arikatla, Andinet Enquobahrie, and Suvranu De. "Virtual interactive suturing for the Fundamentals of Laparoscopic Surgery (FLS)." Journal of biomedical informatics 75 (2017): 48-62.
