//
//	Copyright © 2005 Nemetschek North America 
//	All Rights Reserved.
//
// Created by Vladislav Stanev

#include "StdAfx.h"
//#include "VWPolygon3D.h"

using namespace VWFC::Math;

VWPolygon3D::VWPolygon3D()
{
	fbIsClosed	= false;
}

VWPolygon3D::VWPolygon3D(const VWPolygon3D& src)
{
	size_t	len	= src.farrVertices.size();
	for(size_t i=0; i<len; i++)
		farrVertices.push_back( src.farrVertices[ i ] );

	fbIsClosed	= src.fbIsClosed;
}

VWPolygon3D::VWPolygon3D(const VWPolygon2D& src, double z /*=0*/)
{
	size_t	len	= src.GetVertexCount();
	for(size_t i=0; i<len; i++)
		farrVertices.push_back( VWPoint3D( src.GetVertexAt(i), z ) );

	fbIsClosed	= src.IsClosed();
}

VWPolygon3D::VWPolygon3D(const TSTLPoints3DArray& src)
{
	size_t	len	= src.size();
	for(size_t i=0; i<len; i++)
		farrVertices.push_back( src[ i ] );

	fbIsClosed	= false;
}

 VWPolygon3D::VWPolygon3D(const TSTLPoints3DVWArray& src)
{
	size_t	len	= src.size();
	for(size_t i=0; i<len; i++)
		farrVertices.push_back( src[ i ] );

	fbIsClosed	= false;
}

// create 3d polygon by three points (a plane)
VWPolygon3D::VWPolygon3D(const VWPoint3D& a, const VWPoint3D& b, const VWPoint3D& c)
{
	farrVertices.push_back( a );
	farrVertices.push_back( b );
	farrVertices.push_back( c );
	fbIsClosed		= true;
}

VWPolygon3D::VWPolygon3D(const VWTransformMatrix& plane)
{
	VWPoint3D	offset	= plane.GetOffset();
	farrVertices.push_back( offset );
	farrVertices.push_back( offset + plane.GetUVector() );
	farrVertices.push_back( offset + plane.GetVVector() );
	fbIsClosed		= true;
}

VWPolygon3D::~VWPolygon3D()
{
	farrVertices.clear();
}

VWPolygon3D& VWPolygon3D::operator=(const VWPolygon3D& src)
{
	farrVertices.clear();
	size_t	len	= src.farrVertices.size();
	for(size_t i=0; i<len; i++)
		farrVertices.push_back( src.farrVertices[ i ] );

	fbIsClosed	= src.fbIsClosed;
	return *this;
}

VWPolygon3D& VWPolygon3D::operator=(const VWPolygon2D& src)
{
	farrVertices.clear();
	size_t	len	= src.GetVertexCount();
	for(size_t i=0; i<len; i++)
		farrVertices.push_back( VWPoint3D( src.GetVertexAt(i), 0 ) );

	fbIsClosed	= src.IsClosed();
	return *this;
}

VWPolygon3D& VWPolygon3D::operator=(const TSTLPoints3DArray& src)
{
	farrVertices.clear();
	size_t	len	= src.size();
	for(size_t i=0; i<len; i++)
		farrVertices.push_back( src[ i ] );
	return *this;
}

VWPolygon3D& VWPolygon3D::operator=(const TSTLPoints3DVWArray& src)
{
	farrVertices.clear();
	size_t	len	= src.size();
	for(size_t i=0; i<len; i++)
		farrVertices.push_back( src[ i ] );
	return *this;
}

void VWPolygon3D::SetPolygon(const TSTLPoints3DArray& src)
{
	(*this) = src;
}

void VWPolygon3D::SetPolygon(const TSTLPoints3DVWArray& src)
{
	(*this) = src;
}

void VWPolygon3D::GetPolygon(TSTLPoints3DArray& out) const
{
	out.clear();
	size_t	len	= farrVertices.size();
	for(size_t i=0; i<len; i++)
		out.push_back( farrVertices[i] );
}

void VWPolygon3D::GetPolygon(TSTLPoints3DVWArray& out) const
{
	out.clear();
	out.clear();
	size_t	len	= farrVertices.size();
	for(size_t i=0; i<len; i++)
		out.push_back( farrVertices[i] );
}

