Exemplo n.º 1
0
        public override void Update(GameTime gameTime)
        {
            if (InputService.IsPressed(MouseButtons.Left, true) || InputService.IsPressed(Buttons.RightTrigger, true, LogicalPlayerIndex.One))
            {
                var     cameraPose      = GraphicsScreen.CameraNode.PoseWorld;
                Vector3 cameraPosition  = cameraPose.Position;
                Vector3 cameraDirection = cameraPose.ToWorldDirection(Vector3.Forward);

                // Create a ray for picking.
                RayShape ray = new RayShape(cameraPosition, cameraDirection, 1000);

                // The ray should stop at the first hit. We only want the first object.
                ray.StopsAtFirstHit = true;

                // The collision detection requires a CollisionObject.
                CollisionObject rayCollisionObject = new CollisionObject(new GeometricObject(ray, Pose.Identity));

                // Get the first object that has contact with the ray.
                ContactSet contactSet = Simulation.CollisionDomain.GetContacts(rayCollisionObject).FirstOrDefault();
                if (contactSet != null && contactSet.Count > 0)
                {
                    // The ray has hit something.

                    // The contact set contains all detected contacts between the ray and the rigid body.
                    // Get the first contact in the contact set. (A ray hit usually contains exactly 1 contact.)
                    Contact contact     = contactSet[0];
                    var     hitPosition = contact.Position;
                    var     normal      = contact.Normal;
                    if (contactSet.ObjectA == rayCollisionObject)
                    {
                        normal = -normal;
                    }

                    // The particle parameter arrays are circular buffers. Get the particle array index
                    // where the next particle is created:
                    int particleIndex = (_decals.ParticleStartIndex + _decals.NumberOfActiveParticles) % _decals.MaxNumberOfParticles;

                    // Add 1 particle.
                    int numberOfCreatedParticles = _decals.AddParticles(1, null);
                    if (numberOfCreatedParticles > 0)
                    {
                        // We initialize the particle parameters Position, Normal and Axis manually using
                        // the results of the collision detection:
                        var positionParameter = _decals.Parameters.Get <Vector3>(ParticleParameterNames.Position);
                        positionParameter.Values[particleIndex] = hitPosition + normal * 0.01f; // We add a slight 1 cm offset to avoid z-fighting.

                        var normalParameter = _decals.Parameters.Get <Vector3>("Normal");
                        normalParameter.Values[particleIndex] = normal;

                        var axisParameter = _decals.Parameters.Get <Vector3>("Axis");
                        axisParameter.Values[particleIndex] = (normal == Vector3.Up) ? Vector3.Backward : Vector3.Up;
                    }
                }
            }

            // Synchronize particles <-> graphics.
            _particleSystemNode.Synchronize(GraphicsService);

            Profiler.AddValue("ParticleCount", ParticleHelper.CountNumberOfParticles(ParticleSystemService.ParticleSystems));
        }
Exemplo n.º 2
0
        public override void Update(GameTime gameTime)
        {
            // Synchronize particles <-> graphics.
            _particleSystemNode.Synchronize(GraphicsService);

            Profiler.AddValue("ParticleCount", ParticleHelper.CountNumberOfParticles(ParticleSystemService.ParticleSystems));
        }
Exemplo n.º 3
0
        public override void Update(GameTime gameTime)
        {
            // Move the particle system with the camera.
            _particleSystem.Pose = new Pose(GraphicsScreen.CameraNode.PoseWorld.Position);

            // Synchronize particles <-> graphics.
            _particleSystemNode.Synchronize(GraphicsService);

            Profiler.AddValue("ParticleCount", ParticleHelper.CountNumberOfParticles(ParticleSystemService.ParticleSystems));
        }
Exemplo n.º 4
0
        public override void Update(GameTime gameTime)
        {
            // The bee swarm effect needs the CameraPose to determine the bee texture orientation.
            _beeSwarm.Parameters.Get <Pose>("CameraPose").DefaultValue = GraphicsScreen.CameraNode.PoseWorld;

            // Synchronize particles <-> graphics.
            _particleSystemNode.Synchronize(GraphicsService);

            Profiler.AddValue("ParticleCount", ParticleHelper.CountNumberOfParticles(ParticleSystemService.ParticleSystems));
        }
Exemplo n.º 5
0
        public override void Update(GameTime gameTime)
        {
            // Move the model and the particle system with the rigid body.
            _modelNode.PoseWorld = _rigidBody.Pose;
            _particleSystem.Pose = _rigidBody.Pose;

            // Synchronize particles <-> graphics.
            _particleSystemNode.Synchronize(GraphicsService);

            Profiler.AddValue("ParticleCount", ParticleHelper.CountNumberOfParticles(ParticleSystemService.ParticleSystems));
        }
Exemplo n.º 6
0
        public override void Update(GameTime gameTime)
        {
            if (InputService.IsDown(MouseButtons.Left) || InputService.IsDown(Buttons.RightTrigger, LogicalPlayerIndex.One))
            {
                _flameJet.AddParticles(6);
            }

            // Synchronize particles <-> graphics.
            _particleSystemNode.Synchronize(GraphicsService);

            Profiler.AddValue("ParticleCount", ParticleHelper.CountNumberOfParticles(ParticleSystemService.ParticleSystems));
        }
Exemplo n.º 7
0
        public override void Update(GameTime gameTime)
        {
            // If enough time has passed, trigger the explosion sound and the explosion effect.
            _timeUntilExplosion -= gameTime.ElapsedGameTime;
            if (_timeUntilExplosion <= TimeSpan.Zero)
            {
                _explosion.Explode();
                _explosionSound.Play(0.2f, 0, 0);
                _timeUntilExplosion = ExplosionInterval;
            }

            // Synchronize particles <-> graphics.
            _particleSystemNode.Synchronize(GraphicsService);

            Profiler.AddValue("ParticleCount", ParticleHelper.CountNumberOfParticles(ParticleSystemService.ParticleSystems));
        }
Exemplo n.º 8
0
        public override void Update(GameTime gameTime)
        {
            // Add 2 new particles in each frame.
            _particleSystem.AddParticles(2);

            // Particles are added and simulated when the particle system service is updated.
            // In this example the service is updated in Game1.Update(). The service is updated
            // after all GameComponents, before Game1.Draw().

            // The ParticleSystemNode needs to be synchronized with the ParticleSystem.
            // The Synchronize() method takes a snapshot of the current particles which
            // is then rendered by the graphics service.
            // (This explicit synchronization is necessary because the particle system
            // service and the graphics service may run in parallel on multiple threads.)
            _particleSystemNode.Synchronize(GraphicsService);

            Profiler.AddValue("ParticleCount", ParticleHelper.CountNumberOfParticles(ParticleSystemService.ParticleSystems));
        }
Exemplo n.º 9
0
        public override void Update(GameTime gameTime)
        {
            bool wasDepthSorted = _brownOut.IsDepthSorted;

            _brownOut.IsDepthSorted = !(InputService.IsDown(MouseButtons.Left) || InputService.IsDown(Buttons.RightTrigger, LogicalPlayerIndex.One));
            if (wasDepthSorted != _brownOut.IsDepthSorted)
            {
                // DigitalRune Graphics caches states like IsDepthSorted. To delete the cached data,
                // we can delete the current ParticleSystem.RenderData.
                _brownOut.RenderData = null;
                foreach (var child in _brownOut.Children)
                {
                    child.RenderData = null;
                }
            }

            // Synchronize particles <-> graphics.
            _particleSystemNode.Synchronize(GraphicsService);

            Profiler.AddValue("ParticleCount", ParticleHelper.CountNumberOfParticles(ParticleSystemService.ParticleSystems));
        }
Exemplo n.º 10
0
        public static void Foo()
        {
            Profiler.Start("Foo");

            // Generate a few random numbers.
            var random = new Random();
            int numberOfValuesBelow10 = 0;

            for (int i = 0; i < 10000; i++)
            {
                int x = random.Next(0, 100);
                if (x < 10)
                {
                    numberOfValuesBelow10++;
                }
            }

            // Profilers can also collect other interesting numbers (not only time).
            Profiler.AddValue("ValuesBelow10", numberOfValuesBelow10);

            Profiler.Stop("Foo");
        }
Exemplo n.º 11
0
        private void UpdateProfiler()
        {
            // Show "Update FPS" and "Draw FPS" in upper right corner.
            // (The data is updated every ~0.5 s.)
            if (_stopwatch.Elapsed.TotalSeconds > 0.5)
            {
                {
                    _stringBuilder.Clear();
                    _stringBuilder.Append("Update: ");
                    float fps = (float)Math.Round(_numberOfUpdates / _stopwatch.Elapsed.TotalSeconds);
                    _stringBuilder.AppendNumber((int)fps);
                    _stringBuilder.Append(" fps, ");
                    _stringBuilder.AppendNumber(1 / fps * 1000, 2, AppendNumberOptions.None);
                    _stringBuilder.Append(" ms");
                    _updateFpsTextBlock.Text = _stringBuilder.ToString();
                }
                {
                    _stringBuilder.Clear();
                    _stringBuilder.Append("Draw: ");
                    float fps = (float)Math.Round(_numberOfDraws / _stopwatch.Elapsed.TotalSeconds);
                    _stringBuilder.AppendNumber((int)fps);
                    _stringBuilder.Append(" fps, ");
                    _stringBuilder.AppendNumber(1 / fps * 1000, 2, AppendNumberOptions.None);
                    _stringBuilder.Append(" ms");
                    _drawFpsTextBlock.Text = _stringBuilder.ToString();
                }

                _numberOfUpdates = 0;
                _numberOfDraws   = 0;
                _stopwatch.Reset();
                _stopwatch.Start();
            }

            // Capture general interesting info.
            var simulation = _services.GetInstance <Simulation>();

            Profiler.AddValue("NumBodies", simulation.RigidBodies.Count);
            Profiler.AddValue("NumContacts", simulation.ContactConstraints.Count);
        }
Exemplo n.º 12
0
        public override void Update(GameTime gameTime)
        {
            _waitTime -= gameTime.ElapsedGameTime;

            if (_waitTime < TimeSpan.Zero)
            {
                // Time to start the next effect at a random position.
                var position = _boxDistribution.Next(RandomHelper.Random);

                // Create teleport effect (the effect comes from a resource pool).
                var teleport = _pool.Obtain();
                teleport.Initialize(ContentManager);
                teleport.Pose = new Pose(position);

                // Add the teleport effect to the particle system service and the scene.
                ParticleSystemService.ParticleSystems.Add(teleport.ParticleSystem);
                GraphicsScreen.Scene.Children.Add(teleport.ParticleSystemNode);
                _teleportEffects.Add(teleport);

                _waitTime = TimeSpan.FromSeconds(1);
            }

            // Update teleport effects and recycle them if they are dead.
            for (int i = _teleportEffects.Count - 1; i >= 0; i--)
            {
                var  teleport = _teleportEffects[i];
                bool isAlive  = teleport.Update(GraphicsService);
                if (!isAlive)
                {
                    ParticleSystemService.ParticleSystems.Remove(teleport.ParticleSystem);
                    GraphicsScreen.Scene.Children.Remove(teleport.ParticleSystemNode);
                    _teleportEffects.RemoveAt(i);

                    _pool.Recycle(teleport);
                }
            }

            Profiler.AddValue("ParticleCount", ParticleHelper.CountNumberOfParticles(ParticleSystemService.ParticleSystems));
        }
