TreeMap<Double, AbstractAgent2D<?>> agentsInSensorField = new TreeMap<Double, AbstractAgent2D<?>>();
Random rand = new Random();
AbstractEnvironment2D<?> env = body.getEnvironment();
Vector2D bodyPos = new Vector2D(env.getAgentPosition(body.id()));
//add all AbstractAgent2D except body to check list
List<Integer> agentsToCheck = new ArrayList<Integer>();
for (Object o : env.getAgents()) {
if (o instanceof AbstractAgent2D && !((AbstractAgent2D<?>) o).equals(body)) {
AbstractAgent2D<?> agent = (AbstractAgent2D<?>) o;
agentsToCheck.add(agent.id());
}
}
for(double d = cSensorPrecision; d <= cSensorReach; d += cSensorPrecision){
Vector2D sensorLOV = env.getNormalizedLOV(body.id());
sensorLOV.rotate(Vector2D.NULL_VECTOR, angleToAgent);
sensorLOV.setLength(d);
//construct point on sonar line with a distance of D to body
Vector2D sonarPoint = new Vector2D(sensorLOV).translate(bodyPos);
//a = (x, y) -> b = (-x, y) ==> perpendicular vector
Vector2D sonarLOV = new Vector2D(-sensorLOV.x, sensorLOV.y);
double sonarLength = Math.tan(cSensorWidth/2) * d;
// construct direction vector pointing to sonar line end points perpendicular to sensorLOV and of appropriate length
sonarLOV.setLength(sonarLength);
LineSegment2D sonarLine = new LineSegment2D(
new Vector2D(sonarPoint).translate(sonarLOV),
new Vector2D(sonarPoint).sub(sonarLOV));
for (Iterator<Integer> iterator = agentsToCheck.iterator(); iterator.hasNext();) {
Integer agentID = iterator.next();
//for all remaining agents check for intersection
List<Vector2D> isPoints = env.getAgentShapeInEnvironment(agentID).intersectAll(sonarLine);