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


namespace GtpVisibilityPreprocessor {


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);
  
  double det = md.Det3x3();

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

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


Vector3 Plane3::FindIntersection(const Vector3 &a,						   
								 const Vector3 &b,
								 float *t,
								 bool *coplanar) const 
{
    const Vector3 v = b - a; // line from A to B
    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; // TODO: could be done more efficiently
    
	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; // TODO: could be done more efficiently
}

}