Classes for computational geometry

// geometry.cpp : implementation file
//
// General purpose computational geometry wrapper class for MS VC++
//
// Author      : Chris Maunder (cmaunder@mail.com)
// Date        : First written sometime in 1998
//
// Copyright � Chris Maunder 1998-2002. All Rights Reserved                      
//
// This code may be used in compiled form in any way you desire. This
// file may be redistributed unmodified by any means PROVIDING it is 
// not sold for profit without the authors written consent, and 
// providing that this notice and the authors name is included. If 
// the source code in this file is used in any commercial application 
// then a simple email would be nice.
//
// This file is provided "as is" with no expressed or implied warranty.
// The author accepts no liability for any damage, in any form, caused
// by this code. Use it at your own risk and as with all code expect bugs!
// It's been tested but I'm not perfect.
// 
// Please use and enjoy. Please let me know of any bugs/mods/improvements 
// that you have found/implemented and I will fix/incorporate them into this
// file.

#include "stdafx.h"
#include <math.h>
#include "utils.h"
#include "geometry.h"


C3Point GetClosestPoint2D(C3Point& start, C3Point& end, C3Point& P)
// returns the point on the line defined by [start,end] that is closest to point P
// All calculations are performed in the XY plane - Z coords are ignored.
// The point calculated may lie outside [start,end]
{
	if (ABS(start.x-end.x) < EPSILON)				// start-end form a vertical line
		return C3Point(start.x, P.y, start.z);
	
	if (ABS(end.y - start.y) < EPSILON)				// start-end form a horizontal line
		return C3Point(P.x, start.y, start.z);

	REAL Slope,Intercept, X;

	Slope = (end.y - start.y) / (end.x - start.x);
	Intercept = start.y - start.x*Slope;
	X = Slope/(Slope*Slope+1) * (P.y - Intercept + P.x/Slope);

	return C3Point(X, X*Slope + Intercept, start.z);
}

REAL Angle(C3Point p1, C3Point p2, C3Point p3)
// returns the angle (in degrees) between the 2 vectors formed from p2->p1, p2->p3.
{
	double cosA;
	C3Point v,u;

	v = p2-p1;
	u = p3-p2;

	cosA = u*v / D2Real( sqrt(Real2D( u.Length2()*v.Length2() )) ); 
	return D2Real(max(0.0, acos(cosA) * RAD2DEG));
}

REAL Angle(VECTOR v, VECTOR u)
// returns the angle (in degrees) between 2 vectors
{
	REAL cosA = u*v / D2Real( sqrt(Real2D( u.Length2()*v.Length2() )) ); 
	return D2Real(max(0.0, acos(cosA) * RAD2DEG));
}

VECTOR Normal(C3Point p1, C3Point p2, C3Point p3)
// Computs the normal (NOT unit normal) of a triangle, with points in CCW direction.
// CCW = Counter ClockWise
{
	return Cross(p2-p1, p3-p1);
}

REAL TriArea2(VECTOR u, VECTOR v)
// Calculates twice the area of the 3D triangle given by p1,p2,p3, Positive if CCW
{
	VECTOR CrossP = Cross(u,v);

	if (CrossP.z < D2Real(0.0))
		return -CrossP.Length();
	else
		return CrossP.Length();
}

REAL TriArea2(C3Point p1, C3Point p2, C3Point p3)
// Calculates twice the area of the 3D triangle given by p1,p2,p3, Positive if CCW
{
	VECTOR CrossP = Cross(p2-p1, p3-p1);

	if (CrossP.z < D2Real(0.0))
		return -CrossP.Length();
	else
		return CrossP.Length();
}

inline BOOL Xor(BOOL x, BOOL y)
// XOR: Arguments are negated to ensure that they are 0/1. Then the bitwise Xor operator may 
// apply.	(This idea is due to Michael Baldwin.)
{
	return	!x ^ !y;
}

BOOL IntersectProp(C3Point a, C3Point b, C3Point c, C3Point d)
// Returns true iff ab properly intersects cd: they share a point interior to both segments.
// The properness of the intersection is ensured by using strict leftness.
{
	/* Eliminate improper cases. */
	if (Colinear(a,b,c) || Colinear(a,b,d) || Colinear(c,d,a) ||Colinear(c,d,b))
		return FALSE;

    return Xor(Left(a,b,c), Left(a,b,d))&& Xor(Left(c,d,a), Left(c,d,b));
}

BOOL Between(C3Point a, C3Point b, C3Point c)
// Returns TRUE iff (a,b,c) are collinear and point c lies on the closed segement ab.
{
	if (!Colinear(a, b, c))
		return FALSE;

	/* If ab not vertical, check betweenness on x; else on y. */
	if (a.x != b.x) 
		return	((a.x <= c.x) && (c.x <= b.x)) ||
				((a.x >= c.x) && (c.x >= b.x));
	else
		return	((a.y <= c.y) && (c.y <= b.y)) ||
				((a.y >= c.y) && (c.y >= b.y));
}

