#define CUTE_C2_IMPLEMENTATION
#include "cute_c2_ext.h"
#include <stdbool.h>

double PointToSegmentTOI(c2v p, c2v v, c2v a, c2v b){
    c2v n = c2CCW90(c2Sub(b, a)); //this could be passed in
    c2v PA = c2Sub(a, p);
    c2v PB = c2Sub(b, p);

    double denom = (c2Dot(v, n));
    if(denom == 0) return INFINITY; //parallel, never collide
    double t = (c2Dot(PA, n))/denom;
    //if(t<0) return INFINITY; //collided in the past

    if(c2Det2(PA, v)*c2Det2(PB, v) > 0) return INFINITY; //off the sides, never collide
    return t;
}

//adapted from https://stackoverflow.com/a/1084899
double PointToCircleTOI(c2v p, c2v v, c2v circ_p, double r){
    double a = c2Dot(v, v);
    c2v f = c2Sub(p, circ_p);
    double b = 2*c2Dot(f, v);
    double c = c2Dot(f, f)-r*r;

    double discriminant = b*b-4*a*c;
    if(discriminant < 0) return INFINITY;
    discriminant = c2Sqrt(discriminant);

    /*double t1 = (-b - discriminant)/(2*a);
    double t2 = (-b + discriminant)/(2*a); //this is the one you'd use for exit point TOI
    return c2Min(t1, t2);*/
    return (-b - discriminant)/(2*a);
}

double PolyToPolyTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, const c2Poly* pB, const c2x* bx_ptr, c2v vB, c2v* out_normal, c2v* out_contact_point){
    //return c2TOI(pA, C2_TYPE_POLY, ax_ptr, vA, pB, C2_TYPE_POLY, bx_ptr, vB, true, out_normal, out_contact_point, NULL);

    //degenerate cases, zero movement
    if(vA.x==vB.x && vA.y==vB.y) return INFINITY;

    double t = INFINITY;
    c2v n = {0,0};
    c2v p = {0,0};

    //pre-transform poly vertices to avoid doing it every access, compute which edges and faces count as "leading"
    c2Poly A = *pA;
    c2Poly B = *pB;

    c2v vA2B = c2Sub(vA, vB);
    c2v vB2A = c2Sub(vB, vA);

    bool leading_edges_a[C2_MAX_POLYGON_VERTS] = {0};
    bool leading_verts_a[C2_MAX_POLYGON_VERTS] = {0};
    bool leading_edges_b[C2_MAX_POLYGON_VERTS] = {0};
    bool leading_verts_b[C2_MAX_POLYGON_VERTS] = {0};

    for(int i = 0; i<A.count; i++){
        if(ax_ptr) A.norms[i] = c2Mulrv(ax_ptr->r, A.norms[i]);
        if(c2Dot(A.norms[i], vB2A) < 0) leading_edges_a[i] = 1;
    }
    for(int i = 0; i<A.count; i++){
        if(leading_edges_a[i] || leading_edges_a[(i-1+A.count)%A.count]){
            if(ax_ptr) A.verts[i] = c2Mulxv(*ax_ptr, A.verts[i]);
            leading_verts_a[i] = 1;
        }
    }

    for(int i = 0; i<B.count; i++){
        if(bx_ptr) B.norms[i] = c2Mulrv(bx_ptr->r, B.norms[i]);
        if(c2Dot(B.norms[i], vA2B) < 0) leading_edges_b[i] = 1;
    }
    for(int i = 0; i<B.count; i++){
        if(leading_edges_b[i] || leading_edges_b[(i-1+B.count)%B.count]){
            if(bx_ptr) B.verts[i] = c2Mulxv(*bx_ptr, B.verts[i]);
            leading_verts_b[i] = 1;
        }
    }


    //TOI of A's vertices against B's edges
    for(int j = 0; j<B.count; j++){
        if(leading_edges_b[j]){
            for(int i = 0; i<A.count; i++){
                if(leading_verts_a[i]){
                    double v_t = PointToSegmentTOI(A.verts[i], vA2B, B.verts[j], B.verts[(j+1)%B.count]);
                    if(v_t <= t){
                        if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

                        t = v_t;
                        n = c2Neg(B.norms[j]);
                        p = c2Add(A.verts[i], c2Mulvs(vA, v_t));
                    }
                }
            }
        }
    }

    //TOI of B's vertices against A's edges
    for(int j = 0; j<A.count; j++){
        if(leading_edges_a[j]){
            for(int i = 0; i<B.count; i++){
                if(leading_verts_b[i]){
                    double v_t = PointToSegmentTOI(B.verts[i], vB2A, A.verts[j], A.verts[(j+1)%A.count]);
                    if(v_t <= t){
                        if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

                        t = v_t;
                        n = A.norms[j];
                        p = c2Add(B.verts[i], c2Mulvs(vB, v_t));
                    }
                }
            }
        }
    }

    if(out_normal) *out_normal = c2SafeNorm(n);
    if(out_contact_point) *out_contact_point = p;
    return t;
}


double PolyToCircleTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point){
    //return c2TOI(pA, C2_TYPE_POLY, ax_ptr, vA, &cB, C2_TYPE_CIRCLE, NULL, vB, true, out_normal, out_contact_point, NULL);

    //degenerate cases, zero movement
    if(vA.x==vB.x && vA.y==vB.y) return INFINITY;
    

    double t = INFINITY;
    c2v n = {0,0};
    c2v p = {0,0};

    //pre-transform poly vertices to avoid doing it every access, compute which edges and faces count as "leading"
    c2Poly A = *pA;

    c2v vA2B = c2Sub(vA, vB);
    c2v vB2A = c2Sub(vB, vA);

    bool leading_edges_a[C2_MAX_POLYGON_VERTS] = {0};
    for(int i = 0; i<A.count; i++){
        if(ax_ptr) A.norms[i] = c2Mulrv(ax_ptr->r, A.norms[i]);
        if(c2Dot(A.norms[i], vB2A) < 0) leading_edges_a[i] = 1;
    }


    //TOI of B against A's vertices
    for(int i = 0; i<A.count; i++){
        if(leading_edges_a[i] || leading_edges_a[(i-1+A.count)%A.count]){ //if this is a leading vert
            if(ax_ptr) A.verts[i] = c2Mulxv(*ax_ptr, A.verts[i]);

            double v_t = PointToCircleTOI(A.verts[i], vA2B, cB.p, cB.r);
            if(v_t <= t){
                if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

                t = v_t;
                n = c2Sub(
                    c2Add(c2Mulvs(vB, t), cB.p),
                    c2Add(c2Mulvs(vA, t), A.verts[i])
                );
            }
        }
    }


    //TOI of B's center against offset A edges
    for(int i = 0; i<A.count; i++){
        if(leading_edges_a[i]){
            c2v offset = c2Mulvs(A.norms[i], cB.r);
            double v_t = PointToSegmentTOI(cB.p, vB2A, c2Add(A.verts[i], offset), c2Add(A.verts[(i+1)%A.count], offset));
            if(v_t <= t){
                if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

                t = v_t;
                n = A.norms[i];
            }
        }
    }

    
    n = c2SafeNorm(n);

    if(out_normal) *out_normal = n;
    if(out_contact_point) *out_contact_point = c2Add(cB.p, c2Add(c2Mulvs(c2Neg(n), cB.r), c2Mulvs(vB, t)));
    return t;
}



double CircleToPolyTOI(c2Circle cA, c2v vA, const c2Poly* pB, const c2x* bx_ptr, c2v vB, c2v* out_normal, c2v* out_contact_point){
    double t = PolyToCircleTOI(pB, bx_ptr, vB, cA, vA, out_normal, out_contact_point);
    if(out_normal) *out_normal = c2Neg(*out_normal);
    return t;
}

