1316 lines
31 KiB
C#
1316 lines
31 KiB
C#
#if false
|
|
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 Float3 tail
|
|
{
|
|
get
|
|
{
|
|
return edge.v2;
|
|
}
|
|
set
|
|
{
|
|
edge.v2 = value;
|
|
opposite.edge.v1 = value;
|
|
}
|
|
}
|
|
}
|
|
|
|
public struct Edge
|
|
{
|
|
public Float3 v1, v2;
|
|
|
|
public Edge(Float3 v1, Float3 v2)
|
|
{
|
|
this.v1 = v1;
|
|
this.v2 = v2;
|
|
}
|
|
|
|
public static Edge[] GetEdges(Float3 v1, Float3 v2, Float3 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 Float3 v1, v2, v3;
|
|
public List<HalfEdge> halfEdges;
|
|
public bool visited;
|
|
|
|
public Face(Float3 v1, Float3 v2, Float3 v3)
|
|
{
|
|
this.v1 = v1;
|
|
this.v2 = v2;
|
|
this.v3 = v3;
|
|
halfEdges = new List<HalfEdge>(3);
|
|
}
|
|
|
|
public Edge[] GetEdges()
|
|
{
|
|
return new[]
|
|
{
|
|
new Edge(v1, v2),
|
|
new Edge(v2, v3),
|
|
new Edge(v3, v1),
|
|
};
|
|
}
|
|
|
|
public float DistanceToPoint(Float3 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;
|
|
Float3 areaNorm = Float3.Zero;
|
|
int iters = 0;
|
|
do
|
|
{
|
|
if (iters++ > 1000)
|
|
throw new Exception("merge infinite loop");
|
|
areaNorm += Float3.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 Float3 v1, v2, v3, v4;
|
|
|
|
public Tetrahedron(Float3 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<Float3> points, Vector3 planeNormal)
|
|
{
|
|
Vector3 center = Float3.Zero;
|
|
foreach (var vert in points)
|
|
{
|
|
center += vert;
|
|
}
|
|
|
|
if (points.Count > 0)
|
|
center /= points.Count;
|
|
|
|
points.Sort((v1, v2) =>
|
|
{
|
|
var dot = Float3.Dot(planeNormal, Float3.Cross(v1 - center, v2 - center));
|
|
if (dot > 0)
|
|
return 1;
|
|
else
|
|
return -1;
|
|
});
|
|
}
|
|
|
|
float PointDistanceFromPlane(Float3 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(Float3[] points)
|
|
{
|
|
if (false)
|
|
{
|
|
// TODO: more optimal to find first set of points which are not coplanar?
|
|
|
|
// find the longest edge
|
|
Vector3 v1 = Float3.Zero;
|
|
Vector3 v2 = Float3.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 = Float3.Zero;
|
|
float furthestDist = 0f;
|
|
foreach (var point in points)
|
|
{
|
|
//if (vert == a1 || vert == a2)
|
|
// continue;
|
|
|
|
var edgeDir = (v2 - v1).Normalized;
|
|
var closest = v1 + edgeDir * Float3.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 = Float3.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 = Float3.Zero, v2 = Float3.Zero, v3 = Float3.Zero, v4 = Float3.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<Tuple<Face, Vector3>> 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, Vector3>(face, point));
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
public List<Float3> QuickHull2(Float3[] points)
|
|
{
|
|
Assert.IsTrue(points.Length >= 4, "points.Length >= 4");
|
|
|
|
var tetrahedron = CreateInitialSimplex(points);
|
|
|
|
List<Tuple<Face, Vector3>> outsideSet = new List<Tuple<Face, Vector3>>();
|
|
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<HalfEdge>(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<Float3>();
|
|
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<HalfEdge> horizonEdges = new List<HalfEdge>();
|
|
|
|
List<Face> hullFaces = new List<Face>();
|
|
hullFaces.AddRange(tetrahedron);
|
|
|
|
// stop when none of the faces have any visible outside points
|
|
int iterCount = 0;
|
|
while (outsideSet.Count > 0)
|
|
{
|
|
iterCount++;
|
|
Tuple<Face, Vector3> 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<Face>();
|
|
var unclaimedPoints = new List<Float3>();
|
|
|
|
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<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<Float3> hullPoints = new List<Float3>(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<Float3> list, Vector3 point)
|
|
{
|
|
foreach (var p in list)
|
|
{
|
|
if ((point - p).Length < epsilon)
|
|
return;
|
|
}
|
|
list.Add(point);
|
|
}
|
|
|
|
bool AreCoincident(Float3 a, Vector3 b)
|
|
{
|
|
return (a - b).Length <= epsilon;
|
|
}
|
|
|
|
bool AreCollinear(Float3 a, Vector3 b, Vector3 c)
|
|
{
|
|
return Float3.Cross(c - a, c - b).Length <= epsilon;
|
|
}
|
|
|
|
bool AreCoplanar(Float3 a, Vector3 b, Vector3 c, Vector3 d)
|
|
{
|
|
var n1 = Float3.Cross(c - a, c - b);
|
|
var n2 = Float3.Cross(d - a, d - b);
|
|
|
|
var m1 = n1.Length;
|
|
var m2 = n2.Length;
|
|
|
|
return m1 > epsilon
|
|
&& m2 > epsilon
|
|
&& AreCollinear(Float3.Zero,
|
|
(1.0f / m1) * n1,
|
|
(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 = Float3.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 Float3(float.NaN), new Float3(float.NaN), new Float3(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<Float3> uniqPoints = new List<Float3>();
|
|
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 =
|
|
Float3.Cross(-(new Plane(mergedFace.v1, mergedFace.v2, mergedFace.v3).Normal), edgeVector);
|
|
|
|
HalfEdge testHe = qhe.next;
|
|
do
|
|
{
|
|
Vector3 testVertex = testHe.tail;
|
|
float dist = Float3.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(Float3 point, Face face, List<Float3> unclaimedPoints,
|
|
List<Tuple<Face, Vector3>> outsideSet,
|
|
List<HalfEdge> horizonEdges, List<Face> hullFaces)
|
|
{
|
|
horizonEdges.Clear();
|
|
|
|
CalculateHorizon(face, point, unclaimedPoints, outsideSet, horizonEdges, face.halfEdges[0]);
|
|
|
|
// create new faces
|
|
if (horizonEdges.Count > 0)
|
|
{
|
|
List<Face> newFaces = new List<Face>();
|
|
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<Float3>();
|
|
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<HalfEdge>();
|
|
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<Face> discardedFaces = new List<Face>();
|
|
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(Float3 point, Face face, List<Float3> 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;
|
|
|
|
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<Float3> unclaimedPoints,
|
|
List<Tuple<Face, Vector3>> outsideSet,
|
|
List<HalfEdge> 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);
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif |