/* ------------------------
* メイン関数
------------------------ */
int main(int argc, char *argv[])
{
/* txtデータ読み込み */
LoadTxt("route.txt", routeX, routeY, routeZ, &lineRoute);
LoadTxt("obstacle.txt", obstX, obstY, obstZ, &lineObst);
/* ODEの初期化 */
dInitODE();
/* 描画関数の設定 */
setDrawStuff();
/* ワールド, スペース, 接触点グループの生成 */
world = dWorldCreate();
space = dHashSpaceCreate(0);
contactgroup = dJointGroupCreate(0);
/* 地面, 重力の生成 */
ground = dCreatePlane(space,0,0,1,0);
dWorldSetGravity(world, 0.0, 0.0, -9.8);
/* CFM, ERPの設定 */
dWorldSetCFM(world,1e-3);
dWorldSetERP(world,0.8);
/* 全方向移動ロボットの生成 */
t1 = clock();
MakeBox();
MakeOmni();
/* シミュレーションループ */
dsSimulationLoop(argc,argv,640,480,&fn);
/* 接触点グループ, スペース, ワールドの破壊, ODEの終了 */
dJointGroupDestroy(contactgroup);
dSpaceDestroy(space);
dWorldDestroy(world);
dCloseODE();
return 0;
}
void destruirMundo()
{
for(int i=0; i < 6; i++)//destruir Robos
{
dGeomDestroy (robot[i].box[0]);
dGeomDestroy (robot[i].box[1]);
dGeomDestroy (robot[i].cylinder[0]);
dGeomDestroy (robot[i].cylinder[1]);
}
bola.destruir();
for(int i=0; i < 6; i++) //destruir Campo
dGeomDestroy(wall[i]);
for(int i=0; i < 3; i++) //destruir Gols
{
dGeomDestroy(goalR[i]);
dGeomDestroy(goalL[i]);
}
for(int i=0; i < 4; i++)//destruir Quinas
{
dGeomDestroy(triangle[i]);
}
dJointGroupDestroy(contactgroup); // Destrói o grupo de juntas de contato
dSpaceDestroy (space); // Destrói o espaço
dWorldDestroy (world); // Destrói o mundo
}
int main (int argc, char **argv)
{
// 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";
// create world
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
dWorldSetAutoDisableFlag (world,1);
dWorldSetContactMaxCorrectingVel (world,0.1);
dWorldSetContactSurfaceLayer (world,0.001);
dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
return 0;
}
void DynamicsSolver::Shutdown()
{
if(IsSetup)
{
IsSetup = false;
dWorldDestroy (WorldID);
}
for(int i=0; i<MAX_CONSTRUCTIONS; i++)
{
if(ConstructionList[i])
delete ConstructionList[i];
ConstructionList[i] = NULL;
}
for(int j=0; j<MAX_STRUCTURES; j++)
{
if( StructureList[j] )
delete StructureList[j];
StructureList[j] = NULL;
}
if(StaticSpaceID)
delete [] StaticSpaceID;
StaticSpaceID = NULL;
if(SpaceDist)
delete [] SpaceDist;
SpaceDist = NULL;
nConstructions = 0;
nStructures = 0;
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = 0;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
alloc_arrays();
dInitODE2(0);
dRandSetSeed (time(0));
createTest();
// run simulation
#pragma kaapi parallel
dsSimulationLoop (argc,argv,352,288,&fn);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void ODESimulator::destroy()
{
// These temporary copies are necessary because the
// ODESimulator::~ODESimulator call (due to "delete this") will
// invalidate the data members.
dSpaceID rootSpaceID = mRootSpaceID;
dWorldID worldID = mWorldID;
dJointGroupID contactJointGroupID = mContactJointGroupID;
delete this;
// The following must occur after Simulator::~Simulator() is called;
// otherwise, Simulator::~Simulator() will try to destroy Solids after
// ODE has closed.
dSpaceDestroy(rootSpaceID);
dWorldDestroy(worldID);
dJointGroupDestroy(contactJointGroupID);
// We can only close ODE once.
--mInitCounter;
if (0 == mInitCounter)
{
dCloseODE();
}
}
int main( int argc, char **argv )
{
// 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 = dSimpleSpaceCreate( 0 );
contactgroup = dJointGroupCreate( 0 );
dWorldSetGravity( world,0,0,-0.5 );
dWorldSetCFM( world,1e-5 );
dCreatePlane( space,0,0,1,0 );
memset( obj,0,sizeof( obj ) );
// run simulation
dsSimulationLoop( argc,argv,352,288,&fn );
dJointGroupDestroy( contactgroup );
dSpaceDestroy( space );
dWorldDestroy( world );
dCloseODE();
return 0;
}
/*******************************************************************************
Function to clean the ODE system.
*******************************************************************************/
void closeODE()
{
dJointGroupDestroy(jointgroup);
dJointGroupDestroy( ContactGroup ); //Remove the contact joints.
dSpaceDestroy( Space ); //Remove the space and all of its geoms.
dWorldDestroy( World ); //Destroy all bodies and joints (not in a group).
}
void PhysWorld::DeInitialize()
{
dSpaceDestroy(mSpace);
dWorldDestroy(mWorld);
dCloseODE();
isInitialized = false;
}
PWorld::~PWorld()
{
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
}
Simulation::~Simulation()
{
for(std::list<Element*>::const_iterator iter = elements.begin(), end = elements.end(); iter != end; ++iter)
delete *iter;
if(contactGroup)
dJointGroupDestroy(contactGroup);
if(rootSpace)
dSpaceDestroy(rootSpace);
if(physicalWorld)
{
#ifdef MULTI_THREADING
dThreadingImplementationShutdownProcessing(threading);
dThreadingThreadPoolWaitIdleState(pool);
dThreadingFreeThreadPool(pool);
dWorldSetStepThreadingImplementation(physicalWorld, nullptr, nullptr);
dThreadingFreeImplementation(threading);
#endif
dWorldDestroy(physicalWorld);
dCloseODE();
}
ASSERT(simulation == this);
simulation = 0;
}
// Called by Physics::Server when the Level is removed from the server.
void CLevel::Deactivate()
{
n_assert(ODEWorldID);
n_assert(ODEDynamicSpaceID);
n_assert(ODEStaticSpaceID);
n_assert(ODECommonSpaceID);
for (int i = 0; i < Shapes.Size(); i++) Shapes[i]->Detach();
Shapes.Clear();
for (int i = 0; i < Entities.Size(); i++) Entities[i]->OnRemovedFromLevel();
Entities.Clear();
// delete the Contact group for joints
dJointGroupDestroy(ContactJointGroup);
// shutdown ode
dSpaceDestroy(ODEDynamicSpaceID);
dSpaceDestroy(ODEStaticSpaceID);
dSpaceDestroy(ODECommonSpaceID);
dWorldDestroy(ODEWorldID);
dCloseODE();
ODECommonSpaceID = NULL;
ODEDynamicSpaceID = NULL;
ODEStaticSpaceID = NULL;
ODEWorldID = NULL;
}
int main (int argc, char **argv)
{
doFast = true;
// 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;
dInitODE2(0);
bodies = 0;
joints = 0;
boxes = 0;
spheres = 0;
resetSimulation();
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
ODEDomain::~ODEDomain()
{
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
//deleting Heightfields starting from the end
for (int i=heightfields.size()-1; i>=0; i--)
DeleteHeightfield(i);
heightfields.clear();
//deleting trimeshes starting from the end
for (int i=trimeshes.size()-1; i>=0; i--)
DeleteTriMesh(i);
trimeshes.clear();
//deleting bodies starting from the end
for (int i=bodies.size()-1; i>=0; i--)
DeleteBody(i);
bodies.clear();
//deleting Kinematic_bodies starting from the end
for (int i=kinematic_bodies.size()-1; i>=0; i--)
DeleteKinematicBody(i);
kinematic_bodies.clear();
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
printf("ODEDomain destructor\n");
}
CPhysicManager::~CPhysicManager(){
dWorldDestroy( world );
//dHashSpaceDestroy( space );
dJointGroupDestroy( contactgroup );
LOGGER->LogMsg("-CPhysicManager");
};
void physics_quit (void)
{
printlog(1, "Quit physics");
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
}
void Simulator_Destroy(void) {
dGeomDestroy(ground);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
}
HarrierSim::~HarrierSim()
{
dSpaceDestroy(space);
dWorldDestroy(world);
delete vp;
delete itsWorldDisp;
pthread_mutex_destroy(&itsDispLock);
}
WorldManagerServer::~WorldManagerServer()
{
dSpaceDestroy(mStaticSpace);
dJointGroupEmpty(mContactGroup);
dJointGroupDestroy(mContactGroup);
dWorldDestroy(mWorld);
dCloseODE();
}
int main (int argc, char **argv)
{
// 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
dInitODE();
world = dWorldCreate();
#if 1
space = dHashSpaceCreate (0);
#elif 0
dVector3 center = {0,0,0}, extents = { 100, 100, 100};
space = dQuadTreeSpaceCreate(0, center, extents, 5);
#elif 0
space = dSweepAndPruneSpaceCreate (0, dSAP_AXES_XYZ);
#else
space = dSimpleSpaceCreate(0);
#endif
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
dWorldSetLinearDamping(world, 0.00001);
dWorldSetAngularDamping(world, 0.005);
dWorldSetMaxAngularSpeed(world, 200);
dWorldSetContactSurfaceLayer (world,0.001);
ground = dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
// create lift platform
platform = dCreateBox(space, 4, 4, 1);
dGeomSetCategoryBits(ground, 1ul);
dGeomSetCategoryBits(platform, 2ul);
dGeomSetCollideBits(ground, ~2ul);
dGeomSetCollideBits(platform, ~1ul);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
int main (int argc, char **argv)
{
// 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 = dSimpleSpaceCreate(0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
// note: can't share tridata if intending to trimesh-trimesh collide
TriData1 = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(TriData1, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
TriData2 = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(TriData2, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
TriMesh1 = dCreateTriMesh(space, TriData1, 0, 0, 0);
TriMesh2 = dCreateTriMesh(space, TriData2, 0, 0, 0);
dGeomSetData(TriMesh1, TriData1);
dGeomSetData(TriMesh2, TriData2);
{dGeomSetPosition(TriMesh1, 0, 0, 0.9);
dMatrix3 Rotation;
dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
dGeomSetRotation(TriMesh1, Rotation);}
{dGeomSetPosition(TriMesh2, 1, 0, 0.9);
dMatrix3 Rotation;
dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
dGeomSetRotation(TriMesh2, Rotation);}
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void endWorldModelling()
{
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
printf("God knows his rules and he has determined that this world must be terminated.\n");
printf("The World has been terminated.\n");
}
Physics::~Physics(){
#ifdef ODE
if(worldId!=NULL){
dWorldDestroy(worldId);
}
dCloseODE();
#endif
}
ODESimulator::~ODESimulator()
{
dJointGroupDestroy(contactGroupID);
for(size_t i=0;i<envMeshes.size();i++)
delete envMeshes[i];
for(size_t i=0;i<robots.size();i++)
delete robots[i];
dSpaceDestroy(envSpaceID);
dWorldDestroy(worldID);
}
Simulation::~Simulation()
{
// delete everything
dGeomDestroy(ground);
delete car;
dJointGroupDestroy(contactgroup);
dSpaceDestroy(space);
dWorldDestroy(world);
}
OdeInit::~OdeInit()
{
delete _wrld;
delete _iCub;
delete[] _controls;
dGeomDestroy(ground);
dJointGroupDestroy(contactgroup);
dSpaceDestroy(space);
dWorldDestroy(world);
}
/**
* \brief This functions destroys the ode world.
*
* pre:
* - world_init = true
*
* post:
* - world, space and contactgroup have to be destroyed here
* - afte that, world_init have to become false
*/
void WorldPhysics::freeTheWorld(void) {
MutexLocker locker(&iMutex);
if(world_init) {
//LOG_DEBUG("free physics world");
dJointGroupDestroy(contactgroup);
dSpaceDestroy(space);
dWorldDestroy(world);
world_init = 0;
}
// else debug something
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.stop = 0;
fn.command = 0;
fn.path_to_textures = "../../drawstuff/textures";
dInitODE ();
// create world
world = dWorldCreate ();
space = dHashSpaceCreate (0);
dWorldSetGravity (world,0,0,0); //Original Gravity = -0.2
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
contactgroup = dJointGroupCreate (0);
// create object
sphere0 = dBodyCreate (world);
sphere0_geom = dCreateSphere (space,0.5);
dMassSetSphere (&m,1,0.5);
dBodySetMass (sphere0,&m);
dGeomSetBody (sphere0_geom,sphere0);
sphere1 = dBodyCreate (world);
sphere1_geom = dCreateSphere (space,0.5);
dMassSetSphere (&m,1,0.5);
dBodySetMass (sphere1,&m);
dGeomSetBody (sphere1_geom,sphere1);
sphere2 = dBodyCreate (world);
sphere2_geom = dCreateSphere (space,0.5);
dMassSetSphere (&m,1,0.5);
dBodySetMass (sphere2,&m);
dGeomSetBody (sphere2_geom,sphere2);
// set initial position
dBodySetPosition (sphere0,0,0,4);
dBodySetPosition (sphere1,5,0,4);
dBodySetPosition (sphere2,-2,0,4);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
// clean up
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
int main (int argc, char **argv)
{
// set for drawing
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = NULL;
fn.stop = NULL;
fn.path_to_textures = "../textures";
dInitODE(); // init ODE
world = dWorldCreate(); // create a dynamic world
dWorldSetGravity(world,0,0,-0.1);
dMass m; // a parameter for mass
dMassSetZero (&m); // initialize the parameter
//@a sphere
sphere.body = dBodyCreate (world); // create a rigid body
dReal radius = 0.5; // radius [m]
dMassSetSphere (&m,DENSITY,radius); // calculate a mass parameter for a sphere
dBodySetMass (sphere.body,&m); // set the mass parameter to the body
dBodySetPosition (sphere.body,0,1, 1); // set the position of the body
//@a box
box.body = dBodyCreate (world);
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
dBodySetMass (box.body,&m);
dBodySetPosition (box.body,0,2,1);
// a capsule
capsule.body = dBodyCreate (world);
dMassSetCapsule(&m,DENSITY,3,radius,length);
dBodySetMass (capsule.body,&m);
dBodySetPosition (capsule.body,0,4,1);
// a cylinder
cylinder.body = dBodyCreate (world);
dMassSetCylinder(&m,DENSITY,3,radius,length);
dBodySetMass (cylinder.body,&m);
dBodySetPosition (cylinder.body,0,3,1);
// do the simulation
dsSimulationLoop (argc,argv,960,480,&fn);
dWorldDestroy (world); // destroy the world
dCloseODE(); // close ODE
return 0;
}
int main (int argc, char **argv)
{
int i;
dReal k;
dMass m;
/* setup pointers to drawstuff callback functions */
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = 0;
fn.stop = 0;
fn.path_to_textures = "../../drawstuff/textures";
if(argc==2)
{
fn.path_to_textures = argv[1];
}
/* create world */
dInitODE();
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (1000000);
dWorldSetGravity (world,0,0,-0.5);
dCreatePlane (space,0,0,1,0);
for (i=0; i<NUM; i++) {
body[i] = dBodyCreate (world);
k = i*SIDE;
dBodySetPosition (body[i],k,k,k+0.4);
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
dBodySetMass (body[i],&m);
sphere[i] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[i],body[i]);
}
for (i=0; i<(NUM-1); i++) {
joint[i] = dJointCreateBall (world,0);
dJointAttach (joint[i],body[i],body[i+1]);
k = (i+0.5)*SIDE;
dJointSetBallAnchor (joint[i],k,k,k+0.4);
}
/* run simulation */
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}