Fastest way to compute point to triangle distance in 3D?_问答_开发者

One obvious method for computing the minimum distance from a point to a 3D triangle is to project the point onto the plane of the triangle, determine the barycentric coordinates of the resulting point, and use them to determine whether the projected point lies within the triangle. If not, clamp its the barycentric coord开发者_StackOverflow中文版inates to be in the range [0,1], and that gives you the closest point that lies inside the triangle.

Is there a way to speed this up or simplify it somehow?

There are different approaches to finding the distance from a point P0 to a triangle P1,P2,P3.

The 3D method. Project the point onto the plane of the triangle and use barycentric coordinates or some other means of finding the closest point in the triangle. The distance is found in the usual way.
The 2D method. Apply a translation/rotation to the points so that P1 is on the origin, P2 is on the z-axis, P3 in the yz plane. Projection is of the point P0 is trivial (neglect the x coordinate). This results in a 2D problem. Using the edge equation it's possible to determine the closest vertex or edge of the triangle. Calculating distance is then easy-peasy.

This paper compares the performance of both with the 2D method winning.

I'll lead with my test case results.

Fastest way to compute point to triangle distance in 3D?

The test case code and implementation is in C#

    public void ClosestPointToShouldWork()
    {
        var r = new Random(0);
        double next() => r.NextDouble() * 5 - 1;
        var t = new Triangle(new Vector3(0,0,0), new Vector3(3.5,2,0), new Vector3(3,0.0,0));

        DrawTriangle(t);

        var hash = new Vector3( 0, 0, 0 );
        for (int i = 0; i < 800; i++)
        {
            var pt = new Vector3( next(), next(), 0 );
            var pc = t.ClosestPointTo( pt );
            hash += pc;

            DrawLine(pc,pt);
        }

        // Test the hash
        // If it doesn't match then eyeball the visualization
        // and see what has gone wrong

        hash.ShouldBeApproximately( new Vector3(1496.28118561104,618.196568578824,0),1e-5  );

    }

The implementation code is fiddly as I have a number of framework classes. Hopefully you can treat this as pseudo code and pull out the algorithm. The raw vector types are from https://www.nuget.org/packages/System.DoubleNumerics/.

Note that some of the properties of Triangle could be cached to improve performance.

Note that to return the closest point does not require any square roots and does not require transforming the problem to 2D.

The algorithm first quickly tests if the test point is closest to an end point region. If that is inconclusive it then tests the edge external regions one by one. If those tests fail then the point is inside the triangle. Note that for randomly selected points far from the triangle it is most likely that the closest point will be a corner point of the triangle.

public class Triangle
{
    public Vector3 A => EdgeAb.A;
    public Vector3 B => EdgeBc.A;
    public Vector3 C => EdgeCa.A;

    public readonly Edge3 EdgeAb;
    public readonly Edge3 EdgeBc;
    public readonly Edge3 EdgeCa;

    public Triangle(Vector3 a, Vector3 b, Vector3 c)
    {
        EdgeAb = new Edge3( a, b );
        EdgeBc = new Edge3( b, c );
        EdgeCa = new Edge3( c, a );
        TriNorm = Vector3.Cross(a - b, a - c);
    }

    public Vector3[] Verticies => new[] {A, B, C};

    public readonly Vector3 TriNorm;

    private static readonly RangeDouble ZeroToOne = new RangeDouble(0,1);

    public Plane TriPlane => new Plane(A, TriNorm);

    // The below three could be pre-calculated to
    // trade off space vs time

    public Plane PlaneAb => new Plane(EdgeAb.A, Vector3.Cross(TriNorm, EdgeAb.Delta  ));
    public Plane PlaneBc => new Plane(EdgeBc.A, Vector3.Cross(TriNorm, EdgeBc.Delta  ));
    public Plane PlaneCa => new Plane(EdgeCa.A, Vector3.Cross(TriNorm, EdgeCa.Delta  ));

    public static readonly  RangeDouble Zero1 = new RangeDouble(0,1);

