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.Random;
11
12 import javax.media.j3d.Bounds;
13 import javax.vecmath.Point3d;
14
15 import org.djunits.value.vdouble.scalar.Length;
16 import org.junit.Test;
17 import org.locationtech.jts.geom.Coordinate;
18 import org.locationtech.jts.geom.GeometryFactory;
19
20 import nl.tudelft.simulation.language.d3.CartesianPoint;
21 import nl.tudelft.simulation.language.d3.DirectedPoint;
22
23
24
25
26
27
28
29
30
31
32 public class OTSPoint3DTest
33 {
34
35
36
37 @Test
38 public final void constructorsTest()
39 {
40 OTSPoint3D previousPoint = null;
41 int previousHashCode = 0;
42 double[] values = {Double.NEGATIVE_INFINITY, -99999999, -Math.PI, -1, -0.0000001, 0, 0.0000001, 1, Math.PI, 99999999,
43 Double.MAX_VALUE, Double.POSITIVE_INFINITY, Double.NaN};
44 for (double x : values)
45 {
46 for (double y : values)
47 {
48 for (double z : values)
49 {
50 OTSPoint3D p = new OTSPoint3D(x, y, z);
51 checkXYZ(p, x, y, z);
52 checkXYZ(new OTSPoint3D(new double[] {x, y, z}), x, y, z);
53 checkXYZ(new OTSPoint3D(p), x, y, z);
54 checkXYZ(new OTSPoint3D(new double[] {x, y}), x, y, 0d);
55 checkXYZ(new OTSPoint3D(new Point3d(x, y, z)), x, y, z);
56 checkXYZ(new OTSPoint3D(new CartesianPoint(x, y, z)), x, y, z);
57 checkXYZ(new OTSPoint3D(new DirectedPoint(x, y, z)), x, y, z);
58 checkXYZ(new OTSPoint3D(new Point2D.Double(x, y)), x, y, 0d);
59 checkXYZ(new OTSPoint3D(new Coordinate(x, y)), x, y, 0d);
60 checkXYZ(new OTSPoint3D(new Coordinate(x, y, Double.NaN)), x, y, 0d);
61 checkXYZ(new OTSPoint3D(new Coordinate(x, y, z)), x, y, Double.isNaN(z) ? 0d : z);
62 GeometryFactory gm = new GeometryFactory();
63 checkXYZ(new OTSPoint3D(gm.createPoint(new Coordinate(x, y, z))), x, y, 0d);
64 checkXYZ(new OTSPoint3D(x, y), x, y, 0d);
65
66 Coordinate c = p.getCoordinate();
67 assertEquals("x value", x, c.x, Math.ulp(x));
68 assertEquals("y value", y, c.y, Math.ulp(y));
69 assertEquals("z value", z, c.z, Math.ulp(z));
70 DirectedPoint dp = p.getDirectedPoint();
71 assertEquals("x value", x, dp.x, Math.ulp(x));
72 assertEquals("y value", y, dp.y, Math.ulp(y));
73 assertEquals("z value", z, dp.z, Math.ulp(z));
74 double qX = 100;
75 double qY = 200;
76 double qZ = 300;
77 OTSPoint3D q = new OTSPoint3D(qX, qY, qZ);
78 double expectedDistance = Math.sqrt(Math.pow(x - qX, 2) + Math.pow(y - qY, 2) + Math.pow(z - qZ, 2));
79 assertEquals("Distance to q should be " + expectedDistance, expectedDistance, p.distance(q).si,
80 expectedDistance / 99999);
81 Bounds bounds = p.getBounds();
82
83 assertTrue("Point (0,0,0) is within its bounds", bounds.intersect(new Point3d(0, 0, 0)));
84 assertFalse("Point at distance 1 in any direction is outside its bounds",
85 bounds.intersect(new Point3d(-1, 0, 0)));
86 assertFalse("Point at distance 1 in any direction is outside its bounds",
87 bounds.intersect(new Point3d(1, 0, 0)));
88 assertFalse("Point at distance 1 in any direction is outside its bounds",
89 bounds.intersect(new Point3d(0, -1, 0)));
90 assertFalse("Point at distance 1 in any direction is outside its bounds",
91 bounds.intersect(new Point3d(0, 1, 0)));
92 assertFalse("Point at distance 1 in any direction is outside its bounds",
93 bounds.intersect(new Point3d(0, 0, -1)));
94 assertFalse("Point at distance 1 in any direction is outside its bounds",
95 bounds.intersect(new Point3d(0, 0, 1)));
96 DirectedPoint directedPoint = p.getLocation();
97 assertEquals("Location returns a DirectedPoint at the location of p", x, directedPoint.x, Math.ulp(x));
98 assertEquals("Location returns a DirectedPoint at the location of p", y, directedPoint.y, Math.ulp(y));
99 assertEquals("Location returns a DirectedPoint at the location of p", z, directedPoint.z, Math.ulp(z));
100 String s = p.toString();
101 assertNotNull("toString returns something", s);
102 assertTrue("toString returns string of reasonable length", s.length() > 10);
103 int hashCode = p.hashCode();
104
105 assertFalse("Hash code should be different", previousHashCode == hashCode);
106 previousHashCode = hashCode;
107
108 assertFalse("Successively generated points are all different", p.equals(previousPoint));
109 assertTrue("Point is equal to itself", p.equals(p));
110 assertTrue("Point is equals to duplicate of itself", p.equals(new OTSPoint3D(p)));
111 assertFalse("Point is not equal to some other object", p.equals(s));
112 previousPoint = p;
113 }
114 }
115 }
116 }
117
118
119
120
121
122
123
124
125 private void checkXYZ(final OTSPoint3D otsPoint3D, final double expectedX, final double expectedY, final double expectedZ)
126 {
127 assertEquals("x value", expectedX, otsPoint3D.x, Math.ulp(expectedX));
128 assertEquals("y value", expectedY, otsPoint3D.y, Math.ulp(expectedY));
129 assertEquals("z value", expectedZ, otsPoint3D.z, Math.ulp(expectedZ));
130 Point2D.Double p = (Point2D.Double) otsPoint3D.getPoint2D();
131 assertEquals("x value", expectedX, p.x, Math.ulp(expectedX));
132 assertEquals("y value", expectedY, p.y, Math.ulp(expectedY));
133 }
134
135
136
137
138 @Test
139 public final void interpolateTest()
140 {
141 OTSPoint3D p0 = new OTSPoint3D(123, 234, 345);
142 OTSPoint3D p1 = new OTSPoint3D(567, 678, 789);
143 for (double ratio : new double[] {0, 1, 0.5, 0.1, -10, 10})
144 {
145 OTSPoint3D pi = OTSPoint3D.interpolate(ratio, p0, p1);
146 assertTrue("result of interpolate is not null", null != pi);
147 assertEquals("x of interpolate", (1 - ratio) * p0.x + ratio * p1.x, pi.x, 0.00001);
148 assertEquals("y of interpolate", (1 - ratio) * p0.y + ratio * p1.y, pi.y, 0.00001);
149 assertEquals("z of interpolate", (1 - ratio) * p0.z + ratio * p1.z, pi.z, 0.00001);
150 }
151 }
152
153
154
155
156
157 @Test
158 public final void closestPointTest() throws OTSGeometryException
159 {
160
161 final int numPoints = 100;
162 final double growthPerRevolution = 5;
163 final double heightGainPerPoint = 10;
164 final double pointsPerRevolution = 15;
165 OTSPoint3D[] spiralPoints = new OTSPoint3D[numPoints];
166 final double rotationPerPoint = 2 * Math.PI / pointsPerRevolution;
167 final double maxRevolution = 1.0 * numPoints / pointsPerRevolution;
168 for (int i = 0; i < numPoints; i++)
169 {
170 double radius = i * growthPerRevolution / pointsPerRevolution;
171 spiralPoints[i] = new OTSPoint3D(radius * Math.cos(i * rotationPerPoint), radius * Math.sin(i * rotationPerPoint),
172 i * heightGainPerPoint);
173 }
174 OTSLine3D line = new OTSLine3D(spiralPoints);
175
176 for (double x = 0; x < maxRevolution * growthPerRevolution; x += growthPerRevolution)
177 {
178 OTSPoint3D point = new OTSPoint3D(x, 0, 0);
179 OTSPoint3D result = point.closestPointOnLine2D(line);
180
181 assertEquals("distance to spiral is 0", 0, point.horizontalDistanceSI(result), 0.0001);
182 result = point.closestPointOnLine(line);
183
184 double distance = point.horizontalDistanceSI(result);
185 assertEquals("horizontal distance to spiral is x", x, distance, 0.5);
186
187 Length horizontalDistance = point.horizontalDistance(result);
188 assertEquals("horizontal distance as Length should match result of horizontalDistanceSI", distance,
189 horizontalDistance.si, Math.ulp(distance));
190 }
191
192 }
193
194
195
196
197 @Test
198 public final void lineSegmentIntersectionTest()
199 {
200 Random doubleRandom = new Random(12345);
201 for (double xTranslation = -20; xTranslation <= 20; xTranslation += 10)
202 {
203 for (double yTranslation = -20; yTranslation <= 20; yTranslation += 10)
204 {
205 for (double rotation = 0; rotation < 2 * Math.PI; rotation += 0.5)
206 {
207 OTSPoint3D p1 = makeRotatedTranslatedPoint(new OTSPoint3D(-2, 0, 100 * (doubleRandom.nextDouble() - 0.5)),
208 rotation, xTranslation, yTranslation);
209 OTSPoint3D p2 = makeRotatedTranslatedPoint(new OTSPoint3D(2, 0, 100 * (doubleRandom.nextDouble() - 0.5)),
210 rotation, xTranslation, yTranslation);
211 OTSPoint3D q1 = makeRotatedTranslatedPoint(new OTSPoint3D(0, 10, 100 * (doubleRandom.nextDouble() - 0.5)),
212 rotation, xTranslation, yTranslation);
213
214 for (int x = -4; x <= 4; x++)
215 {
216 OTSPoint3D q2 =
217 makeRotatedTranslatedPoint(new OTSPoint3D(x, -1, 100 * (doubleRandom.nextDouble() - 0.5)),
218 rotation, xTranslation, yTranslation);
219 boolean shouldBeNull = x < -2 || x > 2;
220 checkIntersection(shouldBeNull, OTSPoint3D.intersectionOfLineSegments(p1, p2, q1, q2));
221
222 checkIntersection(shouldBeNull, OTSPoint3D.intersectionOfLineSegments(p1, p2, q2, q1));
223
224 checkIntersection(shouldBeNull, OTSPoint3D.intersectionOfLineSegments(p2, p1, q1, q2));
225
226 checkIntersection(shouldBeNull, OTSPoint3D.intersectionOfLineSegments(p2, p1, q2, q1));
227 q2 = makeRotatedTranslatedPoint(new OTSPoint3D(x, 1, 100 * (doubleRandom.nextDouble() - 0.5)), rotation,
228 xTranslation, yTranslation);
229 checkIntersection(true, OTSPoint3D.intersectionOfLineSegments(p1, p2, q1, q2));
230
231 checkIntersection(true, OTSPoint3D.intersectionOfLineSegments(p1, p2, q2, q1));
232
233 checkIntersection(true, OTSPoint3D.intersectionOfLineSegments(p2, p1, q1, q2));
234
235 checkIntersection(true, OTSPoint3D.intersectionOfLineSegments(p2, p1, q2, q1));
236 }
237 }
238 }
239 }
240 }
241
242
243
244
245
246
247
248
249
250 private OTSPoint3D makeRotatedTranslatedPoint(final OTSPoint3D p, final double rotation, final double dX, final double dY)
251 {
252 double sin = Math.sin(rotation);
253 double cos = Math.cos(rotation);
254 return new OTSPoint3D((p.x * cos + p.y * sin) + dX, (p.y * cos - p.x * cos) + dY, p.z);
255 }
256
257
258
259
260
261
262 private void checkIntersection(final boolean expectNull, final OTSPoint3D point)
263 {
264 if (expectNull)
265 {
266 if (null != point)
267 {
268 System.out.println("problem");
269 }
270 assertNull("there should be an intersection", point);
271 }
272 else
273 {
274 if (null == point)
275 {
276 System.out.println("problem");
277 }
278 assertNotNull("There should not be an intersection", point);
279 }
280
281 }
282
283 }