I'm writing a collision detection algorithm, and so far I've been using Joseph O'Rourke's book "Computational Geometry in C" as reference. It outlines an algorithm to determine whether a point is inside a polyhedron by counting the number of faces that a ray crosses which starts at the point.

I've been using this algorithm to check if any vertex of one polyhedron is inside another polyhedron, but this doesn't necessarily detect collisions between the two.

This post is two-fold:

- I'd like to extend this algorithm to account for a margin around the polyhedra. (i.e. determine whether a point is at least a margin length away from a polyhedron.)
- Is there a relatively fast algorithm used to determine the intersection of two polyhedra?

**Question 1 Attempts**

Given that a plane can be described by $A x + B y + C z = D$, could I just add the margin to D and continue with the algorithm in O'Rourke's book?

**Question 2 Attempts**

I think this could be done by computing the intersection of every segment of one polyhedron with the 3D triangle faces of the other, and then vice versa. This seems very computationally expensive though.

**Personal Motivation**

I'm trying to write a collision detection algorithm to help with a robot arm path planner.