Distance from point to plane implementation

By , last updated December 7, 2019

Calculation of closest points between objects gives us the distance between these objects. These calculations are one of the most powerful in collision detection algorithms in a sense that they can minimize calculations a lot. If the combined maximum movement of the objects is less than the distance between them, a collision will not occur and we can save us a lot of time detecting the impact.

Read also: Simple AABB collision detection

Theory

pcloses point to plane

Given a plane, defined by a point P and a normal vector vec{n}. And an arbitrary point Q in space. Vi need to find the distance from the point to the plane.

The plane satisfies the equation:
Ax+By+Cz+d=0

All points X on the plane satisfy the equation:
vec{n}*(X - P)=0

It means that the vector from P to X is perpendicular to vector vec{n}.

First we need to find distance d, that is a perpendicular distance that the plane needs to be translated along its normal to have the plane pass through the origin. The d is easily found by using a test point already in the plane:

d = vec{n}*P

To find the distance to the closest point on the plane we need to find the dot product between the normal of the plane and point Q and minus the perpendicular distance to the plane:

For normalized plane equation:
distanse = vec{n}*P - d

Not normalized plane equation:
distanse = (vec{n}*P - d) / vec{n}*vec{n}

Implementation

Here is the basic properties of the Point and Plane:

struct Point
{
    Point() {}
    Point( double x, double y, double z )
        : x(x)
        , y(y)
        , z(z)
    {}
    double x = 0.0;
    double y = 0.0;
    double z = 0.0;
    double w = 0.0;
 
    const double operator[](const int idx) const
    {
        if (idx == 0) return x;
        if (idx == 1) return y;
        if (idx == 2) return z;
        if (idx == 3) return w;
 
        assert(0);
    }
};
 
struct Plane
{
    Plane() {}
 
    Plane(const Point & p, const Point & n)
        : p(p)
        , n(n)
    {}
 
    Point p;       
    Point n;
	float d;
};

Then we can make a test to find a signed distance from point Q to the plane P:

float distnacePointPlane(const Point &q, const Plane &p) 
{
	float dist = (dotProduct(p.n, q) - p.d) / dotProduct(p.n, p.n);

	return dist;
}

The closest point R on the plane to Q is the orthogonal projection of point Q onto the plane P. This point is obtained by moving perpendicularly along the normal of the plane toward the plane. In order to find the closest point on the plane we need to solve the following equation:

closestPoint = Q - d*vec{n}

The code for computing the closest point on the plane to a point for non-normalized vector n:

Point closestPointPlane(const Point &q, const Plane &p) 
{
	float dist = distnacePointPlane(q, p);

	return q - dist * p.n;
}

Senior Software Engineer developing all kinds of stuff.