/// <summary>
/// Finds the beyond vertices.
/// </summary>
/// <param name="face">The face.</param>
/// <param name="beyond">The beyond.</param>
private void FindBeyondVertices(ConvexFaceInternal face, IndexBuffer beyond)
{
var beyondVertices = BeyondBuffer;
MaxDistance = double.NegativeInfinity;
FurthestVertex = 0;
int v;
for (var i = 0; i < beyond.Count; i++)
{
v = beyond[i];
if (v == CurrentVertex)
{
continue;
}
IsBeyond(face, beyondVertices, v);
}
face.FurthestVertex = FurthestVertex;
// Pull the old switch a roo (switch the face beyond buffers)
var temp = face.VerticesBeyond;
face.VerticesBeyond = beyondVertices;
if (temp.Count > 0)
{
temp.Clear();
}
BeyondBuffer = temp;
}
/// <summary>
/// Removes the element from the list.
/// </summary>
/// <param name="face">The face.</param>
public void Remove(ConvexFaceInternal face)
{
if (!face.InList)
{
return;
}
face.InList = false;
if (face.Previous != null)
{
face.Previous.Next = face.Next;
}
else if (/*first == face*/ face.Previous == null)
{
First = face.Next;
}
if (face.Next != null)
{
face.Next.Previous = face.Previous;
}
else if (/*last == face*/ face.Next == null)
{
last = face.Previous;
}
face.Next = null;
face.Previous = null;
}
/// <summary>
/// Check whether the vertex v is beyond the given face. If so, add it to beyondVertices.
/// </summary>
/// <param name="face">The face.</param>
/// <param name="beyondVertices">The beyond vertices.</param>
/// <param name="v">The v.</param>
private void IsBeyond(ConvexFaceInternal face, IndexBuffer beyondVertices, int v)
{
var distance = mathHelper.GetVertexDistance(v, face);
if (distance >= PlaneDistanceTolerance)
{
if (distance > MaxDistance)
{
// If it's within the tolerance distance, use the lex. larger point
if (distance - MaxDistance < PlaneDistanceTolerance)
{ // todo: why is this LexCompare necessary. Would seem to favor x over y over z (etc.)?
if (LexCompare(v, FurthestVertex) > 0)
{
MaxDistance = distance;
FurthestVertex = v;
}
}
else
{
MaxDistance = distance;
FurthestVertex = v;
}
}
beyondVertices.Add(v);
}
}
/// <summary>
/// Adds a face to the list.
/// </summary>
/// <param name="face">The face.</param>
public void Add(ConvexFaceInternal 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;
}
}
}
/// <summary>
/// Adds the element to the beginning.
/// </summary>
/// <param name="face">The face.</param>
private void AddFirst(ConvexFaceInternal face)
{
face.InList = true;
First.Previous = face;
face.Next = First;
First = face;
}
/// <summary>
/// Check if 2 faces are adjacent and if so, update their AdjacentFaces array.
/// </summary>
/// <param name="l">The l.</param>
/// <param name="r">The r.</param>
private void UpdateAdjacency(ConvexFaceInternal l, ConvexFaceInternal r)
{
var lv = l.Vertices;
var rv = r.Vertices;
int i;
// reset marks on the 1st face
for (i = 0; i < lv.Length; i++)
{
VertexVisited[lv[i]] = false;
}
// mark all vertices on the 2nd face
for (i = 0; i < rv.Length; i++)
{
VertexVisited[rv[i]] = true;
}
// find the 1st false index
for (i = 0; i < lv.Length; i++)
{
if (!VertexVisited[lv[i]])
{
break;
}
}
// no vertex was marked
if (i == NumOfDimensions)
{
return;
}
// check if only 1 vertex wasn't marked
for (var j = i + 1; j < lv.Length; j++)
{
if (!VertexVisited[lv[j]])
{
return;
}
}
// if we are here, the two faces share an edge
l.AdjacentFaces[i] = r.Index;
// update the adj. face on the other face - find the vertex that remains marked
for (i = 0; i < lv.Length; i++)
{
VertexVisited[lv[i]] = false;
}
for (i = 0; i < rv.Length; i++)
{
if (VertexVisited[rv[i]])
{
break;
}
}
r.AdjacentFaces[i] = l.Index;
}
/// <summary>
/// Check if the vertex is "visible" from the face.
/// The vertex is "over face" if the return value is > Constants.PlaneDistanceTolerance.
/// </summary>
/// <param name="v">The v.</param>
/// <param name="f">The f.</param>
/// <returns>The vertex is "over face" if the result is positive.</returns>
internal double GetVertexDistance(int v, ConvexFaceInternal f)
{
var normal = f.Normal;
var x = v * Dimension;
var distance = f.Offset;
for (var i = 0; i < normal.Length; i++)
{
distance += normal[i] * PositionData[x + i];
}
return(distance);
}
/// <summary>
/// Reallocate the face pool, including the AffectedFaceFlags
/// </summary>
private void ReallocateFacePool()
{
var newPool = new ConvexFaceInternal[2 * FacePoolCapacity];
var newTags = new bool[2 * FacePoolCapacity];
Array.Copy(FacePool, newPool, FacePoolCapacity);
Buffer.BlockCopy(Hull.AffectedFaceFlags, 0, newTags, 0, FacePoolCapacity * sizeof(bool));
FacePoolCapacity = 2 * FacePoolCapacity;
Hull.FacePool = newPool;
FacePool = newPool;
Hull.AffectedFaceFlags = newTags;
}
/// <summary>
/// Initializes a new instance of the <see cref="ConvexHullAlgorithm" /> class.
/// </summary>
/// <param name="vertices">The vertices.</param>
/// <param name="lift">if set to <c>true</c> [lift].</param>
/// <param name="PlaneDistanceTolerance">The plane distance tolerance.</param>
/// <exception cref="System.InvalidOperationException">Dimension of the input must be 2 or greater.</exception>
/// <exception cref="System.ArgumentException">There are too few vertices (m) for the n-dimensional space. (m must be greater +
/// than the n, but m is + NumberOfVertices + and n is + NumOfDimensions</exception>
/// <exception cref="InvalidOperationException">PointTranslationGenerator cannot be null if PointTranslationType is enabled.
/// or
/// Dimension of the input must be 2 or greater.</exception>
/// <exception cref="ArgumentException">There are too few vertices (m) for the n-dimensional space. (m must be greater " +
/// "than the n, but m is " + NumberOfVertices + " and n is " + Dimension</exception>
private ConvexHullAlgorithm(IVertex[] vertices, bool lift, double PlaneDistanceTolerance)
{
IsLifted = lift;
Vertices = vertices;
NumberOfVertices = vertices.Length;
NumOfDimensions = DetermineDimension();
if (IsLifted)
{
NumOfDimensions++;
}
if (NumOfDimensions < 2)
{
throw new InvalidOperationException("Dimension of the input must be 2 or greater.");
}
if (NumberOfVertices <= NumOfDimensions)
{
throw new ArgumentException(
"There are too few vertices (m) for the n-dimensional space. (m must be greater " +
"than the n, but m is " + NumberOfVertices + " and n is " + NumOfDimensions);
}
this.PlaneDistanceTolerance = PlaneDistanceTolerance;
UnprocessedFaces = new FaceList();
ConvexFaces = new IndexBuffer();
FacePool = new ConvexFaceInternal[(NumOfDimensions + 1) * 10]; // must be initialized before object manager
AffectedFaceFlags = new bool[(NumOfDimensions + 1) * 10];
ObjectManager = new ObjectManager(this);
Center = new double[NumOfDimensions];
TraverseStack = new IndexBuffer();
UpdateBuffer = new int[NumOfDimensions];
UpdateIndices = new int[NumOfDimensions];
EmptyBuffer = new IndexBuffer();
AffectedFaceBuffer = new IndexBuffer();
ConeFaceBuffer = new SimpleList <DeferredFace>();
SingularVertices = new HashSet <int>();
BeyondBuffer = new IndexBuffer();
ConnectorTable = new ConnectorList[Constants.ConnectorTableSize];
for (var i = 0; i < Constants.ConnectorTableSize; i++)
{
ConnectorTable[i] = new ConnectorList();
}
VertexVisited = new bool[NumberOfVertices];
Positions = new double[NumberOfVertices * NumOfDimensions];
boundingBoxPoints = new List <int> [NumOfDimensions];
minima = new double[NumOfDimensions];
maxima = new double[NumOfDimensions];
mathHelper = new MathHelper(NumOfDimensions, Positions);
}
/// <summary>
/// Creates a new deferred face.
/// </summary>
/// <param name="face">The face.</param>
/// <param name="faceIndex">Index of the face.</param>
/// <param name="pivot">The pivot.</param>
/// <param name="pivotIndex">Index of the pivot.</param>
/// <param name="oldFace">The old face.</param>
/// <returns>DeferredFace.</returns>
private DeferredFace MakeDeferredFace(ConvexFaceInternal face, int faceIndex, ConvexFaceInternal pivot,
int pivotIndex, ConvexFaceInternal oldFace)
{
var ret = ObjectManager.GetDeferredFace();
ret.Face = face;
ret.FaceIndex = faceIndex;
ret.Pivot = pivot;
ret.PivotIndex = pivotIndex;
ret.OldFace = oldFace;
return(ret);
}
/// <summary>
/// Create a new face and put it in the pool.
/// </summary>
/// <returns>System.Int32.</returns>
private int CreateFace()
{
var index = FacePoolSize;
var face = new ConvexFaceInternal(Dimension, index, GetVertexBuffer());
FacePoolSize++;
if (FacePoolSize > FacePoolCapacity)
{
ReallocateFacePool();
}
FacePool[index] = face;
return(index);
}
/// <summary>
/// Used in the "initialization" code.
/// </summary>
/// <param name="face">The face.</param>
private void FindBeyondVertices(ConvexFaceInternal face)
{
var beyondVertices = face.VerticesBeyond;
MaxDistance = double.NegativeInfinity;
FurthestVertex = 0;
for (var i = 0; i < NumberOfVertices; i++)
{
if (VertexVisited[i])
{
continue;
}
IsBeyond(face, beyondVertices, i);
}
face.FurthestVertex = FurthestVertex;
}
/// <summary>
/// Calculates the normal and offset of the hyper-plane given by the face's vertices.
/// </summary>
/// <param name="face">The face.</param>
/// <param name="center">The center.</param>
/// <returns><c>true</c> if XXXX, <c>false</c> otherwise.</returns>
internal bool CalculateFacePlane(ConvexFaceInternal face, double[] center)
{
var vertices = face.Vertices;
var normal = face.Normal;
FindNormalVector(vertices, normal);
if (double.IsNaN(normal[0]))
{
return(false);
}
var offset = 0.0;
var centerDistance = 0.0;
var fi = vertices[0] * Dimension;
for (var i = 0; i < Dimension; i++)
{
var n = normal[i];
offset += n * PositionData[fi + i];
centerDistance += n * center[i];
}
face.Offset = -offset;
centerDistance -= offset;
if (centerDistance > 0)
{
for (var i = 0; i < Dimension; i++)
{
normal[i] = -normal[i];
}
face.Offset = offset;
face.IsNormalFlipped = true;
}
else
{
face.IsNormalFlipped = false;
}
return(true);
}
/// <summary>
/// Tags all faces seen from the current vertex with 1.
/// </summary>
/// <param name="currentFace">The current face.</param>
private void TagAffectedFaces(ConvexFaceInternal currentFace)
{
AffectedFaceBuffer.Clear();
AffectedFaceBuffer.Add(currentFace.Index);
TraverseAffectedFaces(currentFace.Index);
}
