#include "collider.h"

Real Collider::distance_to_triangle(const Triangle &triangle, const Vector3 &pos, const Vector3 &dir) {

    const Vector3 &n = triangle.normal;

    const Vector3 &a = triangle.vertices[0];

    const Vector3 &b = triangle.vertices[1];

    const Vector3 &c = triangle.vertices[2];

    Real d = n * dir;

    if (fabs(d) < 1e-5) return INFINITY;

    if ((b - a).len() < 1e-5) return INFINITY;

    if ((c - b).len() < 1e-5) return INFINITY;

    if ((a - c).len() < 1e-5) return INFINITY;

    d = 1/d;

    //Real angle = fabs(atan(d));

    Real l = (a - pos)*n*d;

    Vector3 p = pos + dir*l;

    //Vector3 dp = n.cross(dir.cross(n));

    //Real th = 0, ch = 0, co = 0;

    //tool.calc_collision(angle, th, ch, co);

    //Real dist = sqrt(ch*ch + co*co);

    Vector3 pp = p;// + dp*dist;

    if (n.cross(b - a)*(pp - a) < 0) return INFINITY;

    if (n.cross(c - b)*(pp - b) < 0) return INFINITY;

    if (n.cross(a - c)*(pp - c) < 0) return INFINITY;

    return l;// - th;

}

Real Collider::distance_to_model(const Vector3 &pos, const Vector3 &dir) {

    Real distance = INFINITY;

    for(TriangleList::const_iterator i = model.triangles.begin(); i != model.triangles.end(); ++i)

        distance = std::min(distance, distance_to_triangle(*i, pos, dir));

    return distance;

}