1 package org.opentrafficsim.core.geometry;
2
3 import java.awt.geom.Point2D;
4 import java.io.Serializable;
5
6 import javax.media.j3d.BoundingSphere;
7 import javax.media.j3d.Bounds;
8 import javax.vecmath.Point3d;
9
10 import nl.tudelft.simulation.dsol.animation.LocatableInterface;
11 import nl.tudelft.simulation.language.d3.CartesianPoint;
12 import nl.tudelft.simulation.language.d3.DirectedPoint;
13
14 import org.djunits.unit.LengthUnit;
15 import org.djunits.value.vdouble.scalar.Length;
16
17 import com.vividsolutions.jts.geom.Coordinate;
18 import com.vividsolutions.jts.geom.Point;
19
20 /**
21 * An OTSPoint3D implements a 3D-coordinate for OTS. X, y and z are stored as doubles, but it is assumed that the scale is in SI
22 * units, i.e. in meters. A distance between two points is therefore also in meters.
23 * <p>
24 * Copyright (c) 2013-2015 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
25 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
26 * <p>
27 * $LastChangedDate: 2015-07-16 10:20:53 +0200 (Thu, 16 Jul 2015) $, @version $Revision: 1124 $, by $Author: pknoppers $,
28 * initial version Jul 22, 2015 <br>
29 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
30 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
31 * @author <a href="http://www.citg.tudelft.nl">Guus Tamminga</a>
32 */
33 public class OTSPoint3D implements LocatableInterface, Serializable
34 {
35 /** */
36 private static final long serialVersionUID = 20150722L;
37
38 /** The internal representation of the point; x-coordinate. */
39 @SuppressWarnings("checkstyle:visibilitymodifier")
40 public final double x;
41
42 /** The internal representation of the point; y-coordinate. */
43 @SuppressWarnings("checkstyle:visibilitymodifier")
44 public final double y;
45
46 /** The internal representation of the point; z-coordinate. */
47 @SuppressWarnings("checkstyle:visibilitymodifier")
48 public final double z;
49
50 /**
51 * The x, y and z in the point are assumed to be in meters relative to an origin.
52 * @param x x-coordinate
53 * @param y y-coordinate
54 * @param z z-coordinate
55 */
56 public OTSPoint3D(final double x, final double y, final double z)
57 {
58 this.x = x;
59 this.y = y;
60 this.z = z;
61 }
62
63 /**
64 * @param xyz array with three elements; x, y and z are assumed to be in meters relative to an origin.
65 */
66 public OTSPoint3D(final double[] xyz)
67 {
68 this(xyz[0], xyz[1], (xyz.length > 2) ? xyz[2] : 0.0);
69 }
70
71 /**
72 * @param point a point to "clone".
73 */
74 public OTSPoint3D(final OTSPoint3D point)
75 {
76 this(point.x, point.y, point.z);
77 }
78
79 /**
80 * @param point javax.vecmath 3D double point; the x, y and z in the point are assumed to be in meters relative to an
81 * origin.
82 */
83 public OTSPoint3D(final Point3d point)
84 {
85 this(point.x, point.y, point.z);
86 }
87
88 /**
89 * @param point javax.vecmath 3D double point; the x, y and z in the point are assumed to be in meters relative to an
90 * origin.
91 */
92 public OTSPoint3D(final CartesianPoint point)
93 {
94 this(point.x, point.y, point.z);
95 }
96
97 /**
98 * @param point javax.vecmath 3D double point; the x, y and z in the point are assumed to be in meters relative to an
99 * origin.
100 */
101 public OTSPoint3D(final DirectedPoint point)
102 {
103 this(point.x, point.y, point.z);
104 }
105
106 /**
107 * @param point2d java.awt 2D point, z-coordinate will be zero; the x and y in the point are assumed to be in meters
108 * relative to an origin.
109 */
110 public OTSPoint3D(final Point2D point2d)
111 {
112 this(point2d.getX(), point2d.getY(), 0.0);
113 }
114
115 /**
116 * @param coordinate geotools coordinate; the x, y and z in the coordinate are assumed to be in meters relative to an
117 * origin.
118 */
119 public OTSPoint3D(final Coordinate coordinate)
120 {
121 this(coordinate.x, coordinate.y, Double.isNaN(coordinate.z) ? 0.0 : coordinate.z);
122 }
123
124 /**
125 * @param point geotools point; z-coordinate will be zero; the x and y in the point are assumed to be in meters relative to
126 * an origin.
127 */
128 public OTSPoint3D(final Point point)
129 {
130 this(point.getX(), point.getY(), 0.0);
131 }
132
133 /**
134 * The x and y in the point are assumed to be in meters relative to an origin. z will be set to 0.
135 * @param x x-coordinate
136 * @param y y-coordinate
137 */
138 public OTSPoint3D(final double x, final double y)
139 {
140 this(x, y, 0.0);
141 }
142
143 /**
144 * Interpolate (or extrapolate) between (outside) two given points.
145 * @param ratio double; 0 selects the zeroValue point, 1 selects the oneValue point, 0.5 selects a point halfway, etc.
146 * @param zeroValue OTSPoint3D; the point that is returned when ratio equals 0
147 * @param oneValue OTSPoint3D; the point that is returned when ratio equals 1
148 * @return OTSPoint3D
149 */
150 public static OTSPoint3D interpolate(final double ratio, final OTSPoint3D zeroValue, final OTSPoint3D oneValue)
151 {
152 double complement = 1 - ratio;
153 return new OTSPoint3D(complement * zeroValue.x + ratio * oneValue.x, complement * zeroValue.y + ratio
154 * oneValue.y, complement * zeroValue.z + ratio * oneValue.z);
155 }
156
157 /**
158 * Compute the 2D intersection of two line segments. The Z-component of the lines is ignored. Both line segments are defined
159 * by two points (that should be distinct).
160 * @param line1P1 OTSPoint3D; first point of line segment 1
161 * @param line1P2 OTSPoint3D; second point of line segment 1
162 * @param line2P1 OTSPoint3D; first point of line segment 2
163 * @param line2P2 OTSPoint3D; second point of line segment 2
164 * @return OTSPoint3D; the intersection of the two lines, or null if the lines are (almost) parallel, or do not intersect
165 */
166 public static OTSPoint3D intersectionOfLineSegments(final OTSPoint3D line1P1, final OTSPoint3D line1P2,
167 final OTSPoint3D line2P1, final OTSPoint3D line2P2)
168 {
169 double denominator =
170 (line2P2.y - line2P1.y) * (line1P2.x - line1P1.x) - (line2P2.x - line2P1.x) * (line1P2.y - line1P1.y);
171 if (denominator == 0f)
172 {
173 return null; // lines are parallel (they might even be on top of each other, but we don't check that)
174 }
175 double uA =
176 ((line2P2.x - line2P1.x) * (line1P1.y - line2P1.y) - (line2P2.y - line2P1.y) * (line1P1.x - line2P1.x))
177 / denominator;
178 if ((uA < 0f) || (uA > 1f))
179 {
180 return null; // intersection outside line 1
181 }
182 double uB =
183 ((line1P2.x - line1P1.x) * (line1P1.y - line2P1.y) - (line1P2.y - line1P1.y) * (line1P1.x - line2P1.x))
184 / denominator;
185 if (uB < 0 || uB > 1)
186 {
187 return null; // intersection outside line 2
188 }
189 return new OTSPoint3D(line1P1.x + uA * (line1P2.x - line1P1.x), line1P1.y + uA * (line1P2.y - line1P1.y), 0);
190 }
191
192 /**
193 * Compute the 2D intersection of two infinite lines. The Z-component of the lines is ignored. Both lines are defined by two
194 * points (that should be distinct).
195 * @param line1P1 OTSPoint3D; first point of line 1
196 * @param line1P2 OTSPoint3D; second point of line 1
197 * @param line2P1 OTSPoint3D; first point of line 2
198 * @param line2P2 OTSPoint3D; second point of line 2
199 * @return OTSPoint3D; the intersection of the two lines, or null if the lines are (almost) parallel
200 */
201 public static OTSPoint3D intersectionOfLines(final OTSPoint3D line1P1, final OTSPoint3D line1P2,
202 final OTSPoint3D line2P1, final OTSPoint3D line2P2)
203 {
204 double determinant =
205 (line1P1.x - line1P2.x) * (line2P1.y - line2P2.y) - (line1P1.y - line1P2.y) * (line2P1.x - line2P2.x);
206 if (Math.abs(determinant) < 0.0000001)
207 {
208 return null;
209 }
210 return new OTSPoint3D(
211 ((line1P1.x * line1P2.y - line1P1.y * line1P2.x) * (line2P1.x - line2P2.x) - (line1P1.x - line1P2.x)
212 * (line2P1.x * line2P2.y - line2P1.y * line2P2.x))
213 / determinant,
214 ((line1P1.x * line1P2.y - line1P1.y * line1P2.x) * (line2P1.y - line2P2.y) - (line1P1.y - line1P2.y)
215 * (line2P1.x * line2P2.y - line2P1.y * line2P2.x))
216 / determinant);
217 }
218
219 /**
220 * Compute the distance to a line segment (2D; Z-component is ignored). If the projection of this point onto the line lies
221 * outside the line segment; the distance to nearest end point of the line segment is returned. Otherwise the distance to
222 * the line segment is returned. <br>
223 * Adapted from <a href="http://paulbourke.net/geometry/pointlineplane/DistancePoint.java"> example code provided by Paul
224 * Bourke</a>.
225 * @param segmentPoint1 OTSPoint3D; start of line segment
226 * @param segmentPoint2 OTSPoint3D; end of line segment
227 * @return double; the distance of this point to (one of the end points of the line segment)
228 */
229 public final double distanceToLineSegment(final OTSPoint3D segmentPoint1, final OTSPoint3D segmentPoint2)
230 {
231 return closestPointOnSegment(segmentPoint1, segmentPoint2).distanceSI(this);
232 }
233
234 /**
235 * Project a point on a line segment (2D - Z-component is ignored). If the the projected points lies outside the line
236 * segment, the nearest end point of the line segment is returned. Otherwise the returned point lies between the end points
237 * of the line segment. <br>
238 * Adapted from <a href="http://paulbourke.net/geometry/pointlineplane/DistancePoint.java"> example code provided by Paul
239 * Bourke</a>.
240 * @param segmentPoint1 OTSPoint3D; start of line segment
241 * @param segmentPoint2 OTSPoint3D; end of line segment
242 * @return Point2D.Double; either <cite>lineP1</cite>, or <cite>lineP2</cite> or a new OTSPoint3D that lies somewhere in
243 * between those two
244 */
245 public final OTSPoint3D closestPointOnSegment(final OTSPoint3D segmentPoint1, final OTSPoint3D segmentPoint2)
246 {
247 double dX = segmentPoint2.x - segmentPoint1.x;
248 double dY = segmentPoint2.y - segmentPoint1.y;
249 if ((0 == dX) && (0 == dY))
250 {
251 return segmentPoint1;
252 }
253 final double u = ((this.x - segmentPoint1.x) * dX + (this.y - segmentPoint1.y) * dY) / (dX * dX + dY * dY);
254 if (u < 0)
255 {
256 return segmentPoint1;
257 }
258 else if (u > 1)
259 {
260 return segmentPoint2;
261 }
262 else
263 {
264 return new OTSPoint3D(segmentPoint1.x + u * dX, segmentPoint1.y + u * dY); // could use interpolate in stead
265 }
266 }
267
268 /**
269 * @param point the point to which the distance has to be calculated.
270 * @return the distance in 3D according to Pythagoras, expressed in the SI unit for length (meter)
271 */
272 public final double distanceSI(final OTSPoint3D point)
273 {
274 double dx = point.x - this.x;
275 double dy = point.y - this.y;
276 double dz = point.z - this.z;
277
278 return Math.sqrt(dx * dx + dy * dy + dz * dz);
279 }
280
281 /**
282 * @param point the point to which the distance has to be calculated.
283 * @return the distance in 3D according to Pythagoras, expressed in the SI unit for length (meter)
284 */
285 public final double horizontalDistanceSI(final OTSPoint3D point)
286 {
287 double dx = point.x - this.x;
288 double dy = point.y - this.y;
289
290 return Math.sqrt(dx * dx + dy * dy);
291 }
292
293 /**
294 * @param point the point to which the distance has to be calculated.
295 * @return the distance in 3D according to Pythagoras
296 */
297 public final Length.Rel horizontalDistance(final OTSPoint3D point)
298 {
299 return new Length.Rel(horizontalDistanceSI(point), LengthUnit.SI);
300 }
301
302 /**
303 * @param point the point to which the distance has to be calculated.
304 * @return the distance in 3D according to Pythagoras
305 */
306 public final Length.Rel distance(final OTSPoint3D point)
307 {
308 return new Length.Rel(distanceSI(point), LengthUnit.SI);
309 }
310
311 /**
312 * @return the equivalent geotools Coordinate of this point.
313 */
314 public final Coordinate getCoordinate()
315 {
316 return new Coordinate(this.x, this.y, this.z);
317 }
318
319 /**
320 * @return the equivalent DSOL DirectedPoint of this point. Should the result be cached?
321 */
322 public final DirectedPoint getDirectedPoint()
323 {
324 return new DirectedPoint(this.x, this.y, this.z);
325 }
326
327 /**
328 * @return a Point2D with the x and y structure.
329 */
330 public final Point2D getPoint2D()
331 {
332 return new Point2D.Double(this.x, this.y);
333 }
334
335 /** {@inheritDoc} */
336 @Override
337 public final DirectedPoint getLocation()
338 {
339 return getDirectedPoint();
340 }
341
342 /**
343 * This method returns a sphere with a diameter of half a meter as the default bounds for a point. {@inheritDoc}
344 */
345 @Override
346 public final Bounds getBounds()
347 {
348 return new BoundingSphere(new Point3d(0.0, 0.0, 0.0), 0.5);
349 }
350
351 /** {@inheritDoc} */
352 @Override
353 @SuppressWarnings("checkstyle:designforextension")
354 public String toString()
355 {
356 return String.format("(%.3f,%.3f,%.3f)", this.x, this.y, this.z);
357 }
358
359 /** {@inheritDoc} */
360 @Override
361 @SuppressWarnings("checkstyle:designforextension")
362 public int hashCode()
363 {
364 final int prime = 31;
365 int result = 1;
366 long temp;
367 temp = Double.doubleToLongBits(this.x);
368 result = prime * result + (int) (temp ^ (temp >>> 32));
369 temp = Double.doubleToLongBits(this.y);
370 result = prime * result + (int) (temp ^ (temp >>> 32));
371 temp = Double.doubleToLongBits(this.z);
372 result = prime * result + (int) (temp ^ (temp >>> 32));
373 return result;
374 }
375
376 /** {@inheritDoc} */
377 @Override
378 @SuppressWarnings({"checkstyle:designforextension", "checkstyle:needbraces"})
379 public boolean equals(final Object obj)
380 {
381 if (this == obj)
382 return true;
383 if (obj == null)
384 return false;
385 if (getClass() != obj.getClass())
386 return false;
387 OTSPoint3D other = (OTSPoint3D) obj;
388 if (Double.doubleToLongBits(this.x) != Double.doubleToLongBits(other.x))
389 return false;
390 if (Double.doubleToLongBits(this.y) != Double.doubleToLongBits(other.y))
391 return false;
392 if (Double.doubleToLongBits(this.z) != Double.doubleToLongBits(other.z))
393 return false;
394 return true;
395 }
396
397 }