File content as of revision 55:8561b35130fc:

#include "utilities.h"
#include "Point.h"

double heading_err(double angle1, double angle2)
{
    double angle = ((int)((angle2-90 )* -1 + 360) + 180) % 360 - 180;
    double diff_angle = angle1 - angle;
    diff_angle = ((int)diff_angle + 180) % 360 - 180;
    if ( abs(diff_angle) > 90 ) {
        if ( (abs(360 - diff_angle)) > 90 ) {
            diff_angle = abs(360 - diff_angle);
        }
    }
    if ( abs(diff_angle) > 90 ) {
        diff_angle = 0;
    }
    return diff_angle;
}

double calculateDifferenceBetweenAngles(double firstAngle, double secondAngle)
{
    double difference = secondAngle - firstAngle;
    while (difference < -180) difference += 360;
    while (difference > 180) difference -= 360;
    return difference;
}


double ToDeg(double val)
{
    return val * 57.2957795131;
}

double ToRad(double val)
{
    return val / 57.2957795131;
}

/*double map(double variable, double min, double max, double newmin, double newmax)
{
    variable = variable / ( max - min );
    variable *= ( newmax - newmin );
    variable += newmin;
    return variable;
}*/
double map(double x, double in_min, double in_max, double out_min, double out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

double constrains(double variable, double min, double max)
{
    if ( variable < min )
        variable = min;
    if ( variable > max )
        variable = max;
    return variable;
}

double dead_band(double variable, double cut_at)
{
    int sign = (int)pow( variable, 0 );
    variable = abs( variable );
    if ( variable < cut_at )
        variable = cut_at;
    variable *= sign;
    return variable;
}

double head_err(Point pos, Point ls, Point le, Point forward, double track_north, double lookA)
{
    double m = ( le.GetY() - ls.GetY() ) / ( le.GetX() - ls.GetX() );
    double b = ls.GetY() - m * ls.GetY();
    double bsec = forward.GetX()/m+forward.GetY();
    double x =  (forward.GetX()+m*forward.GetY()-b*m )/(2*m*m);
    double y = m * x + b;
    double theta = (ToDeg((1/sin((y-pos.GetY())/(x-pos.GetX()))))-90)*-1;
    return heading_err(theta,track_north);
}