private void UpdateBoundingShape()
{
// ----- Update BoundingShape
// Compute a transformed shape with a box from the minimal AABB.
var boxObject = (GeometricObject)BoundingShape.Child;
var boxShape = (BoxShape)boxObject.Shape;
var vertexArray = (Vertices != null) ? Vertices.Array : null;
if (vertexArray != null && vertexArray.Length > 0)
{
Aabb aabb = new Aabb(vertexArray[0], vertexArray[0]);
var numberOfVertices = vertexArray.Length;
for (int i = 1; i < numberOfVertices; i++)
aabb.Grow(vertexArray[i]);
// Update existing shape.
boxObject.Pose = new Pose(aabb.Center);
boxShape.Extent = aabb.Extent;
}
else
{
// Set an "empty" shape.
boxObject.Pose = Pose.Identity;
boxShape.Extent = Vector3F.Zero;
}
}
public static void CreateMesh(GraphicsDevice graphicsDevice, Path3F path, float defaultWidth,
int maxNumberOfIterations, float tolerance,
out Submesh submesh, out Aabb aabb, out float roadLength)
{
if (graphicsDevice == null)
{
throw new ArgumentNullException("graphicsDevice");
}
if (path == null)
{
throw new ArgumentNullException("path");
}
// Compute list of line segments. (2 points per line segment!)
var flattenedPoints = new List <Vector3F>();
path.Flatten(flattenedPoints, maxNumberOfIterations, tolerance);
// Abort if path is empty.
int numberOfLineSegments = flattenedPoints.Count / 2;
if (numberOfLineSegments <= 0)
{
submesh = null;
aabb = new Aabb();
roadLength = 0;
return;
}
// Compute accumulated lengths. (One entry for each entry in flattenedPoints.)
float[] accumulatedLengths = new float[flattenedPoints.Count];
accumulatedLengths[0] = 0;
for (int i = 1; i < flattenedPoints.Count; i += 2)
{
Vector3F previous = flattenedPoints[i - 1];
Vector3F current = flattenedPoints[i];
float length = (current - previous).Length;
accumulatedLengths[i] = accumulatedLengths[i - 1] + length;
if (i + 1 < flattenedPoints.Count)
{
accumulatedLengths[i + 1] = accumulatedLengths[i];
}
}
// Total road length.
roadLength = accumulatedLengths[accumulatedLengths.Length - 1];
// Create a mapping between accumulatedLength and the path key widths.
// (accumulatedLength --> TerrainRoadPathKey.Width)
var widthKeys = new List <Pair <float, float> >();
{
int index = 0;
foreach (var key in path)
{
Vector3F position = key.Point;
var roadKey = key as TerrainRoadPathKey;
float width = (roadKey != null) ? roadKey.Width : defaultWidth;
for (; index < flattenedPoints.Count; index++)
{
if (Vector3F.AreNumericallyEqual(position, flattenedPoints[index]))
{
widthKeys.Add(new Pair <float, float>(accumulatedLengths[index], width));
break;
}
bool isLastLineSegment = (index + 2 == flattenedPoints.Count);
if (!isLastLineSegment)
{
index++;
}
}
index++;
}
}
// Create a list of interpolated road widths. (One entry for each entry in flattenedPoints.)
var widths = new float[flattenedPoints.Count];
int previousKeyIndex = 0;
var previousKey = widthKeys[0];
var nextKey = widthKeys[1];
widths[0] = widthKeys[0].Second;
for (int i = 1; i < flattenedPoints.Count; i += 2)
{
if (accumulatedLengths[i] > nextKey.First)
{
previousKey = nextKey;
previousKeyIndex++;
nextKey = widthKeys[previousKeyIndex + 1];
}
float p = (accumulatedLengths[i] - previousKey.First) / (nextKey.First - previousKey.First);
widths[i] = InterpolationHelper.Lerp(previousKey.Second, nextKey.Second, p);
if (i + 1 < flattenedPoints.Count)
{
widths[i + 1] = widths[i];
}
}
// Compute vertices and indices.
var vertices = new List <TerrainLayerVertex>(numberOfLineSegments * 2 + 2);
var indices = new List <int>(numberOfLineSegments * 6); // Two triangles per line segment.
Vector3F lastOrthonormal = Vector3F.UnitX;
aabb = new Aabb(flattenedPoints[0], flattenedPoints[0]);
bool isClosed = Vector3F.AreNumericallyEqual(flattenedPoints[0], flattenedPoints[flattenedPoints.Count - 1]);
for (int i = 0; i < flattenedPoints.Count; i++)
{
Vector3F start = flattenedPoints[i];
Vector3F previous;
bool isFirstPoint = (i == 0);
if (!isFirstPoint)
{
previous = flattenedPoints[i - 1];
}
else if (isClosed && path.SmoothEnds)
{
previous = flattenedPoints[flattenedPoints.Count - 2];
}
else
{
previous = start;
}
Vector3F next;
bool isLastPoint = (i + 1 == flattenedPoints.Count);
if (!isLastPoint)
{
next = flattenedPoints[i + 1];
}
else if (isClosed && path.SmoothEnds)
{
next = flattenedPoints[1];
}
else
{
next = start;
}
Vector3F tangent = next - previous;
Vector3F orthonormal = new Vector3F(tangent.Z, 0, -tangent.X);
if (!orthonormal.TryNormalize())
{
orthonormal = lastOrthonormal;
}
// Add indices to add two triangles between the current and the next vertices.
if (!isLastPoint)
{
int baseIndex = vertices.Count;
// 2 3
// x----x
// |\ | ^
// | \ | | road
// | \ | | direction
// | \| |
// x----x
// 0 1
indices.Add(baseIndex);
indices.Add(baseIndex + 1);
indices.Add(baseIndex + 2);
indices.Add(baseIndex + 1);
indices.Add(baseIndex + 3);
indices.Add(baseIndex + 2);
}
// Add two vertices.
Vector3F leftVertex = start - orthonormal * (widths[i] / 2);
Vector3F rightVertex = start + orthonormal * (widths[i] / 2);
vertices.Add(new TerrainLayerVertex(new Vector2(leftVertex.X, leftVertex.Z), new Vector2(0, accumulatedLengths[i])));
vertices.Add(new TerrainLayerVertex(new Vector2(rightVertex.X, rightVertex.Z), new Vector2(1, accumulatedLengths[i])));
// Grow AABB
aabb.Grow(leftVertex);
aabb.Grow(rightVertex);
lastOrthonormal = orthonormal;
// The flattened points list contains 2 points per line segment, which means that there
// are duplicate intermediate points, which we skip.
bool isLastLineSegment = (i + 2 == flattenedPoints.Count);
if (!isLastLineSegment)
{
i++;
}
}
Debug.Assert(vertices.Count == (numberOfLineSegments * 2 + 2));
Debug.Assert(indices.Count == (numberOfLineSegments * 6));
// The road is projected onto the terrain, therefore the computed y limits are not correct.
// (unless the terrain was clamped to the road).
aabb.Minimum.Y = 0;
aabb.Maximum.Y = 0;
// Convert to submesh.
submesh = new Submesh
{
PrimitiveCount = indices.Count / 3,
PrimitiveType = PrimitiveType.TriangleList,
VertexCount = vertices.Count,
VertexBuffer = new VertexBuffer(graphicsDevice, TerrainLayerVertex.VertexDeclaration, vertices.Count, BufferUsage.WriteOnly),
IndexBuffer = new IndexBuffer(graphicsDevice, IndexElementSize.ThirtyTwoBits, indices.Count, BufferUsage.WriteOnly)
};
submesh.VertexBuffer.SetData(vertices.ToArray());
submesh.IndexBuffer.SetData(indices.ToArray());
}
public static void ClampTerrainToRoad(HeightField terrain, Path3F road,
float defaultWidth, float defaultSideFalloff,
int maxNumberOfIterations, float tolerance)
{
if (terrain == null)
{
throw new ArgumentNullException("terrain");
}
if (road == null)
{
throw new ArgumentNullException("road");
}
// Compute list of line segments. (2 points per line segment!)
var flattenedPoints = new List <Vector3F>();
road.Flatten(flattenedPoints, maxNumberOfIterations, tolerance);
// Abort if path is empty.
int numberOfLineSegments = flattenedPoints.Count / 2;
if (numberOfLineSegments <= 0)
{
return;
}
// Compute accumulated lengths. (One entry for each entry in flattenedPoints.)
float[] accumulatedLengths = new float[flattenedPoints.Count];
accumulatedLengths[0] = 0;
for (int i = 1; i < flattenedPoints.Count; i += 2)
{
Vector3F previous = flattenedPoints[i - 1];
Vector3F current = flattenedPoints[i];
float length = (current - previous).Length;
accumulatedLengths[i] = accumulatedLengths[i - 1] + length;
if (i + 1 < flattenedPoints.Count)
{
accumulatedLengths[i + 1] = accumulatedLengths[i];
}
}
// Create a mapping between accumulatedLength and the path keys.
// (accumulatedLength --> key)
var pathLengthsAndKeys = new List <Pair <float, TerrainRoadPathKey> >();
{
int index = 0;
foreach (var key in road)
{
Vector3F position = key.Point;
var roadKey = key as TerrainRoadPathKey;
if (roadKey == null)
{
roadKey = new TerrainRoadPathKey
{
Point = key.Point,
Width = defaultWidth,
SideFalloff = defaultSideFalloff,
};
}
for (; index < flattenedPoints.Count; index++)
{
if (Vector3F.AreNumericallyEqual(position, flattenedPoints[index]))
{
pathLengthsAndKeys.Add(new Pair <float, TerrainRoadPathKey>(accumulatedLengths[index], roadKey));
break;
}
bool isLastLineSegment = (index + 2 == flattenedPoints.Count);
if (!isLastLineSegment)
{
index++;
}
}
index++;
}
}
// Create a list of interpolated road widths and side falloffs. (One entry for each entry in flattenedPoints.)
var halfWidths = new float[flattenedPoints.Count];
var sideFalloffs = new float[flattenedPoints.Count];
int previousKeyIndex = 0;
var previousKey = pathLengthsAndKeys[0];
var nextKey = pathLengthsAndKeys[1];
halfWidths[0] = 0.5f * pathLengthsAndKeys[0].Second.Width;
sideFalloffs[0] = pathLengthsAndKeys[0].Second.SideFalloff;
for (int i = 1; i < flattenedPoints.Count; i += 2)
{
if (accumulatedLengths[i] > nextKey.First)
{
previousKey = nextKey;
previousKeyIndex++;
nextKey = pathLengthsAndKeys[previousKeyIndex + 1];
}
float p = (accumulatedLengths[i] - previousKey.First) / (nextKey.First - previousKey.First);
halfWidths[i] = 0.5f * InterpolationHelper.Lerp(previousKey.Second.Width, nextKey.Second.Width, p);
sideFalloffs[i] = InterpolationHelper.Lerp(previousKey.Second.SideFalloff, nextKey.Second.SideFalloff, p);
if (i + 1 < flattenedPoints.Count)
{
halfWidths[i + 1] = halfWidths[i];
sideFalloffs[i + 1] = sideFalloffs[i];
}
}
// Get AABB of road with the side falloff.
Aabb aabbWithSideFalloffs;
{
Vector3F p = flattenedPoints[0];
float r = halfWidths[0] + sideFalloffs[0];
aabbWithSideFalloffs = new Aabb(new Vector3F(p.X - r, 0, p.Z - r),
new Vector3F(p.X + r, 0, p.Z + r));
for (int i = 1; i < flattenedPoints.Count; i += 2)
{
p = flattenedPoints[i];
r = halfWidths[i] + sideFalloffs[i];
aabbWithSideFalloffs.Grow(new Vector3F(p.X - r, 0, p.Z - r));
aabbWithSideFalloffs.Grow(new Vector3F(p.X + r, 0, p.Z + r));
}
}
// Terrain properties.
int numberOfSamplesX = terrain.NumberOfSamplesX;
int numberOfSamplesZ = terrain.NumberOfSamplesZ;
int numberOfCellsX = numberOfSamplesX - 1;
int numberOfCellsZ = numberOfSamplesZ - 1;
float widthX = terrain.WidthX;
float cellSizeX = widthX / numberOfCellsX;
float widthZ = terrain.WidthZ;
float cellSizeZ = widthZ / numberOfCellsZ;
float cellSizeDiagonal = (float)Math.Sqrt(cellSizeX * cellSizeX + cellSizeZ * cellSizeZ);
bool isClosed = Vector3F.AreNumericallyEqual(flattenedPoints[0], flattenedPoints[flattenedPoints.Count - 1]);
{
// Get the line segments which of the road border.
List <Vector4F> segments = new List <Vector4F>(); // 2 points per segment.
Vector3F lastOrthonormal = Vector3F.Right;
Vector4F previousV1 = Vector4F.Zero;
Vector4F previousV2 = Vector4F.Zero;
for (int i = 0; i < flattenedPoints.Count; i++)
{
Vector3F start = flattenedPoints[i];
Vector3F previous;
bool isFirstPoint = (i == 0);
if (!isFirstPoint)
{
previous = flattenedPoints[i - 1];
}
else if (isClosed && road.SmoothEnds)
{
previous = flattenedPoints[flattenedPoints.Count - 2];
}
else
{
previous = start;
}
Vector3F next;
bool isLastPoint = (i + 1 == flattenedPoints.Count);
if (!isLastPoint)
{
next = flattenedPoints[i + 1];
}
else if (isClosed && road.SmoothEnds)
{
next = flattenedPoints[1];
}
else
{
next = start;
}
Vector3F tangent = next - previous;
Vector3F orthonormal = new Vector3F(tangent.Z, 0, -tangent.X);
if (!orthonormal.TryNormalize())
{
orthonormal = lastOrthonormal;
}
// Add 2 vertices two segments for the road side border.
//
// pV1 pV2 (previous vertices)
// x x
// | |
// x x
// v1 v2 (current vertices)
//
// We store the side falloff with the vertex:
// Vectors are 4D. Height is y. Side falloff is w.
Vector4F v1 = new Vector4F(start - orthonormal * (halfWidths[i] + 0), sideFalloffs[i]);
Vector4F v2 = new Vector4F(start + orthonormal * (halfWidths[i] + 0), sideFalloffs[i]);
if (i > 0)
{
segments.Add(previousV1);
segments.Add(v1);
segments.Add(previousV2);
segments.Add(v2);
if (isLastPoint && !isClosed)
{
// A segment for the end of the road.
segments.Add(v1);
segments.Add(v2);
}
}
else
{
if (!isClosed)
{
// A segment for the start of the road.
segments.Add(v1);
segments.Add(v2);
}
}
previousV1 = v1;
previousV2 = v2;
lastOrthonormal = orthonormal;
// The flattened points list contains 2 points per line segment, which means that there
// are duplicate intermediate points, which we skip.
bool isLastLineSegment = (i + 2 == flattenedPoints.Count);
if (!isLastLineSegment)
{
i++;
}
}
// Apply the side falloff to the terrain heights.
// We use a padding where the road influence is 100% because we want road width to be flat
// but that means that we also have to set some triangle vertices outside the road width to
// full 100% road height.
float padding = cellSizeDiagonal;
ClampHeightsToLineSegments(terrain, aabbWithSideFalloffs, segments, padding);
}
// Clamp the terrain heights to the inner part of the road.
// We create quads for the road mesh and clamp the heights to the quad triangles.
{
Vector3F previousV1 = Vector3F.Zero;
Vector3F previousV2 = Vector3F.Zero;
Vector3F lastOrthonormal = Vector3F.Right;
for (int i = 0; i < flattenedPoints.Count; i++)
{
Vector3F start = flattenedPoints[i];
Vector3F previous;
bool isFirstPoint = (i == 0);
if (!isFirstPoint)
{
previous = flattenedPoints[i - 1];
}
else if (isClosed && road.SmoothEnds)
{
previous = flattenedPoints[flattenedPoints.Count - 2];
}
else
{
previous = start;
}
Vector3F next;
bool isLastPoint = (i + 1 == flattenedPoints.Count);
if (!isLastPoint)
{
next = flattenedPoints[i + 1];
}
else if (isClosed && road.SmoothEnds)
{
next = flattenedPoints[1];
}
else
{
next = start;
}
Vector3F tangent = next - previous;
Vector3F orthonormal = new Vector3F(tangent.Z, 0, -tangent.X);
if (!orthonormal.TryNormalize())
{
orthonormal = lastOrthonormal;
}
// Add 2 vertices to create a mesh like this:
//
// pV1 pV2 (previous vertices)
// x---------------x
// | |
// x---------------x
// v1 v2 (current vertices)
//
// Then we check all height samples against these triangles.
// Vectors are 4D. Height is y. Influence is w.
Vector3F v1 = start - orthonormal * halfWidths[i];
Vector3F v2 = start + orthonormal * halfWidths[i];
if (i > 0)
{
ClampHeightsToQuad(terrain, previousV1, previousV2, v1, v2);
}
previousV1 = v1;
previousV2 = v2;
lastOrthonormal = orthonormal;
// The flattened points list contains 2 points per line segment, which means that there
// are duplicate intermediate points, which we skip.
bool isLastLineSegment = (i + 2 == flattenedPoints.Count);
if (!isLastLineSegment)
{
i++;
}
}
}
terrain.Invalidate();
}
public Aabb GetAabb()
{
if (Vertex == null)
return new Aabb();
Aabb aabb = new Aabb(Vertex.Position, Vertex.Position);
foreach(var v in Vertices)
aabb.Grow(v.Position);
return aabb;
}
// Computes the AABB of the polygon.
private static Aabb GetAabb(Vector3F[] vertices, int numberOfVertices)
{
Aabb aabb = new Aabb(vertices[0], vertices[0]);
for (int i = 1; i < numberOfVertices; i++)
aabb.Grow(vertices[i]);
return aabb;
}
public void GrowFromPoint()
{
var a = new Aabb(new Vector3F(1, 2, 3), new Vector3F(3, 4, 5));
a.Grow(new Vector3F(10, -20, -30));
Assert.AreEqual(new Aabb(new Vector3F(1, -20, -30), new Vector3F(10, 4, 5)), a);
}
public void Grow()
{
var a = new Aabb(new Vector3F(1, 2, 3), new Vector3F(3, 4, 5));
a.Grow(new Aabb(new Vector3F(1, 2, 3), new Vector3F(3, 4, 5)));
Assert.AreEqual(new Aabb(new Vector3F(1, 2, 3), new Vector3F(3, 4, 5)), a);
a.Grow(new Aabb(new Vector3F(-1, 2, 3), new Vector3F(3, 4, 5)));
Assert.AreEqual(new Aabb(new Vector3F(-1, 2, 3), new Vector3F(3, 4, 5)), a);
a.Grow(new Aabb(new Vector3F(1, 2, 3), new Vector3F(3, 5, 5)));
Assert.AreEqual(new Aabb(new Vector3F(-1, 2, 3), new Vector3F(3, 5, 5)), a);
var geo = new GeometricObject(new SphereShape(3), new Pose(new Vector3F(1, 0, 0)));
a.Grow(geo);
Assert.AreEqual(new Aabb(new Vector3F(-2, -3, -3), new Vector3F(4, 5, 5)), a);
}
private void LoadFile()
{
if (_fileIndex == MaxFileIndex)
{
// Instead of a file load procedural test data.
//var model = Game.Content.Load<Model>("Dude");
//_points = TriangleMesh.FromModel(model).Vertices;
//RandomHelper.Random = new Random(_fileIndex);
_points = new List<Vector3F>();
for (int i = 0; i < 12000; i++)
{
_points.Add(new Vector3F(
RandomHelper.Random.NextFloat(-10, -9),
RandomHelper.Random.NextFloat(100, 101),
RandomHelper.Random.NextFloat(-10.001f, -10)));
}
}
else if (_fileIndex == MaxFileIndex - 1)
{
// Instead of a file load procedural test data.
//var model = Game.Content.Load<Model>("Dude");
//_points = TriangleMesh.FromModel(model).Vertices;
//RandomHelper.Random = new Random(_fileIndex);
_points = new List<Vector3F>();
for (int i = 0; i < 12000; i++)
{
_points.Add(new Vector3F(
RandomHelper.Random.NextFloat(-10, -9),
RandomHelper.Random.NextFloat(100, 101),
RandomHelper.Random.NextFloat(-10f, -10)));
}
}
else if (_fileIndex < MaxFileIndex)
{
var path = string.Format("..\\..\\..\\..\\..\\Testcases\\Mesh{0:000}.xml", _fileIndex);
_points = LoadPoints(path);
}
// Adjust camera position and speed.
var aabb = new Aabb(_points[0], _points[0]);
foreach (var point in _points)
aabb.Grow(point);
SetCamera(aabb.Center - aabb.Extent.Length * Vector3F.Forward * 2, 0, 0);
}
private Aabb GetRandomAabb()
{
var point = RandomHelper.Random.NextVector3F(0, 100);
var point2 = RandomHelper.Random.NextVector3F(0, 100);
var newAabb = new Aabb(point, point);
newAabb.Grow(point2);
return newAabb;
}
public void Grow(IEnumerable<Vector3F> points, int vertexLimit, float skinWidth)
{
Debug.Assert(_taggedEdges.Count == 0, "ConvexHullBuilder is in an invalid state.");
Debug.Assert(_faces.Count == 0, "ConvexHullBuilder is in an invalid state.");
var pointList = points.ToList();
if (pointList.Count == 0)
return;
// If the current convex is not spatial, we restart. This allows us to start
// the convex hull with a more robust initial point selection.
if (_type != ConvexHullType.Spatial)
{
pointList.AddRange(_mesh.Vertices.Select(v => v.Position));
Reset();
}
Matrix44D toUnitCube;
Matrix44D fromUnitCube;
#region ----- Convert to Unit Cube -----
{
// Get AABB of existing and new points.
var aabb = new Aabb(pointList[0], pointList[0]);
foreach (var v in _mesh.Vertices)
aabb.Grow(v.Position);
foreach (var p in pointList)
aabb.Grow(p);
var extent = aabb.Extent;
if (!Numeric.IsFinite(extent.X) || !Numeric.IsFinite(extent.Y) || !Numeric.IsFinite(extent.Z))
throw new GeometryException("Cannot build convex hull because the input positions are invalid (e.g. NaN or infinity).");
// Avoid division by 0 for planar point clouds.
if (Numeric.IsZero(extent.X))
extent.X = 1;
if (Numeric.IsZero(extent.Y))
extent.Y = 1;
if (Numeric.IsZero(extent.Z))
extent.Z = 1;
toUnitCube = Matrix44D.CreateScale(2 / extent.X, 2 / extent.Y, 2 / extent.Z) * Matrix44D.CreateTranslation(-aabb.Center);
fromUnitCube = Matrix44D.CreateTranslation(aabb.Center) * Matrix44D.CreateScale(extent / 2);
foreach (var v in _mesh.Vertices)
v.Position = (Vector3F)toUnitCube.TransformPosition(v.Position);
for (int i = 0; i < pointList.Count; i++)
pointList[i] = (Vector3F)toUnitCube.TransformPosition(pointList[i]);
}
#endregion
// Remove duplicate vertices.
GeometryHelper.MergeDuplicatePositions(pointList, Numeric.EpsilonF);
// Find initial tetrahedron and check if points are in a plane.
if (_type != ConvexHullType.Spatial)
_isPlanar = SortPoints(pointList);
else
_isPlanar = false;
// Create convex hull using Incremental Construction.
var numberOfPoints = pointList.Count;
for (int i = 0; i < numberOfPoints; i++)
{
bool prune = false;
// Find a face with a good support point.
// The face is stored in this variable. The point is sorted to the front of the list.
DcelFace supportPointFace = null;
if (_type == ConvexHullType.Spatial)
{
foreach (var face in _faces)
{
// Skip faces which are already on the hull.
if (face.Tag == -1)
continue;
var normal = face.Normal;
if (!normal.TryNormalize()) // Ignore degenerate faces.
continue;
// Plane distance from origin.
var d = Vector3F.Dot(normal, face.Boundary.Origin.Position);
// Find support point for this face under remaining points.
var maxDistance = d;
var maxIndex = i;
for (int j = i; j < numberOfPoints; j++)
{
float distance = Vector3F.Dot(normal, pointList[j]);
if (distance > maxDistance)
{
maxDistance = distance;
maxIndex = j;
}
}
if (maxDistance > d)
{
supportPointFace = face;
var supportPoint = pointList[maxIndex];
// Extrude in support direction to make it numerically more robust.
// This step proved to be very important for the numerical stability!
supportPoint += Numeric.EpsilonF * normal;
// Only prune if we make a significant step (5% of the unit cube size).
prune = (maxDistance - d) > 0.1;
// Swap support point to front.
pointList[maxIndex] = pointList[i];
pointList[i] = supportPoint;
break;
}
else
{
// No support point found. Face must be on the convex hull.
face.Tag = -1;
}
}
}
GrowConvex(pointList[i], supportPointFace);
if (i == 3 && !_isPlanar)
prune = true;
// Prune vertices.
#region ----- Prune Vertices -----
if (prune)
{
for (int j = i + 1; j < numberOfPoints; j++)
{
bool isVisible = false;
foreach (var face in _faces)
{
if (face.Tag >= 0 // No need to test hull faces (tag == -1).
&& DcelMesh.GetCollinearity(pointList[j], face) != Collinearity.NotCollinearBehind)
{
// Face is "probably" visible. Point is outside and must be kept.
// (Pruning uses a conservative test. Exact test is made in the Grow methods.)
isVisible = true;
break;
}
}
if (!isVisible)
{
// Sort point to start of list and increase outer loop index.
var point = pointList[j];
pointList[j] = pointList[i + 1];
pointList[i + 1] = point; // Not strictly necessary but kept for asserts.
i++;
}
}
}
#endregion
}
_mesh.ResetTags();
_faces.Clear();
// TODO: Remove degenerate triangles. (Needs more testing.)
//RemoveDegenerateTriangles();
//#if DEBUG
// foreach (var point in pointList)
// Debug.Assert(_mesh.Contains(point, Numeric.EpsilonF * 10), "A point is outside the convex hull after GrowConvex().");
//#endif
if (_mesh.Faces.Count > 2 && (!Numeric.IsZero(skinWidth) || _mesh.Vertices.Count >= vertexLimit))
{
// ----- Apply Vertex Limit and/or Skin Width
// Skin width must be converted to unit cube.
var skinWidthScale = (Vector3F)toUnitCube.TransformDirection(new Vector3D(skinWidth));
// The assert after the cutting may fail for very low skin widths. But not when
// the debugger is attached :-(.
//skinWidthScale = Vector3F.Max(skinWidthScale, new Vector3F(100 * Numeric.EpsilonF));
_mesh.ModifyConvex(vertexLimit, skinWidthScale);
#if DEBUG
// TODO: This assert may fail - but not when the debugger is attached :-(
//foreach (var point in pointList)
// Debug.Assert(_mesh.Contains(point, 0.01f), "A point is outside the convex hull after plane cutting.");
#endif
}
// Convert back from unit cube.
foreach (var v in _mesh.Vertices)
v.Position = (Vector3F)fromUnitCube.TransformPosition(v.Position);
}
public static void ClampTerrainToRoad(HeightField terrain, Path3F road,
float defaultWidth, float defaultSideFalloff,
int maxNumberOfIterations, float tolerance)
{
if (terrain == null)
throw new ArgumentNullException("terrain");
if (road == null)
throw new ArgumentNullException("road");
// Compute list of line segments. (2 points per line segment!)
var flattenedPoints = new List<Vector3F>();
road.Flatten(flattenedPoints, maxNumberOfIterations, tolerance);
// Abort if path is empty.
int numberOfLineSegments = flattenedPoints.Count / 2;
if (numberOfLineSegments <= 0)
return;
// Compute accumulated lengths. (One entry for each entry in flattenedPoints.)
float[] accumulatedLengths = new float[flattenedPoints.Count];
accumulatedLengths[0] = 0;
for (int i = 1; i < flattenedPoints.Count; i += 2)
{
Vector3F previous = flattenedPoints[i - 1];
Vector3F current = flattenedPoints[i];
float length = (current - previous).Length;
accumulatedLengths[i] = accumulatedLengths[i - 1] + length;
if (i + 1 < flattenedPoints.Count)
accumulatedLengths[i + 1] = accumulatedLengths[i];
}
// Create a mapping between accumulatedLength and the path keys.
// (accumulatedLength --> key)
var pathLengthsAndKeys = new List<Pair<float, TerrainRoadPathKey>>();
{
int index = 0;
foreach (var key in road)
{
Vector3F position = key.Point;
var roadKey = key as TerrainRoadPathKey;
if (roadKey == null)
{
roadKey = new TerrainRoadPathKey
{
Point = key.Point,
Width = defaultWidth,
SideFalloff = defaultSideFalloff,
};
}
for (; index < flattenedPoints.Count; index++)
{
if (Vector3F.AreNumericallyEqual(position, flattenedPoints[index]))
{
pathLengthsAndKeys.Add(new Pair<float, TerrainRoadPathKey>(accumulatedLengths[index], roadKey));
break;
}
bool isLastLineSegment = (index + 2 == flattenedPoints.Count);
if (!isLastLineSegment)
index++;
}
index++;
}
}
// Create a list of interpolated road widths and side falloffs. (One entry for each entry in flattenedPoints.)
var halfWidths = new float[flattenedPoints.Count];
var sideFalloffs = new float[flattenedPoints.Count];
int previousKeyIndex = 0;
var previousKey = pathLengthsAndKeys[0];
var nextKey = pathLengthsAndKeys[1];
halfWidths[0] = 0.5f * pathLengthsAndKeys[0].Second.Width;
sideFalloffs[0] = pathLengthsAndKeys[0].Second.SideFalloff;
for (int i = 1; i < flattenedPoints.Count; i += 2)
{
if (accumulatedLengths[i] > nextKey.First)
{
previousKey = nextKey;
previousKeyIndex++;
nextKey = pathLengthsAndKeys[previousKeyIndex + 1];
}
float p = (accumulatedLengths[i] - previousKey.First) / (nextKey.First - previousKey.First);
halfWidths[i] = 0.5f * InterpolationHelper.Lerp(previousKey.Second.Width, nextKey.Second.Width, p);
sideFalloffs[i] = InterpolationHelper.Lerp(previousKey.Second.SideFalloff, nextKey.Second.SideFalloff, p);
if (i + 1 < flattenedPoints.Count)
{
halfWidths[i + 1] = halfWidths[i];
sideFalloffs[i + 1] = sideFalloffs[i];
}
}
// Get AABB of road with the side falloff.
Aabb aabbWithSideFalloffs;
{
Vector3F p = flattenedPoints[0];
float r = halfWidths[0] + sideFalloffs[0];
aabbWithSideFalloffs = new Aabb(new Vector3F(p.X - r, 0, p.Z - r),
new Vector3F(p.X + r, 0, p.Z + r));
for (int i = 1; i < flattenedPoints.Count; i += 2)
{
p = flattenedPoints[i];
r = halfWidths[i] + sideFalloffs[i];
aabbWithSideFalloffs.Grow(new Vector3F(p.X - r, 0, p.Z - r));
aabbWithSideFalloffs.Grow(new Vector3F(p.X + r, 0, p.Z + r));
}
}
// Terrain properties.
int numberOfSamplesX = terrain.NumberOfSamplesX;
int numberOfSamplesZ = terrain.NumberOfSamplesZ;
int numberOfCellsX = numberOfSamplesX - 1;
int numberOfCellsZ = numberOfSamplesZ - 1;
float widthX = terrain.WidthX;
float cellSizeX = widthX / numberOfCellsX;
float widthZ = terrain.WidthZ;
float cellSizeZ = widthZ / numberOfCellsZ;
float cellSizeDiagonal = (float)Math.Sqrt(cellSizeX * cellSizeX + cellSizeZ * cellSizeZ);
bool isClosed = Vector3F.AreNumericallyEqual(flattenedPoints[0], flattenedPoints[flattenedPoints.Count - 1]);
{
// Get the line segments which of the road border.
List<Vector4F> segments = new List<Vector4F>(); // 2 points per segment.
Vector3F lastOrthonormal = Vector3F.Right;
Vector4F previousV1 = Vector4F.Zero;
Vector4F previousV2 = Vector4F.Zero;
for (int i = 0; i < flattenedPoints.Count; i++)
{
Vector3F start = flattenedPoints[i];
Vector3F previous;
bool isFirstPoint = (i == 0);
if (!isFirstPoint)
previous = flattenedPoints[i - 1];
else if (isClosed && road.SmoothEnds)
previous = flattenedPoints[flattenedPoints.Count - 2];
else
previous = start;
Vector3F next;
bool isLastPoint = (i + 1 == flattenedPoints.Count);
if (!isLastPoint)
next = flattenedPoints[i + 1];
else if (isClosed && road.SmoothEnds)
next = flattenedPoints[1];
else
next = start;
Vector3F tangent = next - previous;
Vector3F orthonormal = new Vector3F(tangent.Z, 0, -tangent.X);
if (!orthonormal.TryNormalize())
orthonormal = lastOrthonormal;
// Add 2 vertices two segments for the road side border.
//
// pV1 pV2 (previous vertices)
// x x
// | |
// x x
// v1 v2 (current vertices)
//
// We store the side falloff with the vertex:
// Vectors are 4D. Height is y. Side falloff is w.
Vector4F v1 = new Vector4F(start - orthonormal * (halfWidths[i] + 0), sideFalloffs[i]);
Vector4F v2 = new Vector4F(start + orthonormal * (halfWidths[i] + 0), sideFalloffs[i]);
if (i > 0)
{
segments.Add(previousV1);
segments.Add(v1);
segments.Add(previousV2);
segments.Add(v2);
if (isLastPoint && !isClosed)
{
// A segment for the end of the road.
segments.Add(v1);
segments.Add(v2);
}
}
else
{
if (!isClosed)
{
// A segment for the start of the road.
segments.Add(v1);
segments.Add(v2);
}
}
previousV1 = v1;
previousV2 = v2;
lastOrthonormal = orthonormal;
// The flattened points list contains 2 points per line segment, which means that there
// are duplicate intermediate points, which we skip.
bool isLastLineSegment = (i + 2 == flattenedPoints.Count);
if (!isLastLineSegment)
i++;
}
// Apply the side falloff to the terrain heights.
// We use a padding where the road influence is 100% because we want road width to be flat
// but that means that we also have to set some triangle vertices outside the road width to
// full 100% road height.
float padding = cellSizeDiagonal;
ClampHeightsToLineSegments(terrain, aabbWithSideFalloffs, segments, padding);
}
// Clamp the terrain heights to the inner part of the road.
// We create quads for the road mesh and clamp the heights to the quad triangles.
{
Vector3F previousV1 = Vector3F.Zero;
Vector3F previousV2 = Vector3F.Zero;
Vector3F lastOrthonormal = Vector3F.Right;
for (int i = 0; i < flattenedPoints.Count; i++)
{
Vector3F start = flattenedPoints[i];
Vector3F previous;
bool isFirstPoint = (i == 0);
if (!isFirstPoint)
previous = flattenedPoints[i - 1];
else if (isClosed && road.SmoothEnds)
previous = flattenedPoints[flattenedPoints.Count - 2];
else
previous = start;
Vector3F next;
bool isLastPoint = (i + 1 == flattenedPoints.Count);
if (!isLastPoint)
next = flattenedPoints[i + 1];
else if (isClosed && road.SmoothEnds)
next = flattenedPoints[1];
else
next = start;
Vector3F tangent = next - previous;
Vector3F orthonormal = new Vector3F(tangent.Z, 0, -tangent.X);
if (!orthonormal.TryNormalize())
orthonormal = lastOrthonormal;
// Add 2 vertices to create a mesh like this:
//
// pV1 pV2 (previous vertices)
// x---------------x
// | |
// x---------------x
// v1 v2 (current vertices)
//
// Then we check all height samples against these triangles.
// Vectors are 4D. Height is y. Influence is w.
Vector3F v1 = start - orthonormal * halfWidths[i];
Vector3F v2 = start + orthonormal * halfWidths[i];
if (i > 0)
ClampHeightsToQuad(terrain, previousV1, previousV2, v1, v2);
previousV1 = v1;
previousV2 = v2;
lastOrthonormal = orthonormal;
// The flattened points list contains 2 points per line segment, which means that there
// are duplicate intermediate points, which we skip.
bool isLastLineSegment = (i + 2 == flattenedPoints.Count);
if (!isLastLineSegment)
i++;
}
}
terrain.Invalidate();
}
public static void CreateMesh(GraphicsDevice graphicsDevice, Path3F path, float defaultWidth,
int maxNumberOfIterations, float tolerance,
out Submesh submesh, out Aabb aabb, out float roadLength)
{
if (graphicsDevice == null)
throw new ArgumentNullException("graphicsDevice");
if (path == null)
throw new ArgumentNullException("path");
// Compute list of line segments. (2 points per line segment!)
var flattenedPoints = new List<Vector3F>();
path.Flatten(flattenedPoints, maxNumberOfIterations, tolerance);
// Abort if path is empty.
int numberOfLineSegments = flattenedPoints.Count / 2;
if (numberOfLineSegments <= 0)
{
submesh = null;
aabb = new Aabb();
roadLength = 0;
return;
}
// Compute accumulated lengths. (One entry for each entry in flattenedPoints.)
float[] accumulatedLengths = new float[flattenedPoints.Count];
accumulatedLengths[0] = 0;
for (int i = 1; i < flattenedPoints.Count; i += 2)
{
Vector3F previous = flattenedPoints[i - 1];
Vector3F current = flattenedPoints[i];
float length = (current - previous).Length;
accumulatedLengths[i] = accumulatedLengths[i - 1] + length;
if (i + 1 < flattenedPoints.Count)
accumulatedLengths[i + 1] = accumulatedLengths[i];
}
// Total road length.
roadLength = accumulatedLengths[accumulatedLengths.Length - 1];
// Create a mapping between accumulatedLength and the path key widths.
// (accumulatedLength --> TerrainRoadPathKey.Width)
var widthKeys = new List<Pair<float, float>>();
{
int index = 0;
foreach (var key in path)
{
Vector3F position = key.Point;
var roadKey = key as TerrainRoadPathKey;
float width = (roadKey != null) ? roadKey.Width : defaultWidth;
for (; index < flattenedPoints.Count; index++)
{
if (Vector3F.AreNumericallyEqual(position, flattenedPoints[index]))
{
widthKeys.Add(new Pair<float, float>(accumulatedLengths[index], width));
break;
}
bool isLastLineSegment = (index + 2 == flattenedPoints.Count);
if (!isLastLineSegment)
index++;
}
index++;
}
}
// Create a list of interpolated road widths. (One entry for each entry in flattenedPoints.)
var widths = new float[flattenedPoints.Count];
int previousKeyIndex = 0;
var previousKey = widthKeys[0];
var nextKey = widthKeys[1];
widths[0] = widthKeys[0].Second;
for (int i = 1; i < flattenedPoints.Count; i += 2)
{
if (accumulatedLengths[i] > nextKey.First)
{
previousKey = nextKey;
previousKeyIndex++;
nextKey = widthKeys[previousKeyIndex + 1];
}
float p = (accumulatedLengths[i] - previousKey.First) / (nextKey.First - previousKey.First);
widths[i] = InterpolationHelper.Lerp(previousKey.Second, nextKey.Second, p);
if (i + 1 < flattenedPoints.Count)
widths[i + 1] = widths[i];
}
// Compute vertices and indices.
var vertices = new List<TerrainLayerVertex>(numberOfLineSegments * 2 + 2);
var indices = new List<int>(numberOfLineSegments * 6); // Two triangles per line segment.
Vector3F lastOrthonormal = Vector3F.UnitX;
aabb = new Aabb(flattenedPoints[0], flattenedPoints[0]);
bool isClosed = Vector3F.AreNumericallyEqual(flattenedPoints[0], flattenedPoints[flattenedPoints.Count - 1]);
for (int i = 0; i < flattenedPoints.Count; i++)
{
Vector3F start = flattenedPoints[i];
Vector3F previous;
bool isFirstPoint = (i == 0);
if (!isFirstPoint)
previous = flattenedPoints[i - 1];
else if (isClosed && path.SmoothEnds)
previous = flattenedPoints[flattenedPoints.Count - 2];
else
previous = start;
Vector3F next;
bool isLastPoint = (i + 1 == flattenedPoints.Count);
if (!isLastPoint)
next = flattenedPoints[i + 1];
else if (isClosed && path.SmoothEnds)
next = flattenedPoints[1];
else
next = start;
Vector3F tangent = next - previous;
Vector3F orthonormal = new Vector3F(tangent.Z, 0, -tangent.X);
if (!orthonormal.TryNormalize())
orthonormal = lastOrthonormal;
// Add indices to add two triangles between the current and the next vertices.
if (!isLastPoint)
{
int baseIndex = vertices.Count;
// 2 3
// x----x
// |\ | ^
// | \ | | road
// | \ | | direction
// | \| |
// x----x
// 0 1
indices.Add(baseIndex);
indices.Add(baseIndex + 1);
indices.Add(baseIndex + 2);
indices.Add(baseIndex + 1);
indices.Add(baseIndex + 3);
indices.Add(baseIndex + 2);
}
// Add two vertices.
Vector3F leftVertex = start - orthonormal * (widths[i] / 2);
Vector3F rightVertex = start + orthonormal * (widths[i] / 2);
vertices.Add(new TerrainLayerVertex(new Vector2(leftVertex.X, leftVertex.Z), new Vector2(0, accumulatedLengths[i])));
vertices.Add(new TerrainLayerVertex(new Vector2(rightVertex.X, rightVertex.Z), new Vector2(1, accumulatedLengths[i])));
// Grow AABB
aabb.Grow(leftVertex);
aabb.Grow(rightVertex);
lastOrthonormal = orthonormal;
// The flattened points list contains 2 points per line segment, which means that there
// are duplicate intermediate points, which we skip.
bool isLastLineSegment = (i + 2 == flattenedPoints.Count);
if (!isLastLineSegment)
i++;
}
Debug.Assert(vertices.Count == (numberOfLineSegments * 2 + 2));
Debug.Assert(indices.Count == (numberOfLineSegments * 6));
// The road is projected onto the terrain, therefore the computed y limits are not correct.
// (unless the terrain was clamped to the road).
aabb.Minimum.Y = 0;
aabb.Maximum.Y = 0;
// Convert to submesh.
submesh = new Submesh
{
PrimitiveCount = indices.Count / 3,
PrimitiveType = PrimitiveType.TriangleList,
VertexCount = vertices.Count,
VertexBuffer = new VertexBuffer(graphicsDevice, TerrainLayerVertex.VertexDeclaration, vertices.Count, BufferUsage.WriteOnly),
IndexBuffer = new IndexBuffer(graphicsDevice, IndexElementSize.ThirtyTwoBits, indices.Count, BufferUsage.WriteOnly)
};
submesh.VertexBuffer.SetData(vertices.ToArray());
submesh.IndexBuffer.SetData(indices.ToArray());
}