/*
   orxonox - the future of 3D-vertical-scrollers

   Copyright (C) 2004 orx

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2, or (at your option)
   any later version.

   ### File Specific:
   main-programmer: Benjamin Grauer
   co-programmer: ...
*/
#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_WORLD_ENTITY


#include "terrain.h"

#include "util/loading/resource_manager.h"
#include "util/loading/load_param.h"
#include "util/loading/factory.h"
#include "spatial_separation.h"

#include "model.h"
#include "static_model.h"
#include "static_model_data.h"
#include "network_game_manager.h"

#include "height_map.h"
#include "material.h"

#include "glincl.h"

#include "state.h"
#include "debug.h"
#include "aabb.h"
#include "cr_defs.h"
#include "cd_engine.h"

#include "class_id_DEPRECATED.h"
#include  <ode/ode.h>
ObjectListDefinitionID(Terrain, CL_TERRAIN);
CREATE_FACTORY(Terrain);

/**
 *  standard constructor
 */
Terrain::Terrain (const TiXmlElement* root)
{
  this->loaded = false;
  this->init();


  if( root != NULL)
    this->loadParams(root);


if(this->getModel())
	{

 	StaticModelData::Pointer   ModelData = ((StaticModel*)this->getModel())->dataPointer();
	//ModelData->buildTriangleList();

	PRINTF(0)(" Dieses Model hat %i Vertices \n", ModelData->getVertices().size());
	PRINTF(0)(" Dieses Model hat %i Dreiecke \n",((ModelData->getTriangles()).size()  ));
	this->Ind = new unsigned int [(ModelData->getTriangles()).size()*3];
	for(int i = 0 ; i < (ModelData->getTriangles()).size(); i++)
		{
			Ind[3*i]    = (ModelData->getTrianglesExt())[i].indexToVertices[0] / 3 ;
			Ind[3*i +1] = (ModelData->getTrianglesExt())[i].indexToVertices[1] / 3;
			Ind[3*i +2] = (ModelData->getTrianglesExt())[i].indexToVertices[2] / 3;
		
		}
	
	//((StaticModel*)(this->getModel()))->acquireData(ModelData);
	this->ODE_Geometry = dGeomTriMeshDataCreate();
	dGeomTriMeshDataBuildSingle(this->ODE_Geometry,&((ModelData->getVertices())[0]), 3*sizeof(float),(ModelData->getVertices().size() /3 )  , &Ind[0] ,((ModelData->getTriangles()).size() *3 ),  3*sizeof(unsigned int) );
    // Create ODE-Data here!
	
	this->world =  dWorldCreate();
 	this->space = dSimpleSpaceCreate(0);
 
   	//!fixme
  	for (int i=0; i<30; i++) {
    		contact[i].surface.mode = dContactBounce | dContactSoftCFM;
    		contact[i].surface.mu = dInfinity;
    		contact[i].surface.mu2 = 0;
    		contact[i].surface.bounce = 0.1;
    		contact[i].surface.bounce_vel = 0.1;
    		contact[i].surface.soft_cfm = 0.01;
  	}

	contactgroup = dJointGroupCreate (0);
 	dWorldSetGravity (world,0,0,-0.5);
  	dWorldSetCFM (world,1e-5);

	this->ODE_Geom_ID = dCreateTriMesh(space,this->ODE_Geometry,0 , 0, 0);

		
	CDEngine::getInstance()->setTerrain(this);
	this->loaded = true;
	this->toList(OM_ENVIRON);
}
  //  if (this->model != NULL)
  //this->ssp = new SpatialSeparation((Model*)this->model, 10.0f);
}


/**
 *  Constructor for loading a Terrain out of a file
 * @param fileName The file to load data from.

   this either loads out of an OBJ-file, or loads a heightmap if no .obj-extension is found.
*/
Terrain::Terrain(const std::string& fileName)
{
  this->loaded = false;
  this->init();

  if (fileName.rfind(".obj" ) != std::string::npos || fileName.rfind(".OBJ") != std::string::npos )
  {
    this->loadModel(fileName);

	
  }
  else
  {
    // load the hightMap here.
  }
}

/**
 *  a Constructor for the Debug-Worlds
 */
Terrain::Terrain(DebugTerrain debugTerrain)
{
  this->init();
  this->buildDebugTerrain(debugTerrain);
}

