Back to main

CSG 2D

This small 2D library performs boolean operations involving cosntructive solid geometry and calculates contours. Other utility functions are included, such as simplifying polygon meshes. I'm releasing the source under the GPL license.

Example

Subtracting the geometry in the first image from a single quad polygon results in the geometry in the second image. Adjacent polygons in the mesh can be merged such that their unions result in a convex set of polygons, but I have not done so in these images to demonstrate the splits. Click to enlarge.

Source

template<class T> class Vec2T
{
public:
	T x, y;

	inline Vec2T<T>() : x(0), y(0) { }
	inline Vec2T<T>(T x, T y) : x(x), y(y) { }

	inline void operator +=(const Vec2T<T>& a) { x += a.x; y += a.y; }
	inline void operator -=(const Vec2T<T>& a) { x -= a.x; y -= a.y; }
	inline void operator *=(T f) { x *= f; y *= f; }

	inline Vec2T<T> operator-() { return Vec2T<T>(-x, -y); }
	inline friend Vec2T<T> operator +(const Vec2T<T>& a, const Vec2T<T>& b) { return Vec2T<T>(a.x + b.x, a.y + b.y); }
	inline friend Vec2T<T> operator -(const Vec2T<T>& a, const Vec2T<T>& b) { return Vec2T<T>(a.x - b.x, a.y - b.y); }

	inline friend Vec2T<T> operator *(const Vec2T<T>& a, float f) { return Vec2T<T>(a.x * f, a.y * f); }
	inline friend Vec2T<T> operator *(float f, const Vec2T<T>& a) { return Vec2T<T>(a.x * f, a.y * f); }

	T Length() const { return (T)sqrt(x * x + y * y); }
	T LengthSq() const { return x * x + y * y; }

	inline void Normalize() 
	{
		T len = Length();
		if (len > (T)0) { x /= len; y /= len; }
	}

	T Dot(const Vec2T<T>& a) const { return x * a.x + y * a.y; }
	
	static T Dot(const Vec2T<T>& a, const Vec2T<T>& b) { return a.Dot(b); }

	static Vec2T<T> Minimize(const Vec2T<T>& a, const Vec2T<T>& b)
	{ return Vec2T<T>(a.x < b.x ? a.x : b.x, a.y < b.y ? a.y : b.y); }

	static Vec2T<T> Maximize(const Vec2T<T>& a, const Vec2T<T>& b) 
	{ return Vec2T<T>(a.x > b.x ? a.x : b.x, a.y > b.y ? a.y : b.y); }
};

typedef Vec2T<float> Vec2f;
typedef Vec2T<double> Vec2d;

typedef double CsgScalar;

#define CSG_MEM_LEAK_DETECTOR
#ifdef CSG_MEM_LEAK_DETECTOR
#include <Windows.h>
#endif

namespace CSG
{
	struct IndexPair
	{
		int i, j;
		IndexPair(int i, int j) : i(i), j(j) { }
	};

	struct Polygon
	{
		void AddPoint(const Vec2T<CsgScalar>& pt) { pts.push_back(pt); }
		void AddPoint(CsgScalar x, CsgScalar y) { AddPoint(Vec2T<CsgScalar>(x, y)); }
		std::vector<Vec2T<CsgScalar>> pts;

		Vec2T<CsgScalar> bb_min, bb_max;

		std::vector<IndexPair> edges;
		std::vector<Vec2T<CsgScalar>> pt_n;
		Vec2T<CsgScalar> center();

		void Build();

		bool ContainsPoint(const Vec2T<CsgScalar>& pt);
		bool ContainsEdge(const Vec2T<CsgScalar>& a, const Vec2T<CsgScalar>& b);
		bool ContainsPolygon(const Polygon* p);
		bool ContainsPolygon(const std::vector<Vec2T<CsgScalar>>& p);

