/// <summary>
        /// Initializes a new instance of the DistanceConstraint class.
        /// </summary>
        /// <param name="body1">The first body.</param>
        /// <param name="body2">The second body.</param>
        /// <param name="anchor1">The anchor point of the first body in world space. 
        /// The distance is given by the initial distance between both anchor points.</param>
        /// <param name="anchor2">The anchor point of the second body in world space.
        /// The distance is given by the initial distance between both anchor points.</param>
        public PointOnPoint(RigidBody body, JVector localAnchor)
            : base(body, null)
        {
            localAnchor1 = localAnchor;

            this.anchor = body.position + JVector.Transform(localAnchor, body.orientation);
        }

        /// <summary>
        /// SupportMapping. Finds the point in the shape furthest away from the given direction.
        /// Imagine a plane with a normal in the search direction. Now move the plane along the normal
        /// until the plane does not intersect the shape. The last intersection point is the result.
        /// </summary>
        /// <param name="direction">The direction.</param>
        /// <param name="result">The result.</param>
        public override void SupportMapping(ref JVector direction, out JVector result)
        {
            result = direction;
            result.Normalize();

            JVector.Multiply(ref result, radius, out result);
        }

 public TraceResult(RigidBody body, float distance, JVector position, JVector normal)
 {
     this.Body = body;
     this.Distance = distance;
     this.Position = position;
     this.Normal = normal;
 }

        public void DrawAabb(JVector from, JVector to, Color color)
        {
            JVector halfExtents = (to - from) * 0.5f;
            JVector center = (to + from) * 0.5f;

            JVector edgecoord = new JVector(1f, 1f, 1f), pa, pb;
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    pa = new JVector(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,
                        edgecoord.Z * halfExtents.Z);
                    pa += center;

                    int othercoord = j % 3;
                    SetElement(ref edgecoord, othercoord, GetElement(edgecoord, othercoord) * -1f);
                    pb = new JVector(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,
                        edgecoord.Z * halfExtents.Z);
                    pb += center;

                    DrawLine(pa, pb, color);
                }
                edgecoord = new JVector(-1f, -1f, -1f);
                if (i < 3)
                    SetElement(ref edgecoord, i, GetElement(edgecoord, i) * -1f);
            }
        }

        public override void Iterate()
        {
            deltaVelocity = TargetVelocity - Body1.LinearVelocity;
            deltaVelocity.Y = 0.0f;

            // determine how 'stiff' the character follows the target velocity
            deltaVelocity *= 0.005f;

            if (deltaVelocity.LengthSquared() != 0.0f)
            {
                // activate it, in case it fall asleep :)
                Body1.IsActive = true;
                Body1.ApplyImpulse(deltaVelocity * Body1.Mass);
            }

            if (shouldIJump)
            {
                Body1.IsActive = true;
                Body1.ApplyImpulse(JumpVelocity * JVector.Up * Body1.Mass);
                System.Diagnostics.Debug.WriteLine("JUMP! " + DateTime.Now.Second.ToString());

                if (!BodyWalkingOn.IsStatic)
                {
                    BodyWalkingOn.IsActive = true;
                    // apply the negative impulse to the other body
                    BodyWalkingOn.ApplyImpulse(-1.0f * JumpVelocity * JVector.Up * Body1.Mass);
                }

            }
        }

        public override void Iterate()
        {
            deltaVelocity = TargetVelocity - Body1.LinearVelocity;
            deltaVelocity.Y = 0.0f;

            // determine how 'stiff' the character follows the target velocity
            deltaVelocity *= 0.02f;

            if (deltaVelocity.LengthSquared() != 0.0f)
            {
                // activate it, in case it fall asleep :)
                Body1.IsActive = true;
                Body1.ApplyImpulse(deltaVelocity * Body1.Mass);
            }

            if (shouldIJump)
            {
                Body1.IsActive = true;
                Body1.ApplyImpulse(jumpVelocity * JVector.Up * Body1.Mass);

                if (!BodyWalkingOn.IsStatic)
                {
                    BodyWalkingOn.IsActive = true;
                    // apply the negative impulse to the other body
                    BodyWalkingOn.ApplyImpulse(-1.0f * jumpVelocity * JVector.Up * Body1.Mass);
                }
            }
        }

        public bool CanJump(RigidBody body)
        {
            Body = WorldItems[body];

            Jitter.Dynamics.RigidBody resultingBody = null;
            JVector normal;
            float fraction;

            var positions = new JVector[] { new JVector(body.Position.X, body.Position.Y, body.Position.Z),
                                            new JVector(body.Position.X + 0.5f, body.Position.Y, body.Position.Z),
                                            new JVector(body.Position.X - 0.5f, body.Position.Y, body.Position.Z),
                                            new JVector(body.Position.X, body.Position.Y, body.Position.Z - 0.5f),
                                            new JVector(body.Position.X, body.Position.Y, body.Position.Z + 0.5f)};

            for (int i = 0; i < positions.Length; i++)
            {
                bool result = World.CollisionSystem.Raycast(new JVector(positions[i].X, positions[i].Y, positions[i].Z),
                                                            new JVector(0, -1, 0),
                                                            RaycastCallback,
                                                            out resultingBody,
                                                            out normal,
                                                            out fraction);

                if (result && fraction <= 1.3f && Body.LinearVelocity.Y < 0.5f)
                {
                    return true;
                }
            }

            return false;
        }

        public void AddCar(JVector position)
        {
            car = new CarObject(Demo);
            this.Demo.Components.Add(car);

            car.carBody.Position = position;
        }

        public override void Iterate()
        {
            DeltaVelocity = TargetVelocity - Body1.LinearVelocity;
            DeltaVelocity.Y = 0.0f;

            var fraction = 0.02f;
            if (WalkingOn == null)
                fraction = 0.0001f;
            DeltaVelocity *= fraction;

            if (DeltaVelocity.LengthSquared() != 0.0f)
            {
                Body1.IsActive = true;
                Body1.ApplyImpulse(DeltaVelocity * Body1.Mass);
            }

            if (ShouldJump)
            {
                Body1.IsActive = true;
                Body1.ApplyImpulse(JumpVelocity * JVector.Up * Body1.Mass);

                if (!WalkingOn.IsStatic)
                {
                    WalkingOn.IsActive = true;
                    WalkingOn.ApplyImpulse(-1.0f * JumpVelocity * JVector.Up * Body1.Mass);
                }
            }
        }

 protected override void Update()
 {
     base.Update();
     if (form.MouseClickedHappenend)
     {
         form.MouseClickedHappenend = false;
         var screenPosition = new JVector(
             -1f + 2* (form.MouseClickPosition.x / form.ClientSize.Width),
             1, -1f + 2 * ((form.MouseClickPosition.y / form.ClientSize.Height)));
         var projectionMatrix = Common.ProjectionMatrix;
         JMatrix jMatrix = new JMatrix(
             projectionMatrix.M11, projectionMatrix.M12, projectionMatrix.M13,
             projectionMatrix.M21, projectionMatrix.M22, projectionMatrix.M23,
             projectionMatrix.M31, projectionMatrix.M32, projectionMatrix.M33);
         JMatrix invertedJMatrix;
     JMatrix.Invert(ref jMatrix, out invertedJMatrix);
         var rayDirection = JVector.Transform(screenPosition, invertedJMatrix);
         RigidBody body;
         JVector normal;
         float fraction;
         Entities.world3D.CollisionSystem.Raycast(JitterDatatypes.ToJVector(Common.CameraPosition),
             rayDirection, null, out body, out normal, out fraction);
         if (body != null && !body.IsStatic)
             body.ApplyImpulse(rayDirection * 200);
     }
 }

        public override void Build()
        {
            AddGround();

            for (int i = 0; i < 11; i++)
            {
                RigidBody box = new RigidBody(new BoxShape(1,0.01f,1));
                this.Demo.World.AddBody(box);
                JVector boxPos = new JVector(-15 + i * 3 + 1, 5, 0);

                box.Position = boxPos;
                box.IsStatic = true;

                RigidBody sphere = new RigidBody(new SphereShape(0.5f));
                this.Demo.World.AddBody(sphere);

                sphere.Position = boxPos + JVector.Up * 30;
                sphere.EnableSpeculativeContacts = true;

                // set restitution
                sphere.Material.Restitution = box.Material.Restitution = 1.0f / 10.0f * i;
                sphere.LinearVelocity = new JVector(0, 0, 0);
    

                sphere.Damping = RigidBody.DampingType.Angular;
            }

         
        }

 /// <summary>
 /// Creates a new instance of the FluidVolume class.
 /// </summary>
 /// <param name="world">The world.</param>
 public Buoyancy(World world)
     : base(world)
 {
     Density = 2.0f;
     Damping = 0.1f;
     Flow = JVector.Zero;
 }

 /// <summary>
 /// Creates a new instance of the TransformedShape struct.
 /// </summary>
 /// <param name="shape">The shape.</param>
 /// <param name="orientation">The orientation this shape should have.</param>
 /// <param name="position">The position this shape should have.</param>
 public TransformedShape(Shape shape, JMatrix orientation, JVector position)
 {
     this.position = position;
     this.orientation = orientation;
     JMatrix.Transpose(ref orientation, out invOrientation);
     this.shape = shape;
 }

        /// <summary>
        /// This method handles explicit object collision logic. Is registered to physics engine CollisionDetected event handler.
        /// Fired on any detected collision, so must check if the collision applies to this object
        /// </summary>
        /// <param name="body1"></param>
        /// <param name="body2"></param>
        /// <param name="point1"></param>
        /// <param name="point2"></param>
        /// <param name="normal"></param>
        /// <param name="penetration"></param>
        virtual public void HandleCollision(RigidBody body1, RigidBody body2, JVector point1, JVector point2, JVector normal, float penetration)
        {
            // work out which, if any, of the collided bodies is this object, and name them semantically
            RigidBody other;
            var self = this.PhysicsDescription;
            if (body1 == self)
                other = body2;
            else if (body2 == self)
                other = body1;
            else return;

            if (other == this.flock.level.endGoal.PhysicsDescription) // we've collided with the end zone
            {
                // be careful of what you modify in this handler as it may be called during an Update()
                // attempting to modify any list (such as destroying game objects, etc) will cause an exception

                if (!ToDestroy) // incremement score once before destroy
                {
                    this.game.incScore(10);
                }
                this.Destroy(true); // remove self
            }    

            Collision(other); // do other collision stuff
        }

 public static Matrix4 ToMatrix4(JMatrix orientation, JVector position)
 {
     return new Matrix4(
         orientation.M11, orientation.M12, orientation.M13, 0,
         orientation.M21, orientation.M22, orientation.M23, 0,
         orientation.M31, orientation.M32, orientation.M33, 0,
         position.X, position.Y, position.Z, 1);
 }

 public static Matrix4 ToMatrix4(JMatrix m, JVector position)
 {
     return new Matrix4(
         m.M11, m.M12, m.M13, 0,
         m.M21, m.M22, m.M23, 0,
         m.M31, m.M32, m.M33, 0,
         position.X, position.Y, position.Z, 1);
 }

 public void DrawPoint(JVector pos)
 {
     GL.End();
     GL.Begin(BeginMode.Points);
     GL.Vertex3(pos.X, pos.Y, pos.Z);
     GL.End();
     GL.Begin(BeginMode.Lines);
 }

        public void HandleInput(GameTime gameTime)
        {
            currentTime += (float)gameTime.ElapsedGameTime.Milliseconds;

            KeyboardState keys = Keyboard.GetState();

            if (keys.IsKeyDown(Keys.Up))
            {
                float x = (float)Math.Sin(facing);
                float z = (float)Math.Cos(facing);

                JVector newPath = new JVector(x, 0f, z) * speed;
                newPath = newPath * currentTime;
                newPath.Normalize();

                forward = newPath;

                body.AddForce(newPath*100f);
                //position = body.Position;
                //position += newPath;

                //body.Position = position;
            }

            if (keys.IsKeyDown(Keys.Left))
            {
                facing -= 0.05f;
            }

            if (keys.IsKeyDown(Keys.Right))
            {
                facing += 0.05f;
            }

            if (keys.IsKeyDown(Keys.Down))
            {
                float x = (float)Math.Sin(facing);
                float z = (float)Math.Cos(facing);

                JVector newPath = new JVector(x, 0f, z) * speed;
                newPath = newPath * currentTime;
                newPath.Normalize();

                body.AddForce(newPath * -100f);
            }

            if (keys.IsKeyDown(Keys.Space))
            {
                body.AddForce(JVector.Up * 100f);
            }

            /*if (keys.IsKeyDown(Keys.A))

            if (keys.IsKeyDown(Keys.S))

            if (keys.IsKeyDown(Keys.W))
            */
        }

        /// <summary>
        /// Initializes a new instance of the HingeJoint class.
        /// </summary>
        /// <param name="world">The world class where the constraints get added to.</param>
        /// <param name="body1">The first body connected to the second one.</param>
        /// <param name="body2">The second body connected to the first one.</param>
        /// <param name="position">The position in world space where both bodies get connected.</param>
        /// <param name="hingeAxis">The axis if the hinge.</param>
        public LimitedHingeJoint(World world, RigidBody body1, RigidBody body2, JVector position, JVector hingeAxis,
            float hingeFwdAngle, float hingeBckAngle)
            : base(world)
        {
            // Create the hinge first, two point constraints

            worldPointConstraint = new PointOnPoint[2];

            hingeAxis *= 0.5f;

            JVector pos1 = position; JVector.Add(ref pos1, ref hingeAxis, out pos1);
            JVector pos2 = position; JVector.Subtract(ref pos2, ref hingeAxis, out pos2);

            worldPointConstraint[0] = new PointOnPoint(body1, body2, pos1);
            worldPointConstraint[1] = new PointOnPoint(body1, body2, pos2);


            // Now the limit, one max distance constraint

            hingeAxis.Normalize();

            // choose a direction that is perpendicular to the hinge
            JVector perpDir = JVector.Up;

            if (JVector.Dot(perpDir, hingeAxis) > 0.1f) perpDir = JVector.Right;

            // now make it perpendicular to the hinge
            JVector sideAxis = JVector.Cross(hingeAxis, perpDir);
            perpDir = JVector.Cross(sideAxis, hingeAxis);
            perpDir.Normalize();

            // the length of the "arm" TODO take this as a parameter? what's
            // the effect of changing it?
            float len = 10.0f * 3;

            // Choose a position using that dir. this will be the anchor point
            // for body 0. relative to hinge
            JVector hingeRelAnchorPos0 = perpDir * len;


            // anchor point for body 2 is chosen to be in the middle of the
            // angle range.  relative to hinge
            float angleToMiddle = 0.5f * (hingeFwdAngle - hingeBckAngle);
            JVector hingeRelAnchorPos1 = JVector.Transform(hingeRelAnchorPos0, JMatrix.CreateFromAxisAngle(hingeAxis, -angleToMiddle / 360.0f * 2.0f * JMath.Pi));

            // work out the "string" length
            float hingeHalfAngle = 0.5f * (hingeFwdAngle + hingeBckAngle);
            float allowedDistance = len * 2.0f * (float)System.Math.Sin(hingeHalfAngle * 0.5f / 360.0f * 2.0f * JMath.Pi);

            JVector hingePos = body1.Position;
            JVector relPos0c = hingePos + hingeRelAnchorPos0;
            JVector relPos1c = hingePos + hingeRelAnchorPos1;

            distance = new PointPointDistance(body1, body2, relPos0c, relPos1c);
            distance.Distance = allowedDistance;
            distance.Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance;

        }

        /// <summary>
        /// Initializes a new instance of the DistanceConstraint class.
        /// </summary>
        /// <param name="body1">The first body.</param>
        /// <param name="body2">The second body.</param>
        /// <param name="anchor1">The anchor point of the first body in world space. 
        /// The distance is given by the initial distance between both anchor points.</param>
        /// <param name="anchor2">The anchor point of the second body in world space.
        /// The distance is given by the initial distance between both anchor points.</param>
        public PointOnPoint(RigidBody body1, RigidBody body2, JVector anchor)
            : base(body1, body2)
        {
            JVector.Subtract(ref anchor, ref body1.position, out localAnchor1);
            JVector.Subtract(ref anchor, ref body2.position, out localAnchor2);

            JVector.Transform(ref localAnchor1, ref body1.invOrientation, out localAnchor1);
            JVector.Transform(ref localAnchor2, ref body2.invOrientation, out localAnchor2);
        }