Vec3r ODESimulator::getGravity() const
{
dVector3 g;
dWorldGetGravity(mWorldID, g);
Vec3r gravity((real) g[0], (real) g[1], (real) g[2]);
return gravity;
}
void CoreComponentModel::updateSensors(boost::shared_ptr<Environment>& env) {
if (sensor_ != NULL) {
dVector3 gravity;
dWorldGetGravity(getPhysicsWorld(), gravity);
sensor_->update(this->getRootPosition(), this->getRootAttitude(),
env->getTimeElapsed(),
osg::Vec3(gravity[0], gravity[1], gravity[2]));
}
}
void PhysicsServer::Render()
{
if (m_WorldID == 0)
{
return;
}
// synchronize solver with physics objects
JObjectIterator it( this );
++it;
while (it)
{
PhysEntity* pObj = obj_cast<PhysEntity>( *it );
pObj->Synchronize( false );
++it;
}
float timePassed = JAnimation::GetContext().m_TimeDelta + m_TimeRepr;
m_NStepsPerFrame = 0;
while (timePassed > 0)
{
StepSolver( m_StepSize );
timePassed -= m_StepSize;
m_NStepsPerFrame++;
}
m_TimeRepr = timePassed;
// synchronize physics objects with solver
JObjectIterator fit( this );
++fit;
while (fit)
{
assert( obj_cast<PhysEntity>( *fit ) );
PhysEntity* pObj = static_cast<PhysEntity*>( *fit );
pObj->Synchronize( true );
++fit;
}
dReal cfm = dWorldGetCFM( m_WorldID );
dReal erp = dWorldGetERP( m_WorldID );
dVector3 gr;
dWorldGetGravity( m_WorldID, gr );
SendSignals();
} // PhysicsServer::Update
// called when a key pressed
static void command (int cmd)
{
switch (cmd) {
case 'h' : case 'H' : case '?' :
printKeyBoardShortCut();
break;
// Force
case 'q' : case 'Q' :
dBodyAddForce(body[D],40,0,0);
break;
case 'w' : case 'W' :
dBodyAddForce(body[D],-40,0,0);
break;
case 'a' : case 'A' :
dBodyAddForce(body[D],0,40,0);
break;
case 's' : case 'S' :
dBodyAddForce(body[D],0,-40,0);
break;
case 'z' : case 'Z' :
dBodyAddForce(body[D],0,0,40);
break;
case 'x' : case 'X' :
dBodyAddForce(body[D],0,0,-40);
break;
// Torque
case 'e': case 'E':
dBodyAddTorque(body[D],0.1,0,0);
break;
case 'r': case 'R':
dBodyAddTorque(body[D],-0.1,0,0);
break;
case 'd': case 'D':
dBodyAddTorque(body[D],0, 0.1,0);
break;
case 'f': case 'F':
dBodyAddTorque(body[D],0,-0.1,0);
break;
case 'c': case 'C':
dBodyAddTorque(body[D],0,0,0.1);
break;
case 'v': case 'V':
dBodyAddTorque(body[D],0,0,0.1);
break;
// Velocity of joint
case ',': case '<' : {
dReal vel = joint->getParam (dParamVel3) - VEL_INC;
joint->setParam (dParamVel3, vel);
std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
}
break;
case '.': case '>' : {
dReal vel = joint->getParam (dParamVel3) + VEL_INC;
joint->setParam (dParamVel3, vel);
std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
}
break;
case 'l': case 'L' : {
dReal aLimit, lLimit, fmax;
if ( joint->getParam (dParamFMax) ) {
aLimit = dInfinity;
lLimit = dInfinity;
fmax = 0;
}
else {
aLimit = 0.25*PI;
lLimit = 0.5*axDim[LENGTH];
fmax = 0.02;
}
joint->setParam (dParamFMax1, fmax);
joint->setParam (dParamFMax2, fmax);
joint->setParam (dParamFMax3, fmax);
switch (joint->getType() ) {
case dJointTypePR : {
dPRJoint *pr = reinterpret_cast<dPRJoint *> (joint);
pr->setParam (dParamLoStop, -lLimit);
pr->setParam (dParamHiStop, -lLimit);
pr->setParam (dParamLoStop2, aLimit);
pr->setParam (dParamHiStop2, -aLimit);
}
break;
case dJointTypePU : {
dPUJoint *pu = reinterpret_cast<dPUJoint *> (joint);
pu->setParam (dParamLoStop1, -aLimit);
pu->setParam (dParamHiStop1, aLimit);
pu->setParam (dParamLoStop2, -aLimit);
pu->setParam (dParamHiStop2, aLimit);
pu->setParam (dParamLoStop3, -lLimit);
pu->setParam (dParamHiStop3, lLimit);
}
break;
default: {} // keep the compiler happy
}
}
break;
case 'g': case 'G' : {
dVector3 g;
dWorldGetGravity(world, g);
if ( g[2]< -0.1 )
dWorldSetGravity(world, 0, 0, 0);
else
dWorldSetGravity(world, 0, 0, -0.5);
}
case 'p' :case 'P' : {
switch (joint->getType() ) {
case dJointTypeSlider : {
dSliderJoint *sj = reinterpret_cast<dSliderJoint *> (joint);
std::cout<<"Position ="<<sj->getPosition() <<"\n";
}
break;
case dJointTypePU : {
dPUJoint *pu = reinterpret_cast<dPUJoint *> (joint);
std::cout<<"Position ="<<pu->getPosition() <<"\n";
std::cout<<"Position Rate="<<pu->getPositionRate() <<"\n";
std::cout<<"Angle1 ="<<pu->getAngle1() <<"\n";
std::cout<<"Angle1 Rate="<<pu->getAngle1Rate() <<"\n";
std::cout<<"Angle2 ="<<pu->getAngle2() <<"\n";
std::cout<<"Angle2 Rate="<<pu->getAngle2Rate() <<"\n";
}
break;
default: {} // keep the compiler happy
}
}
break;
}
}