BOOL Intersect(C3Point a, C3Point b, C3Point c, C3Point d)
// Returns true iff segments ab and cd intersect, properly or improperly.
{
	if (IntersectProp(a, b, c, d))
		return TRUE;
	else if (Between(a, b, c) || Between(a, b, d) || Between(c, d, a) || Between(c, d, b))
        return TRUE;
    else    
		return FALSE;
}

/////////////////////////////////////////////////////////////////////////////////////////////
// General C3Point/VECTOR operations

BOOL C3Point::operator==(C3Point& P)
{
	if (ABS(x - P.x) > EPSILON) return FALSE;
	if (ABS(y - P.y) > EPSILON) return FALSE;
	if (ABS(z - P.z) > EPSILON) return FALSE;
	return TRUE;
}

void C3Point::Normalise()
{
	REAL length = Length();
	if (length == D2Real(0.0)) return;
	Scale(D2Real(1.0)/length);
}

/////////////////////////////////////////////////////////////////////////////
// Class CPolygon - constructors, destructors and operations

CPolygon::CPolygon()
{
	m_point    = NULL;
	m_n		   = 0;
	//m_bFill	   = FALSE;
	//m_crColour = 0;
}

CPolygon::CPolygon(int n)
{
	if (!n) 
		m_point = NULL;
	else {
		m_point = (C3Point*) malloc(n*sizeof(C3Point));
		if (m_point == NULL) 
			AfxMessageBox("Unable to allocate sufficient memory for polygon");
	}

//	m_crColour = 0;
//	m_bFill    = FALSE;
	m_n = (m_point == NULL)? 0 : n;
}

CPolygon::~CPolygon()
{
	if (m_point != NULL) free(m_point);
	m_point = NULL;
	m_n = 0;
}

void CPolygon::operator=(CPolygon &P)
{
	if (this == &P) return;

	SetSize(P.GetSize());

	for (int i=0; i<GetSize(); i++) m_point[i] = P[i];
//	m_crColour = P.m_crColour; 
//	m_bFill    = P.m_bFill;
}

BOOL CPolygon::Closed()
{
	if (ABS(m_point[GetSize()-1].x - m_point[0].x) > EPSILON) return FALSE;
    if (ABS(m_point[GetSize()-1].y - m_point[0].y) > EPSILON) return FALSE;
	if (ABS(m_point[GetSize()-1].z - m_point[0].z) > EPSILON) return FALSE;
	return TRUE;
}

BOOL CPolygon::SetSize(int nSize)
{
	// Case 1: Set polygon as empty
	if (nSize == 0) {
		if (m_point) free(m_point);
		m_point = NULL;
		m_n = 0;
		return TRUE;
	}

	// Case 2: Allocate new memory for polygon
	if (m_point == NULL) {
		m_point = (C3Point*) malloc(nSize*sizeof(C3Point));
		if (m_point == NULL) {
			m_n = 0;
			return Error(NULL,"Low on memory: \nUnable to allocate memory for %d points.",nSize);
		} else {
			m_n = nSize;
			return TRUE;
		}
	}

	// Case 3: Resize existing polygon

	// Try and resize memory. If it doesn't work - leave polygon
	// how it was to begin with.
	C3Point* points = (C3Point*) realloc(m_point, nSize*sizeof(C3Point));
	if (points == NULL) 
		return Error(NULL,"Low on memory:\nUnable to resize polygon to %d points.",nSize);

	m_point = points;
	m_n = nSize;
	return TRUE;
}

// Trims polygon down so that points before "Start" and after "End" are removed.
// Start and End must be in the range 0..GetSize()-1
BOOL CPolygon::Trim(int Start, int End)
{
	ASSERT((End >=0) && (End < GetSize()));
	ASSERT((Start >=0) && (Start < GetSize()));

	if (!Start) 		// Easy case
		return SetSize(End+1);

	if (Start <= End) {
		SetSize(End+1);						// Remove end stuff
		for (int i = 0; i < Start; i++)		// Delete points less than Start
			Delete(i);
		return SetSize(GetSize());			// Remove extraneous stuff left at end
	}

	// Left with case where End < Start  i.e. a section including the 0 point is to be removed
	// Going to assume that the polygon is closed becuase only idiots cut out the centre of a 
	// non-closed polygon. ALTHOUGH - this is therefor highly likely, hence work needs to be done.

	for (int i = End; i<Start; i++)
		Delete(i);
	return SetSize(GetSize());
}

BOOL CPolygon::Close()
// Make polygon closed
{
	if (!Closed()) {
		if (!SetSize(GetSize()+1)) return FALSE;
		m_point[GetSize()-1] = m_point[0];
	}
	return TRUE;
}

BOOL CPolygon::Add(C3Point P)
// Add point to polygon
{
	if (!SetSize(GetSize()+1)) return FALSE;
	m_point[GetSize()-1] = P;
	return TRUE;
}

void CPolygon::Delete(int nPosition)
{
	for (int k = nPosition; k < GetSize()-1; k++) 
		m_point[k] = m_point[k+1];
	m_n--;
}

BOOL CPolygon::InsertAt(int nPosition, C3Point P)
{
	if (!SetSize(GetSize()+1)) return FALSE;
	for (int k = GetSize()-1; k > nPosition; k--) 
		m_point[k] = m_point[k-1];
	m_point[nPosition] = P;
	return TRUE;
}