// create 3d polygon by three points (a plane)
void VWPolygon3D::SetPolygon(const VWPoint3D& a, const VWPoint3D& b, const VWPoint3D& c)
{
	farrVertices.clear();
	farrVertices.push_back( a );
	farrVertices.push_back( b );
	farrVertices.push_back( c );
	fbIsClosed		= true;
}

void VWPolygon3D::SetPolygon(const VWTransformMatrix& plane)
{
	VWPoint3D	offset	= plane.GetOffset();
	farrVertices.clear();
	farrVertices.push_back( offset );
	farrVertices.push_back( offset + plane.GetUVector() );
	farrVertices.push_back( offset + plane.GetVVector() );
	fbIsClosed		= true;
}

void VWPolygon3D::ClearVertices()
{
	farrVertices.clear();
}

void VWPolygon3D::AddVertex(double x, double y, double z)
{
	farrVertices.push_back( VWPoint3D( x, y, z ) );
}

void VWPolygon3D::AddVertex(const VWPoint3D& pt)
{
	farrVertices.push_back( pt );
}

void VWPolygon3D::InsertVertex(size_t at, double x, double y, double z)
{
	farrVertices.insert( farrVertices.begin() + at, VWPoint3D( x, y, z ) );
}

void VWPolygon3D::InsertVertex(size_t at, const VWPoint3D& pt)
{
	farrVertices.insert( farrVertices.begin() + at, pt );
}

void VWPolygon3D::ClearVertex(size_t at)
{
	farrVertices.erase( farrVertices.begin() + at );
}

size_t VWPolygon3D::GetVertexCount() const
{
	return farrVertices.size();
}

bool VWPolygon3D::IsEmpty() const
{
	return farrVertices.size() <= 1;
}

const VWPoint3D& VWPolygon3D::GetVertexAt(size_t at, bool bLoopVerts /*=false*/) const
{
	if ( bLoopVerts ) {
		// loop the vertex indeces inside the array
		VWFC_ASSERT( farrVertices.size() > 0 );
		at		= at % farrVertices.size();
	}
	else {
		VWFC_ASSERT( at < farrVertices.size() );
	}
	return farrVertices[ at ];
}

VWPoint3D& VWPolygon3D::GetVertexAt(size_t at, bool bLoopVerts /*=false*/)
{
	if ( bLoopVerts ) {
		// loop the vertex indeces inside the array
		VWFC_ASSERT( farrVertices.size() > 0 );
		at		= at % farrVertices.size();
	}
	else {
		VWFC_ASSERT( at < farrVertices.size() );
	}
	return farrVertices[ at ];
}

VWLine3D VWPolygon3D::GetLineAt(size_t at) const
{
	VWFC_ASSERT( at < farrVertices.size() );
	size_t	next	= at + 1;
	if ( next >= farrVertices.size() )
		next	= 0;

	return VWLine3D( farrVertices[ at ], farrVertices[ next ] );
}

bool VWPolygon3D::IsClosed() const
{
	return fbIsClosed;
}

void VWPolygon3D::SetClosed(bool bClosed)
{
	fbIsClosed	= bClosed;
}

// return true if the passed polygon has more than 3 different vertices
bool VWPolygon3D::IsValidPolygon(double dEpsilon) const
{
	size_t	len	= farrVertices.size();
	if ( len < 3 )
		return false;

	size_t		count	= 1;
	VWPoint3D	pt		= farrVertices[ 0 ];
	for(size_t i=1; i<len; i++)
	{
		double_gs	dist	= ( pt - farrVertices[ i ] ).Magnitude();
		if ( dist > dEpsilon )
		{
			count++;
			if ( count >= 3 )
				return true;
		}

		pt			= farrVertices[ i ];
	}

	return count >= 3;
}

