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 GenericContactProcess (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
int isHightField;
NewtonBody* body;
NewtonCollision* collision;
NewtonCollisionInfoRecord info;
isHightField = 1;
body = NewtonJointGetBody0 (contactJoint);
collision = NewtonBodyGetCollision(body);
NewtonCollisionGetInfo(collision, &info);
if (info.m_collisionType != SERIALIZE_ID_HEIGHTFIELD) {
body = NewtonJointGetBody1 (contactJoint);
collision = NewtonBodyGetCollision(body);
NewtonCollisionGetInfo(collision, &info);
isHightField = (info.m_collisionType == SERIALIZE_ID_HEIGHTFIELD);
}
#define HOLE_IN_TERRAIN 10
if (isHightField) {
void* nextContact;
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = nextContact) {
int faceID;
NewtonMaterial* material;
nextContact = NewtonContactJointGetNextContact (contactJoint, contact);
material = NewtonContactGetMaterial (contact);
faceID = NewtonMaterialGetContactFaceAttribute (material);
if (faceID == HOLE_INTERRAIN) {
NewtonContactJointRemoveContact (contactJoint, contact);
}
}
}
}
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 tContact::_newtonContactProcess(const NewtonJoint * contactJoint, dFloat timeStep, int threadIndex)
{
Vec3 contactPosit, contactNormal;
float contactBestSpeed = 0.5f;
tContact * mtl = NULL;
const NewtonBody * body0 = NewtonJointGetBody0(contactJoint);
void * contact = NewtonContactJointGetFirstContact (contactJoint);
while (contact)
{
float contactNormalSpeed;
NewtonMaterial * material;
// get the material for this contact;
material = NewtonContactGetMaterial (contact);
contactNormalSpeed = NewtonMaterialGetContactNormalSpeed (material);
if (contactNormalSpeed > contactBestSpeed){
contactBestSpeed = contactNormalSpeed;
contactBestSpeed = contactNormalSpeed;
NewtonMaterialGetContactPositionAndNormal(material, &contactPosit.x, &contactNormal[0]);
mtl = (tContact *)NewtonMaterialGetMaterialPairUserData(material);
}
contact = NewtonContactJointGetNextContact (contactJoint, contact);
}
if (mtl)
mtl->OnContactProcess(&contactPosit, &contactNormal);
}
static void PhysicsNewton_CollisionPuckSurfaceCB(const NewtonJoint *pContactJoint,dFloat fTimeStep,int ThreadIndex)
{
dVector Position(0.0f);
// Get pointer to body
NewtonBody* body = NewtonJointGetBody0(pContactJoint);
dFloat mass, Ixx, Iyy, Izz;
NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
if (mass == 0.0f)
{
body = NewtonJointGetBody1(pContactJoint);
}
dVector tableDir(0.0f, 0.0f, 1.0f, 0.0f);
// Test to see if it is the friction calculation that is causing the side force
// With this the Puck must go straight because it will be frictionless, the net force should not change direction
for (void* contact = NewtonContactJointGetFirstContact (pContactJoint); contact; contact = NewtonContactJointGetNextContact (pContactJoint, contact))
{
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonMaterialContactRotateTangentDirections(material, &tableDir[0]);
// this the wrong way to make a friction less contact
// NewtonMaterialSetContactFrictionCoef (material, 0.0f, 0.0f, 0);
// NewtonMaterialSetContactFrictionCoef (material, 0.0f, 0.0f, 1);
//This is the correct way to make a friction less contact
// NewtonMaterialSetContactFrictionState (material, 0, 0);
// NewtonMaterialSetContactFrictionState (material, 0, 1);
}
}
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);
}
static void UserContactFriction (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
// call the basic call back
GenericContactProcess (contactJoint, timestep, threadIndex);
const NewtonBody* const body0 = NewtonJointGetBody0(contactJoint);
const NewtonBody* const body1 = NewtonJointGetBody1(contactJoint);
const NewtonBody* body = body0;
NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
if (mass == 0.0f) {
body = body1;
}
//now core 300 can have per collision user data
NewtonCollision* const collision = NewtonBodyGetCollision(body);
void* userData = NewtonCollisionGetUserData (collision);
dFloat frictionValue = *((dFloat*)&userData);
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonMaterialSetContactFrictionCoef (material, frictionValue + 0.1f, frictionValue, 0);
NewtonMaterialSetContactFrictionCoef (material, frictionValue + 0.1f, frictionValue, 1);
}
}
static void GetCollisionSubShape(const NewtonJoint* const contactJoint, NewtonBody* const body)
{
NewtonCollisionInfoRecord collisionInfo;
NewtonCollision* const collision = NewtonBodyGetCollision(body);
NewtonCollisionGetInfo (collision, &collisionInfo);
int count = 0;
NewtonCollision* collidingSubShapeArrar[32];
// see if this is a compound collision or any other collision with sub collision shapes
if (collisionInfo.m_collisionType == SERIALIZE_ID_COMPOUND) {
// to get the torque we need the center of gravity in global space
dVector origin;
dMatrix bodyMatrix;
NewtonBodyGetMatrix(body, &bodyMatrix[0][0]);
NewtonBodyGetCentreOfMass(body, &origin[0]);
origin = bodyMatrix.TransformVector(origin);
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
// get the material of this contact,
// this part contain all contact information, the sub colliding shape,
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonCollision* const subShape = NewtonMaterialGetBodyCollidingShape (material, body);
int i = count - 1;
for (; i >= 0; i --) {
if (collidingSubShapeArrar[i] == subShape) {
break;
}
}
if (i < 0) {
collidingSubShapeArrar[count] = subShape;
count ++;
dAssert (count < int (sizeof (collidingSubShapeArrar) / sizeof (collidingSubShapeArrar[0])));
// you can also get the forces here, however when tho function is call form a contact material
// we can only get resting forces, impulsive forces can not be read here since they has no being calculated yet.
// whoever if this function function is call after the NetwonUpdate they the application can read the contact force, that was applied to each contact point
dVector force;
dVector posit;
dVector normal;
NewtonMaterialGetContactForce (material, body, &force[0]);
NewtonMaterialGetContactPositionAndNormal (material, body, &posit[0], &normal[0]);
// the torque on this contact is
dVector torque ((origin - posit) * force);
// do what ever you want wit this
}
}
}
// here we should have an array of all colling sub shapes
if (count) {
// do what you need with this sub shape list
}
}
void GenericContactProcessCompatible(const void* const newtonContactJoint, float64 timestep, int threadIndex)
{
con_assert(newtonContactJoint != nullptr, "zero pointer")
NewtonBody* body0 = NewtonJointGetBody0(static_cast<const NewtonJoint*>(newtonContactJoint));
NewtonBody* body1 = NewtonJointGetBody1(static_cast<const NewtonJoint*>(newtonContactJoint));
con_assert(body0 != nullptr && body1 != nullptr, "zero pointers")
if (body0 != nullptr && body1 != nullptr)
{
iPhysicsBody* physicsBody0 = static_cast<iPhysicsBody*>(NewtonBodyGetUserData(static_cast<const NewtonBody*>(body0)));
iPhysicsBody* physicsBody1 = static_cast<iPhysicsBody*>(NewtonBodyGetUserData(static_cast<const NewtonBody*>(body1)));
con_assert(physicsBody0 != nullptr && physicsBody1 != nullptr, "zero pointers");
void* contact = NewtonContactJointGetFirstContact(static_cast<const NewtonJoint*>(newtonContactJoint));
NewtonMaterial* materialCombo = NewtonContactGetMaterial(contact);
iPhysicsMaterialCombo* physicsMaterialCombo = static_cast<iPhysicsMaterialCombo*>(NewtonMaterialGetMaterialPairUserData(materialCombo));
if (physicsMaterialCombo != nullptr && physicsBody0 != nullptr && physicsBody1 != nullptr)
{
physicsMaterialCombo->contact(physicsBody0, physicsBody1);
}
}
}
static void UserContactRestitution (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBody* body;
NewtonBody* body0;
NewtonBody* body1;
// call the basic call back
GenericContactProcess (contactJoint, timestep, threadIndex);
body0 = NewtonJointGetBody0(contactJoint);
body1 = NewtonJointGetBody1(contactJoint);
body = body0;
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
if (mass == 0.0f) {
body = body1;
}
NewtonCollision* const collision = NewtonBodyGetCollision(body);
void* userData = NewtonCollisionGetUserData (collision);
dFloat restitution = *((float*)&userData);
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonMaterialSetContactElasticity (material, restitution);
}
}
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;
}
//to get the collision points
void HandlecollisionPoints (NewtonJoint* const contactjoint)
{
NewtonBody* const body0 = NewtonJointGetBody0(contactjoint);
NewtonBody* const body1 = NewtonJointGetBody1(contactjoint);
for (void* contact = NewtonContactJointGetFirstContact (contactjoint); contact; contact = NewtonContactJointGetNextContact (contactjoint, contact)) {
NewtonMaterial* material = NewtonContactGetMaterial (contact);
// do whatever you want here
//dFloat forceMag;
dVector point;
dVector normal;
//NewtonMaterialGetContactForce (material, &forceMag);
NewtonMaterialGetContactPositionAndNormal (material, body0, &point.m_x, &normal.m_x);
NewtonMaterialGetContactPositionAndNormal (material, body1, &point.m_x, &normal.m_x);
// do whatever you want with the force
}
}
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 GenericContactProcess (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
dFloat contactBestSpeed;
dVector contactPosit;
SoundEffect* bestSound;
bestSound = NULL;
contactBestSpeed = 0.5f;
NewtonBody* const body0 = NewtonJointGetBody0(contactJoint);
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
dFloat contactNormalSpeed;
NewtonMaterial* material;
// get the material for this contact;
material = NewtonContactGetMaterial (contact);
contactNormalSpeed = NewtonMaterialGetContactNormalSpeed (material);
if (contactNormalSpeed > contactBestSpeed){
dVector normal;
contactBestSpeed = contactNormalSpeed;
NewtonMaterialGetContactPositionAndNormal (material, body0, &contactPosit[0], &normal[0]);
bestSound = (SoundEffect *)NewtonMaterialGetMaterialPairUserData (material);
}
}
// now that we found we can play then
if (bestSound) {
// calculate the volume;
dFloat volume;
dFloat dist2;
dVector eyePoint (GetCameraEyePoint() - contactPosit);
dist2 = eyePoint % eyePoint;
if (dist2 < (MAX_SOUND_DISTANCE * MAX_SOUND_DISTANCE)) {
volume = 1.0f;
if (dist2 > (MIN_SOUND_DISTANCE * MIN_SOUND_DISTANCE)) {
volume = 1.0f - (dSqrt (dist2) - MIN_SOUND_DISTANCE) / (MAX_SOUND_DISTANCE - MIN_SOUND_DISTANCE);
}
bestSound->m_manager->Play(bestSound->m_sound, volume, 0);
}
}
}
void PhysicsWorld::onContactCallback(const NewtonJoint* contactJoint, F32 timestep, int threadIndex)
{
const NewtonBody* body0 = NewtonJointGetBody0(contactJoint);
const NewtonBody* body1 = NewtonJointGetBody1(contactJoint);
F32 friction0 = 0.01;
F32 elasticity0 = 0.001;
F32 friction1 = friction0;
F32 elasticity1 = elasticity0;
void* userData = NewtonBodyGetUserData(body0);
if(userData)
{
friction0 = static_cast<PhysicsBody*>(userData)->getFriction();
elasticity0 = static_cast<PhysicsBody*>(userData)->getElasticity();
}
userData = NewtonBodyGetUserData(body1);
if(userData)
{
friction1 = static_cast<PhysicsBody*>(userData)->getFriction();
elasticity1 = static_cast<PhysicsBody*>(userData)->getElasticity();
}
F32 friction = friction0 + friction1;
F32 elasticity = elasticity0 + elasticity1;
void* contact = NewtonContactJointGetFirstContact(contactJoint);
while(contact)
{
NewtonMaterial* material = NewtonContactGetMaterial(contact);
NewtonMaterialSetContactFrictionCoef(material, friction + 0.1, friction, 0);
NewtonMaterialSetContactFrictionCoef(material, friction + 0.1, friction, 1);
NewtonMaterialSetContactElasticity(material, elasticity);
contact = NewtonContactJointGetNextContact(contactJoint, contact);
}
}
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 applyCarCollisionForce(const NewtonJoint* contact, dFloat timestep, int threadIndex) {
void * userData1 = NewtonBodyGetUserData(NewtonJointGetBody0(contact));
void * userData2 = NewtonBodyGetUserData(NewtonJointGetBody1(contact));
if(userData1 == 0 || userData2 == 0) {
return;
}
Car * car1 = (Car *)userData1;
Car * car2 = (Car *)userData2;
NewtonJoint * carContact = (NewtonJoint *) NewtonContactJointGetFirstContact(contact);
do {
dFloat collisionSpeed = NewtonMaterialGetContactNormalSpeed(
NewtonContactGetMaterial(carContact)
);
collisionSpeed = dAbs(collisionSpeed);
if(collisionSpeed > 3) {
car1->applyDamagePoints(collisionSpeed);
car2->applyDamagePoints(collisionSpeed);
break;
}
carContact = (NewtonJoint *) NewtonContactJointGetNextContact(contact, carContact);
} while(carContact);
}
void PhysWorld3D::ProcessContact(const NewtonJoint* const contactJoint, float timestep, int threadIndex)
{
RigidBody3D* bodyA = static_cast<RigidBody3D*>(NewtonBodyGetUserData(NewtonJointGetBody0(contactJoint)));
RigidBody3D* bodyB = static_cast<RigidBody3D*>(NewtonBodyGetUserData(NewtonJointGetBody1(contactJoint)));
assert(bodyA && bodyB);
using ContactJoint = void*;
// Query all joints first, to prevent removing a joint from the list while iterating on it
StackVector<ContactJoint> contacts = NazaraStackVector(ContactJoint, NewtonContactJointGetContactCount(contactJoint));
for (ContactJoint contact = NewtonContactJointGetFirstContact(contactJoint); contact; contact = NewtonContactJointGetNextContact(contactJoint, contact))
contacts.push_back(contact);
for (ContactJoint contact : contacts)
{
NewtonMaterial* material = NewtonContactGetMaterial(contact);
Callback* callbackData = static_cast<Callback*>(NewtonMaterialGetMaterialPairUserData(material));
assert(callbackData);
assert(callbackData->collisionCallback);
if (!callbackData->collisionCallback(*bodyA, *bodyB))
NewtonContactJointRemoveContact(contactJoint, contact);
}
}
static void UserContactRestitution (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
// call the basic call back
GenericContactProcess (contactJoint, timestep, threadIndex);
const NewtonBody* const body0 = NewtonJointGetBody0(contactJoint);
const NewtonBody* const body1 = NewtonJointGetBody1(contactJoint);
//now core 3.14 can have per collision user data
const NewtonCollision* const collision0 = NewtonBodyGetCollision(body0);
const NewtonCollision* const collision1 = NewtonBodyGetCollision(body1);
NewtonCollisionMaterial material0;
NewtonCollisionMaterial material1;
NewtonCollisionGetMaterial(collision0, &material0);
NewtonCollisionGetMaterial(collision1, &material1);
dAssert((material0.m_userId == 1) || (material1.m_userId == 1));
dFloat restitution = dMax(material0.m_userParam[0], material1.m_userParam[0]);
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonMaterialSetContactElasticity (material, restitution);
}
}
void cPhysicsMaterialNewton::ProcessContactCallback(const NewtonJoint* apJoint,
float afTimeStep,
int alThreadIndex)
{
cPhysicsContactData ContactData;
int lContactNum = 0;
iPhysicsBody * pContactBody1 = (iPhysicsBody*) NewtonBodyGetUserData(NewtonJointGetBody0(apJoint));
iPhysicsBody * pContactBody2 = (iPhysicsBody*) NewtonBodyGetUserData(NewtonJointGetBody1(apJoint));
for (void* pContact = NewtonContactJointGetFirstContact(apJoint);
pContact;
pContact = NewtonContactJointGetNextContact(apJoint, pContact))
{
NewtonMaterial* pMaterial = NewtonContactGetMaterial(pContact);
//Log(" Process contact between body '%s' and '%s'.\n",pContactBody1->GetName().c_str(),
// pContactBody2->GetName().c_str());
//Normal speed
float fNormSpeed = NewtonMaterialGetContactNormalSpeed(pMaterial);
if (ContactData.mfMaxContactNormalSpeed < fNormSpeed)
ContactData.mfMaxContactNormalSpeed = fNormSpeed;
//Tangent speed
float fTanSpeed0 = NewtonMaterialGetContactTangentSpeed(pMaterial, 0);
float fTanSpeed1 = NewtonMaterialGetContactTangentSpeed(pMaterial, 1);
if(std::abs(ContactData.mfMaxContactTangentSpeed) < std::abs(fTanSpeed0))
ContactData.mfMaxContactTangentSpeed = fTanSpeed0;
if(std::abs(ContactData.mfMaxContactTangentSpeed) < std::abs(fTanSpeed1))
ContactData.mfMaxContactTangentSpeed = fTanSpeed1;
//Force
cVector3f vForce;
NewtonMaterialGetContactForce(pMaterial,vForce.v);
ContactData.mvForce += vForce;
//Position and normal
cVector3f vPos, vNormal;
NewtonMaterialGetContactPositionAndNormal(pMaterial,vPos.v, vNormal.v);
ContactData.mvContactNormal += vNormal;
ContactData.mvContactPosition += vPos;
//Log(" Norm: %f Tan0: %f Tan1: %f\n",fNormSpeed, fTanSpeed0, fTanSpeed1);
//Log("Force: %s step %f\n", vForce.ToString().c_str(), afTimestep);
if(pContactBody1->GetWorld()->GetSaveContactPoints())
{
cCollidePoint collidePoint;
collidePoint.mfDepth = 1;
NewtonMaterialGetContactPositionAndNormal (pMaterial, collidePoint.mvPoint.v,
collidePoint.mvNormal.v);
pContactBody1->GetWorld()->GetContactPoints()->push_back(collidePoint);
}
lContactNum++;
}
//Log("--- End contact between body '%s' and '%s'.\n",mpContactBody1->GetName().c_str(),
// mpContactBody2->GetName().c_str());
if(lContactNum <= 0) return;
iPhysicsMaterial *pMaterial1 = pContactBody1->GetMaterial();
iPhysicsMaterial *pMaterial2 = pContactBody2->GetMaterial();
ContactData.mvContactNormal = ContactData.mvContactNormal / (float)lContactNum;
ContactData.mvContactPosition = ContactData.mvContactPosition / (float)lContactNum;
pMaterial1->GetSurfaceData()->CreateImpactEffect(ContactData.mfMaxContactNormalSpeed,
ContactData.mvContactPosition,
lContactNum,pMaterial2->GetSurfaceData());
int lPrio1 = pMaterial1->GetSurfaceData()->GetPriority();
int lPrio2 = pMaterial2->GetSurfaceData()->GetPriority();
if(lPrio1 >= lPrio2)
{
if(std::abs(ContactData.mfMaxContactNormalSpeed) > 0)
pMaterial1->GetSurfaceData()->OnImpact(ContactData.mfMaxContactNormalSpeed,
ContactData.mvContactPosition,
lContactNum,pContactBody1);
if(std::abs(ContactData.mfMaxContactTangentSpeed) > 0)
pMaterial1->GetSurfaceData()->OnSlide(ContactData.mfMaxContactTangentSpeed,
ContactData.mvContactPosition,
lContactNum,pContactBody1,pContactBody2);
}
if(lPrio2 >= lPrio1 && pMaterial2 != pMaterial1)
{
if(std::abs(ContactData.mfMaxContactNormalSpeed) > 0)
pMaterial2->GetSurfaceData()->OnImpact(ContactData.mfMaxContactNormalSpeed,
ContactData.mvContactPosition,
lContactNum,pContactBody2);
if(std::abs(ContactData.mfMaxContactTangentSpeed) > 0)
pMaterial2->GetSurfaceData()->OnSlide(ContactData.mfMaxContactTangentSpeed,
ContactData.mvContactPosition,
lContactNum,pContactBody2,pContactBody1);
}
pContactBody1->OnCollide(pContactBody2,&ContactData);
pContactBody2->OnCollide(pContactBody1,&ContactData);
}
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);
}
}
void GenericContactProcess (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
#if 0
dFloat speed0;
dFloat speed1;
SpecialEffectStruct* currectEffect;
// get the pointer to the special effect structure
currectEffect = (SpecialEffectStruct *)NewtonMaterialGetMaterialPairUserData (material);
// save the contact information
NewtonMaterialGetContactPositionAndNormal (material, &currectEffect->m_position.m_x, &currectEffect->m_normal.m_x);
NewtonMaterialGetContactTangentDirections (material, &currectEffect->m_tangentDir0.m_x, &currectEffect->m_tangentDir1.m_x);
// Get the maximum normal speed of this impact. this can be used for positioning collision sound
speed0 = NewtonMaterialGetContactNormalSpeed (material);
if (speed0 > currectEffect->m_contactMaxNormalSpeed) {
// save the position of the contact (for 3d sound of particles effects)
currectEffect->m_contactMaxNormalSpeed = speed0;
}
// get the maximum of the two sliding contact speed
speed0 = NewtonMaterialGetContactTangentSpeed (material, 0);
speed1 = NewtonMaterialGetContactTangentSpeed (material, 1);
if (speed1 > speed0) {
speed0 = speed1;
}
// Get the maximum tangent speed of this contact. this can be used for particles(sparks) of playing scratch sounds
if (speed0 > currectEffect->m_contactMaxTangentSpeed) {
// save the position of the contact (for 3d sound of particles effects)
currectEffect->m_contactMaxTangentSpeed = speed0;
}
#endif
// read the table direction
// dVector dir (tableDir);
// dVector updir (TableDir);
// NewtonBody* const body = NewtonJointGetBody0(contactJoint);
// for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
// dFloat speed;
// dVector point;
// dVector normal;
// dVector dir0;
// dVector dir1;
// dVector force;
// NewtonMaterial* material;
//
// material = NewtonContactGetMaterial (contact);
// NewtonMaterialGetContactPositionAndNormal (material, body, &point.m_x, &normal.m_x);
//
// // if the normal is vertical is large the say 40 degrees
// if (fabsf (normal % upDir) > 0.7f) {
// // rotate the normal to be aligned with the table direction
// NewtonMaterialContactRotateTangentDirections (material, dir);
// }
// }
NewtonBody* const body = NewtonJointGetBody0(contactJoint);
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
dVector point;
dVector normal;
dVector dir0;
dVector dir1;
dVector force;
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonMaterialGetContactForce (material, body, &force.m_x);
NewtonMaterialGetContactPositionAndNormal (material, body, &point.m_x, &normal.m_x);
NewtonMaterialGetContactTangentDirections (material, body, &dir0.m_x, &dir1.m_x);
//dFloat speed = NewtonMaterialGetContactNormalSpeed(material);
//speed = NewtonMaterialGetContactNormalSpeed(material);
// play sound base of the contact speed.
//
}
}
static void DestroyThisBodyCallback (const NewtonBody* body, const NewtonJoint* contactJoint)
{
NewtonWorld* world;
NewtonMesh* topMesh;
NewtonMesh* bottomMesh;
NewtonMesh* effectMesh;
RenderPrimitive* srcPrimitive;
dMatrix matrix;
dFloat maxForce;
dVector point;
dVector dir0;
dVector dir1;
// Get the world;
world = NewtonBodyGetWorld (body);
// find a the strongest force
maxForce = 0.0f;
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
dVector force;
NewtonMaterial* material;
material = NewtonContactGetMaterial (contact);
NewtonMaterialGetContactForce (material, &force.m_x);
if (force.m_x > maxForce) {
dVector normal;
NewtonMaterialGetContactPositionAndNormal(material, &point[0], &normal[0]);
NewtonMaterialGetContactTangentDirections (material, &dir0[0], &dir0[0]);
}
}
// get the visual primitive
srcPrimitive = (RenderPrimitive*) NewtonBodyGetUserData (body);
// get the effect mesh that is use to create the debris pieces
effectMesh = srcPrimitive->m_specialEffect;
// calculate the cut plane plane
NewtonBodyGetMatrix (body, &matrix[0][0]);
dMatrix clipMatrix (dgGrammSchmidt(dir0) *
dYawMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)) *
dRollMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)));
clipMatrix.m_posit = point;
clipMatrix.m_posit.m_w = 1.0f;
clipMatrix = clipMatrix * matrix.Inverse();
// break the mesh into two pieces
NewtonMeshClip (effectMesh, meshClipper, &clipMatrix[0][0], &topMesh, &bottomMesh);
if (topMesh && bottomMesh) {
dFloat volume;
NewtonMesh* meshPartA = NULL;
NewtonMesh* meshPartB = NULL;
volume = NewtonConvexCollisionCalculateVolume (NewtonBodyGetCollision(body));
// the clipper was able to make a cut now we can create new debris piece for replacement
dMatrix clipMatrix1 (dgGrammSchmidt(dir1) *
dYawMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)) *
dRollMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)));
NewtonMeshClip (bottomMesh, meshClipper, &clipMatrix1[0][0], &meshPartA, &meshPartB);
if (meshPartA && meshPartB) {
// creat another split (you can make as many depend of the FPS)
CreateDebriPiece (body, meshPartA, volume);
CreateDebriPiece (body, meshPartB, volume);
NewtonMeshDestroy(meshPartA);
NewtonMeshDestroy(meshPartB);
} else {
CreateDebriPiece (body, bottomMesh, volume);
}
NewtonMeshDestroy(bottomMesh);
dMatrix clipMatrix2 (dgGrammSchmidt(dir1) *
dYawMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)) *
dRollMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)));
NewtonMeshClip (topMesh, meshClipper, &clipMatrix2[0][0], &meshPartA, &meshPartB);
if (meshPartA && meshPartB) {
// creat another split (you can make as many depend of the FPS)
CreateDebriPiece (body, meshPartA, volume);
CreateDebriPiece (body, meshPartB, volume);
NewtonMeshDestroy(meshPartA);
NewtonMeshDestroy(meshPartB);
} else {
CreateDebriPiece (body, topMesh, volume);
}
NewtonMeshDestroy(topMesh);
// remove the old visual from graphics world
SceneManager* system = (SceneManager*) NewtonWorldGetUserData(world);
delete srcPrimitive;
system->Remove(srcPrimitive);
// finally destroy this body;
NewtonDestroyBody(world, body);
}
}
void* dNewtonContactMaterial::GetFirstContact() const
{
return NewtonContactJointGetFirstContact ((NewtonJoint*)m_materialHandle);
}
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 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);
}
};