public void TestEnabledDisabledStochastic()
{
// Test random Enabled and Pose values.
int numberOfObjects = 20;
int numberOfSteps = 1000;
Shape shape = new SphereShape(1);
//var meshShape = new TriangleMeshShape(shape.GetMesh(0.01f, 4));
//meshShape.Partition = new AabbTree<int>();
//shape = meshShape;
var geometricObjects = new GeometricObject[numberOfObjects];
var collisionObjects = new CollisionObject[numberOfObjects];
var domain = new CollisionDomain(new CollisionDetection());
for (int i = 0; i < numberOfObjects; i++)
{
geometricObjects[i] = new GeometricObject(shape);
collisionObjects[i] = new CollisionObject(geometricObjects[i]);
domain.CollisionObjects.Add(collisionObjects[i]);
}
for (int i = 0; i < numberOfSteps; i++)
{
for (int j = 0; j < numberOfObjects; j++)
{
collisionObjects[j].Enabled = RandomHelper.Random.NextBool();
domain.Update(0);
if (RandomHelper.Random.NextFloat(0, 1) > 0.5f)
{
geometricObjects[j].Pose = new Pose(RandomHelper.Random.NextVector3(-2, 2));
}
domain.Update(0);
}
domain.Update(0);
domain.Update(0);
domain.Update(0);
domain.Update(0);
domain.Update(0);
domain.Update(0);
domain.Update(0);
// Compare result with brute-force check.
for (int j = 0; j < numberOfObjects; j++)
{
for (int k = j + 1; k < numberOfObjects; k++)
{
var haveContact = domain.CollisionDetection.HaveContact(collisionObjects[j], collisionObjects[k]);
Assert.AreEqual(haveContact, domain.ContactSets.GetContacts(collisionObjects[j], collisionObjects[k]) != null);
}
}
}
}
public void UpdateSingle()
{
CollisionDomain domain = new CollisionDomain(new CollisionDetection());
// Add 100 random objects.
for (int i = 0; i < 100; i++)
{
domain.CollisionObjects.Add(new CollisionObject(new GeometricObject(new SphereShape(1), new Pose(RandomHelper.Random.NextVector3F(0, 20)))));
}
for (int i = 0; i < 100; i++)
{
domain.Update(domain.CollisionObjects[33]);
for (int j = 0; j < domain.CollisionObjects.Count; j++)
{
var a = domain.CollisionObjects[j];
for (int k = j + 1; k < domain.CollisionObjects.Count; k++)
{
var b = domain.CollisionObjects[k];
Assert.AreEqual(domain.InternalBroadPhase.CandidatePairs.Contains(a, b),
GeometryHelper.HaveContact(a.GeometricObject.Aabb, b.GeometricObject.Aabb));
}
}
// Set new random position for one.
((GeometricObject)domain.CollisionObjects[33].GeometricObject).Pose = new Pose(RandomHelper.Random.NextVector3F(0, 20));
Console.WriteLine("Candidate pairs: " + domain.InternalBroadPhase.CandidatePairs.Count);
}
}
public void RayCastStopsAtFirstHitWhenChangingFilter()
{
CollisionDomain domain = new CollisionDomain(new CollisionDetection());
domain.CollisionDetection.CollisionFilter = new CollisionFilter();
// 1 ray: at origin shooting into +x
CollisionObject ray = new CollisionObject();
((GeometricObject)ray.GeometricObject).Shape = new RayShape(new Vector3(), new Vector3(1, 0, 0), 100)
{
StopsAtFirstHit = true,
};
//ray.Name = "Ray";
// 2 spheres: at at x=10, b at x=20
CollisionObject a = new CollisionObject();
((GeometricObject)a.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3(10, 0, 0f));
//a.Name = "b";
CollisionObject b = new CollisionObject();
((GeometricObject)b.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3(20, 0, 0f));
//b.Name = "c";
domain.CollisionObjects.Add(ray);
domain.CollisionObjects.Add(a);
domain.CollisionObjects.Add(b);
// Ray touches b.
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, a));
Assert.AreEqual(false, domain.HaveContact(ray, b));
// Disable collisions between ray and a.
// Then ray must hit b.
((CollisionFilter)domain.CollisionDetection.CollisionFilter).Set(ray, a, false);
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(false, domain.HaveContact(ray, a));
Assert.AreEqual(true, domain.HaveContact(ray, b));
}
public void TestEnabledDisabled()
{
var shape = new SphereShape(1);
var goA = new GeometricObject(shape, Pose.Identity);
var coA = new CollisionObject(goA);
var goB = new GeometricObject(shape, Pose.Identity);
var coB = new CollisionObject(goB);
var cd = new CollisionDomain(new CollisionDetection());
cd.CollisionObjects.Add(coA);
cd.CollisionObjects.Add(coB);
// not touching
goB.Pose = new Pose(new Vector3(3, 3, 0));
cd.Update(0);
Assert.AreEqual(0, cd.ContactSets.Count);
// not touching, disabled
coB.Enabled = false;
cd.Update(0);
Assert.AreEqual(0, cd.ContactSets.Count);
// touching, disabled
goB.Pose = new Pose(new Vector3(1, 1, 1));
cd.Update(0);
Assert.AreEqual(0, cd.ContactSets.Count);
// touching, enabled
coB.Enabled = true;
cd.Update(0);
Assert.AreEqual(1, cd.ContactSets.Count);
// not touching - but broadphase overlap, enabled
goB.Pose = new Pose(new Vector3(1.8f, 1.8f, 0));
cd.Update(0);
Assert.AreEqual(0, cd.ContactSets.Count);
// not touching, disabled
coB.Enabled = false;
cd.Update(0);
Assert.AreEqual(0, cd.ContactSets.Count);
// touching, disabled
goB.Pose = new Pose(new Vector3(1, 1, 1));
cd.Update(0);
Assert.AreEqual(0, cd.ContactSets.Count);
// touching, enabled
coB.Enabled = true;
cd.Update(0);
Assert.AreEqual(1, cd.ContactSets.Count);
}
protected override void OnUpdate(TimeSpan deltaTime)
{
// Update direction of picking object.
// TODO(matt) Clean this bs up!
var services = (ServiceContainer)ServiceLocator.Current;
var inputService = services.GetInstance <IInputService>();
var graphicsService = services.GetInstance <IGraphicsService>();
var mousePos = inputService.MousePosition;
var originalCameraMat = _cameraObject.CameraNode.PoseWorld.Inverse.ToMatrix44F();
var originalCameraPos = _cameraObject.CameraNode.PoseWorld.Position;
var mouseWorldPosOld = GraphicsHelper.Unproject(graphicsService.GraphicsDevice.Viewport, new Vector3F(mousePos.X, mousePos.Y, 0),
_cameraObject.CameraNode.Camera.Projection.ToMatrix44F(), originalCameraMat);
((GeometricObject)_pickingObject.GeometricObject).Pose = new Pose(new Vector3F(mouseWorldPosOld.X, mouseWorldPosOld.Y, ((GeometricObject)_pickingObject.GeometricObject).Pose.Position.Z));
// TODO: If figureNodes can move or scale, we have to copy the new Pose
// and Scale from the FigureNodes to their CollisionObjects.
_collisionDomain.Update(deltaTime);
// Reset colors of figure nodes that where "picked" in the last frame.
// TODO: To make this faster, loop over the contact objects of the last
// frame and not over all nodes in the scene.
foreach (var figureNode in _scene.GetDescendants().OfType <FigureNode>())
{
// Figure nodes which were picked, have the color info in the UserData.
if (figureNode.UserData != null)
{
figureNode.StrokeColor = ((Pair <Vector3F>)figureNode.UserData).First;
figureNode.FillColor = ((Pair <Vector3F>)figureNode.UserData).Second;
figureNode.UserData = null;
}
}
// Change the color of all figure nodes which touch the picking object.
foreach (var pickedObject in _collisionDomain.GetContactObjects(_pickingObject))
{
var myGeometricObject = pickedObject.GeometricObject as FigureGeometricObject;
if (myGeometricObject != null)
{
var figureNode = myGeometricObject.FigureNode;
_debugRenderer.DrawText("Picked node: " + figureNode.Name);
// Store original color in UserData.
figureNode.UserData = new Pair <Vector3F>(figureNode.StrokeColor, figureNode.FillColor);
// Change color.
figureNode.StrokeColor = new Vector3F(0.8f, 0.6f, 0.08f);
figureNode.FillColor = new Vector3F(1, 0.7f, 0.1f);
}
}
// Draw the picking object (for debugging).
_debugRenderer.DrawObject(_pickingObject.GeometricObject, Color.Red, true, false);
}
/// <overloads>
/// <summary>
/// Advances the simulation by the given time.
/// </summary>
/// </overloads>
///
/// <summary>
/// Advances the simulation by the given time.
/// </summary>
/// <param name="deltaTime">The size of the time step.</param>
/// <remarks>
/// See <see cref="Simulation"/> for more information.
/// </remarks>
public void Update(TimeSpan deltaTime)
{
if (deltaTime == TimeSpan.Zero)
return;
// Apply speedup factor.
deltaTime = new TimeSpan((long)(deltaTime.Ticks * TimeScaling));
_fixedTimeStep = Settings.Timing.FixedTimeStep;
TimeSpan fixedTimeStep = new TimeSpan((long)(_fixedTimeStep * TimeSpan.TicksPerSecond));
// Negative time steps are not allowed. MaxNumberOfSteps limits the max allowed time step.
// If deltaTime is larger, then some time is lost.
TimeSpan minTimeStep = TimeSpan.Zero;
TimeSpan maxTimeStep = new TimeSpan(fixedTimeStep.Ticks * Settings.Timing.MaxNumberOfSteps);
deltaTime = MathHelper.Clamp(deltaTime, minTimeStep, maxTimeStep);
// Update target time.
TargetTime += deltaTime;
//if (Settings.Timing.UseFixedTimeStep)
// TimeStep = Settings.Timing.FixedTimeStep;
//else
// TimeStep = deltaTime;
// Loop until target time is reached or the difference to target time is less than
// the time step size.
while (TargetTime - Time >= fixedTimeStep)
{
if (Settings.EnableMultithreading)
{
SubTimeStep_Multithreaded(fixedTimeStep);
}
else
{
SubTimeStep_Singlethreaded(fixedTimeStep);
}
}
if (Settings.SynchronizeCollisionDomain)
{
// Update collision domain so that user sees new contacts. But don't recycle
// the old contact sets because they are still referenced by contact constraints.
CollisionDomain.Update(0, false);
}
// Reset user forces.
{
int numberOfRigidBodies = RigidBodies.Count;
for (int i = 0; i < numberOfRigidBodies; i++)
{
RigidBody body = RigidBodies[i];
body.ClearForces();
}
}
}
public void Test3()
{
CollisionDomain domain = new CollisionDomain(new CollisionDetection());
RandomHelper.Random = new Random(123456);
const float testAreaSize = 20;
// Add 100 random objects.
for (int i = 0; i < 100; i++)
{
domain.CollisionObjects.Add(new CollisionObject(new GeometricObject(new SphereShape(1), new Pose(RandomHelper.Random.NextVector3F(0, testAreaSize)))));
}
for (int i = 0; i < 100; i++)
{
domain.Update(0.03f);
for (int j = 0; j < domain.CollisionObjects.Count; j++)
{
var a = domain.CollisionObjects[j];
for (int k = j + 1; k < domain.CollisionObjects.Count; k++)
{
var b = domain.CollisionObjects[k];
bool contained = domain.InternalBroadPhase.CandidatePairs.Contains(a, b);
bool haveContact = GeometryHelper.HaveContact(a.GeometricObject.Aabb, b.GeometricObject.Aabb);
//if (contained != haveContact)
//Debugger.Break();
Assert.AreEqual(contained, haveContact);
}
// Set new random position for a few.
if (RandomHelper.Random.NextFloat(0, 1) < 0.7f)
{
((GeometricObject)a.GeometricObject).Pose = new Pose(RandomHelper.Random.NextVector3F(0, testAreaSize));
}
}
// Add new object.
domain.CollisionObjects.Add(new CollisionObject
{
GeometricObject = new GeometricObject
{
Shape = new SphereShape(1),
Pose = new Pose(RandomHelper.Random.NextVector3F(0, testAreaSize)),
}
});
// Remove random object.
domain.CollisionObjects.Remove(domain.CollisionObjects[RandomHelper.Random.NextInteger(0, domain.CollisionObjects.Count - 1)]);
Console.WriteLine("Candidate pairs: " + domain.InternalBroadPhase.CandidatePairs.Count);
}
}
public void Filtering()
{
CollisionDomain domain = new CollisionDomain(new CollisionDetection());
domain.CollisionDetection.CollisionFilter = new CollisionFilter();
CollisionObject a = new CollisionObject();
((GeometricObject)a.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3(0, 0, 0));
//a.Name = "a";
CollisionObject b = new CollisionObject();
((GeometricObject)b.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3(1, 0, 0f));
//b.Name = "b";
domain.CollisionObjects.Add(a);
domain.CollisionObjects.Add(b);
domain.Update(0.01f);
Assert.AreEqual(1, domain.ContactSets.Count);
b.Enabled = false;
domain.Update(0.01f);
Assert.AreEqual(0, domain.ContactSets.Count);
a.Enabled = false;
b.Enabled = true;
domain.Update(0.01f);
Assert.AreEqual(0, domain.ContactSets.Count);
a.Enabled = true;
((CollisionFilter)domain.CollisionDetection.CollisionFilter).Set(a, b, false);
domain.Update(0.01f);
Assert.AreEqual(0, domain.ContactSets.Count);
((CollisionFilter)domain.CollisionDetection.CollisionFilter).Set(a, b, true);
domain.Update(0.01f);
Assert.AreEqual(1, domain.ContactSets.Count);
}
public void Filtering()
{
CollisionDomain domain = new CollisionDomain(new CollisionDetection());
domain.CollisionDetection.CollisionFilter = new CollisionFilter();
CollisionObject a = new CollisionObject();
((GeometricObject)a.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0));
//a.Name = "a";
CollisionObject b = new CollisionObject();
((GeometricObject)b.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(1, 0, 0f));
//b.Name = "b";
domain.CollisionObjects.Add(a);
domain.CollisionObjects.Add(b);
domain.Update(0.01f);
Assert.AreEqual(1, domain.ContactSets.Count);
b.Enabled = false;
domain.Update(0.01f);
Assert.AreEqual(0, domain.ContactSets.Count);
a.Enabled = false;
b.Enabled = true;
domain.Update(0.01f);
Assert.AreEqual(0, domain.ContactSets.Count);
a.Enabled = true;
((CollisionFilter) domain.CollisionDetection.CollisionFilter).Set(a, b, false);
domain.Update(0.01f);
Assert.AreEqual(0, domain.ContactSets.Count);
((CollisionFilter) domain.CollisionDetection.CollisionFilter).Set(a, b, true);
domain.Update(0.01f);
Assert.AreEqual(1, domain.ContactSets.Count);
}
/// <summary>
/// Updates the scene.
/// </summary>
/// <param name="deltaTime">The time step size in seconds.</param>
/// <remarks>
/// A scene needs to be updated once per frame. The method recomputes the internal information
/// that is used for scene queries (such as frustum culling) and may perform other
/// optimizations.
/// </remarks>
public void Update(TimeSpan deltaTime)
{
// Clear bins.
foreach (var query in _queries)
{
var referenceNode = query.ReferenceNode;
if (referenceNode != null)
{
referenceNode.ClearFlag(SceneNodeFlags.IsDirtyScene);
}
query.Reset();
}
// Update collisions.
_collisionDomain.Update(deltaTime);
}
protected override void OnUpdate(TimeSpan deltaTime)
{
// Update direction of picking object.
((GeometricObject)_pickingObject.GeometricObject).Pose = _cameraObject.CameraNode.PoseWorld;
// TODO: If figureNodes can move or scale, we have to copy the new Pose
// and Scale from the FigureNodes to their CollisionObjects.
_collisionDomain.Update(deltaTime);
// Reset colors of figure nodes that where "picked" in the last frame.
// TODO: To make this faster, loop over the contact objects of the last
// frame and not over all nodes in the scene.
foreach (var figureNode in _scene.GetDescendants().OfType <FigureNode>())
{
// Figure nodes which were picked, have the color info in the UserData.
if (figureNode.UserData != null)
{
figureNode.StrokeColor = ((Pair <Vector3F>)figureNode.UserData).First;
figureNode.FillColor = ((Pair <Vector3F>)figureNode.UserData).Second;
figureNode.UserData = null;
}
}
// Change the color of all figure nodes which touch the picking object.
foreach (var pickedObject in _collisionDomain.GetContactObjects(_pickingObject))
{
var myGeometricObject = pickedObject.GeometricObject as FigureGeometricObject;
if (myGeometricObject != null)
{
var figureNode = myGeometricObject.FigureNode;
_debugRenderer.DrawText("Picked node: " + figureNode.Name);
// Store original color in UserData.
figureNode.UserData = new Pair <Vector3F>(figureNode.StrokeColor, figureNode.FillColor);
// Change color.
figureNode.StrokeColor = new Vector3F(0.8f, 0.6f, 0.08f);
figureNode.FillColor = new Vector3F(1, 0.7f, 0.1f);
}
}
// Draw the picking object (for debugging).
_debugRenderer.DrawObject(_pickingObject.GeometricObject, Color.Red, true, false);
}
public override void Update(GameTime gameTime)
{
// Compute new collision information.
_domain.Update(gameTime.ElapsedGameTime);
// Update the character controller.
ControlCharacter((float)gameTime.ElapsedGameTime.TotalSeconds);
// Check whether the character controller touches the trigger volume.
// If there is a contact, we change the color of the trigger volume.
bool characterTouchesTriggerVolume = _domain.HaveContact(_character.CollisionObject, _triggerVolume);
// ----- Draw everything using the debug renderer of the graphics screen.
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
// Draw all geometric objects (except the trigger volume).
foreach (var collisionObject in _domain.CollisionObjects)
{
if (collisionObject != _triggerVolume)
{
debugRenderer.DrawObject(
collisionObject.GeometricObject,
GraphicsHelper.GetUniqueColor(collisionObject),
false,
false);
}
}
// For debugging: Draw contacts of the character capsule.
// Draw line to visualize contact normal.
debugRenderer.DrawContacts(_domain.ContactSets, 0.1f, Color.White, true);
// Draw trigger volume (transparent using alpha blending).
debugRenderer.DrawObject(
_triggerVolume.GeometricObject,
characterTouchesTriggerVolume ? new Color(255, 0, 0, 128) : new Color(255, 255, 255, 128),
false,
false);
}
public override void Update(GameTime gameTime)
{
var mousePosition = InputService.MousePosition;
var viewport = GraphicsService.GraphicsDevice.Viewport;
var cameraNode = GraphicsScreen.CameraNode;
// Update picking ray.
if (InputService.IsDown(Keys.LeftControl) || InputService.IsDown(Keys.RightControl))
{
// Pick using mouse cursor.
SampleFramework.IsMouseVisible = true;
GraphicsScreen.DrawReticle = false;
// Convert the mouse screen position on the near viewing plane into a
// world space position.
Vector3 rayStart = viewport.Unproject(
new Vector3(mousePosition.X, mousePosition.Y, 0),
cameraNode.Camera.Projection,
(Matrix)cameraNode.View,
Matrix.Identity);
// Convert the mouse screen position on the far viewing plane into a
// world space position.
Vector3 rayEnd = viewport.Unproject(
new Vector3(mousePosition.X, mousePosition.Y, 1),
cameraNode.Camera.Projection,
(Matrix)cameraNode.View,
Matrix.Identity);
// Update ray. The ray should start at the near viewing plane under the
// mouse cursor and shoot into viewing direction. Therefore we change the
// pose of the ray object such that the ray origin is at rayStart and the
// orientation rotates the ray from shooting into +x direction into a ray
// shooting in viewing direction (rayEnd - rayStart).
((GeometricObject)_ray.GeometricObject).Pose = new Pose(
(Vector3)rayStart,
Quaternion.CreateFromRotationMatrix(Vector3.Forward, (Vector3)(rayEnd - rayStart)));
}
else
{
// Pick using reticle.
SampleFramework.IsMouseVisible = false;
GraphicsScreen.DrawReticle = true;
((GeometricObject)_ray.GeometricObject).Pose = cameraNode.PoseWorld;
}
// Update collision domain. This computes new contact information.
_domain.Update(gameTime.ElapsedGameTime);
// Draw objects. Change the color if the ray hits the object.
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
if (_domain.HaveContact(_ray, _box))
{
debugRenderer.DrawObject(_box.GeometricObject, Color.Red, false, false);
}
else
{
debugRenderer.DrawObject(_box.GeometricObject, Color.White, false, false);
}
if (_domain.HaveContact(_ray, _sphere))
{
debugRenderer.DrawObject(_sphere.GeometricObject, Color.Red, false, false);
}
else
{
debugRenderer.DrawObject(_sphere.GeometricObject, Color.White, false, false);
}
if (_domain.HaveContact(_ray, _mesh))
{
debugRenderer.DrawObject(_mesh.GeometricObject, Color.Red, false, false);
}
else
{
debugRenderer.DrawObject(_mesh.GeometricObject, Color.White, false, false);
}
// For triangle meshes we also know which triangle was hit!
// Get the contact information for ray-mesh hits.
ContactSet rayMeshContactSet = _domain.ContactSets.GetContacts(_ray, _mesh);
// rayMeshContactSet is a collection of Contacts between ray and mesh.
// If rayMeshContactSet is null or it contains no Contacts, then we have no contact.
if (rayMeshContactSet != null && rayMeshContactSet.Count > 0)
{
// A contact set contains information for a pair of touching objects A and B.
// We know that the two objects are ray and mesh, but we do not know if A or B
// is the ray.
bool objectAIsRay = rayMeshContactSet.ObjectA == _ray;
// Get the contact.
Contact contact = rayMeshContactSet[0];
// Get the feature index of the mesh feature that was hit.
int featureIndex = objectAIsRay ? contact.FeatureB : contact.FeatureA;
// For triangle meshes the feature index is the index of the triangle that was hit.
// Get the triangle from the triangle mesh shape.
Triangle triangle = ((TriangleMeshShape)_mesh.GeometricObject.Shape).Mesh.GetTriangle(featureIndex);
debugRenderer.DrawShape(new TriangleShape(triangle), _mesh.GeometricObject.Pose, _mesh.GeometricObject.Scale, Color.Yellow, false, false);
}
}
public override void Update(GameTime gameTime)
{
// ----- Move object A with arrow keys.
// Compute displacement.
Vector3F displacement = Vector3F.Zero;
if (InputService.IsDown(Keys.Up))
{
displacement.Y += 0.1f;
}
if (InputService.IsDown(Keys.Left))
{
displacement.X -= 0.1f;
}
if (InputService.IsDown(Keys.Down))
{
displacement.Y -= 0.1f;
}
if (InputService.IsDown(Keys.Right))
{
displacement.X += 0.1f;
}
// Update the position of object A (green box).
Pose pose = _collisionObjectA.GeometricObject.Pose;
pose.Position += displacement;
// Note: We have to cast to GeometricObject because CollisionObject.GeometricObject
// is of type IGeometricObject which does not have a setter for the Pose property.
((GeometricObject)_collisionObjectA.GeometricObject).Pose = pose;
// ----- Update collision domain. This computes new contact information.
float timeStep = (float)gameTime.ElapsedGameTime.TotalSeconds;
_domain.Update(timeStep); // Needs to be called once per frame.
// ----- Draw objects using the DebugRenderer of the graphics screen.
// We reset the DebugRenderer every frame.
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
// Draw geometric objects. If an object has a contact with any other object,
// it is drawn yellow.
bool objectBHasContact = _domain.HasContact(_collisionObjectB);
var color = objectBHasContact ? Color.Yellow : Color.Blue;
debugRenderer.DrawObject(_collisionObjectB.GeometricObject, color, false, false);
bool objectCHasContact = _domain.HasContact(_collisionObjectC);
color = objectCHasContact ? Color.Yellow : Color.Red;
debugRenderer.DrawObject(_collisionObjectC.GeometricObject, color, false, false);
bool objectAHasContact = _domain.HasContact(_collisionObjectA);
color = objectAHasContact ? Color.Yellow : Color.Green;
debugRenderer.DrawObject(_collisionObjectA.GeometricObject, color, false, false);
// Get closest points.
// Closest-point queries are not used as often as contact queries. They are
// not computed by the collision domain. Therefore, we ask the collision
// detection for the closest points.
ContactSet closestPointsAB = _collisionDetection.GetClosestPoints(_collisionObjectA, _collisionObjectB);
ContactSet closestPointsAC = _collisionDetection.GetClosestPoints(_collisionObjectA, _collisionObjectC);
ContactSet closestPointsBC = _collisionDetection.GetClosestPoints(_collisionObjectB, _collisionObjectC);
// Draw closest points.
// Each contact set contains one contact that describes the closest-point pair.
debugRenderer.DrawPoint(closestPointsAB[0].PositionAWorld, Color.White, true);
debugRenderer.DrawPoint(closestPointsAB[0].PositionBWorld, Color.White, true);
debugRenderer.DrawPoint(closestPointsAC[0].PositionAWorld, Color.White, true);
debugRenderer.DrawPoint(closestPointsAC[0].PositionBWorld, Color.White, true);
debugRenderer.DrawPoint(closestPointsBC[0].PositionAWorld, Color.White, true);
debugRenderer.DrawPoint(closestPointsBC[0].PositionBWorld, Color.White, true);
// Draw lines that represent the minimal distances.
debugRenderer.DrawLine(closestPointsAB[0].PositionAWorld, closestPointsAB[0].PositionBWorld, Color.White, true);
debugRenderer.DrawLine(closestPointsAC[0].PositionAWorld, closestPointsAC[0].PositionBWorld, Color.White, true);
debugRenderer.DrawLine(closestPointsBC[0].PositionAWorld, closestPointsBC[0].PositionBWorld, Color.White, true);
}
private void UpdateContacts()
{
CollisionDomain.Update(_fixedTimeStep);
IslandManager.ContactSetLinks.Clear();
// Go through contacts and add contact constraints.
foreach (ContactSet contactSet in CollisionDomain.ContactSets)
{
RigidBody bodyA = contactSet.ObjectA.GeometricObject as RigidBody;
RigidBody bodyB = contactSet.ObjectB.GeometricObject as RigidBody;
if (bodyA != null && bodyB != null)
{
// Check if a dynamic body is involved and if collision response is enabled.
bool responseEnabled = (bodyA.MotionType == MotionType.Dynamic || bodyB.MotionType == MotionType.Dynamic)
&& bodyA.CollisionResponseEnabled
&& bodyB.CollisionResponseEnabled
&& (ResponseFilter == null || ResponseFilter.Filter(new Pair<RigidBody>(bodyA, bodyB)));
if (responseEnabled)
IslandManager.ContactSetLinks.Add(new Pair<RigidBody>(bodyA, bodyB));
int numberOfContacts = contactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
var contact = contactSet[i];
ContactConstraint constraint = contact.UserData as ContactConstraint;
if (constraint != null)
{
if (responseEnabled)
{
// Contact constraint is still in use.
// --> Mark contact constraint as active.
constraint.Used = true;
}
else
{
// The response was disabled.
// --> Remove an old constact constraint.
contact.UserData = null;
}
}
else if (responseEnabled)
{
// Create a new contact constraint.
constraint = ContactConstraint.Create(bodyA, bodyB, contact);
contact.UserData = constraint;
ContactConstraintsInternal.Add(constraint);
constraint.Used = true;
}
}
}
}
// ----- Recycle old contact constraints.
int numberOfConstraints = ContactConstraintsInternal.Count;
int numberOfUsedConstraints = numberOfConstraints;
for (int i = numberOfConstraints - 1; i >= 0; i--)
{
var constraint = ContactConstraintsInternal[i];
if (constraint.Used)
{
// The contact constraint is still in use.
// Keep constraint and reset flag.
constraint.Used = false;
}
else
{
numberOfUsedConstraints--;
// Recycle contact constraint.
constraint.Recycle();
// The contact constraint is no longer in use.
// Swap a used constraint to this index.
ContactConstraintsInternal[i] = ContactConstraintsInternal[numberOfUsedConstraints];
// Not needed because we call List.RemoveRange for the end of the list.
//ContactConstraintsInternal[numberOfUsedConstraints] = constraint;
}
}
// Remove recycled contacts at end of list.
int numberOfUnusedConstraints = numberOfConstraints - numberOfUsedConstraints;
if (numberOfUnusedConstraints > 0)
{
ContactConstraintsInternal.RemoveRange(numberOfUsedConstraints, numberOfUnusedConstraints);
}
}
public CollisionFilterSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
SampleFramework.IsMouseVisible = false;
GraphicsScreen.ClearBackground = true;
GraphicsScreen.BackgroundColor = Color.Gray;
GraphicsScreen.DrawReticle = true;
SetCamera(new Vector3F(0, 0, 10), 0, 0);
// ----- Initialize collision detection system.
// We use one collision domain that manages all objects.
var domain = new CollisionDomain();
// Let's set a filter which disables collision between object in the same collision group.
// We can use a broad phase or a narrow phase filter:
// Option A) Broad phase filter
// The collision detection broad phase computes bounding box overlaps.
// A broad phase filter is best used if the filtering rules are simple and do not change
// during the runtime of your application.
//domain.BroadPhase.Filter = new DelegatePairFilter<CollisionObject>(
// pair => pair.First.CollisionGroup != pair.Second.CollisionGroup);
// Option B) Narrow phase filter
// The collision detection narrow phase computes contacts between objects where the broad
// phase has detected a bounding box overlap. Use a narrow phase filter if the filtering rules
// are complex or can change during the runtime of your application.
var filter = new CollisionFilter();
// Disable collision between objects in the same groups.
filter.Set(0, 0, false);
filter.Set(1, 1, false);
filter.Set(2, 2, false);
filter.Set(3, 3, false);
domain.CollisionDetection.CollisionFilter = filter;
// Create a random list of points.
var points = new List <Vector3F>();
for (int i = 0; i < 100; i++)
{
points.Add(RandomHelper.Random.NextVector3F(-1.5f, 1.5f));
}
// Add lots of spheres to the collision domain. Assign spheres to different collision groups.
var random = new Random();
var sphereShape = new SphereShape(0.7f);
for (int i = 0; i < 20; i++)
{
var randomPosition = new Vector3F(random.NextFloat(-6, 6), random.NextFloat(-3, 3), 0);
var geometricObject = new GeometricObject(sphereShape, new Pose(randomPosition));
var collisionObject = new CollisionObject(geometricObject)
{
// A collision group is simply an integer. We can assign collision objects to collision
// groups to control the collision filtering.
CollisionGroup = random.NextInteger(0, 3),
};
domain.CollisionObjects.Add(collisionObject);
}
// Compute collisions. (The objects do not move in this sample. Therefore, we only have to
// call Update once.)
domain.Update(0);
// Draw objects. The color depends on the collision group.
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
foreach (var collisionObject in domain.CollisionObjects)
{
Color color;
switch (collisionObject.CollisionGroup)
{
case 0: color = Color.LightBlue; break;
case 1: color = Color.Yellow; break;
case 2: color = Color.Orange; break;
default: color = Color.LightGreen; break;
}
debugRenderer.DrawObject(collisionObject.GeometricObject, color, false, false);
}
debugRenderer.DrawContacts(domain.ContactSets, 0.1f, Color.Red, true);
}
public override void Update(GameTime gameTime)
{
_domain.EnableMultithreading = InputService.IsDown(Keys.Space);
MessageBox.Show("Start");
_deltaTime = 1 / 60f;
for (int bla = 0; bla < 10000; bla++)
{
if (ClosestPointQueriesEnabled)
{
// Here we run closest point queries on all object pairs.
// We compare the results with the contact queries.
for (int i = 0; i < _domain.CollisionObjects.Count; i++)
{
for (int j = i + 1; j < _domain.CollisionObjects.Count; j++)
{
CollisionObject a = _domain.CollisionObjects[i];
CollisionObject b = _domain.CollisionObjects[j];
ContactSet closestPointQueryResult = _domain.CollisionDetection.GetClosestPoints(a, b);
ContactSet contactSet = _domain.GetContacts(a, b);
// Ignore height fields and rays.
if (a.GeometricObject.Shape is HeightField || b.GeometricObject.Shape is HeightField)
break;
if (a.GeometricObject.Shape is RayShape || b.GeometricObject.Shape is RayShape)
break;
if (contactSet == null || !contactSet.HaveContact)
{
// No contact in contactSet
if (closestPointQueryResult.HaveContact)
{
// Contact in closest point query. Results are inconsistent.
if (closestPointQueryResult.Count > 0
&& closestPointQueryResult[0].PenetrationDepth > 0.001f)
Debugger.Break();
}
}
else if (!closestPointQueryResult.HaveContact)
{
// contact in contact query, but no contact in closest point query.
// We allow a deviation within a small tolerance.
if (closestPointQueryResult.Count > 0 && contactSet.Count > 0
&& closestPointQueryResult[0].PenetrationDepth + contactSet[0].PenetrationDepth > 0.001f)
Debugger.Break();
}
}
}
}
// Reflect velocity if objects collide:
// The collision domain contains a ContactSet for each pair of touching objects.
foreach (var contactSet in _domain.ContactSets)
{
// Get the touching objects.
var moA = (MovingGeometricObject)contactSet.ObjectA.GeometricObject;
var moB = (MovingGeometricObject)contactSet.ObjectB.GeometricObject;
// Reflect only at boundary objects.
if (!(moA.Shape is PlaneShape) && !(moB.Shape is PlaneShape)
&& !(moA.Shape is HeightField) && !(moB.Shape is HeightField))
continue;
// Get normal vector. If objects are sensors, the contact set does not tell us
// the right normal.
Vector3 normal = Vector3.Zero;
if (contactSet.Count > 0)
{
// Take normal from contact set.
normal = contactSet[0].Normal;
}
else
{
// If we use Trigger CollisionObjects we do not have contacts. --> Reflect at
// bounding planes.
if (moA.Shape is PlaneShape)
normal = ((PlaneShape)moA.Shape).Normal;
else if (moB.Shape is PlaneShape)
normal = -((PlaneShape)moB.Shape).Normal;
else if (moA.Shape is HeightField)
normal = Vector3.UnitY;
else
normal = -Vector3.UnitY;
}
//else if (moA.Shape is Plane || moB.Shape is Plane )
//{
// // Use plane normal.
// IGeometricObject plane = moA.Shape is Plane ? moA : moB;
// normal = plane.Pose.ToWorldDirection(((Plane)plane.Shape).Normal);
// if (moB == plane)
// normal = -normal;
//}
//else if (moA.Shape is HeightField || moB.Shape is HeightField)
//{
// // Use up-vector for height field contacts.
// normal = Vector3.UnitY;
// if (moB.Shape is HeightField)
// normal = -normal;
//}
//else
//{
// // Use random normal.
// normal = RandomHelper.NextVector3(-1, 1).Normalized;
//}
// Check if the objects move towards or away from each other in the direction of the normal.
if (normal != Vector3.Zero && Vector3.Dot(moB.LinearVelocity - moA.LinearVelocity, normal) <= 0)
{
// Objects move towards each other. --> Reflect their velocities.
moA.LinearVelocity -= 2 * Vector3.ProjectTo(moA.LinearVelocity, normal);
moB.LinearVelocity -= 2 * Vector3.ProjectTo(moB.LinearVelocity, normal);
moA.AngularVelocity = -moA.AngularVelocity;
moB.AngularVelocity = -moB.AngularVelocity;
}
}
// Get the size of the current time step.
float timeStep = (float)gameTime.ElapsedGameTime.TotalSeconds;
// Move objects.
var objects = _domain.CollisionObjects.Select(co => co.GeometricObject).OfType<MovingGeometricObject>();
foreach (var obj in objects)
{
// Update position.
Vector3 position = obj.Pose.Position + obj.LinearVelocity * timeStep;
// Update rotation.
Vector3 rotationAxis = obj.AngularVelocity;
float angularSpeed = obj.AngularVelocity.Length;
Matrix rotation = (Numeric.IsZero(angularSpeed))
? Matrix.Identity
: Matrix.CreateRotation(rotationAxis, angularSpeed * timeStep);
var orientation = rotation * obj.Pose.Orientation;
// Incrementally updating the rotation matrix will eventually create a
// matrix which is not a rotation matrix anymore because of numerical
// problems. Re-othogonalization make sure that the matrix represents a
// rotation.
orientation.Orthogonalize();
obj.Pose = new Pose(position, orientation);
}
// Update collision domain. This computes new contact information.
_domain.Update(timeStep);
MessageBox.Show("Finished");
Exit();
// Record some statistics.
int numberOfObjects = _domain.CollisionObjects.Count;
Profiler.SetFormat("NumObjects", 1, "The total number of objects.");
Profiler.AddValue("NumObjects", numberOfObjects);
// If there are n objects, we can have max. n * (n - 1) / 2 collisions.
Profiler.SetFormat("NumObjectPairs", 1, "The number of objects pairs, which have to be checked.");
Profiler.AddValue("NumObjectPairs", numberOfObjects * (numberOfObjects - 1f) / 2f);
// The first part of the collision detection is the "broad-phase" which
// filters out objects that cannot collide (e.g. using a fast bounding box test).
Profiler.SetFormat("BroadPhasePairs", 1, "The number of overlaps reported by the broad phase.");
Profiler.AddValue("BroadPhasePairs", _domain.NumberOfBroadPhaseOverlaps);
// Finally, the collision detection computes the exact contact information and creates
// a ContactSet with the Contacts for each pair of colliding objects.
Profiler.SetFormat("ContactSetCount", 1, "The number of actual collisions.");
Profiler.AddValue("ContactSetCount", _domain.ContactSets.Count);
// Draw objects using the DebugRenderer of the graphics screen.
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
foreach (var collisionObject in _domain.CollisionObjects)
debugRenderer.DrawObject(collisionObject.GeometricObject, GraphicsHelper.GetUniqueColor(collisionObject), false, false);
}
}
public void UpdateSingle()
{
CollisionDomain domain = new CollisionDomain(new CollisionDetection());
// Add 100 random objects.
for (int i = 0; i < 100; i++)
domain.CollisionObjects.Add(new CollisionObject(new GeometricObject(new SphereShape(1), new Pose(RandomHelper.Random.NextVector3F(0, 20)))));
for (int i = 0; i < 100; i++)
{
domain.Update(domain.CollisionObjects[33]);
for (int j = 0; j < domain.CollisionObjects.Count; j++)
{
var a = domain.CollisionObjects[j];
for (int k = j + 1; k < domain.CollisionObjects.Count; k++)
{
var b = domain.CollisionObjects[k];
Assert.AreEqual(domain.InternalBroadPhase.CandidatePairs.Contains(a, b),
GeometryHelper.HaveContact(a.GeometricObject.Aabb, b.GeometricObject.Aabb));
}
}
// Set new random position for one.
((GeometricObject)domain.CollisionObjects[33].GeometricObject).Pose = new Pose(RandomHelper.Random.NextVector3F(0, 20));
Console.WriteLine("Candidate pairs: " + domain.InternalBroadPhase.CandidatePairs.Count);
}
}
public void HaveContact()
{
CollisionDomain domain = new CollisionDomain(new CollisionDetection());
domain.CollisionDetection.CollisionFilter = new CollisionFilter();
CollisionObject a = new CollisionObject();
((GeometricObject)a.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0));
//a.Name = "a";
CollisionObject b = new CollisionObject();
((GeometricObject)b.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(1, 0, 0f));
//b.Name = "b";
CollisionObject c = new CollisionObject();
((GeometricObject)c.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)c.GeometricObject).Pose = new Pose(new Vector3F(1, 0, 0f));
//c.Name = "c";
domain.CollisionObjects.Add(a);
domain.CollisionObjects.Add(b);
domain.Update(0.01f);
Assert.AreEqual(true, domain.HaveContact(a, b));
Assert.AreEqual(true, domain.HaveContact(a, c));
Assert.AreEqual(true, domain.HasContact(a));
Assert.AreEqual(true, domain.HasContact(b));
Assert.AreEqual(true, domain.HasContact(c));
Assert.AreEqual(1, domain.GetContacts(a, b).Count);
Assert.AreEqual(1, domain.GetContacts(a, c).Count);
Assert.AreEqual(1, domain.GetContacts(a).Count());
Assert.AreEqual(2, domain.GetContacts(c).Count());
Assert.AreEqual(1, domain.ContactSets.Count);
b.Enabled = false;
domain.Update(0.01f);
Assert.AreEqual(false, domain.HaveContact(a, b));
Assert.AreEqual(true, domain.HaveContact(a, c));
Assert.AreEqual(false, domain.HasContact(a));
Assert.AreEqual(false, domain.HasContact(b));
Assert.AreEqual(true, domain.HasContact(c));
Assert.AreEqual(null, domain.GetContacts(a, b));
Assert.AreEqual(1, domain.GetContacts(a, c).Count);
Assert.AreEqual(0, domain.GetContacts(a).Count());
Assert.AreEqual(1, domain.GetContacts(c).Count());
Assert.AreEqual(0, domain.ContactSets.Count);
a.Enabled = false;
b.Enabled = true;
domain.Update(0.01f);
Assert.AreEqual(false, domain.HaveContact(a, b));
Assert.AreEqual(false, domain.HaveContact(a, c));
Assert.AreEqual(false, domain.HasContact(a));
Assert.AreEqual(false, domain.HasContact(b));
Assert.AreEqual(true, domain.HasContact(c));
Assert.AreEqual(null, domain.GetContacts(a, b));
Assert.AreEqual(null, domain.GetContacts(a, c));
Assert.AreEqual(0, domain.GetContacts(a).Count());
Assert.AreEqual(1, domain.GetContacts(c).Count());
Assert.AreEqual(0, domain.ContactSets.Count);
c.Enabled = false;
domain.Update(0.01f);
Assert.AreEqual(false, domain.HaveContact(a, b));
Assert.AreEqual(false, domain.HaveContact(a, c));
Assert.AreEqual(false, domain.HasContact(a));
Assert.AreEqual(false, domain.HasContact(b));
Assert.AreEqual(false, domain.HasContact(c));
Assert.AreEqual(null, domain.GetContacts(a, b));
Assert.AreEqual(null, domain.GetContacts(a, c));
Assert.AreEqual(0, domain.GetContacts(a).Count());
Assert.AreEqual(0, domain.GetContacts(c).Count());
Assert.AreEqual(0, domain.ContactSets.Count);
a.Enabled = true;
c.Enabled = true;
((CollisionFilter) domain.CollisionDetection.CollisionFilter).Set(a, b, false);
domain.Update(0.01f);
Assert.AreEqual(false, domain.HaveContact(a, b));
Assert.AreEqual(true, domain.HaveContact(a, c));
Assert.AreEqual(false, domain.HasContact(a));
Assert.AreEqual(false, domain.HasContact(b));
Assert.AreEqual(true, domain.HasContact(c));
Assert.AreEqual(null, domain.GetContacts(a, b));
Assert.AreEqual(1, domain.GetContacts(a, c).Count);
Assert.AreEqual(0, domain.GetContacts(a).Count());
Assert.AreEqual(2, domain.GetContacts(c).Count());
Assert.AreEqual(0, domain.ContactSets.Count);
((CollisionFilter) domain.CollisionDetection.CollisionFilter).Set(a, b, true);
domain.Update(0.01f);
Assert.AreEqual(true, domain.HaveContact(a, b));
Assert.AreEqual(true, domain.HaveContact(a, c));
Assert.AreEqual(true, domain.HasContact(a));
Assert.AreEqual(true, domain.HasContact(b));
Assert.AreEqual(true, domain.HasContact(c));
Assert.AreEqual(1, domain.GetContacts(a, b).Count);
Assert.AreEqual(1, domain.GetContacts(a, c).Count);
Assert.AreEqual(1, domain.GetContacts(a).Count());
Assert.AreEqual(2, domain.GetContacts(c).Count());
Assert.AreEqual(1, domain.ContactSets.Count);
c.Enabled = false;
domain.Update(0.01f);
Assert.AreEqual(true, domain.HaveContact(a, b));
Assert.AreEqual(false, domain.HaveContact(a, c));
Assert.AreEqual(true, domain.HasContact(a));
Assert.AreEqual(true, domain.HasContact(b));
Assert.AreEqual(false, domain.HasContact(c));
Assert.AreEqual(1, domain.GetContacts(a, b).Count);
Assert.AreEqual(null, domain.GetContacts(a, c));
Assert.AreEqual(1, domain.GetContacts(a).Count());
Assert.AreEqual(0, domain.GetContacts(c).Count());
Assert.AreEqual(1, domain.ContactSets.Count);
}
public override void Update(GameTime gameTime)
{
// <T> --> Toggle between bird's-eye view and camera view.
if (InputService.IsPressed(Keys.T, true))
{
_topViewEnabled = !_topViewEnabled;
if (_topViewEnabled)
{
GraphicsScreen.CameraNode = _topDownCameraNode;
}
else
{
GraphicsScreen.CameraNode = _sceneCameraNode;
}
}
// <C> --> Enable or disable frustum culling.
if (InputService.IsPressed(Keys.C, true))
{
_cullingEnabled = !_cullingEnabled;
}
// Elapsed time since the last frame:
float timeStep = (float)gameTime.ElapsedGameTime.TotalSeconds;
// We update the camera movement target all 10 seconds.
const float cameraTargetUpdateInterval = 10;
// Get the current camera position.
var currentPosition = _sceneCameraNode.PoseWorld.Position;
var currentOrientation = _sceneCameraNode.PoseWorld.Orientation;
// Update the camera movement. We move a fraction of the targetMovement / targetRotation.
_sceneCameraNode.PoseWorld = new Pose(
currentPosition + _cameraTargetMovement * timeStep / cameraTargetUpdateInterval,
Matrix33F.CreateRotationY(_cameraTargetRotation * timeStep / cameraTargetUpdateInterval) * currentOrientation);
// When the cameraTargetUpdateInterval has passed, we choose a new random movement
// vector and rotation angle.
_cameraTargetUpdateTime += timeStep;
if (_cameraTargetUpdateTime > cameraTargetUpdateInterval)
{
_cameraTargetUpdateTime = 0;
// Get random rotation angle.
_cameraTargetRotation = RandomHelper.Random.NextFloat(-ConstantsF.TwoPi, ConstantsF.TwoPi);
// Get a random movement vector. We get random vector until we have a movement vector
// that does not move the camera outside of the level boundaries.
do
{
_cameraTargetMovement = RandomHelper.Random.NextVector3F(-LevelSize, LevelSize);
_cameraTargetMovement.Y = 0;
} while (Math.Abs(_sceneCameraNode.PoseWorld.Position.X + _cameraTargetMovement.X) > LevelSize / 2 ||
Math.Abs(_sceneCameraNode.PoseWorld.Position.Z + _cameraTargetMovement.Z) > LevelSize / 2);
}
// Update collision domain.
if (_cullingEnabled)
{
_domain.Update((float)gameTime.ElapsedGameTime.TotalSeconds);
}
// Render objects.
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
debugRenderer.DrawText("\n\nCulling " + (_cullingEnabled ? "Enabled" : "Disabled"));
// Draw frustum.
debugRenderer.DrawObject(_sceneCameraNode, Color.Red, true, false);
if (!_cullingEnabled)
{
Profiler.Start("NoCull");
// Simply render all objects.
// Frustum culling is not used, so we render ALL objects. Most of them will not
// be visible in the _sceneCamera and a lot of rendering time is wasted.
foreach (var collisionObject in _domain.CollisionObjects)
{
var geometricObject = collisionObject.GeometricObject;
debugRenderer.DrawObject(geometricObject, Color.Red, false, false);
}
Profiler.Stop("NoCull");
}
else
{
if (_topViewEnabled)
{
// Render all objects just for debugging.
foreach (var collisionObject in _domain.CollisionObjects)
{
var geometricObject = collisionObject.GeometricObject;
debugRenderer.DrawObject(geometricObject, Color.White, false, false);
}
}
// Use frustum culling:
Profiler.Start("WithCull");
// Get the combined WorldViewProjection matrix of the camera.
Matrix44F worldViewProjection = _sceneCameraNode.Camera.Projection.ToMatrix44F() * _sceneCameraNode.PoseWorld.Inverse;
// Extract the frustum planes of the camera.
_planes.Clear();
GeometryHelper.ExtractPlanes(worldViewProjection, _planes, false);
// Get the broad phase partition.
var partition = (DualPartition <CollisionObject>)_domain.BroadPhase;
// ----- Frustum Culling:
// Use the broad phase partition to get all objects where the axis-aligned
// bounding box (AABB) overlaps the volume defined by the frustum planes.
// We draw these objects and can ignore all other objects.
foreach (var collisionObject in partition.GetOverlaps(_planes))
{
var geometricObject = collisionObject.GeometricObject;
debugRenderer.DrawObject(geometricObject, Color.Red, false, false);
}
Profiler.Stop("WithCull");
}
}
public void TestEnabledDisabled()
{
var shape = new SphereShape(1);
var goA = new GeometricObject(shape, Pose.Identity);
var coA = new CollisionObject(goA);
var goB = new GeometricObject(shape, Pose.Identity);
var coB = new CollisionObject(goB);
var cd = new CollisionDomain(new CollisionDetection());
cd.CollisionObjects.Add(coA);
cd.CollisionObjects.Add(coB);
// not touching
goB.Pose = new Pose(new Vector3F(3, 3, 0));
cd.Update(0);
Assert.AreEqual(0, cd.ContactSets.Count);
// not touching, disabled
coB.Enabled = false;
cd.Update(0);
Assert.AreEqual(0, cd.ContactSets.Count);
// touching, disabled
goB.Pose = new Pose(new Vector3F(1, 1, 1));
cd.Update(0);
Assert.AreEqual(0, cd.ContactSets.Count);
// touching, enabled
coB.Enabled = true;
cd.Update(0);
Assert.AreEqual(1, cd.ContactSets.Count);
// not touching - but broadphase overlap, enabled
goB.Pose = new Pose(new Vector3F(1.8f, 1.8f, 0));
cd.Update(0);
Assert.AreEqual(0, cd.ContactSets.Count);
// not touching, disabled
coB.Enabled = false;
cd.Update(0);
Assert.AreEqual(0, cd.ContactSets.Count);
// touching, disabled
goB.Pose = new Pose(new Vector3F(1, 1, 1));
cd.Update(0);
Assert.AreEqual(0, cd.ContactSets.Count);
// touching, enabled
coB.Enabled = true;
cd.Update(0);
Assert.AreEqual(1, cd.ContactSets.Count);
}
public void TestEnabledDisabledStochastic()
{
// Test random Enabled and Pose values.
int numberOfObjects = 20;
int numberOfSteps = 1000;
Shape shape = new SphereShape(1);
//var meshShape = new TriangleMeshShape(shape.GetMesh(0.01f, 4));
//meshShape.Partition = new AabbTree<int>();
//shape = meshShape;
var geometricObjects = new GeometricObject[numberOfObjects];
var collisionObjects = new CollisionObject[numberOfObjects];
var domain = new CollisionDomain(new CollisionDetection());
for (int i = 0; i < numberOfObjects; i++)
{
geometricObjects[i] = new GeometricObject(shape);
collisionObjects[i] = new CollisionObject(geometricObjects[i]);
domain.CollisionObjects.Add(collisionObjects[i]);
}
for (int i = 0; i < numberOfSteps; i++)
{
for (int j = 0; j < numberOfObjects; j++)
{
collisionObjects[j].Enabled = RandomHelper.Random.NextBool();
domain.Update(0);
if (RandomHelper.Random.NextFloat(0, 1) > 0.5f)
geometricObjects[j].Pose = new Pose(RandomHelper.Random.NextVector3F(-2, 2));
domain.Update(0);
}
domain.Update(0);
domain.Update(0);
domain.Update(0);
domain.Update(0);
domain.Update(0);
domain.Update(0);
domain.Update(0);
// Compare result with brute-force check.
for (int j = 0; j < numberOfObjects; j++)
{
for (int k = j + 1; k < numberOfObjects; k++)
{
var haveContact = domain.CollisionDetection.HaveContact(collisionObjects[j], collisionObjects[k]);
Assert.AreEqual(haveContact, domain.ContactSets.GetContacts(collisionObjects[j], collisionObjects[k]) != null);
}
}
}
}
public void HaveContact()
{
CollisionDomain domain = new CollisionDomain(new CollisionDetection());
domain.CollisionDetection.CollisionFilter = new CollisionFilter();
CollisionObject a = new CollisionObject();
((GeometricObject)a.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3(0, 0, 0));
//a.Name = "a";
CollisionObject b = new CollisionObject();
((GeometricObject)b.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3(1, 0, 0f));
//b.Name = "b";
CollisionObject c = new CollisionObject();
((GeometricObject)c.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)c.GeometricObject).Pose = new Pose(new Vector3(1, 0, 0f));
//c.Name = "c";
domain.CollisionObjects.Add(a);
domain.CollisionObjects.Add(b);
domain.Update(0.01f);
Assert.AreEqual(true, domain.HaveContact(a, b));
Assert.AreEqual(true, domain.HaveContact(a, c));
Assert.AreEqual(true, domain.HasContact(a));
Assert.AreEqual(true, domain.HasContact(b));
Assert.AreEqual(true, domain.HasContact(c));
Assert.AreEqual(1, domain.GetContacts(a, b).Count);
Assert.AreEqual(1, domain.GetContacts(a, c).Count);
Assert.AreEqual(1, domain.GetContacts(a).Count());
Assert.AreEqual(2, domain.GetContacts(c).Count());
Assert.AreEqual(1, domain.ContactSets.Count);
b.Enabled = false;
domain.Update(0.01f);
Assert.AreEqual(false, domain.HaveContact(a, b));
Assert.AreEqual(true, domain.HaveContact(a, c));
Assert.AreEqual(false, domain.HasContact(a));
Assert.AreEqual(false, domain.HasContact(b));
Assert.AreEqual(true, domain.HasContact(c));
Assert.AreEqual(null, domain.GetContacts(a, b));
Assert.AreEqual(1, domain.GetContacts(a, c).Count);
Assert.AreEqual(0, domain.GetContacts(a).Count());
Assert.AreEqual(1, domain.GetContacts(c).Count());
Assert.AreEqual(0, domain.ContactSets.Count);
a.Enabled = false;
b.Enabled = true;
domain.Update(0.01f);
Assert.AreEqual(false, domain.HaveContact(a, b));
Assert.AreEqual(false, domain.HaveContact(a, c));
Assert.AreEqual(false, domain.HasContact(a));
Assert.AreEqual(false, domain.HasContact(b));
Assert.AreEqual(true, domain.HasContact(c));
Assert.AreEqual(null, domain.GetContacts(a, b));
Assert.AreEqual(null, domain.GetContacts(a, c));
Assert.AreEqual(0, domain.GetContacts(a).Count());
Assert.AreEqual(1, domain.GetContacts(c).Count());
Assert.AreEqual(0, domain.ContactSets.Count);
c.Enabled = false;
domain.Update(0.01f);
Assert.AreEqual(false, domain.HaveContact(a, b));
Assert.AreEqual(false, domain.HaveContact(a, c));
Assert.AreEqual(false, domain.HasContact(a));
Assert.AreEqual(false, domain.HasContact(b));
Assert.AreEqual(false, domain.HasContact(c));
Assert.AreEqual(null, domain.GetContacts(a, b));
Assert.AreEqual(null, domain.GetContacts(a, c));
Assert.AreEqual(0, domain.GetContacts(a).Count());
Assert.AreEqual(0, domain.GetContacts(c).Count());
Assert.AreEqual(0, domain.ContactSets.Count);
a.Enabled = true;
c.Enabled = true;
((CollisionFilter)domain.CollisionDetection.CollisionFilter).Set(a, b, false);
domain.Update(0.01f);
Assert.AreEqual(false, domain.HaveContact(a, b));
Assert.AreEqual(true, domain.HaveContact(a, c));
Assert.AreEqual(false, domain.HasContact(a));
Assert.AreEqual(false, domain.HasContact(b));
Assert.AreEqual(true, domain.HasContact(c));
Assert.AreEqual(null, domain.GetContacts(a, b));
Assert.AreEqual(1, domain.GetContacts(a, c).Count);
Assert.AreEqual(0, domain.GetContacts(a).Count());
Assert.AreEqual(2, domain.GetContacts(c).Count());
Assert.AreEqual(0, domain.ContactSets.Count);
((CollisionFilter)domain.CollisionDetection.CollisionFilter).Set(a, b, true);
domain.Update(0.01f);
Assert.AreEqual(true, domain.HaveContact(a, b));
Assert.AreEqual(true, domain.HaveContact(a, c));
Assert.AreEqual(true, domain.HasContact(a));
Assert.AreEqual(true, domain.HasContact(b));
Assert.AreEqual(true, domain.HasContact(c));
Assert.AreEqual(1, domain.GetContacts(a, b).Count);
Assert.AreEqual(1, domain.GetContacts(a, c).Count);
Assert.AreEqual(1, domain.GetContacts(a).Count());
Assert.AreEqual(2, domain.GetContacts(c).Count());
Assert.AreEqual(1, domain.ContactSets.Count);
c.Enabled = false;
domain.Update(0.01f);
Assert.AreEqual(true, domain.HaveContact(a, b));
Assert.AreEqual(false, domain.HaveContact(a, c));
Assert.AreEqual(true, domain.HasContact(a));
Assert.AreEqual(true, domain.HasContact(b));
Assert.AreEqual(false, domain.HasContact(c));
Assert.AreEqual(1, domain.GetContacts(a, b).Count);
Assert.AreEqual(null, domain.GetContacts(a, c));
Assert.AreEqual(1, domain.GetContacts(a).Count());
Assert.AreEqual(0, domain.GetContacts(c).Count());
Assert.AreEqual(1, domain.ContactSets.Count);
}
public void RayCastStopsAtFirstHitWhenChangingFilter()
{
CollisionDomain domain = new CollisionDomain(new CollisionDetection());
domain.CollisionDetection.CollisionFilter = new CollisionFilter();
// 1 ray: at origin shooting into +x
CollisionObject ray = new CollisionObject();
((GeometricObject)ray.GeometricObject).Shape = new RayShape(new Vector3F(), new Vector3F(1, 0, 0), 100)
{
StopsAtFirstHit = true,
};
//ray.Name = "Ray";
// 2 spheres: at at x=10, b at x=20
CollisionObject a = new CollisionObject();
((GeometricObject)a.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(10, 0, 0f));
//a.Name = "b";
CollisionObject b = new CollisionObject();
((GeometricObject)b.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(20, 0, 0f));
//b.Name = "c";
domain.CollisionObjects.Add(ray);
domain.CollisionObjects.Add(a);
domain.CollisionObjects.Add(b);
// Ray touches b.
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, a));
Assert.AreEqual(false, domain.HaveContact(ray, b));
// Disable collisions between ray and a.
// Then ray must hit b.
((CollisionFilter)domain.CollisionDetection.CollisionFilter).Set(ray, a, false);
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(false, domain.HaveContact(ray, a));
Assert.AreEqual(true, domain.HaveContact(ray, b));
}
public void RayCastStopsAtFirstHit()
{
CollisionDomain domain = new CollisionDomain(new CollisionDetection());
CollisionObject ray = new CollisionObject();
((GeometricObject)ray.GeometricObject).Shape = new RayShape(new Vector3F(), new Vector3F(1, 0, 0), 100)
{
StopsAtFirstHit = true,
};
//ray.Name = "Ray";
CollisionObject b = new CollisionObject();
((GeometricObject)b.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(-10, 0, 0f));
//b.Name = "b";
CollisionObject c = new CollisionObject();
((GeometricObject)c.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)c.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0f));
//c.Name = "c";
CollisionObject d = new CollisionObject();
((GeometricObject)d.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)d.GeometricObject).Pose = new Pose(new Vector3F(10, 0, 0f));
//d.Name = "d";
CollisionObject e = new CollisionObject();
((GeometricObject)e.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)e.GeometricObject).Pose = new Pose(new Vector3F(20, 0, 0f));
//e.Name = "e";
CollisionObject f = new CollisionObject();
((GeometricObject)f.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)f.GeometricObject).Pose = new Pose(new Vector3F(110, 0, 0f));
//f.Name = "f";
// Positions: b=-10, c=0, d=10, e=20, f=110
domain.CollisionObjects.Add(ray);
domain.CollisionObjects.Add(b);
domain.CollisionObjects.Add(d);
domain.CollisionObjects.Add(c);
domain.CollisionObjects.Add(e);
domain.CollisionObjects.Add(f);
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, c));
((GeometricObject)c.GeometricObject).Pose = new Pose(new Vector3F(30));
// Positions: b=-10, d=10, e=20, c=30, f=110
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, d));
((GeometricObject)d.GeometricObject).Pose = new Pose(new Vector3F(40));
// Positions: b=-10, e=20, c=30, d=40, f=110
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, e));
((GeometricObject)ray.GeometricObject).Pose = new Pose(((GeometricObject)ray.GeometricObject).Pose.Position, QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
domain.Update(0.01f);
Assert.AreEqual(0, domain.GetContacts(ray).Count());
((GeometricObject)ray.GeometricObject).Pose = new Pose(((GeometricObject)ray.GeometricObject).Pose.Position, QuaternionF.CreateRotationZ(ConstantsF.Pi));
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, b));
((GeometricObject)ray.GeometricObject).Pose = new Pose(((GeometricObject)ray.GeometricObject).Pose.Position, QuaternionF.Identity);
domain.Update(0.01f);
// Positions: b=-10, e=20, c=30, d=40, f=110
CollisionObject gNotInDomain = new CollisionObject();
((GeometricObject)gNotInDomain.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)gNotInDomain.GeometricObject).Pose = new Pose(new Vector3F(10, 0, 0f));
Assert.AreEqual(true, domain.HaveContact(ray, gNotInDomain));
Assert.AreEqual(1, domain.GetContacts(gNotInDomain).Count());
Assert.AreEqual(1, domain.GetContacts(ray, gNotInDomain).Count);
Assert.AreEqual(true, domain.HasContact(gNotInDomain));
((GeometricObject)gNotInDomain.GeometricObject).Pose = new Pose(new Vector3F(25, 0, 0f)); // behind e
Assert.AreEqual(false, domain.HaveContact(ray, gNotInDomain));
Assert.AreEqual(false, domain.HaveContact(gNotInDomain, ray));
Assert.AreEqual(false, domain.HasContact(gNotInDomain));
Assert.AreEqual(0, domain.GetContacts(gNotInDomain).Count());
Assert.IsNull(domain.GetContacts(ray, gNotInDomain));
Assert.IsNull(domain.GetContacts(gNotInDomain, ray));
// Remove ray from domain.
domain.CollisionObjects.Remove(ray);
domain.Update(0.01f);
Assert.AreEqual(0, domain.ContactSets.Count);
// Positions: b=-10, e=20, g=25, c=30, d=40, f=110
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, e));
Assert.AreEqual(false, domain.HaveContact(ray, c));
Assert.AreEqual(false, domain.HaveContact(ray, gNotInDomain));
Assert.IsNull(domain.GetContacts(ray, gNotInDomain));
}
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
// Move the character to the new desired position, sliding along obstacles and stepping
// automatically up and down. Gravity is applied.
public void Move(Vector3F desiredPosition,
float deltaTime, // The size of the time step in seconds.
bool jump) // True if character should jump.
{
// Remember the start position.
_oldPosition = Position;
// Desired movement vector:
Vector3F desiredMovement = desiredPosition - Position;
if (HasGroundContact())
{
// The character starts on the ground.
// Reset velocity from gravity.
_gravityVelocity = Vector3F.Zero;
// Add jump velocity or reset jump velocity.
if (jump)
{
_jumpVelocity = new Vector3F(0, 4, 0);
}
else
{
_jumpVelocity = Vector3F.Zero;
}
}
// Add up movement to desired movement.
desiredMovement += _jumpVelocity * deltaTime;
// Add down movement due to gravity.
_gravityVelocity += new Vector3F(0, -9.81f, 0) * deltaTime;
desiredMovement += _gravityVelocity * deltaTime;
// Compute the total desired position.
_desiredPosition = _oldPosition + desiredMovement;
bool isJumping = _jumpVelocity != Vector3F.Zero;
// Try to slide to desired position.
// If we are jumping we do not stop at the first obstacle. If we are not jumping
// Slide() should stop at the first obstacle because we can try to step up.
if (!Slide(!isJumping))
{
// Try to step up the allowed step height.
bool stepped = StepUp();
// If we could not step up, continue sliding.
if (!stepped)
{
Slide(false);
}
}
// If we are not jumping and do not touch the ground, try a down step.
if (!isJumping && !HasGroundContact())
{
StepDown();
}
// Limit amount of movement to the length of the desired movement.
// (Position corrections could have added additional movement.)
Vector3F actualMovement = Position - _oldPosition;
float desiredMovementLength = (_desiredPosition - _oldPosition).Length;
if (actualMovement.Length > desiredMovementLength)
{
// Correct length of movement.
Position = _oldPosition + actualMovement.Normalized * desiredMovementLength;
// Update collision detection info for new corrected Position.
_collisionDomain.Update(CollisionObject);
}
}
public void Test3()
{
CollisionDomain domain = new CollisionDomain(new CollisionDetection());
RandomHelper.Random = new Random(123456);
const float testAreaSize = 20;
// Add 100 random objects.
for (int i = 0; i < 100; i++)
domain.CollisionObjects.Add(new CollisionObject(new GeometricObject(new SphereShape(1), new Pose(RandomHelper.Random.NextVector3F(0, testAreaSize)))));
for (int i = 0; i < 100; i++)
{
domain.Update(0.03f);
for (int j = 0; j < domain.CollisionObjects.Count; j++)
{
var a = domain.CollisionObjects[j];
for (int k = j + 1; k < domain.CollisionObjects.Count; k++)
{
var b = domain.CollisionObjects[k];
bool contained = domain.InternalBroadPhase.CandidatePairs.Contains(a, b);
bool haveContact = GeometryHelper.HaveContact(a.GeometricObject.Aabb, b.GeometricObject.Aabb);
//if (contained != haveContact)
//Debugger.Break();
Assert.AreEqual(contained, haveContact);
}
// Set new random position for a few.
if (RandomHelper.Random.NextFloat(0, 1) < 0.7f)
((GeometricObject)a.GeometricObject).Pose = new Pose(RandomHelper.Random.NextVector3F(0, testAreaSize));
}
// Add new object.
domain.CollisionObjects.Add(new CollisionObject
{
GeometricObject = new GeometricObject
{
Shape = new SphereShape(1),
Pose = new Pose(RandomHelper.Random.NextVector3F(0, testAreaSize)),
}
});
// Remove random object.
domain.CollisionObjects.Remove(domain.CollisionObjects[RandomHelper.Random.NextInteger(0, domain.CollisionObjects.Count - 1)]);
Console.WriteLine("Candidate pairs: " + domain.InternalBroadPhase.CandidatePairs.Count);
}
}
public void Test1()
{
Assert.NotNull(new CollisionDomain().CollisionDetection);
Assert.NotNull(new CollisionDomain(null).CollisionDetection);
CollisionDomain cd = new CollisionDomain(new CollisionDetection());
CollisionObject a = new CollisionObject();
((GeometricObject)a.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3(0, 0, 0));
//a.Name = "a";
CollisionObject b = new CollisionObject();
((GeometricObject)b.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3(4, 0, 2f));
//b.Name = "b";
CollisionObject c = new CollisionObject();
((GeometricObject)c.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)c.GeometricObject).Pose = new Pose(new Vector3(6, 2, 2f));
//c.Name = "c";
CollisionObject d = new CollisionObject();
((GeometricObject)d.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)d.GeometricObject).Pose = new Pose(new Vector3(8, 3f, 4f));
//d.Name = "d";
Assert.AreEqual(0, cd.CollisionObjects.Count);
cd.CollisionObjects.Add(a);
Assert.AreEqual(1, cd.CollisionObjects.Count);
cd.CollisionObjects.Add(b);
Assert.AreEqual(2, cd.CollisionObjects.Count);
cd.CollisionObjects.Add(c);
Assert.AreEqual(3, cd.CollisionObjects.Count);
cd.CollisionObjects.Add(d);
Assert.AreEqual(4, cd.CollisionObjects.Count);
cd.Update(0.01f);
Assert.AreEqual(0, cd.ContactSets.Count);
((GeometricObject)a.GeometricObject).Pose = new Pose(((GeometricObject)a.GeometricObject).Pose.Position + new Vector3(3.5f, 0, 0.5f));
cd.Update(0.01f);
Assert.AreEqual(1, cd.ContactSets.Count);
((GeometricObject)c.GeometricObject).Pose = new Pose(((GeometricObject)c.GeometricObject).Pose.Position + new Vector3(2, 1, 0));
cd.Update(0.01f);
Assert.AreEqual(2, cd.ContactSets.Count);
((GeometricObject)a.GeometricObject).Pose = new Pose(((GeometricObject)a.GeometricObject).Pose.Position + new Vector3(3, 0, 0));
((GeometricObject)b.GeometricObject).Pose = new Pose(((GeometricObject)b.GeometricObject).Pose.Position + new Vector3(2, 0, 0));
cd.Update(0.01f);
Assert.AreEqual(2, cd.ContactSets.Count);
((GeometricObject)d.GeometricObject).Pose = new Pose(((GeometricObject)d.GeometricObject).Pose.Position + new Vector3(-0.5f, 0, 0));
((GeometricObject)b.GeometricObject).Pose = new Pose(((GeometricObject)b.GeometricObject).Pose.Position + new Vector3(0, 1, 0));
cd.Update(0.01f);
Assert.AreEqual(1, cd.ContactSets.Count);
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3(0, 0, 0));
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3(0, 0, 0));
((GeometricObject)c.GeometricObject).Pose = new Pose(new Vector3(1, 0, 1));
((GeometricObject)d.GeometricObject).Pose = new Pose(new Vector3(0, 1, 0));
cd.Update(0.01f);
Assert.AreEqual(6, cd.ContactSets.Count);
}
public override void Update(GameTime gameTime)
{
// Get elapsed time.
float deltaTime = (float)gameTime.ElapsedGameTime.TotalSeconds;
// Move second ship with arrow keys.
Vector3 shipMovement = Vector3.Zero;
if (InputService.IsDown(Keys.Left))
{
shipMovement.X -= 1 * deltaTime;
}
if (InputService.IsDown(Keys.Right))
{
shipMovement.X += 1 * deltaTime;
}
if (InputService.IsDown(Keys.Down))
{
shipMovement.Y -= 1 * deltaTime;
}
if (InputService.IsDown(Keys.Up))
{
shipMovement.Y += 1 * deltaTime;
}
// The movement is relative to the view of the user. We must rotate the movement vector
// into world space.
shipMovement = GraphicsScreen.CameraNode.PoseWorld.ToWorldDirection(shipMovement);
// Update pose of second ship.
var shipBPose = _shipObjectB.Pose;
_shipObjectB.Pose = new Pose(shipBPose.Position + shipMovement, shipBPose.Orientation);
// Toggle debug drawing with Space key.
if (InputService.IsPressed(Keys.Space, true))
{
_drawDebugInfo = !_drawDebugInfo;
}
// Update collision domain. - This will compute collisions.
_collisionDomain.Update(deltaTime);
// Now we could, for example, ask the collision domain if the ships are colliding.
bool shipsAreColliding = _collisionDomain.HaveContact(
_shipObjectA.CollisionObject,
_shipObjectB.CollisionObject);
// Use the debug renderer of the graphics screen to draw debug info and collision shapes.
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
if (_collisionDomain.ContactSets.Count > 0)
{
debugRenderer.DrawText("\n\nCOLLISION DETECTED");
}
else
{
debugRenderer.DrawText("\n\nNo collision detected");
}
if (_drawDebugInfo)
{
foreach (var collisionObject in _collisionDomain.CollisionObjects)
{
debugRenderer.DrawObject(collisionObject.GeometricObject, Color.Gray, false, false);
}
}
}
public void RayCastStopsAtFirstHit()
{
CollisionDomain domain = new CollisionDomain(new CollisionDetection());
CollisionObject ray = new CollisionObject();
((GeometricObject)ray.GeometricObject).Shape = new RayShape(new Vector3(), new Vector3(1, 0, 0), 100)
{
StopsAtFirstHit = true,
};
//ray.Name = "Ray";
CollisionObject b = new CollisionObject();
((GeometricObject)b.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3(-10, 0, 0f));
//b.Name = "b";
CollisionObject c = new CollisionObject();
((GeometricObject)c.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)c.GeometricObject).Pose = new Pose(new Vector3(0, 0, 0f));
//c.Name = "c";
CollisionObject d = new CollisionObject();
((GeometricObject)d.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)d.GeometricObject).Pose = new Pose(new Vector3(10, 0, 0f));
//d.Name = "d";
CollisionObject e = new CollisionObject();
((GeometricObject)e.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)e.GeometricObject).Pose = new Pose(new Vector3(20, 0, 0f));
//e.Name = "e";
CollisionObject f = new CollisionObject();
((GeometricObject)f.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)f.GeometricObject).Pose = new Pose(new Vector3(110, 0, 0f));
//f.Name = "f";
// Positions: b=-10, c=0, d=10, e=20, f=110
domain.CollisionObjects.Add(ray);
domain.CollisionObjects.Add(b);
domain.CollisionObjects.Add(d);
domain.CollisionObjects.Add(c);
domain.CollisionObjects.Add(e);
domain.CollisionObjects.Add(f);
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, c));
((GeometricObject)c.GeometricObject).Pose = new Pose(new Vector3(30));
// Positions: b=-10, d=10, e=20, c=30, f=110
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, d));
((GeometricObject)d.GeometricObject).Pose = new Pose(new Vector3(40));
// Positions: b=-10, e=20, c=30, d=40, f=110
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, e));
((GeometricObject)ray.GeometricObject).Pose = new Pose(((GeometricObject)ray.GeometricObject).Pose.Position, Quaternion.CreateRotationZ(ConstantsF.PiOver2));
domain.Update(0.01f);
Assert.AreEqual(0, domain.GetContacts(ray).Count());
((GeometricObject)ray.GeometricObject).Pose = new Pose(((GeometricObject)ray.GeometricObject).Pose.Position, Quaternion.CreateRotationZ(ConstantsF.Pi));
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, b));
((GeometricObject)ray.GeometricObject).Pose = new Pose(((GeometricObject)ray.GeometricObject).Pose.Position, Quaternion.Identity);
domain.Update(0.01f);
// Positions: b=-10, e=20, c=30, d=40, f=110
CollisionObject gNotInDomain = new CollisionObject();
((GeometricObject)gNotInDomain.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)gNotInDomain.GeometricObject).Pose = new Pose(new Vector3(10, 0, 0f));
Assert.AreEqual(true, domain.HaveContact(ray, gNotInDomain));
Assert.AreEqual(1, domain.GetContacts(gNotInDomain).Count());
Assert.AreEqual(1, domain.GetContacts(ray, gNotInDomain).Count);
Assert.AreEqual(true, domain.HasContact(gNotInDomain));
((GeometricObject)gNotInDomain.GeometricObject).Pose = new Pose(new Vector3(25, 0, 0f)); // behind e
Assert.AreEqual(false, domain.HaveContact(ray, gNotInDomain));
Assert.AreEqual(false, domain.HaveContact(gNotInDomain, ray));
Assert.AreEqual(false, domain.HasContact(gNotInDomain));
Assert.AreEqual(0, domain.GetContacts(gNotInDomain).Count());
Assert.IsNull(domain.GetContacts(ray, gNotInDomain));
Assert.IsNull(domain.GetContacts(gNotInDomain, ray));
// Remove ray from domain.
domain.CollisionObjects.Remove(ray);
domain.Update(0.01f);
Assert.AreEqual(0, domain.ContactSets.Count);
// Positions: b=-10, e=20, g=25, c=30, d=40, f=110
domain.Update(0.01f);
Assert.AreEqual(1, domain.GetContacts(ray).Count());
Assert.AreEqual(true, domain.HaveContact(ray, e));
Assert.AreEqual(false, domain.HaveContact(ray, c));
Assert.AreEqual(false, domain.HaveContact(ray, gNotInDomain));
Assert.IsNull(domain.GetContacts(ray, gNotInDomain));
}
public CollisionFilterSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
SampleFramework.IsMouseVisible = false;
GraphicsScreen.ClearBackground = true;
GraphicsScreen.BackgroundColor = Color.Gray;
GraphicsScreen.DrawReticle = true;
SetCamera(new Vector3F(0, 0, 10), 0, 0);
// ----- Initialize collision detection system.
// We use one collision domain that manages all objects.
var domain = new CollisionDomain();
// Let's set a filter which disables collision between object in the same collision group.
// We can use a broad phase or a narrow phase filter:
// Option A) Broad phase filter
// The collision detection broad phase computes bounding box overlaps.
// A broad phase filter is best used if the filtering rules are simple and do not change
// during the runtime of your application.
//domain.BroadPhase.Filter = new DelegatePairFilter<CollisionObject>(
// pair => pair.First.CollisionGroup != pair.Second.CollisionGroup);
// Option B) Narrow phase filter
// The collision detection narrow phase computes contacts between objects where the broad
// phase has detected a bounding box overlap. Use a narrow phase filter if the filtering rules
// are complex or can change during the runtime of your application.
var filter = new CollisionFilter();
// Disable collision between objects in the same groups.
filter.Set(0, 0, false);
filter.Set(1, 1, false);
filter.Set(2, 2, false);
filter.Set(3, 3, false);
domain.CollisionDetection.CollisionFilter = filter;
// Create a random list of points.
var points = new List<Vector3F>();
for (int i = 0; i < 100; i++)
points.Add(RandomHelper.Random.NextVector3F(-1.5f, 1.5f));
// Add lots of spheres to the collision domain. Assign spheres to different collision groups.
var random = new Random();
var sphereShape = new SphereShape(0.7f);
for (int i = 0; i < 20; i++)
{
var randomPosition = new Vector3F(random.NextFloat(-6, 6), random.NextFloat(-3, 3), 0);
var geometricObject = new GeometricObject(sphereShape, new Pose(randomPosition));
var collisionObject = new CollisionObject(geometricObject)
{
// A collision group is simply an integer. We can assign collision objects to collision
// groups to control the collision filtering.
CollisionGroup = random.NextInteger(0, 3),
};
domain.CollisionObjects.Add(collisionObject);
}
// Compute collisions. (The objects do not move in this sample. Therefore, we only have to
// call Update once.)
domain.Update(0);
// Draw objects. The color depends on the collision group.
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
foreach (var collisionObject in domain.CollisionObjects)
{
Color color;
switch (collisionObject.CollisionGroup)
{
case 0: color = Color.LightBlue; break;
case 1: color = Color.Yellow; break;
case 2: color = Color.Orange; break;
default: color = Color.LightGreen; break;
}
debugRenderer.DrawObject(collisionObject.GeometricObject, color, false, false);
}
debugRenderer.DrawContacts(domain.ContactSets, 0.1f, Color.Red, true);
}
public void Test1()
{
Assert.NotNull(new CollisionDomain().CollisionDetection);
Assert.NotNull(new CollisionDomain(null).CollisionDetection);
CollisionDomain cd = new CollisionDomain(new CollisionDetection());
CollisionObject a = new CollisionObject();
((GeometricObject)a.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0));
//a.Name = "a";
CollisionObject b = new CollisionObject();
((GeometricObject)b.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(4, 0, 2f));
//b.Name = "b";
CollisionObject c = new CollisionObject();
((GeometricObject)c.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)c.GeometricObject).Pose = new Pose(new Vector3F(6, 2, 2f));
//c.Name = "c";
CollisionObject d = new CollisionObject();
((GeometricObject)d.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)d.GeometricObject).Pose = new Pose(new Vector3F(8, 3f, 4f));
//d.Name = "d";
Assert.AreEqual(0, cd.CollisionObjects.Count);
cd.CollisionObjects.Add(a);
Assert.AreEqual(1, cd.CollisionObjects.Count);
cd.CollisionObjects.Add(b);
Assert.AreEqual(2, cd.CollisionObjects.Count);
cd.CollisionObjects.Add(c);
Assert.AreEqual(3, cd.CollisionObjects.Count);
cd.CollisionObjects.Add(d);
Assert.AreEqual(4, cd.CollisionObjects.Count);
cd.Update(0.01f);
Assert.AreEqual(0, cd.ContactSets.Count);
((GeometricObject)a.GeometricObject).Pose = new Pose(((GeometricObject)a.GeometricObject).Pose.Position + new Vector3F(3.5f, 0, 0.5f));
cd.Update(0.01f);
Assert.AreEqual(1, cd.ContactSets.Count);
((GeometricObject)c.GeometricObject).Pose = new Pose(((GeometricObject)c.GeometricObject).Pose.Position + new Vector3F(2, 1, 0));
cd.Update(0.01f);
Assert.AreEqual(2, cd.ContactSets.Count);
((GeometricObject)a.GeometricObject).Pose = new Pose(((GeometricObject)a.GeometricObject).Pose.Position + new Vector3F(3, 0, 0));
((GeometricObject)b.GeometricObject).Pose = new Pose(((GeometricObject)b.GeometricObject).Pose.Position + new Vector3F(2, 0, 0));
cd.Update(0.01f);
Assert.AreEqual(2, cd.ContactSets.Count);
((GeometricObject)d.GeometricObject).Pose = new Pose(((GeometricObject)d.GeometricObject).Pose.Position + new Vector3F(-0.5f, 0, 0));
((GeometricObject)b.GeometricObject).Pose = new Pose(((GeometricObject)b.GeometricObject).Pose.Position + new Vector3F(0, 1, 0));
cd.Update(0.01f);
Assert.AreEqual(1, cd.ContactSets.Count);
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0));
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0));
((GeometricObject)c.GeometricObject).Pose = new Pose(new Vector3F(1, 0, 1));
((GeometricObject)d.GeometricObject).Pose = new Pose(new Vector3F(0, 1, 0));
cd.Update(0.01f);
Assert.AreEqual(6, cd.ContactSets.Count);
}
public override void Update(GameTime gameTime)
{
// Reflect velocity if objects collide:
// The collision domain contains a ContactSet for each pair of touching objects.
foreach (var contactSet in _domain.ContactSets)
{
// Get the touching objects.
var objectA = (MovingGeometricObject)contactSet.ObjectA.GeometricObject;
var objectB = (MovingGeometricObject)contactSet.ObjectB.GeometricObject;
// In rare cases, the collision detection cannot compute a contact because of
// numerical problems. Ignore this case, we usually get a contact in the next frame.
if (contactSet.Count == 0)
{
continue;
}
// Get the contact normal of the first collision point.
var contact = contactSet[0];
Vector3 normal = contact.Normal;
// Check if the objects move towards or away from each other in the direction of the normal.
if (Vector3.Dot(objectB.LinearVelocity - objectA.LinearVelocity, normal) <= 0)
{
// Objects move towards each other. --> Reflect their velocities.
objectA.LinearVelocity -= 2 * Vector3.ProjectTo(objectA.LinearVelocity, normal);
objectB.LinearVelocity -= 2 * Vector3.ProjectTo(objectB.LinearVelocity, normal);
objectA.AngularVelocity = -objectA.AngularVelocity;
objectB.AngularVelocity = -objectB.AngularVelocity;
}
}
// Get the size of the current time step.
float timeStep = (float)gameTime.ElapsedGameTime.TotalSeconds;
// Move objects.
var objects = _domain.CollisionObjects.Select(co => co.GeometricObject).OfType <MovingGeometricObject>();
foreach (var obj in objects)
{
// Update position.
Vector3 position = obj.Pose.Position + obj.LinearVelocity * timeStep;
// Update rotation.
Vector3 rotationAxis = obj.AngularVelocity;
float angularSpeed = obj.AngularVelocity.Length;
Matrix rotation = (Numeric.IsZero(angularSpeed))
? Matrix.Identity
: Matrix.CreateRotation(rotationAxis, angularSpeed * timeStep);
var orientation = rotation * obj.Pose.Orientation;
// Incrementally updating the rotation matrix will eventually create a
// matrix which is not a rotation matrix anymore because of numerical
// problems. Re-othogonalization makes sure that the matrix represents a
// rotation.
orientation.Orthogonalize();
obj.Pose = new Pose(position, orientation);
}
// Update collision domain. This computes new contact information.
_domain.Update(timeStep);
// Record some statistics.
int numberOfObjects = _domain.CollisionObjects.Count;
Profiler.AddValue("NumObjects", numberOfObjects);
// If there are n objects, we can have max. n * (n - 1) / 2 collisions.
Profiler.AddValue("NumObjectPairs", numberOfObjects * (numberOfObjects - 1f) / 2f);
// The first part of the collision detection is the "broad-phase" which
// filters out objects that cannot collide (e.g. using a fast bounding box test).
Profiler.AddValue("BroadPhasePairs", _domain.NumberOfBroadPhaseOverlaps);
// Finally, the collision detection computes the exact contact information and creates
// a ContactSet with the Contacts for each pair of colliding objects.
Profiler.AddValue("ContactSetCount", _domain.ContactSets.Count);
// Draw objects using the DebugRenderer of the graphics screen.
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
foreach (var collisionObject in _domain.CollisionObjects)
{
debugRenderer.DrawObject(collisionObject.GeometricObject, GraphicsHelper.GetUniqueColor(collisionObject), false, false);
}
}