private void GetConvexHull()
{
SerializeVerticesToPositions();
FindBoundingBoxPoints();
ShiftAndScalePositions();
CreateInitialSimplex();
while (UnprocessedFaces.First != null)
{
ConvexFaceInternal first = UnprocessedFaces.First;
CurrentVertex = first.FurthestVertex;
UpdateCenter();
TagAffectedFaces(first);
if (!SingularVertices.Contains(CurrentVertex) && CreateCone())
{
CommitCone();
}
else
{
HandleSingular();
}
int count = AffectedFaceBuffer.Count;
for (int i = 0; i < count; i++)
{
AffectedFaceFlags[AffectedFaceBuffer[i]] = false;
}
}
}
private void RemoveUpperFaces()
{
IndexBuffer convexFaces = ConvexFaces;
int num = NumOfDimensions - 1;
for (int num2 = convexFaces.Count - 1; num2 >= 0; num2--)
{
int num3 = convexFaces[num2];
ConvexFaceInternal convexFaceInternal = FacePool[num3];
if (convexFaceInternal.Normal[num] >= 0.0)
{
for (int i = 0; i < convexFaceInternal.AdjacentFaces.Length; i++)
{
int num4 = convexFaceInternal.AdjacentFaces[i];
if (num4 >= 0)
{
ConvexFaceInternal convexFaceInternal2 = FacePool[num4];
for (int j = 0; j < convexFaceInternal2.AdjacentFaces.Length; j++)
{
if (convexFaceInternal2.AdjacentFaces[j] == num3)
{
convexFaceInternal2.AdjacentFaces[j] = -1;
}
}
}
}
convexFaces[num2] = convexFaces[convexFaces.Count - 1];
convexFaces.Pop();
}
}
}
public static double GetSimplexVolume(ConvexFaceInternal cell, IList <IVertex> vertices, SimplexVolumeBuffer buffer)
{
int[] numArray = cell.Vertices;
double[] position = vertices[numArray[0]].Position;
double[] data = buffer.Data;
int dimension = buffer.Dimension;
double num2 = 1.0;
int index = 1;
while (index < numArray.Length)
{
num2 *= index + 1;
double[] numArray4 = vertices[numArray[index]].Position;
int num4 = 0;
while (true)
{
if (num4 >= numArray4.Length)
{
index++;
break;
}
data[((num4 * dimension) + index) - 1] = numArray4[num4] - position[num4];
num4++;
}
}
return(Math.Abs(DeterminantDestructive(buffer)) / num2);
}
/// <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;
for (var i = 0; i < beyond.Count; i++)
{
var 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>
/// Recycle face for future use.
/// </summary>
void RecycleFace(ConvexFaceInternal face)
{
for (int i = 0; i < Dimension; i++)
{
face.AdjacentFaces[i] = null;
}
}
/// <summary>
/// Recursively traverse all the relevant faces.
/// </summary>
void TraverseAffectedFaces(ConvexFaceInternal currentFace)
{
TraverseStack.Clear();
TraverseStack.Push(currentFace);
currentFace.Tag = 1;
while (TraverseStack.Count > 0)
{
var top = TraverseStack.Pop();
for (int i = 0; i < Dimension; i++)
{
var adjFace = top.AdjacentFaces[i];
if (adjFace.Tag == 0 && GetVertexDistance(CurrentVertex, adjFace) >= 0)
{
AffectedFaceBuffer.Add(adjFace);
//TraverseAffectedFaces(adjFace);
adjFace.Tag = 1;
TraverseStack.Push(adjFace);
}
}
}
////for (int i = 0; i < Dimension; i++)
////{
//// var adjFace = currentFace.AdjacentFaces[i];
//// if (adjFace.Tag == 0 && GetVertexDistance(CurrentVertex, adjFace) >= 0)
//// {
//// AffectedFaceBuffer.Add(adjFace);
//// TraverseAffectedFaces(adjFace);
//// }
////}
}
/// <summary>
/// Initializes a new instance of the <see cref="ConvexFaceInternal"/> class.
/// </summary>
public ConvexFaceInternal(int dimension, VertexBuffer beyondList)
{
AdjacentFaces = new ConvexFaceInternal[dimension];
VerticesBeyond = beyondList;
Normal = new double[dimension];
Vertices = new VertexWrap[dimension];
}
public void Add(ConvexFaceInternal face)
{
if (face.InList)
{
if (this.first.VerticesBeyond.Count < face.VerticesBeyond.Count)
{
this.Remove(face);
this.AddFirst(face);
}
}
else
{
face.InList = true;
if ((this.first != null) && (this.first.VerticesBeyond.Count < face.VerticesBeyond.Count))
{
this.first.Previous = face;
face.Next = this.first;
this.first = face;
}
else
{
if (this.last != null)
{
this.last.Next = face;
}
face.Previous = this.last;
this.last = face;
this.first ??= face;
}
}
}
private void HandleSingular()
{
this.RollbackCenter();
this.SingularVertices.Add(this.CurrentVertex);
int num = 0;
while (num < this.AffectedFaceBuffer.Count)
{
ConvexFaceInternal face = this.FacePool[this.AffectedFaceBuffer[num]];
IndexBuffer verticesBeyond = face.VerticesBeyond;
int num2 = 0;
while (true)
{
if (num2 >= verticesBeyond.Count)
{
this.ConvexFaces.Add(face.Index);
this.UnprocessedFaces.Remove(face);
this.ObjectManager.DepositVertexBuffer(face.VerticesBeyond);
face.VerticesBeyond = this.EmptyBuffer;
num++;
break;
}
this.SingularVertices.Add(verticesBeyond[num2]);
num2++;
}
}
}
/// <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);
}
}
private unsafe void InitSingle()
{
int[] array = new int[this.Dimension];
for (int i = 0; i < this.Vertices.Length; i++)
{
array[i] = i;
}
ConvexFaceInternal face = this.FacePool[this.ObjectManager.GetFace()];
face.Vertices = array;
Array.Sort <int>(array);
this.MathHelper.CalculateFacePlane(face, this.Center);
if (face.Normal[this.Dimension - 1] >= 0.0)
{
int index = 0;
while (true)
{
if (index >= this.Dimension)
{
face.Offset = -face.Offset;
face.IsNormalFlipped = !face.IsNormalFlipped;
break;
}
double *numPtr1 = &(face.Normal[index]);
numPtr1[0] *= -1.0;
index++;
}
}
this.ConvexFaces.Add(face.Index);
}
/// <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>
/// Create the first faces from (dimension + 1) vertices.
/// </summary>
/// <returns></returns>
ConvexFaceInternal[] InitiateFaceDatabase()
{
var faces = new ConvexFaceInternal[Dimension + 1];
for (var i = 0; i < Dimension + 1; i++)
{
var vertices = ConvexHull.Where((_, j) => i != j).ToArray(); // Skips the i-th vertex
var newFace = new ConvexFaceInternal(Dimension, new VertexBuffer());
newFace.Vertices = vertices;
Array.Sort(vertices, VertexWrapComparer.Instance);
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);
}
/// <summary>
/// Used by update faces.
/// </summary>
/// <param name="face">The face.</param>
/// <param name="beyond">The beyond.</param>
/// <param name="beyond1">The beyond1.</param>
private void FindBeyondVertices(ConvexFaceInternal face, IndexBuffer beyond, IndexBuffer beyond1)
{
var beyondVertices = BeyondBuffer;
MaxDistance = double.NegativeInfinity;
FurthestVertex = 0;
int v;
for (var i = 0; i < beyond1.Count; i++) VertexVisited[beyond1[i]] = true;
VertexVisited[CurrentVertex] = false;
for (var i = 0; i < beyond.Count; i++)
{
v = beyond[i];
if (v == CurrentVertex) continue;
VertexVisited[v] = false;
IsBeyond(face, beyondVertices, v);
}
for (var i = 0; i < beyond1.Count; i++)
{
v = beyond1[i];
if (VertexVisited[v]) 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"></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)
{
this.first = face.Next;
}
if (face.Next != null)
{
face.Next.Previous = face.Previous;
}
else if (/*last == face*/ face.Next == null)
{
this.last = face.Previous;
}
face.Next = null;
face.Previous = null;
}
/// <summary>
/// Adds the element to the beginning.
/// </summary>
/// <param name="face"></param>
void AddFirst(ConvexFaceInternal face)
{
face.InList = true;
this.first.Previous = face;
face.Next = this.first;
this.first = face;
}
/// <summary>
/// Adds a face to the list.
/// </summary>
/// <param name="face"></param>
public void Add(ConvexFaceInternal face)
{
if (face.InList)
{
if (this.first.VerticesBeyond.Count < face.VerticesBeyond.Count)
{
Remove(face);
AddFirst(face);
}
return;
}
face.InList = true;
if (first != null && first.VerticesBeyond.Count < face.VerticesBeyond.Count)
{
this.first.Previous = face;
face.Next = this.first;
this.first = face;
}
else
{
if (this.last != null)
{
this.last.Next = face;
}
face.Previous = this.last;
this.last = face;
if (this.first == null)
{
this.first = face;
}
}
}
/// <summary>
/// Updates the connector.
/// </summary>
/// <param name="face"></param>
/// <param name="edgeIndex"></param>
/// <param name="dim"></param>
public void Update(ConvexFaceInternal face, int edgeIndex, int dim)
{
this.Face = face;
this.EdgeIndex = edgeIndex;
uint hashCode = 23;
unchecked
{
var vs = face.Vertices;
int i, c = 0;
for (i = 0; i < edgeIndex; i++)
{
this.Vertices[c++] = vs[i];
hashCode += 31 * hashCode + (uint)vs[i];
}
for (i = edgeIndex + 1; i < vs.Length; i++)
{
this.Vertices[c++] = vs[i];
hashCode += 31 * hashCode + (uint)vs[i];
}
}
this.HashCode = hashCode;
}
private void IsBeyond(ConvexFaceInternal face, IndexBuffer beyondVertices, int v)
{
double vertexDistance = mathHelper.GetVertexDistance(v, face);
if (vertexDistance >= PlaneDistanceTolerance)
{
if (vertexDistance > MaxDistance)
{
if (vertexDistance - MaxDistance < PlaneDistanceTolerance)
{
if (LexCompare(v, FurthestVertex) > 0)
{
MaxDistance = vertexDistance;
FurthestVertex = v;
}
}
else
{
MaxDistance = vertexDistance;
FurthestVertex = v;
}
}
beyondVertices.Add(v);
}
}
/// <summary>
/// Adds the element to the beginning.
/// </summary>
/// <param name="face"></param>
void AddFirst(ConvexFaceInternal face)
{
face.InList = true;
this.first.Previous = face;
face.Next = this.first;
this.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 2 faces are adjacent and if so, update their AdjacentFaces array.
/// </summary>
/// <param name="l"></param>
/// <param name="r"></param>
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 < Dimension; i++)
{
lv[i].Marked = false;
}
// mark all vertices on the 2nd face
for (i = 0; i < Dimension; i++)
{
rv[i].Marked = true;
}
// find the 1st false index
for (i = 0; i < Dimension; i++)
{
if (!lv[i].Marked)
{
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].Marked)
{
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].Marked = false;
}
for (i = 0; i < Dimension; i++)
{
if (rv[i].Marked)
{
break;
}
}
r.AdjacentFaces[i] = l;
}
public void DepositFace(ConvexFaceInternal face)
{
for (int i = 0; i < Dimension; i++)
{
face.AdjacentFaces[i] = null;
}
RecycledFaceStack.Push(face);
}
public void DepositFace(ConvexFaceInternal face)
{
for (int i = 0; i < Dimension; i++)
{
face.AdjacentFaces[i] = null;
}
RecycledFaceStack.Push(face);
}
/// <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>
internal ConvexHullAlgorithm(IVertex[] vertices, bool lift, double PlaneDistanceTolerance)
{
IsLifted = lift;
Vertices = vertices;
NumberOfVertices = vertices.Length;
NumOfDimensions = DetermineDimension();
if (IsLifted)
{
NumOfDimensions++;
}
if (NumOfDimensions < 2)
{
throw new ConvexHullGenerationException(ConvexHullCreationResultOutcome.DimensionSmallerTwo, "Dimension of the input must be 2 or greater.");
}
if (NumOfDimensions == 2)
{
throw new ConvexHullGenerationException(ConvexHullCreationResultOutcome.DimensionTwoWrongMethod, "Dimension of the input is 2. Thus you should use the Create2D method" +
" instead of the Create.");
}
if (NumberOfVertices <= NumOfDimensions)
{
throw new ConvexHullGenerationException(ConvexHullCreationResultOutcome.NotEnoughVerticesForDimension,
"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);
}
private void CreateInitialSimplex()
{
List <int> list = FindInitialPoints();
int[] array = new int[NumOfDimensions + 1];
for (int i = 0; i < NumOfDimensions + 1; i++)
{
int[] array2 = new int[NumOfDimensions];
int j = 0;
int num = 0;
for (; j <= NumOfDimensions; j++)
{
if (i != j)
{
if (j == list.Count)
{
int num2 = 0;
num2++;
}
int num3 = list[j];
array2[num++] = num3;
}
}
ConvexFaceInternal convexFaceInternal = FacePool[ObjectManager.GetFace()];
convexFaceInternal.Vertices = array2;
Array.Sort(array2);
mathHelper.CalculateFacePlane(convexFaceInternal, Center);
array[i] = convexFaceInternal.Index;
}
for (int k = 0; k < NumOfDimensions; k++)
{
for (int l = k + 1; l < NumOfDimensions + 1; l++)
{
UpdateAdjacency(FacePool[array[k]], FacePool[array[l]]);
}
}
int[] array3 = array;
foreach (int num5 in array3)
{
ConvexFaceInternal convexFaceInternal2 = FacePool[num5];
FindBeyondVertices(convexFaceInternal2);
if (convexFaceInternal2.VerticesBeyond.Count == 0)
{
ConvexFaces.Add(convexFaceInternal2.Index);
}
else
{
UnprocessedFaces.Add(convexFaceInternal2);
}
}
foreach (int item in list)
{
VertexVisited[item] = false;
}
}
public void DepositFace(int faceIndex)
{
ConvexFaceInternal convexFaceInternal = FacePool[faceIndex];
int[] adjacentFaces = convexFaceInternal.AdjacentFaces;
for (int i = 0; i < adjacentFaces.Length; i++)
{
adjacentFaces[i] = -1;
}
FreeFaceIndices.Push(faceIndex);
}
private DeferredFace MakeDeferredFace(ConvexFaceInternal face, int faceIndex, ConvexFaceInternal pivot, int pivotIndex, ConvexFaceInternal oldFace)
{
DeferredFace deferredFace = this.ObjectManager.GetDeferredFace();
deferredFace.Face = face;
deferredFace.FaceIndex = faceIndex;
deferredFace.Pivot = pivot;
deferredFace.PivotIndex = pivotIndex;
deferredFace.OldFace = oldFace;
return(deferredFace);
}
/// <summary>
/// Reallocate the face pool, including the AffectedFaceFlags
/// </summary>
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;
this.FacePool = newPool;
Hull.AffectedFaceFlags = newTags;
}
private void ReallocateFacePool()
{
ConvexFaceInternal[] destinationArray = new ConvexFaceInternal[2 * this.FacePoolCapacity];
bool[] dst = new bool[2 * this.FacePoolCapacity];
Array.Copy(this.FacePool, destinationArray, this.FacePoolCapacity);
Buffer.BlockCopy(this.Hull.AffectedFaceFlags, 0, dst, 0, this.FacePoolCapacity);
this.FacePoolCapacity = 2 * this.FacePoolCapacity;
this.Hull.FacePool = destinationArray;
this.FacePool = destinationArray;
this.Hull.AffectedFaceFlags = dst;
}
private void ReallocateFacePool()
{
ConvexFaceInternal[] array = new ConvexFaceInternal[2 * FacePoolCapacity];
bool[] array2 = new bool[2 * FacePoolCapacity];
Array.Copy(FacePool, array, FacePoolCapacity);
Buffer.BlockCopy(Hull.AffectedFaceFlags, 0, array2, 0, FacePoolCapacity);
FacePoolCapacity = 2 * FacePoolCapacity;
Hull.FacePool = array;
FacePool = array;
Hull.AffectedFaceFlags = array2;
}
/// <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>
/// 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"></param>
/// <param name="f"></param>
/// <returns>The vertex is "over face" if the result is positive.</returns>
public double GetVertexDistance(VertexWrap v, ConvexFaceInternal f)
{
double[] normal = f.Normal;
double[] p = v.PositionData;
double distance = f.Offset;
for (int i = 0; i < Dimension; i++)
{
distance += normal[i] * p[i];
}
return(distance);
}
/// <summary>
/// Computes the volume of an n-dimensional simplex.
/// Buffer needs to be array of shape Dimension x Dimension.
/// </summary>
/// <param name="cell"></param>
/// <param name="vertices"></param>
/// <param name="buffer">Helper for the calculation to avoid unnecessary allocations.</param>
/// <returns></returns>
public static double GetSimplexVolume(ConvexFaceInternal cell, IList<IVertex> vertices, SimplexVolumeBuffer buffer)
{
var xs = cell.Vertices;
var pivot = vertices[xs[0]].Position;
var data = buffer.Data;
var dim = buffer.Dimension;
double f = 1.0;
for (int i = 1; i < xs.Length; i++)
{
f *= i + 1;
var point = vertices[xs[i]].Position;
for (int j = 0; j < point.Length; j++) data[j * dim + i - 1] = point[j] - pivot[j];
}
return Math.Abs(DeterminantDestructive(buffer)) / f;
}
/// <summary>
/// Calculates the normal and offset of the hyper-plane given by the face's vertices.
/// </summary>
/// <param name="face"></param>
/// <param name="center"></param>
/// <returns></returns>
public bool CalculateFacePlane(ConvexFaceInternal face, double[] center)
{
var vertices = face.Vertices;
var normal = face.Normal;
FindNormalVector(vertices, normal);
if (double.IsNaN(normal[0]))
{
return false;
}
double offset = 0.0;
double centerDistance = 0.0;
var fi = vertices[0] * Dimension;
for (int i = 0; i < Dimension; i++)
{
double n = normal[i];
offset += n * PositionData[fi + i];
centerDistance += n * center[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>
/// Create a new face and put it in the pool.
/// </summary>
/// <returns></returns>
int CreateFace()
{
var index = FacePoolSize;
var face = new ConvexFaceInternal(Dimension, index, GetVertexBuffer());
FacePoolSize++;
if (FacePoolSize > FacePoolCapacity) ReallocateFacePool();
FacePool[index] = face;
return index;
}
/// <summary>
/// Check if 2 faces are adjacent and if so, update their AdjacentFaces array.
/// </summary>
/// <param name="l"></param>
/// <param name="r"></param>
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++) VertexMarks[lv[i]] = false;
// mark all vertices on the 2nd face
for (i = 0; i < rv.Length; i++) VertexMarks[rv[i]] = true;
// find the 1st false index
for (i = 0; i < lv.Length; i++) if (!VertexMarks[lv[i]]) break;
// no vertex was marked
if (i == Dimension) return;
// check if only 1 vertex wasn't marked
for (int j = i + 1; j < lv.Length; j++) if (!VertexMarks[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++) VertexMarks[lv[i]] = false;
for (i = 0; i < rv.Length; i++)
{
if (VertexMarks[rv[i]]) break;
}
r.AdjacentFaces[i] = l.Index;
}
/// <summary>
/// Used in the "initialization" code.
/// </summary>
void FindBeyondVertices(ConvexFaceInternal face)
{
var beyondVertices = face.VerticesBeyond;
MaxDistance = double.NegativeInfinity;
FurthestVertex = 0;
int count = Vertices.Length;
for (int i = 0; i < count; i++)
{
if (VertexMarks[i]) continue;
IsBeyond(face, beyondVertices, i);
}
face.FurthestVertex = FurthestVertex;
}
/// <summary>
/// Updates the connector.
/// </summary>
/// <param name="face"></param>
/// <param name="edgeIndex"></param>
/// <param name="dim"></param>
public void Update(ConvexFaceInternal face, int edgeIndex, int dim)
{
this.Face = face;
this.EdgeIndex = edgeIndex;
uint hashCode = 23;
unchecked
{
var vs = face.Vertices;
int i, c = 0;
for (i = 0; i < edgeIndex; i++)
{
this.Vertices[c++] = vs[i];
hashCode += 31 * hashCode + (uint)vs[i];
}
for (i = edgeIndex + 1; i < vs.Length; i++)
{
this.Vertices[c++] = vs[i];
hashCode += 31 * hashCode + (uint)vs[i];
}
}
this.HashCode = hashCode;
}
/// <summary>
/// Adds a face to the list.
/// </summary>
/// <param name="face"></param>
public void Add(ConvexFaceInternal face)
{
if (face.InList)
{
//if (this.first.FurthestDistance < face.FurthestDistance)
if (this.first.VerticesBeyond.Count < face.VerticesBeyond.Count)
{
Remove(face);
AddFirst(face);
}
return;
}
face.InList = true;
//if (first != null && first.FurthestDistance < face.FurthestDistance)
if (first != null && first.VerticesBeyond.Count < face.VerticesBeyond.Count)
{
this.first.Previous = face;
face.Next = this.first;
this.first = face;
}
else
{
if (this.last != null)
{
this.last.Next = face;
}
face.Previous = this.last;
this.last = face;
if (this.first == null)
{
this.first = face;
}
}
}
/// <summary>
/// Removes the element from the list.
/// </summary>
/// <param name="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)
{
this.first = face.Next;
}
if (face.Next != null)
{
face.Next.Previous = face.Previous;
}
else if (/*last == face*/ face.Next == null)
{
this.last = face.Previous;
}
face.Next = null;
face.Previous = null;
}
/// <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"></param>
/// <param name="f"></param>
/// <returns>The vertex is "over face" if the result is positive.</returns>
public double GetVertexDistance(int v, ConvexFaceInternal f)
{
double[] normal = f.Normal;
int x = v * Dimension;
double distance = f.Offset;
for (int i = 0; i < normal.Length; i++) distance += normal[i] * PositionData[x + i];
return distance;
}
/// <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"></param>
/// <param name="f"></param>
/// <returns>The vertex is "over face" if the result is positive.</returns>
public double GetVertexDistance(VertexWrap v, ConvexFaceInternal f)
{
double[] normal = f.Normal;
double[] p = v.PositionData;
double distance = f.Offset;
for (int i = 0; i < Dimension; i++) distance += normal[i] * p[i];
return distance;
}