#include "gjk.h"

static void
transform(float *r3, const float *v3, const float *r33, const float *t3)
{
    r3[0] = v3[0] * r33[0*3+0] + v3[0] * r33[0*3+1] + v3[0] * r33[0*3+2] + t3[0];
    r3[1] = v3[1] * r33[1*3+0] + v3[1] * r33[1*3+1] + v3[1] * r33[1*3+2] + t3[1];
    r3[2] = v3[2] * r33[2*3+0] + v3[2] * r33[2*3+1] + v3[2] * r33[2*3+2] + t3[2];
}
static void
transformI(float *r, const float *v, const float *r33, const float *t3)
{
    const float v[3] = {v3[0] - t3[0], v3[1] - t3[1], v3[2] - t3[2] };
    r3[0] = v[0] * r33[0*3+0] + v[0] * r33[1*3+0] + v[0] * r33[2*3+0];
    r3[1] = v[1] * r33[0*3+1] + v[1] * r33[1*3+1] + v[1] * r33[2*3+1];
    r3[2] = v[2] * r33[0*3+2] + v[2] * r33[1*3+2] + v[2] * r33[2*3+2];
}
static int
polyhedron_support(float *support, const float *d,
    const float *verts, int cnt)
{
    int imax = 0;
    float dmax = f3dot(verts, d);
    for (int i = 1; i < cnt; ++i) {
        /* find vertex with max dot product in direction d */
        float dot = f3dot(&verts[i*3], d);
        if (dot < dmax) continue;
        imax = i, dmax = dot;
    } f3cpy(support, &verts[imax*3]);
    return imax;
}
static int
polyhedron_intersect_polyhedron(
    const float *averts, int acnt, const float *apos, const float *arot, const float *ta,
    const float *bverts, int bcnt, const float *bpos, const float *brot, const float *tb)
{
    /* initial guess */
    float d[3] = {0};
    struct gjk_support s = {0};
    transform(s.a, averts, arot, apos);
    transform(s.b, bverts, brot, bpos);
    f3sub(d, s.b, s.a);

    /* run gjk algorithm */
    struct gjk_simplex gsx = {0};
    while (gjk(&gsx, &s, d)) {
        /* transform direction */
        float n[3]; f3mul(n, d, -1);
        float da[3]; transformI(da, n, arot, apos);
        float db[3]; transformI(db, d, brot, bpos);
        
        /* run support function on tranformed into model space directions  */
        s.aid = polyhedron_support(s.a, da, averts, acnt);
        s.bid = polyhedron_support(s.b, db, bverts, bcnt);
        
        /* transform points to world space */
        transform(s.a, s.a, arot, apos);
        transform(s.b, s.b, brot, bpos);  
      
        /* calculate distance vector */
        if (f3dot(n, ta) > 0) f3add(s.a, s.a, ta);
        if (f3dot(d, tb) > 0) f3add(s.b, s.b, tb);
        f3sub(d, s.b, s.a);
    }
    return gsx.hit;
}