the-world-engine/dist/esm/box2d.ts/build/collision/b2_distance.js

three.js based, unity like game engine for browser

import { b2_epsilon, b2_epsilon_sq, b2_polygonRadius, b2_linearSlop } from "../common/b2_settings.js";

import { b2Max, b2Vec2, b2Rot, b2Transform } from "../common/b2_math.js";

export class b2DistanceProxy {
    constructor() {
        this.m_buffer = b2Vec2.MakeArray(2);
        this.m_vertices = this.m_buffer;
        this.m_count = 0;
        this.m_radius = 0;
    }
    Copy(t) {
        if (t.m_vertices === t.m_buffer) {
            this.m_vertices = this.m_buffer;
            this.m_buffer[0].Copy(t.m_buffer[0]);
            this.m_buffer[1].Copy(t.m_buffer[1]);
        } else {
            this.m_vertices = t.m_vertices;
        }
        this.m_count = t.m_count;
        this.m_radius = t.m_radius;
        return this;
    }
    Reset() {
        this.m_vertices = this.m_buffer;
        this.m_count = 0;
        this.m_radius = 0;
        return this;
    }
    SetShape(t, s) {
        t.SetupDistanceProxy(this, s);
    }
    SetVerticesRadius(t, s, e) {
        this.m_vertices = t;
        this.m_count = s;
        this.m_radius = e;
    }
    GetSupport(t) {
        let s = 0;
        let e = b2Vec2.DotVV(this.m_vertices[0], t);
        for (let i = 1; i < this.m_count; ++i) {
            const c = b2Vec2.DotVV(this.m_vertices[i], t);
            if (c > e) {
                s = i;
                e = c;
            }
        }
        return s;
    }
    GetSupportVertex(t) {
        let s = 0;
        let e = b2Vec2.DotVV(this.m_vertices[0], t);
        for (let i = 1; i < this.m_count; ++i) {
            const c = b2Vec2.DotVV(this.m_vertices[i], t);
            if (c > e) {
                s = i;
                e = c;
            }
        }
        return this.m_vertices[s];
    }
    GetVertexCount() {
        return this.m_count;
    }
    GetVertex(t) {
        return this.m_vertices[t];
    }
}

export class b2SimplexCache {
    constructor() {
        this.metric = 0;
        this.count = 0;
        this.indexA = [ 0, 0, 0 ];
        this.indexB = [ 0, 0, 0 ];
    }
    Reset() {
        this.metric = 0;
        this.count = 0;
        return this;
    }
}

export class b2DistanceInput {
    constructor() {
        this.proxyA = new b2DistanceProxy;
        this.proxyB = new b2DistanceProxy;
        this.transformA = new b2Transform;
        this.transformB = new b2Transform;
        this.useRadii = false;
    }
    Reset() {
        this.proxyA.Reset();
        this.proxyB.Reset();
        this.transformA.SetIdentity();
        this.transformB.SetIdentity();
        this.useRadii = false;
        return this;
    }
}

export class b2DistanceOutput {
    constructor() {
        this.pointA = new b2Vec2;
        this.pointB = new b2Vec2;
        this.distance = 0;
        this.iterations = 0;
    }
    Reset() {
        this.pointA.SetZero();
        this.pointB.SetZero();
        this.distance = 0;
        this.iterations = 0;
        return this;
    }
}

export class b2ShapeCastInput {
    constructor() {
        this.proxyA = new b2DistanceProxy;
        this.proxyB = new b2DistanceProxy;
        this.transformA = new b2Transform;
        this.transformB = new b2Transform;
        this.translationB = new b2Vec2;
    }
}

export class b2ShapeCastOutput {
    constructor() {
        this.point = new b2Vec2;
        this.normal = new b2Vec2;
        this.lambda = 0;
        this.iterations = 0;
    }
}

export let b2_gjkCalls = 0;

export let b2_gjkIters = 0;

export let b2_gjkMaxIters = 0;