// removed all same points in the polygon
void VWPolygon3D::CleanUpPolygon(double dEpsilon)
{
	VWPoint3D					lastPt;
	TSTLPoints3DArray::iterator	it		= farrVertices.begin();
	while( it != farrVertices.end() )
	{
		if ( it == farrVertices.begin() ) 
		{
			lastPt		= *it;
			it++;
		}
		else
		{
			double_gs	dist	= ( lastPt - (*it) ).Magnitude();
			if ( dist < dEpsilon )
			{
				// remove the unnecessary point
				farrVertices.erase( it );
				// start all over
				it		= farrVertices.begin();
			}
			else {
				lastPt		= *it;
				it++;
			}
		}
	}
}

void VWPolygon3D::GetPlanarPolygon(VWPolygon2D& out) const
{
	out.ClearVertices();

	TSTLPoints3DArray::const_iterator	it		= farrVertices.begin();
	while( it != farrVertices.end() ) {
		out.AddVertex( VWPoint2D( it->x, it->y ) );
		++it;
	}

	out.SetClosed( fbIsClosed );
}

void VWPolygon3D::GetXBounds(double& outMinX, double& outMaxX) const
{
	bool								init	= true;
	TSTLPoints3DArray::const_iterator	it		= farrVertices.begin();
	while( it != farrVertices.end() ) {
		if ( init ) {
			init		= false;
			outMinX		= it->x;
			outMaxX		= it->x;
		}
		else {
			if ( it->x < outMinX )	outMinX		= it->x;
			if ( it->x > outMaxX )	outMaxX		= it->x;
		}
		++it;
	}
}

void VWPolygon3D::GetYBounds(double& outMinY, double& outMaxY) const
{
	bool								init	= true;
	TSTLPoints3DArray::const_iterator	it		= farrVertices.begin();
	while( it != farrVertices.end() ) {
		if ( init ) {
			init		= false;
			outMinY		= it->y;
			outMaxY		= it->y;
		}
		else {
			if ( it->y < outMinY )	outMinY		= it->y;
			if ( it->y > outMaxY )	outMaxY		= it->y;
		}
		++it;
	}
}

void VWPolygon3D::GetZBounds(double& outMinZ, double& outMaxZ) const
{
	bool								init	= true;
	TSTLPoints3DArray::const_iterator	it		= farrVertices.begin();
	while( it != farrVertices.end() ) {
		if ( init ) {
			init		= false;
			outMinZ		= it->z;
			outMaxZ		= it->z;
		}
		else {
			if ( it->z < outMinZ )	outMinZ		= it->z;
			if ( it->z > outMaxZ )	outMaxZ		= it->z;
		}
		++it;
	}
}


//////////////////////////////////////////////////////////////////////
// mathematics
//////////////////////////////////////////////////////////////////////
// return the plane equation
// A*x + B*y + C*z + D
bool VWPolygon3D::GetPlaneEquation(double& A, double& B, double& C, double& D) const
{
	size_t		len	= farrVertices.size();
	if ( len < 3 )
		return false;

	VWPoint3D	p0		= farrVertices[ 0 ];
	VWPoint3D	p1		= farrVertices[ 1 ];
	VWPoint3D	p2		= farrVertices[ 2 ];

	VWPoint3D	cross( (p1-p0) * (p2-p0) );
	cross.Normalize();
	A		= cross.x;
	B		= cross.y;
	C		= cross.z;
	D		= - (A * p0.x + B * p0.y + C * p0.z);

	return true;
}

// return transformation which describes this plane
VWTransformMatrix VWPolygon3D::GetTransformMatrix() const
{
	VWTransformMatrix	matrix;
	size_t		len	= farrVertices.size();
	if ( len < 3 )
		return matrix;

	size_t		cnt		= farrVertices.size();
	VWPoint3D	p0		= farrVertices[ 0 ];
	size_t		i		= 1;
	if ( i >= cnt ) 	return matrix;

	while( i < cnt && p0 == farrVertices[ i ] ) {
		i++;
	}
	if ( i >= cnt ) 	return matrix;

	VWPoint3D	p1		= farrVertices[ i ];
	i++;
	while( i < cnt && ( p1 == farrVertices[ i ] || ((p1 - p0) * (farrVertices[ i ] - p0)) == VWPoint3D(0,0,0) ) ) {
		i++;
	}
	if ( i >= cnt ) 	return matrix;

	VWPoint3D	p2		= farrVertices[ i ];

	VWPoint3D	uVec	= p1 - p0;
	VWPoint3D	vVec	= p2 - p0;
	VWPoint3D	wVec	= uVec * vVec;
	vVec				= wVec * uVec;

	uVec.Normalize();
	vVec.Normalize();
	wVec.Normalize();
	matrix.SetMatrix( uVec, vVec, wVec, p0 );

	return matrix;
}

