/*!
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();
}
