#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;
}