math_utils.h

Go to the documentation of this file.

#ifndef H_MATH_UTILS
#define H_MATH_UTILS


/* Sometimes I really don't understand compilers.. */ 
#ifndef M_PI
#define M_PI           3.14159265358979323846
#endif

#ifndef NAN
#define NAN GSL_NAN
#endif
     
double norm_d(const double p[2]);

double distance_d(const double a[2], const double b[2]);
double distance_squared_d(const double a[2], const double b[2]);

double angleDiff(double a, double b);
double square(double x);

double deg2rad(double deg);

double rad2deg(double rad);


int minmax(int from,int to,int x);

void copy_d(const double*from, int n, double*to);

void ominus_d(const double x[3], double res[3]);
void oplus_d(const double x1[3], const double x2[3], double res[3]);
void pose_diff_d(const double second[3], const double first[3], double res[3]);
        
        
void transform_d(const double point2d[2], const double pose[3], double result2d[2]);
        
void projection_on_line_d(
        const double a[2],
        const double b[2],
        const double p[2],
        double res[2],
        double *distance);
        
void projection_on_segment_d(
        const double a[2],
        const double b[2],
        const double P[2],
   double proj[2]);
        
double dist_to_segment_d(const double a[2], const double b[2], const double x[2]);

double dist_to_segment_squared_d(const double a[2], const double b[2], const double x[2]);

/* Executes ray tracing for a segment. p0 and p1 are the segments extrema, eye is the position
of the eye, and direction is the direction of the ray coming out of the eye. Returns true
if the ray intersects the segment, and in that case *range contains the length of the ray. */
int segment_ray_tracing(const double p0[2], const double p1[2], const double eye[2], double direction, double*range);

double segment_alpha(const double p0[2], const double p1[2]);

const char* friendly_pose(const double*pose);

int is_nan(double v);

int any_nan(const double *d, int n);

int count_equal(const int*v, int n, int value);

double normalize_0_2PI(double angle);

double max_in_array(const double*v, int n);

#endif