Пример #1
0
		/// <summary>
		/// Compute the inverse of this matrix, such that inv(A) * A = identity.
		/// </summary>
		public Mat22 Invert()
		{
			float a = Col1.X, b = Col2.X, c = Col1.Y, d = Col2.Y;
			Mat22 B = new Mat22();
			float det = a * d - b * c;
			Box2DXDebug.Assert(det != 0.0f);
			det = 1.0f / det;
			B.Col1.X = det * d; B.Col2.X = -det * b;
			B.Col1.Y = -det * c; B.Col2.Y = det * a;
			return B;
		}
Пример #2
0
		internal PolygonShape(ShapeDef def)
			: base(def)
		{
			Box2DXDebug.Assert(def.Type == ShapeType.PolygonShape);
			_type = ShapeType.PolygonShape;
			PolygonDef poly = (PolygonDef)def;

			// Get the vertices transformed into the body frame.
			_vertexCount = poly.VertexCount;
			Box2DXDebug.Assert(3 <= _vertexCount && _vertexCount <= Settings.MaxPolygonVertices);

			// Copy vertices.
			for (int i = 0; i < _vertexCount; ++i)
			{
				_vertices[i] = poly.Vertices[i];
			}

			// Compute normals. Ensure the edges have non-zero length.
			for (int i = 0; i < _vertexCount; ++i)
			{
				int i1 = i;
				int i2 = i + 1 < _vertexCount ? i + 1 : 0;
				Vec2 edge = _vertices[i2] - _vertices[i1];
				Box2DXDebug.Assert(edge.LengthSquared() > Common.Settings.FLT_EPSILON * Common.Settings.FLT_EPSILON);
				_normals[i] = Vec2.Cross(edge, 1.0f);
				_normals[i].Normalize();
			}

#if DEBUG
			// Ensure the polygon is convex.
			for (int i = 0; i < _vertexCount; ++i)
			{
				for (int j = 0; j < _vertexCount; ++j)
				{
					// Don't check vertices on the current edge.
					if (j == i || j == (i + 1) % _vertexCount)
					{
						continue;
					}

					// Your polygon is non-convex (it has an indentation).
					// Or your polygon is too skinny.
					float s = Vec2.Dot(_normals[i], _vertices[j] - _vertices[i]);
					Box2DXDebug.Assert(s < -Settings.LinearSlop);
				}
			}

			// Ensure the polygon is counter-clockwise.
			for (int i = 1; i < _vertexCount; ++i)
			{
				float cross = Vec2.Cross(_normals[i - 1], _normals[i]);

				// Keep asinf happy.
				cross = Common.Math.Clamp(cross, -1.0f, 1.0f);

				// You have consecutive edges that are almost parallel on your polygon.
				float angle = (float)System.Math.Asin(cross);
				Box2DXDebug.Assert(angle > Settings.AngularSlop);
			}
#endif

			// Compute the polygon centroid.
			_centroid = ComputeCentroid(poly.Vertices, poly.VertexCount);

			// Compute the oriented bounding box.
			ComputeOBB(out _obb, _vertices, _vertexCount);

			// Create core polygon shape by shifting edges inward.
			// Also compute the min/max radius for CCD.
			for (int i = 0; i < _vertexCount; ++i)
			{
				int i1 = i - 1 >= 0 ? i - 1 : _vertexCount - 1;
				int i2 = i;

				Vec2 n1 = _normals[i1];
				Vec2 n2 = _normals[i2];
				Vec2 v = _vertices[i] - _centroid; ;

				Vec2 d = new Vec2();
				d.X = Vec2.Dot(n1, v) - Settings.ToiSlop;
				d.Y = Vec2.Dot(n2, v) - Settings.ToiSlop;

				// Shifting the edge inward by b2_toiSlop should
				// not cause the plane to pass the centroid.

				// Your shape has a radius/extent less than b2_toiSlop.
				Box2DXDebug.Assert(d.X >= 0.0f);
				Box2DXDebug.Assert(d.Y >= 0.0f);
				Mat22 A = new Mat22();
				A.Col1.X = n1.X; A.Col2.X = n1.Y;
				A.Col1.Y = n2.X; A.Col2.Y = n2.Y;
				_coreVertices[i] = A.Solve(d) + _centroid;
			}
		}
Пример #3
0
 /// <summary>
 /// Initialize using a position vector and a rotation matrix.
 /// </summary>
 /// <param name="position"></param>
 /// <param name="R"></param>
 public Transform(Vector2 position, Mat22 rotation)
 {
     this.position = position;
     this.rotation = rotation;
 }
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            // TODO_ERIN block solve with limit.

            Body b1 = _body1;
            Body b2 = _body2;

            float angularError = 0.0f;
            float positionError = 0.0f;

            // Solve angular limit constraint.
            if (_enableLimit && _limitState !=  LimitState.InactiveLimit)
            {
                float angle = b2._sweep.A - b1._sweep.A - _referenceAngle;
                float limitImpulse = 0.0f;

                if (_limitState == LimitState.EqualLimits)
                {
                    // Prevent large angular corrections
                    float C = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
                    limitImpulse = -_motorMass * C;
                    angularError = Box2DXMath.Abs(C);
                }
                else if (_limitState == LimitState.AtLowerLimit)
                {
                    float C = angle - _lowerAngle;
                    angularError = -C;

                    // Prevent large angular corrections and allow some slop.
                    C = Box2DXMath.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
                    limitImpulse = -_motorMass * C;
                }
                else if (_limitState == LimitState.AtUpperLimit)
                {
                    float C = angle - _upperAngle;
                    angularError = C;

                    // Prevent large angular corrections and allow some slop.
                    C = Box2DXMath.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
                    limitImpulse = -_motorMass * C;
                }

                b1._sweep.A -= b1._invI * limitImpulse;
                b2._sweep.A += b2._invI * limitImpulse;

                b1.SynchronizeTransform();
                b2.SynchronizeTransform();
            }

            // Solve point-to-point constraint.
            {
                Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
                Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

                Vec2 C = b2._sweep.C + r2 - b1._sweep.C - r1;
                positionError = C.Length();

                float invMass1 = b1._invMass, invMass2 = b2._invMass;
                float invI1 = b1._invI, invI2 = b2._invI;

                // Handle large detachment.
                float k_allowedStretch = 10.0f * Settings.LinearSlop;
                if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
                {
                    // Use a particle solution (no rotation).
                    Vec2 u = C; u.Normalize();
                    float k = invMass1 + invMass2;
                    Box2DXDebug.Assert(k > Settings.FLT_EPSILON);
                    float m = 1.0f / k;
                    Vec2 impulse = m * (-C);
                    float k_beta = 0.5f;
                    b1._sweep.C -= k_beta * invMass1 * impulse;
                    b2._sweep.C += k_beta * invMass2 * impulse;

                    C = b2._sweep.C + r2 - b1._sweep.C - r1;
                }

                Mat22 K1 = new Mat22();
                K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
                K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;

                Mat22 K2 = new Mat22();
                K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
                K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;

                Mat22 K3 = new Mat22();
                K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
                K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;

                Mat22 K = K1 + K2 + K3;
                Vec2 impulse_ = K.Solve(-C);

                b1._sweep.C -= b1._invMass * impulse_;
                b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse_);

                b2._sweep.C += b2._invMass * impulse_;
                b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse_);

                b1.SynchronizeTransform();
                b2.SynchronizeTransform();
            }

            return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
        }
Пример #5
0
 /// <summary>
 /// Multiply a matrix times a vector. If a rotation matrix is provided,
 /// then this transforms the vector from one frame to another.
 /// </summary>
 public static Vec2 Mul(Mat22 A, Vec2 v)
 {
     return(new Vec2(A.Col1.X * v.X + A.Col2.X * v.Y, A.Col1.Y * v.X + A.Col2.Y * v.Y));
 }
Пример #6
0
 /// <summary>
 /// A * B
 /// </summary>
 public static Mat22 Mul(Mat22 A, Mat22 B)
 {
     return(new Mat22(Mul(A, B.Col1), Mul(A, B.Col2)));
 }
Пример #7
0
 /// Set this based on the position and angle.
 public void Set(Vector2 p, float angle)
 {
     position = p;
     R        = new Mat22(angle);
 }
Пример #8
0
 /// <summary>
 /// Initialize using a position vector and a rotation matrix.
 /// </summary>
 /// <param name="position"></param>
 /// <param name="R"></param>
 public XForm(Vec2 position, Mat22 rotation)
 {
     Position = position;
     R        = rotation;
 }
Пример #9
0
 /// <summary>
 /// Initialize using a position vector and a rotation matrix.
 /// </summary>
 /// <param name="position"></param>
 /// <param name="R"></param>
 public XForm(Vector2 p, Mat22 rotation)
 {
     position = p;
     R        = rotation;
 }
Пример #10
0
 /// <summary>
 /// Set this to the identity transform.
 /// </summary>
 public void SetIdentity()
 {
     position = Vector2.Zero;
     R        = Mat22.Identity;
 }
Пример #11
0
		/// <summary>
		/// Set the position of the body's origin and rotation (radians).
		/// This breaks any contacts and wakes the other bodies.
		/// </summary>
		/// <param name="position">The new world position of the body's origin (not necessarily
		/// the center of mass).</param>
		/// <param name="angle">The new world rotation angle of the body in radians.</param>
		/// <returns>Return false if the movement put a shape outside the world. In this case the
		/// body is automatically frozen.</returns>
		public bool SetTransform(Vector2 position, Mat22 rotation)
Пример #12
0
 /// <summary>
 /// Set this to the identity transform.
 /// </summary>
 public void SetIdentity()
 {
     position = Vector2.zero;
     R = Mat22.Identity;
 }
Пример #13
0
 /// Set this based on the position and angle.
 public void Set(Vector2 p, float angle)
 {
     position = p;
     R = new Mat22(angle);
 }
Пример #14
0
 /// <summary>
 /// Initialize using a position vector and a rotation matrix.
 /// </summary>
 /// <param name="position"></param>
 /// <param name="R"></param>
 public XForm(Vector2 p, Mat22 rotation)
 {
     position = p;
     R = rotation;
 }
Пример #15
0
 internal override void InitVelocityConstraints(TimeStep step)
 {
     Body body = this._body2;
     float mass = body.GetMass();
     float num = 2f * Settings.Pi * this._frequencyHz;
     float num2 = 2f * mass * this._dampingRatio * num;
     float num3 = mass * (num * num);
     Box2DXDebug.Assert(num2 + step.Dt * num3 > Settings.FLT_EPSILON);
     this._gamma = 1f / (step.Dt * (num2 + step.Dt * num3));
     this._beta = step.Dt * num3 * this._gamma;
     Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor - body.GetLocalCenter());
     float invMass = body._invMass;
     float invI = body._invI;
     Mat22 a = default(Mat22);
     a.Col1.X = invMass;
     a.Col2.X = 0f;
     a.Col1.Y = 0f;
     a.Col2.Y = invMass;
     Mat22 b = default(Mat22);
     b.Col1.X = invI * vec.Y * vec.Y;
     b.Col2.X = -invI * vec.X * vec.Y;
     b.Col1.Y = -invI * vec.X * vec.Y;
     b.Col2.Y = invI * vec.X * vec.X;
     Mat22 mat = a + b;
     mat.Col1.X = mat.Col1.X + this._gamma;
     mat.Col2.Y = mat.Col2.Y + this._gamma;
     this._mass = mat.Invert();
     this._C = body._sweep.C + vec - this._target;
     body._angularVelocity *= 0.98f;
     this._impulse *= step.DtRatio;
     Body expr_21D = body;
     expr_21D._linearVelocity += invMass * this._impulse;
     body._angularVelocity += invI * Vec2.Cross(vec, this._impulse);
 }
Пример #16
0
 /// <summary>
 /// Multiply a matrix times a vector. If a rotation matrix is provided,
 /// then this Transforms the vector from one frame to another.
 /// </summary>
 public static Vector2 Mul(Mat22 A, Vector2 v)
 {
     return(new Vector2(A.Col1.x * v.x + A.Col2.x * v.y, A.Col1.y * v.x + A.Col2.y * v.y));
 }
Пример #17
0
 public XForm(Vec2 position, Mat22 rotation)
 {
     this.Position = position;
     this.R = rotation;
 }
Пример #18
0
 public static Mat22 Abs(Mat22 A)
 {
     return(new Mat22(Abs(A.Col1), Abs(A.Col2)));
 }
Пример #19
0
 public XForm(Vec2 position, Mat22 rotation)
 {
     this.Position = position;
     this.R        = rotation;
 }
