public RigidBodyEntity(Vector3 pos)
        {
            body = new RigidBody(pos);

              debug_compass = new DebugCompass(this.CenterOfMass);

              debug_compass.LoadContent(null);
        }
        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);
        }
        public void RigidBody_ForceOnCenterOfMass()
        {
            RigidBody body = new RigidBody(Vector3.Zero);
            body.Awake = true;
            body.Mass = 1f;
            Helpers.AssertBodyAtRest(body);

            body.addForce(Vector3.UnitZ);
            body.integrate(1f);
            Assert.AreEqual(Vector3.Zero, body.AngularMomentum);
            Assert.AreEqual(Vector3.UnitZ, body.LinearMomentum);
            Assert.AreEqual(new Vector3(0f, 0f, 1f), body.Position);

            body.addForce(-Vector3.UnitZ * 2f);
            body.integrate(1f);
            Assert.AreEqual(Vector3.Zero, body.AngularMomentum);
            Assert.AreEqual(-Vector3.UnitZ, body.LinearMomentum);
            Assert.AreEqual(new Vector3(0f, 0f, 0f), body.Position);

            body = new RigidBody(Vector3.Zero);
            body.Awake = true;
            body.Mass = 1f;
            Helpers.AssertBodyAtRest(body);

            body.addForce(-Vector3.UnitZ);
            body.integrate(10f);

            Assert.AreEqual(new Vector3(0f, 0f, -1000f), body.Position);
            Assert.AreEqual(Vector3.Zero, body.AngularMomentum);
        }
            public static RigidBody CreateSphereBody(float mass, float radius)
            {
                RigidBody body = new RigidBody(Vector3.Zero);
                body.Awake = true;

                //simulating hollow sphere of mass and length

                body.Mass = mass;

                float sphere_coeff = ((1f / 12f) * mass * radius * radius);
                Matrix inertiaTensor = new Matrix(
                    sphere_coeff, 0f, 0f, 0f,
                    0f, sphere_coeff, 0f, 0f,
                    0f, 0f, sphere_coeff, 0f,
                    0f, 0f, 0f, sphere_coeff
                );

                body.InertiaTensor = inertiaTensor;
                body.LinearDamping = 1f;
                body.AngularDamping = 1f;

                return body;
            }
            // Just checks that body isn't moving and has default values
            public static void AssertBodyAtRest(RigidBody body, Vector3 expectedPosition = new Vector3())
            {
                Assert.AreEqual(expectedPosition, body.CenterOfMass, "Center of Mass");
                Assert.AreEqual(expectedPosition, body.Position, "Position");
                Assert.AreEqual(Vector3.Zero, body.Velocity, "Velocity");
                Assert.AreEqual(Vector3.Zero, body.Acceler, "Acceleration");
                Assert.AreEqual(Quaternion.Identity, body.Orientation, "Orientation");

                Assert.AreEqual(1f, body.LinearDamping, "LinearDamping");
                Assert.AreEqual(1f, body.AngularDamping, "AngularDamping");
                Assert.AreEqual(Matrix.Identity, body.World, "World Matrix");
            }
 void IntegrateBody(RigidBody rigidBody, float duration, int numIterations)
 {
     for (int i = 0; i < numIterations; i++)
     {
         rigidBody.integrate(duration);
     }
 }
        public void RigidBody_Zero_Case()
        {
            RigidBody body = new RigidBody(Vector3.Zero);

            //starts off awake
            Assert.AreEqual(true, body.Awake);

            //default starting state
            Helpers.AssertBodyAtRest(body);
        }
        public void RigidBody_WorldMatrix()
        {
            RigidBody body = new RigidBody(Vector3.Zero);
            body.Awake = true;
            body.Mass = 1f;

            float angle = (float)Math.PI;
            Quaternion orientation = Quaternion.CreateFromYawPitchRoll(angle, 0f, 0f);
            body.Orientation = orientation;
            body.Position = Vector3.Up;

            Matrix expectedWorld = new Matrix(
                    -1f, 0f, .00000008742278f, 0f,
                    0f, 1f, 0f, 0f,
                    -.00000008742278f, 0f, -1f, 0f,
                    0f, 1f, 0f, 1f
                );

            //integrate once  for update
            body.integrate(1f);

            Assert.AreEqual(expectedWorld, body.World);

            int total_iterations = 1000000;
            for (int i = 1; i <= total_iterations; i++)
            {
                body.integrate(1);
                Assert.AreEqual(expectedWorld, body.World, "Iteration: " + i);

            }

            Assert.AreEqual(expectedWorld, body.World);
        }
        public void RigidBody_Translate()
        {
            RigidBody body = new RigidBody(Vector3.Zero);

            Helpers.AssertBodyAtRest(body);
            body.Translate(Vector3.Up);
            Helpers.AssertBodyAtRest(body, Vector3.Up);
            body.Translate(Vector3.Up);
            Helpers.AssertBodyAtRest(body, Vector3.Up*2f);
        }
        public void RigidBody_Sleeping()
        {
            RigidBody body = new RigidBody(Vector3.Zero);
            body.Mass = 1f; // it's allowed to move if it thinks it shoud...

            //starts out awake
            Assert.AreEqual(true, body.Awake);
            Helpers.AssertBodyAtRest(body);

            // Adding a force, wakes it up
            body.addForce(Vector3.Up);
            Assert.AreEqual(true, body.Awake);

            // putting  it to sleep before integration should bypass any movement calcs.

            body.Awake = false;
            body.integrate(1);

            Helpers.AssertBodyAtRest(body);
        }
        public void RigidBody_InfiniteMass()
        {
            RigidBody body = new RigidBody(Vector3.Zero);
            body.InverseMass = 0f;

            Assert.AreEqual(float.MaxValue, body.Mass, "Mass");
            Assert.False(body.HasFiniteMass, "HasFiniteMass");

            Vector3 force = Vector3.Up;
            Helpers.AssertBodyAtRest(body, Vector3.Zero);

            //add force at center of mass
            body.addForce(force);
            body.integrate(1);
            Helpers.AssertBodyAtRest(body);

            //Now add force at point other than center of mass
            Vector3 worldCoord = Vector3.Right;
            body.addForce(force, worldCoord);
            body.integrate(1);

            Helpers.AssertBodyAtRest(body);

            //Now let's show that infinitely massed objects don't respond to torque application
            Vector3 torque = Vector3.Right;

            body.addTorque(torque);
            body.integrate(1);

            Helpers.AssertBodyAtRest(body);
        }
        public void RigidBody_InertiaTensor()
        {
            RigidBody body = new RigidBody(Vector3.Zero);
            body.Awake = true;

            //simulating hollow sphere of mass and length

            float radius = 2f;
            float mass = 1f;
            body.Mass = mass;

            float sphere_coeff = ((1f / 12f) * mass * radius  * radius);
            Matrix inertiaTensor = new Matrix(
                sphere_coeff, 0f, 0f, 0f,
                0f, sphere_coeff, 0f, 0f,
                0f, 0f, sphere_coeff, 0f,
                0f, 0f, 0f, 1f
            );

            body.InertiaTensor = inertiaTensor;

            Assert.AreEqual(Matrix.Identity, body.World, "Initial World");
            Assert.AreEqual(inertiaTensor, body.InertiaTensor, "Initial Intertia Tensor");
            Assert.AreEqual(Matrix.Invert(inertiaTensor), body.InverseInertiaTensor, "Initial InverseInertiaTensor");

            //no bleed off
            body.LinearDamping = 1f;

            //adding a force once should set it moving, never to stop again
            body.addForce(Vector3.UnitZ, body.Position + (new Vector3(-radius, 0f, 0f)));

            //nothing physically happens until integration time
            Assert.AreEqual(Vector3.Zero, body.Position, "Intitial Position");
            Assert.AreEqual(Vector3.Zero, body.AngularMomentum, "Intial Rotation");
            Assert.AreEqual(Quaternion.Identity, body.Orientation, "Intial Orientation");

            body.integrate(1f);

            Assert.AreEqual(Vector3.UnitZ * 1f, body.Position, "Computed Position");
            Assert.AreEqual(new Vector3(0f, 2f, 0f), body.AngularMomentum, "Computed Rotation");
            Quaternion orientation = new Quaternion(0f, 0.707106769f, 0f, 0.707106769f);
            Assert.AreEqual(orientation.X, body.Orientation.X);
            Assert.AreEqual(orientation.Y, body.Orientation.Y);
            Assert.AreEqual(orientation.Z, body.Orientation.Z);
            Assert.AreEqual(orientation.W, body.Orientation.W);

            Assert.AreEqual(orientation, body.Orientation, "Computed Orientation");
        }
        public ModelEntity CreateDefaultFromModel(Model model, Vector3 pos)
        {
            AABBTree tree = AABBFactory.AABBTree(model);
            BoundingBox box = tree.Root.BBox;

            float radius = BoundingSphere.CreateFromBoundingBox(box).Radius;
            float mass = 1f;
            RigidBody body = new RigidBody(pos);

            body.LinearDamping = LinearDamping;
            body.AngularDamping = AngularDamping;
            body.Awake = true;
            body.Mass = mass;
            body.InertiaTensor = InertiaTensorFactory.Sphere(mass, radius);

            Sphere prim = new Sphere(body, Matrix.Identity, radius);
            EntityGeometryData geoData = new EntityGeometryData();
            geoData.BoundingSphere = new BoundingSphere(Vector3.Zero, radius);
            geoData.Prim = prim;

            return new ModelEntity(model, body, geoData);
        }
        public ModelEntity CreateSphere(Vector3 position, float mass, float dimension)
        {
            Model model = Models[CollideType.Sphere];
            float radius = dimension;

            RigidBody body = new RigidBody(position);

            body.LinearDamping = LinearDamping;
            body.AngularDamping = AngularDamping;
            body.Awake = true;
            body.Mass = mass;
            body.InertiaTensor = InertiaTensorFactory.Sphere(mass, radius);

            Sphere prim = new Sphere(body, Matrix.Identity, radius);

            EntityGeometryData geoData = new EntityGeometryData();

            geoData.Prim = prim;
            geoData.BoundingSphere = new BoundingSphere(Vector3.Zero, prim.Radius);

            return new ModelEntity(model, body, geoData);
        }
Esempio n. 15
0
        protected void SetupRenderContextAndCamera()
        {
            ICamera cam = new EyeCamera();

            renderContext = new RenderContext(
                cam,
                this.ScreenManager.Game.GraphicsDevice
                );

            float radius = 2f;
            float mass = 1f;

            CamBody = new RigidBody(cam.Position);
            CamBody.Label = "Camera Body";
            CamBody.Awake = true;
            CamBody.LinearDamping = 0.9f;
            CamBody.AngularDamping = 0.7f;
            CamBody.Mass = mass;
            CamBody.InertiaTensor = InertiaTensorFactory.Sphere(mass, radius);
            CamControls = new UserControls(PlayerIndex.One, 0.000015f, 0.25f, 0.0003f, 1f);
        }