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

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

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

// Called by Physics::Server when the Level is attached to the server.
void CLevel::Activate()
{
	TimeToSim = 0.0;

	// Initialize ODE //???per-Level?
	dInitODE();
	ODEWorldID = dWorldCreate();
	dWorldSetQuickStepNumIterations(ODEWorldID, 20);

	// FIXME(enno): is a quadtree significantly faster? -- can't count geoms with quadtree
	ODECommonSpaceID  = dSimpleSpaceCreate(NULL);
	ODEDynamicSpaceID = dSimpleSpaceCreate(ODECommonSpaceID);
	ODEStaticSpaceID  = dSimpleSpaceCreate(ODECommonSpaceID);

	dWorldSetGravity(ODEWorldID, Gravity.x, Gravity.y, Gravity.z);
	dWorldSetContactSurfaceLayer(ODEWorldID, 0.001f);
	dWorldSetContactMaxCorrectingVel(ODEWorldID, 100.0f);
	dWorldSetERP(ODEWorldID, 0.2f);     // ODE's default value
	dWorldSetCFM(ODEWorldID, 0.001f);    // the default is 10^-5

	// setup autodisabling
	dWorldSetAutoDisableFlag(ODEWorldID, 1);
	dWorldSetAutoDisableSteps(ODEWorldID, 5);
	//dWorldSetAutoDisableTime(ODEWorldID, 1.f);
	dWorldSetAutoDisableLinearThreshold(ODEWorldID, 0.05f);   // default is 0.01
	dWorldSetAutoDisableAngularThreshold(ODEWorldID, 0.1f);  // default is 0.01

	// create a Contact group for joints
	ContactJointGroup = dJointGroupCreate(0);
}

CPhysicManager::CPhysicManager(IEngine* _Engine):CBaseObject(_Engine){

    SetParam("BaseClass","PhysicManager");
    SetParam("Type","CPhysicManager");
    SetParam("Name","-");

	dSetErrorHandler (SilenceMessage); //затыкаем рот идиотским ошибкам
    dSetDebugHandler (SilenceMessage); //затыкаем рот идиотским ошибкам
    dSetMessageHandler (SilenceMessage); //затыкаем рот идиотским ошибкам


    calc_time= 0.0f;

	world = dWorldCreate();
	dWorldSetGravity(world, 0, 0, -0.01 );
	space=dHashSpaceCreate(0);
	contactgroup = dJointGroupCreate(0);

    LOGGER->LogMsg("+CPhysicManager");

    ptr_world = world;
    ptr_contactgroup = contactgroup;

    dWorldSetCFM(world, 1e-5);
    dWorldSetERP(world, 0.2);
    dWorldSetContactMaxCorrectingVel(world, 0.9);
    dWorldSetContactSurfaceLayer(world, 0.001);
    dWorldSetAutoDisableFlag(world, 1);

//    ground =

#define SHIRINA 1000

    //dGeomID tmp = dCreateBox( space, SHIRINA, SHIRINA, 1);
    //dGeomSetPosition(tmp, SHIRINA/2, SHIRINA/2, 0); // двигаем центр координат

    //tmp = dCreateBox( space, 1, SHIRINA, SHIRINA);
    //dGeomSetPosition(tmp, 0, SHIRINA/2, SHIRINA/2); // двигаем центр координат

    //tmp = dCreateBox( space, SHIRINA, 1, SHIRINA);
    //dGeomSetPosition(tmp, SHIRINA/2, 0, SHIRINA/2); // двигаем центр координат

    dCreatePlane (space,0,0,1,0);

    //dCreatePlane (space,1,1,0,0);

    //dCreatePlane (space,0,1,0,0);
    //dCreatePlane (space,1,0,0,0);

    //dCreatePlane (space,0,0,20,0);
    //dCreatePlane (space,0,10,0,0);
    //dCreatePlane (space,10,0,0,0);

    //dCreatePlane (space,0,0,1,0);
    //dCreatePlane (space,0,0,1,0);
    //dCreatePlane (space,0,0,1,0);
    //dCreatePlane (space,0,0,1,0);
    //dCreatePlane (space,0,0,1,0);

};

