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
}

void SkidSteeringVehicle::destroy() {
    dBodyDestroy(this->vehicleBody);
    dGeomDestroy(this->vehicleGeom);
    
    for(int fr = 0; fr < 2; fr++) {
        for(int lr = 0; lr < 2; lr++) {
            dBodyDestroy(this->wheelBody[fr][lr]);
            dGeomDestroy(this->wheelGeom[fr][lr]);
            dJointDestroy(this->wheelJoint[fr][lr]);
        }
    }
    
    dRigidBodyArrayDestroy(this->bodyArray);
}

void Machine::destroy(void)
{
	int i;
	dBodyDestroy(body[0]);
	dBodyDestroy(body[1]);
	dGeomDestroy(geom[0]);
	dJointDestroy(joint);
	for(i=0; i<2; i++)
		dGeomDestroy(geom[i+2]);
	for(i=0; i<3; i++)
	{
		dBodyDestroy(wheel[i]);
		dGeomDestroy(sphere[i]);
	}
}

//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
CollisionShape::~CollisionShape()
{
    if( m_geometryId != NULL )
    {
        dGeomDestroy( m_geometryId );
    }
}

	std::vector<RaycastResult> & ODESimulator::internal_fireRay(const Rayr& r,
	        real length, const Solid* attachedSolid,
	        unsigned int rayContactGroup)
	{
		Point3r origin = r.getOrigin();
		Vec3r dir = r.getDir().unit();

		mRaycastResults.clear();
		mSensorSolid = attachedSolid;
		mRayContactGroup = rayContactGroup;

		// Create an ODE ray geom.  Make sure its user data pointer is
		// NULL because this is used in the collision callback to
		// distinguish the ray from other geoms.
		dGeomID rayGeomID = dCreateRay(mRootSpaceID, length);
		dGeomRaySet(rayGeomID, origin[0], origin[1], origin[2], dir[0],
		             dir[1], dir[2]);
		dGeomSetData(rayGeomID, NULL);

		// Check for collisions.  This will fill mRaycastResult with valid
		// data.  Its Solid pointer will remain NULL if nothing was hit.
		dSpaceCollide2(rayGeomID, (dGeomID) mRootSpaceID, this,
		                &ode_hidden::internal_raycastCollisionCallback);

		// Finished with ODE ray, so destroy it.
		dGeomDestroy(rayGeomID);

		return mRaycastResults;
	}

void stop()
{
    dGeomDestroy(mesh_geom);
    dGeomTriMeshDataDestroy(mesh_data);

    dBodyDestroy(ball1_body);
    dBodyDestroy(ball2_body);

    dGeomDestroy(ground);

    dJointGroupDestroy(contact_group);

    dSpaceDestroy(space); // will destroy all geoms

    dWorldDestroy(world);
}

void	CPHGeometryOwner::	DestroyGroupSpace()
{
	if(m_group){
		dGeomDestroy((dGeomID)m_group);
		m_group=NULL;
	}
}

 void cPhysicsObject::DestroyGeom()
 {
   if (geom != NULL) {
     dGeomDestroy(geom);
     geom = NULL;
   }
 }

void ComponentPhysicsGeom::load(const PropertyBag &data)
{
	resetMembers();

	desiredHeight = data.getFloat("height");
	collisionRadius = data.getFloat("radius");

	// Create as physics geometry
	if(geom){dGeomDestroy(geom);} geom=0;
	createGeom(data.getString("physicsGeometryType"));

	// Set initial position
	{
		vec3 position;
		if(data.get("position", position)) // optional tag
		{
			setPosition(position);
		}
	}

	// Declare the initial state
	getParentBlackBoard().relayMessage(MessagePositionHasBeenSet(getPosition()));
	getParentBlackBoard().relayMessage(MessageOrientationHasBeenSet(getOrientation()));
	getParentBlackBoard().relayMessage(MessageRequestSetHeight(desiredHeight));
}

LaserBeam::~LaserBeam()
{
    dGeomDestroy(geom);
    dBodyDestroy(me);

    printf("Good bye....\n");
}

//==============================================================================
OdeMesh::~OdeMesh()
{
  dGeomDestroy(mGeomId);

  if (mOdeTriMeshDataId)
    dGeomTriMeshDataDestroy(mOdeTriMeshDataId);
}

