C++ CollisionDetector类代码示例

        void CollisionDestroyPositioner::affect(float ms)
        {
            Entity *entity = getEntity();
            CollisionDetector *collisionDetector =
                    entity->getLevel()->getCollisionDetector();

            /* Convert vectors to ellipse space. */
            Vector2D position = entity->getPosition();
            Vector2D velocity = entity->getVelocity() * ms;

            /* Used later. */
            float time;
            Vector2D collision;

            if(collisionDetector->getClosestCollision(
                    entity->getRadius(), position, velocity,
                    &time, &collision)) {
                entity->setGarbage();
                entity->setPosition(collision);
            } else {
                entity->setPosition(position + velocity);
            }

            velocity = entity->getVelocity() + getGravity() * ms;
            entity->setVelocity(velocity);
        }

/*
Method is responsible for runtime checks:
	- Collision detection
	- Food eaten
	- Drawing
	Return(true) -> GameOver
	Return(false)-> Keep running
*/
bool GameController::performGameRuntimeChecks(CInputManager &inputManager,
											  unsigned long &score, CollisionDetector &collisionDetector,
											  DrawEngine drawEngine, int key, Snake &snake, vector<Cell> &foodList,
											  FoodProvider &foodProvider, Cell lastCell)
{
	//If collision detected
	if(collisionDetector.collisionDetected(snake))
	{
		char message[] = "GAME OVER! <Press any key>";
		drawEngine.showMessage(message);
		inputManager.GetNextKeyWait(&key);
		return true;
	}
	//Food eaten
	else if(collisionDetector.foodEaten(snake, foodList))
	{
		//Setting new score
		score += snake.getCells().size()*2;
		//Draw score
		drawEngine.updateScore(score);
		//Add cell
		snake.addCell();
		//Recieving new foodList
		vector<Cell> &list = foodProvider.provideFood(snake, drawEngine.getWindownWidth(), 
			drawEngine.getMenuBorderHeight());
		foodList = list;
	}
	//Draw snake
	drawEngine.drawSnake(snake, lastCell);
	//Draw food
	drawEngine.drawFood(foodList);
	return false;
}

void GameController::initGame()
{
	//Initializing variables
	DrawEngine drawEngine;
	CInputManager inputManager;
	CLoopTimer loopTimer;
	CollisionDetector collisionDetector;
	FoodProvider foodProvider;
	GameStatus gameStatus;
	
	//Initializing console
	drawEngine.initialize();
	collisionDetector.initialize(drawEngine.getWindownWidth(), drawEngine.getMenuBorderHeight());
	//Set start values
	gameStatus.setGameStatus(gameStatus.MAIN_MENU);
	unsigned long score = 0;
	
	while(gameStatus.getGameStatus() != gameStatus.QUIT)
	{
		//Runs main menu
		if(gameStatus.getGameStatus() == gameStatus.MAIN_MENU)
		{
			drawEngine.showMenu(inputManager, gameStatus);
		}
		//Run game
		else
		{
			score = 0;
			runGame(inputManager, loopTimer, score, collisionDetector, foodProvider, drawEngine);
			gameStatus.setGameStatus(gameStatus.MAIN_MENU);
		}
	}
}

void Player::handleDestructableCollisions(std::vector<Destructable*> destructables, Camera* camera)
{
	CollisionDetector detector;
	for(int i = 0; i < destructables.size(); ++i)
	{
		for(int j = 0; j < collisionPoints.size(); ++j)
		{
			CollisionPoint before = collisionPoints.at(j);
			CollisionPoint translatedPoint(before.x + movementVector.x, before.y + movementVector.y, before.type);
			if (detector.checkCollision(translatedPoint, destructables.at(i), camera))
			{
				switch (collisionPoints.at(j).type)
				{
					case HEAD: 
						movementVector.y = destructables.at(i)->getBoundingBox().y + destructables.at(i)->getBoundingBox().h + 1;
						break;
					case FEET:
						movementVector.y = destructables.at(i)->getBoundingBox().y - m_h;
						m_airbourne = false;
						break;
					case LEFT:
						movementVector.x = destructables.at(i)->getBoundingBox().x + destructables.at(i)->getBoundingBox().w + 1; // + camera->getOffsetX();
						break;
					case RIGHT:
						movementVector.x = destructables.at(i)->getBoundingBox().x - m_w - 1; // + camera->getOffsetX();
						break;
				}
			}
		}
	}
}

