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