export function b2_gjk_reset() {
    b2_gjkCalls = 0;
    b2_gjkIters = 0;
    b2_gjkMaxIters = 0;
}

export class b2SimplexVertex {
    constructor() {
        this.wA = new b2Vec2;
        this.wB = new b2Vec2;
        this.w = new b2Vec2;
        this.a = 0;
        this.indexA = 0;
        this.indexB = 0;
    }
    Copy(t) {
        this.wA.Copy(t.wA);
        this.wB.Copy(t.wB);
        this.w.Copy(t.w);
        this.a = t.a;
        this.indexA = t.indexA;
        this.indexB = t.indexB;
        return this;
    }
}

export class b2Simplex {
    constructor() {
        this.m_v1 = new b2SimplexVertex;
        this.m_v2 = new b2SimplexVertex;
        this.m_v3 = new b2SimplexVertex;
        this.m_vertices = [];
        this.m_count = 0;
        this.m_vertices[0] = this.m_v1;
        this.m_vertices[1] = this.m_v2;
        this.m_vertices[2] = this.m_v3;
    }
    ReadCache(t, s, e, i, c) {
        this.m_count = t.count;
        const n = this.m_vertices;
        for (let h = 0; h < this.m_count; ++h) {
            const o = n[h];
            o.indexA = t.indexA[h];
            o.indexB = t.indexB[h];
            const b = s.GetVertex(o.indexA);
            const r = i.GetVertex(o.indexB);
            b2Transform.MulXV(e, b, o.wA);
            b2Transform.MulXV(c, r, o.wB);
            b2Vec2.SubVV(o.wB, o.wA, o.w);
            o.a = 0;
        }
        if (this.m_count > 1) {
            const s = t.metric;
            const e = this.GetMetric();
            if (e < .5 * s || 2 * s < e || e < b2_epsilon) {
                this.m_count = 0;
            }
        }
        if (this.m_count === 0) {
            const t = n[0];
            t.indexA = 0;
            t.indexB = 0;
            const h = s.GetVertex(0);
            const o = i.GetVertex(0);
            b2Transform.MulXV(e, h, t.wA);
            b2Transform.MulXV(c, o, t.wB);
            b2Vec2.SubVV(t.wB, t.wA, t.w);
            t.a = 1;
            this.m_count = 1;
        }
    }
    WriteCache(t) {
        t.metric = this.GetMetric();
        t.count = this.m_count;
        const s = this.m_vertices;
        for (let e = 0; e < this.m_count; ++e) {
            t.indexA[e] = s[e].indexA;
            t.indexB[e] = s[e].indexB;
        }
    }
    GetSearchDirection(t) {
        switch (this.m_count) {
          case 1:
            return b2Vec2.NegV(this.m_v1.w, t);

          case 2:
            {
                const s = b2Vec2.SubVV(this.m_v2.w, this.m_v1.w, t);
                const e = b2Vec2.CrossVV(s, b2Vec2.NegV(this.m_v1.w, b2Vec2.s_t0));
                if (e > 0) {
                    return b2Vec2.CrossOneV(s, t);
                } else {
                    return b2Vec2.CrossVOne(s, t);
                }
            }

          default:
            return t.SetZero();
        }
    }
    GetClosestPoint(t) {
        switch (this.m_count) {
          case 0:
            return t.SetZero();

          case 1:
            return t.Copy(this.m_v1.w);

          case 2:
            return t.Set(this.m_v1.a * this.m_v1.w.x + this.m_v2.a * this.m_v2.w.x, this.m_v1.a * this.m_v1.w.y + this.m_v2.a * this.m_v2.w.y);

          case 3:
            return t.SetZero();

          default:
            return t.SetZero();
        }
    }
    GetWitnessPoints(t, s) {
        switch (this.m_count) {
          case 0:
            break;

          case 1:
            t.Copy(this.m_v1.wA);
            s.Copy(this.m_v1.wB);
            break;

          case 2:
            t.x = this.m_v1.a * this.m_v1.wA.x + this.m_v2.a * this.m_v2.wA.x;
            t.y = this.m_v1.a * this.m_v1.wA.y + this.m_v2.a * this.m_v2.wA.y;
            s.x = this.m_v1.a * this.m_v1.wB.x + this.m_v2.a * this.m_v2.wB.x;
            s.y = this.m_v1.a * this.m_v1.wB.y + this.m_v2.a * this.m_v2.wB.y;
            break;

          case 3:
            s.x = t.x = this.m_v1.a * this.m_v1.wA.x + this.m_v2.a * this.m_v2.wA.x + this.m_v3.a * this.m_v3.wA.x;
            s.y = t.y = this.m_v1.a * this.m_v1.wA.y + this.m_v2.a * this.m_v2.wA.y + this.m_v3.a * this.m_v3.wA.y;
            break;

          default:
            break;
        }
    }
    GetMetric() {
        switch (this.m_count) {
          case 0:
            return 0;

          case 1:
            return 0;

          case 2:
            return b2Vec2.DistanceVV(this.m_v1.w, this.m_v2.w);

          case 3:
            return b2Vec2.CrossVV(b2Vec2.SubVV(this.m_v2.w, this.m_v1.w, b2Vec2.s_t0), b2Vec2.SubVV(this.m_v3.w, this.m_v1.w, b2Vec2.s_t1));

          default:
            return 0;
        }
    }
    Solve2() {
        const t = this.m_v1.w;
        const s = this.m_v2.w;
        const e = b2Vec2.SubVV(s, t, b2Simplex.s_e12);
        const i = -b2Vec2.DotVV(t, e);
        if (i <= 0) {
            this.m_v1.a = 1;
            this.m_count = 1;
            return;
        }
        const c = b2Vec2.DotVV(s, e);
        if (c <= 0) {
            this.m_v2.a = 1;
            this.m_count = 1;
            this.m_v1.Copy(this.m_v2);
            return;
        }
        const n = 1 / (c + i);
        this.m_v1.a = c * n;
        this.m_v2.a = i * n;
        this.m_count = 2;
    }
    Solve3() {
        const t = this.m_v1.w;
        const s = this.m_v2.w;
        const e = this.m_v3.w;
        const i = b2Vec2.SubVV(s, t, b2Simplex.s_e12);
        const c = b2Vec2.DotVV(t, i);
        const n = b2Vec2.DotVV(s, i);
        const h = n;
        const o = -c;
        const b = b2Vec2.SubVV(e, t, b2Simplex.s_e13);
        const r = b2Vec2.DotVV(t, b);
        const a = b2Vec2.DotVV(e, b);
        const _ = a;
        const p = -r;
        const l = b2Vec2.SubVV(e, s, b2Simplex.s_e23);
        const V = b2Vec2.DotVV(s, l);
        const u = b2Vec2.DotVV(e, l);
        const f = u;
        const x = -V;
        const w = b2Vec2.CrossVV(i, b);
        const S = w * b2Vec2.CrossVV(s, e);
        const m = w * b2Vec2.CrossVV(e, t);
        const C = w * b2Vec2.CrossVV(t, s);
        if (o <= 0 && p <= 0) {
            this.m_v1.a = 1;
            this.m_count = 1;
            return;
        }
        if (h > 0 && o > 0 && C <= 0) {
            const t = 1 / (h + o);
            this.m_v1.a = h * t;
            this.m_v2.a = o * t;
            this.m_count = 2;
            return;
        }
        if (_ > 0 && p > 0 && m <= 0) {
            const t = 1 / (_ + p);
            this.m_v1.a = _ * t;
            this.m_v3.a = p * t;
            this.m_count = 2;
            this.m_v2.Copy(this.m_v3);
            return;
        }
        if (h <= 0 && x <= 0) {
            this.m_v2.a = 1;
            this.m_count = 1;
            this.m_v1.Copy(this.m_v2);
            return;
        }
        if (_ <= 0 && f <= 0) {
            this.m_v3.a = 1;
            this.m_count = 1;
            this.m_v1.Copy(this.m_v3);
            return;
        }
        if (f > 0 && x > 0 && S <= 0) {
            const t = 1 / (f + x);
            this.m_v2.a = f * t;
            this.m_v3.a = x * t;
            this.m_count = 2;
            this.m_v1.Copy(this.m_v3);
            return;
        }
        const k = 1 / (S + m + C);
        this.m_v1.a = S * k;
        this.m_v2.a = m * k;
        this.m_v3.a = C * k;
        this.m_count = 3;
    }
}

