C++ Obstacle::BRadius方法代码示例

ofVec3f SteeringBehaviors::ObstacleAvoidance(const std::vector<Obstacle*>& _obstacles)
{
	//compute length of detection cylinder
	m_detCylL = abs(m_viewDistance  *  (m_Vehicle->Speed() / m_Vehicle->MaxSpeed()))*2;
	m_detCylR = m_Vehicle->BRadius(); //base radius of the cylinder
	//tag all neighboring obstacles
	TagNeighbors(m_Vehicle, _obstacles,  m_detCylL);
	//keep tracks of the closest intersectin object (CIB)
	Obstacle* CIB = NULL;
	float distToCIB = FLT_MAX;
	ofVec3f localObstaclePos, localPosCIB;
	//quaternions used to translate between global and local spaces
	ofQuaternion rot;
	ofVec4f rota;

	std::vector<Obstacle*>::const_iterator it;
	it = _obstacles.begin();
	while (it != _obstacles.end())
	{
		//*******************
		//collision debugging
		(*it)->UnTagColl();
		//*******************
		//discard if not tagged
		if ((*it)->IsTagged())
		{
			//Translate _obstacle in m_Vehicle local space
			//(assuming 0,0,-1 as m_Vehicle base heading)
			rot.makeRotate(ofVec3f(0.0, 0.0, -1.0), m_Vehicle->Heading());
			rot.getRotate(rota.w, rota.x, rota.y, rota.z);
			localObstaclePos = (*it)->Pos() - m_Vehicle->Pos();
			localObstaclePos.rotate(-rota.w, ofVec3f(rota.x, rota.y, rota.z));
			//discard if behind or farther away than cylinder 
			float rad = (*it)->BRadius();
			if ((localObstaclePos.z < 0)
				&& (localObstaclePos.z > -m_detCylL))
			{
				//discard if not within cylinder				
				if((localObstaclePos.y <= m_detCylR+rad)
					&&(localObstaclePos.y >= -m_detCylR-rad)
					&&(localObstaclePos.x <= m_detCylR+rad)
					&&(localObstaclePos.x >= -m_detCylR-rad))
				{
					//get the closest intersecting obstacle
					//(simplifying here -- need to get the closest intersecting point instead)
					if(localObstaclePos.z < distToCIB)
					{
						CIB = (*it);
						localPosCIB = localObstaclePos;
						distToCIB = localObstaclePos.z;
					}

				}
			}
		}
		++it;
	}

	//if obstacle detected...
	if (CIB)
	{
		CIB->TagColl();	//debug for collision draw
		// create escape point in local space (ignoring actual intersection point for now)
		if (localPosCIB.x>0)
		{
			escapePoint.x = localPosCIB.x - CIB->BRadius() - escapeDistance - m_Vehicle->BRadius();
		}
		else
		{
			escapePoint.x = localPosCIB.x + CIB->BRadius() + escapeDistance + m_Vehicle->BRadius();
		}
		escapePoint.y = localPosCIB.y;
		escapePoint.z = localPosCIB.z;
		// translate escape point back to global space
		escapePoint += m_Vehicle->Pos();
		escapePoint.rotate(rota.w, ofVec3f(rota.x, rota.y, rota.z));
		// return force to steer to escape point
		ofVec3f DesiredVelocity; 
		float multiplier = 10; //* ((m_detCylL - distToCIB) / m_detCylL);
		//brakes
		float breakingWeight = abs(.05 / (1 - distToCIB));
		m_Vehicle->SetVelocity(m_Vehicle->Velocity()*breakingWeight);
		DesiredVelocity += (escapePoint - m_Vehicle->Pos()).getNormalized() * (m_Vehicle->MaxSpeed()*multiplier);
		return (DesiredVelocity - m_Vehicle->Velocity());
	}
	return ofVec3f::zero();
}

//---------------------- ObstacleAvoidance -------------------------------
//
//  Given a vector of CObstacles, this method returns a steering force
//  that will prevent the agent colliding with the closest obstacle
//------------------------------------------------------------------------
Vector2D SteeringBehavior::ObstacleAvoidance()
{
	//the detection box length is proportional to the agent's velocity
	float realBoxLength = m_dDBoxLength /* + 
										(m_pMovingEntity->Speed()) * m_dDBoxLength */;

	//this will keep track of the closest intersecting obstacle (CIB)
	BaseGameEntity* ClosestIntersectingObstacle = NULL;

	//this will be used to track the distance to the CIB
	double DistToClosestIP = MaxDouble;

	//this will record the transformed local coordinates of the CIB
	Vector2D LocalPosOfClosestObstacle;

	std::set<Obstacle*>::const_iterator curOb = Obstacle::getAll().begin(),
		endOb = Obstacle::getAll().end();

	while(curOb != endOb)
	{

		Obstacle* obst = (*curOb);

		Vector2D to = obst->Pos() - m_pMovingEntity->Pos();

		//the bounding radius of the other is taken into account by adding it 
		//to the range
		double range = realBoxLength + obst->BRadius();

		//if entity within range, tag for further consideration. (working in
		//distance-squared space to avoid sqrts)
		if ((to.LengthSq() < range*range))
		{

			//calculate this obstacle's position in local space
			Vector2D LocalPos = PointToLocalSpace(obst->Pos(),
				m_pMovingEntity->Heading(),
				m_pMovingEntity->Pos());

			//if the local position has a negative x value then it must lay
			//behind the agent. (in which case it can be ignored)
			if (LocalPos.x >= 0)
			{
				//if the distance from the x axis to the object's position is less
				//than its radius + half the width of the detection box then there
				//is a potential intersection.
				double ExpandedRadius = obst->BRadius() + m_pMovingEntity->BRadius();

				/*if (fabs(LocalPos.y) < ExpandedRadius)
				{*/
				//now to do a line/circle intersection test. The center of the 
				//circle is represented by (cX, cY). The intersection points are 
				//given by the formula x = cX +/-sqrt(r^2-cY^2) for y=0. 
				//We only need to look at the smallest positive value of x because
				//that will be the closest point of intersection.
				double cX = LocalPos.x;
				double cY = LocalPos.y;

				//we only need to calculate the sqrt part of the above equation once
				double SqrtPart = sqrt(ExpandedRadius*ExpandedRadius - cY*cY);

				double ip = cX - SqrtPart;

				if (ip <= 0.0)
				{
					ip = cX + SqrtPart;
				}

				//test to see if this is the closest so far. If it is keep a
				//record of the obstacle and its local coordinates
				if (ip < DistToClosestIP)
				{
					DistToClosestIP = ip;

					ClosestIntersectingObstacle = obst;

					LocalPosOfClosestObstacle = LocalPos;
				}         
			}
			//}
		}

		++curOb;
	}

	//if we have found an intersecting obstacle, calculate a steering 
	//force away from it
	Vector2D SteeringForce;

	if (ClosestIntersectingObstacle)
	{
		//the closer the agent is to an object, the stronger the 
		//steering force should be
		float multiplier = 1.0 + (m_dDBoxLength - LocalPosOfClosestObstacle.x) /
			m_dDBoxLength;
//.........这里部分代码省略.........