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)); }
dgUnsigned32 dgUpVectorConstraint::JacobianDerivative (dgContraintDescritor& params) { dgMatrix matrix0; dgMatrix matrix1; CalculateGlobalMatrixAndAngle (matrix0, matrix1); dgVector lateralDir (matrix0.m_front * matrix1.m_front); dgInt32 ret = 0; dgFloat32 mag = lateralDir % lateralDir; if (mag > dgFloat32 (1.0e-6f)) { mag = dgSqrt (mag); lateralDir = lateralDir.Scale3 (dgFloat32 (1.0f) / mag); dgFloat32 angle = dgAsin (mag); CalculateAngularDerivative (0, params, lateralDir, m_stiffness, angle, &m_jointForce[0]); dgVector frontDir (lateralDir * matrix1.m_front); CalculateAngularDerivative (1, params, frontDir, m_stiffness, dgFloat32 (0.0f), &m_jointForce[1]); ret = 2; } else { CalculateAngularDerivative (0, params, matrix0.m_up, m_stiffness, 0.0, &m_jointForce[0]); CalculateAngularDerivative (1, params, matrix0.m_right, m_stiffness, dgFloat32 (0.0f), &m_jointForce[1]); ret = 2; } return dgUnsigned32 (ret); }
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 }