pre-give up quickhull

Source/Game/Q3MapImporter.cs

@@ -105,6 +105,25 @@ namespace Game
 
 			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
@@ -347,6 +366,19 @@ namespace Game
 				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
@@ -562,7 +594,7 @@ namespace Game
 					foreach (var halfEdge in face.halfEdges)
 					{
 						Assert.IsTrue(halfEdge.opposite.opposite == halfEdge,
-							"2 halfEdge.opposite.opposite == halfEdge");
+							"2 halfEdge.opposite.opposite == halfEdge (degenerate face?)");
 						Assert.IsTrue(hullFaces.Contains(halfEdge.opposite.face),
 							"2 hullFaces.Contains(halfEdge.opposite.face)");
 					}
@@ -583,6 +615,23 @@ namespace Game
 					Assert.Fail("outsideSet overflow");
 			}
 
+			// merge faces with similar normals
+			List<Face> discardedFaces = new List<Face>();
+			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<Vector3> hullPoints = new List<Vector3>(hullFaces.Count * 3);
 			foreach (var face in hullFaces)
 			{
@@ -629,6 +678,240 @@ namespace Game
 				       (1.0f / m2) * n2);
 		}
 
+		private bool PostAdjacentMerge(Face face, List<Face> discardedFaces, List<Face> 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<Face> hullFaces)
+		{
+			asdf++;
+			if (asdf == 22)
+				asdf = asdf;
+			Face face1 = he.face;
+			Face face2 = he.opposite.face;
+
+			// construct the merged face
+			List<HalfEdge> edges = new List<HalfEdge>();
+			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<Vector3> uniqPoints = new List<Vector3>();
+			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<Vector3> unclaimedPoints,
 			List<Tuple<Face, Vector3>> outsideSet,
 			List<HalfEdge> horizonEdges, List<Face> hullFaces)
@@ -715,128 +998,32 @@ namespace Game
 				}
 
 				// merge NONCONVEX_WRT_LARGER_FACE
-				const float tolerance = -1f;
+
 				//List<Face> discardedFaces = new List<Face>();
 				if (false)
-				foreach (var newFace in newFaces)
 				{
-					HalfEdge edge = newFace.halfEdges[0];
-
-					bool convex = true;
-					do
+					foreach (var newFace in newFaces)
 					{
-						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);
-
-						// ?
-						float faceArea, oppositeArea;
-
+						// if face visible?
+						while (AdjacentMerge(point, newFace, unclaimedPoints, outsideSet, true))
 						{
-							HalfEdge areaEdgeStart = edge;
-							HalfEdge areaEdge = areaEdgeStart.previous;
-							Vector3 areaNorm = Vector3.Zero;
-							do
-							{
-								areaNorm += Vector3.Cross(areaEdge.edge.v1 - areaEdgeStart.edge.v1,
-									areaEdge.next.edge.v1 - areaEdgeStart.edge.v1);
-								areaEdge = areaEdge.previous;
-
-							} while (areaEdge != areaEdgeStart);
-
-							faceArea = areaNorm.Length;
-						}
-						{
-							HalfEdge areaEdgeStart = edge.opposite;
-							HalfEdge areaEdge = areaEdgeStart.previous;
-							Vector3 areaNorm = Vector3.Zero;
-							do
-							{
-								areaNorm += Vector3.Cross(areaEdge.edge.v1 - areaEdgeStart.edge.v1,
-									areaEdge.next.edge.v1 - areaEdgeStart.edge.v1);
-								areaEdge = areaEdge.previous;
-
-							} while (areaEdge != areaEdgeStart);
-
-							oppositeArea = areaNorm.Length;
+							// merge until failure
 						}
 
-						if (faceArea > oppositeArea)
+						hullFaces.Add(newFace);
+					}
+
+					foreach (var newFace in newFaces)
+					{
+						// if face non-convex?
+							// mark face as visible?
+						while (AdjacentMerge(point, newFace, unclaimedPoints, outsideSet, false))
 						{
-							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;
+							// merge until failure
 						}
 
-						if (merge)
-						{
-							List<Face> discardedFaces = new List<Face>();
-							{
-								discardedFaces.Add(oppositeFace);
-
-								HalfEdge prev = edge.previous;
-								HalfEdge next = edge.next;
-								HalfEdge oppositePrev = edge.opposite.previous;
-								HalfEdge oppositeNext = edge.opposite.next;
-
-								while (prev.opposite.face == oppositeFace)
-								{
-									prev = prev.previous;
-									oppositeNext = oppositeNext.next;
-								}
-
-								while (next.opposite.face == oppositeFace)
-								{
-									oppositePrev = oppositePrev.previous;
-									next = next.next;
-								}
-
-								for (HalfEdge e = oppositeNext; e != oppositePrev.next; e = e.next)
-									e.face = newFace;
-
-								if (edge == face.halfEdges[0])
-									face.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);
-							}
-
-							foreach (var dface in discardedFaces)
-							{
-								for (int i=0; i<outsideSet.Count; i++)
-								{
-									if (outsideSet[i].Item1 == dface)
-									{
-										float distance = face.DistanceToPoint(point);
-										if (distance > 0)
-											outsideSet[i] = new Tuple<Face, Vector3>(newFace, outsideSet[i].Item2);
-										else
-											unclaimedPoints.Add(outsideSet[i].Item2);
-									}
-								}
-							}
-
-						}
-					} while (edge != newFace.halfEdges[0]);
-
-
-					hullFaces.Add(newFace);
+						hullFaces.Add(newFace);
+					}
 				}
 				else
 					hullFaces.AddRange(newFaces);
@@ -885,6 +1072,128 @@ namespace Game
 			}
 		}
 
+		private bool AdjacentMerge(Vector3 point, Face face, List<Vector3> unclaimedPoints, List<Tuple<Face, Vector3>> 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<Face> discardedFaces = new List<Face>();
+					// mergeAdjacentFace
+					if (!MergeAdjacentFaces(edge, face, face, discardedFaces))
+					{
+						throw new Exception("merge failure");
+					}
+
+					foreach (var dface in discardedFaces)
+					{
+						for (int i=0; i<outsideSet.Count; i++)
+						{
+							if (outsideSet[i].Item1 == dface)
+							{
+								float distance = face.DistanceToPoint(point);
+								if (distance > 0)
+									outsideSet[i] = new Tuple<Face, Vector3>(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<Face> 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;
@@ -899,7 +1208,33 @@ namespace Game
 					face.halfEdges[0] = edge;
 				}
 
-				/*if (oppFace.numVertices() == 3)*/
+				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
@@ -907,15 +1242,17 @@ namespace Game
 
 					//oppFace.mark = DELETED;
 					discardedFace = oppFace;
-				} /*else {
+				}
+				else
+				{
 					hedgeOpp = edge.opposite.next;
 
 					if (oppFace.halfEdges[0] == hedgeOpp.previous) {
 						oppFace.halfEdges[0] = hedgeOpp;
 					}
-					hedgeOpp.previous = hedgeOpp.previous.prev;
+					hedgeOpp.previous = hedgeOpp.previous.previous;
 					hedgeOpp.previous.next = hedgeOpp;
-				}*/
+				}
 				edge.previous = prev.previous;
 				edge.previous.next = edge;