Пример #20
0
 /// <summary>
 /// Multiply a matrix transpose times a vector. If a rotation matrix is provided,
 /// then this transforms the vector from one frame to another (inverse transform).
 /// </summary>
 public static Vec2 MulT(Mat22 A, Vec2 v)
 {
     return(new Vec2(Vec2.Dot(v, A.Col1), Vec2.Dot(v, A.Col2)));
 }
Пример #21
0
		internal override void InitVelocityConstraints(TimeStep step)
		{
			Body b1 = _body1;
			Body b2 = _body2;

			// Compute the effective mass matrix.
			Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
			Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

			// K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
			//      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
			//        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
			float invMass1 = b1._invMass, invMass2 = b2._invMass;
			float invI1 = b1._invI, invI2 = b2._invI;

			Mat22 K1 = new Mat22();
			K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
			K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;

			Mat22 K2 = new Mat22();
			K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
			K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;

			Mat22 K3 = new Mat22();
			K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
			K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;

			Mat22 K = K1 + K2 + K3;
			_pivotMass = K.Invert();

			_motorMass = 1.0f / (invI1 + invI2);

			if (_enableMotor == false)
			{
				_motorForce = 0.0f;
			}

			if (_enableLimit)
			{
				float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle;
				if (Box2DXMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
				{
					_limitState = LimitState.EqualLimits;
				}
				else if (jointAngle <= _lowerAngle)
				{
					if (_limitState != LimitState.AtLowerLimit)
					{
						_limitForce = 0.0f;
					}
					_limitState = LimitState.AtLowerLimit;
				}
				else if (jointAngle >= _upperAngle)
				{
					if (_limitState != LimitState.AtUpperLimit)
					{
						_limitForce = 0.0f;
					}
					_limitState = LimitState.AtUpperLimit;
				}
				else
				{
					_limitState = LimitState.InactiveLimit;
					_limitForce = 0.0f;
				}
			}
			else
			{
				_limitForce = 0.0f;
			}

			if (step.WarmStarting)
			{
				b1._linearVelocity -= Settings.FORCE_SCALE(step.Dt) * invMass1 * _pivotForce;
				b1._angularVelocity -= Settings.FORCE_SCALE(step.Dt) * invI1 * (Vec2.Cross(r1, _pivotForce) + Settings.FORCE_INV_SCALE(_motorForce + _limitForce));

				b2._linearVelocity += Settings.FORCE_SCALE(step.Dt) * invMass2 * _pivotForce;
				b2._angularVelocity += Settings.FORCE_SCALE(step.Dt) * invI2 * (Vec2.Cross(r2, _pivotForce) + Settings.FORCE_INV_SCALE(_motorForce + _limitForce));
			}
			else
			{
				_pivotForce.SetZero();
				_motorForce = 0.0f;
				_limitForce = 0.0f;
			}

			_limitPositionImpulse = 0.0f;
		}
Пример #22
0
		/// <summary>
		/// Initialize using a position vector and a rotation matrix.
		/// </summary>
		/// <param name="position"></param>
		/// <param name="R"></param>
		public XForm(Vec2 position, Mat22 rotation)
		{
			Position = position;
			R = rotation;
		}
Пример #23
0
		internal override bool SolvePositionConstraints()
		{
			Body b1 = _body1;
			Body b2 = _body2;

			float positionError = 0.0f;

			// Solve point-to-point position error.
			Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
			Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

			Vec2 p1 = b1._sweep.C + r1;
			Vec2 p2 = b2._sweep.C + r2;
			Vec2 ptpC = p2 - p1;

			positionError = ptpC.Length();

			// Prevent overly large corrections.
			//b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection);
			//ptpC = b2Clamp(ptpC, -dpMax, dpMax);

			float invMass1 = b1._invMass, invMass2 = b2._invMass;
			float invI1 = b1._invI, invI2 = b2._invI;

			Mat22 K1 = new Mat22();
			K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
			K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;

			Mat22 K2 = new Mat22();
			K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
			K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;

			Mat22 K3 = new Mat22();
			K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
			K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;

			Mat22 K = K1 + K2 + K3;
			Vec2 impulse = K.Solve(-ptpC);

			b1._sweep.C -= b1._invMass * impulse;
			b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse);

			b2._sweep.C += b2._invMass * impulse;
			b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse);

			b1.SynchronizeTransform();
			b2.SynchronizeTransform();

			// Handle limits.
			float angularError = 0.0f;

			if (_enableLimit && _limitState != LimitState.InactiveLimit)
			{
				float angle = b2._sweep.A - b1._sweep.A - _referenceAngle;
				float limitImpulse = 0.0f;

				if (_limitState == LimitState.EqualLimits)
				{
					// Prevent large angular corrections
					float limitC = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
					limitImpulse = -_motorMass * limitC;
					angularError = Box2DXMath.Abs(limitC);
				}
				else if (_limitState == LimitState.AtLowerLimit)
				{
					float limitC = angle - _lowerAngle;
					angularError = Box2DXMath.Max(0.0f, -limitC);

					// Prevent large angular corrections and allow some slop.
					limitC = Box2DXMath.Clamp(limitC + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
					limitImpulse = -_motorMass * limitC;
					float oldLimitImpulse = _limitPositionImpulse;
					_limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f);
					limitImpulse = _limitPositionImpulse - oldLimitImpulse;
				}
				else if (_limitState == LimitState.AtUpperLimit)
				{
					float limitC = angle - _upperAngle;
					angularError = Box2DXMath.Max(0.0f, limitC);

					// Prevent large angular corrections and allow some slop.
					limitC = Box2DXMath.Clamp(limitC - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
					limitImpulse = -_motorMass * limitC;
					float oldLimitImpulse = _limitPositionImpulse;
					_limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f);
					limitImpulse = _limitPositionImpulse - oldLimitImpulse;
				}

				b1._sweep.A -= b1._invI * limitImpulse;
				b2._sweep.A += b2._invI * limitImpulse;

				b1.SynchronizeTransform();
				b2.SynchronizeTransform();
			}

			return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
		}