Exemplo n.º 13
0
        public override void Update(GameTime gameTime)
        {
            // Update SceneNode.LastPoseWorld (required for optional effects, like motion blur).
            _meshNode0.SetLastPose(true);
            _meshNode1.SetLastPose(true);

            // Synchronize pose of rigid body and model.
            _meshNode0.PoseWorld = _rigidBody0.Pose;
            _meshNode1.PoseWorld = _rigidBody1.Pose;

            // The particle system nodes are attached to the mesh nodes. Their pose is
            // updated automatically.

            // _particleSystem0 is relative to world space, we need to update its
            // pose explicitly.
            _particleSystem0.Pose = _rigidBody0.Pose;

            // Synchronize particles <-> graphics.
            _particleSystemNode0.Synchronize(GraphicsService);
            _particleSystemNode1.Synchronize(GraphicsService);

            Profiler.AddValue("ParticleCount", ParticleHelper.CountNumberOfParticles(ParticleSystemService.ParticleSystems));
        }
Exemplo n.º 14
0
    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);
    }
  }
Exemplo n.º 15
0
        public override void Update(GameTime gameTime)
        {
            var debugRenderer = _graphicsScreen.DebugRenderer2D;

            _numberOfUpdates++;

            if ((_stopwatch.Elapsed.TotalSeconds > 0.5 && !_inputService.IsDown(Keys.F4)) ||
                _inputService.IsReleased(Keys.F4))
            {
                // ----- Overlay View
                // Show "Update FPS" and "Draw FPS" in upper right corner.
                // (The data is updated every ~0.5 s or when the profiler view is closed.)
                _graphicsScreen.ClearBackground = false;

                debugRenderer.Clear();
                _stringBuilder.Clear();
                _stringBuilder.Append("Update FPS: ");
                _stringBuilder.AppendNumber((int)Math.Round(_numberOfUpdates / _stopwatch.Elapsed.TotalSeconds));
                _stringBuilder.Append("\nDraw FPS: ");
                _stringBuilder.AppendNumber((int)Math.Round(_numberOfDraws / _stopwatch.Elapsed.TotalSeconds));
#if !WINDOWS_PHONE
                debugRenderer.DrawText(_stringBuilder, new Vector2F(1160, 10), Color.Black);
#else
                debugRenderer.DrawText(_stringBuilder, new Vector2F(680, 10), Color.Black);
#endif

                _numberOfUpdates = 0;
                _numberOfDraws   = 0;
                _stopwatch.Reset();
                _stopwatch.Start();
            }

            // Capture general interesting info.
            var simulation = _services.GetInstance <Simulation>();
            Profiler.AddValue("NumBodies", simulation.RigidBodies.Count);
            Profiler.AddValue("NumContacts", simulation.ContactConstraints.Count);

            if (_inputService.IsPressed(Keys.F4, false))
            {
                // ----- Profiler View
                _graphicsScreen.ClearBackground = true;

                debugRenderer.Clear();
                debugRenderer.DrawText("PROFILE\n\n");
#if DIGITALRUNE_PROFILE
                // Dump profiler.
                debugRenderer.DrawText("-------------------------------------------------------------------------------");
                debugRenderer.DrawText("Profiler:");
                debugRenderer.DrawText("-------------------------------------------------------------------------------");
                debugRenderer.DrawText(Profiler.DumpAll());
                Profiler.ClearAll();

                // Dump all Hierarchical Profilers.
                var hierarchicalProfilers = _services.GetAllInstances <HierarchicalProfiler>();
                foreach (var hierarchicalProfiler in hierarchicalProfilers)
                {
                    debugRenderer.DrawText("");
                    debugRenderer.DrawText("-------------------------------------------------------------------------------");
                    debugRenderer.DrawText("Hierarchical Profilers:");
                    debugRenderer.DrawText("-------------------------------------------------------------------------------");
                    debugRenderer.DrawText(hierarchicalProfiler.Dump(hierarchicalProfiler.Root, int.MaxValue));
                    debugRenderer.DrawText("");
                    hierarchicalProfiler.Reset();
                }
#else
                debugRenderer.DrawText("Profiling is disabled. To enable profiling, define the conditional compilation symbol 'DIGITALRUNE_PROFILE' in the project.");
#endif
            }
        }
Exemplo n.º 16
0
        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);
            }
        }