main/src/georegression/metric/ClosestPoint3D_F64.java
public static Point3D_F64 closestPoint( LineSegment3D_F64 l0 , LineSegment3D_F64 l1 , Point3D_F64 ret ) {
		if( ret == null ) {
			ret = new Point3D_F64();
		}

		ret.x = l0.a.x - l1.a.x;
		ret.y = l0.a.y - l1.a.y;
		ret.z = l0.a.z - l1.a.z;

		double slope0_x = l0.b.x - l0.a.x;
		double slope0_y = l0.b.y - l0.a.y;
		double slope0_z = l0.b.z - l0.a.z;

		double slope1_x = l1.b.x - l1.a.x;
		double slope1_y = l1.b.y - l1.a.y;
		double slope1_z = l1.b.z - l1.a.z;

		// normalize the slopes for easier math
		double n0 = (double) Math.sqrt(slope0_x*slope0_x + slope0_y*slope0_y + slope0_z*slope0_z);
		double n1 = (double) Math.sqrt(slope1_x*slope1_x + slope1_y*slope1_y + slope1_z*slope1_z);

		slope0_x /= n0; slope0_y /= n0; slope0_z /= n0;
		slope1_x /= n1; slope1_y /= n1; slope1_z /= n1;

		// this solution is from: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/
		double dv01v1 = ret.x*slope1_x +  ret.y*slope1_y +  ret.z*slope1_z;
		double dv01v0 = ret.x*slope0_x +  ret.y*slope0_y +  ret.z*slope0_z;
		double dv1v0 = slope1_x*slope0_x + slope1_y*slope0_y + slope1_z*slope0_z;

		double t0 = dv01v1 * dv1v0 - dv01v0;
		double bottom = 1 - dv1v0 * dv1v0;
		if( bottom == 0 )
			return null;

		t0 /= bottom;

		// restrict it to be on the line
		if( t0 < 0 )
			return closestPoint(l1,l0.a,ret);
		if( t0 > 1 )
			return closestPoint(l1,l0.b,ret);

		// ( d1343 + mua d4321 ) / d4343
		double t1 = ( dv01v1 + t0 * dv1v0 );

		if( t1 < 0 )
			return closestPoint(l0,l1.a,ret);
		if( t1 > 1 )
			return closestPoint(l0,l1.b,ret);

		ret.x = (double) 0.5 * ( ( l0.a.x + t0 * slope0_x ) + ( l1.a.x + t1 * slope1_x ) );
		ret.y = (double) 0.5 * ( ( l0.a.y + t0 * slope0_y ) + ( l1.a.y + t1 * slope1_y ) );
		ret.z = (double) 0.5 * ( ( l0.a.z + t0 * slope0_z ) + ( l1.a.z + t1 * slope1_z ) );

		return ret;
	}
public static Point3D_F64 closestPoint( LineSegment3D_F64 l0 , LineSegment3D_F64 l1 , Point3D_F64 ret ) {
		if( ret == null ) {
			ret = new Point3D_F64();
		}

		ret.x = l0.a.x - l1.a.x;
		ret.y = l0.a.y - l1.a.y;
		ret.z = l0.a.z - l1.a.z;

		double slope0_x = l0.b.x - l0.a.x;
		double slope0_y = l0.b.y - l0.a.y;
		double slope0_z = l0.b.z - l0.a.z;

		double slope1_x = l1.b.x - l1.a.x;
		double slope1_y = l1.b.y - l1.a.y;
		double slope1_z = l1.b.z - l1.a.z;

		// normalize the slopes for easier math
		double n0 = (double) Math.sqrt(slope0_x*slope0_x + slope0_y*slope0_y + slope0_z*slope0_z);
		double n1 = (double) Math.sqrt(slope1_x*slope1_x + slope1_y*slope1_y + slope1_z*slope1_z);

		slope0_x /= n0; slope0_y /= n0; slope0_z /= n0;
		slope1_x /= n1; slope1_y /= n1; slope1_z /= n1;

		// this solution is from: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/
		double dv01v1 = ret.x*slope1_x +  ret.y*slope1_y +  ret.z*slope1_z;
		double dv01v0 = ret.x*slope0_x +  ret.y*slope0_y +  ret.z*slope0_z;
		double dv1v0 = slope1_x*slope0_x + slope1_y*slope0_y + slope1_z*slope0_z;

		double t0 = dv01v1 * dv1v0 - dv01v0;
		double bottom = 1 - dv1v0 * dv1v0;
		if( bottom == 0 )
			return null;

		t0 /= bottom;

		// restrict it to be on the line
		if( t0 < 0 )
			return closestPoint(l1,l0.a,ret);
		if( t0 > 1 )
			return closestPoint(l1,l0.b,ret);

		// ( d1343 + mua d4321 ) / d4343
		double t1 = ( dv01v1 + t0 * dv1v0 );

		if( t1 < 0 )
			return closestPoint(l0,l1.a,ret);
		if( t1 > 1 )
			return closestPoint(l0,l1.b,ret);

		ret.x = (double) 0.5 * ( ( l0.a.x + t0 * slope0_x ) + ( l1.a.x + t1 * slope1_x ) );
		ret.y = (double) 0.5 * ( ( l0.a.y + t0 * slope0_y ) + ( l1.a.y + t1 * slope1_y ) );
		ret.z = (double) 0.5 * ( ( l0.a.z + t0 * slope0_z ) + ( l1.a.z + t1 * slope1_z ) );

		return ret;
	}
