/// <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];
}
/// <summary>
/// Find the (dimension+1) initial points and create the simplexes.
/// </summary>
void InitConvexHull()
{
var extremes = FindExtremes();
var initialPoints = FindInitialPoints(extremes);
// Add the initial points to the convex hull.
foreach (var vertex in initialPoints)
{
CurrentVertex = vertex;
// update center must be called before adding the vertex.
UpdateCenter();
ConvexHull.Add(CurrentVertex);
InputVertices.Remove(vertex);
// Because of the AklTou heuristic.
extremes.Remove(vertex);
}
// Create the initial simplexes.
var faces = InitiateFaceDatabase();
// Init the vertex beyond buffers.
foreach (var face in faces)
{
FindBeyondVertices(face);
if (face.VerticesBeyond.Count == 0)
{
ConvexFaces.Add(face); // The face is on the hull
}
else
{
UnprocessedFaces.Add(face);
}
}
}
/// <summary>
/// Fins the convex hull.
/// </summary>
void FindConvexHull()
{
// Find the (dimension+1) initial points and create the simplexes.
InitConvexHull();
// Expand the convex hull and faces.
while (UnprocessedFaces.First != null)
{
var currentFace = UnprocessedFaces.First;
CurrentVertex = currentFace.FurthestVertex;
UpdateCenter();
// The affected faces get tagged
TagAffectedFaces(currentFace);
// Create the cone from the currentVertex and the affected faces horizon.
if (!SingularVertices.Contains(CurrentVertex) && CreateCone())
{
CommitCone();
}
else
{
HandleSingular();
}
// Need to reset the tags
int count = AffectedFaceBuffer.Count;
for (int i = 0; i < count; i++)
{
AffectedFaceBuffer[i].Tag = 0;
}
}
}
/// <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>
/// Used by update faces.
/// </summary>
void FindBeyondVertices(ConvexFaceInternal face, VertexBuffer beyond, VertexBuffer beyond1)
{
var beyondVertices = BeyondBuffer;
MaxDistance = double.NegativeInfinity;
FurthestVertex = null;
VertexWrap v;
int count = beyond1.Count;
for (int i = 0; i < count; i++)
{
beyond1[i].Marked = true;
}
CurrentVertex.Marked = false;
count = beyond.Count;
for (int i = 0; i < count; i++)
{
v = beyond[i];
if (object.ReferenceEquals(v, CurrentVertex))
{
continue;
}
v.Marked = false;
IsBeyond(face, beyondVertices, v);
}
count = beyond1.Count;
for (int i = 0; i < count; i++)
{
v = beyond1[i];
if (v.Marked)
{
IsBeyond(face, beyondVertices, v);
}
}
face.FurthestVertex = FurthestVertex;
//face.FurthestDistance = MaxDistance;
// Pull the old switch a roo
var temp = face.VerticesBeyond;
face.VerticesBeyond = beyondVertices;
if (temp.Count > 0)
{
temp.Clear();
}
BeyondBuffer = temp;
}
/// <summary>
/// Check whether the vertex v is beyond the given face. If so, add it to beyondVertices.
/// </summary>
/// <param name="face"></param>
/// <param name="beyondVertices"></param>
/// <param name="v"></param>
void IsBeyond(ConvexFaceInternal face, VertexBuffer beyondVertices, VertexWrap v)
{
double distance = MathHelper.GetVertexDistance(v, face);
if (distance >= PlaneDistanceTolerance)
{
if (distance > MaxDistance)
{
MaxDistance = distance;
FurthestVertex = v;
}
beyondVertices.Add(v);
}
}
/// <summary>
/// Computes the volume of the (n=initialPoints.Count)D simplex defined by the
/// pivot and initialPoints.
/// This is computed as the determinant of the matrix | initialPoints[i] - pivot |
/// </summary>
/// <param name="pivot"></param>
/// <param name="initialPoints"></param>
/// <returns></returns>
double GetSimplexVolume(VertexWrap pivot, List <VertexWrap> initialPoints)
{
var dim = initialPoints.Count;
var m = nDMatrix;
for (int i = 0; i < dim; i++)
{
var pts = initialPoints[i];
for (int j = 0; j < dim; j++)
{
m[i, j] = pts.PositionData[j] - pivot.PositionData[j];
}
}
return(Math.Abs(StarMath.determinantDestructive(m, dim)));
}
/// <summary>
/// Computes the sum of square distances to the initial points.
/// </summary>
/// <param name="pivot"></param>
/// <param name="initialPoints"></param>
/// <returns></returns>
double GetSquaredDistanceSum(VertexWrap pivot, List <VertexWrap> initialPoints)
{
var initPtsNum = initialPoints.Count;
var sum = 0.0;
for (int i = 0; i < initPtsNum; i++)
{
var initPt = initialPoints[i];
for (int j = 0; j < Dimension; j++)
{
double t = (initPt.PositionData[j] - pivot.PositionData[j]);
sum += t * t;
}
}
return(sum);
}
/// <summary>
/// Used in the "initialization" code.
/// </summary>
void FindBeyondVertices(ConvexFaceInternal face)
{
var beyondVertices = face.VerticesBeyond;
MaxDistance = double.NegativeInfinity;
FurthestVertex = null;
int count = InputVertices.Count;
for (int i = 0; i < count; i++)
{
IsBeyond(face, beyondVertices, InputVertices[i]);
}
face.FurthestVertex = FurthestVertex;
//face.FurthestDistance = MaxDistance;
}
/// <summary>
/// Adds a vertex to the buffer.
/// </summary>
/// <param name="item"></param>
public void Add(VertexWrap item)
{
EnsureCapacity();
items[count++] = item;
}
/// <summary>
/// Finds (dimension + 1) initial points.
/// </summary>
/// <param name="extremes"></param>
/// <returns></returns>
private List <VertexWrap> FindInitialPoints(List <VertexWrap> extremes)
{
List <VertexWrap> initialPoints = new List <VertexWrap>();// { extremes[0], extremes[1] };
VertexWrap first = null, second = null;
double maxDist = 0;
double[] temp = new double[Dimension];
for (int i = 0; i < extremes.Count - 1; i++)
{
var a = extremes[i];
for (int j = i + 1; j < extremes.Count; j++)
{
var b = extremes[j];
MathHelper.SubtractFast(a.PositionData, b.PositionData, temp);
var dist = MathHelper.LengthSquared(temp);
if (dist > maxDist)
{
first = a;
second = b;
maxDist = dist;
}
}
}
initialPoints.Add(first);
initialPoints.Add(second);
for (int i = 2; i <= Dimension; i++)
{
double maximum = 0.000001;
VertexWrap maxPoint = null;
for (int j = 0; j < extremes.Count; j++)
{
var extreme = extremes[j];
if (initialPoints.Contains(extreme))
{
continue;
}
var val = GetSquaredDistanceSum(extreme, initialPoints);
if (val > maximum)
{
maximum = val;
maxPoint = extreme;
}
}
if (maxPoint != null)
{
initialPoints.Add(maxPoint);
}
else
{
int vCount = InputVertices.Count;
for (int j = 0; j < vCount; j++)
{
var point = InputVertices[j];
if (initialPoints.Contains(point))
{
continue;
}
var val = GetSquaredDistanceSum(point, initialPoints);
if (val > maximum)
{
maximum = val;
maxPoint = point;
}
}
if (maxPoint != null)
{
initialPoints.Add(maxPoint);
}
else
{
ThrowSingular();
}
}
}
return(initialPoints);
}
/// <summary>
/// Finds (dimension + 1) initial points.
/// </summary>
/// <param name="extremes"></param>
/// <returns></returns>
private List <VertexWrap> FindInitialPoints(List <VertexWrap> extremes)
{
List <VertexWrap> initialPoints = new List <VertexWrap>();// { extremes[0], extremes[1] };
VertexWrap first = null, second = null;
double maxDist = 0;
for (int i = 0; i < extremes.Count - 1; i++)
{
var a = extremes[i];
for (int j = i + 1; j < extremes.Count; j++)
{
var b = extremes[j];
var dist = StarMath.norm2(StarMath.subtract(a.PositionData, b.PositionData, Dimension), Dimension, true);
if (dist > maxDist)
{
first = a;
second = b;
maxDist = dist;
}
}
}
initialPoints.Add(first);
initialPoints.Add(second);
for (int i = 2; i <= Dimension; i++)
{
double maximum = 0.0001;
VertexWrap maxPoint = null;
for (int j = 0; j < extremes.Count; j++)
{
var extreme = extremes[j];
if (initialPoints.Contains(extreme))
{
continue;
}
var val = GetSimplexVolume(extreme, initialPoints);
if (val > maximum)
{
maximum = val;
maxPoint = extreme;
}
}
if (maxPoint != null)
{
initialPoints.Add(maxPoint);
}
else
{
int vCount = InputVertices.Count;
for (int j = 0; j < vCount; j++)
{
var point = InputVertices[j];
if (initialPoints.Contains(point))
{
continue;
}
var val = GetSimplexVolume(point, initialPoints);
if (val > maximum)
{
maximum = val;
maxPoint = point;
}
}
if (maxPoint != null)
{
initialPoints.Add(maxPoint);
}
else
{
ThrowSingular();
}
}
}
return(initialPoints);
}
