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]); }