[Home]Synnalagma/Utils

Robo Home | Synnalagma | Changes | Preferences | AllPages

I hope this can help some people to avoid some headeaque as I had --Synnalagma
public class Utils {
    /**
     *  Constant for circle / 360 degree
     */
    public final static double CIRCLE = 2 * Math.PI;
    /**
     *  Constant for half circle 180 degree
     */
    public final static double HALF_CIRCLE = Math.PI;


    /**
     *  Classify the angle : angle in one of the nbPas classes determined using maxB, the maximum angle and minB the minimum angle
     *
     * You should prefer using getOutput array
     *
     *@param  angle  angle to classify
     *@param  minB   minimum angle
     *@param  maxB   maximimum angle
     *@param  nbPas  number of different classes
     *@return        the class of angle
     */
    public static int getOutput(double angle, double minB, double maxB, int nbPas) {
        double diffAngle = getMinDiff(minB, maxB);
        double diffB = getMinDiff(minB, angle);
        double re = diffB / diffAngle * (double) nbPas;
        if (Math.abs(re - nbPas) < 0.03) {
            return nbPas - 1;
        }
        if (re < 0 || re >= nbPas) {
            return -1;
        }
        return (int) re;
    }


    /**
     *  Classify angle in the nbPas classes determined using maxB, the maximum angle and minB the minimum angle
     *
     * return an array that can holds 0 or 1. 1 mean that the angle is contained in this class
     *
     *@param  angle  angle to classify
     *@param  error  Acceptable error to classify the angle that can be Math.atan(ROBOT_RADIUS/robot_distance)
     *@param  minB   minimum angle
     *@param  maxB   maximimum angle
     *@param  nbPas  number of different classes
     *@return        an array containing 0 and 1
     */
    public static double[] getArrayOutput(double angle, double error, double minB, double maxB, int nbPas) {
        double[] out = new double[nbPas];
        double diffTotal = getMinDiff(minB, maxB);
        int minIndex = (int) (getMinDiff(minB, angle - error) / diffTotal * (double) nbPas);
        int maxIndex = (int) (getMinDiff(minB, angle + error) / diffTotal * (double) nbPas);
        if ((minIndex < 0 || minIndex >= nbPas) && (maxIndex < 0 || maxIndex >= nbPas)) {
            return out;
        }
        minIndex = Math.max(0, Math.min(nbPas - 1, minIndex));
        maxIndex = Math.max(0, Math.min(nbPas - 1, maxIndex));
        int limit = Math.max(minIndex, maxIndex) + 1;
        for (int i = Math.min(maxIndex, minIndex); i < limit; i++) {
            out[i] = 1;

        }
        return out;
    }


    /**
     *  Return the minimum angular difference between min and max
     *
     *@param  min  first angle
     *@param  max  the other one
     *@return      minimum angular difference, can be negative
     */
    public static double getMinDiff(double min, double max) {
        double diffAngle = max - min;
        if (Math.abs(diffAngle) > HALF_CIRCLE) {
            if (max > min) {
                return -(CIRCLE - max + min);
            } else {
                return (CIRCLE - min + max);
            }
        }
        return diffAngle;
    }


    /**
     *  Exaclty inverse of getOuput
     *
     *@param  minB   minimum angle
     *@param  maxB   maximimum angle
     *@param  index  class of the angle we want to find
     *@param  nbPas  number of different classes
     *@return        angle found
     */
    public static double getAngleFromIndex(double min, double max, int index, int nbPas) {
        double diff = getMinDiff(min, max) / (double) nbPas;
        return absoluteAngle(((double) index * diff + (double) (index + 1) * diff) / 2 + min);
    }


    /**
     *  return an angle between 0 and Math.PI*2
     *
     *@param  angle  angle
     *@return
     */
    public static double absoluteAngle(double angle) {
        while (angle < 0) {
            angle += CIRCLE;
        }
        return angle % CIRCLE;
    }