Пример #24
0
 /// <summary>
 /// Initialize using a position vector and a rotation matrix.
 /// </summary>
 /// <param name="position"></param>
 /// <param name="R"></param>
 public Transform(Vector2 position, Quaternion rotation)
 {
     this.position = position;
     this.rotation = rotation;
 }
Пример #25
0
 internal override bool SolvePositionConstraints(float baumgarte)
 {
     Body body = this._body1;
     Body body2 = this._body2;
     Vec2 vec = body._sweep.C;
     float num = body._sweep.A;
     Vec2 vec2 = body2._sweep.C;
     float num2 = body2._sweep.A;
     float num3 = 0f;
     bool flag = false;
     float y = 0f;
     Mat22 a = new Mat22(num);
     Mat22 a2 = new Mat22(num2);
     Vec2 v = Box2DX.Common.Math.Mul(a, this._localAnchor1 - this._localCenter1);
     Vec2 vec3 = Box2DX.Common.Math.Mul(a2, this._localAnchor2 - this._localCenter2);
     Vec2 vec4 = vec2 + vec3 - vec - v;
     if (this._enableLimit)
     {
         this._axis = Box2DX.Common.Math.Mul(a, this._localXAxis1);
         this._a1 = Vec2.Cross(vec4 + v, this._axis);
         this._a2 = Vec2.Cross(vec3, this._axis);
         float num4 = Vec2.Dot(this._axis, vec4);
         if (Box2DX.Common.Math.Abs(this._upperTranslation - this._lowerTranslation) < 2f * Settings.LinearSlop)
         {
             y = Box2DX.Common.Math.Clamp(num4, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
             num3 = Box2DX.Common.Math.Abs(num4);
             flag = true;
         }
         else
         {
             if (num4 <= this._lowerTranslation)
             {
                 y = Box2DX.Common.Math.Clamp(num4 - this._lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
                 num3 = this._lowerTranslation - num4;
                 flag = true;
             }
             else
             {
                 if (num4 >= this._upperTranslation)
                 {
                     y = Box2DX.Common.Math.Clamp(num4 - this._upperTranslation - Settings.LinearSlop, 0f, Settings.MaxLinearCorrection);
                     num3 = num4 - this._upperTranslation;
                     flag = true;
                 }
             }
         }
     }
     this._perp = Box2DX.Common.Math.Mul(a, this._localYAxis1);
     this._s1 = Vec2.Cross(vec4 + v, this._perp);
     this._s2 = Vec2.Cross(vec3, this._perp);
     float num5 = Vec2.Dot(this._perp, vec4);
     num3 = Box2DX.Common.Math.Max(num3, Box2DX.Common.Math.Abs(num5));
     float num6 = 0f;
     Vec2 vec5;
     if (flag)
     {
         float invMass = this._invMass1;
         float invMass2 = this._invMass2;
         float invI = this._invI1;
         float invI2 = this._invI2;
         float num7 = invMass + invMass2 + invI * this._s1 * this._s1 + invI2 * this._s2 * this._s2;
         float num8 = invI * this._s1 * this._a1 + invI2 * this._s2 * this._a2;
         float y2 = invMass + invMass2 + invI * this._a1 * this._a1 + invI2 * this._a2 * this._a2;
         this._K.Col1.Set(num7, num8);
         this._K.Col2.Set(num8, y2);
         vec5 = this._K.Solve(-new Vec2
         {
             X = num5,
             Y = y
         });
     }
     else
     {
         float invMass = this._invMass1;
         float invMass2 = this._invMass2;
         float invI = this._invI1;
         float invI2 = this._invI2;
         float num7 = invMass + invMass2 + invI * this._s1 * this._s1 + invI2 * this._s2 * this._s2;
         float x = -num5 / num7;
         vec5.X = x;
         vec5.Y = 0f;
     }
     Vec2 v2 = vec5.X * this._perp + vec5.Y * this._axis;
     float num9 = vec5.X * this._s1 + vec5.Y * this._a1;
     float num10 = vec5.X * this._s2 + vec5.Y * this._a2;
     vec -= this._invMass1 * v2;
     num -= this._invI1 * num9;
     vec2 += this._invMass2 * v2;
     num2 += this._invI2 * num10;
     body._sweep.C = vec;
     body._sweep.A = num;
     body2._sweep.C = vec2;
     body2._sweep.A = num2;
     body.SynchronizeTransform();
     body2.SynchronizeTransform();
     return num3 <= Settings.LinearSlop && num6 <= Settings.AngularSlop;
 }
Пример #26
0
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vector2 c1 = b1._sweep.C;
            float a1 = b1._sweep.A;

            Vector2 c2 = b2._sweep.C;
            float a2 = b2._sweep.A;

            // Solve linear limit constraint.
            float linearError = 0.0f, angularError = 0.0f;
            bool active = false;
            float C2 = 0.0f;

            Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2);

            Vector2 r1 = Box2DX.Common.Math.Mul(R1, _localAnchor1 - _localCenter1);
            Vector2 r2 = Box2DX.Common.Math.Mul(R2, _localAnchor2 - _localCenter2);
            Vector2 d = c2 + r2 - c1 - r1;

            if (_enableLimit)
            {
                _axis = Box2DX.Common.Math.Mul(R1, _localXAxis1);

                _a1 = (d + r1).Cross(_axis);
                _a2 = r2.Cross(_axis);

                float translation = Vector2.Dot(_axis, d);
                if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    // Prevent large angular corrections
                    C2 = Mathf.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
                    linearError = Box2DX.Common.Math.Abs(translation);
                    active = true;
                }
                else if (translation <= _lowerTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2 = Mathf.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
                    linearError = _lowerTranslation - translation;
                    active = true;
                }
                else if (translation >= _upperTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2 = Mathf.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
                    linearError = translation - _upperTranslation;
                    active = true;
                }
            }

            _perp = Box2DX.Common.Math.Mul(R1, _localYAxis1);

            _s1 = (d + r1).Cross(_perp);
            _s2 = r2.Cross(_perp);

            Vector3 impulse;
            Vector2 C1 = new Vector2();
            C1.x = Vector2.Dot(_perp, d);
            C1.y = a2 - a1 - _refAngle;

            linearError = Box2DX.Common.Math.Max(linearError, Box2DX.Common.Math.Abs(C1.x));
            angularError = Box2DX.Common.Math.Abs(C1.y);

            if (active)
            {
                float m1 = _invMass1, m2 = _invMass2;
                float i1 = _invI1, i2 = _invI2;

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
                float k12 = i1 * _s1 + i2 * _s2;
                float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
                float k22 = i1 + i2;
                float k23 = i1 * _a1 + i2 * _a2;
                float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;

                _K.Col1 = new Vector3(k11, k12, k13);
                _K.Col2 = new Vector3(k12, k22, k23);
                _K.Col3 = new Vector3(k13, k23, k33);

                Vector3 C = new Vector3();
                C.x = C1.x;
                C.y = C1.y;
                C.z = C2;

                impulse = _K.Solve33(-C);
            }
            else
            {
                float m1 = _invMass1, m2 = _invMass2;
                float i1 = _invI1, i2 = _invI2;

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
                float k12 = i1 * _s1 + i2 * _s2;
                float k22 = i1 + i2;

                _K.Col1 = new Vector3(k11, k12, 0.0f);
                _K.Col2 = new Vector3(k12, k22, 0.0f);

                Vector2 impulse1 = _K.Solve22(-C1);
                impulse.x = impulse1.x;
                impulse.y = impulse1.y;
                impulse.z = 0.0f;
            }

            Vector2 P = impulse.x * _perp + impulse.z * _axis;
            float L1 = impulse.x * _s1 + impulse.y + impulse.z * _a1;
            float L2 = impulse.x * _s2 + impulse.y + impulse.z * _a2;

            c1 -= _invMass1 * P;
            a1 -= _invI1 * L1;
            c2 += _invMass2 * P;
            a2 += _invI2 * L2;

            // TODO_ERIN remove need for this.
            b1._sweep.C = c1;
            b1._sweep.A = a1;
            b2._sweep.C = c2;
            b2._sweep.A = a2;
            b1.SynchronizeTransform();
            b2.SynchronizeTransform();

            return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
        }
