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