/// Initialize the bodies, anchors, axis, and reference angle using the world
		/// anchor and world axis.
		// Linear constraint (point-to-line)
		// d = pB - pA = xB + rB - xA - rA
		// C = dot(ay, d)
		// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
		//      = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
		// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]

		// Spring linear constraint
		// C = dot(ax, d)
		// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
		// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]

		// Motor rotational constraint
		// Cdot = wB - wA
		// J = [0 0 -1 0 0 1]
		public void Initialize(Body bA, Body bB, Vec2 anchor, Vec2 axis) {
			bodyA = bA;
			bodyB = bB;
			localAnchorA = bodyA.GetLocalPoint(anchor);
			localAnchorB = bodyB.GetLocalPoint(anchor);
			localAxisA = bodyA.GetLocalVector(axis);
		}
Exemple #2
0
		/// Initialize the bodies, anchors, and reference angle using a world
		/// anchor point.
		// Point-to-point constraint
		// C = p2 - p1
		// Cdot = v2 - v1
		//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
		// J = [-I -r1_skew I r2_skew ]
		// Identity used:
		// w k % (rx i + ry j) = w * (-ry i + rx j)

		// Angle constraint
		// C = angle2 - angle1 - referenceAngle
		// Cdot = w2 - w1
		// J = [0 0 -1 0 0 1]
		// K = invI1 + invI2
		public void Initialize(Body bA, Body bB, Vec2 anchor) {
			bodyA = bA;
			bodyB = bB;
			localAnchorA = bodyA.GetLocalPoint(anchor);
			localAnchorB = bodyB.GetLocalPoint(anchor);
			referenceAngle = bodyB.GetAngle() - bodyA.GetAngle();
		}
		/// Initialize the bodies, anchors, and length using the world
		/// anchors.
		// 1-D constrained system
		// m (v2 - v1) = lambda
		// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
		// x2 = x1 + h * v2

		// 1-D mass-damper-spring system
		// m (v2 - v1) + h * d * v2 + h * k * 

		// C = norm(p2 - p1) - L
		// u = (p2 - p1) / norm(p2 - p1)
		// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
		// J = [-u -cross(r1, u) u cross(r2, u)]
		// K = J * invM * JT
		//   = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
		public void Initialize(Body b1, Body b2,
						Vec2 anchor1, Vec2 anchor2) {
			bodyA = b1;
			bodyB = b2;
			localAnchorA = bodyA.GetLocalPoint(anchor1);
			localAnchorB = bodyB.GetLocalPoint(anchor2);
			Vec2 d = anchor2 - anchor1;
			length = d.Length();
		}
		/// Initialize the bodies and offsets using the current transforms.
		// Point-to-point constraint
		// Cdot = v2 - v1
		//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
		// J = [-I -r1_skew I r2_skew ]
		// Identity used:
		// w k % (rx i + ry j) = w * (-ry i + rx j)

		// Angle constraint
		// Cdot = w2 - w1
		// J = [0 0 -1 0 0 1]
		// K = invI1 + invI2
		public void Initialize(Body bA, Body bB) {
			bodyA = bA;
			bodyB = bB;
			Vec2 xB = bodyB.GetPosition();
			linearOffset = bodyA.GetLocalPoint(xB);

			float angleA = bodyA.GetAngle();
			float angleB = bodyB.GetAngle();
			angularOffset = angleB - angleA;
		}
Exemple #5
0
		/// Create a rigid body given a definition. No reference to the definition
		/// is retained.
		/// @warning This function is locked during callbacks.
		public Body CreateBody(BodyDef def){
			Utilities.Assert(IsLocked() == false);
			if (IsLocked())
			{
			    return null;
			}
			
			Body b = new Body(def, this);

			// Add to world doubly linked list.
			m_bodyList.Add(b);

			return b;
		}
