碰撞检测 | 详解矩形AABB与OBB碰撞检测算法(附ROS C++可视化)

碰撞检测是计算机图形学、物理引擎以及机器人导航中非常重要的部分，常见的碰撞检测算法包括AABB（Axis-Aligned Bounding Box，轴对齐边界框）和OBB（Oriented Bounding Box，定向包围盒）。在这篇文章中，我们将详细解释AABB和OBB的碰撞检测算法，介绍它们的原理，并提供基于C++在ROS中实现的可视化代码示例。

一、碰撞检测基础

碰撞检测是为了判断两几何形状是否相交，常用的方法有：

• AABB 碰撞检测：矩形的边界平行于坐标轴，简单高效，特别适合快速检测。
• OBB 碰撞检测：矩形可以有任意角度旋转，能够精确描述物体，但计算相对复杂。

AABB 与 OBB 的差异主要在于是否允许旋转，AABB是固定方向的矩形，而OBB是旋转的矩形。

---

二、AABB 碰撞检测算法

AABB 碰撞检测是最基础的碰撞检测方法，假设两个矩形的边始终平行于坐标轴。其检测原理非常简单，只需要判断两个矩形是否在各自的投影区间内相交。

2.1 AABB 碰撞检测原理

假设有两个矩形 A 和 B，我们只需检测它们在 X 轴和 Y 轴上的投影是否相交：

• X 轴检测: A.maxX > B.minX 并且 B.maxX > A.minX
• Y 轴检测: A.maxY > B.minY 并且 B.maxY > A.minY

只有当两个矩形在 X 轴和 Y 轴上的投影都相交时，才算是碰撞。

2.2 AABB 碰撞检测代码实现（C++）

#include <iostream>

struct AABB {
    float minX, minY;
    float maxX, maxY;
};

bool isAABBColliding(const AABB& a, const AABB& b) {
    return (a.maxX > b.minX && a.minX < b.maxX &&
            a.maxY > b.minY && a.minY < b.maxY);
}

int main() {
    AABB rect1 = {0, 0, 4, 4};
    AABB rect2 = {3, 3, 6, 6};

    if (isAABBColliding(rect1, rect2)) {
        std::cout << "AABB Collision Detected!" << std::endl;
    } else {
        std::cout << "No Collision." << std::endl;
    }
    return 0;
}

---

三、OBB 碰撞检测算法

与AABB不同，OBB允许矩形任意旋转，因此在进行碰撞检测时，需要考虑矩形的方向。常用的检测方法是分离轴定理（Separating Axis Theorem, SAT）。

3.1 OBB 碰撞检测原理

OBB碰撞检测基于SAT分离轴定理，其核心思想是：如果两个多边形在任意一个投影轴上没有相交，则它们不发生碰撞。如果它们在所有的投影轴上都有交集，则发生碰撞。

对于两个OBB，我们可以将它们的边作为投影轴，然后将两个OBB的顶点投影到这些轴上，逐一检查投影是否相交。具体步骤如下：

1. 计算两个矩形的边（即每条边的法向量）作为投影轴。
2. 将矩形的顶点投影到这些轴上。
3. 检查投影是否相交，如果在任意一条轴上投影不相交，则说明不碰撞。

3.2 OBB 碰撞检测代码实现（C++）

#include <iostream>
#include <cmath>

struct Vec2 {
    float x, y;
};

struct OBB {
    Vec2 center; // 中心点
    Vec2 halfExtents; // 半边长
    float rotation; // 旋转角度，弧度
};

// 向量点积
float dotProduct(const Vec2& a, const Vec2& b) {
    return a.x * b.x + a.y * b.y;
}

// 获取OBB的四个顶点
void getOBBVertices(const OBB& obb, Vec2 vertices[4]) {
    float cosTheta = std::cos(obb.rotation);
    float sinTheta = std::sin(obb.rotation);

    Vec2 extentsX = {obb.halfExtents.x * cosTheta, obb.halfExtents.x * sinTheta};
    Vec2 extentsY = {-obb.halfExtents.y * sinTheta, obb.halfExtents.y * cosTheta};

    vertices[0] = {obb.center.x - extentsX.x - extentsY.x, obb.center.y - extentsX.y - extentsY.y};
    vertices[1] = {obb.center.x + extentsX.x - extentsY.x, obb.center.y + extentsX.y - extentsY.y};
    vertices[2] = {obb.center.x + extentsX.x + extentsY.x, obb.center.y + extentsX.y + extentsY.y};
    vertices[3] = {obb.center.x - extentsX.x + extentsY.x, obb.center.y - extentsX.y + extentsY.y};
}

