public void EmptyConvexHullOfPoints()
{
ConvexHullOfPoints convexHullOfPoints = new ConvexHullOfPoints(Enumerable.Empty<Vector3F>());
Assert.AreEqual(0, convexHullOfPoints.Points.Count);
Assert.AreEqual(Vector3F.Zero, convexHullOfPoints.InnerPoint);
Assert.AreEqual(new Aabb(), convexHullOfPoints.GetAabb(Pose.Identity));
}
public void OnePoint()
{
Vector3F point = new Vector3F(1, 0, 0);
ConvexHullOfPoints convexHullOfPoints = new ConvexHullOfPoints(new[] { point });
Assert.AreEqual(1, convexHullOfPoints.Points.Count);
Assert.AreEqual(point, convexHullOfPoints.InnerPoint);
Assert.AreEqual(new Aabb(point, point), convexHullOfPoints.GetAabb(Pose.Identity));
}
public void EmptyConvexHullOfPoints()
{
ConvexHullOfPoints convexHullOfPoints = new ConvexHullOfPoints(Enumerable.Empty <Vector3>());
Assert.AreEqual(0, convexHullOfPoints.Points.Count);
Assert.AreEqual(Vector3.Zero, convexHullOfPoints.InnerPoint);
Assert.AreEqual(new Aabb(), convexHullOfPoints.GetAabb(Pose.Identity));
}
public void OnePoint()
{
Vector3 point = new Vector3(1, 0, 0);
ConvexHullOfPoints convexHullOfPoints = new ConvexHullOfPoints(new[] { point });
Assert.AreEqual(1, convexHullOfPoints.Points.Count);
Assert.AreEqual(point, convexHullOfPoints.InnerPoint);
Assert.AreEqual(new Aabb(point, point), convexHullOfPoints.GetAabb(Pose.Identity));
}
public void TwoPoints()
{
Vector3 point0 = new Vector3(1, 0, 0);
Vector3 point1 = new Vector3(10, 0, 0);
ConvexHullOfPoints convexHullOfPoints = new ConvexHullOfPoints(new[] { point0, point1 });
Assert.AreEqual(2, convexHullOfPoints.Points.Count);
Assert.AreEqual((point0 + point1) / 2, convexHullOfPoints.InnerPoint);
Assert.AreEqual(new Aabb(point0, point1), convexHullOfPoints.GetAabb(Pose.Identity));
}
public void ConvexHullOfPointsTest()
{
var s = new BoxShape(1, 2, 3);
var v0 = s.GetVolume(0.001f, 10);
var s1 = new ConvexHullOfPoints(s.GetMesh(0.1f, 1).Vertices);
var v1 = s1.GetVolume(0.001f, 10);
Assert.IsTrue(Numeric.AreEqual(v0, v1, 0.01f * (1 + v0))); // 1% error is allowed.
}
public void RandomConvexHullOfPoints()
{
// 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)));
}
ConvexHullOfPoints convex = new ConvexHullOfPoints(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 ThreePoints()
{
Vector3 point0 = new Vector3(1, 1, 1);
Vector3 point1 = new Vector3(2, 1, 1);
Vector3 point2 = new Vector3(1, 2, 1);
ConvexHullOfPoints convexHullOfPoints = new ConvexHullOfPoints(new[] { point0, point1, point2 });
Assert.AreEqual(3, convexHullOfPoints.Points.Count);
Assert.AreEqual((point0 + point1 + point2) / 3, convexHullOfPoints.InnerPoint);
Assert.AreEqual(new Aabb(new Vector3(1, 1, 1), new Vector3(2, 2, 1)), convexHullOfPoints.GetAabb(Pose.Identity));
}
public void GetSupportPoint()
{
ConvexHullOfPoints emptyConvexHullOfPoints = new ConvexHullOfPoints(Enumerable.Empty<Vector3F>());
Assert.AreEqual(new Vector3F(0, 0, 0), emptyConvexHullOfPoints.GetSupportPoint(new Vector3F(1, 0, 0)));
Assert.AreEqual(new Vector3F(0, 0, 0), emptyConvexHullOfPoints.GetSupportPoint(new Vector3F(0, 1, 0)));
Assert.AreEqual(new Vector3F(0, 0, 0), emptyConvexHullOfPoints.GetSupportPoint(new Vector3F(0, 0, 1)));
Assert.AreEqual(new Vector3F(0, 0, 0), emptyConvexHullOfPoints.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.AreEqual(p0, new ConvexHullOfPoints(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3F(1, 0, 0)));
Assert.AreEqual(p2, new ConvexHullOfPoints(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3F(0, 1, 0)));
Assert.AreEqual(p2, new ConvexHullOfPoints(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3F(0, 0, -1)));
Assert.AreEqual(p1, new ConvexHullOfPoints(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3F(-1, 0, 1)));
}
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),
};
ConvexHullOfPoints convex = new ConvexHullOfPoints(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);
}
}
public void GetSupportPoint()
{
ConvexHullOfPoints emptyConvexHullOfPoints = new ConvexHullOfPoints(Enumerable.Empty <Vector3>());
Assert.AreEqual(new Vector3(0, 0, 0), emptyConvexHullOfPoints.GetSupportPoint(new Vector3(1, 0, 0)));
Assert.AreEqual(new Vector3(0, 0, 0), emptyConvexHullOfPoints.GetSupportPoint(new Vector3(0, 1, 0)));
Assert.AreEqual(new Vector3(0, 0, 0), emptyConvexHullOfPoints.GetSupportPoint(new Vector3(0, 0, 1)));
Assert.AreEqual(new Vector3(0, 0, 0), emptyConvexHullOfPoints.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.AreEqual(p0, new ConvexHullOfPoints(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3(1, 0, 0)));
Assert.AreEqual(p2, new ConvexHullOfPoints(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3(0, 1, 0)));
Assert.AreEqual(p2, new ConvexHullOfPoints(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3(0, 0, -1)));
Assert.AreEqual(p1, new ConvexHullOfPoints(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3(-1, 0, 1)));
}
public void Clone()
{
ConvexHullOfPoints convexHullOfPoints = new ConvexHullOfPoints(
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),
});
ConvexHullOfPoints clone = convexHullOfPoints.Clone() as ConvexHullOfPoints;
Assert.IsNotNull(clone);
for (int i = 0; i < clone.Points.Count; i++)
Assert.AreEqual(convexHullOfPoints.Points[i], clone.Points[i]);
Assert.AreEqual(convexHullOfPoints.GetAabb(Pose.Identity).Minimum, clone.GetAabb(Pose.Identity).Minimum);
Assert.AreEqual(convexHullOfPoints.GetAabb(Pose.Identity).Maximum, clone.GetAabb(Pose.Identity).Maximum);
}
public void ApproximateAabbMass()
{
var s = new ConvexHullOfPoints(new[] { new Vector3F(1, 1, 1), new Vector3F(2, 4, 6) });
float m0;
Vector3F com0;
Matrix33F i0;
MassHelper.GetMass(s, new Vector3F(1.2f, 2.1f, 0.6f), 0.7f, true, 0.001f, 0, out m0, out com0, out i0);
var s2 = new TransformedShape(new GeometricObject(new BoxShape(1, 3, 5), new Pose(new Vector3F(1.5f, 2.5f, 3.5f))));
float m1;
Vector3F com1;
Matrix33F i1;
MassHelper.GetMass(s2, new Vector3F(1.2f, 2.1f, 0.6f), 0.7f, true, 0.001f, 0, out m1, out com1, out i1);
const float e = 0.0001f;
Assert.IsTrue(Numeric.AreEqual(m0, m1, e * (1 + m0)));
Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com1, e * (1 + com0.Length)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0, i1, e * (1 + i0.Trace)));
}
public void Clone()
{
ConvexHullOfPoints convexHullOfPoints = new ConvexHullOfPoints(
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),
});
ConvexHullOfPoints clone = convexHullOfPoints.Clone() as ConvexHullOfPoints;
Assert.IsNotNull(clone);
for (int i = 0; i < clone.Points.Count; i++)
{
Assert.AreEqual(convexHullOfPoints.Points[i], clone.Points[i]);
}
Assert.AreEqual(convexHullOfPoints.GetAabb(Pose.Identity).Minimum, clone.GetAabb(Pose.Identity).Minimum);
Assert.AreEqual(convexHullOfPoints.GetAabb(Pose.Identity).Maximum, clone.GetAabb(Pose.Identity).Maximum);
}
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),
};
ConvexHullOfPoints convex = new ConvexHullOfPoints(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);
}
}
// 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 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);
}
}
private Ragdoll CreateRagdoll(MeshNode meshNode)
{
var mesh = meshNode.Mesh;
var skeleton = mesh.Skeleton;
// Extract the vertices from the mesh sorted per bone.
var verticesPerBone = new List <Vector3F> [skeleton.NumberOfBones];
// Also get the AABB of the model.
Aabb?aabb = null;
foreach (var submesh in mesh.Submeshes)
{
// Get vertex element info.
var vertexDeclaration = submesh.VertexBuffer.VertexDeclaration;
var vertexElements = vertexDeclaration.GetVertexElements();
// Get the vertex positions.
var positionElement = vertexElements.First(e => e.VertexElementUsage == VertexElementUsage.Position);
if (positionElement.VertexElementFormat != VertexElementFormat.Vector3)
{
throw new NotSupportedException("For vertex positions only VertexElementFormat.Vector3 is supported.");
}
var positions = new Vector3[submesh.VertexCount];
submesh.VertexBuffer.GetData(
submesh.StartVertex * vertexDeclaration.VertexStride + positionElement.Offset,
positions,
0,
submesh.VertexCount,
vertexDeclaration.VertexStride);
// Get the bone indices.
var boneIndexElement = vertexElements.First(e => e.VertexElementUsage == VertexElementUsage.BlendIndices);
if (boneIndexElement.VertexElementFormat != VertexElementFormat.Byte4)
{
throw new NotSupportedException();
}
var boneIndicesArray = new Byte4[submesh.VertexCount];
submesh.VertexBuffer.GetData(
submesh.StartVertex * vertexDeclaration.VertexStride + boneIndexElement.Offset,
boneIndicesArray,
0,
submesh.VertexCount,
vertexDeclaration.VertexStride);
// Get the bone weights.
var boneWeightElement = vertexElements.First(e => e.VertexElementUsage == VertexElementUsage.BlendWeight);
if (boneWeightElement.VertexElementFormat != VertexElementFormat.Vector4)
{
throw new NotSupportedException();
}
var boneWeightsArray = new Vector4[submesh.VertexCount];
submesh.VertexBuffer.GetData(
submesh.StartVertex * vertexDeclaration.VertexStride + boneWeightElement.Offset,
boneWeightsArray,
0,
submesh.VertexCount,
vertexDeclaration.VertexStride);
// Sort the vertices per bone.
for (int i = 0; i < submesh.VertexCount; i++)
{
var vertex = (Vector3F)positions[i];
// Here, we only check the first bone index. We could also check the
// bone weights to add the vertex to all bone vertex lists where the
// weight is high...
Vector4 boneIndices = boneIndicesArray[i].ToVector4();
//Vector4 boneWeights = boneWeightsArray[i];
int boneIndex = (int)boneIndices.X;
if (verticesPerBone[boneIndex] == null)
{
verticesPerBone[boneIndex] = new List <Vector3F>();
}
verticesPerBone[boneIndex].Add(vertex);
// Add vertex to AABB.
if (aabb == null)
{
aabb = new Aabb(vertex, vertex);
}
else
{
aabb.Value.Grow(vertex);
}
}
}
// We create a body for each bone with vertices.
int numberOfBodies = verticesPerBone.Count(vertices => vertices != null);
// We use the same mass properties for all bodies. This is not realistic but more stable
// because large mass differences or thin bodies (arms!) are less stable.
// We use the mass properties of sphere proportional to the size of the model.
const float totalMass = 80; // The total mass of the ragdoll.
var massFrame = MassFrame.FromShapeAndMass(new SphereShape(aabb.Value.Extent.Y / 8), Vector3F.One, totalMass / numberOfBodies, 0.1f, 1);
var material = new UniformMaterial();
Ragdoll ragdoll = new Ragdoll();
for (int boneIndex = 0; boneIndex < skeleton.NumberOfBones; boneIndex++)
{
var boneVertices = verticesPerBone[boneIndex];
if (boneVertices != null)
{
var bindPoseInverse = (Pose)skeleton.GetBindPoseAbsoluteInverse(boneIndex);
// Compute bounding capsule.
//float radius;
//float height;
//Pose pose;
//GeometryHelper.ComputeBoundingCapsule(boneVertices, out radius, out height, out pose);
//Shape shape = new TransformedShape(new GeometricObject(new CapsuleShape(radius, height), pose));
// Compute convex hull.
var points = GeometryHelper.CreateConvexHull(boneVertices, 32, 0).ToTriangleMesh().Vertices;
Shape shape = new ConvexHullOfPoints(points.Count > 0 ? points : boneVertices);
ragdoll.Bodies.Add(new RigidBody(shape, massFrame, material));
ragdoll.BodyOffsets.Add(bindPoseInverse);
}
else
{
ragdoll.Bodies.Add(null);
ragdoll.BodyOffsets.Add(Pose.Identity);
}
}
return(ragdoll);
}
public void TwoPoints()
{
Vector3F point0 = new Vector3F(1, 0, 0);
Vector3F point1 = new Vector3F(10, 0, 0);
ConvexHullOfPoints convexHullOfPoints = new ConvexHullOfPoints(new[] { point0, point1 });
Assert.AreEqual(2, convexHullOfPoints.Points.Count);
Assert.AreEqual((point0 + point1) / 2, convexHullOfPoints.InnerPoint);
Assert.AreEqual(new Aabb(point0, point1), convexHullOfPoints.GetAabb(Pose.Identity));
}
public void RandomConvexHullOfPoints()
{
// 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)));
ConvexHullOfPoints convex = new ConvexHullOfPoints(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);
}
}
}
// Creates a lot of random objects.
private void CreateRandomObjects()
{
var random = new Random(12345);
for (int i = 0; i < NumberOfObjects; i++)
{
// Randomly choose a shape.
Shape randomShape;
switch (random.Next(0, 7))
{
case 0:
// Box
randomShape = new BoxShape(ObjectSize, ObjectSize * 2, ObjectSize * 3);
break;
case 1:
// Capsule
randomShape = new CapsuleShape(0.3f * ObjectSize, 2 * ObjectSize);
break;
case 2:
// Cone
randomShape = new ConeShape(1 * ObjectSize, 2 * ObjectSize);
break;
case 3:
// Cylinder
randomShape = new CylinderShape(0.4f * ObjectSize, 2 * ObjectSize);
break;
case 4:
// Sphere
randomShape = 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));
randomShape = 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))));
randomShape = composite;
break;
default:
Trace.Fail("Ups, we shouldn't land here :-(");
randomShape = new SphereShape();
break;
}
// 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 = randomShape,
LinearVelocity = random.NextQuaternion().Rotate(new Vector3(MaxLinearVelocity, 0, 0)),
AngularVelocity = random.NextQuaternion().Rotate(Vector3.Forward)
* RandomHelper.Random.NextFloat(0, MaxAngularVelocity),
};
// Add collision object to collision domain.
_domain.CollisionObjects.Add(new CollisionObject(newObject));
// We will collect a few statistics for debugging.
Profiler.SetFormat("NumObjects", 1, "The total number of objects.");
Profiler.SetFormat("NumObjectPairs", 1, "The number of objects pairs, which have to be checked.");
Profiler.SetFormat("BroadPhasePairs", 1, "The number of overlaps reported by the broad phase.");
Profiler.SetFormat("ContactSetCount", 1, "The number of actual collisions.");
}
}
public void ThreePoints()
{
Vector3F point0 = new Vector3F(1, 1, 1);
Vector3F point1 = new Vector3F(2, 1, 1);
Vector3F point2 = new Vector3F(1, 2, 1);
ConvexHullOfPoints convexHullOfPoints = new ConvexHullOfPoints(new[] { point0, point1, point2 });
Assert.AreEqual(3, convexHullOfPoints.Points.Count);
Assert.AreEqual((point0 + point1 + point2) / 3, convexHullOfPoints.InnerPoint);
Assert.AreEqual(new Aabb(new Vector3F(1, 1, 1), new Vector3F(2, 2, 1)), convexHullOfPoints.GetAabb(Pose.Identity));
}
