void dCustomUpVector::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// if the body ha rotated by some amount, the there will be a plane of rotation
dVector lateralDir (matrix0.m_front.CrossProduct(matrix1.m_front));
dFloat mag = lateralDir.DotProduct3(lateralDir);
if (mag > 1.0e-6f) {
// if the side vector is not zero, it means the body has rotated
mag = dSqrt (mag);
lateralDir = lateralDir.Scale (1.0f / mag);
dFloat angle = dAsin (mag);
// add an angular constraint to correct the error angle
NewtonUserJointAddAngularRow (m_joint, angle, &lateralDir[0]);
// in theory only one correction is needed, but this produces instability as the body may move sideway.
// a lateral correction prevent this from happening.
dVector frontDir (lateralDir.CrossProduct(matrix1.m_front));
NewtonUserJointAddAngularRow (m_joint, 0.0f, &frontDir[0]);
} else {
// if the angle error is very small then two angular correction along the plane axis do the trick
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
}
}
void DemoCamera::SetMatrix (DemoEntityManager& scene, const dQuaternion& rotation, const dVector& position)
{
dMatrix matrix (rotation, position);
m_cameraPitch = dAsin (matrix.m_front.m_y);
m_cameraYaw = dAtan2 (-matrix.m_front.m_z, matrix.m_front.m_x);
DemoEntity::SetMatrix (scene, rotation, position);
}
void InitCamera (const dVector& eyePoint, const dVector& dir)
{
gCameraEyepoint = eyePoint;
gCurrCameraDir = dir.Scale (1.0f / sqrt (dir % dir));
gRollAngle = dAsin (gCurrCameraDir.m_y);
gPrevRollAngle = gYawAngle;
gYawAngle = dAtan2 (-gCurrCameraDir.m_z, gCurrCameraDir.m_x);
gPrevYawAngle = gYawAngle;
}
//dVector dMatrix::GetEulerAngles (dEulerAngleOrder order) const
void dMatrix::GetEulerAngles (dVector & euler0, dVector & euler1, dEulerAngleOrder order) const
{
int a0 = (order >> 8) & 3;
int a1 = (order >> 4) & 3;
int a2 = (order >> 0) & 3;
const dMatrix & matrix = *this;
// Assuming the angles are in radians.
if (matrix[a0][a2] > 0.99995f)
{
dFloat picth0 = 0.0f;
dFloat yaw0 = -3.141592f * 0.5f;
dFloat roll0 = - dAtan2 (matrix[a2][a1], matrix[a1][a1]);
euler0[a0] = picth0;
euler0[a1] = yaw0;
euler0[a2] = roll0;
euler1[a0] = picth0;
euler1[a1] = yaw0;
euler1[a2] = roll0;
}
else
if (matrix[a0][a2] < -0.99995f)
{
dFloat picth0 = 0.0f;
dFloat yaw0 = 3.141592f * 0.5f;
dFloat roll0 = dAtan2 (matrix[a2][a1], matrix[a1][a1]);
euler0[a0] = picth0;
euler0[a1] = yaw0;
euler0[a2] = roll0;
euler1[a0] = picth0;
euler1[a1] = yaw0;
euler1[a2] = roll0;
}
else
{
//euler[a0] = -dAtan2(-matrix[a1][a2], matrix[a2][a2]);
//euler[a1] = -dAsin ( matrix[a0][a2]);
//euler[a2] = -dAtan2(-matrix[a0][a1], matrix[a0][a0]);
dFloat yaw0 = -dAsin ( matrix[a0][a2]);
dFloat yaw1 = 3.141592f - yaw0;
dFloat sign0 = dSign (dCos (yaw0));
dFloat sign1 = dSign (dCos (yaw1));
dFloat picth0 = dAtan2 (matrix[a1][a2] * sign0, matrix[a2][a2] * sign0);
dFloat picth1 = dAtan2 (matrix[a1][a2] * sign1, matrix[a2][a2] * sign1);
dFloat roll0 = dAtan2 (matrix[a0][a1] * sign0, matrix[a0][a0] * sign0);
dFloat roll1 = dAtan2 (matrix[a0][a1] * sign1, matrix[a0][a0] * sign1);
if (yaw1 > 3.141592f)
yaw1 -= 2.0f * 3.141592f;
euler0[a0] = picth0;
euler0[a1] = yaw0;
euler0[a2] = roll0;
euler1[a0] = picth1;
euler1[a1] = yaw1;
euler1[a2] = roll1;
}
euler0[3] = dFloat (0.0f);
euler1[3] = dFloat (0.0f);
#ifdef _DEBUG
if (order == m_pitchYawRoll)
{
dMatrix m0 (dPitchMatrix (euler0[0]) * dYawMatrix (euler0[1]) * dRollMatrix (euler0[2]));
dMatrix m1 (dPitchMatrix (euler1[0]) * dYawMatrix (euler1[1]) * dRollMatrix (euler1[2]));
for (int i = 0; i < 3; i ++)
{
for (int j = 0; j < 3; j ++)
{
dFloat error = dAbs (m0[i][j] - matrix[i][j]);
dAssert (error < 5.0e-2f);
error = dAbs (m1[i][j] - matrix[i][j]);
dAssert (error < 5.0e-2f);
}
}
}
#endif
}
