Пример #1
0
        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");
        }
        public static int sphereAndHalfSpace(Sphere sphere, Plane plane, CollisionData data)
        {
            Vector3 pos = sphere.Body.Position;

            //d = p * L - l
            float ballDist = Vector3.Dot(plane.Normal, pos) - (sphere.Radius - plane.Offset);

            if (ballDist > 0) return 0;

            Contact contact = new Contact();
            contact.Normal = plane.Normal;
            contact.Penetration = -ballDist;
            contact.Point = pos - (plane.Normal * (ballDist + sphere.Radius));

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

            contact.Restitution = data.restitution;
            contact.Friction = data.friction;

            data.contacts.Add(contact);

            return 1;
        }
        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);
        }
        /// <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 IntersectionTests_SphereVsPlane()
        {
            Matrix sphereMatrix;
            float sphereRadius = 1f;
            Forever.Physics.Collide.Plane plane;
            Forever.Physics.Collide.Sphere sphere;
            CollisionData data;
            int contactsFound;
            Contact contact1;

            // plane is always the x-z plane for this test method
            Vector3 planeNormal = Vector3.Up;
            plane = new Forever.Physics.Collide.Plane(new NoBody(), Vector3.Zero, planeNormal);

            // unit sphere centered at y=1 (one point of surface touching)
            sphereMatrix = Matrix.CreateTranslation(Vector3.UnitY);
            sphere = new Sphere( new NoBody(sphereMatrix), Matrix.Identity, sphereRadius);

            data = new CollisionData();
            contactsFound = IntersectionTests.sphereAndHalfSpace(sphere, plane, data);

            Assert.AreEqual(1, contactsFound);

            contact1 = data.contacts[0];

            Assert.AreEqual(Vector3.Zero, contact1.Point);
            Assert.AreEqual(0f, contact1.Penetration);
            Assert.AreEqual(planeNormal, contact1.Normal);

            // unit sphere centered at y=0.9 (.1 penetration)
            sphereMatrix = Matrix.CreateTranslation(Vector3.UnitY  * 0.9f);
            sphere = new Sphere(new NoBody(sphereMatrix), Matrix.Identity, sphereRadius);

            data = new CollisionData();
            contactsFound = IntersectionTests.sphereAndHalfSpace(sphere, plane, data);

            Assert.AreEqual(1, contactsFound);

            contact1 = data.contacts[0];

            Assert.AreEqual(Vector3.Zero, contact1.Point);
            Assert.True(
                TrickyMath.AlmostEquals(0.1f, contact1.Penetration)
                );
            Assert.AreEqual(planeNormal, contact1.Normal);
        }