Exemple #6
0
		protected Joint(JointDef def){
			Utilities.Assert(def.bodyA != def.bodyB);

			m_type = def.type;
			m_prev = null;
			m_next = null;
			m_bodyA = def.bodyA;
			m_bodyB = def.bodyB;
			m_index = 0;
			m_collideConnected = def.collideConnected;
			m_islandFlag = false;
			m_userData = def.userData;

			m_edgeA = new List<JointEdge>();
			m_edgeB = new List<JointEdge>();
		}
		/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
		// Pulley:
		// length1 = norm(p1 - s1)
		// length2 = norm(p2 - s2)
		// C0 = (length1 + ratio * length2)_initial
		// C = C0 - (length1 + ratio * length2)
		// u1 = (p1 - s1) / norm(p1 - s1)
		// u2 = (p2 - s2) / norm(p2 - s2)
		// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
		// J = -[u1 cross(r1, u1) ratio * u2  ratio * cross(r2, u2)]
		// K = J * invM * JT
		//   = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
		public void Initialize(Body bA, Body bB,
						Vec2 groundA, Vec2 groundB,
						Vec2 anchorA, Vec2 anchorB,
						float r) {
			bodyA = bA;
			bodyB = bB;
			groundAnchorA = groundA;
			groundAnchorB = groundB;
			localAnchorA = bodyA.GetLocalPoint(anchorA);
			localAnchorB = bodyB.GetLocalPoint(anchorB);
			Vec2 dA = anchorA - groundA;
			lengthA = dA.Length();
			Vec2 dB = anchorB - groundB;
			lengthB = dB.Length();
			ratio = r;
			Utilities.Assert(ratio > Single.Epsilon);
		}
Exemple #8
0
		public BulletTest()
		{
			{
				BodyDef bd = new BodyDef();
				bd.Position.Set(0.0f, 0.0f);
				Body body = m_world.CreateBody(bd);

				EdgeShape edge = new EdgeShape();

				edge.Set(new Vec2(-10.0f, 0.0f), new Vec2(10.0f, 0.0f));
				edge.Density = 0;
				body.CreateFixture(edge);

				PolygonShape shape = new PolygonShape();
				shape.SetAsBox(0.2f, 1.0f, new Vec2(0.5f, 1.0f), 0.0f);
				shape.Density = 0;
				body.CreateFixture(shape);
			}

			{
				BodyDef bd = new BodyDef();
				bd.type = BodyType._dynamicBody;
				bd.Position.Set(0.0f, 4.0f);

				PolygonShape box = new PolygonShape();
				box.SetAsBox(2.0f, 0.1f);
				box.Density = 100;

				m_body = m_world.CreateBody(bd);
				m_body.CreateFixture(box);

				box.SetAsBox(0.25f, 0.25f);

				//m_x = RandomFloat(-1.0f, 1.0f);
				m_x = 0.20352793f;
				bd.Position.Set(m_x, 10.0f);
				bd.bullet = true;

				m_bullet = m_world.CreateBody(bd);
				m_bullet.CreateFixture(box);

				m_bullet.SetLinearVelocity(new Vec2(0.0f, -50.0f));
			}
		}
		public Body AddNode(Body parent, Vec2 localAnchor, int depth, float offset, float a)
		{
			Vec2 h = new Vec2(0.0f, a);

			Vec2 p = parent.GetPosition() + localAnchor - h;

			BodyDef bodyDef = new BodyDef();
			bodyDef.type = BodyType._dynamicBody;
			bodyDef.Position = p;
			Body body = m_world.CreateBody(bodyDef);

			PolygonShape shape = new PolygonShape();
			shape.SetAsBox(0.25f * a, a);
			shape.Density = 20;
			body.CreateFixture(shape);

			if (depth == e_depth)
			{
				return body;
			}

			shape.SetAsBox(offset, 0.25f * a, new Vec2(0, -a), 0.0f);
			body.CreateFixture(shape);

			Vec2 a1 = new Vec2(offset, -a);
			Vec2 a2 = new Vec2(-offset, -a);
			Body body1 = AddNode(body, a1, depth + 1, 0.5f * offset, a);
			Body body2 = AddNode(body, a2, depth + 1, 0.5f * offset, a);

			RevoluteJointDef jointDef = new RevoluteJointDef();
			jointDef.bodyA = body;
			jointDef.localAnchorB = h;

			jointDef.localAnchorA = a1;
			jointDef.bodyB = body1;
			m_world.CreateJoint(jointDef);

			jointDef.localAnchorA = a2;
			jointDef.bodyB = body2;
			m_world.CreateJoint(jointDef);

			return body;
		}
Exemple #10
0
		// This is used to prevent connected bodies from colliding.
		// It may lie, depending on the collideConnected flag.
		internal bool ShouldCollide(Body other){
			// At least one body should be dynamic.
			if (m_type != BodyType._dynamicBody && other.m_type != BodyType._dynamicBody) {
				return false;
			}

			// Does a joint prevent collision?
			foreach(JointEdge jn in m_jointList){
				if (jn.other == other) {
					if (jn.joint.m_collideConnected == false) {
						return false;
					}
				}
			}

			return true;
		}