		bool IsSplitOnPlane(Vec2T<CsgScalar>& pt, Vec2T<CsgScalar>& n);
		void SplitOnPlane(Vec2T<CsgScalar>& pt, Vec2T<CsgScalar>& n, std::vector<Vec2T<CsgScalar>>& pf, std::vector<Vec2T<CsgScalar>>& pb);
		void ClipAgainstPlane(Vec2T<CsgScalar>& pt, Vec2T<CsgScalar>& n, std::vector<Vec2T<CsgScalar>>& p);

		bool IsAdjacent(Polygon* p);
		
		CsgScalar ExtentMax();
		CsgScalar ExtentMin();
	};

	struct Edge
	{
		Vec2T<CsgScalar> a, b;

		Edge() : a(), b() { }
		Edge(const Vec2T<CsgScalar>& a, const Vec2T<CsgScalar>& b) : a(a), b(b) { }
	};

	struct Contour
	{
		std::vector<Edge> edges;
	};

	void ConvexHull(std::vector<Vec2T<CsgScalar>>& points, std::vector<Vec2T<CsgScalar>>& hull);
	bool IsConvex(std::vector<Vec2T<CsgScalar>>& pts);
	bool IsDegenerate(std::vector<Vec2T<CsgScalar>>& pts);
	CsgScalar Area(std::vector<Vec2T<CsgScalar>>& pts);

	Polygon* PolygonCreate();
	void PolygonDestroy(Polygon** p);
	void PolygonMemLeakTest();
	void PolygonBreakOnCreate(int index);

	bool SplitPolygon(std::vector<Polygon*>& poly, size_t index, Polygon* splitter, bool remove_if_contains);

	void Subtract(std::vector<Polygon*>& poly, Polygon* p);
	void Add(std::vector<Polygon*>& poly, Polygon* p);

	Edge TraceLine(std::vector<Polygon*>& poly, const Vec2T<CsgScalar>& start, const Vec2T<CsgScalar>& end, Polygon* skip_poly);
	Contour ExteriorContour(std::vector<Polygon*>& poly);

	std::vector<float> RenderEdges(std::vector<Polygon*>& poly);
	std::vector<float> RenderTri(std::vector<Polygon*>& poly);

	std::vector<IndexPair> Adjacency(std::vector<Polygon*>& poly);
	void Simplify(std::vector<Polygon*>& poly);
}

#ifndef ASSERT
#define ASSERT(x) { if (!(x)) { __debugbreak(); } }
#endif

typedef Vec2T<CsgScalar> Vec2;
const CsgScalar epsilon = 1e-5;
const CsgScalar epsilon_edge = 1e-3;
const CsgScalar epsilon_contour = 1e-4;
const CsgScalar epsilon_sq = epsilon * epsilon;

int allocs = 0;
int deallocs = 0;
int alloc_break_index = -1;

#ifdef CSG_MEM_LEAK_DETECTOR
#include <map>
#include <iostream>
std::map<CSG::Polygon*, int> alloc_map;

CSG::Polygon* CSG::PolygonCreate()
{ 
	if (alloc_break_index == ++allocs) { __debugbreak(); }
	Polygon* p = new Polygon();
	alloc_map[p] = allocs;
	return p;
}

void CSG::PolygonDestroy(CSG::Polygon** p) 
{ 
	++deallocs; 
	if (*p)
	{
		alloc_map.erase(alloc_map.find(*p));
		delete *p;
	}
	*p = 0;
}

void CSG::PolygonMemLeakTest() 
{ 
	for (std::map<Polygon*, int>::iterator it = alloc_map.begin(); it != alloc_map.end(); ++it)
	{
		if (!it->first) { continue; }
		char buf[1024];
		sprintf_s(buf, 1024, "Leaked CSG polygon alloc ID %d\r\n", it->second);
		OutputDebugString(buf);
	}
	alloc_map.clear();
}