    public Vector3 ClosestPointTo(Vector3 p)
    {
        // Find the projection of the point onto the edge

        var uab = EdgeAb.Project( p );
        var uca = EdgeCa.Project( p );

        if (uca > 1 && uab < 0)
            return A;

        var ubc = EdgeBc.Project( p );

        if (uab > 1 && ubc < 0)
            return B;

        if (ubc > 1 && uca < 0)
            return C;

        if (ZeroToOne.Contains( uab ) && !PlaneAb.IsAbove( p ))
            return EdgeAb.PointAt( uab );

        if (ZeroToOne.Contains( ubc ) && !PlaneBc.IsAbove( p ))
            return EdgeBc.PointAt( ubc );

        if (ZeroToOne.Contains( uca ) && !PlaneCa.IsAbove( p ))
            return EdgeCa.PointAt( uca );

        // The closest point is in the triangle so 
        // project to the plane to find it
        return TriPlane.Project( p );

    }
}

And the edge structure

public struct Edge3
{

    public readonly Vector3 A;
    public readonly Vector3 B;
    public readonly Vector3 Delta;

    public Edge3(Vector3 a, Vector3 b)
    {
        A = a;
        B = b;
        Delta = b -a;
    }

    public Vector3 PointAt(double t) => A + t * Delta;
    public double LengthSquared => Delta.LengthSquared();

    public double Project(Vector3 p) => (p - A).Dot( Delta ) / LengthSquared;

}

And the plane structure

public struct Plane
{
    public Vector3 Point;
    public Vector3 Direction;

    public Plane(Vector3 point, Vector3 direction )
    {
            Point = point;
            Direction = direction;
    }

    public bool IsAbove(Vector3 q) => Direction.Dot(q - Point) > 0;

}

Assuming you're using one of the known fast algorithms, the only way to speed it up is when you are doing a lot of measurements on a lot of triangles. In that case, you can keep a lot of quantities precomputed in "edge" or "winding" structures. Instead of storing the 3 points, you store meshes comprised of edge structures. Projection then becomes very quick and barycentric tests can be coded so that they are branch-predictable.

The real key is to just keep everything in cache. Processors can do MUL and DIV in nearly 1 clock cycle so memory is usually the bottleneck.

Also, consider writing the algo in SSE3 or something similar (such as Mono's SIMD support). It's work, but you can usually do a couple triangles at a time if you think hard enough about it.

I'll try to find some papers on the topic, but you might want to Google for "Ray Mesh Intersection". That will bring up all the great work from the 80s and 90s when people worked hard on optimizing this stuff.

I don't think the barycentric coordinate method can be sped up much per se, but you can pre-compute some some things if you will be testing many points against the same triangle.

Here is some robust code that I wrote for computing this projection, based on this answer combined with this post (which is in turn based on the book Realtime Collision Detection).

Note that you can precompute anything that doesn't depend directly or indirectly upon p (which saves around half the computational work for each test, if you're reusing the same triangle many times).

The code is designed to return null if the orthogonal projection of the point p onto the plane of the triangle does not fall within the triangle. You could extend this to find the closest point on the edge of a triangle if the barycentric coordinates are out of range, by computing the projection of the projection point onto each of the triangle's edge vectors, then checking whether the projection is within any pair of the triangle's vertices. (If not, a corner vertex would be the closest point.) For my purposes though, I only wanted orthogonal projections, which is why this code returns null if the orthogonal projection does not fall within the triangle.

The code is in Java, using the JOML linear algebra library.

/**
 * Find the closest orthogonal projection of a point p onto a triangle given by three vertices
 * a, b and c. Returns either the projection point, or null if the projection is not within
 * the triangle.
 */
