public void act(double time) {
// our default is to move forward in a straight line
double forwardSpeed = 0.024; // 24mm per second straight ahead
double lateralSpeed = 0.0; // Ants *can* move laterally, but ours won't for now
double turningSpeed = 0.0; // no rotational velocity by default
// get a vector towards the nearest thing so we can avoid it
MutableDouble2D ant = new MutableDouble2D();
boolean sawAnt = antBody.getNearestSameTypeVec(ant);
MutableDouble2D wall = new MutableDouble2D();
boolean sawWall = antBody.getNearestObstacleVec(wall);
MutableDouble2D avoidPoint = null;
if (!sawWall && !sawAnt) {
avoidPoint = null;
} else if (!sawAnt) {
avoidPoint = wall;
} else if (!sawWall) {
avoidPoint = ant;
} else {
avoidPoint = (ant.lengthSq() < wall.lengthSq()) ? ant : wall;
}
if (avoidPoint != null) {
if (avoidPoint.y > 0) turningSpeed = -40.0 * (Math.PI / 180.0);
else turningSpeed = 40.0 * (Math.PI / 180.0);
}
antBody.setDesiredVelocity(forwardSpeed, lateralSpeed, turningSpeed);
}
