public void ComputeCollision()
{
CollisionObject a = new CollisionObject();
TriangleMesh mesh = new TriangleMesh();
mesh.Add(new Triangle(new Vector3F(0, 0, 0), new Vector3F(0, 0, 1), new Vector3F(1, 0, 0)), false);
mesh.Add(new Triangle(new Vector3F(1, 0, 0), new Vector3F(0, 0, 1), new Vector3F(1, 0, 1)), false);
TriangleMeshShape meshShape = new TriangleMeshShape();
meshShape.Mesh = mesh;
a.GeometricObject = new GeometricObject(meshShape, Pose.Identity);
CollisionObject b = new CollisionObject(new GeometricObject
{
Shape = new SphereShape(1),
Pose = new Pose(new Vector3F(0, 0.9f, 0)),
});
ContactSet set;
TriangleMeshAlgorithm algo = new TriangleMeshAlgorithm(new CollisionDetection());
set = algo.GetClosestPoints(a, b);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(true, algo.HaveContact(b, a));
Assert.AreEqual(1, set.Count);
Assert.IsTrue(Numeric.AreEqual(0.1f, set[0].PenetrationDepth, 0.001f));
//Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 0, 0), set[0].PositionALocal, 0.01f)); // MPR will not return the perfect contact point.
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 1, 0), set[0].Normal, 0.1f));
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(0.1f, 0.9f, 0.1f));
algo.UpdateContacts(set, 0);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(true, algo.HaveContact(b, a));
Assert.AreEqual(1, set.Count);
Assert.IsTrue(Numeric.AreEqual(0.1f, set[0].PenetrationDepth, 0.001f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0.1f, 0, 0.1f), set[0].PositionALocal, 0.01f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 1, 0), set[0].Normal, 0.1f));
// The same with a swapped set.
set = set.Swapped;
algo.UpdateContacts(set, 0);
Assert.AreEqual(1, set.Count);
Assert.IsTrue(Numeric.AreEqual(0.1f, set[0].PenetrationDepth, 0.001f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0.1f, 0, 0.1f), set[0].PositionBLocal, 0.01f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, -1, 0), set[0].Normal, 0.1f));
// Separation.
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(0.2f, 1.2f, 0.3f));
set = set.Swapped;
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(false, algo.HaveContact(b, a));
algo.UpdateClosestPoints(set, 0);
Assert.IsTrue(Numeric.AreEqual(-0.2f, set[0].PenetrationDepth, 0.001f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0.2f, 0, 0.3f), set[0].PositionALocal, 0.01f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 1, 0), set[0].Normal, 0.1f));
algo.UpdateContacts(set, 0);
Assert.AreEqual(0, set.Count);
}
/// <summary>
/// Converts mesh content to model content with a <see cref="TriangleMeshShape"/>.
/// </summary>
/// <param name="input">The root node content.</param>
/// <param name="context">Context for the specified processor.</param>
/// <returns>The <see cref="Shape"/>.</returns>
public override DRModelNodeContent Process(NodeContent input, ContentProcessorContext context)
{
// ----- Process Model
var model = base.Process(input, context);
// ----- Extract Triangles
var triangleMesh = new TriangleMesh();
// The input node is usually a tree of nodes. We need to collect all MeshContent nodes
// in the tree. The DigitalRune Helper library provides a TreeHelper that can be used
// to traverse trees using LINQ.
// The following returns an IEnumerable that returns all nodes of the tree.
var nodes = TreeHelper.GetSubtree(input, n => n.Children);
// We only need nodes of type MeshContent.
var meshes = nodes.OfType<MeshContent>();
foreach (var mesh in meshes)
{
// Apply any transformations to vertices.
Matrix transform = mesh.AbsoluteTransform;
for (int i = 0; i < mesh.Positions.Count; i++)
mesh.Positions[i] = Vector3.Transform(mesh.Positions[i], transform);
// Extract triangles from submeshes.
foreach (var geometry in mesh.Geometry)
{
int numberOfTriangles = geometry.Indices.Count / 3;
for (int i = 0; i < numberOfTriangles; i++)
{
int index0 = geometry.Indices[3 * i + 0];
int index1 = geometry.Indices[3 * i + 2]; // Note: DigitalRune Geometry uses a different winding
int index2 = geometry.Indices[3 * i + 1]; // order. Therefore, the indices need to be swapped.
Vector3F vertex0 = (Vector3F)geometry.Vertices.Positions[index0];
Vector3F vertex1 = (Vector3F)geometry.Vertices.Positions[index1];
Vector3F vertex2 = (Vector3F)geometry.Vertices.Positions[index2];
triangleMesh.Add(new Triangle(vertex0, vertex1, vertex2), false, Numeric.EpsilonF, true);
}
}
}
// Remove duplicate vertices.
triangleMesh.WeldVertices();
// ----- Create TriangleMeshShape
// Create a TriangleMeshShape that can be used for collision detection.
// Note:
// - Contact-welding is enabled to improve the results of the collision detection.
// - A CompressedAabbTree is used for spatial partitioning to speed up collision detection.
var triangleMeshShape = new TriangleMeshShape(triangleMesh, true, new CompressedAabbTree());
// Export the ModelNode together with the TriangleMeshShape.
model.UserData = triangleMeshShape;
return model;
}
/// <overloads>
/// <summary>
/// Converts the given <see cref="ITriangleMesh"/> to a <see cref="DcelMesh"/>.
/// </summary>
/// </overloads>
///
/// <summary>
/// Converts the given <see cref="ITriangleMesh"/> to a <see cref="DcelMesh"/>.
/// </summary>
/// <param name="mesh">The triangle mesh.</param>
/// <returns>
/// The <see cref="DcelMesh"/>.
/// </returns>
/// <remarks>
/// Currently, creating <see cref="DcelMesh"/>es is not supported if the triangle mesh consists
/// of unconnected sub-meshes or unconnected triangles. All parts of the triangle mesh must be
/// connected via an edge. (But it is not required that the mesh is closed.)
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="mesh"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="mesh"/> has no vertices or vertex indices.
/// </exception>
/// <exception cref="NotSupportedException">
/// <paramref name="mesh"/> consists of several unconnected components or sub-meshes.
/// </exception>
public static DcelMesh FromTriangleMesh(ITriangleMesh mesh)
{
if (mesh == null)
throw new ArgumentNullException("mesh");
// The simple way: Create a TriangleMesh first.
var triangleMesh = new TriangleMesh();
triangleMesh.Add(mesh, false);
triangleMesh.WeldVertices();
return FromTriangleMesh(triangleMesh);
}
/// <overloads>
/// <summary>
/// Converts the given <see cref="ITriangleMesh"/> to a <see cref="DcelMesh"/>.
/// </summary>
/// </overloads>
///
/// <summary>
/// Converts the given <see cref="ITriangleMesh"/> to a <see cref="DcelMesh"/>.
/// </summary>
/// <param name="mesh">The triangle mesh.</param>
/// <returns>
/// The <see cref="DcelMesh"/>.
/// </returns>
/// <remarks>
/// Currently, creating <see cref="DcelMesh"/>es is not supported if the triangle mesh consists
/// of unconnected sub-meshes or unconnected triangles. All parts of the triangle mesh must be
/// connected via an edge. (But it is not required that the mesh is closed.)
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="mesh"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="mesh"/> has no vertices or vertex indices.
/// </exception>
/// <exception cref="NotSupportedException">
/// <paramref name="mesh"/> consists of several unconnected components or sub-meshes.
/// </exception>
public static DcelMesh FromTriangleMesh(ITriangleMesh mesh)
{
if (mesh == null)
{
throw new ArgumentNullException("mesh");
}
// The simple way: Create a TriangleMesh first.
var triangleMesh = new TriangleMesh();
triangleMesh.Add(mesh, false);
triangleMesh.WeldVertices();
return(FromTriangleMesh(triangleMesh));
}
public ConvexDecompositionSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
SampleFramework.IsMouseVisible = false;
GraphicsScreen.ClearBackground = true;
GraphicsScreen.BackgroundColor = Color.CornflowerBlue;
SetCamera(new Vector3F(3, 3, 3), 0.8f, -0.6f);
// Load model.
_modelNode = ContentManager.Load<ModelNode>("Saucer/Saucer").Clone();
// Combine all meshes of the model into a single TriangleMesh.
TriangleMesh mesh = new TriangleMesh();
foreach (var meshNode in _modelNode.GetChildren().OfType<MeshNode>())
{
var childMesh = MeshHelper.ToTriangleMesh(meshNode.Mesh);
childMesh.Transform(meshNode.PoseWorld * Matrix44F.CreateScale(meshNode.ScaleWorld));
mesh.Add(childMesh);
}
// Start convex decomposition on another thread.
_convexDecomposition = new ConvexDecomposition();
_convexDecomposition.ProgressChanged += OnProgressChanged;
_convexDecomposition.AllowedConcavity = 0.8f;
_convexDecomposition.IntermediateVertexLimit = 65536;
_convexDecomposition.VertexLimit = 64;
// 0 gives optimal results but is the slowest. Small positive values improve
// speed but the result might be less optimal.
_convexDecomposition.SmallIslandBoost = 0.02f;
_convexDecomposition.SampleTriangleCenters = true;
_convexDecomposition.SampleTriangleVertices = true;
// Experimental multithreading. Enable at own risk ;-)
_convexDecomposition.EnableMultithreading = true;
_convexDecomposition.DecomposeAsync(mesh);
}
public void ComputeCollisionBvh()
{
CollisionObject a = new CollisionObject();
TriangleMesh meshA = new TriangleMesh();
meshA.Add(new Triangle(new Vector3F(0, 0, 0), new Vector3F(0, 0, 1), new Vector3F(1, 0, 0)), false);
var meshShapeA = new TriangleMeshShape();
meshShapeA.Mesh = meshA;
meshShapeA.Partition = new AabbTree<int>();
((GeometricObject)a.GeometricObject).Shape = meshShapeA;
CollisionObject b = new CollisionObject();
TriangleMesh meshB = new TriangleMesh();
meshB.Add(new Triangle(new Vector3F(2, 0, 0), new Vector3F(2, 0, 1), new Vector3F(3, 0, 0)), false);
TriangleMeshShape meshShapeB = new TriangleMeshShape();
meshShapeB.Mesh = meshB;
meshShapeB.Partition = new AabbTree<int>();
((GeometricObject)b.GeometricObject).Shape = meshShapeB;
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(-2, 0, 0));
CollisionAlgorithm algo = new TriangleMeshAlgorithm(new CollisionDetection());
Assert.AreEqual(true, algo.HaveContact(a, b));
}
public void Validate6()
{
GlobalSettings.ValidationLevel = 0xff;
var scene = new Scene();
// This is allowed.
var n = new TestSceneNode { Shape = Shape.Empty };
scene.Children.Add(n);
// Invalid changes of already added node:
var mesh = new TriangleMesh();
mesh.Add(new Triangle(new Vector3F(1), new Vector3F(2), new Vector3F(3)));
mesh.Add(new Triangle(new Vector3F(4), new Vector3F(float.NaN, 5, 5), new Vector3F(6)));
var meshShape = new TriangleMeshShape(mesh);
Assert.Throws<GraphicsException>(() => n.Shape = meshShape);
}
/// <summary>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
/// <remarks>
/// This method creates a triangle mesh that represents a square lying in the plane. The square
/// has an edge length of <see cref="MeshSize"/>.
/// </remarks>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
Vector3F center = Normal * DistanceFromOrigin;
Vector3F orthoNormal1 = Normal.Orthonormal1;
Vector3F orthoNormal2 = Normal.Orthonormal2;
// Plane
// Make 4 triangles around the center.
TriangleMesh mesh = new TriangleMesh();
mesh.Add(new Triangle
{
Vertex0 = center,
Vertex1 = center + orthoNormal1 * MeshSize / 2 - orthoNormal2 * MeshSize / 2,
Vertex2 = center + orthoNormal1 * MeshSize / 2 + orthoNormal2 * MeshSize / 2,
}, true);
mesh.Add(new Triangle
{
Vertex0 = center,
Vertex1 = center + orthoNormal1 * MeshSize / 2 + orthoNormal2 * MeshSize / 2,
Vertex2 = center - orthoNormal1 * MeshSize / 2 + orthoNormal2 * MeshSize / 2,
}, true);
mesh.Add(new Triangle
{
Vertex0 = center,
Vertex1 = center - orthoNormal1 * MeshSize / 2 + orthoNormal2 * MeshSize / 2,
Vertex2 = center - orthoNormal1 * MeshSize / 2 - orthoNormal2 * MeshSize / 2,
}, true);
mesh.Add(new Triangle
{
Vertex0 = center,
Vertex1 = center - orthoNormal1 * MeshSize / 2 - orthoNormal2 * MeshSize / 2,
Vertex2 = center + orthoNormal1 * MeshSize / 2 - orthoNormal2 * MeshSize / 2,
}, true);
return mesh;
}
/// <summary>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
// Get coordinates of corners:
float near = -Math.Min(Near, Far);
float far = -Math.Max(Near, Far);
float leftNear = Math.Min(Left, Right);
float rightNear = Math.Max(Left, Right);
float topNear = Math.Max(Top, Bottom);
float bottomNear = Math.Min(Top, Bottom);
float farFactor = 1 / near * far; // Multiply near-values by this factor to get far-values.
float leftFar = leftNear * farFactor;
float rightFar = rightNear * farFactor;
float topFar = topNear * farFactor;
float bottomFar = bottomNear * farFactor;
TriangleMesh mesh = new TriangleMesh();
// -y face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(leftNear, bottomNear, near),
Vertex1 = new Vector3F(leftFar, bottomFar, far),
Vertex2 = new Vector3F(rightFar, bottomFar, far),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(rightFar, bottomFar, far),
Vertex1 = new Vector3F(rightNear, bottomNear, near),
Vertex2 = new Vector3F(leftNear, bottomNear, near),
}, true);
// +x face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(rightNear, topNear, near),
Vertex1 = new Vector3F(rightNear, bottomNear, near),
Vertex2 = new Vector3F(rightFar, bottomFar, far),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(rightFar, bottomFar, far),
Vertex1 = new Vector3F(rightFar, topFar, far),
Vertex2 = new Vector3F(rightNear, topNear, near),
}, true);
// -z face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(rightFar, topFar, far),
Vertex1 = new Vector3F(rightFar, bottomFar, far),
Vertex2 = new Vector3F(leftFar, bottomFar, far),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(leftFar, bottomFar, far),
Vertex1 = new Vector3F(leftFar, topFar, far),
Vertex2 = new Vector3F(rightFar, topFar, far),
}, true);
// -x face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(leftFar, topFar, far),
Vertex1 = new Vector3F(leftFar, bottomFar, far),
Vertex2 = new Vector3F(leftNear, bottomNear, near),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(leftNear, bottomNear, near),
Vertex1 = new Vector3F(leftNear, topNear, near),
Vertex2 = new Vector3F(leftFar, topFar, far),
}, true);
// +z face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(leftNear, topNear, near),
Vertex1 = new Vector3F(leftNear, bottomNear, near),
Vertex2 = new Vector3F(rightNear, bottomNear, near),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(rightNear, bottomNear, near),
Vertex1 = new Vector3F(rightNear, topNear, near),
Vertex2 = new Vector3F(leftNear, topNear, near),
}, true);
// +y face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(leftFar, topFar, far),
Vertex1 = new Vector3F(leftNear, topNear, near),
Vertex2 = new Vector3F(rightNear, topNear, near),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(rightNear, topNear, near),
Vertex1 = new Vector3F(rightFar, topFar, far),
Vertex2 = new Vector3F(leftFar, topFar, far),
}, true);
return mesh;
}
private void CreateDualGraph()
{
var triangles = new List <CDTriangle>();
// Convert to TriangleMesh.
var triangleMesh = _mesh as TriangleMesh;
if (triangleMesh == null)
{
triangleMesh = new TriangleMesh();
triangleMesh.Add(_mesh, false);
triangleMesh.WeldVertices();
}
// Initialize vertex normals.
var normals = new Vector3F[triangleMesh.Vertices.Count]; // Vertex normals.
var neighborCounts = new int[triangleMesh.Vertices.Count]; // Numbers of triangles that touch each vertex.
for (int i = 0; i < triangleMesh.Vertices.Count; i++)
{
normals[i] = Vector3F.Zero;
neighborCounts[i] = 0;
}
// Go through all triangles. Add the normal to normals and increase the neighborCounts
for (int i = 0; i < triangleMesh.NumberOfTriangles; i++)
{
Triangle triangle = triangleMesh.GetTriangle(i);
var normal = triangle.Normal;
for (int j = 0; j < 3; j++)
{
var vertexIndex = triangleMesh.Indices[(i * 3) + j];
normals[vertexIndex] = normals[vertexIndex] + normal;
neighborCounts[vertexIndex] = neighborCounts[vertexIndex] + 1;
}
}
// Create triangles.
for (int i = 0; i < triangleMesh.NumberOfTriangles; i++)
{
Triangle triangle = triangleMesh.GetTriangle(i);
var cdTriangle = new CDTriangle
{
Id = i,
Vertices = new[] { triangle.Vertex0, triangle.Vertex1, triangle.Vertex2 },
Normal = triangle.Normal, // TODO: Special care for degenerate triangles needed?
};
for (int j = 0; j < 3; j++)
{
var vertexIndex = triangleMesh.Indices[(i * 3) + j];
var normalSum = normals[vertexIndex];
var neighborCount = neighborCounts[vertexIndex];
if (neighborCount > 0)
{
var normal = normalSum / neighborCount;
normal.TryNormalize();
cdTriangle.VertexNormals[j] = normal;
}
}
triangles.Add(cdTriangle);
}
// Create an island for each triangle.
_islands = new List <CDIsland>(triangles.Count);
for (int i = 0; i < triangles.Count; i++)
{
var triangle = triangles[i];
var island = new CDIsland();
island.Id = i;
island.Triangles = new[] { triangle };
island.Vertices = triangle.Vertices;
island.Aabb = new Aabb(triangle.Vertices[0], triangle.Vertices[0]);
island.Aabb.Grow(triangle.Vertices[1]);
island.Aabb.Grow(triangle.Vertices[2]);
triangle.Island = island;
_islands.Add(island);
}
// Find connectivity (= add neighbor links).
for (int i = 0; i < triangles.Count; i++)
{
var a = triangles[i];
for (int j = i + 1; j < triangles.Count; j++)
{
var b = triangles[j];
CDTriangle.FindNeighbors(a, b);
}
}
// Create links.
_links = new List <CDIslandLink>();
for (int i = 0; i < _islands.Count; i++)
{
var island = _islands[i];
var triangle = island.Triangles[0];
// Go through all neighbors.
// If there is a neighbor, create a link.
// To avoid two links per triangle, we create the link only if the id of this triangle
// is less than the other island id.
for (int j = 0; j < 3; j++)
{
CDTriangle neighborTriangle = triangle.Neighbors[j];
if (neighborTriangle != null && neighborTriangle.Island.Id > i)
{
var link = new CDIslandLink(island, neighborTriangle.Island, AllowedConcavity, SmallIslandBoost, IntermediateVertexLimit, SampleTriangleVertices, SampleTriangleCenters);
_links.Add(link);
}
}
}
// Now, we have a lot of islands with 1 triangle each.
}
/// <summary>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
TriangleMesh mesh = new TriangleMesh();
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(_widthX / 2, _widthY / 2, 0),
Vertex1 = new Vector3F(-_widthX / 2, _widthY / 2, 0),
Vertex2 = new Vector3F(-_widthX / 2, -_widthY / 2, 0),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(-_widthX / 2, -_widthY / 2, 0),
Vertex1 = new Vector3F(_widthX / 2, -_widthY / 2, 0),
Vertex2 = new Vector3F(_widthX / 2, _widthY / 2, 0),
}, true);
return mesh;
}
/// <summary>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
TriangleMesh mesh = new TriangleMesh();
mesh.Add(new Triangle(this), true);
return mesh;
}
public void Setup()
{
// Make a unit cube.
_mesh = new TriangleMesh();
// Bottom
_mesh.Add(new Triangle(new Vector3F(0, 0, 0), new Vector3F(1, 1, 0), new Vector3F(1, 0, 0)), true);
_mesh.Add(new Triangle(new Vector3F(0, 0, 0), new Vector3F(0, 1, 0), new Vector3F(1, 1, 0)), true);
// Top
_mesh.Add(new Triangle(new Vector3F(0, 0, 1), new Vector3F(1, 0, 1), new Vector3F(1, 1, 1)), true);
_mesh.Add(new Triangle(new Vector3F(0, 0, 1), new Vector3F(1, 1, 1), new Vector3F(0, 1, 1)), true);
// Left
_mesh.Add(new Triangle(new Vector3F(0, 0, 0), new Vector3F(1, 0, 0), new Vector3F(1, 0, 1)), true);
_mesh.Add(new Triangle(new Vector3F(0, 0, 0), new Vector3F(1, 0, 1), new Vector3F(0, 0, 1)), true);
// Right
_mesh.Add(new Triangle(new Vector3F(0, 1, 0), new Vector3F(0, 1, 1), new Vector3F(1, 1, 1)), true);
_mesh.Add(new Triangle(new Vector3F(0, 1, 0), new Vector3F(1, 1, 1), new Vector3F(1, 1, 0)), true);
// Front
_mesh.Add(new Triangle(new Vector3F(1, 0, 0), new Vector3F(1, 1, 0), new Vector3F(1, 1, 1)), true);
_mesh.Add(new Triangle(new Vector3F(1, 0, 0), new Vector3F(1, 1, 1), new Vector3F(1, 0, 1)), true);
// Back
_mesh.Add(new Triangle(new Vector3F(0, 0, 0), new Vector3F(0, 0, 1), new Vector3F(0, 1, 1)), true);
_mesh.Add(new Triangle(new Vector3F(0, 0, 0), new Vector3F(0, 1, 1), new Vector3F(0, 1, 0)), true);
_meshShape = new TriangleMeshShape { Mesh = _mesh };
}
/// <summary>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
// Estimate required segment angle for given accuracy.
// (Easy to derive from simple drawing of a circle segment with a triangle used to represent
// the segment.)
float alpha = (float)Math.Acos((Radius - absoluteDistanceThreshold) / Radius) * 2;
int numberOfSegments = (int)Math.Ceiling(ConstantsF.PiOver2 / alpha) * 4;
// Apply the iteration limit - in case absoluteDistanceThreshold is 0.
// Lets say each iteration doubles the number of segments. This is an arbitrary interpretation
// of the "iteration limit".
numberOfSegments = Math.Min(numberOfSegments, 2 << iterationLimit);
alpha = ConstantsF.TwoPi / numberOfSegments;
TriangleMesh mesh = new TriangleMesh();
// The world space vertices are created by rotating "radius vectors" with this rotations.
QuaternionF rotationY = QuaternionF.CreateRotationY(alpha);
QuaternionF rotationZ = QuaternionF.CreateRotationZ(alpha);
// We use two nested loops: In each loop a "radius vector" is rotated further to get a
// new vertex.
Vector3F rLow = Vector3F.UnitX * Radius; // Radius vector for the lower vertex.
for (int i = 1; i <= numberOfSegments / 4; i++)
{
Vector3F rHigh = rotationZ.Rotate(rLow); // Radius vector for the higher vertex.
// In the inner loop we create lines and triangles between 4 vertices, which are created
// with the radius vectors rLow0, rLow1, rHigh0, rHigh1.
Vector3F rLow0 = rLow;
Vector3F rHigh0 = rHigh;
for (int j = 1; j <= numberOfSegments; j++)
{
Vector3F rLow1 = rotationY.Rotate(rLow0);
Vector3F rHigh1 = rotationY.Rotate(rHigh0);
// Two top hemisphere triangles
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, Height / 2 - Radius, 0) + rLow0,
Vertex1 = new Vector3F(0, Height / 2 - Radius, 0) + rLow1,
Vertex2 = new Vector3F(0, Height / 2 - Radius, 0) + rHigh0,
}, false);
if (i < numberOfSegments / 4) // At the "northpole" only a triangle is needed. No quad.
{
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, Height / 2 - Radius, 0) + rLow1,
Vertex1 = new Vector3F(0, Height / 2 - Radius, 0) + rHigh1,
Vertex2 = new Vector3F(0, Height / 2 - Radius, 0) + rHigh0,
}, false);
}
// Two bottom hemisphere triangles
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, -Height / 2 + Radius, 0) - rLow0,
Vertex1 = new Vector3F(0, -Height / 2 + Radius, 0) - rHigh0,
Vertex2 = new Vector3F(0, -Height / 2 + Radius, 0) - rLow1,
}, false);
if (i < numberOfSegments / 4) // At the "southpole" only a triangle is needed. No quad.
{
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, -Height / 2 + Radius, 0) - rLow1,
Vertex1 = new Vector3F(0, -Height / 2 + Radius, 0) - rHigh0,
Vertex2 = new Vector3F(0, -Height / 2 + Radius, 0) - rHigh1,
}, false);
}
// Two side triangles
if (i == 1)
{
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, -Height / 2 + Radius, 0) + rLow0,
Vertex1 = new Vector3F(0, -Height / 2 + Radius, 0) + rLow1,
Vertex2 = new Vector3F(0, Height / 2 - Radius, 0) + rLow0,
}, false);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, -Height / 2 + Radius, 0) + rLow1,
Vertex1 = new Vector3F(0, Height / 2 - Radius, 0) + rLow1,
Vertex2 = new Vector3F(0, Height / 2 - Radius, 0) + rLow0,
}, false);
}
rLow0 = rLow1;
rHigh0 = rHigh1;
}
rLow = rHigh;
}
mesh.WeldVertices();
return mesh;
}
/// <summary>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
// Get coordinates of corners:
float near = -Math.Min(Near, Far);
float far = -Math.Max(Near, Far);
float left = Math.Min(Left, Right);
float right = Math.Max(Left, Right);
float top = Math.Max(Top, Bottom);
float bottom = Math.Min(Top, Bottom);
TriangleMesh mesh = new TriangleMesh();
// -y face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(left, bottom, near),
Vertex1 = new Vector3F(left, bottom, far),
Vertex2 = new Vector3F(right, bottom, far),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(right, bottom, far),
Vertex1 = new Vector3F(right, bottom, near),
Vertex2 = new Vector3F(left, bottom, near),
}, true);
// +x face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(right, top, near),
Vertex1 = new Vector3F(right, bottom, near),
Vertex2 = new Vector3F(right, bottom, far),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(right, bottom, far),
Vertex1 = new Vector3F(right, top, far),
Vertex2 = new Vector3F(right, top, near),
}, true);
// -z face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(right, top, far),
Vertex1 = new Vector3F(right, bottom, far),
Vertex2 = new Vector3F(left, bottom, far),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(left, bottom, far),
Vertex1 = new Vector3F(left, top, far),
Vertex2 = new Vector3F(right, top, far),
}, true);
// -x face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(left, top, far),
Vertex1 = new Vector3F(left, bottom, far),
Vertex2 = new Vector3F(left, bottom, near),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(left, bottom, near),
Vertex1 = new Vector3F(left, top, near),
Vertex2 = new Vector3F(left, top, far),
}, true);
// +z face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(left, top, near),
Vertex1 = new Vector3F(left, bottom, near),
Vertex2 = new Vector3F(right, bottom, near),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(right, bottom, near),
Vertex1 = new Vector3F(right, top, near),
Vertex2 = new Vector3F(left, top, near),
}, true);
// +y face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(left, top, far),
Vertex1 = new Vector3F(left, top, near),
Vertex2 = new Vector3F(right, top, near),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(right, top, near),
Vertex1 = new Vector3F(right, top, far),
Vertex2 = new Vector3F(left, top, far),
}, true);
return mesh;
}
/// <summary>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
/// <remarks>
/// This method returns a mesh with a single degenerate triangle. The triangle represents a
/// line with the length <see cref="MeshSize"/>. The triangle is centered on
/// <see cref="PointOnLine"/>.
/// </remarks>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
Vector3F start = PointOnLine - Direction * (MeshSize / 2);
Vector3F end = PointOnLine + Direction * (MeshSize / 2);
// Make a mesh with 1 degenerate triangle
TriangleMesh mesh = new TriangleMesh();
mesh.Add(new Triangle
{
Vertex0 = start,
Vertex1 = start,
Vertex2 = end,
}, true, Numeric.EpsilonF, false);
return mesh;
}
/// <summary>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
// Convert absolute error to relative error.
float maxExtent = GetAabb(Vector3F.One, Pose.Identity).Extent.LargestComponent;
float relativeThreshold = !Numeric.IsZero(maxExtent)
? absoluteDistanceThreshold / maxExtent
: Numeric.EpsilonF;
// Get meshes of children and add them to mesh in parent space.
TriangleMesh mesh = new TriangleMesh();
int numberOfGeometries = Children.Count;
for (int childIndex = 0; childIndex < numberOfGeometries; childIndex++)
{
IGeometricObject geometricObject = Children[childIndex];
// Get child mesh.
var childMesh = geometricObject.Shape.GetMesh(relativeThreshold, iterationLimit);
// Transform child mesh into local space of this parent shape.
childMesh.Transform(geometricObject.Pose.ToMatrix44F() * Matrix44F.CreateScale(geometricObject.Scale));
// Add to parent mesh.
mesh.Add(childMesh, false);
}
return mesh;
}
/// <summary>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
/// <remarks>
/// A deep copy of the <see cref="Mesh"/> is returned.
/// </remarks>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
var triangleMesh = Mesh as TriangleMesh;
if (triangleMesh != null)
return triangleMesh.Clone();
// Return a copy of the mesh.
TriangleMesh mesh = new TriangleMesh();
int numberOfTriangles = _mesh.NumberOfTriangles;
for (int i = 0; i < numberOfTriangles; i++)
{
Triangle triangle = _mesh.GetTriangle(i);
mesh.Add(triangle, false);
}
mesh.WeldVertices();
return mesh;
}
private void CreateDualGraph()
{
var triangles = new List<CDTriangle>();
// Convert to TriangleMesh.
var triangleMesh = _mesh as TriangleMesh;
if (triangleMesh == null)
{
triangleMesh = new TriangleMesh();
triangleMesh.Add(_mesh, false);
triangleMesh.WeldVertices();
}
// Initialize vertex normals.
var normals = new Vector3F[triangleMesh.Vertices.Count]; // Vertex normals.
var neighborCounts = new int[triangleMesh.Vertices.Count]; // Numbers of triangles that touch each vertex.
for (int i = 0; i < triangleMesh.Vertices.Count; i++)
{
normals[i] = Vector3F.Zero;
neighborCounts[i] = 0;
}
// Go through all triangles. Add the normal to normals and increase the neighborCounts
for (int i = 0; i < triangleMesh.NumberOfTriangles; i++)
{
Triangle triangle = triangleMesh.GetTriangle(i);
var normal = triangle.Normal;
for (int j = 0; j < 3; j++)
{
var vertexIndex = triangleMesh.Indices[(i * 3) + j];
normals[vertexIndex] = normals[vertexIndex] + normal;
neighborCounts[vertexIndex] = neighborCounts[vertexIndex] + 1;
}
}
// Create triangles.
for (int i = 0; i < triangleMesh.NumberOfTriangles; i++)
{
Triangle triangle = triangleMesh.GetTriangle(i);
var cdTriangle = new CDTriangle
{
Id = i,
Vertices = new[] { triangle.Vertex0, triangle.Vertex1, triangle.Vertex2 },
Normal = triangle.Normal, // TODO: Special care for degenerate triangles needed?
};
for (int j = 0; j < 3; j++)
{
var vertexIndex = triangleMesh.Indices[(i * 3) + j];
var normalSum = normals[vertexIndex];
var neighborCount = neighborCounts[vertexIndex];
if (neighborCount > 0)
{
var normal = normalSum / neighborCount;
normal.TryNormalize();
cdTriangle.VertexNormals[j] = normal;
}
}
triangles.Add(cdTriangle);
}
// Create an island for each triangle.
_islands = new List<CDIsland>(triangles.Count);
for (int i = 0; i < triangles.Count; i++)
{
var triangle = triangles[i];
var island = new CDIsland();
island.Id = i;
island.Triangles = new[] { triangle };
island.Vertices = triangle.Vertices;
island.Aabb = new Aabb(triangle.Vertices[0], triangle.Vertices[0]);
island.Aabb.Grow(triangle.Vertices[1]);
island.Aabb.Grow(triangle.Vertices[2]);
triangle.Island = island;
_islands.Add(island);
}
// Find connectivity (= add neighbor links).
for (int i = 0; i < triangles.Count; i++)
{
var a = triangles[i];
for (int j = i + 1; j < triangles.Count; j++)
{
var b = triangles[j];
CDTriangle.FindNeighbors(a, b);
}
}
// Create links.
_links = new List<CDIslandLink>();
for (int i = 0; i < _islands.Count; i++)
{
var island = _islands[i];
var triangle = island.Triangles[0];
// Go through all neighbors.
// If there is a neighbor, create a link.
// To avoid two links per triangle, we create the link only if the id of this triangle
// is less than the other island id.
for (int j = 0; j < 3; j++)
{
CDTriangle neighborTriangle = triangle.Neighbors[j];
if (neighborTriangle != null && neighborTriangle.Island.Id > i)
{
var link = new CDIslandLink(island, neighborTriangle.Island, AllowedConcavity, SmallIslandBoost, IntermediateVertexLimit, SampleTriangleVertices, SampleTriangleCenters);
_links.Add(link);
}
}
}
// Now, we have a lot of islands with 1 triangle each.
}
/// <summary>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
// Estimate required segment angle for given accuracy.
// (Easy to derive from simple drawing of a circle segment with a triangle used to represent
// the segment.)
float alpha = (float)Math.Acos((Radius - absoluteDistanceThreshold) / Radius) * 2;
int numberOfSegments = (int)Math.Ceiling(ConstantsF.TwoPi / alpha);
// Apply the iteration limit - in case absoluteDistanceThreshold is 0.
// Lets say each iteration doubles the number of segments. This is an arbitrary interpretation
// of the "iteration limit".
numberOfSegments = Math.Min(numberOfSegments, 2 << iterationLimit);
alpha = ConstantsF.TwoPi / numberOfSegments;
Vector3F r0 = new Vector3F(Radius, 0, 0);
QuaternionF rotation = QuaternionF.CreateRotationY(alpha);
TriangleMesh mesh = new TriangleMesh();
for (int i = 1; i <= numberOfSegments; i++)
{
Vector3F r1 = rotation.Rotate(r0);
// Bottom triangle
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, -Height / 2, 0),
Vertex1 = new Vector3F(0, -Height / 2, 0) + r1,
Vertex2 = new Vector3F(0, -Height / 2, 0) + r0,
}, false);
// Top triangle
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, +Height / 2, 0),
Vertex1 = new Vector3F(0, +Height / 2, 0) + r0,
Vertex2 = new Vector3F(0, +Height / 2, 0) + r1,
}, false);
// Two side triangles
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, -Height / 2, 0) + r0,
Vertex1 = new Vector3F(0, -Height / 2, 0) + r1,
Vertex2 = new Vector3F(0, +Height / 2, 0) + r0,
}, false);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, -Height / 2, 0) + r1,
Vertex1 = new Vector3F(0, +Height / 2, 0) + r1,
Vertex2 = new Vector3F(0, +Height / 2, 0) + r0,
}, false);
r0 = r1;
}
mesh.WeldVertices();
return mesh;
}
private void CreateObjects()
{
// Create two collision objects with triangle mesh shapes.
var meshA = new TriangleMesh();
meshA.Add(new Triangle(new Vector3F(0, 1, 0), new Vector3F(0, 1, 0), new Vector3F(0, 1, 0)));
meshA.Add(new Triangle(new Vector3F(0, 1, 0), new Vector3F(0, 1, 0), new Vector3F(0, 1, 0)));
var shapeA = new TriangleMeshShape() { Partition = new CompressedAabbTree() };
var poseA = new Pose(new Vector3F(-1, 0, 0));
_objectA = new CollisionObject(new GeometricObject(shapeA, poseA));
var meshB = new BoxShape(0.2f, 2, 1f).GetMesh(0.05f, 4);
var shapeB = new TriangleMeshShape(meshB, true) { Partition = new CompressedAabbTree() };
var poseB = new Pose(new Vector3F(0.1f, 0, 0));
_objectB = new CollisionObject(new GeometricObject(shapeB, poseB));
}
/// <summary>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
/// <remarks>
/// This creates a mesh with a single degenerate triangle that represents the ray.
/// </remarks>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
// Make a mesh with 1 degenerate triangle
TriangleMesh mesh = new TriangleMesh();
mesh.Add(
new Triangle
{
Vertex0 = Origin,
Vertex1 = Origin,
Vertex2 = Origin + Direction * Length,
},
true,
Numeric.EpsilonF,
false);
return mesh;
}
/// <summary>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
// Half extent:
float halfExtentX = _widthX / 2;
float halfExtentY = _widthY / 2;
float halfExtentZ = _widthZ / 2;
TriangleMesh mesh = new TriangleMesh();
// -y face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(-halfExtentX, -halfExtentY, halfExtentZ),
Vertex1 = new Vector3F(-halfExtentX, -halfExtentY, -halfExtentZ),
Vertex2 = new Vector3F(halfExtentX, -halfExtentY, -halfExtentZ),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(halfExtentX, -halfExtentY, -halfExtentZ),
Vertex1 = new Vector3F(halfExtentX, -halfExtentY, halfExtentZ),
Vertex2 = new Vector3F(-halfExtentX, -halfExtentY, halfExtentZ),
}, true);
// +x face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(halfExtentX, halfExtentY, halfExtentZ),
Vertex1 = new Vector3F(halfExtentX, -halfExtentY, halfExtentZ),
Vertex2 = new Vector3F(halfExtentX, -halfExtentY, -halfExtentZ),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(halfExtentX, -halfExtentY, -halfExtentZ),
Vertex1 = new Vector3F(halfExtentX, halfExtentY, -halfExtentZ),
Vertex2 = new Vector3F(halfExtentX, halfExtentY, halfExtentZ),
}, true);
// -z face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(halfExtentX, halfExtentY, -halfExtentZ),
Vertex1 = new Vector3F(halfExtentX, -halfExtentY, -halfExtentZ),
Vertex2 = new Vector3F(-halfExtentX, -halfExtentY, -halfExtentZ),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(-halfExtentX, -halfExtentY, -halfExtentZ),
Vertex1 = new Vector3F(-halfExtentX, halfExtentY, -halfExtentZ),
Vertex2 = new Vector3F(halfExtentX, halfExtentY, -halfExtentZ),
}, true);
// -x face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(-halfExtentX, halfExtentY, -halfExtentZ),
Vertex1 = new Vector3F(-halfExtentX, -halfExtentY, -halfExtentZ),
Vertex2 = new Vector3F(-halfExtentX, -halfExtentY, halfExtentZ),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(-halfExtentX, -halfExtentY, halfExtentZ),
Vertex1 = new Vector3F(-halfExtentX, halfExtentY, halfExtentZ),
Vertex2 = new Vector3F(-halfExtentX, halfExtentY, -halfExtentZ),
}, true);
// +z face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(-halfExtentX, halfExtentY, halfExtentZ),
Vertex1 = new Vector3F(-halfExtentX, -halfExtentY, halfExtentZ),
Vertex2 = new Vector3F(halfExtentX, -halfExtentY, halfExtentZ),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(halfExtentX, -halfExtentY, halfExtentZ),
Vertex1 = new Vector3F(halfExtentX, halfExtentY, halfExtentZ),
Vertex2 = new Vector3F(-halfExtentX, halfExtentY, halfExtentZ),
}, true);
// +y face
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(-halfExtentX, halfExtentY, -halfExtentZ),
Vertex1 = new Vector3F(-halfExtentX, halfExtentY, halfExtentZ),
Vertex2 = new Vector3F(halfExtentX, halfExtentY, halfExtentZ),
}, true);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(halfExtentX, halfExtentY, halfExtentZ),
Vertex1 = new Vector3F(halfExtentX, halfExtentY, -halfExtentZ),
Vertex2 = new Vector3F(-halfExtentX, halfExtentY, -halfExtentZ),
}, true);
return mesh;
}
private void CreateRigidBody()
{
var triangleMesh = new TriangleMesh();
foreach (var meshNode in _modelNode.GetSubtree().OfType<MeshNode>())
{
// Extract the triangle mesh from the DigitalRune Graphics Mesh instance.
var subTriangleMesh = new TriangleMesh();
foreach (var submesh in meshNode.Mesh.Submeshes)
{
submesh.ToTriangleMesh(subTriangleMesh);
}
// Apply the transformation of the mesh node.
subTriangleMesh.Transform(meshNode.PoseWorld * Matrix44F.CreateScale(meshNode.ScaleWorld));
// Combine into final triangle mesh.
triangleMesh.Add(subTriangleMesh);
}
// Create a collision shape that uses the mesh.
var triangleMeshShape = new TriangleMeshShape(triangleMesh);
// Optional: Assign a spatial partitioning scheme to the triangle mesh. (A spatial partition
// adds an additional memory overhead, but it improves collision detection speed tremendously!)
triangleMeshShape.Partition = new CompressedAabbTree
{
// The tree is automatically built using a mixed top-down/bottom-up approach. Bottom-up
// building is slower but produces better trees. If the tree building takes too long,
// we can lower the BottomUpBuildThreshold (default is 128).
BottomUpBuildThreshold = 0,
};
_rigidBody = new RigidBody(triangleMeshShape, new MassFrame(), null)
{
Pose = _pose,
Scale = _scale,
MotionType = MotionType.Static
};
// Add rigid body to physics simulation and model to scene.
var simulation = _services.GetInstance<Simulation>();
simulation.RigidBodies.Add(_rigidBody);
}
public static Submesh CreateSubmesh(GraphicsDevice graphicsDevice, ITriangleMesh mesh, float angleLimit)
{
var tm = mesh as TriangleMesh;
if (tm == null)
{
tm = new TriangleMesh();
tm.Add(mesh);
tm.WeldVertices();
}
return CreateSubmesh(graphicsDevice, tm, angleLimit);
}
private static Submesh CreateGeoClipmapMesh(GraphicsDevice graphicsDevice, int numberOfLevels,
int cellsPerLevel, bool useDiamondTessellation)
{
if (graphicsDevice == null)
throw new ArgumentNullException("graphicsDevice");
if (cellsPerLevel < 1)
throw new ArgumentOutOfRangeException("cellsPerLevel", "The number of cells per level must be greater than 0.");
// Round to even number of cells.
if (cellsPerLevel % 2 != 0)
cellsPerLevel++;
// Allocate a mesh with conservative list capacities.
int cellsPerLevelUpperBound = (cellsPerLevel + 3) * (cellsPerLevel + 3);
int verticesPerLevelUpperBound = useDiamondTessellation ? cellsPerLevelUpperBound * 9 : cellsPerLevelUpperBound * 4;
int indicesPerLevelUpperBound = useDiamondTessellation ? cellsPerLevelUpperBound * 8 * 3 : cellsPerLevelUpperBound * 2 * 3;
var triangleMesh = new TriangleMesh(verticesPerLevelUpperBound * numberOfLevels, indicesPerLevelUpperBound * numberOfLevels);
// The levels are created in a separate mesh and then combined into the final mesh.
var triangleMeshes = new TriangleMesh[numberOfLevels];
triangleMeshes[0] = triangleMesh;
for (int i = 1; i < numberOfLevels; i++)
triangleMeshes[i] = new TriangleMesh(verticesPerLevelUpperBound, indicesPerLevelUpperBound);
//for (int level = 0; level < numberOfLevels; level++)
Parallel.For(0, numberOfLevels, level =>
{
var levelTriangleMesh = triangleMeshes[level];
int cellSize = (1 << level);
float y = level; // Store LOD in Y coordinate.
Debug.Assert(cellsPerLevel % 2 == 0);
int halfWidthInCells = cellsPerLevel / 2;
int halfWidth = cellSize * (halfWidthInCells);
int minInclusive = -halfWidth - cellSize; // We add an extra border on the top and left.
int maxInclusive = halfWidth - cellSize; // Normal loop limit without extra border.
int previousHalfWidth = (level > 0) ? halfWidth / 2 : -1;
if (useDiamondTessellation)
{
#region ----- Diamond tessellation -----
int index = 0;
for (int z = minInclusive; z <= maxInclusive; z += cellSize)
{
for (int x = minInclusive; x <= maxInclusive; x += cellSize)
{
// No triangles in the area which are covered by previous levels.
// (We compare cell centers with radius of last LOD.)
if (Math.Max(Math.Abs(x + cellSize / 2), Math.Abs(z + cellSize / 2)) < previousHalfWidth)
continue;
// Each cell will be tessellated like this:
// A-----B-----C
// | \ | / |
// | \ | / |
// D-----E-----F
// | / | \ |
// | / | \ |
// G-----H-----I
var indexA = index++;
levelTriangleMesh.Vertices.Add(new Vector3F(x, y, z));
var indexB = index++;
levelTriangleMesh.Vertices.Add(new Vector3F(x + 0.5f * cellSize, y, z));
var indexC = index++;
levelTriangleMesh.Vertices.Add(new Vector3F(x + cellSize, y, z));
var indexD = index++;
levelTriangleMesh.Vertices.Add(new Vector3F(x, y, z + 0.5f * cellSize));
var indexE = index++;
levelTriangleMesh.Vertices.Add(new Vector3F(x + 0.5f * cellSize, y, z + 0.5f * cellSize));
var indexF = index++;
levelTriangleMesh.Vertices.Add(new Vector3F(x + cellSize, y, z + 0.5f * cellSize));
var indexG = index++;
levelTriangleMesh.Vertices.Add(new Vector3F(x, y, z + cellSize));
var indexH = index++;
levelTriangleMesh.Vertices.Add(new Vector3F(x + 0.5f * cellSize, y, z + cellSize));
var indexI = index++;
levelTriangleMesh.Vertices.Add(new Vector3F(x + cellSize, y, z + cellSize));
// Triangles using ADEG:
if (x != minInclusive)
{
levelTriangleMesh.Indices.Add(indexE);
levelTriangleMesh.Indices.Add(indexD);
levelTriangleMesh.Indices.Add(indexA);
levelTriangleMesh.Indices.Add(indexE);
levelTriangleMesh.Indices.Add(indexG);
levelTriangleMesh.Indices.Add(indexD);
}
else
{
// The outer cells are tessellated differently to stitch to the next level.
// A-----B-----C
// | \ | / |
// | \ | / |
// | E-----F
// | / | \ |
// | / | \ |
// G-----H-----I
levelTriangleMesh.Indices.Add(indexE);
levelTriangleMesh.Indices.Add(indexG);
levelTriangleMesh.Indices.Add(indexA);
}
// Triangles using ABCE:
if (z != minInclusive)
{
levelTriangleMesh.Indices.Add(indexE);
levelTriangleMesh.Indices.Add(indexB);
levelTriangleMesh.Indices.Add(indexC);
levelTriangleMesh.Indices.Add(indexE);
levelTriangleMesh.Indices.Add(indexA);
levelTriangleMesh.Indices.Add(indexB);
}
else
{
levelTriangleMesh.Indices.Add(indexE);
levelTriangleMesh.Indices.Add(indexA);
levelTriangleMesh.Indices.Add(indexC);
}
// Triangles using CEFI:
if (x != maxInclusive)
{
levelTriangleMesh.Indices.Add(indexE);
levelTriangleMesh.Indices.Add(indexF);
levelTriangleMesh.Indices.Add(indexI);
levelTriangleMesh.Indices.Add(indexE);
levelTriangleMesh.Indices.Add(indexC);
levelTriangleMesh.Indices.Add(indexF);
}
else
{
levelTriangleMesh.Indices.Add(indexE);
levelTriangleMesh.Indices.Add(indexC);
levelTriangleMesh.Indices.Add(indexI);
}
// Triangles using EGHI:
if (z != maxInclusive)
{
levelTriangleMesh.Indices.Add(indexE);
levelTriangleMesh.Indices.Add(indexH);
levelTriangleMesh.Indices.Add(indexG);
levelTriangleMesh.Indices.Add(indexE);
levelTriangleMesh.Indices.Add(indexI);
levelTriangleMesh.Indices.Add(indexH);
}
else
{
levelTriangleMesh.Indices.Add(indexE);
levelTriangleMesh.Indices.Add(indexI);
levelTriangleMesh.Indices.Add(indexG);
}
}
}
Debug.Assert(levelTriangleMesh.Vertices.Count <= verticesPerLevelUpperBound, "Bad estimate for upper bound of vertices.");
Debug.Assert(levelTriangleMesh.Indices.Count <= indicesPerLevelUpperBound, "Bad estimate for upper bound of indices.");
levelTriangleMesh.WeldVertices(0.1f);
#endregion
}
else
{
#region ----- Simple tessellation -----
// Add one extra border to hide gaps.
minInclusive -= cellSize;
maxInclusive += cellSize;
int index = 0;
for (int z = minInclusive; z <= maxInclusive; z += cellSize)
{
for (int x = minInclusive; x <= maxInclusive; x += cellSize)
{
// No triangles in the area which are covered by previous levels.
// (We compare cell centers with radius of last LOD.)
if (Math.Max(Math.Abs(x + cellSize / 2), Math.Abs(z + cellSize / 2)) < previousHalfWidth)
continue;
// Each 2x2 cells will be tessellated like this:
// A-----B
// | / |
// | / |
// C-----D
int indexA = index++;
levelTriangleMesh.Vertices.Add(new Vector3F(x, y, z));
int indexB = index++;
levelTriangleMesh.Vertices.Add(new Vector3F(x + cellSize, y, z));
int indexC = index++;
levelTriangleMesh.Vertices.Add(new Vector3F(x, y, z + cellSize));
int indexD = index++;
levelTriangleMesh.Vertices.Add(new Vector3F(x + cellSize, y, z + cellSize));
levelTriangleMesh.Indices.Add(indexA);
levelTriangleMesh.Indices.Add(indexB);
levelTriangleMesh.Indices.Add(indexC);
levelTriangleMesh.Indices.Add(indexC);
levelTriangleMesh.Indices.Add(indexB);
levelTriangleMesh.Indices.Add(indexD);
}
}
Debug.Assert(levelTriangleMesh.Vertices.Count <= verticesPerLevelUpperBound, "Bad estimate for upper bound of vertices.");
Debug.Assert(levelTriangleMesh.Indices.Count <= indicesPerLevelUpperBound, "Bad estimate for upper bound of indices.");
levelTriangleMesh.WeldVertices(0.1f);
#endregion
}
});
// Combine meshes.
for (int i = 1; i < numberOfLevels; i++)
triangleMesh.Add(triangleMeshes[i]);
var vertices = new TerrainVertex[triangleMesh.Vertices.Count];
{
int index = 0;
for (int i = 0; i < vertices.Length; i++)
{
Vector3F v = triangleMesh.Vertices[i];
vertices[index++] = new TerrainVertex(new HalfVector4(v.X, v.Y, v.Z, 1));
}
}
var submesh = CreateSubmesh(graphicsDevice, vertices, triangleMesh.Indices.ToArray());
//Debug.WriteLine("Number of terrain vertices:" + vertices.Length);
//Debug.WriteLine("Number of terrain triangles:" + triangleMesh.Indices.Count / 3);
return submesh;
}
/// <summary>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
// Estimate required segment angle for given accuracy.
// (Easy to derive from simple drawing of a circle segment with a triangle used to represent
// the segment.)
float alpha = (float)Math.Acos((_radius - absoluteDistanceThreshold) / _radius) * 2;
int numberOfSegments = (int)Math.Ceiling(ConstantsF.TwoPi / alpha);
alpha = ConstantsF.TwoPi / numberOfSegments;
Vector3F r0 = new Vector3F(_radius, 0, 0);
QuaternionF rotation = QuaternionF.CreateRotationZ(alpha);
TriangleMesh mesh = new TriangleMesh();
for (int i = 1; i <= numberOfSegments; i++)
{
Vector3F r1 = rotation.Rotate(r0);
mesh.Add(new Triangle
{
Vertex0 = Vector3F.Zero,
Vertex1 = r0,
Vertex2 = r1,
}, true);
r0 = r1;
}
return mesh;
}
public static TriangleMesh CreateIcosphere(int subdivisions, bool hemisphere)
{
if (subdivisions < 0)
throw new ArgumentOutOfRangeException("subdivisions", "The number of subdivisions must not be negative.");
if (hemisphere && subdivisions < 1)
throw new ArgumentException("The number of subdivisions must greater than 0 to create a hemisphere.");
// The icosphere is created by subdividing an icosahedron.
// (http://en.wikipedia.org/wiki/Icosahedron)
var mesh = CreateIcosahedron();
var tempMesh = new TriangleMesh();
for (int s = 0; s < subdivisions; s++)
{
int numberOfTriangles = mesh.NumberOfTriangles;
for (int i = 0; i < numberOfTriangles; i++)
{
// Split triangle into 4 triangles.
Triangle triangle = mesh.GetTriangle(i);
Vector3F v01 = (triangle.Vertex0 + triangle.Vertex1) / 2;
Vector3F v12 = (triangle.Vertex1 + triangle.Vertex2) / 2;
Vector3F v20 = (triangle.Vertex2 + triangle.Vertex0) / 2;
tempMesh.Add(new Triangle(triangle.Vertex0, v01, v20));
tempMesh.Add(new Triangle(v01, v12, v20));
tempMesh.Add(new Triangle(v01, triangle.Vertex1, v12));
tempMesh.Add(new Triangle(v20, v12, triangle.Vertex2));
}
MathHelper.Swap(ref mesh, ref tempMesh);
tempMesh.Vertices.Clear();
tempMesh.Indices.Clear();
if (hemisphere && s == 0)
{
// Delete negative hemisphere after first subdivision.
numberOfTriangles = mesh.NumberOfTriangles;
for (int i = 0; i < numberOfTriangles; i++)
{
Triangle triangle = mesh.GetTriangle(i);
if (Numeric.IsLess(triangle.Vertex0.Y, 0) || Numeric.IsLess(triangle.Vertex1.Y, 0) || Numeric.IsLess(triangle.Vertex2.Y, 0))
{
// Skip triangles in negative hemisphere.
continue;
}
tempMesh.Add(triangle);
}
MathHelper.Swap(ref mesh, ref tempMesh);
tempMesh.Vertices.Clear();
tempMesh.Indices.Clear();
}
}
mesh.WeldVertices();
// The mesh is now a refined icosahedron. Adjust the position of the
// vertices to approximate a sphere.
int numberOfVertices = mesh.Vertices.Count;
for (int i = 0; i < numberOfVertices; i++)
{
Vector3F v = mesh.Vertices[i];
// Unit sphere: ||v|| = 1
v.Normalize();
mesh.Vertices[i] = v;
}
return mesh;
}
