Exemple #1
0
 /// <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;
 }
 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>
        /// 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);
        }
 internal RigidBodyState(RigidBody _body)
 {
     body = _body;
     savedPosition = body.Position;
     savedOrientation = body.Orientation;
     savedLinearVelocity = body.LinearVelocity;
     savedAngularVelocity = body.AngularVelocity;
 }
 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);
 }
Exemple #6
0
 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 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);
 }
 /// <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;
 }
Exemple #9
0
        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;
        }
Exemple #10
0
        static JMatrix()
        {
            Zero = new JMatrix();

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

            InternalIdentity = Identity;
        }
 public ConvexHullShape2(
     List <Jitter.LinearMath.JVector> vertices,
     Jitter.LinearMath.JMatrix inertia,
     float mass
     )
     :   base(vertices)
 {
     this.customInertia = inertia;
     this.customMass    = mass;
     this.Inertia       = inertia;
     this.Mass          = mass;
 }
 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;
		}
Exemple #14
0
        /// <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;

        }
Exemple #15
0
        /// <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);
 }
Exemple #17
0
        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;
        }
 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;
 }
Exemple #19
0
        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;
        }
        /// <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);
            }

            int phase2 = 0;
            int phase1 = 0;
            bool hit = false;

            // Phase One: Identify a portal
            while (true)
            {
                if (phase1 > MaximumIterations) return false;

                phase1++;

                // Obtain the support point in a direction perpendicular to the existing plane
                // Note: This point is guaranteed to lie off the plane
                JVector.Negate(ref normal, out mn);
                SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
                SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
                JVector.Subtract(ref v32, ref v31, out v3);

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

                // If origin is outside (v1,v0,v3), then eliminate v2 and loop
                JVector.Cross(ref v1, ref v3, out temp1);
                if (JVector.Dot(ref temp1, ref v0) < 0.0f)
                {
                    v2 = v3;
                    v21 = v31;
                    v22 = v32;
                    JVector.Subtract(ref v1, ref v0, out temp1);
                    JVector.Subtract(ref v3, ref v0, out temp2);
                    JVector.Cross(ref temp1, ref temp2, out normal);
                    continue;
                }

                // If origin is outside (v3,v0,v2), then eliminate v1 and loop
                JVector.Cross(ref v3, ref v2, out temp1);
                if (JVector.Dot(ref temp1, ref v0) < 0.0f)
                {
                    v1 = v3;
                    v11 = v31;
                    v12 = v32;
                    JVector.Subtract(ref v3, ref v0, out temp1);
                    JVector.Subtract(ref v2, ref v0, out temp2);
                    JVector.Cross(ref temp1, ref temp2, out normal);
                    continue;
                }

                // Phase Two: Refine the portal
                // We are now inside of a wedge...
                while (true)
                {
                    phase2++;

                    // Compute normal of the wedge face
                    JVector.Subtract(ref v2, ref v1, out temp1);
                    JVector.Subtract(ref v3, ref v1, out temp2);
                    JVector.Cross(ref temp1, ref temp2, out normal);

                    // Can this happen???  Can it be handled more cleanly?
                    if (normal.IsNearlyZero()) return true;

                    normal.Normalize();

                    // Compute distance from origin to wedge face
                    double d = JVector.Dot(ref normal, ref v1);

                    // If the origin is inside the wedge, we have a hit
                    if (d >= 0 && !hit)
                    {
                        // HIT!!!
                        hit = true;
                    }

                    // Find the support point in the direction of the wedge face
                    JVector.Negate(ref normal, out mn);
                    SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v41);
                    SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v42);
                    JVector.Subtract(ref v42, ref v41, out v4);

                    JVector.Subtract(ref v4, ref v3, out temp1);
                    double delta = JVector.Dot(ref temp1, ref normal);
                    penetration = JVector.Dot(ref v4, ref normal);

                    // If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
                    if (delta <= CollideEpsilon || penetration <= 0.0f || phase2 > MaximumIterations)
                    {

                        if (hit)
                        {
                            JVector.Cross(ref v1, ref v2, out temp1);
                            double b0 = JVector.Dot(ref temp1, ref v3);
                            JVector.Cross(ref v3, ref v2, out temp1);
                            double b1 = JVector.Dot(ref temp1, ref v0);
                            JVector.Cross(ref v0, ref v1, out temp1);
                            double b2 = JVector.Dot(ref temp1, ref v3);
                            JVector.Cross(ref v2, ref v1, out temp1);
                            double b3 = JVector.Dot(ref temp1, ref v0);

                            double sum = b0 + b1 + b2 + b3;

                            if (sum <= 0)
                            {
                                b0 = 0;
                                JVector.Cross(ref v2, ref v3, out temp1);
                                b1 = JVector.Dot(ref temp1, ref normal);
                                JVector.Cross(ref v3, ref v1, out temp1);
                                b2 = JVector.Dot(ref temp1, ref normal);
                                JVector.Cross(ref v1, ref v2, out temp1);
                                b3 = JVector.Dot(ref temp1, ref normal);

                                sum = b1 + b2 + b3;
                            }

                            double inv = 1.0f / sum;

                            JVector.Multiply(ref v01, b0, out point);
                            JVector.Multiply(ref v11, b1, out temp1);
                            JVector.Add(ref point, ref temp1, out point);
                            JVector.Multiply(ref v21, b2, out temp1);
                            JVector.Add(ref point, ref temp1, out point);
                            JVector.Multiply(ref v31, b3, out temp1);
                            JVector.Add(ref point, ref temp1, out point);

                            JVector.Multiply(ref v02, b0, out temp2);
                            JVector.Add(ref temp2, ref point, out point);
                            JVector.Multiply(ref v12, b1, out temp1);
                            JVector.Add(ref point, ref temp1, out point);
                            JVector.Multiply(ref v22, b2, out temp1);
                            JVector.Add(ref point, ref temp1, out point);
                            JVector.Multiply(ref v32, b3, out temp1);
                            JVector.Add(ref point, ref temp1, out point);

                            JVector.Multiply(ref point, inv * 0.5f, out point);

                        }

                        // Compute the barycentric coordinates of the origin
                        return hit;
                    }

                    //// Compute the tetrahedron dividing face (v4,v0,v1)
                    //JVector.Cross(ref v4, ref v1, out temp1);
                    //double d1 = JVector.Dot(ref temp1, ref v0);

                    //// Compute the tetrahedron dividing face (v4,v0,v2)
                    //JVector.Cross(ref v4, ref v2, out temp1);
                    //double d2 = JVector.Dot(ref temp1, ref v0);

                    // Compute the tetrahedron dividing face (v4,v0,v3)
                    JVector.Cross(ref v4, ref v0, out temp1);
                    double dot = JVector.Dot(ref temp1, ref v1);

                    if (dot >= 0.0f)
                    {
                        dot = JVector.Dot(ref temp1, ref v2);

                        if (dot >= 0.0f)
                        {
                            // Inside d1 & inside d2 ==> eliminate v1
                            v1 = v4;
                            v11 = v41;
                            v12 = v42;
                        }
                        else
                        {
                            // Inside d1 & outside d2 ==> eliminate v3
                            v3 = v4;
                            v31 = v41;
                            v32 = v42;
                        }
                    }
                    else
                    {
                        dot = JVector.Dot(ref temp1, ref v3);

                        if (dot >= 0.0f)
                        {
                            // Outside d1 & inside d3 ==> eliminate v2
                            v2 = v4;
                            v21 = v41;
                            v22 = v42;
                        }
                        else
                        {
                            // Outside d1 & outside d3 ==> eliminate v1
                            v1 = v4;
                            v11 = v41;
                            v12 = v42;
                        }
                    }

                }
            }
        }
