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