void cppCollisionDetection(double delta)  
{
	s0 = new Surface();
	s1 = new Surface();
	h0 = new Hierarchy();
	h1 = new Hierarchy();

	s0->Add(0,0,0,1,0);
	s0->CreateSurface();
	s1->Add(0,0,0,1,0);
	s1->CreateSurface();

	Triple t0(-delta,0,0);
	Triple t1(delta,0,0);
	RotationMatrix m0,m1;
	h0->Initialize(s0);
	h0->Behavior(&m0,&t0);
	h1->Initialize(s1);
	h1->Behavior(&m1,&t1);

	CollisionDetector cd;
	cd.CheckCollision(h0,h1);
	cd.Select(1);

	s->CreateSurface(s0,s1,delta); 

	delete h0;
	delete h1;

	delete s0;
	delete s1;
}

TEST_F(CollisionDetectorTest, projectile_projectile) {
    CollisionDetector detector;
    NiceMock<MockGameObject> proj_1;
    NiceMock<MockHitbox> proj_1_hitbox;
    NiceMock<MockGameObject> proj_2;
    NiceMock<MockHitbox> proj_2_hitbox;
    ON_CALL(proj_1, hitbox()).WillByDefault(ReturnRef(proj_1_hitbox));
    ON_CALL(proj_1_hitbox, is_overlapping(Ref(proj_2_hitbox))).WillByDefault(Return(true));
    ON_CALL(proj_2, hitbox()).WillByDefault(ReturnRef(proj_2_hitbox));
    ON_CALL(proj_2_hitbox, is_overlapping(Ref(proj_1_hitbox))).WillByDefault(Return(true));
    EXPECT_CALL(proj_1, notify_collision(Ref(proj_2))).Times(0);
    EXPECT_CALL(proj_2, notify_collision(Ref(proj_1))).Times(0);
    detector.add_projectile(Team::FRIENDLY, proj_1);
    detector.add_projectile(Team::ENEMY, proj_2);
    detector.check_collisions();
}

void SimpleCollisionHandler::handleCollisions(TwoDScene &scene, CollisionDetector &detector, const VectorXs &oldpos, VectorXs &oldvel, scalar dt)
{
  class SimpleCollisionCallback : public DetectionCallback
  {
  public:
    SimpleCollisionCallback(TwoDScene &scene, SimpleCollisionHandler &handler) : scene(scene), handler(handler) {}

    virtual void ParticleParticleCallback(int idx1, int idx2)
    {
      VectorXs n(2);
      if(handler.detectParticleParticle(scene, idx1, idx2, n))
	{
	  handler.addParticleParticleImpulse(idx1, idx2, n, 0);
	  handler.respondParticleParticle(scene, idx1, idx2, n);
	}
    }
    
    virtual void ParticleEdgeCallback(int vidx, int eidx)
    {
      VectorXs n(2);
      if(handler.detectParticleEdge(scene, vidx, eidx, n))
	{
	  handler.addParticleEdgeImpulse(vidx, eidx, n, 0);
	  handler.respondParticleEdge(scene, vidx, eidx, n);
	}
    }
    
    virtual void ParticleHalfplaneCallback(int vidx, int hidx)
    {
      VectorXs n(2);
      if(handler.detectParticleHalfplane(scene, vidx, hidx, n))
	{
	  handler.addParticleHalfplaneImpulse(vidx, hidx, n, 0);
	  handler.respondParticleHalfplane(scene, vidx, hidx, n);
	}
    }

    TwoDScene &scene;
    SimpleCollisionHandler &handler;
  };

  SimpleCollisionCallback callback(scene, *this);
  detector.performCollisionDetection(scene, scene.getX(), scene.getX(), callback);
}