void CSG::PolygonBreakOnCreate(int index) { alloc_break_index = index; }
#elif
CSG::Polygon* CSG::PolygonCreate() { ++allocs; return new Polygon(); }
void CSG::PolygonDestroy(CSG::Polygon** p) { ++deallocs; delete *p; *p = 0; }
void CSG::PolygonMemLeakTest() { }
void CSG::PolygonBreakOnCreate(int index) { }
#endif

bool AABB_Intersect(CSG::Polygon* a, CSG::Polygon* b)
{
	return a->bb_min.x < b->bb_max.x + epsilon_edge &&
		a->bb_min.y < b->bb_max.y + epsilon_edge &&
		b->bb_min.x < a->bb_max.x + epsilon_edge &&
		b->bb_min.y < a->bb_max.y + epsilon_edge;
}

CsgScalar LinePointDistance(const Vec2& v, const Vec2& w, const Vec2& p)
{
	CsgScalar t = (p - v).Dot(w - v) / (w - v).Dot(w - v);

	if (t < 0) { return (p - v).Length(); }
	else if (t > 1) { return (p - w).Length(); }
	
	Vec2 proj = v + t * (w - v);
	return (p - proj).Length();
}

bool LineLineIntersection(const Vec2& u1, const Vec2& u2, const Vec2& v1, const Vec2& v2, CsgScalar& dt)
{
	CsgScalar d = ((v2.y - v1.y) * (u2.x - u1.x)) - ((v2.x - v1.x) * (u2.y - u1.y));

	if (fabs(d) < epsilon) { return false; }

	CsgScalar a = ((v2.x - v1.x) * (u1.y - v1.y)) - ((v2.y - v1.y) * (u1.x - v1.x));
	CsgScalar b = ((u2.x - u1.x) * (u1.y - v1.y)) - ((u2.y - u1.y) * (u1.x - v1.x));
	CsgScalar ua = a / d;
	CsgScalar ub = b / d;

	dt = ua;

	if (ua < 0 || ub < 0 || ua > 1 || ub > 1)
	{
		return false;
	}

	return true;
}

bool LineLineIntersection_Edge(const Vec2& u1, const Vec2& u2, const Vec2& v1, const Vec2& v2, CsgScalar& dt)
{
	CsgScalar d = ((v2.y - v1.y) * (u2.x - u1.x)) - ((v2.x - v1.x) * (u2.y - u1.y));

	if (fabs(d) < epsilon) { return false; }

	CsgScalar a = ((v2.x - v1.x) * (u1.y - v1.y)) - ((v2.y - v1.y) * (u1.x - v1.x));
	CsgScalar b = ((u2.x - u1.x) * (u1.y - v1.y)) - ((u2.y - u1.y) * (u1.x - v1.x));
	CsgScalar ua = a / d;
	CsgScalar ub = b / d;

	dt = ua;

	if (ua < -epsilon_edge || ub < -epsilon_edge || ua > 1 + epsilon_edge || ub > 1 + epsilon_edge)
	{
		return false;
	}

	return true;
}

Vec2 LinePlaneIntersection(const Vec2& A, const Vec2& B, const Vec2 origin, const Vec2& normal)
{
	CsgScalar adot = A.Dot(normal);
	CsgScalar bdot = B.Dot(normal);
	CsgScalar scale = (normal.Dot(origin) - adot) / (bdot - adot);
	return A + (scale * (B - A));
}

void CSG::Polygon::Build()
{
	ASSERT(pts.size() >= 3);
	ASSERT(IsConvex(pts));
	ASSERT(!IsDegenerate(pts));

	edges.clear();
	edges.reserve(pts.size());
	for (size_t i = 0; i < pts.size(); i++)
	{
		edges.push_back(IndexPair(i, i + 1 == pts.size() ? 0 : i + 1));
	}
	
	pt_n.clear();
	pt_n.reserve(pts.size());
	for (size_t i = 0; i < pts.size(); i++)
	{
		size_t j = i + 1 == pts.size() ? 0 : i + 1;
		Vec2 n = pts[j] - pts[i];
		n = Vec2(n.y, -n.x);
		n.Normalize();
		pt_n.push_back(n);
	}

	// Convex, clockwise ordering check
	for (size_t i = 0; i < pts.size(); i++)
	{
		Vec2 pt = pts[i];
		Vec2 n = pt_n[i];
		for (size_t j = 0; j < pts.size(); j++)
		{
			CsgScalar d = (pts[j] - pt).Dot(n);
			ASSERT(d < epsilon);
		}
	}

	bb_min = bb_max = pts[0];
	for (size_t i = 1; i < pts.size(); i++)
	{
		bb_min = Vec2::Minimize(pts[i], bb_min);
		bb_max = Vec2::Maximize(pts[i], bb_max);
	}
}

bool CSG::Polygon::ContainsPolygon(const Polygon* poly)
{
	for (std::vector<Vec2>::const_iterator it = poly->pts.begin();
		it != poly->pts.end(); ++it)
	{
		for (size_t i = 0; i < this->pts.size(); i++)
		{
			CsgScalar d = (*it - this->pts[i]).Dot(pt_n[i]);
			if (d > epsilon_edge) { return false; }
		}
	}
	return true;
}

bool CSG::Polygon::ContainsPolygon(const std::vector<Vec2T<CsgScalar>>& pts)
{
	for (std::vector<Vec2>::const_iterator it = pts.begin();
		it != pts.end(); ++it)
	{
		for (size_t i = 0; i < this->pts.size(); i++)
		{
			CsgScalar d = (*it - this->pts[i]).Dot(pt_n[i]);
			if (d > epsilon_edge) { return false; }
		}
	}
	return true;
}

bool CSG::Polygon::ContainsPoint(const Vec2T<CsgScalar>& pt)
{
	for (size_t i = 0; i < pts.size(); i++)
	{
		CsgScalar d = (pt - pts[i]).Dot(pt_n[i]);
		if (d > epsilon) { return false; }
	}
	return true;
}

bool CSG::Polygon::ContainsEdge(const Vec2T<CsgScalar>& a, const Vec2T<CsgScalar>& b)
{
	if (ContainsPoint(a)) { return true; }
	if (ContainsPoint(b)) { return true; }

	for (size_t i = 0; i < edges.size(); i++)
	{
		CsgScalar dt = 0;
		if (LineLineIntersection_Edge(pts[edges[i].i], pts[edges[i].j], a, b, dt)) 
		{ return true; }
	}

	return false;
}

bool CSG::Polygon::IsSplitOnPlane(Vec2T<CsgScalar>& pt, Vec2T<CsgScalar>& n)
{
	bool front = false;
	bool back = false;
	for (size_t i = 0; i < pts.size(); i++)
	{
		CsgScalar d = (pts[i] - pt).Dot(n);
		if (d > epsilon_edge) { front = true; }
		if (d < -epsilon_edge) { back = true; }
	}
	return front && back;
}

void CSG::Polygon::ClipAgainstPlane(Vec2T<CsgScalar>& pt, Vec2T<CsgScalar>& n, std::vector<Vec2T<CsgScalar>>& p)
{
	bool front[256];
	for (size_t i = 0; i < pts.size(); i++)
	{
		front[i] = (pts[i] - pt).Dot(n) > epsilon;
	}

	p.reserve(pts.size() * 2);
	for (size_t i = 0; i < pts.size(); i++)
	{
		bool a = front[i];
		bool b = front[(i + 1 == pts.size()) ? 0 : i + 1];
		if (a)
		{
			p.push_back(pts[i]);
		}
		if (a != b)
		{
			Vec2 local_pt = pt;
			Vec2 local_intersect_pt = LinePlaneIntersection(
				pts[i] - local_pt,
				pts[(i + 1 == pts.size()) ? 0 : i + 1] - local_pt,
				pt - local_pt, n);

			p.push_back(local_pt + local_intersect_pt);
		}
	}
}