Пример #27
0
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Vec2 c1 = b1._sweep.C;
            float a1 = b1._sweep.A;

            Vec2 c2 = b2._sweep.C;
            float a2 = b2._sweep.A;

            // Solve linear limit constraint.
            float linearError = 0.0f, angularError = 0.0f;
            bool active = false;
            float C2 = 0.0f;

            Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2);

            Vec2 r1 = Math.Mul(R1, _localAnchor1 - _localCenter1);
            Vec2 r2 = Math.Mul(R2, _localAnchor2 - _localCenter2);
            Vec2 d = c2 + r2 - c1 - r1;

            if (_enableLimit)
            {
                _axis = Math.Mul(R1, _localXAxis1);

                _a1 = Vec2.Cross(d + r1, _axis);
                _a2 = Vec2.Cross(r2, _axis);

                float translation = Vec2.Dot(_axis, d);
                if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    // Prevent large angular corrections
                    C2 = Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
                    linearError = Math.Abs(translation);
                    active = true;
                }
                else if (translation <= _lowerTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2 = Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
                    linearError = _lowerTranslation - translation;
                    active = true;
                }
                else if (translation >= _upperTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2 = Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
                    linearError = translation - _upperTranslation;
                    active = true;
                }
            }

            _perp = Math.Mul(R1, _localYAxis1);

            _s1 = Vec2.Cross(d + r1, _perp);
            _s2 = Vec2.Cross(r2, _perp);

            Vec2 impulse;
            float C1;
            C1 = Vec2.Dot(_perp, d);

            linearError = Math.Max(linearError, Math.Abs(C1));
            angularError = 0.0f;

            if (active)
            {
                float m1 = _invMass1, m2 = _invMass2;
                float i1 = _invI1, i2 = _invI2;

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
                float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
                float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;

                _K.Col1.Set(k11, k12);
                _K.Col2.Set(k12, k22);

                Vec2 C = new Vec2();
                C.X = C1;
                C.Y = C2;

                impulse = _K.Solve(-C);
            }
            else
            {
                float m1 = _invMass1, m2 = _invMass2;
                float i1 = _invI1, i2 = _invI2;

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;

                float impulse1;
                if (k11 != 0.0f)
                {
                    impulse1 = -C1 / k11;
                }
                else
                {
                    impulse1 = 0.0f;
                }

                impulse.X = impulse1;
                impulse.Y = 0.0f;
            }

            Vec2 P = impulse.X * _perp + impulse.Y * _axis;
            float L1 = impulse.X * _s1 + impulse.Y * _a1;
            float L2 = impulse.X * _s2 + impulse.Y * _a2;

            c1 -= _invMass1 * P;
            a1 -= _invI1 * L1;
            c2 += _invMass2 * P;
            a2 += _invI2 * L2;

            // TODO_ERIN remove need for this.
            b1._sweep.C = c1;
            b1._sweep.A = a1;
            b2._sweep.C = c2;
            b2._sweep.A = a2;
            b1.SynchronizeTransform();
            b2.SynchronizeTransform();

            return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
        }
