From b834b669a6bedb14a62d92e07aafd69fde8675d1 Mon Sep 17 00:00:00 2001 From: GoaLitiuM Date: Tue, 31 Aug 2021 20:03:13 +0300 Subject: [PATCH] split quick hull --- Source/Game/Q3MapImporter.cs | 2533 +--------------------------------- Source/Game/QuickHull.cs | 1312 ++++++++++++++++++ Source/Game/QuickHull2.cs | 1078 +++++++++++++++ 3 files changed, 2393 insertions(+), 2530 deletions(-) create mode 100644 Source/Game/QuickHull.cs create mode 100644 Source/Game/QuickHull2.cs diff --git a/Source/Game/Q3MapImporter.cs b/Source/Game/Q3MapImporter.cs index a499cc2..992b0f9 100644 --- a/Source/Game/Q3MapImporter.cs +++ b/Source/Game/Q3MapImporter.cs @@ -15,1073 +15,6 @@ using Console = Cabrito.Console; namespace Game { - /// - /// An implementation of the quickhull algorithm for generating 3d convex - /// hulls. - /// - /// The algorithm works like this: you start with an initial "seed" hull, - /// that is just a simple tetrahedron made up of four points in the point - /// cloud. This seed hull is then grown until it all the points in the - /// point cloud is inside of it, at which point it will be the convex hull - /// for the entire set. - /// - /// All of the points in the point cloud is divided into two parts, the - /// "open set" and the "closed set". The open set consists of all the - /// points outside of the tetrahedron, and the closed set is all of the - /// points inside the tetrahedron. After each iteration of the algorithm, - /// the closed set gets bigger and the open set get smaller. When the open - /// set is empty, the algorithm is finished. - /// - /// Each point in the open set is assigned to a face that it lies outside - /// of. To grow the hull, the point in the open set which is farthest from - /// it's face is chosen. All faces which are facing that point (I call - /// them "lit faces" in the code, because if you imagine the point as a - /// point light, it's the set of points which would be lit by that point - /// light) are removed, and a "horizon" of edges is found from where the - /// faces were removed. From this horizon, new faces are constructed in a - /// "cone" like fashion connecting the point to the edges. - /// - /// To keep track of the faces, I use a struct for each face which - /// contains the three vertices of the face in CCW order, as well as the - /// three triangles which share an edge. I was considering doing a - /// half-edge structure to store the mesh, but it's not needed. Using a - /// struct for each face and neighbors simplify the algorithm and makes it - /// easy to export it as a mesh. - /// - /// The most subtle part of the algorithm is finding the horizon. In order - /// to properly construct the cone so that all neighbors are kept - /// consistent, you can do a depth-first search from the first lit face. - /// If the depth-first search always proceeeds in a counter-clockwise - /// fashion, it guarantees that the horizon will be found in a - /// counter-clockwise order, which makes it easy to construct the cone of - /// new faces. - /// - /// A note: the code uses a right-handed coordinate system, where the - /// cross-product uses the right-hand rule and the faces are in CCW order. - /// At the end of the algorithm, the hull is exported in a Unity-friendly - /// fashion, with a left-handed mesh. - /// - public class ConvexHullCalculator { - - /// - /// Constant representing a point that has yet to be assigned to a - /// face. It's only used immediately after constructing the seed hull. - /// - const int UNASSIGNED = -2; - - /// - /// Constant representing a point that is inside the convex hull, and - /// thus is behind all faces. In the openSet array, all points with - /// INSIDE are at the end of the array, with indexes larger - /// openSetTail. - /// - const int INSIDE = -1; - - /// - /// Epsilon value. If the coordinates of the point space are - /// exceptionally close to each other, this value might need to be - /// adjusted. - /// - const float EPSILON = 0.001f; - - /// - /// Struct representing a single face. - /// - /// Vertex0, Vertex1 and Vertex2 are the vertices in CCW order. They - /// acutal points are stored in the points array, these are just - /// indexes into that array. - /// - /// Opposite0, Opposite1 and Opposite2 are the keys to the faces which - /// share an edge with this face. Opposite0 is the face opposite - /// Vertex0 (so it has an edge with Vertex2 and Vertex1), etc. - /// - /// Normal is (unsurprisingly) the normal of the triangle. - /// - struct Face { - public int Vertex0; - public int Vertex1; - public int Vertex2; - - public int Opposite0; - public int Opposite1; - public int Opposite2; - - public Vector3 Normal; - - public Face(int v0, int v1, int v2, int o0, int o1, int o2, Vector3 normal) { - Vertex0 = v0; - Vertex1 = v1; - Vertex2 = v2; - Opposite0 = o0; - Opposite1 = o1; - Opposite2 = o2; - Normal = normal; - } - - public bool Equals(Face other) { - return (this.Vertex0 == other.Vertex0) - && (this.Vertex1 == other.Vertex1) - && (this.Vertex2 == other.Vertex2) - && (this.Opposite0 == other.Opposite0) - && (this.Opposite1 == other.Opposite1) - && (this.Opposite2 == other.Opposite2) - && (this.Normal == other.Normal); - } - } - - /// - /// Struct representing a mapping between a point and a face. These - /// are used in the openSet array. - /// - /// Point is the index of the point in the points array, Face is the - /// key of the face in the Key dictionary, Distance is the distance - /// from the face to the point. - /// - struct PointFace { - public int Point; - public int Face; - public float Distance; - - public PointFace(int p, int f, float d) { - Point = p; - Face = f; - Distance = d; - } - } - - /// - /// Struct representing a single edge in the horizon. - /// - /// Edge0 and Edge1 are the vertexes of edge in CCW order, Face is the - /// face on the other side of the horizon. - /// - /// TODO Edge1 isn't actually needed, you can just index the next item - /// in the horizon array. - /// - struct HorizonEdge { - public int Face; - public int Edge0; - public int Edge1; - } - - /// - /// A dictionary storing the faces of the currently generated convex - /// hull. The key is the id of the face, used in the Face, PointFace - /// and HorizonEdge struct. - /// - /// This is a Dictionary, because we need both random access to it, - /// the ability to loop through it, and ability to quickly delete - /// faces (in the ConstructCone method), and Dictionary is the obvious - /// candidate that can do all of those things. - /// - /// I'm wondering if using a Dictionary is best idea, though. It might - /// be better to just have them in a List and mark a face as - /// deleted by adding a field to the Face struct. The downside is that - /// we would need an extra field in the Face struct, and when we're - /// looping through the points in openSet, we would have to loop - /// through all the Faces EVER created in the algorithm, and skip the - /// ones that have been marked as deleted. However, looping through a - /// list is fairly fast, and it might be worth it to avoid Dictionary - /// overhead. - /// - /// TODO test converting to a List instead. - /// - Dictionary faces; - - /// - /// The set of points to be processed. "openSet" is a misleading name, - /// because it's both the open set (points which are still outside the - /// convex hull) and the closed set (points that are inside the convex - /// hull). The first part of the array (with indexes <= openSetTail) - /// is the openSet, the last part of the array (with indexes > - /// openSetTail) are the closed set, with Face set to INSIDE. The - /// closed set is largely irrelevant to the algorithm, the open set is - /// what matters. - /// - /// Storing the entire open set in one big list has a downside: when - /// we're reassigning points after ConstructCone, we only need to - /// reassign points that belong to the faces that have been removed, - /// but storing it in one array, we have to loop through the entire - /// list, and checking litFaces to determine which we can skip and - /// which need to be reassigned. - /// - /// The alternative here is to give each face in Face array it's own - /// openSet. I don't like that solution, because then you have to - /// juggle so many more heap-allocated List's, we'd have to use - /// object pools and such. It would do a lot more allocation, and it - /// would have worse locality. I should maybe test that solution, but - /// it probably wont be faster enough (if at all) to justify the extra - /// allocations. - /// - List openSet; - - /// - /// Set of faces which are "lit" by the current point in the set. This - /// is used in the FindHorizon() DFS search to keep track of which - /// faces we've already visited, and in the ReassignPoints() method to - /// know which points need to be reassigned. - /// - HashSet litFaces; - - /// - /// The current horizon. Generated by the FindHorizon() DFS search, - /// and used in ConstructCone to construct new faces. The list of - /// edges are in CCW order. - /// - List horizon; - - /// - /// If SplitVerts is false, this Dictionary is used to keep track of - /// which points we've added to the final mesh. - /// - Dictionary hullVerts; - - /// - /// The "tail" of the openSet, the last index of a vertex that has - /// been assigned to a face. - /// - int openSetTail = -1; - - /// - /// When adding a new face to the faces Dictionary, use this for the - /// key and then increment it. - /// - int faceCount = 0; - - /// - /// Generate a convex hull from points in points array, and store the - /// mesh in Unity-friendly format in verts and tris. If splitVerts is - /// true, the the verts will be split, if false, the same vert will be - /// used for more than one triangle. - /// - public void GenerateHull( - List points, - bool splitVerts, - ref List verts, - ref List tris, - ref List normals) - { - if (points.Count < 4) { - throw new System.ArgumentException("Need at least 4 points to generate a convex hull"); - } - - Initialize(points, splitVerts); - - GenerateInitialHull(points); - - while (openSetTail >= 0) { - GrowHull(points); - } - - ExportMesh(points, splitVerts, ref verts, ref tris, ref normals); - //VerifyMesh(points, ref verts, ref tris); - } - - /// - /// Make sure all the buffers and variables needed for the algorithm - /// are initialized. - /// - void Initialize(List points, bool splitVerts) { - faceCount = 0; - openSetTail = -1; - - if (faces == null) { - faces = new Dictionary(); - litFaces = new HashSet(); - horizon = new List(); - openSet = new List(points.Count); - } else { - faces.Clear(); - litFaces.Clear(); - horizon.Clear(); - openSet.Clear(); - - if (openSet.Capacity < points.Count) { - // i wonder if this is a good idea... if you call - // GenerateHull over and over with slightly increasing - // points counts, it's going to reallocate every time. Maybe - // i should just use .Add(), and let the List manage the - // capacity, increasing it geometrically every time we need - // to reallocate. - - // maybe do - // openSet.Capacity = Mathf.NextPowerOfTwo(points.Count) - // instead? - - openSet.Capacity = points.Count; - } - } - - if (!splitVerts) { - if (hullVerts == null) { - hullVerts = new Dictionary(); - } else { - hullVerts.Clear(); - } - } - } - - /// - /// Create initial seed hull. - /// - void GenerateInitialHull(List points) { - // Find points suitable for use as the seed hull. Some varieties of - // this algorithm pick extreme points here, but I'm not convinced - // you gain all that much from that. Currently what it does is just - // find the first four points that are not coplanar. - int b0, b1, b2, b3; - FindInitialHullIndices(points, out b0, out b1, out b2, out b3); - - var v0 = points[b0]; - var v1 = points[b1]; - var v2 = points[b2]; - var v3 = points[b3]; - - var above = Dot(v3 - v1, Cross(v1 - v0, v2 - v0)) > 0.0f; - - // Create the faces of the seed hull. You need to draw a diagram - // here, otherwise it's impossible to know what's going on :) - - // Basically: there are two different possible start-tetrahedrons, - // depending on whether the fourth point is above or below the base - // triangle. If you draw a tetrahedron with these coordinates (in a - // right-handed coordinate-system): - - // b0 = (0,0,0) - // b1 = (1,0,0) - // b2 = (0,1,0) - // b3 = (0,0,1) - - // you can see the first case (set b3 = (0,0,-1) for the second - // case). The faces are added with the proper references to the - // faces opposite each vertex - - faceCount = 0; - if (above) { - faces[faceCount++] = new Face(b0, b2, b1, 3, 1, 2, Normal(points[b0], points[b2], points[b1])); - faces[faceCount++] = new Face(b0, b1, b3, 3, 2, 0, Normal(points[b0], points[b1], points[b3])); - faces[faceCount++] = new Face(b0, b3, b2, 3, 0, 1, Normal(points[b0], points[b3], points[b2])); - faces[faceCount++] = new Face(b1, b2, b3, 2, 1, 0, Normal(points[b1], points[b2], points[b3])); - } else { - faces[faceCount++] = new Face(b0, b1, b2, 3, 2, 1, Normal(points[b0], points[b1], points[b2])); - faces[faceCount++] = new Face(b0, b3, b1, 3, 0, 2, Normal(points[b0], points[b3], points[b1])); - faces[faceCount++] = new Face(b0, b2, b3, 3, 1, 0, Normal(points[b0], points[b2], points[b3])); - faces[faceCount++] = new Face(b1, b3, b2, 2, 0, 1, Normal(points[b1], points[b3], points[b2])); - } - - VerifyFaces(points); - - // Create the openSet. Add all points except the points of the seed - // hull. - for (int i = 0; i < points.Count; i++) { - if (i == b0 || i == b1 || i == b2 || i == b3) continue; - - openSet.Add(new PointFace(i, UNASSIGNED, 0.0f)); - } - - // Add the seed hull verts to the tail of the list. - openSet.Add(new PointFace(b0, INSIDE, float.NaN)); - openSet.Add(new PointFace(b1, INSIDE, float.NaN)); - openSet.Add(new PointFace(b2, INSIDE, float.NaN)); - openSet.Add(new PointFace(b3, INSIDE, float.NaN)); - - // Set the openSetTail value. Last item in the array is - // openSet.Count - 1, but four of the points (the verts of the seed - // hull) are part of the closed set, so move openSetTail to just - // before those. - openSetTail = openSet.Count - 5; - - Assert.IsTrue(openSet.Count == points.Count); - - // Assign all points of the open set. This does basically the same - // thing as ReassignPoints() - for (int i = 0; i <= openSetTail; i++) { - Assert.IsTrue(openSet[i].Face == UNASSIGNED); - Assert.IsTrue(openSet[openSetTail].Face == UNASSIGNED); - Assert.IsTrue(openSet[openSetTail + 1].Face == INSIDE); - - var assigned = false; - var fp = openSet[i]; - - Assert.IsTrue(faces.Count == 4); - Assert.IsTrue(faces.Count == faceCount); - for (int j = 0; j < 4; j++) { - Assert.IsTrue(faces.ContainsKey(j)); - - var face = faces[j]; - - var dist = PointFaceDistance(points[fp.Point], points[face.Vertex0], face); - - if (dist > 0) { - fp.Face = j; - fp.Distance = dist; - openSet[i] = fp; - - assigned = true; - break; - } - } - - if (!assigned) { - // Point is inside - fp.Face = INSIDE; - fp.Distance = float.NaN; - - // Point is inside seed hull: swap point with tail, and move - // openSetTail back. We also have to decrement i, because - // there's a new item at openSet[i], and we need to process - // it next iteration - openSet[i] = openSet[openSetTail]; - openSet[openSetTail] = fp; - - openSetTail -= 1; - i -= 1; - } - } - - VerifyOpenSet(points); - } - - /// - /// Find four points in the point cloud that are not coplanar for the - /// seed hull - /// - void FindInitialHullIndices(List points, out int b0, out int b1, out int b2, out int b3) { - var count = points.Count; - - for (int i0 = 0; i0 < count - 3; i0++) { - for (int i1 = i0 + 1; i1 < count - 2; i1++) { - var p0 = points[i0]; - var p1 = points[i1]; - - if (AreCoincident(p0, p1)) continue; - - for (int i2 = i1 + 1; i2 < count - 1; i2++) { - var p2 = points[i2]; - - if (AreCollinear(p0, p1, p2)) continue; - - for (int i3 = i2 + 1; i3 < count - 0; i3++) { - var p3 = points[i3]; - - if(AreCoplanar(p0, p1, p2, p3)) continue; - - b0 = i0; - b1 = i1; - b2 = i2; - b3 = i3; - return; - } - } - } - } - - throw new System.ArgumentException("Can't generate hull, points are coplanar"); - } - - /// - /// Grow the hull. This method takes the current hull, and expands it - /// to encompass the point in openSet with the point furthest away - /// from its face. - /// - void GrowHull(List points) { - Assert.IsTrue(openSetTail >= 0); - Assert.IsTrue(openSet[0].Face != INSIDE); - - // Find farthest point and first lit face. - var farthestPoint = 0; - var dist = openSet[0].Distance; - - for (int i = 1; i <= openSetTail; i++) { - if (openSet[i].Distance > dist) { - farthestPoint = i; - dist = openSet[i].Distance; - } - } - - // Use lit face to find horizon and the rest of the lit - // faces. - FindHorizon( - points, - points[openSet[farthestPoint].Point], - openSet[farthestPoint].Face, - faces[openSet[farthestPoint].Face]); - - VerifyHorizon(); - - // Construct new cone from horizon - ConstructCone(points, openSet[farthestPoint].Point); - - VerifyFaces(points); - - // Reassign points - ReassignPoints(points); - } - - /// - /// Start the search for the horizon. - /// - /// The search is a DFS search that searches neighboring triangles in - /// a counter-clockwise fashion. When it find a neighbor which is not - /// lit, that edge will be a line on the horizon. If the search always - /// proceeds counter-clockwise, the edges of the horizon will be found - /// in counter-clockwise order. - /// - /// The heart of the search can be found in the recursive - /// SearchHorizon() method, but the the first iteration of the search - /// is special, because it has to visit three neighbors (all the - /// neighbors of the initial triangle), while the rest of the search - /// only has to visit two (because one of them has already been - /// visited, the one you came from). - /// - void FindHorizon(List points, Vector3 point, int fi, Face face) { - // TODO should I use epsilon in the PointFaceDistance comparisons? - - litFaces.Clear(); - horizon.Clear(); - - litFaces.Add(fi); - - Assert.IsTrue(PointFaceDistance(point, points[face.Vertex0], face) > 0.0f); - - // For the rest of the recursive search calls, we first check if the - // triangle has already been visited and is part of litFaces. - // However, in this first call we can skip that because we know it - // can't possibly have been visited yet, since the only thing in - // litFaces is the current triangle. - { - var oppositeFace = faces[face.Opposite0]; - - var dist = PointFaceDistance( - point, - points[oppositeFace.Vertex0], - oppositeFace); - - if (dist <= 0.0f) { - horizon.Add(new HorizonEdge { - Face = face.Opposite0, - Edge0 = face.Vertex1, - Edge1 = face.Vertex2, - }); - } else { - SearchHorizon(points, point, fi, face.Opposite0, oppositeFace); - } - } - - if (!litFaces.Contains(face.Opposite1)) { - var oppositeFace = faces[face.Opposite1]; - - var dist = PointFaceDistance( - point, - points[oppositeFace.Vertex0], - oppositeFace); - - if (dist <= 0.0f) { - horizon.Add(new HorizonEdge { - Face = face.Opposite1, - Edge0 = face.Vertex2, - Edge1 = face.Vertex0, - }); - } else { - SearchHorizon(points, point, fi, face.Opposite1, oppositeFace); - } - } - - if (!litFaces.Contains(face.Opposite2)) { - var oppositeFace = faces[face.Opposite2]; - - var dist = PointFaceDistance( - point, - points[oppositeFace.Vertex0], - oppositeFace); - - if (dist <= 0.0f) { - horizon.Add(new HorizonEdge { - Face = face.Opposite2, - Edge0 = face.Vertex0, - Edge1 = face.Vertex1, - }); - } else { - SearchHorizon(points, point, fi, face.Opposite2, oppositeFace); - } - } - } - - /// - /// Recursively search to find the horizon or lit set. - /// - void SearchHorizon(List points, Vector3 point, int prevFaceIndex, int faceCount, Face face) { - Assert.IsTrue(prevFaceIndex >= 0); - Assert.IsTrue(litFaces.Contains(prevFaceIndex)); - Assert.IsTrue(!litFaces.Contains(faceCount)); - Assert.IsTrue(faces[faceCount].Equals(face)); - - litFaces.Add(faceCount); - - // Use prevFaceIndex to determine what the next face to search will - // be, and what edges we need to cross to get there. It's important - // that the search proceeds in counter-clockwise order from the - // previous face. - int nextFaceIndex0; - int nextFaceIndex1; - int edge0; - int edge1; - int edge2; - - if (prevFaceIndex == face.Opposite0) { - nextFaceIndex0 = face.Opposite1; - nextFaceIndex1 = face.Opposite2; - - edge0 = face.Vertex2; - edge1 = face.Vertex0; - edge2 = face.Vertex1; - } else if (prevFaceIndex == face.Opposite1) { - nextFaceIndex0 = face.Opposite2; - nextFaceIndex1 = face.Opposite0; - - edge0 = face.Vertex0; - edge1 = face.Vertex1; - edge2 = face.Vertex2; - } else { - Assert.IsTrue(prevFaceIndex == face.Opposite2); - - nextFaceIndex0 = face.Opposite0; - nextFaceIndex1 = face.Opposite1; - - edge0 = face.Vertex1; - edge1 = face.Vertex2; - edge2 = face.Vertex0; - } - - if (!litFaces.Contains(nextFaceIndex0)) { - var oppositeFace = faces[nextFaceIndex0]; - - var dist = PointFaceDistance( - point, - points[oppositeFace.Vertex0], - oppositeFace); - - if (dist <= 0.0f) { - horizon.Add(new HorizonEdge { - Face = nextFaceIndex0, - Edge0 = edge0, - Edge1 = edge1, - }); - } else { - SearchHorizon(points, point, faceCount, nextFaceIndex0, oppositeFace); - } - } - - if (!litFaces.Contains(nextFaceIndex1)) { - var oppositeFace = faces[nextFaceIndex1]; - - var dist = PointFaceDistance( - point, - points[oppositeFace.Vertex0], - oppositeFace); - - if (dist <= 0.0f) { - horizon.Add(new HorizonEdge { - Face = nextFaceIndex1, - Edge0 = edge1, - Edge1 = edge2, - }); - } else { - SearchHorizon(points, point, faceCount, nextFaceIndex1, oppositeFace); - } - } - } - - /// - /// Remove all lit faces and construct new faces from the horizon in a - /// "cone-like" fashion. - /// - /// This is a relatively straight-forward procedure, given that the - /// horizon is handed to it in already sorted counter-clockwise. The - /// neighbors of the new faces are easy to find: they're the previous - /// and next faces to be constructed in the cone, as well as the face - /// on the other side of the horizon. We also have to update the face - /// on the other side of the horizon to reflect it's new neighbor from - /// the cone. - /// - void ConstructCone(List points, int farthestPoint) { - foreach (var fi in litFaces) { - Assert.IsTrue(faces.ContainsKey(fi)); - faces.Remove(fi); - } - - var firstNewFace = faceCount; - - // Check for coplanar faces - /*var firstNormal = new Vector3(0, 0, 0); - int sameNormals = 1; - for (int i = 0; i < horizon.Count; i++) - { - var v0 = farthestPoint; - var v1 = horizon[i].Edge0; - var v2 = horizon[i].Edge1; - var norm = Normal(points[v0], points[v1], points[v2]); - if (firstNormal.IsZero) - { - firstNormal = norm; - continue; - } - - const float tol = 0.1f; - if ((firstNormal - norm).Length < tol || (firstNormal - norm).Length > 2.0f-tol) - { - sameNormals++; - } - } - - if (sameNormals == horizon.Count) - sameNormals = sameNormals;*/ - - for (int i = 0; i < horizon.Count; i++) { - // Vertices of the new face, the farthest point as well as the - // edge on the horizon. Horizon edge is CCW, so the triangle - // should be as well. - var v0 = farthestPoint; - var v1 = horizon[i].Edge0; - var v2 = horizon[i].Edge1; - - // Opposite faces of the triangle. First, the edge on the other - // side of the horizon, then the next/prev faces on the new cone - var o0 = horizon[i].Face; - var o1 = (i == horizon.Count - 1) ? firstNewFace : firstNewFace + i + 1; - var o2 = (i == 0) ? (firstNewFace + horizon.Count - 1) : firstNewFace + i - 1; - - var fi = faceCount++; - - faces[fi] = new Face( - v0, v1, v2, - o0, o1, o2, - Normal(points[v0], points[v1], points[v2])); - - var horizonFace = faces[horizon[i].Face]; - - if (horizonFace.Vertex0 == v1) { - Assert.IsTrue(v2 == horizonFace.Vertex2); - horizonFace.Opposite1 = fi; - } else if (horizonFace.Vertex1 == v1) { - Assert.IsTrue(v2 == horizonFace.Vertex0); - horizonFace.Opposite2 = fi; - } else { - Assert.IsTrue(v1 == horizonFace.Vertex2); - Assert.IsTrue(v2 == horizonFace.Vertex1); - horizonFace.Opposite0 = fi; - } - - faces[horizon[i].Face] = horizonFace; - } - } - - /// - /// Reassign points based on the new faces added by ConstructCone(). - /// - /// Only points that were previous assigned to a removed face need to - /// be updated, so check litFaces while looping through the open set. - /// - /// There is a potential optimization here: there's no reason to loop - /// through the entire openSet here. If each face had it's own - /// openSet, we could just loop through the openSets in the removed - /// faces. That would make the loop here shorter. - /// - /// However, to do that, we would have to juggle A LOT more List's, - /// and we would need an object pool to manage them all without - /// generating a whole bunch of garbage. I don't think it's worth - /// doing that to make this loop shorter, a straight for-loop through - /// a list is pretty darn fast. Still, it might be worth trying - /// - void ReassignPoints(List points) { - for (int i = 0; i <= openSetTail; i++) { - var fp = openSet[i]; - - if (litFaces.Contains(fp.Face)) { - var assigned = false; - var point = points[fp.Point]; - - foreach (var kvp in faces) { - var fi = kvp.Key; - var face = kvp.Value; - - var dist = PointFaceDistance( - point, - points[face.Vertex0], - face); - - if (dist > EPSILON) { - assigned = true; - - fp.Face = fi; - fp.Distance = dist; - - openSet[i] = fp; - break; - } - } - - if (!assigned) { - // If point hasn't been assigned, then it's inside the - // convex hull. Swap it with openSetTail, and decrement - // openSetTail. We also have to decrement i, because - // there's now a new thing in openSet[i], so we need i - // to remain the same the next iteration of the loop. - fp.Face = INSIDE; - fp.Distance = float.NaN; - - openSet[i] = openSet[openSetTail]; - openSet[openSetTail] = fp; - - i--; - openSetTail--; - } - } - } - } - - /// - /// Final step in algorithm, export the faces of the convex hull in a - /// mesh-friendly format. - /// - /// TODO normals calculation for non-split vertices. Right now it just - /// leaves the normal array empty. - /// - void ExportMesh( - List points, - bool splitVerts, - ref List verts, - ref List tris, - ref List normals) - { - if (verts == null) { - verts = new List(); - } else { - verts.Clear(); - } - - if (tris == null) { - tris = new List(); - } else { - tris.Clear(); - } - - if (normals == null) { - normals = new List(); - } else { - normals.Clear(); - } - - foreach (var face in faces.Values) { - int vi0, vi1, vi2; - - if (splitVerts) { - vi0 = verts.Count; verts.Add(points[face.Vertex0]); - vi1 = verts.Count; verts.Add(points[face.Vertex1]); - vi2 = verts.Count; verts.Add(points[face.Vertex2]); - - normals.Add(face.Normal); - normals.Add(face.Normal); - normals.Add(face.Normal); - } else { - if (!hullVerts.TryGetValue(face.Vertex0, out vi0)) { - vi0 = verts.Count; - hullVerts[face.Vertex0] = vi0; - verts.Add(points[face.Vertex0]); - } - - if (!hullVerts.TryGetValue(face.Vertex1, out vi1)) { - vi1 = verts.Count; - hullVerts[face.Vertex1] = vi1; - verts.Add(points[face.Vertex1]); - } - - if (!hullVerts.TryGetValue(face.Vertex2, out vi2)) { - vi2 = verts.Count; - hullVerts[face.Vertex2] = vi2; - verts.Add(points[face.Vertex2]); - } - } - - tris.Add(vi0); - tris.Add(vi1); - tris.Add(vi2); - } - } - - /// - /// Signed distance from face to point (a positive number means that - /// the point is above the face) - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - float PointFaceDistance(Vector3 point, Vector3 pointOnFace, Face face) { - return Dot(face.Normal, point - pointOnFace); - } - - /// - /// Calculate normal for triangle - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - Vector3 Normal(Vector3 v0, Vector3 v1, Vector3 v2) { - return Cross(v1 - v0, v2 - v0).Normalized; - } - - /// - /// Dot product, for convenience. - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static float Dot(Vector3 a, Vector3 b) { - return a.X*b.X + a.Y*b.Y + a.Z*b.Z; - } - - /// - /// Vector3.Cross i left-handed, the algorithm is right-handed. Also, - /// i wanna test to see if using aggressive inlining makes any - /// difference here. - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static Vector3 Cross(Vector3 a, Vector3 b) { - return new Vector3( - a.Y*b.Z - a.Z*b.Y, - a.Z*b.X - a.X*b.Z, - a.X*b.Y - a.Y*b.X); - } - - /// - /// Check if two points are coincident - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - bool AreCoincident(Vector3 a, Vector3 b) { - return (a - b).Length <= EPSILON; - } - - /// - /// Check if three points are collinear - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - bool AreCollinear(Vector3 a, Vector3 b, Vector3 c) { - return Cross(c - a, c - b).Length <= EPSILON; - } - - /// - /// Check if four points are coplanar - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - bool AreCoplanar(Vector3 a, Vector3 b, Vector3 c, Vector3 d) { - var n1 = Cross(c - a, c - b); - var n2 = Cross(d - a, d - b); - - var m1 = n1.Length; - var m2 = n2.Length; - - return m1 <= EPSILON - || m2 <= EPSILON - || AreCollinear(Vector3.Zero, - (1.0f / m1) * n1, - (1.0f / m2) * n2); - } - - /// - /// Method used for debugging, verifies that the openSet is in a - /// sensible state. Conditionally compiled if DEBUG_QUICKHULL if - /// defined. - /// - void VerifyOpenSet(List points) { - for (int i = 0; i < openSet.Count; i++) { - if (i > openSetTail) { - Assert.IsTrue(openSet[i].Face == INSIDE); - } else { - Assert.IsTrue(openSet[i].Face != INSIDE); - Assert.IsTrue(openSet[i].Face != UNASSIGNED); - - Assert.IsTrue(PointFaceDistance( - points[openSet[i].Point], - points[faces[openSet[i].Face].Vertex0], - faces[openSet[i].Face]) > 0.0f); - } - } - } - - /// - /// Method used for debugging, verifies that the horizon is in a - /// sensible state. Conditionally compiled if DEBUG_QUICKHULL if - /// defined. - /// - void VerifyHorizon() { - for (int i = 0; i < horizon.Count; i++) { - var prev = i == 0 ? horizon.Count - 1 : i - 1; - - Assert.IsTrue(horizon[prev].Edge1 == horizon[i].Edge0); - Assert.IsTrue(HasEdge(faces[horizon[i].Face], horizon[i].Edge1, horizon[i].Edge0)); - } - } - - /// - /// Method used for debugging, verifies that the faces array is in a - /// sensible state. Conditionally compiled if DEBUG_QUICKHULL if - /// defined. - /// - void VerifyFaces(List points) { - foreach (var kvp in faces) { - var fi = kvp.Key; - var face = kvp.Value; - - Assert.IsTrue(faces.ContainsKey(face.Opposite0)); - Assert.IsTrue(faces.ContainsKey(face.Opposite1)); - Assert.IsTrue(faces.ContainsKey(face.Opposite2)); - - Assert.IsTrue(face.Opposite0 != fi); - Assert.IsTrue(face.Opposite1 != fi); - Assert.IsTrue(face.Opposite2 != fi); - - Assert.IsTrue(face.Vertex0 != face.Vertex1); - Assert.IsTrue(face.Vertex0 != face.Vertex2); - Assert.IsTrue(face.Vertex1 != face.Vertex2); - - Assert.IsTrue(HasEdge(faces[face.Opposite0], face.Vertex2, face.Vertex1)); - Assert.IsTrue(HasEdge(faces[face.Opposite1], face.Vertex0, face.Vertex2)); - Assert.IsTrue(HasEdge(faces[face.Opposite2], face.Vertex1, face.Vertex0)); - - Assert.IsTrue((face.Normal - Normal( - points[face.Vertex0], - points[face.Vertex1], - points[face.Vertex2])).Length < EPSILON); - } - } - - /// - /// Method used for debugging, verifies that the final mesh is - /// actually a convex hull of all the points. Conditionally compiled - /// if DEBUG_QUICKHULL if defined. - /// - void VerifyMesh(List points, ref List verts, ref List tris) { - Assert.IsTrue(tris.Count % 3 == 0); - - for (int i = 0; i < points.Count; i++) { - for (int j = 0; j < tris.Count; j+=3) { - var t0 = verts[tris[j]]; - var t1 = verts[tris[j + 1]]; - var t2 = verts[tris[j + 2]]; - - var dot = Dot(points[i] - t0, Vector3.Cross(t1 - t0, t2 - t0)); - //Assert.IsTrue(dot <= EPSILON, $"not convex hull: {dot} > {EPSILON}"); - if (!(dot <= EPSILON)) - Console.PrintError($"not convex hull: {dot} > {EPSILON}"); - } - - } - } - - /// - /// Does face f have a face with vertexes e0 and e1? Used only for - /// debugging. - /// - bool HasEdge(Face f, int e0, int e1) { - return (f.Vertex0 == e0 && f.Vertex1 == e1) - || (f.Vertex1 == e0 && f.Vertex2 == e1) - || (f.Vertex2 == e0 && f.Vertex0 == e1); - } - } public class BrushGeometry { @@ -1123,6 +56,9 @@ namespace Game var calc = new ConvexHullCalculator(); calc.GenerateHull(points.ToList(), true, ref verts, ref tris, ref normals); + //var qh = new QuickHull(); + //verts = qh.QuickHull2(points); + var finalPoints = new List(); foreach (var tri in tris) @@ -2114,1467 +1050,4 @@ namespace Game base.OnDestroy(); } } - -#if false - public struct Edge - { - public Vector3 v1, v2; - - public Edge(Vector3 v1, Vector3 v2) - { - this.v1 = v1; - this.v2 = v2; - } - - public static Edge[] GetEdges(Vector3 v1, Vector3 v2, Vector3 v3) - { - return new[] - { - new Edge(v1, v2), - new Edge(v2, v3), - new Edge(v3, v1), - }; - } - - public override bool Equals(object obj) - { - if (obj is Edge) - { - var other = (Edge) obj; - var d1a = Math.Abs((v1 - other.v1).Length); - var d1b = Math.Abs((v1 - other.v2).Length); - var d2a = Math.Abs((v2 - other.v2).Length); - var d2b = Math.Abs((v2 - other.v1).Length); - - var eps = 1f; - if (d1a < eps && d2a < eps) - return true; - else if (d1b < eps && d2b < eps) - return true; - else - return false; - } - - return base.Equals(obj); - } - - public static bool operator ==(Edge edge, object obj) - { - return edge.Equals(obj); - } - - public static bool operator !=(Edge edge, object obj) - { - return !(edge == obj); - } - } - - public class Face - { - public Vector3 v1, v2, v3; - public List halfEdges; - public bool visited; - - public Face(Vector3 v1, Vector3 v2, Vector3 v3) - { - this.v1 = v1; - this.v2 = v2; - this.v3 = v3; - halfEdges = new List(3); - } - - public Edge[] GetEdges() - { - return new[] - { - new Edge(v1, v2), - new Edge(v2, v3), - new Edge(v3, v1), - }; - } - - public float DistanceToPoint(Vector3 point) - { - Plane plane = new Plane(v1, v2, v3); - - float distance = (point.X * plane.Normal.X) + (point.Y * plane.Normal.Y) + - (point.Z * plane.Normal.Z) + plane.D; - return distance / (float) Math.Sqrt( - (plane.Normal.X * plane.Normal.X) + (plane.Normal.Y * plane.Normal.Y) + - (plane.Normal.Z * plane.Normal.Z)); - } - - public float DistanceToPlane(Face face) - { - Plane plane = new Plane(v1, v2, v3); - - var center = (face.v1 + face.v2 + face.v3) / 3f; - - return plane.Normal.X * center.X + plane.Normal.Y * center.Y + plane.Normal.Z * center.Z - plane.D; - } - - public float GetArea() - { - Q3MapImporter.HalfEdge areaEdgeStart = halfEdges[0]; - Q3MapImporter.HalfEdge areaEdge = areaEdgeStart.previous; - Vector3 areaNorm = Vector3.Zero; - int iters = 0; - do - { - if (iters++ > 1000) - throw new Exception("merge infinite loop"); - areaNorm += Vector3.Cross(areaEdge.edge.v1 - areaEdgeStart.edge.v1, - areaEdge.next.edge.v1 - areaEdgeStart.edge.v1); - areaEdge = areaEdge.previous; - - } while (areaEdge != areaEdgeStart); - - return areaNorm.Length; - } - } - - public struct Tetrahedron - { - public Vector3 v1, v2, v3, v4; - - public Tetrahedron(Vector3 v1, Vector3 v2, Vector3 v3, Vector3 v4) - { - this.v1 = v1; - this.v2 = v2; - this.v3 = v3; - this.v4 = v4; - } - - public Face[] GetFaces() - { - return new[] - { - new Face(v1, v2, v3), - new Face(v1, v3, v4), - new Face(v1, v4, v2), - new Face(v2, v4, v3), - }; - } - } - - public class Q3MapImporter : Script - { - private string mapPath = @"C:\dev\GoakeFlax\Assets\Maps\cube.map"; - //private string mapPath = @"C:\dev\Goake\maps\aerowalk\aerowalk.map"; - - Model model; - public MaterialBase material; - - const float epsilon = 0.00001f; - - private void SortPoints(List points, Vector3 planeNormal) - { - Vector3 center = Vector3.Zero; - foreach (var vert in points) - { - center += vert; - } - - if (points.Count > 0) - center /= points.Count; - - points.Sort((v1, v2) => - { - var dot = Vector3.Dot(planeNormal, Vector3.Cross(v1 - center, v2 - center)); - if (dot > 0) - return 1; - else - return -1; - }); - } - - float PointDistanceFromPlane(Vector3 point, Plane plane) - { - float distance = (point.X * plane.Normal.X) + (point.Y * plane.Normal.Y) + - (point.Z * plane.Normal.Z) + plane.D; - return distance / (float) Math.Sqrt( - (plane.Normal.X * plane.Normal.X) + (plane.Normal.Y * plane.Normal.Y) + - (plane.Normal.Z * plane.Normal.Z)); - } - - private Face[] CreateInitialSimplex(Vector3[] points) - { - if (false) - { - // TODO: more optimal to find first set of points which are not coplanar? - - // find the longest edge - Vector3 v1 = Vector3.Zero; - Vector3 v2 = Vector3.Zero; - foreach (var p1 in points) - { - foreach (var p2 in points) - { - if ((p2 - p1).LengthSquared > (v2 - v1).LengthSquared) - { - v1 = p1; - v2 = p2; - } - } - } - - if (v1 == v2) - v1 = v2; - - Assert.IsTrue(v1 != v2, "a1 != a2"); - - // find the furthest point from the edge to form a face - Vector3 v3 = Vector3.Zero; - float furthestDist = 0f; - foreach (var point in points) - { - //if (vert == a1 || vert == a2) - // continue; - - var edgeDir = (v2 - v1).Normalized; - var closest = v1 + edgeDir * Vector3.Dot(point - v1, edgeDir); - - var dist = (point - closest).Length; - if (dist > furthestDist) - { - v3 = point; - furthestDist = dist; - } - } - - Assert.IsTrue(v3 != v1, "furthest != a1"); - Assert.IsTrue(v3 != v2, "furthest != a2"); - - // find the furthest point from he face - Plane plane = new Plane(v1, v2, v3); - Vector3 v4 = Vector3.Zero; - float fourthDist = 0f; - foreach (var point in points) - { - if (point == v1 || point == v2 || point == v3) - continue; - - float distance = PointDistanceFromPlane(point, plane); - if (Math.Abs(distance) > fourthDist) - { - v4 = point; - fourthDist = distance; - } - } - - // make sure the tetrahedron is in counter-clockwise order - if (fourthDist > 0) - { - return new Face[] - { - new Face(v1, v3, v2), - new Face(v1, v4, v3), - new Face(v1, v2, v4), - new Face(v2, v3, v4), - }; - } - else - { - return new Face[] - { - new Face(v1, v2, v3), - new Face(v1, v3, v4), - new Face(v1, v4, v2), - new Face(v2, v4, v3), - }; - } - } - else - { - Vector3 v1 = Vector3.Zero, v2 = Vector3.Zero, v3 = Vector3.Zero, v4 = Vector3.Zero; - bool found = false; - - foreach (var p1 in points) - { - foreach (var p2 in points) - { - if (p1 == p2) - continue; - - if (AreCoincident(p1, p2)) - continue; - - - foreach (var p3 in points) - { - if (p3 == p2 || p3 == p1) - continue; - - if (AreCollinear(p1, p2, p3)) - continue; - - foreach (var p4 in points) - { - if (p4 == p1 || p4 == p2 || p4 == p3) - continue; - - if (AreCoplanar(p1, p2, p3, p4)) - continue; - - found = true; - v1 = p1; - v2 = p2; - v3 = p3; - v4 = p4; - break; - } - } - } - } - - if (!found) - throw new Exception("CreateInitialSimplex failed"); - - Plane plane = new Plane(v1, v2, v3); - var fourthDist = PointDistanceFromPlane(v4, plane); - - if (fourthDist > 0) - { - return new Face[] - { - new Face(v1, v3, v2), - new Face(v1, v4, v3), - new Face(v1, v2, v4), - new Face(v2, v3, v4), - }; - } - else - { - return new Face[] - { - new Face(v1, v2, v3), - new Face(v1, v3, v4), - new Face(v1, v4, v2), - new Face(v2, v4, v3), - }; - } - } - } - - public class HalfEdge - { - public Face face; - - //public Face oppositeFace; - public HalfEdge opposite; - public HalfEdge previous, next; - - public Edge edge; - //public bool horizonVisited; - - public HalfEdge(Edge edge, Face face) - { - this.edge = edge; - this.face = face; - face.halfEdges.Add(this); - } - - public Vector3 tail - { - get - { - return edge.v2; - } - set - { - edge.v2 = value; - opposite.edge.v1 = value; - } - } - } - - //http://algolist.ru/maths/geom/convhull/qhull3d.php - - private void PopulateOutsideSet(List> outsideSet, Face[] faces, Vector3[] points) - { - foreach (var point in points) - { - foreach (Face face in faces) - { - float distance = face.DistanceToPoint(point); - /*if (Math.Abs(distance) < epsilon) - { - // point is in the plane, this gets merged - distance = distance; - } - else*/ - if (distance > 0) - { - //side.outsideSet.Add(point); - outsideSet.Add(new Tuple(face, point)); - break; - } - } - } - } - - private List QuickHull2(Vector3[] points) - { - Assert.IsTrue(points.Length >= 4, "points.Length >= 4"); - - var tetrahedron = CreateInitialSimplex(points); - - List> outsideSet = new List>(); - PopulateOutsideSet(outsideSet, tetrahedron, points); - - // all points not in side.outsideSet are inside in "inside" set - - // create half-edges - foreach (var face in tetrahedron) - { - var halfEdges = new List(3); - foreach (var edge in face.GetEdges()) - halfEdges.Add(new HalfEdge(edge, face)); - - for (int i = 0; i < halfEdges.Count; i++) - { - halfEdges[i].previous = halfEdges[(i + 2) % 3]; - halfEdges[i].next = halfEdges[(i + 1) % 3]; - } - } - - // verify - { - var tetrapoints = new List(); - foreach (var face in tetrahedron) - { - foreach (var he in face.halfEdges) - { - if (!tetrapoints.Contains(he.edge.v1)) - tetrapoints.Add(he.edge.v1); - } - } - - foreach (var point in tetrapoints) - { - int foundFaces = 0; - - foreach (var face in tetrahedron) - { - if (face.v1 == point) - foundFaces++; - else if (face.v2 == point) - foundFaces++; - else if (face.v3 == point) - foundFaces++; - } - - Assert.IsTrue(foundFaces == 3, "foundFaces == 3"); - } - } - - - foreach (var face in tetrahedron) - { - Assert.IsTrue(face.halfEdges.Count == 3, "side.halfEdges.Count == 3"); - foreach (var halfEdge in face.halfEdges) - { - bool found = false; - foreach (var otherFace in tetrahedron) - { - if (found) - break; - if (face == otherFace) - continue; - - foreach (var otherHalfEdge in otherFace.halfEdges) - { - if (otherHalfEdge.opposite != null) - continue; - - if (halfEdge.edge == otherHalfEdge.edge) - { - halfEdge.opposite = otherHalfEdge; - otherHalfEdge.opposite = halfEdge; - //halfEdge.oppositeFace = otherFace; - //otherHalfEdge.oppositeFace = face; - found = true; - break; - } - } - } - - Assert.IsTrue(halfEdge.previous != null, "halfEdge.previous != null"); - Assert.IsTrue(halfEdge.next != null, "halfEdge.next != null"); - Assert.IsTrue(halfEdge.opposite != null, "halfEdge.opposite != null"); - //Assert.IsTrue(halfEdge.oppositeFace != null, "halfEdge.oppositeFace != null"); - Assert.IsTrue(halfEdge.opposite.face != null, "halfEdge.opposite.face != null"); - //Assert.IsTrue(halfEdge.oppositeFace == halfEdge.opposite.face, "halfEdge.oppositeFace == halfEdge.opposite.face"); - } - } - - - // grow hull - List horizonEdges = new List(); - - List hullFaces = new List(); - hullFaces.AddRange(tetrahedron); - - // stop when none of the faces have any visible outside points - int iterCount = 0; - while (outsideSet.Count > 0) - { - iterCount++; - Tuple pointToAdd = null; - Face pointFace = null; - // get furthest point in outside set - /*for (int sideIndex = 0; sideIndex < sides.Count; sideIndex++) - { - TetrahedronSide side = sides[sideIndex]; - if (side.outsideSet.Count == 0) - continue; - - float furthestDist = 0f; - foreach (var point in side.outsideSet) - { - Assert.IsTrue(point != side.face.v1, "point != side.face.v1"); - Assert.IsTrue(point != side.face.v2, "point != side.face.v2"); - Assert.IsTrue(point != side.face.v3, "point != side.face.v3"); - - float distance = PointDistanceFromPlane(point, side.plane); - if (Math.Abs(distance) > furthestDist) - { - pointToAdd = point; - pointSide = side; - furthestDist = distance; - } - } - }*/ - - float furthestDist = 0f; - foreach (var fp in outsideSet) - { - var face = fp.Item1; - var point = fp.Item2; - - float distance = face.DistanceToPoint(point); - if (Math.Abs(distance) > furthestDist) - //if (distance > furthestDist) - { - pointToAdd = fp; - pointFace = face; - furthestDist = distance; - } - } - - Assert.IsTrue(furthestDist > 0, "furthestDist > 0"); - Assert.IsTrue(pointToAdd != null, "pointToAdd != null"); - - outsideSet.Remove(pointToAdd); - - foreach (var face in hullFaces) - { - face.visited = false; - foreach (var halfEdge in face.halfEdges) - { - Assert.IsTrue(halfEdge.opposite.opposite == halfEdge, "halfEdge.opposite.opposite == halfEdge"); - Assert.IsTrue(hullFaces.Contains(halfEdge.opposite.face), - "hullFaces.Contains(halfEdge.opposite.face)"); - } - } - - var hullFacesNew = new List(); - var unclaimedPoints = new List(); - - AddPointToHull(pointToAdd.Item2, pointFace, unclaimedPoints, outsideSet, horizonEdges, hullFacesNew); - - // remove lit/seen/visited faces, their points were added to unclaimed points - for (int i = 0; i < hullFaces.Count; i++) - { - if (hullFaces[i].visited) - { - hullFaces.RemoveAt(i); - i--; - } - } - - hullFaces.AddRange(hullFacesNew); - - foreach (var face in hullFaces) - { - face.visited = false; - foreach (var halfEdge in face.halfEdges) - { - Assert.IsTrue(halfEdge.opposite.opposite == halfEdge, - "2 halfEdge.opposite.opposite == halfEdge (degenerate face?)"); - Assert.IsTrue(hullFaces.Contains(halfEdge.opposite.face), - "2 hullFaces.Contains(halfEdge.opposite.face)"); - } - } - - foreach (var fb in outsideSet) - unclaimedPoints.Add(fb.Item2); - - outsideSet.Clear(); - PopulateOutsideSet(outsideSet, hullFaces.ToArray(), unclaimedPoints.ToArray()); - - //if (iterCount >= 3) - // break; - - if (hullFaces.Count > 1000 || iterCount > 1000) - Assert.Fail("overflow"); - if (outsideSet.Count > 100000) - Assert.Fail("outsideSet overflow"); - } - - // merge faces with similar normals - List discardedFaces = new List(); - for (int i = 0; i < hullFaces.Count; i++) - { - Face firstFace = hullFaces[i]; - // if visible? - { - while (PostAdjacentMerge(firstFace, discardedFaces, hullFaces)) - { - // - } - } - } - - foreach (var f in discardedFaces) - hullFaces.Remove(f); - - List hullPoints = new List(hullFaces.Count * 3); - foreach (var face in hullFaces) - { - hullPoints.Add(face.v1); - hullPoints.Add(face.v2); - hullPoints.Add(face.v3); - } - - return hullPoints; - } - - private void AddUnique(List list, Vector3 point) - { - foreach (var p in list) - { - if ((point - p).Length < epsilon) - return; - } - list.Add(point); - } - - bool AreCoincident(Vector3 a, Vector3 b) - { - return (a - b).Length <= epsilon; - } - - bool AreCollinear(Vector3 a, Vector3 b, Vector3 c) - { - return Vector3.Cross(c - a, c - b).Length <= epsilon; - } - - bool AreCoplanar(Vector3 a, Vector3 b, Vector3 c, Vector3 d) - { - var n1 = Vector3.Cross(c - a, c - b); - var n2 = Vector3.Cross(d - a, d - b); - - var m1 = n1.Length; - var m2 = n2.Length; - - return m1 > epsilon - && m2 > epsilon - && AreCollinear(Vector3.Zero, - (1.0f / m1) * n1, - (1.0f / m2) * n2); - } - - private bool PostAdjacentMerge(Face face, List discardedFaces, List hullFaces) - { - float maxdot_minang = Mathf.Cos(Mathf.DegreesToRadians * 3f); - HalfEdge edge = face.halfEdges[0]; - - do - { - Face oppFace = edge.opposite.face; - - bool merge = false; - Vector3 ni = new Plane(face.v1, face.v2, face.v3).Normal; - Vector3 nj = new Plane(oppFace.v1, oppFace.v2, oppFace.v3).Normal; - float dotP = Vector3.Dot(ni, nj); - - if (dotP > maxdot_minang) - { - if (face.GetArea() >= oppFace.GetArea()) - { - // check if we can merge the 2 faces - merge = canMergeFaces(edge, hullFaces); - } - } - - if (merge) - { - // mergeAdjacentFace - if (!MergeAdjacentFaces(edge, face, face, discardedFaces)) - { - throw new Exception("merge failure"); - } - return true; - } - edge = edge.next; - } while (edge != face.halfEdges[0]); - - return false; - } - - private static int asdf = 0; - bool canMergeFaces(HalfEdge he, List hullFaces) - { - asdf++; - if (asdf == 22) - asdf = asdf; - Face face1 = he.face; - Face face2 = he.opposite.face; - - // construct the merged face - List edges = new List(); - Face mergedFace = new Face(new Vector3(float.NaN), new Vector3(float.NaN), new Vector3(float.NaN)); - - // copy the first face edges - HalfEdge heTwin = null; - HalfEdge heCopy = null; - HalfEdge startEdge = (face1.halfEdges[0] != he) ? face1.halfEdges[0] : face1.halfEdges[1]; - HalfEdge copyHe = startEdge; - HalfEdge prevEdge = null; - HalfEdge firstEdge = null; - do - { - HalfEdge newEdge = new HalfEdge(copyHe.edge, mergedFace); - newEdge.opposite = copyHe.opposite; - newEdge.face = mergedFace; - newEdge.tail = copyHe.tail; - if(copyHe == he) - { - heTwin = copyHe.opposite; - heCopy = newEdge; - } - - if (firstEdge == null) - firstEdge = newEdge; - - if (prevEdge != null) - { - prevEdge.next = newEdge; - newEdge.previous = prevEdge; - } - - copyHe = copyHe.next; - prevEdge = newEdge; - } while (copyHe != startEdge); - - if (prevEdge != null) - { - prevEdge.next = firstEdge; - firstEdge.previous = prevEdge; - } - if (heCopy == null) - heCopy = firstEdge; - - // copy the second face edges - prevEdge = null; - firstEdge = null; - copyHe = face2.halfEdges[0]; - do - { - HalfEdge newEdge = new HalfEdge(copyHe.edge, mergedFace); - newEdge.opposite = copyHe.opposite; - newEdge.face = mergedFace; - newEdge.tail = copyHe.tail; - - if (firstEdge == null) - firstEdge = newEdge; - - if (heTwin == copyHe) - heTwin = newEdge; - - if (prevEdge != null) - { - prevEdge.next = newEdge; - newEdge.previous = prevEdge; - } - - copyHe = copyHe.next; - prevEdge = newEdge; - } while (copyHe != face2.halfEdges[0]); - - if (prevEdge != null) - { - prevEdge.next = firstEdge; - firstEdge.previous = prevEdge; - } - if (heTwin == null) - heTwin = firstEdge; - - mergedFace.v1 = mergedFace.halfEdges[0].edge.v1; - mergedFace.v2 = mergedFace.halfEdges[1].edge.v1; - mergedFace.v3 = mergedFace.halfEdges[2].edge.v1; - - if (heCopy == null) - heTwin = heTwin; - - Assert.IsTrue(heTwin != null, "heTwin != null"); - - HalfEdge hedgeAdjPrev = heCopy.previous; - HalfEdge hedgeAdjNext = heCopy.next; - HalfEdge hedgeOppPrev = heTwin.previous; - HalfEdge hedgeOppNext = heTwin.next; - - hedgeOppPrev.next = hedgeAdjNext; - hedgeAdjNext.previous = hedgeOppPrev; - - hedgeAdjPrev.next = hedgeOppNext; - hedgeOppNext.previous = hedgeAdjPrev; - - // compute normal and centroid - //mergedFace.computeNormalAndCentroid(); - - // test the vertex distance - float mTolarenace = epsilon;//-1; - float mPlaneTolerance = epsilon;//-1f; - float maxDist = mPlaneTolerance; - List uniqPoints = new List(); - foreach (var hullFace in hullFaces) - { - AddUnique(uniqPoints, hullFace.v1); - AddUnique(uniqPoints, hullFace.v2); - AddUnique(uniqPoints, hullFace.v3); - } - - foreach (var point in uniqPoints) - { - float dist = mergedFace.DistanceToPoint(point); - if (dist > maxDist) - { - return false; - } - } - - // check the convexity - HalfEdge qhe = mergedFace.halfEdges[0]; - Assert.IsTrue(mergedFace.halfEdges.Count == 3, "mergedFace.halfEdges.Count == 3"); - do - { - Vector3 vertex = qhe.tail; - Vector3 nextVertex = qhe.next.tail; - - Vector3 edgeVector = (nextVertex - vertex).Normalized; - Vector3 outVector = Vector3.Cross(-(new Plane(mergedFace.v1, mergedFace.v2, mergedFace.v3).Normal), edgeVector); - - HalfEdge testHe = qhe.next; - do - { - Vector3 testVertex = testHe.tail; - float dist = Vector3.Dot(testVertex - vertex, outVector); - - if (dist > mTolarenace) - return false; - - testHe = testHe.next; - } while (testHe != qhe.next); - - qhe = qhe.next; - } while (qhe != mergedFace.halfEdges[0]); - - - Face oppFace = he.opposite.face; - - HalfEdge hedgeOpp = he.opposite; - - hedgeAdjPrev = he.previous; - hedgeAdjNext = he.next; - hedgeOppPrev = hedgeOpp.previous; - hedgeOppNext = hedgeOpp.next; - - // check if we are lining up with the face in adjPrev dir - while (hedgeAdjPrev.opposite.face == oppFace) - { - hedgeAdjPrev = hedgeAdjPrev.previous; - hedgeOppNext = hedgeOppNext.next; - } - - // check if we are lining up with the face in adjNext dir - while (hedgeAdjNext.opposite.face == oppFace) - { - hedgeOppPrev = hedgeOppPrev.previous; - hedgeAdjNext = hedgeAdjNext.next; - } - - // no redundant merges, just clean merge of 2 neighbour faces - if (hedgeOppPrev.opposite.face == hedgeAdjNext.opposite.face) - { - return false; - } - - if (hedgeAdjPrev.opposite.face == hedgeOppNext.opposite.face) - { - return false; - } - - return true; - } - - private void AddPointToHull(Vector3 point, Face face, List unclaimedPoints, - List> outsideSet, - List horizonEdges, List hullFaces) - { - horizonEdges.Clear(); - - CalculateHorizon(face, point, unclaimedPoints, outsideSet, horizonEdges, face.halfEdges[0]); - - // create new faces - if (horizonEdges.Count > 0) - { - List newFaces = new List(); - HalfEdge firstEdge = horizonEdges.First(); - HalfEdge prevEdge = null; - foreach (var edge in horizonEdges) - { - var newFace = new Face(point, edge.edge.v1, edge.edge.v2); - var newPlane = new Plane(newFace.v1, newFace.v2, newFace.v3); - - var uniqPoints = new List(); - AddUnique(uniqPoints, newFace.v1); - AddUnique(uniqPoints, newFace.v2); - AddUnique(uniqPoints, newFace.v3); - - var fourtPoint = edge.opposite.next.edge.v2; - - AddUnique(uniqPoints, edge.opposite.next.edge.v1); - AddUnique(uniqPoints, edge.opposite.next.edge.v2); - AddUnique(uniqPoints, edge.opposite.previous.edge.v1); - AddUnique(uniqPoints, edge.opposite.previous.edge.v2); - - var distFromPlane = PointDistanceFromPlane(fourtPoint, newPlane); - if (Math.Abs(distFromPlane) < epsilon) - { - // both faces are coplanar, merge them together - - - distFromPlane = distFromPlane; - if (AreCoplanar(newFace.v1, newFace.v2, newFace.v3, fourtPoint)) - distFromPlane = distFromPlane; - } - else if (AreCoplanar(newFace.v1, newFace.v2, newFace.v3, fourtPoint)) - { - distFromPlane = distFromPlane; - } - - - - var newEdges = new List(); - foreach (var ne in newFace.GetEdges()) - newEdges.Add(new HalfEdge(ne, newFace)); - - for (int i = 0; i < newEdges.Count; i++) - { - newEdges[i].previous = newEdges[(i + 2) % 3]; - newEdges[i].next = newEdges[(i + 1) % 3]; - } - - if (prevEdge != null) - { - var prevAdjacentEdge = newFaces.Last().halfEdges.Last(); - var lastAdjacentEdge = newEdges.First(); - lastAdjacentEdge.opposite = prevAdjacentEdge; - prevAdjacentEdge.opposite = lastAdjacentEdge; - } - - //edge.face = newFace; - - newEdges[1].opposite = edge.opposite; - edge.opposite.opposite = newEdges[1]; - - newFaces.Add(newFace); - prevEdge = edge; - } - - if (prevEdge != null) - { - var lastAdjacentEdge = newFaces.Last().halfEdges.Last(); - var firstAdjacentEdge = newFaces.First().halfEdges.First(); - lastAdjacentEdge.opposite = firstAdjacentEdge; - firstAdjacentEdge.opposite = lastAdjacentEdge; - //first.previous.opposite = prev.next; - //prev.next.opposite = first.previous; - } - - // merge NONCONVEX_WRT_LARGER_FACE - - //List discardedFaces = new List(); - if (false) - { - foreach (var newFace in newFaces) - { - // if face visible? - while (AdjacentMerge(point, newFace, unclaimedPoints, outsideSet, true)) - { - // merge until failure - } - - hullFaces.Add(newFace); - } - - foreach (var newFace in newFaces) - { - // if face non-convex? - // mark face as visible? - while (AdjacentMerge(point, newFace, unclaimedPoints, outsideSet, false)) - { - // merge until failure - } - - hullFaces.Add(newFace); - } - } - else - hullFaces.AddRange(newFaces); - - // verify - foreach (var newFace in hullFaces) - { - Assert.IsTrue(newFace.halfEdges.Count == 3, "AddPointToHull: side.halfEdges.Count == 3"); - foreach (var halfEdge in newFace.halfEdges) - { - /*bool found = false; - foreach (var otherFace in hullFaces) - { - if (found) - break; - if (newFace == otherFace) - continue; - - foreach (var otherHalfEdge in otherFace.halfEdges) - { - if (otherHalfEdge.opposite != null) - continue; - - if (halfEdge.edge == otherHalfEdge.edge) - { - halfEdge.opposite = otherHalfEdge; - otherHalfEdge.opposite = halfEdge; - //halfEdge.oppositeFace = otherFace; - //otherHalfEdge.oppositeFace = face; - found = true; - break; - } - } - }*/ - - Assert.IsTrue(halfEdge.previous != null, "AddPointToHull: halfEdge.previous != null"); - Assert.IsTrue(halfEdge.next != null, "AddPointToHull: halfEdge.next != null"); - Assert.IsTrue(halfEdge.next.next.next == halfEdge, "AddPointToHull: halfEdge.next.next.next == halfEdge"); - Assert.IsTrue(halfEdge.previous.previous.previous == halfEdge, "AddPointToHull: halfEdge.previous.previous.previous == halfEdge"); - Assert.IsTrue(halfEdge.opposite != null, "AddPointToHull: halfEdge.opposite != null"); - //Assert.IsTrue(halfEdge.oppositeFace != null, "halfEdge.oppositeFace != null"); - Assert.IsTrue(halfEdge.opposite.face != null, "AddPointToHull: halfEdge.opposite.face != null"); - //Assert.IsTrue(halfEdge.oppositeFace == halfEdge.opposite.face, "halfEdge.oppositeFace == halfEdge.opposite.face"); - } - } - } - } - - private bool AdjacentMerge(Vector3 point, Face face, List unclaimedPoints, List> outsideSet, bool mergeWrtLargerFace) - { - const float tolerance = -1f; - - HalfEdge edge = face.halfEdges[0]; - - bool convex = true; - do - { - Face oppositeFace = edge.opposite.face; - bool merge = false; - - var p1 = new Plane(face.v1, face.v2, face.v3); - var p2 = new Plane(oppositeFace.v1, oppositeFace.v2, oppositeFace.v3); - - if (mergeWrtLargerFace) - { - float faceArea = edge.face.GetArea(); - float oppositeArea = edge.opposite.face.GetArea(); - - if (faceArea > oppositeArea) - { - if (edge.face.DistanceToPlane(edge.opposite.face) > -tolerance) - merge = true; - else if (edge.opposite.face.DistanceToPlane(edge.face) > -tolerance) - convex = false; - } - else - { - if (edge.opposite.face.DistanceToPlane(edge.face) > -tolerance) - merge = true; - else if (edge.face.DistanceToPlane(edge.opposite.face) > -tolerance) - convex = false; - } - } - else - { - if (edge.face.DistanceToPlane(edge.opposite.face) > -tolerance || - edge.opposite.face.DistanceToPlane(edge.face) > -tolerance) - { - merge = true; - } - } - - if (merge) - { - List discardedFaces = new List(); - // mergeAdjacentFace - if (!MergeAdjacentFaces(edge, face, face, discardedFaces)) - { - throw new Exception("merge failure"); - } - - foreach (var dface in discardedFaces) - { - for (int i=0; i 0) - outsideSet[i] = new Tuple(face, outsideSet[i].Item2); - else - unclaimedPoints.Add(outsideSet[i].Item2); - } - } - } - } - } while (edge != face.halfEdges[0]); - - return false; // no merge - } - - private bool MergeAdjacentFaces(HalfEdge edge, Face newFace, Face oldFace, List discardedFaces) - { - Face oppositeFace = edge.opposite.face; - - discardedFaces.Add(oppositeFace); - - HalfEdge prev = edge.previous; - HalfEdge next = edge.next; - HalfEdge oppositePrev = edge.opposite.previous; - HalfEdge oppositeNext = edge.opposite.next; - - HalfEdge breakEdge = prev; - while (prev.opposite.face == oppositeFace) - { - prev = prev.previous; - oppositeNext = oppositeNext.next; - - if (prev == breakEdge) - return false; - } - - breakEdge = next; - while (next.opposite.face == oppositeFace) - { - oppositePrev = oppositePrev.previous; - next = next.next; - - if (next == breakEdge) - return false; - } - - for (HalfEdge e = oppositeNext; e != oppositePrev.next; e = e.next) - e.face = newFace; - - if (edge == oldFace.halfEdges[0]) - oldFace.halfEdges[0] = next; - - Face discardedFace = ConnectHalfEdges(newFace, oppositePrev, next); - Face discardedFace2 = ConnectHalfEdges(newFace, prev, oppositeNext); - - if (discardedFace != null) - discardedFaces.Add(discardedFace); - if (discardedFace2 != null) - discardedFaces.Add(discardedFace2); - - return true; - } - - // merges adjacent faces - private Face ConnectHalfEdges(Face face, HalfEdge prev, HalfEdge edge) - { - Face discardedFace = null; - - if (prev.opposite.face == edge.opposite.face) - { - Face oppFace = edge.opposite.face; - HalfEdge hedgeOpp; - - if (prev == face.halfEdges[0]) - { - face.halfEdges[0] = edge; - } - - bool isDegenerate = false; - { - // is this correct? - HalfEdge s = oppFace.halfEdges[0]; - if (s.next.next.next != s) - isDegenerate = true; - else if (s.previous.previous.previous != s) - isDegenerate = true; - else if (s.next.next == s) - isDegenerate = true; - else if (s.previous.previous == s) - isDegenerate = true; - - HalfEdge ee = s; - int numVerts = 0; - do - { - numVerts++; - ee = ee.next; - } while (ee != s); - - if (numVerts <= 2) - isDegenerate = true; - } - - //if (oppFace.numVertices() == 3) - if (!isDegenerate) - { - // then we can get rid of the - // opposite face altogether - hedgeOpp = edge.opposite.previous.opposite; - - //oppFace.mark = DELETED; - discardedFace = oppFace; - } - else - { - hedgeOpp = edge.opposite.next; - - if (oppFace.halfEdges[0] == hedgeOpp.previous) { - oppFace.halfEdges[0] = hedgeOpp; - } - hedgeOpp.previous = hedgeOpp.previous.previous; - hedgeOpp.previous.next = hedgeOpp; - } - edge.previous = prev.previous; - edge.previous.next = edge; - - edge.opposite = hedgeOpp; - hedgeOpp.opposite = edge; - - // oppFace was modified, so need to recompute - //oppFace.computeNormalAndCentroid(); - } - else - { - prev.next = edge; - edge.previous = prev; - } - - return discardedFace; - } - - // calculates the outermost edges of the geometry seen from the eyePoint - private void CalculateHorizon(Face face, Vector3 eyePoint, List unclaimedPoints, - List> outsideSet, - List horizonEdges, HalfEdge currentEdge) - { - face.visited = true; - - // move outside points of this face to unclaimed points - foreach (var set in outsideSet) - { - if (set.Item1 == face) - unclaimedPoints.Add(set.Item2); - } - - HalfEdge startingEdge = currentEdge; - do - { - Face oppositeFace = currentEdge.opposite.face; - if (!oppositeFace.visited) - { - var dist = oppositeFace.DistanceToPoint(eyePoint); - if (dist > epsilon) - { - // positive distance means this is visible - CalculateHorizon(oppositeFace, eyePoint, unclaimedPoints, outsideSet, horizonEdges, - currentEdge.opposite); - } - /*else if (Math.Abs(dist) <= epsilon) - { - dist = dist; - }*/ - else - { - if (!horizonEdges.Contains(currentEdge)) - horizonEdges.Add(currentEdge); - } - } - - currentEdge = currentEdge.next; - } while (currentEdge != startingEdge); - } - - public override void OnStart() - { - byte[] mapChars = File.ReadAllBytes(mapPath); - var root = MapParser.Parse(mapChars); - - const float cs = 3000f; - - Vector3[] cubePoints = new[] - { - new Vector3(-cs, -cs, -cs), - new Vector3(cs, -cs, -cs), - new Vector3(-cs, cs, -cs), - new Vector3(cs, cs, -cs), - new Vector3(-cs, -cs, cs), - new Vector3(cs, -cs, cs), - new Vector3(-cs, cs, cs), - new Vector3(cs, cs, cs), - }; - Vector3[] cubeVerts = QuickHull2(cubePoints).ToArray(); - - List vertices = new List(); - foreach (var brush in root.entities[0].brushes.Take(1)) - { - List brushVertices = new List(cubeVerts); - //foreach (var plane in new [] { brush.planes.First() }) - foreach (var plane in brush.planes.Reverse().Take(1)) - //foreach (var plane in brush.planes) - { - Plane p = new Plane(plane.v1, plane.v2, plane.v3); - Vector3 planeNormal = p.Normal; - List newBrushVertices = new List(); - List faceVertices = new List(); - - if (true) - { - Func isFront = (f) => f > epsilon; - Func isBack = (f) => f < -epsilon; - - for (int i = 0; i < brushVertices.Count; i++) - { - int i2 = ((i + 1) % 3 == 0) ? (i - 2) : (i + 1); - Vector3 start = brushVertices[i]; - Vector3 end = brushVertices[i2]; - - var d1 = (start.X * planeNormal.X) + (start.Y * planeNormal.Y) + (start.Z * planeNormal.Z) + - p.D; - var d2 = (end.X * planeNormal.X) + (end.Y * planeNormal.Y) + (end.Z * planeNormal.Z) + p.D; - - if (isBack(d1)) - { - // include back - faceVertices.Add(start); - } - - if (isBack(d1) && isFront(d2) || isFront(d1) && isBack(d2)) - { - //if (isFront(d2)) - { - // include clip - Ray ray2 = new Ray(start, (end - start).Normalized); - if (p.Intersects(ref ray2, out Vector3 intersect2)) - faceVertices.Add(intersect2); - else - d1 = d1; - } - } - } - - if (true) - { - var newMeshPoints = new List(); - int duplis = 0; - foreach (var v in faceVertices) - { - bool found = false; - foreach (var vo in newMeshPoints) - { - if ((v - vo).Length < epsilon) - { - found = true; - duplis++; - break; - } - } - - //if (!newMeshPoints.Contains(v)) - if (!found) - newMeshPoints.Add(v); - } - - if (duplis > 0) - Console.Print("duplicates: " + duplis); - - if (newMeshPoints.Count > 0) - { - var hullPoints = QuickHull2(newMeshPoints.ToArray()); - newBrushVertices.Clear(); - newBrushVertices.AddRange(hullPoints); - } - else - newBrushVertices.Clear(); - } - } - - Assert.IsTrue(newBrushVertices.Count % 3 == 0, - "invalid amount of vertices: " + newBrushVertices.Count); - //Assert.IsTrue(newBrushVertices.Count > 0, - // "brush was clipped completely, vertices: " + newBrushVertices.Count); - - brushVertices.Clear(); - brushVertices.AddRange(newBrushVertices); - Console.Print("plane verts: " + newBrushVertices.Count); - } - - vertices.AddRange(brushVertices); - } - - - model = Content.CreateVirtualAsset(); - model.SetupLODs(new int[] {1}); - { - var mesh = model.LODs[0].Meshes[0]; - List triangles = new List(vertices.Count); - for (int i = 0; i < vertices.Count; i++) - triangles.Add(i); - Console.Print("verts: " + vertices.Count); - mesh.UpdateMesh(vertices.ToArray(), triangles.ToArray(), vertices.ToArray()); - } - - StaticModel childModel = Actor.AddChild(); - childModel.Name = "MapModel"; - childModel.Model = model; - childModel.SetMaterial(0, material); - } - - public override void OnEnable() - { - // Here you can add code that needs to be called when script is enabled (eg. register for events) - } - - public override void OnDisable() - { - // Here you can add code that needs to be called when script is disabled (eg. unregister from events) - } - - public override void OnUpdate() - { - // Here you can add code that needs to be called every frame - } - - public override void OnDestroy() - { - Destroy(ref model); - base.OnDestroy(); - } - } -#endif } \ No newline at end of file diff --git a/Source/Game/QuickHull.cs b/Source/Game/QuickHull.cs new file mode 100644 index 0000000..2af13d2 --- /dev/null +++ b/Source/Game/QuickHull.cs @@ -0,0 +1,1312 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Runtime.CompilerServices; +using System.Threading; +using FlaxEngine; +using FlaxEngine.Assertions; +using FlaxEngine.Utilities; + +namespace Game +{ + public class HalfEdge + { + public Face face; + + //public Face oppositeFace; + public HalfEdge opposite; + public HalfEdge previous, next; + + public Edge edge; + //public bool horizonVisited; + + public HalfEdge(Edge edge, Face face) + { + this.edge = edge; + this.face = face; + face.halfEdges.Add(this); + } + + public Vector3 tail + { + get + { + return edge.v2; + } + set + { + edge.v2 = value; + opposite.edge.v1 = value; + } + } + } + + public struct Edge + { + public Vector3 v1, v2; + + public Edge(Vector3 v1, Vector3 v2) + { + this.v1 = v1; + this.v2 = v2; + } + + public static Edge[] GetEdges(Vector3 v1, Vector3 v2, Vector3 v3) + { + return new[] + { + new Edge(v1, v2), + new Edge(v2, v3), + new Edge(v3, v1), + }; + } + + public override bool Equals(object obj) + { + if (obj is Edge) + { + var other = (Edge) obj; + var d1a = Math.Abs((v1 - other.v1).Length); + var d1b = Math.Abs((v1 - other.v2).Length); + var d2a = Math.Abs((v2 - other.v2).Length); + var d2b = Math.Abs((v2 - other.v1).Length); + + var eps = 1f; + if (d1a < eps && d2a < eps) + return true; + else if (d1b < eps && d2b < eps) + return true; + else + return false; + } + + return base.Equals(obj); + } + + public static bool operator ==(Edge edge, object obj) + { + return edge.Equals(obj); + } + + public static bool operator !=(Edge edge, object obj) + { + return !(edge == obj); + } + } + + public class Face + { + public Vector3 v1, v2, v3; + public List halfEdges; + public bool visited; + + public Face(Vector3 v1, Vector3 v2, Vector3 v3) + { + this.v1 = v1; + this.v2 = v2; + this.v3 = v3; + halfEdges = new List(3); + } + + public Edge[] GetEdges() + { + return new[] + { + new Edge(v1, v2), + new Edge(v2, v3), + new Edge(v3, v1), + }; + } + + public float DistanceToPoint(Vector3 point) + { + Plane plane = new Plane(v1, v2, v3); + + float distance = (point.X * plane.Normal.X) + (point.Y * plane.Normal.Y) + + (point.Z * plane.Normal.Z) + plane.D; + return distance / (float) Math.Sqrt( + (plane.Normal.X * plane.Normal.X) + (plane.Normal.Y * plane.Normal.Y) + + (plane.Normal.Z * plane.Normal.Z)); + } + + public float DistanceToPlane(Face face) + { + Plane plane = new Plane(v1, v2, v3); + + var center = (face.v1 + face.v2 + face.v3) / 3f; + + return plane.Normal.X * center.X + plane.Normal.Y * center.Y + plane.Normal.Z * center.Z - plane.D; + } + + public float GetArea() + { + HalfEdge areaEdgeStart = halfEdges[0]; + HalfEdge areaEdge = areaEdgeStart.previous; + Vector3 areaNorm = Vector3.Zero; + int iters = 0; + do + { + if (iters++ > 1000) + throw new Exception("merge infinite loop"); + areaNorm += Vector3.Cross(areaEdge.edge.v1 - areaEdgeStart.edge.v1, + areaEdge.next.edge.v1 - areaEdgeStart.edge.v1); + areaEdge = areaEdge.previous; + + } while (areaEdge != areaEdgeStart); + + return areaNorm.Length; + } + } + + public struct Tetrahedron + { + public Vector3 v1, v2, v3, v4; + + public Tetrahedron(Vector3 v1, Vector3 v2, Vector3 v3, Vector3 v4) + { + this.v1 = v1; + this.v2 = v2; + this.v3 = v3; + this.v4 = v4; + } + + public Face[] GetFaces() + { + return new[] + { + new Face(v1, v2, v3), + new Face(v1, v3, v4), + new Face(v1, v4, v2), + new Face(v2, v4, v3), + }; + } + } + + public class QuickHull + { + const float epsilon = 0.01f; + + private void SortPoints(List points, Vector3 planeNormal) + { + Vector3 center = Vector3.Zero; + foreach (var vert in points) + { + center += vert; + } + + if (points.Count > 0) + center /= points.Count; + + points.Sort((v1, v2) => + { + var dot = Vector3.Dot(planeNormal, Vector3.Cross(v1 - center, v2 - center)); + if (dot > 0) + return 1; + else + return -1; + }); + } + + float PointDistanceFromPlane(Vector3 point, Plane plane) + { + float distance = (point.X * plane.Normal.X) + (point.Y * plane.Normal.Y) + + (point.Z * plane.Normal.Z) + plane.D; + return distance / (float) Math.Sqrt( + (plane.Normal.X * plane.Normal.X) + (plane.Normal.Y * plane.Normal.Y) + + (plane.Normal.Z * plane.Normal.Z)); + } + + private Face[] CreateInitialSimplex(Vector3[] points) + { + if (false) + { + // TODO: more optimal to find first set of points which are not coplanar? + + // find the longest edge + Vector3 v1 = Vector3.Zero; + Vector3 v2 = Vector3.Zero; + foreach (var p1 in points) + { + foreach (var p2 in points) + { + if ((p2 - p1).LengthSquared > (v2 - v1).LengthSquared) + { + v1 = p1; + v2 = p2; + } + } + } + + if (v1 == v2) + v1 = v2; + + Assert.IsTrue(v1 != v2, "a1 != a2"); + + // find the furthest point from the edge to form a face + Vector3 v3 = Vector3.Zero; + float furthestDist = 0f; + foreach (var point in points) + { + //if (vert == a1 || vert == a2) + // continue; + + var edgeDir = (v2 - v1).Normalized; + var closest = v1 + edgeDir * Vector3.Dot(point - v1, edgeDir); + + var dist = (point - closest).Length; + if (dist > furthestDist) + { + v3 = point; + furthestDist = dist; + } + } + + Assert.IsTrue(v3 != v1, "furthest != a1"); + Assert.IsTrue(v3 != v2, "furthest != a2"); + + // find the furthest point from he face + Plane plane = new Plane(v1, v2, v3); + Vector3 v4 = Vector3.Zero; + float fourthDist = 0f; + foreach (var point in points) + { + if (point == v1 || point == v2 || point == v3) + continue; + + float distance = PointDistanceFromPlane(point, plane); + if (Math.Abs(distance) > fourthDist) + { + v4 = point; + fourthDist = distance; + } + } + + // make sure the tetrahedron is in counter-clockwise order + if (fourthDist > 0) + { + return new Face[] + { + new Face(v1, v3, v2), + new Face(v1, v4, v3), + new Face(v1, v2, v4), + new Face(v2, v3, v4), + }; + } + else + { + return new Face[] + { + new Face(v1, v2, v3), + new Face(v1, v3, v4), + new Face(v1, v4, v2), + new Face(v2, v4, v3), + }; + } + } + else + { + Vector3 v1 = Vector3.Zero, v2 = Vector3.Zero, v3 = Vector3.Zero, v4 = Vector3.Zero; + bool found = false; + + foreach (var p1 in points) + { + foreach (var p2 in points) + { + if (p1 == p2) + continue; + + if (AreCoincident(p1, p2)) + continue; + + + foreach (var p3 in points) + { + if (p3 == p2 || p3 == p1) + continue; + + if (AreCollinear(p1, p2, p3)) + continue; + + foreach (var p4 in points) + { + if (p4 == p1 || p4 == p2 || p4 == p3) + continue; + + if (AreCoplanar(p1, p2, p3, p4)) + continue; + + found = true; + v1 = p1; + v2 = p2; + v3 = p3; + v4 = p4; + break; + } + } + } + } + + if (!found) + throw new Exception("CreateInitialSimplex failed"); + + Plane plane = new Plane(v1, v2, v3); + var fourthDist = PointDistanceFromPlane(v4, plane); + + if (fourthDist > 0) + { + return new Face[] + { + new Face(v1, v3, v2), + new Face(v1, v4, v3), + new Face(v1, v2, v4), + new Face(v2, v3, v4), + }; + } + else + { + return new Face[] + { + new Face(v1, v2, v3), + new Face(v1, v3, v4), + new Face(v1, v4, v2), + new Face(v2, v4, v3), + }; + } + } + } + + + + //http://algolist.ru/maths/geom/convhull/qhull3d.php + + private void PopulateOutsideSet(List> outsideSet, Face[] faces, Vector3[] points) + { + foreach (var point in points) + { + foreach (Face face in faces) + { + float distance = face.DistanceToPoint(point); + /*if (Math.Abs(distance) < epsilon) + { + // point is in the plane, this gets merged + distance = distance; + } + else*/ + if (distance > 0) + { + //side.outsideSet.Add(point); + outsideSet.Add(new Tuple(face, point)); + break; + } + } + } + } + + public List QuickHull2(Vector3[] points) + { + Assert.IsTrue(points.Length >= 4, "points.Length >= 4"); + + var tetrahedron = CreateInitialSimplex(points); + + List> outsideSet = new List>(); + PopulateOutsideSet(outsideSet, tetrahedron, points); + + // all points not in side.outsideSet are inside in "inside" set + + // create half-edges + foreach (var face in tetrahedron) + { + var halfEdges = new List(3); + foreach (var edge in face.GetEdges()) + halfEdges.Add(new HalfEdge(edge, face)); + + for (int i = 0; i < halfEdges.Count; i++) + { + halfEdges[i].previous = halfEdges[(i + 2) % 3]; + halfEdges[i].next = halfEdges[(i + 1) % 3]; + } + } + + // verify + { + var tetrapoints = new List(); + foreach (var face in tetrahedron) + { + foreach (var he in face.halfEdges) + { + if (!tetrapoints.Contains(he.edge.v1)) + tetrapoints.Add(he.edge.v1); + } + } + + foreach (var point in tetrapoints) + { + int foundFaces = 0; + + foreach (var face in tetrahedron) + { + if (face.v1 == point) + foundFaces++; + else if (face.v2 == point) + foundFaces++; + else if (face.v3 == point) + foundFaces++; + } + + Assert.IsTrue(foundFaces == 3, "foundFaces == 3"); + } + } + + + foreach (var face in tetrahedron) + { + Assert.IsTrue(face.halfEdges.Count == 3, "side.halfEdges.Count == 3"); + foreach (var halfEdge in face.halfEdges) + { + bool found = false; + foreach (var otherFace in tetrahedron) + { + if (found) + break; + if (face == otherFace) + continue; + + foreach (var otherHalfEdge in otherFace.halfEdges) + { + if (otherHalfEdge.opposite != null) + continue; + + if (halfEdge.edge == otherHalfEdge.edge) + { + halfEdge.opposite = otherHalfEdge; + otherHalfEdge.opposite = halfEdge; + //halfEdge.oppositeFace = otherFace; + //otherHalfEdge.oppositeFace = face; + found = true; + break; + } + } + } + + Assert.IsTrue(halfEdge.previous != null, "halfEdge.previous != null"); + Assert.IsTrue(halfEdge.next != null, "halfEdge.next != null"); + Assert.IsTrue(halfEdge.opposite != null, "halfEdge.opposite != null"); + //Assert.IsTrue(halfEdge.oppositeFace != null, "halfEdge.oppositeFace != null"); + Assert.IsTrue(halfEdge.opposite.face != null, "halfEdge.opposite.face != null"); + //Assert.IsTrue(halfEdge.oppositeFace == halfEdge.opposite.face, "halfEdge.oppositeFace == halfEdge.opposite.face"); + } + } + + + // grow hull + List horizonEdges = new List(); + + List hullFaces = new List(); + hullFaces.AddRange(tetrahedron); + + // stop when none of the faces have any visible outside points + int iterCount = 0; + while (outsideSet.Count > 0) + { + iterCount++; + Tuple pointToAdd = null; + Face pointFace = null; + // get furthest point in outside set + /*for (int sideIndex = 0; sideIndex < sides.Count; sideIndex++) + { + TetrahedronSide side = sides[sideIndex]; + if (side.outsideSet.Count == 0) + continue; + + float furthestDist = 0f; + foreach (var point in side.outsideSet) + { + Assert.IsTrue(point != side.face.v1, "point != side.face.v1"); + Assert.IsTrue(point != side.face.v2, "point != side.face.v2"); + Assert.IsTrue(point != side.face.v3, "point != side.face.v3"); + + float distance = PointDistanceFromPlane(point, side.plane); + if (Math.Abs(distance) > furthestDist) + { + pointToAdd = point; + pointSide = side; + furthestDist = distance; + } + } + }*/ + + float furthestDist = 0f; + foreach (var fp in outsideSet) + { + var face = fp.Item1; + var point = fp.Item2; + + float distance = face.DistanceToPoint(point); + if (Math.Abs(distance) > furthestDist) + //if (distance > furthestDist) + { + pointToAdd = fp; + pointFace = face; + furthestDist = distance; + } + } + + Assert.IsTrue(furthestDist > 0, "furthestDist > 0"); + Assert.IsTrue(pointToAdd != null, "pointToAdd != null"); + + outsideSet.Remove(pointToAdd); + + foreach (var face in hullFaces) + { + face.visited = false; + foreach (var halfEdge in face.halfEdges) + { + Assert.IsTrue(halfEdge.opposite.opposite == halfEdge, "halfEdge.opposite.opposite == halfEdge"); + Assert.IsTrue(hullFaces.Contains(halfEdge.opposite.face), + "hullFaces.Contains(halfEdge.opposite.face)"); + } + } + + var hullFacesNew = new List(); + var unclaimedPoints = new List(); + + AddPointToHull(pointToAdd.Item2, pointFace, unclaimedPoints, outsideSet, horizonEdges, hullFacesNew); + + // remove lit/seen/visited faces, their points were added to unclaimed points + for (int i = 0; i < hullFaces.Count; i++) + { + if (hullFaces[i].visited) + { + hullFaces.RemoveAt(i); + i--; + } + } + + hullFaces.AddRange(hullFacesNew); + + foreach (var face in hullFaces) + { + face.visited = false; + foreach (var halfEdge in face.halfEdges) + { + Assert.IsTrue(halfEdge.opposite.opposite == halfEdge, + "2 halfEdge.opposite.opposite == halfEdge (degenerate face?)"); + Assert.IsTrue(hullFaces.Contains(halfEdge.opposite.face), + "2 hullFaces.Contains(halfEdge.opposite.face)"); + } + } + + foreach (var fb in outsideSet) + unclaimedPoints.Add(fb.Item2); + + outsideSet.Clear(); + PopulateOutsideSet(outsideSet, hullFaces.ToArray(), unclaimedPoints.ToArray()); + + //if (iterCount >= 3) + // break; + + if (hullFaces.Count > 1000 || iterCount > 1000) + Assert.Fail("overflow"); + if (outsideSet.Count > 100000) + Assert.Fail("outsideSet overflow"); + } + + // merge faces with similar normals + List discardedFaces = new List(); + for (int i = 0; i < hullFaces.Count; i++) + { + Face firstFace = hullFaces[i]; + // if visible? + { + while (PostAdjacentMerge(firstFace, discardedFaces, hullFaces)) + { + // + } + } + } + + foreach (var f in discardedFaces) + hullFaces.Remove(f); + + List hullPoints = new List(hullFaces.Count * 3); + foreach (var face in hullFaces) + { + hullPoints.Add(face.v1); + hullPoints.Add(face.v2); + hullPoints.Add(face.v3); + } + + return hullPoints; + } + + private void AddUnique(List list, Vector3 point) + { + foreach (var p in list) + { + if ((point - p).Length < epsilon) + return; + } + list.Add(point); + } + + bool AreCoincident(Vector3 a, Vector3 b) + { + return (a - b).Length <= epsilon; + } + + bool AreCollinear(Vector3 a, Vector3 b, Vector3 c) + { + return Vector3.Cross(c - a, c - b).Length <= epsilon; + } + + bool AreCoplanar(Vector3 a, Vector3 b, Vector3 c, Vector3 d) + { + var n1 = Vector3.Cross(c - a, c - b); + var n2 = Vector3.Cross(d - a, d - b); + + var m1 = n1.Length; + var m2 = n2.Length; + + return m1 > epsilon + && m2 > epsilon + && AreCollinear(Vector3.Zero, + (1.0f / m1) * n1, + (1.0f / m2) * n2); + } + + private bool PostAdjacentMerge(Face face, List discardedFaces, List hullFaces) + { + float maxdot_minang = Mathf.Cos(Mathf.DegreesToRadians * 3f); + HalfEdge edge = face.halfEdges[0]; + + do + { + Face oppFace = edge.opposite.face; + + bool merge = false; + Vector3 ni = new Plane(face.v1, face.v2, face.v3).Normal; + Vector3 nj = new Plane(oppFace.v1, oppFace.v2, oppFace.v3).Normal; + float dotP = Vector3.Dot(ni, nj); + + if (dotP > maxdot_minang) + { + if (face.GetArea() >= oppFace.GetArea()) + { + // check if we can merge the 2 faces + merge = canMergeFaces(edge, hullFaces); + } + } + + if (merge) + { + // mergeAdjacentFace + if (!MergeAdjacentFaces(edge, face, face, discardedFaces)) + { + throw new Exception("merge failure"); + } + return true; + } + edge = edge.next; + } while (edge != face.halfEdges[0]); + + return false; + } + + private static int asdf = 0; + bool canMergeFaces(HalfEdge he, List hullFaces) + { + asdf++; + if (asdf == 22) + asdf = asdf; + Face face1 = he.face; + Face face2 = he.opposite.face; + + // construct the merged face + List edges = new List(); + Face mergedFace = new Face(new Vector3(float.NaN), new Vector3(float.NaN), new Vector3(float.NaN)); + + // copy the first face edges + HalfEdge heTwin = null; + HalfEdge heCopy = null; + HalfEdge startEdge = (face1.halfEdges[0] != he) ? face1.halfEdges[0] : face1.halfEdges[1]; + HalfEdge copyHe = startEdge; + HalfEdge prevEdge = null; + HalfEdge firstEdge = null; + do + { + HalfEdge newEdge = new HalfEdge(copyHe.edge, mergedFace); + newEdge.opposite = copyHe.opposite; + newEdge.face = mergedFace; + newEdge.tail = copyHe.tail; + if(copyHe == he) + { + heTwin = copyHe.opposite; + heCopy = newEdge; + } + + if (firstEdge == null) + firstEdge = newEdge; + + if (prevEdge != null) + { + prevEdge.next = newEdge; + newEdge.previous = prevEdge; + } + + copyHe = copyHe.next; + prevEdge = newEdge; + } while (copyHe != startEdge); + + if (prevEdge != null) + { + prevEdge.next = firstEdge; + firstEdge.previous = prevEdge; + } + if (heCopy == null) + heCopy = firstEdge; + + // copy the second face edges + prevEdge = null; + firstEdge = null; + copyHe = face2.halfEdges[0]; + do + { + HalfEdge newEdge = new HalfEdge(copyHe.edge, mergedFace); + newEdge.opposite = copyHe.opposite; + newEdge.face = mergedFace; + newEdge.tail = copyHe.tail; + + if (firstEdge == null) + firstEdge = newEdge; + + if (heTwin == copyHe) + heTwin = newEdge; + + if (prevEdge != null) + { + prevEdge.next = newEdge; + newEdge.previous = prevEdge; + } + + copyHe = copyHe.next; + prevEdge = newEdge; + } while (copyHe != face2.halfEdges[0]); + + if (prevEdge != null) + { + prevEdge.next = firstEdge; + firstEdge.previous = prevEdge; + } + if (heTwin == null) + heTwin = firstEdge; + + mergedFace.v1 = mergedFace.halfEdges[0].edge.v1; + mergedFace.v2 = mergedFace.halfEdges[1].edge.v1; + mergedFace.v3 = mergedFace.halfEdges[2].edge.v1; + + if (heCopy == null) + heTwin = heTwin; + + Assert.IsTrue(heTwin != null, "heTwin != null"); + + HalfEdge hedgeAdjPrev = heCopy.previous; + HalfEdge hedgeAdjNext = heCopy.next; + HalfEdge hedgeOppPrev = heTwin.previous; + HalfEdge hedgeOppNext = heTwin.next; + + hedgeOppPrev.next = hedgeAdjNext; + hedgeAdjNext.previous = hedgeOppPrev; + + hedgeAdjPrev.next = hedgeOppNext; + hedgeOppNext.previous = hedgeAdjPrev; + + // compute normal and centroid + //mergedFace.computeNormalAndCentroid(); + + // test the vertex distance + float mTolarenace = epsilon;//-1; + float mPlaneTolerance = epsilon;//-1f; + float maxDist = mPlaneTolerance; + List uniqPoints = new List(); + foreach (var hullFace in hullFaces) + { + AddUnique(uniqPoints, hullFace.v1); + AddUnique(uniqPoints, hullFace.v2); + AddUnique(uniqPoints, hullFace.v3); + } + + foreach (var point in uniqPoints) + { + float dist = mergedFace.DistanceToPoint(point); + if (dist > maxDist) + { + return false; + } + } + + // check the convexity + HalfEdge qhe = mergedFace.halfEdges[0]; + Assert.IsTrue(mergedFace.halfEdges.Count == 3, "mergedFace.halfEdges.Count == 3"); + do + { + Vector3 vertex = qhe.tail; + Vector3 nextVertex = qhe.next.tail; + + Vector3 edgeVector = (nextVertex - vertex).Normalized; + Vector3 outVector = Vector3.Cross(-(new Plane(mergedFace.v1, mergedFace.v2, mergedFace.v3).Normal), edgeVector); + + HalfEdge testHe = qhe.next; + do + { + Vector3 testVertex = testHe.tail; + float dist = Vector3.Dot(testVertex - vertex, outVector); + + if (dist > mTolarenace) + return false; + + testHe = testHe.next; + } while (testHe != qhe.next); + + qhe = qhe.next; + } while (qhe != mergedFace.halfEdges[0]); + + + Face oppFace = he.opposite.face; + + HalfEdge hedgeOpp = he.opposite; + + hedgeAdjPrev = he.previous; + hedgeAdjNext = he.next; + hedgeOppPrev = hedgeOpp.previous; + hedgeOppNext = hedgeOpp.next; + + // check if we are lining up with the face in adjPrev dir + while (hedgeAdjPrev.opposite.face == oppFace) + { + hedgeAdjPrev = hedgeAdjPrev.previous; + hedgeOppNext = hedgeOppNext.next; + } + + // check if we are lining up with the face in adjNext dir + while (hedgeAdjNext.opposite.face == oppFace) + { + hedgeOppPrev = hedgeOppPrev.previous; + hedgeAdjNext = hedgeAdjNext.next; + } + + // no redundant merges, just clean merge of 2 neighbour faces + if (hedgeOppPrev.opposite.face == hedgeAdjNext.opposite.face) + { + return false; + } + + if (hedgeAdjPrev.opposite.face == hedgeOppNext.opposite.face) + { + return false; + } + + return true; + } + + private void AddPointToHull(Vector3 point, Face face, List unclaimedPoints, + List> outsideSet, + List horizonEdges, List hullFaces) + { + horizonEdges.Clear(); + + CalculateHorizon(face, point, unclaimedPoints, outsideSet, horizonEdges, face.halfEdges[0]); + + // create new faces + if (horizonEdges.Count > 0) + { + List newFaces = new List(); + HalfEdge firstEdge = horizonEdges.First(); + HalfEdge prevEdge = null; + foreach (var edge in horizonEdges) + { + var newFace = new Face(point, edge.edge.v1, edge.edge.v2); + var newPlane = new Plane(newFace.v1, newFace.v2, newFace.v3); + + var uniqPoints = new List(); + AddUnique(uniqPoints, newFace.v1); + AddUnique(uniqPoints, newFace.v2); + AddUnique(uniqPoints, newFace.v3); + + var fourtPoint = edge.opposite.next.edge.v2; + + AddUnique(uniqPoints, edge.opposite.next.edge.v1); + AddUnique(uniqPoints, edge.opposite.next.edge.v2); + AddUnique(uniqPoints, edge.opposite.previous.edge.v1); + AddUnique(uniqPoints, edge.opposite.previous.edge.v2); + + var distFromPlane = PointDistanceFromPlane(fourtPoint, newPlane); + if (Math.Abs(distFromPlane) < epsilon) + { + // both faces are coplanar, merge them together + + + distFromPlane = distFromPlane; + if (AreCoplanar(newFace.v1, newFace.v2, newFace.v3, fourtPoint)) + distFromPlane = distFromPlane; + } + else if (AreCoplanar(newFace.v1, newFace.v2, newFace.v3, fourtPoint)) + { + distFromPlane = distFromPlane; + } + + + + var newEdges = new List(); + foreach (var ne in newFace.GetEdges()) + newEdges.Add(new HalfEdge(ne, newFace)); + + for (int i = 0; i < newEdges.Count; i++) + { + newEdges[i].previous = newEdges[(i + 2) % 3]; + newEdges[i].next = newEdges[(i + 1) % 3]; + } + + if (prevEdge != null) + { + var prevAdjacentEdge = newFaces.Last().halfEdges.Last(); + var lastAdjacentEdge = newEdges.First(); + lastAdjacentEdge.opposite = prevAdjacentEdge; + prevAdjacentEdge.opposite = lastAdjacentEdge; + } + + //edge.face = newFace; + + newEdges[1].opposite = edge.opposite; + edge.opposite.opposite = newEdges[1]; + + newFaces.Add(newFace); + prevEdge = edge; + } + + if (prevEdge != null) + { + var lastAdjacentEdge = newFaces.Last().halfEdges.Last(); + var firstAdjacentEdge = newFaces.First().halfEdges.First(); + lastAdjacentEdge.opposite = firstAdjacentEdge; + firstAdjacentEdge.opposite = lastAdjacentEdge; + //first.previous.opposite = prev.next; + //prev.next.opposite = first.previous; + } + + // merge NONCONVEX_WRT_LARGER_FACE + + //List discardedFaces = new List(); + if (false) + { + foreach (var newFace in newFaces) + { + // if face visible? + while (AdjacentMerge(point, newFace, unclaimedPoints, outsideSet, true)) + { + // merge until failure + } + + hullFaces.Add(newFace); + } + + foreach (var newFace in newFaces) + { + // if face non-convex? + // mark face as visible? + while (AdjacentMerge(point, newFace, unclaimedPoints, outsideSet, false)) + { + // merge until failure + } + + hullFaces.Add(newFace); + } + } + else + hullFaces.AddRange(newFaces); + + // verify + foreach (var newFace in hullFaces) + { + Assert.IsTrue(newFace.halfEdges.Count == 3, "AddPointToHull: side.halfEdges.Count == 3"); + foreach (var halfEdge in newFace.halfEdges) + { + /*bool found = false; + foreach (var otherFace in hullFaces) + { + if (found) + break; + if (newFace == otherFace) + continue; + + foreach (var otherHalfEdge in otherFace.halfEdges) + { + if (otherHalfEdge.opposite != null) + continue; + + if (halfEdge.edge == otherHalfEdge.edge) + { + halfEdge.opposite = otherHalfEdge; + otherHalfEdge.opposite = halfEdge; + //halfEdge.oppositeFace = otherFace; + //otherHalfEdge.oppositeFace = face; + found = true; + break; + } + } + }*/ + + Assert.IsTrue(halfEdge.previous != null, "AddPointToHull: halfEdge.previous != null"); + Assert.IsTrue(halfEdge.next != null, "AddPointToHull: halfEdge.next != null"); + Assert.IsTrue(halfEdge.next.next.next == halfEdge, "AddPointToHull: halfEdge.next.next.next == halfEdge"); + Assert.IsTrue(halfEdge.previous.previous.previous == halfEdge, "AddPointToHull: halfEdge.previous.previous.previous == halfEdge"); + Assert.IsTrue(halfEdge.opposite != null, "AddPointToHull: halfEdge.opposite != null"); + //Assert.IsTrue(halfEdge.oppositeFace != null, "halfEdge.oppositeFace != null"); + Assert.IsTrue(halfEdge.opposite.face != null, "AddPointToHull: halfEdge.opposite.face != null"); + //Assert.IsTrue(halfEdge.oppositeFace == halfEdge.opposite.face, "halfEdge.oppositeFace == halfEdge.opposite.face"); + } + } + } + } + + private bool AdjacentMerge(Vector3 point, Face face, List unclaimedPoints, List> outsideSet, bool mergeWrtLargerFace) + { + const float tolerance = -1f; + + HalfEdge edge = face.halfEdges[0]; + + bool convex = true; + do + { + Face oppositeFace = edge.opposite.face; + bool merge = false; + + var p1 = new Plane(face.v1, face.v2, face.v3); + var p2 = new Plane(oppositeFace.v1, oppositeFace.v2, oppositeFace.v3); + + if (mergeWrtLargerFace) + { + float faceArea = edge.face.GetArea(); + float oppositeArea = edge.opposite.face.GetArea(); + + if (faceArea > oppositeArea) + { + if (edge.face.DistanceToPlane(edge.opposite.face) > -tolerance) + merge = true; + else if (edge.opposite.face.DistanceToPlane(edge.face) > -tolerance) + convex = false; + } + else + { + if (edge.opposite.face.DistanceToPlane(edge.face) > -tolerance) + merge = true; + else if (edge.face.DistanceToPlane(edge.opposite.face) > -tolerance) + convex = false; + } + } + else + { + if (edge.face.DistanceToPlane(edge.opposite.face) > -tolerance || + edge.opposite.face.DistanceToPlane(edge.face) > -tolerance) + { + merge = true; + } + } + + if (merge) + { + List discardedFaces = new List(); + // mergeAdjacentFace + if (!MergeAdjacentFaces(edge, face, face, discardedFaces)) + { + throw new Exception("merge failure"); + } + + foreach (var dface in discardedFaces) + { + for (int i=0; i 0) + outsideSet[i] = new Tuple(face, outsideSet[i].Item2); + else + unclaimedPoints.Add(outsideSet[i].Item2); + } + } + } + } + } while (edge != face.halfEdges[0]); + + return false; // no merge + } + + private bool MergeAdjacentFaces(HalfEdge edge, Face newFace, Face oldFace, List discardedFaces) + { + Face oppositeFace = edge.opposite.face; + + discardedFaces.Add(oppositeFace); + + HalfEdge prev = edge.previous; + HalfEdge next = edge.next; + HalfEdge oppositePrev = edge.opposite.previous; + HalfEdge oppositeNext = edge.opposite.next; + + HalfEdge breakEdge = prev; + while (prev.opposite.face == oppositeFace) + { + prev = prev.previous; + oppositeNext = oppositeNext.next; + + if (prev == breakEdge) + return false; + } + + breakEdge = next; + while (next.opposite.face == oppositeFace) + { + oppositePrev = oppositePrev.previous; + next = next.next; + + if (next == breakEdge) + return false; + } + + for (HalfEdge e = oppositeNext; e != oppositePrev.next; e = e.next) + e.face = newFace; + + if (edge == oldFace.halfEdges[0]) + oldFace.halfEdges[0] = next; + + Face discardedFace = ConnectHalfEdges(newFace, oppositePrev, next); + Face discardedFace2 = ConnectHalfEdges(newFace, prev, oppositeNext); + + if (discardedFace != null) + discardedFaces.Add(discardedFace); + if (discardedFace2 != null) + discardedFaces.Add(discardedFace2); + + return true; + } + + // merges adjacent faces + private Face ConnectHalfEdges(Face face, HalfEdge prev, HalfEdge edge) + { + Face discardedFace = null; + + if (prev.opposite.face == edge.opposite.face) + { + Face oppFace = edge.opposite.face; + HalfEdge hedgeOpp; + + if (prev == face.halfEdges[0]) + { + face.halfEdges[0] = edge; + } + + bool isDegenerate = false; + { + // is this correct? + HalfEdge s = oppFace.halfEdges[0]; + if (s.next.next.next != s) + isDegenerate = true; + else if (s.previous.previous.previous != s) + isDegenerate = true; + else if (s.next.next == s) + isDegenerate = true; + else if (s.previous.previous == s) + isDegenerate = true; + + HalfEdge ee = s; + int numVerts = 0; + do + { + numVerts++; + ee = ee.next; + } while (ee != s); + + if (numVerts <= 2) + isDegenerate = true; + } + + //if (oppFace.numVertices() == 3) + if (!isDegenerate) + { + // then we can get rid of the + // opposite face altogether + hedgeOpp = edge.opposite.previous.opposite; + + //oppFace.mark = DELETED; + discardedFace = oppFace; + } + else + { + hedgeOpp = edge.opposite.next; + + if (oppFace.halfEdges[0] == hedgeOpp.previous) { + oppFace.halfEdges[0] = hedgeOpp; + } + hedgeOpp.previous = hedgeOpp.previous.previous; + hedgeOpp.previous.next = hedgeOpp; + } + edge.previous = prev.previous; + edge.previous.next = edge; + + edge.opposite = hedgeOpp; + hedgeOpp.opposite = edge; + + // oppFace was modified, so need to recompute + //oppFace.computeNormalAndCentroid(); + } + else + { + prev.next = edge; + edge.previous = prev; + } + + return discardedFace; + } + + // calculates the outermost edges of the geometry seen from the eyePoint + private void CalculateHorizon(Face face, Vector3 eyePoint, List unclaimedPoints, + List> outsideSet, + List horizonEdges, HalfEdge currentEdge) + { + face.visited = true; + + // move outside points of this face to unclaimed points + foreach (var set in outsideSet) + { + if (set.Item1 == face) + unclaimedPoints.Add(set.Item2); + } + + HalfEdge startingEdge = currentEdge; + do + { + Face oppositeFace = currentEdge.opposite.face; + if (!oppositeFace.visited) + { + var dist = oppositeFace.DistanceToPoint(eyePoint); + if (dist > epsilon) + { + // positive distance means this is visible + CalculateHorizon(oppositeFace, eyePoint, unclaimedPoints, outsideSet, horizonEdges, + currentEdge.opposite); + } + /*else if (Math.Abs(dist) <= epsilon) + { + dist = dist; + }*/ + else + { + if (!horizonEdges.Contains(currentEdge)) + horizonEdges.Add(currentEdge); + } + } + + currentEdge = currentEdge.next; + } while (currentEdge != startingEdge); + } + } +} \ No newline at end of file diff --git a/Source/Game/QuickHull2.cs b/Source/Game/QuickHull2.cs new file mode 100644 index 0000000..78b233e --- /dev/null +++ b/Source/Game/QuickHull2.cs @@ -0,0 +1,1078 @@ +using System.Collections.Generic; +using System.Runtime.CompilerServices; +using System.Threading; +using FlaxEngine; +using FlaxEngine.Assertions; +using FlaxEngine.Utilities; + +namespace Game +{ + /// + /// An implementation of the quickhull algorithm for generating 3d convex + /// hulls. + /// + /// The algorithm works like this: you start with an initial "seed" hull, + /// that is just a simple tetrahedron made up of four points in the point + /// cloud. This seed hull is then grown until it all the points in the + /// point cloud is inside of it, at which point it will be the convex hull + /// for the entire set. + /// + /// All of the points in the point cloud is divided into two parts, the + /// "open set" and the "closed set". The open set consists of all the + /// points outside of the tetrahedron, and the closed set is all of the + /// points inside the tetrahedron. After each iteration of the algorithm, + /// the closed set gets bigger and the open set get smaller. When the open + /// set is empty, the algorithm is finished. + /// + /// Each point in the open set is assigned to a face that it lies outside + /// of. To grow the hull, the point in the open set which is farthest from + /// it's face is chosen. All faces which are facing that point (I call + /// them "lit faces" in the code, because if you imagine the point as a + /// point light, it's the set of points which would be lit by that point + /// light) are removed, and a "horizon" of edges is found from where the + /// faces were removed. From this horizon, new faces are constructed in a + /// "cone" like fashion connecting the point to the edges. + /// + /// To keep track of the faces, I use a struct for each face which + /// contains the three vertices of the face in CCW order, as well as the + /// three triangles which share an edge. I was considering doing a + /// half-edge structure to store the mesh, but it's not needed. Using a + /// struct for each face and neighbors simplify the algorithm and makes it + /// easy to export it as a mesh. + /// + /// The most subtle part of the algorithm is finding the horizon. In order + /// to properly construct the cone so that all neighbors are kept + /// consistent, you can do a depth-first search from the first lit face. + /// If the depth-first search always proceeeds in a counter-clockwise + /// fashion, it guarantees that the horizon will be found in a + /// counter-clockwise order, which makes it easy to construct the cone of + /// new faces. + /// + /// A note: the code uses a right-handed coordinate system, where the + /// cross-product uses the right-hand rule and the faces are in CCW order. + /// At the end of the algorithm, the hull is exported in a Unity-friendly + /// fashion, with a left-handed mesh. + /// + public class ConvexHullCalculator { + + /// + /// Constant representing a point that has yet to be assigned to a + /// face. It's only used immediately after constructing the seed hull. + /// + const int UNASSIGNED = -2; + + /// + /// Constant representing a point that is inside the convex hull, and + /// thus is behind all faces. In the openSet array, all points with + /// INSIDE are at the end of the array, with indexes larger + /// openSetTail. + /// + const int INSIDE = -1; + + /// + /// Epsilon value. If the coordinates of the point space are + /// exceptionally close to each other, this value might need to be + /// adjusted. + /// + const float EPSILON = 0.001f; + + /// + /// Struct representing a single face. + /// + /// Vertex0, Vertex1 and Vertex2 are the vertices in CCW order. They + /// acutal points are stored in the points array, these are just + /// indexes into that array. + /// + /// Opposite0, Opposite1 and Opposite2 are the keys to the faces which + /// share an edge with this face. Opposite0 is the face opposite + /// Vertex0 (so it has an edge with Vertex2 and Vertex1), etc. + /// + /// Normal is (unsurprisingly) the normal of the triangle. + /// + struct Face { + public int Vertex0; + public int Vertex1; + public int Vertex2; + + public int Opposite0; + public int Opposite1; + public int Opposite2; + + public Vector3 Normal; + + public Face(int v0, int v1, int v2, int o0, int o1, int o2, Vector3 normal) { + Vertex0 = v0; + Vertex1 = v1; + Vertex2 = v2; + Opposite0 = o0; + Opposite1 = o1; + Opposite2 = o2; + Normal = normal; + } + + public bool Equals(Face other) { + return (this.Vertex0 == other.Vertex0) + && (this.Vertex1 == other.Vertex1) + && (this.Vertex2 == other.Vertex2) + && (this.Opposite0 == other.Opposite0) + && (this.Opposite1 == other.Opposite1) + && (this.Opposite2 == other.Opposite2) + && (this.Normal == other.Normal); + } + } + + /// + /// Struct representing a mapping between a point and a face. These + /// are used in the openSet array. + /// + /// Point is the index of the point in the points array, Face is the + /// key of the face in the Key dictionary, Distance is the distance + /// from the face to the point. + /// + struct PointFace { + public int Point; + public int Face; + public float Distance; + + public PointFace(int p, int f, float d) { + Point = p; + Face = f; + Distance = d; + } + } + + /// + /// Struct representing a single edge in the horizon. + /// + /// Edge0 and Edge1 are the vertexes of edge in CCW order, Face is the + /// face on the other side of the horizon. + /// + /// TODO Edge1 isn't actually needed, you can just index the next item + /// in the horizon array. + /// + struct HorizonEdge { + public int Face; + public int Edge0; + public int Edge1; + } + + /// + /// A dictionary storing the faces of the currently generated convex + /// hull. The key is the id of the face, used in the Face, PointFace + /// and HorizonEdge struct. + /// + /// This is a Dictionary, because we need both random access to it, + /// the ability to loop through it, and ability to quickly delete + /// faces (in the ConstructCone method), and Dictionary is the obvious + /// candidate that can do all of those things. + /// + /// I'm wondering if using a Dictionary is best idea, though. It might + /// be better to just have them in a List and mark a face as + /// deleted by adding a field to the Face struct. The downside is that + /// we would need an extra field in the Face struct, and when we're + /// looping through the points in openSet, we would have to loop + /// through all the Faces EVER created in the algorithm, and skip the + /// ones that have been marked as deleted. However, looping through a + /// list is fairly fast, and it might be worth it to avoid Dictionary + /// overhead. + /// + /// TODO test converting to a List instead. + /// + Dictionary faces; + + /// + /// The set of points to be processed. "openSet" is a misleading name, + /// because it's both the open set (points which are still outside the + /// convex hull) and the closed set (points that are inside the convex + /// hull). The first part of the array (with indexes <= openSetTail) + /// is the openSet, the last part of the array (with indexes > + /// openSetTail) are the closed set, with Face set to INSIDE. The + /// closed set is largely irrelevant to the algorithm, the open set is + /// what matters. + /// + /// Storing the entire open set in one big list has a downside: when + /// we're reassigning points after ConstructCone, we only need to + /// reassign points that belong to the faces that have been removed, + /// but storing it in one array, we have to loop through the entire + /// list, and checking litFaces to determine which we can skip and + /// which need to be reassigned. + /// + /// The alternative here is to give each face in Face array it's own + /// openSet. I don't like that solution, because then you have to + /// juggle so many more heap-allocated List's, we'd have to use + /// object pools and such. It would do a lot more allocation, and it + /// would have worse locality. I should maybe test that solution, but + /// it probably wont be faster enough (if at all) to justify the extra + /// allocations. + /// + List openSet; + + /// + /// Set of faces which are "lit" by the current point in the set. This + /// is used in the FindHorizon() DFS search to keep track of which + /// faces we've already visited, and in the ReassignPoints() method to + /// know which points need to be reassigned. + /// + HashSet litFaces; + + /// + /// The current horizon. Generated by the FindHorizon() DFS search, + /// and used in ConstructCone to construct new faces. The list of + /// edges are in CCW order. + /// + List horizon; + + /// + /// If SplitVerts is false, this Dictionary is used to keep track of + /// which points we've added to the final mesh. + /// + Dictionary hullVerts; + + /// + /// The "tail" of the openSet, the last index of a vertex that has + /// been assigned to a face. + /// + int openSetTail = -1; + + /// + /// When adding a new face to the faces Dictionary, use this for the + /// key and then increment it. + /// + int faceCount = 0; + + /// + /// Generate a convex hull from points in points array, and store the + /// mesh in Unity-friendly format in verts and tris. If splitVerts is + /// true, the the verts will be split, if false, the same vert will be + /// used for more than one triangle. + /// + public void GenerateHull( + List points, + bool splitVerts, + ref List verts, + ref List tris, + ref List normals) + { + if (points.Count < 4) { + throw new System.ArgumentException("Need at least 4 points to generate a convex hull"); + } + + Initialize(points, splitVerts); + + GenerateInitialHull(points); + + while (openSetTail >= 0) { + GrowHull(points); + } + + ExportMesh(points, splitVerts, ref verts, ref tris, ref normals); + //VerifyMesh(points, ref verts, ref tris); + } + + /// + /// Make sure all the buffers and variables needed for the algorithm + /// are initialized. + /// + void Initialize(List points, bool splitVerts) { + faceCount = 0; + openSetTail = -1; + + if (faces == null) { + faces = new Dictionary(); + litFaces = new HashSet(); + horizon = new List(); + openSet = new List(points.Count); + } else { + faces.Clear(); + litFaces.Clear(); + horizon.Clear(); + openSet.Clear(); + + if (openSet.Capacity < points.Count) { + // i wonder if this is a good idea... if you call + // GenerateHull over and over with slightly increasing + // points counts, it's going to reallocate every time. Maybe + // i should just use .Add(), and let the List manage the + // capacity, increasing it geometrically every time we need + // to reallocate. + + // maybe do + // openSet.Capacity = Mathf.NextPowerOfTwo(points.Count) + // instead? + + openSet.Capacity = points.Count; + } + } + + if (!splitVerts) { + if (hullVerts == null) { + hullVerts = new Dictionary(); + } else { + hullVerts.Clear(); + } + } + } + + /// + /// Create initial seed hull. + /// + void GenerateInitialHull(List points) { + // Find points suitable for use as the seed hull. Some varieties of + // this algorithm pick extreme points here, but I'm not convinced + // you gain all that much from that. Currently what it does is just + // find the first four points that are not coplanar. + int b0, b1, b2, b3; + FindInitialHullIndices(points, out b0, out b1, out b2, out b3); + + var v0 = points[b0]; + var v1 = points[b1]; + var v2 = points[b2]; + var v3 = points[b3]; + + var above = Dot(v3 - v1, Cross(v1 - v0, v2 - v0)) > 0.0f; + + // Create the faces of the seed hull. You need to draw a diagram + // here, otherwise it's impossible to know what's going on :) + + // Basically: there are two different possible start-tetrahedrons, + // depending on whether the fourth point is above or below the base + // triangle. If you draw a tetrahedron with these coordinates (in a + // right-handed coordinate-system): + + // b0 = (0,0,0) + // b1 = (1,0,0) + // b2 = (0,1,0) + // b3 = (0,0,1) + + // you can see the first case (set b3 = (0,0,-1) for the second + // case). The faces are added with the proper references to the + // faces opposite each vertex + + faceCount = 0; + if (above) { + faces[faceCount++] = new Face(b0, b2, b1, 3, 1, 2, Normal(points[b0], points[b2], points[b1])); + faces[faceCount++] = new Face(b0, b1, b3, 3, 2, 0, Normal(points[b0], points[b1], points[b3])); + faces[faceCount++] = new Face(b0, b3, b2, 3, 0, 1, Normal(points[b0], points[b3], points[b2])); + faces[faceCount++] = new Face(b1, b2, b3, 2, 1, 0, Normal(points[b1], points[b2], points[b3])); + } else { + faces[faceCount++] = new Face(b0, b1, b2, 3, 2, 1, Normal(points[b0], points[b1], points[b2])); + faces[faceCount++] = new Face(b0, b3, b1, 3, 0, 2, Normal(points[b0], points[b3], points[b1])); + faces[faceCount++] = new Face(b0, b2, b3, 3, 1, 0, Normal(points[b0], points[b2], points[b3])); + faces[faceCount++] = new Face(b1, b3, b2, 2, 0, 1, Normal(points[b1], points[b3], points[b2])); + } + + VerifyFaces(points); + + // Create the openSet. Add all points except the points of the seed + // hull. + for (int i = 0; i < points.Count; i++) { + if (i == b0 || i == b1 || i == b2 || i == b3) continue; + + openSet.Add(new PointFace(i, UNASSIGNED, 0.0f)); + } + + // Add the seed hull verts to the tail of the list. + openSet.Add(new PointFace(b0, INSIDE, float.NaN)); + openSet.Add(new PointFace(b1, INSIDE, float.NaN)); + openSet.Add(new PointFace(b2, INSIDE, float.NaN)); + openSet.Add(new PointFace(b3, INSIDE, float.NaN)); + + // Set the openSetTail value. Last item in the array is + // openSet.Count - 1, but four of the points (the verts of the seed + // hull) are part of the closed set, so move openSetTail to just + // before those. + openSetTail = openSet.Count - 5; + + Assert.IsTrue(openSet.Count == points.Count); + + // Assign all points of the open set. This does basically the same + // thing as ReassignPoints() + for (int i = 0; i <= openSetTail; i++) { + Assert.IsTrue(openSet[i].Face == UNASSIGNED); + Assert.IsTrue(openSet[openSetTail].Face == UNASSIGNED); + Assert.IsTrue(openSet[openSetTail + 1].Face == INSIDE); + + var assigned = false; + var fp = openSet[i]; + + Assert.IsTrue(faces.Count == 4); + Assert.IsTrue(faces.Count == faceCount); + for (int j = 0; j < 4; j++) { + Assert.IsTrue(faces.ContainsKey(j)); + + var face = faces[j]; + + var dist = PointFaceDistance(points[fp.Point], points[face.Vertex0], face); + + if (dist > 0) { + fp.Face = j; + fp.Distance = dist; + openSet[i] = fp; + + assigned = true; + break; + } + } + + if (!assigned) { + // Point is inside + fp.Face = INSIDE; + fp.Distance = float.NaN; + + // Point is inside seed hull: swap point with tail, and move + // openSetTail back. We also have to decrement i, because + // there's a new item at openSet[i], and we need to process + // it next iteration + openSet[i] = openSet[openSetTail]; + openSet[openSetTail] = fp; + + openSetTail -= 1; + i -= 1; + } + } + + VerifyOpenSet(points); + } + + /// + /// Find four points in the point cloud that are not coplanar for the + /// seed hull + /// + void FindInitialHullIndices(List points, out int b0, out int b1, out int b2, out int b3) { + var count = points.Count; + + for (int i0 = 0; i0 < count - 3; i0++) { + for (int i1 = i0 + 1; i1 < count - 2; i1++) { + var p0 = points[i0]; + var p1 = points[i1]; + + if (AreCoincident(p0, p1)) continue; + + for (int i2 = i1 + 1; i2 < count - 1; i2++) { + var p2 = points[i2]; + + if (AreCollinear(p0, p1, p2)) continue; + + for (int i3 = i2 + 1; i3 < count - 0; i3++) { + var p3 = points[i3]; + + if(AreCoplanar(p0, p1, p2, p3)) continue; + + b0 = i0; + b1 = i1; + b2 = i2; + b3 = i3; + return; + } + } + } + } + + throw new System.ArgumentException("Can't generate hull, points are coplanar"); + } + + /// + /// Grow the hull. This method takes the current hull, and expands it + /// to encompass the point in openSet with the point furthest away + /// from its face. + /// + void GrowHull(List points) { + Assert.IsTrue(openSetTail >= 0); + Assert.IsTrue(openSet[0].Face != INSIDE); + + // Find farthest point and first lit face. + var farthestPoint = 0; + var dist = openSet[0].Distance; + + for (int i = 1; i <= openSetTail; i++) { + if (openSet[i].Distance > dist) { + farthestPoint = i; + dist = openSet[i].Distance; + } + } + + // Use lit face to find horizon and the rest of the lit + // faces. + FindHorizon( + points, + points[openSet[farthestPoint].Point], + openSet[farthestPoint].Face, + faces[openSet[farthestPoint].Face]); + + VerifyHorizon(); + + // Construct new cone from horizon + ConstructCone(points, openSet[farthestPoint].Point); + + VerifyFaces(points); + + // Reassign points + ReassignPoints(points); + } + + /// + /// Start the search for the horizon. + /// + /// The search is a DFS search that searches neighboring triangles in + /// a counter-clockwise fashion. When it find a neighbor which is not + /// lit, that edge will be a line on the horizon. If the search always + /// proceeds counter-clockwise, the edges of the horizon will be found + /// in counter-clockwise order. + /// + /// The heart of the search can be found in the recursive + /// SearchHorizon() method, but the the first iteration of the search + /// is special, because it has to visit three neighbors (all the + /// neighbors of the initial triangle), while the rest of the search + /// only has to visit two (because one of them has already been + /// visited, the one you came from). + /// + void FindHorizon(List points, Vector3 point, int fi, Face face) { + // TODO should I use epsilon in the PointFaceDistance comparisons? + + litFaces.Clear(); + horizon.Clear(); + + litFaces.Add(fi); + + Assert.IsTrue(PointFaceDistance(point, points[face.Vertex0], face) > 0.0f); + + // For the rest of the recursive search calls, we first check if the + // triangle has already been visited and is part of litFaces. + // However, in this first call we can skip that because we know it + // can't possibly have been visited yet, since the only thing in + // litFaces is the current triangle. + { + var oppositeFace = faces[face.Opposite0]; + + var dist = PointFaceDistance( + point, + points[oppositeFace.Vertex0], + oppositeFace); + + if (dist <= 0.0f) { + horizon.Add(new HorizonEdge { + Face = face.Opposite0, + Edge0 = face.Vertex1, + Edge1 = face.Vertex2, + }); + } else { + SearchHorizon(points, point, fi, face.Opposite0, oppositeFace); + } + } + + if (!litFaces.Contains(face.Opposite1)) { + var oppositeFace = faces[face.Opposite1]; + + var dist = PointFaceDistance( + point, + points[oppositeFace.Vertex0], + oppositeFace); + + if (dist <= 0.0f) { + horizon.Add(new HorizonEdge { + Face = face.Opposite1, + Edge0 = face.Vertex2, + Edge1 = face.Vertex0, + }); + } else { + SearchHorizon(points, point, fi, face.Opposite1, oppositeFace); + } + } + + if (!litFaces.Contains(face.Opposite2)) { + var oppositeFace = faces[face.Opposite2]; + + var dist = PointFaceDistance( + point, + points[oppositeFace.Vertex0], + oppositeFace); + + if (dist <= 0.0f) { + horizon.Add(new HorizonEdge { + Face = face.Opposite2, + Edge0 = face.Vertex0, + Edge1 = face.Vertex1, + }); + } else { + SearchHorizon(points, point, fi, face.Opposite2, oppositeFace); + } + } + } + + /// + /// Recursively search to find the horizon or lit set. + /// + void SearchHorizon(List points, Vector3 point, int prevFaceIndex, int faceCount, Face face) { + Assert.IsTrue(prevFaceIndex >= 0); + Assert.IsTrue(litFaces.Contains(prevFaceIndex)); + Assert.IsTrue(!litFaces.Contains(faceCount)); + Assert.IsTrue(faces[faceCount].Equals(face)); + + litFaces.Add(faceCount); + + // Use prevFaceIndex to determine what the next face to search will + // be, and what edges we need to cross to get there. It's important + // that the search proceeds in counter-clockwise order from the + // previous face. + int nextFaceIndex0; + int nextFaceIndex1; + int edge0; + int edge1; + int edge2; + + if (prevFaceIndex == face.Opposite0) { + nextFaceIndex0 = face.Opposite1; + nextFaceIndex1 = face.Opposite2; + + edge0 = face.Vertex2; + edge1 = face.Vertex0; + edge2 = face.Vertex1; + } else if (prevFaceIndex == face.Opposite1) { + nextFaceIndex0 = face.Opposite2; + nextFaceIndex1 = face.Opposite0; + + edge0 = face.Vertex0; + edge1 = face.Vertex1; + edge2 = face.Vertex2; + } else { + Assert.IsTrue(prevFaceIndex == face.Opposite2); + + nextFaceIndex0 = face.Opposite0; + nextFaceIndex1 = face.Opposite1; + + edge0 = face.Vertex1; + edge1 = face.Vertex2; + edge2 = face.Vertex0; + } + + if (!litFaces.Contains(nextFaceIndex0)) { + var oppositeFace = faces[nextFaceIndex0]; + + var dist = PointFaceDistance( + point, + points[oppositeFace.Vertex0], + oppositeFace); + + if (dist <= 0.0f) { + horizon.Add(new HorizonEdge { + Face = nextFaceIndex0, + Edge0 = edge0, + Edge1 = edge1, + }); + } else { + SearchHorizon(points, point, faceCount, nextFaceIndex0, oppositeFace); + } + } + + if (!litFaces.Contains(nextFaceIndex1)) { + var oppositeFace = faces[nextFaceIndex1]; + + var dist = PointFaceDistance( + point, + points[oppositeFace.Vertex0], + oppositeFace); + + if (dist <= 0.0f) { + horizon.Add(new HorizonEdge { + Face = nextFaceIndex1, + Edge0 = edge1, + Edge1 = edge2, + }); + } else { + SearchHorizon(points, point, faceCount, nextFaceIndex1, oppositeFace); + } + } + } + + /// + /// Remove all lit faces and construct new faces from the horizon in a + /// "cone-like" fashion. + /// + /// This is a relatively straight-forward procedure, given that the + /// horizon is handed to it in already sorted counter-clockwise. The + /// neighbors of the new faces are easy to find: they're the previous + /// and next faces to be constructed in the cone, as well as the face + /// on the other side of the horizon. We also have to update the face + /// on the other side of the horizon to reflect it's new neighbor from + /// the cone. + /// + void ConstructCone(List points, int farthestPoint) { + foreach (var fi in litFaces) { + Assert.IsTrue(faces.ContainsKey(fi)); + faces.Remove(fi); + } + + var firstNewFace = faceCount; + + // Check for coplanar faces + /*var firstNormal = new Vector3(0, 0, 0); + int sameNormals = 1; + for (int i = 0; i < horizon.Count; i++) + { + var v0 = farthestPoint; + var v1 = horizon[i].Edge0; + var v2 = horizon[i].Edge1; + var norm = Normal(points[v0], points[v1], points[v2]); + if (firstNormal.IsZero) + { + firstNormal = norm; + continue; + } + + const float tol = 0.1f; + if ((firstNormal - norm).Length < tol || (firstNormal - norm).Length > 2.0f-tol) + { + sameNormals++; + } + } + + if (sameNormals == horizon.Count) + sameNormals = sameNormals;*/ + + for (int i = 0; i < horizon.Count; i++) { + // Vertices of the new face, the farthest point as well as the + // edge on the horizon. Horizon edge is CCW, so the triangle + // should be as well. + var v0 = farthestPoint; + var v1 = horizon[i].Edge0; + var v2 = horizon[i].Edge1; + + // Opposite faces of the triangle. First, the edge on the other + // side of the horizon, then the next/prev faces on the new cone + var o0 = horizon[i].Face; + var o1 = (i == horizon.Count - 1) ? firstNewFace : firstNewFace + i + 1; + var o2 = (i == 0) ? (firstNewFace + horizon.Count - 1) : firstNewFace + i - 1; + + var fi = faceCount++; + + faces[fi] = new Face( + v0, v1, v2, + o0, o1, o2, + Normal(points[v0], points[v1], points[v2])); + + var horizonFace = faces[horizon[i].Face]; + + if (horizonFace.Vertex0 == v1) { + Assert.IsTrue(v2 == horizonFace.Vertex2); + horizonFace.Opposite1 = fi; + } else if (horizonFace.Vertex1 == v1) { + Assert.IsTrue(v2 == horizonFace.Vertex0); + horizonFace.Opposite2 = fi; + } else { + Assert.IsTrue(v1 == horizonFace.Vertex2); + Assert.IsTrue(v2 == horizonFace.Vertex1); + horizonFace.Opposite0 = fi; + } + + faces[horizon[i].Face] = horizonFace; + } + } + + /// + /// Reassign points based on the new faces added by ConstructCone(). + /// + /// Only points that were previous assigned to a removed face need to + /// be updated, so check litFaces while looping through the open set. + /// + /// There is a potential optimization here: there's no reason to loop + /// through the entire openSet here. If each face had it's own + /// openSet, we could just loop through the openSets in the removed + /// faces. That would make the loop here shorter. + /// + /// However, to do that, we would have to juggle A LOT more List's, + /// and we would need an object pool to manage them all without + /// generating a whole bunch of garbage. I don't think it's worth + /// doing that to make this loop shorter, a straight for-loop through + /// a list is pretty darn fast. Still, it might be worth trying + /// + void ReassignPoints(List points) { + for (int i = 0; i <= openSetTail; i++) { + var fp = openSet[i]; + + if (litFaces.Contains(fp.Face)) { + var assigned = false; + var point = points[fp.Point]; + + foreach (var kvp in faces) { + var fi = kvp.Key; + var face = kvp.Value; + + var dist = PointFaceDistance( + point, + points[face.Vertex0], + face); + + if (dist > EPSILON) { + assigned = true; + + fp.Face = fi; + fp.Distance = dist; + + openSet[i] = fp; + break; + } + } + + if (!assigned) { + // If point hasn't been assigned, then it's inside the + // convex hull. Swap it with openSetTail, and decrement + // openSetTail. We also have to decrement i, because + // there's now a new thing in openSet[i], so we need i + // to remain the same the next iteration of the loop. + fp.Face = INSIDE; + fp.Distance = float.NaN; + + openSet[i] = openSet[openSetTail]; + openSet[openSetTail] = fp; + + i--; + openSetTail--; + } + } + } + } + + /// + /// Final step in algorithm, export the faces of the convex hull in a + /// mesh-friendly format. + /// + /// TODO normals calculation for non-split vertices. Right now it just + /// leaves the normal array empty. + /// + void ExportMesh( + List points, + bool splitVerts, + ref List verts, + ref List tris, + ref List normals) + { + if (verts == null) { + verts = new List(); + } else { + verts.Clear(); + } + + if (tris == null) { + tris = new List(); + } else { + tris.Clear(); + } + + if (normals == null) { + normals = new List(); + } else { + normals.Clear(); + } + + foreach (var face in faces.Values) { + int vi0, vi1, vi2; + + if (splitVerts) { + vi0 = verts.Count; verts.Add(points[face.Vertex0]); + vi1 = verts.Count; verts.Add(points[face.Vertex1]); + vi2 = verts.Count; verts.Add(points[face.Vertex2]); + + normals.Add(face.Normal); + normals.Add(face.Normal); + normals.Add(face.Normal); + } else { + if (!hullVerts.TryGetValue(face.Vertex0, out vi0)) { + vi0 = verts.Count; + hullVerts[face.Vertex0] = vi0; + verts.Add(points[face.Vertex0]); + } + + if (!hullVerts.TryGetValue(face.Vertex1, out vi1)) { + vi1 = verts.Count; + hullVerts[face.Vertex1] = vi1; + verts.Add(points[face.Vertex1]); + } + + if (!hullVerts.TryGetValue(face.Vertex2, out vi2)) { + vi2 = verts.Count; + hullVerts[face.Vertex2] = vi2; + verts.Add(points[face.Vertex2]); + } + } + + tris.Add(vi0); + tris.Add(vi1); + tris.Add(vi2); + } + } + + /// + /// Signed distance from face to point (a positive number means that + /// the point is above the face) + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + float PointFaceDistance(Vector3 point, Vector3 pointOnFace, Face face) { + return Dot(face.Normal, point - pointOnFace); + } + + /// + /// Calculate normal for triangle + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + Vector3 Normal(Vector3 v0, Vector3 v1, Vector3 v2) { + return Cross(v1 - v0, v2 - v0).Normalized; + } + + /// + /// Dot product, for convenience. + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static float Dot(Vector3 a, Vector3 b) { + return a.X*b.X + a.Y*b.Y + a.Z*b.Z; + } + + /// + /// Vector3.Cross i left-handed, the algorithm is right-handed. Also, + /// i wanna test to see if using aggressive inlining makes any + /// difference here. + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static Vector3 Cross(Vector3 a, Vector3 b) { + return new Vector3( + a.Y*b.Z - a.Z*b.Y, + a.Z*b.X - a.X*b.Z, + a.X*b.Y - a.Y*b.X); + } + + /// + /// Check if two points are coincident + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + bool AreCoincident(Vector3 a, Vector3 b) { + return (a - b).Length <= EPSILON; + } + + /// + /// Check if three points are collinear + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + bool AreCollinear(Vector3 a, Vector3 b, Vector3 c) { + return Cross(c - a, c - b).Length <= EPSILON; + } + + /// + /// Check if four points are coplanar + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + bool AreCoplanar(Vector3 a, Vector3 b, Vector3 c, Vector3 d) { + var n1 = Cross(c - a, c - b); + var n2 = Cross(d - a, d - b); + + var m1 = n1.Length; + var m2 = n2.Length; + + return m1 <= EPSILON + || m2 <= EPSILON + || AreCollinear(Vector3.Zero, + (1.0f / m1) * n1, + (1.0f / m2) * n2); + } + + /// + /// Method used for debugging, verifies that the openSet is in a + /// sensible state. Conditionally compiled if DEBUG_QUICKHULL if + /// defined. + /// + void VerifyOpenSet(List points) { + for (int i = 0; i < openSet.Count; i++) { + if (i > openSetTail) { + Assert.IsTrue(openSet[i].Face == INSIDE); + } else { + Assert.IsTrue(openSet[i].Face != INSIDE); + Assert.IsTrue(openSet[i].Face != UNASSIGNED); + + Assert.IsTrue(PointFaceDistance( + points[openSet[i].Point], + points[faces[openSet[i].Face].Vertex0], + faces[openSet[i].Face]) > 0.0f); + } + } + } + + /// + /// Method used for debugging, verifies that the horizon is in a + /// sensible state. Conditionally compiled if DEBUG_QUICKHULL if + /// defined. + /// + void VerifyHorizon() { + for (int i = 0; i < horizon.Count; i++) { + var prev = i == 0 ? horizon.Count - 1 : i - 1; + + Assert.IsTrue(horizon[prev].Edge1 == horizon[i].Edge0); + Assert.IsTrue(HasEdge(faces[horizon[i].Face], horizon[i].Edge1, horizon[i].Edge0)); + } + } + + /// + /// Method used for debugging, verifies that the faces array is in a + /// sensible state. Conditionally compiled if DEBUG_QUICKHULL if + /// defined. + /// + void VerifyFaces(List points) { + foreach (var kvp in faces) { + var fi = kvp.Key; + var face = kvp.Value; + + Assert.IsTrue(faces.ContainsKey(face.Opposite0)); + Assert.IsTrue(faces.ContainsKey(face.Opposite1)); + Assert.IsTrue(faces.ContainsKey(face.Opposite2)); + + Assert.IsTrue(face.Opposite0 != fi); + Assert.IsTrue(face.Opposite1 != fi); + Assert.IsTrue(face.Opposite2 != fi); + + Assert.IsTrue(face.Vertex0 != face.Vertex1); + Assert.IsTrue(face.Vertex0 != face.Vertex2); + Assert.IsTrue(face.Vertex1 != face.Vertex2); + + Assert.IsTrue(HasEdge(faces[face.Opposite0], face.Vertex2, face.Vertex1)); + Assert.IsTrue(HasEdge(faces[face.Opposite1], face.Vertex0, face.Vertex2)); + Assert.IsTrue(HasEdge(faces[face.Opposite2], face.Vertex1, face.Vertex0)); + + Assert.IsTrue((face.Normal - Normal( + points[face.Vertex0], + points[face.Vertex1], + points[face.Vertex2])).Length < EPSILON); + } + } + + /// + /// Method used for debugging, verifies that the final mesh is + /// actually a convex hull of all the points. Conditionally compiled + /// if DEBUG_QUICKHULL if defined. + /// + void VerifyMesh(List points, ref List verts, ref List tris) { + Assert.IsTrue(tris.Count % 3 == 0); + + for (int i = 0; i < points.Count; i++) { + for (int j = 0; j < tris.Count; j+=3) { + var t0 = verts[tris[j]]; + var t1 = verts[tris[j + 1]]; + var t2 = verts[tris[j + 2]]; + + var dot = Dot(points[i] - t0, Vector3.Cross(t1 - t0, t2 - t0)); + //Assert.IsTrue(dot <= EPSILON, $"not convex hull: {dot} > {EPSILON}"); + if (!(dot <= EPSILON)) + Cabrito.Console.PrintError($"not convex hull: {dot} > {EPSILON}"); + } + + } + } + + /// + /// Does face f have a face with vertexes e0 and e1? Used only for + /// debugging. + /// + bool HasEdge(Face f, int e0, int e1) { + return (f.Vertex0 == e0 && f.Vertex1 == e1) + || (f.Vertex1 == e0 && f.Vertex2 == e1) + || (f.Vertex2 == e0 && f.Vertex0 == e1); + } + } + +} \ No newline at end of file