//---------------------- 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());
}
//---------------------- 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(const std::vector<BaseGameEntity*>& obstacles)
{
//the detection box length is proportional to the agent's velocity
m_dDBoxLength = Prm.MinDetectionBoxLength +
(m_pVehicle->Speed() / m_pVehicle->MaxSpeed()) *
Prm.MinDetectionBoxLength;
//tag all obstacles within range of the box for processing
m_pVehicle->World()->TagObstaclesWithinViewRange(m_pVehicle, 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::vector<BaseGameEntity*>::const_iterator curOb = obstacles.begin();
while (curOb != obstacles.end())
{
//if the obstacle has been tagged within range proceed
if ((*curOb)->IsTagged())
{
//calculate this obstacle's position in local space
Vector2D LocalPos = PointToLocalSpace((*curOb)->Pos(),
m_pVehicle->Heading(),
m_pVehicle->Side(),
m_pVehicle->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 = (*curOb)->BRadius() + m_pVehicle->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 = *curOb;
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
double 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 vehicle.
const double BrakingWeight = 0.2;
SteeringForce.x = (ClosestIntersectingObstacle->BRadius() -
LocalPosOfClosestObstacle.x) *
BrakingWeight;
}
//finally, convert the steering vector from local to world space
return VectorToWorldSpace(SteeringForce,
m_pVehicle->Heading(),
m_pVehicle->Side());
}
//------------------------------ Render ----------------------------------
//------------------------------------------------------------------------
void GameWorld::Render()
{
gdi->TransparentText();
//render any walls
gdi->BlackPen();
for (unsigned int w = 0; w < m_Walls.size(); ++w)
{
m_Walls[w].Render(true); //true flag shows normals
}
//render any obstacles
gdi->BlackPen();
for (unsigned int ob = 0; ob < m_Obstacles.size(); ++ob)
{
gdi->Circle(m_Obstacles[ob]->Pos(), m_Obstacles[ob]->BRadius());
}
//render the agents
for (unsigned int a = 0; a < m_Vehicles.size(); ++a)
{
m_Vehicles[a]->Render();
//render cell partitioning stuff
if (m_bShowCellSpaceInfo && a == 0)
{
gdi->HollowBrush();
InvertedAABBox2D box(m_Vehicles[a]->Pos() - Vector2D(Prm.ViewDistance, Prm.ViewDistance),
m_Vehicles[a]->Pos() + Vector2D(Prm.ViewDistance, Prm.ViewDistance));
box.Render();
gdi->RedPen();
CellSpace()->CalculateNeighbors(m_Vehicles[a]->Pos(), Prm.ViewDistance);
for (BaseGameEntity* pV = CellSpace()->begin(); !CellSpace()->end(); pV = CellSpace()->next())
{
gdi->Circle(pV->Pos(), pV->BRadius());
}
gdi->GreenPen();
gdi->Circle(m_Vehicles[a]->Pos(), Prm.ViewDistance);
}
}
//#define CROSSHAIR
#ifdef CROSSHAIR
//and finally the crosshair
gdi->RedPen();
gdi->Circle(m_vCrosshair, 4);
gdi->Line(m_vCrosshair.x - 8, m_vCrosshair.y, m_vCrosshair.x + 8, m_vCrosshair.y);
gdi->Line(m_vCrosshair.x, m_vCrosshair.y - 8, m_vCrosshair.x, m_vCrosshair.y + 8);
gdi->TextAtPos(5, cyClient() - 20, "Click to move crosshair");
#endif
//gdi->TextAtPos(cxClient() -120, cyClient() - 20, "Press R to reset");
gdi->TextColor(Cgdi::grey);
if (RenderPath())
{
gdi->TextAtPos((int)(cxClient() / 2.0f - 80), cyClient() - 20, "Press 'U' for random path");
m_pPath->Render();
}
if (RenderFPS())
{
gdi->TextColor(Cgdi::grey);
gdi->TextAtPos(5, cyClient() - 20, ttos(1.0 / m_dAvFrameTime));
}
if (m_bShowCellSpaceInfo)
{
m_pCellSpace->RenderCells();
}
}
