public static Kart GetKartFromBody(RigidBody enteredBody)
 {
     if (enteredBody.UserObject is CollisionObjectDataHolder) {
         return (enteredBody.UserObject as CollisionObjectDataHolder).GetThingAsKart();
     }
     return null;
 }

		public unsafe static float SolveAngularLimits(this RotationalLimitMotor obj, float timeStep, ref OpenTK.Vector3 axis, float jacDiagABInv, RigidBody body0, RigidBody body1)
		{
			fixed (OpenTK.Vector3* axisPtr = &axis)
			{
				return obj.SolveAngularLimits(timeStep, ref *(BulletSharp.Math.Vector3*)axisPtr, jacDiagABInv, body0, body1);
			}
		}

        protected override void OnInitializePhysics()
        {
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher = new CollisionDispatcher(CollisionConf);

            Broadphase = new DbvtBroadphase();

            World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
            World.Gravity = new Vector3(0, -10, 0);

            // ground
            CollisionShape groundShape = new BoxShape(50, 1, 50);
            CollisionShapes.Add(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);
            ground.UserObject = "Ground";

            // Objects
            //colShape = new BoxShape(1);
            Vector3[] points0 = {
                new Vector3(1, 0, 0), new Vector3(0, 1, 0), new Vector3(0, 0, 1)
            };
            Vector3[] points1 = {
                new Vector3(1, 0, 0), new Vector3(0, 1, 0), new Vector3(0, 0, 1), new Vector3(0,0,-1), new Vector3(-1,-1,0)
            };
            colShape0 = new ConvexHullShape(points0);
            colShape1 = new ConvexHullShape(points1);
            CollisionShapes.Add(colShape0);
            CollisionShapes.Add(colShape1);

            body2 = LocalCreateRigidBody(0, body2Position, colShape1);

            rotBody = LocalCreateRigidBody(0, rotBodyPosition, colShape0);
            rotBody.CollisionFlags |= CollisionFlags.KinematicObject;
            rotBody.ActivationState = ActivationState.DisableDeactivation;
        }

        /// <summary>
        /// Only applies friction and bounciness. Use a RigidBodyConstructionInfo if you want to set the damping.
        /// </summary>
        public void ApplyMaterial(RigidBody body, string material)
        {
            PhysicsMaterial mat = GetMaterial(material);

            body.Friction = mat.Friction;
            body.Restitution = mat.Bounciness;
        }

 public PhysicalBody(RigidBody rigidBody, CollisionShape shape, TransformationManager manager)
 {
     Body = rigidBody;
     Shape = shape;
     Transformation = manager;
     Enabled = false;
 }

		public HingeConstraint(RigidBody rigidBodyA, Matrix rigidBodyAFrame, bool useReferenceFrameA = false)
			: base(btHingeConstraint_new8(rigidBodyA._native, ref rigidBodyAFrame,
				useReferenceFrameA))
		{
			_rigidBodyA = rigidBodyA;
            _rigidBodyB = GetFixedBody();
		}

        public Physics()
        {
            // collision configuration contains default setup for memory, collision setup
            collisionConf = new DefaultCollisionConfiguration();
            Dispatcher = new CollisionDispatcher(collisionConf);

            Broadphase = new DbvtBroadphase();

            World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, collisionConf);
            World.Gravity = new Vector3(0, -10, 0);

            CollisionShapes = new List<CollisionShape>();

            // create the ground
            CollisionShape groundShape = new BoxShape(50, 1, 50);
            CollisionShapes.Add(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);
            ground.UserObject = "Ground";

            // create a few dynamic rigidbodies
            float mass = 1.0f;

            CollisionShape colShape = new BoxShape(1);
            CollisionShapes.Add(colShape);
            Vector3 localInertia = colShape.CalculateLocalInertia(mass);

            float start_x = StartPosX - ArraySizeX / 2;
            float start_y = StartPosY;
            float start_z = StartPosZ - ArraySizeZ / 2;

            int k, i, j;
            for (k = 0; k < ArraySizeY; k++)
            {
                for (i = 0; i < ArraySizeX; i++)
                {
                    for (j = 0; j < ArraySizeZ; j++)
                    {
                        Matrix startTransform = Matrix.CreateTranslation(
                            new Vector3(
                                2*i + start_x,
                                2*k + start_y,
                                2*j + start_z
                                )
                            );

                        // using motionstate is recommended, it provides interpolation capabilities
                        // and only synchronizes 'active' objects
                        DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
                        RigidBodyConstructionInfo rbInfo =
                            new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia);
                        RigidBody body = new RigidBody(rbInfo);

                        // make it drop from a height
                        body.Translate(new Vector3(0, 20, 0));

                        World.AddRigidBody(body);
                    }
                }
            }
        }

        public void UpdateWheel(RigidBody chassis, RaycastInfo raycastInfo)
        {
            if (raycastInfo.IsInContact)
            {
                float project = Vector3.Dot(raycastInfo.ContactNormalWS, raycastInfo.WheelDirectionWS);
                Vector3 chassis_velocity_at_contactPoint;
                Vector3 relpos = raycastInfo.ContactPointWS - chassis.CenterOfMassPosition;
                chassis_velocity_at_contactPoint = chassis.GetVelocityInLocalPoint(relpos);
                float projVel = Vector3.Dot(raycastInfo.ContactNormalWS, chassis_velocity_at_contactPoint);
                if (project >= -0.1f)
                {
                    SuspensionRelativeVelocity = 0;
                    ClippedInvContactDotSuspension = 1.0f / 0.1f;
                }
                else
                {
                    float inv = -1.0f / project;
                    SuspensionRelativeVelocity = projVel * inv;
                    ClippedInvContactDotSuspension = inv;
                }

            }

            else    // Not in contact : position wheel in a nice (rest length) position
            {
                RaycastInfo.SuspensionLength = SuspensionRestLength;
                SuspensionRelativeVelocity = 0;
                RaycastInfo.ContactNormalWS = -raycastInfo.WheelDirectionWS;
                ClippedInvContactDotSuspension = 1.0f;
            }
        }

		public UniversalConstraint(RigidBody rigidBodyA, RigidBody rigidBodyB, Vector3 anchor,
			Vector3 axis1, Vector3 axis2)
			: base(btUniversalConstraint_new(rigidBodyA._native, rigidBodyB._native,
				ref anchor, ref axis1, ref axis2))
		{
			_rigidBodyA = rigidBodyA;
			_rigidBodyB = rigidBodyB;
		}

		public Generic6DofSpringConstraint(RigidBody rigidBodyA, RigidBody rigidBodyB,
			Matrix frameInA, Matrix frameInB, bool useLinearReferenceFrameA)
			: base(btGeneric6DofSpringConstraint_new(rigidBodyA._native, rigidBodyB._native,
				ref frameInA, ref frameInB, useLinearReferenceFrameA))
		{
			_rigidBodyA = rigidBodyA;
			_rigidBodyB = rigidBodyB;
		}

		public Point2PointConstraint(RigidBody rigidBodyA, RigidBody rigidBodyB,
			Vector3 pivotInA, Vector3 pivotInB)
			: base(btPoint2PointConstraint_new(rigidBodyA._native, rigidBodyB._native,
				ref pivotInA, ref pivotInB))
		{
			_rigidBodyA = rigidBodyA;
			_rigidBodyB = rigidBodyB;
		}

		public FixedConstraint(RigidBody rigidBodyA, RigidBody rigidBodyB, Matrix frameInA,
			Matrix frameInB)
			: base(btFixedConstraint_new(rigidBodyA._native, rigidBodyB._native,
				ref frameInA, ref frameInB))
		{
			_rigidBodyA = rigidBodyA;
			_rigidBodyB = rigidBodyB;
		}

		public GearConstraint(RigidBody rigidBodyA, RigidBody rigidBodyB, Vector3 axisInA,
			Vector3 axisInB)
			: base(btGearConstraint_new(rigidBodyA._native, rigidBodyB._native, ref axisInA,
				ref axisInB))
		{
			_rigidBodyA = rigidBodyA;
			_rigidBodyB = rigidBodyB;
		}

        protected override void Apply(RigidBody obj, int slice)
        {
            Vector3D pos = this.FPosition[slice];
            Vector3D force = this.FImpulse[slice];

            obj.ApplyImpulse(new Vector3((float)force.x, (float)force.y, (float)force.z),
                new Vector3((float)pos.x, (float)pos.y, (float)pos.z));
        }

		public ConeTwistConstraint(RigidBody rigidBodyA, RigidBody rigidBodyB, Matrix rigidBodyAFrame,
			Matrix rigidBodyBFrame)
			: base(btConeTwistConstraint_new(rigidBodyA._native, rigidBodyB._native,
				ref rigidBodyAFrame, ref rigidBodyBFrame))
		{
			_rigidBodyA = rigidBodyA;
			_rigidBodyB = rigidBodyB;
		}

        public void SetUpBulletPhysicsBody(float mass, BulletSharp.MotionState motionState, BulletSharp.CollisionShape collisionShape, Vector3 localInertia)
        {
            BulletSharpPhysics.RigidBodyConstructionInfo rbInfo =
                            new BulletSharpPhysics.RigidBodyConstructionInfo(mass, motionState, collisionShape, localInertia);
            RigidBody = new BulletSharpPhysics.RigidBody(rbInfo);

            bulletPhysics.World.AddRigidBody(RigidBody,GetCollisionFlags(),GetCollisionMask());
           
        }

        //public void Test()
        //{
        //    BulletSharp.Point2PointConstraint pt;
        //    /BulletSharp.Generic6DofConstraint sdof = new Generic6DofConstraint(
        //    pt.Setting.
        //sdof.sett
        //}
        public override TypedConstraint GetConstraint(RigidBody body)
        {
            Point2PointConstraint cst = new Point2PointConstraint(body, this.Pivot);
            cst.Setting.Damping = this.Damping;
            cst.Setting.ImpulseClamp = this.ImpulseClamp;
            cst.Setting.Tau = this.Tau;

            return cst;
        }

 /// <summary>
 /// output some text, show/hide a dialogue, and change the color of the region
 /// </summary>
 void doSomething(TriggerRegion region, RigidBody otherBody, TriggerReportFlags flags, CollisionReportInfo info)
 {
     if (flags.HasFlag(TriggerReportFlags.Enter)) {
         Console.WriteLine(otherBody.GetName() + " has entered trigger area \"" + region.Name + "\"");
         // cycle through the balloon colors
         region.CycleToNextColor();
     }
     else {
         Console.WriteLine(otherBody.GetName() + " has left trigger area \"" + region.Name + "\"");
         region.CycleToNextColor();
     }
 }

        public Prop(CollisionShape shape, float mass, Matrix4 start)
        {
            Physics physics = Game.Instance.Physics;

            Shape = shape;
            IsDynamic = (mass != 0.0f);
            physics.Props.Add(this);
            Vector3 inertia = Vector3.Zero;
            if (IsDynamic) shape.CalculateLocalInertia(mass, out inertia);
            DefaultMotionState motionState = new DefaultMotionState(start);
            RigidBody.RigidBodyConstructionInfo info = new RigidBody.RigidBodyConstructionInfo(mass, motionState, shape, inertia);
            Body = new RigidBody(info);
            physics.World.AddRigidBody(Body);
        }

        void TestContactTest(RigidBody testBody, RigidBody testBody2)
        {
            object context = "your context";
            ContactSensorCallback contactCallback = new ContactSensorCallback(testBody, context);
            world.ContactTest(testBody, contactCallback);

            testBody.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
            testBody2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
            world.ContactPairTest(testBody, testBody2, contactCallback);
            testBody.CollisionFlags &= ~CollisionFlags.CustomMaterialCallback;
            testBody2.CollisionFlags &= ~CollisionFlags.CustomMaterialCallback;

            AddToDisposeQueue(contactCallback);
        }