static void RenderBodyContactsForces (NewtonBody* const body, dFloat scale)
{
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
//draw normal forces in term of acceleration.
//this mean that two bodies with same shape but different mass will display the same force
if (mass > 0.0f) {
scale = scale/mass;
for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(body); joint; joint = NewtonBodyGetNextContactJoint(body, joint)) {
if (NewtonJointIsActive (joint)) {
for (void* contact = NewtonContactJointGetFirstContact (joint); contact; contact = NewtonContactJointGetNextContact (joint, contact)) {
dVector point(0.0f);
dVector normal(0.0f);
dVector tangnetDir0(0.0f);
dVector tangnetDir1(0.0f);
dVector contactForce(0.0f);
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonMaterialGetContactForce(material, body, &contactForce.m_x);
NewtonMaterialGetContactPositionAndNormal (material, body, &point.m_x, &normal.m_x);
dVector normalforce (normal.Scale (contactForce % normal));
dVector p0 (point);
dVector p1 (point + normalforce.Scale (scale));
glVertex3f (p0.m_x, p0.m_y, p0.m_z);
glVertex3f (p1.m_x, p1.m_y, p1.m_z);
// these are the components of the tangents forces at the contact point, the can be display at the contact position point.
NewtonMaterialGetContactTangentDirections(material, body, &tangnetDir0[0], &tangnetDir1[0]);
dVector tangentForce1 (tangnetDir0.Scale ((contactForce % tangnetDir0) * scale));
dVector tangentForce2 (tangnetDir1.Scale ((contactForce % tangnetDir1) * scale));
p1 = point + tangentForce1.Scale (scale);
glVertex3f(p0.m_x, p0.m_y, p0.m_z);
glVertex3f(p1.m_x, p1.m_y, p1.m_z);
p1 = point + tangentForce2.Scale (scale);
glVertex3f(p0.m_x, p0.m_y, p0.m_z);
glVertex3f(p1.m_x, p1.m_y, p1.m_z);
}
}
}
}
}
dVector ForceBetweenBody (NewtonBody* const body0, NewtonBody* const body1)
{
dVector reactionforce (0.0f);
for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(body0); joint; joint = NewtonBodyGetNextContactJoint(body0, joint)) {
if (NewtonJointIsActive(joint) && (NewtonJointGetBody0(joint) == body0) || (NewtonJointGetBody0(joint) == body1)) {
for (void* contact = NewtonContactJointGetFirstContact (joint); contact; contact = NewtonContactJointGetNextContact (joint, contact)) {
dVector point(0.0f);
dVector normal(0.0f);
dVector contactForce(0.0f);
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonMaterialGetContactPositionAndNormal (material, body0, &point.m_x, &normal.m_x);
NewtonMaterialGetContactForce(material, body0, &contactForce[0]);
reactionforce += contactForce;
}
break;
}
}
return reactionforce;
}
dFloat ForceBodyAccelerationMichio (NewtonBody* const body)
{
dVector reactionforce (0.0f);
// calcualte accelration generate by all contacts
for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(body); joint; joint = NewtonBodyGetNextContactJoint(body, joint)) {
if (NewtonJointIsActive(joint)) {
for (void* contact = NewtonContactJointGetFirstContact(joint); contact; contact = NewtonContactJointGetNextContact(joint, contact)) {
dVector contactForce(0.0f);
NewtonMaterial* const material = NewtonContactGetMaterial(contact);
NewtonMaterialGetContactForce(material, body, &contactForce[0]);
reactionforce += contactForce;
}
}
}
dMatrix matrix;
dVector accel;
dVector veloc;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
NewtonBodyGetAcceleration(body, &accel[0]);
accel -= reactionforce.Scale (1.0f/mass);
//calculate centripetal acceleration here.
NewtonBodyGetMatrix(body, &matrix[0][0]);
dVector radius(matrix.m_posit.Scale(-1.0f));
radius.m_w = 0.0f;
dFloat radiusMag = dSqrt(radius.DotProduct3(radius));
dVector radiusDir (radius.Normalize());
NewtonBodyGetVelocity(body, &veloc[0]);
veloc += radiusDir.Scale(veloc.DotProduct3(radiusDir));
dVector centripetalAccel(veloc.DotProduct3(veloc) / radiusMag);
accel += centripetalAccel;
return dSqrt (accel.DotProduct3(accel));
}
