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);
}
}
void CalculatePickForceAndTorque (const NewtonBody* const body, const dVector& pointOnBodyInGlobalSpace, const dVector& targetPositionInGlobalSpace, dFloat timestep)
{
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
const dFloat stiffness = 0.33f;
const dFloat damping = -0.05f;
NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
// calculate the desired impulse
dVector posit(targetPositionInGlobalSpace - pointOnBodyInGlobalSpace);
dVector impulse(posit.Scale(stiffness * mass));
// apply linear impulse
NewtonBodyApplyImpulseArray(body, 1, sizeof (dVector), &impulse[0], &pointOnBodyInGlobalSpace[0], timestep);
// apply linear and angular damping
dMatrix inertia;
dVector linearMomentum(0.0f);
dVector angularMomentum(0.0f);
NewtonBodyGetOmega(body, &angularMomentum[0]);
NewtonBodyGetVelocity(body, &linearMomentum[0]);
NewtonBodyGetInertiaMatrix(body, &inertia[0][0]);
angularMomentum = inertia.RotateVector(angularMomentum);
angularMomentum = angularMomentum.Scale(damping);
linearMomentum = linearMomentum.Scale(mass * damping);
NewtonBodyApplyImpulsePair(body, &linearMomentum[0], &angularMomentum[0], timestep);
}
static dFloat RayCastFilter (const NewtonBody* const body, const NewtonCollision* const collisionHit, const dFloat* const contact, const dFloat* const normal, dLong collisionID, void* const userData, dFloat intersetParam)
{
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
// check if we are hitting a sub shape
const NewtonCollision* const parent = NewtonCollisionGetParentInstance(collisionHit);
if (parent) {
// you can use this to filter sub collision shapes.
dAssert (NewtonCollisionGetSubCollisionHandle (collisionHit));
}
dMousePickClass* const data = (dMousePickClass*) userData;
NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
if ((mass > 0.0f) || (NewtonBodyGetType(body) == NEWTON_KINEMATIC_BODY)) {
data->m_body = body;
}
if (intersetParam < data->m_param) {
data->m_param = intersetParam;
data->m_normal = dVector (normal[0], normal[1], normal[2]);
}
return intersetParam;
}
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);
}
}
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;
NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
if (mass == 0.0f) {
body = body1;
}
NewtonCollision* const collision = NewtonBodyGetCollision(body);
void* userData = NewtonCollisionGetUserData (collision);
dFloat restitution = *((dFloat*)&userData);
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonMaterialSetContactElasticity (material, restitution);
}
}
void ScaleIntertia(NewtonBody* const body, dFloat factor) const
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
NewtonBodySetMassMatrix(body, mass, Ixx * factor, Iyy * factor, Izz * factor);
}
void CustomKinematicController::SetMaxLinearFriction(dFloat accel)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass (m_body0, &mass, &Ixx, &Iyy, &Izz);
m_maxLinearFriction = dAbs (accel) * mass;
}
static void ApplyGravity(const NewtonBody* const body, dFloat timestep, int threadIndex)
{
// apply gravity force to the body
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
dVector gravityForce(0.0f, -9.8f * mass, 0.0f, 0.0f);
NewtonBodySetForce(body, &gravityForce[0]);
}
// add force and torque to rigid body
void PhysicsApplyGravityForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
//mass*= 0.0f;
dVector force (dVector (0.0f, 1.0f, 0.0f).Scale (mass * DEMO_GRAVITY));
NewtonBodySetForce (body, &force.m_x);
}
void dNewtonDynamicBody::InitForceAccumulators()
{
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
NewtonBodyGetMass(m_body, &mass, &Ixx, &Iyy, &Izz);
const dNewtonWorld* const world = (dNewtonWorld*)NewtonWorldGetUserData(NewtonBodyGetWorld(m_body));
m_externalForce = world->GetGravity().Scale(mass);
m_externalTorque = dVector(0.0f);
}
void Friction (DemoEntityManager* const scene)
{
// load the skybox
scene->CreateSkyBox();
// load the scene from a ngd file format
char fileName[2048];
dGetWorkingFileName ("frictionDemo.ngd", fileName);
scene->LoadScene (fileName);
// set a default material call back
NewtonWorld* const world = scene->GetNewton();
int defaultMaterialID = NewtonMaterialGetDefaultGroupID (world);
NewtonMaterialSetCollisionCallback (world, defaultMaterialID, defaultMaterialID, UserOnAABBOverlap, UserContactFriction);
//NewtonMaterialSetDefaultCollidable(world, defaultMaterialID, defaultMaterialID, 0);
// customize the scene after loading
// set a user friction variable in the body for variable friction demos
// later this will be done using LUA script
int index = 0;
for (NewtonBody* body = NewtonWorldGetFirstBody(world); body; body = NewtonWorldGetNextBody(world, body)) {
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
if (mass > 0.0f) {
// use the new instance feature to ass per shape information
NewtonCollision* const collision = NewtonBodyGetCollision(body);
// use the collision user data to save the coefficient of friction
dFloat coefficientOfFriction = dFloat (index) * 0.03f;
NewtonCollisionSetUserData (collision, *((void**)&coefficientOfFriction));
// DemoEntity* const entity = (DemoEntity*) NewtonBodyGetUserData (body);
// dVariable* const friction = entity->CreateVariable (FRICTION_VAR_NAME);
// dAssert (friction);
// friction->SetValue(dFloat (index) * 0.03f);
index ++;
}
}
// place camera into position
dQuaternion rot;
dVector origin (-70.0f, 10.0f, 0.0f, 0.0f);
scene->SetCameraMatrix(rot, origin);
// ExportScene (scene->GetNewton(), "../../../media/test1.ngd");
}
void NewtonRigidBodySetForceCB(const NewtonBody* const body, dFloat timestep, int threadIndex)
{
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
dFloat force[3];
force[0] = 0.0f;
force[1] = mass * (DEMO_GRAVITY * PHYSICS_WORLD_SCALE);
force[2] = 0.0f;
NewtonBodySetForce(body, force);
}
static void RenderBodyContactsForces (NewtonBody* const body, dFloat scale)
{
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
NewtonBodyGetMass (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.DotProduct3(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.DotProduct3(tangnetDir0)) * scale));
dVector tangentForce2 (tangnetDir1.Scale ((contactForce.DotProduct3(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 dNewtonBody::SetCenterOfMass(float com_x, float com_y, float com_z)
{
dVector com;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass(m_body, &mass, &Ixx, &Iyy, &Izz);
NewtonCollision* const collision = NewtonBodyGetCollision(m_body);
NewtonBodySetMassProperties(m_body, mass, NewtonBodyGetCollision(m_body));
NewtonBodyGetCentreOfMass (m_body, &com[0]);
com.m_x += com_x;
com.m_y += com_y;
com.m_z += com_z;
NewtonBodySetCentreOfMass(m_body, &com[0]);
}
void CustomKinematicController::SetMaxAngularFriction(dFloat alpha)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass (m_body0, &mass, &Ixx, &Iyy, &Izz);
// if (Iyy > Ixx) {
// Ixx = Iyy;
// }
// if (Izz > Ixx) {
// Ixx = Izz;
// }
// m_maxAngularFriction = dAbs (alpha) * Ixx;
m_maxAngularFriction = dAbs (alpha) * mass;
}
void dNewtonBody::CalculateBuoyancyForces(const void* plane, void* force, void* torque, float bodyDensity)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass(m_body, &mass, &Ixx, &Iyy, &Izz);
if (mass > 0.0f) {
dMatrix matrix;
dVector cog(0.0f);
dVector accelPerUnitMass(0.0f);
dVector torquePerUnitMass(0.0f);
const dVector gravity(0.0f, -9.8f, 0.0f, 0.0f);
NewtonBodyGetMatrix(m_body, &matrix[0][0]);
NewtonBodyGetCentreOfMass(m_body, &cog[0]);
cog = matrix.TransformVector(cog);
NewtonCollision* const collision = NewtonBodyGetCollision(m_body);
dFloat shapeVolume = NewtonConvexCollisionCalculateVolume(collision);
dFloat fluidDensity = 1.0f / (bodyDensity * shapeVolume);
dFloat viscosity = 0.995f;
NewtonConvexCollisionCalculateBuoyancyAcceleration(collision, &matrix[0][0], &cog[0], &gravity[0], (float*)plane, fluidDensity, viscosity, &accelPerUnitMass[0], &torquePerUnitMass[0]);
dVector finalForce(accelPerUnitMass.Scale(mass));
dVector finalTorque(torquePerUnitMass.Scale(mass));
dVector omega(0.0f);
NewtonBodyGetOmega(m_body, &omega[0]);
omega = omega.Scale(viscosity);
NewtonBodySetOmega(m_body, &omega[0]);
((float*)force)[0] = finalForce.m_x ;
((float*)force)[1] = finalForce.m_y ;
((float*)force)[2] = finalForce.m_z ;
((float*)torque)[0] = finalTorque.m_x;
((float*)torque)[1] = finalTorque.m_y;
((float*)torque)[2] = finalTorque.m_z;
}
}
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));
}
// add force and torque to rigid body
void PhysicsApplyGravityForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
dVector dir(0.0f, 1.0f, 0.0f);
// dVector dir(1.0f, 0.0f, 0.0f);
//mass = 0.0f;
dVector force (dir.Scale (mass * DEMO_GRAVITY));
NewtonBodySetForce (body, &force.m_x);
#ifdef DEMO_CHECK_ASYN_UPDATE
dAssert(g_checkAsyncUpdate);
#endif
// test going to sleep bug
// NewtonBodySetSleepState(body, 0);
}
dVehicleSingleBody::dVehicleSingleBody(dVehicleChassis* const chassis)
:dVehicleInterface(chassis)
,m_gravity(0.0f)
,m_groundNode(NULL)
,m_newtonBody(chassis->GetBody())
{
dVector tmp;
dComplementaritySolver::dBodyState* const chassisBody = GetBody();
m_groundNode.SetWorld(m_world);
m_groundNode.SetLoopNode(true);
// set the inertia matrix;
NewtonBodyGetMass(m_newtonBody, &tmp.m_w, &tmp.m_x, &tmp.m_y, &tmp.m_z);
chassisBody->SetMass(tmp.m_w);
chassisBody->SetInertia(tmp.m_x, tmp.m_y, tmp.m_z);
dMatrix matrix (dGetIdentityMatrix());
NewtonBodyGetCentreOfMass(m_newtonBody, &matrix.m_posit[0]);
matrix.m_posit.m_w = 1.0f;
chassisBody->SetLocalMatrix(matrix);
}
void SetModelMass( dFloat mass, dAnimationJointRoot* const rootNode) const
{
dFloat volume = 0.0f;
for (dAnimationJoint* joint = GetFirstJoint(rootNode); joint; joint = GetNextJoint(joint)) {
volume += NewtonConvexCollisionCalculateVolume(NewtonBodyGetCollision(joint->GetBody()));
}
dFloat density = mass / volume;
for (dAnimationJoint* joint = GetFirstJoint(rootNode); joint; joint = GetNextJoint(joint)) {
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
NewtonBody* const body = joint->GetBody();
NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
dFloat scale = density * NewtonConvexCollisionCalculateVolume(NewtonBodyGetCollision(body));
mass *= scale;
Ixx *= scale;
Iyy *= scale;
Izz *= scale;
NewtonBodySetMassMatrix(body, mass, Ixx, Iyy, Izz);
}
}
// add force and torque to rigid body
void PhysicsApplyGravityForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
//mass*= 0.0f;
dVector force (dVector (0.0f, 1.0f, 0.0f).Scale (mass * DEMO_GRAVITY));
NewtonBodySetForce (body, &force.m_x);
/*
// check that angular momentum is conserved
dMatrix I;
dVector omega(0.0f);
NewtonBodyGetInertiaMatrix(body, &I[0][0]);
NewtonBodyGetOmega(body, &omega[0]);
dVector L (I.RotateVector(omega));
dTrace (("(%f %f %f) (%f %f %f)\n", omega[0], omega[1], omega[2], L[0], L[1], L[2]));
*/
}
void OnInside(NewtonBody* const visitor)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass(visitor, &mass, &Ixx, &Iyy, &Izz);
if (mass > 0.0f) {
dMatrix matrix;
dVector cog(0.0f);
dVector accelPerUnitMass(0.0f);
dVector torquePerUnitMass(0.0f);
const dVector gravity (0.0f, DEMO_GRAVITY, 0.0f, 0.0f);
NewtonBodyGetMatrix (visitor, &matrix[0][0]);
NewtonBodyGetCentreOfMass(visitor, &cog[0]);
cog = matrix.TransformVector (cog);
NewtonCollision* const collision = NewtonBodyGetCollision(visitor);
dFloat shapeVolume = NewtonConvexCollisionCalculateVolume (collision);
dFloat fluidDentity = 1.0f / (m_waterToSolidVolumeRatio * shapeVolume);
dFloat viscosity = 0.995f;
NewtonConvexCollisionCalculateBuoyancyAcceleration (collision, &matrix[0][0], &cog[0], &gravity[0], &m_plane[0], fluidDentity, viscosity, &accelPerUnitMass[0], &torquePerUnitMass[0]);
dVector force (accelPerUnitMass.Scale (mass));
dVector torque (torquePerUnitMass.Scale (mass));
dVector omega(0.0f);
NewtonBodyGetOmega(visitor, &omega[0]);
omega = omega.Scale (viscosity);
NewtonBodySetOmega(visitor, &omega[0]);
NewtonBodyAddForce (visitor, &force[0]);
NewtonBodyAddTorque (visitor, &torque[0]);
}
}
static void ClampAngularVelocity(const NewtonBody* body, dFloat timestep, int threadIndex)
{
dVector omega;
NewtonBodyGetOmega(body, &omega[0]);
omega.m_w = 0.0f;
dFloat mag2 = omega.DotProduct3(omega);
if (mag2 > (100.0f * 100.0f)) {
omega = omega.Normalize().Scale(100.0f);
NewtonBodySetOmega(body, &omega[0]);
}
//PhysicsApplyGravityForce(body, timestep, threadIndex);
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
dFloat gravity = -0.0f;
NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
dVector dir(0.0f, gravity, 0.0f);
dVector force(dir.Scale(mass));
NewtonBodySetForce(body, &force.m_x);
}
dCustomTireSpringDG::dCustomTireSpringDG(const dMatrix& pinAndPivotFrameChild, const dMatrix& pinAndPivotFrameParent, NewtonBody* const child, NewtonBody* const parent)
:dCustomJoint(6, child, parent),
mUseBreak(false),
mUseSteer(false),
mUseTorque(false),
mUseHardBreak(false),
mIxx(0.0f),
mIyy(0.0f),
mIzz(0.0f),
mRealOmega(0.0f),
mTireOmegaCorrection(0.975f),
mFpsRequest(120.0f),
mTireTorque(0.0f),
mBrakeTorque(0.0f),
mSteerAngle(0.0f),
mAttachmentLength(10.0f),
mDistance(0.0f),
mMinSuspenssion(-0.25f),
mMaxSuspenssion(0.0f),
mSpringK(150.0f),
mSpringD(5.0f),
mSpringMassEffective(0.5f),
mAccel(0.0f),
mAttachMass(0.0f),
mCenterInTire(dVector(0.0f, 0.0f, 0.0f)),
mCenterInChassis(dVector(0.0f, 0.0f, 0.0f)),
mChassisPivotMatrix(dGetIdentityMatrix()),
mTirePivotMatrix(dGetIdentityMatrix()),
mRefFrameLocalMatrix(dGetIdentityMatrix())
{
dMatrix dummy;
CalculateLocalMatrix(pinAndPivotFrameChild, m_localMatrix0, dummy);
CalculateLocalMatrix(pinAndPivotFrameParent, dummy, m_localMatrix1);
//
mRefFrameLocalMatrix = m_localMatrix0;
NewtonBodyGetMass(parent, &mAttachMass, &mIxx, &mIyy, &mIzz);
}
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 DemoCameraListener::UpdatePickBody(DemoEntityManager* const scene, dFloat timestep)
{
// handle pick body from the screen
bool mousePickState = scene->GetMouseKeyState(0);
if (!m_targetPicked) {
if (!m_prevMouseState && mousePickState) {
dFloat param;
dVector posit;
dVector normal;
dFloat x = dFloat (m_mousePosX);
dFloat y = dFloat (m_mousePosY);
dVector p0 (m_camera->ScreenToWorld(dVector (x, y, 0.0f, 0.0f)));
dVector p1 (m_camera->ScreenToWorld(dVector (x, y, 1.0f, 0.0f)));
NewtonBody* const body = MousePickBody (scene->GetNewton(), p0, p1, param, posit, normal);
if (body) {
dMatrix matrix;
m_targetPicked = body;
NewtonBodyGetMatrix(m_targetPicked, &matrix[0][0]);
m_pickedBodyParam = param;
#ifdef USE_PICK_BODY_BY_FORCE
// save point local to the body matrix
m_pickedBodyLocalAtachmentPoint = matrix.UntransformVector (posit);
// convert normal to local space
m_pickedBodyLocalAtachmentNormal = matrix.UnrotateVector(normal);
#else
if(m_pickJoint) {
delete m_pickJoint;
m_pickJoint = NULL;
}
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
const dFloat angularFritionAccel = 100.0f;
const dFloat linearFrictionAccel = 100.0f * dAbs (dMax (DEMO_GRAVITY, 10.0f));
const dFloat inertia = dMax (Izz, dMax (Ixx, Iyy));
m_pickJoint = new dCustomKinematicController (body, posit);
m_pickJoint->SetPickMode (0);
m_pickJoint->SetMaxLinearFriction(mass * linearFrictionAccel);
m_pickJoint->SetMaxAngularFriction(inertia * angularFritionAccel);
#endif
}
}
} else {
if (scene->GetMouseKeyState(0)) {
dFloat x = dFloat (m_mousePosX);
dFloat y = dFloat (m_mousePosY);
dVector p0 (m_camera->ScreenToWorld(dVector (x, y, 0.0f, 0.0f)));
dVector p1 (m_camera->ScreenToWorld(dVector (x, y, 1.0f, 0.0f)));
m_pickedBodyTargetPosition = p0 + (p1 - p0).Scale (m_pickedBodyParam);
#ifdef USE_PICK_BODY_BY_FORCE
dMatrix matrix;
NewtonBodyGetMatrix (m_targetPicked, &matrix[0][0]);
dVector point (matrix.TransformVector(m_pickedBodyLocalAtachmentPoint));
CalculatePickForceAndTorque (m_targetPicked, point, m_pickedBodyTargetPosition, timestep);
#else
if (m_pickJoint) {
m_pickJoint->SetTargetPosit (m_pickedBodyTargetPosition);
}
#endif
} else {
if (m_targetPicked) {
NewtonBodySetSleepState (m_targetPicked, 0);
}
if (m_pickJoint) {
delete m_pickJoint;
}
m_pickJoint = NULL;
m_targetPicked = NULL;
m_bodyDestructor = NULL;
}
}
m_prevMouseState = mousePickState;
}
void CalculatePickForceAndTorque (const NewtonBody* const body, const dVector& pointOnBodyInGlobalSpace, const dVector& targetPositionInGlobalSpace, dFloat timestep)
{
dMatrix matrix;
dVector com(0.0f);
dVector omega0(0.0f);
dVector veloc0(0.0f);
dVector omega1(0.0f);
dVector veloc1(0.0f);
dVector pointVeloc(0.0f);
const dFloat stiffness = 0.3f;
const dFloat angularDamp = 0.95f;
dFloat invTimeStep = 1.0f / timestep;
NewtonWorld* const world = NewtonBodyGetWorld (body);
NewtonWorldCriticalSectionLock (world, 0);
// calculate the desired impulse
NewtonBodyGetMatrix(body, &matrix[0][0]);
NewtonBodyGetOmega (body, &omega0[0]);
NewtonBodyGetVelocity (body, &veloc0[0]);
NewtonBodyGetPointVelocity (body, &pointOnBodyInGlobalSpace[0], &pointVeloc[0]);
dVector deltaVeloc (targetPositionInGlobalSpace - pointOnBodyInGlobalSpace);
deltaVeloc = deltaVeloc.Scale (stiffness * invTimeStep) - pointVeloc;
for (int i = 0; i < 3; i ++) {
dVector veloc (0.0f);
veloc[i] = deltaVeloc[i];
NewtonBodyAddImpulse (body, &veloc[0], &pointOnBodyInGlobalSpace[0]);
}
// damp angular velocity
NewtonBodyGetOmega (body, &omega1[0]);
NewtonBodyGetVelocity (body, &veloc1[0]);
omega1 = omega1.Scale (angularDamp);
// restore body velocity and angular velocity
NewtonBodySetOmega (body, &omega0[0]);
NewtonBodySetVelocity(body, &veloc0[0]);
// convert the delta velocity change to a external force and torque
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
dVector angularMomentum (Ixx, Iyy, Izz);
angularMomentum = matrix.RotateVector (angularMomentum.CompProduct(matrix.UnrotateVector(omega1 - omega0)));
dVector force ((veloc1 - veloc0).Scale (mass * invTimeStep));
dVector torque (angularMomentum.Scale(invTimeStep));
NewtonBodyAddForce(body, &force[0]);
NewtonBodyAddTorque(body, &torque[0]);
// make sure the body is unfrozen, if it is picked
NewtonBodySetSleepState (body, 0);
NewtonWorldCriticalSectionUnlock (world);
}
void iPhysics::getMassMatrix(void* newtonBody, float32& mass, float32& Ixx, float32& Iyy, float32& Izz)
{
NewtonBodyGetMass(static_cast<const NewtonBody*>(newtonBody), &mass, &Ixx, &Iyy, &Izz);
}