/**
 *  standard deconstructor

*/
Terrain::~Terrain ()
{
  if (modelList)
    glDeleteLists(this->modelList, 1);
  if( this->ssp)
    delete ssp;

  if(this->heightMap)
    delete heightMap;
}


void Terrain::init()
{
  this->registerObject(this, Terrain::_objectList);
  this->toList(OM_ENVIRON_NOTICK);
  this->toReflectionList();

  this->modelList = 0;
  this->ssp = NULL;
  this->vegetation = NULL;

  this->heightMap = NULL;

  this->heightMapMaterial = new Material();


}


void Terrain::loadParams(const TiXmlElement* root)
{
  WorldEntity::loadParams(root);

  LoadParam(root, "scale", this, Terrain, setScale)
  .describe("The scale in x,y,z direction");

  LoadParam(root, "texture", this, Terrain, loadTexture)
  .describe("The name of the Texture for this heightMap");

  LoadParam(root, "vegetation", this, Terrain, loadVegetation)
  .describe("the fileName of the vegetation, that should be loaded onto this terrain. (must be relative to the data-dir)") ;

  LoadParam(root, "height-map", this, Terrain, loadHeightMap)
  .describe("The HeightMap, splitted into two strings seperated by ','. 1: HeighMap, 2: ColorMap");

}

void Terrain::setScale(float x, float y, float z)
{
  this->terrainScale = Vector(x, y, z);
}

void Terrain::loadHeightMap(const std::string& heightMapFile, const std::string& colorMap)
{
  if (this->heightMap != NULL)
    delete this->heightMap;
  this->heightMap = NULL;

  std::string hmName = Resources::ResourceManager::getInstance()->prependAbsoluteMainPath(heightMapFile);
  std::string hmColorName = Resources::ResourceManager::getInstance()->prependAbsoluteMainPath(colorMap);


  this->heightMap = new HeightMap(hmName, hmColorName);
  //   heightMap->scale(Vector(43.0f,4.7f,43.0f));
  heightMap->scale(this->terrainScale);
  heightMap->setAbsCoor(this->getAbsCoor());
  heightMap->load();
}


void Terrain::loadTexture(const std::string& textureName)
{
  PRINTF(4)("Load texture: %s\n", textureName.c_str());

  heightMapMaterial->setDiffuse(1.0,1.0,1.0);
  heightMapMaterial->setAmbient(1.0,1.0,1.0 );
  heightMapMaterial->setSpecular(1.0,1.0,1.0);
  heightMapMaterial->setShininess(.5);
  heightMapMaterial->setTransparency(1.0);

  heightMapMaterial->setDiffuseMap(textureName);
  //   heightMapMaterial->setAmbientMap(textureName);
  //   heightMapMaterial->setSpecularMap(textureName);
}



void Terrain::loadVegetation(const std::string& vegetationFile)
{
  PRINTF(4)("loadVegetation: %s\n", vegetationFile.c_str());
  if (this->vegetation)
    this->vegetation = 0;
  if (!vegetationFile.empty())
  {
    PRINTF(4)("fetching %s\n", vegetationFile.c_str());
    this->loadModel(vegetationFile, 1.0, 2);
    this->vegetation = this->getModel(2);
  }
  else
    this->vegetation = NULL;
}