inline void CPolygon::FreeExtra()
{
	SetSize(GetSize());
}

int CPolygon::PointSeparation(int Point1, int Point2)
{
	ASSERT(Point1 >= 0 && Point1 < GetSize());
	ASSERT(Point2 >= 0 && Point2 < GetSize());

	int nDistance;

	if (!Closed()) {
		nDistance = Point2 - Point1;
	} else {
		nDistance = abs(Point2 - Point1);
		nDistance = min(nDistance, abs(GetSize()-1 - nDistance));
	}
	return nDistance;
}

BOOL CPolygon::GetAttributes(REAL *pArea, C3Point *pCentroid, C3Point *pNorm,
							 REAL *pSlope, REAL *pAspect)
// Calculates and returns the polygons Area, centroid, norm, slope and aspect.
{
	C3Point SubMidPoint;
	REAL    SubArea;
	VECTOR  SubNorm;
	int nNumUniquePoints = (Closed())? GetSize()-1 : GetSize();

	*pArea     = D2Real(0.0);        // Partial area sum
	*pNorm     = C3Point( D2Real(0.0), D2Real(0.0), D2Real(0.0) );
	*pCentroid = C3Point( D2Real(0.0), D2Real(0.0), D2Real(0.0) );

	CPolygon P(nNumUniquePoints);	// Save current polygon
	if (!P.GetSize()) return FALSE;
	for (int i=0; i < P.GetSize(); i++) P[i] = m_point[i];

    // Start triangulating
	while (P.GetSize() > 3) {
		BOOL bCornerCut = FALSE;
		for (i = 0; i < P.GetSize(); i++ ) {

			int iPlus1 = i+1;        if (iPlus1 == P.GetSize()) iPlus1 = 0;
			int iPlus2 = (iPlus1+1); if (iPlus2 == P.GetSize()) iPlus2 = 0;
			
			if (P.Diagonal(i, iPlus2)) {

				SubArea     = TriArea2(P[i], P[iPlus1], P[iPlus2]);
				SubMidPoint = P[i] + P[iPlus1] + P[iPlus2];
				SubNorm		= Normal(P[i], P[iPlus1], P[iPlus2]);

				*pArea     += SubArea;
				*pCentroid += (SubMidPoint * SubArea);
				*pNorm	   += (SubNorm * SubArea);

				P.Delete(iPlus1);
				bCornerCut = TRUE;
				break;
			}
			if (Colinear(P[i], P[iPlus1], P[iPlus2])) {
				P.Delete(iPlus1);
				bCornerCut = TRUE;
				break;
			}
		}
		if (!bCornerCut) break;		// Couldn't cut at all!
	}

	// Should be left with a single triangle
	//ASSERT(P.GetSize() == 3);

	SubArea     = TriArea2(P[0], P[1], P[2]);	*pArea     += SubArea;
	SubMidPoint = P[0] + P[1] + P[2];			*pCentroid += (SubMidPoint * SubArea);
	SubNorm		= Normal(P[0], P[1], P[2]);		*pNorm	   += (SubNorm * SubArea);

	*pCentroid /= 3.0;
	pNorm->Normalise();

	if (*pArea > EPSILON) 		// Triangle or greater
		*pCentroid /= (*pArea);
	else 						// all points coincident or colinear, so get average
	{
		*pCentroid = C3Point( D2Real(0.0), D2Real(0.0), D2Real(0.0) );
		for (i=0; i<nNumUniquePoints; i++) *pCentroid += m_point[i];
		*pCentroid /= Int2Real(nNumUniquePoints);
	}
	*pArea = (*pArea < D2Real(0.0))? D2Real(-0.5) * *pArea : D2Real(0.5) * *pArea;

	// Slope ( Slope = 1/Slope(Norm) )
	if (pNorm->z < EPSILON)
		*pSlope = REAL_MAX;
	else
		*pSlope = D2Real(sqrt( Real2D(pNorm->x*pNorm->x + pNorm->y*pNorm->y))) / pNorm->z;
	
	// Aspect
	if (*pSlope < 0.01)
		*pAspect = D2Real(0.0);
	else {
		*pAspect = D2Real(atan2(Real2D(pNorm->y), Real2D(pNorm->x))) * RAD2DEG;
		*pAspect = D2Real(90.0) - *pAspect; 
		if (*pAspect < D2Real(0.0)) *pAspect += D2Real(360.0);
	}

	return TRUE;
}

REAL CPolygon::Area()
// Returns the area of the polygon.
{
	REAL sum = D2Real(0.0);        /* Partial area sum */

    for (int i = 1; i < GetSize()-1; i++)
         sum += TriArea2(m_point[0], m_point[i], m_point[i+1]);

	if (sum < D2Real(0.0)) sum = -sum;
    return D2Real(0.5) * sum;
}

