vo rvo orca

vo rvo orca 都属于避障思路（算法）

vo rvo orca区别
vo: Velocity Obstacle
rvo: Reciprocal Velocity Obstacle
orca: Optimal Reciprocal Collision Avoidance

vo算法是一种基于速度障碍的多智能体路径规划算法
rvo算法是vo算法的改进版，增加了对其他智能体的反应
orca算法是一种基于rvo算法的多智能体路径规划算法，通过避免碰撞来规划路径

在实际应用中，orca算法的效果更好，因为它考虑了其他智能体的反应，更加符合实际情况

// 这是一个Orca算法的C++实现
// Orca算法是一种用于多智能体路径规划的算法
// 本算法实现了Orca算法的基本思想，即通过避免碰撞来规划路径

#include <iostream>
#include <vector>
#include <cmath>

using namespace std;

// 定义智能体结构体
struct Agent {
    double x, y; // 智能体的位置
    double vx, vy; // 智能体的速度
    double radius; // 智能体的半径
};

// 定义Orca算法的主函数
void orca(vector<Agent>& agents, int i, double timeStep) {
    // 定义常量
    const double RADIUS_EPSILON = 0.1;
    const double TIME_EPSILON = 0.1;

    // 获取当前智能体
    Agent& agent = agents[i];

    // 定义速度障碍
    vector<pair<double, double>> velocityObstacles;

    // 遍历所有智能体
    for (int j = 0; j < agents.size(); j++) {
        if (i == j) continue;

        // 获取其他智能体
        Agent& other = agents[j];

        // 计算相对位置和速度
        double dx = other.x - agent.x;
        double dy = other.y - agent.y;
        double dvx = other.vx - agent.vx;
        double dvy = other.vy - agent.vy;

        // 计算最小分离距离和时间
        double minDist = agent.radius + other.radius + RADIUS_EPSILON;
        double t = 0;

        // 判断是否会碰撞
        double a = dvx * dvx + dvy * dvy;
        double b = 2 * (dx * dvx + dy * dvy);
        double c = dx * dx + dy * dy - minDist * minDist;

        if (a == 0) {
            if (b < 0) {
                t = -c / b;
            }
            continue;
        }

        double disc = b * b - 4 * a * c;

        if (disc < 0) {
            continue;
        }

        double sqrtDisc = sqrt(disc);
        double t1 = (-b - sqrtDisc) / (2 * a);
        double t2 = (-b + sqrtDisc) / (2 * a);

        if (t1 >= 0 && t1 <= timeStep) {
            t = t1;
        } else if (t2 >= 0 && t2 <= timeStep) {
            t = t2;
        }

        // 计算速度障碍
        double ox = dx + dvx * t;
        double oy = dy + dvy * t;
        double distSq = ox * ox + oy * oy;

        if (distSq > minDist * minDist) {
            continue;
        }

        double w = sqrt(minDist * minDist - distSq);
        double vx = ox * w / minDist;
        double vy = oy * w / minDist;

        velocityObstacles.push_back(make_pair(vx, vy));
    }

    // 计算最终速度
    double vx = agent.vx;
    double vy = agent.vy;

    for (int i = 0; i < velocityObstacles.size(); i++) {
        double ux = velocityObstacles[i].first;
        double uy = velocityObstacles[i].second;

        if (vx * uy - vy * ux > 0) {
            double length = sqrt(ux * ux + uy * uy);
            double s = (agent.radius * timeStep) / length;

            vx = s * ux;
            vy = s * uy;
        }
    }

    // 更新智能体速度
    agent.vx = vx;
    agent.vy = vy;
}

int main() {
    // 定义常量
    const int NUM_AGENTS = 5;
    const double TIME_STEP = 0.1;

    // 定义智能体
    vector<Agent> agents(NUM_AGENTS);

    // 初始化智能体
    agents[0] = {0, 0, 1, 1, 0.5};
    agents[1] = {1, 1, -1, -1, 0.5};
    agents[2] = {1, -1, -1, 1, 0.5};
    agents[3] = {-1, 1, 1, -1, 0.5};
    agents[4] = {-1, -1, 1, 1, 0.5};

    // 运行Orca算法
    for (int i = 0; i < 100; i++) {
        for (int j = 0; j < agents.size(); j++) {
            orca(agents, j, TIME_STEP);
        }

        // 更新智能体位置
        for (int j = 0; j < agents.size(); j++) {
            agents[j].x += agents[j].vx * TIME_STEP;
            agents[j].y += agents[j].vy * TIME_STEP;
        }
    }

    // 计算智能体之间的距离
    for (int i = 0; i < agents.size(); i++) {
        for (int j = i + 1; j < agents.size(); j++) {
            double dx = agents[i].x - agents[j].x;
            double dy = agents[i].y - agents[j].y;
            double dist = sqrt(dx * dx + dy * dy);

            // 如果距离小于等于半径之和，则认为发生了碰撞
            if (dist <= agents[i].radius + agents[j].radius) {
                cout << "Collision detected between agents " << i << " and " << j << endl;
            }
        }
    }
}