ODEDomain::ODEDomain(char const *space_type, Input *input) : BaseDomain(input)
{
    printf("ODEDomain constructor\n");   
    dInitODE();
    world = dWorldCreate();
    contactgroup = dJointGroupCreate (0);
           
    //dWorldSetAutoDisableFlag(world,1);
    
    //dWorldSetCFM(world,1e-5);
    //Set and get the global CFM (constraint force mixing) value. Typical values are in the range 10-9 -- 1. The default is 10-5 if single precision is being used, or 10-10 if double precision is being used.
    
    //TODO HERE HAS TO BE ANOTHER PARAMETR
    //dWorldSetContactSurfaceLayer(world,0.001);

    //dWorldSetQuickStepNumIterations (world,ITERS);
    
    dRandSetSeed (time(NULL));
    setGravity(m_input->gravity_v[0],m_input->gravity_v[1],m_input->gravity_v[2]);

    if (space_type == std::string("quad"))
    {
        dVector3 Center = {boundaries[0][0]+delta[0]*0.5, boundaries[1][0]+delta[1]*0.5, boundaries[2][0]+delta[2]*0.5, 0};
        dVector3 Extents = {delta[0] * 0.55, delta[1] * 0.55, delta[2] * 0.55, 0};                
        printf(":::: Using Quad Tree Space\n");
        space = dQuadTreeSpaceCreate (0, Center, Extents, 6);        
    }
    else if (space_type == std::string("hash"))
    {                
        printf(":::: Using Hash Space\n");
        space = dHashSpaceCreate (0);
        //dHashSpaceSetLevels (space,-10,10);
    }
    else if (space_type == std::string("sap")) 
    {
        printf(":::: Using Sweep And Prune Space\n");
        space = dSweepAndPruneSpaceCreate (0, dSAP_AXES_XYZ);
    }
    else if (space_type == std::string("simple")) 
    {
        printf(":::: Using Simple Space\n");
        space = dSimpleSpaceCreate(0);
    }

    if (!space) 
    {
        printf(":::: Using Sweep And Prune Space\n");
        space = dSweepAndPruneSpaceCreate (0, dSAP_AXES_XYZ);            
    }
    
    //TODO redo tests
    //I did tests: deposition of 10000 particles without& (~catBits[PARTICLES])& (~catBits[PARTICLES]) collisions.
    //SWAP: 6 sec
    //HASH: 7.15 sec
    //QUAD: 41.8 sec
    //SIMLE: 59 sec
    
    printf("ODE conf: %s", dGetConfiguration());      
    printf("\n:::: sizeof(dReal)=%lu\n\n", sizeof(dReal));
}

    /**
     *  \brief This function initializes the ode world.
     *
     * pre:
     *     - world_init = false
     *
     * post:
     *     - world, space and contactgroup should be created
     *     - the ODE world parameters should be set
     *     - at the end world_init have to become true
     */
    void WorldPhysics::initTheWorld(void) {
      MutexLocker locker(&iMutex);
  
      // if world_init = true debug something
      if (!world_init) {
        //LOG_DEBUG("init physics world");
        world = dWorldCreate();
        space = dHashSpaceCreate(0);
        contactgroup = dJointGroupCreate(0);

        old_gravity = world_gravity;
        old_cfm = world_cfm;
        old_erp = world_erp;

        dWorldSetGravity(world, world_gravity.x(), world_gravity.y(), world_gravity.z()); 
        dWorldSetCFM(world, (dReal)world_cfm);
        dWorldSetERP (world, (dReal)world_erp);

        dWorldSetAutoDisableFlag (world,0);
        // if usefull for some tests a ground can be created here
        plane = 0; //dCreatePlane (space,0,0,1,0);
        world_init = 1;
        drawStruct draw;
        draw.ptr_draw = (DrawInterface*)this;
        if(control->graphics)
          control->graphics->addDrawItems(&draw);
      }
      //printf("initTheWorld..\n");
    }

