#include "Plane3.h"
#include "Matrix4x4.h"


namespace CHCDemoEngine 
{

Plane3::Plane3(const Vector3 &a, const Vector3 &b, const Vector3 &c) 
{
    Vector3 v1 = a - b, v2 = c - b;

	mNormal = Normalize(CrossProd(v2, v1));
    mD = -DotProd(b, mNormal);
}


Plane3::Plane3(const Vector3 &normal, const Vector3 &point):
mNormal(normal)
{
	//mNormal = Normalize(normal)
    mD = -DotProd(normal, point);
}


bool PlaneIntersection(const Plane3 &a, 
					   const Plane3 &b, 
					   const Plane3 &c, 
					   Vector3 &result)
{
	Vector3 sx(a.mNormal.x, b.mNormal.x, c.mNormal.x),
		    sy(a.mNormal.y, b.mNormal.y, c.mNormal.y),
		    sz(a.mNormal.z, b.mNormal.z, c.mNormal.z),
		    sd(a.mD,        b.mD,        c.mD);

	Matrix4x4 md(a.mNormal, b.mNormal, c.mNormal), mx, my, mz;

	mx.SetColumns(sd, sy, sz);
	my.SetColumns(sx, sd, sz);
	mz.SetColumns(sx, sy, sd);

	const float det = md.Det3x3();

	if (fabs(det) < TRASH)
		return false;

	result.SetValue(mx.Det3x3() / det, my.Det3x3() / det, mz.Det3x3() / det);

	return true;
}


bool PlaneIntersection(const Plane3 &p1, const Plane3 &p2)
{
	return 
		p1.mNormal.x != p2.mNormal.x ||
		p1.mNormal.y != p2.mNormal.y ||
		p1.mNormal.z != p2.mNormal.z ||
		p1.mD == p2.mD;
}


Vector3 Plane3::FindIntersection(const Vector3 &a,						   
								 const Vector3 &b,
								 float *t,
								 bool *coplanar) const 
{
	// line from A to B
	const Vector3 v = b - a; 
	float dv = DotProd(v, mNormal);
    
    if (signum(dv) == 0)
	{
		if (coplanar) (*coplanar) = true;	

		if (t) (*t) = 0;

		return a;
	}
	
	if (coplanar) (*coplanar) = false;

    float u = - Distance(a) / dv;

	if (t) (*t) = u;

    return a + u * v;
}


int Plane3::Side(const Vector3 &v, const float threshold) const 
{
    return signum(Distance(v), threshold);
}


float Plane3::FindT(const Vector3 &a, const Vector3 &b) const 
{	  
	const Vector3 v = b - a; // line from A to B
	const float dv = DotProd(v, mNormal);
    
	// does not intersect or coincident
	if (signum(dv) == 0)
		return 0;
	
	return - Distance(a) / dv; 
}


void Plane3::Transform(const Matrix4x4 &m)
{
	float w;

	const Matrix4x4 invTranspose = Transpose(Invert(m));

	const Vector3 n = invTranspose.Transform(w, mNormal, -mD);

	const float mag = Magnitude(n);

	mNormal = n / mag;
	// our d is -d of homogen equation
	mD = -w / mag;

	/*
	// find point on plane
	const Vector3 pt = mNormal * -mD;

	// transform point
	const Vector3 newPt = m * pt;

	// transform normal
	nNormal = Normalize(TransformNormal(m, newPt));

	mD = -DotProd(mNormal, newPt);
	*/
}


}