void CSG::Polygon::SplitOnPlane(Vec2T<CsgScalar>& pt, Vec2T<CsgScalar>& n, std::vector<Vec2T<CsgScalar>>& pf, std::vector<Vec2T<CsgScalar>>& pb)
{
	ClipAgainstPlane(pt, n, pf);
	ClipAgainstPlane(pt, -n, pb);
}

bool CSG::SplitPolygon(std::vector<Polygon*>& poly, size_t index, Polygon* splitter, bool remove_if_contains = false)
{
	std::vector<Polygon*> split_list, new_list;
	split_list.reserve(poly.size() * 2);
	bool did_split = false;

	if (!splitter->ContainsPolygon(poly[index])) { split_list.push_back(poly[index]); }
	else { PolygonDestroy(&poly[index]); did_split = true; }

	std::vector<Vec2> f_points, b_points;

	for (size_t i = 0; i < splitter->edges.size(); i++)
	{
		new_list.clear();
		new_list.reserve(split_list.size() * 2);
		for (size_t j = 0; j < split_list.size(); j++)
		{
			if (!AABB_Intersect(split_list[j], splitter))
			{
				new_list.push_back(split_list[j]);
				continue;	
			}

			if (!split_list[j]->ContainsEdge(
				splitter->pts[splitter->edges[i].i], 
				splitter->pts[splitter->edges[i].j]))
			{
				new_list.push_back(split_list[j]);
				continue;
			}

			if (split_list[j]->IsSplitOnPlane(splitter->pts[i], splitter->pt_n[i]))
			{				
				did_split = true;	
				f_points.clear();
				b_points.clear();
				split_list[j]->SplitOnPlane(splitter->pts[i], splitter->pt_n[i], f_points, b_points);
				
				PolygonDestroy(&split_list[j]); split_list[j] = 0;

				if (IsDegenerate(f_points)) { f_points.clear(); }
				if (IsDegenerate(b_points)) { b_points.clear(); }

				if (remove_if_contains && b_points.size() >= 3)
				{
					if (splitter->ContainsPolygon(b_points)) 
					{
						b_points.clear(); 
					}
				}

				if (b_points.size() >= 3)
				{ 
					Polygon* pb = PolygonCreate();
					pb->pts = b_points; 
					pb->Build();
					new_list.push_back(pb);
				}

				if (f_points.size() >= 3)
				{ 
					Polygon* pf = PolygonCreate();
					pf->pts = f_points; 
					pf->Build();
					new_list.push_back(pf); 
				}
			}
			else
			{
				new_list.push_back(split_list[j]);
			}
		}
		split_list = new_list;
	}

	if (split_list.size() == 1)
	{
		if (poly[index] != split_list[0]) { poly[index] = split_list[0]; }
	}
	else
	{
		did_split = true;
		poly.erase(poly.begin() + index);
		for (size_t i = 0; i < split_list.size(); i++)
		{
			poly.push_back(split_list[i]);
		}
	}

	return did_split;
}

bool CSG::Polygon::IsAdjacent(Polygon* p)
{
	for (size_t i = 0; i < pts.size(); i++)
	{
		for (size_t j = 0; j < p->edges.size(); j++)
		{
			Vec2& v1 = p->pts[p->edges[j].i];
			Vec2& v2 = p->pts[p->edges[j].j];
			if (LinePointDistance(v1, v2, pts[i]) < epsilon_edge) { return true; }
		}
	}

	for (size_t i = 0; i < p->pts.size(); i++)
	{
		for (size_t j = 0; j < edges.size(); j++)
		{
			Vec2& v1 = pts[edges[j].i];
			Vec2& v2 = pts[edges[j].j];
			if (LinePointDistance(v1, v2, p->pts[i]) < epsilon_edge) { return true; }
		}
	}

	return false;
}