public static Vector3d closestPoint(Vector3d p, Vector3d a, Vector3d b, Vector3d c) {
    // Find the normal to the plane: n = (b - a) x (c - a)
    Vector3d n = b.sub(a, new Vector3d()).cross(c.sub(a, new Vector3d()));

    // Normalize normal vector
    double nLen = n.length();
    if (nLen < 1.0e-30) {
        return null;  // Triangle is degenerate
    } else {
        n.mul(1.0f / nLen);
    }

    //    Project point p onto the plane spanned by a->b and a->c.
    //
    //    Given a plane
    //
    //        a : point on plane
    //        n : *unit* normal to plane
    //
    //    Then the *signed* distance from point p to the plane
    //    (in the direction of the normal) is
    //
    //        dist = p . n - a . n
    //
    double dist = p.dot(n) - a.dot(n);

    // Project p onto the plane by stepping the distance from p to the plane
    // in the direction opposite the normal: proj = p - dist * n
    Vector3d proj = p.add(n.mul(-dist, new Vector3d()), new Vector3d());

    // Find out if the projected point falls within the triangle -- see:
    // http://blackpawn.com/texts/pointinpoly/default.html

    // Compute edge vectors        
    double v0x = c.x - a.x;
    double v0y = c.y - a.y;
    double v0z = c.z - a.z;
    double v1x = b.x - a.x;
    double v1y = b.y - a.y;
    double v1z = b.z - a.z;
    double v2x = proj.x - a.x;
    double v2y = proj.y - a.y;
    double v2z = proj.z - a.z;

    // Compute dot products
    double dot00 = v0x * v0x + v0y * v0y + v0z * v0z;
    double dot01 = v0x * v1x + v0y * v1y + v0z * v1z;
    double dot02 = v0x * v2x + v0y * v2y + v0z * v2z;
    double dot11 = v1x * v1x + v1y * v1y + v1z * v1z;
    double dot12 = v1x * v2x + v1y * v2y + v1z * v2z;

    // Compute barycentric coordinates (u, v) of projection point
    double denom = (dot00 * dot11 - dot01 * dot01);
    if (Math.abs(denom) < 1.0e-30) {
        return null; // Triangle is degenerate
    }
    double invDenom = 1.0 / denom;
    double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
    double v = (dot00 * dot12 - dot01 * dot02) * invDenom;

    // Check barycentric coordinates
    if ((u >= 0) && (v >= 0) && (u + v < 1)) {
        // Nearest orthogonal projection point is in triangle
        return proj;
    } else {
        // Nearest orthogonal projection point is outside triangle
        return null;
    }
}

To find the distance, first one has to compute the closest point in triangle. The following code for finding the closest point in 3D triangle ABC to point P is taken from highly optimized Embree library:

Vec3fa closestPointTriangle(Vec3fa const& p, Vec3fa const& a, Vec3fa const& b, Vec3fa const& c)
{
  const Vec3fa ab = b - a;
  const Vec3fa ac = c - a;
  const Vec3fa ap = p - a;

  const float d1 = dot(ab, ap);
  const float d2 = dot(ac, ap);
  if (d1 <= 0.f && d2 <= 0.f) return a; //#1

  const Vec3fa bp = p - b;
  const float d3 = dot(ab, bp);
  const float d4 = dot(ac, bp);
  if (d3 >= 0.f && d4 <= d3) return b; //#2

  const Vec3fa cp = p - c;
  const float d5 = dot(ab, cp);
  const float d6 = dot(ac, cp);
  if (d6 >= 0.f && d5 <= d6) return c; //#3

  const float vc = d1 * d4 - d3 * d2;
  if (vc <= 0.f && d1 >= 0.f && d3 <= 0.f)
  {
      const float v = d1 / (d1 - d3);
      return a + v * ab; //#4
  }
    
  const float vb = d5 * d2 - d1 * d6;
  if (vb <= 0.f && d2 >= 0.f && d6 <= 0.f)
  {
      const float v = d2 / (d2 - d6);
      return a + v * ac; //#5
  }
    
  const float va = d3 * d6 - d5 * d4;
  if (va <= 0.f && (d4 - d3) >= 0.f && (d5 - d6) >= 0.f)
  {
      const float v = (d4 - d3) / ((d4 - d3) + (d5 - d6));
      return b + v * (c - b); //#6
  }

  const float denom = 1.f / (va + vb + vc);
  const float v = vb * denom;
  const float w = vc * denom;
  return a + v * ab + w * ac; //#0
}

Here dot(p,q) = p.x*q.x + p.y*q.y + p.z*q.z is dot product of vectors p and q.

The algorithm is based on partitioning the plane with the triangle on 7 regions by drawing two rays from each vertex outside the triangle orthogonal the adjacent edges of the vertex:

Fastest way to compute point to triangle distance in 3D?