#include "math_utils.h"
#include <math.h>


double MathUtils::angle_to_radian(double degree)
{
    double flag = (degree < 0)? -1.0:1.0; 
    if(degree<0)
    {
       degree = degree * (-1.0);
    }
    double angle = degree;
    double result = flag * (angle* M_PI)/180;
    return result;
}


void MathUtils::radian_to_angle(double rad, double ang[])
{
    double flag = (rad < 0)? -1.0 : 1.0;
    if(rad<0)
    {
        rad = rad * (-1.0);
    }
    double result = (rad*180)/ M_PI;
    double degree = int(result);
    double min =(result - degree)*60;
    double second = (min - int(min)) * 60;
    ang[0] = flag * degree;
    ang[1] = int(min);
    ang[2] = second;
}

double MathUtils::cal_distance(vector<int> coords_A, vector<int> coords_B)
{
    int square_sum = 0;
    for (int i = 0; i < 3; ++i) {
        square_sum +=  (coords_A[i] - coords_B[i])*(coords_A[i] - coords_B[i]);
    }
    return sqrt(square_sum);
}