예제 #1
0
        public static Vector2 Multiply(Matrix2 A, Vector2 V)
        {
            float x = A.M11 * V.X + A.M21 * V.Y;
            float y = A.M12 * V.X + A.M22 * V.Y;

            return new Vector2(x, y);
        }
예제 #2
0
        // Multiply Matrices
        public static Matrix2 Multiply(Matrix2 A, Matrix2 B)
        {
            float m11 = A.M11 * B.M11 + A.M21 * B.M12;
            float m12 = A.M12 * B.M11 + A.M22 * B.M12;
            float m21 = A.M11 * B.M21 + A.M21 * B.M22;
            float m22 = A.M12 * B.M21 + A.M22 * B.M22;

            return new Matrix2(m11, m21, m12, m22);
        }
예제 #3
0
        public static Matrix2 Add(Matrix2 A, float C)
        {
            float m11 = A.M11 + C;
            float m12 = A.M12 + C;
            float m21 = A.M21 + C;
            float m22 = A.M22 + C;

            return new Matrix2(m11, m21, m12, m22);
        }
예제 #4
0
        public static Matrix2 Add(Matrix2 A, Matrix2 B)
        {
            float m11 = A.M11 + B.M11;
            float m12 = A.M12 + B.M12;
            float m21 = A.M21 + B.M21;
            float m22 = A.M22 + B.M22;

            return new Matrix2(m11, m21, m12, m22);
        }
예제 #5
0
파일: Joint.cs 프로젝트: padfootx7/Kinetic
        public Joint(Body body1, Body body2, Vector2 anchor)
        {
            Body1 = body1;
            Body2 = body2;

            Matrix2 rotation1 = new Matrix2(body1.Rotation);
            Matrix2 rotation2 = new Matrix2(body2.Rotation);

            Matrix2 rotation1T = rotation1.Transpose();
            Matrix2 rotation2T = rotation2.Transpose();

            LocalAnchor1 = Matrix2.Multiply(rotation1T, Vector2.Subtract(anchor, body1.Position));
            LocalAnchor2 = Matrix2.Multiply(rotation2T, Vector2.Subtract(anchor, body2.Position));

            // Set specific variables
            BiasFactor = 0.2f;
        }
예제 #6
0
        // Invert matrix values
        public Matrix2 Invert()
        {
            Matrix2 returnMatrix = new Matrix2();
            float det = M11 * M22 - M21 * M12;

            det = 1.0f / det;

            returnMatrix.M11 = det * M22;
            returnMatrix.M21 = -det * M21;
            returnMatrix.M12 = -det * M12;
            returnMatrix.M22 = det * M11;

            return returnMatrix;
        }
예제 #7
0
파일: Joint.cs 프로젝트: padfootx7/Kinetic
        public void Prestep(float inverseDt)
        {
            Matrix2 rotation1 = new Matrix2(Body1.Rotation);
            Matrix2 rotation2 = new Matrix2(Body2.Rotation);

            R1 = Matrix2.Multiply(rotation1, LocalAnchor1);
            R2 = Matrix2.Multiply(rotation2, LocalAnchor2);

            // Incoming loads of math
            Matrix2 k1 = new Matrix2
            (
                Body1.InverseMass + Body2.InverseMass, 0f,
                0f, Body1.InverseMass + Body2.InverseMass
            );

            Matrix2 k2 = new Matrix2
            (
                Body1.InverseInertia * R1.Y * R1.Y, -Body1.InverseInertia * R1.X * R1.Y,
                -Body1.InverseInertia * R1.X * R1.Y, Body1.InverseInertia * R1.X * R1.X
            );

            Matrix2 k3 = new Matrix2
            (
                Body1.InverseInertia * R2.Y * R2.Y, -Body1.InverseInertia * R2.X * R2.Y,
                -Body1.InverseInertia * R2.X * R2.Y, Body1.InverseInertia * R2.X * R2.X
            );

            // Add together the matrices
            Matrix2 K = Matrix2.Add(k1, Matrix2.Add(k2, k3));
            K = Matrix2.Add(K, Softness);

            // Set it back to the main Matrix
            M = K.Invert();

            // Position correction?
            Vector2 p1 = Vector2.Add(Body1.Position, R1);
            Vector2 p2 = Vector2.Add(Body2.Position, R2);
            Vector2 dp = p2 - p1;

            if (World.PositionCorrection)
            {
                Bias = -BiasFactor * inverseDt * dp;
            }
            else
            {
                Bias = Vector2.Zero;
            }

            // Impulse accumulation
            if (World.WarmStarting)
            {
                Body1.Velocity = Vector2.Subtract(Body1.Velocity, Vector2.Multiply(AccumulatedImpulses, Body1.InverseMass));
                Body1.AngularVelocity = Body1.AngularVelocity - Body1.InverseInertia * Cross(R1, AccumulatedImpulses);

                Body2.Velocity = Vector2.Add(Body2.Velocity, Vector2.Multiply(AccumulatedImpulses, Body2.InverseMass));
                Body2.AngularVelocity = Body2.AngularVelocity + Body2.InverseInertia * Cross(R2, AccumulatedImpulses);
            }
            else
            {
                AccumulatedImpulses = Vector2.Zero;
            }
        }