C3Point CPolygon::Centroid()
{
	static REAL TotalArea;
	static C3Point Centre;

	Centre = C3Point( D2Real(0.0), D2Real(0.0), D2Real(0.0) );
	TotalArea = D2Real(0.0);

	for (int i=0; i < GetSize()-2; i++) {
		C3Point MidPoint = (Point(0) + Point(i+1) + Point(i+2)) / 3.0;
		REAL TArea = TriArea2(Point(0), Point(i+1), Point(i+2));

		MidPoint.Scale(TArea);		// Wieght current triangle midpoint by area.
		TotalArea += TArea;
		Centre    += MidPoint;
	}

	if (TotalArea > EPSILON) 		// Triangle or greater
	{
		Centre.Scale(D2Real(1.0) / TotalArea); 
		return Centre;
	}
	else 							// all points coincident or colinear
	{
		Centre = C3Point( D2Real(0.0), D2Real(0.0), D2Real(0.0) );
		if (Closed()) {
			for (i=0; i<GetSize()-1; i++) Centre += m_point[i];
			Centre.Scale(D2Real(1.0/(double)(GetSize()-1)));
		} else {
			for (i=0; i<GetSize(); i++) Centre += m_point[i];
			Centre.Scale(D2Real(1.0/(double)GetSize()));
		}
		return Centre;
	}
}

REAL CPolygon::SegmentLength(int start, int end)
{
	int i;
	REAL Width = D2Real(0.0);
	VECTOR v;

	if ((start < 0) || (end < 0) || (start >= GetSize()) || (end > GetSize())) 
		return Width;
	
	if (!start && (end == GetSize())) {		// Full perimeter length
		for (i = 0; i < GetSize()-1; i++) {
			v = m_point[i+1] - m_point[i];
			Width += v.Length();
		}
		v = m_point[0] - m_point[GetSize()-1];
		Width += v.Length();
		return Width;
	}

	if (start > end) {
		for (i = start; i < GetSize()-1; i++) {
			v = m_point[i+1] - m_point[i];
			Width += v.Length();
		}
		v = m_point[0] - m_point[GetSize()-1];
		Width += v.Length();
		for (i = 0; i < end; i++) {
			v = m_point[i+1] - m_point[i];
			Width += v.Length();
		}
	} else if (start < end) {
		for (int i = start; i < end; i++) {
			v = m_point[i+1] - m_point[i];
			Width += v.Length();
		}
	}
	// else right == left, so do nothing and return 0.0

	return Width;
}

// Given a C3Point, returns the nearest C3Point on the given polygon, with the option
// of also returning the index into the CPolygon point array of that point.
// Proximity is based on 2D, not 3D calculations.
C3Point CPolygon::GetClosestPoint(C3Point p, int *nIndex/*=NULL*/)
{
	REAL MinDistance = REAL_MAX;
	int index;

	for (int i = 0; i < GetSize(); i++) {
		REAL Distance =  (p.x - m_point[i].x)*(p.x - m_point[i].x)
					   + (p.y - m_point[i].y)*(p.y - m_point[i].y);
		if (Distance < MinDistance) {
			MinDistance = Distance;
			index = i;
		}
	}
	if (nIndex) *nIndex = index;
	return m_point[index];
}

BOOL CPolygon::Diagonalie(int i, int j)
// Returns TRUE iff (v_i, v_j) is a proper internal *or* external diagonal of this polygon, 
// *ignoring edges incident to v_i and v_j*.
{
	/* For each edge (k,k+1) of P */
	for (int k = 0; k < GetSize(); k++ ) {

		int kPlus1 = (k+1) % GetSize();

		/* Skip edges incident to i or j */
		if ( ! ((k == i) || (kPlus1 == i) || (k == j) || (kPlus1 == j)) )
			if (Intersect(m_point[i], m_point[j], m_point[k], m_point[kPlus1]))
				return	FALSE;
	}
	return	TRUE;
}

BOOL CPolygon::InCone(int i, int j)
// 	Returns true iff the diagonal (i,j) is striclty internal to the polygon in 
//  the neighborhood of the i endpoint.
{
	int	iPlus1  = (i + 1)       % GetSize();
	int	iMinus1 = (i + GetSize() - 1) % GetSize();

	/* If P[i] is a convex vertex [ i+1 left or on (i-1,i) ]. */
	if ( LeftOn(m_point[iMinus1], m_point[i], m_point[iPlus1]) )
		return	Left( m_point[i], m_point[j], m_point[iMinus1] )
		     &&	Left( m_point[j], m_point[i], m_point[iPlus1]  );

	/* Assume (i-1,i,i+1) not collinear. */
	/* else v_i is reflex. */
	else
		return	!(	LeftOn( m_point[i], m_point[j], m_point[iPlus1]  )
		            &&	LeftOn( m_point[j], m_point[i], m_point[iMinus1] ) );
}

BOOL CPolygon::Diagonal(int i, int j)
// Returns TRUE iff (v_i, v_j) is a proper internal diagonal of P.
{
	return InCone(i,j) && Diagonalie(i,j);
}

