Behavior.java

import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;
import simbad.sim.Agent;
import simbad.sim.LightSensor;
import simbad.sim.RangeSensorBelt;

public class Behavior {
    private static final double LightValueTolerance = 0.000001;
    private static final double maxTranslationalVelocity = 0.2;
    private static final double maxRotationalVelocity = 3;
    private static final double SAFETY = 1.2;

    public static double getAverageLux(LightSensor s, int nSamples){
        double sampleSum=0;
        for(int i=0; i<nSamples; i++){
            double lux = Math.pow(s.getLux(), 0.1);
            sampleSum+=lux;

        }
        return sampleSum/nSamples;
    }

    public static void orient(Agent robot, double left, double right){
        // rotate the robot so that it faces the source
        // positive rot velocity = counter-clockwise
        if (Math.abs(left - right) > LightValueTolerance){
            if(left > right){
                robot.setRotationalVelocity(maxRotationalVelocity);
            }
            else{
                robot.setRotationalVelocity(-maxRotationalVelocity);
            }
        }
        else{
            // right orientation
            robot.setRotationalVelocity(0);
        }
    }

    /**
     * Method that stops the robot
     * @param robot Robot instance
     */
    public static void stop(Agent robot){
        robot.setRotationalVelocity(0);
        robot.setTranslationalVelocity(0);
    }

    /**
     * Method that tells us if there is an object within a specified range.
     * @param sonar_belt sonars around the robot
     * @return true if obstacle is found, false otherwise
     */
    public static boolean sensedObstacle(RangeSensorBelt sonar_belt){
        for(int i=0; i<sonar_belt.getNumSensors(); i++){
            // check only front sonars
            if(Math.abs(sonar_belt.getSensorAngle(i)) >= Math.PI / 4 && sonar_belt.getMeasurement(i) < SAFETY){ // SONAR MAX VALUES = 1.5!!!
                return true;
            }
        }
        return false;
    }

    /**
     * Moves the robot forward while not orbiting an obstacle
     * @param robot Robot instance
     * @param left sensor measurement
     * @param right sensor measurement
     */
    public static void move_forward(Agent robot, double left, double right){
        // Fixed translational velocity
        robot.setTranslationalVelocity(1.35 * maxTranslationalVelocity);
        // Rotational velocity is set according to the difference of the measurements
        robot.setRotationalVelocity((left - right) * 200);
    }

    /* From Tools class,by dvrakas */
    private static double wrapToPi(double a){
        if (a > Math.PI)
            return a - Math.PI*2;
        if (a <= -Math.PI)
            return a + Math.PI*2;
        return a;
    }

    /* From Tools class,by dvrakas */
    public static Point3d calculateHitPoint(Agent robot, RangeSensorBelt sonars, int sonar){
        double v;
        if (sonars.hasHit(sonar))
            v=robot.getRadius()+sonars.getMeasurement(sonar);
        else
            v=robot.getRadius()+sonars.getMaxRange();
        double x = v*Math.cos(sonars.getSensorAngle(sonar));
        double z = v*Math.sin(sonars.getSensorAngle(sonar));
        return new Point3d(x,0,z);
    }

    /**
     * Responsible for behavior of robot while orbiting an obstacle
     * @param robot Robot instance
     * @param sonar Sonar sensors
     */
    public static void followObstacle(Agent robot, RangeSensorBelt sonar){
        //Finds sensor closest to obstacle
        int minSensor = 0;
        for(int i=0; i < sonar.getNumSensors(); i++){
            if(sonar.getMeasurement(i) < sonar.getMeasurement(minSensor)){
                minSensor = i;
            }
        }

        double K1 = 3, K2 = 0.8, K3 = 1;
        Point3d hitPoint = calculateHitPoint(robot, sonar, minSensor);
        double phRot = Math.atan(K3 * (hitPoint.distance(new Point3d(0,0,0)) - SAFETY));

        /* from CircumNaviage, by dvrakas */
        Vector3d v = new Vector3d(hitPoint.z,0,-hitPoint.x);
        double phLin = Math.atan2(v.z,v.x);
        double phRef = wrapToPi(phLin+phRot);
        robot.setRotationalVelocity(K1* phRef);
        robot.setTranslationalVelocity(K2* Math.cos(phRef));
    }
}