Vec2 SteeringForce::ObstacleAvoidance(const std::vector<BaseGameEntity *> obstacles)
{
m_dDBoxLength = m_pVehicle->getVeloctity().length()/m_pVehicle->getMaxSpeed()*40+40;
//标记范围内的所有障碍物
GameData::Instance()->tagObstaclesWithinViewRange(m_pVehicle, m_dDBoxLength);
//追踪最近相交的障碍物
BaseGameEntity *ClosestIntersectingObstacle = nullptr;
//追踪到CIB的距离
double DistToClosestIP = MAXFLOAT;
//记录CIB被转化的局部坐标
Vec2 LocalPosOfClosestObstacle;
std::vector<BaseGameEntity*>::const_iterator curOb = obstacles.begin();
while (curOb!= obstacles.end()) {
if ((*curOb)->isTagged()) {
Vec2 LocalPos = m_pVehicle->convertToNodeSpaceAR((*curOb)->getPosition());
//如果举办空间位置有个负的x值，那么它肯定在智能体后面
if (LocalPos.x >=0) {
//如果物体到x轴的距离小于它的半径＋检查盒宽度的一半
//那么可能相交
double ExpandeRadius = (*curOb)->getContentSize().height/2*(*curOb)->getScale()+m_pVehicle->getContentSize().height/2;
if (fabs(LocalPos.y)<ExpandeRadius) {
//现在做线／圆周相交厕所吗 圆周圆心是（cx，cy）;
//相交点的公式是x ＝ cx ＋(-sqrt（ r*r - cY*cY）);
//我们只需看x的最小值，因为那是最近的相交点
double cX = LocalPos.x;
double cY = LocalPos.y;
//我们只需一次计算上面等式的开方
double SqrtPart = sqrt(ExpandeRadius*ExpandeRadius-cY*cY);
double ip = SqrtPart;
if (ip<=0) {
ip = cX+SqrtPart;
}
if (ip<DistToClosestIP) {
DistToClosestIP = ip;
ClosestIntersectingObstacle = *curOb;
LocalPosOfClosestObstacle = LocalPos;
}
}
}
}
++curOb;
}
Vec2 SteeringForce;
if (ClosestIntersectingObstacle) {
//智能体越近，操控力越强
double muliplier =1 + (m_dDBoxLength-LocalPosOfClosestObstacle.x)/m_dDBoxLength;
//计算侧向力
SteeringForce.y = (ClosestIntersectingObstacle->getContentSize().height/2*ClosestIntersectingObstacle->getScale()-LocalPosOfClosestObstacle.y)*muliplier;
if (LocalPosOfClosestObstacle.y>0&&LocalPosOfClosestObstacle.y<ClosestIntersectingObstacle->getContentSize().height/2*ClosestIntersectingObstacle->getScale()) {
SteeringForce.y = -SteeringForce.y;
}
//施加一个制动力，它正比于障碍物到交通工具的距离
const double BrakingWeight = 0.3;
SteeringForce.x = (ClosestIntersectingObstacle->getContentSize().height/2*ClosestIntersectingObstacle->getScale()-LocalPosOfClosestObstacle.x)*BrakingWeight;
}
//最后，把操控向量换成世界坐标
// log("********SteeringForce:%f,%f*********",( m_pVehicle->convertToWorldSpaceAR(SteeringForce)-m_pVehicle->getPosition()).x,( m_pVehicle->convertToWorldSpaceAR(SteeringForce)-m_pVehicle->getPosition()).y);
return m_pVehicle->convertToWorldSpaceAR(SteeringForce)-m_pVehicle->getPosition();
}
