00001
00002 #ifndef vnl_quaternion_txx_
00003 #define vnl_quaternion_txx_
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 #include "vnl_quaternion.h"
00034
00035 #include <vcl_cmath.h>
00036 #include <vcl_limits.h>
00037 #include <vcl_iostream.h>
00038
00039 #include <vnl/vnl_cross.h>
00040 #include <vnl/vnl_math.h>
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 template <class T>
00052 vnl_quaternion<T>::vnl_quaternion (T tx, T ty, T tz, T rea)
00053 {
00054 this->operator[](0) = tx;
00055 this->operator[](1) = ty;
00056 this->operator[](2) = tz;
00057 this->operator[](3) = rea;
00058 }
00059
00060
00061
00062 template <class T>
00063 vnl_quaternion<T>::vnl_quaternion(vnl_vector_fixed<T,3> const& Axis, double Angle)
00064 {
00065 double a = Angle * 0.5;
00066 T s = T(vcl_sin(a));
00067 for (int i = 0; i < 3; i++)
00068 this->operator[](i) = T(s * Axis(i));
00069 this->operator[](3) = T(vcl_cos(a));
00070 }
00071
00072
00073
00074
00075
00076 template <class T>
00077 vnl_quaternion<T>::vnl_quaternion(vnl_vector_fixed<T,3> const& vec)
00078 {
00079 for (unsigned int i = 0; i < 3; ++i)
00080 this->operator[](i) = vec(i);
00081 this->operator[](3) = T(0);
00082 }
00083
00084
00085
00086
00087
00088 template <class T>
00089 vnl_quaternion<T>::vnl_quaternion(vnl_vector_fixed<T,4> const& vec)
00090 {
00091 for (unsigned int i = 0; i < 4; ++i)
00092 this->operator[](i) = vec[i];
00093 }
00094
00095
00096
00097
00098
00099
00100
00101 template <class T>
00102 vnl_quaternion<T>::vnl_quaternion(vnl_matrix_fixed<T,3,3> const& rot)
00103 {
00104 double d0 = rot(0,0), d1 = rot(1,1), d2 = rot(2,2);
00105 double xx = 1.0 + d0 - d1 - d2;
00106 double yy = 1.0 - d0 + d1 - d2;
00107 double zz = 1.0 - d0 - d1 + d2;
00108 double rr = 1.0 + d0 + d1 + d2;
00109
00110 double max = rr;
00111 if (xx > max) max = xx;
00112 if (yy > max) max = yy;
00113 if (zz > max) max = zz;
00114
00115 if (rr == max) {
00116 T r4 = T(vcl_sqrt(rr)*2);
00117 this->r() = r4 / 4;
00118 r4 = T(1) / r4;
00119 this->x() = (rot(1,2) - rot(2,1)) * r4;
00120 this->y() = (rot(2,0) - rot(0,2)) * r4;
00121 this->z() = (rot(0,1) - rot(1,0)) * r4;
00122 }
00123 else if (xx == max) {
00124 T x4 = T(vcl_sqrt(xx)*2);
00125 this->x() = x4 / 4;
00126 x4 = T(1) / x4;
00127 this->y() = (rot(0,1) + rot(1,0)) * x4;
00128 this->z() = (rot(0,2) + rot(2,0)) * x4;
00129 this->r() = (rot(1,2) - rot(2,1)) * x4;
00130 }
00131 else if (yy == max) {
00132 T y4 = T(vcl_sqrt(yy)*2);
00133 this->y() = y4 / 4;
00134 y4 = T(1) / y4;
00135 this->x() = (rot(0,1) + rot(1,0)) * y4;
00136 this->z() = (rot(1,2) + rot(2,1)) * y4;
00137 this->r() = (rot(2,0) - rot(0,2)) * y4;
00138 }
00139 else {
00140 T z4 = T(vcl_sqrt(zz)*2);
00141 this->z() = z4 / 4;
00142 z4 = T(1) / z4;
00143 this->x() = (rot(0,2) + rot(2,0)) * z4;
00144 this->y() = (rot(1,2) + rot(2,1)) * z4;
00145 this->r() = (rot(0,1) - rot(1,0)) * z4;
00146 }
00147 }
00148
00149
00150
00151
00152
00153 template <class T>
00154 vnl_quaternion<T>::vnl_quaternion(T theta_X, T theta_Y, T theta_Z)
00155 {
00156 vnl_quaternion<T> Rx(T(vcl_sin(double(theta_X)*0.5)), 0, 0, T(vcl_cos(double(theta_X)*0.5)));
00157 vnl_quaternion<T> Ry(0, T(vcl_sin(double(theta_Y)*0.5)), 0, T(vcl_cos(double(theta_Y)*0.5)));
00158 vnl_quaternion<T> Rz(0, 0, T(vcl_sin(double(theta_Z)*0.5)), T(vcl_cos(double(theta_Z)*0.5)));
00159 *this = Rz * Ry * Rx;
00160 }
00161
00162
00163
00164
00165
00166
00167 template <class T>
00168 vnl_vector_fixed<T,3> vnl_quaternion<T>::rotation_euler_angles() const
00169 {
00170 vnl_vector_fixed<T,3> angles;
00171
00172 vnl_matrix_fixed<T,4,4> rotM = rotation_matrix_transpose_4();
00173 T xy = T(vcl_sqrt(double(vnl_math_sqr(rotM(0,0)) + vnl_math_sqr(rotM(0,1)))));
00174 if (xy > vcl_numeric_limits<T>::epsilon() * T(8))
00175 {
00176 angles(0) = T(vcl_atan2(double(rotM(1,2)), double(rotM(2,2))));
00177 angles(1) = T(vcl_atan2(double(-rotM(0,2)), double(xy)));
00178 angles(2) = T(vcl_atan2(double(rotM(0,1)), double(rotM(0,0))));
00179 }
00180 else
00181 {
00182 angles(0) = T(vcl_atan2(double(-rotM(2,1)), double(rotM(1,1))));
00183 angles(1) = T(vcl_atan2(double(-rotM(0,2)), double(xy)));
00184 angles(2) = T(0);
00185 }
00186 return angles;
00187 }
00188
00189
00190
00191
00192 template <class T>
00193 double vnl_quaternion<T>::angle() const
00194 {
00195 return 2 * vcl_atan2(double(this->imaginary().magnitude()),
00196 double(this->real()));
00197 }
00198
00199
00200
00201 template <class T>
00202 vnl_vector_fixed<T,3> vnl_quaternion<T>::axis() const
00203 {
00204 vnl_vector_fixed<T,3> direc = this->imaginary();
00205 T mag = direc.magnitude();
00206 if (mag == T(0)) {
00207 vcl_cout << "Axis not well defined for zero Quaternion. Using (0,0,1) instead.\n";
00208 direc[2] = T(1);
00209 }
00210 else
00211 direc /= mag;
00212 return direc;
00213 }
00214
00215
00216
00217
00218
00219
00220 template <class T>
00221 vnl_matrix_fixed<T,3,3> vnl_quaternion<T>::rotation_matrix_transpose() const
00222 {
00223 T x2 = x() * x(), xy = x() * y(), rx = r() * x(),
00224 y2 = y() * y(), yz = y() * z(), ry = r() * y(),
00225 z2 = z() * z(), zx = z() * x(), rz = r() * z(),
00226 r2 = r() * r();
00227 vnl_matrix_fixed<T,3,3> rot;
00228 rot(0,0) = r2 + x2 - y2 - z2;
00229 rot(1,1) = r2 - x2 + y2 - z2;
00230 rot(2,2) = r2 - x2 - y2 + z2;
00231 rot(0,1) = 2 * (xy + rz);
00232 rot(0,2) = 2 * (zx - ry);
00233 rot(1,2) = 2 * (yz + rx);
00234 rot(1,0) = 2 * (xy - rz);
00235 rot(2,0) = 2 * (zx + ry);
00236 rot(2,1) = 2 * (yz - rx);
00237
00238 return rot;
00239 }
00240
00241
00242 template <class T>
00243 vnl_matrix_fixed<T,4,4> vnl_quaternion<T>::rotation_matrix_transpose_4() const
00244 {
00245 vnl_matrix_fixed<T,4,4> rot;
00246 return rot.set_identity().update(this->rotation_matrix_transpose().as_ref());
00247 }
00248
00249
00250
00251 template <class T>
00252 vnl_quaternion<T> vnl_quaternion<T>::conjugate() const
00253 {
00254 return vnl_quaternion<T> (-x(), -y(), -z(), r());
00255 }
00256
00257
00258
00259
00260
00261 template <class T>
00262 vnl_quaternion<T> vnl_quaternion<T>::inverse() const
00263 {
00264 vnl_quaternion<T> inv = this->conjugate();
00265 inv /= vnl_c_vector<T>::dot_product(this->data_, this->data_, 4);
00266 return inv;
00267 }
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278 template <class T>
00279 vnl_quaternion<T> vnl_quaternion<T>::operator* (vnl_quaternion<T> const& rhs) const
00280 {
00281 T r1 = this->real();
00282 T r2 = rhs.real();
00283 vnl_vector_fixed<T,3> i1 = this->imaginary();
00284 vnl_vector_fixed<T,3> i2 = rhs.imaginary();
00285 T real_v = (r1 * r2) - ::dot_product(i1, i2);
00286 vnl_vector_fixed<T,3> img = vnl_cross_3d(i1, i2);
00287 img += (i2 * r1) + (i1 * r2);
00288 return vnl_quaternion<T>(img[0], img[1], img[2], real_v);
00289 }
00290
00291
00292
00293
00294
00295 template <class T>
00296 vnl_vector_fixed<T,3> vnl_quaternion<T>::rotate(vnl_vector_fixed<T,3> const& v) const
00297 {
00298 T rea = this->real();
00299 vnl_vector_fixed<T,3> i = this->imaginary();
00300 vnl_vector_fixed<T,3> i_x_v(vnl_cross_3d(i, v));
00301 return v + i_x_v * T(2*rea) - vnl_cross_3d(i_x_v, i) * T(2);
00302 }
00303
00304 #undef VNL_QUATERNION_INSTANTIATE
00305 #define VNL_QUATERNION_INSTANTIATE(T) \
00306 template class vnl_quaternion<T >;\
00307 VCL_INSTANTIATE_INLINE(vcl_ostream& operator<< (vcl_ostream&, vnl_quaternion<T > const&))
00308
00309 #endif // vnl_quaternion_txx_