public void IntersectionTests_BoxVsBox_Faces()
        {
            NoBody topBody = new NoBody(Matrix.CreateTranslation(new Vector3(1.5f, 2.0f, 0.25f)));

            Box top = new Box(
                topBody,
                Matrix.Identity,
                new Vector3(1f, 1f, 1f));

            NoBody bottomBody =
                new NoBody(Matrix.CreateTranslation(new Vector3(0f, 0f, 0f)));
            Box bottom = new Box(
                bottomBody,
                Matrix.Identity,
                new Vector3(1f, 1f, 1f));

            CollisionData data = new CollisionData();
            int contactsFound = IntersectionTests.boxAndBox(top, bottom, data);

            Assert.AreEqual(1, contactsFound);

            Contact contact = data.contacts[0];
            Assert.AreEqual(new Vector3(1f, 1f, 1f), contact.Point);
            Assert.AreEqual(Vector3.Up, contact.Normal);
            Assert.AreEqual(0f, contact.Penetration);
        }
        public void IntersectionTests_BoxVsBox_NotTouching()
        {
            Vector3 leftPos = new Vector3(-100f, -100f, -100f);
            IRigidBody leftBody = new NoBody(Matrix.CreateTranslation(leftPos));
            Box left = new Box(leftBody, Matrix.Identity, new Vector3(1f, 1f, 1f));

            Vector3 rightPos = new Vector3(100f, 100f, 100f);
            IRigidBody rightBody = new NoBody(Matrix.CreateTranslation(rightPos));
            Box right = new Box(rightBody, Matrix.Identity, new Vector3(1f, 1f, 1f));

            CollisionData data = new CollisionData();
            int contactsFound = IntersectionTests.boxAndBox(left, right, data);

            Assert.AreEqual(0, contactsFound);
            Assert.AreEqual(0, data.contacts.Count);
        }
        public ModelEntity CreateCube(Vector3 position, float mass, float dimension)
        {
            Model model = Models[CollideType.Box];
            float size = dimension;

            RigidBody body = new RigidBody(position);

            body.LinearDamping = LinearDamping;
            body.AngularDamping = AngularDamping;
            body.Awake = true;
            body.Mass = mass;
            body.InertiaTensor = InertiaTensorFactory.Cubiod(mass, size, size, size);

            float halfSize = size;

            Box prim = new Box(body, Matrix.Identity, new Vector3(halfSize, halfSize, halfSize));
            EntityGeometryData geoData = new EntityGeometryData();
            geoData.Prim = prim;

            geoData.BoundingSphere = new BoundingSphere(Vector3.Zero, halfSize * TrickyMath.Sqrt(2f));

            return new ModelEntity(model, body, geoData);
        }
        private static Contact checkBoxAndPoint(Box box, Vector3 point, float restitution, float friction)
        {
            CollisionData pointData = new CollisionData();
            pointData.restitution = restitution;
            pointData.friction = friction;

            int pointContacts = IntersectionTests.boxAndPoint(box, point, pointData);

            if (pointContacts > 0)
            {
                return pointData.contacts[0];
            }
            return null;
        }
        public static int edgeAndEdge(
            Box left, Vector3 leftEdgeOne, Vector3 leftEdgeTwo,
            Box right, Vector3 rightEdgeOne, Vector3 rightEdgeTwo,
            CollisionData data)
        {
            //return 0; //TODO

            Vector3 leftCenter = left.Body.Position;
            Vector3 rightCenter = right.Body.Position;
            Vector3 midLineRight = rightCenter - leftCenter;

            float distCenters = midLineRight.Length();

            Vector3 projectedLeftEdge = TrickyMath.Project(leftEdgeOne - leftCenter, midLineRight);
            float distLeftCenterToEdge = projectedLeftEdge.Length();
            Vector3 projectedRightEdge = TrickyMath.Project(rightEdgeOne - rightCenter, -midLineRight);
            float distRightCenterToEdge = projectedRightEdge.Length();

            if (distCenters > (distLeftCenterToEdge + distRightCenterToEdge))
            {
                return 0;
            }

            float distEdges = (distLeftCenterToEdge + distRightCenterToEdge) - distCenters;
            Contact contact = new Contact();
            contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Edge_Edge;
            contact.Restitution = data.restitution;
            contact.Friction = data.friction;
            contact.Bodies[0] = left.Body;
            contact.Bodies[1] = right.Body;
            contact.Penetration = (distLeftCenterToEdge + distRightCenterToEdge) - distCenters;
            contact.Point = leftCenter + (midLineRight * 0.5f);

            Vector3 x = leftEdgeOne - leftEdgeTwo; ;
            x.Normalize();
            Vector3 y, z;
            TrickyMath.MakeOrthonormalBasis(x, out y, out z);
            Vector3 realY = y;

            Vector3 normal = TrickyMath.VectorProduct(x, realY);
            normal.Normalize();
            contact.Normal = normal;
            data.contacts.Add(contact);
            return 1;
        }
        public static Contact checkEdgeAndEdge(
            Box left, Vector3 leftEdgeOne, Vector3 leftEdgeTwo,
            Box right, Vector3 rightEdgeOne, Vector3 rightEdgeTwo, float restitution, float friction)
        {
            return null;
            CollisionData pointData = new CollisionData();
            pointData.restitution = restitution;
            pointData.friction = friction;

            int pointContacts = IntersectionTests.edgeAndEdge(left, leftEdgeOne, leftEdgeTwo, right, rightEdgeOne, rightEdgeTwo, pointData);

            if (pointContacts > 0)
            {
                return pointData.contacts[0];
            }
            return null;
        }
        public static int boxAndSphere(Box box, Sphere sph, CollisionData data)
        {
            Vector3 center = sph.Body.Position;

            Vector3 relCenter = Vector3.Transform(center, Matrix.Invert(box.Transform));

            double x = relCenter.X;
            double y = relCenter.Y;
            double z = relCenter.Z;

            if (Math.Abs(x) - sph.Radius > box.HalfSizes.X
              || Math.Abs(y) - sph.Radius > box.HalfSizes.Y
              || Math.Abs(z) - sph.Radius > box.HalfSizes.Z)
            {
                return 0;
            }

            Vector3 closest = Vector3.Zero;
            float dist;

            dist = relCenter.X;
            if (dist > box.HalfSizes.X) dist = box.HalfSizes.X;
            if (dist < -box.HalfSizes.X) dist = -box.HalfSizes.X;
            closest.X = dist;

            dist = relCenter.Y;
            if (dist > box.HalfSizes.Y) dist = box.HalfSizes.Y;
            if (dist < -box.HalfSizes.Y) dist = -box.HalfSizes.Y;
            closest.Y = dist;

            dist = relCenter.Z;
            if (dist > box.HalfSizes.Z) dist = box.HalfSizes.Z;
            if (dist < -box.HalfSizes.Z) dist = -box.HalfSizes.Z;
            closest.Z = dist;

            dist = (closest - relCenter).Length();

            if (dist > sph.Radius && !TrickyMath.AlmostEquals(0f, dist - sph.Radius)) return 0;

            Vector3 closestWorld = Vector3.Transform(closest, box.Transform);

            Contact contact = new Contact();
            Vector3 normal = closestWorld - center;// (center - closestWorld);
            normal.Normalize();
            contact.Normal = normal;
            Debug.Sanity(contact.Normal);

            contact.Point = closestWorld;
            contact.Penetration = sph.Radius - dist;// (float)Math.Sqrt((float)dist);

            contact.Bodies[0] = box.Body;
            contact.Bodies[1] = sph.Body;
            contact.Restitution = data.restitution;
            contact.Friction = data.friction;

            data.contacts.Add(contact);

            return 1;
        }
        public void IntersectionTests_BoxVsPoint()
        {
            IRigidBody body = new NoBody();
            Box box;
            Vector3 point;
            CollisionData data;
            Contact contact;

            box = new Box(body, Matrix.Identity, new Vector3(1f, 1f, 1f));
            //////////////////////////  point@+,+,+
            point = new Vector3(0.75f, 1f, 1f);

            data = new CollisionData();
            data.contactsLeft = 1;

            Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
            contact = data.contacts[0];

            Assert.AreEqual(Vector3.Left, contact.Normal);
            Assert.AreEqual(0.25f, contact.Penetration);
            Assert.AreEqual(1, data.contacts.Count);
            Assert.AreEqual(point, contact.Point);
            Assert.Null(contact.Bodies[1]);

            ///////////////////////////// point@center when box is translated

            Vector3 translatedPos = Vector3.UnitY * 10f;
            body = new NoBody(Matrix.CreateTranslation(translatedPos));

            box = new Box(body, Matrix.Identity, new Vector3(1f, 1f, 1f));
            point = translatedPos;

            data = new CollisionData();
            data.contactsLeft = 1;

            Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
            contact = data.contacts[0];

            Assert.AreEqual(1f, contact.Penetration);
            Assert.AreEqual(1, data.contacts.Count);
            Assert.AreEqual(point, contact.Point);
            Assert.AreEqual(Vector3.Right, contact.Normal);
            Assert.Null(contact.Bodies[1]);

            /////////////////////////////////////point@+,+,+ when box is translated

            point = translatedPos + Vector3.Right * 0.75f;
            data = new CollisionData();
            data.contactsLeft = 1;

            Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
            contact = data.contacts[0];

            Assert.AreEqual(0.25f, contact.Penetration);
            Assert.AreEqual(1, data.contacts.Count);
            Assert.AreEqual(point, contact.Point);

            Vector3 normal = Vector3.Left;

            Assert.AreEqual(normal, contact.Normal);

            Assert.Null(contact.Bodies[1]);

            //////////////////////////////////////////point@tip(+,+,+)

            body = new NoBody();

            box = new Box(body, Matrix.Identity, new Vector3(1f, 1f, 1f));

            point = new Vector3(1f, 1f, 1f);
            data = new CollisionData();
            data.contactsLeft = 1;
            Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
            contact = data.contacts[0];

            Assert.AreEqual(0f, contact.Penetration);
            Assert.AreEqual(1, data.contacts.Count);
            Assert.AreEqual(point, contact.Point);
            Assert.AreEqual(Vector3.Left, contact.Normal);
            Assert.Null(contact.Bodies[1]);

            /////////////////////////////////////////point@tip(-,-,-)

            point = new Vector3(-1f, -1f, -1f);
            data = new CollisionData();
            data.contactsLeft = 1;
            Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
            contact = data.contacts[0];

            Assert.AreEqual(0f, contact.Penetration);
            Assert.AreEqual(1, data.contacts.Count);
            Assert.AreEqual(point, contact.Point);
            Assert.AreEqual(Vector3.Right, contact.Normal);
            Assert.Null(contact.Bodies[1]);

            /////////////////////////////////////////point@middle of nowhere

            point = new Vector3(10000f, 10000f, 10000f);
            data = new CollisionData();
            data.contactsLeft = 1;
            Assert.AreEqual(0, IntersectionTests.boxAndPoint(box, point, data));

            /////////////////////////////////////////point@+,0,0 after box is rotated
            float rotY = (float)Math.PI / 4f; //45 degrees

            point = new Vector3(1.001f, 0f, 0f);
            body = new NoBody(Matrix.CreateRotationY(rotY));
            box = new Box(body, Matrix.Identity, new Vector3(1f, 1f, 1f));
            data = new CollisionData();

            Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
            Assert.AreEqual(1, data.contacts.Count);
            contact = data.contacts[0];

            Assert.True(
                TrickyMath.CloseEnoughToBeSame(contact.Normal, new Vector3(-0.7071068f, 0, 0.7071068f) )
            );

            /////////////////////////////////////////////////////point@+,0,0 after box is rotated other direction

            point = new Vector3(1.001f, 0f, 0f);
            body = new NoBody(Matrix.CreateRotationY(-rotY));
            box = new Box(body, Matrix.Identity, new Vector3(1f, 1f, 1f));
            data = new CollisionData();

            Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
            Assert.AreEqual(1, data.contacts.Count);
            contact = data.contacts[0];

            Assert.True(
                TrickyMath.CloseEnoughToBeSame(contact.Normal, new Vector3(-0.7071068f, 0, -0.7071068f))
            );
        }
 private static float transformToAxis(Box box, Vector3 axis)
 {
     return box.HalfSizes.X * TrickyMath.Abs(Vector3.Dot(axis, box.getAxis(0)))
         + box.HalfSizes.Y * TrickyMath.Abs(Vector3.Dot(axis, box.getAxis(1)))
         + box.HalfSizes.Z * TrickyMath.Abs(Vector3.Dot(axis, box.getAxis(2)));
 }
        public static int boxAndBoxBetter(Box left, Box right, CollisionData data)
        {
            Vector3[] leftWorldVerts = left.WorldVerts();
            Vector3[] rightWorldVerts = right.WorldVerts();

            foreach (Vector3 leftWorldVert in leftWorldVerts)
            {

                Contact contact = IntersectionTests.checkBoxAndPoint(right, leftWorldVert, data.restitution, data.friction);
                if (contact != null)
                {
                    contact.Bodies[1] = left.Body;
                    data.contacts.Add(contact);
                }

            }

            if (data.contacts.Count > 0)
            {
                return data.contacts.Count;
            }

            foreach (Vector3 rightWorldVert in rightWorldVerts)
            {
                Contact contact = IntersectionTests.checkBoxAndPoint(left, rightWorldVert, data.restitution, data.friction);
                if (contact != null)
                {
                    contact.Bodies[1] = right.Body;
                    data.contacts.Add(contact);

                }

            }

            // If any point/face contacts get precedent over edge/edge
            if (data.contacts.Count > 0)
            {
                return data.contacts.Count;
            }

            List<Vector3[]> leftEdges = left.WorldEdges();
            List<Vector3[]> rightEdges = right.WorldEdges();
            foreach (Vector3[] leftEdge in leftEdges)
            {
                foreach (Vector3[] rightEdge in rightEdges)
                {
                    Contact contact = checkEdgeAndEdge(left, leftEdge[0], leftEdge[1], right, rightEdge[0], rightEdge[1], data.restitution, data.friction);

                    if (contact != null)
                    {
                        data.contacts.Add(contact);
                    }
                }

            }

            return data.contacts.Count;
        }
        public void IntersectionTests_BoxVsPlane()
        {
            IRigidBody planeBody = new NoBody();
            Box box;
            Forever.Physics.Collide.Plane plane;
            CollisionData data;

            Vector3 planeNormal = Vector3.Up;

            // Plane is x-z plane for all tests

            //////// unit box is sitting at {0,1,0} (surface touching)

            box = new Box(new  NoBody( Matrix.CreateTranslation(new Vector3(0f, 1f, 0f))), Matrix.Identity, new Vector3(1f, 1f, 1f));
            plane = new Forever.Physics.Collide.Plane(planeBody, Vector3.Zero, planeNormal);
            data = new CollisionData();

            int contactsFound = IntersectionTests.boxAndPlane(box, plane, data);

            Assert.AreEqual(4, contactsFound);

            Contact contact1 = data.contacts[0];
            Contact contact2 = data.contacts[1];
            Contact contact3 = data.contacts[2];
            Contact contact4 = data.contacts[3];

            Assert.AreEqual(new Vector3(-1f, 0f, -1f), contact1.Point);
            Assert.AreEqual(planeNormal, contact1.Normal);
            Assert.AreEqual(0, contact1.Penetration);

            Assert.AreEqual(new Vector3(-1f, 0f, 1f), contact2.Point);
            Assert.AreEqual(planeNormal, contact2.Normal);
            Assert.AreEqual(0, contact2.Penetration);

            Assert.AreEqual(new Vector3(1f, 0f, -1f), contact3.Point);
            Assert.AreEqual(planeNormal, contact3.Normal);
            Assert.AreEqual(0, contact3.Penetration);

            Assert.AreEqual(new Vector3(1f, 0f, 1f), contact4.Point);
            Assert.AreEqual(planeNormal, contact4.Normal);
            Assert.AreEqual(0, contact4.Penetration);

            //////// unit box is centered at origin

            box = new Box(new NoBody(Matrix.CreateTranslation(new Vector3(0f, 0f, 0f))), Matrix.Identity, new Vector3(1f, 1f, 1f));
            plane = new Forever.Physics.Collide.Plane(planeBody, Vector3.Zero, planeNormal);
            data = new CollisionData();

            contactsFound = IntersectionTests.boxAndPlane(box, plane, data);

            Assert.AreEqual(4, contactsFound);

            contact1 = data.contacts[0];
            contact2 = data.contacts[1];
            contact3 = data.contacts[2];
            contact4 = data.contacts[3];

            Assert.AreEqual(new Vector3(-1f, 0f, -1f), contact1.Point);
            Assert.AreEqual(planeNormal, contact1.Normal);
            Assert.AreEqual(1f, contact1.Penetration);

            Assert.AreEqual(new Vector3(-1f, 0f, 1f), contact2.Point);
            Assert.AreEqual(planeNormal, contact2.Normal);
            Assert.AreEqual(1f, contact2.Penetration);

            Assert.AreEqual(new Vector3(1f, 0f, -1f), contact3.Point);
            Assert.AreEqual(planeNormal, contact3.Normal);
            Assert.AreEqual(1f, contact3.Penetration);

            Assert.AreEqual(new Vector3(1f, 0f, 1f), contact4.Point);
            Assert.AreEqual(planeNormal, contact4.Normal);
            Assert.AreEqual(1f, contact4.Penetration);

            //// unit box centered at y=1 and rotated 45 degrees on each axis

            float angle = (float)Math.PI * 0.25f;
            Matrix boxMatrix = Matrix.CreateFromYawPitchRoll(angle, angle, angle) * Matrix.CreateTranslation(new Vector3(0f, 1f, 0f));

            box = new Box(new NoBody(boxMatrix), Matrix.Identity, new Vector3(1f, 1f, 1f));
            plane = new Forever.Physics.Collide.Plane(planeBody, Vector3.Zero, planeNormal);
            data = new CollisionData();

            contactsFound = IntersectionTests.boxAndPlane(box, plane, data);

            Assert.AreEqual(1, contactsFound);

            contact1 = data.contacts[0];

            Assert.True(
                TrickyMath.AlmostEquals(0.7071067f, contact1.Penetration)
            );

            Assert.True(
                TrickyMath.CloseEnoughToBeSame(
                    new Vector3(-0.2071069f, 0f, -0.2071068f),
                    contact1.Point));

            ////unit box centered at y=sqrt(2) and rotated 45 degrees on each axis

            boxMatrix = Matrix.CreateFromYawPitchRoll(angle, angle, angle) * Matrix.CreateTranslation(new Vector3(0f, TrickyMath.Sqrt(2), 0f));

            box = new Box(new NoBody(boxMatrix), Matrix.Identity, new Vector3(1f, 1f, 1f));

            plane = new Forever.Physics.Collide.Plane(planeBody, Vector3.Zero, planeNormal);
            data = new CollisionData();

            contactsFound = IntersectionTests.boxAndPlane(box, plane, data);

            Assert.AreEqual(1, contactsFound);

            contact1 = data.contacts[0];

            Assert.True(
                TrickyMath.AlmostEquals(0.292893142f,contact1.Penetration)

            );

            Assert.True(
                TrickyMath.CloseEnoughToBeSame(
                    new Vector3(-0.2071069f, 0f, -0.2071068f),
                    contact1.Point
                )
            );
            Assert.AreEqual(Vector3.UnitY, contact1.Normal);
        }
        public void IntersectionTests_BoxVsSphere_NoIntersection()
        {
            float sphereRadius = 1f;
            CollisionData data;
            int contactsFound;
            Contact contact1;

            Forever.Physics.Collide.Sphere sphere;
            Forever.Physics.Collide.Box box;

            // no intersection

            sphere = new Sphere(
                new NoBody( Matrix.CreateTranslation(new Vector3(-10000f, -10000f, -10000f))),
                Matrix.Identity,
                sphereRadius
                    );

            box = new Box(
                new NoBody(Matrix.CreateTranslation(new Vector3(10000f, 10000f, 10000f))),
                Matrix.Identity,
                new Vector3(1f, 1f, 1f)
                );

            data = new CollisionData();

            contactsFound = IntersectionTests.boxAndSphere(box, sphere, data);

            Assert.AreEqual(0, contactsFound);
        }
        public void IntersectionTests_BoxVsSphere_BoxFace()
        {
            //box face intersection

            float sphereRadius = 1f;
            CollisionData data;
            int contactsFound;
            Contact contact1;

            Forever.Physics.Collide.Sphere sphere;
            Forever.Physics.Collide.Box box;

            sphere = new Sphere(
                new NoBody(Matrix.CreateTranslation(Vector3.Up)),
                Matrix.Identity,
                sphereRadius);

            box = new Box(
                new NoBody(Matrix.CreateTranslation(Vector3.Down * 0.95f)),
                Matrix.Identity,
                new Vector3(1f, 1f, 1f));

            data = new CollisionData();

            contactsFound = IntersectionTests.boxAndSphere(box, sphere, data);

            Assert.AreEqual(1, contactsFound);

            contact1 = data.contacts[0];

            Assert.True(
                TrickyMath.AlmostEquals(0.05f, contact1.Penetration)
                );

            Assert.True(
                TrickyMath.CloseEnoughToBeSame(new Vector3(0f, 0.05f, 0f), contact1.Point)
                );

            Assert.AreEqual(Vector3.Down, contact1.Normal);
        }
        public void IntersectionTests_BoxVsSphere_BoxEdge_ZeroPenetration()
        {
            //box edge intersection

            float sphereRadius = 1f;
            CollisionData data;
            int contactsFound;
            Contact contact1;

            Forever.Physics.Collide.Sphere sphere;
            Forever.Physics.Collide.Box box;

            box = new Box(
                new NoBody(), //box centered at origin
                Matrix.Identity,
                new Vector3(1f, 1f, 1f)
                );

            Vector3 spherePos = new Vector3(0f, 1f, 1f);
            spherePos.Normalize();
            spherePos = new Vector3(0f, 1f, 1f) + spherePos;

            sphere = new Sphere(
                new NoBody(
                    Matrix.CreateTranslation(spherePos)
                    ),
                Matrix.Identity,
                sphereRadius);
            data = new CollisionData();

            contactsFound = IntersectionTests.boxAndSphere(box, sphere, data);

            Assert.AreEqual(1, contactsFound);
            contact1 = data.contacts[0];

            Assert.True(
                TrickyMath.AlmostEquals(0f, contact1.Penetration)
                );

            Assert.AreEqual(new Vector3(0f, 1f, 1f), contact1.Point);
            Assert.True(
                TrickyMath.CloseEnoughToBeSame(
                new Vector3(0f, -0.7071068f, -0.7071068f),
                contact1.Normal)
                );
        }
        private static void fillPointFaceBoxBox(Box one, Box two, Vector3 toCenter, CollisionData data, int best, float penetration)
        {
            Contact contact = new Contact();
            Vector3 normal = one.getAxis(best);
            if (Vector3.Dot(normal, toCenter) > 0)
            {
                normal *= -1f;
            }

            Vector3 vertex = two.HalfSizes;
            float oneDot = Vector3.Dot(two.getAxis(0), normal);
            if (oneDot < 0)
            {
                vertex *= new Vector3(-1f, 0f, 0f);
            }

            float twoDot = Vector3.Dot(two.getAxis(1), normal);
            if (twoDot < 0)
            {
                vertex *= new Vector3(0f, -1f, 0f);
            }

            float threeDot = Vector3.Dot(two.getAxis(2), normal);
            if (threeDot < 0)
            {
                vertex *= new Vector3(0f, 0f, -1f);
            }
            contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Point_Face;
            contact.Normal = normal;
            contact.Penetration = penetration;
            contact.Point = Vector3.Transform(vertex, two.Transform);
            contact.Bodies[0] = one.Body;
            contact.Bodies[1] = two.Body;
            contact.Restitution = data.restitution;
            contact.Friction = data.friction;

            data.contacts.Add(contact);
        }
        private static float penetrationOnAxis(Box one, Box two, Vector3 axis, Vector3 toCenter)
        {
            float oneProject = transformToAxis(one, axis);
            float twoProject = transformToAxis(two, axis);

            float dist = TrickyMath.Abs(TrickyMath.ScalarProduct(toCenter, axis));
            return oneProject + twoProject - dist;
        }
        public static int boxAndBox(Box one, Box two, CollisionData data)
        {
            Vector3 toCenter = two.Body.Position - one.Body.Position;

            float pen = float.MaxValue;
            int best = int.MaxValue;

            if (!tryAxis(one, two, one.getAxis(0), toCenter, 0, ref pen, ref best)) return 0;
            if (!tryAxis(one, two, one.getAxis(1), toCenter, 1, ref pen, ref best)) return 0;
            if (!tryAxis(one, two, one.getAxis(2), toCenter, 2, ref pen, ref best)) return 0;

            if (!tryAxis(one, two, one.getAxis(0), toCenter, 3, ref pen, ref best)) return 0;
            if (!tryAxis(one, two, one.getAxis(1), toCenter, 4, ref pen, ref best)) return 0;
            if (!tryAxis(one, two, one.getAxis(2), toCenter, 5, ref pen, ref best)) return 0;

            int bestSingleAxis = best;

            if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(0), two.getAxis(0)), toCenter, 6, ref pen, ref best)) return 0;
            if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(0), two.getAxis(1)), toCenter, 7, ref pen, ref best)) return 0;
            if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(0), two.getAxis(2)), toCenter, 8, ref pen, ref best)) return 0;
            if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(1), two.getAxis(0)), toCenter, 9, ref pen, ref best)) return 0;
            if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(1), two.getAxis(1)), toCenter, 10, ref pen, ref best)) return 0;
            if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(1), two.getAxis(2)), toCenter, 11, ref pen, ref best)) return 0;
            if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(2), two.getAxis(0)), toCenter, 12, ref pen, ref best)) return 0;
            if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(2), two.getAxis(1)), toCenter, 13, ref pen, ref best)) return 0;
            if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(2), two.getAxis(2)), toCenter, 14, ref pen, ref best)) return 0;

            if (best == int.MaxValue)
            {
                throw new Exception("Found no good axis");
            }

            if (best < 3)
            {
                fillPointFaceBoxBox(one, two, toCenter, data, best, pen);

                return 1;
            }
            else if (best < 6)
            {

                // We've got a vertex of box one on a face of box two.
                // We use the same algorithm as above, but swap around
                // one and two (and therefore also the vector between their
                // centres).
                fillPointFaceBoxBox(two, one, toCenter * -1.0f, data, best - 3, pen);

                return 1;
            }
            else
            {
                best -= 6; //move this back to talk about a cardinal axis
                int oneAxisIndex = best / 3;
                int twoAxisIndex = best % 3;
                Vector3 oneAxis = one.getAxis(oneAxisIndex);
                Vector3 twoAxis = two.getAxis(twoAxisIndex);
                Vector3 axis = TrickyMath.VectorProduct(oneAxis, twoAxis);
                axis.Normalize();
                if (Vector3.Dot(axis, toCenter) > 0)
                {
                    axis *= -1f;
                }

                float[] ptOnOneEdge = TrickyMath.Vector3ToFloatArray(one.HalfSizes);
                float[] ptOnTwoEdge = TrickyMath.Vector3ToFloatArray(two.HalfSizes);

                for (int i = 0; i < 3; i++)
                {
                    if (i == oneAxisIndex)
                    {
                        ptOnOneEdge[i] = 0f;
                    }
                    else if (Vector3.Dot(one.getAxis(i), axis) > 0)
                    {
                        ptOnOneEdge[i] *= -1f;
                    }

                    if (i == twoAxisIndex)
                    {
                        ptOnTwoEdge[i] = 0;
                    }
                    else if (Vector3.Dot(two.getAxis(i), axis) < 0)
                    {
                        ptOnTwoEdge[i] *= -1f;
                    }
                }

                Vector3 vertexOnOneEdge = Vector3.Transform(
                                               TrickyMath.FloatArrayToVector3(ptOnOneEdge),
                                               one.Transform
                                               );

                Vector3 vertexOnTwoEdge = Vector3.Transform(
                                               TrickyMath.FloatArrayToVector3(ptOnTwoEdge),
                                               two.Transform
                                               );

                float[] oneHalfSizes = TrickyMath.Vector3ToFloatArray(one.HalfSizes);
                float[] twoHalfSizes = TrickyMath.Vector3ToFloatArray(two.HalfSizes);
                Vector3 vertex = contactPoint(
                        vertexOnOneEdge, oneAxis, oneHalfSizes[oneAxisIndex],
                        vertexOnTwoEdge, twoAxis, twoHalfSizes[twoAxisIndex],
                        bestSingleAxis > 2
                    );

                Contact contact = new Contact();
                contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Edge_Edge;
                contact.Penetration = pen;
                contact.Normal = axis;
                contact.Point = vertex;
                contact.Bodies[0] = one.Body;
                contact.Bodies[1] = two.Body;
                contact.Restitution = data.restitution;
                contact.Friction = data.friction;

                data.contacts.Add(contact);

                return 1;
            }

            return 0;
        }
        private static bool tryAxis(Box one, Box two, Vector3 axis, Vector3 toCenter,
            int index, ref float smallestPenetration, ref int smallestCase)
        {
            if (axis.LengthSquared() < 0.0001f) return true;

            axis.Normalize();

            float penetration = penetrationOnAxis(one, two, axis, toCenter);

            if (penetration < 0)
            {
                return false;
            }
            else if (penetration < smallestPenetration)
            {
                smallestPenetration = penetration;
                smallestCase = index;
            }
            return true;
        }
        /// <summary>
        /// danger! This works for ContactDemo, but there is one hang up.  The call to Math.Abs() should not
        /// be needed.  It isn't in the orange book text.  However, we only get negative numbers from that Dot.
        /// </summary>
        /// <param name="box"></param>
        /// <param name="plane"></param>
        /// <param name="data"></param>
        /// <returns></returns>
        public static int boxAndPlane(Box box, Plane plane, CollisionData data)
        {
            if (data.contactsLeft <= 0) return 0;

            int contacts_found = 0;

            // p dot L < l

            Vector3[] verts = box.WorldVerts();
            foreach (Vector3 vert in verts)
            {
                //float vertexDist = (float)Math.Abs( (double)Vector3.Dot(plane.Normal, vert));
                float vertexDist = Vector3.Dot(-plane.Normal, vert);

                if (vertexDist >= plane.Offset + data.tolerance)
                {
                    // the contact point is halfway between the vertex and the plane
                    // we multiply the direction by half the spearation distance
                    // and add the vertex location
                    Contact contact = new Contact();
                    contact.DiscoveryHint = ContactDiscoveryHint.BoxOnPlane_Corner;
                    contact.Bodies[0] = box.Body;
                    contact.Bodies[1] = null;
                    contact.Point = plane.ClosestPoint(vert);
                    contact.Normal = plane.Normal;
                    contact.Penetration = vertexDist - plane.Offset;//TrickyMathHelper.Abs(plane.Offset - vertexDist);
                    contact.Restitution = data.restitution;
                    contact.Friction = data.friction;
                    data.contacts.Add(contact);
                    contacts_found++;
                }
            }

            return contacts_found;
        }
        public void Contact_BoxSpinningOnYAxisFallsOnToXZPlane()
        {
            // X-Z plane
            NoBody planeBody = new NoBody(Vector3.Zero);
            planeBody.InverseMass = 0f; //infinite mass
            Forever.Physics.Collide.Plane plane = new Forever.Physics.Collide.Plane(planeBody, Vector3.Zero, Vector3.Up);

            planeBody.calculateDerivedData();

            // Spinning box colliding with the plane
            NoBody boxBody = new NoBody(Vector3.Up * 0.95f);

            boxBody.Mass = 1f;
            boxBody.Rotation = new Vector3(0f, 0.01f, 0f);
            boxBody.Velocity = new Vector3(0f, -1f, 0f);
            boxBody.calculateDerivedData();
            Box box = new Box(boxBody, Matrix.Identity, new Vector3(1f, 1f, 1f));

            Contact contact = new Contact();
            contact.Bodies[0] = planeBody;
            contact.Bodies[1] = boxBody;

            contact.Point = Vector3.Zero;
            contact.Penetration = 0.05f;
            contact.Restitution = 1f;
            contact.Friction = 0f;
            contact.Normal = Vector3.Up;

            contact.ReCalc(1f);

            Assert.AreEqual(Vector3.Zero, contact.CalcLocalVelocity(0, 1f));
            Assert.AreEqual(Vector3.Left, contact.CalcLocalVelocity(1, 1f));

            Assert.AreEqual(Vector3.Zero, contact.RelativeContactPositions[0]);
            Assert.AreEqual(Vector3.Down * 0.95f, contact.RelativeContactPositions[1]);
            Assert.AreEqual(Vector3.Right, contact.ContactVelocity);

            // Position Change

            Vector3[] linearChange = new Vector3[2];
            Vector3[] angularChange = new Vector3[2];
            contact.ApplyPositionChange(
                ref linearChange, ref angularChange, contact.Penetration);

            Assert.AreEqual(Vector3.Zero, angularChange[0], "Zero angular change for object 0");
            Assert.AreEqual(Vector3.Zero, angularChange[1], "Zero angular change for object 1");

            Assert.AreEqual(new Vector3(0.0f, 0f, 0f), linearChange[0],
                "Body 0 is not pushed at all because it has infinite mass");
            Assert.AreEqual(new Vector3(0f, -0.05f, 0f), linearChange[1],
                "Body 1 is pushed up (forward in contact direction) by half penetration");

            //Velocity Change

            Vector3[] velocityChange = new Vector3[2];
            Vector3[] rotationChange = new Vector3[2];

            contact.ApplyVelocityChange(ref velocityChange, ref rotationChange);

            Assert.AreEqual(
                Vector3.Zero,
                rotationChange[0],
                "Zero rotation change for object 0");

            Assert.AreEqual(
                Vector3.Zero,
                rotationChange[1],
                "Zero rotation change for object 1");

            Assert.AreEqual(
                Vector3.Zero,
                velocityChange[0],
                "Zero velocity applied to body 0");

            Assert.AreEqual(
                Vector3.Up * 2f,
                velocityChange[1],
                "Counter velocity applied to body 1");
        }
        //TODO - write unit tests!!!!!!
        public static int boxAndPoint(Box box, Vector3 pointWorld, CollisionData data)
        {
            Vector3 pointLocal = Vector3.Transform(pointWorld, Matrix.Invert(box.Body.World * box.OffsetMatrix));

            Vector3 halfSizes = box.HalfSizes;

            Vector3 absPoint = new Vector3(
                TrickyMath.Abs(pointLocal.X),
                TrickyMath.Abs(pointLocal.Y),
                TrickyMath.Abs(pointLocal.Z)
                );

            Vector3 check = halfSizes - absPoint;

            if (check.X < 0 || check.Y < 0 || check.Z < 0)
            {
                return 0;
            }

            Vector3 depth = new Vector3(
                TrickyMath.Abs(absPoint.X - halfSizes.X),
                TrickyMath.Abs(absPoint.Y - halfSizes.Y),
                TrickyMath.Abs(absPoint.Z - halfSizes.Z)
                );

            depth = new Vector3(
                depth.X != 0f ? depth.X : float.MaxValue,
                depth.Y != 0f ? depth.Y : float.MaxValue,
                depth.Z != 0f ? depth.Z : float.MaxValue

                );

            if (depth.X <= depth.Y && depth.X <= depth.Z)
            {
                Contact contact = new Contact();
                contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Point_Face;
                Vector3 boxAxis = box.getAxis(0);
                contact.Normal = boxAxis * (pointLocal.X > 0 ? -1f : 1f);
                contact.Point = pointWorld;
                contact.Penetration = TrickyMath.Abs(absPoint.X - halfSizes.X);

                contact.Bodies[0] = box.Body;
                contact.Bodies[1] = null;

                contact.Restitution = data.restitution;
                contact.Friction = data.friction;
                data.contacts.Add(contact);
                return 1;

            }
            else if (depth.Y <= depth.X && depth.Y <= depth.Z)
            {

                Contact contact = new Contact();
                contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Point_Face;
                Vector3 boxAxis = box.getAxis(1);
                contact.Normal = boxAxis * (pointLocal.Y > 0 ? -1f : 1f);
                contact.Point = pointWorld;
                contact.Penetration = TrickyMath.Abs(absPoint.Y - halfSizes.Y);

                contact.Bodies[0] = box.Body;
                contact.Bodies[1] = null;

                contact.Restitution = data.restitution;
                contact.Friction = data.friction;
                data.contacts.Add(contact);
                return 1;
            }
            else if (depth.Z <= depth.X && depth.Z <= depth.Y)
            {

                Contact contact = new Contact();
                contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Point_Face;
                Vector3 boxAxis = box.getAxis(2);
                contact.Normal = boxAxis * (pointLocal.Z > 0 ? -1f : 1f);
                contact.Point = pointWorld;
                contact.Penetration = TrickyMath.Abs(absPoint.Z - halfSizes.Z);

                contact.Bodies[0] = box.Body;
                contact.Bodies[1] = null;

                contact.Restitution = data.restitution;
                contact.Friction = data.friction;
                data.contacts.Add(contact);
                return 1;
            }
            else
            {
                return 0;
            }
        }