Change how leaders/followers are found in advanced_driving skill (#…

…3160)

msi.gama.ext/META-INF/MANIFEST.MF

@@ -171,6 +171,8 @@ Export-Package: com.conversantmedia.util.collection,
  net.sourceforge.jtds.jdbcx,
  net.sourceforge.jtds.jdbcx.proxy,
  one.util.streamex,
+ org.apache.commons.collections4,
+ org.apache.commons.collections4.bidimap,
  org.apache.commons.dbcp;
   uses:="javax.naming.spi,
    org.apache.commons.pool.impl,

msi.gama.ext/pom2.xml

@@ -44,7 +44,7 @@
 	  <dependency>
 		<groupId>commons-collections</groupId>
 		<artifactId>commons-collections</artifactId>
-		<version>3.1</version>
+		<version>4.4</version>
 	  </dependency>
 	  <dependency>
 		<groupId>commons-dbcp</groupId>

simtools.gaml.extensions.traffic/.classpath

@@ -1,12 +1,17 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <classpath>
-	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER"/>
-	<classpathentry kind="con" path="org.eclipse.pde.core.requiredPlugins"/>
 	<classpathentry kind="src" path="src"/>
 	<classpathentry kind="src" path="gaml">
 		<attributes>
 			<attribute name="optional" value="true"/>
 		</attributes>
 	</classpathentry>
+	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER">
+		<attributes>
+			<attribute name="module" value="true"/>
+		</attributes>
+	</classpathentry>
+	<classpathentry kind="con" path="org.eclipse.pde.core.requiredPlugins"/>
+	<classpathentry kind="lib" path="/msi.gama.ext/geotools/commons-collections4-4.4.jar"/>
 	<classpathentry kind="output" path="bin"/>
 </classpath>

simtools.gaml.extensions.traffic/META-INF/MANIFEST.MF

@@ -11,3 +11,4 @@ Bundle-ClassPath: .
 Bundle-ActivationPolicy: lazy
 Bundle-RequiredExecutionEnvironment: JavaSE-11
 Automatic-Module-Name: simtools.gaml.extensions.traffic
+Class-Path: ../msi.gama.ext/geotools/commons-collections4-4.4.jar

simtools.gaml.extensions.traffic/models/Driving Skill/models/Drive Random.gaml

