예제 #1
0
		/// Get the inverse of this matrix as a 2-by-2.
		/// Returns the zero matrix if singular.
		public void GetInverse22(out Mat33 M) {
			float a = ex.X, b = ey.X, c = ex.Y, d = ey.Y;
			float det = a * d - b * c;
			if (det != 0.0f) {
				det = 1.0f / det;
			}

			M.ex.X = det * d; M.ey.X = -det * b; M.ex.Z = 0.0f;
			M.ex.Y = -det * c; M.ey.Y = det * a; M.ey.Z = 0.0f;
			M.ez.X = 0.0f; M.ez.Y = 0.0f; M.ez.Z = 0.0f;
		}
예제 #2
0
		/// Get the symmetric inverse of this matrix as a 3-by-3.
		/// Returns the zero matrix if singular.
		public void GetSymInverse33(out Mat33 M) {
			float det = Utilities.Dot(ex, Utilities.Cross(ey, ez));
			if (det != 0.0f) {
				det = 1.0f / det;
			}

			float a11 = ex.X, a12 = ey.X, a13 = ez.X;
			float a22 = ey.Y, a23 = ez.Y;
			float a33 = ez.Z;

			M.ex.X = det * (a22 * a33 - a23 * a23);
			M.ex.Y = det * (a13 * a23 - a12 * a33);
			M.ex.Z = det * (a12 * a23 - a13 * a22);

			M.ey.X = M.ex.Y;
			M.ey.Y = det * (a11 * a33 - a13 * a13);
			M.ey.Z = det * (a13 * a12 - a11 * a23);

			M.ez.X = M.ex.Z;
			M.ez.Y = M.ey.Z;
			M.ez.Z = det * (a11 * a22 - a12 * a12);
		}
예제 #3
0
		/// Multiply a matrix times a vector.
		public static Vec2 Mul22(Mat33 A, Vec2 v)
		{
			return new Vec2(A.ex.X * v.X + A.ey.X * v.Y, A.ex.Y * v.X + A.ey.Y * v.Y);
		}
예제 #4
0
		/// Multiply a matrix times a vector.
		public static Vec3 Mul(Mat33 A, Vec3 v)
		{
			return v.X * A.ex + v.Y * A.ey + v.Z * A.ez;
		}
예제 #5
0
		internal override bool SolvePositionConstraints(SolverData data) {
			Vec2 cA = data.positions[m_indexA].c;
			float aA = data.positions[m_indexA].a;
			Vec2 cB = data.positions[m_indexB].c;
			float aB = data.positions[m_indexB].a;

			Rot qA = new Rot(aA);
			Rot qB = new Rot(aB);

			float mA = m_invMassA, mB = m_invMassB;
			float iA = m_invIA, iB = m_invIB;

			// Compute fresh Jacobians
			Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
			Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
			Vec2 d = cB + rB - cA - rA;

			Vec2 axis = Utilities.Mul(qA, m_localXAxisA);
			float a1 = Utilities.Cross(d + rA, axis);
			float a2 = Utilities.Cross(rB, axis);
			Vec2 perp = Utilities.Mul(qA, m_localYAxisA);

			float s1 = Utilities.Cross(d + rA, perp);
			float s2 = Utilities.Cross(rB, perp);

			Vec3 impulse;
			Vec2 C1;
			C1.X = Utilities.Dot(perp, d);
			C1.Y = aB - aA - m_referenceAngle;

			float linearError = Math.Abs(C1.X);
			float angularError = Math.Abs(C1.Y);

			bool active = false;
			float C2 = 0.0f;
			if (m_enableLimit)
			{
				float translation = Utilities.Dot(axis, d);
				if (Math.Abs(m_upperTranslation - m_lowerTranslation) < 2.0f *Settings._linearSlop)
				{
					// Prevent large angular corrections
					C2 = Utilities.Clamp(translation, -Settings._maxLinearCorrection, Settings._maxLinearCorrection);
					linearError = Math.Max(linearError, Math.Abs(translation));
					active = true;
				}
				else if (translation <= m_lowerTranslation)
				{
					// Prevent large linear corrections and allow some slop.
					C2 = Utilities.Clamp(translation - m_lowerTranslation + Settings._linearSlop, -Settings._maxLinearCorrection, 0.0f);
					linearError = Math.Max(linearError, m_lowerTranslation - translation);
					active = true;
				}
				else if (translation >= m_upperTranslation)
				{
					// Prevent large linear corrections and allow some slop.
					C2 = Utilities.Clamp(translation - m_upperTranslation - Settings._linearSlop, 0.0f, Settings._maxLinearCorrection);
					linearError = Math.Max(linearError, translation - m_upperTranslation);
					active = true;
				}
			}

			if (active)
			{
				float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
				float k12 = iA * s1 + iB * s2;
				float k13 = iA * s1 * a1 + iB * s2 * a2;
				float k22 = iA + iB;
				if (k22 == 0.0f)
				{
					// For fixed rotation
					k22 = 1.0f;
				}
				float k23 = iA * a1 + iB * a2;
				float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;

				Mat33 K = new Mat33();
				K.ex.Set(k11, k12, k13);
				K.ey.Set(k12, k22, k23);
				K.ez.Set(k13, k23, k33);

				Vec3 C = new Vec3();
				C.X = C1.X;
				C.Y = C1.Y;
				C.Z = C2;

				impulse = K.Solve33(-C);
			}
			else
			{
				float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
				float k12 = iA * s1 + iB * s2;
				float k22 = iA + iB;
				if (k22 == 0.0f)
				{
					k22 = 1.0f;
				}

				Mat22 K = new Mat22();
				K.ex.Set(k11, k12);
				K.ey.Set(k12, k22);

				Vec2 impulse1 = K.Solve(-C1);
				impulse.X = impulse1.X;
				impulse.Y = impulse1.Y;
				impulse.Z = 0.0f;
			}

			Vec2 P = impulse.X * perp + impulse.Z * axis;
			float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
			float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2;

			cA -= mA * P;
			aA -= iA * LA;
			cB += mB * P;
			aB += iB * LB;

			data.positions[m_indexA].c = cA;
			data.positions[m_indexA].a = aA;
			data.positions[m_indexB].c = cB;
			data.positions[m_indexB].a = aB;

			return linearError <= Settings._linearSlop && angularError <= Settings._angularSlop;
		}
