void GetForceOnStaticBody (NewtonBody* body, NewtonBody* staticBody) { for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint (body); joint; joint = NewtonBodyGetNextContactJoint (body, joint)) { NewtonBody* body0; NewtonBody* body1; body0 = NewtonJointGetBody0(joint); body1 = NewtonJointGetBody1(joint); if ((body0 == staticBody) || (body1 == staticBody)) { for (void* contact = NewtonContactJointGetFirstContact (joint); contact; contact = NewtonContactJointGetNextContact (joint, contact)) { float forceMag; dVector point; dVector normal; NewtonMaterial* material; material = NewtonContactGetMaterial (contact); NewtonMaterialGetContactForce (material, &forceMag); NewtonMaterialGetContactPositionAndNormal (material, &point.m_x, &normal.m_x); dVector force (normal.Scale (-forceMag)); // do wherever you want withteh force } } } }
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); }
VALUE MSNewton::Bodies::get_force_in_between(VALUE self, VALUE v_body1, VALUE v_body2) { const NewtonBody* body1 = Util::value_to_body(v_body1); const NewtonBody* body2 = Util::value_to_body(v_body2); Util::validate_two_bodies(body1, body2); dVector net_force(0.0f, 0.0f, 0.0f); for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(body1); joint; joint = NewtonBodyGetNextContactJoint(body1, joint)) { if (NewtonJointGetBody0(joint) == body2 || NewtonJointGetBody1(joint) == body2) { for (void* contact = NewtonContactJointGetFirstContact(joint); contact; contact = NewtonContactJointGetNextContact(joint, contact)) { NewtonMaterial* material = NewtonContactGetMaterial(contact); dVector force; NewtonMaterialGetContactForce(material, body1, &force[0]); net_force += force; } } } const NewtonWorld* world = NewtonBodyGetWorld(body1); WorldData* world_data = (WorldData*)NewtonWorldGetUserData(world); //BodyData* body1_data = (BodyData*)NewtonBodyGetUserData(body1); //BodyData* body2_data = (BodyData*)NewtonBodyGetUserData(body2); /*if (world_data->gravity_enabled && (body1_data->gravity_enabled || body2_data->gravity_enabled)) { for (int i = 0; i < 3; ++i) net_force[i] *= world_data->inverse_scale; }*/ return Util::vector_to_value(net_force, world_data->inverse_scale4); }
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); } } } }
// return the collision joint, if the body collide NewtonJoint* CheckIfBodiesCollide (NewtonBody* const body0, NewtonBody* const body1) { for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint (body0); joint; joint = NewtonBodyGetNextContactJoint (body0, joint)) { if ((NewtonJointGetBody0(joint) == body1) || (NewtonJointGetBody1(joint) == body1)) { return joint; } } return NULL; }
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); } } } } }
void GetContactOnBody (NewtonBody* const body) { for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint (body); joint; joint = NewtonBodyGetNextContactJoint (body, joint)) { NewtonBody* const body0 = NewtonJointGetBody0(joint); NewtonBody* const body1 = NewtonJointGetBody1(joint); for (void* contact = NewtonContactJointGetFirstContact (joint); contact; contact = NewtonContactJointGetNextContact (joint, contact)) { NewtonMaterial* material = NewtonContactGetMaterial (contact); //dFloat forceMag; dVector point; dVector normal; //NewtonMaterialGetContactForce (material, &forceMag); NewtonMaterialGetContactPositionAndNormal (material, body1, &point.m_x, &normal.m_x); NewtonMaterialGetContactPositionAndNormal (material, body0, &point.m_x, &normal.m_x); // do whatever you want with the force } } }
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); } } } }
void SimulationLister(DemoEntityManager* const scene, DemoEntityManager::dListNode* const mynode, dFloat timeStep) { m_delay --; if (m_delay > 0) { return; } // see if the net force on the body comes fr a high impact collision dFloat maxInternalForce = 0.0f; for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(m_myBody); joint; joint = NewtonBodyGetNextContactJoint(m_myBody, joint)) { for (void* contact = NewtonContactJointGetFirstContact (joint); contact; contact = NewtonContactJointGetNextContact (joint, contact)) { //dVector point; //dVector normal; dVector contactForce; NewtonMaterial* const material = NewtonContactGetMaterial (contact); //NewtonMaterialGetContactPositionAndNormal (material, &point.m_x, &normal.m_x); NewtonMaterialGetContactForce(material, m_myBody, &contactForce[0]); dFloat forceMag = contactForce % contactForce; if (forceMag > maxInternalForce) { maxInternalForce = forceMag; } } } // if the force is bigger than 4 Gravities, It is considered a collision force dFloat maxForce = BREAK_FORCE_IN_GRAVITIES * m_myweight; if (maxInternalForce > (maxForce * maxForce)) { NewtonWorld* const world = NewtonBodyGetWorld(m_myBody); dFloat Ixx; dFloat Iyy; dFloat Izz; dFloat mass; NewtonBodyGetMassMatrix(m_myBody, &mass, &Ixx, &Iyy, &Izz); dVector com; dVector veloc; dVector omega; dMatrix bodyMatrix; NewtonBodyGetVelocity(m_myBody, &veloc[0]); NewtonBodyGetOmega(m_myBody, &omega[0]); NewtonBodyGetCentreOfMass(m_myBody, &com[0]); NewtonBodyGetMatrix(m_myBody, &bodyMatrix[0][0]); com = bodyMatrix.TransformVector (com); dMatrix matrix (GetCurrentMatrix()); dQuaternion rotation (matrix); for (ShatterEffect::dListNode* node = m_effect.GetFirst(); node; node = node->GetNext()) { ShatterAtom& atom = node->GetInfo(); DemoEntity* const entity = new DemoEntity (NULL); entity->SetMesh (atom.m_mesh); entity->SetMatrix(*scene, rotation, matrix.m_posit); entity->InterpolateMatrix (*scene, 1.0f); scene->Append(entity); int materialId = 0; dFloat debriMass = mass * atom.m_massFraction; dFloat Ixx = debriMass * atom.m_momentOfInirtia.m_x; dFloat Iyy = debriMass * atom.m_momentOfInirtia.m_y; dFloat Izz = debriMass * atom.m_momentOfInirtia.m_z; //create the rigid body NewtonBody* const rigidBody = NewtonCreateBody (world, atom.m_collision, &matrix[0][0]); // set the correct center of gravity for this body NewtonBodySetCentreOfMass (rigidBody, &atom.m_centerOfMass[0]); // calculate the center of mas of the debris dVector center (matrix.TransformVector(atom.m_centerOfMass)); // calculate debris initial velocity dVector v (veloc + omega * (center - com)); // set initial velocity NewtonBodySetVelocity(rigidBody, &v[0]); NewtonBodySetOmega(rigidBody, &omega[0]); // set the debrie center of mass NewtonBodySetCentreOfMass (rigidBody, &atom.m_centerOfMass[0]); // set the mass matrix NewtonBodySetMassMatrix (rigidBody, debriMass, Ixx, Iyy, Izz); // activate // NewtonBodyCoriolisForcesMode (blockBoxBody, 1); // save the pointer to the graphic object with the body. NewtonBodySetUserData (rigidBody, entity); // assign the wood id NewtonBodySetMaterialGroupID (rigidBody, materialId); // set continue collision mode // NewtonBodySetContinuousCollisionMode (rigidBody, continueCollisionMode); // set a destructor for this rigid body NewtonBodySetDestructorCallback (rigidBody, PhysicsBodyDestructor); // set the transform call back function NewtonBodySetTransformCallback (rigidBody, DemoEntity::SetTransformCallback); // set the force and torque call back function NewtonBodySetForceAndTorqueCallback (rigidBody, PhysicsApplyGravityForce); } NewtonDestroyBody(world, m_myBody); scene->RemoveEntity (mynode); } };
void ApplyTracktionForce (dFloat timestep, const NewtonBody* track) { dVector veloc; dVector omega; dMatrix matrix; NewtonBodyGetOmega(m_body0, &omega[0]); NewtonBodyGetVelocity(m_body0, &veloc[0]); NewtonBodyGetMatrix (m_body0, &matrix[0][0]); // itetate over the contact list and condition each contact direction anc contact acclerations for (NewtonJoint* contactJoint = NewtonBodyGetFirstContactJoint (track); contactJoint; contactJoint = NewtonBodyGetNextContactJoint (track, contactJoint)) { _ASSERTE ((NewtonJointGetBody0 (contactJoint) == track) || (NewtonJointGetBody1 (contactJoint) == track)); #ifdef REMOVE_REDUNDAT_CONTACT int contactCount; contactCount = NewtonContactJointGetContactCount(contactJoint); if (contactCount > 2) { // project the contact to the bounday of the conve hull o fteh trhread foot ptint dFloat maxDist; dFloat minDist; void* minContact; void* maxContact; dMatrix matrix; minContact = NULL; maxContact = NULL; NewtonBodyGetMatrix (track, &matrix[0][0]); maxDist = -1.0e10f; minDist = -1.0e10f; //find the best two contacts and remove all others for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) { dFloat dist; dVector point; dVector normal; NewtonMaterial* material; material = NewtonContactGetMaterial (contact); NewtonMaterialGetContactPositionAndNormal(material, &point[0], &normal[0]); dist = matrix.m_front % point; if (dist > maxDist) { maxDist = dist; maxContact = contact; } if (-dist > minDist) { minDist = -dist; minContact = contact; } } // now delete all reduntact contacts void* nextContact; NewtonWorld* world; world = NewtonBodyGetWorld (track); NewtonWorldCriticalSectionLock(world); for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = nextContact) { nextContact = NewtonContactJointGetNextContact (contactJoint, contact); if (!((contact == minContact) || (contact == maxContact))) { NewtonContactJointRemoveContact (contactJoint, contact); } } NewtonWorldCriticalSectionUnlock(world); } #endif for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) { dFloat speed; dFloat accel; dVector point; dVector normal; dVector dir0; dVector dir1; NewtonMaterial* material; material = NewtonContactGetMaterial (contact); NewtonMaterialContactRotateTangentDirections (material, &matrix.m_front[0]); NewtonMaterialGetContactPositionAndNormal(material, &point[0], &normal[0]); NewtonMaterialGetContactTangentDirections (material, &dir0[0], &dir1[0]); dVector posit (point - matrix.m_posit); veloc += omega * posit; speed = veloc % dir0; // accel = m_accel - 0.1f * speed + (((posit % m_matrix.m_right) > 0.0f) ? m_turnAccel : - m_turnAccel); accel = m_veloc + (((posit % matrix.m_right) > 0.0f) ? m_turnVeloc : - m_turnVeloc); accel = (accel - speed) * 0.5f / timestep; // NewtonMaterialSetContactStaticFrictionCoef (material, 1.0f, 0); // NewtonMaterialSetContactKineticFrictionCoef (material, 1.0f, 0); NewtonMaterialSetContactFrictionCoef (material, 1.0f, 1.0f, 0); // NewtonMaterialSetContactStaticFrictionCoef (material, 0.5f, 1); // NewtonMaterialSetContactKineticFrictionCoef (material, 0.5f, 1); NewtonMaterialSetContactFrictionCoef (material, 0.5f, 0.5f, 1); NewtonMaterialSetContactTangentAcceleration (material, accel, 0); } // for debug purpose show the contact ShowJointContacts (contactJoint); } }
void SimulationPostListener(DemoEntityManager* const scene, DemoEntityManager::dListNode* const mynode, dFloat timeStep) { // see if the net force on the body comes fr a high impact collision dFloat breakImpact = 0.0f; for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(m_myBody); joint; joint = NewtonBodyGetNextContactJoint(m_myBody, joint)) { for (void* contact = NewtonContactJointGetFirstContact(joint); contact; contact = NewtonContactJointGetNextContact(joint, contact)) { dVector contactForce; NewtonMaterial* const material = NewtonContactGetMaterial(contact); dFloat impulseImpact = NewtonMaterialGetContactMaxNormalImpact(material); if (impulseImpact > breakImpact) { breakImpact = impulseImpact; } } } // if the force is bigger than N time Gravities, It is considered a collision force breakImpact *= m_myMassInverse; // breakImpact = 1000.0f; if (breakImpact > BREAK_IMPACT_IN_METERS_PER_SECONDS) { NewtonWorld* const world = NewtonBodyGetWorld(m_myBody); dMatrix bodyMatrix; dVector com(0.0f); dVector veloc(0.0f); dVector omega(0.0f); dFloat Ixx; dFloat Iyy; dFloat Izz; dFloat mass; NewtonBodyGetVelocity(m_myBody, &veloc[0]); NewtonBodyGetOmega(m_myBody, &omega[0]); NewtonBodyGetCentreOfMass(m_myBody, &com[0]); NewtonBodyGetMatrix(m_myBody, &bodyMatrix[0][0]); NewtonBodyGetMass(m_myBody, &mass, &Ixx, &Iyy, &Izz); com = bodyMatrix.TransformVector(com); dMatrix matrix(GetCurrentMatrix()); dQuaternion rotation(matrix); // we need to lock the world before creation a bunch of bodies scene->Lock(m_lock); for (FractureEffect::dListNode* node = m_effect.GetFirst(); node; node = node->GetNext()) { FractureAtom& atom = node->GetInfo(); DemoEntity* const entity = new DemoEntity(dMatrix(rotation, matrix.m_posit), NULL); entity->SetMesh(atom.m_mesh, dGetIdentityMatrix()); scene->Append(entity); int materialId = 0; dFloat debriMass = mass * atom.m_massFraction; //create the rigid body NewtonBody* const rigidBody = NewtonCreateDynamicBody(world, atom.m_collision, &matrix[0][0]); // calculate debris initial velocity dVector center(matrix.TransformVector(atom.m_centerOfMass)); dVector v(veloc + omega.CrossProduct(center - com)); // set initial velocity NewtonBodySetVelocity(rigidBody, &v[0]); NewtonBodySetOmega(rigidBody, &omega[0]); // set the debris mass properties, mass, center of mass, and inertia NewtonBodySetMassProperties(rigidBody, debriMass, atom.m_collision); // save the pointer to the graphic object with the body. NewtonBodySetUserData(rigidBody, entity); // assign the wood id NewtonBodySetMaterialGroupID(rigidBody, materialId); // set continuous collision mode // NewtonBodySetContinuousCollisionMode (rigidBody, continueCollisionMode); // set a destructor for this rigid body NewtonBodySetDestructorCallback(rigidBody, PhysicsBodyDestructor); // set the transform call back function NewtonBodySetTransformCallback(rigidBody, DemoEntity::TransformCallback); // set the force and torque call back function NewtonBodySetForceAndTorqueCallback(rigidBody, PhysicsApplyGravityForce); } NewtonDestroyBody(m_myBody); scene->RemoveEntity(mynode); // unlock the work after done with the effect scene->Unlock(m_lock); } }