LaneStructure.java
package org.opentrafficsim.road.gtu.lane.perception;
import java.io.Serializable;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Iterator;
import java.util.Map;
import java.util.Set;
import java.util.SortedSet;
import java.util.TreeMap;
import java.util.TreeSet;
import org.djunits.value.vdouble.scalar.Length;
import org.opentrafficsim.core.gtu.GTU;
import org.opentrafficsim.core.gtu.GTUException;
import org.opentrafficsim.core.gtu.RelativePosition;
import org.opentrafficsim.core.network.route.Route;
import org.opentrafficsim.road.network.lane.CrossSectionLink;
import org.opentrafficsim.road.network.lane.Lane;
import org.opentrafficsim.road.network.lane.object.LaneBasedObject;
import nl.tudelft.simulation.language.Throw;
/**
* This data structure can clearly indicate the lane structure ahead of us, e.g. in the following situation:
*
* <pre>
* (---- a ----)(---- b ----)(---- c ----)(---- d ----)(---- e ----)(---- f ----)(---- g ----)
* __________ __________
* / _________ 1 / _________ 2
* / / / /
* __________/ / _______________________/ /
* 1 ____________ ____________ /_ _ _ _ _ _/____________ /_ _ _ _ _ _ _ _ _ _ _ _ /
* 0 |_ _X_ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ \____________
* -1 |____________|_ _ _ _ _ _ |____________|____________| __________|____________|____________| 3
* -2 / __________/ \ \
* ________/ / \ \___________
* 5 _________/ \____________ 4
* </pre>
*
* When the GTU is looking ahead, it needs to know that when it continues to destination 3, it needs to shift one lane to the
* right at some point, but <b>not</b> two lanes to the right in link b, and not later than at the end of link f. When it needs
* to go to destination 1, it needs to shift to the left in link c. When it has to go to destination 2, it has to shift to the
* left, but not earlier than at link e. At node [de], it is possible to leave the rightmost lane of link e, and go to
* destination 4. The rightmost lane just splits into two lanes at the end of link d, and the GTU can either continue driving to
* destination 3, turn right to destination 4. This means that the right lane of link d has <b>two</b> successor lanes.
* <p>
* In the data structures, lanes are numbered laterally. Suppose that the lane where vehicle X resides would be number 0.
* Consistent with "left is positive" for angles, the lane right of X would have number -1, and entry 5 would have number -2.
* <p>
* In the data structure, this can be indicated as follows (N = next, P = previous, L = left, R = right, D = lane drop, . =
* continued but not in this structure). The merge lane in b is considered "off limits" for the GTUs on the "main" lane -1; the
* "main" lane 0 is considered off limits from the exit lanes on c, e, and f. Still, we need to maintain pointers to these
* lanes, as we are interested in the GTUs potentially driving next to us, feeding into our lane, etc.
*
* <pre>
* 1 0 -1 -2
*
* ROOT
* _____|_____ ___________ ___________
* |_-_|_._|_R_|----|_L_|_._|_-_| |_-_|_._|_-_| a
* | | |
* _____V_____ _____V_____ _____V_____
* |_-_|_N_|_R_|----|_L_|_N_|_R_|<---|_L_|_D_|_-_| b
* | |
* ___________ _____V_____ _____V_____
* |_-_|_N_|_R_|<---|_L_|_N_|_R_|----|_L_|_N_|_-_| c
* | | |
* _____V_____ _____V_____ _____V_____
* |_-_|_._|_-_| |_-_|_N_|_R_|----|_L_|_NN|_-_| d
* | ||_______________
* ___________ _____V_____ _____V_____ _____V_____
* |_-_|_N_|_R_|<---|_L_|_N_|_R_|----|_L_|_N_|_-_| |_-_|_N_|_-_| e
* | | | |
* _____V_____ _____V_____ _____V_____ _____V_____
* |_-_|_N_|_R_|<---|_L_|_D_|_R_|----|_L_|_N_|_-_| |_-_|_._|_-_| f
* | |
* _____V_____ _____V_____
* |_-_|_._|_-_| |_-_|_._|_-_| g
* </pre>
* <p>
* Copyright (c) 2013-2016 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
* BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
* </p>
* $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
* initial version Feb 20, 2016 <br>
* @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
*/
public class LaneStructure implements Serializable
{
/** */
private static final long serialVersionUID = 20160400L;
/** The lanes from which we observe the situation. */
private final LaneStructureRecord rootLSR;
/** Look ahead distance. */
private Length lookAhead;
/** Lane structure records of the cross section. */
private TreeMap<RelativeLane, LaneStructureRecord> crossSectionRecords = new TreeMap<>();
/** Lane structure records grouped per relative lane. */
private final Map<RelativeLane, Set<LaneStructureRecord>> relativeLaneMap = new HashMap<>();
/**
* @param rootLSR the root record.
* @param lookAhead look ahead distance
*/
public LaneStructure(final LaneStructureRecord rootLSR, final Length lookAhead)
{
this.rootLSR = rootLSR;
this.lookAhead = lookAhead;
}
/**
* @return rootLSR
*/
public final LaneStructureRecord getRootLSR()
{
return this.rootLSR;
}
/**
* Returns the cross section.
* @return cross section
*/
public final SortedSet<RelativeLane> getCrossSection()
{
return this.crossSectionRecords.navigableKeySet();
}
/**
* @param lane lane to check
* @return record at given lane
* @throws GTUException if the lane is not in the cross section
*/
public final LaneStructureRecord getLaneLSR(final RelativeLane lane) throws GTUException
{
Throw.when(!this.crossSectionRecords.containsKey(lane), GTUException.class,
"The requested lane %s is not in the most recent cross section.", lane);
return this.crossSectionRecords.get(lane);
}
/**
* Removes all mappings to relative lanes that are not in the most recent cross section.
* @param map map to clear mappings from
*/
public final void removeInvalidMappings(final Map<RelativeLane, ?> map)
{
Iterator<RelativeLane> iterator = map.keySet().iterator();
while (iterator.hasNext())
{
RelativeLane lane = iterator.next();
if (!this.crossSectionRecords.containsKey(lane))
{
iterator.remove();
}
}
}
/**
* Adds a lane structure record in a mapping from relative lanes. The record is also added to the current cross section if
* the start distance is negative, and the start distance plus length is positive. If the relative lane is already in the
* current cross section, it is <b>not</b> overwritten.
* @param lsr lane structure record
* @param relativeLane relative lane
*/
public final void addLaneStructureRecord(final LaneStructureRecord lsr, final RelativeLane relativeLane)
{
if (!this.relativeLaneMap.containsKey(relativeLane))
{
this.relativeLaneMap.put(relativeLane, new HashSet<>());
}
this.relativeLaneMap.get(relativeLane).add(lsr);
if (lsr.getStartDistance().le(Length.ZERO) && lsr.getStartDistance().plus(lsr.getLane().getLength()).ge(Length.ZERO)
&& !this.crossSectionRecords.containsKey(relativeLane))
{
this.crossSectionRecords.put(relativeLane, lsr);
}
}
/**
* Retrieve objects on a lane of a specific type. Returns objects over a maximum length of the look ahead distance
* downstream from the relative position, or as far as the lane map goes.
* @param lane lane
* @param clazz class of objects to find
* @param gtu gtu
* @param pos relative position to start search from
* @param <T> type of objects to find
* @return Sorted set of objects of requested type
* @throws GTUException if lane is not in current set
*/
@SuppressWarnings("unchecked")
public final <T extends LaneBasedObject> SortedSet<Entry<T>> getDownstreamObjects(final RelativeLane lane,
final Class<T> clazz, final GTU gtu, final RelativePosition.TYPE pos) throws GTUException
{
LaneStructureRecord record = this.getLaneLSR(lane);
Length ds = gtu.getRelativePositions().get(pos).getDx().minus(gtu.getReference().getDx());
// the list is ordered, but only for DIR_PLUS, need to do our own ordering
Length minimumPosition;
Length maximumPosition;
if (record.getDirection().isPlus())
{
minimumPosition = ds.minus(record.getStartDistance());
maximumPosition = record.getLane().getLength();
}
else
{
minimumPosition = Length.ZERO;
maximumPosition = record.getLane().getLength().plus(record.getStartDistance()).minus(ds);
}
SortedSet<Entry<T>> set = new TreeSet<>();
Length distance;
for (LaneBasedObject object : record.getLane().getLaneBasedObjects(minimumPosition, maximumPosition))
{
distance = record.getDistanceToPosition(object.getLongitudinalPosition()).minus(ds);
if (clazz.isAssignableFrom(object.getClass()) && distance.le(this.lookAhead)
&& ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
|| (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
{
// unchecked, but the above isAssignableFrom assures correctness
set.add(new Entry<>(distance, (T) object));
}
}
getDownstreamObjectsRecursive(set, record, clazz, ds);
return set;
}
/**
* Retrieve objects on a lane of a specific type. Returns objects over a maximum length of the look ahead distance
* downstream from the relative position, or as far as the lane map goes. Objects on links not on the route are ignored.
* @param lane lane
* @param clazz class of objects to find
* @param gtu gtu
* @param pos relative position to start search from
* @param <T> type of objects to find
* @param route the route
* @return Sorted set of objects of requested type
* @throws GTUException if lane is not in current set
*/
public final <T extends LaneBasedObject> SortedSet<Entry<T>> getDownstreamObjectsOnRoute(final RelativeLane lane,
final Class<T> clazz, final GTU gtu, final RelativePosition.TYPE pos, final Route route) throws GTUException
{
SortedSet<Entry<T>> set = getDownstreamObjects(lane, clazz, gtu, pos);
if (route != null)
{
Iterator<Entry<T>> iterator = set.iterator();
while (iterator.hasNext())
{
Entry<T> entry = iterator.next();
CrossSectionLink link = entry.getLaneBasedObject().getLane().getParentLink();
if (!route.contains(link.getStartNode()) || !route.contains(link.getEndNode())
|| Math.abs(route.indexOf(link.getStartNode()) - route.indexOf(link.getEndNode())) != 1)
{
iterator.remove();
}
}
}
return set;
}
/**
* Recursive search for lane based objects downstream.
* @param set set to store entries into
* @param record current record
* @param clazz class of objects to find
* @param ds distance from reference to chosen relative position
* @param <T> type of objects to find
*/
@SuppressWarnings("unchecked")
private <T extends LaneBasedObject> void getDownstreamObjectsRecursive(final SortedSet<Entry<T>> set,
final LaneStructureRecord record, final Class<T> clazz, final Length ds)
{
if (record.getNext().isEmpty() || record.getNext().get(0).getStartDistance().gt(this.lookAhead))
{
return;
}
for (LaneStructureRecord next : record.getNext())
{
Length distance;
for (LaneBasedObject object : next.getLane().getLaneBasedObjects())
{
distance = next.getDistanceToPosition(object.getLongitudinalPosition()).minus(ds);
if (clazz.isAssignableFrom(object.getClass()) && distance.le(this.lookAhead)
&& ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
|| (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
{
// unchecked, but the above isAssignableFrom assures correctness
set.add(new Entry<>(distance, (T) object));
}
}
getDownstreamObjectsRecursive(set, next, clazz, ds);
}
}
/**
* Retrieve objects on a lane of a specific type. Returns upstream objects from the relative position for as far as the lane
* map goes. Distances to upstream objects are given as positive values.
* @param lane lane
* @param clazz class of objects to find
* @param gtu gtu
* @param pos relative position to start search from
* @param <T> type of objects to find
* @return Sorted set of objects of requested type
* @throws GTUException if lane is not in current set
*/
@SuppressWarnings("unchecked")
public final <T extends LaneBasedObject> SortedSet<Entry<T>> getUpstreamObjects(final RelativeLane lane,
final Class<T> clazz, final GTU gtu, final RelativePosition.TYPE pos) throws GTUException
{
LaneStructureRecord record = this.getLaneLSR(lane);
Length ds = gtu.getReference().getDx().minus(gtu.getRelativePositions().get(pos).getDx());
// the list is ordered, but only for DIR_PLUS, need to do our own ordering
Length minimumPosition;
Length maximumPosition;
if (record.getDirection().isPlus())
{
minimumPosition = Length.ZERO;
maximumPosition = record.getStartDistance().neg().minus(ds);
}
else
{
minimumPosition = record.getLane().getLength().plus(record.getStartDistance()).plus(ds);
maximumPosition = record.getLane().getLength();
}
SortedSet<Entry<T>> set = new TreeSet<>();
Length distance;
for (LaneBasedObject object : record.getLane().getLaneBasedObjects(minimumPosition, maximumPosition))
{
if (clazz.isAssignableFrom(object.getClass())
&& ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
|| (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
{
distance = record.getDistanceToPosition(object.getLongitudinalPosition()).neg().minus(ds);
// unchecked, but the above isAssignableFrom assures correctness
set.add(new Entry<>(distance, (T) object));
}
}
getUpstreamObjectsRecursive(set, record, clazz, ds);
return set;
}
/**
* Recursive search for lane based objects upstream.
* @param set set to store entries into
* @param record current record
* @param clazz class of objects to find
* @param ds distance from reference to chosen relative position
* @param <T> type of objects to find
*/
@SuppressWarnings("unchecked")
private <T extends LaneBasedObject> void getUpstreamObjectsRecursive(final SortedSet<Entry<T>> set,
final LaneStructureRecord record, final Class<T> clazz, final Length ds)
{
for (LaneStructureRecord prev : record.getPrev())
{
Length distance;
for (LaneBasedObject object : prev.getLane().getLaneBasedObjects())
{
if (clazz.isAssignableFrom(object.getClass())
&& ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
|| (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
{
distance = prev.getDistanceToPosition(object.getLongitudinalPosition()).neg().minus(ds);
// unchecked, but the above isAssignableFrom assures correctness
set.add(new Entry<>(distance, (T) object));
}
}
getUpstreamObjectsRecursive(set, prev, clazz, ds);
}
}
/** {@inheritDoc} */
@Override
public final String toString()
{
return "LaneStructure [rootLSR=" + this.rootLSR + "]";
}
/**
* <p>
* Copyright (c) 2013-2016 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
* <br>
* BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
* <p>
* @version $Revision$, $LastChangedDate$, by $Author$, initial version Sep 15, 2016 <br>
* @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
* @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
* @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
* @param <T> class of lane based object contained
*/
public static class Entry<T extends LaneBasedObject> implements Comparable<Entry<T>>
{
/** Distance to lane based object. */
private final Length distance;
/** Lane based object. */
private final T laneBasedObject;
/**
* @param distance distance to lane based object
* @param laneBasedObject lane based object
*/
public Entry(final Length distance, final T laneBasedObject)
{
this.distance = distance;
this.laneBasedObject = laneBasedObject;
}
/**
* @return distance.
*/
public final Length getDistance()
{
return this.distance;
}
/**
* @return laneBasedObject.
*/
public final T getLaneBasedObject()
{
return this.laneBasedObject;
}
/** {@inheritDoc} */
@Override
public final int hashCode()
{
final int prime = 31;
int result = 1;
result = prime * result + ((this.distance == null) ? 0 : this.distance.hashCode());
result = prime * result + ((this.laneBasedObject == null) ? 0 : this.laneBasedObject.hashCode());
return result;
}
/** {@inheritDoc} */
@Override
public final boolean equals(final Object obj)
{
if (this == obj)
{
return true;
}
if (obj == null)
{
return false;
}
if (getClass() != obj.getClass())
{
return false;
}
Entry<?> other = (Entry<?>) obj;
if (this.distance == null)
{
if (other.distance != null)
{
return false;
}
}
else if (!this.distance.equals(other.distance))
{
return false;
}
if (this.laneBasedObject == null)
{
if (other.laneBasedObject != null)
{
return false;
}
}
// laneBasedObject does not implement equals...
else if (!this.laneBasedObject.equals(other.laneBasedObject))
{
return false;
}
return true;
}
/** {@inheritDoc} */
@Override
public final int compareTo(final Entry<T> arg)
{
int d = this.distance.compareTo(arg.distance);
if (d != 0 || this.laneBasedObject.equals(arg.laneBasedObject))
{
return d; // different distance (-1 or 1), or same distance but also equal lane based object (0)
}
return 1; // same distance, unequal lane based object (1)
}
/** {@inheritDoc} */
@Override
public final String toString()
{
return "LaneStructure.Entry [distance=" + this.distance + ", laneBasedObject=" + this.laneBasedObject + "]";
}
}
}