// perpendicular distance to the plane made by the first 3 points
// the distance is reversed if the polygon is CCW
double VWPolygon3D::GetDistance(const VWPoint3D& pt) const
{
	double	A, B, C, D;
	if ( !this->GetPlaneEquation( A, B, C, D ) )
		return 0.0;

	return - (A * pt.x + B * pt.y + C * pt.z + D) / sqrt(A*A + B*B + C*C);
}

// distance to the plane made by the first 3 points
// the vertical distance -- what z for the point to be on the plane
double VWPolygon3D::GetVerticalDistance(const VWPoint3D& pt) const
{
	double reply	= 0.0;

	double	A, B, C, D;
	if ( !this->GetPlaneEquation( A, B, C, D ) )
		return 0.0;

	if ( VWFC_VERIFY( !MathUtils::Equalish( C, 0, VWPoint3D::sEpsilon ) ) ) {	
		reply	= - (A * pt.x + B * pt.y + C * pt.z + D) / C;
	}

	return reply;
}

// normal of the plane made by the first 3 points
VWPoint3D VWPolygon3D::Normal() const
{
	VWPoint3D	normal;

	double	A, B, C, D;
	if ( this->GetPlaneEquation( A, B, C, D ) )
	{
		normal.SetPoint( A, B, C );
	}

	return normal;
}

// get geometry center of the polygon
VWPoint3D VWPolygon3D::GetCenter() const
{
	VWPoint3D	centerPt;

	size_t		polyLen		= farrVertices.size();
	for(size_t i=0; i<polyLen; i++) {
		const VWPoint3D&		pt		= farrVertices[ i ];
		if ( i == 0 )
			centerPt		= pt;
		else
			centerPt		+= pt;
	}

	centerPt	/= polyLen;

	return centerPt;
}

// area3D_Polygon(): computes the area of a 3D planar polygon
//    Return: the (float) area of the polygon
//			positive if polygon is CCW -- negative if CW
double  VWPolygon3D::GetArea() const
{
	size_t		n		= farrVertices.size();
	if ( n < 2 )
		return 0;

#define GET_PTx(i)	((i<n) ? farrVertices[i].x : ( (i==n) ? farrVertices[0].x : ( /*i==n+1*/ farrVertices[1].x ) ))
#define GET_PTy(i)	((i<n) ? farrVertices[i].y : ( (i==n) ? farrVertices[0].y : ( /*i==n+1*/ farrVertices[1].y ) ))
#define GET_PTz(i)	((i<n) ? farrVertices[i].z : ( (i==n) ? farrVertices[0].z : ( /*i==n+1*/ farrVertices[1].z ) ))

	// unit normal vector of the polygon's plane
	VWPoint3D	N		= this->Normal();

	double		area	= 0;
	double		an, ax, ay, az;  // abs value of normal and its coords
	int			coord;           // coord to ignore: 1=x, 2=y, 3=z
	size_t		i, j, k;         // loop indices

	// select largest abs coordinate to ignore for projection
	ax		= (N.x>0 ? N.x : -N.x);		// abs x-coord
	ay		= (N.y>0 ? N.y : -N.y);		// abs y-coord
	az		= (N.z>0 ? N.z : -N.z);		// abs z-coord

	coord	= 3;					// ignore z-coord
	if (ax > ay) {
		if (ax > az) coord = 1;		// ignore x-coord
	}
	else if (ay > az) coord = 2;	// ignore y-coord

	// compute area of the 2D projection
	for (i=1, j=2, k=0; i<=n; i++, j++, k++) {
		switch (coord) {
		case 1:
			area += ( GET_PTy(i) * ( GET_PTz(j) - GET_PTz(k) ) );
			continue;
		case 2:
			area += ( GET_PTx(i) * ( GET_PTz(j) - GET_PTz(k) ) );
			continue;
		case 3:
			area += ( GET_PTx(i) * ( GET_PTy(j) - GET_PTy(k) ) );
			continue;
		}
	}

	// scale to get area before projection
	an = sqrt( ax*ax + ay*ay + az*az);  // length of normal vector
	switch (coord) {
	case 1:
		area *= ( an / ( 2 * ax ) );
		break;
	case 2:
		area *= ( an / ( 2 * ay ) );
		break;
	case 3:
		area *= ( an / ( 2 * az ) );
	}

#undef GET_PTx
#undef GET_PTy
#undef GET_PTz

	return area;
}

