BEPU physics demo - bad broken new object type

using BEPUphysics.BroadPhaseEntries;
using BEPUphysics.Entities.Prefabs;
using BEPUutilities;
using BEPUphysics.CollisionShapes.ConvexShapes;
using BEPUphysics.Entities;
using BEPUphysics.Constraints.SolverGroups;
using BEPUphysics.Paths;
using BEPUphysics.Paths.PathFollowing;
using BEPUphysics.Constraints.TwoEntity.Motors;
using BEPUphysics.CollisionShapes;
using System.Collections.Generic;
using Microsoft.Xna.Framework.Graphics;
// MCMONKEY - pay no mind to me "using" the everything.
using BEPUphysics.BroadPhaseEntries.MobileCollidables;
using System;
using BEPUphysics.BroadPhaseEntries.Events;
using BEPUphysics.OtherSpaceStages;
using BEPUphysics.NarrowPhaseSystems;
using BEPUphysics.BroadPhaseSystems;
using BEPUphysics.NarrowPhaseSystems.Pairs;
using BEPUphysics.CollisionRuleManagement;
using BEPUutilities.DataStructures;
using BEPUphysics.CollisionTests.Manifolds;
using BEPUphysics.Constraints.Collision;
using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK;
using BEPUphysics.PositionUpdating;
using BEPUphysics.Settings;
using BEPUphysics.Materials;
using System.Linq;
using System.Text;
using BEPUphysics.CollisionTests.CollisionAlgorithms;
using BEPUutilities.ResourceManagement;
using BEPUphysics.CollisionTests;
// MCMONKEY - end spam of "using" statements

namespace BEPUphysicsDemos.Demos
{

    // MCMONKEY - begin custom stuffs

    public class VoxelWorldShape : CollisionShape
    {
        // NOTE: No need for an entire voxel system yet... let's just pretend the voxels make up a flat terrain at Y=0, covering 10,000*10,000 units.
        Box testShape = new Box(new Vector3(0, -5000, 0), 10 * 1000, 10 * 1000, 10 * 1000);

        public bool RayCast(ref Ray ray, float maximumLength, out RayHit hit)
        {
            return testShape.CollisionInformation.RayCast(ray, maximumLength, out hit);
        }


        public bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
        {
            return testShape.CollisionInformation.ConvexCast(castShape, ref startingTransform, ref sweep, out hit);
        }
    }

    public class VoxelWorldObject : StaticCollidable
    {
        public static void RegisterMe()
        {
            NarrowPhasePairFactory<ConvexVoxelPairHandler> fact = new NarrowPhasePairFactory<ConvexVoxelPairHandler>();
            NarrowPhaseHelper.CollisionManagers.Add(new TypePair(typeof(ConvexCollidable<BoxShape>), typeof(VoxelWorldObject)), fact);
            NarrowPhaseHelper.CollisionManagers.Add(new TypePair(typeof(ConvexCollidable<SphereShape>), typeof(VoxelWorldObject)), fact);
            NarrowPhaseHelper.CollisionManagers.Add(new TypePair(typeof(ConvexCollidable<CapsuleShape>), typeof(VoxelWorldObject)), fact);
            NarrowPhaseHelper.CollisionManagers.Add(new TypePair(typeof(ConvexCollidable<TriangleShape>), typeof(VoxelWorldObject)), fact);
            NarrowPhaseHelper.CollisionManagers.Add(new TypePair(typeof(ConvexCollidable<CylinderShape>), typeof(VoxelWorldObject)), fact);
            NarrowPhaseHelper.CollisionManagers.Add(new TypePair(typeof(ConvexCollidable<ConeShape>), typeof(VoxelWorldObject)), fact);
            NarrowPhaseHelper.CollisionManagers.Add(new TypePair(typeof(ConvexCollidable<TransformableShape>), typeof(VoxelWorldObject)), fact);
            NarrowPhaseHelper.CollisionManagers.Add(new TypePair(typeof(ConvexCollidable<MinkowskiSumShape>), typeof(VoxelWorldObject)), fact);
            NarrowPhaseHelper.CollisionManagers.Add(new TypePair(typeof(ConvexCollidable<WrappedShape>), typeof(VoxelWorldObject)), fact);
            NarrowPhaseHelper.CollisionManagers.Add(new TypePair(typeof(ConvexCollidable<ConvexHullShape>), typeof(VoxelWorldObject)), fact);
            NarrowPhaseHelper.CollisionManagers.Add(new TypePair(typeof(TriangleCollidable), typeof(VoxelWorldObject)), fact);
        }

