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));
}
dgVector dgQuaternion::CalcAverageOmega (const dgQuaternion &q1, dgFloat32 invdt) const
{
dgQuaternion q0 (*this);
if (q0.DotProduct (q1) < 0.0f) {
q0.Scale(-1.0f);
}
dgQuaternion dq (q0.Inverse() * q1);
dgVector omegaDir (dq.m_q1, dq.m_q2, dq.m_q3, dgFloat32 (0.0f));
dgFloat32 dirMag2 = omegaDir % omegaDir;
if (dirMag2 < dgFloat32(dgFloat32 (1.0e-5f) * dgFloat32 (1.0e-5f))) {
return dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
}
dgFloat32 dirMagInv = dgRsqrt (dirMag2);
dgFloat32 dirMag = dirMag2 * dirMagInv;
dgFloat32 omegaMag = dgFloat32(2.0f) * dgAtan2 (dirMag, dq.m_q0) * invdt;
return omegaDir.Scale3 (dirMagInv * omegaMag);
}
dgUnsigned32 dgUniversalConstraint::JacobianDerivative (dgContraintDescritor& params)
{
dgInt32 ret;
dgFloat32 sinAngle;
dgFloat32 cosAngle;
dgMatrix matrix0;
dgMatrix matrix1;
CalculateGlobalMatrixAndAngle (matrix0, matrix1);
const dgVector& dir0 = matrix0.m_front;
const dgVector& dir1 = matrix1.m_up;
dgVector dir2 (dir0 * dir1);
dgVector dir3 (dir2 * dir0);
dir3 = dir3.Scale3 (dgRsqrt (dir3 % dir3));
const dgVector& p0 = matrix0.m_posit;
const dgVector& p1 = matrix1.m_posit;
dgVector q0 (p0 + dir3.Scale3(MIN_JOINT_PIN_LENGTH));
dgVector q1 (p1 + dir1.Scale3(MIN_JOINT_PIN_LENGTH));
dgPointParam pointDataP;
dgPointParam pointDataQ;
InitPointParam (pointDataP, m_stiffness, p0, p1);
InitPointParam (pointDataQ, m_stiffness, q0, q1);
CalculatePointDerivative (0, params, dir0, pointDataP, &m_jointForce[0]);
CalculatePointDerivative (1, params, dir1, pointDataP, &m_jointForce[1]);
CalculatePointDerivative (2, params, dir2, pointDataP, &m_jointForce[2]);
CalculatePointDerivative (3, params, dir0, pointDataQ, &m_jointForce[3]);
ret = 4;
// dgVector sinAngle0 (matrix1.m_up * matrix0.m_up);
// m_angle0 = dgAsin (ClampValue (sinAngle0 % dir0, -0.9999999f, 0.9999999f));
// if ((matrix0.m_up % matrix1.m_up) < dgFloat32 (0.0f)) {
// m_angle0 = (m_angle0 >= dgFloat32 (0.0f)) ? dgPI - m_angle0 : dgPI + m_angle0;
// }
sinAngle = (matrix1.m_up * matrix0.m_up) % matrix0.m_front;
cosAngle = matrix0.m_up % matrix1.m_up;
// dgAssert (dgAbsf (m_angle0 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f);
m_angle0 = dgAtan2 (sinAngle, cosAngle);
// dgVector sinAngle1 (matrix0.m_front * matrix1.m_front);
// m_angle1 = dgAsin (ClampValue (sinAngle1 % dir1, -0.9999999f, 0.9999999f));
// if ((matrix0.m_front % matrix1.m_front) < dgFloat32 (0.0f)) {
// m_angle1 = (m_angle1 >= dgFloat32 (0.0f)) ? dgPI - m_angle1 : dgPI + m_angle1;
// }
sinAngle = (matrix0.m_front * matrix1.m_front) % matrix1.m_up;
cosAngle = matrix0.m_front % matrix1.m_front;
// dgAssert (dgAbsf (m_angle1 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f);
m_angle1 = dgAtan2 (sinAngle, cosAngle);
if (m_jointAccelFnt) {
dgUnsigned32 code;
dgJointCallbackParam axisParam[2];
// linear acceleration
axisParam[0].m_accel = dgFloat32 (0.0f);
axisParam[0].m_timestep = params.m_timestep;
axisParam[0].m_minFriction = DG_MIN_BOUND;
axisParam[0].m_maxFriction = DG_MAX_BOUND;
// angular acceleration
axisParam[1].m_accel = dgFloat32 (0.0f);
axisParam[1].m_timestep = params.m_timestep;
axisParam[1].m_minFriction = DG_MIN_BOUND;
axisParam[1].m_maxFriction = DG_MAX_BOUND;
code = m_jointAccelFnt (*this, axisParam);
if (code & 1) {
if ((axisParam[0].m_minFriction > DG_MIN_BOUND) || (axisParam[0].m_maxFriction < DG_MAX_BOUND)) {
params.m_forceBounds[ret].m_low = axisParam[0].m_minFriction;
params.m_forceBounds[ret].m_upper = axisParam[0].m_maxFriction;
params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
}
// CalculatePointDerivative (ret, params, dir0, pointDataP, &m_jointForce[ret]);
CalculateAngularDerivative (ret, params, dir0, m_stiffness, dgFloat32 (0.0f), &m_jointForce[ret]);
//params.m_jointAccel[ret] = axisParam[0].m_accel;
SetMotorAcceleration (ret, axisParam[0].m_accel, params);
ret ++;
}
if (code & 2) {
if ((axisParam[1].m_minFriction > DG_MIN_BOUND) || (axisParam[1].m_maxFriction < DG_MAX_BOUND)) {
params.m_forceBounds[ret].m_low = axisParam[1].m_minFriction;
params.m_forceBounds[ret].m_upper = axisParam[1].m_maxFriction;
params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
}
CalculateAngularDerivative (ret, params, dir1, m_stiffness, dgFloat32 (0.0f), &m_jointForce[ret]);
//params.m_jointAccel[ret] = axisParam[1].m_accel;
SetMotorAcceleration (ret, axisParam[1].m_accel, params);
ret ++;
}
}
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
}
