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