To add angular limit large than +=180 degree that application only need to consider that the error angle need to be calculate in the circular space using the functions sin (a - b) and cos (a - b)
Here is an example of a custom hinge modified to handle any arbitrary angular limit
- Code: Select all
class CustomHinge: public NewtonCustomJoint
{
public:
CustomHinge(const dVector& pivot, const dVector& pin, NewtonBody* child, NewtonBody* parent = NULL);
virtual ~CustomHinge();
void EnableLimits(bool state);
void SetLimis(dFloat minAngle, dFloat maxAngle);
protected:
virtual void SubmitConstrainst ();
float CalculateJointAngle (const dMatrix& matrix0, const dMatrix& matrix1) const;
dMatrix m_localMatrix0;
dMatrix m_localMatrix1;
bool m_limitsOn;
dFloat m_minAngle;
dFloat m_maxAngle;
dFloat m_curJointAngle;
};
#define MIN_JOINT_PIN_LENGTH 50.0f
CustomHinge::CustomHinge(const dVector& pivot, const dVector& pin, NewtonBody* child, NewtonBody* parent)
:NewtonCustomJoint(6, child, parent)
{
m_limitsOn = false;
m_minAngle = -45.0f * 3.1416f / 180.0f;
m_maxAngle = 45.0f * 3.1416f / 180.0f;
// the joint current angle is zero at joint creation time
m_curJointAngle = 0.0f;
// calculate the two local matrix of the pivot point
CalculateLocalMatrix (pivot, pin, m_localMatrix0, m_localMatrix1);
}
CustomHinge::~CustomHinge()
{
}
void CustomHinge::EnableLimits(bool state)
{
m_limitsOn = state;
}
void CustomHinge::SetLimis(dFloat minAngle, dFloat maxAngle)
{
//_ASSERTE (minAngle < 0.0f);
//_ASSERTE (maxAngle > 0.0f);
m_minAngle = minAngle;
m_maxAngle = maxAngle;
}
// the funtion calculate the error angular considering angula wraping aroung += 180 degree
float CustomHinge::CalculateJointAngle (const dMatrix& matrix0, const dMatrix& matrix1) const
{
dFloat sinAngle;
dFloat cosAngle;
dFloat sinJointAngle;
dFloat cosJointAngle;
dFloat sin_da;
dFloat cos_da;
// 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;
sinJointAngle = sinf (m_curJointAngle);
cosJointAngle = cosf (m_curJointAngle);
sin_da = sinAngle * cosJointAngle - cosAngle * sinJointAngle;
cos_da = cosAngle * cosJointAngle + sinAngle * sinJointAngle;
return dAtan2 (sin_da, cos_da) + m_curJointAngle;
}
void CustomHinge::SubmitConstrainst ()
{
// dFloat angle;
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]);
// if limit are enable ...
if (m_limitsOn) {
// 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);
m_curJointAngle = CalculateJointAngle (matrix0, matrix1);
if (m_curJointAngle < m_minAngle) {
dFloat relAngle;
relAngle = m_curJointAngle - m_minAngle;
// the angle was clipped save the new clipp limit
m_curJointAngle = 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 (m_curJointAngle > m_maxAngle) {
dFloat relAngle;
relAngle = m_curJointAngle - m_maxAngle;
// the angle was clipped save the new clipp limit
m_curJointAngle = 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);
}
}
}