        public VoxelWorldObject()
        {
            WorldShape = new VoxelWorldShape();
            boundingBox = new BoundingBox(new Vector3(-5 * 1000, -10 * 1000, -5 * 1000), new Vector3(5 * 1000, 0, 5 * 1000));
        }

        public VoxelWorldShape WorldShape;

        public ContactEventManager<VoxelWorldObject> Events = new ContactEventManager<VoxelWorldObject>();

        protected override IContactEventTriggerer EventTriggerer
        {
            get { return Events; }
        }

        protected override IDeferredEventCreator EventCreator
        {
            get { return Events; }
        }

        public override void UpdateBoundingBox()
        {
            boundingBox = new BoundingBox(new Vector3(-5 * 1000, -10 * 1000, -5 * 1000), new Vector3(5 * 1000, 0, 5 * 1000));
        }

        public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func<BroadPhaseEntry, bool> filter, out RayHit hit)
        {
            return WorldShape.ConvexCast(castShape, ref startingTransform, ref sweep, out hit);
        }

        public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
        {
            return ConvexCast(castShape, ref startingTransform, ref sweep, null, out hit);
        }

        public override bool RayCast(Ray ray, float maximumLength, Func<BroadPhaseEntry, bool> filter, out RayHit rayHit)
        {
            return WorldShape.RayCast(ref ray, maximumLength, out rayHit);
        }

