/*****************************************************************************
Name            :   get3pts.c 
C Author        :   Stel Michael

Copyright       :   1992 by VideoLogic Limited. All rights reserved.
		    No part of this software, either material or conceptual
		    may be copied or distributed, transmitted, transcribed,
		    stored in a retrieval system or translated into any
		    human or computer language in any form by any means,
		    electronic, mechanical, manual or other-wise, or
		    disclosed to third parties without the express written
		    permission of VideoLogic Limited, HomePark Estate,
		    King's Langley, Hertfordshire, WD4 8LZ, U.K.

Description     :   Plane intersection functions
Program Type    :   Microsoft Visual C++ V2 
*****************************************************************************/

#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include "sgl.h"

#define TOOBIG (1e004)
#define TOOSMALL (-1e004)
#define ACCURACY 0.001

float *diffvec(sgl_vector ans, sgl_vector x, sgl_vector y)
{
	ans[0]=x[0]-y[0];
	ans[1]=x[1]-y[1];
	ans[2]=x[2]-y[2];
	return ans;
}

float *crossprod(sgl_vector ans, sgl_vector a, sgl_vector b)
{
	ans[0]=a[1]*b[2]-a[2]*b[1];
	ans[1]=a[2]*b[0]-a[0]*b[2];
	ans[2]=a[0]*b[1]-a[1]*b[0];

	return ans;
}

void normalise(sgl_vector n)
{
	float len;
	
	len=(float) sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);
	n[0]/=len;
	n[1]/=len;
	n[2]/=len;
}

float vecsqrlen(sgl_vector a)
{
	return a[0]*a[0]+a[1]*a[1]+a[2]*a[2];
}

void veccpy(sgl_vector x, sgl_vector y)
{
 	x[0]=y[0];
	x[1]=y[1];
	x[2]=y[2];
}


typedef struct
{
	/* Surface Normal - this MUST be a unit normal */
	float *normal;

	/* Representative point */
	float *rep_point;
} Plane;


/*****************************************************************
* Function Name		: PlaneIntersections  
* Inputs			: p0_normal - plane 0 normal
*					  p1_normal - plane 1 normal
*					  p2_normal - plane 2 normal
*					  fD0 - plane 0 d value
*				      fD1 - plane 1 d value
*					  fD2 - plane 2 d value
* Outputs			: intersection - point if intersection 
* Returns			: TRUE					  
* Description		: Finds intersection of three planes
******************************************************************/

