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); } }