Exemple #11
0
		public Joint joint; //pointer			///< the joint

		public JointEdge(Joint j, Body body) {
			this.joint = j;
			this.other = body;
		}
Exemple #12
0
		public void Add(Body body)
		{
			body.m_islandIndex = m_bodies.Count();
			m_bodies.Add(body);
		}
Exemple #13
0
		internal Fixture(){
			m_userData = null;
			m_body = null;
			m_next = null;
			m_proxies = new List<FixtureProxy>();
			m_shape = null;
			m_Density = 0.0f;
		}
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body bA = _bodyA;
            Body bB = _bodyB;

            Vector2 vA = bA._linearVelocity;
            float   wA = bA._angularVelocity;
            Vector2 vB = bB._linearVelocity;
            float   wB = bB._angularVelocity;

            float mA = bA._invMass, mB = bB._invMass;
            float iA = bA._invI, iB = bB._invI;

            Transform xfA, xfB;

            bA.GetTransform(out xfA);
            bB.GetTransform(out xfB);

            Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
            Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());

            // Solve angular friction
            {
                float Cdot    = wB - wA;
                float impulse = -_angularMass * Cdot;

                float oldImpulse = _angularImpulse;
                float maxImpulse = step.dt * _maxTorque;
                _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
                impulse         = _angularImpulse - oldImpulse;

                wA -= iA * impulse;
                wB += iB * impulse;
            }

            // Solve linear friction
            {
                Vector2 Cdot = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);

                Vector2 impulse    = -MathUtils.Multiply(ref _linearMass, Cdot);
                Vector2 oldImpulse = _linearImpulse;
                _linearImpulse += impulse;

                float maxImpulse = step.dt * _maxForce;

                if (_linearImpulse.sqrMagnitude > maxImpulse * maxImpulse)
                {
                    _linearImpulse.Normalize();
                    _linearImpulse *= maxImpulse;
                }

                impulse = _linearImpulse - oldImpulse;

                vA -= mA * impulse;
                wA -= iA * MathUtils.Cross(rA, impulse);

                vB += mB * impulse;
                wB += iB * MathUtils.Cross(rB, impulse);
            }

            bA._linearVelocity  = vA;
            bA._angularVelocity = wA;
            bB._linearVelocity  = vB;
            bB._angularVelocity = wB;
        }
