Hi Matrin.
In the quaternion to matrix page
you mention that you need to normalize the quaternion before you
convert it. However you can normalize the output matrix instead
which is faster since you don't have to use an sqrt.
Since
the matrix is created by multiplying pairs of quaternion
components (x*y, x*z, etc) you can normalize the matrix by
dividing with the square length of the quaternion. Below i have
the modified code. Also the same can be done for rotating a vector
(since it actually uses the same matrix).
public
final void quatToMatrix(Quat4d q){
double sqw =
q.w*q.w;
double sqx = q.x*q.x;
double sqy =
q.y*q.y;
double sqz = q.z*q.z;
// get the invert
square length
double invs = 1 / (sqx + sqy + sqz +
sqw);
// rotation matrix is scaled by inverse
square length
m00 = ( sqx - sqy - sqz + sqw) * invs;
m11
= (-sqx + sqy - sqz + sqw) * invs;
m22 = (-sqx - sqy +
sqz + sqw) * invs;
double tmp1 =
q.x*q.y;
double tmp2 = q.z*q.w;
m10 = 2.0 *
(tmp1 + tmp2) * invs;
m01 = 2.0 * (tmp1 - tmp2) *
invs;
tmp1 = q.x*q.z;
tmp2 =
q.y*q.w;
m20 = 2.0 * (tmp1 - tmp2) * invs;
m02 =
2.0 * (tmp1 + tmp2) * invs;
tmp1 = q.y*q.z;
tmp2
= q.x*q.w;
m21 = 2.0 * (tmp1 + tmp2) * invs;
m12
= 2.0 * (tmp1 - tmp2) * invs;
} |
