多边形碰撞检测算法

1、AABB碰撞检测算法

AABB碰撞检测指轴对齐碰撞箱(Axis-aligned Bounding Box)，是分别从x轴向和y轴向进行碰撞检测的算法。即对于需要检测的物体A和物体B我们需要将其用A盒和B盒套起来，判断A盒和B盒在x轴向和y轴向是否发生碰撞，只有在x轴向和y轴向都发生碰撞我们才判断它发生了碰撞。

在轴对齐包围矩形（Axis Aligned Bounding Box，AABB）基础之上，更为准确的是 转向包围矩形（Oriented Bounding Box，OBB） OBB 与 AABB 的差别在于物体旋转之后，AABB的大小会发生改变，保证每条边与某个坐标轴平行；OBB 的大小则不会改变，且将随物体一起旋转。

class AABB {
    constructor(x, y, width, height) {
        this.x = x;
        this.y = y;
        this.width = width;
        this.height = height;
    }

    collidesWith(other) {
        return this.x < other.x + other.width &&
               this.x + this.width > other.x &&
               this.y < other.y + other.height &&
               this.y + this.height > other.y;
    }
}

// 使用示例
const box1 = new AABB(0, 0, 10, 10);
const box2 = new AABB(5, 5, 10, 10);
console.log(box1.collidesWith(box2)); // true

2、OBB碰撞检测算法

OBB 就是找一个最小的包围物体的矩形，这在自动驾驶系统中也是最常用的，感知模块给出物体的轮廓通常就是此形状。另外，为了准确描述物体轮廓，感知模块在 bounding box 的基础上，通常还会给出 polygon（多边形）的形式，如下图所示。

相对于AABB包围盒来讲，OBB在碰撞精度上要高于AABB，但是精确度的提高同时带来的就是效率的降低，OBB的算法无疑是要比AABB复杂的，同样内存消耗也会更大。
OBB适用于旋转的矩形，需要使用分离轴定理（SAT）进行检测。

class OBB {
    constructor(center, width, height, angle) {
        this.center = center;
        this.width = width;
        this.height = height;
        this.angle = angle;
    }

    getVertices() {
        const rad = this.angle * Math.PI / 180;
        const cos = Math.cos(rad);
        const sin = Math.sin(rad);

        const halfWidth = this.width / 2;
        const halfHeight = this.height / 2;

        return [
            { x: this.center.x + cos * halfWidth - sin * halfHeight, y: this.center.y + sin * halfWidth + cos * halfHeight },
            { x: this.center.x - cos * halfWidth - sin * halfHeight, y: this.center.y - sin * halfWidth + cos * halfHeight },
            { x: this.center.x - cos * halfWidth + sin * halfHeight, y: this.center.y - sin * halfWidth - cos * halfHeight },
            { x: this.center.x + cos * halfWidth + sin * halfHeight, y: this.center.y + sin * halfWidth - cos * halfHeight }
        ];
    }

    getAxes() {
        const vertices = this.getVertices();
        return [
            { x: vertices[1].x - vertices[0].x, y: vertices[1].y - vertices[0].y },
            { x: vertices[2].x - vertices[1].x, y: vertices[2].y - vertices[1].y }
        ];
    }

    project(axis) {
        const vertices = this.getVertices();
        let min = Infinity;
        let max = -Infinity;

        vertices.forEach(vertex => {
            const dot = vertex.x * axis.x + vertex.y * axis.y;
            min = Math.min(min, dot);
            max = Math.max(max, dot);
        });

        return { min, max };
    }

    collidesWith(other) {
        const axes = this.getAxes().concat(other.getAxes());

        for (let axis of axes) {
            const proj1 = this.project(axis);
            const proj2 = other.project(axis);

            if (proj1.max < proj2.min || proj2.max < proj1.min) {
                return false;
            }
        }

        return true;
    }
}

// 使用示例
const obb1 = new OBB({ x: 0, y: 0 }, 10, 10, 45);
const obb2 = new OBB({ x: 5, y: 5 }, 10, 10, 0);
console.log(obb1.collidesWith(obb2)); // true

在了解OBB算法之前我们需要先学习一下分离轴定理。

分离轴定理(SAT)：

分离轴定理(Separating Axis Theorem)的理论依据为超平面分离定理，即 令 A 和 B 是两个不相交的非空凸集，那么存在一个非零向量 v 和 实数 c，使得 <x, v> ≤ c 且 <y, v> ≥ c。其中，x 属于 A，y 属于 B。

简单来说，就是对于两个凸多边形，若存在一条直线将两者分开，则这两个多边形不相交。

上图中的黑线为分离线（Seperating line），与之垂直的绿线为分离轴（Separating axis），图中虚线表示的是多边形在分离轴上的投影。

