点、线、面之间最短距离求法

1、点到线最短距离

通过两个点P1(x1,y1)和P2(x2,y2)定义的直线方程为：P=P1 + u(P2-P1)。

 点P3（x3,y3）,离线段P最短距离，即（P3-P）dot（P2-P1）=0

代入直线方程：[P3-P1-u(P2-P1)] dot （P2-P1） = 0

得到u：

 则：最短距离相交的（x,y）:
                x = x1 + u(x2-x1)

                y = y1 + u(y2-y1)

 public static double distanceToSegment(Point2D p1, Point2D p2, Point2D p3) {

	final double xDelta = p2.getX() - p1.getX();
	final double yDelta = p2.getY() - p1.getY();

	if ((xDelta == 0) && (yDelta == 0)) {
	    throw new IllegalArgumentException("p1 和p2 不能同一个点");
	}

	final double u = ((p3.getX() - p1.getX()) * xDelta + (p3.getY() - p1.getY()) * yDelta) / (xDelta * xDelta + yDelta * yDelta);

	final Point2D closestPoint;
	if (u < 0) {
	    closestPoint = p1;
	} else if (u > 1) {
	    closestPoint = p2;
	} else {
	    closestPoint = new Point2D.Double(p1.getX() + u * xDelta, p1.getY() + u * yDelta);
	}

	return closestPoint.distance(p3);
    }

2、点到平面最小距离

设点Pa = （xa，ya，za）。Pa到平面法线的投影。即最短距离

3、二维两线段的交点

 两直线方程为  ：Pa = P1 + Ua(P2-P1), Pb = P3 + Ub(P4 - P3)

求解 Pa = Pb  得：

x1 + Ua (x2-x1) = x3 + Ub (x4-x3)  和   y1 + Ua(y2-y1) = y3 +Ub (y4-y3) 

Ua和Ub 分别为：

 

 则交点（x, y）为：x = x1 + Ua (x2-x1)，y = y1 + Ua(y2-y1)

 public static bool DoLinesIntersect(Line L1, Line L2, ref Point ptIntersection)
      {      
         double d =
            (L2.Y2 - L2.Y1) * (L1.X2 - L1.X1)
            -
            (L2.X2 - L2.X1) * (L1.Y2 - L1.Y1);

         //n_a and n_b are calculated as seperate values for readability
         double n_a =
            (L2.X2 - L2.X1) * (L1.Y1 - L2.Y1)
            -
            (L2.Y2 - L2.Y1) * (L1.X1 - L2.X1);

         double n_b =
            (L1.X2 - L1.X1) * (L1.Y1 - L2.Y1)
            -
            (L1.Y2 - L1.Y1) * (L1.X1 - L2.X1);

        
         if (d == 0)
            return false;

         double ua = n_a / d;
         double ub = n_b / d;
        
         if (ua >= 0d && ua <= 1d && ub >= 0d && ub <= 1d)
         {
            ptIntersection.X = L1.X1 + (ua * (L1.X2 - L1.X1));
            ptIntersection.Y = L1.Y1 + (ua * (L1.Y2 - L1.Y1));
            return true;
         }
         return false;
      }

4、 3D中两条直线最短距离

 P a = P 1 + mua （P2 - P1)，P b = P 3 + mub （P4 - P3)

则：(Pa - Pb) dot (P2 - P1) = 0，(Pa - Pb) dot (P4 - P3) = 0

即：( P1 - P3 + mua (P2 - P1) - mub (P4 - P3) ) dot (P2 - P1) = 0

( P1 - P3 + mua (P2 - P1) - mub (P4 - P3) ) dot (P4 - P3) = 0

public static bool CalculateLineLineIntersection(Vector3 line1Point1, Vector3 line1Point2, 
	Vector3 line2Point1, Vector3 line2Point2, out Vector3 resultSegmentPoint1, out Vector3 resultSegmentPoint2)
{

   resultSegmentPoint1 = Vector3.Empty;
   resultSegmentPoint2 = Vector3.Empty;
 
   Vector3 p1 = line1Point1;
   Vector3 p2 = line1Point2;
   Vector3 p3 = line2Point1;
   Vector3 p4 = line2Point2;
   Vector3 p13 = p1 - p3;
   Vector3 p43 = p4 - p3;
 
   if (p43.LengthSq() < Math.Epsilon) {
      return false;
   }
   Vector3 p21 = p2 - p1;
   if (p21.LengthSq() < Math.Epsilon) {
      return false;
   }
 
   double d1343 = p13.X * (double)p43.X + (double)p13.Y * p43.Y + (double)p13.Z * p43.Z;
   double d4321 = p43.X * (double)p21.X + (double)p43.Y * p21.Y + (double)p43.Z * p21.Z;
   double d1321 = p13.X * (double)p21.X + (double)p13.Y * p21.Y + (double)p13.Z * p21.Z;
   double d4343 = p43.X * (double)p43.X + (double)p43.Y * p43.Y + (double)p43.Z * p43.Z;
   double d2121 = p21.X * (double)p21.X + (double)p21.Y * p21.Y + (double)p21.Z * p21.Z;
 
   double denom = d2121 * d4343 - d4321 * d4321;
   if (Math.Abs(denom) < Math.Epsilon) {
      return false;
   }
   double numer = d1343 * d4321 - d1321 * d4343;
 
   double mua = numer / denom;
   double mub = (d1343 + d4321 * (mua)) / d4343;
 
   resultSegmentPoint1.X = (float)(p1.X + mua * p21.X);
   resultSegmentPoint1.Y = (float)(p1.Y + mua * p21.Y);
   resultSegmentPoint1.Z = (float)(p1.Z + mua * p21.Z);
   resultSegmentPoint2.X = (float)(p3.X + mub * p43.X);
   resultSegmentPoint2.Y = (float)(p3.Y + mub * p43.Y);
   resultSegmentPoint2.Z = (float)(p3.Z + mub * p43.Z);

   return true;
}