Steering Behaviors系统中的碰撞避免，路径跟随，队长跟随

Collision Avoid

在物体前进的方向，延伸一定长度的向量进行检测。相当于物体对前方一定可使范围进行检测障碍物的碰撞

延伸的向量与碰撞物圆心的距离小于碰撞物的半径，则判断前进方向有阻挡物

我们会对所有障碍物进行碰撞检测，但是只取最近的一个做处理

可以延伸多条不同长度的同方向向量进行检测，以免过于靠近物体而使检测向量超过了碰撞物

前方视野向量检测到有障碍物的话，就会进行躲避，有障碍物圆心减去检测向量得到躲避力的方向

注意这里是和flee的最主要区别。avoid力相当于提前检测前方行进方向是否有障碍物，然后提前开始避开了。而flee力则是用当前自身自身的位置判断是否flee目标点，并以自身位置进行反向逃离计算
这里一个关键点是ahead的向量，如果ahead向量是个固定长度的，会有个问题就是，物体无法停在障碍物的前方。因为可能此生的检测向量一直在障碍物的范围内。如下图

因此ahead向量的长度要通过当前速度与最大速度的比例来设置，如果当前速度接近0，则ahead的长度也接近于0

Vec2 MoveNode::collisionAvoid(float maxSpeed) {
    Vec2 avoidForce = Vec2::ZERO;
    float dynamicLen = _velocity.getLength() / maxSpeed;
    //Vec2 ahead = this->getPosition() + _velocity.getNormalized() * _aheadLen;
    Vec2 ahead = this->getPosition() + _velocity.getNormalized() * dynamicLen;
    Vec2 obstaclePos = findNearestObstacle(maxSpeed);
    if (obstaclePos != Vec2(-1, -1)) {
        avoidForce = ahead - obstaclePos;
        avoidForce.normalize();
        avoidForce *= _avoidForce;
    }
    return avoidForce;
}
//找到最具威胁的阻碍物，ahead为检测向量，与所有障碍物的圆心进行距离判定
Vec2 MoveNode::findNearestObstacle(float maxSpeed) {
    Vec2 pos = this->getPosition();
    /*Vec2 ahead = pos + _velocity.getNormalized() * _aheadLen;
    Vec2 ahead2 = pos + _velocity.getNormalized() * _aheadLen / 2;*/
    float dynamicLen = _velocity.getLength() / maxSpeed;
    Vec2 ahead = pos + _velocity.getNormalized() * dynamicLen;
    Vec2 ahead2 = pos + _velocity.getNormalized() * dynamicLen / 2;
    Vec2 v = Vec2(-1, -1);
    for (auto obstacle : _obstacles) {
        Vec2 center = obstacle.first;
        float radius = obstacle.second;
        bool isCollision = ahead.getDistance(center) < radius || ahead2.getDistance(center) < radius;
        if(isCollision && (v == Vec2(-1,-1) || pos.getDistance(center) < pos.getDistance(v))) v = center;
    }
    return v;
}

效果

Path Following

比较简单的实现是，将路径划分多个路径点，对每个路径点进行seek

因为物体是带有类似惯性的，所有对每个路径点seek时，需要判定到达路径点一定范围内就算到达当前路径点，然后前往下一路径点，否则会在初始点无限循环

注意路径点的到达半径如果太短的话，物体由于速度过快可能会在路径点周围绕一圈，才走进了到达的范围内，才前往下一路径点

//_pathDir表示了路径点索引的方向，我们这里是抵达起始点或终点后进行反向
Vec2 MoveNode::pathFollowing() {
    Vec2 steering = Vec2::ZERO;
    if (_pathNodes.empty()) return steering;
    auto pathNode = _pathNodes[_pathNodeIdx];
    Vec2 pos = pathNode.first;
    float arriveRadius = pathNode.second;
    if (this->getPosition().getDistance(pos) <= arriveRadius) {
        _pathNodeIdx += _pathDir;
        if (_pathNodeIdx >= _pathNodes.size() || _pathNodeIdx < 0) {
            _pathDir *= -1;
            _pathNodeIdx += _pathDir;
        }
    }
    return seek(_pathNodes[_pathNodeIdx].first);
}

Leader Following

跟随一个队长的路径
队长跟随类似追捕目标，只不过追捕目标是预测目标点前进几帧后的方向进行seek，但是跟随是队长前进方向的反向一定距离进行跟随

如上图，求出队长速度的反向，在一定长度的位置behind，队员对behind的位置进行arrive

//_leaderBehindDist：跟随在队长背后的距离
Vec2 MoveNode::leaderFollowing() {
    Vec2 steering = Vec2::ZERO;
    if (_leader == nullptr) return steering;
    Vec2 tv = _leader->getVelocity() * -1;
    tv.normalize();
    tv *= _leaderBehindDist;
    Vec2 followPos = _leader->getPosition() + tv;

    steering += seek(followPos);
    return steering;
}

我们看到队员跟随在队长背后，但是此时队员都挤成了一团，因此对队员加上集群模拟的separation力

另一个问题是，跟随队长的队员，是不允许挡在队长前进的前方的。此时，我们给队长一个前方的视线检测向量和一个检测半径，如果队员在这个视线检测范围内的话，就要对队长进行避让evade

//pos是队员的坐标位置，_leaderAhead是队长视线的检测的长度，与队长速度方向求的检测向量，_leaderAheadRadius是检测的范围半径。该函数判定队员位置是否在队长前进的方向上
bool MoveNode::isInAheadArea(Vec2 pos) {
    Vec2 srcPos = this->getPosition();
    Vec2 aheadPos = _velocity.getNormalized()* _leaderAhead + srcPos;
    return srcPos.getDistance(pos) < _leaderAheadRadius || aheadPos.getDistance(pos) < _leaderAheadRadius;
}

evade的逻辑于pursuit类似，预测队长几帧后的前进位置，对该位置进行flee

Vec2 MoveNode::leaderFollowing() {
    Vec2 steering = Vec2::ZERO;
    if (_leader == nullptr) return steering;
    Vec2 tv = _leader->getVelocity() * -1;
    tv.normalize();
    tv *= _leaderBehindDist;
    Vec2 followPos = _leader->getPosition() + tv;
    
    //如果在队长视野前方，则预测队长行进路线进行逃离evade
    if (_leader->isInAheadArea(this->getPosition())) {
        Vec2 leadPos = _leader->getPosition();
        float t = this->getPosition().getDistance(leadPos) / _dtSpeed;
        Vec2 fleePos = leadPos + _leader->getVelocity() * t;
        steering += flee(fleePos);
    }
    steering += seek(followPos);
    return steering;
}

下面是队长跟随路径移动，队员跟随队长移动的结合例子