实际应用中，遍历所有角度的分离轴是不现实的，受益于多边形的性质，对于两个都是多边形的物体，只需要依次在每条边的垂直线做投影即可，如下图所示。

对于两个都是矩形的物体，则更简单，只需要做四次投影。

以下图中的两个多边形 A 和 B 为例，分离轴定理的具体步骤为：

1. 首先根据边1的两个顶点位置坐标，计算出边1的向量，设为（x，y）；
2. 进而求出边1的法向量，作为分离轴，为（y, -x）或（-y，x）。若需要求两个多边形的最小分离距离，这里的法向量还需要化为单位向量；若只需判断两个多边形是否相交，则不需要化为单位向量；
3. 依次将多边形 A 和 B的所有顶点与原点组成的向量投影到这个分离轴上，并记录两个多边形顶点投影到分离轴上的最小值和最大值（Pmin，Pmax），形成一个投影线段；
4. 判断这两个投影线段是否发生重叠，若不重叠，则有 （PAmax < PBmin）||（PAmin > PBmax）；
5. 若两个投影线段不重叠，则代表存在这样一条直线将两个多边形分开，两个多边形不相交，可以直接退出循环；
6. 若两个投影线段重叠，则回到步骤1，继续以边2的法向量作为分离轴，进行投影计算；
7. 当两个多边形的所有边都检查完之后，找不到这样一条分离的直线，则意味着两个多边形相交。

class Polygon {
    constructor(vertices) {
        this.vertices = vertices;
    }

    getAxes() {
        const axes = [];
        for (let i = 0; i < this.vertices.length; i++) {
            const p1 = this.vertices[i];
            const p2 = this.vertices[(i + 1) % this.vertices.length];
            const edge = { x: p2.x - p1.x, y: p2.y - p1.y };
            const normal = { x: -edge.y, y: edge.x };
            axes.push(normal);
        }
        return axes;
    }

    project(axis) {
        let min = Infinity;
        let max = -Infinity;

        this.vertices.forEach(vertex => {
            const dot = vertex.x * axis.x + vertex.y * axis.y;
            min = Math.min(min, dot);
            max = Math.max(max, dot);
        });

        return { min, max };
    }

    collidesWith(other) {
        const axes = this.getAxes().concat(other.getAxes());

        for (let axis of axes) {
            const proj1 = this.project(axis);
            const proj2 = other.project(axis);

            if (proj1.max < proj2.min || proj2.max < proj1.min) {
                return false;
            }
        }

        return true;
    }
}

// 使用示例
const poly1 = new Polygon([{ x: 0, y: 0 }, { x: 10, y: 0 }, { x: 5, y: 10 }]);
const poly2 = new Polygon([{ x: 5, y: 5 }, { x: 15, y: 5 }, { x: 10, y: 15 }]);
console.log(poly1.collidesWith(poly2)); // true

注意：分离轴定理是一种适用于凸多边形的碰撞检测算法，对于凹多边形则不适用，如下图所示，两个多边形没有碰撞，但找不到这样一条直线，能将两者分开。所以如果是凹多边形的话，需要先将其转换成多个凸多边形。

综上，分离轴定理是一种适用于 bounding box 和 polygon 的精细碰撞检测算法，其优点是算法原理简单，可准确判断两个多边形是否相交；缺点在于当多边形的边数较多时，该算法的效率较低（当两个多边形相交时，需要遍历完所有边进行判断）。

在实际应用中，为了提高效率，通常先使用 基于轴对齐包围矩形（AABB）的方法进行粗略的碰撞检测，然后再使用分离轴定理（SAT）做精细碰撞检测。

3、GJK（Gilbert–Johnson–Keerthi）算法

GJK是由Gilbert，Johnson，Keerthi 三位前辈发明的，用来计算两个凸多面体之间的碰撞检测，以及最近距离。GJK算法可以在O(M+N)的时间复杂度内，检测出碰撞，算法在每次迭代的过程中，都会优先选择靠近原点的方向，因此收敛速度会很快。算法的证明过程比较复杂，但是原理还是比较容易理解的。

相比 SAT 算法，GJK 算法更加高效。 GJK算法的核心就是闵可夫斯基差，即若两个多边形相交，则它们的闵可夫斯基差必然包括原点。

闵可夫斯基差，也可以叫做闵可夫斯基和，它的定义也很好理解，点集A与B的闵可夫斯基和被定义为：

A + B = {a + b |a∈A，b∈B}

如果 A 和 B 是两个凸多边形，则 A + B 也是凸多边形。

闵可夫斯基和从几何上的直观理解是A集合沿B的边际连续运动一周扫过的区域与B集合本身的并集，也可以是B沿着A的边界连续运动扫过区域与A自身的并集。

