math_utils_test.c
Go to the documentation of this file.
00001 #include "csm_all.h"
00002 #include <time.h>
00003 
00004 #include "math_utils.h"
00005 
00006 
00007 inline float myasin(float x) {
00008         const float threshold = .52;
00009         if(x > threshold) {
00010                 static float z, der0, der1,der2,der3;
00011                 static int first = 1;
00012                 if(first) {
00013                         z = 0.5* (threshold + 0.707);
00014                         der0 = asin(z);
00015                         float M = 1-z*z;
00016                         der1 = 1 / sqrt(M);
00017                         der2 = (sqrt(M) * z) / (M*M);
00018                         der3 = (sqrt(M) / ( M * M ) + (3 * (z*z) * sqrt(M) ) / (M * M * M) )  ;
00019                         first = 0;
00020                         printf("%f %f  %f %f \n", der0, der1, der2, der3);
00021                 }
00022                 float m = x-z;
00023                 return der0 + m * ( der1 +  m * ( (der2/2) +   m * (der3/6) ) );
00024         }
00025         else{
00026                 float x2 = x * x;
00027                 float der1 = 1;
00028                 float der3 = 1.0f/6;
00029                 float der5 = 3.0f/40;
00030                 float der7 = 15.0f/ (48*7);
00031                 return x * ( der1 + x2 *( der3 + x2 * (der5  + der7 * x2 )));
00032         }
00033 }
00034 
00035 #define myacos(x)  ( ((float) (M_PI/2) ) - myasin(x))
00036 inline void polar(float x, float y, double* restrict rho, double* restrict theta) {
00037         *rho = sqrtf(x*x+y*y);
00038         if(x > 0) {
00039                 if(y > 0) {
00040                         if(x < y)
00041                                 *theta = myacos(x / *rho);
00042                         else
00043                                 *theta = myasin(y / *rho);
00044                 } else {
00045                         if( x < -y )
00046                                 *theta = - myacos(x / *rho);
00047                         else
00048                                 *theta = - myasin( (-y) / *rho) ;
00049                 } 
00050         } else  {
00051                 if(y > 0) {
00052                         if( (-x) < y)
00053                                 *theta = ((float)M_PI) - myacos(-x / *rho);
00054                         else
00055                                 *theta = ((float)M_PI) - myasin( y / *rho);
00056                 } else {
00057                         if ( (-x) < (-y) )
00058                                 *theta = ((float)M_PI) + myacos(-x / *rho);
00059                         else
00060                                 *theta = ((float)M_PI) + myasin(-y / *rho);
00061                 }
00062         }
00063 }
00064 
00065 inline void polar2(double x, double y, double* restrict rho, double* restrict theta) {
00066         *rho = sqrt(x*x+y*y);
00067         *theta = atan2(y,x);
00068 }
00069 
00070 int main() {
00071         double a[2] = {0, 0};
00072         double b[2] = {5, 5};
00073         double x[5][2] = {
00074                 {0, -10},
00075                 {  2, 1},
00076                 {100, 1},
00077                 {0,0},{5,5}};
00078         
00079         gsl_vector * A = vector_from_array(2,a);
00080         gsl_vector * B = vector_from_array(2,b);
00081 
00082         gsl_vector * res = gsl_vector_alloc(2);
00083         
00084         int i;
00085         #if 0
00086         for(i=0;i<5;i++){
00087                 gsl_vector * X = vector_from_array(2,x[i]);
00088                 
00089 /*              projection_on_segment(A,B,X,res);*/
00090                 
00091                 printf("Projection of %f %f is %f %f \n", gvg(X,0),gvg(X,1),
00092                         gvg(res,0),gvg(res,1));
00093         }
00094         #endif
00095         
00096         int errors = 0;
00097         double should_be_nan[2] = { 0.0 / 0.0, GSL_NAN };
00098         for(i=0;i<2;i++) {
00099                 if(!isnan(should_be_nan[i])) {
00100                         printf("#%d: isnan(%f) failed \n", i, should_be_nan[i]);
00101                         errors++;
00102                 }
00103                 if(!is_nan(should_be_nan[i])) {
00104                         printf("#%d: is_nan(%f) failed \n", i, should_be_nan[i]);
00105                         errors++;
00106                 }
00107         }
00108         
00109         double max_error = 0;
00110         int num = 10000;
00111         for(i=0;i<num;i++) {
00112                 double theta = (i * 2 * M_PI) / (num-1);
00113                 double x = cos(theta), y = sin(theta);
00114                 
00115                 double theta_est, rho;
00116                 polar(x,y,&rho,&theta_est);
00117                 double error = fabs( angleDiff(theta_est,theta) );
00118                 if(error > max_error)
00119                 printf("%d x %f y %f theta %f theta_est %f error %f \n", i, x, y, theta, theta_est, error);
00120                 max_error = GSL_MAX(error, max_error);
00121         }
00122 
00123         int NUM2 = 1000; int k;
00124         float c[num], s[num];
00125         for(i=0;i<num;i++) {
00126                 double theta = (i * 2 * M_PI) / (num-1);
00127                 c[i]= cos(theta); s[i] = sin(theta);
00128         }
00129         
00130         clock_t start1 = clock();
00131         double thetaf,rhof;
00132         for(k=0;k<NUM2;k++)
00133         for(i=0;i<num;i++) {
00134                 polar(c[i],s[i],&rhof, &thetaf);
00135         }
00136         clock_t end1 = clock();
00137         
00138         clock_t start2 = clock();
00139         double rho,theta;
00140         for(k=0;k<NUM2;k++)
00141         for(i=0;i<num;i++) {
00142                 polar2(c[i],s[i],&rho, &theta);
00143         }
00144         clock_t end2 = clock();
00145         
00146         float seconds1 = (end1-start1)/((float)CLOCKS_PER_SEC);
00147         float seconds2 = (end2-start2)/((float)CLOCKS_PER_SEC);
00148         
00149         printf("  polar: %f   polar2: %f\n", seconds1, seconds2);
00150         
00151         printf("Maximum error for polar(): %lf rad = %lf deg\n", max_error, rad2deg(max_error));
00152 /*      printf("Acos() called for   %f  %f\n", min_acos, max_acos);
00153         printf("Asin() called for   %f  %f\n", min_asin, max_asin);
00154 */      return errors;
00155 }


csm_ros
Author(s): Gaƫl Ecorchard , Ivan Dryanovski, William Morris, Andrea Censi
autogenerated on Sat Jun 8 2019 19:52:42