int PlaneIntersections(sgl_vector p0_normal, sgl_vector p1_normal, sgl_vector p2_normal,
		float fD0, float fD1, float fD2, sgl_vector intersection)
{
	sgl_vector matrix[3];
	sgl_vector inverse[3];

	int i,j,k;
	int largest_row;

	float largest_val, val, inv_val;

	/* in the event of a singular matrix, set the intersection */
	intersection[0] = 0.0f;
	intersection[1] = 0.0f;
	intersection[2] = 0.0f;

	/* initialise the matrix and inverse (unwinding the loops) */
	matrix[0][0] = p0_normal[0];
	matrix[0][1] = p0_normal[1];
	matrix[0][2] = p0_normal[2];

	matrix[1][0] = p1_normal[0];
	matrix[1][1] = p1_normal[1];
	matrix[1][2] = p1_normal[2];

	matrix[2][0] = p2_normal[0];
	matrix[2][1] = p2_normal[1];
	matrix[2][2] = p2_normal[2];

	/* Put the identity matrix in the inverse */
	for(i = 0; i < 3 ; i ++)
		for(j = 0; j < 3 ; j ++)
			inverse[i][j] = 0.0f;

	for(i = 0; i < 3 ; i ++)
		inverse[i][i] = 1.0f;

	/* Step through the matrix during row reduction, and
	   apply the same to the inverse: Leap out of the routine
	   in the event of failure */

	for(i=0; i < 3; i ++)
	{
	  	/* starting from row 'i', find the largest entry in column 'i' */
		largest_row = i;
		largest_val = (float) fabs(matrix[i][i]);

		for(j = i + 1; j < 3; j++)
			if(largest_val < ((float) fabs(matrix[j][i])) )
			{
			 	largest_val = (float) fabs(matrix[j][i]);
				largest_row = j;
			}

		/* if the lagest found is 0, then the matrix is singular! */

		if(largest_val < 1.0E-20f)
			return FALSE;

		/* else if the current row is already the largest, then
		   leave it as it is...	*/

		else if(largest_row == i)
		{
			/*do nothing*/
		}
		
		/* Else make sure our current row has a good sized value
		   in entry [i][i]. If they are of opposite signs, then subtract
		   one from the other, else add them. Also do the same to the
		   inverse matrix */

		else if( ((matrix[i][i] < 0.0f) && (matrix[largest_row][i] > 0.0f)) ||
			 ((matrix[i][i] > 0.0f) && (matrix[largest_row][i] < 0.0f)) )
		{
			for(j = 0; j < 3; j ++)
			{
				matrix[i][j]  = matrix[i][j]  - matrix[largest_row][j];
				inverse[i][j] = inverse[i][j] - inverse[largest_row][j];
			}
		}
		else
		{
			for(j = 0; j < 3; j ++)
			{
				matrix[i][j]  = matrix[i][j]  + matrix[largest_row][j];
				inverse[i][j] = inverse[i][j] + inverse[largest_row][j];
			}

		}

		/* Set entry [i][i] to be 1 by dividing the row, because of
		   all the messing about above, we know that this division
		   won't produce stupid results */

		inv_val = 1.0f / matrix[i][i];
		for(j = 0; j < 3; j++)
		{
			matrix[i][j]   = matrix[i][j] * inv_val;
			inverse[i][j]  = inverse[i][j] * inv_val;
		}

		/* eliminate the column from the other rows	*/

		for(k=0; k < 3; k++)
			if(k != i)
			{
				val = matrix[k][i];
				for(j = 0; j < 3; j ++)
				{
					matrix[k][j]   = matrix[k][j]  - val * matrix[i][j];
					inverse[k][j]  = inverse[k][j] - val * inverse[i][j];
				}
			}
	}

	/* Multiply the inverse by the d values to get the intersection point */

	for(j = 0; j < 3; j ++)
	{
		intersection[j] =
		  inverse[j][0] * fD0 + inverse[j][1] * fD1 + inverse[j][2] * fD2;
	} 

	return TRUE;
}


/*****************************************************************
* Function Name		: Parallel  
* Inputs			: n - normal of first plane
*					  m - normal of second plane
* Outputs			: none
* Returns			: result of test					  
* Description		: Test if two planes are parallel
* Notes				: two planes are parallel if the normals either 
*					  have the same or exactly opposite directions
*					  so use absolute value test
******************************************************************/


int Parallel(sgl_vector n, sgl_vector m)
{
	return 	fabs(fabs(n[0])-fabs(m[0]))<ACCURACY && 
			fabs(fabs(n[1])-fabs(m[1]))<ACCURACY && fabs(fabs(n[2])-fabs(m[2]))<ACCURACY;
}

/*****************************************************************
* Function Name		: get3pts  
* Inputs			: noplanes - number of planes
*					  reppt - array of representative points
*					  normal - array of normals (same index as reppt)
*					  inter_check - checks for intersection if true
* Outputs			: pt - points of intersection in clockwise order
* Returns			: void					  
* Description		: For each triple of planes find points of 
*					  intersection in clockwise order
******************************************************************/

