//************************************************************************
// ODE
//************************************************************************

// this is called by dSpaceCollide when two objects in space are
// potentially colliding.

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  int i,n;

  // exit without doing anything if the two bodies are connected by a joint
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);
  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;

 //avoid car part collision
 for (i=0;i<=nCars;i++){
	if (o1==PcCar[i].CarChasis.geom)
	{
		if (o2==PcCar[i].wheelFL.geom||o2==PcCar[i].wheelFR.geom||o2==PcCar[i].wheelRL.geom||o2==PcCar[i].wheelRR.geom||o2==PcCar[i].excelJoin.geom||o2==PcCar[i].excelL.geom||o2==PcCar[i].excelR.geom) return;
	}
	if (o2==PcCar[i].CarChasis.geom)
	{
		if (o1==PcCar[i].wheelFL.geom||o1==PcCar[i].wheelFR.geom||o1==PcCar[i].wheelRL.geom||o1==PcCar[i].wheelRR.geom||o1==PcCar[i].excelJoin.geom||o1==PcCar[i].excelL.geom||o1==PcCar[i].excelR.geom) return;
	}

	//if (o2==PcCar[i].wheelFL.geom||o2==PcCar[i].wheelFR.geom||o2==PcCar[i].wheelRL.geom||o2==PcCar[i].wheelRR.geom) return;
	//if (o2==PcCar[i].excelJoin.geom||o2==PcCar[i].excelL.geom||o2==PcCar[i].excelR.geom) return;
	//if (o1==PcCar[i].excelJoin.geom||o1==PcCar[i].excelL.geom||o1==PcCar[i].excelR.geom) return;
 }
  
  // only collide things with the ground
  //int g1 = (o1 == ground || o1 == ground_box);
  //int g2 = (o2 == ground || o2 == ground_box);
  //if (!(g1 ^ g2)) return;

  const int N = 10;
  dContact contact[N];
  n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
  if (n > 0) {
    for (i=0; i<n; i++) {
      contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
	dContactSoftERP | dContactSoftCFM | dContactApprox1;
      contact[i].surface.mu = 0.5;//dInfinity; // the global mu to use; f > MU * body_mass * GRAVITY
      contact[i].surface.slip1 = 0.1;
      contact[i].surface.slip2 = 0.1;
      contact[i].surface.soft_erp = 0.5;
      contact[i].surface.soft_cfm = 0.3;

      dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
      dJointAttach (c,
		    dGeomGetBody(contact[i].geom.g1),
		    dGeomGetBody(contact[i].geom.g2));
    }
  }
}
//************************************************************************

