void PointMul (Quaternion* q, Point3* Pres, Point3* pt)
{
/* Given a vector u = (x0, y0, z0) and a unit length quaternion
* q = <w, x, y, z>, the vector v = (x1, y1, z1) which represents
* the rotation of u by q is v = q * u * q ^ {-1} where * indicates
* quaternion multiplication and where u is treated as the
* quaternion <0, x0, y0, z0>. Note that q ^ {-1} = <w, -x, -y, -z>,
* so no real work is required to invert q. Now
*
* q * u * q ^ {-1} = q * <0, x0, y0, z0> * q ^ {-1}
* = q * (x0 * i + y0 * j + z0 * k) * q ^ {-1}
* = x0 * (q * i * q ^ {-1}) +
* y0 * (q * j * q ^ {-1}) +
* z0 * (q * k * q ^ {-1})
*
* As 3-vectors, q * i * q ^ {-1}, q * j * q ^ {-1}, and
* 2 * k * q ^ {-1} are the columns of the rotation matrix computed
* in Quaternion::ToRotationMatrix. The vector v is obtained as the
* product of that rotation matrix with vector u. As such, the
* quaternion representation of a rotation matrix requires less
* space than the matrix and more time to compute the rotated
* vector. Typical space-time tradeoff...
*/
float R[3][3];
ToRotationMatrix(q, R);
Pres->x = R[0][0] * pt->x + R[0][1] * pt->y + R[0][2] * pt->z;
Pres->y = R[1][0] * pt->x + R[1][1] * pt->y + R[1][2] * pt->z;
Pres->z = R[2][0] * pt->x + R[2][1] * pt->y + R[2][2] * pt->z;
}
moVector3<Real> moQuaternion<Real>::Rotate (const moVector3<Real>& rkVector)
const
{
// Given a vector u = (x0,y0,z0) and a unit length quaternion
// q = <w,x,y,z>, the vector v = (x1,y1,z1) which represents the
// rotation of u by q is v = q*u*q^{-1} where * indicates quaternion
// multiplication and where u is treated as the quaternion <0,x0,y0,z0>.
// Note that q^{-1} = <w,-x,-y,-z>, so no real work is required to
// invert q. Now
//
// q*u*q^{-1} = q*<0,x0,y0,z0>*q^{-1}
// = q*(x0*i+y0*j+z0*k)*q^{-1}
// = x0*(q*i*q^{-1})+y0*(q*j*q^{-1})+z0*(q*k*q^{-1})
//
// As 3-vectors, q*i*q^{-1}, q*j*q^{-1}, and 2*k*q^{-1} are the columns
// of the rotation matrix computed in moQuaternion<Real>::ToRotationMatrix.
// The vector v is obtained as the product of that rotation matrix with
// vector u. As such, the quaternion representation of a rotation
// matrix requires less space than the matrix and more time to compute
// the rotated vector. Typical space-time tradeoff...
moMatrix3<Real> kRot;
ToRotationMatrix(kRot);
return kRot*rkVector;
}
void Quaternion::getMatrix(Matrix4 &dest) const
{
Matrix3 mat3;
ToRotationMatrix(mat3);
dest = Matrix4(mat3);
//dest.setTrans(translation);
}
void moQuaternion<Real>::ToRotationMatrix (moVector3<Real> akRotColumn[3]) const
{
moMatrix3<Real> kRot;
ToRotationMatrix(kRot);
for (int iCol = 0; iCol < 3; iCol++)
{
akRotColumn[iCol][0] = kRot(0,iCol);
akRotColumn[iCol][1] = kRot(1,iCol);
akRotColumn[iCol][2] = kRot(2,iCol);
}
}
//-----------------------------------------------------------------------
void Quaternion::ToAxes (Vector3* akAxis) const
{
Matrix3 kRot;
ToRotationMatrix(kRot);
for (size_t iCol = 0; iCol < 3; iCol++)
{
akAxis[iCol].x = kRot[0][iCol];
akAxis[iCol].y = kRot[1][iCol];
akAxis[iCol].z = kRot[2][iCol];
}
}
//-----------------------------------------------------------------------
void Quaternion::ToAxes (Vector3& xaxis, Vector3& yaxis, Vector3& zaxis) const
{
Matrix3 kRot;
ToRotationMatrix(kRot);
xaxis.x = kRot[0][0];
xaxis.y = kRot[1][0];
xaxis.z = kRot[2][0];
yaxis.x = kRot[0][1];
yaxis.y = kRot[1][1];
yaxis.z = kRot[2][1];
zaxis.x = kRot[0][2];
zaxis.y = kRot[1][2];
zaxis.z = kRot[2][2];
}
void Quat::ToAxes(Vec3& xAxis, Vec3& yAxis, Vec3& zAxis)
{
Mat33 kRot;
ToRotationMatrix(kRot);
xAxis.x = kRot[0][0];
xAxis.y = kRot[1][0];
xAxis.z = kRot[2][0];
yAxis.x = kRot[0][1];
yAxis.y = kRot[1][1];
yAxis.z = kRot[2][1];
zAxis.x = kRot[0][2];
zAxis.y = kRot[1][2];
zAxis.z = kRot[2][2];
}
