/// <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); }