public static void TestExtMath()
{
float a = 1f;
float b = 1f + 1e-6f;
float c = 1f + 1e-4f;
Debug.Assert(ExtMathf.Equal(a, b));
Debug.Assert(!ExtMathf.Equal(a, c));
Debug.Assert(ExtMathf.Greater(c, a));
Debug.Assert(!ExtMathf.Greater(b, a));
Debug.Assert(ExtMathf.Less(a, c));
Debug.Assert(!ExtMathf.Less(a, b));
Vector3 A = new Vector3(-1, 0, -1);
Vector3 B = new Vector3(1, 0, 1);
Vector3 C = new Vector3(0, 1, 0);
Vector3 D = new Vector3(0, (1f / 3f), 0);
Debug.Assert(ExtMathf.Centroid(A, B, C) == D);
Vector3 xAxis = new Vector3(Random.Range(-1f, 1f), Random.Range(-1f, 1f), Random.Range(-1f, 1f)).normalized;
Vector3 yAxis;
Vector3 zAxis;
ExtMathf.AxesFromAxis(xAxis, out yAxis, out zAxis);
Debug.Assert(ExtMathf.Equal(Vector3.Cross(yAxis, zAxis).normalized, xAxis));
Debug.Assert(ExtMathf.Equal(Vector3.Cross(zAxis, xAxis).normalized, yAxis));
Debug.Assert(ExtMathf.Equal(Vector3.Cross(xAxis, yAxis).normalized, zAxis));
}
public void AlgorithmRandomIncremental()
{
ClearIfNecessary();
if (mDataCloud.Count < 4)
{
Debugger.Get.Log("Insufficient points to generate a tetrahedron.", DebugOption.CH3);
return;
}
int index = 0;
// Pick first point.
KeyValuePair <int, Vector3> P1 = new KeyValuePair <int, Vector3>(index, mDataCloud[index]);
KeyValuePair <int, Vector3> P2 = new KeyValuePair <int, Vector3>(-1, Vector3.zero);
while (P2.Key == -1 && ++index < mDataCloud.Count)
{
// Two points form a line if they are not equal.
Vector3 P = mDataCloud[index];
if (!ExtMathf.Equal(P2.Value, P))
{
P2 = new KeyValuePair <int, Vector3>(index, P);
}
}
if (P2.Key == -1)
{
Debugger.Get.Log("Couldn't find two not equal points.", DebugOption.CH3);
return;
}
// Points on line and on the same plane, still should be considered later on. Happens.
List <int> skippedPoints = new List <int>();
Edge line = new Edge(P1.Value, P2.Value);
KeyValuePair <int, Vector3> P3 = new KeyValuePair <int, Vector3>(-1, Vector3.zero);
while (P3.Key == -1 && ++index < mDataCloud.Count)
{
// Three points form a triangle if they are not on the same line.
Vector3 P = mDataCloud[index];
if (!line.CalculateIfIsOnLine(P))
{
P3 = new KeyValuePair <int, Vector3>(index, P);
}
else
{
skippedPoints.Add(index);
}
}
if (P3.Key == -1)
{
Debugger.Get.Log("Couldn't find three not linear points.", DebugOption.CH3);
return;
}
Triangle planeTriangle = new Triangle(P1.Value, P2.Value, P3.Value);
KeyValuePair <int, Vector3> P4 = new KeyValuePair <int, Vector3>(-1, Vector3.zero);
while (P4.Key == -1 && ++index < mDataCloud.Count)
{
// Four points form a tetrahedron if they are not on the same plane.
Vector3 P = mDataCloud[index];
if (planeTriangle.CalculateRelativePosition(P) != 0)
{
P4 = new KeyValuePair <int, Vector3>(index, P);
}
else
{
skippedPoints.Add(index);
}
}
if (P4.Key == -1)
{
Debugger.Get.Log("Couldn't find four not planar points.", DebugOption.CH3);
return;
}
// Calculate reference centroid of ConvexHull.
Vector3 centroid = ExtMathf.Centroid(P1.Value, P2.Value, P3.Value, P4.Value);
List <BasicTriangle> tetrahedronTriangles = new List <BasicTriangle>();
tetrahedronTriangles.Add(new BasicTriangle(P3.Key, P2.Key, P1.Key));
tetrahedronTriangles.Add(new BasicTriangle(P1.Key, P2.Key, P4.Key));
tetrahedronTriangles.Add(new BasicTriangle(P2.Key, P3.Key, P4.Key));
tetrahedronTriangles.Add(new BasicTriangle(P3.Key, P1.Key, P4.Key));
foreach (BasicTriangle basicTriangle in tetrahedronTriangles)
{
Triangle triangle = new Triangle(
mDataCloud[basicTriangle.v[0]],
mDataCloud[basicTriangle.v[1]],
mDataCloud[basicTriangle.v[2]]
);
if (triangle.CalculateRelativePosition(centroid) != -1)
{ // Centroid of ConvexHull should always be inside.
basicTriangle.Reverse();
}
mBasicTriangles.Add(basicTriangle);
}
Dictionary <int, HashSet <BasicTriangle> > pointFacets = new Dictionary <int, HashSet <BasicTriangle> >();
Dictionary <BasicTriangle, HashSet <int> > facetPoints = new Dictionary <BasicTriangle, HashSet <int> >();
foreach (BasicTriangle basicTriangle in mBasicTriangles)
{
Triangle triangle = new Triangle(
mDataCloud[basicTriangle.v[0]],
mDataCloud[basicTriangle.v[1]],
mDataCloud[basicTriangle.v[2]]
);
for (int p = index; p < mDataCloud.Count; p++)
{
if (triangle.CalculateRelativePosition(mDataCloud[p]) == 1)
{
if (!pointFacets.ContainsKey(p))
{
pointFacets.Add(p, new HashSet <BasicTriangle>());
}
pointFacets[p].Add(basicTriangle);
if (!facetPoints.ContainsKey(basicTriangle))
{
facetPoints.Add(basicTriangle, new HashSet <int>());
}
facetPoints[basicTriangle].Add(p);
}
}
foreach (int p in skippedPoints)
{
if (triangle.CalculateRelativePosition(mDataCloud[p]) == 1)
{
if (!pointFacets.ContainsKey(p))
{
pointFacets.Add(p, new HashSet <BasicTriangle>());
}
pointFacets[p].Add(basicTriangle);
facetPoints[basicTriangle].Add(p);
}
}
}
while (pointFacets.Count > 0)
{
var firstPointFacet = pointFacets.First();
if (firstPointFacet.Value.Count > 0)
{
Dictionary <BasicEdge, BasicEdge> horizon = new Dictionary <BasicEdge, BasicEdge>();
foreach (BasicTriangle basicTriangle in firstPointFacet.Value)
{
for (int a = 2, b = 0; b < 3; a = b++)
{
BasicEdge edge = new BasicEdge(basicTriangle.v[a], basicTriangle.v[b]);
BasicEdge edgeUnordered = edge.Unordered();
if (horizon.ContainsKey(edgeUnordered))
{
horizon.Remove(edgeUnordered);
}
else
{
horizon.Add(edgeUnordered, edge);
}
}
}
int pointKey = firstPointFacet.Key;
foreach (BasicTriangle facet in firstPointFacet.Value)
{
foreach (int facetPointKey in facetPoints[facet])
{
if (facetPointKey != pointKey)
{
pointFacets[facetPointKey].Remove(facet);
}
}
facetPoints.Remove(facet);
mBasicTriangles.Remove(facet);
}
pointFacets.Remove(pointKey);
foreach (KeyValuePair <BasicEdge, BasicEdge> edges in horizon)
{
BasicTriangle newBasicTriangle = new BasicTriangle(edges.Value.v[0], edges.Value.v[1], pointKey);
mBasicTriangles.Add(newBasicTriangle);
if (pointFacets.Count > 0)
{
Triangle newTriangle = new Triangle(
mDataCloud[newBasicTriangle.v[0]],
mDataCloud[newBasicTriangle.v[1]],
mDataCloud[newBasicTriangle.v[2]]
);
foreach (var pointFacetsKey in pointFacets.Keys)
{
if (newTriangle.CalculateRelativePosition(mDataCloud[pointFacetsKey]) == 1)
{
pointFacets[pointFacetsKey].Add(newBasicTriangle);
if (!facetPoints.ContainsKey(newBasicTriangle))
{
facetPoints.Add(newBasicTriangle, new HashSet <int>());
}
facetPoints[newBasicTriangle].Add(pointFacetsKey);
}
}
}
}
}
else
{
pointFacets.Remove(firstPointFacet.Key);
}
}
}
