SAT分离轴定理的c++/python实现

分离轴定理的c++/python实现

现在要对BEV模型检查出来的车辆做NMS，把3d框的平面属性获取到后，配合旋转角度投影到地面就是2D图形。

开始碰撞检测，判断是否重叠，保留置信度高的框就行。

原理

分离轴定理（Separating Axis Theorem，简称 SAT）是一种常用的碰撞检测算法，尤其在检测两个凸多边形或凸多面体是否相交时非常有效。SAT 的核心思想是：如果两个多边形在任何一条“轴”上都没有重叠投影，那么它们就不相交。如果在所有潜在的轴上都存在重叠投影，那么它们就是相交的。

SAT 算法的核心思想：

1. 分离轴的定义：两个凸多边形相交的必要条件是，在某个方向上，两个多边形的投影不重叠。这个方向称为“分离轴”。如果存在这样的轴，称为“分离轴”，那么这两个多边形一定不相交。
2. 投影：将多边形的所有顶点投影到某个轴上，可以得到一个一维的区间。如果两个多边形在某条轴上的投影不重叠，说明在该方向上它们是分开的，所以不可能相交。
3. 边法线作为分离轴：对凸多边形的每一条边，计算其垂直方向（即边的法线），并将所有顶点沿着这个法线方向进行投影。如果在任何一条法线方向上，两个多边形的投影不重叠，则这两个多边形不会相交。
4. 反例与正例：如果在所有法线方向上，两个多边形的投影都重叠，那么它们是相交的；如果找到一个轴使得投影不重叠，那么它们是不相交的。

SAT 算法的步骤：

1. 获取多边形的所有边：对于两个多边形，分别获取它们所有的边。
2. 计算边的法线（轴）：对每一条边，计算其法线。这些法线是我们用来做投影的方向（轴）。
3. 将顶点投影到轴上：对每条轴，分别将两个多边形的所有顶点投影到该轴上，得到两个投影区间。
4. 检查投影区间是否重叠：比较两个多边形在该轴上的投影区间。如果任何一个轴上的投影不重叠，立即返回不相交（即找到了分离轴）。
5. 判断相交：如果所有轴上的投影都重叠，那么可以判断两个多边形相交。

具体例子：

假设有两个二维矩形 A 和 B，算法流程如下：

1. 计算 A 的所有边以及 B 的所有边。
2. 对每条边，计算它的垂直方向作为分离轴。
3. 将 A 和 B 的顶点投影到每条分离轴上，得到两个投影区间。
4. 检查每一条轴上 A 和 B 的投影是否有重叠。
   • 如果在某条轴上投影没有重叠，A 和 B 不相交。
   • 如果在所有轴上投影都重叠，A 和 B 相交。

示例图解：

1. 假设两个矩形相对旋转，SAT 会先找出每个矩形的边，接着计算这些边的法线（垂直方向）。
2. 在这些法线方向上，将矩形的所有顶点投影到这些法线上。
3. 如果在所有法线上的投影都重叠，就说明矩形相交；如果存在一条轴上的投影不重叠，矩形就不相交。

SAT 算法的优点：

• 高效：特别适用于凸多边形，时间复杂度相对较低。
• 通用性强：适用于任何凸多边形，不仅仅是矩形，也包括任意形状的多边形或三维多面体。

SAT 的局限：

• 仅适用于凸多边形：对凹多边形不适用。
• 计算较复杂的形状时需要的边数较多：多边形的边数越多，所需的计算量也会随之增加。

应用场景：

• 游戏开发：在物理引擎中用于碰撞检测，判断玩家、敌人或者其他物体是否相互碰撞。
• 图形处理：用于检测两个二维或三维形状是否相交，通常用于图像合成、动画、模拟仿真等场景。

总结：

分离轴定理通过投影和比较的方式，能够有效地判断两个凸多边形是否相交。它利用边法线作为可能的分离轴，并通过检测是否存在投影不重叠的轴来快速判断是否有碰撞。

cpp实现

#include <vector>
#include <cmath>
#include <iostream>
#include <algorithm>
#include <cassert>
#include <algorithm>
#include <fstream>
#include <vector>
#include <string>
#include <sstream>
struct Box3D {
   
    float conf;
    int sub_cls;
    float x;
    float y;
    float z;
    float w;  // 宽度
    float l;  // 长度
    float h;  // 高度
    float yaw; // 旋转角度
    float velocity_x = 0.0f;
    float velocity_y = 0.0f;
};

// 旋转一个点（x, y）围绕中心点（cx, cy）旋转angle_degrees角度
std::pair<float, float> RotatePoint(float x, float y, float cx, float cy, float angleRad) {
   
    // float angleRad = angleDegrees * M_PI / 180.0f;
    float cosA = std::cos(angleRad);
    float sinA = std::sin(angleRad);
    float rx = cx + (x - cx) * cosA - (y - cy) * sinA;
    float ry = cy + (x - cx) * sinA + (y - cy) * cosA;
    return {
   rx, ry};
}

// 获取旋转后的边框的顶点
std::vector<std::pair<float, float>> GetRotatedBboxVertices(float cx, float cy, float w, float h, float angle) {
   
    float half_w = w / 2;
    float half_h = h / 2;

    // 定义未旋转的矩形的四个顶点
    std::vector<std::pair<float, float>> vertices = {
   
        {
   cx - half_w, cy - half_h},
        {
   cx + half_w, cy - half_h},
        {
   cx + half_w, cy + half_h},
        {
   cx - half_w, cy + half_h}
    };

    // 旋转每个顶点
    for (auto& vertex : vertices) {
   
        vertex = RotatePoint(vertex.first, vertex.second, cx, cy, angle);
    }

    return vertices;
}


// 获取边框的边
std::vector<std::pair<std::pair<float, float>, std::pair<float, float>>> GetEdges(const std::vector<std::pair<float, float>>& vertices) {
   
    std::vector<std::pair<std::pair<float, float>, std::pair<float, float>>> edges;
    for (size_t i = 0; i < vertices.size(); ++i) {
   
        edges.push_back({
   vertices[i], vertices[(i + 1) % vertices.size()]});
    }
    return edges;
}

// 获取边缘的轴
std::pair<float, float> GetAxis(const std::pair<std::pair<float, float>, std::pair<float, float>>& edge) {
   
    float x1 = edge.first.first;
    float y1 = edge.first.second;
    float x2 = edge.second.first;
    float y2 = edge.second.second;
    return {
   y2 - y1, x1 - x2}; // 垂直于边的向量
}

// 投影顶点到轴上
std::pair<float, float> Project(const std::vector<std::pair<float, float>>& vertices, const std::pair<float, float>& axis) {
   
    std::vector<float> dots;
    for (const auto& vertex : vertices) {
   
        dots.push_back(vertex.first * axis.first + vertex.second * axis.second);
    }
    return {
   *std::min_element(dots.begin(), dots.end()), *std::max_element(dots.begin(), dots