* @return Entry<Color,Double>
*/
@SuppressWarnings("rawtypes")
public Entry<Integer, Color> getSensedColor() {
AbstractEnvironment2D env = body.getEnvironment();
Vector2D bodyPos = 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 && env.isCollidingAgent(((AbstractAgent2D<?>) o).id())
&& !((AbstractAgent2D<?>) o).equals(body)) {
AbstractAgent2D<?> agent = (AbstractAgent2D<?>) o;
agentsToCheck.add(agent.id());
}
}
//construct sensor LOV
Vector2D sensorLOV = env.getNormalizedLOV(body.id());
sensorLOV.rotate(Vector2D.NULL_VECTOR, angleToAgent);
sensorLOV.setLength(cSensorReach);
LineSegment2D sensorLine = new LineSegment2D(
new Vector2D(bodyPos),
new Vector2D(bodyPos).translate(sensorLOV));
//find closest agent
Double minDistance = Double.POSITIVE_INFINITY;
AbstractAgent2D closestAgent = null;
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(sensorLine);
if(!isPoints.isEmpty()){
//find intersection point with least distance
for (Vector2D isPoint : isPoints) {
isPoint.sub(bodyPos);
if(Double.compare(isPoint.length(), minDistance) < 0){
minDistance = isPoint.length();
closestAgent = env.getAgent(agentID);
}
}
}
}