std::vector<CSG::IndexPair> CSG::Adjacency(std::vector<Polygon*>& poly)
{
	std::vector<CSG::IndexPair> pairs;
	for (size_t i = 0; i < poly.size(); i++)
	{
		for (size_t j = i + 1; j < poly.size(); j++)
		{
			if (!AABB_Intersect(poly[i], poly[j])) { continue; }
			if (poly[i]->IsAdjacent(poly[j])) { pairs.push_back(IndexPair(i, j)); }
		}
	}
	return pairs;
}

CsgScalar AngleBetween(CsgScalar x1, CsgScalar y1, CsgScalar x2, CsgScalar y2)
{
	CsgScalar t = 0;

	CsgScalar dx = x2 - x1;
	CsgScalar ax = abs(dx);
	CsgScalar dy = y2 - y1;
	CsgScalar ay = abs(dy);
	if (ax + ay == 0)
	{
		t = 360.0f / 9.0f;
	}
	else
	{
		t = dy / (ax + ay);
	}
	if (dx < 0)
	{
		t = 2 - t;
	}
	else if (dy < 0)
	{
		t = 4 + t;
	}
	return t * 90;
}

void CSG::ConvexHull(std::vector<Vec2>& points, std::vector<Vec2>& hull)
{
	hull.clear();
	if (points.size() < 3) { hull = points; return; }

	Vec2 start_pt = points[0];
	int start_pt_index = 0;
	for (size_t i = 0; i < points.size(); i++)
	{
		if (points[i].y < start_pt.y || (points[i].y == start_pt.y && points[i].x < start_pt.x))
		{
			start_pt = points[i];
			start_pt_index = i;
		}
	}

	hull.clear();
	hull.push_back(start_pt);
	points.erase(points.begin() + start_pt_index);

	CsgScalar angle = 0;
	while (true)
	{
		CsgScalar x = hull[hull.size() - 1].x;
		CsgScalar y = hull[hull.size() - 1].y;
		start_pt = points[0];
		CsgScalar best = 3600;
		start_pt_index = 0;
		for (size_t i = 0; i < points.size(); i++)
		{
			CsgScalar t = AngleBetween(x, y, points[i].x, points[i].y);
			if ((t >= angle) && (best > t))
			{
				best = t;
				start_pt = points[i];
				start_pt_index = i;
			}
		}

		CsgScalar first_angle = AngleBetween(x, y, hull[0].x, hull[0].y);
		if ((first_angle >= angle) && (best >= first_angle))
		{
			break;
		}

		hull.push_back(start_pt);
		points.erase(points.begin() + start_pt_index);
		angle = best;
		if (!points.size()) { break; }
	}
}

CsgScalar CSG::Area(std::vector<Vec2T<CsgScalar>>& pts)
{
	CsgScalar area = 0;
	for (int i = 0; i < pts.size(); i++)
	{
		int j = (i + 1 == pts.size()) ? 0 : i + 1;
		area += (pts[i].x + pts[j].x) * (pts[j].y - pts[i].y);
	}
	return abs(area * 0.5f);
}

bool CSG::IsConvex(std::vector<Vec2>& pts)
{
	if (pts.size() < 3) { return false; }

	for (size_t i = 0; i < pts.size(); i++)
	{
		size_t j = i + 1 == pts.size() ? 0 : i + 1;
		Vec2 n = pts[j] - pts[i];
		n = Vec2(n.y, -n.x);
		Vec2 pt = pts[i];

		for (size_t j = 0; j < pts.size(); j++)
		{
			CsgScalar d = (pts[j] - pt).Dot(n);
			if (d > epsilon) { return false; }
		}
	}

	return true;
}

