public void Clone()
{
ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(
new[]
{
new Vector3(0, 0, 0),
new Vector3(1, 0, 0),
new Vector3(0, 2, 0),
new Vector3(0, 0, 3),
new Vector3(1, 5, 0),
new Vector3(0, 1, 7),
});
ConvexPolyhedron clone = convexPolyhedron.Clone() as ConvexPolyhedron;
Assert.IsNotNull(clone);
for (int i = 0; i < clone.Vertices.Count; i++)
{
Assert.AreEqual(convexPolyhedron.Vertices[i], clone.Vertices[i]);
}
Assert.AreEqual(convexPolyhedron.GetAabb(Pose.Identity), clone.GetAabb(Pose.Identity));
Assert.AreEqual(convexPolyhedron.InnerPoint, clone.InnerPoint);
Assert.AreEqual(convexPolyhedron.GetSupportPoint(new Vector3(1, 1, 1)), clone.GetSupportPoint(new Vector3(1, 1, 1)));
Assert.AreEqual(convexPolyhedron.GetAabb(Pose.Identity).Minimum, clone.GetAabb(Pose.Identity).Minimum);
Assert.AreEqual(convexPolyhedron.GetAabb(Pose.Identity).Maximum, clone.GetAabb(Pose.Identity).Maximum);
}
public void SerializationBinary()
{
var a = new ConvexPolyhedron(
new[]
{
new Vector3(0, 0, 0),
new Vector3(1, 0, 0),
new Vector3(0, 2, 0),
new Vector3(0, 0, 3),
new Vector3(1, 5, 0),
new Vector3(0, 1, 7),
});
// Serialize object.
var stream = new MemoryStream();
var formatter = new BinaryFormatter();
formatter.Serialize(stream, a);
// Deserialize object.
stream.Position = 0;
var deserializer = new BinaryFormatter();
var b = (ConvexPolyhedron)deserializer.Deserialize(stream);
for (int i = 0; i < b.Vertices.Count; i++)
{
Assert.AreEqual(a.Vertices[i], b.Vertices[i]);
}
Assert.AreEqual(a.GetAabb(Pose.Identity), b.GetAabb(Pose.Identity));
Assert.AreEqual(a.InnerPoint, b.InnerPoint);
Assert.AreEqual(a.GetSupportPoint(new Vector3(1, 1, 1)), b.GetSupportPoint(new Vector3(1, 1, 1)));
}
public void Clone()
{
ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(
new[]
{
new Vector3F(0, 0, 0),
new Vector3F(1, 0, 0),
new Vector3F(0, 2, 0),
new Vector3F(0, 0, 3),
new Vector3F(1, 5, 0),
new Vector3F(0, 1, 7),
});
ConvexPolyhedron clone = convexPolyhedron.Clone() as ConvexPolyhedron;
Assert.IsNotNull(clone);
for (int i = 0; i < clone.Vertices.Count; i++)
Assert.AreEqual(convexPolyhedron.Vertices[i], clone.Vertices[i]);
Assert.AreEqual(convexPolyhedron.GetAabb(Pose.Identity), clone.GetAabb(Pose.Identity));
Assert.AreEqual(convexPolyhedron.InnerPoint, clone.InnerPoint);
Assert.AreEqual(convexPolyhedron.GetSupportPoint(new Vector3F(1,1,1)), clone.GetSupportPoint(new Vector3F(1, 1, 1)));
Assert.AreEqual(convexPolyhedron.GetAabb(Pose.Identity).Minimum, clone.GetAabb(Pose.Identity).Minimum);
Assert.AreEqual(convexPolyhedron.GetAabb(Pose.Identity).Maximum, clone.GetAabb(Pose.Identity).Maximum);
}
public void EmptyConvexPolyhedron()
{
ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(Enumerable.Empty<Vector3F>());
Assert.AreEqual(0, convexPolyhedron.Vertices.Count);
Assert.AreEqual(Vector3F.Zero, convexPolyhedron.InnerPoint);
Assert.AreEqual(new Aabb(), convexPolyhedron.GetAabb(Pose.Identity));
}
public void EmptyConvexPolyhedron()
{
ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(Enumerable.Empty <Vector3>());
Assert.AreEqual(0, convexPolyhedron.Vertices.Count);
Assert.AreEqual(Vector3.Zero, convexPolyhedron.InnerPoint);
Assert.AreEqual(new Aabb(), convexPolyhedron.GetAabb(Pose.Identity));
}
public void OnePoint()
{
Vector3F point = new Vector3F(1, 0, 0);
ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(new[] { point });
Assert.AreEqual(1, convexPolyhedron.Vertices.Count);
Assert.AreEqual(point, convexPolyhedron.InnerPoint);
Assert.AreEqual(new Aabb(point, point), convexPolyhedron.GetAabb(Pose.Identity));
}
private void CreateCompositeShape()
{
// Convert islands into CompositeShape with convex children.
_decomposition = new CompositeShape();
if (_islands == null)
{
return;
}
foreach (var island in _islands)
{
if (island.Vertices.Length <= 0)
{
continue;
}
// ReSharper disable EmptyGeneralCatchClause
try
{
// ----- Get convex hull mesh.
DcelMesh convexHullMesh;
if (island.ConvexHullBuilder == null)
{
// Create convex hull from scratch.
// Get all vertices of all island triangles.
var points = island.Triangles.SelectMany(t => t.Vertices);
// Create convex hull.
convexHullMesh = GeometryHelper.CreateConvexHull(points, VertexLimit, SkinWidth);
}
else
{
// Use existing convex hull.
convexHullMesh = island.ConvexHullBuilder.Mesh;
if (convexHullMesh.Vertices.Count > VertexLimit || SkinWidth != 0)
{
convexHullMesh.ModifyConvex(VertexLimit, SkinWidth);
}
}
// ----- Add a ConvexPolyhedron to CompositeShape.
if (convexHullMesh.Vertices.Count > 0)
{
var convexHullPoints = convexHullMesh.Vertices.Select(v => v.Position);
var convexPolyhedron = new ConvexPolyhedron(convexHullPoints);
var geometricObject = new GeometricObject(convexPolyhedron);
_decomposition.Children.Add(geometricObject);
}
}
catch
{
// Could not generate convex hull. Ignore object.
}
// ReSharper restore EmptyGeneralCatchClause
}
}
public void OnePoint()
{
Vector3 point = new Vector3(1, 0, 0);
ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(new[] { point });
Assert.AreEqual(1, convexPolyhedron.Vertices.Count);
Assert.AreEqual(point, convexPolyhedron.InnerPoint);
Assert.AreEqual(new Aabb(point, point), convexPolyhedron.GetAabb(Pose.Identity));
}
public void RandomConvexPolyhedron()
{
// Use a fixed seed.
RandomHelper.Random = new Random(12345);
// Try polyhedra with 0, 1, 2, ... points.
for (int numberOfPoints = 0; numberOfPoints < 100; numberOfPoints++)
{
List <Vector3> points = new List <Vector3>(numberOfPoints);
// Create random polyhedra.
for (int i = 0; i < numberOfPoints; i++)
{
points.Add(
new Vector3(
RandomHelper.Random.NextFloat(-10, 10),
RandomHelper.Random.NextFloat(-20, 20),
RandomHelper.Random.NextFloat(-100, 100)));
}
//var convexHull = GeometryHelper.CreateConvexHull(points);
ConvexPolyhedron convex = new ConvexPolyhedron(points);
// Sample primary directions
Vector3 right = new Vector3(2, 0, 0);
AssertSupportPointsAreEquivalent(GetSupportPoint(right, points), convex.GetSupportPoint(right), right);
Vector3 left = new Vector3(-2, 0, 0);
AssertSupportPointsAreEquivalent(GetSupportPoint(left, points), convex.GetSupportPoint(left), left);
Vector3 up = new Vector3(0, 2, 0);
AssertSupportPointsAreEquivalent(GetSupportPoint(up, points), convex.GetSupportPoint(up), up);
Vector3 down = new Vector3(0, -2, 0);
AssertSupportPointsAreEquivalent(GetSupportPoint(down, points), convex.GetSupportPoint(down), down);
Vector3 back = new Vector3(0, 0, 2);
AssertSupportPointsAreEquivalent(GetSupportPoint(back, points), convex.GetSupportPoint(back), back);
Vector3 front = new Vector3(0, 0, -2);
AssertSupportPointsAreEquivalent(GetSupportPoint(front, points), convex.GetSupportPoint(front), front);
// Sample random directions
for (int i = 0; i < 10; i++)
{
Vector3 direction = RandomHelper.Random.NextVector3(-1, 1);
if (direction.IsNumericallyZero)
{
continue;
}
Vector3 supportPoint = convex.GetSupportPoint(direction);
Vector3 reference = GetSupportPoint(direction, points);
// The support points can be different, e.g. if a an edge of face is normal to the
// direction. When projected onto the direction both support points must be at equal
// distance.
AssertSupportPointsAreEquivalent(reference, supportPoint, direction);
}
}
}
public void TwoPoints()
{
Vector3 point0 = new Vector3(1, 0, 0);
Vector3 point1 = new Vector3(10, 0, 0);
ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(new[] { point0, point1 });
Assert.AreEqual(2, convexPolyhedron.Vertices.Count);
Assert.AreEqual((point0 + point1) / 2, convexPolyhedron.InnerPoint);
Assert.AreEqual(new Aabb(point0, point1), convexPolyhedron.GetAabb(Pose.Identity));
}
public void ThreePoints()
{
Vector3 point0 = new Vector3(1, 1, 1);
Vector3 point1 = new Vector3(2, 1, 1);
Vector3 point2 = new Vector3(1, 2, 1);
ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(new[] { point0, point1, point2 });
Assert.AreEqual(3, convexPolyhedron.Vertices.Count);
Assert.AreEqual((point0 + point1 + point2) / 3, convexPolyhedron.InnerPoint);
Assert.AreEqual(new Aabb(new Vector3(1, 1, 1), new Vector3(2, 2, 1)), convexPolyhedron.GetAabb(Pose.Identity));
}
/// <summary>
/// Get points that are inside a polyhedron.
/// </summary>
/// <param name="p">The polyhedron.</param>
/// <returns>The indices of the points.</returns>
public List <int> GetInnerPoints(ConvexPolyhedron p, bool smartUpdate, bool influenceRegion2, Vector3?seedPoint = null)
{
List <int> points = new List <int>();
if (seedPoint != null)
{
if (false)
{
for (int k = 0; k < lastActivePoints.Count; ++k)
{
if (p.IsPointInside(Points[lastActivePoints[k]].RightHandedPosition, influenceRegion2))
{
points.Add(lastActivePoints[k]);
}
}
}
else
{
for (int k = 0; k < Points.Count; ++k)
{
if (p.IsPointInside(Points[k].RightHandedPosition, influenceRegion2))
{
points.Add(k);
}
}
}
}
else
{
Vector3 _seedPoint = (Vector3)seedPoint;
Vector3Int seedKey = GetKey(new Vector3(_seedPoint.x, _seedPoint.z, _seedPoint.y));
for (int i = -2; i <= 2; ++i)
{
for (int j = -2; j <= 2; ++j)
{
for (int l = -2; l <= 2; ++l)
{
var voxelPoints = GetVoxelForEditing(seedKey + new Vector3Int(i, j, l));
for (int k = 0; k < voxelPoints.Count; ++k)
{
if (p.IsPointInside(Points[voxelPoints[k]].RightHandedPosition.normalized, influenceRegion2))
{
points.Add(voxelPoints[k]);
}
}
}
}
}
}
return(points);
}
private List <int> GetInnerPoints(ConvexPolyhedron p, List <int> innerPoints)
{
List <int> points = new List <int>();
for (int k = 0; k < innerPoints.Count; ++k)
{
if (p.IsPointInside(PointCloud[innerPoints[k]].Position, influenceRegion2))
{
points.Add(innerPoints[k]);
}
}
return(points);
}
ShapeData CreateConvexHullShape(ConvexHullShape shape)
{
ConvexPolyhedron poly = shape.ConvexPolyhedron;
if (poly != null)
{
throw new NotImplementedException();
}
ShapeHull hull = new ShapeHull(shape);
hull.BuildHull(shape.Margin);
int indexCount = hull.NumIndices;
UIntArray indices = hull.Indices;
Vector3Array points = hull.Vertices;
ShapeData shapeData = new ShapeData();
shapeData.VertexCount = indexCount;
Vector3[] vertices = new Vector3[indexCount * 2];
int v = 0, i;
for (i = 0; i < indexCount; i += 3)
{
Vector3 v0 = points[(int)indices[i]];
Vector3 v1 = points[(int)indices[i + 1]];
Vector3 v2 = points[(int)indices[i + 2]];
Vector3 v01 = v0 - v1;
Vector3 v02 = v0 - v2;
Vector3 normal = Vector3.Cross(v01, v02);
normal.Normalize();
vertices[v++] = v0;
vertices[v++] = normal;
vertices[v++] = v1;
vertices[v++] = normal;
vertices[v++] = v2;
vertices[v++] = normal;
}
shapeData.SetVertexBuffer(device, vertices);
return(shapeData);
}
public void GetSupportPoint()
{
ConvexPolyhedron emptyConvexPolyhedron = new ConvexPolyhedron(Enumerable.Empty<Vector3F>());
Assert.AreEqual(new Vector3F(0, 0, 0), emptyConvexPolyhedron.GetSupportPoint(new Vector3F(1, 0, 0)));
Assert.AreEqual(new Vector3F(0, 0, 0), emptyConvexPolyhedron.GetSupportPoint(new Vector3F(0, 1, 0)));
Assert.AreEqual(new Vector3F(0, 0, 0), emptyConvexPolyhedron.GetSupportPoint(new Vector3F(0, 0, 1)));
Assert.AreEqual(new Vector3F(0, 0, 0), emptyConvexPolyhedron.GetSupportPoint(new Vector3F(1, 1, 1)));
Vector3F p0 = new Vector3F(2, 0, 0);
Vector3F p1 = new Vector3F(-1, -1, -2);
Vector3F p2 = new Vector3F(0, 2, -3);
Assert.IsTrue(Vector3F.AreNumericallyEqual(p0, new ConvexPolyhedron(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3F(1, 0, 0))));
Assert.IsTrue(Vector3F.AreNumericallyEqual(p2, new ConvexPolyhedron(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3F(0, 1, 0))));
Assert.IsTrue(Vector3F.AreNumericallyEqual(p2, new ConvexPolyhedron(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3F(0, 0, -1))));
Assert.IsTrue(Vector3F.AreNumericallyEqual(p1, new ConvexPolyhedron(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3F(-1, 0, 1))));
}
/// <summary>
/// Find the most appropriate active point for the given edge.
/// </summary>
/// <param name="e_ij">Edge</param>
/// <param name="edges">Set of existing edges</param>
/// <param name="triangles">List of existing triangles</param>
/// <returns>null if there is no active point, the index of the point</returns>
private int?FindActivePoint(Edge e_ij, Dictionary <Edge, Edge> edges, List <Triangle> triangles, HashSet <Edge> fixedEdges, HashSet <int> fixedVertices)
{
findAPCounter++;
int ap1, ap2;
List <int> innerPoints;
ConvexPolyhedron p = BuildPolyhedron(e_ij, edges, out ap1, out ap2, out innerPoints);
List <Vector3> allowedPoints = new List <Vector3>
{
PointCloud[e_ij.vertex1].Position,
PointCloud[e_ij.vertex2].Position
};
if (ap1 != -1 && !fixedVertices.Contains(ap1))
{
allowedPoints.Add(PointCloud[ap1].Position);
}
if (ap2 != -1 && !fixedVertices.Contains(ap2))
{
allowedPoints.Add(PointCloud[ap2].Position);
}
//var points = VoxelSet.GetInnerPoints(p, (PointCloud[e_ij.vertex1].Position + PointCloud[e_ij.vertex2].Position) / 2);
var points = GetInnerPoints(p, innerPoints);
float sum = float.PositiveInfinity;
int? activePoint = null;
HashSet <Triangle> tr = new HashSet <Triangle>();
foreach (var m in points)
{
tr.UnionWith(PointCloud[m].triangles);
}
foreach (var m in points)
{
float currentSum = CalculateEnergy(e_ij.vertex1, e_ij.vertex2, e_ij.vertex3, m);
if (PointCloud[m].status == PointStatus.ACTIVE &&
currentSum < sum &&
(!checkLengths || Distance(m, e_ij.vertex1) <= maxEdgeLength && Distance(m, e_ij.vertex2) <= maxEdgeLength) &&
GeomIntegrity(e_ij, m, tr, allowedPoints.ToArray(), fixedEdges, fixedVertices)) // if do not intersect other triangles
{
activePoint = m;
sum = currentSum;
}
}
return(activePoint);
}
public void SimpleTetrahedron()
{
List <Vector3> points = new List <Vector3>
{
new Vector3(0, 0, 0),
new Vector3(0, 1, 0),
new Vector3(1, 0, 0),
new Vector3(0, 0, 1),
};
ConvexPolyhedron convex = new ConvexPolyhedron(points);
// Sample primary directions
Vector3 right = new Vector3(2, 0, 0);
AssertSupportPointsAreEquivalent(GetSupportPoint(right, points), convex.GetSupportPoint(right), right);
Vector3 left = new Vector3(-2, 0, 0);
AssertSupportPointsAreEquivalent(GetSupportPoint(left, points), convex.GetSupportPoint(left), left);
Vector3 up = new Vector3(0, 2, 0);
AssertSupportPointsAreEquivalent(GetSupportPoint(up, points), convex.GetSupportPoint(up), up);
Vector3 down = new Vector3(0, -2, 0);
AssertSupportPointsAreEquivalent(GetSupportPoint(down, points), convex.GetSupportPoint(down), down);
Vector3 back = new Vector3(0, 0, 2);
AssertSupportPointsAreEquivalent(GetSupportPoint(back, points), convex.GetSupportPoint(back), back);
Vector3 front = new Vector3(0, 0, -2);
AssertSupportPointsAreEquivalent(GetSupportPoint(front, points), convex.GetSupportPoint(front), front);
// Sample random directions
for (int i = 0; i < 10; i++)
{
Vector3 direction = RandomHelper.Random.NextVector3(-1, 1);
Vector3 supportPoint = convex.GetSupportPoint(direction);
Vector3 reference = GetSupportPoint(direction, points);
// The support points can be different, e.g. if a an edge of face is normal to the
// direction. When projected onto the direction both support points must be at equal
// distance.
AssertSupportPointsAreEquivalent(reference, supportPoint, direction);
}
}
// Creates a convex shape for the given MeshContent.
private void LoadConvex(MeshContent mesh, out Pose pose, out Shape shape)
{
// Apply node's transformation to all vertices.
pose = Pose.Identity;
Matrix transform = mesh.AbsoluteTransform;
for (int i = 0; i < mesh.Positions.Count; i++)
{
mesh.Positions[i] = Vector3.Transform(mesh.Positions[i], transform);
}
// Convert the vertices from Microsoft.Xna.Framework.Vector3 to
// DigitalRune.Mathematics.Algebra.Vector3F.
IEnumerable <Vector3F> vertices = mesh.Positions.Select(pos => (Vector3F)pos);
// Return a ConvexPolyhedron (convex hull) consisting of the mesh vertices.
shape = new ConvexPolyhedron(vertices);
}
public void GetSupportPoint()
{
ConvexPolyhedron emptyConvexPolyhedron = new ConvexPolyhedron(Enumerable.Empty <Vector3>());
Assert.AreEqual(new Vector3(0, 0, 0), emptyConvexPolyhedron.GetSupportPoint(new Vector3(1, 0, 0)));
Assert.AreEqual(new Vector3(0, 0, 0), emptyConvexPolyhedron.GetSupportPoint(new Vector3(0, 1, 0)));
Assert.AreEqual(new Vector3(0, 0, 0), emptyConvexPolyhedron.GetSupportPoint(new Vector3(0, 0, 1)));
Assert.AreEqual(new Vector3(0, 0, 0), emptyConvexPolyhedron.GetSupportPoint(new Vector3(1, 1, 1)));
Vector3 p0 = new Vector3(2, 0, 0);
Vector3 p1 = new Vector3(-1, -1, -2);
Vector3 p2 = new Vector3(0, 2, -3);
Assert.IsTrue(Vector3.AreNumericallyEqual(p0, new ConvexPolyhedron(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3(1, 0, 0))));
Assert.IsTrue(Vector3.AreNumericallyEqual(p2, new ConvexPolyhedron(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3(0, 1, 0))));
Assert.IsTrue(Vector3.AreNumericallyEqual(p2, new ConvexPolyhedron(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3(0, 0, -1))));
Assert.IsTrue(Vector3.AreNumericallyEqual(p1, new ConvexPolyhedron(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3(-1, 0, 1))));
}
/// <summary>
/// Get points that are inside a polyhedron.
/// </summary>
/// <param name="p">The polyhedron.</param>
/// <returns>The indices of the points.</returns>
public int[] GetInnerPoints(ConvexPolyhedron p, Vector3?seedPoint = null)
{
List <int> points = new List <int>();
if (seedPoint != null)
{
for (int k = 0; k < Points.Count; ++k)
{
if (p.IsPointInside(Points[k].RightHandedPosition))
{
points.Add(k);
}
}
}
else
{
Vector3 _seedPoint = (Vector3)seedPoint;
Vector3Int seedKey = GetKey(new Vector3(_seedPoint.x, _seedPoint.z, _seedPoint.y));
for (int i = -2; i <= 2; ++i)
{
for (int j = -2; j <= 2; ++j)
{
for (int l = -2; l <= 2; ++l)
{
var voxelPoints = GetVoxelForEditing(seedKey + new Vector3Int(i, j, l));
for (int k = 0; k < voxelPoints.Count; ++k)
{
if (p.IsPointInside(Points[voxelPoints[k]].RightHandedPosition))
{
points.Add(voxelPoints[k]);
}
}
}
}
}
}
return(points.ToArray());
}
/// <summary>
/// Build an influence region of the given edge.
/// </summary>
/// <param name="edge">The edge, which influence region to be foind.</param>
/// <param name="edges">The set of existing edges.</param>
/// <returns>Polyhedron object that is the influence region.</returns>
private ConvexPolyhedron BuildPolyhedron(Edge edge, Dictionary <Edge, Edge> edges, out int ap1, out int ap2, out List <int> innerPoints)
{
if (influenceRegion2)
{
return(BuildPolyhedron2(edge, edges, out ap1, out ap2, out innerPoints));
}
buildPCounter++;
ap1 = -1;
ap2 = -1;
int i = edge.vertex1;
int j = edge.vertex2;
int k = edge.vertex3;
// check if the i and j vertices are chosen correctly
if (Vector3.Dot(Vector3.Cross(PointCloud[k].Position - PointCloud[i].Position, PointCloud[k].Position - PointCloud[j].Position), edge.normal) < 0)
{
i = edge.vertex2;
j = edge.vertex1;
}
// positions of i, j and k
Vector3 ip = PointCloud[i].Position;
Vector3 jp = PointCloud[j].Position;
Vector3 kp = PointCloud[k].Position;
// the size of the polyhedron
float s = (float)Math.Max(PointCloud[i].UniformityDegree, PointCloud[j].UniformityDegree) * (PointCloud[i].MinEdge + PointCloud[j].MinEdge) / 2;
// bary center of the triangle ijk
Vector3 p = (ip + jp + kp) / 3;
// center of the edge ij
Vector3 p_m = (ip + jp) / 2;
// normal of the triangle (polygon) ijk
Vector3 N = edge.normal;
ConvexPolyhedron polyhedron = new ConvexPolyhedron();
Vector3 n1 = Vector3.Cross(edge.normal, jp - ip).normalized;
polyhedron.AddFace(n1, ip); // test
polyhedron.AddFace(N, p_m + 2 * s * N); // top face
polyhedron.AddFace(-N, p_m - 2 * s * N); // bottom face -
polyhedron.AddFace(-Vector3.Cross(N, ip - p).normalized, ip); // face containing pi -
polyhedron.AddFace(Vector3.Cross(N, jp - p).normalized, jp); // face containing pj
Vector3 N5 = Vector3.Cross(jp - ip, N).normalized;
polyhedron.AddFace(N5, p_m + s * N5);
// here it checks whether there are some edges inside the influnce region
// that are connected to i or j
var points = VoxelSet.GetInnerPoints(polyhedron, smartUpdate, influenceRegion2, p_m);
Vector3?pLeft = null, pRight = null;
//float minAngleLeft = 181f, minAngleRight = 181f;
float minAngleLeft = 90 + regionAngle, minAngleRight = 90 + regionAngle;
foreach (var point in points)
{
if (point == i || point == j)
{
continue;
}
Edge left = new Edge(point, i, Vector3.back);
Edge right = new Edge(point, j, Vector3.back);
if (edges.ContainsKey(left))
{
float angleLeft = Vector3.Angle(jp - ip, PointCloud[point].Position - ip);
if (angleLeft < minAngleLeft)
{
minAngleLeft = angleLeft;
pLeft = PointCloud[point].Position;
ap1 = point;
}
}
if (edges.ContainsKey(right))
{
float angleRight = Vector3.Angle(ip - jp, PointCloud[point].Position - jp);
if (angleRight < minAngleRight)
{
minAngleRight = angleRight;
pRight = PointCloud[point].Position;
ap2 = point;
}
}
}
// additional faces that we need to hold geometry integrity
if (pLeft != null)
{
polyhedron.AddFace(-Vector3.Cross(N, (Vector3)pLeft - ip).normalized, ip);
}
if (pRight != null)
{
polyhedron.AddFace(Vector3.Cross(N, (Vector3)pRight - jp).normalized, jp);
}
innerPoints = points;
return(polyhedron);
}
public void SerializationBinary()
{
var a = new ConvexPolyhedron(
new[]
{
new Vector3F(0, 0, 0),
new Vector3F(1, 0, 0),
new Vector3F(0, 2, 0),
new Vector3F(0, 0, 3),
new Vector3F(1, 5, 0),
new Vector3F(0, 1, 7),
});
// Serialize object.
var stream = new MemoryStream();
var formatter = new BinaryFormatter();
formatter.Serialize(stream, a);
// Deserialize object.
stream.Position = 0;
var deserializer = new BinaryFormatter();
var b = (ConvexPolyhedron)deserializer.Deserialize(stream);
for (int i = 0; i < b.Vertices.Count; i++)
Assert.AreEqual(a.Vertices[i], b.Vertices[i]);
Assert.AreEqual(a.GetAabb(Pose.Identity), b.GetAabb(Pose.Identity));
Assert.AreEqual(a.InnerPoint, b.InnerPoint);
Assert.AreEqual(a.GetSupportPoint(new Vector3F(1, 1, 1)), b.GetSupportPoint(new Vector3F(1, 1, 1)));
}
// Creates a lot of random objects.
private void CreateRandomObjects()
{
var random = new Random();
var isFirstHeightField = true;
int currentShape = 0;
int numberOfObjects = 0;
while (true)
{
numberOfObjects++;
if (numberOfObjects > ObjectsPerType)
{
currentShape++;
numberOfObjects = 0;
}
Shape shape;
switch (currentShape)
{
case 0:
// Box
shape = new BoxShape(ObjectSize, ObjectSize * 2, ObjectSize * 3);
break;
case 1:
// Capsule
shape = new CapsuleShape(0.3f * ObjectSize, 2 * ObjectSize);
break;
case 2:
// Cone
shape = new ConeShape(1 * ObjectSize, 2 * ObjectSize);
break;
case 3:
// Cylinder
shape = new CylinderShape(0.4f * ObjectSize, 2 * ObjectSize);
break;
case 4:
// Sphere
shape = new SphereShape(ObjectSize);
break;
case 5:
// Convex hull of several points.
ConvexHullOfPoints hull = new ConvexHullOfPoints();
hull.Points.Add(new Vector3(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize));
hull.Points.Add(new Vector3(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize));
hull.Points.Add(new Vector3(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
hull.Points.Add(new Vector3(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
hull.Points.Add(new Vector3(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize));
shape = hull;
break;
case 6:
// A composite shape: two boxes that form a "T" shape.
var composite = new CompositeShape();
composite.Children.Add(
new GeometricObject(
new BoxShape(ObjectSize, 3 * ObjectSize, ObjectSize),
new Pose(new Vector3(0, 0, 0))));
composite.Children.Add(
new GeometricObject(
new BoxShape(2 * ObjectSize, ObjectSize, ObjectSize),
new Pose(new Vector3(0, 2 * ObjectSize, 0))));
shape = composite;
break;
case 7:
shape = new CircleShape(ObjectSize);
break;
case 8:
{
var compBvh = new CompositeShape();
compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3(0, 0.5f, 0), Matrix.Identity)));
compBvh.Children.Add(new GeometricObject(new BoxShape(0.8f, 0.5f, 0.5f), new Pose(new Vector3(0.5f, 0.7f, 0), Matrix.CreateRotationZ(-MathHelper.ToRadians(15)))));
compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3(0, 1.15f, 0), Matrix.Identity)));
compBvh.Children.Add(new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3(0.6f, 1.15f, 0), Matrix.CreateRotationX(0.3f))));
compBvh.Partition = new AabbTree<int>();
shape = compBvh;
break;
}
case 9:
CompositeShape comp = new CompositeShape();
comp.Children.Add(new GeometricObject(new BoxShape(0.5f * ObjectSize, 1 * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3(0, 0.5f * ObjectSize, 0), Quaternion.Identity)));
comp.Children.Add(new GeometricObject(new BoxShape(0.8f * ObjectSize, 0.5f * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3(0.3f * ObjectSize, 0.7f * ObjectSize, 0), Quaternion.CreateRotationZ(-MathHelper.ToRadians(45)))));
comp.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3(0, 1.15f * ObjectSize, 0), Quaternion.Identity)));
shape = comp;
break;
case 10:
shape = new ConvexHullOfPoints(new[]
{
new Vector3(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize),
new Vector3(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize),
new Vector3(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize)
});
break;
case 11:
ConvexHullOfShapes shapeHull = new ConvexHullOfShapes();
shapeHull.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3(0, 2 * ObjectSize, 0), Matrix.Identity)));
shapeHull.Children.Add(new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize), Pose.Identity));
shape = shapeHull;
break;
case 12:
shape = Shape.Empty;
break;
case 13:
var numberOfSamplesX = 10;
var numberOfSamplesZ = 10;
var samples = new float[numberOfSamplesX * numberOfSamplesZ];
for (int z = 0; z < numberOfSamplesZ; z++)
for (int x = 0; x < numberOfSamplesX; x++)
samples[z * numberOfSamplesX + x] = (float)(Math.Cos(z / 3f) * Math.Sin(x / 2f) * BoxSize / 6);
HeightField heightField = new HeightField(0, 0, 2 * BoxSize, 2 * BoxSize, samples, numberOfSamplesX, numberOfSamplesZ);
shape = heightField;
break;
//case 14:
//shape = new LineShape(new Vector3(0.1f, 0.2f, 0.3f), new Vector3(0.1f, 0.2f, -0.3f).Normalized);
//break;
case 15:
shape = new LineSegmentShape(
new Vector3(0.1f, 0.2f, 0.3f), new Vector3(0.1f, 0.2f, 0.3f) + 3 * ObjectSize * new Vector3(0.1f, 0.2f, -0.3f));
break;
case 16:
shape = new MinkowskiDifferenceShape
{
ObjectA = new GeometricObject(new SphereShape(0.1f * ObjectSize)),
ObjectB = new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize))
};
break;
case 17:
shape = new MinkowskiSumShape
{
ObjectA = new GeometricObject(new SphereShape(0.1f * ObjectSize)),
ObjectB = new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize)),
};
break;
case 18:
shape = new OrthographicViewVolume(0, ObjectSize, 0, ObjectSize, ObjectSize / 2, ObjectSize * 2);
break;
case 19:
shape = new PerspectiveViewVolume(MathHelper.ToRadians(60f), 16f / 10, ObjectSize / 2, ObjectSize * 3);
break;
case 20:
shape = new PointShape(0.1f, 0.3f, 0.2f);
break;
case 21:
shape = new RayShape(new Vector3(0.2f, 0, -0.12f), new Vector3(1, 2, 3).Normalized, ObjectSize * 2);
break;
case 22:
shape = new RayShape(new Vector3(0.2f, 0, -0.12f), new Vector3(1, 2, 3).Normalized, ObjectSize * 2)
{
StopsAtFirstHit = true
};
break;
case 23:
shape = new RectangleShape(ObjectSize, ObjectSize * 2);
break;
case 24:
shape = new TransformedShape(
new GeometricObject(
new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize),
new Pose(new Vector3(0.1f, 1, -0.2f))));
break;
case 25:
shape = new TriangleShape(
new Vector3(ObjectSize, 0, 0), new Vector3(0, ObjectSize, 0), new Vector3(ObjectSize, ObjectSize, ObjectSize));
break;
//case 26:
// {
// // Create a composite object from which we get the mesh.
// CompositeShape compBvh = new CompositeShape();
// compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3(0, 0.5f, 0), Matrix.Identity)));
// compBvh.Children.Add(
// new GeometricObject(
// new BoxShape(0.8f, 0.5f, 0.5f),
// new Pose(new Vector3(0.5f, 0.7f, 0), Matrix.CreateRotationZ(-(float)MathHelper.ToRadians(15)))));
// compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3(0, 1.15f, 0), Matrix.Identity)));
// compBvh.Children.Add(
// new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3(0.6f, 1.15f, 0), Matrix.CreateRotationX(0.3f))));
// TriangleMeshShape meshBvhShape = new TriangleMeshShape { Mesh = compBvh.GetMesh(0.01f, 3) };
// meshBvhShape.Partition = new AabbTree<int>();
// shape = meshBvhShape;
// break;
// }
//case 27:
// {
// // Create a composite object from which we get the mesh.
// CompositeShape compBvh = new CompositeShape();
// compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3(0, 0.5f, 0), Quaternion.Identity)));
// compBvh.Children.Add(
// new GeometricObject(
// new BoxShape(0.8f, 0.5f, 0.5f),
// new Pose(new Vector3(0.5f, 0.7f, 0), Quaternion.CreateRotationZ(-(float)MathHelper.ToRadians(15)))));
// compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3(0, 1.15f, 0), Quaternion.Identity)));
// compBvh.Children.Add(
// new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3(0.6f, 1.15f, 0), Quaternion.CreateRotationX(0.3f))));
// TriangleMeshShape meshBvhShape = new TriangleMeshShape { Mesh = compBvh.GetMesh(0.01f, 3) };
// meshBvhShape.Partition = new AabbTree<int>();
// shape = meshBvhShape;
// break;
// }
case 28:
shape = new ConvexPolyhedron(new[]
{
new Vector3(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize),
new Vector3(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize),
new Vector3(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize)
});
break;
case 29:
return;
default:
currentShape++;
continue;
}
// Create an object with the random shape, pose, color and velocity.
Pose randomPose = new Pose(
random.NextVector3(-BoxSize + ObjectSize * 2, BoxSize - ObjectSize * 2),
random.NextQuaternion());
var newObject = new MovingGeometricObject
{
Pose = randomPose,
Shape = shape,
LinearVelocity = random.NextQuaternion().Rotate(new Vector3(MaxLinearVelocity, 0, 0)),
AngularVelocity = random.NextQuaternion().Rotate(Vector3.Forward)
* RandomHelper.Random.NextFloat(0, MaxAngularVelocity),
};
if (RandomHelper.Random.NextBool())
newObject.LinearVelocity = Vector3.Zero;
if (RandomHelper.Random.NextBool())
newObject.AngularVelocity = Vector3.Zero;
if (shape is LineShape || shape is HeightField)
{
// Do not move lines or the height field.
newObject.LinearVelocity = Vector3.Zero;
newObject.AngularVelocity = Vector3.Zero;
}
// Create only 1 heightField!
if (shape is HeightField)
{
if (isFirstHeightField)
{
isFirstHeightField = true;
newObject.Pose = new Pose(new Vector3(-BoxSize, -BoxSize, -BoxSize));
}
else
{
currentShape++;
numberOfObjects = 0;
continue;
}
}
// Add collision object to collision domain.
_domain.CollisionObjects.Add(new CollisionObject(newObject));
//co.Type = CollisionObjectType.Trigger;
//co.Name = "Object" + shape.GetType().Name + "_" + i;
}
}
public ConstraintVehicleObject(IServiceLocator services)
{
Name = "Vehicle";
_services = services;
_inputService = services.GetInstance <IInputService>();
_simulation = services.GetInstance <Simulation>();
// Load models for rendering.
var contentManager = services.GetInstance <ContentManager>();
_vehicleModelNode = contentManager.Load <ModelNode>("Car/Car").Clone();
_wheelModelNodes = new ModelNode[4];
_wheelModelNodes[0] = contentManager.Load <ModelNode>("Car/Wheel").Clone();
_wheelModelNodes[1] = _wheelModelNodes[0].Clone();
_wheelModelNodes[2] = _wheelModelNodes[0].Clone();
_wheelModelNodes[3] = _wheelModelNodes[0].Clone();
// Add wheels under the car model node.
_vehicleModelNode.Children.Add(_wheelModelNodes[0]);
_vehicleModelNode.Children.Add(_wheelModelNodes[1]);
_vehicleModelNode.Children.Add(_wheelModelNodes[2]);
_vehicleModelNode.Children.Add(_wheelModelNodes[3]);
// ----- Create the chassis of the car.
// The Vehicle needs a rigid body that represents the chassis. This can be any shape (e.g.
// a simple BoxShape). In this example we will build a convex polyhedron from the car model.
// 1. Extract the vertices from the car model.
// The car model has ~10,000 vertices. It consists of a MeshNode for the glass
// parts and a MeshNode "Car" for the chassis.
var meshNode = _vehicleModelNode.GetDescendants()
.OfType <MeshNode>()
.First(mn => mn.Name == "Car");
var mesh = MeshHelper.ToTriangleMesh(meshNode.Mesh);
// Apply the transformation of the mesh node.
mesh.Transform(meshNode.PoseWorld * Matrix.CreateScale(meshNode.ScaleWorld));
// 2. (Optional) Create simplified convex hull from mesh.
// We could also skip this step and directly create a convex polyhedron from the mesh using
// var chassisShape = new ConvexPolyhedron(mesh.Vertices);
// However, the convex polyhedron would still have 500-600 vertices.
// We can reduce the number of vertices by using the GeometryHelper.
// Create a convex hull for mesh with max. 64 vertices. Additional, shrink the hull by 4 cm.
var convexHull = GeometryHelper.CreateConvexHull(mesh.Vertices, 64, -0.04f);
// 3. Create convex polyhedron shape using the vertices of the convex hull.
var chassisShape = new ConvexPolyhedron(convexHull.Vertices.Select(v => v.Position));
// (Note: Building convex hulls and convex polyhedra are time-consuming. To save loading time
// we should build the shape in the XNA content pipeline. See other DigitalRune Physics
// Samples.)
// The mass properties of the car. We use a mass of 800 kg.
var mass = MassFrame.FromShapeAndMass(chassisShape, Vector3.One, 800, 0.1f, 1);
// Trick: We artificially modify the center of mass of the rigid body. Lowering the center
// of mass makes the car more stable against rolling in tight curves.
// We could also modify mass.Inertia for other effects.
var pose = mass.Pose;
pose.Position.Y -= 0.5f; // Lower the center of mass.
pose.Position.Z = -0.5f; // The center should be below the driver.
// (Note: The car model is not exactly centered.)
mass.Pose = pose;
// Material for the chassis.
var material = new UniformMaterial
{
Restitution = 0.1f,
StaticFriction = 0.2f,
DynamicFriction = 0.2f
};
var chassis = new RigidBody(chassisShape, mass, material)
{
Pose = new Pose(new Vector3(0, 2, 0)), // Start position
UserData = "NoDraw", // (Remove this line to render the collision model.)
};
// ----- Create the vehicle.
Vehicle = new ConstraintVehicle(_simulation, chassis);
// Add 4 wheels.
Vehicle.Wheels.Add(new ConstraintWheel {
Offset = new Vector3(-0.9f, 0.6f, -2.0f), Radius = 0.36f, SuspensionRestLength = 0.55f, MinSuspensionLength = 0.25f, Friction = 2
}); // Front left
Vehicle.Wheels.Add(new ConstraintWheel {
Offset = new Vector3(0.9f, 0.6f, -2.0f), Radius = 0.36f, SuspensionRestLength = 0.55f, MinSuspensionLength = 0.25f, Friction = 2
}); // Front right
Vehicle.Wheels.Add(new ConstraintWheel {
Offset = new Vector3(-0.9f, 0.6f, 0.98f), Radius = 0.36f, SuspensionRestLength = 0.55f, MinSuspensionLength = 0.25f, Friction = 1.8f
}); // Back left
Vehicle.Wheels.Add(new ConstraintWheel {
Offset = new Vector3(0.9f, 0.6f, 0.98f), Radius = 0.36f, SuspensionRestLength = 0.55f, MinSuspensionLength = 0.25f, Friction = 1.8f
}); // Back right
// Vehicles are disabled per default. This way we can create the vehicle and the simulation
// objects are only added when needed.
Vehicle.Enabled = false;
}
public void SimpleTetrahedron()
{
List<Vector3F> points = new List<Vector3F>
{
new Vector3F(0, 0, 0),
new Vector3F(0, 1, 0),
new Vector3F(1, 0, 0),
new Vector3F(0, 0, 1),
};
ConvexPolyhedron convex = new ConvexPolyhedron(points);
// Sample primary directions
Vector3F right = new Vector3F(2, 0, 0);
AssertSupportPointsAreEquivalent(GetSupportPoint(right, points), convex.GetSupportPoint(right), right);
Vector3F left = new Vector3F(-2, 0, 0);
AssertSupportPointsAreEquivalent(GetSupportPoint(left, points), convex.GetSupportPoint(left), left);
Vector3F up = new Vector3F(0, 2, 0);
AssertSupportPointsAreEquivalent(GetSupportPoint(up, points), convex.GetSupportPoint(up), up);
Vector3F down = new Vector3F(0, -2, 0);
AssertSupportPointsAreEquivalent(GetSupportPoint(down, points), convex.GetSupportPoint(down), down);
Vector3F back = new Vector3F(0, 0, 2);
AssertSupportPointsAreEquivalent(GetSupportPoint(back, points), convex.GetSupportPoint(back), back);
Vector3F front = new Vector3F(0, 0, -2);
AssertSupportPointsAreEquivalent(GetSupportPoint(front, points), convex.GetSupportPoint(front), front);
// Sample random directions
for (int i = 0; i < 10; i++)
{
Vector3F direction = RandomHelper.Random.NextVector3F(-1, 1);
Vector3F supportPoint = convex.GetSupportPoint(direction);
Vector3F reference = GetSupportPoint(direction, points);
// The support points can be different, e.g. if a an edge of face is normal to the
// direction. When projected onto the direction both support points must be at equal
// distance.
AssertSupportPointsAreEquivalent(reference, supportPoint, direction);
}
}
public ShapesSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0))
{
Name = "GroundPlane", // Names are not required but helpful for debugging.
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(groundPlane);
// ----- Add a sphere.
Shape sphere = new SphereShape(0.5f);
Simulation.RigidBodies.Add(new RigidBody(sphere));
// ----- Add a box.
BoxShape box = new BoxShape(0.5f, 0.9f, 0.7f);
Simulation.RigidBodies.Add(new RigidBody(box));
// ----- Add a capsule.
CapsuleShape capsule = new CapsuleShape(0.4f, 1.2f);
Simulation.RigidBodies.Add(new RigidBody(capsule));
// ----- Add a cone.
ConeShape cone = new ConeShape(0.5f, 1f);
Simulation.RigidBodies.Add(new RigidBody(cone));
// ----- Add a cylinder.
CylinderShape cylinder = new CylinderShape(0.3f, 1f);
Simulation.RigidBodies.Add(new RigidBody(cylinder));
// ----- Add a convex hull of random points.
ConvexHullOfPoints convexHullOfPoints = new ConvexHullOfPoints();
for (int i = 0; i < 20; i++)
{
convexHullOfPoints.Points.Add(RandomHelper.Random.NextVector3F(-0.5f, 0.5f));
}
Simulation.RigidBodies.Add(new RigidBody(convexHullOfPoints));
// ----- Add a convex polyhedron.
// (A ConvexPolyhedron is similar to the ConvexHullOfPoints. The difference is that
// the points in a ConvexHullOfPoints can be changed at runtime. A ConvexPolyhedron
// cannot be changed at runtime, but it is faster.)
List <Vector3F> points = new List <Vector3F>();
for (int i = 0; i < 20; i++)
{
points.Add(RandomHelper.Random.NextVector3F(-0.7f, 0.7f));
}
ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(points);
Simulation.RigidBodies.Add(new RigidBody(convexPolyhedron));
// ----- Add a composite shape (a table that consists of 5 boxes).
CompositeShape composite = new CompositeShape();
composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(-0.75f, 0.4f, -0.5f))));
composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(0.75f, 0.4f, -0.5f))));
composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(-0.75f, 0.4f, 0.5f))));
composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(0.75f, 0.4f, 0.5f))));
composite.Children.Add(new GeometricObject(new BoxShape(1.8f, 0.1f, 1.1f), new Pose(new Vector3F(0, 0.8f, 0))));
Simulation.RigidBodies.Add(new RigidBody(composite));
// ----- Add a convex hull of multiple shapes.
ConvexHullOfShapes convexHullOfShapes = new ConvexHullOfShapes();
convexHullOfShapes.Children.Add(new GeometricObject(new CylinderShape(0.2f, 0.8f), new Pose(new Vector3F(-0.4f, 0, 0))));
convexHullOfShapes.Children.Add(new GeometricObject(new CylinderShape(0.2f, 0.8f), new Pose(new Vector3F(+0.4f, 0, 0))));
Simulation.RigidBodies.Add(new RigidBody(convexHullOfShapes));
// ----- Add the Minkowski sum of two shapes.
// (The Minkowski sum is a mathematical operation that combines two shapes.
// Here a circle is combined with a sphere. The result is a wheel.)
MinkowskiSumShape minkowskiSum = new MinkowskiSumShape();
minkowskiSum.ObjectA = new GeometricObject(new SphereShape(0.2f), Pose.Identity);
minkowskiSum.ObjectB = new GeometricObject(new CircleShape(0.5f), Pose.Identity);
Simulation.RigidBodies.Add(new RigidBody(minkowskiSum));
// Create another Minkowski sum. (Here a small sphere is added to a box to create a
// box with rounded corners.)
minkowskiSum = new MinkowskiSumShape();
minkowskiSum.ObjectA = new GeometricObject(new SphereShape(0.1f), Pose.Identity);
minkowskiSum.ObjectB = new GeometricObject(new BoxShape(0.2f, 0.5f, 0.8f), Pose.Identity);
Simulation.RigidBodies.Add(new RigidBody(minkowskiSum));
// ----- Add a triangle mesh.
// A triangle mesh could be loaded from a file or built from an XNA model.
// Here we first create a composite shape and convert the shape into a triangle
// mesh. (Any Shape in DigitalRune.Geometry can be converted to a triangle mesh.)
CompositeShape dumbbell = new CompositeShape();
dumbbell.Children.Add(new GeometricObject(new SphereShape(0.4f), new Pose(new Vector3F(0.6f, 0.0f, 0.0f))));
dumbbell.Children.Add(new GeometricObject(new SphereShape(0.4f), new Pose(new Vector3F(-0.6f, 0.0f, 0.0f))));
dumbbell.Children.Add(new GeometricObject(new CylinderShape(0.1f, 0.6f), new Pose(Matrix33F.CreateRotationZ(ConstantsF.PiOver2))));
TriangleMeshShape triangleMeshShape = new TriangleMeshShape(dumbbell.GetMesh(0.01f, 2));
// Optional: We can enable "contact welding". When this flag is enabled, the triangle shape
// precomputes additional internal information for the mesh. The collision detection will
// be able to compute better contacts (e.g. better normal vectors at triangle edges).
// Pro: Collision detection can compute better contact information.
// Con: Contact welding information needs a bit of memory. And the collision detection is
// a bit slower.
triangleMeshShape.EnableContactWelding = true;
// 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 AabbTree <int>();
Simulation.RigidBodies.Add(new RigidBody(triangleMeshShape));
// ----- Set random positions/orientations.
// (Start with the second object. The first object is the ground plane which should
// not be changed.)
for (int i = 1; i < Simulation.RigidBodies.Count; i++)
{
RigidBody body = Simulation.RigidBodies[i];
Vector3F position = RandomHelper.Random.NextVector3F(-3, 3);
position.Y = 3; // Position the objects 3m above ground.
QuaternionF orientation = RandomHelper.Random.NextQuaternionF();
body.Pose = new Pose(position, orientation);
}
}
public void ThreePoints()
{
Vector3F point0 = new Vector3F(1, 1, 1);
Vector3F point1 = new Vector3F(2, 1, 1);
Vector3F point2 = new Vector3F(1, 2, 1);
ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(new[] { point0, point1, point2 });
Assert.AreEqual(3, convexPolyhedron.Vertices.Count);
Assert.AreEqual((point0 + point1 + point2) / 3, convexPolyhedron.InnerPoint);
Assert.AreEqual(new Aabb(new Vector3F(1, 1, 1), new Vector3F(2, 2, 1)), convexPolyhedron.GetAabb(Pose.Identity));
}
public void TwoPoints()
{
Vector3F point0 = new Vector3F(1, 0, 0);
Vector3F point1 = new Vector3F(10, 0, 0);
ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(new[] { point0, point1 });
Assert.AreEqual(2, convexPolyhedron.Vertices.Count);
Assert.AreEqual((point0 + point1) / 2, convexPolyhedron.InnerPoint);
Assert.AreEqual(new Aabb(point0, point1), convexPolyhedron.GetAabb(Pose.Identity));
}
public void RandomConvexPolyhedron()
{
// Use a fixed seed.
RandomHelper.Random = new Random(12345);
// Try polyhedra with 0, 1, 2, ... points.
for (int numberOfPoints = 0; numberOfPoints < 100; numberOfPoints++)
{
List<Vector3F> points = new List<Vector3F>(numberOfPoints);
// Create random polyhedra.
for (int i = 0; i < numberOfPoints; i++)
points.Add(
new Vector3F(
RandomHelper.Random.NextFloat(-10, 10),
RandomHelper.Random.NextFloat(-20, 20),
RandomHelper.Random.NextFloat(-100, 100)));
//var convexHull = GeometryHelper.CreateConvexHull(points);
ConvexPolyhedron convex = new ConvexPolyhedron(points);
// Sample primary directions
Vector3F right = new Vector3F(2, 0, 0);
AssertSupportPointsAreEquivalent(GetSupportPoint(right, points), convex.GetSupportPoint(right), right);
Vector3F left = new Vector3F(-2, 0, 0);
AssertSupportPointsAreEquivalent(GetSupportPoint(left, points), convex.GetSupportPoint(left), left);
Vector3F up = new Vector3F(0, 2, 0);
AssertSupportPointsAreEquivalent(GetSupportPoint(up, points), convex.GetSupportPoint(up), up);
Vector3F down = new Vector3F(0, -2, 0);
AssertSupportPointsAreEquivalent(GetSupportPoint(down, points), convex.GetSupportPoint(down), down);
Vector3F back = new Vector3F(0, 0, 2);
AssertSupportPointsAreEquivalent(GetSupportPoint(back, points), convex.GetSupportPoint(back), back);
Vector3F front = new Vector3F(0, 0, -2);
AssertSupportPointsAreEquivalent(GetSupportPoint(front, points), convex.GetSupportPoint(front), front);
// Sample random directions
for (int i = 0; i < 10; i++)
{
Vector3F direction = RandomHelper.Random.NextVector3F(-1, 1);
if (direction.IsNumericallyZero)
continue;
Vector3F supportPoint = convex.GetSupportPoint(direction);
Vector3F reference = GetSupportPoint(direction, points);
// The support points can be different, e.g. if a an edge of face is normal to the
// direction. When projected onto the direction both support points must be at equal
// distance.
AssertSupportPointsAreEquivalent(reference, supportPoint, direction);
}
}
}
Mesh CreateConvexHullShape(ConvexHullShape shape)
{
ConvexPolyhedron poly = shape.ConvexPolyhedron;
if (poly != null)
{
throw new NotImplementedException();
}
ShapeHull hull = new ShapeHull(shape);
hull.BuildHull(shape.Margin);
UIntArray hullIndices = hull.Indices;
Vector3Array points = hull.Vertices;
int vertexCount = hull.NumIndices;
int faceCount = hull.NumTriangles;
bool index32 = vertexCount > 65536;
Mesh mesh = new Mesh(device, faceCount, vertexCount,
MeshFlags.SystemMemory | (index32 ? MeshFlags.Use32Bit : 0), VertexFormat.Position | VertexFormat.Normal);
SlimDX.DataStream indices = mesh.LockIndexBuffer(LockFlags.Discard);
int i;
if (index32)
{
for (i = 0; i < vertexCount; i++)
{
indices.Write(i);
}
}
else
{
for (i = 0; i < vertexCount; i++)
{
indices.Write((short)i);
}
}
mesh.UnlockIndexBuffer();
SlimDX.DataStream verts = mesh.LockVertexBuffer(LockFlags.Discard);
Vector3 scale = Vector3.Multiply(shape.LocalScaling, 1.0f + shape.Margin);
for (i = 0; i < vertexCount; i += 3)
{
verts.Write(Vector3.Modulate(points[(int)hullIndices[i]], scale));
verts.Position += 12;
verts.Write(Vector3.Modulate(points[(int)hullIndices[i + 1]], scale));
verts.Position += 12;
verts.Write(Vector3.Modulate(points[(int)hullIndices[i + 2]], scale));
verts.Position += 12;
}
mesh.UnlockVertexBuffer();
mesh.ComputeNormals();
shapes.Add(shape, mesh);
return(mesh);
}
/// <summary>
/// Build open influence region with 3 faces for a given edge.
/// </summary>
/// <param name="edge">The given edge.</param>
/// <param name="edges">All existing edges.</param>
/// <param name="ap1">Allowed point 1</param>
/// <param name="ap2">Allowed point 2</param>s
/// <param name="innerPoints">Points that approximately lie inside the influence region (polyhedron).</param>
/// <returns>The polyhedron.</returns>
private ConvexPolyhedron BuildPolyhedron2(Edge edge, Dictionary <Edge, Edge> edges, out int ap1, out int ap2, out List <int> innerPoints)
{
buildPCounter++;
ap1 = -1;
ap2 = -1;
int i = edge.vertex1;
int j = edge.vertex2;
int k = edge.vertex3;
// check if the i and j vertices are chosen correctly
if (Vector3.Dot(Vector3.Cross(PointCloud[k].Position - PointCloud[i].Position, PointCloud[k].Position - PointCloud[j].Position), edge.normal) < 0)
{
i = edge.vertex2;
j = edge.vertex1;
}
// positions of i, j and k
Vector3 ip = PointCloud[i].Position;
Vector3 jp = PointCloud[j].Position;
Vector3 kp = PointCloud[k].Position;
Vector3 n1 = Vector3.Cross(edge.normal, jp - ip).normalized;
Vector3 n3 = (Vector3.Cross(n1 + Mathf.Tan(Mathf.Deg2Rad * regionAngle) * (ip - jp).normalized, edge.normal)).normalized;
Vector3 n2 = Vector3.Cross(edge.normal, n1 + Mathf.Tan(Mathf.Deg2Rad * regionAngle) * (jp - ip).normalized).normalized;
float s = sMult * (float)Math.Max(PointCloud[i].UniformityDegree, PointCloud[j].UniformityDegree) * (PointCloud[i].MinEdge + PointCloud[j].MinEdge) / 2;
ConvexPolyhedron polyhedron = new ConvexPolyhedron();
polyhedron.AddFace(n1, ip);
polyhedron.AddFace(n2, ip);
polyhedron.AddFace(n3, jp);
polyhedron.AddFace(-n1, s * -n1 + (ip + jp) / 2);
polyhedron.AddFace(edge.normal, ip + s * edge.normal); // top face
polyhedron.AddFace(-edge.normal, ip - s * edge.normal); // bottom face -
innerPoints = VoxelSet.GetInnerPoints(polyhedron, false, influenceRegion2, (ip + jp) / 2);
Vector3?pLeft = null, pRight = null;
float minAngleLeft = 181f, minAngleRight = 181f;
foreach (var point in innerPoints)
{
if (point == i || point == j)
{
continue;
}
Edge left = new Edge(point, i, Vector3.back);
Edge right = new Edge(point, j, Vector3.back);
if (edges.ContainsKey(left))
{
float angleLeft = Vector3.Angle(ip - jp, PointCloud[point].Position - ip);
if (angleLeft < minAngleLeft)
{
minAngleLeft = angleLeft;
pLeft = PointCloud[point].Position;
ap1 = point;
}
}
if (edges.ContainsKey(right))
{
float angleRight = Vector3.Angle(jp - ip, PointCloud[point].Position - jp);
if (angleRight < minAngleRight)
{
minAngleRight = angleRight;
pRight = PointCloud[point].Position;
ap2 = point;
}
}
}
ConvexPolyhedron p = new ConvexPolyhedron();
p.AddFace(n1, ip);
if (pLeft == null)
{
p.AddFace(n2, ip);
}
else
{
p.AddFace(Vector3.Cross((Vector3)pLeft - ip, edge.normal).normalized, ip);
}
if (pRight == null)
{
p.AddFace(n3, jp);
}
else
{
p.AddFace(-Vector3.Cross((Vector3)pRight - jp, edge.normal).normalized, jp);
}
polyhedron.AddFace(-n1, s * -n1 + (ip + jp) / 2);
return(p);
}