Exemple #15
0
		/// Destroy a rigid body given a definition. No reference to the definition
		/// is retained. This function is locked during callbacks.
		/// @warning This automatically deletes all associated shapes and joints.
		/// @warning This function is locked during callbacks.
		public void DestroyBody(Body body){
			throw new NotImplementedException();
			//Utilities.Assert(m_bodyList.Count() > 0);
			//Utilities.Assert(IsLocked() == false);
			//if (IsLocked())
			//{
			//    return;
			//}

			//// Delete the attached joints.
			//JointEdge* je = b.m_jointList;
			//while (je)
			//{
			//    JointEdge* je0 = je;
			//    je = je.next;

			//    if (m_destructionListener)
			//    {
			//        m_destructionListener.SayGoodbye(je0.joint);
			//    }

			//    DestroyJoint(je0.joint);

			//    b.m_jointList = je;
			//}
			//b.m_jointList = null;

			//// Delete the attached contacts.
			//ContactEdge* ce = b.m_contactList;
			//while (ce)
			//{
			//    ContactEdge* ce0 = ce;
			//    ce = ce.next;
			//    m_contactManager.Destroy(ce0.contact);
			//}
			//b.m_contactList = null;

			//// Delete the attached fixtures. This destroys broad-phase proxies.
			//Fixture* f = b.m_fixtureList;
			//while (f)
			//{
			//    Fixture* f0 = f;
			//    f = f.m_next;

			//    if (m_destructionListener)
			//    {
			//        m_destructionListener.SayGoodbye(f0);
			//    }

			//    f0.DestroyProxies(&m_contactManager.m_broadPhase);
			//    f0.Destroy(&m_blockAllocator);
			//    f0.~Fixture();
			//    m_blockAllocator.Free(f0, sizeof(Fixture));

			//    b.m_fixtureList = f;
			//    b.m_fixtureCount -= 1;
			//}
			//b.m_fixtureList = null;
			//b.m_fixtureCount = 0;

			//// Remove world body list.
			//if (b.m_prev)
			//{
			//    b.m_prev.m_next = b.m_next;
			//}

			//if (b.m_next)
			//{
			//    b.m_next.m_prev = b.m_prev;
			//}

			//if (b == m_bodyList)
			//{
			//    m_bodyList = b.m_next;
			//}

			//--m_bodyList.Count();
			//b.~Body();
			//m_blockAllocator.Free(b, sizeof(Body));
		}
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

            // Compute the effective mass matrix.
            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

            _u = b2._sweep.c + r2 - b1._sweep.c - r1;

            // Handle singularity.
            float length = _u.magnitude;

            if (length < _length)
            {
                return;
            }

            if (length > Settings.b2_linearSlop)
            {
                _u *= 1.0f / length;
            }
            else
            {
                _u = new Vector2(0.0f, 0.0f);
            }

            float cr1u    = MathUtils.Cross(r1, _u);
            float cr2u    = MathUtils.Cross(r2, _u);
            float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;

            //Debug.Assert(invMass > Settings.b2_epsilon);
            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

            if (_frequencyHz > 0.0f)
            {
                float C = length - _length;

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

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

                // Spring stiffness
                float k = _mass * omega * omega;

                // magic formulas
                _gamma = step.dt * (d + step.dt * k);
                _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
                _bias  = C * step.dt * k * _gamma;

                _mass = invMass + _gamma;
                _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
            }

            if (step.warmStarting)
            {
                // Scale the impulse to support a variable time step.
                _impulse *= step.dtRatio;

                Vector2 P = _impulse * _u;
                b1._linearVelocity  -= b1._invMass * P;
                b1._angularVelocity -= b1._invI * MathUtils.Cross(r1, P);
                b2._linearVelocity  += b2._invMass * P;
                b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P);
            }
            else
            {
                _impulse = 0.0f;
            }
        }
		public override void Step(TestSettings settings)
		{
			base.Step(settings);

			// We are going to destroy some bodies according to contact
			// points. We must buffer the bodies that should be destroyed
			// because they may belong to multiple contact points.
			const int k_maxNuke = 6;
			Body[] nuke = new Body[k_maxNuke];
			int nukeCount = 0;

			// Traverse the contact results. Destroy bodies that
			// are touching heavier bodies.
			for (int i = 0; i < m_pointCount; ++i)
			{
				ContactPoint point = m_points[i];

				Body body1 = point.fixtureA.GetBody();
				Body body2 = point.fixtureB.GetBody();
				float mass1 = body1.GetMass();
				float mass2 = body2.GetMass();

				if (mass1 > 0.0f && mass2 > 0.0f)
				{
					if (mass2 > mass1)
					{
						nuke[nukeCount++] = body1;
					}
					else
					{
						nuke[nukeCount++] = body2;
					}

					if (nukeCount == k_maxNuke)
					{
						break;
					}
				}
			}

			// Sort the nuke array to group duplicates.
			throw new NotImplementedException();
			//std::sort(nuke, nuke + nukeCount);

			// Destroy the bodies, skipping duplicates.
			int i2 = 0;
			while (i2 < nukeCount)
			{
				Body b = nuke[i2++];
				while (i2 < nukeCount && nuke[i2] == b)
				{
					++i2;
				}

				if (b != m_bomb)
				{
					m_world.DestroyBody(b);
				}
			}
		}
