#include "gjk.h"

static void
transform(float *r, const float *v, const float *r33, const float *t3)
{
    for (int i = 0; i < 3; ++i) {
        r[i]  = v[i] * r33[i*3+0];
        r[i] += v[i] * r33[i*3+1];
        r[i] += v[i] * r33[i*3+2];
        r[i] += t3[i];
    }
}
static void
transformS(float *v, const float *r33, const float *t3)
{
    float tmp[3]; f3cpy(tmp, v);
    transform(v, tmp, r33, t3);
}
static void
transformT(float *r, const float *v, const float *r33, const float *t3)
{
    for (int i = 0; i < 3; ++i) {
        float p = v[i] - t3[i];
        r[i]  = p * r33[0*3+i];
        r[i] += p * r33[1*3+i];
        r[i] += p * r33[2*3+i];
    }
}
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};
    f3cpy(s.a, averts);
    f3cpy(s.b, bverts);
    
    transformS(s.a, arot, apos);
    transformS(s.b, 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]; transformT(da, n, arot, apos);
        float db[3]; transformT(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 */
        transformS(s.a, arot, apos);
        transformS(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;
}