动手学运动规划:1.3.c 碰撞检测算法:AABB, SAT代码解析

用知识武装自己!☺ —漫长的季节 龚飙

🏰代码及环境配置：请参考0.2 环境配置和代码运行 | 动手学运动规划!

---

本节提供了AABB和SAT算法的代码测试.

python3 tests/basic/1.3sat_collision_check_test.py

1.3.c.1 AABB, SAT算法实现

我们在common/geometry.py 定义了一个Box类, 类内实现了碰撞检测函数has_overlap() .参考了Apollo中modules/common/math/box2d.cc 的实现.代码如下:

def has_overlap(self, box):
				#AABB check
        if (
            box.max_x < self.min_x
            or box.min_x > self.max_x
            or box.max_y < self.min_y
            or box.min_y > self.max_y
        ):
            return False

        shift_x = box.center.x - self.center.x
        shift_y = box.center.y - self.center.y

        dx1 = self.cos_heading * self.length / 2
        dy1 = self.sin_heading * self.length / 2
        dx2 = self.sin_heading * self.width / 2
        dy2 = -self.cos_heading * self.width / 2
        dx3 = box.cos_heading * box.length / 2
        dy3 = box.sin_heading * box.length / 2
        dx4 = box.sin_heading * box.width / 2
        dy4 = -box.cos_heading * box.width / 2

        return (
            abs(shift_x * self.cos_heading + shift_y * self.sin_heading)
            <= abs(dx3 * self.cos_heading + dy3 * self.sin_heading)
            + abs(dx4 * self.cos_heading + dy4 * self.sin_heading)
            + self.length / 2
            and abs(shift_x * self.sin_heading - shift_y * self.cos_heading)
            <= abs(dx3 * self.sin_heading - dy3 * self.cos_heading)
            + abs(dx4 * self.sin_heading - dy4 * self.cos_heading)
            + self.width / 2
            and abs(shift_x * box.cos_heading + shift_y * box.sin_heading)
            <= abs(dx1 * box.cos_heading + dy1 * box.sin_heading)
            + abs(dx2 * box.cos_heading + dy2 * box.sin_heading)
            + box.length / 2
            and abs(shift_x * box.sin_heading - shift_y * box.cos_heading)
            <= abs(dx1 * box.sin_heading - dy1 * box.cos_heading)
            + abs(dx2 * box.sin_heading - dy2 * box.cos_heading)
            + box.width / 2
        )

该函数先进行了AABB check,如果不通过则进行SAT算法判定.把两个box的每条边投影到了两个box每条边上.由于box的有两组平行边,因此计算量可以减少一半.只需要投影到每个box的两组边,一共4组边即可.

1.3.c.2 AABB, SAT算法测试

在tests/basic/sat_collision_check_test.py中进行了算法测试,通过生成随机的两个box,进行碰撞校验.代码如下:

def aabb_and_sat():
    fig = plt.figure()
    plt.title("AABB and SAT Collision Check")

    for i in range(0, 10):
        plt.cla()
        x1 = random.uniform(0.0, 10.0)
        y1 = random.uniform(0.0, 10.0)
        theta1 = random.uniform(-pi, pi)
        length1 = random.uniform(2.0, 4.0)
        width1 = random.uniform(1.0, 3.0)
        box1 = Box(x1, y1, theta1, length1, width1)

        x2 = random.uniform(0.0, 10.0)
        y2 = random.uniform(0.0, 10.0)
        theta2 = random.uniform(-pi, pi)
        length2 = random.uniform(2.0, 4.0)
        width2 = random.uniform(1.0, 3.0)
        box2 = Box(x2, y2, theta2, length2, width2)

        has_overlap = box1.has_overlap(box2)

测试结果如图,红色代表碰撞,黑色代表没有碰撞.

---

🏎️自动驾驶小白说官网：https://www.helloxiaobai.cn

🐮GitHub代码仓：https://github.com/Hello-Xiao-Bai/Planning-XiaoBai!

🌠代码配合官网教程食用更佳！

🚀知乎，微信，知识星球全平台同号！