예제 #6
0
        internal override bool SolvePositionConstraints(SolverData data)
        {
            Vec2  cA = data.positions[m_indexA].c;
            float aA = data.positions[m_indexA].a;
            Vec2  cB = data.positions[m_indexB].c;
            float aB = data.positions[m_indexB].a;

            Rot qA = new Rot(aA);
            Rot qB = new Rot(aB);

            float mA = m_invMassA, mB = m_invMassB;
            float iA = m_invIA, iB = m_invIB;

            // Compute fresh Jacobians
            Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
            Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
            Vec2 d  = cB + rB - cA - rA;

            Vec2  axis = Utilities.Mul(qA, m_localXAxisA);
            float a1   = Utilities.Cross(d + rA, axis);
            float a2   = Utilities.Cross(rB, axis);
            Vec2  perp = Utilities.Mul(qA, m_localYAxisA);

            float s1 = Utilities.Cross(d + rA, perp);
            float s2 = Utilities.Cross(rB, perp);

            Vec3 impulse;
            Vec2 C1;

            C1.X = Utilities.Dot(perp, d);
            C1.Y = aB - aA - m_referenceAngle;

            float linearError  = Math.Abs(C1.X);
            float angularError = Math.Abs(C1.Y);

            bool  active = false;
            float C2     = 0.0f;

            if (m_enableLimit)
            {
                float translation = Utilities.Dot(axis, d);
                if (Math.Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * Settings._linearSlop)
                {
                    // Prevent large angular corrections
                    C2          = Utilities.Clamp(translation, -Settings._maxLinearCorrection, Settings._maxLinearCorrection);
                    linearError = Math.Max(linearError, Math.Abs(translation));
                    active      = true;
                }
                else if (translation <= m_lowerTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2          = Utilities.Clamp(translation - m_lowerTranslation + Settings._linearSlop, -Settings._maxLinearCorrection, 0.0f);
                    linearError = Math.Max(linearError, m_lowerTranslation - translation);
                    active      = true;
                }
                else if (translation >= m_upperTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2          = Utilities.Clamp(translation - m_upperTranslation - Settings._linearSlop, 0.0f, Settings._maxLinearCorrection);
                    linearError = Math.Max(linearError, translation - m_upperTranslation);
                    active      = true;
                }
            }

            if (active)
            {
                float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
                float k12 = iA * s1 + iB * s2;
                float k13 = iA * s1 * a1 + iB * s2 * a2;
                float k22 = iA + iB;
                if (k22 == 0.0f)
                {
                    // For fixed rotation
                    k22 = 1.0f;
                }
                float k23 = iA * a1 + iB * a2;
                float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;

                Mat33 K = new Mat33();
                K.ex.Set(k11, k12, k13);
                K.ey.Set(k12, k22, k23);
                K.ez.Set(k13, k23, k33);

                Vec3 C = new Vec3();
                C.X = C1.X;
                C.Y = C1.Y;
                C.Z = C2;

                impulse = K.Solve33(-C);
            }
            else
            {
                float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
                float k12 = iA * s1 + iB * s2;
                float k22 = iA + iB;
                if (k22 == 0.0f)
                {
                    k22 = 1.0f;
                }

                Mat22 K = new Mat22();
                K.ex.Set(k11, k12);
                K.ey.Set(k12, k22);

                Vec2 impulse1 = K.Solve(-C1);
                impulse.X = impulse1.X;
                impulse.Y = impulse1.Y;
                impulse.Z = 0.0f;
            }

            Vec2  P  = impulse.X * perp + impulse.Z * axis;
            float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
            float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2;

            cA -= mA * P;
            aA -= iA * LA;
            cB += mB * P;
            aB += iB * LB;

            data.positions[m_indexA].c = cA;
            data.positions[m_indexA].a = aA;
            data.positions[m_indexB].c = cB;
            data.positions[m_indexB].a = aB;

            return(linearError <= Settings._linearSlop && angularError <= Settings._angularSlop);
        }