void Terrain::draw () const
{
  glPushMatrix();

  /* translate */
  glTranslatef (this->getAbsCoor ().x,
                this->getAbsCoor ().y,
                this->getAbsCoor ().z);
  /* rotate */
  // Vector tmpRot = this->getAbsDir().getSpacialAxis();
  //glRotatef (this->getAbsDir().getSpacialAxisAngle(), tmpRot.x, tmpRot.y, tmpRot.z );

  if (this->modelList)
    glCallList(this->modelList);
  else if (this->getModel())
    this->getModel()->draw();

  if (this->vegetation)
    this->vegetation->draw();

  if( this->heightMap)
  {
    this->heightMapMaterial->select();

    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
    this->heightMap->draw();
  }
  glPopMatrix();


  /*
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glLoadIdentity();
    Vector camera =   State::getCameraNode()->getAbsCoor(); // Go on here ..........!!!

    float height =    heightMap->getHeight(camera.x, camera.z);

    glEnable (GL_COLOR_MATERIAL) ;
    glBegin(GL_QUADS);            // Draw The Cube Using quads
    glColor3f(0.0f,1.0f,0.0f);  // Color Blue
    glVertex3f(camera.x + 63.0f,Terrain->getHeight(camera.x+63.0f, camera.z-10.0f)+13.0f,camera.z-10.0f);      // Top Right Of The Quad (Top)
    glVertex3f(camera.x-63.0f, getHeight(camera.x+63.0f, camera.z-10.0f)+13.0f,camera.z-10.0f);      // Top Left Of The Quad (Top)
    glVertex3f(camera.x-63.0f, getHeight(camera.x+63.0f, camera.z+10.0f)+13.0f, camera.z+10.0f);      // Bottom Left Of The Quad (Top)
    glVertex3f(camera.x+ 63.0f, getHeight(camera.x+63.0f, camera.z+10.0f)+13.0f, camera.z+10.0f);      // Bottom Right Of The Quad (Top)
    glEnd();                      // End Drawing The Plan

    glPopMatrix();*/


  /* THIS IS ONLY FOR DEBUGGING INFORMATION */
  //  if (this->ssp != NULL)
  //    this->ssp->drawQuadtree();
}


