void GetConnectedBodiesByJoints (NewtonBody* const body)
{
for (NewtonJoint* joint = NewtonBodyGetFirstJoint(body); joint; joint = NewtonBodyGetNextJoint(body, joint)) {
CustomJoint* const customJoint = (CustomJoint*) NewtonJointGetUserData(joint);
NewtonBody* const body0 = customJoint->GetBody0();
NewtonBody* const body1 = customJoint->GetBody1();
NewtonBody* const otherBody = (body0 == body) ? body1 : body0;
// do whatever you need to do here
NewtonBodySetFreezeState (otherBody, 0);
}
}
static void SetTransform (const NewtonBody* body, const dFloat* matrix, int threadId)
{
NewtonUserJoint* player;
PlayerController* controller;
// find the player joint;
player = NULL;
for (NewtonJoint* joint = NewtonBodyGetFirstJoint(body); joint; joint = NewtonBodyGetNextJoint(body, joint)) {
NewtonUserJoint* tmp;
tmp = (NewtonUserJoint*) NewtonJointGetUserData(joint);
if (CustomGetJointID (tmp) == PLAYER_JOINT_ID) {
player = tmp;
break;
}
}
// call the generic transform callback
controller = (PlayerController*) CustomGetUserData(player);
#if 1
// this will project the visual mesh to the ground
dMatrix visualMatrix;
CustomPlayerControllerGetVisualMaTrix (player, &visualMatrix[0][0]);
#else
// this will display the player at the collision shape position
const dMatrix& visualMatrix = *((dMatrix*) matrix);
#endif
controller->m_setTransformOriginal (body, &visualMatrix[0][0], threadId);
// now we will set the camera to follow the player
dVector eyePoint (visualMatrix.TransformVector(controller->m_point));
// check if the player wants third person view
static int prevCKeyDown = IsKeyDown ('C');
int isCkeyDwon = IsKeyDown ('C');
if (isCkeyDwon && !prevCKeyDown) {
controller->m_isThirdView = !controller->m_isThirdView;
}
prevCKeyDown = isCkeyDwon;
if (controller->m_isThirdView) {
dVector dir (GetCameraDir ());
eyePoint -= dir.Scale (8.0f);
}
SetCameraEyePoint (eyePoint);
// NewtonBodyGetMatrix (body, &matrix[0][0]);
// cameraEyepoint = matrix.m_posit;
// cameraEyepoint.m_y += 1.0f;
}
dCustomJoint* dSkeletonBone::GetParentJoint() const
{
if (m_parent) {
for (NewtonJoint* joint = NewtonBodyGetFirstJoint(m_body); joint; joint = NewtonBodyGetNextJoint(m_body, joint)) {
dCustomJoint* const customJoint = (dCustomJoint*)NewtonJointGetUserData(joint);
dAssert(customJoint);
if (((customJoint->GetBody0() == m_body) && (customJoint->GetBody1() == m_parent->m_body)) ||
((customJoint->GetBody1() == m_body) && (customJoint->GetBody0() == m_parent->m_body))) {
return customJoint;
}
}
dAssert(0);
}
return NULL;
}
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();
}
