dgVector dgMatrix::CalcPitchYawRoll () const
{
const hacd::HaF32 minSin = hacd::HaF32(0.99995f);
const dgMatrix& matrix = *this;
hacd::HaF32 roll = hacd::HaF32(0.0f);
hacd::HaF32 pitch = hacd::HaF32(0.0f);
hacd::HaF32 yaw = dgAsin (-ClampValue (matrix[0][2], hacd::HaF32(-0.999999f), hacd::HaF32(0.999999f)));
HACD_ASSERT (dgCheckFloat (yaw));
if (matrix[0][2] < minSin) {
if (matrix[0][2] > (-minSin)) {
roll = dgAtan2 (matrix[0][1], matrix[0][0]);
pitch = dgAtan2 (matrix[1][2], matrix[2][2]);
} else {
pitch = dgAtan2 (matrix[1][0], matrix[1][1]);
}
} else {
pitch = -dgAtan2 (matrix[1][0], matrix[1][1]);
}
#ifdef _DEBUG
dgMatrix m (dgPitchMatrix (pitch) * dgYawMatrix(yaw) * dgRollMatrix(roll));
for (hacd::HaI32 i = 0; i < 3; i ++) {
for (hacd::HaI32 j = 0; j < 3; j ++) {
hacd::HaF32 error = dgAbsf (m[i][j] - matrix[i][j]);
HACD_ASSERT (error < 5.0e-2f);
}
}
#endif
return dgVector (pitch, yaw, roll, hacd::HaF32(0.0f));
}
void ShowMousePicking (const dVector& p0, const dVector& p1)
{
dFloat radius = 0.25f;
// set the color of the cube's surface
GLfloat cubeColor[] = { 1.0f, 1.0f, 1.0f, 1.0 };
glMaterialfv(GL_FRONT, GL_SPECULAR, cubeColor);
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, cubeColor);
glMaterialf(GL_FRONT, GL_SHININESS, 50.0);
// set up the cube's texture
glDisable(GL_TEXTURE_2D);
GLUquadricObj *pObj;
glPushMatrix();
dMatrix matrix (GetIdentityMatrix());
matrix.m_posit = p0;
glMultMatrix(&matrix[0][0]);
// Get a new Quadric off the stack
pObj = gluNewQuadric();
gluQuadricTexture(pObj, true);
gluSphere(pObj, radius, 10, 10);
glPopMatrix();
glPushMatrix();
matrix.m_posit = p1;
glMultMatrix(&matrix[0][0]);
gluSphere(pObj, radius, 10, 10);
gluDeleteQuadric(pObj);
glPopMatrix();
dVector dir (p1 - p0);
dFloat lenght (dir % dir);
if (lenght > 1.0e-2f) {
glPushMatrix();
lenght = sqrt (lenght);
dMatrix align (dgYawMatrix(0.5f * 3.1426f));
align.m_posit.m_x = -lenght * 0.5f;
matrix = align * dgGrammSchmidt(dir);
matrix.m_posit += (p1 + p0).Scale (0.5f);
glMultMatrix(&matrix[0][0]);
// Get a new Quadric off the stack
pObj = gluNewQuadric();
gluCylinder(pObj, radius * 0.5f, radius * 0.5f, lenght, 10, 2);
gluDeleteQuadric(pObj);
glPopMatrix();
}
}
void dgMatrix::CalcPitchYawRoll (dgVector& euler0, dgVector& euler1) const
{
const dgMatrix& matrix = *this;
dgAssert (dgAbsf (((matrix[0] * matrix[1]) % matrix[2]) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
// Assuming the angles are in radians.
if (matrix[0][2] > dgFloat32 (0.99995f)) {
dgFloat32 picth0 = dgFloat32 (0.0f);
dgFloat32 yaw0 = dgFloat32 (-3.141592f * 0.5f);
dgFloat32 roll0 = - dgAtan2(matrix[2][1], matrix[1][1]);
euler0[0] = picth0;
euler0[1] = yaw0;
euler0[2] = roll0;
euler1[0] = picth0;
euler1[1] = yaw0;
euler1[2] = roll0;
} else if (matrix[0][2] < dgFloat32 (-0.99995f)) {
dgFloat32 picth0 = dgFloat32 (0.0f);
dgFloat32 yaw0 = dgFloat32 (3.141592f * 0.5f);
dgFloat32 roll0 = dgAtan2(matrix[2][1], matrix[1][1]);
euler0[0] = picth0;
euler0[1] = yaw0;
euler0[2] = roll0;
euler1[0] = picth0;
euler1[1] = yaw0;
euler1[2] = roll0;
} else {
dgFloat32 yaw0 = -dgAsin ( matrix[0][2]);
dgFloat32 yaw1 = dgFloat32 (3.141592f) - yaw0;
dgFloat32 sign0 = dgSign(dgCos (yaw0));
dgFloat32 sign1 = dgSign(dgCos (yaw1));
dgFloat32 picth0 = dgAtan2(matrix[1][2] * sign0, matrix[2][2] * sign0);
dgFloat32 picth1 = dgAtan2(matrix[1][2] * sign1, matrix[2][2] * sign1);
dgFloat32 roll0 = dgAtan2(matrix[0][1] * sign0, matrix[0][0] * sign0);
dgFloat32 roll1 = dgAtan2(matrix[0][1] * sign1, matrix[0][0] * sign1);
if (yaw1 > dgFloat32 (3.141592f)) {
yaw1 -= dgFloat32 (2.0f * 3.141592f);
}
euler0[0] = picth0;
euler0[1] = yaw0;
euler0[2] = roll0;
euler1[0] = picth1;
euler1[1] = yaw1;
euler1[2] = roll1;
}
euler0[3] = dgFloat32(0.0f);
euler1[3] = dgFloat32(0.0f);
#ifdef _DEBUG
dgMatrix m0 (dgPitchMatrix (euler0[0]) * dgYawMatrix(euler0[1]) * dgRollMatrix(euler0[2]));
dgMatrix m1 (dgPitchMatrix (euler1[0]) * dgYawMatrix(euler1[1]) * dgRollMatrix(euler1[2]));
for (int i = 0; i < 3; i ++) {
for (int j = 0; j < 3; j ++) {
dgFloat32 error = dgAbsf (m0[i][j] - matrix[i][j]);
dgAssert (error < 5.0e-2f);
error = dgAbsf (m1[i][j] - matrix[i][j]);
dgAssert (error < 5.0e-2f);
}
}
#endif
}
void dgMatrix::PolarDecomposition (dgMatrix& transformMatrix, dgVector& scale, dgMatrix& stretchAxis, const dgMatrix* const initialStretchAxis) const
{
// a polar decomposition decompose matrix A = O * S
// where S = sqrt (transpose (L) * L)
/*
// calculate transpose (L) * L
dgMatrix LL ((*this) * Transpose());
// check is this is a pure uniformScale * rotation * translation
dgFloat32 det2 = (LL[0][0] + LL[1][1] + LL[2][2]) * dgFloat32 (1.0f / 3.0f);
dgFloat32 invdet2 = 1.0f / det2;
dgMatrix pureRotation (LL);
pureRotation[0] = pureRotation[0].Scale3 (invdet2);
pureRotation[1] = pureRotation[1].Scale3 (invdet2);
pureRotation[2] = pureRotation[2].Scale3 (invdet2);
dgFloat32 sign = ((((*this)[0] * (*this)[1]) % (*this)[2]) > 0.0f) ? 1.0f : -1.0f;
dgFloat32 det = (pureRotation[0] * pureRotation[1]) % pureRotation[2];
if (dgAbsf (det - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f)) {
// this is a pure scale * rotation * translation
det = sign * dgSqrt (det2);
scale[0] = det;
scale[1] = det;
scale[2] = det;
det = dgFloat32 (1.0f)/ det;
transformMatrix.m_front = m_front.Scale3 (det);
transformMatrix.m_up = m_up.Scale3 (det);
transformMatrix.m_right = m_right.Scale3 (det);
transformMatrix[0][3] = dgFloat32 (0.0f);
transformMatrix[1][3] = dgFloat32 (0.0f);
transformMatrix[2][3] = dgFloat32 (0.0f);
transformMatrix.m_posit = m_posit;
stretchAxis = dgGetIdentityMatrix();
} else {
stretchAxis = LL;
stretchAxis.EigenVectors (scale);
// I need to deal with by seeing of some of the Scale are duplicated
// do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work)
// for now just us the matrix
scale[0] = sign * dgSqrt (scale[0]);
scale[1] = sign * dgSqrt (scale[1]);
scale[2] = sign * dgSqrt (scale[2]);
scale[3] = dgFloat32 (0.0f);
dgMatrix scaledAxis;
scaledAxis[0] = stretchAxis[0].Scale3 (dgFloat32 (1.0f) / scale[0]);
scaledAxis[1] = stretchAxis[1].Scale3 (dgFloat32 (1.0f) / scale[1]);
scaledAxis[2] = stretchAxis[2].Scale3 (dgFloat32 (1.0f) / scale[2]);
scaledAxis[3] = stretchAxis[3];
dgMatrix symetricInv (stretchAxis.Transpose() * scaledAxis);
transformMatrix = symetricInv * (*this);
transformMatrix.m_posit = m_posit;
}
*/
// test the f*****g factorization
dgMatrix xxxxx(dgRollMatrix(30.0f * 3.1416f / 180.0f));
xxxxx = dgYawMatrix(30.0f * 3.1416f / 180.0f) * xxxxx;
dgMatrix xxxxx1(dgGetIdentityMatrix());
xxxxx1[0][0] = 2.0f;
dgMatrix xxxxx2(xxxxx.Inverse() * xxxxx1 * xxxxx);
dgMatrix xxxxx3 (xxxxx2);
xxxxx2.EigenVectors(scale);
dgMatrix xxxxx4(xxxxx2.Inverse() * xxxxx1 * xxxxx2);
//dgFloat32 sign = ((((*this)[0] * (*this)[1]) % (*this)[2]) > 0.0f) ? 1.0f : -1.0f;
dgFloat32 sign = dgSign(((*this)[0] * (*this)[1]) % (*this)[2]);
stretchAxis = (*this) * Transpose();
stretchAxis.EigenVectors (scale);
// I need to deal with by seeing of some of the Scale are duplicated
// do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work)
// for now just us the matrix
scale[0] = sign * dgSqrt (scale[0]);
scale[1] = sign * dgSqrt (scale[1]);
scale[2] = sign * dgSqrt (scale[2]);
scale[3] = dgFloat32 (0.0f);
dgMatrix scaledAxis;
scaledAxis[0] = stretchAxis[0].Scale3 (dgFloat32 (1.0f) / scale[0]);
scaledAxis[1] = stretchAxis[1].Scale3 (dgFloat32 (1.0f) / scale[1]);
scaledAxis[2] = stretchAxis[2].Scale3 (dgFloat32 (1.0f) / scale[2]);
scaledAxis[3] = stretchAxis[3];
dgMatrix symetricInv (stretchAxis.Transpose() * scaledAxis);
transformMatrix = symetricInv * (*this);
transformMatrix.m_posit = m_posit;
}
// Keyboard handler.
void Keyboard()
{
// check for termination
if (dGetKeyState (VK_ESCAPE) & 0x8000) {
exit(0);
}
// read the mouse position and set the camera direction
static MOUSE_POINT mouse0;
static dFloat yawAngle = 90.0f * 3.1416 / 180.0f;
static dFloat rollAngle = 0.0f;
MOUSE_POINT mouse1;
GetCursorPos(mouse1);
if (dGetKeyState (VK_LBUTTON) & 0x8000) {
if (mouse1.x > (mouse0.x + 1)) {
yawAngle += 1.0f * 3.1416 / 180.0f;
if (yawAngle > (360.0f * 3.1416 / 180.0f)) {
yawAngle -= (360.0f * 3.1416 / 180.0f);
}
} else if (mouse1.x < (mouse0.x - 1)) {
yawAngle -= 1.0f * 3.1416 / 180.0f;
if (yawAngle < 0.0f) {
yawAngle += (360.0f * 3.1416 / 180.0f);
}
}
if (mouse1.y > (mouse0.y + 1)) {
rollAngle += 1.0f * 3.1416 / 180.0f;
if (rollAngle > (80.0f * 3.1416 / 180.0f)) {
rollAngle = 80.0f * 3.1416 / 180.0f;
}
} else if (mouse1.y < (mouse0.y - 1)) {
rollAngle -= 1.0f * 3.1416 / 180.0f;
if (rollAngle < -(80.0f * 3.1416 / 180.0f)) {
rollAngle = -80.0f * 3.1416 / 180.0f;
}
}
dMatrix cameraDirMat (dgRollMatrix(rollAngle) * dgYawMatrix(yawAngle));
cameraDir = cameraDirMat.m_front;
}
mouse0 = mouse1;
// camera control
if (dGetKeyState ('W') & 0x8000) {
cameraEyepoint += cameraDir.Scale (CAMERA_SPEED / 60.0f);
} else if (dGetKeyState ('S') & 0x8000) {
cameraEyepoint -= cameraDir.Scale (CAMERA_SPEED / 60.0f);
}
if (dGetKeyState ('D') & 0x8000) {
dVector up (0.0f, 1.0f, 0.0f);
dVector right (cameraDir * up);
cameraEyepoint += right.Scale (CAMERA_SPEED / 60.0f);
} else if (dGetKeyState ('A') & 0x8000) {
dVector up (0.0f, 1.0f, 0.0f);
dVector right (cameraDir * up);
cameraEyepoint -= right.Scale (CAMERA_SPEED / 60.0f);
}
}