        public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit)
        {
            return RayCast(ray, maximumLength, null, out rayHit);
        }
    }

    public class ConvexVoxelPairHandler : StandardPairHandler
    {
        VoxelWorldObject VWO = null;

        ConvexCollidable convex = null;

        private NonConvexContactManifoldConstraint contactConstraint;

        public override Collidable CollidableA
        {
            get { return convex; }
        }

        public override Collidable CollidableB
        {
            get { return VWO; }
        }

        public override Entity EntityA
        {
            get { return convex.Entity; }
        }

        public override Entity EntityB
        {
            get { return null; }
        }

        public override ContactManifoldConstraint ContactConstraint
        {
            get { return contactConstraint; }
        }

        public override ContactManifold ContactManifold
        {
            get { return contactManifold; }
        }

        VoxelContactManifold contactManifold = new VoxelContactManifold();

        public ConvexVoxelPairHandler()
        {
            contactConstraint = new NonConvexContactManifoldConstraint(this);
        }

        bool noRecurse = false;

        public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
        {
            if (noRecurse) // NOTE: Needed because UpdateMaterialProperties will call this,
            { // because we can't access the internal method that does the same code without calling init :(
                return;
            }
            noRecurse = true;
            VWO = entryA as VoxelWorldObject;
            convex = entryB as ConvexCollidable;
            if (VWO == null || convex == null)
            {
                VWO = entryB as VoxelWorldObject;
                convex = entryA as ConvexCollidable;
                if (VWO == null || convex == null)
                {
                    throw new ArgumentException("Inappropriate types used to initialize pair.");
                }
            }
            BroadPhaseOverlap = new BEPUphysics.BroadPhaseSystems.BroadPhaseOverlap(convex, VWO);
            UpdateMaterialProperties(convex.Entity != null ? convex.Entity.Material : null, VWO.Material);
            base.Initialize(entryA, entryB);
            // NOTE: Just in case, force friction off for testing's sake
            InteractionProperties ip = contactConstraint.MaterialInteraction;
            ip.StaticFriction = 0f;
            ip.KineticFriction = 0f;
            ip.Bounciness = 0.5f;
            contactConstraint.MaterialInteraction = ip;
            noRecurse = false;
        }

        ///<summary>
        /// Cleans up the pair handler.
        ///</summary>
        public override void CleanUp()
        {
            base.CleanUp();

            VWO = null;
            convex = null;
        }

        public override void UpdateTimeOfImpact(Collidable requester, float dt)
        {
            if (convex.Entity != null && convex.Entity.ActivityInformation.IsActive && convex.Entity.PositionUpdateMode == PositionUpdateMode.Continuous)
            {
                timeOfImpact = 1;
                RigidTransform rt = new RigidTransform(convex.Entity.Position, convex.Entity.Orientation);
                Vector3 sweep = convex.Entity.LinearVelocity;
                sweep *= dt;
                RayHit rh;
                if (VWO.ConvexCast(convex.Shape, ref rt, ref sweep, out rh))
                {
                    timeOfImpact = rh.T;
                }
                if (TimeOfImpact < 0)
                {
                    timeOfImpact = 0;
                }
            }
        }

        protected override void GetContactInformation(int index, out ContactInformation info)
        {
            ContactInformation ci = new ContactInformation();
            ci.Contact = contactManifold.ctcts[index];
            ci.Pair = this;
            ReadOnlyList<ContactPenetrationConstraint> list = contactConstraint.ContactPenetrationConstraints;
            float totalimp = 0;
            for (int i = 0; i < list.Count; i++)
            {
                totalimp += list[i].NormalImpulse;
            }
            ci.NormalImpulse = list[index].NormalImpulse;
            ci.FrictionImpulse = (ci.NormalImpulse / totalimp) * list[index].RelativeVelocity;
            if (convex.Entity != null)
            {
                Vector3 velocity;
                Vector3 cep = convex.Entity.Position;
                Vector3 ceav = convex.Entity.AngularVelocity;
                Vector3 celv = convex.Entity.LinearVelocity;
                Vector3.Subtract(ref ci.Contact.Position, ref cep, out velocity);
                Vector3.Cross(ref ceav, ref velocity, out velocity);
                Vector3.Add(ref velocity, ref celv, out ci.RelativeVelocity);
            }
            else
            {
                ci.RelativeVelocity = new Vector3(0, 0, 0);
            }
            info = ci;
        }
    }


    public class VoxelContactManifold : ContactManifold
    {
        protected ConvexCollidable convex;

        protected VoxelWorldObject vwo;

        public override void Initialize(Collidable newCollidableA, Collidable newCollidableB)
        {
            convex = newCollidableA as ConvexCollidable;
            vwo = newCollidableB as VoxelWorldObject;
            if (convex == null || vwo == null)
            {
                convex = newCollidableB as ConvexCollidable;
                vwo = newCollidableA as VoxelWorldObject;
                if (convex == null || vwo == null)
                {
                    throw new ArgumentException("Inappropriate types used to initialize contact manifold.");
                }
            }
        }

        public VoxelContactManifold()
        {
            contacts = new RawList<Contact>();
            unusedContacts = new UnsafeResourcePool<Contact>(4);
            contactIndicesToRemove = new RawList<int>();
        }

        public RawList<Contact> ctcts
        {
            get
            {
                return contacts;
            }
        }

        public static bool IsNaNOrInfOrZero(ref Vector3 vec)
        {
            return float.IsInfinity(vec.X) || float.IsNaN(vec.X)
                || float.IsInfinity(vec.Y) || float.IsNaN(vec.Y)
                || float.IsInfinity(vec.Z) || float.IsNaN(vec.Z) || (vec.X == 0 && vec.Y == 0 && vec.Z == 0);
        }

        public static bool IsNaNOrInf(ref Vector3 vec)
        {
            return float.IsInfinity(vec.X) || float.IsNaN(vec.X)
                || float.IsInfinity(vec.Y) || float.IsNaN(vec.Y)
                || float.IsInfinity(vec.Z) || float.IsNaN(vec.Z);
        }

        public override void Update(float dt)
        {

            RigidTransform rt = convex.Entity == null ? convex.WorldTransform : new RigidTransform(convex.Entity.Position, convex.Entity.Orientation);
            if (IsNaNOrInf(ref rt.Position))
            {
                for (int i = contacts.Count - 1; i >= 0; i--)
                {
                    Remove(i);
                }
                return;
            }
            Vector3 sw = new Vector3(0, 0, 1f);
            if (convex.Entity != null)
            {
                sw = convex.Entity.LinearVelocity;
            }
            RayHit rh;
            bool hit = vwo.ConvexCast(convex.Shape, ref rt, ref sw, out rh);
            if (!hit || IsNaNOrInfOrZero(ref rh.Normal))
            {
                for (int i = contacts.Count - 1; i >= 0; i--)
                {
                    Remove(i);
                }
                return;
            }
            float pendef = convex.BoundingBox.Min.Y; // NOTE: not sure whether to negate this
            Vector3 norm;
            RigidTransform rtx = new RigidTransform(Vector3.Zero, rt.Orientation);
            RigidTransform.Transform(ref rh.Normal, ref rtx, out norm);
            norm = -norm; // TODO: Why must we negate here?
            for (int i = contacts.Count - 1; i >= 0; i--)
            {
                contacts[i].Normal = norm;
                contacts[i].Position = rh.Location;
                contacts[i].PenetrationDepth = pendef;
            }
            if (Contacts.Count == 0)
            {
                ContactData cd = new ContactData();
                cd.Normal = norm;
                cd.Position = rh.Location;
                cd.PenetrationDepth = pendef;
                cd.Id = contacts.Count;
                Add(ref cd);
            }
        }

        public override void CleanUp()
        {
            convex = null;
            vwo = null;
            base.CleanUp();
        }
    }

    // MCMONKEY - end custom stuffs

    /// <summary>
    /// A playground for the character controller to frolic in.
    /// </summary>
    public class CharacterPlaygroundDemo : StandardDemo
    {
        /// <summary>
        /// Constructs a new demo.
        /// </summary>
        /// <param name="game">Game owning this demo.</param>
        public CharacterPlaygroundDemo(DemosGame game)
            : base(game)
        {
            VoxelWorldObject.RegisterMe(); // MCMONKEY -> Register the voxel world!
            Space.Add(new VoxelWorldObject()); // MCMONKEY -> Add a voxel world to the space!

            game.Camera.Position = new Vector3(-10, 7, 5);
            game.Camera.ViewDirection = new Vector3(0, 0, 1);
            //Since this is the character playground, turn on the character by default.
            character.Activate();
            //Having the character body visible would be a bit distracting.
            character.CharacterController.Body.Tag = "noDisplayObject";

            //Load in mesh data for the environment.
            Vector3[] staticTriangleVertices;
            int[] staticTriangleIndices;


            var playgroundModel = game.Content.Load<Model>("CharacterControllerTestTerrain");
            //This is a little convenience method used to extract vertices and indices from a model.
            //It doesn't do anything special; any approach that gets valid vertices and indices will work.
            ModelDataExtractor.GetVerticesAndIndicesFromModel(playgroundModel, out staticTriangleVertices, out staticTriangleIndices);
            var staticMesh = new StaticMesh(staticTriangleVertices, staticTriangleIndices, new AffineTransform(new Vector3(0.01f, 0.01f, 0.01f), Quaternion.Identity, new Vector3(0, 0, 0)));
            staticMesh.Sidedness = TriangleSidedness.Counterclockwise;

            Space.Add(staticMesh);
            game.ModelDrawer.Add(staticMesh);



            //Add a spinning blade for the character to ram itself into.
            var fanBase = new Cylinder(new Vector3(-13, .5f, 50), 1.1f, 1);
            var fanBlade = new Box(fanBase.Position + new Vector3(0, .8f, 0), 5, .1f, 1f, 5);
            var fanJoint = new RevoluteJoint(fanBase, fanBlade, (fanBase.Position + fanBlade.Position) * .5f, Vector3.Up);
            fanJoint.Motor.IsActive = true;
            fanJoint.Motor.Settings.VelocityMotor.GoalVelocity = 30;
            fanJoint.Motor.Settings.MaximumForce = 300;
            Space.Add(fanBase);
            Space.Add(fanBlade);
            Space.Add(fanJoint);

            //Add a bridge connecting the two towers.
            Vector3 startPosition = new Vector3(-19.3f, 10.5f - .25f, 23 - .85f);
            var startPlatform = new Box(startPosition - new Vector3(0, 0, 2.2f), 4, .5f, 6);
            Space.Add(startPlatform);
            Vector3 offset = new Vector3(0, 0, 1.7f);
            Box previousLink = startPlatform;
            Vector3 position = new Vector3();
            for (int i = 1; i <= 7; i++)
            {
                position = startPosition + offset * i;
                Box link = new Box(position, 3, .3f, 1.5f, 50);
                link.LinearDamping = .1f;
                link.AngularDamping = .1f;
                Space.Add(link);
                Space.Add(new RevoluteJoint(previousLink, link, position - offset * .5f, Vector3.Right));

                previousLink = link;
            }
            var endPlatform = new Box(position - new Vector3(0, 0, -3.8f), 4, .5f, 6);
            Space.Add(endPlatform);

            Space.Add(new RevoluteJoint(previousLink, endPlatform, position + offset * .5f, Vector3.Right));


            //Add in a floating platform controlled by a curve to serve as an elevator.
            Entity movingEntity = new Box(new Vector3(-10, 0, -10), 3, 1, 3);

            var positionCurve = new CardinalSpline3D();

            positionCurve.PreLoop = CurveEndpointBehavior.Mirror;
            positionCurve.PostLoop = CurveEndpointBehavior.Mirror;

            positionCurve.ControlPoints.Add(-1, new Vector3(-19.3f, 0, 43));
            positionCurve.ControlPoints.Add(0, new Vector3(-19.3f, 0, 43));
            positionCurve.ControlPoints.Add(2, new Vector3(-19.3f, 0, 43));
            positionCurve.ControlPoints.Add(3, new Vector3(-19.3f, 0, 43));
            positionCurve.ControlPoints.Add(4, new Vector3(-19.3f, 5, 43));
            positionCurve.ControlPoints.Add(5f, new Vector3(-19.3f, 10, 43));
            positionCurve.ControlPoints.Add(6f, new Vector3(-19.3f, 10, 43));
            positionCurve.ControlPoints.Add(8f, new Vector3(-19.3f, 10, 43));
            positionCurve.ControlPoints.Add(9f, new Vector3(-19.3f, 10, 43));

            elevatorMover = new EntityMover(movingEntity);
            Space.Add(elevatorMover);
            Space.Add(movingEntity);

            elevatorPath = positionCurve;

            //Add in another floating platform controlled by a curve for horizontal transport.
            movingEntity = new Box(new Vector3(-10, 0, -10), 2.5f, .5f, 2.5f);

            var platformCurve = new LinearInterpolationCurve3D();

            platformCurve.PreLoop = CurveEndpointBehavior.Mirror;
            platformCurve.PostLoop = CurveEndpointBehavior.Mirror;

            platformCurve.ControlPoints.Add(0, new Vector3(-1.75f, 10, 21.5f));
            platformCurve.ControlPoints.Add(2, new Vector3(-1.75f, 10, 21.5f));
            platformCurve.ControlPoints.Add(5, new Vector3(-1.75f, 10, 15.5f));
            platformCurve.ControlPoints.Add(10, new Vector3(-19.3f, 10, 15.5f));
            platformCurve.ControlPoints.Add(12, new Vector3(-19.3f, 10, 15.5f));
            platformCurve.ControlPoints.Add(15, new Vector3(-25, 10, 15.5f));
            platformCurve.ControlPoints.Add(22, new Vector3(-25, 10, 38));
            platformCurve.ControlPoints.Add(23, new Vector3(-22.75f, 10, 38));
            platformCurve.ControlPoints.Add(25, new Vector3(-22.75f, 10, 38));

            //Make it spin too.  That'll be fun.  Or something.
            var platformRotationCurve = new QuaternionSlerpCurve();
            platformRotationCurve.PreLoop = CurveEndpointBehavior.Mirror;
            platformRotationCurve.PostLoop = CurveEndpointBehavior.Mirror;
            platformRotationCurve.ControlPoints.Add(0, Quaternion.Identity);
            platformRotationCurve.ControlPoints.Add(15, Quaternion.Identity);
            platformRotationCurve.ControlPoints.Add(22, Quaternion.CreateFromAxisAngle(Vector3.Up, MathHelper.PiOver2));
            platformRotationCurve.ControlPoints.Add(25, Quaternion.CreateFromAxisAngle(Vector3.Up, MathHelper.PiOver2));

            platformMover = new EntityMover(movingEntity);
            platformRotator = new EntityRotator(movingEntity);
            Space.Add(platformMover);
            Space.Add(platformRotator);
            Space.Add(movingEntity);

            platformPath = platformCurve;
            platformOrientationPath = platformRotationCurve;

            //Add in a diving board.

            Box divingBoardBase = new Box(new Vector3(-9, 10, 39.3f), 5, 1, 3);
            Box divingBoard = new Box(divingBoardBase.Position + new Vector3(-2, 0, 3.5f), 1, .3f, 3, 5);
            var divingBoardJoint = new RevoluteJoint(divingBoardBase, divingBoard, divingBoard.Position + new Vector3(0, 0, -1.5f), Vector3.Right);
            divingBoardJoint.Motor.IsActive = true;
            divingBoardJoint.Motor.Settings.Mode = MotorMode.Servomechanism;
            divingBoardJoint.Motor.Settings.Servo.Goal = 0;
            divingBoardJoint.Motor.Settings.Servo.SpringSettings.Stiffness = 5000;
            divingBoardJoint.Motor.Settings.Servo.SpringSettings.Damping = 0;

            Space.Add(divingBoardBase);
            Space.Add(divingBoard);
            Space.Add(divingBoardJoint);


            //Add a second diving board for comparison.

            Box divingBoard2 = new Box(divingBoardBase.Position + new Vector3(2, 0, 5f), 1, .3f, 6, 5);
            var divingBoardJoint2 = new RevoluteJoint(divingBoardBase, divingBoard2, divingBoard2.Position + new Vector3(0, 0, -3), Vector3.Right);
            divingBoardJoint2.Motor.IsActive = true;
            divingBoardJoint2.Motor.Settings.Mode = MotorMode.Servomechanism;
            divingBoardJoint2.Motor.Settings.Servo.Goal = 0;
            divingBoardJoint2.Motor.Settings.Servo.SpringSettings.Stiffness = 10000;
            divingBoardJoint2.Motor.Settings.Servo.SpringSettings.Damping = 0;

            Space.Add(divingBoard2);
            Space.Add(divingBoardJoint2);

            //Add a seesaw for people to jump on.
            Box seesawBase = new Box(new Vector3(-7, .45f, 52), 1, .9f, .3f);
            Box seesawPlank = new Box(seesawBase.Position + new Vector3(0, .65f, 0), 1.2f, .2f, 6, 3);
            RevoluteJoint seesawJoint = new RevoluteJoint(seesawBase, seesawPlank, seesawPlank.Position, Vector3.Right);
            Space.Add(seesawJoint);
            Space.Add(seesawBase);
            Space.Add(seesawPlank);

            Space.Add(new Box(seesawPlank.Position + new Vector3(0, 1.3f, 2), 1, 1, 1, 5));


            //Add in some boxes to bump and jump on.
            int numColumns = 3;
            int numRows = 3;
            int numHigh = 3;
            float xSpacing = 1.01f;
            float ySpacing = 1.01f;
            float zSpacing = 1.01f;
            for (int i = 0; i < numRows; i++)
                for (int j = 0; j < numColumns; j++)
                    for (int k = 0; k < numHigh; k++)
                    {
                        Space.Add(new Box(new Vector3(
                                                 5 + xSpacing * i - (numRows - 1) * xSpacing / 2f,
                                                 1.58f + k * (ySpacing),
                                                 45 + zSpacing * j - (numColumns - 1) * zSpacing / 2f),
                                             .5f, .5f, .5f, 5));
                    }



            //Add a log to roll!
            //Make it a compound so some boxes can be added to let the player know it's actually spinning.
            CompoundBody log = new CompoundBody(new List<CompoundShapeEntry>()
            {
                new CompoundShapeEntry(new CylinderShape(4, 1.8f), Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2), 20),
                new CompoundShapeEntry(new BoxShape(.5f, .5f, 3.7f),  new Vector3(1.75f, 0,0), 0),
                new CompoundShapeEntry(new BoxShape(.5f, 3.7f, .5f), new Vector3(1.75f, 0,0), 0),
                new CompoundShapeEntry(new BoxShape(.5f, .5f, 3.7f),  new Vector3(-1.75f, 0,0), 0),
                new CompoundShapeEntry(new BoxShape(.5f, 3.7f, .5f), new Vector3(-1.75f, 0,0), 0)
            }, 50);
            log.Position = new Vector3(-14.5f, 10, 41);
            log.AngularDamping = 0;


            RevoluteJoint logJointA = new RevoluteJoint(divingBoardBase, log, log.Position + new Vector3(2.5f, 0, 0), Vector3.Right);
            RevoluteJoint logJointB = new RevoluteJoint(endPlatform, log, log.Position + new Vector3(-2.5f, 0, 0), Vector3.Right);
            Space.Add(logJointA);
            Space.Add(logJointB);

            Space.Add(log);


            //Put some planks to stand on that show various slopes.
            int numPads = 10;
            for (int i = 0; i < numPads; i++)
            {
                offset = new Vector3(0, 0, 4);
                Box a = new Box(new Vector3(i * 1.5f + 3.5f, 10, 24), 1.5f, 1, 4);
                Box b = new Box(new Vector3(i * 1.5f + 3.5f, 10, 24), 1.5f, 1, 4);
                float angle = -i * MathHelper.PiOver2 / numPads;
                b.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Right, angle);
                b.Position += offset * .5f + Quaternion.Transform(offset * .5f, b.Orientation);

                Space.Add(a);
                Space.Add(b);
            }

        }

        EntityMover elevatorMover;
        Path<Vector3> elevatorPath;
        EntityMover platformMover;
        EntityRotator platformRotator;
        Path<Vector3> platformPath;
        Path<Quaternion> platformOrientationPath;
        double pathTime;


        public override void Update(float dt)
        {
            //Increment the time.  Note that the space's timestep is used
            //instead of the method's dt.  This is because the demos, by
            //default, update the space once each game update.  Using the
            //space's update time keeps things synchronized.
            //If the engine is using internal time stepping,
            //the passed in dt should be used instead (or put this logic into
            //an updateable that runs with space updates).
            pathTime += Space.TimeStepSettings.TimeStepDuration;
            elevatorMover.TargetPosition = elevatorPath.Evaluate(pathTime);
            platformMover.TargetPosition = platformPath.Evaluate(pathTime);
            platformRotator.TargetOrientation = platformOrientationPath.Evaluate(pathTime);
            base.Update(dt);
        }

        public override void DrawUI()
        {
#if XBOX360
            Game.DataTextDrawer.Draw("Press \"A\" to toggle the character.", new Microsoft.Xna.Framework.(50, 50));
#else
            Game.DataTextDrawer.Draw("Press \"C\" to toggle the character.", new Microsoft.Xna.Framework.Vector2(50, 50));
#endif
            base.DrawUI();
        }

        /// <summary>
        /// Gets the name of the simulation.
        /// </summary>
        public override string Name
        {
            get { return "Character Playground"; }
        }
    }
}