/// <inheritdoc/>
public bool HaveContact(SceneNode nodeA, SceneNode nodeB)
{
if (nodeA == null)
{
throw new ArgumentNullException("nodeA");
}
if (nodeB == null)
{
throw new ArgumentNullException("nodeB");
}
var shapeA = nodeA.Shape;
var shapeB = nodeB.Shape;
if (shapeA is EmptyShape || shapeB is EmptyShape)
{
return(false);
}
if (shapeA is InfiniteShape || shapeB is InfiniteShape)
{
return(nodeA.ActualIsEnabled && nodeB.ActualIsEnabled);
}
var collisionObjectA = GetCollisionObject(nodeA, false);
var collisionObjectB = GetCollisionObject(nodeB, false);
return(_collisionDomain.HaveContact(collisionObjectA, collisionObjectB));
}
public void Update()
{
if (collisionDomain.HasContact(CollisionObject))
{
if (triggeredRigidbodies.Count != 0)
{
triggeredRigidbodies.RemoveAll(rigidBody1 => !collisionDomain.HaveContact(CollisionObject, rigidBody1.CollisionObject));
}
var characters = collisionDomain.GetContactObjects(CollisionObject)
.Select(collisionObject => collisionObject.GeometricObject)
.OfType <RigidBody>()
.Where(rigidBody1 => rigidBody1.Name == nameOfTarget)
.Except(triggeredRigidbodies);
foreach (var character in characters)
{
triggeredRigidbodies.Add(character);
OnTriggerEnter(character.UserData);
}
}
else
{
if (triggeredRigidbodies.Count != 0)
{
triggeredRigidbodies.Clear();
}
}
}
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));
}
private void ComputeTimeOfImpact_Singlethreaded(ContactSet contactSet)
{
RigidBody bodyA = contactSet.ObjectA.GeometricObject as RigidBody;
RigidBody bodyB = contactSet.ObjectB.GeometricObject as RigidBody;
// Note: We do CCD only between rigid bodies.
// TODO: Support CCD between RigidBody and general CollisionObject of the user.
if (bodyA != null && bodyB != null)
{
// Check if at least one object needs CCD.
if (bodyA.IsCcdActive || bodyB.IsCcdActive)
{
// Check CCD filter. (We check this filter first because it hopefully filters out a lot
// of unnecessary checks (e.g. body against debris).)
Func<RigidBody, RigidBody, bool> ccdFilter = Settings.Motion.CcdFilter;
if (ccdFilter != null && !ccdFilter(bodyA, bodyB))
return;
// Check collision filter. (Using the internal method CanCollide that uses a cache.)
if (!CollisionDomain.CanCollide(contactSet))
return;
// If convex bodies are touching we do not need to compute TOI because they are either
// moving more toward each other (TOI = 0) or separating (TOI = 1). If they are moving
// toward each other, then the contact constraint has failed and CCD is not the problem.
// For objects vs. concave objects we still have to make a check because the bullet
// could be separating and colliding with another part of the concave object.
// To avoid that objects stick: GetTimeOfImpact() should not return 0 for separating
// objects and we use 2 * AllowedPenetration so that the current contact does not
// count.
if (!(bodyA.Shape is ConvexShape)
|| !(bodyB.Shape is ConvexShape)
|| !CollisionDomain.HaveContact(contactSet.ObjectA, contactSet.ObjectB))
{
// Get time of impact.
float timeOfImpact = CollisionDomain.CollisionDetection.GetTimeOfImpact(
contactSet.ObjectA,
bodyA.IsCcdActive ? bodyA.TargetPose : bodyA.Pose,
contactSet.ObjectB,
bodyB.IsCcdActive ? bodyB.TargetPose : bodyB.Pose,
Settings.Constraints.AllowedPenetration * 2f);
// Store minimal time of impact.
if (timeOfImpact < bodyA.TimeOfImpact)
bodyA.TimeOfImpact = timeOfImpact;
if (timeOfImpact < bodyB.TimeOfImpact)
bodyB.TimeOfImpact = timeOfImpact;
}
}
}
}
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);
}
}
private void ComputeTimeOfImpact_Multithreaded(int i)
{
var contactSet = CollisionDomain.InternalBroadPhase.CandidatePairs[i];
if (contactSet == null)
return;
RigidBody bodyA = contactSet.ObjectA.GeometricObject as RigidBody;
RigidBody bodyB = contactSet.ObjectB.GeometricObject as RigidBody;
if (bodyA != null && bodyB != null)
{
if (bodyA.IsCcdActive || bodyB.IsCcdActive)
{
Func<RigidBody, RigidBody, bool> ccdFilter = Settings.Motion.CcdFilter;
if (ccdFilter != null && !ccdFilter(bodyA, bodyB))
return;
if (!CollisionDomain.CanCollide(contactSet))
return;
if (!(bodyA.Shape is ConvexShape)
|| !(bodyB.Shape is ConvexShape)
|| !CollisionDomain.HaveContact(contactSet.ObjectA, contactSet.ObjectB))
{
float timeOfImpact = CollisionDomain.CollisionDetection.GetTimeOfImpact(
contactSet.ObjectA,
bodyA.IsCcdActive ? bodyA.TargetPose : bodyA.Pose,
contactSet.ObjectB,
bodyB.IsCcdActive ? bodyB.TargetPose : bodyB.Pose,
Settings.Constraints.AllowedPenetration * 2f);
float initialToi;
do
{
initialToi = bodyA.TimeOfImpact;
} while (timeOfImpact < initialToi && Interlocked.CompareExchange(ref bodyA.TimeOfImpact, timeOfImpact, initialToi) != initialToi);
do
{
initialToi = bodyB.TimeOfImpact;
} while (timeOfImpact < initialToi && Interlocked.CompareExchange(ref bodyB.TimeOfImpact, timeOfImpact, initialToi) != initialToi);
#else
if (timeOfImpact < bodyA.TimeOfImpact) // Double-check to avoid unnecessary locking.
{
lock (bodyA)
{
if (timeOfImpact < bodyA.TimeOfImpact)
bodyA.TimeOfImpact = timeOfImpact;
}
}
if (timeOfImpact < bodyB.TimeOfImpact) // Double-check to avoid unnecessary locking.
{
lock (bodyB)
{
if (timeOfImpact < bodyB.TimeOfImpact)
bodyB.TimeOfImpact = timeOfImpact;
}
}
}
}
}
}
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 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 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));
}
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 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);
}