428 lines
15 KiB
C#
428 lines
15 KiB
C#
|
// -----------------------------------------------------------------------
|
||
|
|
// <copyright file="TriangleQuadTree.cs" company="">
|
||
|
|
// Original code by Frank Dockhorn, [not available anymore: http://sourceforge.net/projects/quadtreesim/]
|
||
|
|
// Triangle.NET code by Christian Woltering, http://triangle.codeplex.com/
|
||
|
|
// </copyright>
|
||
|
|
// -----------------------------------------------------------------------
|
||
|
|
|
||
|
|
namespace UnityEngine.U2D.Animation.TriangleNet
|
||
|
|
.Tools
|
||
|
|
{
|
||
|
|
using System.Collections.Generic;
|
||
|
|
using System.Linq;
|
||
|
|
using Animation.TriangleNet.Geometry;
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// A Quadtree implementation optimized for triangles.
|
||
|
|
/// </summary>
|
||
|
|
internal class TriangleQuadTree
|
||
|
|
{
|
||
|
|
QuadNode root;
|
||
|
|
|
||
|
|
internal ITriangle[] triangles;
|
||
|
|
|
||
|
|
internal int sizeBound;
|
||
|
|
internal int maxDepth;
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Initializes a new instance of the <see cref="TriangleQuadTree" /> class.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="mesh">Mesh containing triangles.</param>
|
||
|
|
/// <param name="maxDepth">The maximum depth of the tree.</param>
|
||
|
|
/// <param name="sizeBound">The maximum number of triangles contained in a leaf.</param>
|
||
|
|
/// <remarks>
|
||
|
|
/// The quadtree does not track changes of the mesh. If a mesh is refined or
|
||
|
|
/// changed in any other way, a new quadtree has to be built to make the point
|
||
|
|
/// location work.
|
||
|
|
///
|
||
|
|
/// A node of the tree will be split, if its level if less than the max depth parameter
|
||
|
|
/// AND the number of triangles in the node is greater than the size bound.
|
||
|
|
/// </remarks>
|
||
|
|
public TriangleQuadTree(Mesh mesh, int maxDepth = 10, int sizeBound = 10)
|
||
|
|
{
|
||
|
|
this.maxDepth = maxDepth;
|
||
|
|
this.sizeBound = sizeBound;
|
||
|
|
|
||
|
|
triangles = mesh.Triangles.ToArray();
|
||
|
|
|
||
|
|
int currentDepth = 0;
|
||
|
|
|
||
|
|
root = new QuadNode(mesh.Bounds, this, true);
|
||
|
|
root.CreateSubRegion(++currentDepth);
|
||
|
|
}
|
||
|
|
|
||
|
|
public ITriangle Query(double x, double y)
|
||
|
|
{
|
||
|
|
var point = new Point(x, y);
|
||
|
|
var indices = root.FindTriangles(point);
|
||
|
|
|
||
|
|
foreach (var i in indices)
|
||
|
|
{
|
||
|
|
var tri = this.triangles[i];
|
||
|
|
|
||
|
|
if (IsPointInTriangle(point, tri.GetVertex(0), tri.GetVertex(1), tri.GetVertex(2)))
|
||
|
|
{
|
||
|
|
return tri;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return null;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Test, if a given point lies inside a triangle.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="p">Point to locate.</param>
|
||
|
|
/// <param name="t0">Corner point of triangle.</param>
|
||
|
|
/// <param name="t1">Corner point of triangle.</param>
|
||
|
|
/// <param name="t2">Corner point of triangle.</param>
|
||
|
|
/// <returns>True, if point is inside or on the edge of this triangle.</returns>
|
||
|
|
internal static bool IsPointInTriangle(Point p, Point t0, Point t1, Point t2)
|
||
|
|
{
|
||
|
|
// TODO: no need to create new Point instances here
|
||
|
|
Point d0 = new Point(t1.x - t0.x, t1.y - t0.y);
|
||
|
|
Point d1 = new Point(t2.x - t0.x, t2.y - t0.y);
|
||
|
|
Point d2 = new Point(p.x - t0.x, p.y - t0.y);
|
||
|
|
|
||
|
|
// crossproduct of (0, 0, 1) and d0
|
||
|
|
Point c0 = new Point(-d0.y, d0.x);
|
||
|
|
|
||
|
|
// crossproduct of (0, 0, 1) and d1
|
||
|
|
Point c1 = new Point(-d1.y, d1.x);
|
||
|
|
|
||
|
|
// Linear combination d2 = s * d0 + v * d1.
|
||
|
|
//
|
||
|
|
// Multiply both sides of the equation with c0 and c1
|
||
|
|
// and solve for s and v respectively
|
||
|
|
//
|
||
|
|
// s = d2 * c1 / d0 * c1
|
||
|
|
// v = d2 * c0 / d1 * c0
|
||
|
|
|
||
|
|
double s = DotProduct(d2, c1) / DotProduct(d0, c1);
|
||
|
|
double v = DotProduct(d2, c0) / DotProduct(d1, c0);
|
||
|
|
|
||
|
|
if (s >= 0 && v >= 0 && ((s + v) <= 1))
|
||
|
|
{
|
||
|
|
// Point is inside or on the edge of this triangle.
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
internal static double DotProduct(Point p, Point q)
|
||
|
|
{
|
||
|
|
return p.x * q.x + p.y * q.y;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// A node of the quadtree.
|
||
|
|
/// </summary>
|
||
|
|
class QuadNode
|
||
|
|
{
|
||
|
|
const int SW = 0;
|
||
|
|
const int SE = 1;
|
||
|
|
const int NW = 2;
|
||
|
|
const int NE = 3;
|
||
|
|
|
||
|
|
const double EPS = 1e-6;
|
||
|
|
|
||
|
|
static readonly byte[] BITVECTOR = { 0x1, 0x2, 0x4, 0x8 };
|
||
|
|
|
||
|
|
Rectangle bounds;
|
||
|
|
Point pivot;
|
||
|
|
TriangleQuadTree tree;
|
||
|
|
QuadNode[] regions;
|
||
|
|
List<int> triangles;
|
||
|
|
|
||
|
|
byte bitRegions;
|
||
|
|
|
||
|
|
public QuadNode(Rectangle box, TriangleQuadTree tree)
|
||
|
|
: this(box, tree, false)
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
public QuadNode(Rectangle box, TriangleQuadTree tree, bool init)
|
||
|
|
{
|
||
|
|
this.tree = tree;
|
||
|
|
|
||
|
|
this.bounds = new Rectangle(box.Left, box.Bottom, box.Width, box.Height);
|
||
|
|
this.pivot = new Point((box.Left + box.Right) / 2, (box.Bottom + box.Top) / 2);
|
||
|
|
|
||
|
|
this.bitRegions = 0;
|
||
|
|
|
||
|
|
this.regions = new QuadNode[4];
|
||
|
|
this.triangles = new List<int>();
|
||
|
|
|
||
|
|
if (init)
|
||
|
|
{
|
||
|
|
int count = tree.triangles.Length;
|
||
|
|
|
||
|
|
// Allocate memory upfront
|
||
|
|
triangles.Capacity = count;
|
||
|
|
|
||
|
|
for (int i = 0; i < count; i++)
|
||
|
|
{
|
||
|
|
triangles.Add(i);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public List<int> FindTriangles(Point searchPoint)
|
||
|
|
{
|
||
|
|
int region = FindRegion(searchPoint);
|
||
|
|
if (regions[region] == null)
|
||
|
|
{
|
||
|
|
return triangles;
|
||
|
|
}
|
||
|
|
return regions[region].FindTriangles(searchPoint);
|
||
|
|
}
|
||
|
|
|
||
|
|
public void CreateSubRegion(int currentDepth)
|
||
|
|
{
|
||
|
|
// The four sub regions of the quad tree
|
||
|
|
// +--------------+
|
||
|
|
// | nw 2 | ne 3 |
|
||
|
|
// |------+pivot--|
|
||
|
|
// | sw 0 | se 1 |
|
||
|
|
// +--------------+
|
||
|
|
Rectangle box;
|
||
|
|
|
||
|
|
var width = bounds.Right - pivot.x;
|
||
|
|
var height = bounds.Top - pivot.y;
|
||
|
|
|
||
|
|
// 1. region south west
|
||
|
|
box = new Rectangle(bounds.Left, bounds.Bottom, width, height);
|
||
|
|
regions[0] = new QuadNode(box, tree);
|
||
|
|
|
||
|
|
// 2. region south east
|
||
|
|
box = new Rectangle(pivot.x, bounds.Bottom, width, height);
|
||
|
|
regions[1] = new QuadNode(box, tree);
|
||
|
|
|
||
|
|
// 3. region north west
|
||
|
|
box = new Rectangle(bounds.Left, pivot.y, width, height);
|
||
|
|
regions[2] = new QuadNode(box, tree);
|
||
|
|
|
||
|
|
// 4. region north east
|
||
|
|
box = new Rectangle(pivot.x, pivot.y, width, height);
|
||
|
|
regions[3] = new QuadNode(box, tree);
|
||
|
|
|
||
|
|
Point[] triangle = new Point[3];
|
||
|
|
|
||
|
|
// Find region for every triangle vertex
|
||
|
|
foreach (var index in triangles)
|
||
|
|
{
|
||
|
|
ITriangle tri = tree.triangles[index];
|
||
|
|
|
||
|
|
triangle[0] = tri.GetVertex(0);
|
||
|
|
triangle[1] = tri.GetVertex(1);
|
||
|
|
triangle[2] = tri.GetVertex(2);
|
||
|
|
|
||
|
|
AddTriangleToRegion(triangle, index);
|
||
|
|
}
|
||
|
|
|
||
|
|
for (int i = 0; i < 4; i++)
|
||
|
|
{
|
||
|
|
if (regions[i].triangles.Count > tree.sizeBound && currentDepth < tree.maxDepth)
|
||
|
|
{
|
||
|
|
regions[i].CreateSubRegion(currentDepth + 1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void AddTriangleToRegion(Point[] triangle, int index)
|
||
|
|
{
|
||
|
|
bitRegions = 0;
|
||
|
|
if (TriangleQuadTree.IsPointInTriangle(pivot, triangle[0], triangle[1], triangle[2]))
|
||
|
|
{
|
||
|
|
AddToRegion(index, SW);
|
||
|
|
AddToRegion(index, SE);
|
||
|
|
AddToRegion(index, NW);
|
||
|
|
AddToRegion(index, NE);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
FindTriangleIntersections(triangle, index);
|
||
|
|
|
||
|
|
if (bitRegions == 0)
|
||
|
|
{
|
||
|
|
// we didn't find any intersection so we add this triangle to a point's region
|
||
|
|
int region = FindRegion(triangle[0]);
|
||
|
|
regions[region].triangles.Add(index);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void FindTriangleIntersections(Point[] triangle, int index)
|
||
|
|
{
|
||
|
|
// PLEASE NOTE:
|
||
|
|
// Handling of component comparison is tightly associated with the implementation
|
||
|
|
// of the findRegion() function. That means when the point to be compared equals
|
||
|
|
// the pivot point the triangle must be put at least into region 2.
|
||
|
|
//
|
||
|
|
// Linear equations are in parametric form.
|
||
|
|
// pivot.x = triangle[0].x + t * (triangle[1].x - triangle[0].x)
|
||
|
|
// pivot.y = triangle[0].y + t * (triangle[1].y - triangle[0].y)
|
||
|
|
|
||
|
|
int k = 2;
|
||
|
|
|
||
|
|
double dx, dy;
|
||
|
|
// Iterate through all triangle laterals and find bounding box intersections
|
||
|
|
for (int i = 0; i < 3; k = i++)
|
||
|
|
{
|
||
|
|
dx = triangle[i].x - triangle[k].x;
|
||
|
|
dy = triangle[i].y - triangle[k].y;
|
||
|
|
|
||
|
|
if (dx != 0.0)
|
||
|
|
{
|
||
|
|
FindIntersectionsWithX(dx, dy, triangle, index, k);
|
||
|
|
}
|
||
|
|
if (dy != 0.0)
|
||
|
|
{
|
||
|
|
FindIntersectionsWithY(dx, dy, triangle, index, k);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void FindIntersectionsWithX(double dx, double dy, Point[] triangle, int index, int k)
|
||
|
|
{
|
||
|
|
double t;
|
||
|
|
|
||
|
|
// find intersection with plane x = m_pivot.dX
|
||
|
|
t = (pivot.x - triangle[k].x) / dx;
|
||
|
|
if (t < (1 + EPS) && t > -EPS)
|
||
|
|
{
|
||
|
|
// we have an intersection
|
||
|
|
double yComponent = triangle[k].y + t * dy;
|
||
|
|
|
||
|
|
if (yComponent < pivot.y && yComponent >= bounds.Bottom)
|
||
|
|
{
|
||
|
|
AddToRegion(index, SW);
|
||
|
|
AddToRegion(index, SE);
|
||
|
|
}
|
||
|
|
else if (yComponent <= bounds.Top)
|
||
|
|
{
|
||
|
|
AddToRegion(index, NW);
|
||
|
|
AddToRegion(index, NE);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// find intersection with plane x = m_boundingBox[0].dX
|
||
|
|
t = (bounds.Left - triangle[k].x) / dx;
|
||
|
|
if (t < (1 + EPS) && t > -EPS)
|
||
|
|
{
|
||
|
|
// we have an intersection
|
||
|
|
double yComponent = triangle[k].y + t * dy;
|
||
|
|
|
||
|
|
if (yComponent < pivot.y && yComponent >= bounds.Bottom)
|
||
|
|
{
|
||
|
|
AddToRegion(index, SW);
|
||
|
|
}
|
||
|
|
else if (yComponent <= bounds.Top) // TODO: check && yComponent >= pivot.Y
|
||
|
|
{
|
||
|
|
AddToRegion(index, NW);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// find intersection with plane x = m_boundingBox[1].dX
|
||
|
|
t = (bounds.Right - triangle[k].x) / dx;
|
||
|
|
if (t < (1 + EPS) && t > -EPS)
|
||
|
|
{
|
||
|
|
// we have an intersection
|
||
|
|
double yComponent = triangle[k].y + t * dy;
|
||
|
|
|
||
|
|
if (yComponent < pivot.y && yComponent >= bounds.Bottom)
|
||
|
|
{
|
||
|
|
AddToRegion(index, SE);
|
||
|
|
}
|
||
|
|
else if (yComponent <= bounds.Top)
|
||
|
|
{
|
||
|
|
AddToRegion(index, NE);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void FindIntersectionsWithY(double dx, double dy, Point[] triangle, int index, int k)
|
||
|
|
{
|
||
|
|
double t, xComponent;
|
||
|
|
|
||
|
|
// find intersection with plane y = m_pivot.dY
|
||
|
|
t = (pivot.y - triangle[k].y) / dy;
|
||
|
|
if (t < (1 + EPS) && t > -EPS)
|
||
|
|
{
|
||
|
|
// we have an intersection
|
||
|
|
xComponent = triangle[k].x + t * dx;
|
||
|
|
|
||
|
|
if (xComponent > pivot.x && xComponent <= bounds.Right)
|
||
|
|
{
|
||
|
|
AddToRegion(index, SE);
|
||
|
|
AddToRegion(index, NE);
|
||
|
|
}
|
||
|
|
else if (xComponent >= bounds.Left)
|
||
|
|
{
|
||
|
|
AddToRegion(index, SW);
|
||
|
|
AddToRegion(index, NW);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// find intersection with plane y = m_boundingBox[0].dY
|
||
|
|
t = (bounds.Bottom - triangle[k].y) / dy;
|
||
|
|
if (t < (1 + EPS) && t > -EPS)
|
||
|
|
{
|
||
|
|
// we have an intersection
|
||
|
|
xComponent = triangle[k].x + t * dx;
|
||
|
|
|
||
|
|
if (xComponent > pivot.x && xComponent <= bounds.Right)
|
||
|
|
{
|
||
|
|
AddToRegion(index, SE);
|
||
|
|
}
|
||
|
|
else if (xComponent >= bounds.Left)
|
||
|
|
{
|
||
|
|
AddToRegion(index, SW);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// find intersection with plane y = m_boundingBox[1].dY
|
||
|
|
t = (bounds.Top - triangle[k].y) / dy;
|
||
|
|
if (t < (1 + EPS) && t > -EPS)
|
||
|
|
{
|
||
|
|
// we have an intersection
|
||
|
|
xComponent = triangle[k].x + t * dx;
|
||
|
|
|
||
|
|
if (xComponent > pivot.x && xComponent <= bounds.Right)
|
||
|
|
{
|
||
|
|
AddToRegion(index, NE);
|
||
|
|
}
|
||
|
|
else if (xComponent >= bounds.Left)
|
||
|
|
{
|
||
|
|
AddToRegion(index, NW);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
int FindRegion(Point point)
|
||
|
|
{
|
||
|
|
int b = 2;
|
||
|
|
if (point.y < pivot.y)
|
||
|
|
{
|
||
|
|
b = 0;
|
||
|
|
}
|
||
|
|
if (point.x > pivot.x)
|
||
|
|
{
|
||
|
|
b++;
|
||
|
|
}
|
||
|
|
return b;
|
||
|
|
}
|
||
|
|
|
||
|
|
void AddToRegion(int index, int region)
|
||
|
|
{
|
||
|
|
//if (!(m_bitRegions & BITVECTOR[region]))
|
||
|
|
if ((bitRegions & BITVECTOR[region]) == 0)
|
||
|
|
{
|
||
|
|
regions[region].triangles.Add(index);
|
||
|
|
bitRegions |= BITVECTOR[region];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|