bool CSG::IsDegenerate(std::vector<Vec2>& pts)
{
	if (pts.size() < 3) { return true; }

	for (size_t i = 0; i < pts.size(); i++)
	{
		size_t j = i + 1 == pts.size() ? 0 : i + 1;
		CsgScalar d = (pts[i] - pts[j]).LengthSq();
		if (d < epsilon_sq) { return true; }
	}

	return false;
}

void CSG::Simplify(std::vector<Polygon*>& poly)
{
	std::vector<CSG::IndexPair> adj_pairs = CSG::Adjacency(poly);

	for (int i = 0; i < adj_pairs.size(); i++)
	{
		std::vector<Vec2T<CsgScalar>> hull_src, hull;
		for (std::vector<Vec2>::iterator it = poly[adj_pairs[i].i]->pts.begin();
			it != poly[adj_pairs[i].i]->pts.end(); ++it)
		{
			hull_src.push_back(*it);
		}
		for (std::vector<Vec2>::iterator it = poly[adj_pairs[i].j]->pts.begin();
			it != poly[adj_pairs[i].j]->pts.end(); ++it)
		{
			hull_src.push_back(*it);
		}
		ConvexHull(hull_src, hull);
		
		for (int i = 0; i < hull.size(); i++)
		{
			int j = i + 1 == hull.size() ? 0 : i + 1;
			if ((hull[i] - hull[j]).LengthSq() < epsilon_sq)
			{
				hull.erase(hull.begin() + i--);
			}
		}
		
		CsgScalar a1 = Area(poly[adj_pairs[i].i]->pts) + Area(poly[adj_pairs[i].j]->pts);
		CsgScalar a2 = Area(hull);
	
		if (IsConvex(hull) && abs(a1 - a2) < epsilon)
		{
			poly[adj_pairs[i].i]->pts = hull;
			poly[adj_pairs[i].i]->Build();
			PolygonDestroy(&poly[adj_pairs[i].j]);
			poly.erase(poly.begin() + adj_pairs[i].j);
			adj_pairs = CSG::Adjacency(poly);
			i = -1;
			continue;
		}
	}
}

void CSG::Subtract(std::vector<Polygon*>& poly, Polygon* p)
{
	for (int i = 0; i < poly.size(); i++)
	{
		if (SplitPolygon(poly, i, p, true)) { i--; }
	}
	CSG::PolygonDestroy(&p);
}

void CSG::Add(std::vector<Polygon*>& poly, Polygon* p)
{
	size_t idx = poly.size();
	for (size_t i = 0; i < idx; i++)
	{
		if (poly[i]->ContainsPolygon(p))
		{
			CSG::PolygonDestroy(&p);
			return; 
		}
	}
	poly.push_back(p);
	for (size_t i = 0; i < idx; i++)
	{
		for (int j = idx; j < poly.size(); j++)
		{
			if (SplitPolygon(poly, j, poly[i], true)) { j--; }
		}
	}
}

Vec2 CSG::Polygon::center()
{
	Vec2 p(0, 0);
	CsgScalar count = 0;

	for (std::vector<Vec2>::iterator it = pts.begin(); it != pts.end(); ++it)
	{
		p += *it;
		++count;
	}

	if (count) { count = ((CsgScalar)1) / count; }
	p *= count;
	return p;
}

CsgScalar CSG::Polygon::ExtentMax()
{
	CsgScalar best = 0;
	for (size_t i = 0; i < pts.size(); i++)
	{
		for (size_t j = i + 1; j < pts.size(); j++)
		{
			CsgScalar d = (pts[j] - pts[i]).Dot(pt_n[i]);
			d = -d;
			if (d > best) { best = d; }
		}
	}
	return best;
}

CsgScalar CSG::Polygon::ExtentMin()
{
	CsgScalar best = -1;
	for (size_t i = 0; i < pts.size(); i++)
	{
		for (size_t j = i + 1; j < pts.size(); j++)
		{
			CsgScalar d = (pts[j] - pts[i]).Dot(pt_n[i]);
			d = -d;
			if (d < epsilon_edge) { continue; }
			if (d < best || best < 0) { best = d; }
		}
	}
	if (best < 0) { best = 0; }
	return best;
}