void initBuggy()
{

  int i;
  dMass m;
  double sides[3] = {1,1,1};//fake initialize


////////////////
	//Car here
	int DisX=10,DisY=10,DisZ=3;
	for (i=0;i<=nCars;i++)
	{

		DisX+=1;DisY+=10;DisZ+=0;

		//Wheels
		int mySphereRADIUS = 1;
		int wheelMass=4;
  		PcCar[i].wheelFL.body = dBodyCreate (world);
		dBodySetPosition (PcCar[i].wheelFL.body,3.75+DisX,2+DisY,1+DisZ);
		dMassSetSphere(&m,wheelMass,mySphereRADIUS);
		dMassAdjust (&m,wheelMass);
		dBodySetMass (PcCar[i].wheelFL.body,&m);
		PcCar[i].wheelFL.geom = dCreateSphere(0, mySphereRADIUS);
		dGeomSetBody (PcCar[i].wheelFL.geom,PcCar[i].wheelFL.body);
		//dSpaceAdd (space, PcCar[i].wheelFL.geom);

		PcCar[i].wheelFR.body = dBodyCreate (world);
		dBodySetPosition (PcCar[i].wheelFR.body,3.75+DisX,-2+DisY,1+DisZ);
		dMassSetSphere(&m,wheelMass,mySphereRADIUS);
		dMassAdjust (&m,wheelMass);
		dBodySetMass (PcCar[i].wheelFR.body,&m);
		PcCar[i].wheelFR.geom = dCreateSphere(0, mySphereRADIUS);
		dGeomSetBody (PcCar[i].wheelFR.geom,PcCar[i].wheelFR.body);
		//dSpaceAdd (space, PcCar[i].wheelFR.geom);

		PcCar[i].wheelRL.body = dBodyCreate (world);
		dBodySetPosition (PcCar[i].wheelRL.body,-2.25+DisX,2+DisY,1+DisZ);
		dMassSetSphere(&m,wheelMass,mySphereRADIUS);
		dMassAdjust (&m,wheelMass);
		dBodySetMass (PcCar[i].wheelRL.body,&m);
		PcCar[i].wheelRL.geom = dCreateSphere(0, mySphereRADIUS);
		dGeomSetBody (PcCar[i].wheelRL.geom,PcCar[i].wheelRL.body);
		//dSpaceAdd (space, PcCar[i].wheelRL.geom);

		PcCar[i].wheelRR.body = dBodyCreate (world);
		dBodySetPosition (PcCar[i].wheelRR.body,-2.25+DisX,-2+DisY,1+DisZ);
		dMassSetSphere(&m,wheelMass,mySphereRADIUS);
		dMassAdjust (&m,wheelMass);
		dBodySetMass (PcCar[i].wheelRR.body,&m);
		PcCar[i].wheelRR.geom = dCreateSphere(0, mySphereRADIUS);
		dGeomSetBody (PcCar[i].wheelRR.geom,PcCar[i].wheelRR.body);
		//dSpaceAdd (space, PcCar[i].wheelRR.geom);

		sides[0] = 10; sides[1] = 5, sides[2] = 1.5;
		PcCar[i].CarChasis.body = dBodyCreate (world);
		dBodySetPosition (PcCar[i].CarChasis.body,0.75+DisX,0+DisY,2+DisZ); //<--POSITION
		dMassSetBox(&m,20,sides[0],sides[1], sides[2]);
		dMassAdjust (&m,20);
		dBodySetMass (PcCar[i].CarChasis.body,&m);
			PcCar[i].CarChasis.geom = dCreateBox(0,sides[0],sides[1],sides[2]);
			dGeomSetBody (PcCar[i].CarChasis.geom,PcCar[i].CarChasis.body);
		dBodySetAutoDisableFlag(PcCar[i].CarChasis.body, 1);

		sides[0] = 5; sides[1] = 5, sides[2] = 1.5;
		PcCar[i].carTop.body = dBodyCreate (world);
		dBodySetPosition (PcCar[i].carTop.body,0+DisX,0+DisY,3.5+DisZ); //<--POSITION
		dMassSetBox(&m,.1,sides[0],sides[1], sides[2]);
		dMassAdjust (&m,.1);
		dBodySetMass (PcCar[i].carTop.body,&m);
			PcCar[i].carTop.geom = dCreateBox(0,sides[0],sides[1],sides[2]);
			dGeomSetBody (PcCar[i].carTop.geom,PcCar[i].carTop.body);
		dBodySetAutoDisableFlag(PcCar[i].carTop.body, 1);

		//excels boxes
		sides[0] = 2; sides[1] = .5, sides[2] = .1;
		PcCar[i].excelL.body = dBodyCreate (world);
		dBodySetPosition (PcCar[i].excelL.body,3+DisX,2+DisY,1+DisZ); //<--POSITION
		dMassSetBox(&m,1,sides[0],sides[1], sides[2]);
		dMassAdjust (&m,1);
		dBodySetMass (PcCar[i].excelL.body,&m);
			PcCar[i].excelL.geom = dCreateBox(0,sides[0],sides[1],sides[2]);
			dGeomSetBody (PcCar[i].excelL.geom,PcCar[i].excelL.body);
		dBodySetAutoDisableFlag(PcCar[i].excelL.body, 1);

		sides[0] = 2; sides[1] = .5, sides[2] = .1;
		PcCar[i].excelR.body = dBodyCreate (world);
		dBodySetPosition (PcCar[i].excelR.body,3+DisX,-2+DisY,1+DisZ); //<--POSITION
		dMassSetBox(&m,1,sides[0],sides[1], sides[2]);
		dMassAdjust (&m,1);
		dBodySetMass (PcCar[i].excelR.body,&m);
			PcCar[i].excelR.geom = dCreateBox(0,sides[0],sides[1],sides[2]);
			dGeomSetBody (PcCar[i].excelR.geom,PcCar[i].excelR.body);
		dBodySetAutoDisableFlag(PcCar[i].excelR.body, 1);

		sides[0] = .2; sides[1] = 5, sides[2] = .1;

		PcCar[i].excelJoin.body = dBodyCreate (world);
		dBodySetPosition (PcCar[i].excelJoin.body,2.25+DisX,0+DisY,1+DisZ); //<--POSITION
		dMassSetBox(&m,1,sides[0],sides[1], sides[2]);
		dMassAdjust (&m,1);
		dBodySetMass (PcCar[i].excelJoin.body,&m);
			PcCar[i].excelJoin.geom = dCreateBox(0,sides[0],sides[1],sides[2]);
			dGeomSetBody (PcCar[i].excelJoin.geom,PcCar[i].excelJoin.body);
		dBodySetAutoDisableFlag(PcCar[i].excelJoin.body, 1);

		//////////////

		//Joints of car
		PcCar[i].hChasisRoofBack = dJointCreateHinge (world,0);
		dJointAttach (PcCar[i].hChasisRoofBack,PcCar[i].carTop.body,PcCar[i].CarChasis.body);
		dJointSetHingeAxis (PcCar[i].hChasisRoofBack,0,-90,0);
		dJointSetHingeAnchor (PcCar[i].hChasisRoofBack,-2.5+DisX,0+DisY,2.75+DisZ);
		dJointSetHingeParam (PcCar[i].hChasisRoofBack, dParamLoStop, 0);
		dJointSetHingeParam (PcCar[i].hChasisRoofBack, dParamHiStop, 0);

		PcCar[i].hChasisRoofFront = dJointCreateHinge (world,0);
		dJointAttach (PcCar[i].hChasisRoofFront,PcCar[i].carTop.body,PcCar[i].CarChasis.body);
		dJointSetHingeAxis (PcCar[i].hChasisRoofFront,0,-90,0);
		dJointSetHingeAnchor (PcCar[i].hChasisRoofFront,2.5+DisX,0+DisY,2.75+DisZ);
		dJointSetHingeParam (PcCar[i].hChasisRoofFront, dParamLoStop, 0);
		dJointSetHingeParam (PcCar[i].hChasisRoofFront, dParamHiStop, 0);

		//rear wheel joints
		//left
		PcCar[i].hWheelRBChasis = dJointCreateHinge2 (world,0);
		dJointAttach (PcCar[i].hWheelRBChasis,PcCar[i].CarChasis.body,PcCar[i].wheelRL.body);
		const dReal *lbw = dBodyGetPosition (PcCar[i].wheelRL.body);
		dJointSetHinge2Anchor (PcCar[i].hWheelRBChasis,lbw[0],lbw[1],lbw[2]);
		dJointSetHinge2Axis1 (PcCar[i].hWheelRBChasis,0,0,1);
		dJointSetHinge2Axis2 (PcCar[i].hWheelRBChasis,0,1,0);
		
		dJointSetHinge2Param (PcCar[i].hWheelRBChasis,dParamSuspensionERP,0.4);
		dJointSetHinge2Param (PcCar[i].hWheelRBChasis,dParamSuspensionCFM,0.8);
		// lock back wheels along the steering axis, set stops to make sure wheels always stay in alignment
		dJointSetHinge2Param (PcCar[i].hWheelRBChasis,dParamLoStop,0);
		dJointSetHinge2Param (PcCar[i].hWheelRBChasis,dParamHiStop,0);

		//right
		PcCar[i].hWheelLBChasis = dJointCreateHinge2 (world,0);
		dJointAttach (PcCar[i].hWheelLBChasis,PcCar[i].CarChasis.body,PcCar[i].wheelRR.body);
		const dReal *rbw = dBodyGetPosition (PcCar[i].wheelRR.body);
		dJointSetHinge2Anchor (PcCar[i].hWheelLBChasis,rbw[0],rbw[1],rbw[2]);
		dJointSetHinge2Axis1 (PcCar[i].hWheelLBChasis,0,0,1);
		dJointSetHinge2Axis2 (PcCar[i].hWheelLBChasis,0,1,0);

		dJointSetHinge2Param (PcCar[i].hWheelLBChasis,dParamSuspensionERP,0.4);
		dJointSetHinge2Param (PcCar[i].hWheelLBChasis,dParamSuspensionCFM,0.8);
		// lock back wheels along the steering axis, set stops to make sure wheels always stay in alignment
		dJointSetHinge2Param (PcCar[i].hWheelLBChasis,dParamLoStop,0);
		dJointSetHinge2Param (PcCar[i].hWheelLBChasis,dParamHiStop,0);

		//front wheel and excel
		PcCar[i].hLeftWheelExcel = dJointCreateHinge (world,0);
		dJointAttach (PcCar[i].hLeftWheelExcel,PcCar[i].wheelFL.body,PcCar[i].excelL.body);
		dJointSetHingeAxis (PcCar[i].hLeftWheelExcel,0,-90,0);
		dJointSetHingeAnchor (PcCar[i].hLeftWheelExcel,3.75+DisX,2.25+DisY,1+DisZ);
		
		PcCar[i].hRightWheelExcel = dJointCreateHinge (world,0);
		dJointAttach (PcCar[i].hRightWheelExcel,PcCar[i].wheelFR.body,PcCar[i].excelR.body);
		dJointSetHingeAxis (PcCar[i].hRightWheelExcel,0,-90,0);
		dJointSetHingeAnchor (PcCar[i].hRightWheelExcel,3.75+DisX,-2.25+DisY,1+DisZ);

		PcCar[i].hChasisExcelL = dJointCreateHinge (world,0);
		dJointAttach (PcCar[i].hChasisExcelL,PcCar[i].CarChasis.body,PcCar[i].excelL.body);
		dJointSetHingeAxis (PcCar[i].hChasisExcelL,0,0,-90);
		dJointSetHingeAnchor (PcCar[i].hChasisExcelL,3.75+DisX,2+DisY,.75+DisZ);

		PcCar[i].hChasisExcelR = dJointCreateHinge (world,0);
		dJointAttach (PcCar[i].hChasisExcelR,PcCar[i].CarChasis.body,PcCar[i].excelR.body);
		dJointSetHingeAxis (PcCar[i].hChasisExcelR,0,0,-90);
		dJointSetHingeAnchor (PcCar[i].hChasisExcelR,3.75+DisX,-2+DisY,.75+DisZ);

		PcCar[i].hexcelJoinExcelL = dJointCreateHinge (world,0);
		dJointAttach (PcCar[i].hexcelJoinExcelL,PcCar[i].excelJoin.body,PcCar[i].excelL.body);
		dJointSetHingeAxis (PcCar[i].hexcelJoinExcelL,0,0,-90);
		dJointSetHingeAnchor (PcCar[i].hexcelJoinExcelL,2.25+DisX,2+DisY,.75+DisZ);

		PcCar[i].hexcelJoinExcelR = dJointCreateHinge (world,0);
		dJointAttach (PcCar[i].hexcelJoinExcelR,PcCar[i].excelJoin.body,PcCar[i].excelR.body);
		dJointSetHingeAxis (PcCar[i].hexcelJoinExcelR,0,0,-90);
		dJointSetHingeAnchor (PcCar[i].hexcelJoinExcelR,2.25+DisX,-2+DisY,.75+DisZ);

		//////////////
		// create car space and add it to the top level space
		car_space[i] = dSimpleSpaceCreate (space);
		dSpaceSetCleanup (car_space[i],0);
		dSpaceAdd (car_space[i],PcCar[i].carTop.geom);
		dSpaceAdd (car_space[i],PcCar[i].CarChasis.geom);
		dSpaceAdd (car_space[i],PcCar[i].wheelFL.geom);
		dSpaceAdd (car_space[i],PcCar[i].wheelFR.geom);
		dSpaceAdd (car_space[i],PcCar[i].wheelRL.geom);
		dSpaceAdd (car_space[i],PcCar[i].wheelRR.geom);
		dSpaceAdd (car_space[i],PcCar[i].excelJoin.geom);
		dSpaceAdd (car_space[i],PcCar[i].excelL.geom);
		dSpaceAdd (car_space[i],PcCar[i].excelR.geom);

		PcCar[i].colR=1, PcCar[i].colG=.5, PcCar[i].colB=1;
	}
	PcCar[0].colR=1, PcCar[0].colG=0, PcCar[0].colB=0;
	PcCar[1].colR=0, PcCar[1].colG=0, PcCar[1].colB=1;
	PcCar[2].colR=0, PcCar[2].colG=1, PcCar[2].colB=0;



	//End Car
	//////////////////////////////

}

