123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251 |
- /* calc_funcs.c
- *
- * Calculation Functions
- *
- * Ver 1.0 1998.08.25 Keiji nagatani
- * Ver 3.0 2001.02.13 Ryosuke Sakai
- * (rsakai@andrew)
- * Ver 3.2 2002.05.25 Deryck Morales
- * <deryck@alumni.carnegiemellon.edu>
- * Nick Jong
- * <nkj@andrew.cmu.edu>
- */
- #include "calc_funcs.h"
-
-
- /*************************
- * PRIVATE PROTOTYPES *
- *************************/
- double sin_deg(double value);
- double cos_deg(double value);
-
-
- /*************************
- * PUBLIC FUNCTIONS *
- *************************/
-
- /*
- * Scales the given angle to its value in the range: [-180,180] degrees.
- * Units for "angle" are in 1/10 degree (so 180 deg = 1800)
- *
- * The scaled value in 1/10 degrees is returned.
- */
- int fix_angle(int angle)
- {
- while (angle < -1800)
- angle += 3600;
- while (angle > 1800)
- angle -= 3600;
- return(angle);
- }
-
- /*
- * Calculates angle between dir1 and dir2 and scales to the range: [1,3599]
- * This calculated value is then returned.
- * Units are in 1/10 degrees.
- */
- int calc_sup_angle(int dir1, int dir2)
- {
- if ((dir2 - dir1) > 0) return(dir2 - dir1);
- else return(dir2 + 3600 - dir1);
- }
-
-
- /*
- * The angle between x and y is scaled to the range: [0,1800]
- * This calculated value is then returned.
- * Units are in 1/10 degrees.
- */
- int calc_diff_angle(int x, int y)
- {
- int diff;
-
- diff = abs(x - y);
- if (diff > 1800) return(abs(diff - 3600));
- else return(diff);
- }
-
-
- /*
- * The bisector of angles dir1 and dir2 is calculated and returned.
- * Units are in 1/10 degrees.
- */
- int calc_middle_angle(int dir1, int dir2)
- {
- if ((dir2 - dir1) > 0){return(fix_angle((dir2 + dir1)/2));}
- else {return(fix_angle((dir2 + 3600 + dir1)/2));}
- }
-
-
- /*
- * Angle "val" is converted to (160 index), which corresponds to the ring of 16 sonar.
- * This index value is then returned.
- * Units are 1/10 degrees and 160 index (1/10th of 16).
- */
- int deg_to_index_160(int val)
- {
- int idx;
- idx = val * 2 / 45; /* (idx = val/22.5) */
- if (idx < 0) idx += 160;
- return(idx);
- }
-
-
- /*
- * 160 index value "idx" is converted to 1/10 degrees.
- * This angle is then returned.
- * Units are 1/10 degrees and 160 index. (see deg_to_index_160).
- */
- int index_160_to_deg(int idx)
- {
- return(fix_angle(idx * 45 / 2));
- }
-
-
- /*
- * Rotate <x,y> ==> <nx,ny> by theta
- * The point (x,y) is rotated about the origin by "ang" degrees.
- * The resultant point is stored to "nx" and "ny" (new x and y)
- */
- void calc_rotation(int x, int y, int ang, int *nx, int *ny)
- {
- double dx, dy;
-
- dx = (double)x; dy = (double)y;
- *nx = (int)(dx * cos_deg((double)ang/10.0) - dy*sin_deg((double)ang/10.0));
- *ny = (int)(dx * sin_deg((double)ang/10.0) + dy*cos_deg((double)ang/10.0));
- }
-
-
- /*
- * Convert <degree, value> ==> <x,y>
- * Polar to rectangular conversion.
- *
- * ^ \
- * *y | / r = val
- * | / deg = deg
- * |/\
- * +---> *x
- */
- void calc_XY_from_deg_val(int deg, int val, int *x, int *y)
- {
- double ddeg;
-
- /* calculate radian */
- ddeg = ((double)deg)*PAI/1800.0;
-
- /* calculate distance */
- *x = (int)(cos(ddeg)*(double)val);
- *y = (int)(sin(ddeg)*(double)val);
- }
-
-
- /*
- * Convert <vx, vy> ==> <degree>
- * Given vector coordinates vx,vy, calculate vector angle.
- * This calculated value is returned.
- * Units are 1/10 degrees.
- */
- int calc_vec_to_deg(int vx, int vy)
- {
- double deg;
-
- deg = atan2((double)vy, (double)vx)*1800.0 / PAI;
- if (deg < 0.0) deg+=3600.0;
-
- return((int)deg);
- }
-
- /* -------------------------------------------
- * calculate the point on line L<lx,ly,ldir>
- * which is vertical to P<px,py>
- * Called by:
- * ROBOT-FUNCS\trace.c
- *
- * point P
- * |
- * |
- * lineL ---L---R---------
- * -------------------------------------------
- */
- void calc_vertical_point_on_line(int px, /* (i) point P */
- int py, /* (i) point P */
- int lx, /* (i) point on line L */
- int ly, /* (i) point on line L */
- int ldir, /* (i) deg of line L */
- int *result_x, /* (o) the point on line L which is vertical to P */
- int *result_y) /* (o) the point on line L which is vertical to P */
- {
- int vx, vy, a, b;
-
- /* convert <degree, value> ==> <x,y>
- * ^ \
- * vy | / r = 100
- * | / deg = ldir
- * |/\
- * +---> vx
- */
- calc_XY_from_deg_val(ldir, /* (i) deg */
- 100, /* (i) radius */
- &vx, /* (o) vx = 100 * cos(ldir) */
- &vy); /* (o) vy = 100 * sin(ldir) */
-
- a = vx*px + vy*py;
- b = vy*lx - vx*ly;
-
- *result_x = (vx*a + vy*b ) / 10000;
- *result_y = (vy*a - vx*b ) / 10000;
- }
-
-
- /*************************
- * PRIVATE FUNCTIONS *
- *************************/
-
- /*
- * Double absolute value
- */
- double dabs(double val)
- {
- if (val < 0) return(-val);
- else return( val);
- }
-
- /*
- * Cosine where value is in degrees (normally radians)
- */
- double cos_deg(double value)
- {
- return(cos(value * PAI / 180.0));
- }
-
- /*
- * Sine where value is in degrees (normally radians)
- */
- double sin_deg(double value)
- {
- return(sin(value * PAI / 180.0));
- }
-
- /*
- * Returns 0 if imaginary solutions, otherwise
- * updates x1,x2 with equation roots and returns 1
- */
- int calc_quadratic_equation(double a, double b, double c, double *x1, double *x2)
- {
- double d;
-
- d = b*b-4*a*c;
-
- if ( d < 0.0) {
- return(0);
- }else {
- *x1 = (-b+sqrt(d))/(2*a);
- *x2 = (-b-sqrt(d))/(2*a);
-
- return(1);
- }
- }
-
- /* EOF calc_funcs.c */
- /********************************************************************************/
|