Exemple #21
0
        /// <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;
        }
Exemple #22
0
 public static JVector Transform(JVector position, JMatrix matrix)
 {
     JVector result;
     JVector.Transform(ref position, ref matrix, out result);
     return result;
 }
 /// <summary>
 /// Creates the transposed matrix.
 /// </summary>
 /// <param name="matrix">The matrix which should be transposed.</param>
 /// <param name="result">The transposed JMatrix.</param>
 public static void Transpose(ref JMatrix matrix, out JMatrix result)
 {
     result.M11 = matrix.M11;
     result.M12 = matrix.M21;
     result.M13 = matrix.M31;
     result.M21 = matrix.M12;
     result.M22 = matrix.M22;
     result.M23 = matrix.M32;
     result.M31 = matrix.M13;
     result.M32 = matrix.M23;
     result.M33 = matrix.M33;
 }
 public static JMatrix Transpose(JMatrix matrix)
 {
     JMatrix result;
     JMatrix.Transpose(ref matrix, out result);
     return result;
 }
 /// <summary>
 /// Multiply a matrix by a scalefactor.
 /// </summary>
 /// <param name="matrix1">The matrix.</param>
 /// <param name="scaleFactor">The scale factor.</param>
 /// <param name="result">A JMatrix multiplied by the scale factor.</param>
 public static void Multiply(ref JMatrix matrix1, double scaleFactor, out JMatrix result)
 {
     double num = scaleFactor;
     result.M11 = matrix1.M11 * num;
     result.M12 = matrix1.M12 * num;
     result.M13 = matrix1.M13 * num;
     result.M21 = matrix1.M21 * num;
     result.M22 = matrix1.M22 * num;
     result.M23 = matrix1.M23 * num;
     result.M31 = matrix1.M31 * num;
     result.M32 = matrix1.M32 * num;
     result.M33 = matrix1.M33 * num;
 }
        /// <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)
        {
            box = octree.rootNodeBox;

            #region Expand Spherical
            box.Min.X -= sphericalExpansion;
            box.Min.Y -= sphericalExpansion;
            box.Min.Z -= sphericalExpansion;
            box.Max.X += sphericalExpansion;
            box.Max.Y += sphericalExpansion;
            box.Max.Z += sphericalExpansion;
            #endregion

            box.Transform(ref orientation);
        }
