////////////////////////////////////////////////////////////////////////////////////////////////
//                    3d geometry classes - implements some 3d stuff
//
//
////////////////////////////////////////////////////////////////////////////////////////////////

/*==============================
Copyright (c) 2006 g.j.hawkesford 

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
1. Redistributions of source code must retain the above copyright
   notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright
   notice, this list of conditions and the following disclaimer in the
   documentation and/or other materials provided with the distribution.
3. The name of the author may not be used to endorse or promote products
   derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
==============================*/


#include "geometry.h"
using namespace geoff_geometry;

#ifdef PEPSDLL
	#include "vdm.h"
	#include "pepsdll.h"
	#include "realds.h"
#endif
////////////////////////////////////////////////////////////////////////////////////////////////
// matrix
////////////////////////////////////////////////////////////////////////////////////////////////
namespace geoff_geometry {

	Matrix::Matrix(){
		Unit();
	}
	Matrix::Matrix(double m[16]) {
		memcpy(e, m, sizeof(e));
		this->IsUnit();
		this->IsMirrored();
	}

	Matrix::Matrix( const Matrix& m)
	{
		*this = m;
	}

	bool Matrix::operator==(const Matrix &m)const{
		// m1 == m2
		if(this->m_unit != m.m_unit || this->m_mirrored != m.m_mirrored) return false;
		for(int i = 0; i < 16; i++)
			if(FEQ(this->e[i], m.e[i], TIGHT_TOLERANCE) == false) return false;
		return true;
	}

#if 0
	const Matrix& Matrix::operator=( Matrix &m) {
		for(int i = 0; i < 16; i++) e[i] = m.e[i];
		m_unit = m.m_unit;
		m_mirrored = m.m_mirrored;
		return *this;
	}
#endif
	void	Matrix::Unit()
	{
		// homogenous matrix - set as unit matrix
		memset(e, 0, sizeof(e));
		e[0] = e[5] = e[10] = e[15] = 1;
		m_unit = true;
		m_mirrored = false;
	}

	void	Matrix::Get(double* p) const
	{
		// copy the matrix
		memcpy(p, e, sizeof(e));
	}
	void	Matrix::Put(double* p)
	{
		// assign the matrix
		memcpy(e, p, sizeof(e));
		m_unit = false;		// don't know
		m_mirrored = -1;	// don't know

	}
	void	Matrix::Translate(double x, double y, double z)
	{
		// translation
		e[3]  += x;
		e[7]  += y;
		e[11] += z;
		m_unit = false;
	}

	void	Matrix::Rotate(double angle, Vector3d *rotAxis) {
		/// Rotation about rotAxis with angle
		Rotate(sin(angle), cos(angle), rotAxis);
	}

	void	Matrix::Rotate(double sinang, double cosang, Vector3d *rotAxis) {
		/// Rotation about rotAxis with cp & dp
		Matrix rotate;
		double oneminusc = 1.0 - cosang;

		rotate.e[0] = rotAxis->getx() * rotAxis->getx() * oneminusc + cosang;
		rotate.e[1] = rotAxis->getx() * rotAxis->gety() * oneminusc - rotAxis->getz() * sinang;
		rotate.e[2] = rotAxis->getx() * rotAxis->getz() * oneminusc + rotAxis->gety() * sinang;

		rotate.e[4] = rotAxis->getx() * rotAxis->gety() * oneminusc + rotAxis->getz() * sinang;
		rotate.e[5] = rotAxis->gety() * rotAxis->gety() * oneminusc + cosang;
		rotate.e[6] = rotAxis->gety() * rotAxis->getz() * oneminusc - rotAxis->getx() * sinang;

		rotate.e[8] = rotAxis->getx() * rotAxis->getz() * oneminusc - rotAxis->gety() * sinang;
		rotate.e[9] = rotAxis->gety() * rotAxis->getz() * oneminusc + rotAxis->getx() * sinang;
		rotate.e[10] = rotAxis->getz() * rotAxis->getz() * oneminusc  + cosang;
		Multiply(rotate); // concatinate rotation with this matrix
		m_unit = false;
		m_mirrored = -1;	// don't know
	}


