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); }