1 package org.opentrafficsim.core.geometry;
2
3 import static org.junit.Assert.assertEquals;
4 import static org.junit.Assert.assertFalse;
5 import static org.junit.Assert.assertNotNull;
6 import static org.junit.Assert.assertNull;
7 import static org.junit.Assert.assertTrue;
8
9 import java.awt.geom.Point2D;
10 import java.util.List;
11 import java.util.Random;
12
13 import org.djunits.value.vdouble.scalar.Length;
14 import org.djutils.draw.point.Point3d;
15 import org.junit.Test;
16 import org.locationtech.jts.geom.Coordinate;
17 import org.locationtech.jts.geom.GeometryFactory;
18
19 import nl.tudelft.simulation.language.d3.CartesianPoint;
20
21
22
23
24
25
26
27
28
29
30 public class OTSPoint3DTest
31 {
32
33
34
35 @Test
36 public final void constructorsTest()
37 {
38 OTSPoint3D previousPoint = null;
39 int previousHashCode = 0;
40 double[] values = {Double.NEGATIVE_INFINITY, -99999999, -Math.PI, -1, -0.0000001, 0, 0.0000001, 1, Math.PI, 99999999,
41 Double.MAX_VALUE, Double.POSITIVE_INFINITY};
42 for (double x : values)
43 {
44 for (double y : values)
45 {
46 for (double z : values)
47 {
48 OTSPoint3D p = new OTSPoint3D(x, y, z);
49 checkXYZ(p, x, y, z);
50 checkXYZ(new OTSPoint3D(new double[] {x, y, z}), x, y, z);
51 checkXYZ(new OTSPoint3D(p), x, y, z);
52 checkXYZ(new OTSPoint3D(new double[] {x, y}), x, y, 0d);
53 checkXYZ(new OTSPoint3D(new Point3d(x, y, z)), x, y, z);
54 checkXYZ(new OTSPoint3D(new CartesianPoint(x, y, z)), x, y, z);
55 checkXYZ(new OTSPoint3D(new DirectedPoint(x, y, z)), x, y, z);
56 checkXYZ(new OTSPoint3D(new Point2D.Double(x, y)), x, y, 0d);
57 checkXYZ(new OTSPoint3D(new Coordinate(x, y)), x, y, 0d);
58 checkXYZ(new OTSPoint3D(new Coordinate(x, y, Double.NaN)), x, y, 0d);
59 checkXYZ(new OTSPoint3D(new Coordinate(x, y, z)), x, y, Double.isNaN(z) ? 0d : z);
60 GeometryFactory gm = new GeometryFactory();
61 checkXYZ(new OTSPoint3D(gm.createPoint(new Coordinate(x, y, z))), x, y, 0d);
62 checkXYZ(new OTSPoint3D(x, y), x, y, 0d);
63
64 Coordinate c = p.getCoordinate();
65 assertEquals("x value", x, c.x, Math.ulp(x));
66 assertEquals("y value", y, c.y, Math.ulp(y));
67 assertEquals("z value", z, c.getZ(), Math.ulp(z));
68 DirectedPoint dp = p.getDirectedPoint();
69 assertEquals("x value", x, dp.x, Math.ulp(x));
70 assertEquals("y value", y, dp.y, Math.ulp(y));
71 assertEquals("z value", z, dp.z, Math.ulp(z));
72 double qX = 100;
73 double qY = 200;
74 double qZ = 300;
75 OTSPoint3D q = new OTSPoint3D(qX, qY, qZ);
76 double expectedDistance = Math.sqrt(Math.pow(x - qX, 2) + Math.pow(y - qY, 2) + Math.pow(z - qZ, 2));
77 assertEquals("Distance to q should be " + expectedDistance, expectedDistance, p.distance(q).si,
78 expectedDistance / 99999);
79 Bounds bounds = p.getBounds();
80 DirectedPoint directedPoint = p.getLocation();
81 assertEquals("Location returns a DirectedPoint at the location of p", x, directedPoint.x, Math.ulp(x));
82 assertEquals("Location returns a DirectedPoint at the location of p", y, directedPoint.y, Math.ulp(y));
83 assertEquals("Location returns a DirectedPoint at the location of p", z, directedPoint.z, Math.ulp(z));
84 String s = p.toString();
85 assertNotNull("toString returns something", s);
86 assertTrue("toString returns string of reasonable length", s.length() > 10);
87 int hashCode = p.hashCode();
88
89 assertFalse("Hash code should be different", previousHashCode == hashCode);
90 previousHashCode = hashCode;
91
92 assertFalse("Successively generated points are all different", p.equals(previousPoint));
93 assertTrue("Point is equal to itself", p.equals(p));
94 assertTrue("Point is equals to duplicate of itself", p.equals(new OTSPoint3D(p)));
95 assertFalse("Point is not equal to some other object", p.equals(s));
96 previousPoint = p;
97 }
98 }
99 }
100 }
101
102
103
104
105
106
107
108
109 private void checkXYZ(final OTSPoint3D otsPoint3D, final double expectedX, final double expectedY, final double expectedZ)
110 {
111 assertEquals("x value", expectedX, otsPoint3D.x, Math.ulp(expectedX));
112 assertEquals("y value", expectedY, otsPoint3D.y, Math.ulp(expectedY));
113 assertEquals("z value", expectedZ, otsPoint3D.z, Math.ulp(expectedZ));
114 Point2D.Double p = (Point2D.Double) otsPoint3D.getPoint2D();
115 assertEquals("x value", expectedX, p.x, Math.ulp(expectedX));
116 assertEquals("y value", expectedY, p.y, Math.ulp(expectedY));
117 }
118
119
120
121
122 @Test
123 public final void interpolateTest()
124 {
125 OTSPoint3D p0 = new OTSPoint3D(123, 234, 345);
126 OTSPoint3D p1 = new OTSPoint3D(567, 678, 789);
127 for (double ratio : new double[] {0, 1, 0.5, 0.1, -10, 10})
128 {
129 OTSPoint3D pi = OTSPoint3D.interpolate(ratio, p0, p1);
130 assertTrue("result of interpolate is not null", null != pi);
131 assertEquals("x of interpolate", (1 - ratio) * p0.x + ratio * p1.x, pi.x, 0.00001);
132 assertEquals("y of interpolate", (1 - ratio) * p0.y + ratio * p1.y, pi.y, 0.00001);
133 assertEquals("z of interpolate", (1 - ratio) * p0.z + ratio * p1.z, pi.z, 0.00001);
134 }
135 }
136
137
138
139
140
141 @Test
142 public final void closestPointTest() throws OTSGeometryException
143 {
144
145 final int numPoints = 100;
146 final double growthPerRevolution = 5;
147 final double heightGainPerPoint = 10;
148 final double pointsPerRevolution = 15;
149 OTSPoint3D[] spiralPoints = new OTSPoint3D[numPoints];
150 final double rotationPerPoint = 2 * Math.PI / pointsPerRevolution;
151 final double maxRevolution = 1.0 * numPoints / pointsPerRevolution;
152 for (int i = 0; i < numPoints; i++)
153 {
154 double radius = i * growthPerRevolution / pointsPerRevolution;
155 spiralPoints[i] = new OTSPoint3D(radius * Math.cos(i * rotationPerPoint), radius * Math.sin(i * rotationPerPoint),
156 i * heightGainPerPoint);
157 }
158 OTSLine3D line = new OTSLine3D(spiralPoints);
159
160 for (double x = 0; x < maxRevolution * growthPerRevolution; x += growthPerRevolution)
161 {
162 OTSPoint3D point = new OTSPoint3D(x, 0, 0);
163 OTSPoint3D result = point.closestPointOnLine2D(line);
164
165 assertEquals("distance to spiral is 0", 0, point.horizontalDistanceSI(result), 0.0001);
166 result = point.closestPointOnLine(line);
167
168 double distance = point.horizontalDistanceSI(result);
169 assertEquals("horizontal distance to spiral is x", x, distance, 0.5);
170
171 Length horizontalDistance = point.horizontalDistance(result);
172 assertEquals("horizontal distance as Length should match result of horizontalDistanceSI", distance,
173 horizontalDistance.si, Math.ulp(distance));
174 }
175
176 }
177
178
179
180
181 @Test
182 public final void lineSegmentIntersectionTest()
183 {
184 Random doubleRandom = new Random(12345);
185 for (double xTranslation = -20; xTranslation <= 20; xTranslation += 10)
186 {
187 for (double yTranslation = -20; yTranslation <= 20; yTranslation += 10)
188 {
189 for (double rotation = 0; rotation < 2 * Math.PI; rotation += 0.5)
190 {
191 OTSPoint3D p1 = makeRotatedTranslatedPoint(new OTSPoint3D(-2, 0, 100 * (doubleRandom.nextDouble() - 0.5)),
192 rotation, xTranslation, yTranslation);
193 OTSPoint3D p2 = makeRotatedTranslatedPoint(new OTSPoint3D(2, 0, 100 * (doubleRandom.nextDouble() - 0.5)),
194 rotation, xTranslation, yTranslation);
195 OTSPoint3D q1 = makeRotatedTranslatedPoint(new OTSPoint3D(0, 10, 100 * (doubleRandom.nextDouble() - 0.5)),
196 rotation, xTranslation, yTranslation);
197
198 for (int x = -4; x <= 4; x++)
199 {
200 OTSPoint3D q2 =
201 makeRotatedTranslatedPoint(new OTSPoint3D(x, -1, 100 * (doubleRandom.nextDouble() - 0.5)),
202 rotation, xTranslation, yTranslation);
203 boolean shouldBeNull = x < -2 || x > 2;
204 checkIntersection(shouldBeNull, OTSPoint3D.intersectionOfLineSegments(p1, p2, q1, q2));
205
206 checkIntersection(shouldBeNull, OTSPoint3D.intersectionOfLineSegments(p1, p2, q2, q1));
207
208 checkIntersection(shouldBeNull, OTSPoint3D.intersectionOfLineSegments(p2, p1, q1, q2));
209
210 checkIntersection(shouldBeNull, OTSPoint3D.intersectionOfLineSegments(p2, p1, q2, q1));
211 q2 = makeRotatedTranslatedPoint(new OTSPoint3D(x, 1, 100 * (doubleRandom.nextDouble() - 0.5)), rotation,
212 xTranslation, yTranslation);
213 checkIntersection(true, OTSPoint3D.intersectionOfLineSegments(p1, p2, q1, q2));
214
215 checkIntersection(true, OTSPoint3D.intersectionOfLineSegments(p1, p2, q2, q1));
216
217 checkIntersection(true, OTSPoint3D.intersectionOfLineSegments(p2, p1, q1, q2));
218
219 checkIntersection(true, OTSPoint3D.intersectionOfLineSegments(p2, p1, q2, q1));
220 }
221 }
222 }
223 }
224 }
225
226
227
228
229
230
231
232
233
234 private OTSPoint3D makeRotatedTranslatedPoint(final OTSPoint3D p, final double rotation, final double dX, final double dY)
235 {
236 double sin = Math.sin(rotation);
237 double cos = Math.cos(rotation);
238 return new OTSPoint3D((p.x * cos + p.y * sin) + dX, (p.y * cos - p.x * cos) + dY, p.z);
239 }
240
241
242
243
244
245
246 private void checkIntersection(final boolean expectNull, final OTSPoint3D point)
247 {
248 if (expectNull)
249 {
250 if (null != point)
251 {
252 System.out.println("problem");
253 }
254 assertNull("there should be an intersection", point);
255 }
256 else
257 {
258 if (null == point)
259 {
260 System.out.println("problem");
261 }
262 assertNotNull("There should not be an intersection", point);
263 }
264 }
265
266
267
268
269 @Test
270 public void directionTest()
271 {
272 for (OTSPoint3D reference : new OTSPoint3D[] {new OTSPoint3D(0, 0, 0), new OTSPoint3D(100, 200, 300),
273 new OTSPoint3D(-50, -80, 20)})
274 {
275 for (double actualDirectionDegrees : new double[] {0, 0.1, 10, 30, 89, 90, 91, 150, 179, 181, 269, 271, 359})
276 {
277 double actualDirectionRadians = Math.toRadians(actualDirectionDegrees);
278 for (double actualDistance : new double[] {0.001, 10, 999, 99999})
279 {
280 for (double dZ : new double[] {0, 123, 98766, -876})
281 {
282 OTSPoint3D other = new OTSPoint3D(reference.x + Math.cos(actualDirectionRadians) * actualDistance,
283 reference.y + Math.sin(actualDirectionRadians) * actualDistance, reference.z + dZ);
284 double angle = reference.horizontalDirectionSI(other);
285 double angle2 = reference.horizontalDirection(other).si;
286 if (angle < 0)
287 {
288 angle += 2 * Math.PI;
289 }
290 if (angle2 < 0)
291 {
292 angle2 += 2 * Math.PI;
293 }
294 assertEquals("horizontalDirectionSI returns correct direction", actualDirectionRadians, angle, 0.01);
295 assertEquals("horizontalDirection returns correct direction", actualDirectionRadians, angle2, 0.01);
296 }
297 }
298 }
299 }
300 }
301
302
303
304
305 @Test
306 public void circleCenterTest()
307 {
308 OTSPoint3D p1 = new OTSPoint3D(1, 0, 0);
309 OTSPoint3D p2 = new OTSPoint3D(-1, 0, 0);
310 assertEquals("Radius too short returns empty list", 0, OTSPoint3D.circleCenter(p1, p2, 0.999).size());
311 List<OTSPoint3D> list = OTSPoint3D.circleCenter(p1, p2, 1);
312 assertEquals("Radius exactly right and nice integer coordinates returns list with one point", 1, list.size());
313
314 for (OTSPoint3D reference : new OTSPoint3D[] {new OTSPoint3D(0, 0, 0), new OTSPoint3D(100, 200, 300),
315 new OTSPoint3D(-50, -80, 20)})
316 {
317 for (double actualDirectionDegrees : new double[] {0, 0.1, 10, 30, 89, 90, 91, 150, 179, 181, 269, 271, 359})
318 {
319 double actualDirectionRadians = Math.toRadians(actualDirectionDegrees);
320 for (double horizontalDistance : new double[] {0.1, 10, 999, 99999})
321 {
322 for (double dZ : new double[] {0, 123, -876})
323 {
324 OTSPoint3D other = new OTSPoint3D(reference.x + Math.cos(actualDirectionRadians) * horizontalDistance,
325 reference.y + Math.sin(actualDirectionRadians) * horizontalDistance, reference.z + dZ);
326 double actualDistance = reference.distanceSI(other);
327 list = OTSPoint3D.circleCenter(reference, other, actualDistance * 0.499);
328 assertEquals("Radius too short returns empty list", 0, list.size());
329 System.out.println(String.format("actualDistance=%f", actualDistance));
330 double factor = Math.sqrt(0.5);
331 list = OTSPoint3D.circleCenter(reference, other, actualDistance * factor);
332 assertEquals("Radius slightly larger returns list with 2 elements", 2, list.size());
333 for (OTSPoint3D p : list)
334 {
335 System.out.println(String.format("ref=%s, oth=%s p=%s, distance should be %f, got %f", reference,
336 other, p, actualDistance * factor, reference.distanceSI(p)));
337 assertEquals("Z is average of input points", (reference.z + other.z) / 2, p.z, 0.001);
338 assertEquals("horizontal distance from reference is R", actualDistance * factor,
339 reference.distanceSI(p), 0.001);
340 System.out.println(String.format("ref=%s, oth=%s p=%s, distance should be %f, got %f", reference,
341 other, p, actualDistance * factor, other.distanceSI(p)));
342 assertEquals("horizontal distance from other is R", actualDistance * factor, other.distanceSI(p),
343 0.001);
344 }
345 }
346 }
347 }
348 }
349 }
350
351
352
353
354 @Test
355 public void circleIntersectionsTest()
356 {
357 OTSPoint3D c1 = new OTSPoint3D(10, 20, 30);
358 OTSPoint3D c2 = new OTSPoint3D(20, 10, 0);
359 double centerDistance = c1.distanceSI(c2);
360 for (int r1 = 0; r1 < 50; r1++)
361 {
362 for (int r2 = 0; r2 < 50; r2++)
363 {
364 List<OTSPoint3D> intersections = OTSPoint3D.circleIntersections(c1, r1, c2, r2);
365 if (centerDistance < r1 + r2)
366 {
367
368 }
369 if (centerDistance > r1 + r2)
370 {
371 if (intersections.size() != 2)
372 {
373 System.out.println(
374 String.format("sphere 1: center %s radius %d, sphere2: center %s radius %d", c1, r1, c2, r2));
375 for (int index = 0; index < intersections.size(); index++)
376 {
377 System.out.println(" result " + index + ": " + intersections.get(index));
378 }
379 OTSPoint3D.circleIntersections(c1, r1, c2, r2);
380 }
381
382 }
383 }
384 }
385 }
386
387 }