void ODESimulator::setSolverAccuracy(SolverAccuracyLevel level)
{
Simulator::setSolverAccuracy(level);
switch (level)
{
case SOLVER_ACCURACY_VERY_LOW:
mSolverType = SOLVER_QUICKSTEP;
dWorldSetQuickStepNumIterations(mWorldID, 5);
break;
case SOLVER_ACCURACY_LOW:
mSolverType = SOLVER_QUICKSTEP;
dWorldSetQuickStepNumIterations(mWorldID, 10);
break;
case SOLVER_ACCURACY_MEDIUM:
mSolverType = SOLVER_QUICKSTEP;
// 20 is the default in ODE
dWorldSetQuickStepNumIterations(mWorldID, 20);
break;
case SOLVER_ACCURACY_HIGH:
mSolverType = SOLVER_QUICKSTEP;
dWorldSetQuickStepNumIterations(mWorldID, 40);
break;
case SOLVER_ACCURACY_VERY_HIGH:
mSolverType = SOLVER_WORLDSTEP;
break;
default:
assert(false);
break;
}
}
// Called by Physics::Server when the Level is attached to the server.
void CLevel::Activate()
{
TimeToSim = 0.0;
// Initialize ODE //???per-Level?
dInitODE();
ODEWorldID = dWorldCreate();
dWorldSetQuickStepNumIterations(ODEWorldID, 20);
// FIXME(enno): is a quadtree significantly faster? -- can't count geoms with quadtree
ODECommonSpaceID = dSimpleSpaceCreate(NULL);
ODEDynamicSpaceID = dSimpleSpaceCreate(ODECommonSpaceID);
ODEStaticSpaceID = dSimpleSpaceCreate(ODECommonSpaceID);
dWorldSetGravity(ODEWorldID, Gravity.x, Gravity.y, Gravity.z);
dWorldSetContactSurfaceLayer(ODEWorldID, 0.001f);
dWorldSetContactMaxCorrectingVel(ODEWorldID, 100.0f);
dWorldSetERP(ODEWorldID, 0.2f); // ODE's default value
dWorldSetCFM(ODEWorldID, 0.001f); // the default is 10^-5
// setup autodisabling
dWorldSetAutoDisableFlag(ODEWorldID, 1);
dWorldSetAutoDisableSteps(ODEWorldID, 5);
//dWorldSetAutoDisableTime(ODEWorldID, 1.f);
dWorldSetAutoDisableLinearThreshold(ODEWorldID, 0.05f); // default is 0.01
dWorldSetAutoDisableAngularThreshold(ODEWorldID, 0.1f); // default is 0.01
// create a Contact group for joints
ContactJointGroup = dJointGroupCreate(0);
}
void avancarPasso(dReal dt)
{
dWorldSetQuickStepNumIterations (world, 20);
dSpaceCollide(space,0,&nearCallback);
dWorldQuickStep(world, dt); // Esse valor deve ser sincronizado com as chamadas da função.
dJointGroupEmpty(contactgroup);
}
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)));
}
int main(int argc, char **argv)
{
dInitODE();
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
world = dWorldCreate();
dWorldSetGravity(world, 0, 0, -0.5);
dWorldSetQuickStepNumIterations(world, 50); // <-- increase for more stability
space = dSimpleSpaceCreate(0);
contactgroup = dJointGroupCreate(0);
dGeomID ground = dCreatePlane(space, 0, 0, 1, 0);
place_cards();
// run simulation
dsSimulationLoop (argc, argv, 640, 480, &fn);
levels = 0;
place_cards();
dJointGroupDestroy(contactgroup);
dWorldDestroy(world);
dGeomDestroy(ground);
dSpaceDestroy(space);
dCloseODE();
}
bool Simulation::loadFile(const std::string& filename, std::list<std::string>& errors)
{
ASSERT(scene == 0);
Parser parser;
if(!parser.parse(filename, errors))
return false;
ASSERT(scene);
dInitODE();
physicalWorld = dWorldCreate();
rootSpace = dHashSpaceCreate(0);
staticSpace = dHashSpaceCreate(rootSpace);
movableSpace = dHashSpaceCreate(rootSpace);
contactGroup = dJointGroupCreate(0);
dWorldSetGravity(physicalWorld, 0, 0, scene->gravity);
if(scene->erp != -1.f)
dWorldSetERP(physicalWorld, scene->erp);
if(scene->cfm != -1.f)
dWorldSetCFM(physicalWorld, scene->cfm);
if(scene->quickSolverIterations != -1)
dWorldSetQuickStepNumIterations(physicalWorld, scene->quickSolverIterations);
#ifdef MULTI_THREADING
threading = dThreadingAllocateMultiThreadedImplementation();
pool = dThreadingAllocateThreadPool(std::thread::hardware_concurrency(), 0, dAllocateFlagBasicData, nullptr);
dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading);
dWorldSetStepThreadingImplementation(physicalWorld, dThreadingImplementationGetFunctions(threading), threading);
#endif
scene->createPhysics();
renderer.init();
return true;
}
bool physics_init(void)
{
printlog(0, "Initiating physics");
dInitODE2(0);
dAllocateODEDataForThread(dAllocateFlagBasicData | dAllocateFlagCollisionData);
world = dWorldCreate();
//TODO: move to "dQuadTreeSpaceCreate()" - much better performance!!!
printlog(1, "TODO: create world space using dQuadTreeSpaceCreate() - much better performance!");
space = dHashSpaceCreate(0);
contactgroup = dJointGroupCreate(0);
dWorldSetQuickStepNumIterations (world, internal.iterations);
//autodisable
dWorldSetAutoDisableFlag (world, 1);
dWorldSetAutoDisableLinearThreshold (world, internal.dis_linear);
dWorldSetAutoDisableAngularThreshold (world, internal.dis_angular);
dWorldSetAutoDisableSteps (world, internal.dis_steps);
dWorldSetAutoDisableTime (world, internal.dis_time);
return true;
}
int main (int argc, char **argv)
{
dMass m;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
dWorldSetQuickStepNumIterations (world, 32);
dCreatePlane (space,0,0,1, 0.0);
cylbody = dBodyCreate (world);
dQuaternion q;
#if 0
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
#else
// dQFromAxisAndAngle (q,1,0,0, M_PI * 1.0);
dQFromAxisAndAngle (q,1,0,0, M_PI * -0.77);
#endif
dBodySetQuaternion (cylbody,q);
dMassSetCylinder (&m,1.0,3,CYLRADIUS,CYLLENGTH);
dBodySetMass (cylbody,&m);
cylgeom = dCreateCylinder(0, CYLRADIUS, CYLLENGTH);
dGeomSetBody (cylgeom,cylbody);
dBodySetPosition (cylbody, 0, 0, 3);
dSpaceAdd (space, cylgeom);
sphbody = dBodyCreate (world);
dMassSetSphere (&m,1,SPHERERADIUS);
dBodySetMass (sphbody,&m);
sphgeom = dCreateSphere(0, SPHERERADIUS);
dGeomSetBody (sphgeom,sphbody);
dBodySetPosition (sphbody, 0, 0, 5.5);
dSpaceAdd (space, sphgeom);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
dGeomDestroy(sphgeom);
dGeomDestroy (cylgeom);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void resetSimulation()
{
int i;
i = 0;
// destroy world if it exists
if (bodies)
{
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
}
for (i = 0; i < 1000; i++)
wb_stepsdis[i] = 0;
// recreate world
world = dWorldCreate();
// space = dHashSpaceCreate( 0 );
// space = dSimpleSpaceCreate( 0 );
space = dSweepAndPruneSpaceCreate( 0, dSAP_AXES_XYZ );
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-1.5);
dWorldSetCFM (world, 1e-5);
dWorldSetERP (world, 0.8);
dWorldSetQuickStepNumIterations (world,ITERS);
ground = dCreatePlane (space,0,0,1,0);
bodies = 0;
joints = 0;
boxes = 0;
spheres = 0;
wb = 0;
#ifdef CARS
for (dReal x = 0.0; x < COLS*(LENGTH+RADIUS); x += LENGTH+RADIUS)
for (dReal y = -((ROWS-1)*(WIDTH/2+RADIUS)); y <= ((ROWS-1)*(WIDTH/2+RADIUS)); y += WIDTH+RADIUS*2)
makeCar(x, y, bodies, joints, boxes, spheres);
#endif
#ifdef WALL
bool offset = false;
for (dReal z = WBOXSIZE/2.0; z <= WALLHEIGHT; z+=WBOXSIZE)
{
offset = !offset;
for (dReal y = (-WALLWIDTH+z)/2; y <= (WALLWIDTH-z)/2; y+=WBOXSIZE)
{
wall_bodies[wb] = dBodyCreate (world);
dBodySetPosition (wall_bodies[wb],-20,y,z);
dMassSetBox (&m,1,WBOXSIZE,WBOXSIZE,WBOXSIZE);
dMassAdjust (&m, WALLMASS);
dBodySetMass (wall_bodies[wb],&m);
wall_boxes[wb] = dCreateBox (space,WBOXSIZE,WBOXSIZE,WBOXSIZE);
dGeomSetBody (wall_boxes[wb],wall_bodies[wb]);
//dBodyDisable(wall_bodies[wb++]);
wb++;
}
}
dMessage(0,"wall boxes: %i", wb);
#endif
#ifdef BALLS
for (dReal x = -7; x <= -4; x+=1)
for (dReal y = -1.5; y <= 1.5; y+=1)
for (dReal z = 1; z <= 4; z+=1)
{
b = dBodyCreate (world);
dBodySetPosition (b,x*RADIUS*2,y*RADIUS*2,z*RADIUS*2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, BALLMASS);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
#endif
#ifdef ONEBALL
b = dBodyCreate (world);
dBodySetPosition (b,0,0,2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
#endif
#ifdef BALLSTACK
for (dReal z = 1; z <= 6; z+=1)
{
b = dBodyCreate (world);
dBodySetPosition (b,0,0,z*RADIUS*2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 0.1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
#endif
#ifdef CENTIPEDE
dBodyID lastb = 0;
for (dReal y = 0; y < 10*LENGTH; y+=LENGTH+0.1)
{
// chassis body
b = body[bodies] = dBodyCreate (world);
dBodySetPosition (body[bodies],-15,y,STARTZ);
dMassSetBox (&m,1,WIDTH,LENGTH,HEIGHT);
dMassAdjust (&m,CMASS);
dBodySetMass (body[bodies],&m);
box[boxes] = dCreateBox (space,WIDTH,LENGTH,HEIGHT);
dGeomSetBody (box[boxes++],body[bodies++]);
for (dReal x = -17; x > -20; x-=RADIUS*2)
{
body[bodies] = dBodyCreate (world);
dBodySetPosition(body[bodies], x, y, STARTZ);
dMassSetSphere(&m, 1, RADIUS);
dMassAdjust(&m, WMASS);
dBodySetMass(body[bodies], &m);
sphere[spheres] = dCreateSphere (space, RADIUS);
dGeomSetBody (sphere[spheres++], body[bodies]);
joint[joints] = dJointCreateHinge2 (world,0);
if (x == -17)
dJointAttach (joint[joints],b,body[bodies]);
else
dJointAttach (joint[joints],body[bodies-2],body[bodies]);
const dReal *a = dBodyGetPosition (body[bodies++]);
dJointSetHinge2Anchor (joint[joints],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[joints],0,0,1);
dJointSetHinge2Axis2 (joint[joints],1,0,0);
dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[joints],dParamLoStop,0);
dJointSetHinge2Param (joint[joints],dParamHiStop,0);
dJointSetHinge2Param (joint[joints],dParamVel2,-10.0);
dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
body[bodies] = dBodyCreate (world);
dBodySetPosition(body[bodies], -30 - x, y, STARTZ);
dMassSetSphere(&m, 1, RADIUS);
dMassAdjust(&m, WMASS);
dBodySetMass(body[bodies], &m);
sphere[spheres] = dCreateSphere (space, RADIUS);
dGeomSetBody (sphere[spheres++], body[bodies]);
joint[joints] = dJointCreateHinge2 (world,0);
if (x == -17)
dJointAttach (joint[joints],b,body[bodies]);
else
dJointAttach (joint[joints],body[bodies-2],body[bodies]);
const dReal *b = dBodyGetPosition (body[bodies++]);
dJointSetHinge2Anchor (joint[joints],b[0],b[1],b[2]);
dJointSetHinge2Axis1 (joint[joints],0,0,1);
dJointSetHinge2Axis2 (joint[joints],1,0,0);
dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[joints],dParamLoStop,0);
dJointSetHinge2Param (joint[joints],dParamHiStop,0);
dJointSetHinge2Param (joint[joints],dParamVel2,10.0);
dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
}
if (lastb)
{
dJointID j = dJointCreateFixed(world,0);
dJointAttach (j, b, lastb);
dJointSetFixed(j);
}
lastb = b;
}
#endif
#ifdef BOX
body[bodies] = dBodyCreate (world);
dBodySetPosition (body[bodies],0,0,HEIGHT/2);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m, 1);
dBodySetMass (body[bodies],&m);
box[boxes] = dCreateBox (space,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (box[boxes++],body[bodies++]);
#endif
#ifdef CANNON
cannon_ball_body = dBodyCreate (world);
cannon_ball_geom = dCreateSphere (space,CANNON_BALL_RADIUS);
dMassSetSphereTotal (&m,CANNON_BALL_MASS,CANNON_BALL_RADIUS);
dBodySetMass (cannon_ball_body,&m);
dGeomSetBody (cannon_ball_geom,cannon_ball_body);
dBodySetPosition (cannon_ball_body,CANNON_X,CANNON_Y,CANNON_BALL_RADIUS);
#endif
}
int main (int argc, char **argv)
{
dMass m;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
dWorldSetQuickStepNumIterations (world, 20);
int i;
for (i=0; i<SEGMCNT; i++)
{
segbodies[i] = dBodyCreate (world);
dBodySetPosition(segbodies[i], i - SEGMCNT/2.0, 0, 5);
dMassSetBox (&m, 1, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
dBodySetMass (segbodies[i], &m);
seggeoms[i] = dCreateBox (0, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
dGeomSetBody (seggeoms[i], segbodies[i]);
dSpaceAdd (space, seggeoms[i]);
}
for (i=0; i<SEGMCNT-1; i++)
{
hinges[i] = dJointCreateHinge (world,0);
dJointAttach (hinges[i], segbodies[i],segbodies[i+1]);
dJointSetHingeAnchor (hinges[i], i + 0.5 - SEGMCNT/2.0, 0, 5);
dJointSetHingeAxis (hinges[i], 0,1,0);
dJointSetHingeParam (hinges[i],dParamFMax, 8000.0);
// NOTE:
// Here we tell ODE where to put the feedback on the forces for this hinge
dJointSetFeedback (hinges[i], jfeedbacks+i);
stress[i]=0;
}
for (i=0; i<STACKCNT; i++)
{
stackbodies[i] = dBodyCreate(world);
dMassSetBox (&m, 2.0, 2, 2, 0.6);
dBodySetMass(stackbodies[i],&m);
stackgeoms[i] = dCreateBox(0, 2, 2, 0.6);
dGeomSetBody(stackgeoms[i], stackbodies[i]);
dBodySetPosition(stackbodies[i], 0,0,8+2*i);
dSpaceAdd(space, stackgeoms[i]);
}
sliders[0] = dJointCreateSlider (world,0);
dJointAttach(sliders[0], segbodies[0], 0);
dJointSetSliderAxis (sliders[0], 1,0,0);
dJointSetSliderParam (sliders[0],dParamFMax, 4000.0);
dJointSetSliderParam (sliders[0],dParamLoStop, 0.0);
dJointSetSliderParam (sliders[0],dParamHiStop, 0.2);
sliders[1] = dJointCreateSlider (world,0);
dJointAttach(sliders[1], segbodies[SEGMCNT-1], 0);
dJointSetSliderAxis (sliders[1], 1,0,0);
dJointSetSliderParam (sliders[1],dParamFMax, 4000.0);
dJointSetSliderParam (sliders[1],dParamLoStop, 0.0);
dJointSetSliderParam (sliders[1],dParamHiStop, -0.2);
groundgeom = dCreatePlane(space, 0,0,1,0);
for (i=0; i<SEGMCNT; i++)
colours[i]=0.0;
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
// First destroy seggeoms, then space, then the world.
for (i=0; i<SEGMCNT; i++)
dGeomDestroy (seggeoms[i]);
for (i=0; i<STACKCNT; i++)
dGeomDestroy (stackgeoms[i]);
dSpaceDestroy(space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
int main (int argc, char **argv)
{
dMass m;
dMatrix3 R;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = "../../drawstuff/textures";
if(argc==2)
fn.path_to_textures = argv[1];
// create world
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
dWorldSetQuickStepNumIterations (world, 64);
// Create a static world using a triangle mesh that we can collide with.
int numv = sizeof(world_vertices)/(3*sizeof(float));
int numi = sizeof(world_indices)/ sizeof(int);
printf("numv=%d, numi=%d\n", numv, numi);
dTriMeshDataID Data = dGeomTriMeshDataCreate();
// fprintf(stderr,"Building Single Precision Mesh\n");
dGeomTriMeshDataBuildSingle
(
Data,
world_vertices,
3 * sizeof(float),
numv,
world_indices,
numi,
3 * sizeof(int)
);
world_mesh = dCreateTriMesh(space, Data, 0, 0, 0);
dGeomTriMeshEnableTC(world_mesh, dSphereClass, false);
dGeomTriMeshEnableTC(world_mesh, dBoxClass, false);
dGeomSetPosition(world_mesh, 0, 0, 0.5);
dRSetIdentity(R);
//dIASSERT(dVALIDMAT3(R));
dGeomSetRotation (world_mesh, R);
float sx=0.0, sy=3.40, sz=6.80;
sphbody = dBodyCreate (world);
dMassSetSphere (&m,1,RADIUS);
dBodySetMass (sphbody,&m);
sphgeom = dCreateSphere(0, RADIUS);
dGeomSetBody (sphgeom,sphbody);
reset_ball();
dSpaceAdd (space, sphgeom);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
// Causes segm violation? Why?
// (because dWorldDestroy() destroys body connected to geom; must call first!)
dGeomDestroy(sphgeom);
dGeomDestroy (world_mesh);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
return 0;
}
int main (int argc, char **argv)
{
dMass m;
dMatrix3 R;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
if(argc==2)
{
fn.path_to_textures = argv[1];
}
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
dWorldSetQuickStepNumIterations (world, 12);
// Create a static world using a triangle mesh that we can collide with.
int numv = sizeof(world_vertices)/(3*sizeof(float));
int numi = sizeof(world_indices)/ sizeof(dTriIndex);
printf("numv=%d, numi=%d\n", numv, numi);
dTriMeshDataID Data = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle
(
Data,
world_vertices,
3 * sizeof(float),
numv,
world_indices,
numi,
3 * sizeof(dTriIndex)
);
world_mesh = dCreateTriMesh(space, Data, 0, 0, 0);
dGeomSetPosition(world_mesh, 0, 0, 0.5);
dRFromAxisAndAngle (R, 0,1,0, 0.0);
dGeomSetRotation (world_mesh, R);
#ifdef BOX
boxbody = dBodyCreate (world);
dMassSetBox (&m,1, BOXSZ, BOXSZ, BOXSZ);
dMassAdjust (&m, 1);
dBodySetMass (boxbody,&m);
boxgeom = dCreateBox (0, BOXSZ, BOXSZ, BOXSZ);
dGeomSetBody (boxgeom,boxbody);
dSpaceAdd (space, boxgeom);
#endif
#ifdef CYL
cylbody = dBodyCreate (world);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m,WMASS);
dBodySetMass (cylbody,&m);
cylgeom = dCreateCylinder(0, RADIUS, WHEELW);
dGeomSetBody (cylgeom,cylbody);
#if defined(CYL_GEOM_OFFSET)
dMatrix3 mat;
dRFromAxisAndAngle(mat,1.0f,0.0f,0.0f,M_PI/2.0);
dGeomSetOffsetRotation(cylgeom,mat);
#endif
dSpaceAdd (space, cylgeom);
#endif
reset_state();
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
// First destroy geoms, then space, then the world.
#ifdef CYL
dGeomDestroy (cylgeom);
#endif
#ifdef BOX
dGeomDestroy (boxgeom);
#endif
dGeomDestroy (world_mesh);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
(void)world_normals; // get rid of compiler warnings
}
virtual void Execute (LPCSTR args)
{
CCC_Integer::Execute (args);
dWorldSetQuickStepNumIterations(NULL,phIterations);
}