Exemple #27
0
 /// <summary>
 /// Transforms a vector by the given matrix.
 /// </summary>
 /// <param name="position">The vector to transform.</param>
 /// <param name="matrix">The transform matrix.</param>
 /// <returns>The transformed vector.</returns>
 #region public static JVector Transform(JVector position, JMatrix matrix)
 public static JVector Transform(JVector position, JMatrix matrix)
 {
     Transform(ref position, ref matrix, out var result);
     return(result);
 }
Exemple #28
0
        /// <summary>
        /// Multiplies two matrices.
        /// </summary>
        /// <param name="value1">The first matrix.</param>
        /// <param name="value2">The second matrix.</param>
        /// <returns>The product of both values.</returns>
        #region public static JMatrix operator *(JMatrix value1,JMatrix value2)
        public static JMatrix operator *(JMatrix value1, JMatrix value2)
        {
            JMatrix result; JMatrix.Multiply(ref value1, ref value2, out result);

            return(result);
        }
        /// <summary>
        /// Called once before iteration starts.
        /// </summary>
        /// <param name="timestep">The 5simulation timestep</param>
        public override void PrepareForIteration(float timestep)
        {
            effectiveMass = body1.invInertiaWorld;

            softnessOverDt = softness / timestep;

            effectiveMass.M11 += softnessOverDt;
            effectiveMass.M22 += softnessOverDt;
            effectiveMass.M33 += softnessOverDt;

            JMatrix.Inverse(ref effectiveMass, out effectiveMass);

            JMatrix q = JMatrix.Transpose(orientation) * body1.orientation;
            JVector axis;

            float x = q.M32 - q.M23;
            float y = q.M13 - q.M31;
            float z = q.M21 - q.M12;

            float r = JMath.Sqrt(x * x + y * y + z * z);
            float t = q.M11 + q.M22 + q.M33;

            float angle = (float)Math.Atan2(r, t - 1);
            axis = new JVector(x, y, z) * angle;

            if (r != 0.0f) axis = axis * (1.0f / r);

            bias = axis * biasFactor * (-1.0f / timestep);

            // Apply previous frame solution as initial guess for satisfying the constraint.
            if (!body1.IsStatic) body1.angularVelocity += JVector.Transform(accumulatedImpulse, body1.invInertiaWorld);
        }
 public static JMatrix Multiply(JMatrix matrix1, double scaleFactor)
 {
     JMatrix result;
     JMatrix.Multiply(ref matrix1, scaleFactor, out result);
     return result;
 }
 /// <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)
     : base(body1, null)
 {
     orientation = body1.orientation;
 }
