/*! Converts this quaternion to a 3x3 rotation matrix. */ void Quaternion::ToRotationMatrix(mat3 &R) const { double tx = 2.0*x; double ty = 2.0*y; double tz = 2.0*z; double twx = tx*w; double twy = ty*w; double twz = tz*w; double txx = tx*x; double txy = ty*x; double txz = tz*x; double tyy = ty*y; double tyz = tz*y; double tzz = tz*z; R.element(0,0) = 1.0-(tyy+tzz); R.element(1,0) = txy-twz; R.element(2,0) = txz+twy; R.element(0,1) = txy+twz; R.element(1,1) = 1.0-(txx+tzz); R.element(2,1) = tyz-twx; R.element(0,2) = txz-twy; R.element(1,2) = tyz+twx; R.element(2,2) = 1.0-(txx+tyy); }
/*! Sets this quaternion by converting from rotation matrix \a R. */ void Quaternion::set(const mat3 &R) { // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes // article "Quaternion Calculus and Fast Animation". double trace = R.element(0,0)+R.element(1,1)+R.element(2,2); double root; if ( trace > 0.0 ) { // |w| > 1/2, may as well choose w > 1/2 root = sqrt(trace+1.0); // 2w w = 0.5*root; root = 0.5/root; // 1/(4w) x = (R.element(1,2)-R.element(2,1))*root; y = (R.element(2,0)-R.element(0,2))*root; z = (R.element(0,1)-R.element(1,0))*root; } else { // |w| <= 1/2 static int next[3] = { 1, 2, 0 }; int i = 0; if ( R.element(1,1) > R.element(0,0) ) i = 1; if ( R.element(2,2) > R.element(i,i) ) i = 2; int j = next[i]; int k = next[j]; root = sqrt(R.element(i,i)-R.element(j,j)-R.element(k,k)+1.0); double* quat[3] = { &x, &y, &z }; *quat[i] = 0.5*root; root = 0.5/root; w = (R.element(j,k)-R.element(k,j))*root; *quat[j] = (R.element(i,j)+R.element(j,i))*root; *quat[k] = (R.element(i,k)+R.element(k,i))*root; } normalise(); }