/*
        --Michael Chang Spring 2003
        
        Useful programming and math tools mostly dealing with
        2d coordinates and translations, distance formulas, etc.

        Use these however you wish. Steal, pirate, etc.

        Include this file by having this line of code near the
        top of your own code

        #include "tools.h"
*/


/*
        Angle formula:
        x+=cos(M_PI/180*ANGLE)*double(DISTANCE);
        y-=sin(M_PI/180*ANGLE)*double(DISTANCE);

        This is the way you orient anything towards a
        particular angle and, if you want, move it by
        a distance.

        ANGLE being the angle you want to move a point.
        DISTANCE being how far you want to displace it.

        You must perform the operation on x and y seperately,
        although you can easily write a function to do both
        at the same time.
*/


/*
        position is a structure you can use to quickly
        identify and manage pairs of x and y coordinates.
        Remember that y decends vertically on a computer
        moniter, however this can be used for any
        coordinate system.

        example code to make a position:

        position myPoint;
        myPoint.x=5;
        myPoint.y=8;

        this creates a position myPoint that has
        coordinates of 5,8.

        Another way to create a position is to use its
        constructor like so:

        position myPoint2(5,8);

        This creates myPoint2 with coordinates 5,8 as well.

        You can also use member functions to add and subtract
        position coordinates. These either take a double or
        a position.
*/
struct position
{
	double x;
	double y;
        position();
        position(double,double);
        position add(position);
        position add(double);
        position subtract(position);
        position subtract(double);
};
position::position()
{}
position::position(double a,double b)
{
        x=a;
        y=b;
}
position position::add(position b)
{
        position temp;
        temp.x=x+b.x;
        temp.y=y+b.y;
        return temp;
}
position position::add(double b)
{
        position temp(x,y);
        temp.x+=b;
        temp.y+=b;
        return temp;
}
position position::subtract(position b)
{
        position temp;
        temp.x=x-b.x;
        temp.y=y-b.y;
        return temp;
}
position position::subtract(double b)
{
        position temp(x,y);
        temp.x-=b;
        temp.y-=b;
        return temp;
}
/*
        getRange() is a useful function to get the exact distance
        between two position points.

        example:
        myDistance=getRange(myPosition1,myPosition2)

        given that myPosition1 and myPosition2 are of type position.
*/
double getRange(position a, position b)
{
        double x1=a.x;
        double y1=a.y;
        double x2=b.x;
        double y2=b.y;
        double xo = x2-x1;
        double yo = y2-y1;
        double h = sqrt( xo*xo + yo*yo );
        return h;
}
/*
        normalizeHeading() returns you the proper heading
        range in degrees.

        example:
        normalizeHeading(460);

        result:
        100.

        Works for negative values as well. It will take
        types double or any compatible type.

*/
double normalizeHeading(double ang)
{
        if (ang > 360)
        ang -= 360;
        if (ang < 0)
        ang += 360;
        return ang;
}
/*
        absBearing() gets you the absolute bearing in angles
        between any two points, relative to the first point
        you give it. It accepts variables of type position.

        example:
        angle=absBearing(myPosition1,myPosition2);

        It gives you the output in angles between those two points.
*/
double absBearing(position a,position b)
{
        double x1=a.x;
        double y1=a.y;
        double x2=b.x;
        double y2=b.y;
        double xo = x2-x1;
        double yo = y2-y1;
        double h = getRange(a,b);
        if( xo > 0 && yo > 0 )
        {
                return sin( xo / h );
        }
        if( xo > 0 && yo < 0 )
        {
                return M_PI - sin( xo / h );
        }
        if( xo < 0 && yo < 0 )
        {
                return M_PI + sin( -xo / h );
        }
        if( xo < 0 && yo > 0 )
        {
                return 2.0*M_PI - sin( -xo / h );
        }
        return 0;
}
/*
        destination() is an incremental function that will
        return the position you give it modified by the angle
        you want it to be adjusted and the distance to adjust
        it by.

        example:
        destination(myPosition,45,10);

        It will return you the value of myPosition by moving it
        10 units at 45 degrees relative to itself (local axis).
*/
position destination(position p,double direction,double distance)
{
        p.x+=cos(M_PI/180*direction)*double(distance);
        p.y-=sin(M_PI/180*direction)*double(distance);
        return p;
}
/*
        swap() is a simple swap function. It uses references
        so it will actually replace the variables you give it.

        exmaple:
        x=5;
        y=10;
        swap(x,y);

        x will equal 10 and y will equal y.
*/
void swap(double &a,double &b)
{
        double temp=a;
        a=b;
        b=temp;
        return;
}