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);
}
public void act(double time) {
// System.out.println("Ant body:"+antBody);
double[] rv = new double[3];
MutableDouble2D ant = new MutableDouble2D();
boolean sawAnt = antBody.getNearestSameTypeVec(ant);
MutableDouble2D wall = new MutableDouble2D();
boolean sawWall = antBody.getNearestObstacleVec(wall);
MutableDouble2D home = new MutableDouble2D();
boolean sawHome = antBody.getPoiDir(home, "nest");
MutableDouble2D food = new MutableDouble2D();
boolean sawFood = antBody.getNearestPreyVec(food);
double[] sensorVec = new double[FEATURE_DIM];
double[][] nearestK = new double[5][3];
sensorVec[0] = ant.x;
sensorVec[1] = ant.y;
sensorVec[2] = wall.x;
sensorVec[3] = wall.y;
// System.out.println("Sensor vec: ["+sensorVec[0]+", "+sensorVec[1]+", "+sensorVec[2]+",
// "+sensorVec[3]+"]");
sensorVec[4] = home.x;
sensorVec[5] = home.y;
// sensorVec[6] = food.x;
// sensorVec[7] = food.y;
// sensorVec[4] = prevVel[0];
// sensorVec[5] = prevVel[1];
// sensorVec[6] = prevVel[2];
// sensorVec[7] = home.x;
// sensorVec[8] = home.y;
knn.query(sensorVec, nearestK);
// now, do median, average, or random selection
// average
for (int i = 0; i < rv.length; i++) rv[i] = 0.0;
for (int i = 0; i < nearestK.length; i++) {
for (int j = 0; j < nearestK[i].length; j++) {
rv[j] += nearestK[i][j];
}
}
for (int i = 0; i < rv.length; i++) rv[i] = rv[i] / (double) nearestK.length;
// if(rv[0] < 0 ) System.out.println("Movin backwards!");
prevVel[0] = rv[0];
prevVel[1] = rv[1];
prevVel[2] = rv[2];
// System.out.println("rv: ["+rv[0]+", "+rv[1]+", "+rv[2]+"]");
antBody.setDesiredVelocity(rv[0], rv[1], rv[2]);
}