Exemple #32
0
        /// <summary>
        /// Adds two matrices.
        /// </summary>
        /// <param name="value1">The first matrix.</param>
        /// <param name="value2">The second matrix.</param>
        /// <returns>The sum of both values.</returns>
        #region public static JMatrix operator +(JMatrix value1, JMatrix value2)
        public static JMatrix operator +(JMatrix value1, JMatrix value2)
        {
            JMatrix result; JMatrix.Add(ref value1, ref value2, out result);

            return(result);
        }
 public void Rotate(JMatrix orientation, JVector center)
 {
     for (int i = 0; i < points.Count; i++)
     {
         points[i].position = JVector.Transform(points[i].position - center, orientation);
     }
 }
        /// <summary>
        /// Multiply two matrices. Notice: matrix multiplication is not commutative.
        /// </summary>
        /// <param name="matrix1">The first matrix.</param>
        /// <param name="matrix2">The second matrix.</param>
        /// <param name="result">The product of both matrices.</param>
        public static void Multiply(ref JMatrix matrix1, ref JMatrix matrix2, out JMatrix result)
        {
            double num0 = ((matrix1.M11 * matrix2.M11) + (matrix1.M12 * matrix2.M21)) + (matrix1.M13 * matrix2.M31);
            double num1 = ((matrix1.M11 * matrix2.M12) + (matrix1.M12 * matrix2.M22)) + (matrix1.M13 * matrix2.M32);
            double num2 = ((matrix1.M11 * matrix2.M13) + (matrix1.M12 * matrix2.M23)) + (matrix1.M13 * matrix2.M33);
            double num3 = ((matrix1.M21 * matrix2.M11) + (matrix1.M22 * matrix2.M21)) + (matrix1.M23 * matrix2.M31);
            double num4 = ((matrix1.M21 * matrix2.M12) + (matrix1.M22 * matrix2.M22)) + (matrix1.M23 * matrix2.M32);
            double num5 = ((matrix1.M21 * matrix2.M13) + (matrix1.M22 * matrix2.M23)) + (matrix1.M23 * matrix2.M33);
            double num6 = ((matrix1.M31 * matrix2.M11) + (matrix1.M32 * matrix2.M21)) + (matrix1.M33 * matrix2.M31);
            double num7 = ((matrix1.M31 * matrix2.M12) + (matrix1.M32 * matrix2.M22)) + (matrix1.M33 * matrix2.M32);
            double num8 = ((matrix1.M31 * matrix2.M13) + (matrix1.M32 * matrix2.M23)) + (matrix1.M33 * matrix2.M33);

            result.M11 = num0;
            result.M12 = num1;
            result.M13 = num2;
            result.M21 = num3;
            result.M22 = num4;
            result.M23 = num5;
            result.M31 = num6;
            result.M32 = num7;
            result.M33 = num8;
        }