double CircleToCircleTOI(c2Circle cA, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point){
    //return c2TOI(&cA, C2_TYPE_CIRCLE, NULL, vA, &cB, C2_TYPE_CIRCLE, NULL, vB, true, out_normal, out_contact_point, NULL);

    c2v vA2B = c2Sub(vA, vB);
    double t = PointToCircleTOI(cA.p, vA2B, cB.p, cB.r+cA.r);

    if(t < 0) return INFINITY;

    c2v pA = c2Add(c2Mulvs(vA, t), cA.p);
    c2v pB = c2Add(c2Mulvs(vB, t), cB.p);
    c2v n = c2SafeNorm(c2Sub(pB, pA));

    if(out_normal) *out_normal = n;
    if(out_contact_point) *out_contact_point = c2Add(pA, c2Mulvs(n, cA.r));
    return t;
}


double PolyToCapsuleTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point){
    //return c2TOI(pA, C2_TYPE_POLY, ax_ptr, vA, &cB, C2_TYPE_CAPSULE, NULL, vB, true, out_normal, out_contact_point, NULL);

    //degenerate cases, zero movement
    if(vA.x==vB.x && vA.y==vB.y) return INFINITY;

    double t = INFINITY;
    c2v n = {0,0};
    c2v p = {0,0};

    //pre-transform poly vertices to avoid doing it every access, compute which edges and faces count as "leading"
    c2Poly A = *pA;

    c2v vA2B = c2Sub(vA, vB);
    c2v vB2A = c2Sub(vB, vA);

    bool leading_edges_a[C2_MAX_POLYGON_VERTS] = {0};
    for(int i = 0; i<A.count; i++){
        if(ax_ptr) A.norms[i] = c2Mulrv(ax_ptr->r, A.norms[i]);
        if(c2Dot(A.norms[i], vB2A) < 0) leading_edges_a[i] = 1;
    }

    bool p_on_cap = true;

    //TOI of B's endcircles against A's vertices
    for(int i = 0; i<A.count; i++){
        if(leading_edges_a[i] || leading_edges_a[(i-1+A.count)%A.count]){ //if this is a leading vert
            if(ax_ptr) A.verts[i] = c2Mulxv(*ax_ptr, A.verts[i]);

            { //a
                double v_t = PointToCircleTOI(A.verts[i], vA2B, cB.a, cB.r);
                if(v_t <= t){
                    if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

                    t = v_t;
                    n = c2Sub(
                        c2Add(c2Mulvs(vB, t), cB.a),
                        c2Add(c2Mulvs(vA, t), A.verts[i])
                    );
                    p = cB.a;
                }
            }
            { //b
                double v_t = PointToCircleTOI(A.verts[i], vA2B, cB.b, cB.r);
                if(v_t <= t){
                    if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

                    t = v_t;
                    n = c2Sub(
                        c2Add(c2Mulvs(vB, t), cB.b),
                        c2Add(c2Mulvs(vA, t), A.verts[i])
                    );
                    p = cB.b;
                }
            }
        }
    }

    //TOI of B's endpoints against offset A edges
    for(int i = 0; i<A.count; i++){
        if(leading_edges_a[i]){
            { //a
                c2v offset = c2Mulvs(A.norms[i], cB.r);
                double v_t = PointToSegmentTOI(cB.a, vB2A, c2Add(A.verts[i], offset), c2Add(A.verts[(i+1)%A.count], offset));
                if(v_t <= t){
                    if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

                    t = v_t;
                    n = A.norms[i];
                    p = cB.a;
                }
            }
            { //b
                c2v offset = c2Mulvs(A.norms[i], cB.r);
                double v_t = PointToSegmentTOI(cB.b, vB2A, c2Add(A.verts[i], offset), c2Add(A.verts[(i+1)%A.count], offset));
                if(v_t <= t){
                    if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

                    t = v_t;
                    n = A.norms[i];
                    p = cB.b;
                }
            }
        }
    }

    //TOI of A's vertices against B's leading edge

    c2v leading_capsule_n = c2SafeNorm(c2CCW90(c2Sub(cB.b, cB.a)));
    double d = c2Dot(leading_capsule_n, vA2B);
    if(d > 0){
        leading_capsule_n = c2Neg(leading_capsule_n);
    }

    if(d != 0){
        c2v cap_offset = c2Mulvs(leading_capsule_n, cB.r);

        for(int i = 0; i<A.count; i++){
            if(leading_edges_a[i] || leading_edges_a[(i-1+A.count)%A.count]){ //if this is a leading vert
                double v_t = PointToSegmentTOI(A.verts[i], vA2B, c2Add(cap_offset, cB.a), c2Add(cap_offset, cB.b));
                if(v_t <= t){
                    if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

                    t = v_t;
                    p = c2Add(A.verts[i], c2Mulvs(vA, v_t));
                    n = c2Neg(leading_capsule_n);
                    p_on_cap = false;
                }
            }
        }
    }


    n = c2SafeNorm(n);

    if(out_normal) *out_normal = n;
    if(p_on_cap){
        if(out_contact_point) *out_contact_point = c2Add(p, c2Add(c2Mulvs(c2Neg(n), cB.r), c2Mulvs(vB, t)));
    } else {
        if(out_contact_point) *out_contact_point = p;
    }
    return t;
}

