To fix this, the line:
- Code: Select all
m_curJointAngle.m_angle = angle;
should be added, for example after line 116 of CustomHinge.cpp:
- Code: Select all
angle = m_curJointAngle.CalculateJointAngle (cosAngle, sinAngle);
Fixed submitContraint:
- Code: Select all
void CustomHinge::SubmitConstraints (dFloat timestep, int threadIndex)
{
// dFloat angle;
// dFloat sinAngle;
// dFloat cosAngle;
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]);
// get a point along the pin axis at some reasonable large distance from the pivot
dVector q0 (matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
dVector q1 (matrix1.m_posit + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));
// two constraints row perpendicular to the pin vector
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_right[0]);
dFloat angle;
dFloat sinAngle;
dFloat cosAngle;
// the joint angle can be determine by getting the angle between any two non parallel vectors
// sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
// cosAngle = matrix0.m_up % matrix1.m_up;
// angle = dAtan2 (sinAngle, cosAngle);
// the joint angle can be determine by getting the angle between any two non parallel vectors
sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
cosAngle = matrix0.m_up % matrix1.m_up;
angle = m_curJointAngle.CalculateJointAngle (cosAngle, sinAngle);
m_curJointAngle.m_angle = angle;
// if limit are enable ...
if (m_limitsOn) {
// if (angle < m_minAngle) {
if (angle < m_minAngle) {
dFloat relAngle;
// relAngle = angle - m_minAngle;
relAngle = angle - m_minAngle;
// the angle was clipped save the new clip limit
m_curJointAngle.m_angle = m_minAngle;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle > m_maxAngle) {
dFloat relAngle;
// relAngle = angle - m_maxAngle;
relAngle = angle - m_maxAngle;
// the angle was clipped save the new clip limit
m_curJointAngle.m_angle = m_maxAngle;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
}
// save the current joint Omega
dVector omega0(0.0f, 0.0f, 0.0f, 0.0f);
dVector omega1(0.0f, 0.0f, 0.0f, 0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
m_jointOmega = (omega0 - omega1) % matrix0.m_front;
}