#include "Polyhedron.h"

/************************************************************************
*									*
* Class destructor							*
*									*
************************************************************************/

Polyhedron::~Polyhedron()
{
int i;

// delete the vertices and the plane equations
delete vList;
delete pList;

// delete the facets
for(i = 0; i < facetN; i++)
    delete iList[i];
delete iList;
}

/************************************************************************
*									*
* This method computes the intersection between a ray and the		*
* polyhedron.  It returns the distance between the origin of the ray	*
* and the closest point of intersection (or 0.0 if not intersection	*
* occurs).								*
* The algorithm operates as follows:					*
* For each facet of the polyhedron					*
*  + determine the intersection between the ray and the plane		*
*    supporting the facet.						*
*  + if the intersection exists, project the facet on a 2D plane.	*
*  + Test if the intersection point is inside the resulting polygon.	*
*									*
************************************************************************/

double Polyhedron::intersect(vec3& ray_org, vec3& ray_dir)
{
int	i, j,
	sNum = 0,   // Number of intersections.
	axis,	    // Axis along which the facet will be projected.
	iNum;	    // Number of intersections between an imaginary half line
		    // and the edges of a facet (used to determine inclusion).
vec3	normal;	    // Normal to the current facet.
vec2	p1, p2,	    // Two consecutive points on a facet.
	p,	    // A 2D projection of the point to test.
	n;	    // Normal to one of the edges of a facet.
double	s[facetN],  // Space to store at most N intersections.
	vd,
	vn;

for (i=0; i<facetN; i++) {
    // Test the case in which the ray is parallel to the facet (vd=0.0)
    vd = vec3(pList[i],PD) * ray_dir;
    if (vd == 0.0)
	continue;

    // Find a projection axis which maximizes the area of the facet
    (normal = vec3(pList[i], PD)).apply(&fabs);
    if (normal[VX] >= normal[VY])
	if(normal[VX] >= normal[VZ]) axis = VX;
	else axis = VZ;
    else if(normal[VY] >= normal[VZ]) axis = VY;
	else axis = VZ;

    // Compute the point of intersection between the ray and the facet plane.
    // Project it along the desired axis.
    vn = pList[i] * vec4(ray_org);
    s[sNum] = -vn / vd;
    p = vec2(ray_org + s[sNum] * ray_dir, axis);

    // Test if the intersection point is inside the facet.
    // We do this by checking for each edge of the facet if there is an
    // intersection between this edge and a half-line, starting at p and
    // going towards positive X. We count the number of intersections: if it
    // is odd, the point is inside the facet, if not, it is outside.

    iNum = 0;
    p1 = vec2(vList[iList[i][iList[i][0]]], axis);
    for (j=1; j<= iList[i][0]; j++) {
	p2 = vec2(vList[iList[i][j]], axis);

	// Is p in the band generated by sweeping the [p1p2] segment in the
	// positive X direction ?
	if ((p1[VY] - p[VY]) * (p2[VY] - p[VY]) <= 0.0) {

	    // Does the half-line trivially intersect the edge ?
	    if (p[VX] < min(p1[VX], p2[VX]))
		iNum++;

	    // Does the half-line trivially miss the edge ?
	    else if (p[VX] < max(p1[VX], p2[VX])) {

		// tough case: compute the normal to the edge pointing towards
		// positive X use the dot product between this normal and the
		// p1p vector to see if the edge and the half line intersect.
		// (This is similar to the Cyrus-Beck line-clipping test)
		n[VX] = p1[VY] - p2[VY];
		n[VY] = p2[VX] - p1[VX];
		if (n[VX] < 0.0)
		    n = -n;
		if (n * (p - p1) <= 0.0)
		    iNum++;
		}
	    }
	p1 = p2;
	}
    if (iNum % 2)
	sNum++;
    }
return closest_intersection(s, sNum);
}

/************************************************************************
*									*
* This method computes the normal vector to the polyhedron at the point *
* of intersection.							*
* It does so by finding out which plane the intersection point belongs	*
* to. Once this plane is determined, it simply returns the normal to	*
* this plane, which is already normalized.				*
*									*
************************************************************************/

vec3 Polyhedron::normalAt(vec3& p)
{
int	i,
	index = 0;	// index of the facet being hit by the ray
double	h,
	hmin = 1e16;	// initialize hmin to some very large number

for (i=0; i<facetN; i++) {
    h = fabs(pList[i] * vec4(p));
    if (h < hmin) {
	index = i;
	hmin = h;
	}
    }
return vec3(pList[index], PD);
}

/************************************************************************
*									*
* Input from stream.							*
*									*
************************************************************************/

istream& operator >> (istream& s, Polyhedron& a)
{
int	i,j,M;
mat4	T;	// local coordinates to world coordinates transformation
vec3	n;	// normal to the facet

// call the implementation of the super-class
s >> *((Primitive*) &a);

// create the matrix to transform local coordinates to world coordinates
T = translation3D(a.pos) * (a.orient.transpose());

// read the vertices. Transform them on the fly to world coordinates
s >> a.vertexN;
a.vList = new vec3[a.vertexN];
for (i = 0; i < a.vertexN; i++) {
    s >> a.vList[i];
    a.vList[i] = T * a.vList[i];
    }

// read the facets.
s >> a.facetN;
a.iList = new int*[a.facetN];
for (i = 0; i< a.facetN; i++) {
    s >> M;
    a.iList[i] = new int[M+1];
    a.iList[i][0] = M;
    for (j=1; j<=M; j++)
	s >> a.iList[i][j];
    }

// compute the normal and plane equation of the facet using Newell method
a.pList = new vec4[a.facetN];
for (i = 0; i < a.facetN; i++) {
    n = vec3(0.0);
    for (j = 1; j <= a.iList[i][0]; j++)
	if (j != a.iList[i][0])
	    n += a.vList[a.iList[i][j]] ^ a.vList[a.iList[i][j+1]];
	else
	    n += a.vList[a.iList[i][j]] ^ a.vList[a.iList[i][1]];
    n.normalize();
    a.pList[i] = vec4(n, - a.vList[a.iList[i][1]] * n);
    }
return s;
}