// initialize server (supply world dimensions)
bool PhysicsServer::init(int groundSizeX, int groundSizeY)
{
	if (groundSizeX <= 20 || groundSizeY <= 20)
		return false;

	// create global ODE world
	_world = dWorldCreate();

	// create global space
	_space = dHashSpaceCreate(0);

	// create group for handling collisions
	_contactgroup = dJointGroupCreate (0);

	// make ground a bit slippery :)
	_envData.slip = 0.15; 
	// TODO: let user specify ground surface properties

	// set world properties
	dWorldSetGravity (_world,0,0,-(PHYS_GRAVITY*PHYS_MASS_SCALE));
	dWorldSetERP (_world, 0.6);

	// create ground and walls (0,0,0 is the ground's center)
	// and put them in their own space
	_envSpace = dSimpleSpaceCreate(_space);
	_ground = dCreatePlane (_envSpace,0, 0, 1, 0);
	dGeomSetData(_ground, (void*)&_envData);

	_wall[0] = dCreatePlane(_envSpace,  1, 0,  0, -(groundSizeX/2.0) ); // a,b,c,d
	_wall[1] = dCreatePlane(_envSpace, -1, 0,  0, -(groundSizeX/2.0) ); // a,b,c,d
	_wall[2] = dCreatePlane(_envSpace,  0, 1,  0, -(groundSizeY/2.0) ); // a,b,c,d
	_wall[3] = dCreatePlane(_envSpace,  0, -1, 0, -(groundSizeY/2.0) ); // a,b,c,d

	return true;
}

void PhysicsServer::Init()
{
    if (m_bInited)
    {
        return;
    }
    RemoveChildren();
    m_Objects.clear();

    dInitODE();

    m_WorldID           = dWorldCreate();
    m_DefaultSpaceID    = dHashSpaceCreate( NULL );
    m_ContactGroupID    = dJointGroupCreate( 0 );

    dWorldSetAutoDisableFlag( m_WorldID, 1 );
    dWorldSetAutoDisableAverageSamplesCount( m_WorldID, 1 );

    if (m_WorldID)
    {
        SetERP( m_ERP );
        SetCFM( m_CFM );
        SetGravity( m_Gravity );
    }

    m_pDefMaterial = g_pObjectServer->FindObject<PhysMaterial>( "defaullt_mtl", this );
    if (!m_pDefMaterial) 
    {
        m_pDefMaterial = new PhysMaterial();
        m_pDefMaterial->SetName( "defaullt_mtl" );
        AddChild( m_pDefMaterial );
    }

    m_bInited = true;
} // PhysicsServer::Init

void HarrierSim::start2()
{
  //        setDrawStuff();
  world = dWorldCreate();
  space = dHashSpaceCreate(0);
  contactgroup = dJointGroupCreate(0);
  ground = dCreatePlane(space, 0, 0, 1, 0);
  dWorldSetGravity(world,0,0,-0.5);

  dWorldSetCFM (world,1e-6);
  dWorldSetERP (world,1);
  //dWorldSetAutoDisableFlag (world,1);
  dWorldSetContactMaxCorrectingVel (world,0.1);
  //set the contact penetration
  dWorldSetContactSurfaceLayer(world, 0.001);


  makeHarrier();

  itsHarrierObject = vp->load3DSObject("./etc/spaceship.3ds", "./etc/textures/spaceshiptexture.ppm");
  itsHarrierObject.scale = 0.1;


  //set the viewpoint
  double xyz[3] = {0 , 25.0, 20};
  double hpr[3] = {-90.0,-35,0.0};
  vp->dsSetViewpoint (xyz,hpr);
  vp->setZoom(1.5);

}