bool checkCollisions(Device::Ptr device, const State &state, const CollisionDetector &detector, const Q &q) {
	State testState;
	CollisionDetector::QueryResult data;
	bool colFrom;

	testState = state;
	device->setQ(q,testState);
	colFrom = detector.inCollision(testState,&data);
	if (colFrom) {
		cerr << "Configuration in collision: " << q << endl;
		cerr << "Colliding frames: " << endl;
		FramePairSet fps = data.collidingFrames;
		for (FramePairSet::iterator it = fps.begin(); it != fps.end(); it++) {
			cerr << (*it).first->getName() << " " << (*it).second->getName() << endl;
		}
		return false;
	}
	return true;
}

//.........这里部分代码省略.........
    for (auto j = (*it).edge_begin(); j != (*it).edge_end(); ++j){
       (*j).value().L = (*j).length();
       (*j).value().K = 16000;
    }
  }


  // Launch the SDLViewer
  CS207::SDLViewer viewer;
  auto node_map = viewer.empty_node_map(mesh);
  viewer.launch();

  viewer.add_nodes(mesh.node_begin(), mesh.node_end(), node_map);
  viewer.add_edges(mesh.edge_begin(), mesh.edge_end(), node_map);
  viewer.add_nodes(mesh2.node_begin(), mesh2.node_end(), node_map);
  viewer.add_edges(mesh2.edge_begin(), mesh2.edge_end(), node_map);

  viewer.center_view();
  
  //Begin the mass-spring simulation
  double dt = 0.0002;
  double t_start = 0.0;
  double t_end   = 10.0;
   
  //three color parameter
  int color1 = 1; 
  int color2 = 1; 
  int color3 = 1; 
  
  //Create listener
  Pause_listener* pause = new Pause_listener(dt); 
  Speed_listener* speed = new Speed_listener(dt, dt); 
  XYZ_listener<MeshType>* xyz = new XYZ_listener<MeshType>(&mesh);
  Color_listener* col = new Color_listener(&color1, &color2, &color3);
  
  //add listener
  viewer.add_listener(pause);
  viewer.add_listener(speed);
  viewer.add_listener(xyz);
  viewer.add_listener(col);

  //Initialize forces
  WindForce wind_force(Point(0,100,200));
  PressureForce<typename MeshType::node_type, MeshType> pressure_force(1, 2500, &mesh);
  DampingForce damp_force(float(1)/mesh.num_nodes());
  //auto force = make_combined_force(MassSpringForce(), GravityForce(), make_combined_force(pressure_force, damp_force, wind_force));
  auto force = make_combined_force(MassSpringForce(), make_combined_force(pressure_force, damp_force, wind_force));
  
  //Initialize constriants
  auto constraint = PlaneConstraint<MeshType>(-4);

  //simulation processing   
  for (double t = t_start; t < t_end; t += dt) {
    constraint(mesh, 0);
    constraint(mesh2, 0);
    //define a collision constrain
    auto collision_constrain = CollisionConstraint<MeshType>();
    //add the forces to the meshs at each dt
    symp_euler_step(mesh, t, dt, force);
    symp_euler_step(mesh2, t, dt, force);
    
    //detec the collision betweent the two meshes
    CollisionDetector<MeshType> c;
    c.add_object(mesh);
    c.add_object(mesh2);
    c.check_collisions();
    std::vector<unsigned> collision;
    std::vector<unsigned> collision2;

    //find the corresponding mesh for each node
    for (auto it=c.begin(); it!= c.end(); ++it){
      auto boom = *it;
      Node n = boom.n1;
      if (boom.mesh1 == &mesh)
        collision.push_back(n.index());
      if (boom.mesh1 == &mesh2)
        collision2.push_back(n.index());
    }

    //add the collision constrain to the meshes
    collision_constrain(mesh,mesh2,collision,collision2);
    viewer.set_label(t);
    
    //update with removed nodes
    //clear teh viewer's node
    viewer.clear();
    node_map.clear();
    //update viewer with new positions and new edges
    viewer.add_nodes(mesh.node_begin(), mesh.node_end(), color(color1, color2, color3), node_map);
    viewer.add_edges(mesh.edge_begin(), mesh.edge_end(), node_map);
    viewer.add_nodes(mesh2.node_begin(), mesh2.node_end(), color(color1, color2, color3), node_map);
    viewer.add_edges(mesh2.edge_begin(), mesh2.edge_end(), node_map);

    // These lines slow down the animation for small graphs
    if (mesh.num_nodes() < 100)
      CS207::sleep(0.001);
  }

  return 0;
}

