math_utils.c
Go to the documentation of this file.
00001 #include <assert.h>
00002 #include <gsl/gsl_nan.h>
00003 
00004 #include "csm_all.h"
00005 
00006 int minmax(int from, int to, int x) {
00007         return GSL_MAX(GSL_MIN(x,to),from);
00008 }
00009 
00010 void possible_interval(
00011         const double *p_i_w, LDP ld, 
00012         double max_angular_correction_deg, double max_linear_correction, int*from, int*to, int*start_cell) 
00013 {
00014         double angle_res = (ld->max_theta-ld->min_theta)/ld->nrays;
00015 
00016         /* Delta for the angle */
00017         double delta = fabs(deg2rad(max_angular_correction_deg)) +
00018                 fabs(atan(max_linear_correction/norm_d(p_i_w)));
00019 
00020         /* Dimension of the cell range */
00021         int range = (int) ceil(delta/angle_res);
00022 
00023         /* To be turned into an interval of cells */
00024         double start_theta = atan2(p_i_w[1], p_i_w[0]);
00025         
00026         /* Make sure that start_theta is in the interval [min_theta,max_theta]. 
00027            For example, -1 is not in [0, 2pi] */
00028         if(start_theta<ld->min_theta) start_theta += 2*M_PI;
00029         if(start_theta>ld->max_theta) start_theta -= 2*M_PI;
00030         
00031         *start_cell  = (int)
00032                 ((start_theta - ld->min_theta) / (ld->max_theta-ld->min_theta) * ld->nrays);
00033 
00034         *from = minmax(0,ld->nrays-1, *start_cell-range);
00035         *to =   minmax(0,ld->nrays-1, *start_cell+range);
00036 
00037         if(0)
00038         printf("from: %d to: %d delta: %f start_theta: %f min/max theta: [%f,%f] range: %d start_cell: %d\n",
00039                 *from, *to,
00040                 delta,start_theta,ld->min_theta,ld->max_theta, range, *start_cell);
00041 }
00042 
00043 
00044 
00045 
00046 int distance_counter = 0;
00047 
00048 double distance_squared_d(const double a[2], const double b[2]) {
00049         distance_counter++;
00050         double x = a[0]-b[0];
00051         double y = a[1]-b[1];
00052         return x*x + y*y;
00053 }
00054 
00055 double distance_d(const double a[2], const double b[2]) {
00056         return sqrt(distance_squared_d(a,b));
00057 }
00058 
00059 
00060 int is_nan(double v) {
00061         return v == v ? 0 : 1;
00062 }
00063 
00064 int any_nan(const double *d, int n) {
00065         int i; for(i=0;i<n;i++) 
00066                 if(is_nan(d[i]))
00067                         return 1;
00068         return 0;
00069 }
00070 
00071 double norm_d(const double p[2]) {
00072         return sqrt(p[0]*p[0]+p[1]*p[1]);
00073 }
00074 
00075 double deg2rad(double deg) {
00076         return deg * (M_PI / 180);
00077 }
00078 
00079 double rad2deg(double rad) {
00080         return rad * (180 / M_PI);      
00081 }
00082 
00083 void copy_d(const double*from, int n, double*to) {
00084         int i; for(i=0;i<n;i++) to[i] = from[i];
00085 }
00086 
00087 void ominus_d(const double x[3], double res[3]) {
00088         double c = cos(x[2]);
00089         double s = sin(x[2]);
00090         res[0] = -c*x[0]-s*x[1];
00091         res[1] =  s*x[0]-c*x[1];
00092         res[2] = -x[2];
00093 }
00094 
00096 void oplus_d(const double x1[3], const double x2[3], double res[3]) {
00097         double c = cos(x1[2]);
00098         double s = sin(x1[2]);
00099         double x = x1[0]+c*x2[0]-s*x2[1];
00100         double y = x1[1]+s*x2[0]+c*x2[1];
00101         double theta = x1[2]+x2[2];
00102         res[0]=x;
00103         res[1]=y;
00104         res[2]=theta;
00105 }
00106 
00107 
00108 void transform_d(const double point2d[2], const double pose[3], double result2d[2]) {
00109         double theta = pose[2];
00110         double c = cos(theta); double s = sin(theta);
00111         result2d[0] = pose[0] + c * point2d[0] - s * point2d[1];
00112         result2d[1] = pose[1] + s * point2d[0] + c * point2d[1];
00113 }
00114 
00115 void pose_diff_d(const double pose2[3], const double pose1[3], double res[3]) {
00116         double temp[3];
00117         ominus_d(pose1, temp);
00118         oplus_d(temp, pose2, res);
00119         
00120         while(res[2] > +M_PI) res[2] -= 2*M_PI;
00121         while(res[2] < -M_PI) res[2] += 2*M_PI;
00122 }
00123 
00124 double square(double x) {
00125         return x*x;
00126 }
00127 
00128 double angleDiff(double a, double b) {
00129         double t = a - b;
00130         while(t<-M_PI) t+= 2*M_PI;
00131         while(t>M_PI)  t-= 2*M_PI;
00132         return t;
00133 }
00134 
00135 void projection_on_line_d(const double a[2],
00136         const double b[2],
00137         const double p[2],
00138         double res[2], double *distance)
00139 {
00140         double t0 = a[0]-b[0];
00141         double t1 = a[1]-b[1];
00142         double one_on_r = 1 / sqrt(t0*t0+t1*t1);
00143         /* normal */
00144         double nx = t1  * one_on_r ;
00145         double ny = -t0 * one_on_r ;
00146         double c= nx, s = ny; 
00147         double rho = c*a[0]+s*a[1];
00148 
00149         res[0] =   c*rho + s*s*p[0] - c*s*p[1] ;
00150         res[1] =   s*rho - c*s*p[0] + c*c*p[1] ;        
00151         
00152         if(distance)
00153                 *distance = fabs(rho-(c*p[0]+s*p[1]));
00154 }
00155 
00156 void projection_on_segment_d(
00157         const double a[2],
00158         const double b[2],
00159         const double x[2],
00160         double proj[2]) 
00161 {
00162         projection_on_line_d(a,b,x,proj,0);
00163         if ((proj[0]-a[0])*(proj[0]-b[0]) +
00164             (proj[1]-a[1])*(proj[1]-b[1]) < 0 ) {
00165                 /* the projection is inside the segment */
00166         } else 
00167                 if(distance_squared_d(a,x) < distance_squared_d(b,x)) 
00168                         copy_d(a,2,proj);
00169                 else
00170                         copy_d(b,2,proj);
00171 }
00172 
00173 
00174 double dist_to_segment_squared_d(const double a[2], const double b[2], const double x[2]) {
00175         double projection[2];
00176         projection_on_segment_d(a, b, x, projection);
00177         double distance_sq_d = distance_squared_d(projection, x);
00178         return distance_sq_d;
00179 }
00180 
00181 double dist_to_segment_d(const double a[2], const double b[2], const double x[2]) {
00182         double proj[2]; double distance;
00183         projection_on_line_d(a,b,x,proj, &distance);
00184         if ((proj[0]-a[0])*(proj[0]-b[0]) +
00185             (proj[1]-a[1])*(proj[1]-b[1]) < 0 ) {
00186                 /* the projection is inside the segment */
00187                 return distance;
00188         } else 
00189                 return sqrt(GSL_MIN( distance_squared_d(a,x), distance_squared_d(b,x)));
00190 }
00191 
00192 int count_equal(const int*v, int n, int value) {
00193         int num = 0, i;
00194         for(i=0;i<n;i++) if(value == v[i]) num++;
00195         return num;
00196 }
00197 
00198 double normalize_0_2PI(double t) {
00199         if(is_nan(t)) {
00200                 sm_error("Passed NAN to normalize_0_2PI().\n");
00201                 return GSL_NAN;
00202         }
00203         while(t<0) t+=2*M_PI;
00204         while(t>=2*M_PI) t-=2*M_PI;
00205         return t;
00206 }
00207 
00208 double dot_d(const double p[2], const double q[2]);
00209 
00210 
00211 double dot_d(const double p[2], const double q[2]) {
00212         return p[0]*q[0] + p[1]*q[1];
00213 }
00214 
00215 /* Executes ray tracing for a segment. p0 and p1 are the segments extrema, eye is the position
00216 of the eye, and direction is the direction of the ray coming out of the eye. Returns true
00217 if the ray intersects the segment, and in that case *range contains the length of the ray. */
00218 int segment_ray_tracing(const double p0[2], const double p1[2], const double eye[2], double direction, double*range) {
00219         
00220         *range = NAN;
00221         
00222         // p0 - p1
00223         double arrow[2] = {p1[0]-p0[0],p1[1]-p0[1]};
00224         // Normal to segment line
00225         double S[2] = { -arrow[1], arrow[0]};
00226         // Viewing direction
00227         double N[2] = { cos(direction), sin(direction)};
00228         // If S*N = 0 then they cannot cross
00229         double S_dot_N = dot_d(S,N);
00230         if( S_dot_N == 0) return 0;
00231         // Rho of the line in polar coordinates (multiplied by |S|)
00232         double line_rho = dot_d(p0,S);
00233         // Rho of the eye  (multiplied by |S|)
00234         double eye_rho = dot_d(eye,S);
00235         // Black magic
00236         double dist = (line_rho - eye_rho) / S_dot_N;
00237         if(dist<=0) return 0;
00238         
00239         // Now we check whether the crossing point
00240         // with the line lies within the segment
00241         
00242         // Crossing point
00243         double crossing[2] = {eye[0] + N[0]*dist, eye[1]+N[1]*dist};
00244         // Half of the segment
00245         double midpoint[2] = { 0.5*(p1[0]+p0[0]),0.5*(p1[1]+p0[1])};
00246         
00247         double seg_size = distance_d(p0, p1);
00248         double dist_to_midpoint = distance_d(crossing, midpoint);
00249         
00250         if(dist_to_midpoint > seg_size/2 )
00251                 return 0;
00252         
00253         *range = dist;
00254         return 1;
00255 }
00256 
00257 double segment_alpha(const double p0[2], const double p1[2]) {
00258         double arrow[2] = {p1[0]-p0[0],p1[1]-p0[1]};
00259         // Normal to segment line
00260         double S[2] = { -arrow[1], arrow[0]};
00261         return atan2(S[1], S[0]);
00262 }
00263 
00264 
00265 static char tmp_buf[1024];
00266 const char* friendly_pose(const double*pose) {
00267         sprintf(tmp_buf, "(%4.2f mm, %4.2f mm, %4.4f deg)",
00268                 1000*pose[0],1000*pose[1],rad2deg(pose[2]));
00269         return tmp_buf;
00270 }
00271 
00272 
00273 double max_in_array(const double*v, int n) {
00274         assert(n>0);
00275         double m = v[0];
00276         int i; 
00277         for(i=0;i<n;i++)
00278                 if(v[i]>m) m = v[i];
00279         return m;
00280 }


csm
Author(s): Andrea Censi
autogenerated on Mon Jan 16 2017 03:48:29