File content as of revision 56:456d454d9ced:

#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 angle_normal(double angle, double min, double max)
{
    while (angle < min) angle += (max-min);
    while (angle > max) angle -= (max-min);
    return 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 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);
}

double arr_avg(double *arr, int amount)
{
    return arr_sum(amount,arr)/amount;
}

double arr_degangleavg(double *a, int numb)
{
    double x = 0;
    double y = 0;
    for ( int i = 0; i < numb; i++ ) {
        x += cos(ToRad(a[i]));
        y += sin(ToRad(a[i]));
    }
    return ToDeg(atan2(y, x));
}

double arr_sum(int a, double *arr)
{
    double cummul = 0;
    for ( int i = 0; i < a; i++ ) {
        cummul += arr[i];
    }
    return cummul;
}

double arr_radangleavg(double *a, int numb)
{
    double x = 0;
    double y = 0;
    for ( int i = 0; i < numb; i++ ) {
        x += cos(a[i]);
        y += sin(a[i]);
    }
    return atan2(y, x);
}

void arr_pushback(double new_num, double *arr, int elements)
{
    for( int i = 0; i < elements; i++ ) {
        arr[i+1] = arr[i];
    }
    arr[0] = new_num;
}