// reverse the order of the points (if CW to CCW or CCW to CW)
void VWPolygon3D::ReversePoints()
{
 	size_t	len	= farrVertices.size();
	for(size_t i=0; i<len / 2; i++)
	{
		VWPoint3D		temp		= farrVertices[ i ];
		farrVertices[ i ]			= farrVertices[ len-i-1 ];
		farrVertices[ len-i-1 ]		= temp;
	}
}

// determine if the polygon is Clock Wise oriented
bool VWPolygon3D::IsClockWise() const
{
	double	area	= this->GetArea();
	return area <= 0;
}

// intersect the plane made by the first 3 points with a specified line
//  return false if line is parallel to plane
bool VWPolygon3D::Intersect(const VWLine3D& line, VWPoint3D& outIntersPt, double dEpsilon) const
{
	VWPoint3D	b	= line.GetStart();
	VWPoint3D	m	= (line.GetEnd() - b).Normalize();

	return this->Intersect( b, m, outIntersPt, dEpsilon );
}

// intersect the plane made by the first 3 points with a specified line given by points and direction
//  return false if line is parallel to plane
bool VWPolygon3D::Intersect(const VWPoint3D& startPt, const VWPoint3D& dir, VWPoint3D& outIntersPt, double dEpsilon) const
{
	double	A, B, C, D;
	if ( !this->GetPlaneEquation( A, B, C, D ) )
		return false;

	double		f0	= A * startPt.x + B * startPt.y + C * startPt.z + D;
	double		f1	= A * -dir.x + B * -dir.y + C * -dir.z;
	if ( f1 < -dEpsilon || f1 > dEpsilon )
	{
		double	t	= f0 / f1;

		outIntersPt	= startPt + dir * t;
		return true;
	}
	else
	{
		return false;
	}
}

// intersect this plane made by the first 3 points
// with the specified plane (first 3 points)
//  return false is planes are parallel
bool VWPolygon3D::Intersect(const VWPolygon3D& plane, VWLine3D& outIntersLine, double dEpsilon) const
{
	VWPoint3D	n0	= - this->Normal();
	VWPoint3D	n1	= - plane.Normal();

	double		n00	= n0 % n0;
	double		n01	= n0 % n1;
	double		n11	= n1 % n1;
	double		det	= n00*n11 - n01*n01;

	if ( Abs(det) < dEpsilon )
		return false;
	else
	{
		double	A1, B1, C1, D1;
		if ( !this->GetPlaneEquation( A1, B1, C1, D1 ) )
			return false;
		double	A2, B2, C2, D2;
		if ( !plane.GetPlaneEquation( A2, B2, C2, D2 ) )
			return false;

		double		inv_det	= 1.0 / det;
		double		c0		= (n11 * D1 - n01 * D2) * inv_det;
		double		c1		= (n00 * D2 - n01 * D1) * inv_det;

		VWPoint3D	m		= n0 * n1;
		VWPoint3D	b		= n0 * c0 + n1 * c1;

		outIntersLine.SetLine( b, b + m );

		return true;
	}
}

// determines if a point is in the area of the projected polygon (the 2D projection)
bool VWPolygon3D::IsPtInProjPoly2D(const VWPoint2D& pt, double dEpsilon) const
{
	return this->IsPtInProjPoly2D( pt.x, pt.y, dEpsilon );
}

