math_utils_gsl.c
Go to the documentation of this file.
00001 #include "csm_all.h"
00002 
00003 void transform(const gsl_vector* p, const gsl_vector* x, gsl_vector*res) {
00004         double theta = gvg(x,2);
00005         double c = cos(theta); double s = sin(theta);
00006         gsl_vector_set(res, 0, c * gvg(p,0) -s*gvg(p,1) + gvg(x,0));
00007         gsl_vector_set(res, 1, s * gvg(p,0) +c*gvg(p,1) + gvg(x,1));
00008 }
00009 
00010 void gsl_vector_set_nan(gsl_vector*v) {
00011         gvs(v,0,GSL_NAN);
00012         gvs(v,1,GSL_NAN);
00013 }
00014 
00015 double norm(const gsl_vector*a){
00016         double x = gvg(a,0);
00017         double y = gvg(a,1);
00018         return sqrt(x*x+y*y);
00019 }
00020 
00021 gsl_vector * vector_from_array(unsigned int n, double *x) {
00022         gsl_vector * v = gsl_vector_alloc(n);
00023         unsigned int i;
00024         for(i=0;i<n;i++)
00025                 gvs(v,i,x[i]);
00026         
00027         return v;
00028 }
00029 
00030 void copy_from_array(gsl_vector*v, double*x) {
00031         size_t i; 
00032         for(i=0;i<v->size;i++)
00033                 gsl_vector_set(v,i, x[i]);
00034 }
00035 
00036 void vector_to_array(const gsl_vector*v, double*x){
00037         size_t i; 
00038         for(i=0;i<v->size;i++)
00039                 x[i] = gvg(v,i);
00040 }
00041 
00042 void oplus(const gsl_vector*x1,const gsl_vector*x2, gsl_vector*res) {
00043         double c = cos(gvg(x1,2));
00044         double s = sin(gvg(x1,2));
00045         gvs(res,0,  gvg(x1,0)+c*gvg(x2,0)-s*gvg(x2,1));
00046         gvs(res,1,  gvg(x1,1)+s*gvg(x2,0)+c*gvg(x2,1));
00047         gvs(res,2,  gvg(x1,2)+gvg(x2,2));
00048 }
00049 
00050 void ominus(const gsl_vector*x, gsl_vector*res) {
00051         double c = cos(gvg(x,2));
00052         double s = sin(gvg(x,2));
00053         gvs(res,0,  -c*gvg(x,0)-s*gvg(x,1));
00054         gvs(res,1,   s*gvg(x,0)-c*gvg(x,1));
00055         gvs(res,2,  -gvg(x,2));
00056 }
00057 
00058 void pose_diff(const gsl_vector*pose2,const gsl_vector*pose1,gsl_vector*res) {
00059         gsl_vector* temp = gsl_vector_alloc(3);
00060         ominus(pose1, temp);
00061         oplus(temp, pose2, res);
00062         gsl_vector_free(temp);
00063 }
00064 
00065 const char* gsl_friendly_pose(gsl_vector*v) {
00066         return friendly_pose(v->data);
00067 }
00068 
00069 static char egsl_tmp_buf[1024];
00070 const char* egsl_friendly_pose(val v) {
00071         sprintf(egsl_tmp_buf, "(%4.2f mm, %4.2f mm, %4.4f deg)",
00072                 1000*egsl_atv(v,0),
00073                 1000*egsl_atv(v,1),
00074                 rad2deg(egsl_atv(v,2)));
00075         return egsl_tmp_buf;
00076 }
00077 
00078 const char* egsl_friendly_cov(val cov) {
00079         
00080         double limit_x  = 2 * sqrt(egsl_atm(cov, 0, 0));
00081         double limit_y  = 2 * sqrt(egsl_atm(cov, 1, 1));
00082         double limit_th = 2 * sqrt(egsl_atm(cov, 2, 2));
00083         
00084         sprintf(egsl_tmp_buf, "(+- %4.2f mm,+- %4.2f mm,+- %4.4f deg)",
00085                 1000*limit_x,
00086                 1000*limit_y,
00087                 rad2deg(limit_th));
00088         return egsl_tmp_buf;
00089 }
00090 
00091 
00092 /*double distance(const gsl_vector* a, const gsl_vector* b) {
00093         distance_counter++;
00094         double x = gvg(a,0)-gvg(b,0);
00095         double y = gvg(a,1)-gvg(b,1);
00096         return sqrt(x*x+y*y);
00097 }
00098 
00099 double distance_squared(const gsl_vector* a, const gsl_vector* b) {
00100         distance_counter++;
00101         double x = gvg(a,0)-gvg(b,0);
00102         double y = gvg(a,1)-gvg(b,1);
00103         return x*x+y*y;
00104 }*/


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