depth_tracker_ros_vr8: quat2rot.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * quat2rot.cpp
00003  *
00004  * Code generation for function 'quat2rot'
00005  *
00006  * C source code generated on: Wed Jul 24 16:11:35 2013
00007  *
00008  */
00009 
00010 /* Include files */
00011 #include "rt_nonfinite.h"
00012 #include "Optimal_affine_tracking_3d16_fast_realtime.h"
00013 #include "quat2rot.h"
00014 
00015 /* Type Definitions */
00016 
00017 /* Named Constants */
00018 
00019 /* Variable Declarations */
00020 
00021 /* Variable Definitions */
00022 
00023 /* Function Declarations */
00024 
00025 /* Function Definitions */
00026 void quat2rot(const real_T Q[4], real_T R[9])
00027 {
00028   /*  QUAT2ROT */
00029   /*    R = QUAT2ROT(Q) converts a quaternion (4x1 or 1x4) into a 3x3 rotation mattrix */
00030   /*  */
00031   /*    reference: google! */
00032   /*  */
00033   /*    Babak Taati, 2003 */
00034   /*    (revised 2009) */
00035   memset(&R[0], 0, 9U * sizeof(real_T));
00036   R[0] = ((Q[0] * Q[0] + Q[1] * Q[1]) - Q[2] * Q[2]) - Q[3] * Q[3];
00037   R[3] = 2.0 * (Q[1] * Q[2] - Q[0] * Q[3]);
00038   R[6] = 2.0 * (Q[1] * Q[3] + Q[0] * Q[2]);
00039   R[1] = 2.0 * (Q[1] * Q[2] + Q[0] * Q[3]);
00040   R[4] = ((Q[0] * Q[0] - Q[1] * Q[1]) + Q[2] * Q[2]) - Q[3] * Q[3];
00041   R[7] = 2.0 * (Q[2] * Q[3] - Q[0] * Q[1]);
00042   R[2] = 2.0 * (Q[1] * Q[3] - Q[0] * Q[2]);
00043   R[5] = 2.0 * (Q[2] * Q[3] + Q[0] * Q[1]);
00044   R[8] = ((Q[0] * Q[0] - Q[1] * Q[1]) - Q[2] * Q[2]) + Q[3] * Q[3];
00045 }
00046 
00047 /* End of code generation (quat2rot.cpp) */