//body - body case
float E_NLD(dBodyID b1,dBodyID b2,const dReal* norm)// norm - from 2 to 1
{
dMass m1,m2;
dBodyGetMass(b1,&m1);dBodyGetMass(b2,&m2);
const dReal* vel1 =dBodyGetLinearVel(b1);
const dReal* vel2 =dBodyGetLinearVel(b2);
dReal vel_pr1=dDOT(vel1,norm);
dReal vel_pr2=dDOT(vel2,norm);
if(vel_pr1>vel_pr2) return 0.f; //exit if the bodies are departing
dVector3 impuls1={vel1[0]*m1.mass,vel1[1]*m1.mass,vel1[2]*m1.mass};
dVector3 impuls2={vel2[0]*m2.mass,vel2[1]*m2.mass,vel2[2]*m2.mass};
dVector3 c_mas_impuls={impuls1[0]+impuls2[0],impuls1[1]+impuls2[1],impuls1[2]+impuls2[2]};
dReal cmass=m1.mass+m2.mass;
dVector3 c_mass_vel={c_mas_impuls[0]/cmass,c_mas_impuls[1]/cmass,c_mas_impuls[2]/cmass};
dReal c_mass_vel_prg=dDOT(c_mass_vel,norm);
dReal kin_energy_start=vel_pr1*vel_pr1*m1.mass/2.f+vel_pr2*vel_pr2*m2.mass/2.f;
dReal kin_energy_end=c_mass_vel_prg*c_mass_vel_prg*cmass/2.f;
return (kin_energy_start-kin_energy_end);
}
void PhysicsObject::attachObject(boost::shared_ptr<PhysicsObject> po,
const v3& position,
const qv4& orientation)
{
po->mBodyOffset = position;
GeomMapT::const_iterator it = po->mGeometry.begin();
for (; it != po->mGeometry.end(); ++it)
{
// Calculate new relative position
dGeomID geom = it->second.geomId;
const dReal* offset = dGeomGetOffsetPosition(geom);
v3 newPos(offset);
newPos += position;
// Attach
dGeomSetBody(geom, mOdeBody);
dGeomSetOffsetPosition(geom, newPos.x, newPos.y, newPos.z);
dSpaceRemove(po->mSpaceId, geom);
dSpaceAdd(mSpaceId, geom);
}
// add the two masses
dMass otherMass;
dBodyGetMass(po->mOdeBody, &otherMass);
// dbglog << "OtherMass: " << otherMass.mass;
// dbglog << "OtherCenter: " << odeVectorOut(otherMass.c);
// dbglog << "OtherInertia: " << odeMatrixOut(otherMass.I);
dBodyGetMass(mOdeBody, &mOriginalMass);
// dbglog << "OwnMass: " << mOriginalMass.mass;
// dbglog << "OwnCenter: " << odeVectorOut(mOriginalMass.c);
// dbglog << "OwnInertia: " << odeMatrixOut(mOriginalMass.I);
dMassAdd(&mOriginalMass, &otherMass);
dMassTranslate(&mOriginalMass, -mOriginalMass.c[0], -mOriginalMass.c[1], -mOriginalMass.c[2]);
dBodySetMass(mOdeBody, &mOriginalMass);
// dbglog << "NewMass: " << mOriginalMass.mass;
// dbglog << "NewCenter: " << odeVectorOut(mOriginalMass.c);
// dbglog << "NewInertia: " << odeMatrixOut(mOriginalMass.I);
// Disable old body
dBodyDisable(po->mOdeBody);
notifyControlAboutChangeInMass();
}
//limit for angular accel
void dBodyAngAccelFromTorqu(const dBodyID body, dReal* ang_accel, const dReal* torque){
dMass m;
dMatrix3 invI;
dBodyGetMass(body,&m);
dInvertPDMatrix (m.I, invI, 3);
dMULTIPLY1_333(ang_accel,invI, torque);
}
const dReal* ODE_Particle::getCOM()
{
dMass m;
dBodyGetMass (body,&m);
//com=m.c;
return m.c;
}
void TSRODERigidBody::AddCylinderGeometry( TSRPhysicsWorld* _pWorldInterface, const TSRMatrix4& _bodyToGeomTransform, float _fRadius,float _fLength, float _fDensity )
{
TSRODEPhysicsWorld* _pWorld = ( TSRODEPhysicsWorld* ) _pWorldInterface;
dMass totalMass;
dBodyGetMass( m_BodyID, &totalMass );
if ( m_GeomIDs.size() == 0 )
{
dMassSetZero( &totalMass );
}
dMatrix4 R;
dVector3 P;
Matrix4ToODE( _bodyToGeomTransform, R, P );
dGeomID geomTransform = dCreateGeomTransform( _pWorld->m_SpaceID );
dGeomID encapsulatedGeom = 0;
dMass currMass;
dMassSetZero( &currMass );
encapsulatedGeom = dCreateCylinder( 0, _fRadius, _fLength );
dMassSetCylinder( &currMass, _fDensity, 0, _fRadius, _fLength );
dMassRotate( &currMass, R );
//dMassTranslate(&currMass,P[0],P[1],P[2]);
dMassAdd( &totalMass, &currMass );
dGeomSetPosition( encapsulatedGeom, P[ 0 ], P[ 1 ], P[ 2 ] );
dGeomSetRotation( encapsulatedGeom, R );
dGeomTransformSetCleanup( geomTransform, 1 );
dGeomTransformSetGeom( geomTransform, encapsulatedGeom );
dGeomSetBody( geomTransform, m_BodyID );
m_GeomIDs.push_back( geomTransform );
dBodySetMass( m_BodyID, &totalMass );
}
/* returns the total kinetic energy of a body */
t_real body_total_kinetic_energy (dBodyID b) {
const t_real *v, *omega;
dVector3 wb;
t_real trans_energy, rot_energy, energy;
dMass m;
/* get the linear and angular velocities */
dBodyGetMass (b, &m);
v = dBodyGetLinearVel (b);
omega = dBodyGetAngularVel (b);
dBodyVectorFromWorld (b, omega [0], omega [1], omega [2], wb);
/* and assign the energy */
/* TODO: restore */
/* energy = 0.5 * m.mass * (v [0] * v [0] + v [1] * v [1] + v [2] * v [2]);
energy += 0.5 * (m.I [0] * wb[0] * wb[0] + m.I[5] * wb[1] * wb[1] + m.I[10] * wb[2] * wb[2]); */
trans_energy = 0.5 * m.mass * (v [0] * v [0] + v [1] * v [1] + v [2] * v [2]);
rot_energy = 0.5 * (m.I [0] * wb[0] * wb[0] + m.I[5] * wb[1] * wb[1] + m.I[10] * wb[2] * wb[2]);
energy = trans_energy + rot_energy;
/* printf ("trans: %.3f rot: %.3f\n", trans_energy, rot_energy); */
/* and return */
return energy;
}
/* returns the principal components of the inertia tensor */
void body_inertia (t_real *res, dBodyID b) {
dMass m;
dBodyGetMass (b,&m);
res [0] = m.I [0];
res [1] = m.I [5];
res [2] = m.I [10];
}
StickyObj::StickyObj(const char * string){
name = string;
geomID = dWebotsGetGeomFromDEF(name);
if(geomID){
if(DEBUG)dWebotsConsolePrintf("%s geom has been found !! ", name);
bodyID = dGeomGetBody(geomID);
dMass dmass;
dBodyGetMass(bodyID, &dmass);
mass = dmass.mass;
}else{
dWebotsConsolePrintf("ERROR %s geom has NOT been found !! ERROR ", name);
}
linkJoint = dBodyGetJoint(bodyID,0);
dJointSetFeedback(linkJoint, &linkJointFeedback);
adhesiveForce = Vector3D(0.0,0.0,0.0);
adheringPoints = 0;
surfaceArea = 0.0;
collidingPoints = 0;
state = 0;
elapsedDetachingTimer = 0;
elapsedAttachingTimer = 0;
rho = 1;
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldQuickStep (world,0.02);
if (write_world) {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF (world,f,"X");
fclose (f);
}
write_world = 0;
}
if (doFeedback)
{
if (fbnum>MAX_FEEDBACKNUM)
printf("joint feedback buffer overflow!\n");
else
{
dVector3 sum = {0, 0, 0};
printf("\n");
for (int i=0; i<fbnum; i++) {
dReal* f = feedbacks[i].first?feedbacks[i].fb.f1:feedbacks[i].fb.f2;
printf("%f %f %f\n", f[0], f[1], f[2]);
sum[0] += f[0];
sum[1] += f[1];
sum[2] += f[2];
}
printf("Sum: %f %f %f\n", sum[0], sum[1], sum[2]);
dMass m;
dBodyGetMass(obj[selected].body, &m);
printf("Object G=%f\n", GRAVITY*m.mass);
}
doFeedback = 0;
fbnum = 0;
}
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (i==selected) {
dsSetColor (0,0.7,1);
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0.8,0);
}
else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
}
void BodyCutForce(dBodyID body,float l_limit,float w_limit)
{
const dReal wa_limit=w_limit/fixed_step;
const dReal* force= dBodyGetForce(body);
dReal force_mag=dSqrt(dDOT(force,force));
//body mass
dMass m;
dBodyGetMass(body,&m);
dReal force_limit =l_limit/fixed_step*m.mass;
if(force_mag>force_limit)
{
dBodySetForce(body,
force[0]/force_mag*force_limit,
force[1]/force_mag*force_limit,
force[2]/force_mag*force_limit
);
}
const dReal* torque=dBodyGetTorque(body);
dReal torque_mag=dSqrt(dDOT(torque,torque));
if(torque_mag<0.001f) return;
dMatrix3 tmp,invI,I;
// compute inertia tensor in global frame
dMULTIPLY2_333 (tmp,m.I,body->R);
dMULTIPLY0_333 (I,body->R,tmp);
// compute inverse inertia tensor in global frame
dMULTIPLY2_333 (tmp,body->invI,body->R);
dMULTIPLY0_333 (invI,body->R,tmp);
//angular accel
dVector3 wa;
dMULTIPLY0_331(wa,invI,torque);
dReal wa_mag=dSqrt(dDOT(wa,wa));
if(wa_mag>wa_limit)
{
//scale w
for(int i=0;i<3;++i)wa[i]*=wa_limit/wa_mag;
dVector3 new_torqu;
dMULTIPLY0_331(new_torqu,I,wa);
dBodySetTorque
(
body,
new_torqu[0],
new_torqu[1],
new_torqu[2]
);
}
}
void PhysicsBody::addMassOf( dBodyID otherBody )
{
dMass myMass, otherMass;
getMassStruct(myMass);
dBodyGetMass(otherBody, &otherMass);
dMassAdd(&myMass, &otherMass);
setMassStruct(myMass);
}
////Energy of non Elastic collision;
//body - static case
float E_NlS(dBodyID body,const dReal* norm,float norm_sign)//if body c.geom.g1 norm_sign + else -
{ //norm*norm_sign - to body
const dReal* vel=dBodyGetLinearVel(body);
dReal prg=-dDOT(vel,norm)*norm_sign;
prg=prg<0.f ? prg=0.f : prg;
dMass mass;
dBodyGetMass(body,&mass);
return mass.mass*prg*prg/2;
}
IoObject *IoODEBody_mass(IoODEBody *self, IoObject *locals, IoMessage *m)
{
IoODEBody_assertValidBody(self, locals, m);
{
IoODEMass *mass = IoODEMass_new(IOSTATE);
dBodyGetMass(BODYID, IoODEMass_dMassStruct(mass));
return mass;
}
}
void SParts::setMass(double mass)
{
m_mass = mass;
if(m_odeobj != NULL) {
dMass m;
dBodyGetMass(m_odeobj->body(), &m);
dMassAdjust(&m, m_mass);
dBodyID body = m_odeobj->body();
dBodySetMass(body, &m);
}
}
/* returns the rotational z kinetic energy of a body */
t_real body_zrot_kinetic_energy (dBodyID b) {
dMass m;
const t_real *omega;
dVector3 wb;
dBodyGetMass (b, &m);
omega = dBodyGetAngularVel (b);
dBodyVectorFromWorld (b, omega [0], omega [1], omega [2], wb);
return 0.5*m.I[10]*wb[2]*wb[2];
}
void
Else::AcrobotArticulatedBody
::displayMass( void )
{
dMass mass;
dBodyGetMass( myBodies[0], &mass );
std::cerr << "Mass, kg = " << mass.mass << "\n"
<< "\tCenter: "
<< mass.c[0] << ", "
<< mass.c[1] << ", "
<< mass.c[2] << "\n";
}
double
Else::AcrobotArticulatedBody
::scaleTorque( double argTorque )
{
assert( argTorque <= 1.0 && argTorque >= -1.0 );
dMass mass;
dBodyGetMass( myBodies[0], &mass );
double I_center = 0.5*mass.mass*myRadius*myRadius;
double I = I_center + mass.mass*myLength*myLength;
double torque = I * argTorque;
return torque;
}
void CPHContactBodyEffector::Apply()
{
const dReal* linear_velocity =dBodyGetLinearVel(m_body);
dReal linear_velocity_smag =dDOT(linear_velocity,linear_velocity);
dReal linear_velocity_mag =_sqrt(linear_velocity_smag);
dReal effect =10000.f*m_recip_flotation*m_recip_flotation;
dMass mass;
dBodyGetMass(m_body,&mass);
dReal l_air=linear_velocity_mag*effect;//force/velocity !!!
if(l_air>mass.mass/fixed_step) l_air=mass.mass/fixed_step;//validate
if(!fis_zero(l_air))
{
dVector3 force={
-linear_velocity[0]*l_air,
-linear_velocity[1]*l_air,
-linear_velocity[2]*l_air,
0.f
};
if(!m_material->Flags.is(SGameMtl::flPassable))
{
dVector3& norm=m_contact.geom.normal;
accurate_normalize(norm);
dMass m;
dBodyGetMass(m_body,&m);
dReal prg=1.f*dDOT(force,norm);//+dDOT(linear_velocity,norm)*m.mass/fixed_step
force[0]-=prg*norm[0];
force[1]-=prg*norm[1];
force[2]-=prg*norm[2];
}
dBodyAddForce(
m_body,
force[0],
force[1],
force[2]
);
}
dBodySetData(m_body,NULL);
}
void draw_phys_body(dBodyID body, const float c[3])
{
dMass mass;
dBodyGetMass(body, &mass);
glPushAttrib(GL_ENABLE_BIT);
{
glDisable(GL_LIGHTING);
glDisable(GL_TEXTURE_2D);
glEnable(GL_COLOR_MATERIAL);
opengl_draw_xyz(c[0], c[1], c[2]);
}
glPopAttrib();
}
void Test_ODEBodies::testSphereBody() {
Core::resetCore();
ODE_SimulationAlgorithm *algorithm = new ODE_SimulationAlgorithm();
Physics::getPhysicsManager()->setPhysicalSimulationAlgorithm(algorithm);
ODE_SphereBody *sphere = new ODE_SphereBody("Sphere", 0.01);
algorithm->resetPhysics();
QVERIFY(sphere->getRigidBodyID() != 0);
QCOMPARE((double) dBodyGetPosition(sphere->getRigidBodyID())[0], 0.0);
QCOMPARE((double) dBodyGetPosition(sphere->getRigidBodyID())[1], 0.0);
QCOMPARE((double) dBodyGetPosition(sphere->getRigidBodyID())[2], 0.0);
sphere->createODECollisionObjects();
dBodySetPosition(sphere->getRigidBodyID(), 2.0, -1.0, 3.3);
QCOMPARE(sphere->getPositionValue()->getX(), 0.0);
QCOMPARE(sphere->getPositionValue()->getY(), 0.0);
QCOMPARE(sphere->getPositionValue()->getZ(), 0.0);
sphere->synchronizeWithPhysicalModel(algorithm);
QCOMPARE(sphere->getPositionValue()->getX(), 2.0);
QCOMPARE(sphere->getPositionValue()->getY(), -1.0);
QCOMPARE(sphere->getPositionValue()->getZ(), 3.3);
dMass mass;
dBodyGetMass(sphere->getRigidBodyID(), &mass);
QCOMPARE((double) mass.mass, 0.1);
ODE_SphereBody *sphere2 = new ODE_SphereBody("Sphere2");
sphere2->getParameter("Dynamic")->setValueFromString("F");
algorithm->resetPhysics();
QVERIFY(sphere2->getRigidBodyID() == 0);
QCOMPARE(dynamic_cast<DoubleValue*>(sphere2->getParameter("Radius"))->get(), 0.00001);
ODE_SphereBody *copy = dynamic_cast<ODE_SphereBody*>(sphere->createCopy());
QVERIFY(copy != 0);
QCOMPARE(copy->getPositionValue()->getX(), 2.0);
QCOMPARE((double) dBodyGetPosition(copy->getRigidBodyID())[0], 2.0);
QCOMPARE((double) dBodyGetPosition(copy->getRigidBodyID())[1], -1.0);
QCOMPARE((double) dBodyGetPosition(copy->getRigidBodyID())[2], 3.3);
delete sphere;
delete sphere2;
delete copy;
}
void PhysicsBody::initDefaults(void)
{
setAutoDisableFlag(dBodyGetAutoDisableFlag(_BodyID));
setAutoDisableLinearThreshold(dBodyGetAutoDisableLinearThreshold(_BodyID));
setAutoDisableAngularThreshold(dBodyGetAutoDisableAngularThreshold(_BodyID));
setAutoDisableSteps(dBodyGetAutoDisableSteps(_BodyID));
setAutoDisableTime(dBodyGetAutoDisableTime(_BodyID));
setFiniteRotationMode(dBodyGetFiniteRotationMode(_BodyID));
dVector3 odeVec;
dBodyGetFiniteRotationAxis(_BodyID, odeVec);
setFiniteRotationAxis(Vec3f(odeVec[0], odeVec[1], odeVec[3]));
setGravityMode(dBodyGetGravityMode(_BodyID));
dMass TheMass;
dBodyGetMass(_BodyID, &TheMass);
setMassStruct(TheMass);
}
void Car::getSprocketMass(dMass * mass)
{
if (sprocket[0])
dBodyGetMass(sprocket[0], mass);
}
const dReal ODE_Particle::getMass()
{
dMass m;
dBodyGetMass (body,&m);
return m.mass;
}
void TContactShotMark(CDB::TRI* T,dContactGeom* c)
{
dBodyID b=dGeomGetBody(c->g1);
dxGeomUserData* data;
bool b_invert_normal=false;
if(!b)
{
b=dGeomGetBody(c->g2);
data=dGeomGetUserData(c->g2);
b_invert_normal=true;
}
else
{
data=dGeomGetUserData(c->g1);
}
if(!b) return;
dVector3 vel;
dMass m;
dBodyGetMass(b,&m);
dBodyGetPointVel(b,c->pos[0],c->pos[1],c->pos[2],vel);
dReal vel_cret=dFabs(dDOT(vel,c->normal))* _sqrt(m.mass);
Fvector to_camera;to_camera.sub(cast_fv(c->pos),Device.vCameraPosition);
float square_cam_dist=to_camera.square_magnitude();
if(data)
{
SGameMtlPair* mtl_pair = GMLib.GetMaterialPair(T->material,data->material);
if(mtl_pair)
{
//if(vel_cret>Pars.vel_cret_wallmark && !mtl_pair->CollideMarks.empty())
if(vel_cret>Pars::vel_cret_wallmark && !mtl_pair->m_pCollideMarks->empty())
{
//ref_shader pWallmarkShader = mtl_pair->CollideMarks[::Random.randI(0,mtl_pair->CollideMarks.size())];
wm_shader WallmarkShader = mtl_pair->m_pCollideMarks->GenerateWallmark();
//ref_shader pWallmarkShader = mtl_pair->CollideMarks[::Random.randI(0,mtl_pair->CollideMarks.size())];
Level().ph_commander().add_call(new CPHOnesCondition(),new CPHWallMarksCall( *((Fvector*)c->pos),T,WallmarkShader));
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
if(square_cam_dist<SQUARE_SOUND_EFFECT_DIST)
{
SGameMtl* static_mtl = GMLib.GetMaterialByIdx(T->material);
if(!static_mtl->Flags.test(SGameMtl::flPassable))
{
if(vel_cret>Pars::vel_cret_sound)
{
if(!mtl_pair->CollideSounds.empty())
{
float volume=collide_volume_min+vel_cret*(collide_volume_max-collide_volume_min)/(_sqrt(mass_limit)*default_l_limit-Pars::vel_cret_sound);
GET_RANDOM(mtl_pair->CollideSounds).play_no_feedback(0,0,0,((Fvector*)c->pos),&volume);
}
}
}
else
{
if(data->ph_ref_object&&!mtl_pair->CollideSounds.empty())
{
CPHSoundPlayer* sp=NULL;
sp=data->ph_ref_object->ph_sound_player();
if(sp) sp->Play(mtl_pair,*(Fvector*)c->pos);
}
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
if(square_cam_dist<SQUARE_PARTICLE_EFFECT_DIST)
{
if(vel_cret>Pars::vel_cret_particles && !mtl_pair->CollideParticles.empty())
{
LPCSTR ps_name = *mtl_pair->CollideParticles[::Random.randI(0,mtl_pair->CollideParticles.size())];
//отыграть партиклы столкновения материалов
Level().ph_commander().add_call(new CPHOnesCondition(),new CPHParticlesPlayCall(*c,b_invert_normal,ps_name));
}
}
}
}
}
void PhysicsBody::getMassStruct(dMass &mass )
{
dBodyGetMass(_BodyID, &mass);
}
void PhysicsBody::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
Inherited::changed(whichField, origin, details);
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & WorldFieldMask)
{
if(_BodyID != 0)
{
dBodyDestroy(_BodyID);
}
if(getWorld() != NULL)
{
_BodyID = dBodyCreate(getWorld()->getWorldID());
}
}
if( ((whichField & PositionFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetPosition(_BodyID, getPosition().x(),getPosition().y(),getPosition().z());
}
if( ((whichField & RotationFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dMatrix3 rotation;
Vec4f v1 = getRotation()[0];
Vec4f v2 = getRotation()[1];
Vec4f v3 = getRotation()[2];
rotation[0] = v1.x();
rotation[1] = v1.y();
rotation[2] = v1.z();
rotation[3] = 0;
rotation[4] = v2.x();
rotation[5] = v2.y();
rotation[6] = v2.z();
rotation[7] = 0;
rotation[8] = v3.x();
rotation[9] = v3.y();
rotation[10] = v3.z();
rotation[11] = 0;
dBodySetRotation(_BodyID, rotation);
}
if( ((whichField & QuaternionFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dQuaternion q;
q[0]=getQuaternion().w();
q[1]=getQuaternion().x();
q[2]=getQuaternion().y();
q[3]=getQuaternion().z();
dBodySetQuaternion(_BodyID,q);
}
if( ((whichField & LinearVelFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetLinearVel(_BodyID, getLinearVel().x(),getLinearVel().y(),getLinearVel().z());
}
if( ((whichField & AngularVelFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAngularVel(_BodyID, getAngularVel().x(),getAngularVel().y(),getAngularVel().z());
}
if( ((whichField & ForceFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetForce(_BodyID, getForce().x(),getForce().y(),getForce().z());
}
if( ((whichField & TorqueFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetTorque(_BodyID, getTorque().x(),getTorque().y(),getTorque().z());
}
if( ((whichField & AutoDisableFlagFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableFlag(_BodyID, getAutoDisableFlag());
}
if( ((whichField & AutoDisableLinearThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableLinearThreshold(_BodyID, getAutoDisableLinearThreshold());
}
if( ((whichField & AutoDisableAngularThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableAngularThreshold(_BodyID, getAutoDisableAngularThreshold());
}
if( ((whichField & AutoDisableStepsFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableSteps(_BodyID, getAutoDisableSteps());
}
if( ((whichField & AutoDisableTimeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableTime(_BodyID, getAutoDisableTime());
}
if( ((whichField & FiniteRotationModeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetFiniteRotationMode(_BodyID, getFiniteRotationMode());
}
if( ((whichField & FiniteRotationModeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetFiniteRotationMode(_BodyID, getFiniteRotationMode());
}
if( ((whichField & FiniteRotationAxisFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetFiniteRotationAxis(_BodyID, getFiniteRotationAxis().x(),getFiniteRotationAxis().y(),getFiniteRotationAxis().z());
}
if( ((whichField & GravityModeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetGravityMode(_BodyID, getGravityMode());
}
if( ((whichField & LinearDampingFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetLinearDamping(_BodyID, getLinearDamping());
}
if( ((whichField & AngularDampingFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAngularDamping(_BodyID, getAngularDamping());
}
if( ((whichField & LinearDampingThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetLinearDampingThreshold(_BodyID, getLinearDampingThreshold());
}
if( ((whichField & AngularDampingThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAngularDampingThreshold(_BodyID, getAngularDampingThreshold());
}
if( ((whichField & MaxAngularSpeedFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
if(getMaxAngularSpeed() >= 0.0)
{
dBodySetMaxAngularSpeed(_BodyID, getMaxAngularSpeed());
}
else
{
dBodySetMaxAngularSpeed(_BodyID, dInfinity);
}
}
if( ((whichField & MassFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dMass TheMass;
dBodyGetMass(_BodyID, &TheMass);
TheMass.mass = getMass();
dBodySetMass(_BodyID, &TheMass);
}
if( ((whichField & MassCenterOfGravityFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
//dMass TheMass;
//dBodyGetMass(_BodyID, &TheMass);
////TheMass.c[0] = getMassCenterOfGravity().x();
////TheMass.c[1] = getMassCenterOfGravity().y();
////TheMass.c[2] = getMassCenterOfGravity().z();
//Vec4f v1 = getMassInertiaTensor()[0];
//Vec4f v2 = getMassInertiaTensor()[1];
//Vec4f v3 = getMassInertiaTensor()[2];
//TheMass.I[0] = v1.x();
//TheMass.I[1] = v1.y();
//TheMass.I[2] = v1.z();
//TheMass.I[3] = getMassCenterOfGravity().x();
//TheMass.I[4] = v2.x();
//TheMass.I[5] = v2.y();
//TheMass.I[6] = v2.z();
//TheMass.I[7] = getMassCenterOfGravity().y();
//TheMass.I[8] = v3.x();
//TheMass.I[9] = v3.y();
//TheMass.I[10] = v3.z();
//TheMass.I[11] = getMassCenterOfGravity().z();
//dBodySetMass(_BodyID, &TheMass);
}
if( ((whichField & MassInertiaTensorFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dMass TheMass;
dBodyGetMass(_BodyID, &TheMass);
Vec4f v1 = getMassInertiaTensor()[0];
Vec4f v2 = getMassInertiaTensor()[1];
Vec4f v3 = getMassInertiaTensor()[2];
TheMass.I[0] = v1.x();
TheMass.I[1] = v1.y();
TheMass.I[2] = v1.z();
TheMass.I[3] = 0;
TheMass.I[4] = v2.x();
TheMass.I[5] = v2.y();
TheMass.I[6] = v2.z();
TheMass.I[7] = 0;
TheMass.I[8] = v3.x();
TheMass.I[9] = v3.y();
TheMass.I[10] = v3.z();
TheMass.I[11] = 0;
dBodySetMass(_BodyID, &TheMass);
}
if( ((whichField & KinematicFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
if(getKinematic())
{
dBodySetKinematic(_BodyID);
}
else
{
dBodySetDynamic(_BodyID);
}
}
}
double SParts::getMass()
{
dMass m;
dBodyGetMass(m_odeobj->body(), &m);
return m.mass;
}
/* returns the mass of the body */
t_real body_mass (dBodyID b) {
dMass m_id;
dBodyGetMass (b, &m_id);
return m_id.mass;
}
dMass RigidBody::getMass() const
{
dMass m;
dBodyGetMass(m_id,&m);
return m;
}
void ODESimulator::stepPhysics()
{
// Apply linear and angular damping; if using the "add opposing
// forces" method, be sure to do this before calling ODE step
// function.
std::vector<Solid*>::iterator iter;
for (iter = mSolidList.begin(); iter != mSolidList.end(); ++iter)
{
ODESolid* solid = (ODESolid*) (*iter);
if (!solid->isStatic())
{
if (solid->isSleeping())
{
// Reset velocities, force, & torques of objects that go
// to sleep; ideally, this should happen in the ODE
// source only once - when the object initially goes to
// sleep.
dBodyID bodyID = ((ODESolid*) solid)->internal_getBodyID();
dBodySetLinearVel(bodyID, 0, 0, 0);
dBodySetAngularVel(bodyID, 0, 0, 0);
dBodySetForce(bodyID, 0, 0, 0);
dBodySetTorque(bodyID, 0, 0, 0);
}
else
{
// Dampen Solid motion. 3 possible methods:
// 1) apply a force/torque in the opposite direction of
// motion scaled by the body's velocity
// 2) same as 1, but scale the opposing force by
// the body's momentum (this automatically handles
// different mass values)
// 3) reduce the body's linear and angular velocity by
// scaling them by 1 - damping * stepsize
dBodyID bodyID = solid->internal_getBodyID();
dMass mass;
dBodyGetMass(bodyID, &mass);
// Method 1
//const dReal* l = dBodyGetLinearVel(bodyID);
//dReal damping = -solid->getLinearDamping();
//dBodyAddForce(bodyID, damping*l[0], damping*l[1], damping*l[2]);
//const dReal* a = dBodyGetAngularVel(bodyID);
//damping = -solid->getAngularDamping();
//dBodyAddTorque(bodyID, damping*a[0], damping*a[1], damping*a[2]);
// Method 2
// Since the ODE mass.I inertia matrix is local, angular
// velocity and torque also need to be local.
// Linear damping
real linearDamping = solid->getLinearDamping();
if (0 != linearDamping)
{
const dReal * lVelGlobal = dBodyGetLinearVel(bodyID);
// The damping force depends on the damping amount,
// mass, and velocity (i.e. damping amount and
// momentum).
dReal dampingFactor = -linearDamping * mass.mass;
dVector3 dampingForce = {
dampingFactor * lVelGlobal[0],
dampingFactor * lVelGlobal[1],
dampingFactor * lVelGlobal[2] };
// Add a global force opposite to the global linear
// velocity.
dBodyAddForce(bodyID, dampingForce[0],
dampingForce[1], dampingForce[2]);
}
// Angular damping
real angularDamping = solid->getAngularDamping();
if (0 != angularDamping)
{
const dReal * aVelGlobal = dBodyGetAngularVel(bodyID);
dVector3 aVelLocal;
dBodyVectorFromWorld(bodyID, aVelGlobal[0],
aVelGlobal[1], aVelGlobal[2], aVelLocal);
// The damping force depends on the damping amount,
// mass, and velocity (i.e. damping amount and
// momentum).
//dReal dampingFactor = -angularDamping * mass.mass;
dReal dampingFactor = -angularDamping;
dVector3 aMomentum;
// Make adjustments for inertia tensor.
dMULTIPLYOP0_331(aMomentum, = , mass.I, aVelLocal);
dVector3 dampingTorque = {
dampingFactor * aMomentum[0],
dampingFactor * aMomentum[1],
dampingFactor * aMomentum[2] };
// Add a local torque opposite to the local angular
// velocity.
dBodyAddRelTorque(bodyID, dampingTorque[0],
dampingTorque[1], dampingTorque[2]);
}
//dMass mass;
//dBodyGetMass(bodyID, &mass);
//const dReal* l = dBodyGetLinearVel(bodyID);
//dReal damping = -solid->getLinearDamping() * mass.mass;
//dBodyAddForce(bodyID, damping*l[0], damping*l[1], damping*l[2]);
//const dReal* aVelLocal = dBodyGetAngularVel(bodyID);
////dVector3 aVelLocal;
////dBodyVectorFromWorld(bodyID, aVelGlobal[0], aVelGlobal[1], aVelGlobal[2], aVelLocal);
//damping = -solid->getAngularDamping();
//dVector3 aMomentum;
//dMULTIPLYOP0_331(aMomentum, =, aVelLocal, mass.I);
//dBodyAddTorque(bodyID, damping*aMomentum[0], damping*aMomentum[1],
// damping*aMomentum[2]);
// Method 3
//const dReal* l = dBodyGetLinearVel(bodyID);
//dReal damping = (real)1.0 - solid->getLinearDamping() * mStepSize;
//dBodySetLinearVel(bodyID, damping*l[0], damping*l[1], damping*l[2]);
//const dReal* a = dBodyGetAngularVel(bodyID);
//damping = (real)1.0 - solid->getAngularDamping() * mStepSize;
//dBodySetAngularVel(bodyID, damping*a[0], damping*a[1], damping*a[2]);
}
}
}
// Do collision detection; add contacts to the contact joint group.
dSpaceCollide(mRootSpaceID, this,
&ode_hidden::internal_collisionCallback);
// Take a simulation step.
if (SOLVER_QUICKSTEP == mSolverType)
{
dWorldQuickStep(mWorldID, mStepSize);
}
else
{
dWorldStep(mWorldID, mStepSize);
}
// Remove all joints from the contact group.
dJointGroupEmpty(mContactJointGroupID);
// Fix angular velocities for freely-spinning bodies that have
// gained angular velocity through explicit integrator inaccuracy.
for (iter = mSolidList.begin(); iter != mSolidList.end(); ++iter)
{
ODESolid* s = (ODESolid*) (*iter);
if (!s->isSleeping() && !s->isStatic())
{
s->internal_doAngularVelFix();
}
}
}