    /**
     *  return an angle between -Math.PI and Math.PI
     *
     *@param  angle  Description of the Parameter
     *@return        Description of the Return Value
     */
    public static double relativeAngle(double angle) {
        while (angle > HALF_CIRCLE) {
            angle -= CIRCLE;
        }
        while (angle < -HALF_CIRCLE) {
            angle += CIRCLE;
        }
        return angle;
    }


    /**
     *  return the max distance that a bot can do in a certain time
     *
     *@param  velocity  velocity of the bot
     *@param  time      time that the bot is going
     *@param  sign      1 for going forward, -1 for backward
     *@return           distance max
     */
    public static double getMaxDist(double velocity, double time, int sign) {
        double dist = 0;
        while (time > 0) {
            dist += Math.max(-8, Math.min(8, ((sign * velocity) > 0 ? (velocity += sign) : (velocity += sign * 2))));
            time--;
        }
        return dist;
    }


    /**
     *  get the maximum angle that a bot can go assuming is going always at the same distance (on a circle of radius robot_distance)
     *
     *@param  heading   heading of the bot should be absolute
     *@param  bearing   bearing of the bot
     *@param  velocity  velocity of the bot
     *@param  distance  distance of the bot
     *@param  time      the time for the bullet to reach the bot
     *@param  sign      1 for forward, -1 for backward
     *@return           The maxAngle value
     */
    public static double getMaxAngle(double heading, double bearing, double velocity, double distance, double time, int sign) {
        bearing = absoluteAngle(bearing);
        return absoluteAngle(bearing+(heading>Math.PI?-sign:sign)* ((bearing>Math.PI/2&&bearing<1.5*Math.PI)?-1:1)* Math.abs(Utils.getMaxDist(velocity,time, sign))/distance);
    }

}


====Comment suggestions welcome====

This code is a bit risky:

public static double absoluteAngle(double angle) {
        while (angle < 0) {
            angle += CIRCLE;
        }
        return angle % CIRCLE;
    }

I once used something similar and got caught more than once in infinite loops, when angle == Double.NEGATIVE_INFINITY. I sugest something like this:

public static double absoluteAngle(double angle) {
        angle %= CIRCLE;
        if (angle < 0) angle += CIRCLE;
        return angle;
    }

-- ABC

Shouldn't the Math.IEEERemainder() (I think that's where it is) function normalize the absolute angle to positive for you? -- Vuen

I don't get it exactly but as I understand IEEERemainder it keep the sign of the dividend. but it could be used as a first step. I never had problem with infinite loop with this. --Synnalagma

Maybe if you try fighting WhatTheHeck? :-p -- Kawigi

Math.IEEERemainder is in fact normalizeAngle just do Math.IEEERemainder(angle,2*Math.PI). --Synnalagma

Solving the problem in a direct way.

 /** Note: you need an appropriate implementation of the functions mentioned here. Also
  * note that zero heading vector and its normal could be hardcoded if needed. For the sake of
  * example, let's not do that here.
  * @param angle The angle to be normalized
  * @return Normalized angle value, in the range (-PI, PI]
  */
 public double absoluteAngle(double angle) {
   Vector unit = Math.unitvec(angle); // get the unit vector with angle as bearing
   Vector zero = Math.unitvec(0);     // get the vector with angle 0
   Vector norm = Math.getNormal(zero);// get the normal to unit, rotated in positive direction
   double cosangle = Math.cosangle(unit,zero); // get the cosine of the angle, using dot product
   int sign = Math.sign( Math.dot(unit, norm) );
   double absangle = sign * java.lang.Math.acos(cosangle);
   return absangle;
 }

Using dot product etc is quite a good way to do away with awkward angle normalization. I have not seen that using classes such as Vector and more operations slow things down. -- filmil


Robo Home | Synnalagma | Changes | Preferences | AllPages
Edit text of this page | View other revisions
Last edited May 6, 2004 0:18 EST by Filmil (diff)
Search: