(*	Steven Skiena
	Computational Geometry Package
	11/6/90

		This is a package of various algorithms of interest in
	Computational Geometry.  Functions are included to construct convex
	hulls, Delauney triangulations, and Voronoi diagrams, and to perform
	point location and range queries.
  		This package can best be viewed as an extension of
	Combinatorica, since all two-dimensional geometric objects
	can be viewed as embedded graph structures.  Graph structures
	take the form Graph[e,v], where e is an adjacency matrix and v a
	list of the {x,y} coordinates of each vertex.  Functions are
	also included to construct random point sets and simple polygons.
*) 

(*	DONE:

		ConvexHull
		RandomPointSet
		SimplePolygon
		PointLocation
		PlanarSweep
		ConvexQ
		RangeQuery
		OrthogonalRangeQuery
		SurfaceInterpolation
		DelaunayTriangulation
		VoronoiDiagram
*)


(*	Constructing Geometric Objects: RandomPointSet and SimplePolygon

		These functions construct random instances of simple
	geometric objects, to serve as test data and examples for the
	more sophisticated functions in the package.
*)

RandomPointSet::usage = "RandomPointSet[n] returns n randomly selected points from the unit square.  RandomPointSet[n,{xlow,xhigh},{ylow,yhigh}] selects random points from the specified rectangle."

RandomPointSet[n_Integer] := RandomPointSet[n,{0,1},{0,1}]

RandomPointSet[n_Integer,xrange_List,yrange_List] :=
	ChangeVertices[
		EmptyGraph[n],
		Table[{Random[Real,xrange],Random[Real,yrange]}, {n}]
	]

SimplePolygon::usage = "SimplePolygon[v] connects the vertices of graph or list v to form a simple, non-intersecting polygon."

SimplePolygon[Graph[_,v_]] := SimplePolygon[v]

