public static IEnumerable <Triangle> SampleMesh(IEnumerable <Triangle> triangles, List <Vector2> samplePoints)
{
var points3d = new List <Vector3>();
for (int i = 0; i < samplePoints.Count; i++)
{
var projected = triangles.Where(t => t.ContainsXZ(samplePoints[i])).Select(t => Math3d.ProjectFromGroundToPlane(samplePoints[i], t.Plane));
if (!projected.Any())
{
samplePoints.RemoveAt(i);
i--;
}
else
{
points3d.Add(projected.OrderByDescending(p => p.y).First());
}
}
var distinctPoints = new List <Vector3>();
foreach (var point in points3d.OrderByDescending(p => p.y))
{
if (!distinctPoints.Any(p => Mathf.Pow(p.x - point.x, 2.0f) + Mathf.Pow(p.z - point.z, 2.0f) < Mathf.Pow(minDistance, 2.0f)))
{
distinctPoints.Add(point);
}
}
var indices = PointMeshCreator.Triangulate(distinctPoints.Select(p => new Vector2(p.x, p.z)).ToList()).ToArray();
for (int i = 0; i < indices.Length; i += 3)
{
var triangle = new Triangle(distinctPoints[indices[i]], distinctPoints[indices[i + 1]], distinctPoints[indices[i + 2]]);
var center = triangle.Center;
if (!triangles.Any(t => t.ContainsXZ(center)) || Mathf.Abs(Vector3.Angle(triangle.Normal, Vector3.up) - 90.0f) < 5.0f)
{
continue;
}
yield return(triangle);
}
}
public void CreateMesh()
{
this.CheckForPlanes();
Timekeeping.Reset();
var result = new List <Triangle>();
var remainingPointIndices = Enumerable.Range(0, this.PointCloud.Points.Length).ToList();
foreach (var plane in this.Planes.Take(8))
{
var onPlane = this.PointCloud.Points.Where(p => Mathf.Abs(plane.GetDistanceToPoint(p)) < HoughPlaneFinder.MaxDistance);
Timekeeping.CompleteTask("Select points");
var planeCoordinates = new PlaneCoordinates(plane);
var planePoints = onPlane.Select(p => planeCoordinates.ToPlane(p)).ToList();
var triangles = PointMeshCreator.Triangulate(planePoints).ToArray();
Timekeeping.CompleteTask("Triangulate");
const float maxDistance = 1.5f;
var edges = new List <Tuple <int, int> >();
for (int i = 0; i < triangles.Count(); i += 3)
{
float dst1 = (planePoints[triangles[i]] - planePoints[triangles[i + 1]]).magnitude;
float dst2 = (planePoints[triangles[i + 1]] - planePoints[triangles[i + 2]]).magnitude;
float dst3 = (planePoints[triangles[i + 2]] - planePoints[triangles[i]]).magnitude;
if (dst1 < maxDistance && dst2 < maxDistance && dst2 < maxDistance)
{
edges.Add(new Tuple <int, int>(triangles[i], triangles[i + 1]));
edges.Add(new Tuple <int, int>(triangles[i + 1], triangles[i + 2]));
edges.Add(new Tuple <int, int>(triangles[i + 2], triangles[i]));
}
}
Timekeeping.CompleteTask("Discard bad triangles");
var neighbours = new Dictionary <int, HashSet <int> >();
foreach (var edge in edges)
{
if (!neighbours.ContainsKey(edge.Value1))
{
neighbours[edge.Value1] = new HashSet <int>();
}
neighbours[edge.Value1].Add(edge.Value2);
if (!neighbours.ContainsKey(edge.Value2))
{
neighbours[edge.Value2] = new HashSet <int>();
}
neighbours[edge.Value2].Add(edge.Value1);
}
var outsideEdges = new List <Tuple <int, int> >();
foreach (var edge in edges)
{
var rayBase = planePoints[edge.Value1];
var rayDirection = planePoints[edge.Value2] - planePoints[edge.Value1];
var orthogonal = new Vector2(-rayDirection.y, rayDirection.x);
var candidates = neighbours[edge.Value1].Where(i => neighbours[edge.Value2].Contains(i)).Select(i => planePoints[i]);
bool left = false;
bool right = false;
foreach (var point in candidates)
{
if (Vector2.Angle(point - rayBase, orthogonal) < 90.0f)
{
left = true;
}
else
{
right = true;
}
if (left && right)
{
break;
}
}
if (!left || !right)
{
outsideEdges.Add(edge);
}
}
Timekeeping.CompleteTask("Find outside edges");
var nextOutsideEdge = new Dictionary <int, HashSet <Tuple <int, int> > >();
foreach (var edge in outsideEdges)
{
if (!nextOutsideEdge.ContainsKey(edge.Value1))
{
nextOutsideEdge[edge.Value1] = new HashSet <Tuple <int, int> >();
}
nextOutsideEdge[edge.Value1].Add(edge);
if (!nextOutsideEdge.ContainsKey(edge.Value2))
{
nextOutsideEdge[edge.Value2] = new HashSet <Tuple <int, int> >();
}
nextOutsideEdge[edge.Value2].Add(edge);
}
var visitedEdges = new HashSet <Tuple <int, int> >();
var resultPoints = new List <Vector3>();
foreach (var edge in outsideEdges)
{
if (visitedEdges.Contains(edge))
{
continue;
}
var polygonIndices = new List <int>();
var currentPoint = edge.Value1;
polygonIndices.Add(currentPoint);
visitedEdges.Add(edge);
while (true)
{
var nextEdge = nextOutsideEdge[currentPoint].FirstOrDefault(e => !visitedEdges.Contains(e));
if (nextEdge == null)
{
break;
}
visitedEdges.Add(nextEdge);
if (nextEdge.Value1 == currentPoint)
{
currentPoint = nextEdge.Value2;
}
else
{
currentPoint = nextEdge.Value1;
}
polygonIndices.Add(currentPoint);
}
if (polygonIndices.Count > 10)
{
IEnumerable <Vector2> polygon = this.simplifyPolygon(polygonIndices.Select(i => planePoints[i]), 2.0f, 20.0f, 160.0f, 5);
polygon = this.snapPoints(polygon, planeCoordinates, resultPoints, 2f);
resultPoints.AddRange(polygon.Select(p => planeCoordinates.ToWorld(p)));
var meshTriangles = this.polygonToTriangle(polygon, planeCoordinates);
var score = meshTriangles.Sum(triangle => triangle.GetPointCount(this.PointCloud, remainingPointIndices));
Debug.Log(score);
if (score > 60)
{
result.AddRange(meshTriangles);
remainingPointIndices = remainingPointIndices.Where(i => !meshTriangles.Any(triangle => triangle.Contains(this.PointCloud.Points[i]))).ToList();
}
}
}
Timekeeping.CompleteTask("Find polygons");
}
this.Triangles = result;
}