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(); }