void Terrain::buildDebugTerrain(DebugTerrain debugTerrain)
{
  // if the terrain is the Terrain of Dave
  if (debugTerrain == TERRAIN_DAVE)
  {
    modelList = glGenLists(1);
    glNewList (modelList, GL_COMPILE);

    glColor3f(1.0,0,0);

    int sizeX = 100;
    int sizeZ = 80;
    float length = 1000;
    float width = 200;
    float widthX = float (length /sizeX);
    float widthZ = float (width /sizeZ);

    float height [sizeX][sizeZ];
    Vector normal_vectors[sizeX][sizeZ];


    for ( int i = 0; i<sizeX-1; i+=1)
      for (int j = 0; j<sizeZ-1;j+=1)
        //height[i][j] = rand()/20046 + (j-25)*(j-25)/30;
#ifdef __WIN32__
        height[i][j]=(sin((float)j/3)*rand()*i/182400)*.5;
#else
        height[i][j]=(sin((float)j/3)*rand()*(long)i/6282450500.0)*.5;
#endif

    //Die Huegel ein wenig glaetten
    for (int h=1; h<2;h++)
      for (int i=1;i<sizeX-2 ;i+=1 )
        for(int j=1;j<sizeZ-2;j+=1)
          height[i][j]=(height[i+1][j]+height[i][j+1]+height[i-1][j]+height[i][j-1])/4;

    //Berechnung von normalen Vektoren
    for(int i=1;i<sizeX-2;i+=1)
      for(int j=1;j<sizeZ-2 ;j+=1)
      {
        Vector v1 = Vector (widthX*(1),      height[i][j],      widthZ*(j) );
        Vector v2 = Vector (widthX*(i-1),    height[i-1][j],    widthZ*(j));
        Vector v3 = Vector (widthX*(i),      height[i][j+1],    widthZ*(j+1));
        Vector v4 = Vector (widthX*(i+1),    height[i+1][j],    widthZ*(j));
        Vector v5 = Vector (widthX*(i),      height[i][j-1],    widthZ*(j-1));

        Vector c1 = v2 - v1;
        Vector c2 = v3 - v1;
        Vector c3=  v4 - v1;
        Vector c4 = v5 - v1;
        Vector zero = Vector (0,0,0);
        normal_vectors[i][j]=c1.cross(v3-v5)+c2.cross(v4-v2)+c3.cross(v5-v3)+c4.cross(v2-v4);
        normal_vectors[i][j].normalize();
      }

    glBegin(GL_QUADS);
    int snowheight=3;
    for ( int i = 0; i<sizeX; i+=1)
      for (int j = 0; j<sizeZ;j+=1)
      {
        Vector v1 = Vector (widthX*(i),      height[i][j]-20,       widthZ*(j)  -width/2);
        Vector v2 = Vector (widthX*(i+1),    height[i+1][j]-20,     widthZ*(j)  -width/2);
        Vector v3 = Vector (widthX*(i+1),    height[i+1][j+1]-20,   widthZ*(j+1)-width/2);
        Vector v4 = Vector (widthX*(i),      height[i][j+1]-20,     widthZ*(j+1)-width/2);
        float a[3];
        if(height[i][j]<snowheight)
        {
          a[0]=0;
          a[1]=1.0-height[i][j]/10-.3;
          a[2]=0;
          glMaterialfv(GL_FRONT,GL_DIFFUSE,a);
        }
        else
        {
          a[0]=1.0;
          a[1]=1.0;
          a[2]=1.0;
          glMaterialfv(GL_FRONT,GL_DIFFUSE,a);

        }
        glNormal3f(normal_vectors[i][j].x, normal_vectors[i][j].y, normal_vectors[i][j].z);
        glVertex3f(v1.x, v1.y, v1.z);
        if(height[i+1][j]<snowheight)
        {
          a[0]=0;
          a[1] =1.0-height[i+1][j]/10-.3;
          a[2]=0;
          glMaterialfv(GL_FRONT,GL_DIFFUSE,a);
        }
        else
        {
          a[0]=1.0;
          a[1]=1.0;
          a[2]=1.0;
          glMaterialfv(GL_FRONT,GL_DIFFUSE,a);

        }
        glNormal3f(normal_vectors[i+1][j].x, normal_vectors[i+1][j].y, normal_vectors[i+1][j].z);
        glVertex3f(v2.x, v2.y, v2.z);
        if(height[i+1][j+1]<snowheight)
        {
          a[0]=0;
          a[1] =1.0-height[i+1][j+1]/10-.3;
          a[2]=0;
          glMaterialfv(GL_FRONT,GL_DIFFUSE,a);
        }
        else
        {
          a[0]=1.0;
          a[1]=1.0;
          a[2]=1.0;
          glMaterialfv(GL_FRONT,GL_DIFFUSE,a);


        }
        glNormal3f(normal_vectors[i+1][j+1].x, normal_vectors[i+1][j+1].y, normal_vectors[i+1][j+1].z);
        glVertex3f(v3.x, v3.y, v3.z);
        if(height[i][j+1]<snowheight)
        {
          a[0]=0;
          a[1] =1.0-height[i+1][j+1]/10-.3;
          a[2]=0;
          glMaterialfv(GL_FRONT,GL_DIFFUSE,a);
        }
        else
        {
          a[0]=1.0;
          a[1]=1.0;
          a[2]=1.0;
          glMaterialfv(GL_FRONT,GL_DIFFUSE,a);
        }
        glNormal3f(normal_vectors[i][j+1].x, normal_vectors[i][j+1].y, normal_vectors[i][j+1].z);
        glVertex3f(v4.x, v4.y, v4.z);

      }
    glEnd();
    glEndList();
  }

  if (debugTerrain == TERRAIN_BENSCH)
  {
    /*
      this->model = (OBJModel*) new Model();
    this->model->setName("CUBE");
    this->model->addVertex (-0.5, -0.5, 0.5);
    this->model->addVertex (0.5, -0.5, 0.5);
    this->model->addVertex (-0.5, 0.5, 0.5);
    this->model->addVertex (0.5, 0.5, 0.5);
    this->model->addVertex (-0.5, 0.5, -0.5);
    this->model->addVertex (0.5, 0.5, -0.5);
    this->model->addVertex (-0.5, -0.5, -0.5);
    this->model->addVertex (0.5, -0.5, -0.5);

    this->model->addVertexTexture (0.0, 0.0);
    this->model->addVertexTexture (1.0, 0.0);
    this->model->addVertexTexture (0.0, 1.0);
    this->model->addVertexTexture (1.0, 1.0);
    this->model->addVertexTexture (0.0, 2.0);
    this->model->addVertexTexture (1.0, 2.0);
    this->model->addVertexTexture (0.0, 3.0);
    this->model->addVertexTexture (1.0, 3.0);
    this->model->addVertexTexture (0.0, 4.0);
    this->model->addVertexTexture (1.0, 4.0);
    this->model->addVertexTexture (2.0, 0.0);
    this->model->addVertexTexture (2.0, 1.0);
    this->model->addVertexTexture (-1.0, 0.0);
    this->model->addVertexTexture (-1.0, 1.0);

    this->model->finalize();
    */
  }
}

float Terrain::getHeight(float x, float y)
{
  if(this->heightMap != NULL)
    return (this->heightMap->getHeight(x, y));
  return 0;
}

void Terrain::go()
	{
	PRINTF(0)("It's me!   :-)");
	}

