NavigatingIterable.java
package org.opentrafficsim.road.gtu.lane.perception.structure;
import java.util.Collection;
import java.util.Iterator;
import java.util.LinkedHashMap;
import java.util.List;
import java.util.Map;
import java.util.NoSuchElementException;
import java.util.function.BiFunction;
import java.util.function.Function;
import org.djunits.value.vdouble.scalar.Length;
import org.djutils.exceptions.Throw;
import org.opentrafficsim.road.gtu.lane.perception.structure.NavigatingIterable.Entry;
/**
* Iterable over entries (with distance and merge distance stored) of objects. The iterable uses a navigator, lister and
* distancer.
* <ul>
* <li><i>navigator</i>; returns a collection of lane records to continue a search from a covered lane record.</li>
* <li><i>lister</i>; returns a list of objects from a lane record. The list must be ordered in the search direction (close to
* far). Objects of any type may be returned as the navigating iterator will check whether objects are of type {@code T}. In
* order to only include objects in the correct range, the lister must account for the start distance of the record, and any
* possible relative position of a GTU.</li>
* <li><i>distancer</i>; returns distance of an object. The distancer must account for any possible relative position of the
* GTUs.</li>
* </ul>
* <p>
* Copyright (c) 2024-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/wjschakel">Wouter Schakel</a>
* @param <T> type of object
* @param <L> type of lane structure record
*/
public class NavigatingIterable<T, L extends LaneRecordInterface<L>> implements Iterable<Entry<T>>
{
/** Class of object type to be found from lanes. */
private final Class<T> clazz;
/** Range within which objects are included. */
private final Length range;
/** Start collection of lane records. */
private final Collection<L> start;
/** Navigator which gives the next records. */
private final Function<L, Collection<L>> navigator;
/** Obtains ordered list of objects from lane. */
private final Function<L, List<?>> lister;
/** Returns distance of object. */
private final BiFunction<T, L, Length> distancer;
/**
* Constructor.
* @param clazz class of lane-based object type.
* @param range range within which objects are included.
* @param start start collection of lane records.
* @param navigator navigator.
* @param lister obtains ordered list of objects from lane.
* @param distancer returns distance of object.
*/
public NavigatingIterable(final Class<T> clazz, final Length range, final Collection<L> start,
final Function<L, Collection<L>> navigator, final Function<L, List<?>> lister,
final BiFunction<T, L, Length> distancer)
{
this.clazz = clazz;
this.range = range;
this.start = start;
this.navigator = navigator;
this.lister = lister;
this.distancer = distancer;
}
@Override
public Iterator<Entry<T>> iterator()
{
// map of currently iterated records and their object iterators, create map from initial start
Map<L, ObjectIterator> map = new LinkedHashMap<>();
for (L startRecord : this.start)
{
map.put(startRecord, new ObjectIterator(startRecord, NavigatingIterable.this.clazz, NavigatingIterable.this.lister,
NavigatingIterable.this.distancer));
}
return new Iterator<Entry<T>>()
{
/** Next entry as found by {@code hasNext()}. */
private Entry<T> next;
@Override
public boolean hasNext()
{
if (this.next != null)
{
return true;
}
// update the map with records that have something to produce
Map<L, ObjectIterator> mapCopy = new LinkedHashMap<>(map);
for (Map.Entry<L, ObjectIterator> mapEntry : mapCopy.entrySet())
{
if (!mapEntry.getValue().hasNext())
{
updateMapRecursive(mapEntry.getKey());
}
}
if (map.isEmpty())
{
this.next = null;
}
else if (map.size() == 1)
{
this.next = map.values().iterator().next().next();
}
else
{
// loop map and find closest object
Length minDistance = Length.POSITIVE_INFINITY;
ObjectIterator closestObjectIterator = null;
for (ObjectIterator objectIterator : map.values())
{
Entry<T> entry = objectIterator.poll();
if (entry.distance().lt(minDistance))
{
minDistance = entry.distance();
closestObjectIterator = objectIterator;
}
}
this.next = closestObjectIterator.next(); // advance the object iterator; it was only polled above
}
if (this.next != null && this.next.distance().gt(NavigatingIterable.this.range))
{
this.next = null; // next object out of range
}
return this.next != null;
}
@Override
public Entry<T> next()
{
Throw.when(!hasNext(), NoSuchElementException.class, "No more object of type %s.",
NavigatingIterable.this.clazz);
Entry<T> n = this.next;
this.next = null;
return n;
}
/**
* Updates the map so it contains a record only if it has an object to return. If not, further records are added to
* the map through the navigator and consecutively checked.
* @param record lane record.
*/
private void updateMapRecursive(final L record)
{
if (!map.containsKey(record) || map.get(record).hasNext())
{
return;
}
map.remove(record);
for (L next : NavigatingIterable.this.navigator.apply(record))
{
map.put(next, new ObjectIterator(next, NavigatingIterable.this.clazz, NavigatingIterable.this.lister,
NavigatingIterable.this.distancer));
updateMapRecursive(next);
}
}
};
}
/**
* Iterator over objects on a {@code LaneRecordInterface}. This is used by {@code NavigatingIterable} to find object.
* <p>
* Copyright (c) 2024-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/wjschakel">Wouter Schakel</a>
*/
private class ObjectIterator implements Iterator<Entry<T>>
{
/** Lane record. */
private final L record;
/** Class of lane-based object type. */
private final Class<T> clazz;
/** List of objects from lane, within correct range for downstream. */
private final List<?> list;
/** Index of current entry. */
private int index;
/** Returns distance of object. */
private final BiFunction<T, L, Length> distancer;
/** Poll entry, that next will return. */
private Entry<T> poll;
/**
* Constructor. The lister may return objects of any type. This class will check whether objects are of type T.
* @param record lane record.
* @param clazz class of lane-based object type.
* @param lister obtains ordered list of objects from lane.
* @param distancer returns distance of object.
*/
ObjectIterator(final L record, final Class<T> clazz, final Function<L, List<?>> lister,
final BiFunction<T, L, Length> distancer)
{
this.record = record;
this.clazz = clazz;
this.list = lister.apply(record);
this.distancer = distancer;
}
@Override
public boolean hasNext()
{
while (this.index < this.list.size() && !this.list.get(this.index).getClass().isAssignableFrom(this.clazz))
{
this.index++;
}
return this.index < this.list.size();
}
@Override
public Entry<T> next()
{
Entry<T> entry = poll();
this.index++;
this.poll = null;
return entry;
}
/**
* Returns the entry that {@code next()} will return, without advancing the iterator.
* @return poll entry.
*/
public Entry<T> poll()
{
Throw.when(!hasNext(), NoSuchElementException.class, "No more object of type %s.", this.clazz);
if (this.poll == null)
{
@SuppressWarnings("unchecked") // isAssignableFrom in hasNext() checks this
T t = (T) this.list.get(this.index);
this.poll = new Entry<>(this.distancer.apply(t, this.record), this.record.getMergeDistance(), t);
}
return this.poll;
}
}
/**
* Container for a perceived object with the distance towards it and the distance until the road of the object and the road
* of the perceiving GTU merge.
* <p>
* Copyright (c) 2024-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/wjschakel">Wouter Schakel</a>
* @param <T> type of object
* @param distance distance to object
* @param merge ego-distance until the road of the object and the road of the perceiving GTU merge
* @param object the perceived object
*/
public record Entry<T>(Length distance, Length merge, T object)
{
}
}