b2Simplex.s_e12 = new b2Vec2;

b2Simplex.s_e13 = new b2Vec2;

b2Simplex.s_e23 = new b2Vec2;

const b2Distance_s_simplex = new b2Simplex;

const b2Distance_s_saveA = [ 0, 0, 0 ];

const b2Distance_s_saveB = [ 0, 0, 0 ];

const b2Distance_s_p = new b2Vec2;

const b2Distance_s_d = new b2Vec2;

const b2Distance_s_normal = new b2Vec2;

const b2Distance_s_supportA = new b2Vec2;

const b2Distance_s_supportB = new b2Vec2;

export function b2Distance(t, s, e) {
    ++b2_gjkCalls;
    const i = e.proxyA;
    const c = e.proxyB;
    const n = e.transformA;
    const h = e.transformB;
    const o = b2Distance_s_simplex;
    o.ReadCache(s, i, n, c, h);
    const b = o.m_vertices;
    const r = 20;
    const a = b2Distance_s_saveA;
    const _ = b2Distance_s_saveB;
    let p = 0;
    let l = 0;
    while (l < r) {
        p = o.m_count;
        for (let t = 0; t < p; ++t) {
            a[t] = b[t].indexA;
            _[t] = b[t].indexB;
        }
        switch (o.m_count) {
          case 1:
            break;

          case 2:
            o.Solve2();
            break;

          case 3:
            o.Solve3();
            break;

          default:
            break;
        }
        if (o.m_count === 3) {
            break;
        }
        const t = o.GetSearchDirection(b2Distance_s_d);
        if (t.LengthSquared() < b2_epsilon_sq) {
            break;
        }
        const s = b[o.m_count];
        s.indexA = i.GetSupport(b2Rot.MulTRV(n.q, b2Vec2.NegV(t, b2Vec2.s_t0), b2Distance_s_supportA));
        b2Transform.MulXV(n, i.GetVertex(s.indexA), s.wA);
        s.indexB = c.GetSupport(b2Rot.MulTRV(h.q, t, b2Distance_s_supportB));
        b2Transform.MulXV(h, c.GetVertex(s.indexB), s.wB);
        b2Vec2.SubVV(s.wB, s.wA, s.w);
        ++l;
        ++b2_gjkIters;
        let e = false;
        for (let t = 0; t < p; ++t) {
            if (s.indexA === a[t] && s.indexB === _[t]) {
                e = true;
                break;
            }
        }
        if (e) {
            break;
        }
        ++o.m_count;
    }
    b2_gjkMaxIters = b2Max(b2_gjkMaxIters, l);
    o.GetWitnessPoints(t.pointA, t.pointB);
    t.distance = b2Vec2.DistanceVV(t.pointA, t.pointB);
    t.iterations = l;
    o.WriteCache(s);
    if (e.useRadii) {
        const s = i.m_radius;
        const e = c.m_radius;
        if (t.distance > s + e && t.distance > b2_epsilon) {
            t.distance -= s + e;
            const i = b2Vec2.SubVV(t.pointB, t.pointA, b2Distance_s_normal);
            i.Normalize();
            t.pointA.SelfMulAdd(s, i);
            t.pointB.SelfMulSub(e, i);
        } else {
            const s = b2Vec2.MidVV(t.pointA, t.pointB, b2Distance_s_p);
            t.pointA.Copy(s);
            t.pointB.Copy(s);
            t.distance = 0;
        }
    }
}

