#include "shape.h"

CilinderShape::CilinderShape(Real radius, const Vector3 &pos, const Vector3 &dir) {

    Vector3 vx = dir.perp().norm();

    Vector3 vy = vx.cross(dir);

    matrix = Matrix4(

        vx.x*radius, vx.y*radius, vx.z*radius, 0,

        vy.x*radius, vy.y*radius, vy.z*radius, 0,

        dir.x      , dir.y      , dir.z      , 0,

        pos.x      , pos.y      , pos.z      , 1 ).inv();

}

Real CilinderShape::distance_to_triangle(const Triangle &triangle) const {

    Real dist = INFINITY;

    Vector3 v[3], d[3];

    Real A[3], B[3], C[3];

    for(int i = 0; i < 3; ++i) {

        v[i] = matrix.transform( triangle.vertices[i] );

        C[i] = v[i].x*v[i].x + v[i].y*v[i].y - 1;

        // collision with vertex

        if (C[i] <= precision)

            dist = std::min(dist, v[i].z);

    }

    for(int i = 0; i < 3; ++i) {

        d[i] = v[(i+1)%3] - v[i];

        A[i] = d[i].x*d[i].x + d[i].y*d[i].y;

        B[i] = 2*(d[i].x*v[i].x + d[i].y*v[i].y);

        // collision with edge

        Real roots[2];

        int count = solve(roots, C[i], B[i], A[i]);

        for(int j = 0; j < count; ++j)

            if (roots[j] >= -precision && roots[j] <= 1 + precision)

                dist = std::min(dist, d[i].z*roots[j] + v[i].z);

    }

    // collision with plane

    Vector3 perp = d[1].cross(d[2]);

    if (perp.z < 0) perp = Vector3(-perp.x, -perp.y, -perp.z);

    if (perp.z > precision) {

        // nearest plane touch point

        Vector3 p(0, 0, perp*v[0]/perp.z);

        Real xy = sqrt(perp.x*perp.x + perp.y*perp.y);

        if (xy > precision) {

            Real dxy = 1/xy;

            p.x = perp.x*dxy;

            p.y = perp.y*dxy;

            p.z -= xy/perp.z;

        }

        // is touch point inside tringle

        Real s =  sign( perp.cross(d[0])*(p - v[0]), 0.1*precision );

        if ( s

          && s == sign( perp.cross(d[1])*(p - v[1]), 0.1*precision )

          && s == sign( perp.cross(d[2])*(p - v[2]), 0.1*precision ) )

            dist = std::min(dist, p.z);

    }

    return dist;

}