void Terrain::checkCollisionTerrain(WorldEntity* worldEntity) {
	if(!this->loaded) return;
	
	 AABB* box = worldEntity->getModelAABB();
   dReal  aabbox [6];

if(  box != NULL)
{
	
	dGeomID RayX =    dCreateRay(space,box->halfLength[0] *2.0f);
	dGeomRaySet (RayX, worldEntity->getAbsCoor().x  - box->halfLength[0]  ,worldEntity->getAbsCoor().y, worldEntity->getAbsCoor().z,
                  1.0f, 0.0f, 0.0f); 
	dGeomID RayY =    dCreateRay(space,box->halfLength[1] *2.0f);
	dGeomRaySet (RayY, worldEntity->getAbsCoor().x    ,worldEntity->getAbsCoor().y - box->halfLength[1], worldEntity->getAbsCoor().z,
                  0.0f, 1.0f, 0.0f); 
	dGeomID RayZ =    dCreateRay(space,box->halfLength[2] *2.0f);
	dGeomRaySet (RayZ, worldEntity->getAbsCoor().x    ,worldEntity->getAbsCoor().y, worldEntity->getAbsCoor().z - box->halfLength[2],
                  0.0f, 0.0f, 1.0f); 

	dGeomID RayXPos =    dCreateRay(space,box->halfLength[0]);
	dGeomRaySet (RayX, worldEntity->getAbsCoor().x    ,worldEntity->getAbsCoor().y, worldEntity->getAbsCoor().z,
                  1.0f, 0.0f, 0.0f); 
	dGeomID RayYPos =    dCreateRay(space,box->halfLength[1] );
	dGeomRaySet (RayY, worldEntity->getAbsCoor().x    ,worldEntity->getAbsCoor().y, worldEntity->getAbsCoor().z,
                  0.0f, 1.0f, 0.0f); 
	dGeomID RayZPos =    dCreateRay(space,box->halfLength[2] );
	dGeomRaySet (RayZ, worldEntity->getAbsCoor().x    ,worldEntity->getAbsCoor().y, worldEntity->getAbsCoor().z,
                  0.0f, 0.0f, 1.0f); 

	dGeomID RayXNeg =    dCreateRay(space,box->halfLength[0] * 40.0f );
	dGeomRaySet (RayX, worldEntity->getAbsCoor().x    ,worldEntity->getAbsCoor().y, worldEntity->getAbsCoor().z,
                  -1.0f, 0.0f, 0.0f); 
	dGeomID RayYNeg =    dCreateRay(space,box->halfLength[1] );
	dGeomRaySet (RayY, worldEntity->getAbsCoor().x    ,worldEntity->getAbsCoor().y , worldEntity->getAbsCoor().z,
                  0.0f, -1.0f, 0.0f); 
	dGeomID RayZNeg =    dCreateRay(space,box->halfLength[2] );
	dGeomRaySet (RayZ, worldEntity->getAbsCoor().x    ,worldEntity->getAbsCoor().y, worldEntity->getAbsCoor().z ,
                  0.0f, 0.0f, -1.0f); 

 
    	dGeomID BBOX = dCreateBox (space,box->halfLength[0]*2.0f, box->halfLength[1]*40.0f  ,2.0f* box->halfLength[2]);
    	//dGeomSetPosition (BBOX, worldEntity->getAbsCoor().x  ,worldEntity->getAbsCoor().y, worldEntity->getAbsCoor().z);
   	dGeomSetPosition (BBOX, worldEntity->getAbsCoor().x + box->center.x ,worldEntity->getAbsCoor().y+box->center.y, worldEntity->getAbsCoor().z+box->center.z);



 
   int g;
   bool collision = false;

 
			// dGeomGetAABB (this->bspFile->ODE_Geom_IDs[i],aabbox );
			// BBOX2 = dCreateBox (space, aabbox[1]-aabbox[0], aabbox[3]-aabbox[2], aabbox[5]-aabbox[4]);
			//  dGeomSetPosition (BBOX2,(aabbox[1]+ aabbox[0])/2, (aabbox[3]+ aabbox[2])/2, (aabbox[4]+ aabbox[5])/2);
			// dGeomDestroy(BBOX2);
			 g = 0;
			 if(true || dCollide(this->ODE_Geom_ID,BBOX, 10, &contact[0].geom, sizeof(dContact)) ) {
			
				//PRINTF(0)("Collision \n");
				for(int i = 0; i <1 ; i++)
					{ 
				// worldEntity->registerCollision(COLLISION_TYPE_AXIS_Y_NEG , this, worldEntity, Vector(0.0f, 1.0f, 0.0f),
                                  //              Vector(contact[i].geom.pos[0],contact[i].geom.pos[1],contact[i].geom.pos[2]), false);
					}
					
					if(dCollide(this->ODE_Geom_ID,RayX, 1, &contact[0].geom, sizeof(dContact)))  {
						if(dCollide(this->ODE_Geom_ID,RayXPos, 1, &contact[0].geom, sizeof(dContact)))  {
						         worldEntity->registerCollision(COLLISION_TYPE_AXIS_X , this, worldEntity, Vector(1.0f, 0.0f, 0.0f),
                                                         Vector((float)contact[0].geom.pos[0],(float)contact[0].geom.pos[1],contact[0].geom.pos[2]), false);	
							
						}//if		
						if(dCollide(this->ODE_Geom_ID,RayXNeg, 1, &contact[0].geom, sizeof(dContact)))  {
							  worldEntity->registerCollision(COLLISION_TYPE_AXIS_X_NEG , this, worldEntity, Vector(1.0f, 0.0f, 0.0f),
                                                         Vector(contact[0].geom.pos[0],contact[0].geom.pos[1],contact[0].geom.pos[2]), false);
							 
						}//if
					}//if
				
					
					if(dCollide(this->ODE_Geom_ID,RayY, 1, &contact[0].geom, sizeof(dContact)))  {
						
						
						if(dCollide(this->ODE_Geom_ID,RayYNeg, 1, &contact[0].geom, sizeof(dContact)))  {
							 worldEntity->registerCollision(COLLISION_TYPE_AXIS_Y_NEG , this, worldEntity, Vector(0.0f, 1.0f, 0.0f),
                                                         Vector(contact[0].geom.pos[0],contact[0].geom.pos[1],contact[0].geom.pos[2]), false);
						
							
						}//if
						else if(dCollide(this->ODE_Geom_ID,RayYPos, 1, &contact[0].geom, sizeof(dContact)))  {
							  worldEntity->registerCollision(COLLISION_TYPE_AXIS_Y_NEG , this, worldEntity, Vector(0.0f, 1.0f, 0.0f),
                                                         Vector(contact[0].geom.pos[0],contact[0].geom.pos[1] ,contact[0].geom.pos[2]), false);
							
						}//if
						else {

						worldEntity->registerCollision(COLLISION_TYPE_AXIS_Y_NEG , this, worldEntity, Vector(0.0f, 1.0f, 0.0f),
                                                        Vector(contact[0].geom.pos[0],contact[0].geom.pos[1],contact[0].geom.pos[2]), false);

						}

					}//if
				
					if(dCollide(this->ODE_Geom_ID,RayZ, 1, &contact[0].geom, sizeof(dContact)))  {
						if(dCollide(this->ODE_Geom_ID,RayZPos, 1, &contact[0].geom, sizeof(dContact)))  {
						//	  worldEntity->registerCollision(COLLISION_TYPE_AXIS_Z , this, worldEntity, Vector(0.0f, 0.0f, 1.0f),
                                                  //       Vector(contact[0].geom.pos[0],contact[0].geom.pos[1],contact[0].geom.pos[2]), false);
							
						}//if
						if(dCollide(this->ODE_Geom_ID,RayZNeg, 1, &contact[0].geom, sizeof(dContact)))  {
							//  worldEntity->registerCollision(COLLISION_TYPE_AXIS_Z_NEG , this, worldEntity, Vector(0.0f, 0.0f, 1.0f),
                                                         // Vector(contact[0].geom.pos[0],contact[0].geom.pos[1],contact[0].geom.pos[2]), false);
							
						}//if


					}//if
			
			} //if
		
			
      dGeomDestroy(RayX);
      dGeomDestroy(RayY);
      dGeomDestroy(RayZ);
      dGeomDestroy(RayXPos);
      dGeomDestroy(RayYPos);
      dGeomDestroy(RayZPos);
      dGeomDestroy(RayXNeg);
      dGeomDestroy(RayYNeg);
      dGeomDestroy(RayZNeg);
	
     dGeomDestroy(BBOX);
} // if(bbox != 0)

}