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