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