public CollisionDetectionSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
SampleFramework.IsMouseVisible = false;
GraphicsScreen.ClearBackground = true;
GraphicsScreen.BackgroundColor = Color.CornflowerBlue;
SetCamera(new Vector3F(0, 1, 10), 0, 0);
// ----- Initialize collision detection and create objects.
// Create a geometric object with a box shape.
// Position it on the left with an arbitrary rotation.
var geometricObjectA = new GeometricObject(
new BoxShape(1, 2, 3),
new Pose(new Vector3F(-2, -1, 0), Matrix33F.CreateRotationZ(0.1f)));
// Create a geometric object with a capsule shape.
// Position it on the right with an arbitrary rotation.
var geometricObjectB = new GeometricObject(
new CapsuleShape(1, 3),
new Pose(new Vector3F(2, -1, 0), Matrix33F.CreateRotationZ(-0.2f)));
// Create a geometric object with a complex shape that is the convex hull of
// a circle and a rectangle. Position it on the top with an arbitrary rotation.
// (A ConvexHullOfShapes is a collection of different shapes with different
// positions and orientations. The ConvexHullOfShapes combines these shapes
// into a single shape by building their convex hull.)
var complexShape = new ConvexHullOfShapes();
complexShape.Children.Add(new GeometricObject(new RectangleShape(1, 1), new Pose(new Vector3F(0, 0, 1))));
complexShape.Children.Add(new GeometricObject(new CircleShape(1), new Pose(new Vector3F(0, 0, -1))));
var geometricObjectC = new GeometricObject(
complexShape,
new Pose(new Vector3F(0, 2, 0), QuaternionF.CreateRotation(Vector3F.UnitZ, new Vector3F(1, 1, 1))));
// Create collision objects for the geometric objects.
// (A collision object is just a wrapper around the geometric object that
// stores additional information that is required by the collision detection.)
_collisionObjectA = new CollisionObject(geometricObjectA);
_collisionObjectB = new CollisionObject(geometricObjectB);
_collisionObjectC = new CollisionObject(geometricObjectC);
// Create a collision detection.
// (The CollisionDetection stores general parameters and it can be used to
// perform closest-point and contact queries.)
_collisionDetection = new CollisionDetection();
// Create a new collision domain and add the collision objects.
// (A CollisionDomain manages multiple collision objects. It improves the
// performance of contact queries by reusing results of the last frame.)
_domain = new CollisionDomain(_collisionDetection);
_domain.CollisionObjects.Add(_collisionObjectA);
_domain.CollisionObjects.Add(_collisionObjectB);
_domain.CollisionObjects.Add(_collisionObjectC);
}
public ContinuousCollisionDetectionSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
GraphicsScreen.ClearBackground = true;
SetCamera(new Vector3F(0, 1, 10), 0, 0);
// ----- Initialize collision detection and create objects.
// Create a geometric object with a capsule shape.
// Position it on the top with an arbitrary rotation.
_startPoseA = new Pose(new Vector3F(0, 2, 0), Matrix33F.CreateRotationZ(0.1f));
var geometricObjectA = new GeometricObject(new CapsuleShape(0.2f, 1), _startPoseA);
_collisionObjectA = new CollisionObject(geometricObjectA);
// Object A moves to the bottom of the screen.
_targetPoseA = new Pose(new Vector3F(0, -2, 0), Matrix33F.CreateRotationZ(0.63f));
// Create a geometric object with a composite shape.
// Position it on the left with an arbitrary rotation.
_startPoseB = new Pose(new Vector3F(-3, -1, 0), Matrix33F.CreateRotationZ(0.2f));
var composite = new CompositeShape();
composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 1, 0.1f), new Pose(new Vector3F(-0.75f, 0.5f, -0.5f))));
composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 1, 0.1f), new Pose(new Vector3F(0.75f, 0.5f, -0.5f))));
composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 1, 0.1f), new Pose(new Vector3F(-0.75f, 0.5f, 0.5f))));
composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 1, 0.1f), new Pose(new Vector3F(0.75f, 0.5f, 0.5f))));
composite.Children.Add(new GeometricObject(new BoxShape(1.8f, 0.1f, 1.1f), new Pose(new Vector3F(0, 1f, 0))));
var geometricObjectB = new GeometricObject(composite, _startPoseB);
// Object B moves to the left of the screen.
_targetPoseB = new Pose(new Vector3F(3, -1, 0), Matrix33F.CreateRotationZ(0.3f));
// Create collision objects for the geometric objects.
// (A collision object is just a wrapper around the geometric object that stores additional
// information that is required by the collision detection.)
_collisionObjectA = new CollisionObject(geometricObjectA);
_collisionObjectB = new CollisionObject(geometricObjectB);
// Create a collision detection.
// (The CollisionDetection stores general parameters and it can be used to perform
// closest-point and contact queries.)
_collisionDetection = new CollisionDetection();
}
public CollisionAlgorithmMatrix(CollisionDetection collisionDetection)
{
// Initialize with dummy collision algorithms.
var noAlgo = new NoCollisionAlgorithm(collisionDetection); // Definitely no collision wanted.
var infiniteAlgo = new InfiniteShapeAlgorithm(collisionDetection); // Returns always a collision.
// Build default configuration:
var gjk = new Gjk(collisionDetection);
var gjkBoxAlgorithm = new CombinedCollisionAlgorithm(collisionDetection, gjk, new BoxBoxAlgorithm(collisionDetection));
var gjkMprAlgorithm = new CombinedCollisionAlgorithm(collisionDetection, gjk, new MinkowskiPortalRefinement(collisionDetection));
var gjkTriTriAlgorithm = new CombinedCollisionAlgorithm(collisionDetection, gjk, new TriangleTriangleAlgorithm(collisionDetection));
BoxSphereAlgorithm boxSphereAlgorithm = new BoxSphereAlgorithm(collisionDetection);
CompositeShapeAlgorithm compositeAlgorithm = new CompositeShapeAlgorithm(collisionDetection);
HeightFieldAlgorithm heightFieldAlgorithm = new HeightFieldAlgorithm(collisionDetection);
LineAlgorithm lineAlgorithm = new LineAlgorithm(collisionDetection);
PlaneBoxAlgorithm planeBoxAlgorithm = new PlaneBoxAlgorithm(collisionDetection);
PlaneConvexAlgorithm planeConvexAlgorithm = new PlaneConvexAlgorithm(collisionDetection);
PlaneRayAlgorithm planeRayAlgorithm = new PlaneRayAlgorithm(collisionDetection);
PlaneSphereAlgorithm planeSphereAlgorithm = new PlaneSphereAlgorithm(collisionDetection);
RayBoxAlgorithm rayBoxAlgorithm = new RayBoxAlgorithm(collisionDetection);
RayConvexAlgorithm rayConvexAlgorithm = new RayConvexAlgorithm(collisionDetection);
RaySphereAlgorithm raySphereAlgorithm = new RaySphereAlgorithm(collisionDetection);
RayTriangleAlgorithm rayTriangleAlgorithm = new RayTriangleAlgorithm(collisionDetection);
SphereSphereAlgorithm sphereSphereAlgorithm = new SphereSphereAlgorithm(collisionDetection);
TransformedShapeAlgorithm transformedShapeAlgorithm = new TransformedShapeAlgorithm(collisionDetection);
TriangleMeshAlgorithm triangleMeshAlgorithm = new TriangleMeshAlgorithm(collisionDetection);
RayCompositeAlgorithm rayCompositeAlgorithm = new RayCompositeAlgorithm(collisionDetection);
RayTriangleMeshAlgorithm rayTriangleMeshAlgorithm = new RayTriangleMeshAlgorithm(collisionDetection);
RayHeightFieldAlgorithm rayHeightFieldAlgorithm = new RayHeightFieldAlgorithm(collisionDetection);
this[typeof(PointShape), typeof(PointShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(LineShape)] = lineAlgorithm;
this[typeof(PointShape), typeof(RayShape)] = rayConvexAlgorithm;
this[typeof(PointShape), typeof(LineSegmentShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(TriangleShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(RectangleShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(BoxShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(EmptyShape)] = noAlgo;
this[typeof(PointShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(PointShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(PointShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(PointShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(PointShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(PointShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(LineShape), typeof(LineShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(RayShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(LineSegmentShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(TriangleShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(RectangleShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(BoxShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(ConvexShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(ScaledConvexShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(CircleShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(SphereShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(CapsuleShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(ConeShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(CylinderShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(EmptyShape)] = noAlgo;
this[typeof(LineShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(LineShape), typeof(PlaneShape)] = noAlgo;
this[typeof(LineShape), typeof(HeightField)] = noAlgo;
this[typeof(LineShape), typeof(TriangleMeshShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(TransformedShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(CompositeShape)] = lineAlgorithm;
this[typeof(RayShape), typeof(RayShape)] = noAlgo;
this[typeof(RayShape), typeof(LineSegmentShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(TriangleShape)] = rayTriangleAlgorithm;
this[typeof(RayShape), typeof(RectangleShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(BoxShape)] = rayBoxAlgorithm;
this[typeof(RayShape), typeof(ConvexShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(ScaledConvexShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(CircleShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(SphereShape)] = raySphereAlgorithm;
this[typeof(RayShape), typeof(CapsuleShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(ConeShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(CylinderShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(EmptyShape)] = noAlgo;
this[typeof(RayShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(RayShape), typeof(PlaneShape)] = planeRayAlgorithm;
this[typeof(RayShape), typeof(HeightField)] = rayHeightFieldAlgorithm;
this[typeof(RayShape), typeof(TriangleMeshShape)] = rayTriangleMeshAlgorithm;
this[typeof(RayShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(RayShape), typeof(CompositeShape)] = rayCompositeAlgorithm;
this[typeof(LineSegmentShape), typeof(LineSegmentShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(TriangleShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(RectangleShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(BoxShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(EmptyShape)] = noAlgo;
this[typeof(LineSegmentShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(LineSegmentShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(LineSegmentShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(LineSegmentShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(LineSegmentShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(LineSegmentShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(TriangleShape), typeof(TriangleShape)] = gjkTriTriAlgorithm;
this[typeof(TriangleShape), typeof(RectangleShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(BoxShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(EmptyShape)] = noAlgo;
this[typeof(TriangleShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(TriangleShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(TriangleShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(TriangleShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(TriangleShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(TriangleShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(RectangleShape), typeof(RectangleShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(BoxShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(EmptyShape)] = noAlgo;
this[typeof(RectangleShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(RectangleShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(RectangleShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(RectangleShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(RectangleShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(RectangleShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(BoxShape), typeof(BoxShape)] = gjkBoxAlgorithm;
this[typeof(BoxShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(SphereShape)] = boxSphereAlgorithm;
this[typeof(BoxShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(EmptyShape)] = noAlgo;
this[typeof(BoxShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(BoxShape), typeof(PlaneShape)] = planeBoxAlgorithm;
this[typeof(BoxShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(BoxShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(BoxShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(BoxShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(ConvexShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(EmptyShape)] = noAlgo;
this[typeof(ConvexShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(ConvexShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(ConvexShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(ConvexShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(ConvexShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(ConvexShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(ScaledConvexShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(EmptyShape)] = noAlgo;
this[typeof(ScaledConvexShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(ScaledConvexShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(ScaledConvexShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(ScaledConvexShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(ScaledConvexShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(ScaledConvexShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(CircleShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(EmptyShape)] = noAlgo;
this[typeof(CircleShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(CircleShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(CircleShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(CircleShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(CircleShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(CircleShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(SphereShape), typeof(SphereShape)] = sphereSphereAlgorithm;
this[typeof(SphereShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(SphereShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(SphereShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(SphereShape), typeof(EmptyShape)] = noAlgo;
this[typeof(SphereShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(SphereShape), typeof(PlaneShape)] = planeSphereAlgorithm;
this[typeof(SphereShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(SphereShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(SphereShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(SphereShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(CapsuleShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(CapsuleShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(CapsuleShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(CapsuleShape), typeof(EmptyShape)] = noAlgo;
this[typeof(CapsuleShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(CapsuleShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(CapsuleShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(CapsuleShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(CapsuleShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(CapsuleShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(ConeShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(ConeShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(ConeShape), typeof(EmptyShape)] = noAlgo;
this[typeof(ConeShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(ConeShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(ConeShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(ConeShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(ConeShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(ConeShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(CylinderShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(CylinderShape), typeof(EmptyShape)] = noAlgo;
this[typeof(CylinderShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(CylinderShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(CylinderShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(CylinderShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(CylinderShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(CylinderShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(EmptyShape), typeof(EmptyShape)] = noAlgo;
this[typeof(EmptyShape), typeof(InfiniteShape)] = noAlgo; // No collision between Empty and Infinite.
this[typeof(EmptyShape), typeof(PlaneShape)] = noAlgo;
this[typeof(EmptyShape), typeof(HeightField)] = noAlgo;
this[typeof(EmptyShape), typeof(TriangleMeshShape)] = noAlgo;
this[typeof(EmptyShape), typeof(TransformedShape)] = noAlgo;
this[typeof(EmptyShape), typeof(CompositeShape)] = noAlgo;
this[typeof(InfiniteShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(PlaneShape)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(HeightField)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(TriangleMeshShape)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(TransformedShape)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(CompositeShape)] = infiniteAlgo;
this[typeof(PlaneShape), typeof(PlaneShape)] = noAlgo;
this[typeof(PlaneShape), typeof(HeightField)] = noAlgo;
this[typeof(PlaneShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(PlaneShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(PlaneShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(HeightField), typeof(HeightField)] = noAlgo;
// We could also call triangleMeshAlgorithm. But since HeightField has usually larger parts it
// is better to call the heightFieldAlgorithm. The heightFieldAlgorithm will cull all but a
// few height field cells very quickly.
this[typeof(HeightField), typeof(TriangleMeshShape)] = heightFieldAlgorithm;
this[typeof(HeightField), typeof(TransformedShape)] = transformedShapeAlgorithm;
// Same as for triangle meshes: Call height field algorithm first.
this[typeof(HeightField), typeof(CompositeShape)] = heightFieldAlgorithm;
this[typeof(TriangleMeshShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(TriangleMeshShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(TriangleMeshShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(TransformedShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(TransformedShape), typeof(CompositeShape)] = transformedShapeAlgorithm;
this[typeof(CompositeShape), typeof(CompositeShape)] = compositeAlgorithm;
}
public void CreateBoundingShape()
{
int numberOfBoxes = 0;
int numberOfSpheres = 0;
const int numberOfTests = 100;
RandomHelper.Random = new Random(727);
for (int test = 0; test < numberOfTests; test++)
{
// Fill list with a random number of random points.
int numberOfPoints = RandomHelper.Random.NextInteger(2, 100);
List<Vector3F> points = new List<Vector3F>();
for (int i = 0; i < numberOfPoints; i++)
points.Add(RandomHelper.Random.NextVector3F(-10, 100));
Shape shape = GeometryHelper.CreateBoundingShape(points);
GeometricObject geometry = new GeometricObject(shape);
CollisionObject boundingObject = new CollisionObject(geometry);
if (shape is BoxShape)
numberOfBoxes++;
if (shape is SphereShape)
numberOfSpheres++;
if (((TransformedShape)shape).Child.Shape is BoxShape)
numberOfBoxes++;
else
numberOfSpheres++;
Assert.IsNotNull(shape);
var cd = new CollisionDetection();
// Test if all points are in the bounding shape.
for (int i = 0; i < numberOfPoints; i++)
{
var point = points[i];
// Test against a sphere around the point. Some points are exactly on the surface
// and are very sensitive to tiny numerical errors.
var pointGeometry = new GeometricObject(new SphereShape(Numeric.EpsilonF * 10), new Pose(point));
Assert.IsTrue(cd.HaveContact(new CollisionObject(pointGeometry), boundingObject));
}
}
Console.WriteLine("ShapeHelper.CreateBoundingShape: Number of Boxes : Number of Spheres = " + numberOfBoxes + " : " + numberOfSpheres);
}
public void ComputeCapsuleWithRandomPoints()
{
float height;
float radius;
Pose pose;
const int numberOfTests = 100;
RandomHelper.Random = new Random(377);
for (int test = 0; test < numberOfTests; test++)
{
// Fill list with a random number of random points.
int numberOfPoints = RandomHelper.Random.NextInteger(2, 100);
List<Vector3F> points = new List<Vector3F>();
for (int i = 0; i < numberOfPoints; i++)
points.Add(RandomHelper.Random.NextVector3F(-10, 100));
GeometryHelper.ComputeBoundingCapsule(points, out radius, out height, out pose);
// Check if sphere can be valid.
Assert.IsTrue(radius >= 0);
Assert.IsTrue(height >= 2 * radius);
Assert.IsTrue(!float.IsNaN(pose.Position.Length));
Assert.IsTrue(pose.Orientation.IsRotation);
// Test if all points are in the shape.
var cd = new CollisionDetection();
GeometricObject geometry = new GeometricObject(new CapsuleShape(radius, height), pose);
CollisionObject boundingObject = new CollisionObject(geometry);
// Test if all points are in the bounding shape.
for (int i = 0; i < numberOfPoints; i++)
{
var point = points[i];
// Test against a sphere around the point. Some points are exactly on the surface
// and are very sensitive to tiny numerical errors.
var pointGeometry = new GeometricObject(new SphereShape(Numeric.EpsilonF * (height + 1)), new Pose(point));
Assert.IsTrue(cd.HaveContact(new CollisionObject(pointGeometry), boundingObject));
}
}
}
public void ComputeBoundingShapeCenteredSphere()
{
RandomHelper.Random = new Random(123);
var radius = new Vector3F(3, 0, 0);
var points = new List<Vector3F>
{
new Vector3F(3, 0, 0), new Vector3F(-3, 0, 0),
new Vector3F(0, 3, 0), new Vector3F(0, -3, 0),
new Vector3F(0, 0, 3), new Vector3F(0, 0, -3),
};
for (int i = 0; i < 40; i++)
points.Add(RandomHelper.Random.NextQuaternionF().Rotate(radius));
var shape = GeometryHelper.CreateBoundingShape(points);
SphereShape s = (SphereShape)shape;
Assert.IsTrue(Numeric.AreEqual(3, s.Radius));
var cd = new CollisionDetection();
GeometricObject geometry = new GeometricObject(shape);
CollisionObject boundingObject = new CollisionObject(geometry);
// Test if all points are in the bounding shape.
for (int i = 0; i < points.Count; i++)
{
var point = points[i];
// Test against a sphere around the point. Some points are exactly on the surface
// and are very sensitive to tiny numerical errors.
var pointGeometry = new GeometricObject(new SphereShape(Numeric.EpsilonF * 10), new Pose(point));
Assert.IsTrue(cd.HaveContact(new CollisionObject(pointGeometry), boundingObject));
}
}
public CollisionAlgorithmMatrix(CollisionDetection collisionDetection)
{
// Initialize with dummy collision algorithms.
var noAlgo = new NoCollisionAlgorithm(collisionDetection); // Definitely no collision wanted.
var infiniteAlgo = new InfiniteShapeAlgorithm(collisionDetection); // Returns always a collision.
// Build default configuration:
var gjk = new Gjk(collisionDetection);
var gjkBoxAlgorithm = new CombinedCollisionAlgorithm(collisionDetection, gjk, new BoxBoxAlgorithm(collisionDetection));
var gjkMprAlgorithm = new CombinedCollisionAlgorithm(collisionDetection, gjk, new MinkowskiPortalRefinement(collisionDetection));
var gjkTriTriAlgorithm = new CombinedCollisionAlgorithm(collisionDetection, gjk, new TriangleTriangleAlgorithm(collisionDetection));
BoxSphereAlgorithm boxSphereAlgorithm = new BoxSphereAlgorithm(collisionDetection);
CompositeShapeAlgorithm compositeAlgorithm = new CompositeShapeAlgorithm(collisionDetection);
HeightFieldAlgorithm heightFieldAlgorithm = new HeightFieldAlgorithm(collisionDetection);
LineAlgorithm lineAlgorithm = new LineAlgorithm(collisionDetection);
PlaneBoxAlgorithm planeBoxAlgorithm = new PlaneBoxAlgorithm(collisionDetection);
PlaneConvexAlgorithm planeConvexAlgorithm = new PlaneConvexAlgorithm(collisionDetection);
PlaneRayAlgorithm planeRayAlgorithm = new PlaneRayAlgorithm(collisionDetection);
PlaneSphereAlgorithm planeSphereAlgorithm = new PlaneSphereAlgorithm(collisionDetection);
RayBoxAlgorithm rayBoxAlgorithm = new RayBoxAlgorithm(collisionDetection);
RayConvexAlgorithm rayConvexAlgorithm = new RayConvexAlgorithm(collisionDetection);
RaySphereAlgorithm raySphereAlgorithm = new RaySphereAlgorithm(collisionDetection);
RayTriangleAlgorithm rayTriangleAlgorithm = new RayTriangleAlgorithm(collisionDetection);
SphereSphereAlgorithm sphereSphereAlgorithm = new SphereSphereAlgorithm(collisionDetection);
TransformedShapeAlgorithm transformedShapeAlgorithm = new TransformedShapeAlgorithm(collisionDetection);
TriangleMeshAlgorithm triangleMeshAlgorithm = new TriangleMeshAlgorithm(collisionDetection);
RayCompositeAlgorithm rayCompositeAlgorithm = new RayCompositeAlgorithm(collisionDetection);
RayTriangleMeshAlgorithm rayTriangleMeshAlgorithm = new RayTriangleMeshAlgorithm(collisionDetection);
RayHeightFieldAlgorithm rayHeightFieldAlgorithm = new RayHeightFieldAlgorithm(collisionDetection);
this[typeof(PointShape), typeof(PointShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(LineShape)] = lineAlgorithm;
this[typeof(PointShape), typeof(RayShape)] = rayConvexAlgorithm;
this[typeof(PointShape), typeof(LineSegmentShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(TriangleShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(RectangleShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(BoxShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(EmptyShape)] = noAlgo;
this[typeof(PointShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(PointShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(PointShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(PointShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(PointShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(PointShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(LineShape), typeof(LineShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(RayShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(LineSegmentShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(TriangleShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(RectangleShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(BoxShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(ConvexShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(ScaledConvexShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(CircleShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(SphereShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(CapsuleShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(ConeShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(CylinderShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(EmptyShape)] = noAlgo;
this[typeof(LineShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(LineShape), typeof(PlaneShape)] = noAlgo;
this[typeof(LineShape), typeof(HeightField)] = noAlgo;
this[typeof(LineShape), typeof(TriangleMeshShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(TransformedShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(CompositeShape)] = lineAlgorithm;
this[typeof(RayShape), typeof(RayShape)] = noAlgo;
this[typeof(RayShape), typeof(LineSegmentShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(TriangleShape)] = rayTriangleAlgorithm;
this[typeof(RayShape), typeof(RectangleShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(BoxShape)] = rayBoxAlgorithm;
this[typeof(RayShape), typeof(ConvexShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(ScaledConvexShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(CircleShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(SphereShape)] = raySphereAlgorithm;
this[typeof(RayShape), typeof(CapsuleShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(ConeShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(CylinderShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(EmptyShape)] = noAlgo;
this[typeof(RayShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(RayShape), typeof(PlaneShape)] = planeRayAlgorithm;
this[typeof(RayShape), typeof(HeightField)] = rayHeightFieldAlgorithm;
this[typeof(RayShape), typeof(TriangleMeshShape)] = rayTriangleMeshAlgorithm;
this[typeof(RayShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(RayShape), typeof(CompositeShape)] = rayCompositeAlgorithm;
this[typeof(LineSegmentShape), typeof(LineSegmentShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(TriangleShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(RectangleShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(BoxShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(EmptyShape)] = noAlgo;
this[typeof(LineSegmentShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(LineSegmentShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(LineSegmentShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(LineSegmentShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(LineSegmentShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(LineSegmentShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(TriangleShape), typeof(TriangleShape)] = gjkTriTriAlgorithm;
this[typeof(TriangleShape), typeof(RectangleShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(BoxShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(EmptyShape)] = noAlgo;
this[typeof(TriangleShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(TriangleShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(TriangleShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(TriangleShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(TriangleShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(TriangleShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(RectangleShape), typeof(RectangleShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(BoxShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(EmptyShape)] = noAlgo;
this[typeof(RectangleShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(RectangleShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(RectangleShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(RectangleShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(RectangleShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(RectangleShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(BoxShape), typeof(BoxShape)] = gjkBoxAlgorithm;
this[typeof(BoxShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(SphereShape)] = boxSphereAlgorithm;
this[typeof(BoxShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(EmptyShape)] = noAlgo;
this[typeof(BoxShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(BoxShape), typeof(PlaneShape)] = planeBoxAlgorithm;
this[typeof(BoxShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(BoxShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(BoxShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(BoxShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(ConvexShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(EmptyShape)] = noAlgo;
this[typeof(ConvexShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(ConvexShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(ConvexShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(ConvexShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(ConvexShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(ConvexShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(ScaledConvexShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(EmptyShape)] = noAlgo;
this[typeof(ScaledConvexShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(ScaledConvexShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(ScaledConvexShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(ScaledConvexShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(ScaledConvexShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(ScaledConvexShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(CircleShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(EmptyShape)] = noAlgo;
this[typeof(CircleShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(CircleShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(CircleShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(CircleShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(CircleShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(CircleShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(SphereShape), typeof(SphereShape)] = sphereSphereAlgorithm;
this[typeof(SphereShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(SphereShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(SphereShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(SphereShape), typeof(EmptyShape)] = noAlgo;
this[typeof(SphereShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(SphereShape), typeof(PlaneShape)] = planeSphereAlgorithm;
this[typeof(SphereShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(SphereShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(SphereShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(SphereShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(CapsuleShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(CapsuleShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(CapsuleShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(CapsuleShape), typeof(EmptyShape)] = noAlgo;
this[typeof(CapsuleShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(CapsuleShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(CapsuleShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(CapsuleShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(CapsuleShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(CapsuleShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(ConeShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(ConeShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(ConeShape), typeof(EmptyShape)] = noAlgo;
this[typeof(ConeShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(ConeShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(ConeShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(ConeShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(ConeShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(ConeShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(CylinderShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(CylinderShape), typeof(EmptyShape)] = noAlgo;
this[typeof(CylinderShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(CylinderShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(CylinderShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(CylinderShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(CylinderShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(CylinderShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(EmptyShape), typeof(EmptyShape)] = noAlgo;
this[typeof(EmptyShape), typeof(InfiniteShape)] = noAlgo; // No collision between Empty and Infinite.
this[typeof(EmptyShape), typeof(PlaneShape)] = noAlgo;
this[typeof(EmptyShape), typeof(HeightField)] = noAlgo;
this[typeof(EmptyShape), typeof(TriangleMeshShape)] = noAlgo;
this[typeof(EmptyShape), typeof(TransformedShape)] = noAlgo;
this[typeof(EmptyShape), typeof(CompositeShape)] = noAlgo;
this[typeof(InfiniteShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(PlaneShape)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(HeightField)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(TriangleMeshShape)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(TransformedShape)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(CompositeShape)] = infiniteAlgo;
this[typeof(PlaneShape), typeof(PlaneShape)] = noAlgo;
this[typeof(PlaneShape), typeof(HeightField)] = noAlgo;
this[typeof(PlaneShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(PlaneShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(PlaneShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(HeightField), typeof(HeightField)] = noAlgo;
// We could also call triangleMeshAlgorithm. But since HeightField has usually larger parts it
// is better to call the heightFieldAlgorithm. The heightFieldAlgorithm will cull all but a
// few height field cells very quickly.
this[typeof(HeightField), typeof(TriangleMeshShape)] = heightFieldAlgorithm;
this[typeof(HeightField), typeof(TransformedShape)] = transformedShapeAlgorithm;
// Same as for triangle meshes: Call height field algorithm first.
this[typeof(HeightField), typeof(CompositeShape)] = heightFieldAlgorithm;
this[typeof(TriangleMeshShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(TriangleMeshShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(TriangleMeshShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(TransformedShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(TransformedShape), typeof(CompositeShape)] = transformedShapeAlgorithm;
this[typeof(CompositeShape), typeof(CompositeShape)] = compositeAlgorithm;
}
/// <summary>
/// Performs more collision tests while slightly rotating one collision object.
/// </summary>
/// <param name="collisionDetection">The collision detection.</param>
/// <param name="contactSet">
/// The contact set; must contain at least 1 <see cref="Contact"/>.
/// </param>
/// <param name="perturbB">
/// if set to <see langword="true"/> collision object B will be rotated; otherwise collision
/// object A will be rotated.
/// </param>
/// <param name="testMethod">The test method that is called to compute contacts.</param>
/// <remarks>
/// This method rotates one object 3 times and calls contact computation for the new
/// orientations. It is recommended to call this method only when the contact set has 1 new
/// contact.
/// </remarks>
internal static void TestWithPerturbations(CollisionDetection collisionDetection, ContactSet contactSet, bool perturbB, Action<ContactSet> testMethod)
{
Debug.Assert(contactSet != null);
Debug.Assert(contactSet.Count > 0 && contactSet.HaveContact || !contactSet.IsPerturbationTestAllowed);
Debug.Assert(testMethod != null);
// Make this test only if there is 1 contact.
// If there are 0 contacts, we assume that the contact pair is separated.
// If there are more than 3 contacts, then we already have a lot of contacts to work with, no
// need to search for more.
if (!contactSet.HaveContact || contactSet.Count == 0 || contactSet.Count >= 4 || !contactSet.IsPerturbationTestAllowed)
return;
// Get data of object that will be rotated.
var collisionObject = (perturbB) ? contactSet.ObjectB : contactSet.ObjectA;
var geometricObject = collisionObject.GeometricObject;
var pose = geometricObject.Pose;
// Get normal, pointing to the test object.
var normal = contactSet[0].Normal;
if (!perturbB)
normal = -normal;
var contactPosition = contactSet[0].Position;
var centerToContact = contactPosition - pose.Position;
// Compute a perturbation angle proportional to the dimension of the object.
var radius = geometricObject.Aabb.Extent.Length;
var angle = collisionDetection.ContactPositionTolerance / radius;
// axis1 is in the contact tangent plane, orthogonal to normal.
var axis1 = Vector3F.Cross(normal, centerToContact);
// If axis1 is zero then normal and centerToContact are collinear. This happens
// for example for spheres or cone tips against flat faces. In these cases we assume
// that there will be max. 1 contact.
if (axis1.IsNumericallyZero)
return;
var axis1Local = pose.ToLocalDirection(axis1);
var rotation = Matrix33F.CreateRotation(axis1Local, -angle);
// Use temporary test objects.
var testGeometricObject = TestGeometricObject.Create();
testGeometricObject.Shape = geometricObject.Shape;
testGeometricObject.Scale = geometricObject.Scale;
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation);
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(collisionObject, testGeometricObject);
var testContactSet = perturbB ? ContactSet.Create(contactSet.ObjectA, testCollisionObject)
: ContactSet.Create(testCollisionObject, contactSet.ObjectB);
testContactSet.IsPerturbationTestAllowed = false; // Avoid recursive perturbation tests!
testContactSet.PreferredNormal = contactSet.PreferredNormal;
// Compute next contacts.
testMethod(testContactSet);
if (testContactSet.Count > 0)
{
// axis2 is in the contact tangent plane, orthogonal to axis1.
var axis2 = Vector3F.Cross(axis1, normal);
var axis2Local = pose.ToLocalDirection(axis2);
var rotation2 = Matrix33F.CreateRotation(axis2Local, -angle);
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2);
// Compute next contacts.
testMethod(testContactSet);
// Invert rotation2.
rotation2.Transpose();
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2);
// Compute next contacts.
testMethod(testContactSet);
}
// Set HaveContact. It is reset when a perturbation separates the objects.
testContactSet.HaveContact = true;
// TODO: Test if we need this:
// The contact world positions are not really correct because one object was rotated.
// UpdateContacts recomputes the world positions from the local positions.
UpdateContacts(testContactSet, 0, collisionDetection.ContactPositionTolerance);
// Merge contacts of testContactSet into contact set, but do not change existing contacts.
foreach (var contact in testContactSet)
{
// We call TryMerge() to see if the contact is similar to an existing contact.
bool exists = TryMergeWithNearestContact(
contactSet,
contact,
collisionDetection.ContactPositionTolerance,
false); // The existing contact must no be changed!
if (exists)
{
// We can throw away the new contact because a similar is already in the contact set.
contact.Recycle();
}
else
{
// Add new contact.
contactSet.Add(contact);
}
}
// Recycle temporary objects.
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
}
public override void Update(GameTime gameTime)
{
var debugRenderer = GraphicsScreen.DebugRenderer;
var cd = new CollisionDetection();
float sizeB = _part2.GeometricObject.Aabb.Extent.Length;
var perturbationEpsilon = sizeB * Math.Max(0.001f / 10, Numeric.EpsilonF * 10);
var perturbationAngle = perturbationEpsilon / sizeB;
Vector3F v;
v = new Vector3F(1, 0, 0);
var translation = v * perturbationEpsilon;
var rotation = Matrix33F.CreateRotation(v, perturbationAngle);
var poseB = _part2.GeometricObject.Pose;
var origPose = poseB;
poseB.Position = translation + poseB.Position;
poseB.Orientation = rotation * poseB.Orientation;
//((GeometricObject)_part2.GeometricObject).Pose = poseB;
var cp1 = cd.GetClosestPoints(_part1A, _part2);
//var cp2 = cd.GetClosestPoints(_part1B, _part2);
((GeometricObject)_part2.GeometricObject).Pose = origPose;
debugRenderer.Clear();
debugRenderer.DrawObject(_part1A.GeometricObject, Color.DarkGreen, true, false);
//debugRenderer.DrawObject(_part1B.GeometricObject, Color.DarkBlue, false, false);
debugRenderer.DrawObject(_part2.GeometricObject, Color.DarkViolet, true, false);
debugRenderer.DrawContact(cp1[0], 0.1f, Color.Yellow, true);
//debugRenderer.DrawContact(cp2[0], 0.1f, Color.Pink, true);
}
/// <summary>
/// Performs more collision tests while slightly rotating one collision object.
/// </summary>
/// <param name="collisionDetection">The collision detection.</param>
/// <param name="contactSet">
/// The contact set; must contain at least 1 <see cref="Contact"/>.
/// </param>
/// <param name="perturbB">
/// if set to <see langword="true"/> collision object B will be rotated; otherwise collision
/// object A will be rotated.
/// </param>
/// <param name="testMethod">The test method that is called to compute contacts.</param>
/// <remarks>
/// This method rotates one object 3 times and calls contact computation for the new
/// orientations. It is recommended to call this method only when the contact set has 1 new
/// contact.
/// </remarks>
internal static void TestWithPerturbations(CollisionDetection collisionDetection, ContactSet contactSet, bool perturbB, Action <ContactSet> testMethod)
{
Debug.Assert(contactSet != null);
Debug.Assert(contactSet.Count > 0 && contactSet.HaveContact || !contactSet.IsPerturbationTestAllowed);
Debug.Assert(testMethod != null);
// Make this test only if there is 1 contact.
// If there are 0 contacts, we assume that the contact pair is separated.
// If there are more than 3 contacts, then we already have a lot of contacts to work with, no
// need to search for more.
if (!contactSet.HaveContact || contactSet.Count == 0 || contactSet.Count >= 4 || !contactSet.IsPerturbationTestAllowed)
{
return;
}
// Get data of object that will be rotated.
var collisionObject = (perturbB) ? contactSet.ObjectB : contactSet.ObjectA;
var geometricObject = collisionObject.GeometricObject;
var pose = geometricObject.Pose;
// Get normal, pointing to the test object.
var normal = contactSet[0].Normal;
if (!perturbB)
{
normal = -normal;
}
var contactPosition = contactSet[0].Position;
var centerToContact = contactPosition - pose.Position;
// Compute a perturbation angle proportional to the dimension of the object.
var radius = geometricObject.Aabb.Extent.Length;
var angle = collisionDetection.ContactPositionTolerance / radius;
// axis1 is in the contact tangent plane, orthogonal to normal.
var axis1 = Vector3F.Cross(normal, centerToContact);
// If axis1 is zero then normal and centerToContact are collinear. This happens
// for example for spheres or cone tips against flat faces. In these cases we assume
// that there will be max. 1 contact.
if (axis1.IsNumericallyZero)
{
return;
}
var axis1Local = pose.ToLocalDirection(axis1);
var rotation = Matrix33F.CreateRotation(axis1Local, -angle);
// Use temporary test objects.
var testGeometricObject = TestGeometricObject.Create();
testGeometricObject.Shape = geometricObject.Shape;
testGeometricObject.Scale = geometricObject.Scale;
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation);
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(collisionObject, testGeometricObject);
var testContactSet = perturbB ? ContactSet.Create(contactSet.ObjectA, testCollisionObject)
: ContactSet.Create(testCollisionObject, contactSet.ObjectB);
testContactSet.IsPerturbationTestAllowed = false; // Avoid recursive perturbation tests!
testContactSet.PreferredNormal = contactSet.PreferredNormal;
// Compute next contacts.
testMethod(testContactSet);
if (testContactSet.Count > 0)
{
// axis2 is in the contact tangent plane, orthogonal to axis1.
var axis2 = Vector3F.Cross(axis1, normal);
var axis2Local = pose.ToLocalDirection(axis2);
var rotation2 = Matrix33F.CreateRotation(axis2Local, -angle);
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2);
// Compute next contacts.
testMethod(testContactSet);
// Invert rotation2.
rotation2.Transpose();
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2);
// Compute next contacts.
testMethod(testContactSet);
}
// Set HaveContact. It is reset when a perturbation separates the objects.
testContactSet.HaveContact = true;
// TODO: Test if we need this:
// The contact world positions are not really correct because one object was rotated.
// UpdateContacts recomputes the world positions from the local positions.
UpdateContacts(testContactSet, 0, collisionDetection.ContactPositionTolerance);
// Merge contacts of testContactSet into contact set, but do not change existing contacts.
foreach (var contact in testContactSet)
{
// We call TryMerge() to see if the contact is similar to an existing contact.
bool exists = TryMergeWithNearestContact(
contactSet,
contact,
collisionDetection.ContactPositionTolerance,
false); // The existing contact must no be changed!
if (exists)
{
// We can throw away the new contact because a similar is already in the contact set.
contact.Recycle();
}
else
{
// Add new contact.
contactSet.Add(contact);
}
}
// Recycle temporary objects.
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
}
//--------------------------------------------------------------
/// <summary>
/// Initializes a new instance of the <see cref="Simulation"/> class.
/// </summary>
public Simulation()
{
Settings = new SimulationSettings();
var collisionDetection = new CollisionDetection
{
CollisionFilter = new CollisionFilter(),
FullContactSetPerFrame = true
};
CollisionDomain = new CollisionDomain(collisionDetection);
ConstraintSolver = new SequentialImpulseBasedSolver(this);
Constraints = new ConstraintCollection();
Constraints.CollectionChanged += OnConstraintsChanged;
ForceEffects = new ForceEffectCollection();
ForceEffects.CollectionChanged += OnForceEffectsChanged;
RigidBodies = new RigidBodyCollection();
RigidBodies.CollectionChanged += OnRigidBodiesChanged;
ContactConstraintsInternal = new List<ContactConstraint>();
IslandManager = new SimulationIslandManager(this);
// Define the "World" as a rigid body.
// - World is an imaginary body that is used to define the space of the simulation.
// - The user can use World in constraints e.g. to anchor objects in the world.
// - No contacts are computed for World.
World = new RigidBody(new BoxShape(20000, 20000, 20000))
{
CollisionResponseEnabled = false,
CollisionObject = { Type = CollisionObjectType.Trigger },
MotionType = MotionType.Static,
Name = "World",
Simulation = this,
};
// Remove "World" from the collision domain.
CollisionDomain.CollisionObjects.Remove(World.CollisionObject);
#if STOPWATCH
Diagnostics = new SimulationDiagnostics();
#endif
// Store delegate methods to avoid garbage when multithreading.
_updateVelocityMethod = i =>
{
var body = RigidBodies[i];
body.UpdateVelocity(_fixedTimeStep);
};
_solveIslandMethod = SolveIsland;
_updatePoseMethod = i =>
{
var body = RigidBodies[i];
body.UpdatePose(_fixedTimeStep);
};
_computeTimeOfImpactMethod = ComputeTimeOfImpact_Multithreaded;
_moveToTimeOfImpactMethod = MoveToTimeOfImpact;
}
public void OrthographicTest()
{
Vector3F position = new Vector3F(1, 2, 3);
QuaternionF orientation = QuaternionF.CreateRotation(new Vector3F(2, 3, 6), 0.123f);
CameraInstance cameraInstance = new CameraInstance(new Camera(new OrthographicProjection()))
{
PoseLocal = new Pose(position, orientation),
};
((OrthographicProjection)cameraInstance.Camera.Projection).Set(4, 3, 2, 10);
Matrix44F projection = Matrix44F.CreateOrthographic(4, 3, 2, 10);
Assert.AreEqual(position, cameraInstance.PoseWorld.Position);
Assert.AreEqual(orientation.ToRotationMatrix33(), cameraInstance.PoseWorld.Orientation);
Assert.AreEqual(4, cameraInstance.Camera.Projection.Width);
Assert.AreEqual(3, cameraInstance.Camera.Projection.Height);
Assert.AreEqual(4.0f / 3.0f, cameraInstance.Camera.Projection.AspectRatio);
Assert.AreEqual(2, cameraInstance.Camera.Projection.Near);
Assert.AreEqual(10, cameraInstance.Camera.Projection.Far);
Assert.AreEqual(-2, cameraInstance.Camera.Projection.Left);
Assert.AreEqual(2, cameraInstance.Camera.Projection.Right);
Assert.AreEqual(-1.5f, cameraInstance.Camera.Projection.Bottom);
Assert.AreEqual(1.5f, cameraInstance.Camera.Projection.Top);
Assert.AreEqual(8, cameraInstance.Camera.Projection.Depth);
Assert.IsTrue(Matrix44F.AreNumericallyEqual(projection, cameraInstance.Camera.Projection));
Assert.IsTrue(Matrix44F.AreNumericallyEqual(projection.Inverse, cameraInstance.Camera.Projection.Inverse));
Assert.IsNotNull(cameraInstance.BoundingShape);
// Test shape using collision detection. Remove rotation to simplify test.
cameraInstance.PoseWorld = new Pose(cameraInstance.PoseWorld.Position);
CollisionDetection collisionDetection = new CollisionDetection();
var point = new PointShape();
CollisionObject pointCollisionObject = new CollisionObject(new GeometricObject(point));
CollisionObject cameraCollisionObject = new CollisionObject(cameraInstance);
point.Position = position + new Vector3F(-2, -1.5f, -2);
Assert.IsTrue(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(-2.1f, -1.6f, -1.9f);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(2, 1.5f, -10);
Assert.IsTrue(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(2.1f, 1.6f, -10.1f);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
cameraInstance.PoseWorld = new Pose(position, orientation);
((OrthographicProjection)cameraInstance.Camera.Projection).Set(8, 4, 1, 100);
cameraInstance.Camera.Projection.Near = 1;
cameraInstance.Camera.Projection.Far = 100;
projection = Matrix44F.CreateOrthographic(8, 4, 1, 100);
Assert.IsTrue(Matrix44F.AreNumericallyEqual(projection.Inverse, cameraInstance.Camera.Projection.Inverse));
Assert.IsTrue(Matrix44F.AreNumericallyEqual(projection, cameraInstance.Camera.Projection));
// Test shape using collision detection. Remove rotation to simplify test.
cameraInstance.PoseWorld = new Pose(cameraInstance.PoseWorld.Position);
point.Position = position + new Vector3F(-4, -2f, -1);
Assert.IsTrue(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(-4.1f, -1.9f, -0.9f);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(4, 2f, -100);
Assert.IsTrue(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(4.1f, 2.1f, -100.1f);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
}
public void PerspectiveOffCenterTest()
{
Vector3F position = new Vector3F(1, 2, 3);
QuaternionF orientation = QuaternionF.CreateRotation(new Vector3F(2, 3, 6), 0.123f);
CameraInstance cameraInstance = new CameraInstance(new Camera(new PerspectiveProjection()))
{
PoseLocal = new Pose(position, orientation),
};
((PerspectiveProjection)cameraInstance.Camera.Projection).SetOffCenter(1, 5, 2, 5, 1, 10);
Matrix44F projection = Matrix44F.CreatePerspectiveOffCenter(1, 5, 2, 5, 1, 10);
Assert.AreEqual(position, cameraInstance.PoseWorld.Position);
Assert.AreEqual(orientation.ToRotationMatrix33(), cameraInstance.PoseWorld.Orientation);
Assert.IsTrue(Numeric.AreEqual(MathHelper.ToRadians(33.690067f), cameraInstance.Camera.Projection.FieldOfViewX));
Assert.IsTrue(Numeric.AreEqual(MathHelper.ToRadians(15.255119f), cameraInstance.Camera.Projection.FieldOfViewY));
Assert.AreEqual(4.0f / 3.0f, cameraInstance.Camera.Projection.AspectRatio);
Assert.AreEqual(4, cameraInstance.Camera.Projection.Width);
Assert.AreEqual(3, cameraInstance.Camera.Projection.Height);
Assert.AreEqual(1, cameraInstance.Camera.Projection.Left);
Assert.AreEqual(5, cameraInstance.Camera.Projection.Right);
Assert.AreEqual(2, cameraInstance.Camera.Projection.Bottom);
Assert.AreEqual(5, cameraInstance.Camera.Projection.Top);
Assert.AreEqual(1, cameraInstance.Camera.Projection.Near);
Assert.AreEqual(10, cameraInstance.Camera.Projection.Far);
Assert.AreEqual(9, cameraInstance.Camera.Projection.Depth);
Assert.IsTrue(Matrix44F.AreNumericallyEqual(projection, cameraInstance.Camera.Projection));
Assert.IsTrue(Matrix44F.AreNumericallyEqual(projection.Inverse, cameraInstance.Camera.Projection.Inverse));
Assert.IsNotNull(cameraInstance.BoundingShape);
// Test shape using collision detection. Remove rotation to simplify test.
cameraInstance.PoseWorld = new Pose(cameraInstance.PoseWorld.Position);
CollisionDetection collisionDetection = new CollisionDetection();
var point = new PointShape();
CollisionObject pointCollisionObject = new CollisionObject(new GeometricObject(point));
CollisionObject cameraCollisionObject = new CollisionObject(cameraInstance);
point.Position = position + new Vector3F(3, 3, -1);
Assert.IsTrue(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(30, 30, -10);
Assert.IsTrue(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(3, 3, -0.9f);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(30, 30, -10.1f);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(1, 2, -1);
Assert.IsTrue(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(0.9f, 2, -1);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(1, 1.9f, -1);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(5, 5, -1);
Assert.IsTrue(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(5.1f, 5, -1);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(5, 5.1f, -1);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(10, 20, -10);
Assert.IsTrue(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(9.9f, 20, -10);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(10, 19.9f, -10);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(50, 50, -10);
Assert.IsTrue(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(50.1f, 50, -10);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(50, 50.1f, -10);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
}
public void PerspectiveTest()
{
Vector3F position = new Vector3F(1, 2, 3);
QuaternionF orientation = QuaternionF.CreateRotation(new Vector3F(2, 3, 6), 0.123f);
CameraInstance cameraInstance = new CameraInstance(new Camera(new PerspectiveProjection()))
{
PoseLocal = new Pose(position, orientation),
};
((PerspectiveProjection)cameraInstance.Camera.Projection).SetFieldOfView(MathHelper.ToRadians(60), 16.0f / 10.0f, 1, 10);
Matrix44F projection = Matrix44F.CreatePerspectiveFieldOfView(MathHelper.ToRadians(60), 16.0f / 10.0f, 1, 10);
Assert.AreEqual(position, cameraInstance.PoseWorld.Position);
Assert.AreEqual(orientation.ToRotationMatrix33(), cameraInstance.PoseWorld.Orientation);
Assert.AreEqual(MathHelper.ToRadians(60), cameraInstance.Camera.Projection.FieldOfViewY);
Assert.IsTrue(Numeric.AreEqual(MathHelper.ToRadians(85.4601055f), cameraInstance.Camera.Projection.FieldOfViewX));
Assert.AreEqual(16.0f / 10.0f, cameraInstance.Camera.Projection.AspectRatio);
Assert.IsTrue(Numeric.AreEqual(1.8475209f, cameraInstance.Camera.Projection.Width));
Assert.IsTrue(Numeric.AreEqual(1.1547005f, cameraInstance.Camera.Projection.Height));
Assert.AreEqual(1, cameraInstance.Camera.Projection.Near);
Assert.AreEqual(10, cameraInstance.Camera.Projection.Far);
Assert.IsTrue(Numeric.AreEqual(-0.9237604f, cameraInstance.Camera.Projection.Left));
Assert.IsTrue(Numeric.AreEqual(0.9237604f, cameraInstance.Camera.Projection.Right));
Assert.IsTrue(Numeric.AreEqual(-0.5773503f, cameraInstance.Camera.Projection.Bottom));
Assert.IsTrue(Numeric.AreEqual(0.5773503f, cameraInstance.Camera.Projection.Top));
Assert.AreEqual(9, cameraInstance.Camera.Projection.Depth);
Assert.IsTrue(Matrix44F.AreNumericallyEqual(projection, cameraInstance.Camera.Projection));
Assert.IsTrue(Matrix44F.AreNumericallyEqual(projection.Inverse, cameraInstance.Camera.Projection.Inverse));
Assert.IsNotNull(cameraInstance.BoundingShape);
// Test shape using collision detection. Remove rotation to simplify test.
cameraInstance.PoseWorld = new Pose(cameraInstance.PoseWorld.Position);
CollisionDetection collisionDetection = new CollisionDetection();
var point = new PointShape();
CollisionObject pointCollisionObject = new CollisionObject(new GeometricObject(point));
CollisionObject cameraCollisionObject = new CollisionObject(cameraInstance);
point.Position = position + new Vector3F(0, 0, -1);
Assert.IsTrue(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(0, 0, -10);
Assert.IsTrue(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(0, 0, -0.9f);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(0, 0, -10.1f);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(-0.9237604f, -0.5773f, -1);
Assert.IsTrue(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(-0.924f, -0.5773f, -1);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(-0.9237604f, -0.58f, -1);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(0.9237604f, 0.5773f, -1);
Assert.IsTrue(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(0.924f, 0.5773f, -1);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(0.9237604f, 0.58f, -1);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(-9.237604f, -5.773f, -10);
Assert.IsTrue(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(-9.24f, -5.773f, -10);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(-9.237604f, -5.8f, -10);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(9.237604f, 5.773f, -10);
Assert.IsTrue(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(9.24f, 5.773f, -10);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
point.Position = position + new Vector3F(9.237604f, 5.8f, -10);
Assert.IsFalse(collisionDetection.HaveContact(pointCollisionObject, cameraCollisionObject));
}
