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;
}

int main (int argc, char **argv)
{
    LOG(LEVEL_INFO) << "\n\nUnBall Robot Soccer Team"
                    << "\nSimulation Module\n";

    // Parte de comunicação
    Comunicador _comunicador ("Simulador");
    _com = & _comunicador;

    inicializarComunicacao();

    // Setup pointers to drawstuff callback functions
    initDrawStuff();

    construirMundo();

    LOG(LEVEL_INFO) << "Start simulation";

    // Run simulation
    dsSimulationLoop(argc,argv,352,288,&fn);

    // Destroy world
    destruirMundo();
    dCloseODE();

    return 0;
}

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;
}

/* ------------------------
* メイン関数
------------------------ */
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;
}

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");   
}

// 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()
{
  dInitODE();
  testRandomNumberGenerator();
  testInfinity();
  testPad();
  testCrossProduct();
  testSetZero();
  testNormalize3();
  //testReorthonormalize();     ... not any more
  testPlaneSpace();
  testMatrixMultiply();
  testSmallMatrixMultiply();
  testCholeskyFactorization();
  testCholeskySolve();
  testInvertPDMatrix();
  testIsPositiveDefinite();
  testFastLDLTFactorization();
  testSolveLDLT();
  testLDLTAddTL();
  testLDLTRemove();
  testMassFunctions();
  testRtoQandQtoR();
  testQuaternionMultiply();
  testRotationFunctions();
  dTestMatrixComparison();
  dTestSolveLCP();
  // dTestDataStructures();
  dCloseODE();
  return 0;
}

 /**
  * \brief Close ODE environment
  *
  * pre:
  *     - none
  *
  * post:
  *     - everthing that was created should be destroyed
  *
  */
 WorldPhysics::~WorldPhysics(void) {
   // free the ode objects
   freeTheWorld();
   // and close the ODE ...
   MutexLocker locker(&iMutex);
   dCloseODE();
 }

void PhysWorld::DeInitialize()
{
    dSpaceDestroy(mSpace);
    dWorldDestroy(mWorld);
    dCloseODE();
	isInitialized = false;
}

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;
}

int main (int argc, char **argv)
{
  int i;
  dInitODE();

  // process the command line args. anything that starts with `-' is assumed
  // to be a drawstuff argument.
  for (i=1; i<argc; i++) {
    if ( argv[i][0]=='-' && argv[i][1]=='i' && argv[i][2]==0) cmd_interactive = 1;
    else if ( argv[i][0]=='-' && argv[i][1]=='g' && argv[i][2]==0) cmd_graphics = 0;
    else if ( argv[i][0]=='-' && argv[i][1]=='e' && argv[i][2]==0) cmd_graphics = 0;
    else if ( argv[i][0]=='-' && argv[i][1]=='n' && isdigit(argv[i][2]) ) {
      char *endptr;
      long int n = strtol (&(argv[i][2]),&endptr,10);
			if (*endptr == 0) cmd_test_num = n;
		}
    else
      cmd_path_to_textures = argv[i];
  }

  // do the tests
  if (cmd_test_num == -1) {
    for (i=0; i<NUM_JOINTS*100; i++) doTest (argc,argv,i,0);
  }
  else {
    doTest (argc,argv,cmd_test_num,1);
  }

  dCloseODE();
  return 0;
}

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;
}

int main (int argc, char **argv)
{
  // setup all tests

  memset (testslot,0,sizeof(testslot));
  dInitODE2(0);

  MAKE_TEST(1,test_sphere_point_depth);
  MAKE_TEST(2,test_box_point_depth);
  MAKE_TEST(3,test_ccylinder_point_depth);
  MAKE_TEST(4,test_plane_point_depth);

  MAKE_TEST(10,test_ray_and_sphere);
  MAKE_TEST(11,test_ray_and_box);
  MAKE_TEST(12,test_ray_and_ccylinder);
  MAKE_TEST(13,test_ray_and_plane);
  MAKE_TEST(14,test_ray_and_cylinder);

  MAKE_TEST(100,test_dBoxTouchesBox);
  MAKE_TEST(101,test_dBoxBox);

  do_tests (argc,argv);
  dCloseODE();
  return 0;
}

PWorld::~PWorld()
{
  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
  dCloseODE();
}

void Simulator_Destroy(void) {

	dGeomDestroy(ground);
	dJointGroupDestroy (contactgroup);
	dSpaceDestroy (space);
	dWorldDestroy (world);
	dCloseODE();
}

void physics_quit (void)
{
	printlog(1, "Quit physics");
	dJointGroupDestroy (contactgroup);
	dSpaceDestroy (space);
	dWorldDestroy (world);
	dCloseODE();
}

void BlackBoardApp::closeEvent(QCloseEvent *e)
{
    QVariant var(size());
    
	PrefManager::get().writeValue("mainWindowSize", var);

	// De-init ODE
	dCloseODE();
}

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;
  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;
}