1 package org.opentrafficsim.core.geometry;
2
3 import java.awt.geom.Point2D;
4 import java.io.Serializable;
5 import java.util.ArrayList;
6 import java.util.List;
7
8 import javax.media.j3d.BoundingSphere;
9 import javax.media.j3d.Bounds;
10 import javax.vecmath.Point3d;
11
12 import org.djunits.unit.LengthUnit;
13 import org.djunits.value.vdouble.scalar.Length;
14
15 import com.vividsolutions.jts.geom.Coordinate;
16 import com.vividsolutions.jts.geom.Point;
17
18 import nl.tudelft.simulation.dsol.animation.Locatable;
19 import nl.tudelft.simulation.language.d3.CartesianPoint;
20 import nl.tudelft.simulation.language.d3.DirectedPoint;
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35 public class OTSPoint3D implements Locatable, Serializable
36 {
37
38 private static final long serialVersionUID = 20150722L;
39
40
41 @SuppressWarnings("checkstyle:visibilitymodifier")
42 public final double x;
43
44
45 @SuppressWarnings("checkstyle:visibilitymodifier")
46 public final double y;
47
48
49 @SuppressWarnings("checkstyle:visibilitymodifier")
50 public final double z;
51
52
53
54
55
56
57
58 public OTSPoint3D(final double x, final double y, final double z)
59 {
60 this.x = x;
61 this.y = y;
62 this.z = z;
63 }
64
65
66
67
68 public OTSPoint3D(final double[] xyz)
69 {
70 this(xyz[0], xyz[1], (xyz.length > 2) ? xyz[2] : 0.0);
71 }
72
73
74
75
76 public OTSPoint3D(final OTSPoint3D point)
77 {
78 this(point.x, point.y, point.z);
79 }
80
81
82
83
84
85 public OTSPoint3D(final Point3d point)
86 {
87 this(point.x, point.y, point.z);
88 }
89
90
91
92
93
94 public OTSPoint3D(final CartesianPoint point)
95 {
96 this(point.x, point.y, point.z);
97 }
98
99
100
101
102
103 public OTSPoint3D(final DirectedPoint point)
104 {
105 this(point.x, point.y, point.z);
106 }
107
108
109
110
111
112 public OTSPoint3D(final Point2D point2d)
113 {
114 this(point2d.getX(), point2d.getY(), 0.0);
115 }
116
117
118
119
120
121 public OTSPoint3D(final Coordinate coordinate)
122 {
123 this(coordinate.x, coordinate.y, Double.isNaN(coordinate.z) ? 0.0 : coordinate.z);
124 }
125
126
127
128
129
130 public OTSPoint3D(final Point point)
131 {
132 this(point.getX(), point.getY(), 0.0);
133 }
134
135
136
137
138
139
140 public OTSPoint3D(final double x, final double y)
141 {
142 this(x, y, 0.0);
143 }
144
145
146
147
148
149
150
151
152 public static OTSPoint3D interpolate(final double ratio, final OTSPoint3D zeroValue, final OTSPoint3D oneValue)
153 {
154 double complement = 1 - ratio;
155 return new OTSPoint3D(complement * zeroValue.x + ratio * oneValue.x, complement * zeroValue.y + ratio * oneValue.y,
156 complement * zeroValue.z + ratio * oneValue.z);
157 }
158
159
160
161
162
163
164
165
166
167
168 public static OTSPoint3D intersectionOfLineSegments(final OTSPoint3D line1P1, final OTSPoint3D line1P2,
169 final OTSPoint3D line2P1, final OTSPoint3D line2P2)
170 {
171 double denominator =
172 (line2P2.y - line2P1.y) * (line1P2.x - line1P1.x) - (line2P2.x - line2P1.x) * (line1P2.y - line1P1.y);
173 if (denominator == 0f)
174 {
175 return null;
176 }
177 double uA = ((line2P2.x - line2P1.x) * (line1P1.y - line2P1.y) - (line2P2.y - line2P1.y) * (line1P1.x - line2P1.x))
178 / denominator;
179 if ((uA < 0f) || (uA > 1f))
180 {
181 return null;
182 }
183 double uB = ((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;
188 }
189 return new OTSPoint3D(line1P1.x + uA * (line1P2.x - line1P1.x), line1P1.y + uA * (line1P2.y - line1P1.y), 0);
190 }
191
192
193
194
195
196
197
198
199
200
201 public static OTSPoint3D intersectionOfLines(final OTSPoint3D line1P1, final OTSPoint3D line1P2, final OTSPoint3D line2P1,
202 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)
212 - (line1P1.x - line1P2.x) * (line2P1.x * line2P2.y - line2P1.y * line2P2.x)) / determinant,
213 ((line1P1.x * line1P2.y - line1P1.y * line1P2.x) * (line2P1.y - line2P2.y)
214 - (line1P1.y - line1P2.y) * (line2P1.x * line2P2.y - line2P1.y * line2P2.x)) / determinant);
215 }
216
217
218
219
220
221
222
223
224
225
226
227
228 public final OTSPoint3D closestPointOnSegment(final OTSPoint3D segmentPoint1, final OTSPoint3D segmentPoint2)
229 {
230 double dX = segmentPoint2.x - segmentPoint1.x;
231 double dY = segmentPoint2.y - segmentPoint1.y;
232 if ((0 == dX) && (0 == dY))
233 {
234 return segmentPoint1;
235 }
236 final double u = ((this.x - segmentPoint1.x) * dX + (this.y - segmentPoint1.y) * dY) / (dX * dX + dY * dY);
237 if (u < 0)
238 {
239 return segmentPoint1;
240 }
241 else if (u > 1)
242 {
243 return segmentPoint2;
244 }
245 else
246 {
247 return interpolate(u, segmentPoint1, segmentPoint2);
248 }
249 }
250
251
252
253
254
255
256
257
258 private OTSPoint3D internalClosestPointOnLine(final OTSLine3D line, final boolean useHorizontalDistance)
259 {
260 OTSPoint3D prevPoint = null;
261 double distance = Double.MAX_VALUE;
262 OTSPoint3D result = null;
263 for (OTSPoint3D nextPoint : line.getPoints())
264 {
265 if (null != prevPoint)
266 {
267 OTSPoint3D closest = closestPointOnSegment(prevPoint, nextPoint);
268 double thisDistance = useHorizontalDistance ? horizontalDistanceSI(closest) : distanceSI(closest);
269 if (thisDistance < distance)
270 {
271 result = closest;
272 distance = thisDistance;
273 }
274 }
275 prevPoint = nextPoint;
276 }
277 return result;
278 }
279
280
281
282
283
284
285 public final OTSPoint3D closestPointOnLine(final OTSLine3D line)
286 {
287 return internalClosestPointOnLine(line, false);
288 }
289
290
291
292
293
294
295
296 public final OTSPoint3D closestPointOnLine2D(final OTSLine3D line)
297 {
298 return internalClosestPointOnLine(line, true);
299 }
300
301
302
303
304
305 public final OTSPoint3D normalize()
306 {
307 double length = Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
308 return this.translate(length);
309 }
310
311
312
313
314
315
316 public final OTSPoint3D translate(final double factor)
317 {
318 return new OTSPoint3D(this.x / factor, this.y / factor, this.z / factor);
319 }
320
321
322
323
324
325
326
327
328 public static final List<OTSPoint3D> circleCenter(final OTSPoint3D point1, final OTSPoint3D point2, final double radius)
329 {
330 List<OTSPoint3D> center = new ArrayList<>();
331 OTSPoint3D m = interpolate(0.5, point1, point2);
332 double h = point1.distanceSI(m);
333 if (radius < h)
334 {
335 return center;
336 }
337 if (radius == h)
338 {
339 center.add(m);
340 return center;
341 }
342 OTSPoint3D p = new OTSPoint3D(point2.y - point1.y, point1.x - point2.x).normalize();
343 double d = Math.sqrt(radius * radius - h * h);
344 center.add(new OTSPoint3D(m.x + d * p.x, m.y + d * p.y, m.z));
345 center.add(new OTSPoint3D(m.x - d * p.x, m.y - d * p.y, m.z));
346 return center;
347 }
348
349
350
351
352
353
354
355
356
357 public static final List<OTSPoint3D> circleIntersections(final OTSPoint3D center1, final double radius1,
358 final OTSPoint3D center2, final double radius2)
359 {
360 List<OTSPoint3D> center = new ArrayList<>();
361 OTSPoint3D m = interpolate(radius1 / (radius1 + radius2), center1, center2);
362 double h = center1.distanceSI(m);
363 if (radius1 < h)
364 {
365 return center;
366 }
367 if (radius1 == h)
368 {
369 center.add(m);
370 return center;
371 }
372 OTSPoint3D p = new OTSPoint3D(center2.y - center1.y, center1.x - center2.x).normalize();
373 double d = Math.sqrt(radius1 * radius1 - h * h);
374 center.add(new OTSPoint3D(m.x + d * p.x, m.y + d * p.y, m.z));
375 center.add(new OTSPoint3D(m.x - d * p.x, m.y - d * p.y, m.z));
376 return center;
377 }
378
379
380
381
382
383 public final double distanceSI(final OTSPoint3D point)
384 {
385 double dx = point.x - this.x;
386 double dy = point.y - this.y;
387 double dz = point.z - this.z;
388
389 return Math.sqrt(dx * dx + dy * dy + dz * dz);
390 }
391
392
393
394
395
396 public final double horizontalDistanceSI(final OTSPoint3D point)
397 {
398 double dx = point.x - this.x;
399 double dy = point.y - this.y;
400
401 return Math.sqrt(dx * dx + dy * dy);
402 }
403
404
405
406
407
408 public final Length horizontalDistance(final OTSPoint3D point)
409 {
410 return new Length(horizontalDistanceSI(point), LengthUnit.SI);
411 }
412
413
414
415
416
417 public final Length distance(final OTSPoint3D point)
418 {
419 return new Length(distanceSI(point), LengthUnit.SI);
420 }
421
422
423
424
425 public final Coordinate getCoordinate()
426 {
427 return new Coordinate(this.x, this.y, this.z);
428 }
429
430
431
432
433 public final DirectedPoint getDirectedPoint()
434 {
435 return new DirectedPoint(this.x, this.y, this.z);
436 }
437
438
439
440
441 public final Point2D getPoint2D()
442 {
443 return new Point2D.Double(this.x, this.y);
444 }
445
446
447 @Override
448 public final DirectedPoint getLocation()
449 {
450 return getDirectedPoint();
451 }
452
453
454
455
456 @Override
457 public final Bounds getBounds()
458 {
459 return new BoundingSphere(new Point3d(0.0, 0.0, 0.0), 0.5);
460 }
461
462
463 @Override
464 @SuppressWarnings("checkstyle:designforextension")
465 public String toString()
466 {
467 return String.format("(%.3f,%.3f,%.3f)", this.x, this.y, this.z);
468 }
469
470
471 @Override
472 @SuppressWarnings("checkstyle:designforextension")
473 public int hashCode()
474 {
475 final int prime = 31;
476 int result = 1;
477 long temp;
478 temp = Double.doubleToLongBits(this.x);
479 result = prime * result + (int) (temp ^ (temp >>> 32));
480 temp = Double.doubleToLongBits(this.y);
481 result = prime * result + (int) (temp ^ (temp >>> 32));
482 temp = Double.doubleToLongBits(this.z);
483 result = prime * result + (int) (temp ^ (temp >>> 32));
484 return result;
485 }
486
487
488 @Override
489 @SuppressWarnings({ "checkstyle:designforextension", "checkstyle:needbraces" })
490 public boolean equals(final Object obj)
491 {
492 if (this == obj)
493 return true;
494 if (obj == null)
495 return false;
496 if (getClass() != obj.getClass())
497 return false;
498 OTSPoint3D other = (OTSPoint3D) obj;
499 if (Double.doubleToLongBits(this.x) != Double.doubleToLongBits(other.x))
500 return false;
501 if (Double.doubleToLongBits(this.y) != Double.doubleToLongBits(other.y))
502 return false;
503 if (Double.doubleToLongBits(this.z) != Double.doubleToLongBits(other.z))
504 return false;
505 return true;
506 }
507
508 }