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);
     }
 }

 /// <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>
        /// Constraints two bodies to always have the same relative
        /// orientation to each other. Combine the AngleConstraint with a PointOnLine
        /// Constraint to get a prismatic joint.
        /// </summary>
        public FixedAngle(RigidBody body1, RigidBody body2) : base(body1, body2)
        {
            initialOrientation1 = body1.orientation;
            initialOrientation2 = body2.orientation;

            //orientationDifference = body1.orientation * body2.invOrientation;
            //orientationDifference = JMatrix.Transpose(orientationDifference);
        }

 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 static Matrix4 ToTritonMatrix(JMatrix matrix)
 {
     return new Matrix4(
         matrix.M11, matrix.M12, matrix.M13, 0.0f,
         matrix.M21, matrix.M22, matrix.M23, 0.0f,
         matrix.M31, matrix.M32, matrix.M33, 0.0f,
         0.0f, 0.0f, 0.0f, 1.0f);
 }

 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);
 }

 /// <summary>
 /// Calculates the bounding box of the sphere.
 /// </summary>
 /// <param name="orientation">The orientation of the shape.</param>
 /// <param name="box">The resulting axis aligned bounding box.</param>
 public override void GetBoundingBox(ref JMatrix orientation, out JBBox box)
 {
     box.Min.X = -radius;
     box.Min.Y = -radius;
     box.Min.Z = -radius;
     box.Max.X = radius;
     box.Max.Y = radius;
     box.Max.Z = radius;
 }

        public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
            ref JMatrix orientation2, ref JVector position1, ref JVector position2,
            out JVector p1, out JVector p2, out JVector normal)
        {
            VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
            simplexSolver.Reset();

            p1 = p2 = JVector.Zero;

            JVector r = position1 - position2;
            JVector w, v;

            JVector supVertexA;
            JVector rn,vn;

            rn = JVector.Negate(r);

            SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA);

            JVector supVertexB;
            SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB);

            v = supVertexA - supVertexB;

            normal = JVector.Zero;

            int maxIter = 15;

            float distSq = v.LengthSquared();
            float epsilon = 0.00001f;

            while ((distSq > epsilon) && (maxIter-- != 0))
            {
                vn = JVector.Negate(v);
                SupportMapTransformed(support1, ref orientation1, ref position1, ref vn, out supVertexA);
                SupportMapTransformed(support2, ref orientation2, ref position2, ref v, out supVertexB);
                w = supVertexA - supVertexB;

                if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, supVertexA, supVertexB);
                if (simplexSolver.Closest(out v))
                {
                    distSq = v.LengthSquared();
                    normal = v;
                }
                else distSq = 0.0f;
            }

            simplexSolver.ComputePoints(out p1, out p2);

            if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon)
                normal.Normalize();

            simplexSolverPool.GiveBack(simplexSolver);

            return true;
        }

        static JMatrix()
        {
            Zero = new JMatrix();

            Identity = new JMatrix();
            Identity.M11 = 1.0f;
            Identity.M22 = 1.0f;
            Identity.M33 = 1.0f;

            InternalIdentity = Identity;
        }

 public static void Absolute(ref JMatrix matrix,out JMatrix result)
 {
     result.M11 = Math.Abs(matrix.M11);
     result.M12 = Math.Abs(matrix.M12);
     result.M13 = Math.Abs(matrix.M13);
     result.M21 = Math.Abs(matrix.M21);
     result.M22 = Math.Abs(matrix.M22);
     result.M23 = Math.Abs(matrix.M23);
     result.M31 = Math.Abs(matrix.M31);
     result.M32 = Math.Abs(matrix.M32);
     result.M33 = Math.Abs(matrix.M33);
 }

		public static void Convert(JMatrix matrix, ref Matrix result)
		{
			result[0] = matrix.M11;
			result[1] = matrix.M12;
			result[2] = matrix.M13;
			result[4] = matrix.M21;
			result[5] = matrix.M22;
			result[6] = matrix.M23;
			result[8] = matrix.M31;
			result[9] = matrix.M32;
			result[10] = matrix.M33;
		}

        /// <summary>
        /// Checks if given point is within a shape.
        /// </summary>
        /// <param name="support">The supportmap implementation representing the shape.</param>
        /// <param name="orientation">The orientation of the shape.</param>
        /// <param name="invOrientation">The inverse orientation of the shape.</param>
        /// <param name="position">The position of the shape.</param>
        /// <param name="point">The point to check.</param>
        /// <returns>Returns true if the point is within the shape, otherwise false.</returns>
        public static bool Pointcast(ISupportMappable support, ref JMatrix orientation,ref JVector position,ref JVector point)
        {
            JVector arbitraryPoint; 

            SupportMapTransformed(support, ref orientation, ref position, ref point, out arbitraryPoint);
            JVector.Subtract(ref point, ref arbitraryPoint, out arbitraryPoint);

            JVector r; support.SupportCenter(out r);
            JVector.Transform(ref r, ref orientation, out r);
            JVector.Add(ref position, ref r, out r);
            JVector.Subtract(ref point, ref r, out r);

            JVector x = point;
            JVector w, p;
            float VdotR;

            JVector v; JVector.Subtract(ref x, ref arbitraryPoint, out v);
            float dist = v.LengthSquared();
            float epsilon = 0.0001f;

            int maxIter = MaxIterations;

            VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();

            simplexSolver.Reset();

            while ((dist > epsilon) && (maxIter-- != 0))
            {
                SupportMapTransformed(support, ref orientation, ref position, ref v, out p);
                JVector.Subtract(ref x, ref p, out w);

                float VdotW = JVector.Dot(ref v, ref w);

                if (VdotW > 0.0f)
                {
                    VdotR = JVector.Dot(ref v, ref r);

                    if (VdotR >= -(JMath.Epsilon * JMath.Epsilon)) { simplexSolverPool.GiveBack(simplexSolver); return false; }
                    else simplexSolver.Reset();
                }
                if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, x, p);

                if (simplexSolver.Closest(out v)) dist = v.LengthSquared();
                else dist = 0.0f;
            }

            simplexSolverPool.GiveBack(simplexSolver);
            return true;

        }

        /// <summary>
        /// Gets the axis aligned bounding box of the orientated shape. This includes
        /// the whole shape.
        /// </summary>
        /// <param name="orientation">The orientation of the shape.</param>
        /// <param name="box">The axis aligned bounding box of the shape.</param>
        public override void GetBoundingBox(ref JMatrix orientation, out JBBox box)
        {
            JBBox helpBox = JBBox.LargeBox;
            int length = this.Prepare(ref helpBox);

            box = JBBox.SmallBox;

            for (int i = 0; i < length; i++)
            {
                this.SetCurrentShape(i);
                base.GetBoundingBox(ref orientation, out helpBox);
                JBBox.CreateMerged(ref box, ref helpBox, out box);
            }
        }

 public static Matrix ToXNAMatrix(JMatrix matrix)
 {
     return new Matrix(matrix.M11,
                     matrix.M12,
                     matrix.M13,
                     0.0f,
                     matrix.M21,
                     matrix.M22,
                     matrix.M23,
                     0.0f,
                     matrix.M31,
                     matrix.M32,
                     matrix.M33,
                     0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
 }

        private static void SupportMapTransformed(ISupportMappable support,
            ref JMatrix orientation, ref JVector position, ref JVector direction, out JVector result)
        {
            // THIS IS *THE* HIGH FREQUENCY CODE OF THE COLLLISION PART OF THE ENGINE

            result.X = ((direction.X * orientation.M11) + (direction.Y * orientation.M12)) + (direction.Z * orientation.M13);
            result.Y = ((direction.X * orientation.M21) + (direction.Y * orientation.M22)) + (direction.Z * orientation.M23);
            result.Z = ((direction.X * orientation.M31) + (direction.Y * orientation.M32)) + (direction.Z * orientation.M33);

            support.SupportMapping(ref result, out result);

            float x = ((result.X * orientation.M11) + (result.Y * orientation.M21)) + (result.Z * orientation.M31);
            float y = ((result.X * orientation.M12) + (result.Y * orientation.M22)) + (result.Z * orientation.M32);
            float z = ((result.X * orientation.M13) + (result.Y * orientation.M23)) + (result.Z * orientation.M33);

            result.X = position.X + x;
            result.Y = position.Y + y;
            result.Z = position.Z + z;
        }

        private static void SupportMapTransformed(ISupportMappable support, ref JMatrix orientation, ref JVector position, ref JVector direction, out JVector result)
        {
            //JVector.Transform(ref direction, ref invOrientation, out result);
            //support.SupportMapping(ref result, out result);
            //JVector.Transform(ref result, ref orientation, out result);
            //JVector.Add(ref result, ref position, out result);

            result.X = ((direction.X * orientation.M11) + (direction.Y * orientation.M12)) + (direction.Z * orientation.M13);
            result.Y = ((direction.X * orientation.M21) + (direction.Y * orientation.M22)) + (direction.Z * orientation.M23);
            result.Z = ((direction.X * orientation.M31) + (direction.Y * orientation.M32)) + (direction.Z * orientation.M33);

            support.SupportMapping(ref result, out result);

            float x = ((result.X * orientation.M11) + (result.Y * orientation.M21)) + (result.Z * orientation.M31);
            float y = ((result.X * orientation.M12) + (result.Y * orientation.M22)) + (result.Z * orientation.M32);
            float z = ((result.X * orientation.M13) + (result.Y * orientation.M23)) + (result.Z * orientation.M33);

            result.X = position.X + x;
            result.Y = position.Y + y;
            result.Z = position.Z + z;
        }

 public static DMatrix ToDoubleMatrix(JMatrix matrix)
 {
     DMatrix TM = new DMatrix();
     TM.M11 = (float)matrix.M11;
     TM.M12 = (float)matrix.M12;
     TM.M13 = (float)matrix.M13;
     //TM.M14 = 0.0f;
     TM.M21 = (float)matrix.M21;
     TM.M22 = (float)matrix.M22;
     TM.M23 = (float)matrix.M23;
     //TM.M24 = 0.0f;
     TM.M31 = (float)matrix.M31;
     TM.M32 = (float)matrix.M32;
     TM.M33 = (float)matrix.M33;
     //TM.M34 = 0.0f;
     //TM.M41 = 0.0f;
     //TM.M42 = 0.0f;
     //TM.M43 = 0.0f;
     TM.M44 = 1.0f;
     return TM;
 }

        /// <summary>
        /// Checks two shapes for collisions.
        /// </summary>
        /// <param name="support1">The SupportMappable implementation of the first shape to test.</param>
        /// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param>
        /// <param name="orientation1">The orientation of the first shape.</param>
        /// <param name="orientation2">The orientation of the second shape.</param>
        /// <param name="position1">The position of the first shape.</param>
        /// <param name="position2">The position of the second shape</param>
        /// <param name="point">The pointin world coordinates, where collision occur.</param>
        /// <param name="normal">The normal pointing from body2 to body1.</param>
        /// <param name="penetration">Estimated penetration depth of the collision.</param>
        /// <returns>Returns true if there is a collision, false otherwise.</returns>
        public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
            ref JMatrix orientation2, ref JVector position1, ref JVector position2,
            out JVector point, out JVector normal, out double penetration)
        {
            // Used variables
            JVector temp1, temp2;
            JVector v01, v02, v0;
            JVector v11, v12, v1;
            JVector v21, v22, v2;
            JVector v31, v32, v3;
            JVector v41, v42, v4;
            JVector mn;

            // Initialization of the output
            point = normal = JVector.Zero;
            penetration = 0.0f;

            //JVector right = JVector.Right;

            // Get the center of shape1 in world coordinates -> v01
            support1.SupportCenter(out v01);
            JVector.Transform(ref v01, ref orientation1, out v01);
            JVector.Add(ref position1, ref v01, out v01);

            // Get the center of shape2 in world coordinates -> v02
            support2.SupportCenter(out v02);
            JVector.Transform(ref v02, ref orientation2, out v02);
            JVector.Add(ref position2, ref v02, out v02);

            // v0 is the center of the minkowski difference
            JVector.Subtract(ref v02, ref v01, out v0);

            // Avoid case where centers overlap -- any direction is fine in this case
            if (v0.IsNearlyZero()) v0 = new JVector(0.00001f, 0, 0);

            // v1 = support in direction of origin
            mn = v0;
            JVector.Negate(ref v0, out normal);

            SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
            SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
            JVector.Subtract(ref v12, ref v11, out v1);

            if (JVector.Dot(ref v1, ref normal) <= 0.0f) return false;

            // v2 = support perpendicular to v1,v0
            JVector.Cross(ref v1, ref v0, out normal);

            if (normal.IsNearlyZero())
            {
                JVector.Subtract(ref v1, ref v0, out normal);

                normal.Normalize();

                point = v11;
                JVector.Add(ref point, ref v12, out point);
                JVector.Multiply(ref point, 0.5f, out point);

                JVector.Subtract(ref v12, ref v11, out temp1);
                penetration = JVector.Dot(ref temp1, ref normal);

                //point = v11;
                //point2 = v12;
                return true;
            }

            JVector.Negate(ref normal, out mn);
            SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21);
            SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22);
            JVector.Subtract(ref v22, ref v21, out v2);

            if (JVector.Dot(ref v2, ref normal) <= 0.0f) return false;

            // Determine whether origin is on + or - side of plane (v1,v0,v2)
            JVector.Subtract(ref v1, ref v0, out temp1);
            JVector.Subtract(ref v2, ref v0, out temp2);
            JVector.Cross(ref temp1, ref temp2, out normal);

            double dist = JVector.Dot(ref normal, ref v0);

            // If the origin is on the - side of the plane, reverse the direction of the plane
            if (dist > 0.0f)
            {
                JVector.Swap(ref v1, ref v2);
                JVector.Swap(ref v11, ref v21);
                JVector.Swap(ref v12, ref v22);
                JVector.Negate(ref normal, out normal);
//.........这里部分代码省略.........

        /// <summary>
        /// Transforms a vector by the transposed of the given Matrix.
        /// </summary>
        /// <param name="position">The vector to transform.</param>
        /// <param name="matrix">The transform matrix.</param>
        /// <param name="result">The transformed vector.</param>
        public static void TransposedTransform(ref JVector position, ref JMatrix matrix, out JVector result)
        {
            float num0 = ((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13);
            float num1 = ((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23);
            float num2 = ((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33);

            result.X = num0;
            result.Y = num1;
            result.Z = num2;
        }

 public static JVector Transform(JVector position, JMatrix matrix)
 {
     JVector result;
     JVector.Transform(ref position, ref matrix, out result);
     return result;
 }