1 package org.opentrafficsim.core.gtu.plan.operational;
2
3 import static org.junit.Assert.assertEquals;
4 import static org.junit.Assert.fail;
5
6 import org.djunits.unit.AccelerationUnit;
7 import org.djunits.unit.DurationUnit;
8 import org.djunits.unit.LengthUnit;
9 import org.djunits.unit.SpeedUnit;
10 import org.djunits.unit.TimeUnit;
11 import org.djunits.value.vdouble.scalar.Acceleration;
12 import org.djunits.value.vdouble.scalar.Duration;
13 import org.djunits.value.vdouble.scalar.Length;
14 import org.djunits.value.vdouble.scalar.Speed;
15 import org.djunits.value.vdouble.scalar.Time;
16 import org.junit.Test;
17 import org.opentrafficsim.core.geometry.OTSGeometryException;
18 import org.opentrafficsim.core.geometry.OTSLine3D;
19 import org.opentrafficsim.core.geometry.OTSPoint3D;
20
21 import nl.tudelft.simulation.language.d3.DirectedPoint;
22
23
24
25
26
27
28
29
30
31
32
33 public class OperationalPlanTest
34 {
35
36
37
38
39
40 @Test
41 public final void testOperationalPlan() throws OperationalPlanException, OTSGeometryException
42 {
43 DirectedPoint waitPoint = new DirectedPoint(12, 13, 14, 15, 16, 17);
44 Time startTime = new Time(100, TimeUnit.BASE);
45 Duration duration = new Duration(1, DurationUnit.MINUTE);
46 OperationalPlan op = new OperationalPlan(null, waitPoint, startTime, duration);
47 assertEquals("Start speed is 0", 0, op.getStartSpeed().si, 0);
48 assertEquals("End speed is 0", 0, op.getEndSpeed().si, 0);
49 assertEquals("Start time is " + startTime, startTime.si, op.getStartTime().si, 0);
50 assertEquals("End time is " + startTime.plus(duration), startTime.plus(duration).si, op.getEndTime().si, 0.0001);
51 assertEquals("Segment list contains 1 segment", 1, op.getOperationalPlanSegmentList().size());
52 OperationalPlan.Segment segment = op.getOperationalPlanSegmentList().get(0);
53 assertEquals("Duration is " + duration, duration.si, segment.getDuration().si, 0.00001);
54 assertEquals("DurationSI is " + duration.si, duration.si, segment.getDurationSI(), 0.00001);
55 assertEquals("End location is " + waitPoint, 0, waitPoint.distance(op.getEndLocation()), 0.0001);
56 try
57 {
58 op.getLocation(new Duration(-0.1, DurationUnit.SI));
59 fail("getLocation for negative relative time should have thrown an OperationalPlanException");
60 }
61 catch (OperationalPlanException ope)
62 {
63
64 }
65 try
66 {
67 op.getLocation(new Time(99.5, TimeUnit.BASE));
68 fail("getLocation for absolute time before start time should have thrown an OperationalPlanException");
69 }
70 catch (OperationalPlanException ope)
71 {
72
73 }
74 op.getLocation(new Time(100.1, TimeUnit.BASE));
75 op.getLocation(new Time(159.9, TimeUnit.BASE));
76 try
77 {
78 op.getLocation(new Time(160.1, TimeUnit.BASE));
79 fail("getLocation for absolute time after end time should have thrown an OperationalPlanException");
80 }
81 catch (OperationalPlanException ope)
82 {
83
84 }
85 for (int i = 0; i <= duration.si; i++)
86 {
87 Time t = startTime.plus(new Duration(i, DurationUnit.SECOND));
88 DirectedPoint locationAtT = op.getLocation(t);
89
90
91 assertEquals("Distance from wait point at " + t + " is 0", 0, waitPoint.distance(locationAtT), 0.002);
92 }
93 assertEquals("end location matches start location", 0,
94 new OTSPoint3D(op.getEndLocation()).distance(new OTSPoint3D(waitPoint)).si, 0.0001);
95 OTSLine3D path = new OTSLine3D(new OTSPoint3D(12, 13, 14), new OTSPoint3D(123, 234, 345));
96 Speed startSpeed = new Speed(20, SpeedUnit.KM_PER_HOUR);
97 Speed endSpeed = new Speed(50, SpeedUnit.KM_PER_HOUR);
98 Acceleration maxAcceleration = new Acceleration(1, AccelerationUnit.METER_PER_SECOND_2);
99 Acceleration maxDeceleration = new Acceleration(6, AccelerationUnit.METER_PER_SECOND_2);
100 op = OperationalPlanBuilder.buildGradualAccelerationPlan(null, path, startTime, startSpeed, endSpeed, maxAcceleration,
101 maxDeceleration);
102 assertEquals("Start speed is " + startSpeed, startSpeed.si, op.getStartSpeed().si, 0.00001);
103 assertEquals("Start time is " + startTime, startTime.si, op.getStartTime().si, 0.00001);
104 assertEquals("End speed is " + endSpeed, endSpeed.si, op.getEndSpeed().si, 0.00001);
105
106
107
108 OTSLine3D returnedPath = op.getPath();
109
110 assertEquals("size of path should match", path.size(), returnedPath.size());
111 for (int i = 0; i < path.size(); i++)
112 {
113 assertEquals("position of point " + i, 0, path.get(i).distance(returnedPath.get(i)).si, 0.0001);
114 }
115 double pathLength = path.getLengthSI();
116
117
118
119 double speedDifference = endSpeed.minus(startSpeed).si;
120
121
122
123
124
125
126 double t = pathLength / (startSpeed.si + 0.5 * speedDifference);
127 Time endTime = startTime.plus(new Duration(t, DurationUnit.SECOND));
128
129 Acceleration a = new Acceleration(speedDifference / t, AccelerationUnit.SI);
130
131
132 double actualLength = startSpeed.si * t + 0.5 * a.si * t * t;
133
134 assertEquals("Driven length is " + actualLength, actualLength, pathLength, 0.00001);
135 double actualEndSpeed = startSpeed.si + t * a.si;
136
137 assertEquals("Speed at end is " + endSpeed, endSpeed.si, actualEndSpeed, 0.000001);
138 assertEquals("End time is " + endTime, endTime.si, op.getEndTime().si, 0.00001);
139
140 assertEquals("Required acceleration is " + a, a.si, op.getAcceleration(startTime).si, 0.000001);
141 assertEquals("total duration", endTime.minus(startTime).si, op.getTotalDuration().si, 0.00001);
142 DirectedPoint dp = op.getEndLocation();
143 try
144 {
145 assertEquals("end location", 0, new OTSPoint3D(dp).distanceSI(path.get(1)), 0.00001);
146 }
147 catch (OTSGeometryException exception)
148 {
149 fail("Caught unexpected exception");
150 exception.printStackTrace();
151 }
152 int steps = 20;
153 for (int i = 0; i <= steps; i++)
154 {
155 double stepTime = startTime.si + t * i / steps * 0.9999;
156 Time absTime = new Time(stepTime, TimeUnit.BASE);
157 double deltaT = stepTime - startTime.si;
158 Duration relTime = new Duration(deltaT, DurationUnit.SI);
159 double expectedDistance = startSpeed.si * deltaT + 0.5 * a.si * deltaT * deltaT;
160 double fraction = expectedDistance / path.getLength().si;
161 OTSPoint3D expectedPosition = new OTSPoint3D(path.getLocationFraction(fraction));
162 DirectedPoint actualPosition = op.getLocation(absTime);
163 assertEquals("Position at abs time " + deltaT, 0, expectedPosition.distance(new OTSPoint3D(actualPosition)).si,
164 0.002);
165 actualPosition = op.getLocation(relTime);
166 assertEquals("Position at rel time " + deltaT, 0, expectedPosition.distance(new OTSPoint3D(actualPosition)).si,
167 0.002);
168 double expectedSpeed = startSpeed.si + a.si * deltaT;
169 Speed actualSpeed = op.getSpeed(absTime);
170 assertEquals("Speed at abs time " + deltaT, expectedSpeed, actualSpeed.si, 0.0001);
171 actualSpeed = op.getSpeed(relTime);
172 assertEquals("Speed at rel time " + deltaT, expectedSpeed, actualSpeed.si, 0.0001);
173 Time actualTimeAtPosition = op.timeAtDistance(new Length(fraction * path.getLength().si, LengthUnit.SI));
174 assertEquals("TimeAtDistance matches time", startTime.si + deltaT, actualTimeAtPosition.si, 0.0001);
175 double actualAcceleration = op.getAcceleration(absTime).si;
176 assertEquals("acceleration at abs time", a.si, actualAcceleration, 0.00001);
177 actualAcceleration = op.getAcceleration(relTime).si;
178 assertEquals("acceleration at rel time", a.si, actualAcceleration, 0.00001);
179 assertEquals("traveled distance at abs time", expectedDistance, op.getTraveledDistance(absTime).si, 0.0001);
180 assertEquals("traveled distance at rel time", expectedDistance, op.getTraveledDistance(relTime).si, 0.0001);
181 assertEquals("traveled distanceSI at abs time", expectedDistance, op.getTraveledDistanceSI(absTime), 0.0001);
182 assertEquals("traveled distanceSI at rel time", expectedDistance, op.getTraveledDistanceSI(relTime), 0.0001);
183 }
184 }
185 }