NeighborsUtil.java
package org.opentrafficsim.road.gtu.lane.perception.categories.neighbors;
import java.util.Collection;
import java.util.Comparator;
import java.util.ConcurrentModificationException;
import java.util.Iterator;
import java.util.LinkedHashSet;
import java.util.Set;
import java.util.SortedMap;
import java.util.SortedSet;
import java.util.TreeMap;
import java.util.TreeSet;
import org.djunits.value.vdouble.scalar.Length;
import org.djunits.value.vdouble.scalar.Time;
import org.djutils.exceptions.Try;
import org.opentrafficsim.base.parameters.ParameterException;
import org.opentrafficsim.core.gtu.GTUException;
import org.opentrafficsim.core.gtu.RelativePosition;
import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
import org.opentrafficsim.road.gtu.lane.perception.LaneStructureRecord;
import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
/**
* Utilities to perceive neighbors.
* <p>
* Copyright (c) 2013-2019 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 19 okt. 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>
*/
public final class NeighborsUtil
{
/**
* Constructor.
*/
private NeighborsUtil()
{
//
}
/**
* Returns a set of first leaders per branch, relative to given relative position. Helper method to find first leaders and
* GTU's alongside.
* @param startRecord LaneStructureRecord; lane structure record to start the search from
* @param egoRelativePosition RelativePosition; position of GTU to start search from
* @param egoFrontPosition RelativePosition; front position of GTU to determine headway
* @param otherRelativePosition RelativePosition.TYPE; position of other GTU that has to be downstream
* @param now Time; current time
* @return set of first leaders per branch
* @throws GTUException if the GTU was not initialized
* @throws ParameterException if a parameter was not present or out of bounds
*/
public static SortedSet<DistanceGTU> getFirstDownstreamGTUs(final LaneStructureRecord startRecord,
final RelativePosition egoRelativePosition, final RelativePosition egoFrontPosition,
final RelativePosition.TYPE otherRelativePosition, final Time now) throws GTUException, ParameterException
{
SortedSet<DistanceGTU> headwaySet = new TreeSet<>();
Set<LaneStructureRecord> currentSet = new LinkedHashSet<>();
Set<LaneStructureRecord> nextSet = new LinkedHashSet<>();
Length dxSearch = egoRelativePosition.getDx();
Length dxHeadway = egoFrontPosition.getDx();
LaneStructureRecord record = startRecord;
branchUpstream(record, dxSearch, currentSet);
// move downstream over branches as long as no vehicles are found
while (!currentSet.isEmpty())
{
Iterator<LaneStructureRecord> iterator = currentSet.iterator();
while (iterator.hasNext())
{
record = iterator.next();
/*-
* _ _ _ ______________________ _ _ _
* _|___ |
* find any vehicle downstream of this point on lane A | |__o__| A |
* _ _ _ ___________|_______|__ _ _ _
* (--------) negative distance
*/
LaneBasedGTU down = record.getLane().getGtuAhead(record.getStartDistance().neg().plus(dxSearch),
record.getDirection(), otherRelativePosition, now);
if (down != null)
{
// GTU found, add to set
headwaySet.add(new DistanceGTU(down,
record.getStartDistance().plus(down.position(record.getLane(), down.getRear())).minus(dxHeadway)));
}
else
{
// no GTU found, search on next lanes in next loop and maintain cumulative length
for (LaneStructureRecord next : record.getNext())
{
nextSet.add(next);
}
}
}
currentSet = nextSet;
nextSet = new LinkedHashSet<>();
}
return headwaySet;
}
/**
* Returns a set of lanes to start from for a downstream search, upstream of the reference lane if the tail is before this
* lane.
* @param record LaneStructureRecord; start record
* @param dx Length; distance between reference point and point to search from
* @param set Set<LaneStructureRecord>; set of lanes that is recursively built up, starting with the reference record
*/
private static void branchUpstream(final LaneStructureRecord record, final Length dx, final Set<LaneStructureRecord> set)
{
Length pos = record.getStartDistance().neg().minus(dx);
if (pos.lt0() && !record.getPrev().isEmpty())
{
for (LaneStructureRecord prev : record.getPrev())
{
branchUpstream(prev, dx, set);
}
}
else
{
set.add(record);
}
}
/**
* Returns a set of first followers per branch, relative to given relative position. Helper method to find first followers
* and GTU's alongside.
* @param startRecord LaneStructureRecord; lane structure record to start the search from
* @param egoRelativePosition RelativePosition; position of GTU to start search from
* @param egoRearPosition RelativePosition; rear position of GTU to determine headway
* @param otherRelativePosition RelativePosition.TYPE; type of position of other GTU that has to be upstream
* @param now Time; current time
* @return set of first followers per branch
* @throws GTUException if the GTU was not initialized
* @throws ParameterException if a parameter was not present or out of bounds
*/
public static SortedSet<DistanceGTU> getFirstUpstreamGTUs(final LaneStructureRecord startRecord,
final RelativePosition egoRelativePosition, final RelativePosition egoRearPosition,
final RelativePosition.TYPE otherRelativePosition, final Time now) throws GTUException, ParameterException
{
SortedSet<DistanceGTU> headwaySet = new TreeSet<>();
Set<LaneStructureRecord> currentSet = new LinkedHashSet<>();
Set<LaneStructureRecord> prevSet = new LinkedHashSet<>();
Length dxSearch = egoRelativePosition.getDx();
Length dxHeadway = egoRearPosition.getDx();
LaneStructureRecord record = startRecord;
branchDownstream(record, dxSearch, currentSet);
// move upstream over branches as long as no vehicles are found
while (!currentSet.isEmpty())
{
Iterator<LaneStructureRecord> iterator = currentSet.iterator();
while (iterator.hasNext())
{
record = iterator.next();
/*-
* _ _ _ ______________________ _ _ _
* | ___|_
* | A |__o__| | find any upstream of this point on lane A
* _ _ _ __|_______|___________ _ _ _
* (----------------) distance
*/
LaneBasedGTU up = record.getLane().getGtuBehind(record.getStartDistance().neg().plus(dxSearch),
record.getDirection(), otherRelativePosition, now);
if (up != null)
{
// GTU found, add to set
headwaySet.add(new DistanceGTU(up, record.getStartDistance().neg()
.minus(up.position(record.getLane(), up.getFront())).plus(dxHeadway)));
}
else
{
// no GTU found, search on next lanes in next loop and maintain cumulative length
for (LaneStructureRecord prev : record.getPrev())
{
prevSet.add(prev);
}
}
}
currentSet = prevSet;
prevSet = new LinkedHashSet<>();
}
return headwaySet;
}
/**
* Returns a set of lanes to start from for an upstream search, downstream of the reference lane if the front is after this
* lane.
* @param record LaneStructureRecord; start record
* @param dx Length; distance between reference point and point to search from
* @param set Set<LaneStructureRecord>; set of lanes that is recursively built up, starting with the reference record
*/
private static void branchDownstream(final LaneStructureRecord record, final Length dx, final Set<LaneStructureRecord> set)
{
Length pos = record.getStartDistance().neg().plus(dx);
if (pos.gt(record.getLane().getLength()))
{
for (LaneStructureRecord next : record.getNext())
{
branchDownstream(next, dx, set);
}
}
else
{
set.add(record);
}
}
/**
* Translation from a set of {@code DistanceGTU}'s, to a sorted set of {@code HeadwayGTU}'s. This bridges the gap between a
* raw network search, and the perceived result.
* @param base SortedSet<DistanceGTU>; base set of GTU's at distance
* @param headwayGtuType HeadwayGtuType; headway type for perceived GTU's
* @param perceivingGtu LaneBasedGTU; perceiving GTU
* @param downstream boolean; whether the GTU's are downstream
* @return SortedSet<HeadwayGTU>; set of perceived GTU's
*/
public static SortedSet<HeadwayGTU> perceive(final SortedSet<DistanceGTU> base, final HeadwayGtuType headwayGtuType,
final LaneBasedGTU perceivingGtu, final boolean downstream)
{
return new SortedNeighborsSet(base, headwayGtuType, perceivingGtu, downstream);
}
/**
* GTU at a distance, as preliminary info towards perceiving it. For instance, as a set from a search algorithm.
* <p>
* Copyright (c) 2013-2019 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 22 apr. 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>
*/
public static class DistanceGTU implements Comparable<DistanceGTU>
{
/** GTU. */
private LaneBasedGTU gtu;
/** Distance. */
private Length distance;
/**
* Constructor.
* @param gtu LaneBasedGTU; GTU
* @param distance Length; distance
*/
DistanceGTU(final LaneBasedGTU gtu, final Length distance)
{
this.gtu = gtu;
this.distance = distance;
}
/**
* Returns the GTU.
* @return LaneBasedGTU; GTU
*/
public LaneBasedGTU getGTU()
{
return this.gtu;
}
/**
* Returns the distance.
* @return Length; distance
*/
public Length getDistance()
{
return this.distance;
}
/** {@inheritDoc} */
@Override
public int compareTo(final DistanceGTU o)
{
return this.distance.compareTo(o.distance);
}
}
/**
* Translation from a set of {@code DistanceGTU}'s, to a sorted set of {@code HeadwayGTU}'s. This bridges the gap between a
* raw network search, and the perceived result.
* <p>
* Copyright (c) 2013-2019 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 22 apr. 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 static class SortedNeighborsSet implements SortedSet<HeadwayGTU>
{
/** Base set of GTU's at distance. */
private final SortedSet<DistanceGTU> base;
/** Headway type for perceived GTU's. */
private final HeadwayGtuType headwayGtuType;
/** Perceiving GTU. */
private final LaneBasedGTU perceivingGtu;
/** Whether the GTU's are downstream. */
private final boolean downstream;
/** Contains all GTU's perceived so far, to prevent re-perception. */
private final SortedMap<String, HeadwayGTU> all = new TreeMap<>();
/**
* Constructor.
* @param base SortedSet<DistanceGTU>; base set of GTU's at distance
* @param headwayGtuType HeadwayGtuType; headway type for perceived GTU's
* @param perceivingGtu LaneBasedGTU; perceiving GTU
* @param downstream boolean; whether the GTU's are downstream
*/
SortedNeighborsSet(final SortedSet<DistanceGTU> base, final HeadwayGtuType headwayGtuType,
final LaneBasedGTU perceivingGtu, final boolean downstream)
{
this.base = base;
this.headwayGtuType = headwayGtuType;
this.perceivingGtu = perceivingGtu;
this.downstream = downstream;
}
/** {@inheritDoc} */
@Override
public int size()
{
return this.base.size();
}
/** {@inheritDoc} */
@Override
public boolean isEmpty()
{
return this.base.isEmpty();
}
/**
* Make sure all GTU are available in perceived for. Helper method.
*/
private void getAll()
{
Iterator<HeadwayGTU> it = iterator();
while (it.hasNext())
{
@SuppressWarnings("unused")
HeadwayGTU gtu = it.next(); // iterator creates all HeadwayGTU's
}
}
/** {@inheritDoc} */
@Override
public boolean contains(final Object o)
{
getAll();
return this.all.containsValue(o);
}
/** {@inheritDoc} */
@Override
public Iterator<HeadwayGTU> iterator()
{
return new Iterator<HeadwayGTU>()
{
@SuppressWarnings("synthetic-access")
private Iterator<DistanceGTU> it = SortedNeighborsSet.this.base.iterator();
@Override
public boolean hasNext()
{
return this.it.hasNext();
}
@SuppressWarnings("synthetic-access")
@Override
public HeadwayGTU next()
{
DistanceGTU next = this.it.next();
if (next == null)
{
throw new ConcurrentModificationException();
}
HeadwayGTU out = SortedNeighborsSet.this.all.get(next.getGTU().getId());
if (out == null)
{
out = Try.assign(() -> SortedNeighborsSet.this.headwayGtuType.createHeadwayGtu(
SortedNeighborsSet.this.perceivingGtu, next.getGTU(), next.getDistance(),
SortedNeighborsSet.this.downstream), "Exception while perceiving a neighbor.");
SortedNeighborsSet.this.all.put(next.getGTU().getId(), out);
}
return out;
}
};
}
/** {@inheritDoc} */
@Override
public Object[] toArray()
{
getAll();
return this.all.values().toArray();
}
/** {@inheritDoc} */
@Override
public <T> T[] toArray(final T[] a)
{
getAll();
return this.all.values().toArray(a);
}
/** {@inheritDoc} */
@Override
public boolean add(final HeadwayGTU e)
{
throw new UnsupportedOperationException();
}
/** {@inheritDoc} */
@Override
public boolean remove(final Object o)
{
throw new UnsupportedOperationException();
}
/** {@inheritDoc} */
@Override
public boolean containsAll(final Collection<?> c)
{
getAll();
return this.all.values().containsAll(c);
}
/** {@inheritDoc} */
@Override
public boolean addAll(final Collection<? extends HeadwayGTU> c)
{
throw new UnsupportedOperationException();
}
/** {@inheritDoc} */
@Override
public boolean retainAll(final Collection<?> c)
{
throw new UnsupportedOperationException();
}
/** {@inheritDoc} */
@Override
public boolean removeAll(final Collection<?> c)
{
throw new UnsupportedOperationException();
}
/** {@inheritDoc} */
@Override
public void clear()
{
throw new UnsupportedOperationException();
}
/** {@inheritDoc} */
@Override
public Comparator<? super HeadwayGTU> comparator()
{
return null;
}
/**
* Helper method for sub-lists to find distance-GTU from the perceived GTU.
* @param element HeadwayGTU; perceived GTU
* @return DistanceGTU; pertaining to given GTU
*/
private DistanceGTU getGTU(final HeadwayGTU element)
{
for (DistanceGTU distanceGtu : this.base)
{
if (distanceGtu.getGTU().getId().equals(element.getId()))
{
return distanceGtu;
}
}
throw new IllegalArgumentException("GTU used to obtain a subset is not in the set.");
}
/** {@inheritDoc} */
@Override
public SortedSet<HeadwayGTU> subSet(final HeadwayGTU fromElement, final HeadwayGTU toElement)
{
return new SortedNeighborsSet(this.base.subSet(getGTU(fromElement), getGTU(toElement)), this.headwayGtuType,
this.perceivingGtu, this.downstream);
}
/** {@inheritDoc} */
@Override
public SortedSet<HeadwayGTU> headSet(final HeadwayGTU toElement)
{
return new SortedNeighborsSet(this.base.headSet(getGTU(toElement)), this.headwayGtuType, this.perceivingGtu,
this.downstream);
}
/** {@inheritDoc} */
@Override
public SortedSet<HeadwayGTU> tailSet(final HeadwayGTU fromElement)
{
return new SortedNeighborsSet(this.base.tailSet(getGTU(fromElement)), this.headwayGtuType, this.perceivingGtu,
this.downstream);
}
/** {@inheritDoc} */
@Override
public HeadwayGTU first()
{
return iterator().next();
}
/** {@inheritDoc} */
@Override
public HeadwayGTU last()
{
getAll();
return this.all.get(this.all.lastKey());
}
}
}