BOOL CPolygon::Triangulate(CPolygon* TriPoly)
// Triangulates this polygon and saves triangle edges in TriPoly.
// Triangles are stored CCW, with each set of 3 consecutive points in TriPoly
// representing 1 triangle.
// Assumes this polygon is closed.
{
	if (GetSize() < 4) return FALSE;

	if (!TriPoly->SetSize((GetSize()-3)*3))	 // Resize to max size needed, but set TriPoly.GetSize()=0
		return FALSE;						 // so it can be incremented in the loop.
	TriPoly->m_n = 0;						 // This is a paranoia kludge in case the trianglulation
											 // does not find all triangles needed.

	CPolygon P(GetSize()-1);				 // Save current polygon
	if (!P.GetSize()) return FALSE;
	for (int i=0; i < P.GetSize(); i++) P[i] = m_point[i];

    // Start triangulating
	while (P.GetSize() > 3) {
		BOOL bCornerCut = FALSE;
		for (i = 0; i < P.GetSize(); i++ ) {

			int iPlus1 = i+1;        if (iPlus1 == P.GetSize()) iPlus1 = 0;
			int iPlus2 = (iPlus1+1); if (iPlus2 == P.GetSize()) iPlus2 = 0;
			
			if (P.Diagonal(i, iPlus2)) {
				
				(*TriPoly)[TriPoly->m_n++] = P[i];
				(*TriPoly)[TriPoly->m_n++] = P[iPlus1];
				(*TriPoly)[TriPoly->m_n++] = P[iPlus2];
			
				P.Delete(iPlus1);
				bCornerCut = TRUE;
				break;
			}
			if (Colinear(P[i], P[iPlus1], P[iPlus2])) {
				P.Delete(iPlus1);
				bCornerCut = TRUE;
				break;
			}
		}
		if (!bCornerCut) {		// Error - bad poly
			TriPoly->SetSize(TriPoly->GetSize());		// resize to the actual size needed.
			return FALSE;
		}
	}

	// Should be left with a single triangle
	ASSERT(P.GetSize() == 3);
	(*TriPoly)[TriPoly->m_n++] = P[0];
	(*TriPoly)[TriPoly->m_n++] = P[1];
	(*TriPoly)[TriPoly->m_n++] = P[2];

	TriPoly->SetSize(TriPoly->GetSize());		// Now resize to the actual size needed.

	return TRUE;
}

BOOL CPolygon::CPTriangulate(CPolygon* TriPoly, C3Point C)
// Triangulates polygon P and saves triangle edges in TriPoly.
// Triangles are stored CCW, with each set of 3 consecutive points in TriPoly
// representing 1 triangle.
// Assumes polygon P is closed.
{		
	if (GetSize() == 4) {
		if (!TriPoly->SetSize(3)) return FALSE;
		for (int i=0; i<3; i++) (*TriPoly)[i] = m_point[i];
	} 
	else 
	{
		if (!TriPoly->SetSize((m_n-1)*3)) return FALSE;
		TriPoly->m_n = 0;
		for (int i = 0; i < (m_n-1); i++) {
			(*TriPoly)[(TriPoly->m_n)++] = C;
			(*TriPoly)[(TriPoly->m_n)++] = m_point[i];
			(*TriPoly)[(TriPoly->m_n)++] = m_point[i+1];
		}
	}
	return TRUE;
}

void CPolygon::Rationalise(int nAngle)
// Combines adjacent line segments if the angle between them is less than nAngle.
{
	for (int i=1; i<(GetSize()-1); i++) 
		if (Angle(m_point[i-1], m_point[i], m_point[i+1]) < nAngle)
			Delete(i);

	SetSize(GetSize());
}

void CPolygon::Translate(VECTOR v)
{
	for (int j=0; j<GetSize(); j++) m_point[j] += v;
}

BOOL CPolygon::InSpan(int start, int end, int index)
{
	if ((index < 0) || (index >= GetSize())) return FALSE;

	if (start <= end) 
		return ( (index >= start) && (index <= end) );

	// (start > end) => the section spanned by [start, end] crosses over the 0 point
	return !((index > end) && (index < start));

	// Alternate (longer) method
	//if ( ((index >= start) && (index <= GetSize())) || ((index >=0) && (index <= end)) )
	//	return TRUE;
	//return FALSE;
}	

BOOL CPolygon::InSpanProper(int start, int end, int index)
{
	if ((index < 0) || (index >= GetSize())) return FALSE;

	if (start <= end) 
		return ( (index > start) && (index < end) );

	// (start > end) => the section spanned by [start, end] crosses over the 0 point
	return !((index >= end) && (index <= start));
}	