	void	Matrix::Rotate(double angle, int Axis)
	{	// Rotation (Axis 1 = x , 2 = y , 3 = z 
		Rotate(sin(angle), cos(angle), Axis);
	}

	void	Matrix::Rotate(double sinang, double cosang, int Axis)
	{	// Rotation (Axis 1 = x , 2 = y , 3 = z 
		Matrix rotate;
		rotate.Unit();

		switch(Axis)
		{
		case 1:
			// about x axis
			rotate.e[5] = rotate.e[10] = cosang;
			rotate.e[6] = -sinang;
			rotate.e[9] = sinang;
			break;
		case 2:
			// about y axis
			rotate.e[0] = rotate.e[10] = cosang;
			rotate.e[2] = sinang;
			rotate.e[8] = -sinang;
			break;
		case 3:
			// about z axis
			rotate.e[0] = rotate.e[5] = cosang;
			rotate.e[1] = -sinang;
			rotate.e[4] = sinang;
			break;
		}
		Multiply(rotate); // concatinate rotation with this matrix
		m_unit = false;
		m_mirrored = -1;	// don't know
	}

	void	Matrix::Scale(double scale)
	{
		// add a scale
		Scale(scale, scale, scale);
	}

	void	Matrix::Scale(double scalex, double scaley, double scalez)
	{
		// add a scale
		Matrix temp;
		temp.Unit();

		temp.e[0]  = scalex;
		temp.e[5]  = scaley;
		temp.e[10] = scalez;
		Multiply(temp);
		m_unit = false;
		m_mirrored = -1;	// don't know
	}
	void	Matrix::Multiply(Matrix& m)
	{
		// multiply this by give matrix - concatinate
		int i, k, l;
		Matrix ret;

		for (i = 0; i < 16; i++)
		{
			l = i - (k = (i % 4));
			ret.e[i] =  m.e[l] * e[k] + m.e[l+1] * e[k+4] + m.e[l+2] * e[k+8] + m.e[l+3] * e[k+12];
		}
		
		*this = ret;
		this->IsUnit();
	}

	void	Matrix::Transform(double p0[3], double p1[3]) const
	{
		// transform p0 thro' this matrix
		if(m_unit)
			memcpy(p1, p0, 3 * sizeof(double));
		else {
			p1[0] = p0[0] * e[0] + p0[1] * e[1] + p0[2] * e[2]  + e[3];
			p1[1] = p0[0] * e[4] + p0[1] * e[5] + p0[2] * e[6]  + e[7];
			p1[2] = p0[0] * e[8] + p0[1] * e[9] + p0[2] * e[10] + e[11];
		}
	}
	void	Matrix::Transform2d(double p0[2], double p1[2]) const
	{
		// transform p0 thro' this matrix (2d only)
		if(m_unit)
			memcpy(p1, p0, 2 * sizeof(double));
		else {
			p1[0] = p0[0] * e[0] + p0[1] * e[1] + e[3];
			p1[1] = p0[0] * e[4] + p0[1] * e[5] + e[7];
		}
	}

	void	Matrix::Transform(double p0[3]) const
	{
		double p1[3];
		if(!m_unit) {
			Transform(p0, p1);
			memcpy(p0, p1, 3 * sizeof(double));
		}
	}

	int	Matrix::IsMirrored()
	{
		// returns true if matrix has a mirror
		if(m_unit)
			m_mirrored = false;
		else if(m_mirrored == -1) {

			m_mirrored =  ((e[0] * (e[5] * e[10] - e[6] * e[9])
				- e[1] * (e[4] * e[10] - e[6] * e[8])
				+ e[2] * (e[4] * e[9]  - e[5] * e[8])) < 0);
		}
		return m_mirrored;
	}
	int Matrix::IsUnit() {
		// returns true if unit matrix
		for(int i = 0; i < 16; i++) {
			if(i == 0 || i == 5 || i == 10 || i == 15) {
				if(e[i] != 1) return m_unit = false;
			}
			else {
				if(e[i] != 0) return m_unit = false;
			}
		}
		m_mirrored = false;
		return m_unit = true;
	}

