core/vnl/vnl_rotation_matrix.cxx
Go to the documentation of this file.
00001 // This is core/vnl/vnl_rotation_matrix.cxx
00002 #include "vnl_rotation_matrix.h"
00003 
00004 #include <vcl_cmath.h>
00005 
00006 bool vnl_rotation_matrix(double const x[3], double **R)
00007 {
00008   // start with an identity matrix.
00009   for (unsigned i=0; i<3; ++i)
00010     for (unsigned j=0; j<3; ++j)
00011       R[i][j] = (i==j ? 1 : 0);
00012 
00013   // normalize x to a unit vector u, of norm 'angle'.
00014   double u[3] = {x[0], x[1], x[2]};
00015   double angle = vcl_sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
00016   if (angle == 0)
00017     return true;
00018   u[0] /= angle;
00019   u[1] /= angle;
00020   u[2] /= angle;
00021 
00022   // add (cos(angle)-1)*(1 - u u').
00023   double cos_angle = vcl_cos(angle);
00024   for (unsigned i=0; i<3; ++i)
00025     for (unsigned j=0; j<3; ++j)
00026       R[i][j] += (cos_angle-1) * ((i==j ? 1:0) - u[i]*u[j]);
00027 
00028   // add sin(angle) * [u]
00029   double sin_angle = vcl_sin(angle);
00030   /* */                      R[0][1] -= sin_angle*u[2]; R[0][2] += sin_angle*u[1];
00031   R[1][0] += sin_angle*u[2]; /* */                      R[1][2] -= sin_angle*u[0];
00032   R[2][0] -= sin_angle*u[1]; R[2][1] += sin_angle*u[0]; /* */
00033 
00034 #if 0
00035   vcl_cerr << "axis = [" << axis[0] << ' ' << axis[1] << ' ' << axis[2] << "];\n";
00036 
00037   vcl_cerr << "R=[\n";
00038   for (unsigned i=0; i<3; ++i) {
00039     for (unsigned j=0; j<3; ++j)
00040       vcl_cerr << ' ' << R[i][j];
00041     vcl_cerr << vcl_endl;
00042   }
00043   vcl_cerr << "];\n";
00044   vcl_exit(1);
00045 #endif
00046   return true;
00047 }
00048 
00049 bool vnl_rotation_matrix(double const axis[3], double R[3][3])
00050 {
00051   double *R_[3] = { R[0], R[1], R[2] };
00052   return vnl_rotation_matrix(axis, R_);
00053 }
00054 
00055 bool vnl_rotation_matrix(double const axis[3], double *R0, double *R1, double *R2)
00056 {
00057   double *R[3] = { R0, R1, R2 };
00058   return vnl_rotation_matrix(axis, R);
00059 }
00060 
00061 #include <vnl/vnl_vector_fixed.h>
00062 #include <vnl/vnl_matrix_fixed.h>
00063 
00064 bool vnl_rotation_matrix(vnl_vector_fixed<double,3> const& axis,
00065                          vnl_matrix_fixed<double,3,3>& R)
00066 {
00067   return vnl_rotation_matrix(&axis[0], R[0], R[1], R[2]);
00068 }
00069 
00070 vnl_matrix_fixed<double,3,3> vnl_rotation_matrix(vnl_vector_fixed<double,3> const& axis)
00071 {
00072   vnl_matrix_fixed<double,3,3> R;
00073   vnl_rotation_matrix(&axis[0], R[0], R[1], R[2]);
00074   return R;
00075 }
00076 
00077 #include <vnl/vnl_vector.h>
00078 #include <vnl/vnl_matrix.h>
00079 
00080 bool vnl_rotation_matrix(vnl_vector<double> const &axis, vnl_matrix<double> &R)
00081 {
00082   return vnl_rotation_matrix(&axis[0], R.data_array());
00083 }
00084 
00085 vnl_matrix<double> vnl_rotation_matrix(vnl_vector<double> const &axis)
00086 {
00087   vnl_matrix<double> R(3, 3);
00088   vnl_rotation_matrix(&axis[0], R.data_array());
00089   return R;
00090 }