bool VWPolygon3D::IsPtInProjPoly2D(double ptX, double ptY, double dEpsilon) const
{
	// not using epsilon -- but we should
	dEpsilon;

	int		cn			= 0;    // the crossing number counter

	size_t	polyPtCnt	= farrVertices.size();
	// loop through all edges of the polygon
	for (size_t i=0; i<polyPtCnt; i++) {    // edge from V[i] to V[i+1]
		const VWPoint3D&	pt		= farrVertices[i];
		const VWPoint3D&	nextPt	= i+1 < polyPtCnt ? farrVertices[i+1] : farrVertices[0];

		double				Vix		= pt.x;
		double				Viy		= pt.y;
		double				Vi1x	= nextPt.x;
		double				Vi1y	= nextPt.y;

		if (((Viy <= ptY) && (Vi1y > ptY))    // an upward crossing
			|| ((Viy > ptY) && (Vi1y <= ptY))) { // a downward crossing
			// compute the actual edge-ray intersect x-coordinate
			double vt = (double)(ptY - Viy) / (Vi1y - Viy);
			if ( ptX < Vix + vt * (Vi1x - Vix) ) // P.x < intersect
				++cn;   // a valid crossing of y=P.y right of P.x
		}
	}

	return (cn&1);    // 0 if even (out), and 1 if odd (in)
}

bool VWPolygon3D::IsPtInOrOnProjPoly2D(double ptX, double ptY, double dEpsilon) const
{
	// not using epsilon -- but we should
	dEpsilon;

	VWPoint2D	inPt( ptX, ptY );

	bool	inPoly		= false;
	bool	onPoly		= false;

	int		cn			= 0;    // the crossing number counter

	size_t	polyPtCnt	= farrVertices.size();
	// loop through all edges of the polygon
	for (size_t i=0; i<polyPtCnt; i++) {    // edge from V[i] to V[i+1]
		const VWPoint3D&	pt		= farrVertices[i];
		const VWPoint3D&	nextPt	= i+1 < polyPtCnt ? farrVertices[i+1] : farrVertices[0];

		onPoly	= VWLine2D::PtOnLine( inPt, pt, nextPt, dEpsilon );
		// if it is on poly break -- we have 'on poly' and dont have 'in poly'
		if ( onPoly ) 
			break;

		double				Vix		= pt.x;
		double				Viy		= pt.y;
		double				Vi1x	= nextPt.x;
		double				Vi1y	= nextPt.y;

		if (((Viy <= ptY) && (Vi1y > ptY))    // an upward crossing
			|| ((Viy > ptY) && (Vi1y <= ptY))) { // a downward crossing
			// compute the actual edge-ray intersect x-coordinate
			double vt = (double)(ptY - Viy) / (Vi1y - Viy);
			if ( ptX < Vix + vt * (Vi1x - Vix) ) // P.x < intersect
				++cn;   // a valid crossing of y=P.y right of P.x
		}
	}

	// if we have 'on poly' means that we dont have 'in poly'
	if ( ! onPoly ) {
		inPoly	= ( cn & 1 ) ? true : false;    // 0 if even (out), and 1 if odd (in)
	}
	else {
		inPoly	= false;
	}

	return onPoly || inPoly;
}

void VWPolygon3D::PointsTransform(const VWTransformMatrix& matrix)
{
 	size_t	len	= farrVertices.size();
	for(size_t i=0; i<len; i++) {
		VWPoint3D&	pt		= farrVertices[i];
		pt					= matrix.PointTransform( pt );
	}
}

void VWPolygon3D::InversePointsTransform(const VWTransformMatrix& matrix)
{
 	size_t	len	= farrVertices.size();
	for(size_t i=0; i<len; i++) {
		VWPoint3D&	pt		= farrVertices[i];
		pt					= matrix.InversePointTransform( pt );
	}
}

void VWPolygon3D::MovePoints(double dx, double dy, double dz)
{
 	size_t	len	= farrVertices.size();
	for(size_t i=0; i<len; i++) {
		VWPoint3D&	pt		= farrVertices[i];
		pt.x				+= dx;
		pt.y				+= dy;
		pt.z				+= dz;
	}
}

void VWPolygon3D::MovePoints(const VWPoint3D& amount)
{
 	size_t	len	= farrVertices.size();
	for(size_t i=0; i<len; i++) {
		VWPoint3D&	pt		= farrVertices[i];
		pt					+= amount;
	}
}