AbstractPerceptionIterable.java
package org.opentrafficsim.road.gtu.lane.perception;
import java.util.HashMap;
import java.util.Iterator;
import java.util.LinkedList;
import java.util.Map;
import java.util.Queue;
import java.util.Set;
import java.util.SortedMap;
import java.util.TreeMap;
import org.djunits.value.vdouble.scalar.Length;
import org.opentrafficsim.core.gtu.GTUException;
import org.opentrafficsim.core.gtu.RelativePosition;
import org.opentrafficsim.core.gtu.Try;
import org.opentrafficsim.core.network.Link;
import org.opentrafficsim.core.network.route.Route;
import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
/**
* Abstract iterable that figures out how to find the next nearest object, including splits.
* <p>
* Copyright (c) 2013-2018 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/node/13">OpenTrafficSim License</a>.
* <p>
* @version $Revision$, $LastChangedDate$, by $Author$, initial version 16 feb. 2018 <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 <H> headway type
* @param <U> underlying object type
* @param <C> counter type
*/
public abstract class AbstractPerceptionIterable<H extends Headway, U, C> extends AbstractPerceptionReiterable<H, U>
{
/** Root record. */
private final LaneRecord<?> root;
/** Initial position. */
private final Length initialPosition;
/** Search downstream (or upstream). */
private final boolean downstream;
/** Max distance. */
private final double maxDistance;
/** Position to which distance are calculated by subclasses. */
private final RelativePosition relativePosition;
/** Route of the GTU. */
private final Route route;
/**
* Constructor.
* @param perceivingGtu LaneBasedGTU; perceiving GTU
* @param root R; root record
* @param initialPosition Length; initial position
* @param downstream boolean; search downstream (or upstream)
* @param maxDistance Length; max distance to search
* @param relativePosition RelativePosition; position to which distance are calculated by subclasses
* @param route Route; route of the GTU, may be {@code null}
*/
public AbstractPerceptionIterable(final LaneBasedGTU perceivingGtu, final LaneRecord<?> root, final Length initialPosition,
final boolean downstream, final Length maxDistance, final RelativePosition relativePosition, final Route route)
{
super(perceivingGtu);
this.root = root;
this.initialPosition = initialPosition;
this.downstream = downstream;
this.maxDistance = maxDistance.si;
this.relativePosition = relativePosition;
this.route = route;
}
/** {@inheritDoc} */
@Override
public Iterator<PrimaryIteratorEntry> primaryIterator()
{
return new PrimaryIterator();
}
/**
* Returns the next object(s) on the lane represented by the record. This should only consider objects on the given lane.
* This method should not check the distance towards objects with the maximum distance.
* @param record R; record representing the lane and direction
* @param position Length; position to look beyond
* @param counter C; counter
* @return next object(s) on the lane or {@code null} if none
* @throws GTUException on any exception in the process
*/
protected abstract Entry getNext(LaneRecord<?> record, Length position, C counter) throws GTUException;
/**
* Returns the distance to the object. The position fed in to this method is directly taken from an {@code Entry} returned
* by {@code getNext}. The two methods need to be consistent with each other.
* @param object U; underlying object
* @param record R; record representing the lane and direction
* @param position Length; position of the object on the lane
* @return Length; distance to the object
*/
protected abstract Length getDistance(U object, LaneRecord<?> record, Length position);
/**
* Returns the longitudinal length of the relevant relative position such that distances to this points can be calculated.
* @return Length; the longitudinal length of the relevant relative position such that distances to this points can be
* calculated
*/
protected Length getDx()
{
return this.relativePosition.getDx();
}
/**
* The primary iterator is used by all returned iterators to find the next object. This contains the core algorithm to deal
* with splits and multiple objects at a single location.
* <p>
* Copyright (c) 2013-2018 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/node/13">OpenTrafficSim License</a>.
* <p>
* @version $Revision$, $LastChangedDate$, by $Author$, initial version 16 feb. 2018 <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>
*/
private class PrimaryIterator implements Iterator<PrimaryIteratorEntry>
{
/** Map containing the objects found per branch. */
private SortedMap<PrimaryIteratorEntry, LaneRecord<?>> map;
/** Position on the lane of each object. */
private Map<U, Length> positions = new HashMap<>();
/** Sets of remaining objects at the same location. */
private Map<LaneRecord<?>, Queue<PrimaryIteratorEntry>> queues = new HashMap<>();
/** Counter objects per lane. */
private Map<LaneRecord<?>, C> counters = new HashMap<>();
/** Record regarding a postponed call to {@code getNext()}. */
private LaneRecord<?> postponedRecord = null;
/** Position regarding a postponed call to {@code getNext()}. */
private Length postponedPosition = null;
/** Constructor. */
PrimaryIterator()
{
//
}
/** {@inheritDoc} */
@Override
public boolean hasNext()
{
// (re)start the process
startProcess();
// check next value
return !this.map.isEmpty();
}
/** {@inheritDoc} */
@Override
public PrimaryIteratorEntry next()
{
// (re)start the process
startProcess();
// get and remove next
PrimaryIteratorEntry nextEntry = this.map.firstKey();
U next = nextEntry.object;
LaneRecord<?> record = this.map.get(nextEntry);
Length position = this.positions.get(next);
this.map.remove(nextEntry);
// see if we can obtain the next from a queue
Queue<PrimaryIteratorEntry> queue = this.queues.get(next);
if (queue != null)
{
PrimaryIteratorEntry nextNext = queue.poll();
this.map.put(nextNext, record); // next object is made available in the map
this.positions.put(nextNext.object, position);
if (queue.isEmpty())
{
this.queues.remove(record);
}
return new PrimaryIteratorEntry(nextNext.object, getDistance(nextNext.object, record, position));
}
// prepare for next
this.postponedRecord = record;
this.postponedPosition = position;
return new PrimaryIteratorEntry(next, getDistance(next, record, position));
}
/**
* Starts or restarts the process.
*/
@SuppressWarnings("synthetic-access")
private void startProcess()
{
if (this.postponedRecord != null)
{
// restart the process; perform prepareNext() that was postponed
prepareNext(this.postponedRecord, this.postponedPosition);
this.postponedRecord = null;
this.postponedPosition = null;
}
else if (this.map == null)
{
// start the process
this.map = new TreeMap<>();
prepareNext(AbstractPerceptionIterable.this.root, AbstractPerceptionIterable.this.initialPosition);
}
}
/**
* Iterative method that continues a search on the next lanes if no object is found.
* @param record R; record
* @param position Length; position
*/
@SuppressWarnings("synthetic-access")
private void prepareNext(final LaneRecord<?> record, final Length position)
{
Entry next = Try.assign(() -> AbstractPerceptionIterable.this.getNext(record, position, this.counters.get(record)),
"Exception while deriving next object.");
if (next == null)
{
this.counters.remove(record);
double distance = AbstractPerceptionIterable.this.downstream
? record.getStartDistance().si + record.getLength().si : -record.getStartDistance().si;
// TODO this let's us ignore an object that is registered on the next lane, but who's tail may be on this lane
if (distance < AbstractPerceptionIterable.this.maxDistance)
{
if (AbstractPerceptionIterable.this.downstream)
{
for (LaneRecord<?> nextRecord : record.getNext())
{
if (isOnRoute(nextRecord))
{
prepareNext(nextRecord, Length.createSI(-1e-9));
}
}
}
else
{
for (LaneRecord<?> nextRecord : record.getPrev())
{
if (isOnRoute(nextRecord))
{
prepareNext(nextRecord, nextRecord.getLength());
}
}
}
}
}
else
{
this.counters.put(record, next.counter);
if (next.isSet())
{
Iterator<U> it = next.set.iterator();
U nextNext = it.next();
Length distance = getDistance(nextNext, record, next.position);
if (distance == null // null means the object overlaps and is close
|| distance.si <= AbstractPerceptionIterable.this.maxDistance)
{
// next object is made available in the map
this.map.put(new PrimaryIteratorEntry(nextNext, distance), record);
this.positions.put(nextNext, next.position);
if (next.set.size() > 1)
{
// remaining at this location are made available in a queue
Queue<PrimaryIteratorEntry> queue = new LinkedList<>();
while (it.hasNext())
{
nextNext = it.next();
queue.add(new PrimaryIteratorEntry(nextNext, getDistance(nextNext, record, next.position)));
}
this.queues.put(record, queue);
}
}
}
else
{
Length distance = getDistance(next.object, record, next.position);
if (distance == null // null means the object overlaps and is close
|| distance.si <= AbstractPerceptionIterable.this.maxDistance)
{
// next object is made available in the map
this.map.put(new PrimaryIteratorEntry(next.object, distance), record);
this.positions.put(next.object, next.position);
}
}
}
}
}
/**
* Returns whether the record is on the route.
* @param record R; record
* @return boolean; whether the record is on the route
*/
final boolean isOnRoute(final LaneRecord<?> record)
{
if (this.route == null)
{
return true;
}
Link link = record.getLane().getParentLink();
int from;
int to;
if (record.getDirection().isPlus())
{
from = this.route.indexOf(link.getStartNode());
to = this.route.indexOf(link.getEndNode());
}
else
{
from = this.route.indexOf(link.getEndNode());
to = this.route.indexOf(link.getStartNode());
}
return from != -1 && to != -1 && to - from == 1;
}
/**
* Class of objects for subclasses to return. This can contain either a single object, or a set if there are multiple
* objects at a single location.
* <p>
* Copyright (c) 2013-2018 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/node/13">OpenTrafficSim License</a>.
* <p>
* @version $Revision$, $LastChangedDate$, by $Author$, initial version 16 feb. 2018 <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>
*/
protected class Entry
{
/** Set. */
private final Set<U> set;
/** Object. */
private final U object;
/** Counter. */
private final C counter;
/** Position on the lane. */
private final Length position;
/**
* Constructor.
* @param object U; object
* @param counter C; counter, may be {@code null}
* @param position Length; position
*/
public Entry(final U object, final C counter, final Length position)
{
this.set = null;
this.object = object;
this.counter = counter;
this.position = position;
}
/**
* Constructor.
* @param set Set<U>; set
* @param counter C; counter, may be {@code null}
* @param position Length; position
*/
public Entry(final Set<U> set, final C counter, final Length position)
{
this.set = set;
this.object = null;
this.counter = counter;
this.position = position;
}
/**
* Returns whether this entry contains a set.
* @return whether this entry contains a set
*/
final boolean isSet()
{
return this.set != null;
}
}
}