int main(int argc, char* args[]) {
	Renderer* renderer = new Renderer();

	if(!renderer->init()) {
		printf( "Failed to initialize!\n" );
	} else {
		Character* c = new Character(new Texture("res/img/dot.bmp", 200, 200));
		renderer->addTexture(new Texture("res/img/waterlevel2.png", 0, 0));

		renderer->render();

		renderer->addTexture(c->getTexture());

		bool quit = false;

		SDL_Event e;

		LoadCollisionMap lcm;
		list<Circle> circlesList;
		circlesList = lcm.load();

		list<Circle> l;
		Circle c1(200,0,10);
		l.push_back(c1);

		CollisionDetector cd;
		int y = 0;

		while(!cd.hasCollision(circlesList, l)) {
			l.pop_back();
			y += 1;
			Circle c1(200,y,10);
			l.push_back(c1);
		}
		y -= 1;
		c->setPosX(200);
		c->setPosY(y);

		renderer->render();

		int mVelX = 0;
		int DOT_VEL = 1;

		while(!quit) {
			while(SDL_PollEvent(&e) != 0) {
				if(e.type == SDL_QUIT) {
					quit = true;
				}
				if(e.type == SDL_KEYDOWN && e.key.repeat == 0) {
					switch(e.key.keysym.sym) {
						case SDLK_LEFT: mVelX -= DOT_VEL; break;
						case SDLK_RIGHT: mVelX += DOT_VEL; break;
					}
				} else if(e.type == SDL_KEYUP && e.key.repeat == 0) {
					switch(e.key.keysym.sym) {
						case SDLK_LEFT: mVelX += DOT_VEL; break;
						case SDLK_RIGHT: mVelX -= DOT_VEL; break;
					}
				}
			}
			c->setPosX(c->getPosX() + mVelX);


			LoadCollisionMap lcm;
			list<Circle> circlesList;
			circlesList = lcm.load();

			int mPosX = c->getPosX();
			int mPosY = c->getPosY();

			list<Circle> l;
			Circle c1(mPosX,mPosY,10);
			l.push_back(c1);

			CollisionDetector cd;
			int y = mPosY;
			while(cd.hasCollision(circlesList, l)) {
				l.pop_back();
				y -= 1;
				Circle c1(mPosX,y,10);
				l.push_back(c1);
			};
			while(!cd.hasCollision(circlesList, l)) {
				l.pop_back();
				y += 1;
				Circle c1(mPosX,y,10);
				l.push_back(c1);
			};

			c->setPosY(y);

			renderer->render();
		}
	}
	return 0;
}

