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.DirectionUnit;
13 import org.djunits.unit.LengthUnit;
14 import org.djunits.unit.Unit;
15 import org.djunits.value.storage.StorageType;
16 import org.djunits.value.vdouble.scalar.Direction;
17 import org.djunits.value.vdouble.scalar.Length;
18 import org.djunits.value.vdouble.scalar.base.DoubleScalarInterface;
19 import org.djunits.value.vdouble.vector.base.DoubleVector;
20 import org.djunits.value.vdouble.vector.base.DoubleVectorInterface;
21 import org.locationtech.jts.geom.Coordinate;
22 import org.locationtech.jts.geom.Point;
23
24 import nl.tudelft.simulation.dsol.animation.Locatable;
25 import nl.tudelft.simulation.language.d3.CartesianPoint;
26 import nl.tudelft.simulation.language.d3.DirectedPoint;
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41 public class OTSPoint3D implements Locatable, Serializable
42 {
43
44 private static final long serialVersionUID = 20150722L;
45
46
47 @SuppressWarnings("checkstyle:visibilitymodifier")
48 public final double x;
49
50
51 @SuppressWarnings("checkstyle:visibilitymodifier")
52 public final double y;
53
54
55 @SuppressWarnings("checkstyle:visibilitymodifier")
56 public final double z;
57
58
59
60
61
62
63
64 public OTSPoint3D(final double x, final double y, final double z)
65 {
66 this.x = x;
67 this.y = y;
68 this.z = z;
69 }
70
71
72
73
74 public OTSPoint3D(final double[] xyz)
75 {
76 this(xyz[0], xyz[1], (xyz.length > 2) ? xyz[2] : 0.0);
77 }
78
79
80
81
82 public OTSPoint3Deometry/OTSPoint3D.html#OTSPoint3D">OTSPoint3D(final OTSPoint3D point)
83 {
84 this(point.x, point.y, point.z);
85 }
86
87
88
89
90
91 public OTSPoint3D(final Point3d point)
92 {
93 this(point.x, point.y, point.z);
94 }
95
96
97
98
99
100 public OTSPoint3D(final CartesianPoint point)
101 {
102 this(point.x, point.y, point.z);
103 }
104
105
106
107
108
109 public OTSPoint3D(final DirectedPoint point)
110 {
111 this(point.x, point.y, point.z);
112 }
113
114
115
116
117
118 public OTSPoint3D(final Point2D point2d)
119 {
120 this(point2d.getX(), point2d.getY(), 0.0);
121 }
122
123
124
125
126
127 public OTSPoint3D(final Coordinate coordinate)
128 {
129 this(coordinate.x, coordinate.y, Double.isNaN(coordinate.getZ()) ? 0.0 : coordinate.getZ());
130 }
131
132
133
134
135
136 public OTSPoint3D(final Point point)
137 {
138 this(point.getX(), point.getY(), 0.0);
139 }
140
141
142
143
144
145
146 public OTSPoint3D(final double x, final double y)
147 {
148 this(x, y, 0.0);
149 }
150
151
152
153
154
155
156
157
158 public static OTSPoint3D interpolate(finalOTSPoint3DPoint3D.html#OTSPoint3D">OTSPoint3Ddouble ratio, final OTSPoint3DPoint3D.html#OTSPoint3D">OTSPoint3D zeroValue, final OTSPoint3D oneValue)
159 {
160 double complement = 1 - ratio;
161 return new OTSPoint3D(complement * zeroValue.x + ratio * oneValue.x, complement * zeroValue.y + ratio * oneValue.y,
162 complement * zeroValue.z + ratio * oneValue.z);
163 }
164
165
166
167
168
169
170
171
172
173
174
175 @Deprecated
176 public static OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3Dt3D">OTSPoint3D intersectionOfLineSegmentsDumb(final OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3D line1P1, final OTSPoint3D line1P2,
177 final OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3D line2P1, final OTSPoint3D line2P2)
178 {
179 double denominator =
180 (line2P2.y - line2P1.y) * (line1P2.x - line1P1.x) - (line2P2.x - line2P1.x) * (line1P2.y - line1P1.y);
181 if (denominator == 0f)
182 {
183 return null;
184 }
185 double uA = ((line2P2.x - line2P1.x) * (line1P1.y - line2P1.y) - (line2P2.y - line2P1.y) * (line1P1.x - line2P1.x))
186 / denominator;
187 if ((uA < 0f) || (uA > 1f))
188 {
189 return null;
190 }
191 double uB = ((line1P2.x - line1P1.x) * (line1P1.y - line2P1.y) - (line1P2.y - line1P1.y) * (line1P1.x - line2P1.x))
192 / denominator;
193 if (uB < 0 || uB > 1)
194 {
195 return null;
196 }
197 return new OTSPoint3D(line1P1.x + uA * (line1P2.x - line1P1.x), line1P1.y + uA * (line1P2.y - line1P1.y), 0);
198 }
199
200
201
202
203
204
205
206
207
208
209 public static OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3DPoint3D">OTSPoint3D intersectionOfLineSegments(final OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3D line1P1, final OTSPoint3D line1P2,
210 final OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3D line2P1, final OTSPoint3D line2P2)
211 {
212 double l1p1x = line1P1.x;
213 double l1p1y = line1P1.y;
214 double l1p2x = line1P2.x - l1p1x;
215 double l1p2y = line1P2.y - l1p1y;
216 double l2p1x = line2P1.x - l1p1x;
217 double l2p1y = line2P1.y - l1p1y;
218 double l2p2x = line2P2.x - l1p1x;
219 double l2p2y = line2P2.y - l1p1y;
220 double denominator = (l2p2y - l2p1y) * l1p2x - (l2p2x - l2p1x) * l1p2y;
221 if (denominator == 0f)
222 {
223 return null;
224 }
225 double uA = ((l2p2x - l2p1x) * (-l2p1y) - (l2p2y - l2p1y) * (-l2p1x)) / denominator;
226
227 if ((uA < 0f) || (uA > 1f))
228 {
229 return null;
230 }
231 double uB = (l1p2y * l2p1x - l1p2x * l2p1y) / denominator;
232
233 if (uB < 0 || uB > 1)
234 {
235 return null;
236 }
237 return new OTSPoint3D(line1P1.x + uA * l1p2x, line1P1.y + uA * l1p2y, 0);
238 }
239
240
241
242
243
244
245
246
247
248
249 @Deprecated
250 public static OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3DOTSPoint3D">OTSPoint3D intersectionOfLinesDumb(final OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3D line1P1, final OTSPoint3D line1P2,
251 final OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3D line2P1, final OTSPoint3D line2P2)
252 {
253 double determinant =
254 (line1P1.x - line1P2.x) * (line2P1.y - line2P2.y) - (line1P1.y - line1P2.y) * (line2P1.x - line2P2.x);
255 if (Math.abs(determinant) < 0.0000001)
256 {
257 return null;
258 }
259 return new OTSPoint3D(
260 ((line1P1.x * line1P2.y - line1P1.y * line1P2.x) * (line2P1.x - line2P2.x)
261 - (line1P1.x - line1P2.x) * (line2P1.x * line2P2.y - line2P1.y * line2P2.x)) / determinant,
262 ((line1P1.x * line1P2.y - line1P1.y * line1P2.x) * (line2P1.y - line2P2.y)
263 - (line1P1.y - line1P2.y) * (line2P1.x * line2P2.y - line2P1.y * line2P2.x)) / determinant);
264 }
265
266
267
268
269
270
271
272
273
274
275 public static OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3Dtml#OTSPoint3D">OTSPoint3D intersectionOfLines(final OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3D line1P1, final OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3D line1P2, final OTSPoint3D line2P1,
276 final OTSPoint3D line2P2)
277 {
278 double l1p1x = line1P1.x;
279 double l1p1y = line1P1.y;
280 double l1p2x = line1P2.x - l1p1x;
281 double l1p2y = line1P2.y - l1p1y;
282 double l2p1x = line2P1.x - l1p1x;
283 double l2p1y = line2P1.y - l1p1y;
284 double l2p2x = line2P2.x - l1p1x;
285 double l2p2y = line2P2.y - l1p1y;
286 double determinant = (0 - l1p2x) * (l2p1y - l2p2y) - (0 - l1p2y) * (l2p1x - l2p2x);
287 if (Math.abs(determinant) < 0.0000001)
288 {
289 return null;
290 }
291 return new OTSPoint3D(l1p1x + (l1p2x * (l2p1x * l2p2y - l2p1y * l2p2x)) / determinant,
292 l1p1y + (l1p2y * (l2p1x * l2p2y - l2p1y * l2p2x)) / determinant);
293 }
294
295
296
297
298
299
300
301
302
303
304
305
306 public final OTSPoint3Dt3D.html#OTSPoint3D">OTSPoint3Dl#OTSPoint3D">OTSPoint3D closestPointOnSegment(final OTSPoint3Dt3D.html#OTSPoint3D">OTSPoint3D segmentPoint1, final OTSPoint3D segmentPoint2)
307 {
308 double dX = segmentPoint2.x - segmentPoint1.x;
309 double dY = segmentPoint2.y - segmentPoint1.y;
310 if ((0 == dX) && (0 == dY))
311 {
312 return segmentPoint1;
313 }
314 final double u = ((this.x - segmentPoint1.x) * dX + (this.y - segmentPoint1.y) * dY) / (dX * dX + dY * dY);
315 if (u < 0)
316 {
317 return segmentPoint1;
318 }
319 else if (u > 1)
320 {
321 return segmentPoint2;
322 }
323 else
324 {
325 return interpolate(u, segmentPoint1, segmentPoint2);
326 }
327 }
328
329
330
331
332
333
334
335
336 private OTSPoint3D internalClosestPointOnLine(final OTSLine3D line, final boolean useHorizontalDistance)
337 {
338 OTSPoint3D prevPoint = null;
339 double distance = Double.MAX_VALUE;
340 OTSPoint3D result = null;
341 for (OTSPoint3D nextPoint : line.getPoints())
342 {
343 if (null != prevPoint)
344 {
345 OTSPoint3D closest = closestPointOnSegment(prevPoint, nextPoint);
346 double thisDistance = useHorizontalDistance ? horizontalDistanceSI(closest) : distanceSI(closest);
347 if (thisDistance < distance)
348 {
349 result = closest;
350 distance = thisDistance;
351 }
352 }
353 prevPoint = nextPoint;
354 }
355 return result;
356 }
357
358
359
360
361
362
363 public final OTSPoint3D closestPointOnLine(final OTSLine3D line)
364 {
365 return internalClosestPointOnLine(line, false);
366 }
367
368
369
370
371
372
373
374 public final OTSPoint3D closestPointOnLine2D(final OTSLine3D line)
375 {
376 return internalClosestPointOnLine(line, true);
377 }
378
379
380
381
382
383 public final OTSPoint3D normalize()
384 {
385 double length = Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
386 return this.translate(length);
387 }
388
389
390
391
392
393
394 public final OTSPoint3D translate(final double factor)
395 {
396 return new OTSPoint3D(this.x / factor, this.y / factor, this.z / factor);
397 }
398
399
400
401
402
403
404
405
406
407
408 public static final List<OTSPoint3D> circleCenter(final OTSPoint3DOTSPoint3D.html#OTSPoint3D">OTSPoint3D point1, final OTSPoint3D point2, final double radius)
409 {
410 List<OTSPoint3D> result = new ArrayList<>();
411 OTSPoint3D m = interpolate(0.5, point1, point2);
412 double h = point1.distanceSI(m);
413 if (radius < h)
414 {
415 return result;
416 }
417 if (radius == h)
418 {
419 result.add(m);
420 return result;
421 }
422 OTSPoint3Dmetry/OTSPoint3D.html#OTSPoint3D">OTSPoint3D p = new OTSPoint3D(point2.y - point1.y, point1.x - point2.x).normalize();
423 double d = Math.sqrt(radius * radius - h * h);
424 d = Math.sqrt(radius * radius - h * h);
425 result.add(new OTSPoint3D(m.x + d * p.x, m.y + d * p.y, m.z));
426 result.add(new OTSPoint3D(m.x - d * p.x, m.y - d * p.y, m.z));
427 return result;
428 }
429
430
431
432
433
434
435
436
437
438 public static final List<OTSPoint3D> circleIntersections(final OTSPoint3D center1, final double radius1,
439 final OTSPoint3D center2, final double radius2)
440 {
441 List<OTSPoint3D> center = new ArrayList<>();
442 OTSPoint3D m = interpolate(radius1 / (radius1 + radius2), center1, center2);
443 double h = center1.distanceSI(m);
444 if (radius1 < h)
445 {
446 return center;
447 }
448 if (radius1 == h)
449 {
450 center.add(m);
451 return center;
452 }
453 OTSPoint3Dmetry/OTSPoint3D.html#OTSPoint3D">OTSPoint3D p = new OTSPoint3D(center2.y - center1.y, center1.x - center2.x).normalize();
454 double d = Math.sqrt(radius1 * radius1 - h * h);
455 center.add(new OTSPoint3D(m.x + d * p.x, m.y + d * p.y, m.z));
456 center.add(new OTSPoint3D(m.x - d * p.x, m.y - d * p.y, m.z));
457 return center;
458 }
459
460
461
462
463
464 public final double distanceSI(final OTSPoint3D point)
465 {
466 double dx = point.x - this.x;
467 double dy = point.y - this.y;
468 double dz = point.z - this.z;
469
470 return Math.sqrt(dx * dx + dy * dy + dz * dz);
471 }
472
473
474
475
476
477 public final double horizontalDistanceSI(final OTSPoint3D point)
478 {
479 double dx = point.x - this.x;
480 double dy = point.y - this.y;
481
482 return Math.sqrt(dx * dx + dy * dy);
483 }
484
485
486
487
488
489 public final Length horizontalDistance(final OTSPoint3D point)
490 {
491 return new Length(horizontalDistanceSI(point), LengthUnit.SI);
492 }
493
494
495
496
497
498
499 public final Length distance(final OTSPoint3D point)
500 {
501 return new Length(distanceSI(point), LengthUnit.SI);
502 }
503
504
505
506
507
508
509 public final double horizontalDirectionSI(final OTSPoint3D point)
510 {
511 return Math.atan2(point.y - this.y, point.x - this.x);
512 }
513
514
515
516
517
518
519 public final Direction horizontalDirection(final OTSPoint3D point)
520 {
521 return Direction.instantiateSI(Math.atan2(point.y - this.y, point.x - this.x));
522 }
523
524
525
526
527 public final Coordinate getCoordinate()
528 {
529 return new Coordinate(this.x, this.y, this.z);
530 }
531
532
533
534
535 public final DirectedPoint getDirectedPoint()
536 {
537 return new DirectedPoint(this.x, this.y, this.z);
538 }
539
540
541
542
543 public final Point2D getPoint2D()
544 {
545 return new Point2D.Double(this.x, this.y);
546 }
547
548
549 @Override
550 public final DirectedPoint getLocation()
551 {
552 return getDirectedPoint();
553 }
554
555
556
557
558 @Override
559 public final Bounds getBounds()
560 {
561 return new BoundingSphere(new Point3d(0.0, 0.0, 0.0), 0.5);
562 }
563
564
565
566
567
568
569
570
571
572 public <U extends Unit<U>, S extends DoubleScalarInterface<U, S>,
573 V extends DoubleVectorInterface<U, S, V>> V doubleVector(final U unit)
574 {
575 return DoubleVector.instantiate(new double[] { this.x, this.y, this.z }, unit, StorageType.DENSE);
576 }
577
578
579
580
581
582
583
584
585
586 public static Direction direction(final DirectedPoint directedPoint, final DirectionUnit directionUnit)
587 {
588 return new Direction(directedPoint.getRotZ(), directionUnit);
589 }
590
591
592 @Override
593 @SuppressWarnings("checkstyle:designforextension")
594 public String toString()
595 {
596 return String.format("(%.3f,%.3f,%.3f)", this.x, this.y, this.z);
597 }
598
599
600 @Override
601 @SuppressWarnings("checkstyle:designforextension")
602 public int hashCode()
603 {
604 final int prime = 31;
605 int result = 1;
606 long temp;
607 temp = Double.doubleToLongBits(this.x);
608 result = prime * result + (int) (temp ^ (temp >>> 32));
609 temp = Double.doubleToLongBits(this.y);
610 result = prime * result + (int) (temp ^ (temp >>> 32));
611 temp = Double.doubleToLongBits(this.z);
612 result = prime * result + (int) (temp ^ (temp >>> 32));
613 return result;
614 }
615
616
617 @Override
618 @SuppressWarnings({ "checkstyle:designforextension", "checkstyle:needbraces" })
619 public boolean equals(final Object obj)
620 {
621 if (this == obj)
622 return true;
623 if (obj == null)
624 return false;
625 if (getClass() != obj.getClass())
626 return false;
627 OTSPoint3D../../../org/opentrafficsim/core/geometry/OTSPoint3D.html#OTSPoint3D">OTSPoint3D other = (OTSPoint3D) obj;
628 if (Double.doubleToLongBits(this.x) != Double.doubleToLongBits(other.x))
629 return false;
630 if (Double.doubleToLongBits(this.y) != Double.doubleToLongBits(other.y))
631 return false;
632 if (Double.doubleToLongBits(this.z) != Double.doubleToLongBits(other.z))
633 return false;
634 return true;
635 }
636
637 }