#ifndef _HPluckerH_
#define _HPluckerH_

#include "vector.h"

class HQRPlucker {
public:
	HQRPlucker() {}
	HQRPlucker(HQRVector* origin,HQRVector* second) { 
		//Origin, first point, second, second point on line->Not direction
		//No nasty cases, can use as constructor
		MakePluckerFromVec(origin,second); 	
	} 

	inline bool RetVectorLine(HQRVector* retval) { //Retval[0] = Direction, Retval[1] = Line Point (Closest to origin)
		if (!GrassmanCheck()) return false; //Not real line, can't return vectors
		retval[0].Copy(m_coords); //First Vector is direction
		retval[1].VecCrossProduct(m_coords[0],m_coords[1]);
		retval[1].Scale(1/(m_coords[1].VecDotProduct(m_coords[1]))); //Divide, because homogenous coord
		return true;
	}

	inline bool MakePluckerFromPlanes(HQRPlane* pone,HQRPlane* ptwo) { 
		//NOTE; Returns false if planes parallel -> Degenerate Case can't use as constructor
		if ((ptwo->Normal()->VecDotProduct(*pone->Normal()))==1) return false;
		
		HQRVector temp;
		//NOTE; Scalar is the D value to the plane
		m_coords[0].VecCrossProduct(*ptwo->Normal(),*pone->Normal());
		m_coords[1].CopyWithScale(ptwo->Normal(),pone->Scalar());
		temp.CopyWithScale(pone->Normal(),ptwo->Scalar());
		m_coords[1].subtract(temp);
		return true;
	}
	
	inline void MakePluckerFromVec(HQRVector* Q,HQRVector* P) {
		//Q vector is first vector, P is second vector in directed line
		m_coords[0].Copy(P);	
		m_coords[0].subtract(*Q);
		m_coords[1].VecCrossProduct(*P,*Q);
	}

	inline float PDotProduct(HQRPlucker* against) { //Performs dot product with "against" and this.
		return (m_coords[0].VecDotProduct(against->Coords(1)) + 
			m_coords[1].VecDotProduct(against->Coords(0)));		
    }

	inline bool PSideTest(HQRPlucker* against) { 
		//Does a dotproduct, returns true if ccw orientation/intersection, false if cw.
		if (PDotProduct(against)<0) return false;
		return true;
	}

	inline HQRVector Direction() {return m_coords[0];}  //Direction
	inline HQRVector* Coords() {return m_coords;} //Pointer to coords
	inline HQRVector Coords(unsigned int n) {return m_coords[n];} //Coords by number

	inline bool GrassmanCheck() { //Returns true if real line.
		if (EpCheck(PDotProduct(this))) return true;
		return false;
	}

	
	inline bool TriangleHit(HQRPlucker** edges) { 
		//Returns True if this line stabs the triangle (3 edges)
		bool result;
		
		result=(PDotProduct(edges[0])<0);
		if (result!=(PDotProduct(edges[1])<0)) return false;
		if (result!=(PDotProduct(edges[2])<0)) return false;

		return true;
	}

	inline bool PolyHit(HQRPlucker** edges,unsigned int numedges) {
		//Returns True is this line stabs the polygon (numedge) of edges
		bool result;

		result=(PDotProduct(edges[0])<0);
		for (unsigned int ii=1; ii<numedges; ii++) {
			if (result!=(PDotProduct(edges[ii])<0)) return false;
		}
		return true;
	}
private:
  HQRVector m_coords[2]; //The coords. 
};


#endif