const b2ShapeCast_s_n = new b2Vec2;

const b2ShapeCast_s_simplex = new b2Simplex;

const b2ShapeCast_s_wA = new b2Vec2;

const b2ShapeCast_s_wB = new b2Vec2;

const b2ShapeCast_s_v = new b2Vec2;

const b2ShapeCast_s_p = new b2Vec2;

const b2ShapeCast_s_pointA = new b2Vec2;

const b2ShapeCast_s_pointB = new b2Vec2;

export function b2ShapeCast(t, s) {
    t.iterations = 0;
    t.lambda = 1;
    t.normal.SetZero();
    t.point.SetZero();
    const e = s.proxyA;
    const i = s.proxyB;
    const c = b2Max(e.m_radius, b2_polygonRadius);
    const n = b2Max(i.m_radius, b2_polygonRadius);
    const h = c + n;
    const o = s.transformA;
    const b = s.transformB;
    const r = s.translationB;
    const a = b2ShapeCast_s_n.Set(0, 0);
    let _ = 0;
    const p = b2ShapeCast_s_simplex;
    p.m_count = 0;
    const l = p.m_vertices;
    let V = e.GetSupport(b2Rot.MulTRV(o.q, b2Vec2.NegV(r, b2Vec2.s_t1), b2Vec2.s_t0));
    let u = b2Transform.MulXV(o, e.GetVertex(V), b2ShapeCast_s_wA);
    let f = i.GetSupport(b2Rot.MulTRV(b.q, r, b2Vec2.s_t0));
    let x = b2Transform.MulXV(b, i.GetVertex(f), b2ShapeCast_s_wB);
    const w = b2Vec2.SubVV(u, x, b2ShapeCast_s_v);
    const S = b2Max(b2_polygonRadius, h - b2_polygonRadius);
    const m = .5 * b2_linearSlop;
    const C = 20;
    let k = 0;
    while (k < C && w.Length() - S > m) {
        t.iterations += 1;
        V = e.GetSupport(b2Rot.MulTRV(o.q, b2Vec2.NegV(w, b2Vec2.s_t1), b2Vec2.s_t0));
        u = b2Transform.MulXV(o, e.GetVertex(V), b2ShapeCast_s_wA);
        f = i.GetSupport(b2Rot.MulTRV(b.q, w, b2Vec2.s_t0));
        x = b2Transform.MulXV(b, i.GetVertex(f), b2ShapeCast_s_wB);
        const s = b2Vec2.SubVV(u, x, b2ShapeCast_s_p);
        w.Normalize();
        const c = b2Vec2.DotVV(w, s);
        const n = b2Vec2.DotVV(w, r);
        if (c - S > _ * n) {
            if (n <= 0) {
                return false;
            }
            _ = (c - S) / n;
            if (_ > 1) {
                return false;
            }
            a.Copy(w).SelfNeg();
            p.m_count = 0;
        }
        const h = l[p.m_count];
        h.indexA = f;
        h.wA.Copy(x).SelfMulAdd(_, r);
        h.indexB = V;
        h.wB.Copy(u);
        h.w.Copy(h.wB).SelfSub(h.wA);
        h.a = 1;
        p.m_count += 1;
        switch (p.m_count) {
          case 1:
            break;

          case 2:
            p.Solve2();
            break;

          case 3:
            p.Solve3();
            break;

          default:
        }
        if (p.m_count === 3) {
            return false;
        }
        p.GetClosestPoint(w);
        ++k;
    }
    if (k === 0) {
        return false;
    }
    const D = b2ShapeCast_s_pointA;
    const R = b2ShapeCast_s_pointB;
    p.GetWitnessPoints(D, R);
    if (w.LengthSquared() > 0) {
        a.Copy(w).SelfNeg();
        a.Normalize();
    }
    t.normal.Copy(a);
    t.lambda = _;
    t.iterations = k;
    return true;
}