BOOL CPolygon::PointIn(C3Point P)
// Tests if point within polygon, or on an edge or vertex, by shooting a ray along x axis
{
int     j, inside_flag, xflag0 ;
REAL    dv0 ;
int     crossings, yflag0, yflag1 ;
C3Point Vertex0, Vertex1;

	Vertex0 = m_point[GetSize()-1];

	/* get test bit for above/below Y axis */
    yflag0 = ( dv0 = Vertex0.y - P.y ) >= 0.0 ;

    crossings = 0 ;
    for ( j = 0 ; j < GetSize() ; j++ ) {

       // cleverness:  bobble between filling endpoints of edges, so
       // that the previous edge's shared endpoint is maintained.
       if ( j & 0x1 ) {
           Vertex0 = m_point[j] ;
           yflag0 = ( dv0 = Vertex0.y - P.y ) >= 0.0 ;
       } else {
           Vertex1 = m_point[j] ;
           yflag1 = ( Vertex1.y >= P.y ) ;
       }

       /* check if points not both above/below X axis - can't hit ray */
       if ( yflag0 != yflag1 ) {
            /* check if points on same side of Y axis */
            if ( ( xflag0 = ( Vertex0.x >= P.x ) ) == ( Vertex1.x >= P.x ) ) {

                    if ( xflag0 ) crossings++ ;
                } else {
                    /* compute intersection of pgon segment with X ray, note
                     * if > point's X.
                     */
                    crossings += (Vertex0.x -
                        dv0*( Vertex1.x-Vertex0.x)/(Vertex1.y-Vertex0.y)) >= P.x ;
                }
            }
        }

        // test if crossings is odd
        // if all we care about is winding number > 0, then just:
        //       inside_flag = crossings > 0;

		inside_flag = crossings & 0x01 ;

    return (inside_flag) ;
}

/* OR - (not so reliable)
  int i, j, right, inside = 0;
  for (i = 0, j = n-1; i < n; j = i++) {
    if ((y[i] <= Y) != (y[j]<= Y)) {
      if ((right = x[i] >= X) != (x[j] >= X)) {
        if ((X - x[i]) * (y[j] - y[i]) < (x[j] - x[i]) * (Y - y[i]))
          inside = !inside;
      }
      else if (right) inside = !inside; 
    }
  }
*/

BOOL CPolygon::DelauneyTri(CPolygon *TriPoly)
// 2D Delauney triangulation in the X-Y plane. (Z coordinates of polygon vertices ignored)
{
	int	i, j, k, m;					// indices of four points 
	REAL xn, yn, zn;				// outward vector normal to (i,j,k) 
	int	flag;						// TRUE if m above of (i,j,k) 
	REAL *z;

	ASSERT(GetSize() > 2);

	if (!TriPoly->SetSize((GetSize()-2)*3)) // Resize to max size needed, but set 
		return FALSE;						// TriPoly.m_n=0 so it can be incremented 
	TriPoly->m_n = 0;						// in the loop.	 						
		  
	z = new REAL[GetSize()];
	if (z == NULL) return FALSE;

	for (i=0; i<GetSize(); i++)	
		z[i] = m_point[i].x*m_point[i].x + m_point[i].y*m_point[i].y;

	// For each triple (i,j,k) 
	for ( i = 0; i < GetSize() - 2; i++ )
	for ( j = i + 1; j < GetSize(); j++ )
	for ( k = i + 1; k < GetSize(); k++ ) 
	if ( j != k ) {

		// Compute normal to triangle (i,j,k). 
		xn = (m_point[j].y-m_point[i].y)*(z[k]-z[i]) - (m_point[k].y-m_point[i].y)*(z[j]-z[i]);
		yn = (m_point[k].x-m_point[i].x)*(z[j]-z[i]) - (m_point[j].x-m_point[i].x)*(z[k]-z[i]);
		zn = (m_point[j].x-m_point[i].x)*(m_point[k].y-m_point[i].y) 
									 - (m_point[k].x-m_point[i].x)*(m_point[j].y-m_point[i].y);
		flag = (zn < 0);
		if (flag)		// Only examine faces on bottom of paraboloid: zn < 0.
			for (m = 0; m < GetSize(); m++)	// For each other point m check if m above (i,j,k).
				flag = flag && 
				         ((m_point[m].x-m_point[i].x)*xn + 
								(m_point[m].y-m_point[i].y)*yn + (z[m]-z[i])*zn <= 0);
		if (flag) {
			(*TriPoly)[TriPoly->m_n++] = m_point[i];
			(*TriPoly)[TriPoly->m_n++] = m_point[j];
			(*TriPoly)[TriPoly->m_n++] = m_point[k];
		}
	}
	delete [] z;
	TriPoly->SetSize(TriPoly->m_n);		// Now resize to the actual size needed.  
	return TRUE;
}

BOOL CPolygon::LoadXY(LPCTSTR Filename, REAL Zdefault /* = D2Real(0.0) */)
// Loads the polygon in the file "Filename"
{
	float x,y;
	FILE *fp;
	int nNum;
	char buffer[100];

	// empty polygon
	RemoveAll();

	// open file for reading
	if (!FileOpen(&fp,Filename,"r"))
		return FALSE;

	// Get no. of vertices
	if (fscanf(fp,"%d", &nNum) != 1) {
		fclose(fp);
		return FALSE;
	}
	// read off rest of line
	fgets(buffer, 100, fp); 

	// Get some memory for polygon
	if (!SetSize(nNum)) {
		fclose(fp);
		return FALSE;
	}

	// read in polygon points (should do some error checking here...)
	for (int i=0; i<nNum; i++) {
		fscanf(fp,"%f %f", &x, &y);
		m_point[i] = C3Point(F2Real(x), F2Real(y), Zdefault);
	}
    fclose(fp);
	
	return TRUE;
}