Body* SimObjectRenderer::selectObject(const Vector3<>& projectedClick)
{
  if(&simObject != Simulation::simulation->scene)
    return nullptr;

  class Callback
  {
  public:
    Body* closestBody;
    float closestSqrDistance;
    const Vector3<>& cameraPos;

    Callback(const Vector3<>& cameraPos) : closestBody(0), cameraPos(cameraPos) {}

    static void staticCollisionCallback(Callback* callback, dGeomID geom1, dGeomID geom2)
    {
      ASSERT(!dGeomIsSpace(geom1));
      ASSERT(!dGeomIsSpace(geom2));
      ASSERT(dGeomGetBody(geom1) || dGeomGetBody(geom2));
      dContact contact[1];
      if(dCollide(geom1, geom2, 1, &contact[0].geom, sizeof(dContact)) < 1)
        return;

      dGeomID geom = geom2;
      dBodyID bodyId = dGeomGetBody(geom2);
      if(!bodyId)
      {
        bodyId = dGeomGetBody(geom1);
        geom = geom1;
      }
      const dReal* pos = dGeomGetPosition(geom);
      float sqrDistance = (Vector3<>((float) pos[0], (float) pos[1], (float) pos[2]) - callback->cameraPos).squareAbs();
      if(!callback->closestBody || sqrDistance < callback->closestSqrDistance)
      {
        callback->closestBody = (Body*)dBodyGetData(bodyId);
        callback->closestSqrDistance = sqrDistance;
      }
    }

    static void staticCollisionWithSpaceCallback(Callback* callback, dGeomID geom1, dGeomID geom2)
    {
      ASSERT(!dGeomIsSpace(geom1));
      ASSERT(dGeomIsSpace(geom2));
      dSpaceCollide2(geom1, geom2, callback, (dNearCallback*)&staticCollisionCallback);
    }
  };

  Callback callback(cameraPos);
  dGeomID ray = dCreateRay(Simulation::simulation->staticSpace, 10000.f);
  Vector3<> dir = projectedClick - cameraPos;
  dGeomRaySet(ray, cameraPos.x, cameraPos.y, cameraPos.z, dir.x, dir.y, dir.z);
  dSpaceCollide2(ray, (dGeomID)Simulation::simulation->movableSpace, &callback, (dNearCallback*)&Callback::staticCollisionWithSpaceCallback);
  dGeomDestroy(ray);

  if(!callback.closestBody)
    return nullptr;
  Body* body = callback.closestBody;
  return body->rootBody;
}

MyODEGeom::~MyODEGeom() {
    if (mBody) {
        dBodyDestroy(mBody);
    }
    if (mODEGeom) {
        dGeomDestroy(mODEGeom);
    }
}

// ジオメトリのみの物体を破壊する関数。例：ブロック崩しのブロック等
void dmDestroyBox0(dmObject *obj)
{
	if (obj->geom != NULL)  // ジオメトリが存在していれば
	{
		dGeomDestroy(obj->geom); // ジオメトリを破壊
		obj->geom = NULL;            // ポインタにNULLを設定
	}
}

void CShape::Detach()
{
	n_assert(IsAttached());
	n_assert(ODEGeomID);
	dGeomDestroy(ODEGeomID);
	ODEGeomID = NULL;
	ODESpaceID = NULL;
}

ODE_Particle::~ODE_Particle()
{
    /*printf("Particle=%u destructor!\n",id);
    fflush(stdout);*/
        
    dBodyDestroy(body);
    dGeomDestroy(geom);   
}

CollidableObject::~CollidableObject()
{
	//Disconnect any objects in the scene
	mSceneNode->removeAndDestroyAllChildren();
	mSceneNode->getParentSceneNode()->removeAndDestroyChild(mSceneNode->getName());
	if(isInWorld)
		dGeomDestroy(mGeom);
}

void Simulator_Destroy(void) {

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

 Primitive::~Primitive () {
   QMP_CRITICAL(8);
   // 20091023; guettler:
   // hack for tasked simulations; there are some problems if running in parallel mode,
   // if you do not destroy the geom, everything is fine (should be no problem because world is destroying geoms too)
   if(destroyGeom && geom) dGeomDestroy( geom );
   if(body && ((mode & _Transform) == 0) ) dBodyDestroy( body );
   QMP_END_CRITICAL(8);
 }

void BoxObstacle::remove() {
	dGeomDestroy(boxGeom_);
	if (box_ != 0) {
		for(int i=0; i< dBodyGetNumJoints(box_); i++) {
			dJointDestroy(dBodyGetJoint(box_, i));
		}
		dBodyDestroy(box_);
	}
}