//==============================================================================
void ConstraintTest::SingleContactTest(const std::string& _fileName)
{
  using namespace std;
  using namespace Eigen;
  using namespace dart::math;
  using namespace dart::collision;
  using namespace dart::constraint;
  using namespace dart::dynamics;
  using namespace dart::simulation;
  using namespace dart::utils;

  //----------------------------------------------------------------------------
  // Settings
  //----------------------------------------------------------------------------
  // Number of random state tests for each skeletons
#ifndef NDEBUG  // Debug mode
  // size_t testCount = 1;
#else
  // size_t testCount = 1;
#endif

  World* world = new World;
  EXPECT_TRUE(world != NULL);
  world->setGravity(Vector3d(0.0, -10.00, 0.0));
  world->setTimeStep(0.001);
  world->getConstraintSolver()->setCollisionDetector(
        new DARTCollisionDetector());

  Skeleton* sphereSkel = createSphere(0.05, Vector3d(0.0, 1.0, 0.0));
  BodyNode* sphere = sphereSkel->getBodyNode(0);
  Joint* sphereJoint = sphere->getParentJoint();
  sphereJoint->setVelocity(3, random(-2.0, 2.0));  // x-axis
  sphereJoint->setVelocity(5, random(-2.0, 2.0));  // z-axis
  world->addSkeleton(sphereSkel);
  EXPECT_EQ(sphereSkel->getGravity(), world->getGravity());
  assert(sphere);

  Skeleton* boxSkel = createBox(Vector3d(1.0, 1.0, 1.0),
                                Vector3d(0.0, 1.0, 0.0));
  BodyNode* box = boxSkel->getBodyNode(0);
  Joint* boxJoint = box->getParentJoint();
  boxJoint->setVelocity(3, random(-2.0, 2.0));  // x-axis
  boxJoint->setVelocity(5, random(-2.0, 2.0));  // z-axis
//  world->addSkeleton(boxSkel);
//  EXPECT_EQ(boxSkel->getGravity(), world->getGravity());
//  assert(box);

  Skeleton* groundSkel = createGround(Vector3d(10000.0, 0.1, 10000.0),
                                      Vector3d(0.0, -0.05, 0.0));
  groundSkel->setMobile(false);
  // BodyNode* ground = groundSkel->getBodyNode(0);
  world->addSkeleton(groundSkel);
  EXPECT_EQ(groundSkel->getGravity(), world->getGravity());
  // assert(ground);

  EXPECT_EQ(world->getNumSkeletons(), 2);

  ConstraintSolver* cs = world->getConstraintSolver();
  CollisionDetector* cd = cs->getCollisionDetector();

  // Lower and upper bound of configuration for system
  // double lb = -1.5 * DART_PI;
  // double ub =  1.5 * DART_PI;

  int maxSteps = 500;
  for (int i = 0; i < maxSteps; ++i)
  {
//    Vector3d pos1 = sphere->getWorldTransform().translation();
//    Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);

//    std::cout << "pos1:" << pos1.transpose() << std::endl;
//    std::cout << "vel1:" << vel1.transpose() << std::endl;

    cd->detectCollision(true, true);
    if (cd->getNumContacts() == 0)
    {
      world->step();
      continue;
    }

    // for (size_t j = 0; j < cd->getNumContacts(); ++j)
    // {
      // Contact contact = cd->getContact(j);
      // Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
      // Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);

      // std::cout << "pos1:" << pos1.transpose() << std::endl;
      // std::cout << "vel1:" << vel1.transpose() << std::endl;
    // }

    world->step();

    for (size_t j = 0; j < cd->getNumContacts(); ++j)
    {
      Contact contact = cd->getContact(j);
      Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
      Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);

//      std::cout << "pos1:" << pos1.transpose() << std::endl;
//.........这里部分代码省略.........

/**
   @return top of the geometry indices assigned to the body.
   The geometry id corresponding to a link is <the top index> + <link->index()>.
*/
int cnoid::addBodyToCollisionDetector(Body& body, CollisionDetector& detector, bool enableSelfCollisions)
{
    const int idTop = detector.numGeometries();
    const int numLinks = body.numLinks();
    int excludeTreeDepth = 3;
    boost::dynamic_bitset<> exclusions(numLinks);
    boost::dynamic_bitset<> staticFlags(numLinks);
    
    const Mapping& cdInfo = *body.info()->findMapping("collisionDetection");
    if(cdInfo.isValid()){
        excludeTreeDepth = cdInfo.get("excludeTreeDepth", excludeTreeDepth);
        const Listing& excludeLinks = *cdInfo.findListing("excludeLinks");
        for(int i=0; i < excludeLinks.size(); ++i){
            Link* link = body.link(excludeLinks[i].toString());
            if(link){
                exclusions[link->index()] = true;
            }
        }
    }

    if(body.isStaticModel() || body.rootLink()->isFixedJoint()){
        setStaticFlags(body.rootLink(), staticFlags);
    }
    for(int i=0; i < numLinks; ++i){
        if(exclusions[i]){
            detector.addGeometry(0);
        } else {
            int id = detector.addGeometry(body.link(i)->collisionShape());
            if(staticFlags[i]){
                detector.setGeometryStatic(id);
            }
        }
    }

    if(!enableSelfCollisions){
        // exclude all the self link pairs
        for(int i=0; i < numLinks; ++i){
            if(!exclusions[i]){
                for(int j=i+1; j < numLinks; ++j){
                    if(!exclusions[j]){
                        detector.setNonInterfarenceGeometyrPair(i + idTop, j + idTop);
                    }
                }
            }
        }
    } else {
        // exclude the link pairs whose distance in the tree is less than excludeTreeDepth
        for(int i=0; i < numLinks; ++i){
            if(!exclusions[i]){
                Link* link1 = body.link(i);
                for(int j=i+1; j < numLinks; ++j){
                    if(!exclusions[j]){
                        Link* link2 = body.link(j);
                        Link* parent1 = link1;
                        Link* parent2 = link2;
                        for(int k=0; k < excludeTreeDepth; ++k){
                            if(parent1){
                                parent1 = parent1->parent();
                            }
                            if(parent2){
                                parent2 = parent2->parent();
                            }
                            if(!parent1 && !parent2){
                                break;
                            }
                            if(parent1 == link2 || parent2 == link1){
                                detector.setNonInterfarenceGeometyrPair(i + idTop, j + idTop);
                            }
                        }
                    }
                }
            }
        }
    }

    return idTop;
}

void GameEngine::run()
{
	spriteVec.push_back(levelVec.at(currentLevel)->getBackground());
	spriteVec.push_back(player);
	bool goOn = true;
	long laps = 0;
	while (goOn) {
		Uint32 nextTick = SDL_GetTicks() + ticks;
		SDL_RenderClear(win->getRen());
		if (initialLoop) {
			textInput();
			initialLoop = false;
		}
		SDL_Event eve2;
		for (size_t i = 0; i < spriteVec.size(); i++) {
			spriteVec.at(i)->draw();
			cd.checkCollisions(spriteVec.at(i));
		}
		removeSprites();
		cd.clearVector();
		for (size_t i = 0; i < textBoxVec.size(); i++) {
			textBoxVec.at(i)->draw();
		}
		if (pointCounter != nullptr) {
			pointCounter->draw();
		}
		SDL_RenderPresent(win->getRen());
		
		if (player == nullptr) {
			playerDied();
		}
		while (SDL_PollEvent(&eve2)) {
			switch (eve2.type) {
			case SDL_QUIT: goOn = false;
				break;
			case SDL_KEYDOWN:
				if (functionMap.count(eve2.key.keysym.sym) != 0) {
					functionMap[eve2.key.keysym.sym]();
				}
				for (size_t i = 0; i < memberFuncVec.size(); i++) {
					memberFuncVec.at(i)->perform(eve2.key.keysym.sym);
				}
				for (size_t i = 0; i < spriteVec.size(); i++) {
					spriteVec.at(i)->draw(eve2);
				}
				break;
			case SDL_KEYUP:
				for (size_t i = 0; i < spriteVec.size(); i++) {
					spriteVec.at(i)->draw(eve2);
				}
				break;
			}
		}
		laps++;
		
		levelVec.at(currentLevel)->spawnWave(this, laps);
		
		deleteSpawns(laps);

		//This is triggered when the level is over:
		if (laps > levelVec.at(currentLevel)->getEndFrame()) {
			SDL_RenderClear(win->getRen());
			for (size_t i = 0; i < spriteVec.size(); i++) {
				if (spriteVec.at(i)->getIsPlayer() == false) {
					spriteVec.at(i)->destroy(); //Prepare every sprite for removal except the player
				}
			}
			removeSprites();
			spriteVec.clear();
			deleteAllSpawns();
			SDL_RenderClear(win->getRen());
			levelSwap(); //Needs to be set by the game programmer
			currentLevel++;
			return this->run();
		}
		if (SDL_GetTicks() < nextTick) {
			SDL_Delay(nextTick - SDL_GetTicks());
		}
	}
}