1549 lines
40 KiB
C#
1549 lines
40 KiB
C#
// Copyright (c) 2013-2015 Robert Rouhani <robert.rouhani@gmail.com> and other contributors (see CONTRIBUTORS file).
|
|
// Licensed under the MIT License - https://raw.github.com/Robmaister/SharpNav/master/LICENSE
|
|
|
|
using System;
|
|
using System.Collections.Generic;
|
|
|
|
using SharpNav.Geometry;
|
|
|
|
#if MONOGAME
|
|
using Vector3 = Microsoft.Xna.Framework.Vector3;
|
|
#elif OPENTK
|
|
using Vector3 = OpenTK.Vector3;
|
|
#elif SHARPDX
|
|
using Vector3 = SharpDX.Vector3;
|
|
#endif
|
|
|
|
namespace SharpNav
|
|
{
|
|
/// <summary>
|
|
/// The PolyMeshDetail class is a combination of a PolyMesh and a CompactHeightfield merged together
|
|
/// </summary>
|
|
public class PolyMeshDetail
|
|
{
|
|
//9 x 2
|
|
private static readonly int[] VertexOffset =
|
|
{
|
|
0, 0,
|
|
-1, -1,
|
|
0, -1,
|
|
1, -1,
|
|
1, 0,
|
|
1, 1,
|
|
0, 1,
|
|
-1, 1,
|
|
-1, 0
|
|
};
|
|
|
|
private MeshData[] meshes;
|
|
private Vector3[] verts;
|
|
private TriangleData[] tris;
|
|
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="PolyMeshDetail"/> class.
|
|
/// </summary>
|
|
/// <param name="mesh">The <see cref="PolyMesh"/>.</param>
|
|
/// <param name="compactField">The <see cref="CompactHeightfield"/> used to add height detail.</param>
|
|
/// <param name="settings">The settings to build with.</param>
|
|
public PolyMeshDetail(PolyMesh mesh, CompactHeightfield compactField, NavMeshGenerationSettings settings)
|
|
: this(mesh, compactField, settings.SampleDistance, settings.MaxSampleError)
|
|
{
|
|
}
|
|
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="PolyMeshDetail"/> class.
|
|
/// </summary>
|
|
/// <remarks>
|
|
/// <see cref="PolyMeshDetail"/> uses a <see cref="CompactHeightfield"/> to add in details to a
|
|
/// <see cref="PolyMesh"/>. This detail is triangulated into a new mesh and can be used to approximate height in the walkable
|
|
/// areas of a scene.
|
|
/// </remarks>
|
|
/// <param name="mesh">The <see cref="PolyMesh"/>.</param>
|
|
/// <param name="compactField">The <see cref="CompactHeightfield"/> used to add height detail.</param>
|
|
/// <param name="sampleDist">The sampling distance.</param>
|
|
/// <param name="sampleMaxError">The maximum sampling error allowed.</param>
|
|
public PolyMeshDetail(PolyMesh mesh, CompactHeightfield compactField, float sampleDist, float sampleMaxError)
|
|
{
|
|
if (mesh.VertCount == 0 || mesh.PolyCount == 0)
|
|
return;
|
|
|
|
Vector3 origin = mesh.Bounds.Min;
|
|
|
|
int maxhw = 0, maxhh = 0;
|
|
|
|
BBox2i[] bounds = new BBox2i[mesh.PolyCount];
|
|
Vector3[] poly = new Vector3[mesh.NumVertsPerPoly];
|
|
|
|
var storedVertices = new List<Vector3>();
|
|
var storedTriangles = new List<TriangleData>();
|
|
|
|
//find max size for polygon area
|
|
for (int i = 0; i < mesh.PolyCount; i++)
|
|
{
|
|
var p = mesh.Polys[i];
|
|
|
|
int xmin = compactField.Width;
|
|
int xmax = 0;
|
|
int zmin = compactField.Length;
|
|
int zmax = 0;
|
|
|
|
for (int j = 0; j < mesh.NumVertsPerPoly; j++)
|
|
{
|
|
var pj = p.Vertices[j];
|
|
if (pj == PolyMesh.NullId)
|
|
break;
|
|
|
|
var v = mesh.Verts[pj];
|
|
|
|
xmin = Math.Min(xmin, v.X);
|
|
xmax = Math.Max(xmax, v.X);
|
|
zmin = Math.Min(zmin, v.Z);
|
|
zmax = Math.Max(zmax, v.Z);
|
|
}
|
|
|
|
xmin = Math.Max(0, xmin - 1);
|
|
xmax = Math.Min(compactField.Width, xmax + 1);
|
|
zmin = Math.Max(0, zmin - 1);
|
|
zmax = Math.Min(compactField.Length, zmax + 1);
|
|
|
|
if (xmin >= xmax || zmin >= zmax)
|
|
continue;
|
|
|
|
maxhw = Math.Max(maxhw, xmax - xmin);
|
|
maxhh = Math.Max(maxhh, zmax - zmin);
|
|
|
|
bounds[i] = new BBox2i(xmin, zmin, xmax, zmax);
|
|
}
|
|
|
|
HeightPatch hp = new HeightPatch(0, 0, maxhw, maxhh);
|
|
|
|
this.meshes = new MeshData[mesh.PolyCount];
|
|
|
|
for (int i = 0; i < mesh.PolyCount; i++)
|
|
{
|
|
var p = mesh.Polys[i];
|
|
|
|
//store polygon vertices for processing
|
|
int npoly = 0;
|
|
for (int j = 0; j < mesh.NumVertsPerPoly; j++)
|
|
{
|
|
int pvi = p.Vertices[j];
|
|
if (pvi == PolyMesh.NullId)
|
|
break;
|
|
|
|
PolyVertex pv = mesh.Verts[pvi];
|
|
Vector3 v = new Vector3(pv.X, pv.Y, pv.Z);
|
|
v.X *= mesh.CellSize;
|
|
v.Y *= mesh.CellHeight;
|
|
v.Z *= mesh.CellSize;
|
|
poly[j] = v;
|
|
npoly++;
|
|
}
|
|
|
|
//get height data from area of polygon
|
|
BBox2i bound = bounds[i];
|
|
hp.Resize(bound.Min.X, bound.Min.Y, bound.Max.X - bound.Min.X, bound.Max.Y - bound.Min.Y);
|
|
GetHeightData(compactField, p, npoly, mesh.Verts, mesh.BorderSize, hp);
|
|
|
|
List<Vector3> tempVerts = new List<Vector3>();
|
|
List<TriangleData> tempTris = new List<TriangleData>(128);
|
|
List<EdgeInfo> edges = new List<EdgeInfo>(16);
|
|
List<SamplingData> samples = new List<SamplingData>(128);
|
|
BuildPolyDetail(poly, npoly, sampleDist, sampleMaxError, compactField, hp, tempVerts, tempTris, edges, samples);
|
|
|
|
//more detail verts
|
|
for (int j = 0; j < tempVerts.Count; j++)
|
|
{
|
|
Vector3 tv = tempVerts[j];
|
|
|
|
Vector3 v;
|
|
v.X = tv.X + origin.X;
|
|
v.Y = tv.Y + origin.Y + compactField.CellHeight;
|
|
v.Z = tv.Z + origin.Z;
|
|
|
|
tempVerts[j] = v;
|
|
}
|
|
|
|
for (int j = 0; j < npoly; j++)
|
|
{
|
|
Vector3 po = poly[j];
|
|
|
|
po.X += origin.X;
|
|
po.Y += origin.Y;
|
|
po.Z += origin.Z;
|
|
|
|
poly[j] = po;
|
|
}
|
|
|
|
//save data
|
|
this.meshes[i].VertexIndex = storedVertices.Count;
|
|
this.meshes[i].VertexCount = tempVerts.Count;
|
|
this.meshes[i].TriangleIndex = storedTriangles.Count;
|
|
this.meshes[i].TriangleCount = tempTris.Count;
|
|
|
|
//store vertices
|
|
storedVertices.AddRange(tempVerts);
|
|
|
|
//store triangles
|
|
for (int j = 0; j < tempTris.Count; j++)
|
|
{
|
|
storedTriangles.Add(new TriangleData(tempTris[j], tempVerts, poly, npoly));
|
|
}
|
|
}
|
|
|
|
this.verts = storedVertices.ToArray();
|
|
this.tris = storedTriangles.ToArray();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Determines whether an edge has been created or not.
|
|
/// </summary>
|
|
private enum EdgeValues
|
|
{
|
|
/// <summary>
|
|
/// Edge has not been initialized
|
|
/// </summary>
|
|
Undefined = -1,
|
|
|
|
/// <summary>
|
|
/// Edge is hull
|
|
/// </summary>
|
|
Hull = -2
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the number of meshes (MeshData)
|
|
/// </summary>
|
|
public int MeshCount
|
|
{
|
|
get
|
|
{
|
|
if (meshes == null)
|
|
return 0;
|
|
|
|
return meshes.Length;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the number of vertices
|
|
/// </summary>
|
|
public int VertCount
|
|
{
|
|
get
|
|
{
|
|
if (verts == null)
|
|
return 0;
|
|
|
|
return verts.Length;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the number of triangles
|
|
/// </summary>
|
|
public int TrisCount
|
|
{
|
|
get
|
|
{
|
|
if (tris == null)
|
|
return 0;
|
|
|
|
return tris.Length;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the mesh data
|
|
/// </summary>
|
|
public MeshData[] Meshes
|
|
{
|
|
get
|
|
{
|
|
return meshes;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the vertex data
|
|
/// </summary>
|
|
public Vector3[] Verts
|
|
{
|
|
get
|
|
{
|
|
return verts;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the triangle data
|
|
/// </summary>
|
|
public TriangleData[] Tris
|
|
{
|
|
get
|
|
{
|
|
return tris;
|
|
}
|
|
}
|
|
|
|
#region Black Magic
|
|
|
|
/// <summary>
|
|
/// Offset for the x-coordinate
|
|
/// </summary>
|
|
/// <param name="i">Starting number</param>
|
|
/// <returns>A new offset</returns>
|
|
private static float GetJitterX(int i)
|
|
{
|
|
return (((i * 0x8da6b343) & 0xffff) / 65535.0f * 2.0f) - 1.0f;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Offset for the y-coordinate
|
|
/// </summary>
|
|
/// <param name="i">Starting number</param>
|
|
/// <returns>A new offset</returns>
|
|
private static float GetJitterY(int i)
|
|
{
|
|
return (((i * 0xd8163841) & 0xffff) / 65535.0f * 2.0f) - 1.0f;
|
|
}
|
|
|
|
#endregion
|
|
|
|
/// <summary>
|
|
/// Determine whether an edge of the triangle is part of the polygon (1 if true, 0 if false)
|
|
/// </summary>
|
|
/// <param name="va">Triangle vertex A</param>
|
|
/// <param name="vb">Triangle vertex B</param>
|
|
/// <param name="vpoly">Polygon vertex data</param>
|
|
/// <returns>1 if the vertices are close, 0 if otherwise</returns>
|
|
private static int GetEdgeFlags(Vector3 va, Vector3 vb, Vector3[] vpoly, int npoly)
|
|
{
|
|
//true if edge is part of polygon
|
|
float thrSqr = 0.001f * 0.001f;
|
|
|
|
for (int i = 0, j = npoly - 1; i < npoly; j = i++)
|
|
{
|
|
Vector3 pt1 = va;
|
|
Vector3 pt2 = vb;
|
|
|
|
//the vertices pt1 (va) and pt2 (vb) are extremely close to the polygon edge
|
|
if (Distance.PointToSegment2DSquared(ref pt1, ref vpoly[j], ref vpoly[i]) < thrSqr
|
|
&& Distance.PointToSegment2DSquared(ref pt2, ref vpoly[j], ref vpoly[i]) < thrSqr)
|
|
return 1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
private void GetHeightData(CompactHeightfield compactField, PolyMesh.Polygon poly, int polyCount, PolyVertex[] verts, int borderSize, HeightPatch hp)
|
|
{
|
|
var stack = new List<CompactSpanReference>();
|
|
bool empty = true;
|
|
hp.Clear();
|
|
|
|
for (int y = 0; y < hp.Length; y++)
|
|
{
|
|
int hy = hp.Y + y + borderSize;
|
|
for (int x = 0; x < hp.Width; x++)
|
|
{
|
|
int hx = hp.X + x + borderSize;
|
|
var cells = compactField.Cells[hy * compactField.Width + hx];
|
|
for (int i = cells.StartIndex, end = cells.StartIndex + cells.Count; i < end; i++)
|
|
{
|
|
var span = compactField.Spans[i];
|
|
|
|
if (span.Region == poly.RegionId)
|
|
{
|
|
hp[x, y] = span.Minimum;
|
|
empty = false;
|
|
|
|
bool border = false;
|
|
for (var dir = Direction.West; dir <= Direction.South; dir++)
|
|
{
|
|
if (span.IsConnected(dir))
|
|
{
|
|
int ax = hx + dir.GetHorizontalOffset();
|
|
int ay = hy + dir.GetVerticalOffset();
|
|
int ai = compactField.Cells[ay * compactField.Width + ax].StartIndex + CompactSpan.GetConnection(ref span, dir);
|
|
|
|
if (compactField.Spans[ai].Region != poly.RegionId)
|
|
{
|
|
border = true;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (border)
|
|
stack.Add(new CompactSpanReference(hx, hy, i));
|
|
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (empty)
|
|
GetHeightDataSeedsFromVertices(compactField, poly, polyCount, verts, borderSize, hp, stack);
|
|
|
|
const int RetractSize = 256;
|
|
int head = 0;
|
|
|
|
while (head < stack.Count)
|
|
{
|
|
var cell = stack[head++];
|
|
var cs = compactField[cell];
|
|
|
|
if (head >= RetractSize)
|
|
{
|
|
head = 0;
|
|
if (stack.Count > RetractSize)
|
|
{
|
|
for (int i = 0; i < stack.Count - RetractSize; i++)
|
|
stack[i] = stack[i + RetractSize];
|
|
}
|
|
|
|
int targetSize = stack.Count % RetractSize;
|
|
while (stack.Count > targetSize)
|
|
stack.RemoveAt(stack.Count - 1);
|
|
}
|
|
|
|
//loop in all four directions
|
|
for (var dir = Direction.West; dir <= Direction.South; dir++)
|
|
{
|
|
//skip
|
|
if (!cs.IsConnected(dir))
|
|
continue;
|
|
|
|
int ax = cell.X + dir.GetHorizontalOffset();
|
|
int ay = cell.Y + dir.GetVerticalOffset();
|
|
int hx = ax - hp.X - borderSize;
|
|
int hy = ay - hp.Y - borderSize;
|
|
|
|
if (hx < 0 || hx >= hp.Width || hy < 0 || hy >= hp.Length)
|
|
continue;
|
|
|
|
//only continue if height is unset
|
|
if (hp.IsSet(hy * hp.Width + hx))
|
|
continue;
|
|
|
|
//get new span
|
|
int ai = compactField.Cells[ay * compactField.Width + ax].StartIndex + CompactSpan.GetConnection(ref cs, dir);
|
|
CompactSpan ds = compactField.Spans[ai];
|
|
|
|
hp[hx, hy] = ds.Minimum;
|
|
|
|
stack.Add(new CompactSpanReference(ax, ay, ai));
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Floodfill heightfield to get 2D height data, starting at vertex locations
|
|
/// </summary>
|
|
/// <param name="compactField">Original heightfield data</param>
|
|
/// <param name="poly">Polygon in PolyMesh</param>
|
|
/// <param name="polyCount">Number of vertices per polygon</param>
|
|
/// <param name="verts">PolyMesh Vertices</param>
|
|
/// <param name="borderSize">Heightfield border size</param>
|
|
/// <param name="hp">HeightPatch which extracts heightfield data</param>
|
|
/// <param name="stack">Temporary stack of CompactSpanReferences</param>
|
|
private void GetHeightDataSeedsFromVertices(CompactHeightfield compactField, PolyMesh.Polygon poly, int polyCount, PolyVertex[] verts, int borderSize, HeightPatch hp, List<CompactSpanReference> stack)
|
|
{
|
|
hp.SetAll(0);
|
|
|
|
//use poly vertices as seed points
|
|
for (int j = 0; j < polyCount; j++)
|
|
{
|
|
var csr = new CompactSpanReference(0, 0, -1);
|
|
int dmin = int.MaxValue;
|
|
|
|
var v = verts[poly.Vertices[j]];
|
|
|
|
for (int k = 0; k < 9; k++)
|
|
{
|
|
//get vertices and offset x and z coordinates depending on current drection
|
|
int ax = v.X + VertexOffset[k * 2 + 0];
|
|
int ay = v.Y;
|
|
int az = v.Z + VertexOffset[k * 2 + 1];
|
|
|
|
//skip if out of bounds
|
|
if (ax < hp.X || ax >= hp.X + hp.Width || az < hp.Y || az >= hp.Y + hp.Length)
|
|
continue;
|
|
|
|
//get new cell
|
|
CompactCell c = compactField.Cells[(az + borderSize) * compactField.Width + (ax + borderSize)];
|
|
|
|
//loop through all the spans
|
|
for (int i = c.StartIndex, end = c.StartIndex + c.Count; i < end; i++)
|
|
{
|
|
CompactSpan s = compactField.Spans[i];
|
|
|
|
//find minimum y-distance
|
|
int d = Math.Abs(ay - s.Minimum);
|
|
if (d < dmin)
|
|
{
|
|
csr = new CompactSpanReference(ax, az, i);
|
|
dmin = d;
|
|
}
|
|
}
|
|
}
|
|
|
|
//only add if something new found
|
|
if (csr.Index != -1)
|
|
{
|
|
stack.Add(csr);
|
|
}
|
|
}
|
|
|
|
//find center of polygon using flood fill
|
|
int pcx = 0, pcz = 0;
|
|
for (int j = 0; j < polyCount; j++)
|
|
{
|
|
var v = verts[poly.Vertices[j]];
|
|
pcx += v.X;
|
|
pcz += v.Z;
|
|
}
|
|
|
|
pcx /= polyCount;
|
|
pcz /= polyCount;
|
|
|
|
//stack groups 3 elements as one part
|
|
foreach (var cell in stack)
|
|
{
|
|
int idx = (cell.Y - hp.Y) * hp.Width + (cell.X - hp.X);
|
|
hp[idx] = 1;
|
|
}
|
|
|
|
//process the entire stack
|
|
while (stack.Count > 0)
|
|
{
|
|
var cell = stack[stack.Count - 1];
|
|
stack.RemoveAt(stack.Count - 1);
|
|
|
|
//check if close to center of polygon
|
|
if (Math.Abs(cell.X - pcx) <= 1 && Math.Abs(cell.Y - pcz) <= 1)
|
|
{
|
|
//clear the stack and add a new group
|
|
stack.Clear();
|
|
|
|
stack.Add(cell);
|
|
break;
|
|
}
|
|
|
|
CompactSpan cs = compactField[cell];
|
|
|
|
//check all four directions
|
|
for (var dir = Direction.West; dir <= Direction.South; dir++)
|
|
{
|
|
//skip if disconnected
|
|
if (!cs.IsConnected(dir))
|
|
continue;
|
|
|
|
//get neighbor
|
|
int ax = cell.X + dir.GetHorizontalOffset();
|
|
int ay = cell.Y + dir.GetVerticalOffset();
|
|
|
|
//skip if out of bounds
|
|
if (ax < hp.X || ax >= (hp.X + hp.Width) || ay < hp.Y || ay >= (hp.Y + hp.Length))
|
|
continue;
|
|
|
|
if (hp[(ay - hp.Y) * hp.Width + (ax - hp.X)] != 0)
|
|
continue;
|
|
|
|
//get the new index
|
|
int ai = compactField.Cells[(ay + borderSize) * compactField.Width + (ax + borderSize)].StartIndex + CompactSpan.GetConnection(ref cs, dir);
|
|
|
|
//save data
|
|
int idx = (ay - hp.Y) * hp.Width + (ax - hp.X);
|
|
hp[idx] = 1;
|
|
|
|
//push to stack
|
|
stack.Add(new CompactSpanReference(ax, ay, ai));
|
|
}
|
|
}
|
|
|
|
//clear the heightpatch
|
|
hp.Clear();
|
|
|
|
//mark start locations
|
|
for (int i = 0; i < stack.Count; i++)
|
|
{
|
|
var c = stack[i];
|
|
|
|
//set new heightpatch data
|
|
int idx = (c.Y - hp.Y) * hp.Width + (c.X - hp.X);
|
|
CompactSpan cs = compactField.Spans[c.Index];
|
|
hp[idx] = cs.Minimum;
|
|
|
|
stack[i] = new CompactSpanReference(c.X + borderSize, c.Y + borderSize, c.Index);
|
|
}
|
|
}
|
|
|
|
private static float PolyMinExtent(Vector3[] verts)
|
|
{
|
|
float minDist = float.MaxValue;
|
|
for (int i = 0; i < verts.Length; i++)
|
|
{
|
|
int ni = (i + 1) % verts.Length;
|
|
Vector3 p0 = verts[i];
|
|
Vector3 p1 = verts[ni];
|
|
|
|
float maxEdgeDist = 0;
|
|
for (int j = 0; j < verts.Length; j++)
|
|
{
|
|
if (j == i || j == ni)
|
|
continue;
|
|
|
|
float d = Distance.PointToSegment2DSquared(ref verts[j], ref p0, ref p1);
|
|
maxEdgeDist = Math.Max(maxEdgeDist, d);
|
|
}
|
|
|
|
minDist = Math.Min(minDist, maxEdgeDist);
|
|
}
|
|
|
|
return (float)Math.Sqrt(minDist);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Generate the PolyMeshDetail using the PolyMesh and HeightPatch
|
|
/// </summary>
|
|
/// <param name="polyMeshVerts">PolyMesh Vertex data</param>
|
|
/// <param name="numMeshVerts">Number of PolyMesh vertices</param>
|
|
/// <param name="sampleDist">Sampling distance</param>
|
|
/// <param name="sampleMaxError">Maximum sampling error</param>
|
|
/// <param name="compactField">THe compactHeightfield</param>
|
|
/// <param name="hp">The heightPatch</param>
|
|
/// <param name="verts">Detail verts</param>
|
|
/// <param name="tris">Detail triangles</param>
|
|
/// <param name="edges">The edge array</param>
|
|
/// <param name="samples">The samples array</param>
|
|
private void BuildPolyDetail(Vector3[] polyMeshVerts, int numMeshVerts, float sampleDist, float sampleMaxError, CompactHeightfield compactField, HeightPatch hp, List<Vector3> verts, List<TriangleData> tris, List<EdgeInfo> edges, List<SamplingData> samples)
|
|
{
|
|
const int MAX_VERTS = 127;
|
|
const int MAX_TRIS = 255;
|
|
const int MAX_VERTS_PER_EDGE = 32;
|
|
Vector3[] edge = new Vector3[MAX_VERTS_PER_EDGE + 1];
|
|
List<int> hull = new List<int>(MAX_VERTS);
|
|
|
|
//fill up vertex array
|
|
for (int i = 0; i < numMeshVerts; ++i)
|
|
verts.Add(polyMeshVerts[i]);
|
|
|
|
float cs = compactField.CellSize;
|
|
float ics = 1.0f / cs;
|
|
|
|
float minExtent = PolyMinExtent(polyMeshVerts);
|
|
|
|
//tessellate outlines
|
|
if (sampleDist > 0)
|
|
{
|
|
for (int i = 0, j = verts.Count - 1; i < verts.Count; j = i++)
|
|
{
|
|
Vector3 vi = verts[i];
|
|
Vector3 vj = verts[j];
|
|
bool swapped = false;
|
|
|
|
//make sure order is correct, otherwise swap data
|
|
if (Math.Abs(vj.X - vi.X) < 1E-06f)
|
|
{
|
|
if (vj.Z > vi.Z)
|
|
{
|
|
Vector3 temp = vj;
|
|
vj = vi;
|
|
vi = temp;
|
|
swapped = true;
|
|
}
|
|
}
|
|
else if (vj.X > vi.X)
|
|
{
|
|
Vector3 temp = vj;
|
|
vj = vi;
|
|
vi = temp;
|
|
swapped = true;
|
|
}
|
|
|
|
//create samples along the edge
|
|
Vector3 dv;
|
|
Vector3.Subtract(ref vi, ref vj, out dv);
|
|
float d = (float)Math.Sqrt(dv.X * dv.X + dv.Z * dv.Z);
|
|
int nn = 1 + (int)Math.Floor(d / sampleDist);
|
|
|
|
if (nn >= MAX_VERTS_PER_EDGE)
|
|
nn = MAX_VERTS_PER_EDGE - 1;
|
|
|
|
if (verts.Count + nn >= MAX_VERTS)
|
|
nn = MAX_VERTS - 1 - verts.Count;
|
|
|
|
for (int k = 0; k <= nn; k++)
|
|
{
|
|
float u = (float)k / (float)nn;
|
|
Vector3 pos;
|
|
|
|
Vector3 tmp;
|
|
Vector3.Multiply(ref dv, u, out tmp);
|
|
Vector3.Add(ref vj, ref tmp, out pos);
|
|
|
|
pos.Y = GetHeight(pos, ics, compactField.CellHeight, hp) * compactField.CellHeight;
|
|
|
|
edge[k] = pos;
|
|
}
|
|
|
|
//simplify samples
|
|
int[] idx = new int[MAX_VERTS_PER_EDGE];
|
|
idx[0] = 0;
|
|
idx[1] = nn;
|
|
int nidx = 2;
|
|
|
|
for (int k = 0; k < nidx - 1;)
|
|
{
|
|
int a = idx[k];
|
|
int b = idx[k + 1];
|
|
Vector3 va = edge[a];
|
|
Vector3 vb = edge[b];
|
|
|
|
//find maximum deviation along segment
|
|
float maxd = 0;
|
|
int maxi = -1;
|
|
for (int m = a + 1; m < b; m++)
|
|
{
|
|
float dev = Distance.PointToSegmentSquared(ref edge[m], ref va, ref vb);
|
|
if (dev > maxd)
|
|
{
|
|
maxd = dev;
|
|
maxi = m;
|
|
}
|
|
}
|
|
|
|
if (maxi != -1 && maxd > (sampleMaxError * sampleMaxError))
|
|
{
|
|
//shift data to the right
|
|
for (int m = nidx; m > k; m--)
|
|
idx[m] = idx[m - 1];
|
|
|
|
//set new value
|
|
idx[k + 1] = maxi;
|
|
nidx++;
|
|
}
|
|
else
|
|
{
|
|
k++;
|
|
}
|
|
}
|
|
|
|
hull.Add(j);
|
|
|
|
//add new vertices
|
|
if (swapped)
|
|
{
|
|
for (int k = nidx - 2; k > 0; k--)
|
|
{
|
|
hull.Add(verts.Count);
|
|
verts.Add(edge[idx[k]]);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for (int k = 1; k < nidx - 1; k++)
|
|
{
|
|
hull.Add(verts.Count);
|
|
verts.Add(edge[idx[k]]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//tesselate base mesh
|
|
edges.Clear();
|
|
tris.Clear();
|
|
|
|
if (minExtent < sampleDist * 2)
|
|
{
|
|
TriangulateHull(verts, hull, tris);
|
|
return;
|
|
}
|
|
|
|
TriangulateHull(verts, hull, tris);
|
|
|
|
if (tris.Count == 0)
|
|
{
|
|
Console.WriteLine("Can't triangulate polygon, adding default data.");
|
|
return;
|
|
}
|
|
|
|
if (sampleDist > 0)
|
|
{
|
|
//create sample locations
|
|
BBox3 bounds = new BBox3();
|
|
bounds.Min = polyMeshVerts[0];
|
|
bounds.Max = polyMeshVerts[0];
|
|
|
|
for (int i = 1; i < numMeshVerts; i++)
|
|
{
|
|
Vector3Extensions.ComponentMin(ref bounds.Min, ref polyMeshVerts[i], out bounds.Min);
|
|
Vector3Extensions.ComponentMax(ref bounds.Max, ref polyMeshVerts[i], out bounds.Max);
|
|
}
|
|
|
|
int x0 = (int)Math.Floor(bounds.Min.X / sampleDist);
|
|
int x1 = (int)Math.Ceiling(bounds.Max.X / sampleDist);
|
|
int z0 = (int)Math.Floor(bounds.Min.Z / sampleDist);
|
|
int z1 = (int)Math.Ceiling(bounds.Max.Z / sampleDist);
|
|
|
|
samples.Clear();
|
|
|
|
for (int z = z0; z < z1; z++)
|
|
{
|
|
for (int x = x0; x < x1; x++)
|
|
{
|
|
Vector3 pt = new Vector3(x * sampleDist, (bounds.Max.Y + bounds.Min.Y) * 0.5f, z * sampleDist);
|
|
|
|
//make sure samples aren't too close to edge
|
|
if (Distance.PointToPolygonSquared(pt, polyMeshVerts, numMeshVerts) > -sampleDist * 0.5f)
|
|
continue;
|
|
|
|
SamplingData sd = new SamplingData(x, GetHeight(pt, ics, compactField.CellHeight, hp), z, false);
|
|
samples.Add(sd);
|
|
}
|
|
}
|
|
|
|
//added samples
|
|
for (int iter = 0; iter < samples.Count; iter++)
|
|
{
|
|
if (verts.Count >= MAX_VERTS)
|
|
break;
|
|
|
|
//find sample with most error
|
|
Vector3 bestPt = Vector3.Zero;
|
|
float bestDistance = 0;
|
|
int bestIndex = -1;
|
|
|
|
for (int i = 0; i < samples.Count; i++)
|
|
{
|
|
SamplingData sd = samples[i];
|
|
if (sd.IsSampled)
|
|
continue;
|
|
|
|
//jitter sample location to remove effects of bad triangulation
|
|
Vector3 pt;
|
|
pt.X = sd.X * sampleDist + GetJitterX(i) * compactField.CellSize * 0.1f;
|
|
pt.Y = sd.Y * compactField.CellHeight;
|
|
pt.Z = sd.Z * sampleDist + GetJitterY(i) * compactField.CellSize * 0.1f;
|
|
float d = DistanceToTriMesh(pt, verts, tris);
|
|
|
|
if (d < 0)
|
|
continue;
|
|
|
|
if (d > bestDistance)
|
|
{
|
|
bestDistance = d;
|
|
bestIndex = i;
|
|
bestPt = pt;
|
|
}
|
|
}
|
|
|
|
if (bestDistance <= sampleMaxError || bestIndex == -1)
|
|
break;
|
|
|
|
SamplingData bsd = samples[bestIndex];
|
|
bsd.IsSampled = true;
|
|
samples[bestIndex] = bsd;
|
|
|
|
verts.Add(bestPt);
|
|
|
|
//create new triangulation
|
|
edges.Clear();
|
|
tris.Clear();
|
|
DelaunayHull(verts, hull, tris, edges);
|
|
}
|
|
}
|
|
|
|
int ntris = tris.Count;
|
|
if (ntris > MAX_TRIS)
|
|
{
|
|
//TODO we're using lists... let the user have super detailed meshes?
|
|
//Perhaps just a warning saying there's a lot of tris?
|
|
//tris.RemoveRange(MAX_TRIS + 1, tris.Count - MAX_TRIS);
|
|
//Console.WriteLine("WARNING: shrinking number of triangles.");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Use the HeightPatch data to obtain a height for a certain location.
|
|
/// </summary>
|
|
/// <param name="loc">The location</param>
|
|
/// <param name="invCellSize">Reciprocal of cell size</param>
|
|
/// <param name="cellHeight">Cell height</param>
|
|
/// <param name="hp">Height patch</param>
|
|
/// <returns>The height</returns>
|
|
private int GetHeight(Vector3 loc, float invCellSize, float cellHeight, HeightPatch hp)
|
|
{
|
|
int ix = (int)Math.Floor(loc.X * invCellSize + 0.01f);
|
|
int iz = (int)Math.Floor(loc.Z * invCellSize + 0.01f);
|
|
ix = MathHelper.Clamp(ix - hp.X, 0, hp.Width - 1);
|
|
iz = MathHelper.Clamp(iz - hp.Y, 0, hp.Length - 1);
|
|
int h;
|
|
|
|
if (!hp.TryGetHeight(ix, iz, out h))
|
|
{
|
|
//go in counterclockwise direction starting from west, ending in northwest
|
|
int[] off =
|
|
{
|
|
-1, 0,
|
|
-1, -1,
|
|
0, -1,
|
|
1, -1,
|
|
1, 0,
|
|
1, 1,
|
|
0, 1,
|
|
-1, 1
|
|
};
|
|
|
|
float dmin = float.MaxValue;
|
|
|
|
for (int i = 0; i < 8; i++)
|
|
{
|
|
int nx = ix + off[i * 2 + 0];
|
|
int nz = iz + off[i * 2 + 1];
|
|
|
|
if (nx < 0 || nz < 0 || nx >= hp.Width || nz >= hp.Length)
|
|
continue;
|
|
|
|
int nh;
|
|
if (!hp.TryGetHeight(nx, nz, out nh))
|
|
continue;
|
|
|
|
float d = Math.Abs(nh * cellHeight - loc.Y);
|
|
if (d < dmin)
|
|
{
|
|
h = nh;
|
|
dmin = d;
|
|
}
|
|
}
|
|
}
|
|
|
|
return h;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the previous vertex index
|
|
/// </summary>
|
|
/// <param name="i">The current index</param>
|
|
/// <param name="n">The max number of vertices</param>
|
|
/// <returns>The previous index</returns>
|
|
private static int Prev(int i, int n)
|
|
{
|
|
return i - 1 >= 0 ? i - 1 : n - 1;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the next vertex index
|
|
/// </summary>
|
|
/// <param name="i">The current index</param>
|
|
/// <param name="n">The max number of vertices</param>
|
|
/// <returns>The next index</returns>
|
|
private static int Next(int i, int n)
|
|
{
|
|
return i + 1 < n ? i + 1 : 0;
|
|
}
|
|
|
|
private void TriangulateHull(List<Vector3> pts, List<int> hull, List<TriangleData> tris)
|
|
{
|
|
int start = 0, left = 1, right = hull.Count - 1;
|
|
|
|
float dmin = 0;
|
|
for (int i = 0; i < hull.Count; i++)
|
|
{
|
|
int pi = Prev(i, hull.Count);
|
|
int ni = Next(i, hull.Count);
|
|
Vector3 pv = pts[hull[pi]];
|
|
Vector3 cv = pts[hull[i]];
|
|
Vector3 nv = pts[hull[ni]];
|
|
float d = 0;
|
|
float dtmp;
|
|
Vector3Extensions.Distance2D(ref pv, ref cv, out dtmp);
|
|
d += dtmp;
|
|
Vector3Extensions.Distance2D(ref cv, ref nv, out dtmp);
|
|
d += dtmp;
|
|
Vector3Extensions.Distance2D(ref nv, ref pv, out dtmp);
|
|
d += dtmp;
|
|
|
|
if (d < dmin)
|
|
{
|
|
start = i;
|
|
left = ni;
|
|
right = pi;
|
|
dmin = d;
|
|
}
|
|
}
|
|
|
|
tris.Add(new TriangleData(hull[start], hull[left], hull[right], 0));
|
|
|
|
while (Next(left, hull.Count) != right)
|
|
{
|
|
int nleft = Next(left, hull.Count);
|
|
int nright = Prev(right, hull.Count);
|
|
|
|
Vector3 cvleft = pts[hull[left]];
|
|
Vector3 nvleft = pts[hull[nleft]];
|
|
Vector3 cvright = pts[hull[right]];
|
|
Vector3 nvright = pts[hull[nright]];
|
|
|
|
float dleft = 0, dright = 0;
|
|
float dtmp;
|
|
Vector3Extensions.Distance2D(ref cvleft, ref nvleft, out dtmp);
|
|
dleft += dtmp;
|
|
Vector3Extensions.Distance2D(ref nvleft, ref cvright, out dtmp);
|
|
dleft += dtmp;
|
|
|
|
Vector3Extensions.Distance2D(ref cvright, ref nvright, out dtmp);
|
|
dright += dtmp;
|
|
Vector3Extensions.Distance2D(ref cvleft, ref nvright, out dtmp);
|
|
dright += dtmp;
|
|
|
|
if (dleft < dright)
|
|
{
|
|
tris.Add(new TriangleData(hull[left], hull[nleft], hull[right], 0));
|
|
left = nleft;
|
|
}
|
|
else
|
|
{
|
|
tris.Add(new TriangleData(hull[left], hull[nright], hull[right], 0));
|
|
right = nright;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Delaunay triangulation is used to triangulate the polygon after adding detail to the edges. The result is a mesh.
|
|
/// </summary>
|
|
/// <param name="pts">Vertex data (each vertex has 3 elements x,y,z)</param>
|
|
/// <param name="hull">The hull (purpose?)</param>
|
|
/// <param name="tris">The triangles formed.</param>
|
|
/// <param name="edges">The edge connections formed.</param>
|
|
private void DelaunayHull(List<Vector3> pts, List<int> hull, List<TriangleData> tris, List<EdgeInfo> edges)
|
|
{
|
|
int nfaces = 0;
|
|
edges.Clear();
|
|
|
|
for (int i = 0, j = hull.Count - 1; i < hull.Count; j = i++)
|
|
AddEdge(edges, hull[j], hull[i], (int)EdgeValues.Hull, (int)EdgeValues.Undefined);
|
|
|
|
for (int i = 0; i < edges.Count; i++)
|
|
{
|
|
if (edges[i].LeftFace == (int)EdgeValues.Undefined)
|
|
CompleteFacet(pts, edges, ref nfaces, i);
|
|
|
|
if (edges[i].RightFace == (int)EdgeValues.Undefined)
|
|
CompleteFacet(pts, edges, ref nfaces, i);
|
|
}
|
|
|
|
/*int currentEdge = 0;
|
|
while (currentEdge < edges.Count)
|
|
{
|
|
if (edges[currentEdge].LeftFace == (int)EdgeValues.Undefined)
|
|
CompleteFacet(pts, edges, ref nfaces, currentEdge);
|
|
if (edges[currentEdge].RightFace == (int)EdgeValues.Undefined)
|
|
CompleteFacet(pts, edges, ref nfaces, currentEdge);
|
|
|
|
currentEdge++;
|
|
}*/
|
|
|
|
//create triangles
|
|
tris.Clear();
|
|
for (int i = 0; i < nfaces; i++)
|
|
tris.Add(new TriangleData(-1, -1, -1, -1));
|
|
|
|
for (int i = 0; i < edges.Count; i++)
|
|
{
|
|
EdgeInfo e = edges[i];
|
|
|
|
if (e.RightFace >= 0)
|
|
{
|
|
//left face
|
|
var tri = tris[e.RightFace];
|
|
|
|
if (tri.VertexHash0 == -1)
|
|
{
|
|
tri.VertexHash0 = e.EndPt0;
|
|
tri.VertexHash1 = e.EndPt1;
|
|
}
|
|
else if (tri.VertexHash0 == e.EndPt1)
|
|
{
|
|
tri.VertexHash2 = e.EndPt0;
|
|
}
|
|
else if (tri.VertexHash1 == e.EndPt0)
|
|
{
|
|
tri.VertexHash2 = e.EndPt1;
|
|
}
|
|
|
|
tris[e.RightFace] = tri;
|
|
}
|
|
|
|
if (e.LeftFace >= 0)
|
|
{
|
|
//right
|
|
var tri = tris[e.LeftFace];
|
|
|
|
if (tri.VertexHash0 == -1)
|
|
{
|
|
tri.VertexHash0 = e.EndPt1;
|
|
tri.VertexHash1 = e.EndPt0;
|
|
}
|
|
else if (tri.VertexHash0 == e.EndPt0)
|
|
{
|
|
tri.VertexHash2 = e.EndPt1;
|
|
}
|
|
else if (tri.VertexHash1 == e.EndPt1)
|
|
{
|
|
tri.VertexHash2 = e.EndPt0;
|
|
}
|
|
|
|
tris[e.LeftFace] = tri;
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < tris.Count; i++)
|
|
{
|
|
var t = tris[i];
|
|
if (t.VertexHash0 == -1 || t.VertexHash1 == -1 || t.VertexHash2 == -1)
|
|
{
|
|
//remove dangling face
|
|
Console.WriteLine("WARNING: removing dangling face.");
|
|
tris[i] = tris[tris.Count - 1];
|
|
tris.RemoveAt(tris.Count - 1);
|
|
i--;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// If a face has missing edges, then fill in those edges
|
|
/// </summary>
|
|
/// <param name="pts">List of points</param>
|
|
/// <param name="edges">List of edges</param>
|
|
/// <param name="nfaces">The total number of faces</param>
|
|
/// <param name="curEdge">The current index in the edge list</param>
|
|
private void CompleteFacet(List<Vector3> pts, List<EdgeInfo> edges, ref int nfaces, int curEdge)
|
|
{
|
|
const float EPS = 1e-5f;
|
|
|
|
EdgeInfo e = edges[curEdge];
|
|
|
|
//cache s and t
|
|
int s, t;
|
|
if (e.LeftFace == (int)EdgeValues.Undefined)
|
|
{
|
|
s = e.EndPt0;
|
|
t = e.EndPt1;
|
|
}
|
|
else if (e.RightFace == (int)EdgeValues.Undefined)
|
|
{
|
|
s = e.EndPt1;
|
|
t = e.EndPt0;
|
|
}
|
|
else
|
|
{
|
|
//edge already completed
|
|
return;
|
|
}
|
|
|
|
//find best point on left edge
|
|
int pt = pts.Count;
|
|
Vector3 c = Vector3.Zero;
|
|
float r = -1;
|
|
float cross;
|
|
for (int u = 0; u < pts.Count; u++)
|
|
{
|
|
if (u == s || u == t)
|
|
continue;
|
|
|
|
cross = Vector3Extensions.Cross2D(pts[s], pts[t], pts[u]);
|
|
if (cross > EPS)
|
|
{
|
|
if (r < 0)
|
|
{
|
|
//update circle now
|
|
pt = u;
|
|
CircumCircle(pts[s], pts[t], pts[u], ref c, out r);
|
|
continue;
|
|
}
|
|
|
|
float d = Vector3Extensions.Distance2D(c, pts[u]);
|
|
float tol = 0.001f;
|
|
|
|
if (d > r * (1 + tol))
|
|
{
|
|
//outside circumcircle
|
|
continue;
|
|
}
|
|
else if (d < r * (1 - tol))
|
|
{
|
|
//inside circumcircle, update
|
|
pt = u;
|
|
CircumCircle(pts[s], pts[t], pts[u], ref c, out r);
|
|
}
|
|
else
|
|
{
|
|
//inside epsilon circumcircle
|
|
if (OverlapEdges(pts, edges, s, u))
|
|
continue;
|
|
|
|
if (OverlapEdges(pts, edges, t, u))
|
|
continue;
|
|
|
|
//edge is valid
|
|
pt = u;
|
|
CircumCircle(pts[s], pts[t], pts[u], ref c, out r);
|
|
}
|
|
}
|
|
}
|
|
|
|
//add new triangle or update edge if s-t on hull
|
|
if (pt < pts.Count)
|
|
{
|
|
EdgeInfo.UpdateLeftFace(ref e, s, t, nfaces);
|
|
edges[curEdge] = e;
|
|
|
|
curEdge = AddEdge(edges, pt, s, nfaces, (int)EdgeValues.Undefined);
|
|
if (curEdge != (int)EdgeValues.Undefined)
|
|
{
|
|
e = edges[curEdge];
|
|
EdgeInfo.UpdateLeftFace(ref e, pt, s, nfaces);
|
|
edges[curEdge] = e;
|
|
}
|
|
|
|
curEdge = AddEdge(edges, t, pt, nfaces, (int)EdgeValues.Undefined);
|
|
if (curEdge != (int)EdgeValues.Undefined)
|
|
{
|
|
e = edges[curEdge];
|
|
EdgeInfo.UpdateLeftFace(ref e, t, pt, nfaces);
|
|
edges[curEdge] = e;
|
|
}
|
|
|
|
nfaces++;
|
|
}
|
|
else
|
|
{
|
|
e = edges[curEdge];
|
|
EdgeInfo.UpdateLeftFace(ref e, s, t, (int)EdgeValues.Hull);
|
|
edges[curEdge] = e;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Add an edge to the edge list if it hasn't been done so already
|
|
/// </summary>
|
|
/// <param name="edges">Edge list</param>
|
|
/// <param name="s">Endpt 0</param>
|
|
/// <param name="t">Endpt 1</param>
|
|
/// <param name="leftFace">Left face value</param>
|
|
/// <param name="rightFace">Right face value</param>
|
|
/// <returns>The index of the edge (edge can already exist in the list).</returns>
|
|
private int AddEdge(List<EdgeInfo> edges, int s, int t, int leftFace, int rightFace)
|
|
{
|
|
//add edge
|
|
int e = FindEdge(edges, s, t);
|
|
if (e == -1)
|
|
{
|
|
EdgeInfo edge = new EdgeInfo(s, t, rightFace, leftFace);
|
|
edges.Add(edge);
|
|
|
|
return edges.Count - 1;
|
|
}
|
|
else
|
|
return e;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Search for an edge within the edge list
|
|
/// </summary>
|
|
/// <param name="edges">Edge list</param>
|
|
/// <param name="s">Endpt 0</param>
|
|
/// <param name="t">Endpt 1</param>
|
|
/// <returns>If found, return the edge's index. Otherwise, return -1.</returns>
|
|
private int FindEdge(List<EdgeInfo> edges, int s, int t)
|
|
{
|
|
for (int i = 0; i < edges.Count; i++)
|
|
{
|
|
EdgeInfo e = edges[i];
|
|
if ((e.EndPt0 == s && e.EndPt1 == t) || (e.EndPt0 == t && e.EndPt1 == s))
|
|
return i;
|
|
}
|
|
|
|
return -1;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Determine whether edges overlap with the points
|
|
/// </summary>
|
|
/// <param name="pts">Individual points</param>
|
|
/// <param name="edges">Edge list</param>
|
|
/// <param name="s1">An edge's endpt 0</param>
|
|
/// <param name="t1">An edge's endpt1</param>
|
|
/// <returns>True if there is overlap, false if not</returns>
|
|
private bool OverlapEdges(List<Vector3> pts, List<EdgeInfo> edges, int s1, int t1)
|
|
{
|
|
Vector3 ps1 = pts[s1], pt1 = pts[t1];
|
|
|
|
for (int i = 0; i < edges.Count; i++)
|
|
{
|
|
int s0 = edges[i].EndPt0;
|
|
int t0 = edges[i].EndPt1;
|
|
|
|
//same or connected edges do not overlap
|
|
if (s0 == s1 || s0 == t1 || t0 == s1 || t0 == t1)
|
|
continue;
|
|
|
|
Vector3 ps0 = pts[s0], pt0 = pts[t0];
|
|
if (Intersection.SegmentSegment2D(ref ps0, ref pt0, ref ps1, ref pt1))
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Form a triangle ABC out of the three vectors and calculate the center and radius
|
|
/// of the resulting circumcircle
|
|
/// </summary>
|
|
/// <param name="p1">Point A</param>
|
|
/// <param name="p2">Point B</param>
|
|
/// <param name="p3">point C</param>
|
|
/// <param name="c">Circumcirlce center</param>
|
|
/// <param name="r">Circumcircle radius</param>
|
|
/// <returns>True, if a circumcirle can be found. False, if otherwise.</returns>
|
|
private bool CircumCircle(Vector3 p1, Vector3 p2, Vector3 p3, ref Vector3 c, out float r)
|
|
{
|
|
const float EPS = 1e-6f;
|
|
float cp;
|
|
Vector3Extensions.Cross2D(ref p1, ref p2, ref p3, out cp);
|
|
|
|
if (Math.Abs(cp) > EPS)
|
|
{
|
|
//find magnitude of each point
|
|
float p1sq, p2sq, p3sq;
|
|
|
|
Vector3Extensions.Dot2D(ref p1, ref p1, out p1sq);
|
|
Vector3Extensions.Dot2D(ref p2, ref p2, out p2sq);
|
|
Vector3Extensions.Dot2D(ref p3, ref p3, out p3sq);
|
|
|
|
c.X = (p1sq * (p2.Z - p3.Z) + p2sq * (p3.Z - p1.Z) + p3sq * (p1.Z - p2.Z)) / (2 * cp);
|
|
c.Z = (p1sq * (p3.X - p2.X) + p2sq * (p1.X - p3.X) + p3sq * (p2.X - p1.X)) / (2 * cp);
|
|
|
|
float dx = p1.X - c.X;
|
|
float dy = p1.Z - c.Z;
|
|
r = (float)Math.Sqrt(dx * dx + dy * dy);
|
|
return true;
|
|
}
|
|
|
|
c.X = p1.X;
|
|
c.Z = p1.Z;
|
|
r = 0;
|
|
return false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Find the distance from a point to a triangle mesh.
|
|
/// </summary>
|
|
/// <param name="p">Individual point</param>
|
|
/// <param name="verts">Vertex array</param>
|
|
/// <param name="tris">Triange list</param>
|
|
/// <returns>The distance</returns>
|
|
private float DistanceToTriMesh(Vector3 p, List<Vector3> verts, List<TriangleData> tris)
|
|
{
|
|
float dmin = float.MaxValue;
|
|
|
|
for (int i = 0; i < tris.Count; i++)
|
|
{
|
|
Vector3 va = verts[tris[i].VertexHash0];
|
|
Vector3 vb = verts[tris[i].VertexHash1];
|
|
Vector3 vc = verts[tris[i].VertexHash2];
|
|
float d = Distance.PointToTriangle(p, va, vb, vc);
|
|
if (d < dmin)
|
|
dmin = d;
|
|
}
|
|
|
|
if (dmin == float.MaxValue)
|
|
return -1;
|
|
|
|
return dmin;
|
|
}
|
|
|
|
/// <summary>
|
|
/// The MeshData struct contains information about vertex and triangle base and offset values for array indices
|
|
/// </summary>
|
|
public struct MeshData
|
|
{
|
|
public int VertexIndex;
|
|
public int VertexCount;
|
|
public int TriangleIndex;
|
|
public int TriangleCount;
|
|
}
|
|
|
|
/// <summary>
|
|
/// The triangle info contains three vertex hashes and a flag
|
|
/// </summary>
|
|
public struct TriangleData
|
|
{
|
|
public int VertexHash0;
|
|
public int VertexHash1;
|
|
public int VertexHash2;
|
|
public int Flags; //indicates which 3 vertices are part of the polygon
|
|
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="TriangleData" /> struct.
|
|
/// </summary>
|
|
/// <param name="hash0">Vertex A</param>
|
|
/// <param name="hash1">Vertex B</param>
|
|
/// <param name="hash2">Vertex C</param>
|
|
public TriangleData(int hash0, int hash1, int hash2)
|
|
{
|
|
VertexHash0 = hash0;
|
|
VertexHash1 = hash1;
|
|
VertexHash2 = hash2;
|
|
Flags = 0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="TriangleData" /> struct.
|
|
/// </summary>
|
|
/// <param name="hash0">Vertex A</param>
|
|
/// <param name="hash1">Vertex B</param>
|
|
/// <param name="hash2">Vertex C</param>
|
|
/// <param name="flags">The triangle flags</param>
|
|
public TriangleData(int hash0, int hash1, int hash2, int flags)
|
|
{
|
|
VertexHash0 = hash0;
|
|
VertexHash1 = hash1;
|
|
VertexHash2 = hash2;
|
|
Flags = flags;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="TriangleData" /> struct.
|
|
/// </summary>
|
|
/// <param name="data">The triangle itself</param>
|
|
/// <param name="verts">The list of all the vertices</param>
|
|
/// <param name="vpoly">The list of the polygon's vertices</param>
|
|
public TriangleData(TriangleData data, List<Vector3> verts, Vector3[] vpoly, int npoly)
|
|
{
|
|
VertexHash0 = data.VertexHash0;
|
|
VertexHash1 = data.VertexHash1;
|
|
VertexHash2 = data.VertexHash2;
|
|
Flags = GetTriFlags(ref data, verts, vpoly, npoly);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets a triangle's particular vertex
|
|
/// </summary>
|
|
/// <param name="index">Vertex index</param>
|
|
/// <returns>Triangle vertex hash</returns>
|
|
public int this[int index]
|
|
{
|
|
get
|
|
{
|
|
switch (index)
|
|
{
|
|
case 0:
|
|
return VertexHash0;
|
|
case 1:
|
|
return VertexHash1;
|
|
case 2:
|
|
default:
|
|
return VertexHash2;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Determine which edges of the triangle are part of the polygon
|
|
/// </summary>
|
|
/// <param name="t">A triangle.</param>
|
|
/// <param name="verts">The vertex buffer that the triangle is referencing.</param>
|
|
/// <param name="vpoly">Polygon vertex data.</param>
|
|
/// <returns>The triangle's flags.</returns>
|
|
public static int GetTriFlags(ref TriangleData t, List<Vector3> verts, Vector3[] vpoly, int npoly)
|
|
{
|
|
int flags = 0;
|
|
|
|
//the triangle flags store five bits ?0?0? (like 10001, 10101, etc..)
|
|
//each bit stores whether two vertices are close enough to a polygon edge
|
|
//since triangle has three vertices, there are three distinct pairs of vertices (va,vb), (vb,vc) and (vc,va)
|
|
flags |= GetEdgeFlags(verts[t.VertexHash0], verts[t.VertexHash1], vpoly, npoly) << 0;
|
|
flags |= GetEdgeFlags(verts[t.VertexHash1], verts[t.VertexHash2], vpoly, npoly) << 2;
|
|
flags |= GetEdgeFlags(verts[t.VertexHash2], verts[t.VertexHash0], vpoly, npoly) << 4;
|
|
|
|
return flags;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// The EdgeInfo struct contains two enpoints and the faces/polygons to the left and right of that edge.
|
|
/// </summary>
|
|
private struct EdgeInfo
|
|
{
|
|
public int EndPt0;
|
|
public int EndPt1;
|
|
public int RightFace;
|
|
public int LeftFace;
|
|
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="EdgeInfo"/> struct.
|
|
/// </summary>
|
|
/// <param name="endPt0">Point A</param>
|
|
/// <param name="endPt1">Point B</param>
|
|
/// <param name="rightFace">The face to the left of the edge</param>
|
|
/// <param name="leftFace">The face to the right of the edge</param>
|
|
public EdgeInfo(int endPt0, int endPt1, int rightFace, int leftFace)
|
|
{
|
|
this.EndPt0 = endPt0;
|
|
this.EndPt1 = endPt1;
|
|
this.RightFace = rightFace;
|
|
this.LeftFace = leftFace;
|
|
}
|
|
|
|
/// <summary>
|
|
/// If the left face is undefined, assign it a value
|
|
/// </summary>
|
|
/// <param name="e">The current edge</param>
|
|
/// <param name="s">Endpoint A</param>
|
|
/// <param name="t">Endpoint B</param>
|
|
/// <param name="f">The face value</param>
|
|
public static void UpdateLeftFace(ref EdgeInfo e, int s, int t, int f)
|
|
{
|
|
if (e.EndPt0 == s && e.EndPt1 == t && e.LeftFace == (int)EdgeValues.Undefined)
|
|
e.LeftFace = f;
|
|
else if (e.EndPt1 == s && e.EndPt0 == t && e.RightFace == (int)EdgeValues.Undefined)
|
|
e.RightFace = f;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// The SamplingData struct contains information about sampled vertices from the PolyMesh
|
|
/// </summary>
|
|
private struct SamplingData
|
|
{
|
|
public int X;
|
|
public int Y;
|
|
public int Z;
|
|
public bool IsSampled;
|
|
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="SamplingData"/> struct.
|
|
/// </summary>
|
|
/// <param name="x">The x-coordinate</param>
|
|
/// <param name="y">The y-coordinate</param>
|
|
/// <param name="z">The z-coordinate</param>
|
|
/// <param name="isSampled">Whether or not the vertex has been sampled</param>
|
|
public SamplingData(int x, int y, int z, bool isSampled)
|
|
{
|
|
this.X = x;
|
|
this.Y = y;
|
|
this.Z = z;
|
|
this.IsSampled = isSampled;
|
|
}
|
|
}
|
|
}
|
|
}
|