/// <summary>
/// Create the first faces from (dimension + 1) vertices.
/// </summary>
private SimplexWrap <VERTEX>[] InitiateFaceDatabase()
{
SimplexWrap <VERTEX>[] faces = new SimplexWrap <VERTEX> [Dimension + 1];
for (var i = 0; i < Dimension + 1; i++)
{
var vertices = Vertices.Where((_, j) => i != j).ToArray(); // Skips the i-th vertex
var newFace = new SimplexWrap <VERTEX>(Dimension, new VertexBuffer <VERTEX>());
newFace.Vertices = vertices;
Array.Sort(vertices, new VertexIdComparer <VERTEX>());
CalculateFacePlane(newFace);
faces[i] = newFace;
}
// update the adjacency (check all pairs of faces)
for (var i = 0; i < Dimension; i++)
{
for (var j = i + 1; j < Dimension + 1; j++)
{
UpdateAdjacency(faces[i], faces[j]);
}
}
return(faces);
}
public ObjectBuffer(int dimension)
{
Dimension = dimension;
ConvexSimplexs = new List <SimplexWrap <VERTEX> >();
MaxDistance = float.NegativeInfinity;
UnprocessedFaces = new SimplexList <VERTEX>();
AffectedFaceBuffer = new List <SimplexWrap <VERTEX> >();
TraverseStack = new Stack <SimplexWrap <VERTEX> >();
SingularVertices = new HashSet <VERTEX>();
ConeFaceBuffer = new List <DeferredSimplex <VERTEX> >();
UpdateBuffer = new SimplexWrap <VERTEX> [Dimension];
UpdateIndices = new int[Dimension];
ObjectManager = new ObjectManager <VERTEX>(Dimension);
EmptyBuffer = new VertexBuffer <VERTEX>();
BeyondBuffer = new VertexBuffer <VERTEX>();
ConnectorTable = new ConnectorList <VERTEX> [CONNECTOR_TABLE_SIZE];
for (int i = 0; i < CONNECTOR_TABLE_SIZE; i++)
{
ConnectorTable[i] = new ConnectorList <VERTEX>();
}
}
/// <summary>
/// Adds the element to the beginning.
/// </summary>
private void AddFirst(SimplexWrap <VERTEX> face)
{
face.InList = true;
First.Previous = face;
face.Next = First;
First = face;
}
/// <summary>
/// Removes the element from the list.
/// </summary>
internal void Remove(SimplexWrap <VERTEX> face)
{
if (!face.InList)
{
return;
}
face.InList = false;
if (face.Previous != null)
{
face.Previous.Next = face.Next;
}
else if (face.Previous == null)
{
First = face.Next;
}
if (face.Next != null)
{
face.Next.Previous = face.Previous;
}
else if (face.Next == null)
{
Last = face.Previous;
}
face.Next = null;
face.Previous = null;
}
/// <summary>
///
/// </summary>
internal SimplexWrap(int dimension, VertexBuffer <VERTEX> beyondList)
{
AdjacentFaces = new SimplexWrap <VERTEX> [dimension];
VerticesBeyond = beyondList;
Normal = new float[dimension];
Vertices = new VERTEX[dimension];
}
/// <summary>
/// Recursively traverse all the relevant faces.
/// </summary>
private void TraverseAffectedFaces(SimplexWrap <VERTEX> currentFace)
{
Buffer.TraverseStack.Clear();
Buffer.TraverseStack.Push(currentFace);
currentFace.Tag = 1;
while (Buffer.TraverseStack.Count > 0)
{
SimplexWrap <VERTEX> top = Buffer.TraverseStack.Pop();
for (int i = 0; i < Dimension; i++)
{
SimplexWrap <VERTEX> adjFace = top.AdjacentFaces[i];
if (adjFace == null)
{
throw new NullReferenceException("(2) Adjacent Face should never be null");
}
if (adjFace.Tag == 0 && MathHelper <VERTEX> .GetVertexDistance(Buffer.CurrentVertex, adjFace) >= PLANE_DISTANCE_TOLERANCE)
{
Buffer.AffectedFaceBuffer.Add(adjFace);
adjFace.Tag = 1;
Buffer.TraverseStack.Push(adjFace);
}
}
}
}
public void Clear()
{
UpdateBuffer = new SimplexWrap <VERTEX> [Dimension];
UpdateIndices = new int[Dimension];
InputVertices = null;
CurrentVertex = null;
FurthestVertex = null;
MaxDistance = float.NegativeInfinity;
ConvexSimplexs.Clear();
AffectedFaceBuffer.Clear();
TraverseStack.Clear();
SingularVertices.Clear();
ConeFaceBuffer.Clear();
ObjectManager.Clear();
UnprocessedFaces.Clear();
EmptyBuffer.Clear();
BeyondBuffer.Clear();
for (int i = 0; i < CONNECTOR_TABLE_SIZE; i++)
{
ConnectorTable[i].Clear();
}
}
/// <summary>
/// Check if 2 faces are adjacent and if so, update their AdjacentFaces array.
/// </summary>
private void UpdateAdjacency(SimplexWrap <VERTEX> l, SimplexWrap <VERTEX> r)
{
VERTEX[] lv = l.Vertices;
VERTEX[] rv = r.Vertices;
int i;
// reset marks on the 1st face
for (i = 0; i < Dimension; i++)
{
lv[i].Tag = 0;
}
// mark all vertices on the 2nd face
for (i = 0; i < Dimension; i++)
{
rv[i].Tag = 1;
}
// find the 1st false index
for (i = 0; i < Dimension; i++)
{
if (lv[i].Tag == 0)
{
break;
}
}
// no vertex was marked
if (i == Dimension)
{
return;
}
// check if only 1 vertex wasn't marked
for (int j = i + 1; j < Dimension; j++)
{
if (lv[j].Tag == 0)
{
return;
}
}
// if we are here, the two faces share an edge
l.AdjacentFaces[i] = r;
// update the adj. face on the other face - find the vertex that remains marked
for (i = 0; i < Dimension; i++)
{
lv[i].Tag = 0;
}
for (i = 0; i < Dimension; i++)
{
if (rv[i].Tag == 1)
{
break;
}
}
r.AdjacentFaces[i] = l;
}
/// <summary>
/// Used by update faces.
/// </summary>
private void FindBeyondVertices(SimplexWrap <VERTEX> face, VertexBuffer <VERTEX> beyond, VertexBuffer <VERTEX> beyond1)
{
var beyondVertices = Buffer.BeyondBuffer;
Buffer.MaxDistance = float.NegativeInfinity;
Buffer.FurthestVertex = null;
VERTEX v;
int count = beyond1.Count;
for (int i = 0; i < count; i++)
{
beyond1[i].Tag = 1;
}
Buffer.CurrentVertex.Tag = 0;
count = beyond.Count;
for (int i = 0; i < count; i++)
{
v = beyond[i];
if (ReferenceEquals(v, Buffer.CurrentVertex))
{
continue;
}
v.Tag = 0;
IsBeyond(face, beyondVertices, v);
}
count = beyond1.Count;
for (int i = 0; i < count; i++)
{
v = beyond1[i];
if (v.Tag == 1)
{
IsBeyond(face, beyondVertices, v);
}
}
face.FurthestVertex = Buffer.FurthestVertex;
// Pull the old switch a roo
var temp = face.VerticesBeyond;
face.VerticesBeyond = beyondVertices;
if (temp.Count > 0)
{
temp.Clear();
}
Buffer.BeyondBuffer = temp;
}
internal void DepositFace(SimplexWrap <VERTEX> face)
{
face.Previous = null;
face.Next = null;
for (int i = 0; i < Dimension; i++)
{
face.AdjacentFaces[i] = null;
}
RecycledFaceStack.Push(face);
}
/// <summary>
/// Creates a new deferred face.
/// </summary>
private DeferredSimplex <VERTEX> MakeDeferredFace(SimplexWrap <VERTEX> face, int faceIndex, SimplexWrap <VERTEX> pivot, int pivotIndex, SimplexWrap <VERTEX> oldFace)
{
DeferredSimplex <VERTEX> ret = Buffer.ObjectManager.GetDeferredSimplex();
ret.Face = face;
ret.FaceIndex = faceIndex;
ret.Pivot = pivot;
ret.PivotIndex = pivotIndex;
ret.OldFace = oldFace;
return(ret);
}
/// <summary>
/// Check if the vertex is "visible" from the face.
/// The vertex is "over face" if the return value is > Constants.PlaneDistanceTolerance.
/// </summary>
/// <returns>The vertex is "over face" if the result is positive.</returns>
internal static float GetVertexDistance(VERTEX v, SimplexWrap <VERTEX> f)
{
float[] normal = f.Normal;
float[] p = v.Position;
float distance = f.Offset;
for (int i = 0; i < v.Dimension; i++)
{
distance += normal[i] * p[i];
}
return(distance);
}
/// <summary>
/// Check whether the vertex v is beyond the given face. If so, add it to beyondVertices.
/// </summary>
private void IsBeyond(SimplexWrap <VERTEX> face, VertexBuffer <VERTEX> beyondVertices, VERTEX v)
{
float distance = MathHelper <VERTEX> .GetVertexDistance(v, face);
if (distance >= PLANE_DISTANCE_TOLERANCE)
{
if (distance > Buffer.MaxDistance)
{
Buffer.MaxDistance = distance;
Buffer.FurthestVertex = v;
}
beyondVertices.Add(v);
}
}
/// <summary>
/// Used in the "initialization" code.
/// </summary>
private void FindBeyondVertices(SimplexWrap <VERTEX> face)
{
VertexBuffer <VERTEX> beyondVertices = face.VerticesBeyond;
Buffer.MaxDistance = float.NegativeInfinity;
Buffer.FurthestVertex = default(VERTEX);
int count = Buffer.InputVertices.Count;
for (int i = 0; i < count; i++)
{
IsBeyond(face, beyondVertices, Buffer.InputVertices[i]);
}
face.FurthestVertex = Buffer.FurthestVertex;
}
/// <summary>
/// Calculates the normal and offset of the hyper-plane given by the face's vertices.
/// </summary>
private bool CalculateFacePlane(SimplexWrap <VERTEX> face)
{
VERTEX[] vertices = face.Vertices;
float[] normal = face.Normal;
MathHelper <VERTEX> .FindNormalVector(vertices, normal);
if (float.IsNaN(normal[0]))
{
return(false);
}
float offset = 0.0f;
float centerDistance = 0.0f;
float[] fi = vertices[0].Position;
for (int i = 0; i < Dimension; i++)
{
float n = normal[i];
offset += n * fi[i];
centerDistance += n * Centroid[i];
}
face.Offset = -offset;
centerDistance -= offset;
if (centerDistance > 0)
{
for (int i = 0; i < Dimension; i++)
{
normal[i] = -normal[i];
}
face.Offset = offset;
face.IsNormalFlipped = true;
}
else
{
face.IsNormalFlipped = false;
}
return(true);
}
/// <summary>
/// Updates the connector.
/// </summary>
internal void Update(SimplexWrap <VERTEX> face, int edgeIndex, int dim)
{
Face = face;
EdgeIndex = edgeIndex;
uint hashCode = 31;
var vs = face.Vertices;
for (int i = 0, c = 0; i < dim; i++)
{
if (i != edgeIndex)
{
int v = vs[i].Id;
Vertices[c++] = v;
hashCode += unchecked (23 * hashCode + (uint)v);
}
}
HashCode = hashCode;
}
/// <summary>
/// Adds a face to the list.
/// </summary>
internal void Add(SimplexWrap <VERTEX> face)
{
if (face.InList)
{
if (First.VerticesBeyond.Count < face.VerticesBeyond.Count)
{
Remove(face);
AddFirst(face);
}
return;
}
face.InList = true;
if (First != null && First.VerticesBeyond.Count < face.VerticesBeyond.Count)
{
First.Previous = face;
face.Next = First;
First = face;
}
else
{
if (Last != null)
{
Last.Next = face;
}
face.Previous = Last;
Last = face;
if (First == null)
{
First = face;
}
}
}
internal void Clear()
{
First = null;
Last = null;
}
public void Generate(IList <VERTEX> input, bool assignIds = true, bool checkInput = false)
{
Clear();
Buffer = new ObjectBuffer <VERTEX>(Dimension);
int inputCount = input.Count;
if (inputCount < Dimension + 1)
{
return;
}
Buffer.AddInput(input, assignIds, checkInput);
InitConvexHull();
// Expand the convex hull and faces.
while (Buffer.UnprocessedFaces.First != null)
{
SimplexWrap <VERTEX> currentFace = Buffer.UnprocessedFaces.First;
Buffer.CurrentVertex = currentFace.FurthestVertex;
UpdateCenter();
// The affected faces get tagged
TagAffectedFaces(currentFace);
// Create the cone from the currentVertex and the affected faces horizon.
if (!Buffer.SingularVertices.Contains(Buffer.CurrentVertex) && CreateCone())
{
CommitCone();
}
else
{
HandleSingular();
}
// Need to reset the tags
int count = Buffer.AffectedFaceBuffer.Count;
for (int i = 0; i < count; i++)
{
Buffer.AffectedFaceBuffer[i].Tag = 0;
}
}
for (int i = 0; i < Buffer.ConvexSimplexs.Count; i++)
{
SimplexWrap <VERTEX> wrap = Buffer.ConvexSimplexs[i];
wrap.Tag = i;
Simplexs.Add(new Simplex <VERTEX>(Dimension));
}
for (int i = 0; i < Buffer.ConvexSimplexs.Count; i++)
{
SimplexWrap <VERTEX> wrap = Buffer.ConvexSimplexs[i];
Simplex <VERTEX> simplex = Simplexs[i];
simplex.IsNormalFlipped = wrap.IsNormalFlipped;
simplex.Offset = wrap.Offset;
for (int j = 0; j < Dimension; j++)
{
simplex.Normal[j] = wrap.Normal[j];
simplex.Vertices[j] = wrap.Vertices[j];
if (wrap.AdjacentFaces[j] != null)
{
simplex.Adjacent[j] = Simplexs[wrap.AdjacentFaces[j].Tag];
}
else
{
simplex.Adjacent[j] = null;
}
}
simplex.CalculateCentroid();
}
Buffer.Clear();
Buffer = null;
}
/// <summary>
/// Commits a cone and adds a vertex to the convex hull.
/// </summary>
private void CommitCone()
{
// Add the current vertex.
Vertices.Add(Buffer.CurrentVertex);
// Fill the adjacency.
for (int i = 0; i < Buffer.ConeFaceBuffer.Count; i++)
{
DeferredSimplex <VERTEX> face = Buffer.ConeFaceBuffer[i];
SimplexWrap <VERTEX> newFace = face.Face;
SimplexWrap <VERTEX> adjacentFace = face.Pivot;
SimplexWrap <VERTEX> oldFace = face.OldFace;
int orderedPivotIndex = face.FaceIndex;
newFace.AdjacentFaces[orderedPivotIndex] = adjacentFace;
adjacentFace.AdjacentFaces[face.PivotIndex] = newFace;
// let there be a connection.
for (int j = 0; j < Dimension; j++)
{
if (j == orderedPivotIndex)
{
continue;
}
SimplexConnector <VERTEX> connector = Buffer.ObjectManager.GetConnector();
connector.Update(newFace, j, Dimension);
ConnectFace(connector);
}
// This could slightly help...
if (adjacentFace.VerticesBeyond.Count < oldFace.VerticesBeyond.Count)
{
FindBeyondVertices(newFace, adjacentFace.VerticesBeyond, oldFace.VerticesBeyond);
}
else
{
FindBeyondVertices(newFace, oldFace.VerticesBeyond, adjacentFace.VerticesBeyond);
}
// This face will definitely lie on the hull
if (newFace.VerticesBeyond.Count == 0)
{
Buffer.ConvexSimplexs.Add(newFace);
Buffer.UnprocessedFaces.Remove(newFace);
Buffer.ObjectManager.DepositVertexBuffer(newFace.VerticesBeyond);
newFace.VerticesBeyond = Buffer.EmptyBuffer;
}
else // Add the face to the list
{
Buffer.UnprocessedFaces.Add(newFace);
}
// recycle the object.
Buffer.ObjectManager.DepositDeferredSimplex(face);
}
// Recycle the affected faces.
for (int fIndex = 0; fIndex < Buffer.AffectedFaceBuffer.Count; fIndex++)
{
var face = Buffer.AffectedFaceBuffer[fIndex];
Buffer.UnprocessedFaces.Remove(face);
Buffer.ObjectManager.DepositFace(face);
}
}
/// <summary>
/// Tags all faces seen from the current vertex with 1.
/// </summary>
private void TagAffectedFaces(SimplexWrap <VERTEX> currentFace)
{
Buffer.AffectedFaceBuffer.Clear();
Buffer.AffectedFaceBuffer.Add(currentFace);
TraverseAffectedFaces(currentFace);
}
/// <summary>
/// Removes the faces "covered" by the current vertex and adds the newly created ones.
/// </summary>
private bool CreateCone()
{
int currentVertexIndex = Buffer.CurrentVertex.Id;
Buffer.ConeFaceBuffer.Clear();
for (int fIndex = 0; fIndex < Buffer.AffectedFaceBuffer.Count; fIndex++)
{
SimplexWrap <VERTEX> oldFace = Buffer.AffectedFaceBuffer[fIndex];
// Find the faces that need to be updated
int updateCount = 0;
for (int i = 0; i < Dimension; i++)
{
SimplexWrap <VERTEX> af = oldFace.AdjacentFaces[i];
if (af == null)
{
throw new NullReferenceException("(3) Adjacent Face should never be null");
}
if (af.Tag == 0) // Tag == 0 when oldFaces does not contain af
{
Buffer.UpdateBuffer[updateCount] = af;
Buffer.UpdateIndices[updateCount] = i;
++updateCount;
}
}
for (int i = 0; i < updateCount; i++)
{
SimplexWrap <VERTEX> adjacentFace = Buffer.UpdateBuffer[i];
int oldFaceAdjacentIndex = 0;
SimplexWrap <VERTEX>[] adjFaceAdjacency = adjacentFace.AdjacentFaces;
for (int j = 0; j < Dimension; j++)
{
if (object.ReferenceEquals(oldFace, adjFaceAdjacency[j]))
{
oldFaceAdjacentIndex = j;
break;
}
}
// Index of the face that corresponds to this adjacent face
int forbidden = Buffer.UpdateIndices[i];
SimplexWrap <VERTEX> newFace;
int oldVertexIndex;
VERTEX[] vertices;
newFace = Buffer.ObjectManager.GetFace();
vertices = newFace.Vertices;
for (int j = 0; j < Dimension; j++)
{
vertices[j] = oldFace.Vertices[j];
}
oldVertexIndex = vertices[forbidden].Id;
int orderedPivotIndex;
// correct the ordering
if (currentVertexIndex < oldVertexIndex)
{
orderedPivotIndex = 0;
for (int j = forbidden - 1; j >= 0; j--)
{
if (vertices[j].Id > currentVertexIndex)
{
vertices[j + 1] = vertices[j];
}
else
{
orderedPivotIndex = j + 1;
break;
}
}
}
else
{
orderedPivotIndex = Dimension - 1;
for (int j = forbidden + 1; j < Dimension; j++)
{
if (vertices[j].Id < currentVertexIndex)
{
vertices[j - 1] = vertices[j];
}
else
{
orderedPivotIndex = j - 1;
break;
}
}
}
vertices[orderedPivotIndex] = Buffer.CurrentVertex;
if (!CalculateFacePlane(newFace))
{
return(false);
}
Buffer.ConeFaceBuffer.Add(MakeDeferredFace(newFace, orderedPivotIndex, adjacentFace, oldFaceAdjacentIndex, oldFace));
}
}
return(true);
}
