mit_darpa_logs_player: /home/laradmin/lar/utils/mit_darpa_logs

00001 /**************************************************************************************************
00002  Software License Agreement (BSD License)
00003 
00004  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
00005  All rights reserved.
00006 
00007  Redistribution and use in source and binary forms, with or without modification, are permitted
00008  provided that the following conditions are met:
00009 
00010   *Redistributions of source code must retain the above copyright notice, this list of
00011    conditions and the following disclaimer.
00012   *Redistributions in binary form must reproduce the above copyright notice, this list of
00013    conditions and the following disclaimer in the documentation and/or other materials provided
00014    with the distribution.
00015   *Neither the name of the University of Aveiro nor the names of its contributors may be used to
00016    endorse or promote products derived from this software without specific prior written permission.
00017  
00018  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
00019  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00020  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
00021  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00022  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00023  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
00024  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
00025  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 ***************************************************************************************************/
00033 #include <stdio.h>
00034 #include <string.h>
00035 #include <math.h>
00036 #include <assert.h>
00037 
00038 #include "rotations.h"
00039 
00040 static inline int
00041 feq (double a, double b) {
00042     return fabs (a - b) < 1e-9;
00043 }
00044 
00045 static inline int
00046 qeq (double *q, double a, double b, double c, double d)
00047 {
00048     return feq(q[0],a) && feq(q[1],b) && feq(q[2],c) && feq(q[3],d);
00049 }
00050 
00051 static inline int
00052 rpyeq (double roll, double pitch, double yaw, double r, double p, double y) 
00053 {
00054     return feq(roll,r) && feq(pitch,p) && feq(yaw,y);
00055 }
00056 
00067 void
00068 rot_quat_mult (double c[4], const double a[4], const double b[4])
00069 {
00070     c[0] = a[0]*b[0] - a[1]*b[1] - a[2]*b[2] - a[3]*b[3];
00071     c[1] = a[0]*b[1] + a[1]*b[0] + a[2]*b[3] - a[3]*b[2];
00072     c[2] = a[0]*b[2] - a[1]*b[3] + a[2]*b[0] + a[3]*b[1];
00073     c[3] = a[0]*b[3] + a[1]*b[2] - a[2]*b[1] + a[3]*b[0];
00074 }
00075 
00086 void
00087 rot_quat_rotate (const double rot[4], double v[3])
00088 {
00089     double a[4], b[4], c[4];
00090 
00091     b[0] = 0;
00092     memcpy (b+1, v, 3 * sizeof (double));
00093 
00094     rot_quat_mult (a, rot, b);
00095     b[0] = rot[0];
00096     b[1] = -rot[1];
00097     b[2] = -rot[2];
00098     b[3] = -rot[3];
00099     rot_quat_mult (c, a, b);
00100 
00101     memcpy (v, c+1, 3 * sizeof (double));
00102 }
00103 
00115 void
00116 rot_quat_rotate_rev (const double rot[4], double v[3])
00117 {
00118     double a[4], b[4], c[4];
00119 
00120     b[0] = 0;
00121     memcpy (b+1, v, 3 * sizeof (double));
00122 
00123     rot_quat_mult (a, b, rot);
00124     b[0] = rot[0];
00125     b[1] = -rot[1];
00126     b[2] = -rot[2];
00127     b[3] = -rot[3];
00128     rot_quat_mult (c, b, a);
00129 
00130     memcpy (v, c+1, 3 * sizeof (double));
00131 }
00132 
00133 void
00134 rot_angle_axis_to_quat (double theta, const double axis[3], double q[4])
00135 {
00136     double x = axis[0], y = axis[1], z = axis[2];
00137     double norm = sqrt (x*x + y*y + z*z);
00138     if (0 == norm) {
00139         q[0] = 1;
00140         q[1] = q[2] = q[3] = 0;
00141         return;
00142     }
00143 
00144     double t = sin(theta/2) / norm;
00145 
00146     q[0] = cos(theta / 2);
00147     q[1] = x * t;
00148     q[2] = y * t;
00149     q[3] = z * t;
00150 }
00151 
00152 void
00153 rot_quat_to_angle_axis (const double q[4], double *theta, double axis[3])
00154 {
00155     double halftheta = acos (q[0]);
00156     *theta = halftheta * 2;
00157     double sinhalftheta = sin (halftheta);
00158     if (feq (halftheta, 0)) {
00159         axis[0] = 0;
00160         axis[0] = 0;
00161         axis[1] = 1;
00162         *theta = 0;
00163     } else {
00164         axis[0] = q[1] / sinhalftheta;
00165         axis[1] = q[2] / sinhalftheta;
00166         axis[2] = q[3] / sinhalftheta;
00167     }
00168 }
00169 
00170 void 
00171 rot_roll_pitch_yaw_to_quat (const double rpy[3], double q[4])
00172 {
00173     double roll = rpy[0], pitch = rpy[1], yaw = rpy[2];
00174 
00175     double halfroll = roll / 2;
00176     double halfpitch = pitch / 2;
00177     double halfyaw = yaw / 2;
00178 
00179     double sin_r2 = sin (halfroll);
00180     double sin_p2 = sin (halfpitch);
00181     double sin_y2 = sin (halfyaw);
00182 
00183     double cos_r2 = cos (halfroll);
00184     double cos_p2 = cos (halfpitch);
00185     double cos_y2 = cos (halfyaw);
00186 
00187     q[0] = cos_r2 * cos_p2 * cos_y2 + sin_r2 * sin_p2 * sin_y2;
00188     q[1] = sin_r2 * cos_p2 * cos_y2 - cos_r2 * sin_p2 * sin_y2;
00189     q[2] = cos_r2 * sin_p2 * cos_y2 + sin_r2 * cos_p2 * sin_y2;
00190     q[3] = cos_r2 * cos_p2 * sin_y2 - sin_r2 * sin_p2 * cos_y2;
00191 }
00192 
00193 void 
00194 rot_quat_to_roll_pitch_yaw (const double q[4], double rpy[3]) 
00195 {
00196     double roll_a = 2 * (q[0]*q[1] + q[2]*q[3]);
00197     double roll_b = 1 - 2 * (q[1]*q[1] + q[2]*q[2]);
00198     rpy[0] = atan2 (roll_a, roll_b);
00199 
00200     double pitch_sin = 2 * (q[0]*q[2] - q[3]*q[1]);
00201     rpy[1] = asin (pitch_sin);
00202 
00203     double yaw_a = 2 * (q[0]*q[3] + q[1]*q[2]);
00204     double yaw_b = 1 - 2 * (q[2]*q[2] + q[3]*q[3]);
00205     rpy[2] = atan2 (yaw_a, yaw_b);
00206 }
00207 
00208 void 
00209 rot_roll_pitch_yaw_to_angle_axis (const double rpy[3], double *theta,
00210         double axis[3])
00211 {
00212     double q[4];
00213     rot_roll_pitch_yaw_to_quat(rpy, q);
00214     rot_quat_to_angle_axis (q, theta, axis);
00215 }
00216 
00217 void 
00218 rot_angle_axis_to_roll_pitch_yaw (double theta, const double axis[3],
00219         double rpy[3])
00220 {
00221     double q[4];
00222     rot_angle_axis_to_quat (theta, axis, q);
00223     rot_quat_to_roll_pitch_yaw (q, rpy);
00224 }
00225 
00226 
00227 int 
00228 rot_quat_to_matrix(const double quat[4], double rot[9])
00229 {
00230   double norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] +
00231     quat[3]*quat[3];
00232   if (fabs(norm) < 1e-10)
00233     return -1;
00234 
00235   norm = 1/norm;
00236   double x = quat[1]*norm;
00237   double y = quat[2]*norm;
00238   double z = quat[3]*norm;
00239   double w = quat[0]*norm;
00240 
00241   double x2 = x*x;
00242   double y2 = y*y;
00243   double z2 = z*z;
00244   double w2 = w*w;
00245   double xy = 2*x*y;
00246   double xz = 2*x*z;
00247   double yz = 2*y*z;
00248   double wx = 2*w*x;
00249   double wy = 2*w*y;
00250   double wz = 2*w*z;
00251 
00252   rot[0] = w2+x2-y2-z2;  rot[1] = xy-wz;  rot[2] = xz+wy;
00253   rot[3] = xy+wz;  rot[4] = w2-x2+y2-z2;  rot[5] = yz-wx;
00254   rot[6] = xz-wy;  rot[7] = yz+wx;  rot[8] = w2-x2-y2+z2;
00255 
00256   return 0;
00257 }
00258 
00259 int 
00260 rot_quat_pos_to_matrix(const double quat[4], const double pos[3], double mat[16])
00261 {
00262     double rot[9];
00263     rot_quat_to_matrix(quat, rot);
00264 
00265     mat[0] = rot[0];
00266     mat[1] = rot[1];
00267     mat[2] = rot[2];
00268     mat[3] = pos[0];
00269 
00270     mat[4] = rot[3];
00271     mat[5] = rot[4];
00272     mat[6] = rot[5];
00273     mat[7] = pos[1];
00274 
00275     mat[8] = rot[6];
00276     mat[9] = rot[7];
00277     mat[10] = rot[8];
00278     mat[11] = pos[2];
00279 
00280     mat[12] = 0;
00281     mat[13] = 0;
00282     mat[14] = 0;
00283     mat[15] = 1;
00284 
00285     return 0;
00286 }
00287 
00288 int 
00289 rot_matrix_to_quat(const double rot[9], double quat[4])
00290 {
00291   quat[0] = 0.5*sqrt(rot[0]+rot[4]+rot[8]+1);
00292 
00293   if (fabs(quat[0]) > 1e-8) {
00294     double w4 = 1/4/quat[0];
00295     quat[1] = (rot[7]-rot[5]) * w4;
00296     quat[2] = (rot[2]-rot[6]) * w4;
00297     quat[3] = (rot[3]-rot[1]) * w4;
00298   }
00299   else {
00300     quat[1] = sqrt(fabs(-0.5*(rot[4]+rot[8])));
00301     quat[2] = sqrt(fabs(-0.5*(rot[0]+rot[8])));
00302     quat[3] = sqrt(fabs(-0.5*(rot[0]+rot[4])));
00303   }
00304 
00305   double norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] +
00306     quat[3]*quat[3];
00307   if (fabs(norm) < 1e-10)
00308     return -1;
00309 
00310   norm = 1/norm;
00311   quat[0] *= norm;
00312   quat[1] *= norm;
00313   quat[2] *= norm;
00314   quat[3] *= norm;
00315 
00316   return 0;
00317 }
00318 
00319 int
00320 rot_quaternion_test()
00321 {
00322 #define FAIL_TEST { fprintf(stderr, "rot_quaternion_test failed at line %d\n", \
00323         __LINE__); return 0; }
00324 
00325     fprintf(stderr, "running quaternion test\n");
00326     double theta = 0;
00327     double rvec[] = { 0, 0, 1 };
00328     double q[4];
00329     double rpy[3];
00330     double roll, pitch, yaw;
00331 
00332     rot_angle_axis_to_quat(theta, rvec, q);
00333 
00334     if (! qeq (q, 1, 0, 0, 0)) FAIL_TEST;
00335 
00336     rot_quat_to_roll_pitch_yaw (q, rpy);
00337     roll = rpy[0]; pitch = rpy[1]; yaw = rpy[2];
00338 
00339     if (! rpyeq (roll,pitch,yaw, 0,0,0)) FAIL_TEST;
00340 
00341     // quat_from_angle_axis
00342     theta = M_PI;
00343     rot_angle_axis_to_quat(theta, rvec, q);
00344 
00345     fprintf(stderr,"<%.3f, %.3f, %.3f, %.3f>\n", q[0], q[1], q[2], q[3]);
00346     if (! qeq (q, 0, 0, 0, 1)) FAIL_TEST;
00347 
00348     // rot_quat_to_angle_axis
00349     rot_quat_to_angle_axis (q, &theta, rvec);
00350     if (!feq (theta, M_PI)) FAIL_TEST;
00351     if (!feq(rvec[0], 0) || !feq(rvec[1], 0) || !feq(rvec[2], 1)) FAIL_TEST;
00352 
00353     rot_quat_to_roll_pitch_yaw (q, rpy);
00354 
00355     if (! rpyeq (roll,pitch,yaw, 0,0,M_PI)) FAIL_TEST;
00356 
00357     double q2[4];
00358     double q3[4];
00359     double axis1[] = { 0, 1, 0 };
00360     double axis2[] = { 0, 0, 1 };
00361     rot_angle_axis_to_quat (M_PI/2, axis1, q);
00362     rot_angle_axis_to_quat (M_PI/2, axis2, q);
00363     rot_quat_mult (q3, q, q2);
00364     rvec[0] = 0; rvec[1] = 0; rvec[2] = 1;
00365     rot_quat_rotate (q, rvec);
00366     fprintf(stderr, "by q: [ %.2f, %.2f, %.2f ]\n", rvec[0], rvec[1], rvec[2]);
00367     rvec[0] = 0; rvec[1] = 0; rvec[2] = 1;
00368     rot_quat_rotate (q2, rvec);
00369     fprintf(stderr, "by q2: [ %.2f, %.2f, %.2f ]\n", rvec[0], rvec[1], rvec[2]);
00370     rvec[0] = 0; rvec[1] = 0; rvec[2] = 1;
00371     rot_quat_rotate (q3, rvec);
00372     fprintf(stderr, "by q*q2: [ %.2f, %.2f, %.2f ]\n", 
00373             rvec[0], rvec[1], rvec[2]);
00374     rvec[0] = 0; rvec[1] = 0; rvec[2] = 1;
00375     rot_quat_mult (q3, q2, q);
00376     rot_quat_rotate (q3, rvec);
00377     fprintf(stderr, "by q2*q: [ %.2f, %.2f, %.2f ]\n", 
00378             rvec[0], rvec[1], rvec[2]);
00379 
00380     // TODO
00381 
00382 #undef FAIL_TEST
00383 
00384     fprintf(stderr, "rot_quaternion_test complete\n");
00385     return 1;
00386 }
/home/laradmin/lar/utils/mit_darpa_logs_player/src/rotations.cpp
