Go to the documentation of this file.00001
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
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
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
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
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 }