void dgBallConstraint::SetLimits(const dgVector& coneDir,
dgFloat32 minConeAngle, dgFloat32 maxConeAngle, dgFloat32 maxTwistAngle,
const dgVector& bilateralDir, dgFloat32 negativeBilateralConeAngle__,
dgFloat32 positiveBilateralConeAngle__)
{
dgMatrix matrix0;
dgMatrix matrix1;
CalculateGlobalMatrixAndAngle(matrix0, matrix1);
_ASSERTE(m_body0);
_ASSERTE(m_body1);
const dgMatrix& body0_Matrix = m_body0->GetMatrix();
dgVector lateralDir(bilateralDir * coneDir);
if ((lateralDir % lateralDir) < dgFloat32(1.0e-3f))
{
dgMatrix tmp(coneDir);
lateralDir = tmp.m_up;
}
m_localMatrix0.m_front = body0_Matrix.UnrotateVector(coneDir);
m_localMatrix0.m_up = body0_Matrix.UnrotateVector(lateralDir);
m_localMatrix0.m_posit = body0_Matrix.UntransformVector(matrix1.m_posit);
m_localMatrix0.m_front =
m_localMatrix0.m_front.Scale(
dgFloat32(
1.0f) / dgSqrt (m_localMatrix0.m_front % m_localMatrix0.m_front));
m_localMatrix0.m_up = m_localMatrix0.m_up.Scale(
dgFloat32(1.0f) / dgSqrt (m_localMatrix0.m_up % m_localMatrix0.m_up));
m_localMatrix0.m_right = m_localMatrix0.m_front * m_localMatrix0.m_up;
m_localMatrix0.m_front.m_w = dgFloat32(0.0f);
m_localMatrix0.m_up.m_w = dgFloat32(0.0f);
m_localMatrix0.m_right.m_w = dgFloat32(0.0f);
m_localMatrix0.m_posit.m_w = dgFloat32(1.0f);
// dgMatrix body1_Matrix (dgGetIdentityMatrix());
// if (m_body1) {
// body1_Matrix = m_body1->GetMatrix();
// }
const dgMatrix& body1_Matrix = m_body1->GetMatrix();
m_twistAngle = ClampValue(maxTwistAngle, dgFloat32(5.0f) * dgDEG2RAD,
dgFloat32(90.0f) * dgDEG2RAD);
m_coneAngle = ClampValue((maxConeAngle - minConeAngle) * dgFloat32(0.5f),
dgFloat32(5.0f) * dgDEG2RAD, 175.0f * dgDEG2RAD);
m_coneAngleCos = dgCos (m_coneAngle);
dgMatrix coneMatrix(
dgPitchMatrix((maxConeAngle + minConeAngle) * dgFloat32(0.5f)));
m_localMatrix0 = coneMatrix * m_localMatrix0;
m_localMatrix1 = m_localMatrix0 * body0_Matrix * body1_Matrix.Inverse();
}
void RenderJointsDebugInfo (NewtonWorld* const world, dFloat size)
{
glDisable(GL_TEXTURE_2D);
glDisable (GL_LIGHTING);
glBegin(GL_LINES);
// this will go over the joint list twice,
for (NewtonBody* body = NewtonWorldGetFirstBody(world); body; body = NewtonWorldGetNextBody(world, body)) {
for (NewtonJoint* joint = NewtonBodyGetFirstJoint(body); joint; joint = NewtonBodyGetNextJoint(body, joint)) {
NewtonJointRecord info;
NewtonJointGetInfo (joint, &info);
if (strcmp (info.m_descriptionType, "customJointNotInfo")) {
// draw first frame
dMatrix matrix0;
NewtonBodyGetMatrix (info.m_attachBody_0, &matrix0[0][0]);
matrix0 = dMatrix (&info.m_attachmenMatrix_0[0][0]) * matrix0;
dVector o0 (matrix0.m_posit);
dVector x (o0 + matrix0.RotateVector (dVector (size, 0.0f, 0.0f, 0.0f)));
glColor3f (1.0f, 0.0f, 0.0f);
glVertex3f (o0.m_x, o0.m_y, o0.m_z);
glVertex3f (x.m_x, x.m_y, x.m_z);
dVector y (o0 + matrix0.RotateVector (dVector (0.0f, size, 0.0f, 0.0f)));
glColor3f (0.0f, 1.0f, 0.0f);
glVertex3f (o0.m_x, o0.m_y, o0.m_z);
glVertex3f (y.m_x, y.m_y, y.m_z);
dVector z (o0 + matrix0.RotateVector (dVector (0.0f, 0.0f, size, 0.0f)));
glColor3f (0.0f, 0.0f, 1.0f);
glVertex3f (o0.m_x, o0.m_y, o0.m_z);
glVertex3f (z.m_x, z.m_y, z.m_z);
// draw second frame
dMatrix matrix1 (dGetIdentityMatrix());
if (info.m_attachBody_1) {
NewtonBodyGetMatrix (info.m_attachBody_1, &matrix1[0][0]);
}
matrix1 = dMatrix (&info.m_attachmenMatrix_1[0][0]) * matrix1;
dVector o1 (matrix1.m_posit);
x = o1 + matrix1.RotateVector (dVector (size, 0.0f, 0.0f, 0.0f));
glColor3f (1.0f, 0.0f, 0.0f);
glVertex3f (o1.m_x, o1.m_y, o1.m_z);
glVertex3f (x.m_x, x.m_y, x.m_z);
y = o1 + matrix1.RotateVector (dVector (0.0f, size, 0.0f, 0.0f));
glColor3f (0.0f, 1.0f, 0.0f);
glVertex3f (o1.m_x, o1.m_y, o1.m_z);
glVertex3f (y.m_x, y.m_y, y.m_z);
z = o1 + matrix1.RotateVector (dVector (0.0f, 0.0f, size, 0.0f));
glColor3f (0.0f, 0.0f, 1.0f);
glVertex3f (o1.m_x, o1.m_y, o1.m_z);
glVertex3f (z.m_x, z.m_y, z.m_z);
if (!strcmp (info.m_descriptionType, "limitballsocket")) {
// draw the cone limit of this joint
int steps = 12;
dMatrix coneMatrix (dRollMatrix(info.m_maxAngularDof[1]));
dMatrix ratationStep (dPitchMatrix(2.0f * 3.14151693f / steps));
dVector p0 (coneMatrix.RotateVector(dVector (size * 0.5f, 0.0f, 0.0f, 0.0f)));
dVector q0 (matrix1.TransformVector(p0));
glColor3f (1.0f, 1.0f, 0.0f);
for (int i = 0; i < (steps + 1); i ++) {
dVector p1 (ratationStep.RotateVector(p0));
dVector q1 (matrix1.TransformVector(p1));
glVertex3f (o0.m_x, o0.m_y, o0.m_z);
glVertex3f (q0.m_x, q0.m_y, q0.m_z);
glVertex3f (q0.m_x, q0.m_y, q0.m_z);
glVertex3f (q1.m_x, q1.m_y, q1.m_z);
p0 = p1;
q0 = q1;
}
}
}
}
}
glEnd();
}