void get3pts(int noplanes, sgl_vector* reppt, sgl_vector* normal, sgl_vector *pt, sgl_bool inter_check)
{
	int i,j,k;

	int count;

	sgl_vector intersect;
	sgl_vector nearest, nearest2nd;
	sgl_vector dummy[2];
	sgl_vector pn;

	float dist, nearest_dist, nearest2nd_dist;

	float d0, d1, d2;

	/* normalise all normals */
	for(i=0;i<noplanes;i++)
	{
		normalise(normal[i]);		
	}

	/*
	Note incredible (O(n^3)) lack of efficiency here!
	general alg: 
	for each plane
		1. get points of intersection for each triple of planes, and
			determine 2 points nearest/furthest the right - to the right?
		2. make sure clockwise order is determined, and that's them
	*/

	for(i=0;i<noplanes;i++)
	{
		count=1;
		d0=reppt[i][0]*normal[i][0]+reppt[i][1]*normal[i][1]+reppt[i][2]*normal[i][2];

		veccpy(pt[i*3], reppt[i]);

		for(j=0; j<noplanes;j++)
		{
			if(i==j)
				continue;

			if( Parallel(normal[i], normal[j]) )
			{
				continue;
			}

			d1=reppt[j][0]*normal[j][0]+reppt[j][1]*normal[j][1]+reppt[j][2]*normal[j][2];

			for(k=0; k<noplanes; k++)
			{
				if(k==i || k==j)
					continue;				
									
				d2=reppt[k][0]*normal[k][0]+reppt[k][1]*normal[k][1]+reppt[k][2]*normal[k][2];

				/* if kth and jth planes are parallel there is no intersection */
				if( !Parallel(normal[j], normal[k]) )
				{					
			 		if(!PlaneIntersections( normal[i], normal[j], normal[k], d0, d1, d2, intersect))
					{
						/* singular matrix - can't find solution set */
						continue;
					}

					/* if intersect isn't on the ith plane, and flag is set */
					if(inter_check && fabs(intersect[0]*normal[i][0]+intersect[1]*normal[i][1]+
						intersect[2]*normal[i][2]-d0)>ACCURACY)
					{
						continue;
					} 

					/* determine the 2 nearest to the right of the reppt.
					   note that closest to the right also includes points to 
					   the left - which obtain negative closeness. Currently 
					   treating all closeness as the same */		
					
					if( count==1 )
					{						
						nearest_dist=vecsqrlen(diffvec(dummy[0], intersect, reppt[i]));
						if(nearest_dist!=0.0f || nearest_dist>TOOBIG) 
						{
							count=2;
							veccpy(nearest, intersect); // nearest= this vec 
						}
					}
					else if(count==2)
					{
						nearest2nd_dist=vecsqrlen(diffvec(dummy[0], intersect, reppt[i])); 
						if(nearest2nd_dist!=0.0f || nearest2nd_dist>TOOBIG)
						{
							if(nearest2nd_dist>nearest_dist)
							{
								veccpy(nearest2nd, nearest);
								veccpy(nearest, intersect);
							}
							else
							{
								veccpy(nearest2nd, intersect); 
							}
							count=FALSE;
						}						
					}
					else
					{
					 	dist=vecsqrlen(diffvec(dummy[0], intersect, reppt[i])); 
						if(dist==0.0f || nearest_dist>TOOBIG)
						{
							/* do nothing */
						}
						else if( dist>nearest_dist)
						{
							veccpy(nearest, intersect);
						}
						else if( dist>nearest2nd_dist)
						{
							veccpy(nearest2nd, intersect); 
						}
					}
				}
			}
		}

		/* obtain the normal that would be obtained */
		crossprod(pn, diffvec(dummy[1], reppt[i], nearest), diffvec(dummy[0], reppt[i], nearest2nd));

		normalise(pn);

		if( 	fabs(pn[0]-normal[i][0])<ACCURACY && fabs(pn[1]-normal[i][1])<ACCURACY
			&&	fabs(pn[2]-normal[i][2])<ACCURACY) // if pn=normal 
		{	
			/* order is nearest, nearest2nd */
			veccpy(pt[i*3+1], nearest);
			veccpy(pt[i*3+2], nearest2nd);
		}
		else
		{
			/* order is nearest2nd, nearest */
			veccpy(pt[i*3+1], nearest2nd);
			veccpy(pt[i*3+2], nearest);
		}
	}
}
