Java Code Examples for com.vividsolutions.jts.geom.Coordinate#equals3D()

public static double distancePointSegment(Coordinate p,
		Coordinate A, Coordinate B) {
    // if start = end, then just compute distance to one of the endpoints
    if (A.equals3D(B))
      return distance(p, A);

    // otherwise use comp.graphics.algorithms Frequently Asked Questions method
    /*
     * (1) r = AC dot AB 
     *         --------- 
     *         ||AB||^2 
     *         
     * r has the following meaning: 
     *   r=0 P = A 
     *   r=1 P = B 
     *   r<0 P is on the backward extension of AB 
     *   r>1 P is on the forward extension of AB 
     *   0<r<1 P is interior to AB
     */

    double len2 = (B.x - A.x) * (B.x - A.x) + (B.y - A.y) * (B.y - A.y) + (B.z - A.z) * (B.z - A.z);
    if (Double.isNaN(len2))
    	throw new IllegalArgumentException("Ordinates must not be NaN");
    double r = ((p.x - A.x) * (B.x - A.x) + (p.y - A.y) * (B.y - A.y) + (p.z - A.z) * (B.z - A.z))
        / len2;

    if (r <= 0.0)
      return distance(p, A);
    if (r >= 1.0)
      return distance(p, B);

    // compute closest point q on line segment
    double qx = A.x + r * (B.x - A.x);
    double qy = A.y + r * (B.y - A.y);
    double qz = A.z + r * (B.z - A.z);
    // result is distance from p to q
    double dx = p.x - qx;
    double dy = p.y - qy;
    double dz = p.z - qz;
    return Math.sqrt(dx*dx + dy*dy + dz*dz);
}
 

/**
 * Computes the distance between two 3D segments.
 * 
 * @param A the start point of the first segment
 * @param B the end point of the first segment
 * @param C the start point of the second segment
 * @param D the end point of the second segment
 * @return the distance between the segments
 */
public static double distanceSegmentSegment(
		Coordinate A, Coordinate B, Coordinate C, Coordinate D) 
{
	/**
	 * This calculation is susceptible to roundoff errors when 
	 * passed large ordinate values.
	 * It may be possible to improve this by using {@link DD} arithmetic.
	 */
    if (A.equals3D(B))
	      return distancePointSegment(A, C, D);
    if (C.equals3D(B))
	      return distancePointSegment(C, A, B);
    
    /**
     * Algorithm derived from http://softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm
     */
	double a = Vector3D.dot(A, B, A, B);
	double b = Vector3D.dot(A, B, C, D);
	double c = Vector3D.dot(C, D, C, D);
	double d = Vector3D.dot(A, B, C, A);
	double e = Vector3D.dot(C, D, C, A);
	
	double denom = a*c - b*b;
    if (Double.isNaN(denom))
    	throw new IllegalArgumentException("Ordinates must not be NaN");
	
	double s;
	double t;
	if (denom <= 0.0) {
		/**
		 * The lines are parallel. 
		 * In this case solve for the parameters s and t by assuming s is 0.
		 */
		s = 0;
		// choose largest denominator for optimal numeric conditioning
		if (b > c)
			t = d/b;
		else 
			t = e/c;
	}
	else {
		s = (b*e - c*d) / denom;
		t = (a*e - b*d) / denom;
	}
	if (s < 0) 
		return distancePointSegment(A, C, D);
	else if (s > 1)
		return distancePointSegment(B, C, D);
	else if (t < 0)	
		return distancePointSegment(C, A, B);
	else if(t > 1) {
		return distancePointSegment(D, A, B);
	}
	/**
	 * The closest points are in interiors of segments,
	 * so compute them directly
	 */
	double x1 = A.x + s * (B.x - A.x);
	double y1 = A.y + s * (B.y - A.y);
	double z1 = A.z + s * (B.z - A.z);

	double x2 = C.x + t * (D.x - C.x);
	double y2 = C.y + t * (D.y - C.y);
	double z2 = C.z + t * (D.z - C.z);
	
	// length (p1-p2)
	return distance(new Coordinate(x1, y1, z1), new Coordinate(x2, y2, z2));
}