Click here to Skip to main content
Click here to Skip to main content
Add your own
alternative version

Classes for computational geometry

, 26 Dec 2001 CPOL
Some classes and utility functions for general computational geometry
geometry.zip
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;
}

By viewing downloads associated with this article you agree to the Terms of Service and the article's licence.

If a file you wish to view isn't highlighted, and is a text file (not binary), please let us know and we'll add colourisation support for it.

License

This article, along with any associated source code and files, is licensed under The Code Project Open License (CPOL)

Share

About the Author

Chris Maunder
Founder CodeProject
Canada Canada
Chris is the Co-founder, Administrator, Architect, Chief Editor and Shameless Hack who wrote and runs The Code Project. He's been programming since 1988 while pretending to be, in various guises, an astrophysicist, mathematician, physicist, hydrologist, geomorphologist, defence intelligence researcher and then, when all that got a bit rough on the nerves, a web developer. He is a Microsoft Visual C++ MVP both globally and for Canada locally.
 
His programming experience includes C/C++, C#, SQL, MFC, ASP, ASP.NET, and far, far too much FORTRAN. He has worked on PocketPCs, AIX mainframes, Sun workstations, and a CRAY YMP C90 behemoth but finds notebooks take up less desk space.
 
He dodges, he weaves, and he never gets enough sleep. He is kind to small animals.
 
Chris was born and bred in Australia but splits his time between Toronto and Melbourne, depending on the weather. For relaxation he is into road cycling, snowboarding, rock climbing, and storm chasing.
Follow on   Twitter   Google+   LinkedIn

| Advertise | Privacy | Terms of Use | Mobile
Web04 | 2.8.150302.1 | Last Updated 27 Dec 2001
Article Copyright 2001 by Chris Maunder
Everything else Copyright © CodeProject, 1999-2015
Layout: fixed | fluid