1 package org.opentrafficsim.road.gtu.lane.plan.operational;
2
3 import org.djunits.value.vdouble.scalar.Direction;
4 import org.djunits.value.vdouble.scalar.Length;
5 import org.djunits.value.vdouble.scalar.Time;
6 import org.djutils.draw.point.OrientedPoint2d;
7 import org.djutils.draw.point.Point2d;
8 import org.opentrafficsim.core.geometry.OtsGeometryException;
9 import org.opentrafficsim.core.geometry.OtsLine2d;
10 import org.opentrafficsim.core.geometry.OtsLine2d.FractionalFallback;
11 import org.opentrafficsim.core.gtu.GtuException;
12 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
13 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
14 import org.opentrafficsim.core.gtu.plan.operational.Segments;
15 import org.opentrafficsim.road.gtu.lane.LaneBasedGtu;
16 import org.opentrafficsim.road.network.lane.Lane;
17 import org.opentrafficsim.road.network.lane.LanePosition;
18
19 /**
20 * An operational plan with some extra information about the lanes and lane changes so this information does not have to be
21 * recalculated multiple times. Furthermore, it is quite expensive to check whether a lane change is part of the oprtational
22 * plan based on geographical data.
23 * <p>
24 * Copyright (c) 2013-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
25 * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
26 * </p>
27 * @author <a href="https://github.com/averbraeck">Alexander Verbraeck</a>
28 * @author <a href="https://tudelft.nl/staff/p.knoppers-1">Peter Knoppers</a>
29 */
30 public class LaneBasedOperationalPlan extends OperationalPlan
31 {
32 /** */
33 private static final long serialVersionUID = 20160120L;
34
35 /** Deviative; meaning not along lane center lines. */
36 private final boolean deviative;
37
38 /**
39 * Construct an operational plan with or without a lane change.
40 * @param gtu LaneBasedGtu; the GTU for debugging purposes
41 * @param path OtsLine2d; the path to follow from a certain time till a certain time. The path should have <i>at
42 * least</i> the length
43 * @param startTime Time; the absolute start time when we start executing the path
44 * @param segments Segments; the segments that make up the path with an acceleration, constant
45 * speed or deceleration profile
46 * @param deviative boolean; whether the path is not along lane center lines
47 * @throws OperationalPlanException when the path is too short for the operation
48 */
49 @SuppressWarnings("checkstyle:parameternumber")
50 public LaneBasedOperationalPlan(final LaneBasedGtu gtu, final OtsLine2d path, final Time startTime,
51 final Segments segments, final boolean deviative) throws OperationalPlanException
52 {
53 super(gtu, path, startTime, segments);
54 this.deviative = deviative;
55 }
56
57 /**
58 * Check if we deviate from the center line.
59 * @return whether this maneuver involves deviation from the center line.
60 */
61 public final boolean isDeviative()
62 {
63 return this.deviative;
64 }
65
66 /**
67 * Returns the total length along the reference lane that the GTU travels. In case of a deviative plan this involves
68 * projection of the actual path to the lane center lines.
69 * @param gtu LaneBasedGtu; GTU
70 * @return Length; total length along the path
71 * @throws GtuException if the GTU has not reference position
72 */
73 public final Length getTotalLengthAlongLane(final LaneBasedGtu gtu) throws GtuException
74 {
75 if (!this.deviative)
76 {
77 // along the lane center lines
78 return getTotalLength();
79 }
80
81 // let's project the end position of the plan
82 return getDistanceAlongLane(gtu, getEndLocation());
83 }
84
85 /**
86 * Helper method to get rotation at start or end of lane.
87 * @param lane Lane; lane
88 * @param start boolean; start (or end)
89 * @return rotation at start or end of lane
90 */
91 private double getRotZAtFraction(final Lane lane, final boolean start)
92 {
93 double f = start ? 0.0 : 1.0;
94 try
95 {
96 return lane.getCenterLine().getLocationFraction(f).getDirZ();
97 }
98 catch (OtsGeometryException exception)
99 {
100 // should not occur, we use 0.0 and 1.0
101 throw new RuntimeException("Unexpected exception while assessing if a GTU is between lanes.", exception);
102 }
103 }
104
105 /**
106 * Returns the distance along the reference lane that the GTU travels from the current location up to the point.
107 * @param gtu LaneBasedGtu; GTU
108 * @param point OrientedPoint2d; point where the GTU is or will be
109 * @return Length; total length along the path
110 * @throws GtuException if the GTU has not reference position
111 */
112 public final Length getDistanceAlongLane(final LaneBasedGtu gtu, final OrientedPoint2d point) throws GtuException
113 {
114
115 // start lane center lines at current reference lane
116 LanePosition pos = gtu.getReferencePosition();
117 Lane lane = pos.lane();
118
119 // initialize loop data
120 double length = -lane.coveredDistance(pos.position().si / pos.lane().getLength().si).si;
121 double f = Double.NaN;
122 Direction prevDir = Direction.instantiateSI(getRotZAtFraction(lane, true));
123
124 // move to next lane while projection fails
125 while (Double.isNaN(f))
126 {
127 Lane nextLane = gtu.getNextLaneForRoute(lane);
128 Direction nextDir = Direction.instantiateSI(nextLane == null ? getRotZAtFraction(lane, false)
129 : .5 * getRotZAtFraction(lane, false) + .5 * getRotZAtFraction(nextLane, true));
130 f = lane.getCenterLine().projectFractional(prevDir, nextDir, point.x, point.y, FractionalFallback.NaN);
131
132 // check if the GTU is adjacent to the bit between the lanes (if there is such a bit)
133 if (Double.isNaN(f))
134 {
135 if (nextLane == null)
136 {
137 // projection error on dad-end lane, add the length of the lane
138 f = 1.0;
139 length += lane.coveredDistance(f).si;
140 }
141 else
142 {
143 try
144 {
145 // compose gap line
146 Point2d last = lane.getCenterLine().getLast();
147 Point2d first = nextLane.getCenterLine().get(0);
148 if (!(last).equals(first))
149 {
150 OtsLine2d gap = new OtsLine2d(last, first);
151 double fGap = gap.projectFractional(null, null, point.x, point.y, FractionalFallback.NaN);
152 if (!Double.isNaN(fGap))
153 {
154 f = (lane.getLength().si + fGap * gap.getLength().si) / lane.getLength().si;
155 }
156 else
157 {
158 // gap, but no successful projection, use next lane in next loop, increase length so far
159 length += lane.getLength().si;
160 lane = nextLane;
161 prevDir = nextDir;
162 }
163 }
164 else
165 {
166 // no gap, use next lane in next loop, increase length so far
167 length += lane.getLength().si;
168 lane = nextLane;
169 prevDir = nextDir;
170 }
171 }
172 catch (OtsGeometryException exception)
173 {
174 // should not occur, we use get(0) and getLast()
175 throw new RuntimeException("Unexpected exception while assessing if a GTU is between lanes.",
176 exception);
177 }
178 }
179 }
180 else
181 {
182 // projection is ok on lane, increase length so far
183 length += lane.coveredDistance(f).si;
184 }
185 }
186 // add length on lane where the reference position was projected to (or to it's consecutive gap between lanes)
187 return Length.instantiateSI(length);
188 }
189
190 /** {@inheritDoc} */
191 @Override
192 public final String toString()
193 {
194 return "LaneBasedOperationalPlan [deviative=" + this.deviative + "]";
195 }
196 }