void Quaternion::transformToMatrix4x4(double *matrix4x4) const {
if (matrix4x4 != NULL) {
Matrix3x3 result = transformToMatrix3x3();
matrix4x4[0] = result.getXX();
matrix4x4[1] = result.getXY();
matrix4x4[2] = result.getXZ();
matrix4x4[3] = 0;
// Second row
matrix4x4[4] = result.getYX();
matrix4x4[5] = result.getYY();
matrix4x4[6] = result.getYZ();
matrix4x4[7] = 0;
// Third row
matrix4x4[8] = result.getZX();
matrix4x4[9] = result.getZY();
matrix4x4[10] = result.getZZ();
matrix4x4[11] = 0.;
// Fourth row
matrix4x4[12] = 0;
matrix4x4[13] = 0;
matrix4x4[14] = 0;
matrix4x4[15] = 1;
}
}
Point3 Point3::operator*(const Matrix3x3 &m) const {
const double x = getX() * m.getXX() + getY() * m.getXY() + getZ() * m.getXZ();
const double y = getX() * m.getYX() + getY() * m.getYY() + getZ() * m.getYZ();
const double z = getX() * m.getZX() + getY() * m.getZY() + getZ() * m.getZZ();
Point3 cc(x, y, z);
return cc;
}
