GJK算法实现

1. GJK原理解析

具体可参考下列文章：

gjk-gilbert-johnson-keerthi

知乎专栏

2. GJK向量类

export class Vector {
  x: number;
  y: number;

  static ZERO_VECTOR = new Vector(0, 0);

  constructor(x: number, y: number) {
    this.x = x;
    this.y = y;
  }

  dot(vector: Vector) {
    return this.x * vector.x + this.y * vector.y;
  }

  negate(): Vector {
    return new Vector(-this.x, -this.y);
  }

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

  cross(vector: Vector) {
    return this.x * vector.y - this.y * vector.x;
  }

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

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

  toString() {
    return `${this.x},${this.y}`;
  }

  toArray() {
    return [this.x, this.y];
  }

  toObject() {
    return {
      x: this.x,
      y: this.y,
    };
  }

  clone() {
    return new Vector(this.x, this.y);
  }

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

  length() {
    return Math.sqrt(this.lengthSquare());
  }

  static tripleProduct(a: Vector, b: Vector, c: Vector) {
    // perform a.dot(c)
    const ac = a.dot(c);
    // perform b.dot(c)
    const bc = b.dot(c);
    // perform b * a.dot(c) - a * b.dot(c)
    return new Vector(b.x * ac - a.x * bc, b.y * ac - a.y * bc);
  }

  norm() {
    const dis = Math.sqrt(this.lengthSquare());
    return new Vector(this.x / dis, this.y / dis);
  }

  rotate(angle: number, center: Vector = Vector.ZERO_VECTOR) {
    const sin = Math.sin(angle);
    const cos = Math.cos(angle);
    const diffX = this.x - center.x;
    const diffY = this.y - center.y;
    return new Vector(diffX * cos - diffY * sin + center.x, diffX * sin + diffY * cos + center.y);
  }

  scale(scale: { x: number; y: number }, center: Vector = Vector.ZERO_VECTOR) {
    return new Vector((this.x - center.x) * scale.x + center.x, (this.y - center.y) * scale.y + center.y);
  }
}

3. GJK基础碰撞盒模型

（1）基础模型类

import { Vector } from './Vector';

export abstract class GJKShape {
  cos: number;
  sin: number;
  center: Vector;

  abstract support(vector: Vector): Vector;

  constructor(deg: number, center: Vector) {
    // 角度需要转弧度制
    const rad = (deg * Math.PI) / 180;
    this.cos = Math.cos(rad);
    this.sin = Math.sin(rad);
    this.center = center;
  }

  /**
   * 绕原点旋转 -angle 度
   * @param vec
   */
  unrotate(vec: Vector): Vector {
    return new Vector(vec.x * this.cos + vec.y * this.sin, -vec.x * this.sin + vec.y * this.cos);
  }

  /**
   * 绕中心旋转
   * @param vec
   */
  transform(vec: Vector): Vector {
    const diffX = vec.x - this.center.x;
    const diffY = vec.y - this.center.y;
    return new Vector(
      diffX * this.cos - diffY * this.sin + this.center.x,
      diffX * this.sin + diffY * this.cos + this.center.y
    );
  }
}

（2）矩形

import { GJKShape } from './GJKShape';
import { Vector } from './Vector';

export class GJKPolygon extends GJKShape {
  vertices: Vector[];

  constructor(p: Vector[], angle: number, center: Vector) {
    super(angle, center);
    this.vertices = p;
  }

  support(_vec: Vector): Vector {
    const vec = this.unrotate(_vec);
    const p = this.vertices[0];
    let maxp = p;
    let maxv = p.dot(vec);
    for (let i = 1, len = this.vertices.length; i < len; i++) {
      const vertex = this.vertices[i];
      const v = vertex.dot(vec);
      if (v > maxv) {
        maxv = v;
        maxp = vertex;
      }
    }
    return this.transform(maxp);
  }
}

（3）圆形

import { GJKShape } from './GJKShape';
import { Vector } from './Vector';

export class GJKCircle extends GJKShape {
  radius: number;
  anchor: Vector;

  constructor(radius: number, angle: number, anchor: Vector, center: Vector = anchor) {
    super(angle, center);
    this.radius = radius;
    this.anchor = anchor;
  }

  support(_vec: Vector): Vector {
    const vec = this.unrotate(_vec);
    const len = Math.sqrt(vec.lengthSquare());
    const { x, y } = this.anchor;
    const radius = this.radius;
    return this.transform(new Vector((vec.x / len) * radius + x, (vec.y / len) * radius + y));
  }
}

4. GJK检测算法完整代码

import { GJKShape } from '../collision-shape';
import { Vector } from '../collision-shape';

// rewrite from https://dyn4j.org/2010/04/gjk-gilbert-johnson-keerthi/
// https://github.com/dyn4j/dyn4j/blob/master/src/main/java/org/dyn4j/collision/narrowphase/Gjk.java
export const GJK = (P: GJKShape, Q: GJKShape, D: Vector = new Vector(1, 0)) => {
  const S: Vector[] = [];
  const MAX_ITERATION = 20;
  let index = 0;

  let supportP = P.support(D);
  let supportQ = Q.support(D.negate());
  let A: Vector = supportP.subtract(supportQ);

  if (A.dot(D) < 0) {
    return false;
  }

  S[index] = A;
  let _D = A.negate();

  for (let iter = 0; iter < MAX_ITERATION; iter++) {
    supportP = P.support(_D);
    supportQ = Q.support(_D.negate());
    A = supportP.subtract(supportQ);
    S[++index] = A;

    if (A.dot(_D) < 0) {
      break;
    }

    const a = A;
    const ao = a.negate();

    if (index < 2) {
      const b = S[0];
      const ab = b.subtract(a);
      const d = Vector.tripleProduct(ab, ao, ab);
      if (d.lengthSquare() <= 1e-10) {
        _D = d.rperp();
      } else {
        _D = d;
      }
      continue;
    }

    const b = S[1];
    const c = S[0];
    const ab = b.subtract(a);
    const ac = c.subtract(a);

    const acperp = Vector.tripleProduct(ab, ac, ac);

    if (acperp.dot(ao) >= 0) {
      _D = acperp;
    } else {
      const abperp = Vector.tripleProduct(ac, ab, ab);
      if (abperp.dot(ao) < 0) {
        return true;
      }
      S[0] = S[1];
      _D = abperp;
    }
    S[1] = S[2];
    --index;
  }

  return false;
};