@@ -11,7 +11,7 @@ import "Traffic.gaml"
 
 global {
 	float traffic_light_interval parameter: 'Traffic light interval' init: 60#s;
-	float step <- 0.1#s;
+	float step <- 0.2#s;
     graph full_road_graph;
 
 	string map_name;

simtools.gaml.extensions.traffic/models/Driving Skill/models/Traffic.gaml

@@ -18,7 +18,7 @@ species road skills: [skill_road] {
 	string oneway;
 
 	aspect base {
-		draw shape color: color end_arrow: 2;
+		draw shape color: color end_arrow: 1;
 	}
 }
 
@@ -107,7 +107,7 @@ species base_vehicle skills: [advanced_driving] {
 		// Shifts the position of the vehicle perpendicularly to the road,
 		// in order to visualize different lanes
 		if (current_road != nil) {
-			float dist <- (road(current_road).lanes - current_lane -
+			float dist <- (road(current_road).num_lanes - current_lane -
 				mean(range(num_lanes_occupied - 1)) - 0.5) * lane_width;
 			if violating_oneway {
 				dist <- -dist;

simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/DrivingOperators.java

@@ -13,11 +13,13 @@
  **********************************************************************************************/
 package simtools.gaml.extensions.traffic;
 
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.LinkedList;
 import java.util.List;
 
-import org.locationtech.jts.geom.Coordinate;
+import org.apache.commons.collections4.OrderedBidiMap;
 
-import msi.gama.common.geometry.GeometryUtils;
 import msi.gama.metamodel.agent.IAgent;
 import msi.gama.metamodel.shape.IShape;
 import msi.gama.metamodel.topology.graph.GamaSpatialGraph;
@@ -28,11 +30,9 @@
 import msi.gama.precompiler.IConcept;
 import msi.gama.precompiler.ITypeProvider;
 import msi.gama.runtime.IScope;
-import msi.gama.util.GamaListFactory;
 import msi.gama.util.IContainer;
-import msi.gama.util.IList;
 import msi.gama.util.graph.IGraph;
-import msi.gaml.types.Types;
+import simtools.gaml.extensions.traffic.carfollowing.CustomDualTreeBidiMap;
 
 @SuppressWarnings({ "unchecked", "rawtypes" })
 public class DrivingOperators {
@@ -47,23 +47,27 @@ public static IGraph spatialDrivingFromEdges(final IScope scope, final IContaine
 		final IGraph graph = new GamaSpatialGraph(edges, nodes, scope);
 		for (final Object edge : edges.iterable(scope)) {
 			if (edge instanceof IShape) {
-				final IAgent ag = ((IShape) edge).getAgent();
-				if (ag.hasAttribute(RoadSkill.NUM_LANES) && ag.hasAttribute(RoadSkill.AGENTS_ON)) {
-					final int numLanes = RoadSkill.getNumLanes(ag);
-					if (numLanes > 0) {
-						final IList agentsOn = (IList) ag.getAttribute(RoadSkill.AGENTS_ON);
-						for (int i = 0; i < numLanes; i++) {
-							final int nbSeg = ag.getInnerGeometry().getNumPoints() - 1;
-							final IList lisSg = GamaListFactory.create(Types.NO_TYPE);
-							for (int j = 0; j < nbSeg; j++) {
-								lisSg.add(GamaListFactory.create(Types.NO_TYPE));
+				IAgent agent = ((IShape) edge).getAgent();
+				int numLanes = RoadSkill.getNumLanes(agent);
+				List<OrderedBidiMap<IAgent, Double>> res = new LinkedList<>();
+				//TODO: this should be tied to the setter of numLanes
+				for (int i = 0; i < numLanes; i += 1) {
+					res.add(
+						new CustomDualTreeBidiMap<IAgent, Double>(new Comparator<IAgent>() {
+							@Override
+							public int compare(IAgent a, IAgent b) {
+								int r = a.getSpeciesName().compareTo(b.getSpeciesName());
+								if (r != 0) {
+									return r;
+								} else {
+									return Integer.compare(a.getIndex(), b.getIndex());
+								}
 							}
-							agentsOn.add(lisSg);
-						}
-					}
-					ag.setAttribute(RoadSkill.ALL_AGENTS, GamaListFactory.create(Types.NO_TYPE));
+						}, 
+						Collections.reverseOrder())
+					);
 				}
-
+				RoadSkill.setVehicleOrdering(agent, res);
 			}
 		}
 		return graph;

simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/DrivingSkill.java

@@ -19,16 +19,13 @@
 import java.util.stream.Collectors;
 import java.util.stream.IntStream;
 
-import com.google.common.collect.Sets;
-
 import org.apache.commons.lang3.tuple.Pair;
 import org.locationtech.jts.geom.Coordinate;
 
 import msi.gama.common.interfaces.IKeyword;
 import msi.gama.metamodel.agent.AbstractAgent;
 import msi.gama.metamodel.agent.IAgent;
 import msi.gama.metamodel.shape.GamaPoint;
-
 import msi.gama.metamodel.shape.IShape;
 import msi.gama.metamodel.topology.graph.GamaSpatialGraph;
 import msi.gama.precompiler.GamlAnnotations.action;
@@ -371,7 +368,13 @@
 		name = DrivingSkill.DISTANCE_TO_GOAL,
 		type = IType.FLOAT,
 		init = "0.0",
-		doc = @doc("euclidean distance to the next point of the current segment")
+		doc = @doc("euclidean distance to the endpoint of the current segment")
+	),
+	@variable(
+		name = DrivingSkill.DISTANCE_TO_CURRENT_TARGET,
+		type = IType.FLOAT,
+		init = "0.0",
+		doc = @doc("euclidean distance to the current target node")
 	),
 	@variable(
 		name = DrivingSkill.SEGMENT_INDEX,
@@ -398,6 +401,12 @@
 		type = IType.FLOAT,
 		init = "nil",
 		doc = @doc("the speed of the leading vehicle")
+	),
+	@variable(
+		name = DrivingSkill.FOLLOWER,
+		type = IType.AGENT,
+		init = "nil",
+		doc = @doc("the vehicle following this vehicle")
 	)
 })
 @skill(
@@ -422,6 +431,7 @@ public class DrivingSkill extends MovingSkill {
 	@Deprecated public static final String CURRENT_LANE = "current_lane";
 	public static final String LOWEST_LANE = "lowest_lane";
 	public static final String DISTANCE_TO_GOAL = "distance_to_goal";
+	public static final String DISTANCE_TO_CURRENT_TARGET = "distance_to_current_target";
 	public static final String VEHICLE_LENGTH = "vehicle_length";
 	@Deprecated public static final String PROBA_LANE_CHANGE_UP = "proba_lane_change_up";
 	@Deprecated public static final String PROBA_LANE_CHANGE_DOWN = "proba_lane_change_down";
@@ -460,6 +470,7 @@ public class DrivingSkill extends MovingSkill {
 	public static final String LEADING_VEHICLE = "leading_vehicle";
 	public static final String LEADING_DISTANCE = "leading_distance";
 	public static final String LEADING_SPEED = "leading_speed";
+	public static final String FOLLOWER = "follower";
 
 	// NOTE: Due to approximations in IDM, vehicles will never have the exact same location as its target.
 	// Therefore we consider the vehicle has reached its goal when distToGoal is smaller than this threshold.
@@ -868,6 +879,16 @@ public static double getDistanceToGoal(final IAgent vehicle) {
 		return (Double) vehicle.getAttribute(DISTANCE_TO_GOAL);
 	}
 
+	@getter(DISTANCE_TO_CURRENT_TARGET)
+	public static double getDistanceToCurrentTarget(final IAgent vehicle) {
+		return (Double) vehicle.getAttribute(DISTANCE_TO_CURRENT_TARGET);
+	}
+
+	@setter(DISTANCE_TO_CURRENT_TARGET)
+	public static void setDistanceToCurrentTarget(final IAgent vehicle, final double dist) {
+		vehicle.setAttribute(DISTANCE_TO_CURRENT_TARGET, dist);
+	}
+
 	@getter(MIN_SECURITY_DISTANCE)
 	public static double getMinSecurityDistance(final IAgent vehicle) {
 		return (Double) vehicle.getAttribute(MIN_SECURITY_DISTANCE);
@@ -912,6 +933,11 @@ public static void setLeadingVehicle(final IAgent vehicle, final IAgent leadingV
 		vehicle.setAttribute(LEADING_VEHICLE, leadingVehicle);
 	}
 
+	@getter(LEADING_DISTANCE)
+	public static double getLeadingDistance(final IAgent vehicle) {
+		return (double) vehicle.getAttribute(LEADING_DISTANCE);
+	}
+
 	@setter(LEADING_DISTANCE)
 	public static void setLeadingDistanceReadOnly(final IAgent vehicle, final double leadingDist) {
 		// read-only
@@ -930,6 +956,10 @@ public static void setLeadingSpeed(final IAgent vehicle, final double leadingSpe
 		vehicle.setAttribute(LEADING_SPEED, leadingSpeed);
 	}
 
+	public static void setFollower(final IAgent vehicle, final IAgent follower) {
+		vehicle.setAttribute(FOLLOWER, follower);
+	}
+
 	@action(
 		name = "advanced_follow_driving",
 		args = {
@@ -1328,10 +1358,12 @@ public void primDriveRandom(final IScope scope) throws GamaRuntimeException {
 
 				// Choose a lane on the new road
 				int firstSegment = !violatingOneway ? 0 : RoadSkill.getNumSegments(newRoad) - 1;
-				int secondPtIdx = !violatingOneway ? 1 : RoadSkill.getNumSegments(newRoad) - 1;
-				GamaPoint firstSegmentEndPt = new GamaPoint(newRoad.getInnerGeometry().getCoordinates()[secondPtIdx]);
-				double firstSegmentLength = loc.euclidianDistanceTo(firstSegmentEndPt);
-				laneAndAccPair = MOBIL.chooseLane(scope, vehicle, newTarget, newRoad, firstSegment, firstSegmentLength);
+//				int secondPtIdx = !violatingOneway ? 1 : RoadSkill.getNumSegments(newRoad) - 1;
+//				GamaPoint firstSegmentEndPt = new GamaPoint(newRoad.getInnerGeometry().getCoordinates()[secondPtIdx]);
+				double firstSegmentLength = RoadSkill.getSegmentLengths(newRoad).get(firstSegment);
+				double newRoadLength = RoadSkill.getTotalLength(newRoad);
+				laneAndAccPair = MOBIL.chooseLane(scope, vehicle, newTarget, newRoad, firstSegment, firstSegmentLength,
+						newRoadLength);
 				if (laneAndAccPair == null) {
 					return;
 				}
@@ -1371,7 +1403,9 @@ public void primDriveRandom(final IScope scope) throws GamaRuntimeException {
 				IAgent currentTarget = getCurrentTarget(vehicle);
 				int currentSegment = getSegmentIndex(vehicle);
 				double distToGoal = getDistanceToGoal(vehicle);
-				laneAndAccPair = MOBIL.chooseLane(scope, vehicle, currentTarget, currentRoad, currentSegment, distToGoal);
+				double distToCurrentTarget = getDistanceToCurrentTarget(vehicle);
+				laneAndAccPair = MOBIL.chooseLane(scope, vehicle, currentTarget, currentRoad, currentSegment, distToGoal,
+						distToCurrentTarget);
 			}
 			int lowestLane = laneAndAccPair.getLeft();
 			double accel = laneAndAccPair.getRight();
@@ -1471,12 +1505,16 @@ public void primDrive(final IScope scope) throws GamaRuntimeException {
 
 				GamaPoint srcNodeLoc = (GamaPoint) RoadSkill.getSourceNode(newRoad).getLocation();
 				boolean violatingOneway = !loc.equals(srcNodeLoc);
+
 				int firstSegment = !violatingOneway ? 0 : RoadSkill.getNumSegments(newRoad) - 1;
-				int secondPtIdx = !violatingOneway ? 1 : RoadSkill.getNumSegments(newRoad) - 1;
-				GamaPoint firstSegmentEndPt = new GamaPoint(newRoad.getInnerGeometry().getCoordinates()[secondPtIdx]);
-				double firstSegmentLength = loc.euclidianDistanceTo(firstSegmentEndPt);
+//				int secondPtIdx = !violatingOneway ? 1 : RoadSkill.getNumSegments(newRoad) - 1;
+//				GamaPoint firstSegmentEndPt = new GamaPoint(newRoad.getInnerGeometry().getCoordinates()[secondPtIdx]);
+				double firstSegmentLength = RoadSkill.getSegmentLengths(newRoad).get(firstSegment);
+				double newRoadLength = RoadSkill.getTotalLength(newRoad);
 				// Choose a lane on the new road
-				laneAndAccPair = MOBIL.chooseLane(scope, vehicle, newTarget, newRoad, firstSegment, firstSegmentLength);
+				laneAndAccPair = MOBIL.chooseLane(scope, vehicle, newTarget, newRoad, firstSegment, firstSegmentLength,
+						newRoadLength);
+
 				if (laneAndAccPair == null) {
 					return;
 				}
@@ -1523,7 +1561,9 @@ public void primDrive(final IScope scope) throws GamaRuntimeException {
 				IAgent currentTarget = getCurrentTarget(vehicle);
 				int currentSegment = getSegmentIndex(vehicle);
 				double distToGoal = getDistanceToGoal(vehicle);
-				laneAndAccPair = MOBIL.chooseLane(scope, vehicle, currentTarget, currentRoad, currentSegment, distToGoal);
+				double distToCurrentTarget = getDistanceToCurrentTarget(vehicle);
+				laneAndAccPair = MOBIL.chooseLane(scope, vehicle, currentTarget, currentRoad, currentSegment, distToGoal,
+						distToCurrentTarget);
 			}
 			int lowestLane = laneAndAccPair.getLeft();
 			double accel = laneAndAccPair.getRight();
@@ -1662,28 +1702,19 @@ private void updateLaneSegment(final IScope scope, final int newLowestLane,
 		Set<Integer> newLanes = IntStream.range(newLowestLane, newLowestLane + numLanesOccupied)
 				.boxed().collect(Collectors.toCollection(HashSet::new));
 
-		// Small optimization to not touch the lists associated with unchanged lanes
-		Set<Integer> lanesToRemove, lanesToAdd;
-		if (newSegment != currentSegment) {
-			// if entering new segment, we have to update the lists for all related lanes and segments
-			lanesToRemove = oldLanes;
-			lanesToAdd = newLanes;
-		} else {
-			// otherwise the vehicle is still in the same segment, so we only update (possibly few) relevant lists
-			lanesToRemove = Sets.difference(oldLanes, newLanes);
-			lanesToAdd = Sets.difference(newLanes, oldLanes);
-		}
-
 		IAgent correctRoad = getCurrentRoad(vehicle);
-		for (int lane : lanesToRemove) {
-			List<IAgent> oldVehicleList = RoadSkill.getVehiclesOnLaneSegment(
-				scope, correctRoad, lane, currentSegment);
-			oldVehicleList.remove(vehicle);
+		int numLanesCorrect = RoadSkill.getNumLanes(correctRoad);
+		double dist;
+		for (int lane : oldLanes) {
+			RoadSkill.getVehiclesOnLaneSegment(scope, correctRoad, lane).remove(vehicle);
 		}
-		for (int lane : lanesToAdd) {
-			List<IAgent> newVehicleList = RoadSkill.getVehiclesOnLaneSegment(
-				scope, correctRoad, lane, newSegment);
-			newVehicleList.add(vehicle);
+		for (int lane : newLanes) {
+			if (lane < numLanesCorrect) {
+				dist = getDistanceToCurrentTarget(vehicle);
+			} else {
+				dist = RoadSkill.getTotalLength(correctRoad) - getDistanceToCurrentTarget(vehicle);
+			}
+			RoadSkill.getVehiclesOnLaneSegment(scope, correctRoad, lane).put(vehicle, dist);
 		}
 
 		setLowestLane(vehicle, newLowestLane);
@@ -1731,31 +1762,37 @@ private double move(final IScope scope,
 		GamaPoint endPt = !violatingOneway ?
 			new GamaPoint(coords[currentSegment + 1]) : new GamaPoint(coords[currentSegment]);
 		if (distMoved > distToGoal || distToGoal < EPSILON) {
+			setDistanceToCurrentTarget(vehicle, 
+					getDistanceToCurrentTarget(vehicle) - distToGoal);
+			updateLaneSegment(scope, newLowestLane, currentSegment);
 			if (endPt.equals(currentTarget.getLocation())) {
 				// Move to a new road
 				setLocation(vehicle, endPt);
 				setDistanceToGoal(vehicle, 0.0);
 				// Return to the main loop in `drive` to continue moving across the intersection
 				return distToGoal < EPSILON ? time : distToGoal / newSpeed;
 			} else {
+				// TODO: need a loop to skip multiple segments
 				// Move to a new segment
 				distMoved -= distToGoal;
 				loc = endPt;
 				currentSegment = !violatingOneway ? currentSegment + 1 : currentSegment - 1;
 				endPt = !violatingOneway ?
 					new GamaPoint(coords[currentSegment + 1]) : new GamaPoint(coords[currentSegment]);
-				distToGoal = loc.distance(endPt);
+				distToGoal = RoadSkill.getSegmentLengths(currentRoad).get(currentSegment);
 			}
 		}
 
 		double ratio = distMoved / distToGoal;
 		double newX = loc.getX() + ratio * (endPt.getX() - loc.getX());
 		double newY = loc.getY() + ratio * (endPt.getY() - loc.getY());
 		setLocation(vehicle, new GamaPoint(newX, newY));
-		updateLaneSegment(scope, newLowestLane, currentSegment);
-		setDistanceToGoal(vehicle, distToGoal - distMoved);
 		setSpeed(vehicle, newSpeed);
 		setAcceleration(vehicle, accel);
+		setDistanceToGoal(vehicle, distToGoal - distMoved);
+		setDistanceToCurrentTarget(vehicle, getDistanceToCurrentTarget(vehicle) - distMoved);
+
+		updateLaneSegment(scope, newLowestLane, currentSegment);
 		return 0.0;
 	}