~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

Linux Cross Reference
Tina4/src/vision/calib/conv_quat.c

Version: ~
Architecture: ~ [ i386 ] ~ [ alpha ] ~ [ m68k ] ~ [ mips ] ~ [ ppc ] ~ [ sparc ] ~ [ sparc64 ] ~

  1 /**@(#)
  2 **/
  3 #include <math.h>
  4 #include <tina/sys.h>
  5 #include <tina/math.h>
  6 #include <tina/mathfuncs.h>
  7 #include <tina/vision.h>
  8 
  9 void    conv_rot_to_quat(Mat3 * mat, double *q)
 10 {
 11     q[0] = 0.5 * sqrt(mat->el[0][0] + mat->el[1][1] + mat->el[2][2] + 1.0);
 12     if (q[0] > 1.0)
 13     {
 14         q[0] = 1.0;
 15         q[1] = 0.0;
 16         q[2] = 0.0;
 17         q[3] = 0.0;
 18     } else if (q[0] == 0.0)
 19     {
 20         q[1] = 1.0;
 21         q[2] = 0.0;
 22         q[3] = 0.0;
 23     } else
 24     {
 25         q[1] = (mat->el[2][1] - mat->el[1][2]) / (4.0 * q[0]);
 26         q[2] = (mat->el[0][2] - mat->el[2][0]) / (4.0 * q[0]);
 27         q[3] = (mat->el[1][0] - mat->el[0][1]) / (4.0 * q[0]);
 28     }
 29 }
 30 
 31 void    conv_quat_to_rot(double *q, Mat3 * mat)
 32 {
 33     mat->el[0][0] = (float)(q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
 34     mat->el[1][0] = (float)(2.0 * (q[1] * q[2] + q[0] * q[3]));
 35     mat->el[2][0] = (float)(2.0 * (q[1] * q[3] - q[0] * q[2]));
 36     mat->el[0][1] = (float)(2.0 * (q[1] * q[2] - q[0] * q[3]));
 37     mat->el[1][1] = (float)(q[0] * q[0] - q[1] * q[1] + q[2] * q[2] - q[3] * q[3]);
 38     mat->el[2][1] = (float)(2.0 * (q[2] * q[3] + q[0] * q[1]));
 39     mat->el[0][2] = (float)(2.0 * (q[1] * q[3] + q[0] * q[2]));
 40     mat->el[1][2] = (float)(2.0 * (q[2] * q[3] - q[0] * q[1]));
 41     mat->el[2][2] = (float)(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
 42 }
 43 

~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

This page was automatically generated by the LXR engine.
Visit the LXR main site for more information.