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));
}
/// <summary>
/// Checks whether point 'V' lies on a line that passes through this Edge.
/// </summary>
public bool CalculateIfIsOnLine(Vector3 P)
{
float dV = (V[0] - V[1]).sqrMagnitude;
float dP1 = (V[0] - P).sqrMagnitude;
float dP2 = (P - V[1]).sqrMagnitude;
float dP = dP1 + dP2;
return(ExtMathf.Equal(dV, dP));
}
public static bool SolveAxb(this Matrix4x4 matrix, Vector3 b, ref Vector3 x)
{
float v00 = matrix.m00;
float v10 = matrix.m10;
float v20 = matrix.m20;
float v01 = matrix.m01;
float v11 = matrix.m11;
float v21 = matrix.m21;
float v02 = matrix.m02;
float v12 = matrix.m12;
float v22 = matrix.m22;
float av00 = Mathf.Abs(v00);
float av10 = Mathf.Abs(v10);
float av20 = Mathf.Abs(v20);
// Find which item in first column has largest absolute value.
if (av10 >= av00 && av10 >= av20)
{
Utilities.Swap(ref v00, ref v10);
Utilities.Swap(ref v01, ref v11);
Utilities.Swap(ref v02, ref v12);
Utilities.Swap(ref b.x, ref b.y);
}
else if (av20 >= av00)
{
Utilities.Swap(ref v00, ref v20);
Utilities.Swap(ref v01, ref v21);
Utilities.Swap(ref v02, ref v22);
Utilities.Swap(ref b.x, ref b.z);
}
/* a b c | x
* d e f | y
* g h i | z , where |a| >= |d| && |a| >= |g| */
if (ExtMathf.Equal(av00, 0f))
{
return(false);
}
// Scale row so that leading element is one.
float denom = 1f / v00;
v01 *= denom;
v02 *= denom;
b.x *= denom;
/* 1 b c | x
* d e f | y
* g h i | z */
// Zero first column of second and third rows.
v11 -= v10 * v01;
v12 -= v10 * v02;
b.y -= v10 * b.x;
v21 -= v20 * v01;
v22 -= v20 * v02;
b.z -= v20 * b.x;
/* 1 b c | x
* 0 e f | y
* 0 h i | z */
// Pivotize again.
if (Mathf.Abs(v21) > Mathf.Abs(v11))
{
Utilities.Swap(ref v11, ref v21);
Utilities.Swap(ref v12, ref v22);
Utilities.Swap(ref b.y, ref b.z);
}
if (ExtMathf.Equal(Mathf.Abs(v11), 0f))
{
return(false);
}
/* 1 b c | x
* 0 e f | y
* 0 h i | z, where |e| >= |h| */
denom = 1f / v11;
v12 *= denom;
b.y *= denom;
/* 1 b c | x
* 0 1 f | y
* 0 h i | z */
v22 -= v21 * v12;
b.z -= v21 * b.y;
/* 1 b c | x
* 0 1 f | y
* 0 0 i | z */
if (ExtMathf.Equal(Mathf.Abs(v22), 0f))
{
return(false);
}
x.z = b.z / v22;
x.y = b.y - x.z * v12;
x.x = b.x - x.z * v02 - x.y * v01;
return(true);
}
public void AlgorithmOptimized()
{
/* Outline of the algorithm:
* 0. Compute the convex hull of the point set (given as input to this function) O(VlogV)
* 1. Compute vertex adjacency data, i.e. given a vertex, return a list of its neighboring vertices. O(V)
* 2. Precompute face normal direction vectors, since these are needed often. (does not affect big-O complexity, just a micro-opt) O(F)
* 3. Compute edge adjacency data, i.e. given an edge, return the two indices of its neighboring faces. O(V)
* 4. Precompute antipodal vertices for each edge. O(A*ElogV), where A is the size of antipodal vertices per edge. A ~ O(1) on average.
* 5. Precompute all sidepodal edges for each edge. O(E*S), where S is the size of sidepodal edges per edge. S ~ O(sqrtE) on average.
* - Sort the sidepodal edges to a linear order so that it's possible to do fast set intersection computations on them. O(E*S*logS), or O(E*sqrtE*logE).
* 6. Test all configurations where all three edges are on adjacent faces. O(E*S^2) = O(E^2) or if smart with graph search, O(ES) = O(E*sqrtE)?
* 7. Test all configurations where two edges are on opposing faces, and the third one is on a face adjacent to the two. O(E*sqrtE*logV)?
* 8. Test all configurations where two edges are on the same face (OBB aligns with a face of the convex hull). O(F*sqrtE*logV).
* 9. Return the best found OBB.
*/
mVolume = float.MaxValue;
var dataCloud = mConvexHull3.DataCloud;
var basicTriangles = mConvexHull3.BasicTriangles;
var edges = mConvexHull3.BasicEdges;
var vertices = mConvexHull3.Vertices;
int verticesCount = vertices.Count;
if (verticesCount < 4)
{
return;
}
// Precomputation: For each vertex in the convex hull, compute their neighboring vertices.
var adjacencyData = mConvexHull3.GetVertexAdjacencyData(); // O(V)
// Precomputation: for each triangle create and store its normal, for later use.
List <Vector3> faceNormals = new List <Vector3>(basicTriangles.Count);
foreach (BasicTriangle basicTriangle in basicTriangles) // O(F)
{
Triangle triangle = Triangle.FromBasicTriangle(basicTriangle, dataCloud);
faceNormals.Add(triangle.N);
}
// Precomputation: For each edge in convex hull, compute both connected faces data.
Dictionary <int, List <int> > facesForEdge = mConvexHull3.GetEdgeFacesData(); // edgeFaces
HashSet <int> internalEdges = new HashSet <int>();
Func <int, bool> IsInternalEdge = (int edgeIndex) => { return(internalEdges.Contains(edgeIndex)); };
for (int edgeIndex = 0; edgeIndex < edges.Count; edgeIndex++)
{
float dot = Vector3.Dot(
faceNormals[facesForEdge[edgeIndex].First()],
faceNormals[facesForEdge[edgeIndex].Second()]
);
bool isInternal = ExtMathf.Equal(dot, 1f, 1e-4f);
if (isInternal)
{
internalEdges.Add(edgeIndex);
}
}
// For each vertex pair (v, u) through which there is an edge, specifies the index i of the edge that passes through them.
// This map contains duplicates, so both (v, u) and (u, v) map to the same edge index.
List <int> vertexPairToEdges = mConvexHull3.GetUniversalEdgesList();
// Throughout the whole algorithm, this array stores an auxiliary structure for performing graph searches
// on the vertices of the convex hull. Conceptually each index of the array stores a boolean whether we
// have visited that vertex or not during the current search. However storing such booleans is slow, since
// we would have to perform a linear-time scan through this array before next search to reset each boolean
// to unvisited false state. Instead, store a number, called a "color" for each vertex to specify whether
// that vertex has been visited, and manage a global color counter floodFillVisitColor that represents the
// visited vertices. At any given time, the vertices that have already been visited have the value
// floodFillVisited[i] == floodFillVisitColor in them. This gives a win that we can perform constant-time
// clears of the floodFillVisited array, by simply incrementing the "color" counter to clear the array.
List <int> floodFillVisited = new List <int>().Populate(-1, verticesCount);
int floodFillVisitColor = 1;
Action ClearGraphSearch = () => { floodFillVisitColor++; };
Action <int> MarkVertexVisited = (int v) => { floodFillVisited[v] = floodFillVisitColor; };
Func <int, bool> HaveVisitedVertex = (int v) => { return(floodFillVisited[v] == floodFillVisitColor); };
List <List <int> > antipodalPointsForEdge = new List <List <int> >().Populate(new List <int>(), edges.Count); // edgeAntipodalVertices
// The currently best variant for establishing a spatially coherent traversal order.
HashSet <int> spatialFaceOrder = new HashSet <int>();
HashSet <int> spatialEdgeOrder = new HashSet <int>();
{ // Explicit scope for variables that are not needed after this.
List <Pair <int, int> > traverseStackEdge = new List <Pair <int, int> >();
List <bool> visitedEdges = new List <bool>().Populate(false, edges.Count);
List <bool> visitedFaces = new List <bool>().Populate(false, basicTriangles.Count);
traverseStackEdge.Add(new Pair <int, int>(0, adjacencyData[0].First()));
while (traverseStackEdge.Count != 0)
{
Pair <int, int> e = traverseStackEdge.Last();
traverseStackEdge.RemoveLast();
int thisEdge = vertexPairToEdges[e.First * verticesCount + e.Second];
if (visitedEdges[thisEdge])
{
continue;
}
visitedEdges[thisEdge] = true;
if (!visitedFaces[facesForEdge[thisEdge].First()])
{
visitedFaces[facesForEdge[thisEdge].First()] = true;
spatialFaceOrder.Add(facesForEdge[thisEdge].First());
}
if (!visitedFaces[facesForEdge[thisEdge].Second()])
{
visitedFaces[facesForEdge[thisEdge].Second()] = true;
spatialFaceOrder.Add(facesForEdge[thisEdge].Second());
}
if (!IsInternalEdge(thisEdge))
{
spatialEdgeOrder.Add(thisEdge);
}
int v0 = e.Second;
int sizeBefore = traverseStackEdge.Count;
foreach (int v1 in adjacencyData[v0])
{
int e1 = vertexPairToEdges[v0 * verticesCount + v1];
if (visitedEdges[e1])
{
continue;
}
traverseStackEdge.Add(new Pair <int, int>(v0, v1));
}
//Take a random adjacent edge
int nNewEdges = (traverseStackEdge.Count - sizeBefore);
if (nNewEdges > 0)
{
int r = UnityEngine.Random.Range(0, nNewEdges - 1);
var swapTemp = traverseStackEdge[traverseStackEdge.Count - 1];
traverseStackEdge[traverseStackEdge.Count - 1] = traverseStackEdge[sizeBefore + r];
traverseStackEdge[sizeBefore + r] = swapTemp;
}
}
}
// Stores a memory of yet unvisited vertices for current graph search.
List <int> traverseStack = new List <int>();
// Since we do several extreme vertex searches, and the search directions have a lot of spatial locality,
// always start the search for the next extreme vertex from the extreme vertex that was found during the
// previous iteration for the previous edge. This has been profiled to improve overall performance by as
// much as 15-25%.
int debugCount = 0;
var startPoint = TimeMeasure.Start();
// Precomputation: for each edge, we need to compute the list of potential antipodal points (points on
// the opposing face of an enclosing OBB of the face that is flush with the given edge of the polyhedron).
// This is O(E*log(V)) ?
foreach (int i in spatialEdgeOrder)
{
Vector3 f1a = faceNormals[facesForEdge[i].First()];
Vector3 f1b = faceNormals[facesForEdge[i].Second()];
MinMaxPair breadthData;
int startingVertex = mConvexHull3.GetAlongAxisData(f1a, out breadthData);
ClearGraphSearch(); // Search through the graph for all adjacent antipodal vertices.
traverseStack.Add(startingVertex);
MarkVertexVisited(startingVertex);
while (!traverseStack.Empty())
{ // In amortized analysis, only a constant number of vertices are antipodal points for any edge?
int v = traverseStack.Last();
traverseStack.RemoveLast();
var neighbours = adjacencyData[v];
if (IsVertexAntipodalToEdge(v, neighbours, f1a, f1b))
{
if (edges[i].ContainsVertex(v))
{
Debug.LogFormat("Edge {0} is antipodal to vertex {1} which is part of the same edge!" +
"This should be possible only if the input is degenerate planar!",
edges[i], v);
return;
}
antipodalPointsForEdge[i].Add(v);
foreach (int neighboursVertex in neighbours)
{
if (!HaveVisitedVertex(neighboursVertex))
{
traverseStack.Add(neighboursVertex);
MarkVertexVisited(neighboursVertex);
}
debugCount++;
}
}
debugCount++;
}
// Robustness: If the above search did not find any antipodal points, add the first found extreme
// point at least, since it is always an antipodal point. This is known to occur very rarely due
// to numerical imprecision in the above loop over adjacent edges.
if (antipodalPointsForEdge[i].Empty())
{
Debug.LogFormat("Got to place which is most likely a bug...");
// Fall back to linear scan, which is very slow.
for (int j = 0; j < verticesCount; j++)
{
if (IsVertexAntipodalToEdge(j, adjacencyData[j], f1a, f1b))
{
antipodalPointsForEdge[i].Add(j);
}
}
}
debugCount++;
}
Debug.Log("@ First loop count: " + debugCount + " in time: " + TimeMeasure.Stop(startPoint));
// Stores for each edge i the list of all sidepodal edge indices j that it can form an OBB with.
List <HashSet <int> > compatibleEdges = new List <HashSet <int> >().Populate(new HashSet <int>(), edges.Count);
// Use a O(E*V) data structure for sidepodal vertices.
bool[] sidepodalVertices = new bool[edges.Count * verticesCount].Populate(false);
// Compute all sidepodal edges for each edge by performing a graph search. The set of sidepodal edges is
// connected in the graph, which lets us avoid having to iterate over each edge pair of the convex hull.
// Total running time is O(E*sqrtE).
debugCount = 0;
startPoint = TimeMeasure.Start();
foreach (int i in spatialEdgeOrder)
{
Vector3 f1a = faceNormals[facesForEdge[i].First()];
Vector3 f1b = faceNormals[facesForEdge[i].Second()];
Vector3 deadDirection = (f1a + f1b) * .5f;
Vector3 basis1, basis2;
deadDirection.PerpendicularBasis(out basis1, out basis2);
Vector3 dir = f1a.Perpendicular();
MinMaxPair breadthData;
int startingVertex = mConvexHull3.GetAlongAxisData(-dir, out breadthData);
ClearGraphSearch();
traverseStack.Add(startingVertex);
while (!traverseStack.Empty()) // O(sqrt(E))
{
int v = traverseStack.Last();
traverseStack.RemoveLast();
if (HaveVisitedVertex(v))
{
continue;
}
MarkVertexVisited(v);
foreach (int vAdj in adjacencyData[v])
{
if (HaveVisitedVertex(vAdj))
{
continue;
}
int edge = vertexPairToEdges[v * verticesCount + vAdj];
if (AreEdgesCompatibleForOBB(f1a, f1b, faceNormals[facesForEdge[edge].First()], faceNormals[facesForEdge[edge].Second()]))
{
if (i <= edge)
{
if (!IsInternalEdge(edge))
{
compatibleEdges[i].Add(edge);
}
sidepodalVertices[i * verticesCount + edges[edge].v[0]] = true;
sidepodalVertices[i * verticesCount + edges[edge].v[1]] = true;
if (i != edge)
{
if (!IsInternalEdge(edge))
{
compatibleEdges[edge].Add(i);
}
sidepodalVertices[edge * verticesCount + edges[i].v[0]] = true;
sidepodalVertices[edge * verticesCount + edges[i].v[1]] = true;
}
}
traverseStack.Add(vAdj);
}
debugCount++;
}
debugCount++;
}
debugCount++;
}
Debug.Log("@ Second loop count: " + debugCount + " in time: " + TimeMeasure.Stop(startPoint));
// Take advantage of spatial locality: start the search for the extreme vertex from the extreme vertex
// that was found during the previous iteration for the previous edge. This speeds up the search since
// edge directions have some amount of spatial locality and the next extreme vertex is often close
// to the previous one. Track two hint variables since we are performing extreme vertex searches to
// two opposing directions at the same time.
// Stores a memory of yet unvisited vertices that are common sidepodal vertices to both currently chosen edges for current graph search.
List <int> traverseStackCommonSidepodals = new List <int>();
Debug.Log("@ Edges count: " + edges.Count);
Debug.Log("@ Vertices count: " + verticesCount);
debugCount = 0;
startPoint = TimeMeasure.Start();
Debug.Log("@ Spatial edges: " + spatialEdgeOrder.Count);
foreach (int i in spatialEdgeOrder)
{
Vector3 f1a = faceNormals[facesForEdge[i].First()];
Vector3 f1b = faceNormals[facesForEdge[i].Second()];
Vector3 deadDirection = (f1a + f1b) * .5f;
Debug.Log("@ Compatible edges: " + compatibleEdges[i].Count);
foreach (int edgeJ in compatibleEdges[i])
{
if (edgeJ <= i) // Remove symmetry.
{
continue;
}
Vector3 f2a = faceNormals[facesForEdge[edgeJ].First()];
Vector3 f2b = faceNormals[facesForEdge[edgeJ].Second()];
Vector3 deadDirection2 = (f2a + f2b) * .5f;
Vector3 searchDir = Vector3.Cross(deadDirection, deadDirection2).normalized;
float length = searchDir.magnitude;
if (ExtMathf.Equal(length, 0f))
{
searchDir = Vector3.Cross(f1a, f2a).normalized;
length = searchDir.magnitude;
if (ExtMathf.Equal(length, 0f))
{
searchDir = f1a.Perpendicular();
}
}
ClearGraphSearch();
MinMaxPair breadthData;
int extremeVertexSearchHint1 = mConvexHull3.GetAlongAxisData(-searchDir, out breadthData);
ClearGraphSearch();
MinMaxPair breadthData2;
int extremeVertexSearchHint2 = mConvexHull3.GetAlongAxisData(searchDir, out breadthData2);
ClearGraphSearch();
int secondSearch = -1;
if (sidepodalVertices[edgeJ * verticesCount + extremeVertexSearchHint1])
{
traverseStackCommonSidepodals.Add(extremeVertexSearchHint1);
}
else
{
traverseStack.Add(extremeVertexSearchHint1);
}
if (sidepodalVertices[edgeJ * verticesCount + extremeVertexSearchHint2])
{
traverseStackCommonSidepodals.Add(extremeVertexSearchHint2);
}
else
{
secondSearch = extremeVertexSearchHint2;
}
// Bootstrap to a good vertex that is sidepodal to both edges.
ClearGraphSearch();
while (!traverseStack.Empty())
{
int v = traverseStack.First();
traverseStack.RemoveFirst();
if (HaveVisitedVertex(v))
{
continue;
}
MarkVertexVisited(v);
foreach (int vAdj in adjacencyData[v])
{
if (!HaveVisitedVertex(vAdj) && sidepodalVertices[i * verticesCount + vAdj])
{
if (sidepodalVertices[edgeJ * verticesCount + vAdj])
{
traverseStack.Clear();
if (secondSearch != -1)
{
traverseStack.Add(secondSearch);
secondSearch = -1;
MarkVertexVisited(vAdj);
}
traverseStackCommonSidepodals.Add(vAdj);
break;
}
else
{
traverseStack.Add(vAdj);
}
}
debugCount++;
}
debugCount++;
}
ClearGraphSearch();
while (!traverseStackCommonSidepodals.Empty())
{
int v = traverseStackCommonSidepodals.Last();
traverseStackCommonSidepodals.RemoveLast();
if (HaveVisitedVertex(v))
{
continue;
}
MarkVertexVisited(v);
foreach (int vAdj in adjacencyData[v])
{
int edgeK = vertexPairToEdges[v * verticesCount + vAdj];
if (IsInternalEdge(edgeK)) // Edges inside faces with 180 degrees dihedral angles can be ignored.
{
continue;
}
if (sidepodalVertices[i * verticesCount + vAdj] && sidepodalVertices[edgeJ * verticesCount + vAdj])
{
if (!HaveVisitedVertex(vAdj))
{
traverseStackCommonSidepodals.Add(vAdj);
}
if (edgeJ < edgeK)
{
Vector3 f3a = faceNormals[facesForEdge[edgeK].First()];
Vector3 f3b = faceNormals[facesForEdge[edgeK].Second()];
Vector3[] n1 = new Vector3[2];
Vector3[] n2 = new Vector3[2];
Vector3[] n3 = new Vector3[2];
int nSolutions = ComputeBasis(f1a, f1b, f2a, f2b, f3a, f3b, ref n1, ref n2, ref n3);
for (int s = 0; s < nSolutions; s++)
{
BoxFromNormalAxes(n1[s], n2[s], n3[s]);
}
}
}
debugCount++;
}
debugCount++;
}
debugCount++;
}
debugCount++;
}
Debug.Log("@ Third loop count: " + debugCount + " in time: " + TimeMeasure.Stop(startPoint));
HashSet <int> antipodalEdges = new HashSet <int>();
List <Vector3> antipodalEdgeNormals = new List <Vector3>();
// Main algorithm body for finding all search directions where the OBB is flush with the edges of the
// convex hull from two opposing faces. This is O(E*sqrtE*logV)?
debugCount = 0;
startPoint = TimeMeasure.Start();
foreach (int i in spatialEdgeOrder)
{
Vector3 f1a = faceNormals[facesForEdge[i].First()];
Vector3 f1b = faceNormals[facesForEdge[i].Second()];
antipodalEdges.Clear();
antipodalEdgeNormals.Clear();
foreach (int antipodalVertex in antipodalPointsForEdge[i])
{
foreach (int vAdj in adjacencyData[antipodalVertex])
{
if (vAdj < antipodalVertex) // We search unordered edges, so no need to process edge (v1, v2) and (v2, v1)
{
continue; // twice - take the canonical order to be antipodalVertex < vAdj
}
int edge = vertexPairToEdges[antipodalVertex * verticesCount + vAdj];
if (i > edge) // We search pairs of edges, so no need to process twice - take the canonical order to be i < edge.
{
continue;
}
if (IsInternalEdge(edge))
{
continue; // Edges inside faces with 180 degrees dihedral angles can be ignored.
}
Vector3 f2a = faceNormals[facesForEdge[edge].First()];
Vector3 f2b = faceNormals[facesForEdge[edge].Second()];
Vector3 normal;
bool success = AreCompatibleOpposingEdges(f1a, f1b, f2a, f2b, out normal);
if (success)
{
antipodalEdges.Add(edge);
antipodalEdgeNormals.Add(normal.normalized);
}
debugCount++;
}
debugCount++;
}
var compatibleEdgesI = compatibleEdges[i];
foreach (int edgeJ in compatibleEdgesI)
{
int k = 0;
foreach (int edgeK in antipodalEdges)
{
Vector3 n1 = antipodalEdgeNormals[k++];
MinMaxPair N1 = new MinMaxPair();
N1.Min = Vector3.Dot(n1, dataCloud[edges[edgeK].v[0]]);
N1.Max = Vector3.Dot(n1, dataCloud[edges[i].v[0]]);
Debug.Assert(N1.Min < N1.Max);
// Test all mutual compatible edges.
Vector3 f3a = faceNormals[facesForEdge[edgeJ].First()];
Vector3 f3b = faceNormals[facesForEdge[edgeJ].Second()];
float num = Vector3.Dot(n1, f3b);
float denom = Vector3.Dot(n1, f3b - f3a);
RefAction <float, float> MoveSign = (ref float dst, ref float src) =>
{
if (src < 0f)
{
dst = -dst;
src = -src;
}
};
MoveSign(ref num, ref denom);
float epsilon = 1e-4f;
if (denom < epsilon)
{
num = (Mathf.Abs(num) == 0f) ? 0f : -1f;
denom = 1f;
}
if (num >= denom * -epsilon && num <= denom * (1f + epsilon))
{
float v = num / denom;
Vector3 n3 = (f3b + (f3a - f3b) * v).normalized;
Vector3 n2 = Vector3.Cross(n3, n1).normalized;
BoxFromNormalAxes(n1, n2, n3);
}
debugCount++;
}
debugCount++;
}
debugCount++;
}
Debug.Log("@ Fourth loop count: " + debugCount + " in time: " + TimeMeasure.Stop(startPoint));
// Main algorithm body for computing all search directions where the OBB touches two edges on the same face.
// This is O(F*sqrtE*logV)?
debugCount = 0;
startPoint = TimeMeasure.Start();
foreach (int i in spatialFaceOrder)
{
Vector3 n1 = faceNormals[i];
// Find two edges on the face. Since we have flexibility to choose from multiple edges of the same face,
// choose two that are possibly most opposing to each other, in the hope that their sets of sidepodal
// edges are most mutually exclusive as possible, speeding up the search below.
int e1 = -1;
int v0 = basicTriangles[i].v[2];
for (int j = 0; j < basicTriangles[i].v.Length; j++)
{
int v1 = basicTriangles[i].v[j];
int e = vertexPairToEdges[v0 * verticesCount + v1];
if (!IsInternalEdge(e))
{
e1 = e;
break;
}
v0 = v1;
}
if (e1 == -1)
{
continue; // All edges of this face were degenerate internal edges! Just skip processing the whole face.
}
var compatibleEdgesI = compatibleEdges[e1];
foreach (int edge3 in compatibleEdgesI)
{
Vector3 f3a = faceNormals[facesForEdge[edge3].First()];
Vector3 f3b = faceNormals[facesForEdge[edge3].Second()];
float num = Vector3.Dot(n1, f3b);
float denom = Vector3.Dot(n1, f3b - f3a);
float v;
if (!ExtMathf.Equal(Mathf.Abs(denom), 0f))
{
v = num / denom;
}
else
{
v = ExtMathf.Equal(Mathf.Abs(num), 0f) ? 0f : -1f;
}
const float epsilon = 1e-4f;
if (v >= 0f - epsilon && v <= 1f + epsilon)
{
Vector3 n3 = (f3b + (f3a - f3b) * v).normalized;
Vector3 n2 = Vector3.Cross(n3, n1).normalized;
BoxFromNormalAxes(n1, n2, n3);
}
debugCount++;
}
debugCount++;
}
Debug.Log("@ Fifth loop count: " + debugCount + " in time: " + TimeMeasure.Stop(startPoint));
}
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);
}
}
}