Пример #28
0
		public static Mat22 operator +(Mat22 A, Mat22 B)
		{
			Mat22 C = new Mat22();
			C.Set(A.Col1 + B.Col1, A.Col2 + B.Col2);
			return C;
		}
Пример #29
0
 /// <summary>
 /// Compute the inverse of this matrix, such that inv(A) * A = identity.
 /// </summary>
 public Mat22 GetInverse()
 {
     float a = Col1.x, b = Col2.x, c = Col1.y, d = Col2.y;
     Mat22 B = new Mat22();
     float det = a * d - b * c;
     Box2DXDebug.Assert(det != 0.0f);
     det = 1.0f / det;
     B.Col1.x = det * d; B.Col2.x = -det * b;
     B.Col1.y = -det * c; B.Col2.y = det * a;
     return B;
 }
Пример #30
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b = _body2;

            float mass = b.GetMass();

            // Frequency
            float omega = 2.0f * Settings.Pi * _frequencyHz;

            // Damping coefficient
            float d = 2.0f * mass * _dampingRatio * omega;

            // Spring stiffness
            float k = mass * (omega * omega);

            // magic formulas
            // gamma has units of inverse mass.
            // beta has units of inverse time.
            Box2DXDebug.Assert(d + step.Dt * k > Settings.FLT_EPSILON);
            _gamma = 1.0f / (step.Dt * (d + step.Dt * k));
            _beta = step.Dt * k * _gamma;

            // Compute the effective mass matrix.
            Vector2 r = b.GetTransform().TransformDirection(_localAnchor - b.GetLocalCenter());

            // K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
            //      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
            //        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
            float invMass = b._invMass;
            float invI = b._invI;

            Mat22 K1 = new Mat22();
            K1.Col1.x = invMass; K1.Col2.x = 0.0f;
            K1.Col1.y = 0.0f; K1.Col2.y = invMass;

            Mat22 K2 = new Mat22();
            K2.Col1.x = invI * r.y * r.y; K2.Col2.x = -invI * r.x * r.y;
            K2.Col1.y = -invI * r.x * r.y; K2.Col2.y = invI * r.x * r.x;

            Mat22 K = K1 + K2;
            K.Col1.x += _gamma;
            K.Col2.y += _gamma;

            _mass = K.GetInverse();

            _C = b._sweep.C + r - _target;

            // Cheat with some damping
            b._angularVelocity *= 0.98f;

            // Warm starting.
            _impulse *= step.DtRatio;
            b._linearVelocity += invMass * _impulse;
            b._angularVelocity += invI * r.Cross(_impulse);
        }
