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