Exemple #18
0
		public ApplyForce() {
			m_world.SetGravity(new Vec2(0.0f, 0.0f));

			const float k_restitution = 0.4f;

			Body ground;
			{
				BodyDef bd = new BodyDef();
				bd.Position.Set(0.0f, 20.0f);
				ground = m_world.CreateBody(bd);

				EdgeShape shape = new EdgeShape();

				FixtureDef sd = new FixtureDef();
				sd.shape = shape;
				sd.Density = 0.0f;
				sd.restitution = k_restitution;

				// Left vertical
				shape.Set(new Vec2(-20.0f, -20.0f), new Vec2(-20.0f, 20.0f));
				ground.CreateFixture(sd);

				// Right vertical
				shape.Set(new Vec2(20.0f, -20.0f), new Vec2(20.0f, 20.0f));
				ground.CreateFixture(sd);

				// Top horizontal
				shape.Set(new Vec2(-20.0f, 20.0f), new Vec2(20.0f, 20.0f));
				ground.CreateFixture(sd);

				// Bottom horizontal
				shape.Set(new Vec2(-20.0f, -20.0f), new Vec2(20.0f, -20.0f));
				ground.CreateFixture(sd);
			}

			{
				Transform xf1 = new Transform();
				xf1.q.Set(0.3524f * (float)Math.PI);
				xf1.p = xf1.q.GetXAxis();

				Vec2[] vertices = new Vec2[3];
				vertices[0] = Utilities.Mul(xf1, new Vec2(-1.0f, 0.0f));
				vertices[1] = Utilities.Mul(xf1, new Vec2(1.0f, 0.0f));
				vertices[2] = Utilities.Mul(xf1, new Vec2(0.0f, 0.5f));

				PolygonShape poly1 = new PolygonShape();
				poly1.Set(vertices, 3);

				FixtureDef sd1 = new FixtureDef();
				sd1.shape = poly1;
				sd1.Density = 4.0f;

				Transform xf2 = new Transform();
				xf2.q.Set(-0.3524f * (float)Math.PI);
				xf2.p = -xf2.q.GetXAxis();

				vertices[0] = Utilities.Mul(xf2, new Vec2(-1.0f, 0.0f));
				vertices[1] = Utilities.Mul(xf2, new Vec2(1.0f, 0.0f));
				vertices[2] = Utilities.Mul(xf2, new Vec2(0.0f, 0.5f));

				PolygonShape poly2 = new PolygonShape();
				poly2.Set(vertices, 3);

				FixtureDef sd2 = new FixtureDef();
				sd2.shape = poly2;
				sd2.Density = 2.0f;

				BodyDef bd = new BodyDef();
				bd.type = BodyType._dynamicBody;
				bd.angularDamping = 5.0f;
				bd.linearDamping = 0.1f;

				bd.Position.Set(0.0f, 2.0f);
				bd.angle = (float)Math.PI;
				bd.allowSleep = false;
				m_body = m_world.CreateBody(bd);
				m_body.CreateFixture(sd1);
				m_body.CreateFixture(sd2);
			}

			{
				PolygonShape shape = new PolygonShape();
				shape.SetAsBox(0.5f, 0.5f);

				FixtureDef fd = new FixtureDef();
				fd.shape = shape;
				fd.Density = 1.0f;
				fd.friction = 0.3f;

				for (int i = 0; i < 10; ++i) {
					BodyDef bd = new BodyDef();
					bd.type = BodyType._dynamicBody;

					bd.Position.Set(0.0f, 5.0f + 1.54f * i);
					Body body = m_world.CreateBody(bd);

					body.CreateFixture(fd);

					float gravity = 10.0f;
					float I = body.GetInertia();
					float mass = body.GetMass();

					// For a circle: I = 0.5 * m * r * r ==> r = sqrt(2 * I / m)
					float radius = (float)Math.Sqrt(2.0f * I / mass);

					FrictionJointDef jd = new FrictionJointDef();
					jd.localAnchorA.SetZero();
					jd.localAnchorB.SetZero();
					jd.bodyA = ground;
					jd.bodyB = body;
					jd.collideConnected = true;
					jd.maxForce = mass * gravity;
					jd.maxTorque = mass * radius * gravity;

					m_world.CreateJoint(jd);
				}
			}
		}