GJK算法用到的不是闵可夫斯基和，而是闵可夫斯基差，即：

A – B = {a – b |a∈A，b∈B}

虽然使用的是减法运算，但其仍然是闵可夫斯基和，相当于先对B中的所有点做负运算（相对原点的镜像），然后再与A做加法。

先来看两个例子。

对于两个不相交的多边形，shape1为矩形，shape2为三角形，如下图所示。

它们的闵可夫斯基差如下图所示，其闵可夫斯基差不包括原点，且两个多边形之间的距离就是其闵可夫斯基差到原点的距离。事实上，GJK 算法发明出来的初衷就是为了计算两个凸多边形之间的距离。

对于两个相交的多边形，shape1为矩形，shape2为三角形，如下图所示。

它们的闵可夫斯基差则如下图所示，可以看到，闵可夫斯基差是包括原点的。这也很好理解，两个相交的多边形，必然有一点既属于shape1，也属于shape2，相减则为原点（0，0）。

3.1 单纯形
k阶单纯形（simplex），指的是k维空间中的多胞形，该多胞形是k+1个顶点组成的凸包。

在GJK算法中，单纯形被大量使用。单纯形指的是点、线段、三角形或四面体。例如，0阶单纯形是点，1阶单纯形是线段，2阶单纯形是三角形，3阶单纯形是四面体。

对于2维空间的多边形，最多用到2阶单纯形。那单纯形到底有什么作用呢？

对于上面两个相交的多边形例子，实际应用中，其实不需要求出完整的闵可夫斯基差，只需要在闵可夫斯基差内形成一个多边形，如下图所示，并使这个多边形尽可能包围原点，这个多边形就称为单纯形。即假如单纯形包围原点，则闵可夫斯基差必然包围原点。

3.2 Support 函数
Support函数的作用是计算多边形在给定方向上的最远点。如下图所示，在向量 a 方向的最远点为 A 点，在向量 b 方向的最远点为 B 点。这里在寻找给定方向上的最远点时，需要用到向量的点乘。

为什么需要Support函数呢？这是因为在构建单纯形时，我们希望尽可能得到闵可夫斯基差的顶点，而不是其内部的一个点，这样产生的单纯形才能包含最大的区域，增加算法的快速收敛性。

如下图所示，在给定向量 a 方向上，shape1 的最远点为（4，2），在向量 -a 的方向上，shape2 的最远点为（5，3），这两个点作差即得到点（-1，-1）。利用这种方式得到的点都在闵可夫斯基差的边上。

class Vector {
    constructor(x, y) {
        this.x = x;
        this.y = y;
    }

    subtract(other) {
        return new Vector(this.x - other.x, this.y - other.y);
    }

    dot(other) {
        return this.x * other.x + this.y * other.y;
    }

    cross(other) {
        return this.x * other.y - this.y * other.x;
    }

    lengthSquared() {
        return this.x * this.x + this.y * this.y;
    }

    normalize() {
        const length = Math.sqrt(this.lengthSquared());
        return new Vector(this.x / length, this.y / length);
    }
}

function support(polygonA, polygonB, direction) {
    const pointA = polygonA.reduce((max, p) => p.dot(direction) > max.dot(direction) ? p : max);
    const pointB = polygonB.reduce((max, p) => p.dot(direction.negate()) > max.dot(direction.negate()) ? p : max);
    return pointA.subtract(pointB);
}

function containsOrigin(simplex) {
    const a = simplex[0];
    const b = simplex[1];
    const c = simplex[2];

    const ab = b.subtract(a);
    const ac = c.subtract(a);

    const abPerp = new Vector(-ab.y, ab.x);
    const acPerp = new Vector(-ac.y, ac.x);

    if (abPerp.dot(c.subtract(a)) > 0) {
        simplex.splice(2, 1);
        return false;
    }

    if (acPerp.dot(b.subtract(a)) > 0) {
        simplex.splice(1, 1);
        return false;
    }

    return true;
}

function gjk(polygonA, polygonB) {
    let simplex = [];
    let direction = new Vector(1, 0);

    simplex.push(support(polygonA, polygonB, direction));

    direction = direction.negate();

    while (true) {
        simplex.push(support(polygonA, polygonB, direction));

        if (simplex[simplex.length - 1].dot(direction) <= 0) {
            return false;
        }

        if (containsOrigin(simplex)) {
            return true;
        }

        direction = simplex[simplex.length - 1].negate();
    }
}

// 使用示例
const polygonA = [new Vector(0, 0), new Vector(10, 0), new Vector(5, 10)];
const polygonB = [new Vector(5, 5), new Vector(15, 5), new Vector(10, 15)];
console.log(gjk(polygonA, polygonB)); // true