//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(); } } }