	void	Matrix::GetTranslate(double& x, double& y, double& z) const
	{
		// return translation
		x = e[3];
		y = e[7];
		z = e[11];
	}
	void	Matrix::GetScale(double& sx, double& sy, double& sz) const
	{
		// return the scale
		if(m_unit) {
			sx = sy = sz = 1;
		}
		else {
			sx = sqrt(e[0] * e[0] + e[1] * e[1] + e[2] * e[2]);
			sy = sqrt(e[4] * e[4] + e[5] * e[5] + e[6] * e[6]);
			sz = sqrt(e[8] * e[8] + e[9] * e[9] + e[10] * e[10]);
		}
	}
	bool	Matrix::GetScale(double& sx) const
	{
		// return a uniform scale  (false if differential)
		double sy, sz;
		if(m_unit) {
			sx = 1;
			return true;
		}
		GetScale(sx, sy, sz);
		return (fabs(fabs(sx) - fabs(sy)) < 0.000001)?true : false;
	}
	void	Matrix::GetRotation(double& ax, double& ay, double& az) const
	{
		// return the rotations
		if(m_unit) {
			ax = ay = az = 0;
			return;
		}
		double    a; /* cos(bx) */
		double    b; /* sin(bx) */
		double    c; /* cos(by) */
		double    d; /* sin(by) */
		double    ee; /* cos(bz) */
		double    f; /* sin(bz) */
		double sx, sy, sz;
		GetScale(sx, sy, sz);
		if(this->m_mirrored == -1) FAILURE(L"Don't know mirror - use IsMirrored method on object");
		if(this->m_mirrored) sx = -sx;

		// solve for d and decide case and solve for a, b, c, e and f
		d = - e[8] / sz;
		if((c = (1 - d) * (1 + d)) > 0.001)
		{
			// case 1
			c = sqrt( c );
			a = e[10] / sz / c;
			b = e[9]  / sz / c;
			ee = e[0]  / sx / c;
			f = e[4]  / sy / c;
		}
		else
		{
			// case 2
			double   coef;
			double   p, q;

			d = ( d < 0 ) ? -1 : 1 ;
			c = 0 ;
			p = d * e[5] / sy - e[2] / sx;
			q = d * e[6] / sy + e[1] / sx;
			if((coef = sqrt( p * p + q * q )) > 0.001) {
				a = q / coef;
				b = p / coef;
				ee = b;
				f = -d * b;
			}
			else
			{
				/* dependent pairs */
				a =  e[5] / sy;
				b = -e[6] / sy;
				ee = 1 ;
				f = 0 ;
			}
		}

		// solve and return ax, ay and az
		ax = atan2( b, a );
		ay = atan2( d, c );
		az = atan2( f, ee );
	}

