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;
//calculate the lateral force
SteeringForce.y = (ClosestIntersectingObstacle->BRadius()-
LocalPosOfClosestObstacle.y) * multiplier;
//apply a braking force proportional to the obstacles distance from
//the MovingEntity.
const float BrakingWeight = 0.2f;
SteeringForce.x = (ClosestIntersectingObstacle->BRadius() -
LocalPosOfClosestObstacle.x) *
BrakingWeight;
}
//finally, convert the steering vector from local to world space
return VectorToWorldSpace(SteeringForce,
m_pMovingEntity->Heading());
}