// 判断是否在投影轴上分离
bool isSeparatedOnAxis(const Vec2& axis, const Vec2 verticesA[4], const Vec2 verticesB[4]) {
    float minA = dotProduct(axis, verticesA[0]);
    float maxA = minA;

    for (int i = 1; i < 4; i++) {
        float projection = dotProduct(axis, verticesA[i]);
        minA = std::min(minA, projection);
        maxA = std::max(maxA, projection);
    }

    float minB = dotProduct(axis, verticesB[0]);
    float maxB = minB;

    for (int i = 1; i < 4; i++) {
        float projection = dotProduct(axis, verticesB[i]);
        minB = std::min(minB, projection);
        maxB = std::max(maxB, projection);
    }

    return !(minA <= maxB && maxA >= minB);
}

// OBB碰撞检测
bool isOBBColliding(const OBB& a, const OBB& b) {
    Vec2 verticesA[4], verticesB[4];
    getOBBVertices(a, verticesA);
    getOBBVertices(b, verticesB);

    Vec2 axes[4] = {
        {verticesA[1].x - verticesA[0].x, verticesA[1].y - verticesA[0].y},
        {verticesA[3].x - verticesA[0].x, verticesA[3].y - verticesA[0].y},
        {verticesB[1].x - verticesB[0].x, verticesB[1].y - verticesB[0].y},
        {verticesB[3].x - verticesB[0].x, verticesB[3].y - verticesB[0].y}
    };

    for (const Vec2& axis : axes) {
        if (isSeparatedOnAxis(axis, verticesA, verticesB)) {
            return false; // 在某一轴上分离，没碰撞
        }
    }
    return true;
}

int main() {
    OBB rect1 = {{0, 0}, {2, 1}, 0}; // OBB1，中心在(0,0)，半边长(2,1)，无旋转
    OBB rect2 = {{3, 2}, {2, 1}, M_PI / 4}; // OBB2，中心在(3,2)，半边长(2,1)，45度旋转

    if (isOBBColliding(rect1, rect2)) {
        std::cout << "OBB Collision Detected!" << std::endl;
    } else {
        std::cout << "No Collision." << std::endl;
    }
    return 0;
}

---

四、ROS C++可视化

通过ROS和RViz工具，我们可以将AABB和OBB的碰撞检测结果可视化。使用C++编写ROS节点，发布矩形的位置、尺寸和碰撞结果到RViz中展示。

4.1 ROS 节点编写

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

// 发布矩形的可视化
void publishRectangle(ros::Publisher& marker_pub, const OBB& obb, const std::string& ns, int id) {
    visualization_msgs::Marker marker;
    marker.header.frame_id = "map";
    marker.header.stamp = ros::

Time::now();
    marker.ns = ns;
    marker.id = id;
    marker.type = visualization_msgs::Marker::CUBE;
    marker.action = visualization_msgs::Marker::ADD;

    marker.pose.position.x = obb.center.x;
    marker.pose.position.y = obb.center.y;
    marker.pose.position.z = 0;
    marker.pose.orientation = tf::createQuaternionMsgFromYaw(obb.rotation);

    marker.scale.x = obb.halfExtents.x * 2;
    marker.scale.y = obb.halfExtents.y * 2;
    marker.scale.z = 0.1;

    marker.color.r = 0.0f;
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0f;

    marker_pub.publish(marker);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "collision_visualization");
    ros::NodeHandle nh;

    ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);

    OBB obb1 = {{0, 0}, {2, 1}, 0};
    OBB obb2 = {{3, 2}, {2, 1}, M_PI / 4};

    ros::Rate r(10);
    while (ros::ok()) {
        publishRectangle(marker_pub, obb1, "rect1", 0);
        publishRectangle(marker_pub, obb2, "rect2", 1);

        ros::spinOnce();
        r.sleep();
    }

    return 0;
}

此代码会在RViz中绘制两个矩形，并且可以根据碰撞检测结果调整颜色来直观展示是否发生碰撞。

---

五、总结

AABB和OBB碰撞检测算法在计算机图形学和机器人领域中都有着广泛的应用。AABB算法简单高效，适合用于快速检测，而OBB算法则更加灵活，能够处理任意方向的物体。通过ROS进行可视化，我们可以将这些检测算法更加直观地展示出来。在实际应用中，往往会结合两者以达到更高效的检测效果。

希望通过这篇文章，大家对AABB与OBB的碰撞检测算法有了更加深入的理解。