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();
}