double CapsuleToPolyTOI(c2Capsule cA, c2v vA, const c2Poly* pB, const c2x* bx_ptr, c2v vB, c2v* out_normal, c2v* out_contact_point){
    double t = PolyToCapsuleTOI(pB, bx_ptr, vB, cA, vA, out_normal, out_contact_point);
    if(out_normal) *out_normal = c2Neg(*out_normal);
    return t;
}



double CapsuleToCapsuleTOI(c2Capsule cA, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point){
    //return c2TOI(&cA, C2_TYPE_CAPSULE, NULL, vA, &cB, C2_TYPE_CAPSULE, NULL, vB, true, out_normal, out_contact_point, NULL);

    //degenerate cases, zero movement
    if(vA.x==vB.x && vA.y==vB.y) return INFINITY;

    double t = INFINITY;
    c2v n = {0,0};
    c2v p = {0,0};

    c2v vA2B = c2Sub(vA, vB);
    c2v vB2A = c2Sub(vB, vA);

    double sum_radius = cA.r+cB.r;

    //TOI of circles of B against leading edge of A
    c2v leading_edge = c2SafeNorm(c2CCW90(c2Sub(cA.b, cA.a)));
    double d = c2Dot(leading_edge, vB2A);
    if(d > 0){
        leading_edge = c2Neg(leading_edge);
    }
    bool p_on_a = true;

    if(d != 0){
        c2v cap_offset = c2Mulvs(leading_edge, sum_radius);

        {//a
            double v_t = PointToSegmentTOI(cB.a, vB2A, c2Add(cap_offset, cA.a), c2Add(cap_offset, cA.b));
            if(v_t <= t){
                if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

                t = v_t;
                p = cB.a;
                p_on_a = false;
                n = leading_edge;
            }
        }

        {//b
            double v_t = PointToSegmentTOI(cB.b, vB2A, c2Add(cap_offset, cA.a), c2Add(cap_offset, cA.b));
            if(v_t <= t){
                if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

                t = v_t;
                p = cB.b;
                p_on_a = false;
                n = leading_edge;
            }
        }
    }


    //TOI of circles of A against leading edge of B
    leading_edge = c2SafeNorm(c2CCW90(c2Sub(cB.b, cB.a)));
    d = c2Dot(leading_edge, vA2B);
    if(d > 0){
        leading_edge = c2Neg(leading_edge);
    }
    if(d != 0){
        c2v cap_offset = c2Mulvs(leading_edge, sum_radius);

        {//a
            double v_t = PointToSegmentTOI(cA.a, vA2B, c2Add(cap_offset, cB.a), c2Add(cap_offset, cB.b));
            if(v_t <= t){
                if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

                t = v_t;
                p = cA.a;
                p_on_a = true;
                n = c2Neg(leading_edge);
            }
        }

        {//a
            double v_t = PointToSegmentTOI(cA.b, vA2B, c2Add(cap_offset, cB.a), c2Add(cap_offset, cB.b));
            if(v_t <= t){
                if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

                t = v_t;
                p = cA.b;
                p_on_a = true;
                n = c2Neg(leading_edge);
            }
        }
    }


    //TOI circles of B against circles of A

    {//aa
        double v_t = PointToCircleTOI(cB.a, vB2A, cA.a, sum_radius);
        if(v_t <= t){
            if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

            t = v_t;
            p = cA.a;
            p_on_a = true;
            c2v pA = c2Add(c2Mulvs(vA, t), cA.a);
            c2v pB = c2Add(c2Mulvs(vB, t), cB.a);
            n = c2Sub(pB, pA);
        }
    }

    {//ba
        double v_t = PointToCircleTOI(cB.b, vB2A, cA.a, sum_radius);
        if(v_t <= t){
            if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

            t = v_t;
            p = cA.a;
            p_on_a = true;
            c2v pA = c2Add(c2Mulvs(vA, t), cA.a);
            c2v pB = c2Add(c2Mulvs(vB, t), cB.b);
            n = c2Sub(pB, pA);
        }
    }
    {//ab
        double v_t = PointToCircleTOI(cB.a, vB2A, cA.b, sum_radius);
        if(v_t <= t){
            if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

            t = v_t;
            p = cA.b;
            p_on_a = true;
            c2v pA = c2Add(c2Mulvs(vA, t), cA.b);
            c2v pB = c2Add(c2Mulvs(vB, t), cB.a);
            n = c2Sub(pB, pA);
        }
    }

    {//bb
        double v_t = PointToCircleTOI(cB.b, vB2A, cA.b, sum_radius);
        if(v_t <= t){
            if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

            t = v_t;
            p = cA.b;
            p_on_a = true;
            c2v pA = c2Add(c2Mulvs(vA, t), cA.b);
            c2v pB = c2Add(c2Mulvs(vB, t), cB.b);
            n = c2Sub(pB, pA);
        }
    }


    n = c2SafeNorm(n);
    if(out_normal) *out_normal = n;
    if(out_contact_point) {
        if(p_on_a){
            *out_contact_point = c2Add(p, c2Add(c2Mulvs(n, cA.r), c2Mulvs(vA, t)));
        } else {
            *out_contact_point = c2Add(p, c2Add(c2Mulvs(c2Neg(n), cB.r), c2Mulvs(vB, t)));
        }
    }
    return t;
}