BOOL CPolygon::LoadXY(FILE* fp, REAL Zdefault /* = D2Real(0.0) */)
// Loads the polygon in the already open file, leaving it open
{
	float x,y;
	int nNum;
	char buffer[100];

	RemoveAll();

	if (fscanf(fp,"%d", &nNum) != 1) return FALSE;	// get No. points in polygon
	fgets(buffer, 100, fp);							// read off rest of line
	if (!SetSize(nNum))		     return FALSE;		// get memory for polygon

	// read in polygon
	for (int i=0; i<nNum; i++) {
		fscanf(fp,"%f %f", &x, &y);
		m_point[i] = C3Point(F2Real(x), F2Real(y), Zdefault);
	}

	return TRUE;
}


BOOL CPolygon::LoadXYZ(LPCTSTR Filename, BOOL bWarn /*=TRUE*/)
// Loads the polygon in the file "Filename". If bWarn = TRUE it will post an 
// error message if the file couldn't be loaded.
{
	float x,y,z;
	FILE *fp;

	if (!FileOpen(&fp,Filename,"r", bWarn)) {
		RemoveAll();
		return FALSE;
	}

	while (fscanf(fp,"%f %f %f", &x, &y, &z) == 3)
		Add(C3Point(x,y,z));
	fclose(fp);

	return TRUE;
}

BOOL CPolygon::LoadXYZ(FILE* fp)
// Loads the polygon in the already open file, leaving it open
{
	double x,y,z;
	int nNum;
	char buffer[100];

	RemoveAll();

	if (fscanf(fp,"%d", &nNum) != 1) return FALSE;	// get No. points in polygon
	fgets(buffer, 100, fp);							// read off rest of line
	if (!SetSize(nNum))		     return FALSE;		// get memory for polygon

	// read in polygon
	for (int i=0; i<nNum; i++) {
		fscanf(fp,"%lf %lf %lf", &x, &y, &z);
		m_point[i] = C3Point(D2Real(x), D2Real(y), D2Real(z));
	}

	return TRUE;
}

BOOL CPolygon::Save(LPCTSTR Filename, BOOL bAsPoints /*= FALSE */ , BOOL bWarn /*= TRUE*/)
{
	FILE *fp;

	if (!FileOpen(&fp,Filename,"w", bWarn))
		return FALSE;

	if (bAsPoints) {
		for (int i = 0; i < GetSize(); i++)
			fprintf(fp, "%10.2f %10.2f %10.2f", m_point[i].x, m_point[i].y, m_point[i].z);
	} else {
		fprintf(fp, "%6d %10.2f\n",GetSize(), m_point[0].z);
		for (int i = 0; i < GetSize(); i++) {
			fprintf(fp,"%10.2f %10.2f", m_point[i].x, m_point[i].y);
			if ((i+1)%4 == 0) fprintf(fp,"\n");
		}
		if ((GetSize()+1)%4) fprintf(fp,"\n");
	}

	fclose(fp);
	return TRUE;
}


// Natural and Clamped Cubic Splines
//
// Input:  A CPolygon of N points (x_i,y_i), 0 <= i <= N-1, which are sampled
// from a function, y_i = f(x_i).  The function f is unknown.  For natural
// cubic splines, it is assumed f"(x_0) = f"(x_N-1) = 0.  For clamped cubic
// splines, the derivatives f'(x_0) and f'(x_N-1) are user-specified.
//
// Output: b_i, c_i, d_i, 0 <= i <= N-2, which are coefficients for the cubic
// spline S_i(x) = a_i + b_i(x-x_i) + c_i(x-x_i)^2 + d_i(x-x_i)^3 for
// x_i <= x < x_{i+1}.
//
// Algorithm implemented from
//    Numerical Analysis, 3rd edition
//    Richard L. Burden and J. Douglas Faires
//    Prindle, Weber & Schmidt
//    Boston, 1985, pp. 122-124.
//-----------------------------------------------------------------------------

void CPolygon::NaturalSpline(double*& b, double*& c, double*& d)
{
	const double oneThird = 1.0/3.0;

	int i;
	double* h = new double[GetSize()-1];
	double* hdiff = new double[GetSize()-1];
	double* alpha = new double[GetSize()-1];

	for (i = 0; i < GetSize()-1; i++)
		h[i] = m_point[i+1].x-m_point[i].x;

	for (i = 1; i < GetSize()-1; i++)
		hdiff[i] = m_point[i+1].x-m_point[i-1].x;

	for (i = 1; i < GetSize()-1; i++)
	{
		double numer = 3.0*(m_point[i+1].y*h[i-1]-m_point[i].y*hdiff[i]+m_point[i-1].y*h[i]);
		double denom = h[i-1]*h[i];
		alpha[i] = numer/denom;
	}

	double* ell = new double[GetSize()];
	double* mu = new double[GetSize()-1];
	double* z = new double[GetSize()];
	double recip;

	ell[0] = 1.0;
	mu[0] = 0.0;
	z[0] = 0.0;

	for (i = 1; i < GetSize()-1; i++)
	{
		ell[i] = 2.0*hdiff[i]-h[i-1]*mu[i-1];
		recip = 1.0/ell[i];
		mu[i] = recip*h[i];
		z[i] = recip*(alpha[i]-h[i-1]*z[i-1]);
	}
	ell[GetSize()-1] = 1.0;
	z[GetSize()-1] = 0.0;

	b = new double[GetSize()-1];
	c = new double[GetSize()];
	d = new double[GetSize()-1];

	c[GetSize()-1] = 0.0;

	for (i = GetSize()-2; i >= 0; i--)
	{
		c[i] = z[i]-mu[i]*c[i+1];
		recip = 1.0/h[i];
		b[i] = recip*(m_point[i+1].y-m_point[i].y)-h[i]*(c[i+1]+2.0*c[i])*oneThird;
		d[i] = oneThird*recip*(c[i+1]-c[i]);
	}

	delete[] h;
	delete[] hdiff;
	delete[] alpha;
	delete[] ell;
	delete[] mu;
	delete[] z;
}

