1057 lines
33 KiB
C#
1057 lines
33 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics;
|
|
using Microsoft.Xna.Framework;
|
|
|
|
namespace FarseerPhysics.Common.Decomposition
|
|
{
|
|
//From the Poly2Tri project by Mason Green: http://code.google.com/p/poly2tri/source/browse?repo=archive#hg/scala/src/org/poly2tri/seidel
|
|
|
|
/// <summary>
|
|
/// Convex decomposition algorithm based on Raimund Seidel's paper "A simple and fast incremental randomized
|
|
/// algorithm for computing trapezoidal decompositions and for triangulating polygons"
|
|
/// See also: "Computational Geometry", 3rd edition, by Mark de Berg et al, Chapter 6.2
|
|
/// "Computational Geometry in C", 2nd edition, by Joseph O'Rourke
|
|
/// </summary>
|
|
public static class SeidelDecomposer
|
|
{
|
|
/// <summary>
|
|
/// Decompose the polygon into several smaller non-concave polygon.
|
|
/// </summary>
|
|
/// <param name="vertices">The polygon to decompose.</param>
|
|
/// <param name="sheer">The sheer to use. If you get bad results, try using a higher value. The default value is 0.001</param>
|
|
/// <returns>A list of triangles</returns>
|
|
public static List<Vertices> ConvexPartition(Vertices vertices, float sheer)
|
|
{
|
|
List<Point> compatList = new List<Point>(vertices.Count);
|
|
|
|
foreach (Vector2 vertex in vertices)
|
|
{
|
|
compatList.Add(new Point(vertex.X, vertex.Y));
|
|
}
|
|
|
|
Triangulator t = new Triangulator(compatList, sheer);
|
|
|
|
List<Vertices> list = new List<Vertices>();
|
|
|
|
foreach (List<Point> triangle in t.Triangles)
|
|
{
|
|
Vertices verts = new Vertices(triangle.Count);
|
|
|
|
foreach (Point point in triangle)
|
|
{
|
|
verts.Add(new Vector2(point.X, point.Y));
|
|
}
|
|
|
|
list.Add(verts);
|
|
}
|
|
|
|
return list;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Decompose the polygon into several smaller non-concave polygon.
|
|
/// </summary>
|
|
/// <param name="vertices">The polygon to decompose.</param>
|
|
/// <param name="sheer">The sheer to use. If you get bad results, try using a higher value. The default value is 0.001</param>
|
|
/// <returns>A list of trapezoids</returns>
|
|
public static List<Vertices> ConvexPartitionTrapezoid(Vertices vertices, float sheer)
|
|
{
|
|
List<Point> compatList = new List<Point>(vertices.Count);
|
|
|
|
foreach (Vector2 vertex in vertices)
|
|
{
|
|
compatList.Add(new Point(vertex.X, vertex.Y));
|
|
}
|
|
|
|
Triangulator t = new Triangulator(compatList, sheer);
|
|
|
|
List<Vertices> list = new List<Vertices>();
|
|
|
|
foreach (Trapezoid trapezoid in t.Trapezoids)
|
|
{
|
|
Vertices verts = new Vertices();
|
|
|
|
List<Point> points = trapezoid.Vertices();
|
|
foreach (Point point in points)
|
|
{
|
|
verts.Add(new Vector2(point.X, point.Y));
|
|
}
|
|
|
|
list.Add(verts);
|
|
}
|
|
|
|
return list;
|
|
}
|
|
}
|
|
|
|
internal class MonotoneMountain
|
|
{
|
|
private const float PiSlop = 3.1f;
|
|
public List<List<Point>> Triangles;
|
|
private HashSet<Point> _convexPoints;
|
|
private Point _head;
|
|
|
|
// Monotone mountain points
|
|
private List<Point> _monoPoly;
|
|
|
|
// Triangles that constitute the mountain
|
|
|
|
// Used to track which side of the line we are on
|
|
private bool _positive;
|
|
private int _size;
|
|
private Point _tail;
|
|
|
|
// Almost Pi!
|
|
|
|
public MonotoneMountain()
|
|
{
|
|
_size = 0;
|
|
_tail = null;
|
|
_head = null;
|
|
_positive = false;
|
|
_convexPoints = new HashSet<Point>();
|
|
_monoPoly = new List<Point>();
|
|
Triangles = new List<List<Point>>();
|
|
}
|
|
|
|
// Append a point to the list
|
|
public void Add(Point point)
|
|
{
|
|
if (_size == 0)
|
|
{
|
|
_head = point;
|
|
_size = 1;
|
|
}
|
|
else if (_size == 1)
|
|
{
|
|
// Keep repeat points out of the list
|
|
_tail = point;
|
|
_tail.Prev = _head;
|
|
_head.Next = _tail;
|
|
_size = 2;
|
|
}
|
|
else
|
|
{
|
|
// Keep repeat points out of the list
|
|
_tail.Next = point;
|
|
point.Prev = _tail;
|
|
_tail = point;
|
|
_size += 1;
|
|
}
|
|
}
|
|
|
|
// Remove a point from the list
|
|
public void Remove(Point point)
|
|
{
|
|
Point next = point.Next;
|
|
Point prev = point.Prev;
|
|
point.Prev.Next = next;
|
|
point.Next.Prev = prev;
|
|
_size -= 1;
|
|
}
|
|
|
|
// Partition a x-monotone mountain into triangles O(n)
|
|
// See "Computational Geometry in C", 2nd edition, by Joseph O'Rourke, page 52
|
|
public void Process()
|
|
{
|
|
// Establish the proper sign
|
|
_positive = AngleSign();
|
|
// create monotone polygon - for dubug purposes
|
|
GenMonoPoly();
|
|
|
|
// Initialize internal angles at each nonbase vertex
|
|
// Link strictly convex vertices into a list, ignore reflex vertices
|
|
Point p = _head.Next;
|
|
while (p.Neq(_tail))
|
|
{
|
|
float a = Angle(p);
|
|
// If the point is almost colinear with it's neighbor, remove it!
|
|
if (a >= PiSlop || a <= -PiSlop || a == 0.0)
|
|
Remove(p);
|
|
else if (IsConvex(p))
|
|
_convexPoints.Add(p);
|
|
p = p.Next;
|
|
}
|
|
|
|
Triangulate();
|
|
}
|
|
|
|
private void Triangulate()
|
|
{
|
|
while (_convexPoints.Count != 0)
|
|
{
|
|
IEnumerator<Point> e = _convexPoints.GetEnumerator();
|
|
e.MoveNext();
|
|
Point ear = e.Current;
|
|
|
|
_convexPoints.Remove(ear);
|
|
Point a = ear.Prev;
|
|
Point b = ear;
|
|
Point c = ear.Next;
|
|
List<Point> triangle = new List<Point>(3);
|
|
triangle.Add(a);
|
|
triangle.Add(b);
|
|
triangle.Add(c);
|
|
|
|
Triangles.Add(triangle);
|
|
|
|
// Remove ear, update angles and convex list
|
|
Remove(ear);
|
|
if (Valid(a))
|
|
_convexPoints.Add(a);
|
|
if (Valid(c))
|
|
_convexPoints.Add(c);
|
|
}
|
|
|
|
Debug.Assert(_size <= 3, "Triangulation bug, please report");
|
|
}
|
|
|
|
private bool Valid(Point p)
|
|
{
|
|
return p.Neq(_head) && p.Neq(_tail) && IsConvex(p);
|
|
}
|
|
|
|
// Create the monotone polygon
|
|
private void GenMonoPoly()
|
|
{
|
|
Point p = _head;
|
|
while (p != null)
|
|
{
|
|
_monoPoly.Add(p);
|
|
p = p.Next;
|
|
}
|
|
}
|
|
|
|
private float Angle(Point p)
|
|
{
|
|
Point a = (p.Next - p);
|
|
Point b = (p.Prev - p);
|
|
return (float)Math.Atan2(a.Cross(b), a.Dot(b));
|
|
}
|
|
|
|
private bool AngleSign()
|
|
{
|
|
Point a = (_head.Next - _head);
|
|
Point b = (_tail - _head);
|
|
return Math.Atan2(a.Cross(b), a.Dot(b)) >= 0;
|
|
}
|
|
|
|
// Determines if the inslide angle is convex or reflex
|
|
private bool IsConvex(Point p)
|
|
{
|
|
if (_positive != (Angle(p) >= 0))
|
|
return false;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
// Node for a Directed Acyclic graph (DAG)
|
|
internal abstract class Node
|
|
{
|
|
protected Node LeftChild;
|
|
public List<Node> ParentList;
|
|
protected Node RightChild;
|
|
|
|
protected Node(Node left, Node right)
|
|
{
|
|
ParentList = new List<Node>();
|
|
LeftChild = left;
|
|
RightChild = right;
|
|
|
|
if (left != null)
|
|
left.ParentList.Add(this);
|
|
if (right != null)
|
|
right.ParentList.Add(this);
|
|
}
|
|
|
|
public abstract Sink Locate(Edge s);
|
|
|
|
// Replace a node in the graph with this node
|
|
// Make sure parent pointers are updated
|
|
public void Replace(Node node)
|
|
{
|
|
foreach (Node parent in node.ParentList)
|
|
{
|
|
// Select the correct node to replace (left or right child)
|
|
if (parent.LeftChild == node)
|
|
parent.LeftChild = this;
|
|
else
|
|
parent.RightChild = this;
|
|
}
|
|
ParentList.AddRange(node.ParentList);
|
|
}
|
|
}
|
|
|
|
// Directed Acyclic graph (DAG)
|
|
// See "Computational Geometry", 3rd edition, by Mark de Berg et al, Chapter 6.2
|
|
internal class QueryGraph
|
|
{
|
|
private Node _head;
|
|
|
|
public QueryGraph(Node head)
|
|
{
|
|
_head = head;
|
|
}
|
|
|
|
private Trapezoid Locate(Edge edge)
|
|
{
|
|
return _head.Locate(edge).Trapezoid;
|
|
}
|
|
|
|
public List<Trapezoid> FollowEdge(Edge edge)
|
|
{
|
|
List<Trapezoid> trapezoids = new List<Trapezoid>();
|
|
trapezoids.Add(Locate(edge));
|
|
int j = 0;
|
|
|
|
while (edge.Q.X > trapezoids[j].RightPoint.X)
|
|
{
|
|
if (edge.IsAbove(trapezoids[j].RightPoint))
|
|
{
|
|
trapezoids.Add(trapezoids[j].UpperRight);
|
|
}
|
|
else
|
|
{
|
|
trapezoids.Add(trapezoids[j].LowerRight);
|
|
}
|
|
j += 1;
|
|
}
|
|
return trapezoids;
|
|
}
|
|
|
|
private void Replace(Sink sink, Node node)
|
|
{
|
|
if (sink.ParentList.Count == 0)
|
|
_head = node;
|
|
else
|
|
node.Replace(sink);
|
|
}
|
|
|
|
public void Case1(Sink sink, Edge edge, Trapezoid[] tList)
|
|
{
|
|
YNode yNode = new YNode(edge, Sink.Isink(tList[1]), Sink.Isink(tList[2]));
|
|
XNode qNode = new XNode(edge.Q, yNode, Sink.Isink(tList[3]));
|
|
XNode pNode = new XNode(edge.P, Sink.Isink(tList[0]), qNode);
|
|
Replace(sink, pNode);
|
|
}
|
|
|
|
public void Case2(Sink sink, Edge edge, Trapezoid[] tList)
|
|
{
|
|
YNode yNode = new YNode(edge, Sink.Isink(tList[1]), Sink.Isink(tList[2]));
|
|
XNode pNode = new XNode(edge.P, Sink.Isink(tList[0]), yNode);
|
|
Replace(sink, pNode);
|
|
}
|
|
|
|
public void Case3(Sink sink, Edge edge, Trapezoid[] tList)
|
|
{
|
|
YNode yNode = new YNode(edge, Sink.Isink(tList[0]), Sink.Isink(tList[1]));
|
|
Replace(sink, yNode);
|
|
}
|
|
|
|
public void Case4(Sink sink, Edge edge, Trapezoid[] tList)
|
|
{
|
|
YNode yNode = new YNode(edge, Sink.Isink(tList[0]), Sink.Isink(tList[1]));
|
|
XNode qNode = new XNode(edge.Q, yNode, Sink.Isink(tList[2]));
|
|
Replace(sink, qNode);
|
|
}
|
|
}
|
|
|
|
internal class Sink : Node
|
|
{
|
|
public Trapezoid Trapezoid;
|
|
|
|
private Sink(Trapezoid trapezoid)
|
|
: base(null, null)
|
|
{
|
|
Trapezoid = trapezoid;
|
|
trapezoid.Sink = this;
|
|
}
|
|
|
|
public static Sink Isink(Trapezoid trapezoid)
|
|
{
|
|
if (trapezoid.Sink == null)
|
|
return new Sink(trapezoid);
|
|
return trapezoid.Sink;
|
|
}
|
|
|
|
public override Sink Locate(Edge edge)
|
|
{
|
|
return this;
|
|
}
|
|
}
|
|
|
|
// See "Computational Geometry", 3rd edition, by Mark de Berg et al, Chapter 6.2
|
|
internal class TrapezoidalMap
|
|
{
|
|
// Trapezoid container
|
|
public HashSet<Trapezoid> Map;
|
|
|
|
// AABB margin
|
|
|
|
// Bottom segment that spans multiple trapezoids
|
|
private Edge _bCross;
|
|
|
|
// Top segment that spans multiple trapezoids
|
|
private Edge _cross;
|
|
private float _margin;
|
|
|
|
public TrapezoidalMap()
|
|
{
|
|
Map = new HashSet<Trapezoid>();
|
|
_margin = 50.0f;
|
|
_bCross = null;
|
|
_cross = null;
|
|
}
|
|
|
|
public void Clear()
|
|
{
|
|
_bCross = null;
|
|
_cross = null;
|
|
}
|
|
|
|
// Case 1: segment completely enclosed by trapezoid
|
|
// break trapezoid into 4 smaller trapezoids
|
|
public Trapezoid[] Case1(Trapezoid t, Edge e)
|
|
{
|
|
Trapezoid[] trapezoids = new Trapezoid[4];
|
|
trapezoids[0] = new Trapezoid(t.LeftPoint, e.P, t.Top, t.Bottom);
|
|
trapezoids[1] = new Trapezoid(e.P, e.Q, t.Top, e);
|
|
trapezoids[2] = new Trapezoid(e.P, e.Q, e, t.Bottom);
|
|
trapezoids[3] = new Trapezoid(e.Q, t.RightPoint, t.Top, t.Bottom);
|
|
|
|
trapezoids[0].UpdateLeft(t.UpperLeft, t.LowerLeft);
|
|
trapezoids[1].UpdateLeftRight(trapezoids[0], null, trapezoids[3], null);
|
|
trapezoids[2].UpdateLeftRight(null, trapezoids[0], null, trapezoids[3]);
|
|
trapezoids[3].UpdateRight(t.UpperRight, t.LowerRight);
|
|
|
|
return trapezoids;
|
|
}
|
|
|
|
// Case 2: Trapezoid contains point p, q lies outside
|
|
// break trapezoid into 3 smaller trapezoids
|
|
public Trapezoid[] Case2(Trapezoid t, Edge e)
|
|
{
|
|
Point rp;
|
|
if (e.Q.X == t.RightPoint.X)
|
|
rp = e.Q;
|
|
else
|
|
rp = t.RightPoint;
|
|
|
|
Trapezoid[] trapezoids = new Trapezoid[3];
|
|
trapezoids[0] = new Trapezoid(t.LeftPoint, e.P, t.Top, t.Bottom);
|
|
trapezoids[1] = new Trapezoid(e.P, rp, t.Top, e);
|
|
trapezoids[2] = new Trapezoid(e.P, rp, e, t.Bottom);
|
|
|
|
trapezoids[0].UpdateLeft(t.UpperLeft, t.LowerLeft);
|
|
trapezoids[1].UpdateLeftRight(trapezoids[0], null, t.UpperRight, null);
|
|
trapezoids[2].UpdateLeftRight(null, trapezoids[0], null, t.LowerRight);
|
|
|
|
_bCross = t.Bottom;
|
|
_cross = t.Top;
|
|
|
|
e.Above = trapezoids[1];
|
|
e.Below = trapezoids[2];
|
|
|
|
return trapezoids;
|
|
}
|
|
|
|
// Case 3: Trapezoid is bisected
|
|
public Trapezoid[] Case3(Trapezoid t, Edge e)
|
|
{
|
|
Point lp;
|
|
if (e.P.X == t.LeftPoint.X)
|
|
lp = e.P;
|
|
else
|
|
lp = t.LeftPoint;
|
|
|
|
Point rp;
|
|
if (e.Q.X == t.RightPoint.X)
|
|
rp = e.Q;
|
|
else
|
|
rp = t.RightPoint;
|
|
|
|
Trapezoid[] trapezoids = new Trapezoid[2];
|
|
|
|
if (_cross == t.Top)
|
|
{
|
|
trapezoids[0] = t.UpperLeft;
|
|
trapezoids[0].UpdateRight(t.UpperRight, null);
|
|
trapezoids[0].RightPoint = rp;
|
|
}
|
|
else
|
|
{
|
|
trapezoids[0] = new Trapezoid(lp, rp, t.Top, e);
|
|
trapezoids[0].UpdateLeftRight(t.UpperLeft, e.Above, t.UpperRight, null);
|
|
}
|
|
|
|
if (_bCross == t.Bottom)
|
|
{
|
|
trapezoids[1] = t.LowerLeft;
|
|
trapezoids[1].UpdateRight(null, t.LowerRight);
|
|
trapezoids[1].RightPoint = rp;
|
|
}
|
|
else
|
|
{
|
|
trapezoids[1] = new Trapezoid(lp, rp, e, t.Bottom);
|
|
trapezoids[1].UpdateLeftRight(e.Below, t.LowerLeft, null, t.LowerRight);
|
|
}
|
|
|
|
_bCross = t.Bottom;
|
|
_cross = t.Top;
|
|
|
|
e.Above = trapezoids[0];
|
|
e.Below = trapezoids[1];
|
|
|
|
return trapezoids;
|
|
}
|
|
|
|
// Case 4: Trapezoid contains point q, p lies outside
|
|
// break trapezoid into 3 smaller trapezoids
|
|
public Trapezoid[] Case4(Trapezoid t, Edge e)
|
|
{
|
|
Point lp;
|
|
if (e.P.X == t.LeftPoint.X)
|
|
lp = e.P;
|
|
else
|
|
lp = t.LeftPoint;
|
|
|
|
Trapezoid[] trapezoids = new Trapezoid[3];
|
|
|
|
if (_cross == t.Top)
|
|
{
|
|
trapezoids[0] = t.UpperLeft;
|
|
trapezoids[0].RightPoint = e.Q;
|
|
}
|
|
else
|
|
{
|
|
trapezoids[0] = new Trapezoid(lp, e.Q, t.Top, e);
|
|
trapezoids[0].UpdateLeft(t.UpperLeft, e.Above);
|
|
}
|
|
|
|
if (_bCross == t.Bottom)
|
|
{
|
|
trapezoids[1] = t.LowerLeft;
|
|
trapezoids[1].RightPoint = e.Q;
|
|
}
|
|
else
|
|
{
|
|
trapezoids[1] = new Trapezoid(lp, e.Q, e, t.Bottom);
|
|
trapezoids[1].UpdateLeft(e.Below, t.LowerLeft);
|
|
}
|
|
|
|
trapezoids[2] = new Trapezoid(e.Q, t.RightPoint, t.Top, t.Bottom);
|
|
trapezoids[2].UpdateLeftRight(trapezoids[0], trapezoids[1], t.UpperRight, t.LowerRight);
|
|
|
|
return trapezoids;
|
|
}
|
|
|
|
// Create an AABB around segments
|
|
public Trapezoid BoundingBox(List<Edge> edges)
|
|
{
|
|
Point max = edges[0].P + _margin;
|
|
Point min = edges[0].Q - _margin;
|
|
|
|
foreach (Edge e in edges)
|
|
{
|
|
if (e.P.X > max.X) max = new Point(e.P.X + _margin, max.Y);
|
|
if (e.P.Y > max.Y) max = new Point(max.X, e.P.Y + _margin);
|
|
if (e.Q.X > max.X) max = new Point(e.Q.X + _margin, max.Y);
|
|
if (e.Q.Y > max.Y) max = new Point(max.X, e.Q.Y + _margin);
|
|
if (e.P.X < min.X) min = new Point(e.P.X - _margin, min.Y);
|
|
if (e.P.Y < min.Y) min = new Point(min.X, e.P.Y - _margin);
|
|
if (e.Q.X < min.X) min = new Point(e.Q.X - _margin, min.Y);
|
|
if (e.Q.Y < min.Y) min = new Point(min.X, e.Q.Y - _margin);
|
|
}
|
|
|
|
Edge top = new Edge(new Point(min.X, max.Y), new Point(max.X, max.Y));
|
|
Edge bottom = new Edge(new Point(min.X, min.Y), new Point(max.X, min.Y));
|
|
Point left = bottom.P;
|
|
Point right = top.Q;
|
|
|
|
return new Trapezoid(left, right, top, bottom);
|
|
}
|
|
}
|
|
|
|
internal class Point
|
|
{
|
|
// Pointers to next and previous points in Monontone Mountain
|
|
public Point Next, Prev;
|
|
public float X, Y;
|
|
|
|
public Point(float x, float y)
|
|
{
|
|
X = x;
|
|
Y = y;
|
|
Next = null;
|
|
Prev = null;
|
|
}
|
|
|
|
public static Point operator -(Point p1, Point p2)
|
|
{
|
|
return new Point(p1.X - p2.X, p1.Y - p2.Y);
|
|
}
|
|
|
|
public static Point operator +(Point p1, Point p2)
|
|
{
|
|
return new Point(p1.X + p2.X, p1.Y + p2.Y);
|
|
}
|
|
|
|
public static Point operator -(Point p1, float f)
|
|
{
|
|
return new Point(p1.X - f, p1.Y - f);
|
|
}
|
|
|
|
public static Point operator +(Point p1, float f)
|
|
{
|
|
return new Point(p1.X + f, p1.Y + f);
|
|
}
|
|
|
|
public float Cross(Point p)
|
|
{
|
|
return X * p.Y - Y * p.X;
|
|
}
|
|
|
|
public float Dot(Point p)
|
|
{
|
|
return X * p.X + Y * p.Y;
|
|
}
|
|
|
|
public bool Neq(Point p)
|
|
{
|
|
return p.X != X || p.Y != Y;
|
|
}
|
|
|
|
public float Orient2D(Point pb, Point pc)
|
|
{
|
|
float acx = X - pc.X;
|
|
float bcx = pb.X - pc.X;
|
|
float acy = Y - pc.Y;
|
|
float bcy = pb.Y - pc.Y;
|
|
return acx * bcy - acy * bcx;
|
|
}
|
|
}
|
|
|
|
internal class Edge
|
|
{
|
|
// Pointers used for building trapezoidal map
|
|
public Trapezoid Above;
|
|
public float B;
|
|
public Trapezoid Below;
|
|
|
|
// Equation of a line: y = m*x + b
|
|
// Slope of the line (m)
|
|
|
|
// Montone mountain points
|
|
public HashSet<Point> MPoints;
|
|
public Point P;
|
|
public Point Q;
|
|
public float Slope;
|
|
|
|
// Y intercept
|
|
|
|
public Edge(Point p, Point q)
|
|
{
|
|
P = p;
|
|
Q = q;
|
|
|
|
if (q.X - p.X != 0)
|
|
Slope = (q.Y - p.Y) / (q.X - p.X);
|
|
else
|
|
Slope = 0;
|
|
|
|
B = p.Y - (p.X * Slope);
|
|
Above = null;
|
|
Below = null;
|
|
MPoints = new HashSet<Point>();
|
|
MPoints.Add(p);
|
|
MPoints.Add(q);
|
|
}
|
|
|
|
public bool IsAbove(Point point)
|
|
{
|
|
return P.Orient2D(Q, point) < 0;
|
|
}
|
|
|
|
public bool IsBelow(Point point)
|
|
{
|
|
return P.Orient2D(Q, point) > 0;
|
|
}
|
|
|
|
public void AddMpoint(Point point)
|
|
{
|
|
foreach (Point mp in MPoints)
|
|
if (!mp.Neq(point))
|
|
return;
|
|
|
|
MPoints.Add(point);
|
|
}
|
|
}
|
|
|
|
internal class Trapezoid
|
|
{
|
|
public Edge Bottom;
|
|
public bool Inside;
|
|
public Point LeftPoint;
|
|
|
|
// Neighbor pointers
|
|
public Trapezoid LowerLeft;
|
|
public Trapezoid LowerRight;
|
|
|
|
public Point RightPoint;
|
|
public Sink Sink;
|
|
|
|
public Edge Top;
|
|
public Trapezoid UpperLeft;
|
|
public Trapezoid UpperRight;
|
|
|
|
public Trapezoid(Point leftPoint, Point rightPoint, Edge top, Edge bottom)
|
|
{
|
|
LeftPoint = leftPoint;
|
|
RightPoint = rightPoint;
|
|
Top = top;
|
|
Bottom = bottom;
|
|
UpperLeft = null;
|
|
UpperRight = null;
|
|
LowerLeft = null;
|
|
LowerRight = null;
|
|
Inside = true;
|
|
Sink = null;
|
|
}
|
|
|
|
// Update neighbors to the left
|
|
public void UpdateLeft(Trapezoid ul, Trapezoid ll)
|
|
{
|
|
UpperLeft = ul;
|
|
if (ul != null) ul.UpperRight = this;
|
|
LowerLeft = ll;
|
|
if (ll != null) ll.LowerRight = this;
|
|
}
|
|
|
|
// Update neighbors to the right
|
|
public void UpdateRight(Trapezoid ur, Trapezoid lr)
|
|
{
|
|
UpperRight = ur;
|
|
if (ur != null) ur.UpperLeft = this;
|
|
LowerRight = lr;
|
|
if (lr != null) lr.LowerLeft = this;
|
|
}
|
|
|
|
// Update neighbors on both sides
|
|
public void UpdateLeftRight(Trapezoid ul, Trapezoid ll, Trapezoid ur, Trapezoid lr)
|
|
{
|
|
UpperLeft = ul;
|
|
if (ul != null) ul.UpperRight = this;
|
|
LowerLeft = ll;
|
|
if (ll != null) ll.LowerRight = this;
|
|
UpperRight = ur;
|
|
if (ur != null) ur.UpperLeft = this;
|
|
LowerRight = lr;
|
|
if (lr != null) lr.LowerLeft = this;
|
|
}
|
|
|
|
// Recursively trim outside neighbors
|
|
public void TrimNeighbors()
|
|
{
|
|
if (Inside)
|
|
{
|
|
Inside = false;
|
|
if (UpperLeft != null) UpperLeft.TrimNeighbors();
|
|
if (LowerLeft != null) LowerLeft.TrimNeighbors();
|
|
if (UpperRight != null) UpperRight.TrimNeighbors();
|
|
if (LowerRight != null) LowerRight.TrimNeighbors();
|
|
}
|
|
}
|
|
|
|
// Determines if this point lies inside the trapezoid
|
|
public bool Contains(Point point)
|
|
{
|
|
return (point.X > LeftPoint.X && point.X < RightPoint.X && Top.IsAbove(point) && Bottom.IsBelow(point));
|
|
}
|
|
|
|
public List<Point> Vertices()
|
|
{
|
|
List<Point> verts = new List<Point>(4);
|
|
verts.Add(LineIntersect(Top, LeftPoint.X));
|
|
verts.Add(LineIntersect(Bottom, LeftPoint.X));
|
|
verts.Add(LineIntersect(Bottom, RightPoint.X));
|
|
verts.Add(LineIntersect(Top, RightPoint.X));
|
|
return verts;
|
|
}
|
|
|
|
private Point LineIntersect(Edge edge, float x)
|
|
{
|
|
float y = edge.Slope * x + edge.B;
|
|
return new Point(x, y);
|
|
}
|
|
|
|
// Add points to monotone mountain
|
|
public void AddPoints()
|
|
{
|
|
if (LeftPoint != Bottom.P)
|
|
{
|
|
Bottom.AddMpoint(LeftPoint);
|
|
}
|
|
if (RightPoint != Bottom.Q)
|
|
{
|
|
Bottom.AddMpoint(RightPoint);
|
|
}
|
|
if (LeftPoint != Top.P)
|
|
{
|
|
Top.AddMpoint(LeftPoint);
|
|
}
|
|
if (RightPoint != Top.Q)
|
|
{
|
|
Top.AddMpoint(RightPoint);
|
|
}
|
|
}
|
|
}
|
|
|
|
internal class XNode : Node
|
|
{
|
|
private Point _point;
|
|
|
|
public XNode(Point point, Node lChild, Node rChild)
|
|
: base(lChild, rChild)
|
|
{
|
|
_point = point;
|
|
}
|
|
|
|
public override Sink Locate(Edge edge)
|
|
{
|
|
if (edge.P.X >= _point.X)
|
|
// Move to the right in the graph
|
|
return RightChild.Locate(edge);
|
|
// Move to the left in the graph
|
|
return LeftChild.Locate(edge);
|
|
}
|
|
}
|
|
|
|
internal class YNode : Node
|
|
{
|
|
private Edge _edge;
|
|
|
|
public YNode(Edge edge, Node lChild, Node rChild)
|
|
: base(lChild, rChild)
|
|
{
|
|
_edge = edge;
|
|
}
|
|
|
|
public override Sink Locate(Edge edge)
|
|
{
|
|
if (_edge.IsAbove(edge.P))
|
|
// Move down the graph
|
|
return RightChild.Locate(edge);
|
|
|
|
if (_edge.IsBelow(edge.P))
|
|
// Move up the graph
|
|
return LeftChild.Locate(edge);
|
|
|
|
// s and segment share the same endpoint, p
|
|
if (edge.Slope < _edge.Slope)
|
|
// Move down the graph
|
|
return RightChild.Locate(edge);
|
|
|
|
// Move up the graph
|
|
return LeftChild.Locate(edge);
|
|
}
|
|
}
|
|
|
|
internal class Triangulator
|
|
{
|
|
// Trapezoid decomposition list
|
|
public List<Trapezoid> Trapezoids;
|
|
public List<List<Point>> Triangles;
|
|
|
|
// Initialize trapezoidal map and query structure
|
|
private Trapezoid _boundingBox;
|
|
private List<Edge> _edgeList;
|
|
private QueryGraph _queryGraph;
|
|
private float _sheer = 0.001f;
|
|
private TrapezoidalMap _trapezoidalMap;
|
|
private List<MonotoneMountain> _xMonoPoly;
|
|
|
|
public Triangulator(List<Point> polyLine, float sheer)
|
|
{
|
|
_sheer = sheer;
|
|
Triangles = new List<List<Point>>();
|
|
Trapezoids = new List<Trapezoid>();
|
|
_xMonoPoly = new List<MonotoneMountain>();
|
|
_edgeList = InitEdges(polyLine);
|
|
_trapezoidalMap = new TrapezoidalMap();
|
|
_boundingBox = _trapezoidalMap.BoundingBox(_edgeList);
|
|
_queryGraph = new QueryGraph(Sink.Isink(_boundingBox));
|
|
|
|
Process();
|
|
}
|
|
|
|
// Build the trapezoidal map and query graph
|
|
private void Process()
|
|
{
|
|
foreach (Edge edge in _edgeList)
|
|
{
|
|
List<Trapezoid> traps = _queryGraph.FollowEdge(edge);
|
|
|
|
// Remove trapezoids from trapezoidal Map
|
|
foreach (Trapezoid t in traps)
|
|
{
|
|
_trapezoidalMap.Map.Remove(t);
|
|
|
|
bool cp = t.Contains(edge.P);
|
|
bool cq = t.Contains(edge.Q);
|
|
Trapezoid[] tList;
|
|
|
|
if (cp && cq)
|
|
{
|
|
tList = _trapezoidalMap.Case1(t, edge);
|
|
_queryGraph.Case1(t.Sink, edge, tList);
|
|
}
|
|
else if (cp && !cq)
|
|
{
|
|
tList = _trapezoidalMap.Case2(t, edge);
|
|
_queryGraph.Case2(t.Sink, edge, tList);
|
|
}
|
|
else if (!cp && !cq)
|
|
{
|
|
tList = _trapezoidalMap.Case3(t, edge);
|
|
_queryGraph.Case3(t.Sink, edge, tList);
|
|
}
|
|
else
|
|
{
|
|
tList = _trapezoidalMap.Case4(t, edge);
|
|
_queryGraph.Case4(t.Sink, edge, tList);
|
|
}
|
|
// Add new trapezoids to map
|
|
foreach (Trapezoid y in tList)
|
|
{
|
|
_trapezoidalMap.Map.Add(y);
|
|
}
|
|
}
|
|
_trapezoidalMap.Clear();
|
|
}
|
|
|
|
// Mark outside trapezoids
|
|
foreach (Trapezoid t in _trapezoidalMap.Map)
|
|
{
|
|
MarkOutside(t);
|
|
}
|
|
|
|
// Collect interior trapezoids
|
|
foreach (Trapezoid t in _trapezoidalMap.Map)
|
|
{
|
|
if (t.Inside)
|
|
{
|
|
Trapezoids.Add(t);
|
|
t.AddPoints();
|
|
}
|
|
}
|
|
|
|
// Generate the triangles
|
|
CreateMountains();
|
|
}
|
|
|
|
// Build a list of x-monotone mountains
|
|
private void CreateMountains()
|
|
{
|
|
foreach (Edge edge in _edgeList)
|
|
{
|
|
if (edge.MPoints.Count > 2)
|
|
{
|
|
MonotoneMountain mountain = new MonotoneMountain();
|
|
|
|
// Sorting is a perfromance hit. Literature says this can be accomplised in
|
|
// linear time, although I don't see a way around using traditional methods
|
|
// when using a randomized incremental algorithm
|
|
|
|
// Insertion sort is one of the fastest algorithms for sorting arrays containing
|
|
// fewer than ten elements, or for lists that are already mostly sorted.
|
|
|
|
List<Point> points = new List<Point>(edge.MPoints);
|
|
points.Sort((p1, p2) => p1.X.CompareTo(p2.X));
|
|
|
|
foreach (Point p in points)
|
|
mountain.Add(p);
|
|
|
|
// Triangulate monotone mountain
|
|
mountain.Process();
|
|
|
|
// Extract the triangles into a single list
|
|
foreach (List<Point> t in mountain.Triangles)
|
|
{
|
|
Triangles.Add(t);
|
|
}
|
|
|
|
_xMonoPoly.Add(mountain);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Mark the outside trapezoids surrounding the polygon
|
|
private void MarkOutside(Trapezoid t)
|
|
{
|
|
if (t.Top == _boundingBox.Top || t.Bottom == _boundingBox.Bottom)
|
|
t.TrimNeighbors();
|
|
}
|
|
|
|
// Create segments and connect end points; update edge event pointer
|
|
private List<Edge> InitEdges(List<Point> points)
|
|
{
|
|
List<Edge> edges = new List<Edge>();
|
|
|
|
for (int i = 0; i < points.Count - 1; i++)
|
|
{
|
|
edges.Add(new Edge(points[i], points[i + 1]));
|
|
}
|
|
edges.Add(new Edge(points[0], points[points.Count - 1]));
|
|
return OrderSegments(edges);
|
|
}
|
|
|
|
private List<Edge> OrderSegments(List<Edge> edgeInput)
|
|
{
|
|
// Ignore vertical segments!
|
|
List<Edge> edges = new List<Edge>();
|
|
|
|
foreach (Edge e in edgeInput)
|
|
{
|
|
Point p = ShearTransform(e.P);
|
|
Point q = ShearTransform(e.Q);
|
|
|
|
// Point p must be to the left of point q
|
|
if (p.X > q.X)
|
|
{
|
|
edges.Add(new Edge(q, p));
|
|
}
|
|
else if (p.X < q.X)
|
|
{
|
|
edges.Add(new Edge(p, q));
|
|
}
|
|
}
|
|
|
|
// Randomized triangulation improves performance
|
|
// See Seidel's paper, or O'Rourke's book, p. 57
|
|
Shuffle(edges);
|
|
return edges;
|
|
}
|
|
|
|
private static void Shuffle<T>(IList<T> list)
|
|
{
|
|
Random rng = new Random();
|
|
int n = list.Count;
|
|
while (n > 1)
|
|
{
|
|
n--;
|
|
int k = rng.Next(n + 1);
|
|
T value = list[k];
|
|
list[k] = list[n];
|
|
list[n] = value;
|
|
}
|
|
}
|
|
|
|
// Prevents any two distinct endpoints from lying on a common vertical line, and avoiding
|
|
// the degenerate case. See Mark de Berg et al, Chapter 6.3
|
|
private Point ShearTransform(Point point)
|
|
{
|
|
return new Point(point.X + _sheer * point.Y, point.Y);
|
|
}
|
|
}
|
|
} |