double CapsuleToCircleTOI(c2Capsule cA, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point){
    double t = CircleToCapsuleTOI(cB, vB, cA, vA, out_normal, out_contact_point);
    if(out_normal) *out_normal = c2Neg(*out_normal);
    return t;
}
double CircleToCapsuleTOI(c2Circle cA, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point){
    //return c2TOI(&cA, C2_TYPE_CIRCLE, NULL, vA, &cB, C2_TYPE_CAPSULE, NULL, vB, true, out_normal, out_contact_point, NULL);

    //degenerate cases, zero movement
    if(vA.x==vB.x && vA.y==vB.y) return INFINITY;

    double t = INFINITY;
    c2v n = {0,0};
    c2v p = {0,0};

    c2v vA2B = c2Sub(vA, vB);
    c2v vB2A = c2Sub(vB, vA);

    double sum_radius = cA.r+cB.r;



    //TOI of A against leading edge of B
    c2v leading_edge = c2SafeNorm(c2CCW90(c2Sub(cB.b, cB.a)));
    double d = c2Dot(leading_edge, vA2B);
    if(d > 0){
        leading_edge = c2Neg(leading_edge);
    }
    if(d != 0){
        c2v cap_offset = c2Mulvs(leading_edge, sum_radius);

        double v_t = PointToSegmentTOI(cA.p, vA2B, c2Add(cap_offset, cB.a), c2Add(cap_offset, cB.b));
        if(v_t <= t){
            if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

            t = v_t;
            n = c2Neg(leading_edge);
        }
    }

    //TOI of A against circles of B

    {//a
        double v_t = PointToCircleTOI(cA.p, vA2B, cB.a, sum_radius);
        if(v_t <= t){
            if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

            t = v_t;
            c2v pA = c2Add(c2Mulvs(vA, t), cA.p);
            c2v pB = c2Add(c2Mulvs(vB, t), cB.a);
            n = c2Sub(pB, pA);
        }
    }

    {//b
        double v_t = PointToCircleTOI(cA.p, vA2B, cB.b, sum_radius);
        if(v_t <= t){
            if(v_t<0) return INFINITY; //point collided in the past, therefore this is a degenerate case

            t = v_t;
            c2v pA = c2Add(c2Mulvs(vA, t), cA.p);
            c2v pB = c2Add(c2Mulvs(vB, t), cB.b);
            n = c2Sub(pB, pA);
        }
    }



    n = c2SafeNorm(n);
    if(out_normal) *out_normal = n;
    if(out_contact_point) *out_contact_point = c2Add(cA.p, c2Add(c2Mulvs(n, cA.r), c2Mulvs(vA, t)));
    return t;

}