SimplePolygon[v_List] :=
	Block[{start, vnew},
                start = MinimumPointSelect[v,Last];
		vnew = Map[({Apply[ARCTAN,#-start],#})&, Complement[v,start]];
                ChangeVertices[
			Cycle[Length[v]],
			Join[{start}, Drop[Map[Last, Sort[vnew]], -1]]
		]
	]

(*      Finding Convex Hulls: ConvexHull and GrahamScan

		These functions construct the convex hull of a set of
	points using the Graham scan algorithm.  The Graham scan sorts
	the points in angular order around one hull vertex, and constructs
	the hull by inserting the points in this order.  On each insertion,
	points are deleted from the hull until no reflex angles remain.
*)

ConvexHull::usage = "ConvexHull[g] constructs the convex hull of the vertices ofg.  ConvexHull[g,All] also returns the non-hull vertices."

GrahamScan[Graph[_,v_]] :=
	Block[{start, vnew, hull={}, n},
		start = MinimumPointSelect[v,Last];
		vnew = Map[({Apply[ARCTAN,#-start],#})&, Complement[v,start]];
		vnew = Map[Last, Sort[vnew]];
		hull = Join[{start}, Take[vnew,2]];
		Scan[
			(While [CCW[hull[[-1]],hull[[-2]],#] >= 0,
				hull = Drop[hull,-1]
			 ];
			 AppendTo[hull,#] )&,
			vnew
		];
		If [start == Last[hull], hull = Drop[hull,-1]];
		n = Length[hull];
		FromUnorderedPairs[
                        Join[Partition[Range[n],2,1], {{1,n}}],
                        hull
                ]
	]

ConvexHull[g_Graph] := GrahamScan[g]

ConvexHull[g_Graph,All] := 
	Block[{n, hull = Vertices[GrahamScan[g]]},
		n = Length[hull];
		FromUnorderedPairs[
                        Join[Partition[Range[n],2,1], {{1,n}}],
                        Join[hull, Complement[Vertices[g],hull]]
                ]
	]


(*	Testing Convexity: CCW, ConvexPolygonQ, and LineSegmentsIntersectQ

	A function to accurately test whether three successive points form
	a clockwise or counterclockwise turn has severall applications.
	Insuring correctness in a CCW test is a surprisingly tricky task, and
	our implementation is adapted from Sedgewick's "Algorithms in C".

	In a convex polygon, all external angles are > 180 degrees.  Walking
	around such a polygon, the next point will always be to the left
	of the supporting line defined by each edge of the polygon, if
	we traverse it in a counter-clockwise fashion, and to the right on
	a clockwise traversal.  Another application is line segment
	intersection testing.
*)

CCW::usage = "CCW[p0,p1,p2] returns 1 if the three points describe a counter-clockwise turn, -1 if the turn is clockwise, and 0 if the three points are collinear"

CCW[p0_,p1_,p2_] :=
	Block[{d1=p1-p0, d2=p2-p1},
		If [d1[[1]]*d2[[2]] > d1[[2]]*d2[[1]], Return[1]];
		If [d1[[1]]*d2[[2]] < d1[[2]]*d2[[1]], Return[-1]];
		If [(d1[[1]]*d2[[1]]<0) || (d1[[2]]*d2[[2]]<0), Return[-1]];
		If [(d1[[1]]^2+d1[[2]]^2) < (d2[[1]]^2+d2[[2]]^2), Return[1]];
		Return[0]
	]

ConvexPolygonQ::usage = "ConvexPolygonQ[g] returns true if the graph
or list of vertices g is ordered to form a convex polygon."

ConvexPolygonQ[g_Graph] := ConvexPolygonQ[Vertices[g]]

(*ADD FIND CYCLE TESTING FOR CONVEXPOLYGONQ *)

ConvexPolygonQ[v1_List] :=
	Block[{v = Join[v1,Take[v1,3]]},
		Apply[Equal, Map[(Apply[CCW,#])&, Partition[v,3,1]]]
	]

LineSegmentsIntersectQ::usage = "LineSegmentsIntersectQ[{p1,p2},{q1,q2}] returns True iff the line segments defined by the pairs of points intersect."

LineSegmentsIntersectQ[{p1_,p2_},{q1_,q2_}] :=
	(CCW[p1,p2,q1] * CCW[p1,p2,q2] <= 0) &&
	(CCW[q1,q2,p1] * CCW[q1,q2,p2] <= 0)


(*	Point placement queries: InsideQ, QuickOutsideQ, InsideConvexQ

	The intricacy of testing whether a point is within a within a
	given polygon depends upon its representation and convexity.
	A quick way of testing whether a point is outside a polygon is
	to test if it lies within the orthogonal rectangle defined by
	its maximum and minimum x- and y-coordinates.  For all polygons,
	if a line through the query point intersects an even number of
	edges before reaching the query point, it is outside the polygon,
	otherwise it is inside.  Care must be taken with degenerate cases,
	such as the line passing through vertices and horizontal edges.

	For convex polygons, a simpler algorithm is testing whether the
	point lies on the same side of each edge as we walk around the
	polygon in a clockwise or counter-clockwise order.

	If a polygon has an implicit representation, where the vertices
	are listed in the graph structure succesive order, the adjacency
	matrix does not have to touched.  InsideConvexQ takes advantage
	of this optimization, and hence takes a list of vertices instead of
	a graph structure to represent the polygon.
*)

QuickOutsideQ[{xp_,yp_},v_List] :=
	Block[{x = Map[First,v], y = Map[Last,v]},
		!( (Min[x] <= xp <= Max[x]) && (Min[y] <= yp <= Max[y]) )
	]

InsideConvexQ::usage = "InsideConvexQ[p,v] tests whether point p lies inside the convex polygon represented by the list of vertices v."

InsideConvexQ[p_,v1_List] :=
	Block[{v},
		If [QuickOutside[p,v1], Return[False]];
		v = Append[v1,First[v1]];
                Apply[Equal, Map[(CCW[#[[1]],#[[2]],p])&, Partition[v,2,1]]]
        ]

InsideQ::usage = "InsideQ[p,g] tests whether point p lies within simple g."

InsideQ[p_,poly_Graph] := False	/; QuickOutsideQ[p,Vertices[poly]]

InsideQ[p_,poly_Graph] :=
	Block[{p1, v, g, i, j=1, count=0},
		g = If [OrderedCycleQ[poly], poly, OrderCycle[poly]];
		v = Vertices[g];
		p1 = {p[[1]], Max[v]+1};
		v = Join[{Last[v]}, v, {First[v]}];
		Do [
			If [!LineSegmentsIntersectQ[{v[[i]],v[[i]]},{p,p1}],
			    If [LineSegmentsIntersectQ[{v[[i]],v[[j]]},{p,p1}],
					count++
			    ];
			    j = i;
			],
			{i,2,V[poly]+1}
		];
		OddQ[count]
	]


(*	Permuting the vertices of a polygon so that the (i)th vertex is
	adjacent to the (i+1)st is a fairly clumsy operation.  The
	OrderedCycleQ predicate is significantly faster than reordering
	a preordered polygon with OrderCycle.
*)

OrderCycle[p_Graph] :=
	Block[{edges = ToAdjacencyLists[p], start, end, last=1, cycle},
	 	{start, end} = edges[[1]];	
		cycle = {end};
		While [start != last,
			If [edges[[end,1]] != last,
				{last, end} = {end, edges[[end,1]]},
				{last, end} = {end, edges[[end,2]]}
			];
			AppendTo[cycle,end]
		];
		InduceSubgraph[p,cycle]
	]

OrderedCycleQ[Graph[e_,_]] :=
	Apply[
		And,
		Map[
			(e[[#[[1]]]][[#[[2]]]] != 0)&,
			Partition[ Append[Range[Length[e]],1], 2, 1]
		]
	]

(*	Range Queries: OrthogonalRangeQuery and RangeQuery

	A range query identifies which of a set of points lies within
	a given polygon.  Range queries can be answered by repeated point
	location queries, although orthogonal range queries, which
	return the points within a given rectangle are simpler and more
	efficient.
*)

OrthogonalRangeQuery::usage = "OrthogonalRangeQuery[g,{xmin,xmax},{ymin,ymax}] identies which points of graph g lie within the rectangle defined by xmin, xmax, ymin, and ymax."

OrthogonalRangeQuery[Graph[_,v_], {xmin_,xmax_}, {ymin_,ymax_}] :=
	Select[ v, ((xmin <= #[[1]] <= ymax) && (ymin <= #[[2]] <= ymax))& ]


RangeQuery::usage = "RangeQuery[g,p] identifies the vertices of graph p which lie within polygon g."

RangeQuery[p_Graph,Graph[_,v_]] :=
	Block[{poly},
		poly = If[ OrderedCycleQ[p], p, OrderCycle[p] ];
		Select[ v, (InsideQ[#,poly])&]
	]


(*	Constructing Planar Subdivisions: PlanarSweep

	Given a convex planar subdivision, such as a triangulation, it is
	often necessary to identify the regions defined by the subdivision,
	in this case triangular faces.  The algorithm for doing this is
	a planar sweep, where a vertical line starting at x = -Infinity
	sweeps to the left, stoping at each vertex of the subdivision.
	At each vertex, at least one region comes to an end and another
	begins.

	Note that this implementation does not correctly treat subdivisions
	with vertical lines.
*)

PlanarSweep::usage = "PlanarSweep[p] returns a list of all regions in the finite planar convex subdivision p."

PlanarSweep[p_Graph] :=
        Block[{queue=SortXAxis[p],frontier,v,e,l,r,s,top,bottom,regions={}},
                e = ToAdjacencyLists[p];
                {v,queue} = {First[queue], Rest[queue]};
                frontier = InitializeFrontier[v,p,e[[v]]];
                While[queue != {},
                        {v, queue} = {First[queue], Rest[queue]};
                        {l,s,r} = LeftSameRight[p,v,e[[v]]];
                        If [s =!= {}, Print["Vertical Line in Point Set"]];
                        top = First[ Position[frontier,v] ] [[1]];
                        bottom = Last[ Position[frontier,v] ] [[1]];
                        regions = Join[
                                regions,
                                Map[
                                        (Append[#,v])&,
                                        Take[frontier,{top+1,bottom-1}]
                                ]
                        ];
                        If[ queue =!= {}, frontier = Join[
                                Take[frontier,{1,top-1}],
                                {Append[frontier[[top]],First[r]]},
                                NewRegions[v,p,r],
                                {Append[frontier[[bottom]],Last[r]]},
                                Take[frontier,{bottom+1,Length[frontier]}]
                        ] ];
                ];
		Map[(OrderRegion[#,e])&, regions]
	]

OrderRegion[region_,e_List] :=
	Block[{ru = Union[region], r},
		r = Intersection[ru, e[[First[ru]]] ];
		r = {First[r],First[ru],Last[r]};
		ru = Complement[ru,r];
		While[ (v = Intersection[e[[Last[r]]], ru]) =!= {},
			AppendTo[r,First[v]];
			ru = Complement[ru,v];
		];
		r
	]


(*	Partition the vertices of points according to whether they
	are to the left, right or have the same x-coordinate as z.
	Left and right points are ordered by angle about z.
*)
	
LeftSameRight[Graph[_,v_],z_Integer,points_] :=
	Block[{l = s = r = {}, o=v[[z]]},
		Map[
			(If [v[[z,1]]==v[[#,1]], AppendTo[s,#],
			 If [v[[z,1]]<v[[#,1]], AppendTo[r,#], AppendTo[l,#]]])&,
		     	points
		];
		{Sort[l, (Apply[ARCTAN,v[[#1]]-o] < Apply[ARCTAN,v[[#2]]-o])&],
		 s,
		 Sort[r, (Apply[ArcTan,v[[#1]]-o] > Apply[ArcTan,v[[#2]]-o])&]}
	]

(*	Let x be the leftmost vertex, and the ones it is adjacent to be
	the list adj.
*)

InitializeFrontier[x_,Graph[_,v_],adj_] :=
	Block[{l},
		l = Sort[ Map[({Apply[ArcTan,v[[#]]-v[[x]]],#})&, adj] ];
		l = Reverse[ Map[Last,l] ];
		Join[
			{{x,First[l]}},
			Map[({#[[1]],x,#[[2]]})&, Partition[l,2,1]],
			{{x,Last[l]}}
		]
	]

NewRegions[x_,Graph[_,v_],adj_] :=
	Block[{l},
                l = Sort[ Map[({Apply[ArcTan,v[[#]]-v[[x]]],#})&, adj] ];
		Map[
			({#[[1]],x,#[[2]]})&,
                	Partition[ Reverse[ Map[Last,l] ], 2, 1]
		]
        ]

SortXAxis[Graph[_,v_]] := Sort[ Range[Length[v]], (v[[#1,1]] <= v[[#2,1]])& ]
SortYAxis[Graph[_,v_]] := Sort[ Range[Length[v]], (v[[#1,2]] <= v[[#2,2]])& ]

SortOrigin[g_Graph,origin_] := SortOrigin[g,origin,Range[V[g]]] 
SortOrigin[Graph[_,v_],o_,points_List] := 
	Sort[ points, (Apply[ARCTAN,v[[#1]]-o] <= Apply[ARCTAN,v[[#2]]-o])& ]


(*	Planar Point Location:	PointLocation

	Given a convex planar subdivision, we are often interested in
	identifying which region a query point lies in.  This operation
	is faster when the subdivision has been precomputed.
*)

PointLocation::usage = "PointLocation[p,g,sub] identifies which region point p lies in the convex subdivision sub associated with graph g.  PointLocation[p,g] computes the associated subdivision."

PointLocation[p_,poly_Graph] := PointLocation[p,poly,PlanarSweep[poly]]

PointLocation[p_,Graph[_,v_],subdiv_List] :=
	Block[{i},
		For [i=1, i<=Length[subdiv], i++,
			If[ InsideConvexQ[p, v[[ subdiv[[i]] ]] ],
				Return[subdiv[[i]]]
			]
		];
		{}
	]


(*	Point set Triangulation: Triangulation

	Construct a triangulation of a set of points, by constructing the
	convex hull and repeatedly doing point location on the subdivision
	with the non-hull points.  Each point is then located within a
	convex region, which is then triangulated.
*)

Triangulation::usage = "Triangulation[g] constructs a triangulation of point set g."

Triangulation[g_Graph] :=
	Block[{p,triangles,rest,v,hull,tri},
		p = ConvexHull[g];
		v = Join[ Vertices[p], Complement[Vertices[g],Vertices[p]] ];
		hull = Range[Length[Vertices[p]]];
		triangles = Map[
			(Append[#,First[hull]])&,
			 Partition[Rest[hull],2,1]
		];
		p = ChangeVertices[g,v];
		Scan[
			(tri = PointLocation[v[[#]],p,triangles];
			 triangles = Join[ Complement[triangles,{tri}],
				{ {tri[[1]],tri[[2]],#}, {tri[[2]],tri[[3]],#},
				  {tri[[3]],tri[[1]],#} }
			 ])&,
			Complement[Range[V[g]],hull]
		];
		SubdivisionToGraph[triangles,v]
	]

SubdivisionToGraph[subdivision_List,v_List] :=
	FromUnorderedPairs[
		Apply[
			Join,
			Map[
				(Append[Partition[#,2,1],{Last[#],First[#]}])&,
				subdivision
			]
		],
		v
	]

(*	Constructing Delaunay Triangulations: DelaunayTriangulation

	Of all possible triangulations of a point set, the Delaunay
	triangulation maximizes the minimum angle, and so is prized
	for avoiding long, skinny triangles.  The property lets us
	construct them, for the Delaunay triangulation can be constructed
	through a sequence of edge flip operations, where each flip
	increases the lexicographically ordered angle vector of the 
	two triangles it lies between.
*)

DelaunayTriangulation::usage = "DelaunayTriangulation[g] constructs the
Delaunay triangulation of point set g."

DelaunayTriangulation[v_List] := 
	DelaunayTriangulation[ Graph[IdentityMatrix[Length[v]],v] ]

DelaunayTriangulation[g_Graph] :=
	Block[{t = Triangulation[g], e, v, l, r, done=False, vl,x,y},
		e = ToAdjacencyLists[t];
		v = Vertices[t];
		While [!done,
			done = True;
			Scan [
				({l,r} = NeighboringFacesTest[#,t,e];
				 If [(l =!= {}) && (r =!= {}),
				 	{x,y} = #;
				 	vl = v[[ {x,l,y,r} ]];
				 	If [Apply[FlipEdgeQ,vl] &&
							ConvexPolygonQ[vl],
						e[[x]] = Complement[e[[x]],{y}];
						e[[y]] = Complement[e[[y]],{x}];
						e[[l]] = Append[e[[l]],r];
						e[[r]] = Append[e[[r]],l];
						done = False;
					]
				 ])&,
				 ToUnorderedPairs[t]
			];
			t = FromAdjacencyLists[e,v]
		];
		t
	]

Angle[p1_,p2_,p3_] :=
	Block[{a},
		a = Abs[ Apply[ARCTAN,p1-p2] - Apply[ARCTAN,p3-p2] ];
		If [a > N[Pi], N[2 Pi - a], a]
	]

LexicographicallySmallerVector[{},{}] := False

LexicographicallySmallerVector[v1_List,v2_List] :=
	If [First[v1] < First[v2], True,
		If [First[v1] > First[v2], False,
			LexicographicallySmallerVector[Rest[v1],Rest[v2]]
		]
	]

(*	Identify the two vertices defining the triangles to the left
	and right of edge {v1,v2}.
*)

NeighboringFacesTest[{v1_Integer,v2_Integer},Graph[_,v_],e_List]:=
	Block[{left,right},
		left = Select[e[[v2]], (CCW[v[[v1]],v[[v2]],v[[#]]] == 1)&];
		right = Complement[e[[v2]], Append[left,v1]];
		{ If [left == {}, {},
			Sort[ Map[({Angle[v[[v1]],v[[v2]],v[[#]]],#})&, left]
			] [[1,2]] ],
		  If [right == {}, {},
               		Sort[ Map[({Angle[v[[v1]],v[[v2]],v[[#]]],#})&, right]
			] [[1,2]] ]
		}
	]

NeighboringFaces[g_Graph] :=
	Block[{e = ToAdjacencyLists[g]},
		Map[({#, NeighboringFacesTest[#,g,e]})&, ToUnorderedPairs[g]]
	]


(*	Test if the edge {e1,e2} should be in the Delaunay triangulation
	instead fliping it to {p1,p2}.
*)

FlipEdgeQ[e1_,p1_,e2_,p2_] :=
	Block[{current,new},
		current = Sort[
			{Angle[p1,e1,e2], Angle[e1,e2,p1], Angle[e2,p1,e1],
			 Angle[e1,e2,p2], Angle[p2,e1,e2], Angle[e2,p2,e1]}
		];
		new = Sort[
			{Angle[p1,e2,p2], Angle[e2,p2,p1], Angle[p2,p1,e2],
			 Angle[p1,e1,p2], Angle[e1,p2,p1], Angle[p2,p1,e1]}
		];
		LexicographicallySmallerVector[current,new]
	]



(*	3D Surface Interpolation:	SurfaceInterpolation

	To perform a 3D surface interpolation, construct the Delaunay
	triangulation of a two-dimensional projection of the points,
	and project up along the third dimension.  This function constructs
	and draws such a surface.
*)

SurfaceInterpolation::usage = "SurfaceInterpolation[v] displays a 3D surface passing through each of the points in v."

SurfaceInterpolation[Graph[_,v_]] := SurfaceInterpolation[v]

SurfaceInterpolation[v_List] :=
	Show[ Graphics3D[
		Map[
			(Polygon[ v[[#]] ])&,
			PlanarSweep[DelaunayTriangulation[Map[(Take[#,2])&,v]]]
		]
	] ]


(*	Constructing Voronoi diagrams:	DualTriangulation and VoronoiDiagram

	The Voronoi Diagram of a set of points S partitions the plane
	into regions in which all points are closest to the same point
	of S.  Voronoi diagrams can be constructed by taking the dual
	of the Delaunay triagulation.
*)

DualTriangulation::usage = "DualTriangulation[g] constructs the dual of the triangulation g.  DualTriangulation[g,t] uses t as the subdivision defined by g."

DualTriangulation[g_Graph] := DualTriangulation[g,PlanarSweep[g]]

DualTriangulation[Graph[_,v_],triangles_] :=
	Block[{i,j,g, m = Length[v]+Length[triangles]},
		g = Table[0, {i,1,m}, {j,1,m}];
		Do [
			If [Length[ Intersection[triangles[[i]],
				triangles[[j]]] ] == 2,
					g[[i,j]] = g[[j,i]] = 1
			],
			{i,Length[triangles]}, {j,Length[triangles]}
		];
		Graph[ g, Join[Map[(DualPoint[v[[#]]])&,triangles], v] ]
	]

VoronoiDiagram::usage = "VoronoiDiagram[g] constructs the Voronoi diagram of point set g."

VoronoiDiagram[g_Graph] := DualTriangulation[DelaunayTriangulation[g]]

DualPoint[{{x1_,y1_},{x2_,y2_},{x3_,y3_}}] :=
	Block[{m1,b1,m2,b2,x},
		If [(y1 - y2) != 0,
			{m1,b1} = PerpendicularBisector[{x1,y1},{x2,y2}];
			If [(y2 - y3) != 0,
				{m2,b2}=PerpendicularBisector[{x2,y2},{x3,y3}],
				{m2,b2}=PerpendicularBisector[{x1,y3},{x3,y3}]
			],
			{m1,b1}=PerpendicularBisector[{x2,y2},{x3,y3}];
                        {m2,b2}=PerpendicularBisector[{x1,y3},{x3,y3}];
		];
		x = (b2-b1)/(m1-m2);
		{x, m1 x + b1}
	]

PerpendicularBisector[{x1_,y1_},{x2_,y2_}] :=
	Block[{m,b},
		m = (x2-x1)/(y1-y2);
		b = (y1+y2)/2 - m (x1+x2)/2;
		{m,b}
	]

(*	Utility functions 	*)

ARCTAN[x_,y_] := 
	Block[{theta = ARCTAN1[Chop[x],Chop[y]]},
		If [theta < 0, N[2 Pi + theta], theta]
	]

ARCTAN1[0,0] := Infinity
ARCTAN1[x_,y_] := ArcTan[x,y]

MinimumPointSelect[v_List,property_] :=
	v [[ First[Position[ Map[property,v], Min[ Map[property,v] ] ]] ]] [[1]]

