#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);
|
}
|
