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()); }