int main(int argc, char*argv[])
{
	EventManager::Initialize();
	Renderer::Initialize();

	World world;

	// If Running on Windows Then Play Background Music
	// PlaySound(TEXT("../Assets/Sounds/Danger-Zone.wav"), NULL, SND_FILENAME | SND_ASYNC | SND_LOOP);

	/*loading screen block*/ {

		LoadingScreen loadingScreen;
		loadingScreen.Draw();

		//	world.LoadScene("../Assets/Scenes/AnimatedSceneWithParticles.scene");
		//	world.LoadScene("../Assets/Scenes/AnimatedScene.scene");
		//	world.LoadScene("../Assets/Scenes/Spline.scene");
		//	world.LoadScene("../Assets/Scenes/StaticScene.scene");
		//	world.LoadScene("../Assets/Scenes/CoordinateSystem.scene");
		//	world.LoadScene("../Assets/Scenes/CollisionDemo.scene");
		//	world.LoadScene("../Assets/Scenes/Animal.scene");
		//	world.LoadScene("../Assets/Scenes/StaticScene.scene");
		//	world.LoadScene("../Assets/Scenes/CoordinateSystem.scene");
		//	world.LoadScene("../Assets/Scenes/Discoball.scene");
		//	world.LoadScene("../Assets/Scenes/Player.scene");
			world.LoadScene();
	}

	double fps = 1.0f / FPS;
	double dtStep = 1.0f / PHYSICS_FPS;
	
	double dtAcc = 0;

#ifdef DEBUG_FRAME_RATE
	long seconds = 0;
	double currentTime = 0.0;
	long remainingMsAcc = 0;
	long sleepCtr = 0;
#endif

	// Reset the polled values in the EventManager because scene loading is long.
	EventManager::Update();

	do
	{
		double start = glfwGetTime();

		// Update Event Manager - Frame time / input / events processing 
		EventManager::Update();

		float dt = EventManager::GetFrameTime();

		// Apply fixed delta time steps to each world update,
		// and drawing can be done if at least 1 world update was done.
		dtAcc += dt;
		bool draw = false;
		while (dtAcc >= dtStep) {
			dtAcc -= dtStep;
			world.Update(dtStep);
			draw = true;
		}

		if (draw) {
			world.Draw();
		}

#ifndef DEBUG_FRAME_RATE
		// Each frame should be "fps" seconds long.
		// If updating and rendering took less than fps seconds long then sleep for the remainder.
		int remainingMs = (start + fps - glfwGetTime()) * 1000;
		if (remainingMs > 0) {
			SLEEP_FUNC(remainingMs);
		}
#else
		long remainingMs = (start + fps - glfwGetTime()) * 1000;
		remainingMsAcc += remainingMs;

		currentTime += dt;
		if (currentTime - seconds >= 1) {
			seconds++;

			double avgRemaingMs = remainingMsAcc / (double)sleepCtr;
			cout << "avg remaining ms " << avgRemaingMs << endl;

			remainingMsAcc = 0;
			sleepCtr = 0;
		}

		if (remainingMs < 0) {
			cout << "OVER FRAME TIME BY " << abs(remainingMs) << " ms" << endl;
		}

		if (remainingMs > 0) {
			sleepCtr++;
			SLEEP_FUNC(remainingMs);
		}
#endif
	}
	while(EventManager::ExitRequested() == false);
//.........这里部分代码省略.........

int OrthographicGridPathfinder::getGridWidth()
{
    GameStateManager *gsm = game->getGSM();
    World *world = gsm->getWorld();
    return world->getWorldWidth()/numColumns;
}

void GuidedGraspPlanningTask::start()
{
  //get the details of the planning task itself
  if (!mDBMgr->GetPlanningTaskRecord(mRecord.taskId, &mPlanningTask)) {
    DBGA("Failed to get planning record for task");
    mStatus = ERROR;
    return;
  }

  World *world = graspItGUI->getIVmgr()->getWorld();

  //check if the currently selected hand is the same as the one we need
  //if not, load the hand we need
  if (world->getCurrentHand() && 
      GraspitDBGrasp::getHandDBName(world->getCurrentHand()) == QString(mPlanningTask.handName.c_str())) {
    DBGA("Grasp Planning Task: using currently loaded hand");
    mHand = world->getCurrentHand();
  } else {
    QString handPath = GraspitDBGrasp::getHandGraspitPath(QString(mPlanningTask.handName.c_str()));
    handPath = QString(getenv("GRASPIT")) + handPath;
    DBGA("Grasp Planning Task: loading hand from " << handPath.latin1());	      
    mHand = static_cast<Hand*>(world->importRobot(handPath));
    if ( !mHand ) {
      DBGA("Failed to load hand");
      mStatus = ERROR;
      return;
    }
  }
  //check for virtual contacts
  if (mHand->getNumVirtualContacts()==0) {
    DBGA("Specified hand does not have virtual contacts defined");
    mStatus = ERROR;
    return;
  }
  
  //load the object
  GraspitDBModel *model = static_cast<GraspitDBModel*>(mPlanningTask.model);
  if (model->load(world) != SUCCESS) {
    DBGA("Grasp Planning Task: failed to load model");
    mStatus = ERROR;
    return;
  }
  mObject = model->getGraspableBody();
  mObject->addToIvc();
  world->addBody(mObject);
  
  //initialize the planner
  GraspPlanningState seed(mHand);
  seed.setObject(mObject);
  seed.setPositionType(SPACE_AXIS_ANGLE);
  seed.setPostureType(POSE_EIGEN);
  seed.setRefTran(mObject->getTran());
  seed.reset();
  
  mPlanner = new GuidedPlanner(mHand);
  mPlanner->setModelState(&seed);	
  mPlanner->setContactType(CONTACT_PRESET);

  if (mPlanningTask.taskTime >= 0) mPlanner->setMaxTime(mPlanningTask.taskTime);
  else mPlanner->setMaxTime(-1);
  
  QObject::connect(mPlanner, SIGNAL(update()), this, SLOT(plannerUpdate()));
  QObject::connect(mPlanner, SIGNAL(complete()), this, SLOT(plannerComplete()));

  if (!mPlanner->resetPlanner()) {
    DBGA("Grasp Planning Task: failed to reset planner");
    mStatus = ERROR;
    return;
  }
  mLastSolution = 0;
  mPlanner->startPlanner();
  mStatus = RUNNING;
}

void IrrDriver::renderFixed(float dt)
{
    World *world = World::getWorld(); // Never NULL.

    m_video_driver->beginScene(/*backBuffer clear*/ true, /*zBuffer*/ true,
                               world->getClearColor());

    irr_driver->getVideoDriver()->enableMaterial2D();

    RaceGUIBase *rg = world->getRaceGUI();
    if (rg) rg->update(dt);


    for(unsigned int i=0; i<Camera::getNumCameras(); i++)
    {
        Camera *camera = Camera::getCamera(i);

        std::ostringstream oss;
        oss << "drawAll() for kart " << i;
        PROFILER_PUSH_CPU_MARKER(oss.str().c_str(), (i+1)*60,
                                 0x00, 0x00);
        camera->activate();
        rg->preRenderCallback(camera);   // adjusts start referee

        m_renderpass = ~0;
        m_scene_manager->drawAll();

        PROFILER_POP_CPU_MARKER();

        // Note that drawAll must be called before rendering
        // the bullet debug view, since otherwise the camera
        // is not set up properly. This is only used for
        // the bullet debug view.
        if (UserConfigParams::m_artist_debug_mode)
            World::getWorld()->getPhysics()->draw();
    }   // for i<world->getNumKarts()

    // Set the viewport back to the full screen for race gui
    m_video_driver->setViewPort(core::recti(0, 0,
                                            UserConfigParams::m_width,
                                            UserConfigParams::m_height));

    for(unsigned int i=0; i<Camera::getNumCameras(); i++)
    {
        Camera *camera = Camera::getCamera(i);
        std::ostringstream oss;
        oss << "renderPlayerView() for kart " << i;

        PROFILER_PUSH_CPU_MARKER(oss.str().c_str(), 0x00, 0x00, (i+1)*60);
        rg->renderPlayerView(camera, dt);
        PROFILER_POP_CPU_MARKER();

    }  // for i<getNumKarts

    // Either render the gui, or the global elements of the race gui.
    GUIEngine::render(dt);

    // Render the profiler
    if(UserConfigParams::m_profiler_enabled)
    {
        PROFILER_DRAW();
    }

#ifdef DEBUG
    drawDebugMeshes();
#endif

    m_video_driver->endScene();
}

//-----------------------------------------------------------------------------
void SoccerWorld::onCheckGoalTriggered(bool first_goal)
{
    if (isRaceOver())
        return;

    if (m_can_score_points)
    {
        m_team_goals[first_goal ? 0 : 1]++;

        World *world = World::getWorld();
        world->setPhase(WorldStatus::GOAL_PHASE);
        m_goal_sound->play();
        if(m_lastKartToHitBall != -1)
        {
            if(first_goal)
            {
                m_redScorers.push_back(m_lastKartToHitBall);
                if(race_manager->hasTimeTarget())
                    m_redScoreTimes.push_back(race_manager->getTimeTarget() - world->getTime());
                else
                    m_redScoreTimes.push_back(world->getTime());
            }
            else
            {
                m_blueScorers.push_back(m_lastKartToHitBall);
                if(race_manager->hasTimeTarget())
                    m_blueScoreTimes.push_back(race_manager->getTimeTarget() - world->getTime());
                else
                    m_blueScoreTimes.push_back(world->getTime());
            }
        }
    }

    // Reset original positions for the soccer balls
    TrackObjectManager* tom = getTrack()->getTrackObjectManager();
    assert(tom);

    PtrVector<TrackObject>& objects = tom->getObjects();
    for(unsigned int i=0; i<objects.size(); i++)
    {
        TrackObject* obj = objects.get(i);
        if(!obj->isSoccerBall())
            continue;

        obj->reset();
        obj->getPhysicalObject()->reset();
    }

    //Resetting the ball triggers the goal check line one more time.
    //This ensures that only one goal is counted, and the second is ignored.
    m_can_score_points = !m_can_score_points;

    //for(int i=0 ; i < getNumKarts() ; i++

    /*if(World::getWorld()->getTrack()->isAutoRescueEnabled() &&
        !getKartAnimation() && fabs(getRoll())>60*DEGREE_TO_RAD &&
                              fabs(getSpeed())<3.0f                )
    {
        new RescueAnimation(this, true);
    }*/

    // TODO: rescue the karts
}   // onCheckGoalTriggered

	void Server::broadcastCollectItem(Entities::Pickup* pi, Entities::LivingEntity* e)
	{
		World* w = e->getWorld();
		for (size_t i = 0; i < w->playerids.size(); i++)
			w->getPlayer(w->playerids.at(i))->getClientCon()->sendCollectItem(pi, e);
	}

    void IntersectorShortStack::Process(World const& world)
    {

        // If something has been changed we need to rebuild BVH
        if (!m_bvh || world.has_changed() || world.GetStateChange() != ShapeImpl::kStateChangeNone)
        {
            if (m_bvh)
            {
                m_device->DeleteBuffer(m_gpudata->bvh);
                m_device->DeleteBuffer(m_gpudata->vertices);
            }

            // Check if we can allocate enough stack memory
            Calc::DeviceSpec spec;
            m_device->GetSpec(spec);
            if (spec.max_alloc_size <= kMaxBatchSize * kMaxStackSize * sizeof(int))
            {
                throw ExceptionImpl("fatbvh accelerator can't allocate enough stack memory, try using bvh instead");
            }

            int numshapes = (int)world.shapes_.size();
            int numvertices = 0;
            int numfaces = 0;

            // This buffer tracks mesh start index for next stage as mesh face indices are relative to 0
            std::vector<int> mesh_vertices_start_idx(numshapes);
            std::vector<int> mesh_faces_start_idx(numshapes);

            auto builder = world.options_.GetOption("bvh.builder");
            auto splits = world.options_.GetOption("bvh.sah.use_splits");
            auto maxdepth = world.options_.GetOption("bvh.sah.max_split_depth");
            auto overlap = world.options_.GetOption("bvh.sah.min_overlap");
            auto tcost = world.options_.GetOption("bvh.sah.traversal_cost");
            auto node_budget = world.options_.GetOption("bvh.sah.extra_node_budget");
            auto nbins = world.options_.GetOption("bvh.sah.num_bins");

            bool use_sah = false;
            bool use_splits = false;
            int max_split_depth = maxdepth ? (int)maxdepth->AsFloat() : 10;
            int num_bins = nbins ? (int)nbins->AsFloat() : 64;
            float min_overlap = overlap ? overlap->AsFloat() : 0.05f;
            float traversal_cost = tcost ? tcost->AsFloat() : 10.f;
            float extra_node_budget = node_budget ? node_budget->AsFloat() : 0.5f;

            if (builder && builder->AsString() == "sah")
            {
                use_sah = true;
            }

            if (splits && splits->AsFloat() > 0.f)
            {
                use_splits = true;
            }

            m_bvh.reset(use_splits ?
                new SplitBvh(traversal_cost, num_bins, max_split_depth, min_overlap, extra_node_budget) :
                new Bvh(traversal_cost, num_bins, use_sah)
            );

            // Partition the array into meshes and instances
            std::vector<Shape const*> shapes(world.shapes_);

            auto firstinst = std::partition(shapes.begin(), shapes.end(),
                [&](Shape const* shape)
            {
                return !static_cast<ShapeImpl const*>(shape)->is_instance();
            });

            // Count the number of meshes
            int nummeshes = (int)std::distance(shapes.begin(), firstinst);
            // Count the number of instances
            int numinstances = (int)std::distance(firstinst, shapes.end());

            for (int i = 0; i < nummeshes; ++i)
            {
                Mesh const* mesh = static_cast<Mesh const*>(shapes[i]);

                mesh_faces_start_idx[i] = numfaces;
                mesh_vertices_start_idx[i] = numvertices;

                numfaces += mesh->num_faces();
                numvertices += mesh->num_vertices();
            }

            for (int i = nummeshes; i < nummeshes + numinstances; ++i)
            {
                Instance const* instance = static_cast<Instance const*>(shapes[i]);
                Mesh const* mesh = static_cast<Mesh const*>(instance->GetBaseShape());

                mesh_faces_start_idx[i] = numfaces;
                mesh_vertices_start_idx[i] = numvertices;

                numfaces += mesh->num_faces();
                numvertices += mesh->num_vertices();
            }


            // We can't avoild allocating it here, since bounds aren't stored anywhere
            std::vector<bbox> bounds(numfaces);

//.........这里部分代码省略.........

int main(int argc, char *argv[])
{
    using namespace Sord;

    int num = 1;

    if (argc > 1) {
        num = atoi(argv[1]);
    }

    World world;

    world.add_prefix("rdf", RDF(""));
    world.add_prefix("maths", MATHS(""));
    world.add_prefix("spatial", SPATIAL(""));
    world.add_prefix("tracking", TRACKING(""));
    world.add_prefix("vom", VOM(""));
    world.add_prefix("mea", MEA(""));
    world.add_prefix("xsd", XSD(""));

    Model model(world, "http://test.arvida.de/");

    std::cout << "Producing " << num << " poses" << std::endl;

    MIDDLEWARENEWSBRIEF_PROFILER_TIME_TYPE start, finish, elapsed;

    start = MIDDLEWARENEWSBRIEF_PROFILER_GET_TIME;

    for (int i = 0; i < num; ++i)
    {
        const std::string uuid_url = "http://test.arvida.de/UUID" + std::to_string(i);

        model.add_statement(
            URI(world, uuid_url),
            URI(world, RDF("type")),
            URI(world, SPATIAL("SpatialRelationship")));

        {
            Node n1 = Node::blank_id(world);

            model.add_statement(
                URI(world, uuid_url),
                URI(world, SPATIAL("sourceCoordinateSystem")),
                n1
            );

            model.add_statement(
                n1,
                URI(world, RDF("type")),
                URI(world, MATHS("LeftHandedCartesianCoordinateSystem3D")));
        }


        {
            Node n1 = Node::blank_id(world);

            model.add_statement(
                URI(world, uuid_url),
                URI(world, SPATIAL("targetCoordinateSystem")),
                n1
            );

            model.add_statement(
                n1,
                URI(world, RDF("type")),
                URI(world, MATHS("RightHandedCartesianCoordinateSystem2D"))
            );
        }

        URI xsd_double(world, XSD("double"));

        {

            // translation
            Node n1 = Node::blank_id(world);
            Node n2 = Node::blank_id(world);

            model.add_statement(
                URI(world, uuid_url),
                URI(world, SPATIAL("translation")),
                n1
            );

            model.add_statement(
                n1,
                URI(world, RDF("type")),
                URI(world, SPATIAL("Translation3D"))
            );

            model.add_statement(
                n1,
                URI(world, VOM("quantityValue")),
                n2
            );

            model.add_statement(
                n2,
                URI(world, RDF("type")),
                URI(world, MATHS("Vector3D"))
            );
//.........这里部分代码省略.........

    void WorldTest::simulationTestUncached() {
        World* world = new World(Vector3(200000, 200000, 200000));
        world->setUseCollisionCaching(false);
        Vector3 boxDimensions(10, 10, 10);
        Proxy* box1 = world->createProxy(new Box(boxDimensions));
        //Proxy* childBox = world->createProxy(new Box(boxDimensions));
        //childBox->translate(5, 5, 5);
        //box->addChild(childBox);

        world->addProxy(box1);

        Proxy* box2 = world->createProxy(new Box(boxDimensions));
        box2->translate(0, -20, 0);
        world->addProxy(box2);

        Proxy* box3 = world->createProxy(new Box(boxDimensions)); //always colliding with box2
        box3->translate(5, -25, 0);
        world->addProxy(box3);

        
        Proxy* moveBox = world->createProxy(new Box(Vector3(10, 50, 10)));
        moveBox->translate(20, -25, 0); //starting in no-collision state
        world->addProxy(moveBox);

        //WorldCollisions coll;
        
        world->prepareSimulation();
//std::cout << "-----------Starting simulation test-----------"<<std::endl;
//std::cout << "doing pre-move collisionCheck"<<std::endl;
        //test and ensure 1 BPC and rigid C
        WorldCollisions coll0 = world->calculateAllCollisions();
        CPPUNIT_ASSERT_EQUAL(1,
                (int)coll0.getBroadPhaseCollisions()->getResults().size());
        CPPUNIT_ASSERT_EQUAL(1, (int)coll0.getRigidBoundingVolumeCollisions().size());

        //Step 1 -> move moveBox to collidy with box3
//std::cout <<std::endl<< "doing step 1 - move in to collide with box3"<<std::endl;
        moveBox->translate(-6, 0, 0);

        //ensure 2 BPC/RC
        WorldCollisions coll1  = world->calculateAllCollisions();
        CPPUNIT_ASSERT_EQUAL(2,
                (int)coll1.getBroadPhaseCollisions()->getResults().size());
        CPPUNIT_ASSERT_EQUAL(2, (int)coll1.getRigidBoundingVolumeCollisions().size());

        //Step 2-> move to collide with all boxes
//std::cout <<std::endl<<"doing step 2 - move further to collide with all"<<std::endl;
        moveBox->translate(-5, 0, 0);
        //ensure 4 collisions
        WorldCollisions coll2  = world->calculateAllCollisions();
        CPPUNIT_ASSERT_EQUAL(4,
                (int)coll2.getBroadPhaseCollisions()->getResults().size());
        CPPUNIT_ASSERT_EQUAL(4, (int)coll2.getRigidBoundingVolumeCollisions().size());

        //Step 3-> move out again
//std::cout << std::endl<<"doing step 3 - moving out"<<std::endl;
        moveBox->translate(11, 0, 0);
        //ensure 1 collisions
        WorldCollisions coll3  = world->calculateAllCollisions();
        CPPUNIT_ASSERT_EQUAL(1,
                (int)coll3.getBroadPhaseCollisions()->getResults().size());
        CPPUNIT_ASSERT_EQUAL(1, (int)coll3.getRigidBoundingVolumeCollisions().size());

        //Step 4-> move in again
//std::cout << std::endl<<"doing step 4 - moving back in"<<std::endl;
        moveBox->translate(-11, 0, 0);
        //ensure 4 collisions
        WorldCollisions coll4  = world->calculateAllCollisions();
        CPPUNIT_ASSERT_EQUAL(4,
                (int)coll4.getBroadPhaseCollisions()->getResults().size());
        CPPUNIT_ASSERT_EQUAL(4, (int)coll4.getRigidBoundingVolumeCollisions().size());

        delete world;
    }

//-----------------------------------------------------------------------------
void InputManager::handleStaticAction(int key, int value)
{
    static bool control_is_pressed = false;
    World *world = World::getWorld();

    // When no players... a cutscene
    if (race_manager->getNumPlayers() == 0 && world != NULL && value > 0 &&
        (key == KEY_SPACE || key == KEY_RETURN))
    {
        world->onFirePressed(NULL);
    }

    switch (key)
    {
#ifdef DEBUG
        // Special debug options for profile mode: switch the 
        // camera to show a different kart.
        case KEY_KEY_1:
        case KEY_KEY_2:
        case KEY_KEY_3:
        case KEY_KEY_4:
        case KEY_KEY_5:
        case KEY_KEY_6:
        case KEY_KEY_7:
        case KEY_KEY_8:
        case KEY_KEY_9:
        {
            if(!ProfileWorld::isProfileMode() || !world) break;
            int kart_id = key - KEY_KEY_1;
            if(kart_id<0 || kart_id>=(int)world->getNumKarts()) break;
            for(unsigned int i=0; i<world->getNumKarts(); i++)
            {
                if(world->getKart(i)->getCamera())
                {
                    world->getKart(i)->getCamera()
                                     ->changeOwner(world->getKart(kart_id));

                }
            }
            break;
        }
#endif
        case KEY_CONTROL:
        case KEY_RCONTROL:
        case KEY_LCONTROL:
        case KEY_RMENU:
        case KEY_LMENU:
        case KEY_LWIN:
            control_is_pressed = value!=0;
            break;

        case KEY_KEY_I:
        {
            if (!world || !UserConfigParams::m_artist_debug_mode) break;
            
            AbstractKart* kart = world->getLocalPlayerKart(0);
            if (kart == NULL) break;
            
            kart->flyUp();
            break;
        }
        case KEY_KEY_K:
        {
            if (!world || !UserConfigParams::m_artist_debug_mode) break;
            
            AbstractKart* kart = world->getLocalPlayerKart(0);
            if (kart == NULL) break;
            
            kart->flyDown();
            break;
        }
        case KEY_PRINT:
            if (value != 0)
                irr_driver->requestScreenshot();
            break;
            
        case KEY_F1:
            if (UserConfigParams::m_artist_debug_mode && world)
            {
                AbstractKart* kart = world->getLocalPlayerKart(0);
                kart->setPowerup(PowerupManager::POWERUP_BUBBLEGUM, 10000);
#ifdef FORCE_RESCUE_ON_FIRST_KART
                // Can be useful for debugging places where the AI gets into
                // a rescue loop: rescue, drive, crash, rescue to same place
                world->getKart(0)->forceRescue();
#endif
                // FIXME: remove after testing the animated plunger
                world->getLocalPlayerKart(0)->blockViewWithPlunger();
            }
            break;
        case KEY_F2:
            if (UserConfigParams::m_artist_debug_mode && world)
            {
                AbstractKart* kart = world->getLocalPlayerKart(0);
                kart->setPowerup(PowerupManager::POWERUP_PLUNGER, 10000);
            }
            break;
        case KEY_F3:
            if (UserConfigParams::m_artist_debug_mode && world)
//.........这里部分代码省略.........

/*! Will also load the hand, even though the hand is not explicitly used. It is needed for
  the grasp allocator, plus it might be needed for retrieving DOF values, for example if
  the grasp is stored in the database as eigengrasp values.
*/
void GraspClusteringTask::start()
{
  //get the details of the planning task itself
  if (!mDBMgr->GetPlanningTaskRecord(mRecord.taskId, &mPlanningTask)) {
    DBGA("Failed to get planning record for task");
    mStatus = FAILED;
    return;
  }

  World *world = graspitCore->getWorld();
  Hand *hand;

  //check if the currently selected hand is the same as the one we need
  //if not, load the hand
  if (world->getCurrentHand() && world->getCurrentHand()->getDBName() == QString(mPlanningTask.handName.c_str())) {
    DBGA("Grasp Planning Task: using currently loaded hand");
    hand = world->getCurrentHand();
  } else {
    QString handPath = mDBMgr->getHandGraspitPath(QString(mPlanningTask.handName.c_str()));
    handPath = QString(getenv("GRASPIT")) + handPath;
    DBGA("Grasp Planning Task: loading hand from " << handPath.latin1());
    hand = static_cast<Hand *>(world->importRobot(handPath));
    if (!hand) {
      DBGA("Failed to load hand");
      mStatus = FAILED;
      return;
    }
  }
  mDBMgr->SetGraspAllocator(new GraspitDBGraspAllocator(hand));

  //load all the grasps
  std::vector<db_planner::Grasp *> graspList;
  if (!mDBMgr->GetGrasps(*(mPlanningTask.model), mPlanningTask.handName, &graspList)) {
    DBGA("Load grasps failed");
    mStatus = FAILED;
    while (!graspList.empty()) {
      delete graspList.back();
      graspList.pop_back();
    }
    return;
  }

  //sort grasps by energy (hard-coded in, maybe later we'll allow other sorting)
  std::sort(graspList.begin(), graspList.end(), db_planner::Grasp::CompareEnergy);

  //if all goes well, we are done
  mStatus = DONE;
  int clusters = 0;
  DBGA("Clustering " << graspList.size() << " grasps");

  while (!graspList.empty()) {
    //pop the front (best grasp)
    db_planner::Grasp *repGrasp = graspList.front();
    graspList.erase(graspList.begin());

    //compliant_copy grasps are ignored by clustering tasks
    if (repGrasp->CompliantCopy()) {
      delete repGrasp;
      continue;
    }

    //mark it as cluster center in the database
    if (!mDBMgr->SetGraspClusterRep(repGrasp, true)) {
      DBGA("Failed to mark cluster rep in database");
      mStatus = FAILED;
      delete repGrasp;
      break;
    }
    clusters++;

    //find other grasps in its cluster
    int cloud = 0;
    std::vector<db_planner::Grasp *>::iterator it = graspList.begin();
    while (it != graspList.end()) {

      //compliant_copy grasps are ignored by clustering tasks
      if (!(*it)->CompliantCopy() &&
          clusterGrasps(static_cast<GraspitDBGrasp *>(repGrasp), static_cast<GraspitDBGrasp *>(*it))) {
        (*it)->SetClusterRep(false);
        //mark it as non-center in the database
        if (!mDBMgr->SetGraspClusterRep(*it, false)) {
          DBGA("Failed to mark non-cluster rep in database");
          mStatus = FAILED;
          break;
        }
        cloud++;
        delete *it;
        it = graspList.erase(it);
      } else {
        it++;
      }

    }
    DBGA("  Marked cluster of size " << cloud);
    delete repGrasp;
    if (mStatus == FAILED) { break; }
//.........这里部分代码省略.........

bool Socket::Flush()
{
    int b=0;
    int w=0;
    int status = Z_OK;
    int length = 0;
    int i = 0;
    World* world = World::GetPtr();

    if (!_outBuffer.length())
        {
            return true;
        }
//prepend buffer to prompt
    _totalSent += _outBuffer.length();
    if ((_mobile!=NULL)&&(_con==ConnectionType::Game))
        {
            _outBuffer+="\r\n"+world->BuildPrompt(_mobile->GetPrompt(), _mobile)+TELNET_IAC+TELNET_GA;
        }

    if (!_compressing)
        {
            //we are not compressing outbound data.
            while (_outBuffer.length() > 0)
                {
                    b = _outBuffer.length();
                    // any write failures ?
                    if (_control!=-1)
                        {
                            if ((w = send(_control, _outBuffer.c_str(), b, 0)) == -1)
                                {
                                    return false;
                                }
                        }
                    // move the buffer down
                    _outBuffer.erase(0, w);
                }
        } //end sending raw data
    else
        {
            //we are compressing, wheee!
            unsigned char* buff= new unsigned char[_outBuffer.length()];

            zstream.avail_in = _outBuffer.length();
            zstream.next_in = (unsigned char*)const_cast<char*>(_outBuffer.c_str());
            zstream.next_out = buff;

            while (zstream.avail_in)
                {
                    zstream.avail_out = _outBuffer.length() -(zstream.next_out-buff);
                    if (zstream.avail_out)
                        {
                            status = deflate(&zstream, Z_SYNC_FLUSH);
                            if (status != Z_OK)
                                {
                                    delete []buff;
                                    return false;
                                }
                        }
                }
            length = zstream.next_out-buff;
            if (length)
                {
                    _compressedSent += length;
                    b = 0;
                    for (i = 0; i < length; i +=b)
                        {
                            w = Min<int>(length-i, 4096);
                            b = send(_control, buff+i, w, 0);
                        }
                }
            _outBuffer.clear();
            delete [] buff;
        }

    return true;
}

/** Draws the mini map and the position of all karts on it.
 */
void RaceGUI::drawGlobalMiniMap()
{
#ifndef SERVER_ONLY
    // draw a map when arena has a navigation mesh.
    Track *track = Track::getCurrentTrack();
    if ( (track->isArena() || track->isSoccer()) && !(track->hasNavMesh()) )
        return;

    int upper_y = irr_driver->getActualScreenSize().Height - m_map_bottom - m_map_height;
    int lower_y = irr_driver->getActualScreenSize().Height - m_map_bottom;

    core::rect<s32> dest(m_map_left, upper_y,
                         m_map_left + m_map_width, lower_y);

    track->drawMiniMap(dest);

    World *world = World::getWorld();
    for(unsigned int i=0; i<world->getNumKarts(); i++)
    {
        const AbstractKart *kart = world->getKart(i);
        const SpareTireAI* sta =
            dynamic_cast<const SpareTireAI*>(kart->getController());
        // don't draw eliminated kart
        if(kart->isEliminated() && !(sta && sta->isMoving())) continue;
        const Vec3& xyz = kart->getXYZ();
        Vec3 draw_at;
        track->mapPoint2MiniMap(xyz, &draw_at);
        draw_at *= UserConfigParams::m_scale_rtts_factor;

        video::ITexture* icon = sta ?
            irr_driver->getTexture(FileManager::GUI, "heart.png") :
            kart->getKartProperties()->getMinimapIcon();

        // int marker_height = m_marker->getSize().Height;
        core::rect<s32> source(core::position2di(0, 0), icon->getSize());
        int marker_half_size = (kart->getController()->isLocalPlayerController()
                                ? m_minimap_player_size
                                : m_minimap_ai_size                        )>>1;
        core::rect<s32> position(m_map_left+(int)(draw_at.getX()-marker_half_size),
                                 lower_y   -(int)(draw_at.getY()+marker_half_size),
                                 m_map_left+(int)(draw_at.getX()+marker_half_size),
                                 lower_y   -(int)(draw_at.getY()-marker_half_size));
        draw2DImage(icon, position, source, NULL, NULL, true);
    }   // for i<getNumKarts

    SoccerWorld *sw = dynamic_cast<SoccerWorld*>(World::getWorld());
    if (sw)
    {
        Vec3 draw_at;
        track->mapPoint2MiniMap(sw->getBallPosition(), &draw_at);
        draw_at *= UserConfigParams::m_scale_rtts_factor;
        video::ITexture* icon =
            irr_driver->getTexture(FileManager::GUI, "soccer_ball_normal.png");

        core::rect<s32> source(core::position2di(0, 0), icon->getSize());
        core::rect<s32> position(m_map_left+(int)(draw_at.getX()-(m_minimap_player_size/2.5f)),
                                 lower_y   -(int)(draw_at.getY()+(m_minimap_player_size/2.5f)),
                                 m_map_left+(int)(draw_at.getX()+(m_minimap_player_size/2.5f)),
                                 lower_y   -(int)(draw_at.getY()-(m_minimap_player_size/2.5f)));
        draw2DImage(icon, position, source, NULL, NULL, true);
    }
#endif
}   // drawGlobalMiniMap

int main()
{
    // Initialize globals
    initGlobals();

    // Window
    window.setSize(sf::Vector2u(window_width, window_height));
    window.setPosition(sf::Vector2i(200, 200));
    window.setFramerateLimit(FRAMES_PER_SECOND);
    //window.setVerticalSyncEnabled(true);
    window.setKeyRepeatEnabled(false);

    // Camera view
    sf::View windowView;

    // Menu
    initializeMenu();

    // UI
    ui.init();

    // Minimap
//    Minimap minimap;

    // Create & Set contactlistener
    BoxContactListener boxContactListener;
    physicsWorld.SetContactListener(&boxContactListener);

    // Build world
    //gameWorld.generateWorld(physicsWorld);

    // ------------------------------- MAIN LOOP -------------------------------
    while(window.isOpen())
    {
        // ------------------------------- Input & Views -------------------------------
        sf::Event event;
        gameEvents.processEvents(window, event);

        // Update view in case of window resize
        window_width = window.getSize().x;
        window_height = window.getSize().y;
        windowView.setSize(window_width, window_height);

//        if(minimap.updateViewThroughMinimap)
//        {
//            windowView.setCenter(minimap.newViewCenter.x, minimap.newViewCenter.y);
//        }

        if(player.hasNewFocus)
        {
            windowView.setCenter(player.getNewFocus().x, player.getNewFocus().y);
        }

        // Update normal view with inputs
        windowView = updateView(windowView);

        if(gameWorld.completionStarted && gameWorld.completionTimer.timeReached())
        {
            windowView.setCenter(window_width/2, window_height/2);
            global_levelComplete = false;
        }

        // Clear window
        window.clear(sf::Color(255, 255, 255, 255));

        if(global_isMenu)
        {
            ui.setSize(window_width, window_height);
            window.setView(ui.getView());

            menu_logo_bottom->setPosition(window_width - textures["menu_logo_bottom"].getSize().x, window_height - textures["menu_logo_bottom"].getSize().y);

            menu.update(gameEvents);
            menu.draw(window);

            // Instructions
            if(show_instructions)
            {
                showInstructions();

                if(show_instructions_controls)
                {
                    showControls();
                }

                if(show_instructions_gameplay)
                {
                    showGameplay();
                }
            }
        }
        else
        {
            window.setView(windowView);

            // ------------------------------- Updates -------------------------------

            // Player
            player.update(physicsWorld, gameEvents, gameWorld);

//.........这里部分代码省略.........

	void Server::broadcastEntity(Entities::Entity* e)
	{
		World* w = e->getWorld();
		for (size_t i = 0; i < w->playerids.size(); i++)
			w->getPlayer(w->playerids.at(i))->getClientCon()->sendEntity(e);
	}

		//Overloaded Create 
		void JointFixed::Create(World &world, JointGroup &jointGroup)
		{
			if(this->_id) dJointDestroy(this->_id);
			_id = dJointCreateFixed(world.Id(), jointGroup.Id());
		}

	void Server::broadcastPickupSpawn(Entities::Pickup* p)
	{
		World* w  = p->getWorld();
		for (size_t i = 0; i < w->playerids.size(); i++)
			w->getPlayer(w->playerids.at(i))->getClientCon()->sendPickupSpawn(p);
	}

bool TMXMapImporter::buildWorldFromInfo(Game *game)
{
	TextureManager *worldTextureManager = game->getGraphics()->getWorldTextureManager();
	if (mapType == MapType::ORTHOGONAL_MAP)
	{
		World *world = game->getGSM()->getWorld();
		int largestLayerWidth = 0;
		int largestLayerHeight = 0;

		// LET'S FIRST FIGURE OUT THE WORLD WIDTH AND HEIGHT

		// FIRST THE IMAGE LAYERS
		map<string, ImageLayerInfo>::const_iterator iliIt = imageLayerInfos.begin();
		while (iliIt != imageLayerInfos.end())
		{
			string key = iliIt->first;
			ImageLayerInfo ili = imageLayerInfos[key];
			if (ili.imagewidth > largestLayerWidth)
				largestLayerWidth = ili.imagewidth;
			if (ili.imageheight > largestLayerHeight)
				largestLayerHeight = ili.imageheight;

			iliIt++;
		}
		// AND THE TILED LAYERS
		map<string, TiledLayerInfo>::const_iterator tliIt = tiledLayerInfos.begin();
		while (tliIt != tiledLayerInfos.end())
		{
			string key = tliIt->first;
			TiledLayerInfo tli = tiledLayerInfos[key];
			int layerWidth = tli.width * tli.tileSetInfo->tilewidth;
			if (layerWidth > largestLayerWidth)
				largestLayerWidth = layerWidth;
			int layerHeight = tli.height * tli.tileSetInfo->tileheight;
			if (layerHeight > largestLayerHeight)
				largestLayerHeight = layerHeight;
			tliIt++;
		}
		unsigned int idOffset = worldTextureManager->getWStringTable()->getNumWStringsInTable();

		// FIRST LOAD ALL THE TILE SETS
		map<string, TileSetInfo>::const_iterator tsiIt = tileSetInfos.begin();
		while (tsiIt != tileSetInfos.end())
		{
			string key = tsiIt->first;
			TileSetInfo tsi = tileSetInfos[key];
			wstring sourceImageW(tsi.sourceImage.begin(), tsi.sourceImage.end());
			bool success = worldTextureManager->loadTileSetFromTexture(game, dir, sourceImageW, tsi.tilewidth, tsi.tileheight);
			if (!success) return false;
			tsiIt++;
		}

		// NOW LOAD THE IMAGE LAYERS, IF THERE ARE ANY
		iliIt = imageLayerInfos.begin();
		while (iliIt != imageLayerInfos.end())
		{
			string key = iliIt->first;
			ImageLayerI