Exemple #19
0
		// We need separation create/destroy functions from the constructor/destructor because
		// the destructor cannot access the allocator (no destructor arguments allowed by C++).
		internal void Create(Body body, FixtureDef def){
			m_userData = def.UserData;
			m_friction = def.friction;
			m_restitution = def.restitution;

			m_body = body;
			m_next = null;

			m_filter = def.Filter;

			m_isSensor = def.IsSensor;

			m_shape = def.shape.Clone();

			// Reserve proxy space
			int childCount = m_shape.GetChildCount();
			m_proxies = new List<FixtureProxy>();

			m_Density = def.Density;
		}
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body bA = _bodyA;
            Body bB = _bodyB;

            Transform xfA, xfB;

            bA.GetTransform(out xfA);
            bB.GetTransform(out xfB);

            // Compute the effective mass matrix.
            Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
            Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());

            // J = [-I -r1_skew I r2_skew]
            //     [ 0       -1 0       1]
            // r_skew = [-ry; rx]

            // Matlab
            // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
            //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
            //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]

            float mA = bA._invMass, mB = bB._invMass;
            float iA = bA._invI, iB = bB._invI;

            Mat22 K1 = new Mat22();

            K1.col1.x = mA + mB;    K1.col2.x = 0.0f;
            K1.col1.y = 0.0f;               K1.col2.y = mA + mB;

            Mat22 K2 = new Mat22();

            K2.col1.x = iA * rA.y * rA.y;  K2.col2.x = -iA * rA.x * rA.y;
            K2.col1.y = -iA * rA.x * rA.y;  K2.col2.y = iA * rA.x * rA.x;

            Mat22 K3 = new Mat22();

            K3.col1.x = iB * rB.y * rB.y;  K3.col2.x = -iB * rB.x * rB.y;
            K3.col1.y = -iB * rB.x * rB.y;  K3.col2.y = iB * rB.x * rB.x;

            Mat22 K12;

            Mat22.Add(ref K1, ref K2, out K12);
            Mat22 K;

            Mat22.Add(ref K12, ref K3, out K);
            _linearMass = K.GetInverse();

            _angularMass = iA + iB;
            if (_angularMass > 0.0f)
            {
                _angularMass = 1.0f / _angularMass;
            }

            if (step.warmStarting)
            {
                // Scale impulses to support a variable time step.
                _linearImpulse  *= step.dtRatio;
                _angularImpulse *= step.dtRatio;

                Vector2 P = new Vector2(_linearImpulse.x, _linearImpulse.y);

                bA._linearVelocity  -= mA * P;
                bA._angularVelocity -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);

                bB._linearVelocity  += mB * P;
                bB._angularVelocity += iB * (MathUtils.Cross(rB, P) + _angularImpulse);
            }
            else
            {
                _linearImpulse  = Vector2.zero;
                _angularImpulse = 0.0f;
            }
        }
Exemple #21
0
        // Perform one solver iteration. Returns true if converged.
        public bool Solve(float baumgarte)
        {
            float minSeparation = 0.0f;

            for (int i = 0; i < _count; ++i)
            {
                TOIConstraint c     = _constraints[i];
                Body          bodyA = c.bodyA;
                Body          bodyB = c.bodyB;

                float massA = bodyA._mass;
                float massB = bodyB._mass;

                // Only the TOI body should move.
                if (bodyA == _toiBody)
                {
                    massB = 0.0f;
                }
                else
                {
                    massA = 0.0f;
                }

                float invMassA = massA * bodyA._invMass;
                float invIA    = massA * bodyA._invI;
                float invMassB = massB * bodyB._invMass;
                float invIB    = massB * bodyB._invI;

                // Solve normal constraints
                for (int j = 0; j < c.pointCount; ++j)
                {
                    TOISolverManifold psm = new TOISolverManifold(ref c, j);

                    Vector2 normal     = psm.normal;
                    Vector2 point      = psm.point;
                    float   separation = psm.separation;

                    Vector2 rA = point - bodyA._sweep.c;
                    Vector2 rB = point - bodyB._sweep.c;

                    // Track max constraint error.
                    minSeparation = Math.Min(minSeparation, separation);

                    // Prevent large corrections and allow slop.
                    float C = MathUtils.Clamp(baumgarte * (separation + Settings.b2_linearSlop), -Settings.b2_maxLinearCorrection, 0.0f);

                    // Compute the effective mass.
                    float rnA = MathUtils.Cross(rA, normal);
                    float rnB = MathUtils.Cross(rB, normal);
                    float K   = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;

                    // Compute normal impulse
                    float impulse = K > 0.0f ? -C / K : 0.0f;

                    Vector2 P = impulse * normal;

                    bodyA._sweep.c -= invMassA * P;
                    bodyA._sweep.a -= invIA * MathUtils.Cross(rA, P);
                    bodyA.SynchronizeTransform();

                    bodyB._sweep.c += invMassB * P;
                    bodyB._sweep.a += invIB * MathUtils.Cross(rB, P);
                    bodyB.SynchronizeTransform();
                }
            }

            // We can't expect minSpeparation >= -b2_linearSlop because we don't
            // push the separation above -b2_linearSlop.
            return(minSeparation >= -1.5f * Settings.b2_linearSlop);
        }