1 package org.djutils.draw.curve;
2
3 import static org.junit.jupiter.api.Assertions.assertEquals;
4 import static org.junit.jupiter.api.Assertions.assertTrue;
5 import static org.junit.jupiter.api.Assertions.fail;
6
7 import java.util.Random;
8
9 import org.djutils.draw.curve.Flattener2d.NumSegments;
10 import org.djutils.draw.function.ContinuousPiecewiseLinearFunction;
11 import org.djutils.draw.line.PolyLine2d;
12 import org.djutils.draw.point.DirectedPoint2d;
13 import org.djutils.draw.point.Point2d;
14 import org.djutils.math.AngleUtil;
15 import org.junit.jupiter.api.Test;
16
17
18
19
20
21
22
23
24
25
26
27 public class ClothoidTest
28 {
29
30
31 private static final int SEGMENTS = 64;
32
33
34 private static final int RUNS = 10000;
35
36
37
38
39
40
41
42 private static final double ANGLE_TOLERANCE = 4 * Math.PI / SEGMENTS;
43
44
45 private static final double DISTANCE_TOLERANCE = 1e-2;
46
47
48
49
50 @Test
51 public void testPoints()
52 {
53 Random r = new Random(3);
54 for (int i = 0; i < RUNS; i++)
55 {
56 DirectedPoint2d start =
57 new DirectedPoint2d(r.nextDouble() * 10.0, r.nextDouble() * 10.0, (r.nextDouble() * 2 - 1) * Math.PI);
58 DirectedPoint2d end =
59 new DirectedPoint2d(r.nextDouble() * 10.0, r.nextDouble() * 10.0, (r.nextDouble() * 2 - 1) * Math.PI);
60 Clothoid2d clothoid = new Clothoid2d(start, end);
61
62
63
64 PolyLine2d line = clothoid.toPolyLine(new Flattener2d.NumSegments(64));
65 verifyLine(start, clothoid, line, null, null, null);
66 assertTrue(clothoid.getAppliedShape().equals("Arc") || clothoid.getAppliedShape().equals("Clothoid"),
67 "Clothoid identifies itself correctly");
68 if (clothoid.getAppliedShape().equals("Arc"))
69 {
70 assertEquals(0, clothoid.getPoint(0.0).distance(start), 0.0001, "start point of clothoid that became an arc");
71 assertEquals(0, clothoid.getPoint(1.0).distance(end), 0.0001, "end point of clothoid that became an arc");
72 Point2d midPoint = clothoid.getPoint(0.5);
73 assertEquals(midPoint.distance(start), midPoint.distance(end), 0.0001,
74 "mid point has same distance to start as it has to end");
75 assertEquals(start.dirZ, clothoid.getDirection(0.0), 0.01, "start direction of clothoid that became an arc");
76 assertEquals(end.dirZ, clothoid.getDirection(1.0), 0.01, "end direction of clothoid that became an arc");
77 }
78 }
79 }
80
81
82
83
84 @Test
85 public void testClothoidConstructors()
86 {
87 try
88 {
89 new Clothoid2d(new DirectedPoint2d(1, 2, 3), -0.5, 10.0, 4.0);
90 fail("Negative a should have thrown an IllegalArgumentException");
91 }
92 catch (IllegalArgumentException iae)
93 {
94
95 }
96
97 try
98 {
99 Clothoid2d.withLength(new DirectedPoint2d(1, 2, 3), -0.5, 10.0, 4.0);
100 fail("Negative length should have thrown an IllegalArgumentException");
101 }
102 catch (IllegalArgumentException iae)
103 {
104
105 }
106
107
108 DirectedPoint2d start = new DirectedPoint2d(0, 10, 0);
109 DirectedPoint2d end = new DirectedPoint2d(20, 10, 0);
110 Clothoid2d cl2d = new Clothoid2d(start, end);
111 assertEquals(0, cl2d.getDirection(0.0), 0.00001, "start direction of degenerate Clothoid");
112 assertEquals(0, cl2d.getDirection(0.5), 0.00001, "direction half way of degenerate Clothoid");
113 assertEquals(0, cl2d.getDirection(1.0), 0.00001, "direction at end of degenerate Clothoid");
114 assertEquals(0, cl2d.getPoint(0.0).distance(start), 0.00001, "start point of degenerate Clothoid");
115 assertEquals(0, cl2d.getPoint(1.0).distance(end), 0.00001, "end point of degenerate Clothoid");
116 PolyLine2d pl = cl2d.toPolyLine(new Flattener2d.NumSegments(10));
117
118 assertEquals(2, pl.size(), "polyline has two points");
119 assertEquals(0, pl.get(0).distance(start), 0.00001, "polyline starts at start");
120 assertEquals(0, pl.get(1).distance(end), 0.00001, "polyline ends at end");
121 }
122
123
124
125
126
127 @Test
128 public void testStraight()
129 {
130 Random r = new Random(3);
131 double tolerance = 2.0 * Math.PI / 3600.0;
132 double startAng = -Math.PI;
133 double dAng = Math.PI * 2 / 100;
134 double sign = 1.0;
135 for (double ang = startAng; ang < Math.PI; ang += dAng)
136 {
137 double x = Math.cos(ang);
138 double y = Math.sin(ang);
139 DirectedPoint2d start = new DirectedPoint2d(x, y, ang - tolerance + r.nextDouble() * tolerance * 2);
140 DirectedPoint2d end = new DirectedPoint2d(3 * x, 3 * y, ang - tolerance + r.nextDouble() * tolerance * 2);
141
142 Clothoid2d clothoid = new Clothoid2d(start, end);
143 NumSegments numSegments64 = new NumSegments(64);
144 PolyLine2d line = clothoid.toPolyLine(numSegments64);
145 assertEquals(line.size(), 2, "Clothoid between point on line did not become a straight");
146 assertTrue(clothoid.getAppliedShape().equals("Straight"), "Clothoid identifies itself correctly");
147
148 start = new DirectedPoint2d(x, y, ang + sign * tolerance * 1.1);
149 end = new DirectedPoint2d(3 * x, 3 * y, ang + sign * tolerance * 1.1);
150 sign *= -1.0;
151 clothoid = new Clothoid2d(start, end);
152 line = clothoid.toPolyLine(numSegments64);
153 assertTrue(line.size() > 2, "Clothoid between point just not on line should not become a straight");
154 assertTrue(clothoid.getAppliedShape().equals("Clothoid"), "Clothoid identifies itself correctly");
155 }
156 }
157
158
159
160
161 @Test
162 public void testLength()
163 {
164 Random r = new Random(3);
165 for (int i = 0; i < RUNS; i++)
166 {
167 DirectedPoint2d start =
168 new DirectedPoint2d(r.nextDouble() * 10.0, r.nextDouble() * 10.0, (r.nextDouble() * 2 - 1) * Math.PI);
169 double length = 10.0 + r.nextDouble() * 500.0;
170 double sign = r.nextBoolean() ? 1.0 : -1.0;
171 double startCurvature = sign / (50.0 + r.nextDouble() * 1000.0);
172 sign = r.nextBoolean() ? 1.0 : -1.0;
173 double endCurvature = sign / (50.0 + r.nextDouble() * 1000.0);
174
175 Clothoid2d clothoid = Clothoid2d.withLength(start, length, startCurvature, endCurvature);
176 PolyLine2d line = clothoid.toPolyLine(new NumSegments(64));
177 verifyLine(start, clothoid, line, startCurvature, endCurvature, null);
178 }
179 }
180
181
182
183
184 @Test
185 public void testA()
186 {
187 Random r = new Random(3);
188 for (int i = 0; i < RUNS; i++)
189 {
190 DirectedPoint2d start =
191 new DirectedPoint2d(r.nextDouble() * 10.0, r.nextDouble() * 10.0, (r.nextDouble() * 2 - 1) * Math.PI);
192 double sign = r.nextBoolean() ? 1.0 : -1.0;
193 double startCurvature = sign / (50.0 + r.nextDouble() * 1000.0);
194 sign = r.nextBoolean() ? 1.0 : -1.0;
195 double endCurvature = sign / (50.0 + r.nextDouble() * 1000.0);
196 double a = Math.sqrt((10.0 + r.nextDouble() * 500.0) / Math.abs(endCurvature - startCurvature));
197
198 Clothoid2d clothoid = new Clothoid2d(start, a, startCurvature, endCurvature);
199 PolyLine2d line = clothoid.toPolyLine(new NumSegments(64));
200 verifyLine(start, clothoid, line, startCurvature, endCurvature, a);
201 assertEquals(1 / startCurvature, clothoid.getStartRadius(), 0.001, "Start radius can be retrieved");
202 assertEquals(1 / endCurvature, clothoid.getEndRadius(), 0.001, "End radius can be retrieved");
203 }
204 }
205
206
207
208
209
210
211
212
213
214
215 private void verifyLine(final DirectedPoint2d start, final Clothoid2d clothoid, final PolyLine2d line,
216 final Double startCurvature, final Double endCurvature, final Double a)
217 {
218 assertEquals(0.0, Math.hypot(start.x - line.get(0).x, start.y - line.get(0).y), DISTANCE_TOLERANCE,
219 "Start location deviates");
220 assertEquals(0.0, Math.hypot(clothoid.getEndPoint().x - line.get(line.size() - 1).x,
221 clothoid.getEndPoint().y - line.get(line.size() - 1).y), DISTANCE_TOLERANCE, "End location deviates");
222 assertEquals(0.0, AngleUtil.normalizeAroundZero(start.dirZ - line.get(0).directionTo(line.get(1))), ANGLE_TOLERANCE,
223 "Start direction deviates");
224 assertEquals(0.0,
225 AngleUtil.normalizeAroundZero(
226 clothoid.getEndPoint().dirZ - line.get(line.size() - 2).directionTo(line.get(line.size() - 1))),
227 ANGLE_TOLERANCE, "End direction deviates");
228 assertEquals(0.0, start.distance(clothoid.getStartPoint()), DISTANCE_TOLERANCE, "Start location deviates");
229 double lengthRatio = line.getLength() / clothoid.getLength();
230 assertEquals(1.0, lengthRatio, 0.01, "Length is more than 1% shorter or longer than theoretical");
231 if (startCurvature != null)
232 {
233 double curveatureRatio = clothoid.getStartCurvature() / startCurvature;
234 assertEquals(1.0, curveatureRatio, 0.01, "Start curvature is more than 1% shorter or longer than theoretical");
235 }
236 if (endCurvature != null)
237 {
238 double curveatureRatio = clothoid.getEndCurvature() / endCurvature;
239 assertEquals(1.0, curveatureRatio, 0.01, "End curvature is more than 1% shorter or longer than theoretical");
240 }
241 if (a != null)
242 {
243 double aRadius = clothoid.getA() / a;
244 assertEquals(1.0, aRadius, 0.01, "A-value is more than 1% less or more than theoretical");
245 }
246 assertTrue(clothoid.toString().startsWith("Clothoid ["), "toString method returns something descriptive");
247 }
248
249
250
251
252
253 @Test
254 public void testOffset()
255 {
256 Flattener2d flattener = new Flattener2d.NumSegments(32);
257 OffsetFlattener2d offsetFlattener = new OffsetFlattener2d.NumSegments(32);
258
259 for (double yA = -30.0; yA < 35.0; yA += 20.0)
260 {
261
262 for (double xB = -20.0; xB < 25.0; xB += 20.0 * 2.0 / 3.0)
263 {
264
265 DirectedPoint2d a = new DirectedPoint2d(0.0, yA, xB < 0.0 ? Math.PI : 0.0);
266
267 DirectedPoint2d b = new DirectedPoint2d(xB, 0.0, yA < 0.0 ? Math.PI / 2 : -Math.PI / 2);
268 Clothoid2d clothoid = new Clothoid2d(a, b);
269 PolyLine2d flattened = clothoid.toPolyLine(flattener);
270 assertEquals(a.x, flattened.getX(0), 0.0001, "start x");
271 assertEquals(a.y, flattened.getY(0), 0.0001, "start y");
272 assertEquals(b.x, flattened.getX(flattened.size() - 1), 0.0001, "end x");
273 assertEquals(b.y, flattened.getY(flattened.size() - 1), 0.0001, "end y");
274
275 for (double offset = -2.0; offset < 3.0; offset += 4.0)
276 {
277 flattened = clothoid.toPolyLine(offsetFlattener,
278 new ContinuousPiecewiseLinearFunction(0.0, offset, 1.0, offset));
279 Point2d start = flattened.get(0);
280 Point2d end = flattened.get(flattened.size() - 1);
281 assertEquals(0.0, start.x, 0.00001);
282 assertEquals(yA + (xB > 0.0 ? offset : -offset), start.y, 0.00001);
283 assertEquals(xB + (yA > 0.0 ? offset : -offset), end.x, 0.00001);
284 assertEquals(0.0, end.y, 0.00001);
285 }
286 }
287 }
288 }
289
290 }