#include "../inc/shared.h"

#define EPS 0.99
/*
   Determine whether or not the line segment p1,p2
   Intersects the 3 vertex facet bounded by pa,pb,pc
   Return true/false and the intersection point p

   The equation of the line is p = p1 + mu (p2 - p1)
   The equation of the plane is a x + b y + c z + d = 0
                                n[0] x + n[1] y + n[2] z + d = 0
  
   Source: http://astronomy.swin.edu.au/~pbourke/geometry/linefacet/
*/
bool LinePolyIntersect3( Vec3 p1, Vec3 p2, Vec3 pa, Vec3 pb, Vec3 pc, Vec3 *p )
{
   double d;
   double a1,a2,a3;
   double total,denom,mu;
   Vec3 n,pa1,pa2,pa3;

   /* Calculate the parameters for the plane */
   n[0] = (pb[1] - pa[1])*(pc[2] - pa[2]) - (pb[2] - pa[2])*(pc[1] - pa[1]);
   n[1] = (pb[2] - pa[2])*(pc[0] - pa[0]) - (pb[0] - pa[0])*(pc[2] - pa[2]);
   n[2] = (pb[0] - pa[0])*(pc[1] - pa[1]) - (pb[1] - pa[1])*(pc[0] - pa[0]);
   n.Normalize();
   d = - n[0] * pa[0] - n[1] * pa[1] - n[2] * pa[2];

   /* Calculate the position on the line that intersects the plane */
   denom = n[0] * (p2[0] - p1[0]) + n[1] * (p2[1] - p1[1]) + n[2] * (p2[2] - p1[2]);
   if (ARIabs(denom) < EPS)         /* Line and plane don't intersect */
      return(false);
   mu = - (d + n[0] * p1[0] + n[1] * p1[1] + n[2] * p1[2]) / denom;
   (*p)[0] = p1[0] + mu * (p2[0] - p1[0]);
   (*p)[1] = p1[1] + mu * (p2[1] - p1[1]);
   (*p)[2] = p1[2] + mu * (p2[2] - p1[2]);
   if (mu < 0 || mu > 1)   /* Intersection not along line segment */
      return(false);

   /* Determine whether or not the intersection point is bounded by pa,pb,pc */
   pa1[0] = pa[0] - (*p)[0];
   pa1[1] = pa[1] - (*p)[1];
   pa1[2] = pa[2] - (*p)[2];
   pa1.Normalize();
   pa2[0] = pb[0] - (*p)[0];
   pa2[1] = pb[1] - (*p)[1];
   pa2[2] = pb[2] - (*p)[2];
   pa2.Normalize();
   pa3[0] = pc[0] - (*p)[0];
   pa3[1] = pc[1] - (*p)[1];
   pa3[2] = pc[2] - (*p)[2];
   pa3.Normalize();
   a1 = pa1[0]*pa2[0] + pa1[1]*pa2[1] + pa1[2]*pa2[2];
   a2 = pa2[0]*pa3[0] + pa2[1]*pa3[1] + pa2[2]*pa3[2];
   a3 = pa3[0]*pa1[0] + pa3[1]*pa1[1] + pa3[2]*pa1[2];
   total = rad2deg(acos(a1) + acos(a2) + acos(a3));
   if (ARIabs(total - 360) > EPS)
      return(false);

   return(true);
}