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));
}
// Fills bounds with all current planes that limit the movement of the character
// at the given position.
// Note: All planes are relative to the given position - usually the bottom of the
// character controller.
private void AddBounds(List <Plane> bounds, Vector3F position)
{
// Get contact sets from domain and add a plane for each contact.
foreach (ContactSet contactSet in _collisionDomain.GetContacts(CollisionObject))
{
foreach (Contact contact in contactSet)
{
// Get the contact normal vector pointing to the character controller.
Vector3F normal = (contactSet.ObjectB == CollisionObject) ? contact.Normal : -contact.Normal;
// The penetration depth measures how much the character controller penetrates the
// obstacle.
float penetration = contact.PenetrationDepth;
// We allow a bit of penetration. For numerical stability, we use only 90% of
// the allowed penetration.
penetration -= AllowedPenetration * 0.9f;
// Add a plane that represents this movement limit.
bounds.Add(new Plane(normal, position + normal * penetration));
}
}
}
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 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);
}