Beispiel #1
0
/* ------------------------
* メイン関数
------------------------ */
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;
}
Beispiel #4
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;


}
Beispiel #5
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;
}
Beispiel #6
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;
}
Beispiel #8
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;
}
Beispiel #10
0
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;
}
Beispiel #12
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;
}
Beispiel #14
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");
};
Beispiel #16
0
void physics_quit (void)
{
	printlog(1, "Quit physics");
	dJointGroupDestroy (contactgroup);
	dSpaceDestroy (space);
	dWorldDestroy (world);
	dCloseODE();
}
Beispiel #17
0
void Simulator_Destroy(void) {

	dGeomDestroy(ground);
	dJointGroupDestroy (contactgroup);
	dSpaceDestroy (space);
	dWorldDestroy (world);
	dCloseODE();
}
Beispiel #18
0
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();
}
Beispiel #20
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;

    // 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;
}
Beispiel #22
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");
}
Beispiel #23
0
Physics::~Physics(){

#ifdef ODE
	if(worldId!=NULL){
		dWorldDestroy(worldId);
	}

	dCloseODE();
#endif
}
Beispiel #24
0
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);
}
Beispiel #25
0
Simulation::~Simulation()
{
	// delete everything

	dGeomDestroy(ground);

	delete car;
	dJointGroupDestroy(contactgroup);
	dSpaceDestroy(space);
	dWorldDestroy(world);
}
Beispiel #26
0
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;
}
Beispiel #29
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;
}
Beispiel #30
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;
}