// add force and torque to rigid body
void PhysicsApplyGravityForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
//mass*= 0.0f;
dVector force (0.0f, mass * DEMO_GRAVITY, -0.0f, 0.0f);
NewtonBodySetForce (body, &force.m_x);
/*
// check that angular momentum is conserved
dMatrix xxx;
dVector omega;
NewtonBodyGetInertiaMatrix(body, &xxx[0][0]);
NewtonBodyGetOmega(body, &omega[0]);
//dVector l (xxx.RotateVector(omega));
NewtonBodyGetMatrix(body, &xxx[0][0]);
dVector l (xxx.RotateVector(omega));
dTrace (("%f %f %f\n", omega[0], omega[1], omega[2]));
*/
}
/*
=============
PhysicsEntityThink
=============
*/
static void PhysicsEntityThink ( const NewtonBody* body, dFloat timestep, int threadIndex ) {
dFloat Ixx, Iyy, Izz, mass;
dMatrix matr;
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
dVector force (0.0f, 0.0f, (mass * /*-313.92f*/ -9.81f));
NewtonBodySetForce (body, &force.m_x);
}
void dVehicleSingleBody::StatesToRigidBody(dFloat timestep)
{
dComplementaritySolver::dBodyState* const chassisBody = GetBody();
dVector force(chassisBody->GetForce());
dVector torque(chassisBody->GetTorque());
NewtonBodySetForce(m_newtonBody, &force[0]);
NewtonBodySetTorque(m_newtonBody, &torque[0]);
dVehicleInterface::StatesToRigidBody(timestep);
}
// set the tranformation of a rigid body
void PhysicsApplyForceAndTorque (const NewtonBody* body)
{
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
dVector force (0.0f, -mass * 9.8f, 0.0f);
NewtonBodySetForce (body, &force.m_x);
}
void Car::physicsApplyForceAndTorque(const NewtonBody* body)
{
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
NewtonBodyGetMassMatrix(body, &mass, &Ixx, &Iyy, &Izz);
float force[3]={0.0f, -mass * 9.87f, 0.0f};
NewtonBodySetForce(body, force);
}
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]);
}
void Body::__applyForceAndTorqueCallback(const NewtonBody* body, dFloat timestep, int threadIndex)
{
//std::cout << "\tforce " << threadIndex << " " << body << std::endl;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMassMatrix(body, &mass, &Ixx, &Iyy, &Izz);
Vec4f gravityForce(0.0f, mass * newton::gravity, 0.0f, 1.0f);
NewtonBodySetForce(body, &gravityForce[0]);
//std::cout << "\tforce end " << threadIndex << " " << body << std::endl;
}
// callback to apply external forces to body
void ApplyForceAndTorqueCallback (const NewtonBody* body, dFloat timestep, int threadIndex)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
// for this tutorial the only external force in the Gravity
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
dVector gravityForce (0.0f, mass * GRAVITY, 0.0f, 1.0f);
NewtonBodySetForce(body, &gravityForce[0]);
}
void BoxGravityCallback (const NewtonBody* body, float timestep, int threadIndex)
{
float mass, ix, iy, iz;
NewtonBodyGetMassMatrix(body, &mass, &ix, &iy, &iz);
Vector4 gravityForce = Vector4(0.0f, mass * GRAVITY, 0.0f, 1.0f);
NewtonBodySetForce(body, (float*)&gravityForce);
/*CMesh *object = (CMesh*)NewtonBodyGetUserData(body);
int sleep = NewtonBodyGetSleepState(body);
Vector3 speed;
NewtonBodyGetVelocity(body, &speed[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 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);
}
void LaunchPuck()
{
if (!m_launched) {
m_launched = true;
NewtonInvalidateCache (NewtonBodyGetWorld(m_puckBody));
dVector zeros(0.0f, 0.0f, 0.0f, 0.0f);
NewtonBodySetVelocity(m_puckBody, &zeros.m_x);
NewtonBodySetOmega(m_puckBody, &zeros.m_x);
NewtonBodySetForce(m_puckBody, &zeros.m_x);
NewtonBodySetTorque(m_puckBody, &zeros.m_x);
dVector vel(171.299469f, 0.0f, 0.0f);
NewtonBodySetVelocity(m_puckBody, &vel.m_x);
}
}
void CEffectsGame::ApplyForceAndTorqueCallback (const NewtonBody* body, float timestep, int threadIndex)
{
float mass, ix, iy, iz;
NewtonBodyGetMassMatrix(pInstance->pBoxBody, &mass, &ix, &iy, &iz);
Vector4 gravityForce = Vector4(0.0f, mass * GRAVITY, 0.0f, 1.0f);
if (gInput.WasKeyPressed(K_MOUSE1))
{
Vector4 rnd = Vector4(frand(-1,1), 1, frand(-1,1), 0);
rnd *= 100.0f;
gravityForce += rnd;
}
NewtonBodySetForce(pInstance->pBoxBody, (float*)&gravityForce);
}
static void MagneticFieldNoForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
dMatrix matrix;
dVector zero (0.0f, 0.0f, 0.0f, 0.0f);
NewtonBodySetForce (body, &zero[0]);
NewtonBodySetTorque (body, &zero[0]);
NewtonBodySetOmega (body, &zero[0]);
NewtonBodySetVelocity (body, &zero[0]);
// telepor the magnetic field trigger to the center of the core
Magnet* magnet;
magnet = (Magnet*)NewtonBodyGetUserData(body);
NewtonBodyGetMatrix (magnet->m_magneticCore, &matrix[0][0]);
dMatrix posit (GetIdentityMatrix());
posit.m_posit = matrix.m_posit;
NewtonBodySetMatrix (body, &posit[0][0]);
}
void NzPhysObject::ForceAndTorqueCallback(const NewtonBody* body, float timeStep, int threadIndex)
{
NazaraUnused(timeStep);
NazaraUnused(threadIndex);
NzPhysObject* me = static_cast<NzPhysObject*>(NewtonBodyGetUserData(body));
if (!NzNumberEquals(me->m_gravityFactor, 0.f))
me->m_forceAccumulator += me->m_world->GetGravity() * me->m_gravityFactor * me->m_mass;
/*for (std::set<PhysObjectListener*>::iterator it = me->m_listeners.begin(); it != me->m_listeners.end(); ++it)
(*it)->PhysObjectApplyForce(me);*/
NewtonBodySetForce(body, me->m_forceAccumulator);
NewtonBodySetTorque(body, me->m_torqueAccumulator);
me->m_torqueAccumulator.Set(0.f);
me->m_forceAccumulator.Set(0.f);
///TODO: Implanter la force gyroscopique?
}
// 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);
}
// 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]));
*/
}
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);
}
// callback to apply external forces to body
void ApplyForceAndTorqueCallback (const NewtonBody* body, float timestep, int threadIndex)
{
float Ixx;
float Iyy;
float Izz;
float mass;
// for this tutorial the only external force in the Gravity
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
float y=0.0;
float x=0.0;
if(glfwGetKey(GLFW_KEY_UP))
y=1.0;
if(glfwGetKey(GLFW_KEY_DOWN))
y=-1.0;
if(glfwGetKey(GLFW_KEY_LEFT))
x=-1.0;
if(glfwGetKey(GLFW_KEY_RIGHT))
x=1.0;
glm::vec4 gravityForce (300.0f*x*mass, 300.0f*y*mass, mass * -100.0f, 1.0f);
NewtonBodySetForce(body, &gravityForce[0]);
}
void iPhysics::setForce(void* newtonBody, const iaVector3f& force)
{
NewtonBodySetForce(static_cast<const NewtonBody*>(newtonBody), force.getData());
}
void CarBoxCallback(const NewtonBody* body, float timestep, int threadIndex)
{
float mass, ix, iy, iz;
NewtonBodyGetMassMatrix(body, &mass, &ix, &iy, &iz);
Vector4 gravityForce = Vector4(0.0f, mass * GRAVITY, 0.0f, 1.0f);
// set gravity as a force
//NewtonBodySetForce(body, (float*)&gravityForce);
CMesh *object = (CMesh*)NewtonBodyGetUserData(body);
Vector3 force;
if (Keydown('w')) force = object->forward;
if (Keydown('s')) force = -object->forward;
//if (Keydown('a')) force -= object->right;
//if (Keydown('d')) force += object->right;
force *= cv_speed.GetFloat(); // speed
//force.y = GRAVITY;
force *= mass;
/*float f[3];
NewtonBodyGetForce(body, &f[0]);
Debug("BODY: %f %f %f", f[0], f[1], f[2]);
Debug("FORCE: %f %f %f", force.x, force.y, force.z);
*/
NewtonBodySetForce(body, &force[0]);
//NewtonBodyAddImpulse(body, &force[0], &object->GetWorldPosition()[0]);
//NewtonBodySetVelocity(body, &force[0]);
float torqueForce = cv_rspeed.GetFloat();
Vector3 torque;
if (Keydown('a')) torque.y -= torqueForce;
if (Keydown('d')) torque.y += torqueForce;
// Damping has no effect at all
/* static float damp = 0.5f;
if (KeyPressed('p')) damp += 0.1f;
if (KeyPressed('o')) damp -= 0.1f;
Vector3 damping = Vector3(damp,damp,damp);
damping *= damp;
NewtonBodySetAngularDamping(body, &damping[0]);
*/
NewtonBodySetOmega(body, &torque[0]);
//torque *= mass;
//NewtonBodyAddTorque(body, &torque[0]);
int sleep = NewtonBodyGetSleepState(body);
object->color = sleep == 1 ? GREEN : RED;
//Vector3 speed;
//NewtonBodyGetVelocity(body, &speed[0]);
}
void dNewtonDynamicBody::OnForceAndTorque(dFloat timestep, int threadIndex)
{
NewtonBodySetForce(m_body, &m_externalForce[0]);
NewtonBodySetTorque(m_body, &m_externalTorque[0]);
}
