void RenderContactPoints (NewtonWorld* const world)
{
glDisable (GL_LIGHTING);
glDisable(GL_TEXTURE_2D);
glPointSize(8.0f);
glColor3f(1.0f, 0.0f, 0.0f);
glBegin(GL_POINTS);
for (NewtonBody* body = NewtonWorldGetFirstBody(world); body; body = NewtonWorldGetNextBody(world, body)) {
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);
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonMaterialGetContactPositionAndNormal (material, body, &point.m_x, &normal.m_x);
// if we are display debug info we need to block other threads from writing the data at the same time
glVertex3f (point.m_x, point.m_y, point.m_z);
}
}
}
}
glEnd();
glPointSize(1.0f);
}
void CustomTriggerManager::UpdateTrigger (CustomTriggerController* const controller)
{
NewtonBody* const triggerBody = controller->GetBody();
dTree<NewtonBody*,NewtonBody*>& manifest = controller->m_manifest;
for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint (triggerBody); joint; joint = NewtonBodyGetNextContactJoint (triggerBody, joint)) {
int isActive = NewtonJointIsActive (joint);
NewtonBody* const body0 = NewtonJointGetBody0(joint);
NewtonBody* const body1 = NewtonJointGetBody1(joint);
NewtonBody* const passangerBody = (body0 != triggerBody) ? body0 : body1;
if (isActive) {
dTree<NewtonBody*,NewtonBody*>::dTreeNode* const passengerNode = manifest.Find (passangerBody);
if (passengerNode) {
EventCallback (controller, m_inTrigger, passangerBody);
} else {
CustomScopeLock lock (&m_lock);
manifest.Insert (passangerBody, passangerBody);
EventCallback (controller, m_enterTrigger, passangerBody);
}
} else {
dTree<NewtonBody*,NewtonBody*>::dTreeNode* const passengerNode = manifest.Find (passangerBody);
if (passengerNode) {
EventCallback (controller, m_exitTrigger, passangerBody);
CustomScopeLock lock (&m_lock);
manifest.Remove (passengerNode);
}
}
}
}
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));
}
static void RenderBodyContactsAndTangentDiretions (NewtonBody* const body, dFloat length)
{
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);
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonMaterialGetContactPositionAndNormal (material, body, &point.m_x, &normal.m_x);
dVector tangentDir0(0.0f);
dVector tangentDir1(0.0f);
NewtonMaterialGetContactTangentDirections(material, body, &tangentDir0[0], &tangentDir1[0]);
// if we are display debug info we need to block other threads from writing the data at the same time
dVector p1 (point + normal.Scale (length));
dVector p0 (point);
glVertex3f (p0.m_x, p0.m_y, p0.m_z);
glVertex3f (p1.m_x, p1.m_y, p1.m_z);
}
}
}
}