void initFloor()
{
	  dMass m;
	  double sides[3] = {1,1,1};//fake initialize
	  int xInc,yInc,zInc;
	  xInc=100;yInc=100;zInc=0;

		Floors[0].Roof.body = dBodyCreate (world);
		dBodySetPosition (Floors[0].Roof.body,0+xInc,-.75+yInc,10.25+zInc); //<--POSITION
		sides[0] = 40; sides[1] = 30; sides[2] =.1;
		dMassSetBox(&m,5.01,sides[0],sides[1], sides[2]);
		dMassAdjust (&m,5.01);
		dBodySetMass (Floors[0].Roof.body,&m);
			Floors[0].Roof.geom =  dCreateBox(space,sides[0],sides[1],sides[2]);
			dGeomSetBody (Floors[0].Roof.geom,Floors[0].Roof.body);
		dBodySetAutoDisableFlag(Floors[0].Roof.body, 1);

		Floors[0].Ramp.body = dBodyCreate (world);
		dBodySetPosition (Floors[0].Ramp.body,15+xInc,-30+yInc,10+zInc); //<--POSITION
		sides[0] = 10; sides[1] = 30; sides[2] =.1;
		dMassSetBox(&m,.01,sides[0],sides[1], sides[2]);
		dMassAdjust (&m,0.01);
		dBodySetMass (Floors[0].Ramp.body,&m);
			Floors[0].Ramp.geom =  dCreateBox(space,sides[0],sides[1],sides[2]);
			dGeomSetBody (Floors[0].Ramp.geom,Floors[0].Ramp.body);
		dBodySetAutoDisableFlag(Floors[0].Ramp.body, 1);

		Floors[0].Pillar1.body = dBodyCreate (world);
		dBodySetPosition (Floors[0].Pillar1.body,-16.25+xInc,11+yInc,5+zInc); //<--POSITION
		sides[0] = 5; sides[1] = 5; sides[2] =10;
		dMassSetBox(&m,.01,sides[0],sides[1], sides[2]);
		dMassAdjust (&m,0.01);
		dBodySetMass (Floors[0].Pillar1.body,&m);
			Floors[0].Pillar1.geom =  dCreateBox(space,sides[0],sides[1],sides[2]);
			dGeomSetBody (Floors[0].Pillar1.geom,Floors[0].Pillar1.body);
		dBodySetAutoDisableFlag(Floors[0].Pillar1.body, 1);

		Floors[0].Pillar2.body = dBodyCreate (world);
		dBodySetPosition (Floors[0].Pillar2.body,16+xInc,12.25+yInc,5+zInc); //<--POSITION
		sides[0] = 5; sides[1] = 5; sides[2] =10;
		dMassSetBox(&m,.01,sides[0],sides[1], sides[2]);
		dMassAdjust (&m,0.01);
		dBodySetMass (Floors[0].Pillar2.body,&m);
			Floors[0].Pillar2.geom =  dCreateBox(space,sides[0],sides[1],sides[2]);
			dGeomSetBody (Floors[0].Pillar2.geom,Floors[0].Pillar2.body);
		dBodySetAutoDisableFlag(Floors[0].Pillar2.body, 1);

		Floors[0].Pillar3.body = dBodyCreate (world);
		dBodySetPosition (Floors[0].Pillar3.body,-16.25+xInc,-11.75+yInc,5+zInc); //<--POSITION
		sides[0] = 5; sides[1] = 5; sides[2] =10;
		dMassSetBox(&m,.01,sides[0],sides[1], sides[2]);
		dMassAdjust (&m,0.01);
		dBodySetMass (Floors[0].Pillar3.body,&m);
			Floors[0].Pillar3.geom =  dCreateBox(space,sides[0],sides[1],sides[2]);
			dGeomSetBody (Floors[0].Pillar3.geom,Floors[0].Pillar3.body);
		dBodySetAutoDisableFlag(Floors[0].Pillar3.body, 1);

		Floors[0].Pillar4.body = dBodyCreate (world);
		dBodySetPosition (Floors[0].Pillar4.body,14.75+xInc,-11.25+yInc,5+zInc); //<--POSITION
		sides[0] = 5; sides[1] = 9; sides[2] =10;
		dMassSetBox(&m,.01,sides[0],sides[1], sides[2]);
		dMassAdjust (&m,0.01);
		dBodySetMass (Floors[0].Pillar4.body,&m);
			Floors[0].Pillar4.geom =  dCreateBox(space,sides[0],sides[1],sides[2]);
			dGeomSetBody (Floors[0].Pillar4.geom,Floors[0].Pillar4.body);
		dBodySetAutoDisableFlag(Floors[0].Pillar4.body, 1);

		Floors[i].hFloorRamp = dJointCreateHinge (world,0);
		dJointAttach (Floors[i].hFloorRamp,Floors[i].Ramp.body,Floors[i].Roof.body);
		dJointSetHingeAxis (Floors[i].hFloorRamp,-90,0,0);
		dJointSetHingeAnchor (Floors[i].hFloorRamp,15+xInc,-15.75+yInc,10.25+zInc);

}

