public static Node Create(Vector3 pos)
        {
            var cmp = new MySprite ();

            var spr = new Sprite (64, 64);
            spr.AddTexture (new Texture ("media/Box-64x64.png"));

            var col = new CollisionObject ();
            col.Shape = new BoxShape (spr.Width / 2, spr.Height / 2, 100);
            col.SetOffset (spr.Width / 2, spr.Height / 2, 0);

            var mbox1 = new MailBox ("MouseSelect");
            var mbxo2 = new MailBox ("MouseDeselect");

            var node = new Node ("MySprite");
            node.Attach (cmp);
            node.Attach (spr);
            node.Attach (col);
            node.Attach (mbox1);
            node.Attach (mbxo2);

            node.Translation = pos;

            return node;
        }

        public void Test_Overlaps()
        {
            var node1 = new Node ("Node1");
            var col1 = new CollisionObject ();
            col1.Shape = new BoxShape (1, 1, 1);
            node1.Attach (col1);

            var node2 = new Node ("Node2");
            var col2 = new CollisionObject ();
            col2.Shape = new BoxShape (1, 1, 1);
            node2.Attach (col2);

            var wld = new World ();
            wld.AddChild (node1);
            wld.AddChild (node2);

            // コリジョン発生
            wld.CollisionUpdate ();

            Assert.AreEqual (1, col1.OverlappingObjectCount);
            Assert.AreEqual (1, col2.OverlappingObjectCount);

            // コリジョン消失
            node2.Translate (10, 0, 0);
            wld.CollisionUpdate ();

            Assert.AreEqual (0, col1.OverlappingObjectCount);
            Assert.AreEqual (0, col2.OverlappingObjectCount);
        }

        public void Test_Distance_Box_to_Box()
        {
            var node1 = new Node ("Node1");

            var col1 = new CollisionObject ();
            col1.Shape = new BoxShape (1, 1, 1);
            col1.SetOffset (0, 0, 0);

            node1.Attach (col1);

            var node2 = new Node ("Node2");
            var col2 = new CollisionObject ();
            col2.Shape = new BoxShape (1,1,1);
            col2.SetOffset (0, 10, 0);

            node2.Attach (col2);

            node1.Translate (0, 0, 0);
            node2.Translate (10, 0, 0);

            // 10*√2

            var wld = new World ();
            wld.AddChild (node1);
            wld.AddChild (node2);

            wld.CollisionUpdate ();

            Assert.AreEqual(10*1.4142f - 2*1.4142f, wld.Distance (node1, node2), 0.05f);

            wld.Destroy ();
        }

        public void Test_Overlap()
        {
            var node1 = new Node ("Node1");
            var col1 = new CollisionObject ();
            col1.Shape = new BoxShape (1, 1, 1);
            node1.Attach (col1);

            var node2 = new Node ("Node1");
            var col2 = new CollisionObject ();
            col2.Shape = new BoxShape (1, 1, 1);
            node2.Attach (col2);

            var wld = new World ();
            wld.AddChild (node1);
            wld.AddChild (node2);

            node1.Translate (0, 0, 0);
            node2.Translate (0, 0, 0);
            Assert.AreEqual (true, wld.Overlap (node1, node2));

            node1.Translate (0, 0, 0);
            node2.Translate (10, 0, 0);
            Assert.AreEqual (false, wld.Overlap (node1, node2));

            node1.Rotate (0, 0, 0, 0);
            node2.Rotate (45, 0, 0, 1);
            Assert.AreEqual (false, wld.Overlap (node1, node2));

            node1.Detach (col1);
            node2.Detach (col2);
            Assert.AreEqual (false, wld.Overlap (node1, node2));
        }

        public void TestSeparated()
        {
            PlaneConvexAlgorithm algo = new PlaneConvexAlgorithm(new CollisionDetection());

              CollisionObject a = new CollisionObject(new GeometricObject
              {
            Shape = new BoxShape(1, 2, 3),
            Pose = new Pose(new Vector3F(0, 2, 0)),
              });
              CollisionObject b = new CollisionObject(new GeometricObject
              {
            Shape = new PlaneShape(new Vector3F(0, 1, 0), 0),
              });

              Assert.AreEqual(false, algo.HaveContact(a, b));
              Assert.AreEqual(1,algo.GetClosestPoints(a, b).Count);
              Assert.AreEqual(new Vector3F(0, -1, 0), algo.GetClosestPoints(a, b)[0].Normal);
              Assert.AreEqual(new Vector3F(0.5f, 0.5f, 1.5f), algo.GetClosestPoints(a, b)[0].Position);
              Assert.IsTrue(Numeric.AreEqual(-1, algo.GetClosestPoints(a, b)[0].PenetrationDepth));

              // Test swapped.
              Assert.AreEqual(false, algo.HaveContact(b, a));
              Assert.AreEqual(1, algo.GetClosestPoints(b, a).Count);
              Assert.AreEqual(new Vector3F(0, 1, 0), algo.GetClosestPoints(b, a)[0].Normal);
              Assert.AreEqual(new Vector3F(0.5f, 0.5f, 1.5f), algo.GetClosestPoints(b, a)[0].Position);
              Assert.IsTrue(Numeric.AreEqual(-1, algo.GetClosestPoints(b, a)[0].PenetrationDepth));

              Assert.AreEqual(0, algo.GetContacts(a, b).Count);
        }

        public static Node Create(Vector3 pos)
        {
            var cmp = new MyComponent ();

            var spr = new Sprite (480, 300);
            spr.AddTexture (new Texture ("media/Vanity.jpg"));
            spr.AddTexture (new Texture ("media/Tanks.png"));
            spr.AddTexture (new Texture ("media/TatamiRoom.png"));
            spr.AutoScale = true;

            Console.WriteLine ("tex = " + spr.GetTexture (0));
            Console.WriteLine ("spr = " + spr);

            var col = new CollisionObject();
            col.Shape  = new BoxShape(spr.Width/2, spr.Height/2, 100);
            col.SetOffset (spr.Width/2, spr.Height/2, 0);

            var ctr = new AnimationController ();

            var node = new Node ();
            node.Attach (cmp);
            node.Attach (spr);
            node.Attach (col);
            node.Attach (ctr);

            node.Translation = pos;

            var clip = new SoundClip ("Sound");
            clip.AddTrack (new SoundEffectTrack ("media/PinPon.wav"));

            node.UserData.Add (clip.Name, clip);

            return node;
        }

        public static Node Create(string fileName)
        {
            var cmp = new MyTiledMap ();

            var tiledMap = new TiledMapComposer ();

            var node = new Node ("TiledMap");
            node.Attach (tiledMap);
            node.Attach (cmp);

            tiledMap.LoadFromFile (fileName);

            var halfWidth = tiledMap.TileWidth / 2;
            var halfHeight = tiledMap.TileHeight / 2;

            // コリジョンなどのゲームロジックはすべて
            // 仮想の2D直交座標系で作成する
            // 表示だけアイソメトリック
            var colMap = node.Find("CollisionMap");
            foreach (var tile in colMap.Downwards.Skip(1)) {
                var col = new CollisionObject();
                col.Shape = new BoxShape (halfWidth, halfHeight, 1);
                col.SetOffset (halfWidth, halfHeight, 1);

                tile.Attach (col);
            }

            return node;
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="pos"></param>
        /// <param name="groupID"></param>
        /// <returns></returns>
        public static Node Create(Vector3 pos, int groupID)
        {
            var cmp = new MyCharacter ();

            var spr = new Sprite (64, 64);
            spr.AddTexture (Resource.GetDefaultTexture ());
            spr.Color = Color.Red;
            spr.SetOffset (-32, -32);

            var col = new CollisionObject ();
            col.Shape = new BoxShape (spr.Width / 2, spr.Height / 2, 1);
            //col.SetOffset (spr.Width / 2, spr.Height / 2, 1);

            var label = new Label ();
            label.Text = "ID = 0x" + groupID.ToString ("x");
            label.SetOffset (-spr.Width / 2, -spr.Height / 2);

            var node = new Node ("MyCharacter");
            node.Attach (cmp);
            node.Attach (spr);
            node.Attach (col);
            node.Attach (label);

            node.Translation = pos;
            node.GroupID = groupID;

            return node;
        }

        public void TestMethods()
        {
            NoCollisionAlgorithm algo = new NoCollisionAlgorithm(new CollisionDetection());

              CollisionObject a = new CollisionObject { GeometricObject = new GeometricObject { Shape = new SphereShape(1) } };
              CollisionObject b = new CollisionObject { GeometricObject = new GeometricObject { Shape = new SphereShape(2) } };

              Assert.AreEqual(a, algo.GetClosestPoints(a, b).ObjectA);
              Assert.AreEqual(b, algo.GetClosestPoints(a, b).ObjectB);
              Assert.AreEqual(0, algo.GetClosestPoints(a, b).Count);

              Assert.AreEqual(a, algo.GetContacts(a, b).ObjectA);
              Assert.AreEqual(b, algo.GetContacts(a, b).ObjectB);
              Assert.AreEqual(0, algo.GetContacts(a, b).Count);

              Assert.AreEqual(false, algo.HaveContact(a, b));

              ContactSet cs = ContactSet.Create(a, b);
              algo.UpdateClosestPoints(cs, 0);
              Assert.AreEqual(a, cs.ObjectA);
              Assert.AreEqual(b, cs.ObjectB);
              Assert.AreEqual(0, cs.Count);

              cs = ContactSet.Create(a, b);
              cs.Add(Contact.Create());
              algo.UpdateContacts(cs, 0);
              Assert.AreEqual(a, cs.ObjectA);
              Assert.AreEqual(b, cs.ObjectB);
              Assert.AreEqual(0, cs.Count);
        }

        public void Test_Distance()
        {
            var node1 = new Node ("Node1");
            var col1 = new CollisionObject ();
            col1.Shape = new BoxShape (1, 1, 1);
            node1.Attach (col1);

            var node2 = new Node ("Node2");
            var col2 = new CollisionObject ();
            col2.Shape = new BoxShape (1, 1, 1);
            node2.Attach (col2);

            var wld = new World ();
            wld.AddChild (node1);
            wld.AddChild (node2);

            node1.Translate (0, 0, 0);
            node2.Translate (0, 0, 0);
            Assert.AreEqual (0, wld.Distance (node1, node2));

            node1.Translate (0, 0, 0);
            node2.Translate (10, 0, 0);
            Assert.AreEqual (8, wld.Distance (node1, node2), 0.01f);

            node1.Rotate (0, 0, 0, 0);
            node2.Rotate (45, 0, 0, 1);
            Assert.AreEqual (7.6f, wld.Distance (node1, node2), 0.01f);

            node1.Detach (col1);
            node2.Detach (col2);
            Assert.AreEqual (Single.NaN, wld.Distance (node1, node2), 0.01f);
        }

        public void ComputeCollisionLineOther()
        {
            CollisionObject line0 = new CollisionObject();
              //line0.Name = "line0";
              ((GeometricObject)line0.GeometricObject).Shape = new LineShape(new Vector3F(0, 0, 1), new Vector3F(1, 0, 0));
              ((GeometricObject)line0.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 2));

              CollisionObject sphere = new CollisionObject();
              //sphere.Name = "sphere";
              ((GeometricObject)sphere.GeometricObject).Shape = new SphereShape(1);
              ((GeometricObject)sphere.GeometricObject).Pose = Pose.Identity;

              LineAlgorithm algo = new LineAlgorithm(new CollisionDetection());

              ContactSet set;

              set = algo.GetClosestPoints(line0, sphere);
              Assert.IsTrue(Numeric.AreEqual(-2, set[0].PenetrationDepth));
              Assert.IsTrue(Vector3F.AreNumericallyEqual(-Vector3F.UnitZ, set[0].Normal));
              Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 0, 2), set[0].Position, 0.001f));
              Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 0, 1), set[0].PositionALocal, 0.001f));
              Assert.AreEqual(false, algo.HaveContact(line0, sphere));

              set = set.Swapped;
              ((GeometricObject)sphere.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 2.1f));
              algo.UpdateContacts(set, 0);
              Assert.IsTrue(Numeric.AreEqual(0.1f, set[0].PenetrationDepth, 0.001f));
              Assert.IsTrue(Vector3F.AreNumericallyEqual(Vector3F.UnitZ, set[0].Normal, 0.1f));   // Large epsilon because MPR for spheres is not very accurate.
              Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 0, 3), set[0].Position, 0.1f));
              Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 0, 1), set[0].PositionALocal, 0.1f));
              Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 0, 1), set[0].PositionBLocal, 0.1f));
              Assert.AreEqual(true, algo.HaveContact(line0, sphere));
        }

        public static Node Create(string name, string texture, Vector3 pos)
        {
            var cmp = new MyTarget ();

            var spr = new Sprite (64, 128);
            spr.AddTexture (new Texture (texture));

            var col = new CollisionObject ();
            col.Shape = new BoxShape (spr.Width/2, spr.Height/2, 100);
            col.SetOffset (spr.Width/2, spr.Height/2, 0);

            var mbox1 = new MailBox ("MouseSelect");
            var mbox2 = new MailBox ("MouseDeselect");

            var node = new Node (name);
            node.Attach (cmp);
            node.Attach (col);
            node.Attach (spr);
            node.Attach (mbox1);
            node.Attach (mbox2);

            node.Translation = pos;

            return node;
        }

        public void updateSingleAabb(CollisionObject colObj)
        {
            btVector3 minAabb, maxAabb;
            colObj.CollisionShape.getAabb(colObj.WorldTransform, out minAabb, out maxAabb);
            //need to increase the aabb for contact thresholds
            btVector3 contactThreshold = new btVector3(PersistentManifold.gContactBreakingThreshold, PersistentManifold.gContactBreakingThreshold, PersistentManifold.gContactBreakingThreshold);
            //minAabb -= contactThreshold;
            //maxAabb += contactThreshold;
            minAabb.Subtract(ref contactThreshold);
            maxAabb.Add(ref contactThreshold);

            //IBroadphaseInterface bp = m_broadphasePairCache;

            //moving objects should be moderately sized, probably something wrong if not
            if (colObj.isStaticObject || ((maxAabb - minAabb).Length2 < 1e12f))
            {
                m_broadphasePairCache.setAabb(colObj.BroadphaseHandle,ref minAabb,ref maxAabb, m_dispatcher1);
            }
            else
            {
                //something went wrong, investigate
                //this assert is unwanted in 3D modelers (danger of loosing work)
                colObj.ActivationState = ActivationStateFlags.DISABLE_SIMULATION;

                if (reportMe && m_debugDrawer != null)
                {
                    reportMe = false;
                    m_debugDrawer.reportErrorWarning("Overflow in AABB, object removed from simulation");
                    m_debugDrawer.reportErrorWarning("If you can reproduce this, please email [email protected]\n");
                    m_debugDrawer.reportErrorWarning("Please include above information, your Platform, version of OS.\n");
                    m_debugDrawer.reportErrorWarning("Thanks.\n");
                }
            }
        }

        public static Node Create(Vector3 pos, int collisionMask)
        {
            var cmp = new MyBlock ();

            var spr = new Sprite (128, 64);
            spr.AddTexture (Resource.GetDefaultTexture ());
            spr.Color = Color.Cyan;

            var col = new CollisionObject ();
            col.Shape = new BoxShape (spr.Width/2, spr.Height/2, 1);
            col.SetOffset (spr.Width/2, spr.Height/2, 1);
            col.CollideWith = collisionMask;

            var label = new Label ();
            label.Text = "Mask = 0x" + collisionMask.ToString("x");

            var node = new Node ("Block");
            node.Attach (cmp);
            node.Attach (spr);
            node.Attach (col);
            node.Attach (label);

            node.Translation = pos;

            return node;
        }

        public void ComputeCollision()
        {
            RayConvexAlgorithm algo = new RayConvexAlgorithm(new CollisionDetection());

              CollisionObject ray = new CollisionObject(new GeometricObject
              {
            Shape = new RayShape(new Vector3F(0, 0, 0), new Vector3F(-1, 0, 0), 10),
            Pose = new Pose(new Vector3F(11, 0, 0))
              });

              CollisionObject triangle = new CollisionObject(new GeometricObject
              {
            Shape = new TriangleShape(new Vector3F(0, 0, 0), new Vector3F(0, 1, 0), new Vector3F(0, 0, 1)),
            Pose = Pose.Identity,
              });

              ContactSet set;

              // Separated
              set = algo.GetClosestPoints(ray, triangle);
              Assert.AreEqual(new Vector3F(1, 0, 0), set[0].PositionAWorld);
              Assert.AreEqual(new Vector3F(0, 0, 0), set[0].PositionBWorld);
              Assert.AreEqual(-1, set[0].PenetrationDepth);
              Assert.AreEqual(false, algo.HaveContact(ray, triangle));
              Assert.AreEqual(false, algo.HaveContact(triangle, ray));
              Assert.AreEqual(0, algo.GetContacts(ray, triangle).Count);

              // Touching
              Pose newPose = ray.GeometricObject.Pose;
              newPose.Position = new Vector3F(5, 0, 0);
              ((GeometricObject)ray.GeometricObject).Pose = newPose;
              set = algo.GetClosestPoints(triangle, ray);
              Assert.AreEqual(new Vector3F(0, 0, 0), set[0].PositionBWorld);
              Assert.AreEqual(new Vector3F(0, 0, 0), set[0].PositionAWorld);
              Assert.AreEqual(5, set[0].PenetrationDepth);
              Assert.AreEqual(true, algo.HaveContact(ray, triangle));
              Assert.AreEqual(true, algo.HaveContact(triangle, ray));

              newPose = ray.GeometricObject.Pose;
              newPose.Position = new Vector3F(4, 0.1f, 0.1f);
              ((GeometricObject)ray.GeometricObject).Pose = newPose;
              algo.UpdateContacts(set, 0);
              Assert.AreEqual(new Vector3F(0, 0.1f, 0.1f), set[0].PositionBWorld);
              Assert.AreEqual(new Vector3F(0, 0.1f, 0.1f), set[0].PositionAWorld);
              Assert.AreEqual(new Vector3F(0, 0.1f, 0.1f), set[0].Position);
              Assert.AreEqual(4, set[0].PenetrationDepth);
              Assert.AreEqual(true, algo.HaveContact(ray, triangle));
              Assert.AreEqual(true, algo.HaveContact(triangle, ray));

              // Through triangle plane but separated.
              newPose = ray.GeometricObject.Pose;
              newPose.Position = new Vector3F(5, 1.1f, 0.1f);
              ((GeometricObject)ray.GeometricObject).Pose = newPose;
              algo.UpdateContacts(set, 0);
              Assert.AreEqual(0, set.Count);
              algo.UpdateClosestPoints(set, 0);
              Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 1.1f, 0.1f), set[0].PositionBWorld));
              Assert.AreEqual(false, algo.HaveContact(ray, triangle));
              Assert.AreEqual(false, algo.HaveContact(triangle, ray));
        }

        public static Node Create(string name, string target, Vector3 pos)
        {
            var cmp = new MyButton ();
            cmp.target = target;

            var spr = new Sprite (64, 64);
            spr.AddTexture (new Texture("media/ButtonRed-Active-64x64.png"));
            spr.AddTexture (new Texture ("media/ButtonGreen-Active-64x64.png"));
            spr.AddTexture (new Texture ("media/ButtonBlue-Active-64x64.png"));
            switch (target) {
                case "A子": spr.ActiveTexture = 0; break;
                case "B子": spr.ActiveTexture = 1; break;
                case "C子": spr.ActiveTexture = 2; break;
            }

            var col = new CollisionObject ();
            col.Shape = new BoxShape (40, 25, 1);
            col.SetOffset (40, 25, 0);

            var node = new Node (name);
            node.Attach (cmp);
            node.Attach (spr);
            node.Attach (col);

            node.UserData.Add ("PinPon", new SoundEffectTrack ("media/PinPon.wav"));

            node.Translation = pos;

            return node;
        }

        public void TestContainment()
        {
            PlaneSphereAlgorithm algo = new PlaneSphereAlgorithm(new CollisionDetection());

              CollisionObject objectA = new CollisionObject();
              ((GeometricObject)objectA.GeometricObject).Shape = new SphereShape(1);
              ((GeometricObject)objectA.GeometricObject).Pose = new Pose(new Vector3F(0, -2, 0));

              CollisionObject objectB = new CollisionObject();
              ((GeometricObject)objectB.GeometricObject).Shape = new PlaneShape(new Vector3F(0, 1, 0).Normalized, 0);
              ((GeometricObject)objectB.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0));

              ContactSet cs = ContactSet.Create(objectA, objectB);
              algo.UpdateContacts(cs, 0);

              Assert.AreEqual(objectA, cs.ObjectA);
              Assert.AreEqual(objectB, cs.ObjectB);
              Assert.AreEqual(1, cs.Count);
              Assert.AreEqual(new Vector3F(0, -1.5f, 0), cs[0].Position);
              Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, -1, 0), cs[0].Normal));
              Assert.IsTrue(Numeric.AreEqual(3, cs[0].PenetrationDepth));

              // Test swapped case:
              cs = ContactSet.Create(objectB, objectA);
              algo.UpdateContacts(cs, 0);

              Assert.AreEqual(objectB, cs.ObjectA);
              Assert.AreEqual(objectA, cs.ObjectB);
              Assert.AreEqual(1, cs.Count);
              Assert.AreEqual(new Vector3F(0, -1.5f, 0), cs[0].Position);
              Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 1, 0), cs[0].Normal));
              Assert.IsTrue(Numeric.AreEqual(3, cs[0].PenetrationDepth));
        }

        public void Containment()
        {
            MinkowskiPortalRefinement algo = new MinkowskiPortalRefinement(new CollisionDetection());

              CollisionObject a = new CollisionObject(new GeometricObject
              {
            Shape = new SphereShape(2),
            Pose = new Pose(new Vector3F(1, 2, 3)),
              });

              CollisionObject b = new CollisionObject(new GeometricObject
              {
            Shape = new SphereShape(1),
            Pose = new Pose(new Vector3F(1, 2, 3)),
              });

              ContactSet set;

              set = algo.GetContacts(a, b);
              Assert.AreEqual(true, algo.HaveContact(a, b));
              Assert.AreEqual(3, set[0].PenetrationDepth);

              ((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(2, 2, 3));
              algo.UpdateContacts(set, 0);
              Assert.AreEqual(true, algo.HaveContact(a, b));
              Assert.AreEqual(2, set[0].PenetrationDepth);
        }

        public void AxisAlignedBoundingBox()
        {
            CollisionObject obj = new CollisionObject(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3F(1, 2, 3))));

              Assert.AreEqual(new Aabb(new Vector3F(0.7f, 1.7f, 2.7f), new Vector3F(1.3f, 2.3f, 3.3f)),
                      obj.GeometricObject.Aabb);
        }

        public void Test2()
        {
            // Test following: A ray touches a plane at the ray-end. Then the plane moves so that
              // they are separated.

              PlaneRayAlgorithm algo = new PlaneRayAlgorithm(new CollisionDetection());

              // Plane in xz plane.
              CollisionObject plane = new CollisionObject(new GeometricObject
              {
            Shape = new PlaneShape(Vector3F.UnitY, 1),
            Pose = new Pose(new Vector3F(0, -1, 0)),
              });

              // Ray
              CollisionObject ray = new CollisionObject(new GeometricObject
              {
            Shape = new RayShape(new Vector3F(0, 0, 0), new Vector3F(0, -1, 0), 10),
            Pose = new Pose(new Vector3F(0, 10, 0)),
              });

              ContactSet contacts = algo.GetContacts(plane, ray);
              Assert.AreEqual(true, contacts.HaveContact);
              Assert.AreEqual(1, contacts.Count);
              Assert.AreEqual(10, contacts[0].PenetrationDepth);

              // Move plane less than contact position tolerance, but into a separated state.
              ((GeometricObject)plane.GeometricObject).Pose = new Pose(new Vector3F(0, -1.001f, 0));
              algo.UpdateClosestPoints(contacts, 0);
              Assert.AreEqual(false, contacts.HaveContact);
              Assert.AreEqual(1, contacts.Count);
              Assert.IsTrue(Numeric.AreEqual(-0.001f, contacts[0].PenetrationDepth));
        }