Doc Classic

Class CGAlgorithms3D

  1 /*
  2  * Copyright (c) 2016 Martin Davis.
  3  *
  4  * All rights reserved. This program and the accompanying materials
  5  * are made available under the terms of the Eclipse Public License 2.0
  6  * and Eclipse Distribution License v. 1.0 which accompanies this distribution.
  7  * The Eclipse Public License is available at http://www.eclipse.org/legal/epl-v20.html
  8  * and the Eclipse Distribution License is available at
  9  *
 10  * http://www.eclipse.org/org/documents/edl-v10.php.
 11  */
 12  
 13 package org.locationtech.jts.algorithm;
 14  
 15 import org.locationtech.jts.geom.Coordinate;
 16 import org.locationtech.jts.math.Vector3D;
 17  
 18 /**
 19  * Basic computational geometry algorithms 
 20  * for geometry and coordinates defined in 3-dimensional Cartesian space.
 21  * 
 22  * @author mdavis
 23  *
 24  */
 25 public class CGAlgorithms3D 
 26 {
 27     private CGAlgorithms3D() {}
 28  
 29     public static double distance(Coordinate p0, Coordinate p1)
 30     {
 31         // default to 2D distance if either Z is not set
 32         if (Double.isNaN(p0.getZ()) || Double.isNaN(p1.getZ()))
 33             return p0.distance(p1);
 34         
 35         double dx = p0.x - p1.x;
 36         double dy = p0.y - p1.y;
 37         double dz = p0.getZ() - p1.getZ();
 38         return Math.sqrt(dx * dx + dy * dy + dz * dz);
 39     }
 40  
 41     public static double distancePointSegment(Coordinate p,
 42             Coordinate A, Coordinate B) {
 43         // if start = end, then just compute distance to one of the endpoints
 44         if (A.equals3D(B))
 45           return distance(p, A);
 46  
 47         // otherwise use comp.graphics.algorithms Frequently Asked Questions method
 48         /*
 49          * (1) r = AC dot AB 
 50          *         --------- 
 51          *         ||AB||^2 
 52          *         
 53          * r has the following meaning: 
 54          *   r=0 P = A 
 55          *   r=1 P = B 
 56          *   r<0 P is on the backward extension of AB 
 57          *   r>1 P is on the forward extension of AB 
 58          *   0<r<1 P is interior to AB
 59          */
 60  
 61         double len2 = (B.x - A.x) * (B.x - A.x) + (B.y - A.y) * (B.y - A.y) + (B.getZ() - A.getZ()) * (B.getZ() - A.getZ());
 62         if (Double.isNaN(len2))
 63             throw new IllegalArgumentException("Ordinates must not be NaN");
 64         double r = ((p.x - A.x) * (B.x - A.x) + (p.y - A.y) * (B.y - A.y) + (p.getZ() - A.getZ()) * (B.getZ() - A.getZ()))
 65             / len2;
 66  
 67         if (r <= 0.0)
 68           return distance(p, A);
 69         if (r >= 1.0)
 70           return distance(p, B);
 71  
 72         // compute closest point q on line segment
 73         double qx = A.x + r * (B.x - A.x);
 74         double qy = A.y + r * (B.y - A.y);
 75         double qz = A.getZ() + r * (B.getZ() - A.getZ());
 76         // result is distance from p to q
 77         double dx = p.x - qx;
 78         double dy = p.y - qy;
 79         double dz = p.getZ() - qz;
 80         return Math.sqrt(dx*dx + dy*dy + dz*dz);
 81     }
 82     
 83  
 84     /**
 85      * Computes the distance between two 3D segments.
 86      * 
 87      * @param A the start point of the first segment
 88      * @param B the end point of the first segment
 89      * @param C the start point of the second segment
 90      * @param D the end point of the second segment
 91      * @return the distance between the segments
 92      */
 93     public static double distanceSegmentSegment(
 94             Coordinate A, Coordinate B, Coordinate C, Coordinate D) 
 95     {
 96         /**
 97          * This calculation is susceptible to roundoff errors when 
 98          * passed large ordinate values.
 99          * It may be possible to improve this by using {@link DD} arithmetic.
100          */
101         if (A.equals3D(B))
102               return distancePointSegment(A, C, D);
103         if (C.equals3D(B))
104               return distancePointSegment(C, A, B);
105         
106         /**
107          * Algorithm derived from http://softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm
108          */
109         double a = Vector3D.dot(A, B, A, B);
110         double b = Vector3D.dot(A, B, C, D);
111         double c = Vector3D.dot(C, D, C, D);
112         double d = Vector3D.dot(A, B, C, A);
113         double e = Vector3D.dot(C, D, C, A);
114         
115         double denom = a*c - b*b;
116         if (Double.isNaN(denom))
117             throw new IllegalArgumentException("Ordinates must not be NaN");
118         
119         double s;
120         double t;
121         if (denom <= 0.0) {
122             /**
123              * The lines are parallel. 
124              * In this case solve for the parameters s and t by assuming s is 0.
125              */
126             s = 0;
127             // choose largest denominator for optimal numeric conditioning
128             if (b > c)
129                 t = d/b;
130             else 
131                 t = e/c;
132         }
133         else {
134             s = (b*e - c*d) / denom;
135             t = (a*e - b*d) / denom;
136         }
137         if (s < 0) 
138             return distancePointSegment(A, C, D);
139         else if (s > 1)
140             return distancePointSegment(B, C, D);
141         else if (t < 0)    
142             return distancePointSegment(C, A, B);
143         else if(t > 1) {
144             return distancePointSegment(D, A, B);
145         }
146         /**
147          * The closest points are in interiors of segments,
148          * so compute them directly
149          */
150         double x1 = A.x + s * (B.x - A.x);
151         double y1 = A.y + s * (B.y - A.y);
152         double z1 = A.getZ() + s * (B.getZ() - A.getZ());
153  
154         double x2 = C.x + t * (D.x - C.x);
155         double y2 = C.y + t * (D.y - C.y);
156         double z2 = C.getZ() + t * (D.getZ() - C.getZ());
157         
158         // length (p1-p2)
159         return distance(new Coordinate(x1, y1, z1), new Coordinate(x2, y2, z2));
160     }
161  
162     
163 }
164