void initOde()
{
	// Create world

	int i;
	dMass m;
	// create world
	dInitODE();
	world = dWorldCreate();
	space = dHashSpaceCreate (0);
	//dWorldSetCFM (world,1e-5);
	//dWorldSetContactMaxCorrectingVel (world,1.1);
	//dWorldSetContactSurfaceLayer (world,0.001);
	//dWorldSetQuickStepNumIterations (world,30);//20 is def.
	sGravity=-0.5;
	contactgroup = dJointGroupCreate (0);
	dWorldSetGravity (world,0,0,sGravity);
	ground = dCreatePlane (space,0,0,1,0);
	//dWorldSetAutoDisableFlag (world, 1);

	//  dWorldSetAutoDisableAngularThreshold (world, .101);
	//  dWorldSetAutoDisableLinearThreshold (world, 1.1);
	//  dWorldSetAutoDisableSteps (world, 10);

	//mi Box'es, pillars!
  
	initBuggy();
	initFloor();

	double sides[3] = {2,2,10};
	cntBox=0;
	int rx=-2000,ry=3;

	while (cntBox<nPillars)
	{
		miBox[cntBox].body = dBodyCreate (world);
		dBodySetPosition (miBox[cntBox].body,rx,ry,5); //<--POSITION
		dMassSetBox(&m,.01,sides[0],sides[1], sides[2]);
		dMassAdjust (&m,0.01);
		dBodySetMass (miBox[cntBox].body,&m);
			miBox[cntBox].geom =  dCreateBox(space,sides[0],sides[1],sides[2]);
			dGeomSetBody (miBox[cntBox].geom,miBox[cntBox].body);
		dBodySetAutoDisableFlag(miBox[cntBox].body, 1);

		ry=ry+2;
		if (ry>2){ry=3;rx=rx+100;}
		cntBox++;
	}

	dBodySetPosition (miBox[cntBox-1].body,2000,2000,5);
	dBodySetPosition (miBox[cntBox-2].body,-2000,-2000,5);
	dBodySetPosition (miBox[cntBox-3].body,2000,-2000,5);
	dBodySetPosition (miBox[cntBox-4].body,-2000,-2000,5);

	//The box! the controllabe box
	  sides[0] = 2;sides[1] = 2;sides[2] = 0.2;
  	  miChair.body = dBodyCreate (world);
	  dBodySetPosition (miChair.body,0,10,STARTZ+100);
	  dMassSetBox(&m,0.5,sides[0],sides[1], sides[2]);
	  dMassAdjust (&m,0.5);
	  dBodySetMass (miChair.body,&m);
		miChair.geom =  dCreateBox(space,sides[0],sides[1],sides[2]);
		dGeomSetBody (miChair.geom,miChair.body);

	  // A sphere ball
	  int mySphereRADIUS = 1;
  	  mySphere[0].body = dBodyCreate (world);
	  dBodySetPosition (mySphere[0].body,10,5,5);
	  dMassSetSphere(&m,1.0,mySphereRADIUS);
	  dMassAdjust (&m,1.0);
	  dBodySetMass (mySphere[0].body,&m);
	  mySphere[0].geom = dCreateSphere(0, mySphereRADIUS);
	  dGeomSetBody (mySphere[0].geom,mySphere[0].body);
	  dSpaceAdd (space, mySphere[0].geom);

   //////////////////////////////

   // environment , the walls and roof, floor
/*	wall[0] = dCreateBox (space,50,1,12);
		dGeomSetPosition (wall[0],23,33,6);
	wall[1] = dCreateBox (space,50,1,12);
		dGeomSetPosition (wall[1],23,-13,6);
	wall[2] = dCreateBox (space,1,46,12);
		dGeomSetPosition (wall[2],48.2,10,6);
	wall[3] = dCreateBox (space,1,46,12);
		dGeomSetPosition (wall[3],-2,10,6);
	wall[4] = dCreateBox (space,50,46,1);//top
		dGeomSetPosition (wall[4],23,10,-.2);

	wall[5] = dCreateBox (space,50,46,1);//bottom
		dGeomSetPosition (wall[5],23,10,-1);
*/	


	//Billboards
	dMatrix3 Rtx;
	dRFromAxisAndAngle (Rtx,0,1,0,-0.15);
	miBB[0].geom=dCreateBox(space,20,20,3.5);
	dGeomSetPosition (miBB[0].geom,200,0,-0.34);
	dGeomSetRotation (miBB[0].geom,Rtx);

	dRFromAxisAndAngle (Rtx,0,1,0,-0.3);
	miBB[1].geom=dCreateBox(space,40,20,3.5);
	dGeomSetPosition (miBB[1].geom,500,0,2);
	dGeomSetRotation (miBB[1].geom,Rtx);

}