	Matrix	Matrix::Inverse()
	{
		// matrix inversion routine

		// a is input matrix destroyed & replaced by inverse
		// method used is gauss-jordan (ref ibm applications)

		double  hold , biga ;
		int i , j , k , nk , kk , ij , iz ;
		int ki , ji , jp , jk , kj , jq , jr , ik;

		int n = 4;			// 4 x 4 matrix only
		Matrix a = *this;
		int l[4], m[4];

		if(a.m_unit) return a;	// unit matrix

		// search for largest element
		nk =  - n ;
		for ( k = 0 ; k < n ; k++ ) {
			nk += n ;
			l [ k ]  = m [ k ] = k ;
			kk = nk + k ;
			biga = a.e[ kk ]  ;

			for ( j = k ; j < n ; j++ ) {
				iz = n * j  ;
				for ( i = k ; i < n ; i++ ) {
					ij = iz + i ;
					if ( fabs ( biga )  < fabs ( a.e[ ij ]  )  ) {
						biga = a.e[ ij ]  ;
						l[ k ]  = i ;
						m[ k ] = j ;
					}
				}
			}


			// interchange rows
			j = l[ k ]  ;
			if ( j > k ) {
				ki = k - n ;

				for ( i = 0 ; i < n ; i++ ) {
					ki += n ;
					hold =  - a.e[ ki ]  ;
					ji = ki - k + j ;
					a.e[ ki ]  = a.e[ ji ]  ;
					a.e[ ji ]  = hold ;
				}
			}

			// interchange columns
			i = m[ k ]  ;
			if ( i > k ) {
				jp = n * i ;
				for ( j = 0 ; j < n ; j++ ) {
					jk = nk + j ;
					ji = jp + j ;
					hold =  - a.e[ jk ]  ;
					a.e[ jk ]  = a.e[ ji ]  ;
					a.e[ ji ]  = hold ;
				}
			}

			// divide columns by minus pivot (value of pivot element is contained in biga)
			if ( fabs ( biga )  < 1.0e-10 )FAILURE(getMessage(L"Singular Matrix - Inversion failure",GEOMETRY_ERROR_MESSAGES, -1));	// singular matrix

			for ( i = 0 ; i < n ; i++ ) {
				if ( i != k ) {
					ik = nk + i ;
					a.e[ ik ]  =  - a.e[ ik ] /biga ;
				}
			}

			// reduce matrix
			for ( i = 0  ; i < n ; i++ ) {
				ik = nk + i ;
				hold = a.e[ ik ]  ;
				ij = i - n ;

				for ( j = 0 ; j < n ; j++ ) {
					ij = ij + n ;
					if ( i != k && j != k ) {
						kj = ij - i + k ;
						a.e[ ij ] = hold * a.e[ kj ]  + a.e[ ij ]  ;
					}
				}
			}

			// divide row by pivot
			kj = k - n ;
			for ( j = 0 ; j < n ; j++ ) {
				kj = kj + n ;
				if ( j != k )  a.e[ kj]  = a.e[ kj ] /biga ;
			}

			// replace pivot by reciprocal
			a.e[ kk ] = 1 / biga ;
		}

		// final row and column interchange
		k = n - 1 ;

		while ( k > 0 ) {
			i = l[ --k ]  ;
			if ( i > k ) {
				jq = n * k ;
				jr = n * i ;

				for ( j = 0 ; j < n ; j++ ) {
					jk = jq + j ;
					hold = a.e[jk]  ;
					ji = jr + j ;
					a.e[jk]  =  - a.e[ji]  ;
					a.e[ji]  = hold ;
				}
			}

			j = m[ k ]  ;
			if ( j > k ) {
				ki = k - n ;

				for ( i = 1 ; i <= n ; i ++ ) {
					ki = ki + n ;
					hold = a.e[ ki ]  ;
					ji = ki - k + j ;
					a.e[ ki ] =  - a.e[ ji ]  ;
					a.e[ ji ]  = hold ;
				}
			}
		}

		return a;
	}

#ifdef PEPSDLL
	void Matrix::ToPeps(int id)
	{
		int set = PepsVdmMake(id, VDM_MATRIX_TYPE , VDM_LOCAL);
		if(set < 0) FAILURE(L"Failed to create Matrix VDM");
		struct kgm_header pepsm;

		Get(pepsm.matrix);
		pepsm.off_rad = 0;
		pepsm.off_dir = pepsm.origin_id = 0;

		PepsVdmWriteTmx(set , &pepsm );

		PepsVdmClose(set);

	}

	void Matrix::FromPeps(int id)
	{
		//	if(id) {
		int set = PepsVdmOpen(id, VDM_MATRIX_TYPE , VDM_READ_ONLY | VDM_LOCAL);
		if(set < 0) FAILURE(L"Failed to open Matrix VDM");

		struct kgm_header pepsm;
		PepsVdmReadTmx(set , &pepsm);
		memcpy(e, pepsm.matrix, sizeof(pepsm.matrix));
		m_unit = true;
		for(int i = 0; i < 16; i++) {
			// copy over matrix and check for unit matrix
			if(i == 0 || i == 5 || i == 10 || i == 15) {
				if((e[i] = pepsm.matrix[i]) != 1) m_unit = false;
			}
			else {
				if((e[i] = pepsm.matrix[i]) != 0) m_unit = false;
			}
		}
		PepsVdmClose(set);
		m_mirrored = IsMirrored();
		//	}
	}
#endif

	Matrix UnitMatrix;					// a global unit matrix