//AABB stuff
double PolyToAABBTOI(const c2Poly* pA, const c2x* ax_ptr, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point){
    c2Poly pB;
    c2BBVerts(pB.verts, &bB);
    pB.count = 4;
    c2Norms(pB.verts, pB.norms, 4);

    return PolyToPolyTOI(pA, ax_ptr, vA, &pB, NULL, vB, out_normal, out_contact_point);
}
double CircleToAABBTOI(c2Circle cA, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point){
    c2Poly pB;
    c2BBVerts(pB.verts, &bB);
    pB.count = 4;
    c2Norms(pB.verts, pB.norms, 4);

    return CircleToPolyTOI(cA, vA, &pB, NULL, vB, out_normal, out_contact_point);
}
double CapsuleToAABBTOI(c2Capsule cA, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point){
    c2Poly pB;
    c2BBVerts(pB.verts, &bB);
    pB.count = 4;
    c2Norms(pB.verts, pB.norms, 4);

    return CapsuleToPolyTOI(cA, vA, &pB, NULL, vB, out_normal, out_contact_point);
}

double AABBToPolyTOI(c2AABB bA, c2v vA, const c2Poly* pB, const c2x* bx_ptr, c2v vB, c2v* out_normal, c2v* out_contact_point){
    c2Poly pA;
    c2BBVerts(pA.verts, &bA);
    pA.count = 4;
    c2Norms(pA.verts, pA.norms, 4);

    return PolyToPolyTOI(&pA, NULL, vA, pB, bx_ptr, vB, out_normal, out_contact_point);
}
double AABBToCircleTOI(c2AABB bA, c2v vA, c2Circle cB, c2v vB, c2v* out_normal, c2v* out_contact_point){
    c2Poly pA;
    c2BBVerts(pA.verts, &bA);
    pA.count = 4;
    c2Norms(pA.verts, pA.norms, 4);

    return PolyToCircleTOI(&pA, NULL, vA, cB, vB, out_normal, out_contact_point);
}
double AABBToCapsuleTOI(c2AABB bA, c2v vA, c2Capsule cB, c2v vB, c2v* out_normal, c2v* out_contact_point){
    c2Poly pA;
    c2BBVerts(pA.verts, &bA);
    pA.count = 4;
    c2Norms(pA.verts, pA.norms, 4);

    return PolyToCapsuleTOI(&pA, NULL, vA, cB, vB, out_normal, out_contact_point);
}
double AABBToAABBTOI(c2AABB bA, c2v vA, c2AABB bB, c2v vB, c2v* out_normal, c2v* out_contact_point){
    c2Poly pA;
    c2BBVerts(pA.verts, &bA);
    pA.count = 4;
    c2Norms(pA.verts, pA.norms, 4);

    c2Poly pB;
    c2BBVerts(pB.verts, &bB);
    pB.count = 4;
    c2Norms(pB.verts, pB.norms, 4);

    return PolyToPolyTOI(&pA, NULL, vA, &pB, NULL, vB, out_normal, out_contact_point);
}