private static void ComputeInitialTetrahedron(RawList <Vector3> points, RawList <int> outsidePointCandidates, RawList <int> triangleIndices, out Vector3 centroid)
{
//Find four points on the hull.
//We'll start with using the x axis to identify two points on the hull.
int a, b, c, d;
Vector3 direction;
//Find the extreme points along the x axis.
Fix64 minimumX = Fix64.MaxValue, maximumX = -Fix64.MaxValue;
int minimumXIndex = 0, maximumXIndex = 0;
for (int i = 0; i < points.Count; ++i)
{
var v = points.Elements[i];
if (v.X > maximumX)
{
maximumX = v.X;
maximumXIndex = i;
}
else if (v.X < minimumX)
{
minimumX = v.X;
minimumXIndex = i;
}
}
a = minimumXIndex;
b = maximumXIndex;
//Check for redundancies..
if (a == b)
{
throw new ArgumentException("Point set is degenerate; convex hulls must have volume.");
}
//Now, use a second axis perpendicular to the two points we found.
Vector3 ab;
Vector3.Subtract(ref points.Elements[b], ref points.Elements[a], out ab);
Vector3.Cross(ref ab, ref Toolbox.UpVector, out direction);
if (direction.LengthSquared() < Toolbox.Epsilon)
{
Vector3.Cross(ref ab, ref Toolbox.RightVector, out direction);
}
Fix64 minimumDot, maximumDot;
int minimumIndex, maximumIndex;
GetExtremePoints(ref direction, points, out maximumDot, out minimumDot, out maximumIndex, out minimumIndex);
//Compare the location of the extreme points to the location of the axis.
Fix64 dot;
Vector3.Dot(ref direction, ref points.Elements[a], out dot);
//Use the point further from the axis.
if (Fix64.Abs(dot - minimumDot) > Fix64.Abs(dot - maximumDot))
{
//In this case, we should use the minimum index.
c = minimumIndex;
}
else
{
//In this case, we should use the maximum index.
c = maximumIndex;
}
//Check for redundancies..
if (a == c || b == c)
{
throw new ArgumentException("Point set is degenerate; convex hulls must have volume.");
}
//Use a third axis perpendicular to the plane defined by the three unique points a, b, and c.
Vector3 ac;
Vector3.Subtract(ref points.Elements[c], ref points.Elements[a], out ac);
Vector3.Cross(ref ab, ref ac, out direction);
GetExtremePoints(ref direction, points, out maximumDot, out minimumDot, out maximumIndex, out minimumIndex);
//Compare the location of the extreme points to the location of the plane.
Vector3.Dot(ref direction, ref points.Elements[a], out dot);
//Use the point further from the plane.
if (Fix64.Abs(dot - minimumDot) > Fix64.Abs(dot - maximumDot))
{
//In this case, we should use the minimum index.
d = minimumIndex;
}
else
{
//In this case, we should use the maximum index.
d = maximumIndex;
}
//Check for redundancies..
if (a == d || b == d || c == d)
{
throw new ArgumentException("Point set is degenerate; convex hulls must have volume.");
}
//Add the triangles.
triangleIndices.Add(a);
triangleIndices.Add(b);
triangleIndices.Add(c);
triangleIndices.Add(a);
triangleIndices.Add(b);
triangleIndices.Add(d);
triangleIndices.Add(a);
triangleIndices.Add(c);
triangleIndices.Add(d);
triangleIndices.Add(b);
triangleIndices.Add(c);
triangleIndices.Add(d);
//The centroid is guaranteed to be within the convex hull. It will be used to verify the windings of triangles throughout the hull process.
Vector3.Add(ref points.Elements[a], ref points.Elements[b], out centroid);
Vector3.Add(ref centroid, ref points.Elements[c], out centroid);
Vector3.Add(ref centroid, ref points.Elements[d], out centroid);
Vector3.Multiply(ref centroid, F64.C0p25, out centroid);
for (int i = 0; i < triangleIndices.Count; i += 3)
{
var vA = points.Elements[triangleIndices.Elements[i]];
var vB = points.Elements[triangleIndices.Elements[i + 1]];
var vC = points.Elements[triangleIndices.Elements[i + 2]];
//Check the signed volume of a parallelepiped with the edges of this triangle and the centroid.
Vector3 cross;
Vector3.Subtract(ref vB, ref vA, out ab);
Vector3.Subtract(ref vC, ref vA, out ac);
Vector3.Cross(ref ac, ref ab, out cross);
Vector3 offset;
Vector3.Subtract(ref vA, ref centroid, out offset);
Fix64 volume;
Vector3.Dot(ref offset, ref cross, out volume);
//This volume/cross product could also be used to check for degeneracy, but we already tested for that.
if (Fix64.Abs(volume) < Toolbox.BigEpsilon)
{
throw new ArgumentException("Point set is degenerate; convex hulls must have volume.");
}
if (volume < F64.C0)
{
//If the signed volume is negative, that means the triangle's winding is opposite of what we want.
//Flip it around!
var temp = triangleIndices.Elements[i];
triangleIndices.Elements[i] = triangleIndices.Elements[i + 1];
triangleIndices.Elements[i + 1] = temp;
}
}
//Points which belong to the tetrahedra are guaranteed to be 'in' the convex hull. Do not allow them to be considered.
var tetrahedronIndices = CommonResources.GetIntList();
tetrahedronIndices.Add(a);
tetrahedronIndices.Add(b);
tetrahedronIndices.Add(c);
tetrahedronIndices.Add(d);
//Sort the indices to allow a linear time loop.
Array.Sort(tetrahedronIndices.Elements, 0, 4);
int tetrahedronIndex = 0;
for (int i = 0; i < points.Count; ++i)
{
if (tetrahedronIndex < 4 && i == tetrahedronIndices[tetrahedronIndex])
{
//Don't add a tetrahedron index. Now that we've found this index, though, move on to the next one.
++tetrahedronIndex;
}
else
{
outsidePointCandidates.Add(i);
}
}
CommonResources.GiveBack(tetrahedronIndices);
}
///<summary>
/// Transforms a vector by an affine transform.
///</summary>
///<param name="position">Position to transform.</param>
///<param name="transform">Transform to apply.</param>
///<param name="transformed">Transformed position.</param>
public static void Transform(ref Vector3 position, ref AffineTransform transform, out Vector3 transformed)
{
Matrix3x3.Transform(ref position, ref transform.LinearTransform, out transformed);
Vector3.Add(ref transformed, ref transform.Translation, out transformed);
}
/// <summary>
/// Computes a point along a ray given the length along the ray from the ray position.
/// </summary>
/// <param name="t">Length along the ray from the ray position in terms of the ray's direction.</param>
/// <param name="v">Point along the ray at the given location.</param>
public void GetPointOnRay(Fix64 t, out Vector3 v)
{
Vector3.Multiply(ref Direction, t, out v);
Vector3.Add(ref v, ref Position, out v);
}