Пример #31
0
		internal override void InitVelocityConstraints(TimeStep step)
		{
			Body b = _body2;

			// Compute the effective mass matrix.
			Vec2 r = Common.Math.Mul(b.GetXForm().R, _localAnchor - b.GetLocalCenter());

			// K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
			//      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
			//        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
			float invMass = b._invMass;
			float invI = b._invI;

			Mat22 K1 = new Mat22();
			K1.Col1.X = invMass; K1.Col2.X = 0.0f;
			K1.Col1.Y = 0.0f; K1.Col2.Y = invMass;

			Mat22 K2 = new Mat22();
			K2.Col1.X = invI * r.Y * r.Y; K2.Col2.X = -invI * r.X * r.Y;
			K2.Col1.Y = -invI * r.X * r.Y; K2.Col2.Y = invI * r.X * r.X;

			Mat22 K = K1 + K2;
			K.Col1.X += _gamma;
			K.Col2.Y += _gamma;

			_mass = K.Invert();

			_C = b._sweep.C + r - _target;

			// Cheat with some damping
			b._angularVelocity *= 0.98f;

			// Warm starting.
			Vec2 P = Settings.FORCE_SCALE(step.Dt) * _impulse;
			b._linearVelocity += invMass * P;
			b._angularVelocity += invI * Vec2.Cross(r, P);
		}