	// vector
	 Vector2d::Vector2d(const Vector3d &v){
		if(FEQZ(v.getz())) FAILURE(L"Converting Vector3d to Vector2d illegal");
		dx = v.getx();
		dy = v.gety();
	}

	 bool Vector2d::operator==(const Vector2d &v)const {
		return FEQ(dx, v.getx(), 1.0e-06) && FEQ(dy, v.gety(), 1.0e-06);
	 }

	 void Vector2d::Transform(const Matrix& m) {
		 // transform vector
		 if(m.m_unit == false) {
			double dxt = dx * m.e[0] + dy * m.e[1];
			double dyt = dx * m.e[4] + dy * m.e[5];
			dx = dxt;
			dy = dyt;
		}
		 this->normalise();
	 }

	 void Vector3d::Transform(const Matrix& m) {
		 // transform vector
		 if(m.m_unit == false) {
			double dxt = dx * m.e[0] + dy * m.e[1] + dz * m.e[2];
			double dyt = dx * m.e[4] + dy * m.e[5] + dz * m.e[6];
			double dzt = dx * m.e[8] + dy * m.e[9] + dz * m.e[10];
			dx = dxt;
			dy = dyt;
			dz = dzt;
		}
		 this->normalise();
	 }

	 void Vector3d::arbitrary_axes(Vector3d& x, Vector3d& y){
		// arbitrary axis algorithm - acad method of generating an arbitrary but
		// consistant set of axes from a single normal ( z )
		// arbitrary x & y axes

		if ( ( fabs ( this->getx() ) < 1.0/64.0 ) && (fabs(this->gety()) < 1.0/64.0))
			x = Y_VECTOR ^ *this;
		else
			x = Z_VECTOR ^ *this;

		y = *this ^ x;
	}

	 int Vector3d::setCartesianAxes(Vector3d& b, Vector3d& c) {
#define a *this
	// computes a RH triad of Axes (Cartesian) starting from a (normalised)
	// if a & b are perpendicular then c = a ^ b
	// if a & c are perpendicular then b = c ^ a
	// if neither are perpendicular to a, then return arbitrary axes from a

	// calling sequence for RH cartesian
	//	x y z
	//  y z x
	//  z x y
	if(a == NULL_VECTOR) FAILURE(L"SetAxes given a NULL Vector");
	double epsilon = 1.0e-09;
	bool bNull = (b == NULL_VECTOR);
	bool cNull = (c == NULL_VECTOR);
	bool abPerp = !bNull;
	if(abPerp) abPerp = (fabs(a * b) < epsilon);

	bool acPerp = !cNull;
	if(acPerp) acPerp = (fabs(a * c) < epsilon);

	if(abPerp) {
		c = a ^ b;
		return 1;
	}

	if(acPerp) {
		b = c ^ a;
		return 1;
	}

	arbitrary_axes(b, c);
	b.normalise();
	c.normalise();
	return 2;
}


	 void Plane::Mirrored(Matrix* tmMirrored) {
		 // calculates a mirror transformation that mirrors 2d about plane
	 
		//Point3d p1 = this->Near(Point3d(0.,0.,0.));
		if(tmMirrored->m_unit == false) tmMirrored->Unit();

		double nx = this->normal.getx();
		double ny = this->normal.gety();
		double nz = this->normal.getz();
	   
		// the translation
		tmMirrored->e[ 3] = -2. * nx * this->d;
		tmMirrored->e[ 7] = -2. * ny * this->d;
		tmMirrored->e[11] = -2. * nz * this->d;
	 
		// the rest
		tmMirrored->e[ 0] = 1. - 2. * nx * nx;
		tmMirrored->e[ 5] = 1. - 2. * ny * ny;
		tmMirrored->e[10] = 1. - 2. * nz * nz;
		tmMirrored->e[ 1] = tmMirrored->e[ 4] = -2. * nx * ny;
		tmMirrored->e[ 2] = tmMirrored->e[ 8] = -2. * nz * nx;
		tmMirrored->e[ 6] = tmMirrored->e[ 9] = -2. * ny * nz;
	 
		tmMirrored->m_unit = false;
		tmMirrored->m_mirrored = true;
	}
}
