public CollisionCheck(UUID pov, RoadLocation startFrom, RoadLocation finishAt) {
super();
this.self = pov;
this.startFrom = startFrom;
this.finishAt = finishAt;
// build collision candidate set
int startOffset = startFrom.getOffset();
int finishOffset = finishAt.getOffset();
if (finishOffset < startOffset) finishOffset += areaLength;
for (int lane = 0; lane < getAreaService().getSizeX(); lane++) {
collisionCandidates.addAll(getAgentsInLane(lane, startOffset, finishOffset));
}
collisionCandidates.remove(this.self);
laneChange = startFrom.getLane() != finishAt.getLane();
for (UUID a : collisionCandidates) {
candidateLocs.put(a, (RoadLocation) getLocationService().getAgentLocation(a));
}
}
public Set<UUID> checkForCollision() {
Set<UUID> collided = new HashSet<UUID>();
int collisionsOccured = 0;
Set<UUID> agentsOnCurrentCell =
getAreaService().getCell(finishAt.getLane(), finishAt.getOffset(), 0);
if (agentsOnCurrentCell.size() > 1) {
logger.warn(
"Collision Occurred: Multiple agents on one cell. Cell: "
+ this.finishAt
+ ", agents: "
+ agentsOnCurrentCell);
collisionsOccured++;
for (UUID a : agentsOnCurrentCell) {
if (!a.equals(self)) {
collided.add(a);
}
}
}
for (UUID a : collisionCandidates) {
// FIXME ? This stops agents that have left being checked
if (getLocationService() == null) {
System.out.println(); // stop here
}
if (a == null) {
System.out.println(); // or here
}
if (!hasLeft(a)) {
RoadLocation current = (RoadLocation) getLocationService().getAgentLocation(a);
// new code here
if ((current.getLane() == finishAt.getLane()) && !laneChange) {
// same lane, if he is behind us then it is a collision
int hisOffset = current.getOffset();
int myOffset = finishAt.getOffset();
boolean heWrapped = hisOffset < candidateLocs.get(a).getOffset();
boolean iWrapped = myOffset < startFrom.getOffset();
if (!iWrapped && heWrapped) {
hisOffset += areaLength;
}
if (hisOffset < myOffset) {
logger.warn(
"Collision Occured: Agent "
+ agentsOnCurrentCell
+ " went through "
+ a
+ " on cell "
+ finishAt);
collisionsOccured++;
collided.add(a);
}
}
// commenting out old code
/* if (current.getLane() == finishAt.getLane()) {
// same lane, if he is behind us then it is a collision
int hisOffset = current.getOffset();
int myOffset = finishAt.getOffset();
boolean heWrapped = hisOffset < candidateLocs.get(a).getOffset();
boolean iWrapped = myOffset < startFrom.getOffset();
if(!iWrapped && heWrapped) {
hisOffset += areaLength;
}
if (hisOffset < myOffset) {
logger.warn("Collision Occured: Agent "
+ agentsOnCurrentCell + " went through " + a + " on cell " + finishAt);
collisionsOccured++;
}
}
if (laneChange && current.getLane() == startFrom.getLane()
&& finishAt.getLane() == candidateLocs.get(a).getLane()
&& current.getOffset() <= finishAt.getOffset()) {
logger.warn("Collision Occured: Agent "
+ agentsOnCurrentCell + " crossed paths with " + a + " between cells " + finishAt);
collisionsOccured++;
}*/
}
}
if (collisionsOccured > 0) {
System.err.println(collisionsOccured + " collisions occurred.");
}
return collided;
}
@Override
public Input handle(Action action, UUID actor) throws ActionHandlingException {
CellMove m = (CellMove) action;
int prevSpeed = speedService.getAgentSpeed(actor);
int maxSpeed = roadEnvironmentService.getMaxSpeed();
int maxAccel = roadEnvironmentService.getMaxAccel();
int maxDecel = roadEnvironmentService.getMaxDecel();
// check move direction is positive or 0
if (m.getY() < 0) {
throw new ActionHandlingException("Cannot move backwards. Move was: " + m);
}
// check move is not too fast
if (m.getY() > maxSpeed) {
throw new ActionHandlingException(
"Cannot move faster than the maximum speed (" + maxSpeed + "). Move was: " + m);
}
// check acceleration is not too fast
if (m.getY() > prevSpeed) {
if ((m.getY() - prevSpeed) > maxAccel) {
throw new ActionHandlingException(
"Cannot accelerate faster than the maximum acceleration ("
+ maxAccel
+ "). Move was: "
+ m
+ " and previous speed was "
+ prevSpeed);
}
}
// check deceleration is not too fast
if (m.getY() < prevSpeed) {
if ((prevSpeed - m.getY()) > maxDecel) {
throw new ActionHandlingException(
"Cannot decelerate faster than the maximum deceleration ("
+ maxDecel
+ "). Move was: "
+ m
+ " and previous speed was "
+ prevSpeed);
}
}
// check move sideways magnitude is 0 or 1
if (Math.abs(m.getX()) > 1) {
throw new ActionHandlingException(
"Cannot change greater than one lane at once. Move was: " + m);
}
// cannot change lane without forward movement
if (Math.abs(m.getX()) > 0 && (int) m.getY() == 0) {
throw new ActionHandlingException("Cannot change lane while stationary");
}
RoadLocation start = (RoadLocation) getLocationService().getAgentLocation(actor);
RoadLocation target = new RoadLocation(start.add(m));
if (!target.in(environment.getArea())) {
// check if it's a junction location
if (target.getLane() == -1) {
Integer exitPoint = null;
Integer startOffset = MathsUtils.mod(start.getOffset(), roadEnvironmentService.getLength());
Integer targetOffset =
MathsUtils.mod(target.getOffset(), roadEnvironmentService.getLength());
if (startOffset > targetOffset) {
startOffset = startOffset - roadEnvironmentService.getLength();
}
for (int junction : roadEnvironmentService.getJunctionLocations()) {
if ((startOffset <= junction) && (targetOffset >= junction)) {
exitPoint = junction;
logger.info("Agent " + actor + " left the road at " + exitPoint);
eventBus.publish(new AgentLeftScenario(actor, exitPoint, SimTime.get()));
// turning off means avoiding the possibility of crashing... ohwells: sliproads are
// LOOOONG
return null;
}
}
logger.warn(
"Agent "
+ actor
+ " tried to turn off using move "
+ m
+ " between "
+ start
+ " and "
+ target
+ " and offsets "
+ startOffset
+ ","
+ targetOffset
+ " but was not near a junction");
}
/*if ( (target.getLane()==-1) && (roadEnvironmentService.isJunctionOffset(target.getOffset())) ) {
// do stuff
logger.info("Agent " + actor + " left the road at " + target.getOffset());
eventBus.publish(new AgentLeftScenario(actor,target.getOffset(), SimTime.get()));
// turning off means avoiding the possibility of crashing... ohwells: sliproads are LOOOONG
return null;
}
else {*/
try {
final Move mNew = environment.getArea().getValidMove(start, m);
target = new RoadLocation(start.add(mNew));
} catch (EdgeException e) {
throw new ActionHandlingException(e);
}
// }
}
this.getLocationService().setAgentLocation(actor, target);
this.speedService.setAgentSpeed(actor, (int) m.getY());
checks.add(new CollisionCheck(actor, start, target));
logger.info(actor + " move: " + m);
return null;
}