REAL CPolygon::Curvature(int nIndex, int nSampleSize)
{
	ASSERT(nIndex >= 0 && nIndex < GetSize());

	// Get a range of point on the polygon to sample
	int nMinPt = (nSampleSize > nIndex)? 0 : nIndex-nSampleSize;
	int nMaxPt = (nIndex+nSampleSize >= GetSize())? GetSize()-1 : nIndex+nSampleSize;

	// Store these points in a temporary polygon
	CPolygon XVals(nMaxPt-nMinPt+1), 
		     YVals(nMaxPt-nMinPt+1);

	REAL IndexDistance = 0.0, distance = 0.0;
	for (int i = nMinPt; i <= nMaxPt; i++) {
		if (i > nMinPt)
			distance += D2Real(sqrt( (m_point[i].x-m_point[i-1].x)*(m_point[i].x-m_point[i-1].x)
								  +  (m_point[i].y-m_point[i-1].y)*(m_point[i].y-m_point[i-1].y)));

		XVals[i].x = YVals[i].x = distance;
		XVals[i].y = m_point[i].x;
		YVals[i].y = m_point[i].y;
		if (i == nIndex) 
			IndexDistance = distance;
	}

	// fit a spline to the points in the temp polygon.
	double *b1,*c1,*d1, *b2, *c2, *d2;
	XVals.NaturalSpline(b1,c1,d1);
	YVals.NaturalSpline(b2,c2,d2);

	// The point in question is nIndex-nMinPt. The spline curve for
	// this point is 
	int nPoint = nIndex-nMinPt;

	double numerator = b1[nPoint] * 2.0*c2[nPoint] - b2[nPoint] * 2.0*c1[nPoint];
	double denomenator = pow(b1[nPoint]*b1[nPoint] + b2[nPoint]*b2[nPoint], 1.5);
      
	delete [] b1; delete [] c1; delete [] d1;
	delete [] b2; delete [] c2; delete [] d2;

	if (denomenator > 0)    
		return D2Real(numerator/denomenator);    
	else    
		return D2Real(0.0);  // could be nonzero in the limit    
}

REAL CPolygon::Curvature(int i)
// Calculates the curvature for the segment i, defined by points i and i+1
{
	VECTOR dRdsA, dRdsB, CurvVector;
	REAL Length, LengthFracA, LengthFracB;

	if (!i || i==GetSize()-1) return 0.0;

	ASSERT(i > 0 && i < GetSize()-1);

	// if R(t) is a parametric curve with arc length s, then the tangent vector
	// to the curve R(t) is T(t) = R'(t) / |R(t)|, which can be rewritten as
	// T(s) = dR/ds

	// Calculate unit tangent vector to curve using points i and i-1
	Length = D2Real(sqrt( (m_point[i].x-m_point[i-1].x)*(m_point[i].x-m_point[i-1].x) +
						  (m_point[i].y-m_point[i-1].y)*(m_point[i].y-m_point[i-1].y)));
	ASSERT(Length > D2Real(0.0));

	LengthFracA = D2Real(1.0) / Length;
	dRdsA.x = (m_point[i].x-m_point[i-1].x) * LengthFracA;
	dRdsA.y = (m_point[i].y-m_point[i-1].y) * LengthFracA;

	// Calculate unit tangent vector to curve using points i and i+1
	Length = D2Real(sqrt( (m_point[i+1].x-m_point[i].x)*(m_point[i+1].x-m_point[i].x) +
						  (m_point[i+1].y-m_point[i].y)*(m_point[i+1].y-m_point[i].y)));
	ASSERT(Length > D2Real(0.0));

	LengthFracB = D2Real(1.0) / Length;
	dRdsB.x = (m_point[i+1].x-m_point[i].x) * LengthFracB;
	dRdsB.y = (m_point[i+1].y-m_point[i].y) * LengthFracB;

	// The curvature is defined as k(s) = |dT/ds| ( = |T'|/|R'|, '=d/dt)
	// Warning: dodgy averaging used here :)

	CurvVector.x = (dRdsB.x - dRdsA.x) * LengthFracA;
	CurvVector.y = (dRdsB.y - dRdsA.y) * LengthFracA;

	REAL Curvature = D2Real(sqrt(CurvVector.x*CurvVector.x + CurvVector.y*CurvVector.y));

	if (!Left(m_point[i-1],m_point[i], m_point[i+1]))
		Curvature = -Curvature;

	return Curvature;
}