std::vector<float> CSG::RenderEdges(std::vector<Polygon*>& poly)
{
	std::vector<float> v;

	for (std::vector<Polygon*>::iterator ip = poly.begin(); ip != poly.end(); ++ip)
	{
		for (std::vector<CSG::IndexPair>::iterator it = (*ip)->edges.begin(); it != (*ip)->edges.end(); ++it)
		{
			Vec2& v1 = (*ip)->pts[it->i];
			Vec2& v2 = (*ip)->pts[it->j];
			v.push_back(v1.x); v.push_back(v1.y);
			v.push_back(v2.x); v.push_back(v2.y);

			//Vec2 mid = (v1 + v2) * 0.5f;
			//v.push_back(mid.x); v.push_back(mid.y);
			//mid += (*ip)->pt_n[it->i] * 0.2f;
			//v.push_back(mid.x); v.push_back(mid.y);
		}
	}

	return v;
}

std::vector<float> CSG::RenderTri(std::vector<Polygon*>& poly)
{
	std::vector<float> v;
	for (std::vector<Polygon*>::iterator ip = poly.begin(); ip != poly.end(); ++ip)
	{
		for (size_t i = 2; i < (*ip)->pts.size(); i++)
		{
			v.push_back((*ip)->pts[0].x); v.push_back((*ip)->pts[0].y);
			v.push_back((*ip)->pts[i - 1].x); v.push_back((*ip)->pts[i - 1].y);
			v.push_back((*ip)->pts[i].x); v.push_back((*ip)->pts[i].y);
		}
	}
	return v;
}

CSG::Edge CSG::TraceLine(std::vector<Polygon*>& poly, const Vec2T<CsgScalar>& start, const Vec2T<CsgScalar>& end, CSG::Polygon* skip_poly = 0)
{
	CSG::Edge e;
	e.a = start;
	e.b = end;

	CsgScalar best_dt = 1;

	for (std::vector<Polygon*>::iterator ip = poly.begin(); ip != poly.end(); ++ip)
	{
		if (*ip == skip_poly) { continue; }
		if ((*ip)->ContainsPoint(start)) { best_dt = 0; break; }

		for (std::vector<CSG::IndexPair>::iterator it = (*ip)->edges.begin(); it != (*ip)->edges.end(); ++it)
		{
			Vec2 v1 = (*ip)->pts[it->i];
			Vec2 v2 = (*ip)->pts[it->j];

			CsgScalar dt = 0;
			if (LineLineIntersection(e.a, e.b, v1, v2, dt))
			{
				if (dt < best_dt) { best_dt = dt; }
			}

		}
	}

	e.b = e.a + (e.b - e.a) * best_dt;
	return e;
}

CSG::Contour CSG::ExteriorContour(std::vector<Polygon*>& poly)
{
	CSG::Contour contour;
	for (std::vector<Polygon*>::iterator ip = poly.begin(); ip != poly.end(); ++ip)
	{
		for (std::vector<CSG::IndexPair>::iterator it = (*ip)->edges.begin(); it != (*ip)->edges.end(); ++it)
		{
			Vec2 v1 = (*ip)->pts[it->i] + epsilon_contour * (*ip)->pt_n[it->i];
			Vec2 v2 = (*ip)->pts[it->j] + epsilon_contour * (*ip)->pt_n[it->i];
			
			Vec2 tangent = v2 - v1;
			tangent.Normalize();
			tangent *= epsilon_contour;

			v1 += tangent;
			v2 -= tangent;

			CSG::Edge& trace_edge = TraceLine(poly, v1, v2, *ip);
			if ((trace_edge.b - trace_edge.a).LengthSq() > epsilon_sq)
			{ contour.edges.push_back(trace_edge); }

		}
	}

	return contour;
}