//***********************************************************************

void endOde()
{
  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
}
//**********************************************************************************


//**********************************************************************************
// simulation loop

static void simLoop (int pause)
{
	for (i=0;i<=nCars;i++)
	{
		// Motor
		dJointSetHinge2Param (PcCar[i].hWheelRBChasis,dParamVel2,-speed[i]);
		dJointSetHinge2Param (PcCar[i].hWheelRBChasis,dParamFMax2,2.0);

		dJointSetHinge2Param (PcCar[i].hWheelLBChasis,dParamVel2,-speed[i]);
		dJointSetHinge2Param (PcCar[i].hWheelLBChasis,dParamFMax2,2.0);

		// Steer
		double velCoeff = 1.0,p;
		static double angle = 0.5 * M_PI;

		p =  dJointGetHingeAngle(PcCar[i].hChasisExcelL);
		dReal z = steer[i]  -  p;
		if (p > 0.24 * M_PI) angle = - 0.25 * M_PI;
		else {
			if (p < - 0.24 * M_PI) angle = 0.25 * M_PI;
		}
		dJointSetHingeParam(PcCar[i].hChasisExcelL, dParamVel,  velCoeff*z); 
		dJointSetHingeParam(PcCar[i].hChasisExcelL, dParamFMax, 10);
	}

	/////
    dSpaceCollide (space,0,&nearCallback);
    dWorldStep (world,0.05);
	//dWorldQuickStep (world,0.05); //def .05

    // remove all contact joints
    dJointGroupEmpty (contactgroup);
}
