void CDynamics3DEngine::Init(TConfigurationNode& t_tree) {
/* Init parent */
CPhysicsEngine::Init(t_tree);
/* Parse the XML */
GetNodeAttributeOrDefault(t_tree, "gravity", m_cGravity, CVector3(0.0f, 0.0f, -9.81f));
GetNodeAttributeOrDefault<Real>(t_tree, "erp", m_fERP, 0.8);
GetNodeAttributeOrDefault<Real>(t_tree, "cfm", m_fCFM, 0.01);
GetNodeAttributeOrDefault<UInt32>(t_tree, "iterations", m_unIterations, 20);
GetNodeAttributeOrDefault<size_t>(t_tree, "max_contacts", m_unMaxContacts, 32);
/* Init ODE stuff */
m_tWorldID = dWorldCreate();
m_tSpaceID = dHashSpaceCreate(0);
dSpaceSetSublevel(m_tSpaceID, 0);
m_tContactJointGroupID = dJointGroupCreate(0);
dWorldSetGravity(m_tWorldID, 0.0f, 0.0f, -9.81f);
dWorldSetERP(m_tWorldID, m_fERP);
dWorldSetCFM(m_tWorldID, m_fCFM);
dWorldSetQuickStepNumIterations(m_tWorldID, m_unIterations);
dInitODE();
/* Initialize contact information */
m_ptContacts = new dContact[m_unMaxContacts];
for(UInt32 i = 0; i < m_unMaxContacts; ++i) {
::memset(&(m_ptContacts[i].surface), 0, sizeof(dSurfaceParameters));
m_ptContacts[i].surface.mode = dContactMu2;
m_ptContacts[i].surface.mu = dInfinity;
m_ptContacts[i].surface.mu2 = dInfinity;
}
/* Add a planar floor */
m_tFloor = dCreatePlane(m_tSpaceID, 0, 0, 1.0f, 0.0f);
/* Set the random seed from a random number taken from ARGoS RNG */
m_pcRNG = CARGoSRandom::CreateRNG("argos");
dRandSetSeed(m_pcRNG->Uniform(CRange<UInt32>(1,65535)));
}
void PhysicsSpace::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
Inherited::changed(whichField, origin, details);
if(whichField & CleanupFieldMask)
{
dSpaceSetCleanup(_SpaceID, getCleanup() ? 1 : 0);
}
if(whichField & SublevelFieldMask)
{
dSpaceSetSublevel(_SpaceID, getSublevel());
}
}
CDynamics3DEntity::CDynamics3DEntity(CDynamics3DEngine& c_engine,
CEmbodiedEntity& c_entity) :
CPhysicsEngineEntity(c_entity),
m_cEngine(c_engine),
m_tEngineWorld(c_engine.GetWorldID()),
m_tEngineSpace(c_engine.GetSpaceID()),
m_tBody(dBodyCreate(m_tEngineWorld)),
m_tEntitySpace(dSimpleSpaceCreate(m_tEngineSpace)) {
/* Zero the body mass */
dMassSetZero(&m_tMass);
/*
* Set the level of the body space
* Necessary for dSpaceCollide2 to work properly
*/
dSpaceSetSublevel(m_tEngineSpace, 1);
}
void PhysicsSpace::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 & CleanupFieldMask)
{
dSpaceSetCleanup(_SpaceID, getCleanup() ? 1 : 0);
}
if(whichField & SublevelFieldMask)
{
dSpaceSetSublevel(_SpaceID, getSublevel());
}
}
