LaneOperationalPlanBuilder.java
package org.opentrafficsim.road.gtu.lane.plan.operational;
import java.util.ArrayList;
import java.util.LinkedHashSet;
import java.util.List;
import java.util.Optional;
import java.util.Set;
import org.djunits.value.vdouble.scalar.Acceleration;
import org.djunits.value.vdouble.scalar.Angle;
import org.djunits.value.vdouble.scalar.Direction;
import org.djunits.value.vdouble.scalar.Duration;
import org.djunits.value.vdouble.scalar.Length;
import org.djutils.draw.curve.BezierCubic2d;
import org.djutils.draw.curve.Flattener2d;
import org.djutils.draw.curve.Flattener2d.MaxDeviationAndAngle;
import org.djutils.draw.point.DirectedPoint2d;
import org.djutils.draw.point.Point2d;
import org.djutils.exceptions.Throw;
import org.djutils.exceptions.Try;
import org.djutils.math.AngleUtil;
import org.opentrafficsim.base.geometry.OtsLine2d;
import org.opentrafficsim.base.geometry.OtsLine2d.FractionalFallback;
import org.opentrafficsim.core.gtu.plan.operational.Segments;
import org.opentrafficsim.core.network.LateralDirectionality;
import org.opentrafficsim.road.gtu.lane.LaneBasedGtu;
import org.opentrafficsim.road.gtu.lane.LaneBookkeeping;
import org.opentrafficsim.road.network.lane.Lane;
import org.opentrafficsim.road.network.lane.LanePosition;
/**
* Builder for several often used operational plans. E.g., decelerate to come to a full stop at the end of a shape; accelerate
* to reach a certain speed at the end of a curve; drive constant on a curve; decelerate or accelerate to reach a given end
* speed at the end of a curve, etc.<br>
* <p>
* Copyright (c) 2013-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
* BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
* </p>
* @author <a href="https://github.com/averbraeck">Alexander Verbraeck</a>
* @author <a href="https://github.com/peter-knoppers">Peter Knoppers</a>
* @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
*/
public final class LaneOperationalPlanBuilder
{
/** Length within which GTUs snap to the lane center line. */
private static final Length SNAP = Length.ofSI(1e-3);
/** Typical lane width for which the typical lane change duration applies. */
private static final Length LANE_WIDTH = Length.ofSI(3.5);
/** Max angle of numerical simplification. */
private static final double FLATTEN_ANGLE = Math.PI / 360.0;
/** Flattener for paths. */
private static final Flattener2d FLATTENER = new MaxDeviationAndAngle(0.01, FLATTEN_ANGLE);
/** Constructor. */
private LaneOperationalPlanBuilder()
{
// class should not be instantiated
}
/**
* Builds a plan from any position not on a lane, towards a reasonable location on a lane on the route.
* @param gtu GTU
* @param acceleration acceleration
* @param duration time step
* @param tManeuver maneuver time, e.g. lane change
* @return plan from any position not on a lane, to a reasonable location on a lane on the route
* @throws IllegalStateException if the GTU is on a lane
*/
public static LaneBasedOperationalPlan buildRoamingPlan(final LaneBasedGtu gtu, final Acceleration acceleration,
final Duration duration, final Duration tManeuver)
{
gtu.setLaneChangeDirection(LateralDirectionality.NONE);
LanePosition nearestPosition = gtu.getRoamingPosition();
Length deviation = Length.ZERO;
boolean deviative = true;
OtsLine2d path = getPath(gtu, nearestPosition, acceleration, duration, tManeuver, deviation, deviative);
Duration now = gtu.getSimulator().getSimulatorTime();
Segments segments = Segments.off(gtu.getSpeed(), duration, acceleration);
return Try.assign(() -> new LaneBasedOperationalPlan(gtu, path, now, segments, deviative),
"Building roaming plan produced inconsistent LaneBasedOperationalPlan.");
}
/**
* Build operational plan from a simple plan. Sets or resets the GTU lane change direction.
* @param gtu GTU
* @param simplePlan simple operational plan
* @param tManeuver maneuver time, e.g. lane change
* @return plan from position on a lane
* @throws IllegalStateException if the GTU is not on a lane
*/
public static LaneBasedOperationalPlan buildPlanFromSimplePlan(final LaneBasedGtu gtu,
final SimpleOperationalPlan simplePlan, final Duration tManeuver)
{
Throw.when(gtu.getLane() == null, IllegalStateException.class,
"Requested to build plan from simple plan for roaming GTU.");
boolean deviative = false;
LanePosition nearestPosition;
if (simplePlan.isLaneChange())
{
boolean start = gtu.getBookkeeping().equals(LaneBookkeeping.START)
|| (gtu.getBookkeeping().equals(LaneBookkeeping.START_AND_EDGE)
&& gtu.getSpeed().lt(LaneBookkeeping.START_THRESHOLD));
if (gtu.getBookkeeping().equals(LaneBookkeeping.INSTANT) || start)
{
// changes the bookkeeping only, not the position
gtu.changeLaneInstantaneously(simplePlan.getLaneChangeDirection());
nearestPosition = gtu.getPosition();
// only for INSTANT deviative should be false to make a jump to the center line
deviative = start;
}
else
{
gtu.setLaneChangeDirection(simplePlan.getLaneChangeDirection());
deviative = true;
Lane lane = gtu.getPosition().lane().getAdjacentLane(simplePlan.getLaneChangeDirection(), gtu.getType())
.orElseThrow(() -> new IllegalStateException("Starting lane change without adjacent lane."));
double fraction = lane.getCenterLine().projectFractional(lane.getLink().getStartNode().getHeading(),
lane.getLink().getEndNode().getHeading(), gtu.getLocation().x, gtu.getLocation().y,
FractionalFallback.ENDPOINT);
nearestPosition = new LanePosition(lane, lane.getLength().times(fraction));
}
}
else
{
gtu.setLaneChangeDirection(LateralDirectionality.NONE);
nearestPosition = gtu.getPosition();
deviative = deviative || nearestPosition.getLocation().distance(gtu.getLocation()) > SNAP.si;
}
deviative = deviative || simplePlan.getDeviation().si > SNAP.si;
OtsLine2d path = getPath(gtu, nearestPosition, simplePlan.getAcceleration(), simplePlan.getDuration(), tManeuver,
simplePlan.getDeviation(), deviative);
Duration now = gtu.getSimulator().getSimulatorTime();
Segments segments = Segments.off(gtu.getSpeed(), simplePlan.getDuration(), simplePlan.getAcceleration());
boolean finalDeviative = deviative;
return Try.assign(() -> new LaneBasedOperationalPlan(gtu, path, now, segments, finalDeviative),
"Building operational plan produced inconsistent LaneBasedOperationalPlan.");
}
/**
* Returns a path towards a target point that is found by moving along the lanes from the nearest position.
* @param gtu GTU
* @param nearestPosition nearest position, i.e. the the location of the GTU projected to the (target) lane, or some closest
* point during roaming
* @param acceleration acceleration
* @param timeStep time step
* @param tManeuver maneuver time, e.g. lane change
* @param deviation desired deviation from lane center
* @param deviative true if the GTU will not strictly follow the center line
* @return path towards a target point that is found by moving along the lanes from the start position
*/
private static OtsLine2d getPath(final LaneBasedGtu gtu, final LanePosition nearestPosition,
final Acceleration acceleration, final Duration timeStep, final Duration tManeuver, final Length deviation,
final boolean deviative)
{
Length laneGap = Length.ZERO;
HorizonSpace horizonSpace =
getHorizonSpace(gtu, nearestPosition, acceleration, timeStep, tManeuver, deviation, laneGap);
if (deviative)
{
return bezierToHorizon(gtu, nearestPosition, deviation, horizonSpace);
}
return getCenterLinePath(gtu, nearestPosition, horizonSpace.horizon(), acceleration, timeStep, tManeuver, deviation);
}
/**
* Create path as a Bezier to a target point that will be on the horizon.
* @param gtu GTU
* @param nearestPosition nearest position, i.e. the the location of the GTU projected to the (target) lane, or some closest
* point during roaming
* @param deviation desired deviation from lane center
* @param horizonSpace information regarding the considered horizon
* @return path as a Bezier to a target point that will be on the horizon
*/
private static OtsLine2d bezierToHorizon(final LaneBasedGtu gtu, final LanePosition nearestPosition, final Length deviation,
final HorizonSpace horizonSpace)
{
DirectedPoint2d target = getTargetPoint(gtu, nearestPosition, deviation, horizonSpace);
target = extrapolateToHorizon(gtu, target, horizonSpace.horizon());
return bezierToTarget(gtu, target);
}
/**
* Returns a path constructed from lane center lines. If a gap between lanes is found, this method will revert back to a
* deviative path. The horizon is then recalculated including the found lane gap. A path to a target point on the horizon is
* then returned.
* @param gtu GTU
* @param nearestPosition start position
* @param length minimum
* @param acceleration acceleration
* @param timeStep time step
* @param tManeuver maneuver time, e.g. lane change
* @param deviation desired deviation from lane center
* @return path constructed from lane center lines, or a bezier path in case of a lane gap between center lines
*/
private static OtsLine2d getCenterLinePath(final LaneBasedGtu gtu, final LanePosition nearestPosition, final Length length,
final Acceleration acceleration, final Duration timeStep, final Duration tManeuver, final Length deviation)
{
Length startDistance = nearestPosition.position();
Lane lane = nearestPosition.lane();
List<Point2d> points = new ArrayList<>();
Point2d lastPoint = null;
double cumulDist = 0.0;
while (lane != null)
{
OtsLine2d centerLine =
startDistance.gt0() ? lane.getCenterLine().extract(startDistance, lane.getLength()) : lane.getCenterLine();
if (lastPoint != null && lastPoint.distance(centerLine.getFirst()) > SNAP.si)
{
// It is not appropriate to follow the lane center lines due to a lane gap
Length laneGap = Length.ofSI(lastPoint.distance(centerLine.getFirst()));
HorizonSpace horizonSpace =
getHorizonSpace(gtu, nearestPosition, acceleration, timeStep, tManeuver, deviation, laneGap);
return bezierToHorizon(gtu, nearestPosition, deviation, horizonSpace);
}
for (Point2d point : centerLine)
{
if (lastPoint != null)
{
double d = lastPoint.distance(point);
if (d < SNAP.si)
{
continue;
}
cumulDist += d;
}
points.add(point);
if (cumulDist >= length.si)
{
return new OtsLine2d(points);
}
lastPoint = point;
}
startDistance = Length.ZERO;
lane = gtu.getNextLaneForRoute(lane).orElse(null);
}
// Minimum length not reached, add extrapolated point to reach required length
Point2d lastLastPoint = points.get(points.size() - 2);
double direction = lastLastPoint.directionTo(lastPoint);
double r = length.si - cumulDist + SNAP.si;
points.add(lastPoint.translate(r * Math.cos(direction), r * Math.sin(direction)));
return new OtsLine2d(points);
}
/**
* Returns relevant information regarding the considered horizon. The horizon (radius) is determined as:
* <ul>
* <li>At least the length of the operational plan</li>
* <li>At least the GTU turn radius (=diameter)</li>
* <li>At least several factors on tManeuver at the current speed:
* <ul>
* <li>[0...1] for a deviation from the desired deviation of [0...3.5]m, or 1 for larger deviation</li>
* <li>[0...1] for an angle of [0...pi/4] between the direction of the GTU and the direction at the target, or 1 for larger
* angles</li>
* <li>[2...0] for an angle of [0...pi/2] between the direction of the GTU and the direction towards the target, or 0 for
* larger angles</li>
* </ul>
* </li>
* </ul>
* The bullet on deviation implements a typical lane change horizon.<br>
* The before-last bullet reflects that maneuvers take more length when the GTU is at an angle.<br>
* The last bullet reflects roaming situations where the GTU is approaching the target at a near-right angle, in which case
* more rotation is required than a normal maneuver, and hence a factor larger than 1 is applied.
* @param gtu GTU
* @param nearestPosition nearest position, i.e. the location of the GTU projected to the (target) lane, or some closest
* point during roaming
* @param acceleration acceleration
* @param timeStep time step
* @param tManeuver maneuver time, e.g. lane change
* @param deviation desired deviation from lane center
* @param laneGap known gap between longitudinally connected lanes
* @return information regarding the considered horizon
*/
private static HorizonSpace getHorizonSpace(final LaneBasedGtu gtu, final LanePosition nearestPosition,
final Acceleration acceleration, final Duration timeStep, final Duration tManeuver, final Length deviation,
final Length laneGap)
{
DirectedPoint2d nearestPoint = nearestPosition.getLocation();
double dx = nearestPoint.x - gtu.getLocation().x;
double dy = nearestPoint.y - gtu.getLocation().y;
double dCenterLine = Math.hypot(dx, dy);
double leftOfLane;
if (dCenterLine <= SNAP.si)
{
leftOfLane = 1.0;
}
else if (dy == 0.0)
{
leftOfLane = -Math.signum(dx);
}
else if (dx == 0.0)
{
leftOfLane = -Math.signum(dy);
}
else
{
leftOfLane = Math.signum(Math.cos(Math.sin(nearestPoint.dirZ) * dx - nearestPoint.dirZ) * dy);
}
Length distanceToNearest = Length.ofSI(Math.abs(deviation.si - leftOfLane * dCenterLine));
// Lateral deviation: 0 to 1 within first {LANE_WIDTH}m (also applies to lane gap)
double fLatDeviation = Math.min(1.0, Math.max(distanceToNearest.si, laneGap.si) / LANE_WIDTH.si);
// Difference direction at target point and vehicle direction: 0 to 1 within pi/4
double dDirection = Math.abs(AngleUtil.normalizeAroundZero(nearestPoint.dirZ - gtu.getLocation().dirZ));
double fDirection = Math.min(1, 4 * dDirection / Math.PI);
// Difference direction to target and vehicle direction: 2 to 0 within pi/2
// For these maneuvers more time is required than a normal lane change
Direction dirToNearest = Direction.ofSI(Math.atan2(dy, dx));
double fToTarget = 0.0;
if (dCenterLine > SNAP.si)
{
double dToTarget = Math.abs(AngleUtil.normalizeAroundZero(dirToNearest.si - gtu.getLocation().dirZ));
fToTarget = Math.max(0, 2 - 4 * dToTarget / Math.PI);
}
// Combine factors
double tHorizon = tManeuver.si * Math.max(Math.max(fLatDeviation, fDirection), fToTarget);
// Figure out horizon
Length turnDiameter = gtu.getVehicleModel().getTurnRadius(gtu);
double rPlan;
if (acceleration.lt0() && gtu.getSpeed().si / -acceleration.si < timeStep.si)
{
double t = gtu.getSpeed().si / -acceleration.si;
rPlan = gtu.getSpeed().si * t + .5 * acceleration.si * t * t;
}
else
{
rPlan = gtu.getSpeed().si * timeStep.si + .5 * acceleration.si * timeStep.si * timeStep.si;
}
Length horizon = Length.ofSI(Math.max(Math.max(turnDiameter.si, rPlan), gtu.getSpeed().si * tHorizon));
return new HorizonSpace(distanceToNearest, dirToNearest, horizon, turnDiameter, nearestPoint);
}
/**
* Hold information on the horizon.
* @param distanceToNearest distance towards the nearest point
* @param dirToNearest distance to the nearest point
* @param horizon horizon radius
* @param turnDiameter GTU turn radius (=diameter)
* @param nearestPoint nearest point, e.g. GTU position projected to (target) lane
*/
private record HorizonSpace(Length distanceToNearest, Direction dirToNearest, Length horizon, Length turnDiameter,
DirectedPoint2d nearestPoint)
{
}
/**
* Returns a target point that is found by moving along the lanes from the nearest position. The general procedure is:
* <ul>
* <li>Far (closest point on lane is beyond horizon)</li>
* <ul>
* <li>Ahead (closest point on lane is within pi/4 of straight ahead)</li>
* <ul>
* <li>go to closest point on lane</li>
* </ul>
* <li>Behind</li>
* <ul>
* <li>turn to horizon edge closest to closest point on lane</li>
* </ul>
* </ul>
* <li>Close</li>
* <ul>
* <li>Intersect (horizon intersects lane within pi/4* of straight ahead)</li>
* <ul>
* <li>go to point where horizon intersects lane</li>
* </ul>
* <li>Prevent turn flipping (horizon edges closest to same point on lane)</li>
* <ul>
* <li>go to point on lane closest to either horizon edge</li>
* </ul>
* <li>Turn</li>
* <ul>
* <li>turn to horizon edge closest to lane</li>
* </ul>
* </ul>
* </ul>
* *) or narrower when limited by vehicle turning radius
* @param gtu GTU
* @param nearestPosition nearest position, i.e. the the location of the GTU projected to the (target) lane, or some closest
* point during roaming
* @param deviation desired deviation from lane center
* @param horizonSpace information on horizon
* @return target point that is found by moving along the lanes from the nearest position
*/
private static DirectedPoint2d getTargetPoint(final LaneBasedGtu gtu, final LanePosition nearestPosition,
final Length deviation, final HorizonSpace horizonSpace)
{
if (horizonSpace.distanceToNearest().gt(horizonSpace.horizon()))
{
// Far away: lane is beyond horizon, go to closest point
if (Math.abs(horizonSpace.dirToNearest().si - gtu.getLocation().dirZ) < Math.PI / 4.0)
{
// Closest point is ahead, use direction but limit to horizon for reasonable path curvature
return new DirectedPoint2d(
gtu.getLocation().x + horizonSpace.horizon().si * Math.cos(horizonSpace.dirToNearest().si),
gtu.getLocation().y + horizonSpace.horizon().si * Math.sin(horizonSpace.dirToNearest().si),
horizonSpace.dirToNearest().si);
}
else
{
// Closest point is behind, go to edge of the horizon (left or right) that is closest to closest point on lane
double alpha = Math.PI / 2.0;
double alphaMin = gtu.getLocation().dirZ - alpha;
double alphaMax = gtu.getLocation().dirZ + alpha;
double x1 = gtu.getLocation().x + horizonSpace.horizon().si * Math.cos(alphaMin);
double y1 = gtu.getLocation().y + horizonSpace.horizon().si * Math.sin(alphaMin);
Point2d nearest1 = nearestPosition.lane().getCenterLine().closestPointOnPolyLine(new Point2d(x1, y1));
double dist1sq = Math.hypot(x1 - nearest1.x, y1 - nearest1.y);
double x2 = gtu.getLocation().x + horizonSpace.horizon().si * Math.cos(alphaMax);
double y2 = gtu.getLocation().y + horizonSpace.horizon().si * Math.sin(alphaMax);
Point2d nearest2 = nearestPosition.lane().getCenterLine().closestPointOnPolyLine(new Point2d(x2, y2));
double dist2sq = Math.hypot(x2 - nearest2.x, y2 - nearest2.y);
if (nearest1.directionTo(nearest2) < SNAP.si)
{
// Same point, let's not flip left/right each step, but just go there
double dirToAdjustedTarget = Math.atan2(nearest1.y - gtu.getLocation().y, nearest1.x - gtu.getLocation().x);
return new DirectedPoint2d(nearest1, dirToAdjustedTarget);
}
if (dist1sq <= dist2sq)
{
return new DirectedPoint2d(x1, y1, gtu.getLocation().dirZ + Math.PI);
}
return new DirectedPoint2d(x2, y2, gtu.getLocation().dirZ - Math.PI);
}
}
// Close(ish): go to point where horizon intersects lane
double alpha = Math.PI / 4.0;
double rVehicle = horizonSpace.turnDiameter().si;
double rHorizon = horizonSpace.horizon().si;
if (rVehicle > .5 * rHorizon)
{
// Not the whole horizon might be reachable due to turn radius
/* {@formatter:off}
* Intersection of 2 circles to find max horizon angle (alpha|a)
* 1) circle with radius rHorizon around (0, 0) = A
* 2) circle with radius rVehicle around (-rVehicle, 0) = B
* Intersection P creates triangle with circle centers A and B.
* Arc A-P is how the vehicle can turn towards horizon at P.
* Side A-P of length rHorizon is split at mid-point "#".
* A new line B-# creates two right triangles /\#-B-P & /\#-B-A.
* Line B-# has length "z". To find alpha:
* - Note that /_P-A-B is 90deg - a. Angle /_#-B-A is 90deg
* minus /_P-A-B, and therefore /_#-B-A = a.
* - From this: a = arctan(.5 * rHorizon / z) =>
* z = .5 * rHorizon / tan(a)
* - Pythagoras gives: z = sqrt(rVehicle^2 - (.5 * rHorizon)^2)
* - Equating and solving for a, simplifying fraction in tan by
* multiplying numerator and denominator by 2=sqrt(4), gives:
*
* a = arctan(rHorizon / sqrt(4 * rVehicle^2 - rHorizon^2))
*
* T | | <--"ahead" in plane of vehicle
* '.2a| |
* '.P-''|''-..
* rVehicle .-' \ |rHorizon
* .-'/ rHor#a| \
* .-' / \| \ <--horizon at rHorizon
* B---------------A |
* ^ rVehicle (0, 0) /
* | \ /
* turn radius '. .'
* '-..___..-'
*
* {@formatter:on}
*/
alpha = Math.min(alpha, Math.atan(rHorizon / Math.sqrt(4 * rVehicle * rVehicle - rHorizon * rHorizon)));
}
// Return intersection of lane path and horizon
LanePosition endPosition = getTargetLanePosition(gtu, nearestPosition, horizonSpace.horizon(), Angle.ofSI(alpha));
if (endPosition != null)
{
return endPosition.getLocation();
}
// Make turn
double alphaMin = gtu.getLocation().dirZ - alpha;
double alphaMax = gtu.getLocation().dirZ + alpha;
Point2d pMin = new Point2d(gtu.getLocation().x + rHorizon * Math.cos(alphaMin),
gtu.getLocation().y + rHorizon * Math.sin(alphaMin));
Point2d nearest1 = nearestPosition.lane().getCenterLine().closestPointOnPolyLine(pMin);
double dist1sq = nearest1.distance(pMin);
Point2d pMax = new Point2d(gtu.getLocation().x + rHorizon * Math.cos(alphaMax),
gtu.getLocation().y + rHorizon * Math.sin(alphaMax));
Point2d nearest2 = nearestPosition.lane().getCenterLine().closestPointOnPolyLine(pMax);
double dist2sq = nearest2.distance(pMax);
if (nearest1.distance(nearest2) < SNAP.si)
{
// Same point, let's not flip left/right every step, but just go there
return new DirectedPoint2d(nearest1, gtu.getLocation().directionTo(nearest1));
}
else if (dist1sq <= dist2sq)
{
/*
* 2 * alpha is added or subtracted. This is to obtain the direction of line line P-T in the figure above, which is
* the same as the angle of the arc A-P at P. The arc hits P at an angle 'a' with the line A-P (symmetry). The line
* A-P is also at an angle 'a' relative to vertical. Hence, line P-T makes an angle of 2a as this is the opposite
* corner between vertical and P-T.
*/
return new DirectedPoint2d(nearest1, gtu.getLocation().dirZ - 2 * alpha);
}
return new DirectedPoint2d(nearest2, gtu.getLocation().dirZ + 2 * alpha);
}
/**
* Returns the target position by following lanes from the start position until a point is found that is at the horizon
* distance removed from the GTU. If such a point is not within the viewport, {@code null} is returned. This method is part
* of {@code getTargetPoint()}.
* @param gtu GTU
* @param startPosition start position of path, which should be the projected location on an adjacent lane for a lane change
* @param horizon distance as the crow flies of the next target point
* @param viewport horizontal angle within which the horizon is considered, relative to straight ahead, both left and right
* @return lane path for a movement step of a GTU, {@code null} if no such point within viewport
*/
private static LanePosition getTargetLanePosition(final LaneBasedGtu gtu, final LanePosition startPosition,
final Length horizon, final Angle viewport)
{
DirectedPoint2d loc0 = gtu.getLocation();
OtsLine2d laneCenter =
startPosition.lane().getCenterLine().extract(startPosition.position(), startPosition.lane().getLength());
Lane lane = startPosition.lane();
Set<Lane> coveredLanes = new LinkedHashSet<>();
double r2 = horizon.si * horizon.si;
double distCumulLane = startPosition.position().si;
while (!coveredLanes.contains(lane))
{
coveredLanes.add(lane);
// If the first point on the next lane is beyond the horizon, the horizon is in a gap between two lanes. The first
// point of the next lane can be considered the focus point.
if (Math.hypot(laneCenter.getFirst().x - loc0.x, laneCenter.getFirst().y - loc0.y) > horizon.si)
{
double alpha = Math.atan2(laneCenter.getFirst().y - loc0.y, laneCenter.getFirst().x - loc0.x) - loc0.dirZ;
return Math.abs(alpha) <= viewport.si ? new LanePosition(lane, Length.ZERO) : null;
}
// Find horizon point on lane
for (int i = 0; i < laneCenter.size() - 1; i++)
{
double dx = laneCenter.getX(i + 1) - laneCenter.getX(i);
double dy = laneCenter.getY(i + 1) - laneCenter.getY(i);
double dr = Math.hypot(dx, dy);
// Check if line segment crosses circle (method from https://mathworld.wolfram.com/Circle-LineIntersection.html)
double det = (laneCenter.getX(i) - loc0.x) * (laneCenter.getY(i + 1) - loc0.y)
- (laneCenter.getX(i + 1) - loc0.x) * (laneCenter.getY(i) - loc0.y);
double dr2 = dr * dr;
double det2 = det * det;
double discriminant = r2 * dr2 - det2;
if (discriminant >= 0.0)
{
double sgn = dy < 0.0 ? -1.0 : 1.0;
double sqrtDisc = Math.sqrt(discriminant);
// Up to two crossing points
double pointSign = 1.0;
for (int j = 0; j < 2; j++)
{
double xP = loc0.x + (det * dy + pointSign * sgn * dx * sqrtDisc) / dr2;
double yP = loc0.y + (-det * dx + pointSign * Math.abs(dy) * sqrtDisc) / dr2;
double alpha = AngleUtil.normalizeAroundZero(Math.atan2(yP - loc0.y, xP - loc0.x) - loc0.dirZ);
double f = Math.abs(dx) > 0.0 && Math.abs(dx) > Math.abs(dy) ? (xP - laneCenter.getX(i)) / dx
: (dy == 0.0 ? 0.0 : (yP - laneCenter.getY(i)) / dy);
// Crosses within line segment?
if (f >= 0.0 && f <= 1.0)
{
// In viewing port?
if (Math.abs(alpha) <= viewport.si)
{
return new LanePosition(lane, Length.ofSI(distCumulLane + f * dr));
}
else
{
// Crossing with center line outside of viewport (could be turn radius). Increase horizon and
// try again but with full default viewport.
return getTargetLanePosition(gtu, startPosition, horizon.times(2.0), Angle.ofSI(Math.PI / 4.0));
}
}
pointSign = -1.0;
}
}
distCumulLane += dr;
}
// Move to next lane
Optional<Lane> nextLane = gtu.getNextLaneForRoute(lane);
if (nextLane.isEmpty())
{
return new LanePosition(lane, Length.ofSI(startPosition.lane().getCenterLine().getLength()));
}
lane = nextLane.get();
laneCenter = lane.getCenterLine();
distCumulLane = 0.0;
}
// Encountered a loop, return last position if within alpha
double alpha = AngleUtil
.normalizeAroundZero(Math.atan2(laneCenter.getLast().y - loc0.y, laneCenter.getLast().x - loc0.x) - loc0.dirZ);
return Math.abs(alpha) <= viewport.si ? new LanePosition(lane, lane.getLength()) : null;
}
/**
* Extrapolate target point further in its direction up to horizon, if it is closer than horizon. This can happen if the end
* of a route is reached.
* @param gtu GTU
* @param target target point
* @param horizon horizon
* @return extrapolated target point further in its direction up to horizon
*/
private static DirectedPoint2d extrapolateToHorizon(final LaneBasedGtu gtu, final DirectedPoint2d target,
final Length horizon)
{
double dx = target.x - gtu.getLocation().x;
double dy = target.y - gtu.getLocation().y;
double dist = Math.hypot(dx, dy);
if (dist >= horizon.si)
{
return target;
}
/*
* {@formatter:off}
* Relative to vehicle (A), we need a point P on the horizon at an
* angle 'a'. This point is 'h' extrapolated beyond target (B) at
* (dx, dy) at angle target.dirZ.
* x = dx + h * cos(target.dirZ) = horizon * cos(a) [1]
* y = dy + h * sin(target.dirZ) = horizon * sin(a) [2]
* Solving a = f(...) for [2] and substituting in [1], and
* solving this for h, gives a large equation with two solutions.
*
* ..--''' <-- Horizon at horizon from A
* target.dirZ .-'h
* <-------P------B (dx, dy) <-- target
* (x, y)' ''-. a\
* | ''-.A <-- vehicle
* . (0, 0)
* {@formatter:on}
*/
double cosTarget = Math.cos(target.dirZ);
double sinTarget = Math.sin(target.dirZ);
double c = cosTarget * dx;
double s = sinTarget * dy;
double d = Math.sqrt(
-cosTarget * cosTarget * dy * dy + 2.0 * c * s - sinTarget * sinTarget * dx * dx + horizon.si * horizon.si);
double x = Math.max(-c - s - d, -c - s + d); // positive solution
return new DirectedPoint2d(target.x + x * cosTarget, target.y + x * sinTarget, target.dirZ);
}
/**
* Create Bezier path from current location of the GTU towards the target point. The path is flattened with a default
* flattener using a maximum deviation of 0.1m and maximum angle 0.5 degrees.
* @param gtu GTU
* @param target target point
* @return Bezier path from current location of the GTU towards the target point
*/
private static OtsLine2d bezierToTarget(final LaneBasedGtu gtu, final DirectedPoint2d target)
{
double angleShift = Math.abs(AngleUtil.normalizeAroundZero(gtu.getLocation().dirZ - target.dirZ));
double dirToTarget = gtu.getLocation().directionTo(target);
if (angleShift < FLATTEN_ANGLE && Math.abs(AngleUtil.normalizeAroundZero(dirToTarget - target.dirZ)) < FLATTEN_ANGLE
&& Math.abs(AngleUtil.normalizeAroundZero(dirToTarget - gtu.getLocation().dirZ)) < FLATTEN_ANGLE)
{
// current position and direction sufficiently in line with target position and direction to simplify as straight
return new OtsLine2d(gtu.getLocation(), target);
}
// Shape points at shapeFactor of inter-point distance:
// 1/3rd when angle between points < pi/2
// then increases linearly to 2/3rds for an angle of pi
double shapeFactor = (1.0 + 2.0 * Math.max(0.0, angleShift - 0.5 * Math.PI) / Math.PI) / 3;
double rControl = shapeFactor * Math.hypot(gtu.getLocation().x - target.x, gtu.getLocation().y - target.y);
Point2d p2 = new Point2d(gtu.getLocation().x + rControl * Math.cos(gtu.getLocation().dirZ),
gtu.getLocation().y + rControl * Math.sin(gtu.getLocation().dirZ));
Point2d p3 = new Point2d(target.x - rControl * Math.cos(target.dirZ), target.y - rControl * Math.sin(target.dirZ));
BezierCubic2d bezier = new BezierCubic2d(gtu.getLocation(), p2, p3, target);
return new OtsLine2d(bezier.toPolyLine(FLATTENER));
}
}