public static Point3D_F64 closestPoint ( LineSegment3D_F64 l0 , LineSegment3D_F64 l1 , Point3D_F64 ret ) { if ( ret == null ) { ret = new Point3D_F64 ( ) ; } ret . x = l0 . a . x - l1 . a . x ; ret . y = l0 . a . y - l1 . a . y ; ret . z = l0 . a . z - l1 . a . z ; double slope0_x = l0 . b . x - l0 . a . x ; double slope0_y = l0 . b . y - l0 . a . y ; double slope0_z = l0 . b . z - l0 . a . z ; double slope1_x = l1 . b . x - l1 . a . x ; double slope1_y = l1 . b . y - l1 . a . y ; double slope1_z = l1 . b . z - l1 . a . z ; // normalize the slopes for easier math double n0 = ( double ) Math . sqrt ( slope0_x * slope0_x + slope0_y * slope0_y + slope0_z * slope0_z ) ; double n1 = ( double ) Math . sqrt ( slope1_x * slope1_x + slope1_y * slope1_y + slope1_z * slope1_z ) ; slope0_x /= n0 ; slope0_y /= n0 ; slope0_z /= n0 ; slope1_x /= n1 ; slope1_y /= n1 ; slope1_z /= n1 ; // this solution is from: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/ double dv01v1 = ret . x * slope1_x + ret . y * slope1_y + ret . z * slope1_z ; double dv01v0 = ret . x * slope0_x + ret . y * slope0_y + ret . z * slope0_z ; double dv1v0 = slope1_x * slope0_x + slope1_y * slope0_y + slope1_z * slope0_z ; double t0 = dv01v1 * dv1v0 - dv01v0 ; double bottom = 1 - dv1v0 * dv1v0 ; if ( bottom == 0 ) return null ; t0 /= bottom ; // restrict it to be on the line if ( t0 < 0 ) return closestPoint ( l1 , l0 . a , ret ) ; if ( t0 > 1 ) return closestPoint ( l1 , l0 . b , ret ) ; // ( d1343 + mua d4321 ) / d4343 double t1 = ( dv01v1 + t0 * dv1v0 ) ; if ( t1 < 0 ) return closestPoint ( l0 , l1 . a , ret ) ; if ( t1 > 1 ) return closestPoint ( l0 , l1 . b , ret ) ; ret . x = ( double ) 0.5 * ( ( l0 . a . x + t0 * slope0_x ) + ( l1 . a . x + t1 * slope1_x ) ) ; ret . y = ( double ) 0.5 * ( ( l0 . a . y + t0 * slope0_y ) + ( l1 . a . y + t1 * slope1_y ) ) ; ret . z = ( double ) 0.5 * ( ( l0 . a . z + t0 * slope0_z ) + ( l1 . a . z + t1 * slope1_z ) ) ; return ret ; }
Find the point which minimizes its distance from the two line segments.

@param l0 First line.  Not modified.
@param l1 Second line.  Not modified.
@param ret (Optional) Storage for the solution.  Can be same as instance as 'pt'. If null is passed in a new point is created. Modified.
@return The closest point
Find the point which minimizes its distance from the two line segments .