void Simulator_Initialize(void) {

	dInitODE2(0);
	world = dWorldCreate();
	space = dHashSpaceCreate (0);
	contactgroup = dJointGroupCreate (0);
	dWorldSetGravity (world,0,0,-0.5);
	ground = dCreatePlane (space,0,0,1,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;
}

WorldManagerServer::WorldManagerServer()
{
	dInitODE();
	mWorld = dWorldCreate();
	mGlobalSpace = dHashSpaceCreate(0);
	mContactGroup = dJointGroupCreate(0);
	dWorldSetGravity(mWorld, 0, -100, 0);

	mStaticSpace = dSimpleSpaceCreate(mGlobalSpace);
}

/*** シミュレーションの初期化 ***/
void dmInit()
{
	dInitODE();                              // ODEの初期化
	world = dWorldCreate();                  // 世界の創造
	dWorldSetGravity(world, 0.0,0.0,-9.8);        // 重力設定

	space        = dHashSpaceCreate(0);   // 衝突用空間の創造
	contactgroup = dJointGroupCreate(0);  // ジョイントグループの生成
	ground = dCreatePlane(space,0,0,1,0); // 地面（平面ジオメトリ）の生成
	dsSetSphereQuality(3);
}

PhysicsSpace::PhysicsSpace(const PhysicsSpace &source) :
    Inherited(source)
{
    _ContactJoints.resize(MAX_PHYS_CONTACTS);
    _ColJointGroupId = dJointGroupCreate(0);

    if(getDefaultCollisionParameters() == NULL)
    {
        CollisionContactParametersUnrecPtr DefaultParams(createDefaultContactParams());
        setDefaultCollisionParameters(DefaultParams);
    }
}

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

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

int startup_physics(void)
{
    dInitODE();

    world = dWorldCreate();
    space = dHashSpaceCreate(0);
    group = dJointGroupCreate(0);

    dWorldSetGravity(world, 0, (dReal) -9.8, 0);
    // dWorldSetAutoDisableFlag(world, 1);

    return 1;
}

void initWorldModelling(int testcase)
{
    /* create world */
    dRandSetSeed(1);
    dInitODE();
    //dInitODE2(dInitFlagManualThreadCleanup);
    //dAllocateODEDataForThread(dAllocateMaskAll);
    world = dWorldCreate();
    space = dHashSpaceCreate (0);

    //dWorldSetAutoDisableFlag(World, 1);

    // The parameter needs to be zero.
    contactgroup = dJointGroupCreate (0);
    dWorldSetGravity (world,0,-9.81f,0);
    dWorldSetCFM (world,1e-2f);   //1e-5f was the originally value used.
    dWorldSetERP(world,1.0f);   // 1 is Error Correction is applied.

    // Set Damping
    dWorldSetLinearDamping(world, 0.01f);  // 0.00001
    dWorldSetAngularDamping(world, 0.005f);     // 0.005
    dWorldSetMaxAngularSpeed(world, 200);

    // Set and get the depth of the surface layer around all geometry objects. Contacts are allowed to sink into the surface layer up to the given depth before coming to rest. The default value is zero. Increasing this to some small value (e.g. 0.001) can help prevent jittering problems due to contacts being repeatedly made and broken.
    dWorldSetContactSurfaceLayer (world,0.001f);

    ground = dCreatePlane (space,0,1,0,0);



    switch(testcase)
    {
    case 1:initIslands();test1();break;// Carrier stability
    case 2:initIslands();test1();test2();break;// Manta landing on island.
    case 3:initIslands();test1();test2();test3();break;// Manta crashing on structure
    case 4:initIslands();test1();test2();test3();test4();break;// Manta landing on runway
    case 5:initIslands();test1();test2();break;// Manta landing on aircraft
    case 6:initIslands();test1();test6();break;// Carrier stranded on island
    case 7:test1();test2();test7();break; //Manta crashing on water.
    case 8:initIslands();test1();test8();break; // Walrus reaching island.
    case 9:test1();test9();break; // Walrus stability.
    case 10:initIslands();test1();test10();break; // Walrus arrive to island and build the command center.
    case 11:initIslands();test11();break; // Carrier stability far away.
    case 12:initIslands();test1();test12();break; // Bullets
    case 13:initIslands();test13();break;
    default:initIslands();test1();break;
    }

    testing = testcase;

}