Esempio n. 1
0
		internal override void InitVelocityConstraints(SolverData data){
			m_indexA = m_bodyA.m_islandIndex;
			m_indexB = m_bodyB.m_islandIndex;
			m_localCenterA = m_bodyA.m_sweep.localCenter;
			m_localCenterB = m_bodyB.m_sweep.localCenter;
			m_invMassA = m_bodyA.m_invMass;
			m_invMassB = m_bodyB.m_invMass;
			m_invIA = m_bodyA.m_invI;
			m_invIB = m_bodyB.m_invI;

			float aA = data.positions[m_indexA].a;
			Vec2 vA = data.velocities[m_indexA].v;
			float wA = data.velocities[m_indexA].w;

			float aB = data.positions[m_indexB].a;
			Vec2 vB = data.velocities[m_indexB].v;
			float wB = data.velocities[m_indexB].w;

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

			m_rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
			m_rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);

			// 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 = m_invMassA, mB = m_invMassB;
			float iA = m_invIA, iB = m_invIB;

			Mat33 K;
			K.ex.X = mA + mB + m_rA.Y * m_rA.Y * iA + m_rB.Y * m_rB.Y * iB;
			K.ey.X = -m_rA.Y * m_rA.X * iA - m_rB.Y * m_rB.X * iB;
			K.ez.X = -m_rA.Y * iA - m_rB.Y * iB;
			K.ex.Y = K.ey.X;
			K.ey.Y = mA + mB + m_rA.X * m_rA.X * iA + m_rB.X * m_rB.X * iB;
			K.ez.Y = m_rA.X * iA + m_rB.X * iB;
			K.ex.Z = K.ez.X;
			K.ey.Z = K.ez.Y;
			K.ez.Z = iA + iB;

			if (m_frequencyHz > 0.0f)
			{
				K.GetInverse22(out m_mass);

				float invM = iA + iB;
				float m = invM > 0.0f ? 1.0f / invM : 0.0f;

				float C = aB - aA - m_referenceAngle;

				// Frequency
				float omega = 2.0f * (float)Math.PI * m_frequencyHz;

				// Damping coefficient
				float d = 2.0f * m * m_dampingRatio * omega;

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

				// magic formulas
				float h = data.step.dt;
				m_gamma = h * (d + h * k);
				m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
				m_bias = C * h * k * m_gamma;

				invM += m_gamma;
				m_mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f;
			}
			else
			{
				K.GetSymInverse33(out m_mass);
				m_gamma = 0.0f;
				m_bias = 0.0f;
			}

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

				Vec2 P = new Vec2(m_impulse.X, m_impulse.Y);

				vA -= mA * P;
				wA -= iA * (Utilities.Cross(m_rA, P) + m_impulse.Z);

				vB += mB * P;
				wB += iB * (Utilities.Cross(m_rB, P) + m_impulse.Z);
			}
			else
			{
				m_impulse.SetZero();
			}

			data.velocities[m_indexA].v = vA;
			data.velocities[m_indexA].w = wA;
			data.velocities[m_indexB].v = vB;
			data.velocities[m_indexB].w = wB;
		}
Esempio n. 2
0
		internal override void InitVelocityConstraints(SolverData data){
			m_indexA = m_bodyA.m_islandIndex;
			m_indexB = m_bodyB.m_islandIndex;
			m_localCenterA = m_bodyA.m_sweep.localCenter;
			m_localCenterB = m_bodyB.m_sweep.localCenter;
			m_invMassA = m_bodyA.m_invMass;
			m_invMassB = m_bodyB.m_invMass;
			m_invIA = m_bodyA.m_invI;
			m_invIB = m_bodyB.m_invI;

			Vec2 cA = data.positions[m_indexA].c;
			float aA = data.positions[m_indexA].a;
			Vec2 vA = data.velocities[m_indexA].v;
			float wA = data.velocities[m_indexA].w;

			Vec2 cB = data.positions[m_indexB].c;
			float aB = data.positions[m_indexB].a;
			Vec2 vB = data.velocities[m_indexB].v;
			float wB = data.velocities[m_indexB].w;

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

			m_rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
			m_rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);

			// Get the pulley axes.
			m_uA = cA + m_rA - m_groundAnchorA;
			m_uB = cB + m_rB - m_groundAnchorB;

			float lengthA = m_uA.Length();
			float lengthB = m_uB.Length();

			if (lengthA > 10.0f *Settings._linearSlop)
			{
				m_uA *= 1.0f / lengthA;
			}
			else
			{
				m_uA.SetZero();
			}

			if (lengthB > 10.0f *Settings._linearSlop)
			{
				m_uB *= 1.0f / lengthB;
			}
			else
			{
				m_uB.SetZero();
			}

			// Compute effective mass.
			float ruA = Utilities.Cross(m_rA, m_uA);
			float ruB = Utilities.Cross(m_rB, m_uB);

			float mA = m_invMassA + m_invIA * ruA * ruA;
			float mB = m_invMassB + m_invIB * ruB * ruB;

			m_mass = mA + m_ratio * m_ratio * mB;

			if (m_mass > 0.0f)
			{
				m_mass = 1.0f / m_mass;
			}

			if (data.step.warmStarting)
			{
				// Scale impulses to support variable time steps.
				m_impulse *= data.step.dtRatio;

				// Warm starting.
				Vec2 PA = -(m_impulse) * m_uA;
				Vec2 PB = (-m_ratio * m_impulse) * m_uB;

				vA += m_invMassA * PA;
				wA += m_invIA * Utilities.Cross(m_rA, PA);
				vB += m_invMassB * PB;
				wB += m_invIB * Utilities.Cross(m_rB, PB);
			}
			else
			{
				m_impulse = 0.0f;
			}

			data.velocities[m_indexA].v = vA;
			data.velocities[m_indexA].w = wA;
			data.velocities[m_indexB].v = vB;
			data.velocities[m_indexB].w = wB;
		}
Esempio n. 3
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);

			Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
			Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
			Vec2 u = cB + rB - cA - rA;

			float length = u.Normalize();
			float C = length - m_maxLength;

			C = Utilities.Clamp(C, 0.0f, Settings._maxLinearCorrection);

			float impulse = -m_mass * C;
			Vec2 P = impulse * u;

			cA -= m_invMassA * P;
			aA -= m_invIA * Utilities.Cross(rA, P);
			cB += m_invMassB * P;
			aB += m_invIB * Utilities.Cross(rB, P);

			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 length - m_maxLength <Settings._linearSlop;
		}
Esempio n. 4
0
		internal override void InitVelocityConstraints(SolverData data){
			m_indexA = m_bodyA.m_islandIndex;
			m_indexB = m_bodyB.m_islandIndex;
			m_localCenterA = m_bodyA.m_sweep.localCenter;
			m_localCenterB = m_bodyB.m_sweep.localCenter;
			m_invMassA = m_bodyA.m_invMass;
			m_invMassB = m_bodyB.m_invMass;
			m_invIA = m_bodyA.m_invI;
			m_invIB = m_bodyB.m_invI;

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

			Vec2 cA = data.positions[m_indexA].c;
			float aA = data.positions[m_indexA].a;
			Vec2 vA = data.velocities[m_indexA].v;
			float wA = data.velocities[m_indexA].w;

			Vec2 cB = data.positions[m_indexB].c;
			float aB = data.positions[m_indexB].a;
			Vec2 vB = data.velocities[m_indexB].v;
			float wB = data.velocities[m_indexB].w;

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

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

			// Point to line constraint
			{
				m_ay = Utilities.Mul(qA, m_localYAxisA);
				m_sAy = Utilities.Cross(d + rA, m_ay);
				m_sBy = Utilities.Cross(rB, m_ay);

				m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy;

				if (m_mass > 0.0f)
				{
					m_mass = 1.0f / m_mass;
				}
			}

			// Spring constraint
			m_springMass = 0.0f;
			m_bias = 0.0f;
			m_gamma = 0.0f;
			if (m_frequencyHz > 0.0f)
			{
				m_ax = Utilities.Mul(qA, m_localXAxisA);
				m_sAx = Utilities.Cross(d + rA, m_ax);
				m_sBx = Utilities.Cross(rB, m_ax);

				float invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx;

				if (invMass > 0.0f)
				{
					m_springMass = 1.0f / invMass;

					float C = Utilities.Dot(d, m_ax);

					// Frequency
					float omega = 2.0f * (float)Math.PI * m_frequencyHz;

					// Damping coefficient
					float df = 2.0f * m_springMass * m_dampingRatio * omega;

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

					// magic formulas
					float h = data.step.dt;
					m_gamma = h * (df + h * k);
					if (m_gamma > 0.0f)
					{
						m_gamma = 1.0f / m_gamma;
					}

					m_bias = C * h * k * m_gamma;

					m_springMass = invMass + m_gamma;
					if (m_springMass > 0.0f)
					{
						m_springMass = 1.0f / m_springMass;
					}
				}
			}
			else
			{
				m_springImpulse = 0.0f;
			}

			// Rotational motor
			if (m_enableMotor)
			{
				m_motorMass = iA + iB;
				if (m_motorMass > 0.0f)
				{
					m_motorMass = 1.0f / m_motorMass;
				}
			}
			else
			{
				m_motorMass = 0.0f;
				m_motorImpulse = 0.0f;
			}

			if (data.step.warmStarting)
			{
				// Account for variable time step.
				m_impulse *= data.step.dtRatio;
				m_springImpulse *= data.step.dtRatio;
				m_motorImpulse *= data.step.dtRatio;

				Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax;
				float LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse;
				float LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse;

				vA -= m_invMassA * P;
				wA -= m_invIA * LA;

				vB += m_invMassB * P;
				wB += m_invIB * LB;
			}
			else
			{
				m_impulse = 0.0f;
				m_springImpulse = 0.0f;
				m_motorImpulse = 0.0f;
			}

			data.velocities[m_indexA].v = vA;
			data.velocities[m_indexA].w = wA;
			data.velocities[m_indexB].v = vB;
			data.velocities[m_indexB].w = wB;
		}
Esempio n. 5
0
		internal override void InitVelocityConstraints(SolverData data){
			m_indexA = m_bodyA.m_islandIndex;
			m_indexB = m_bodyB.m_islandIndex;
			m_localCenterA = m_bodyA.m_sweep.localCenter;
			m_localCenterB = m_bodyB.m_sweep.localCenter;
			m_invMassA = m_bodyA.m_invMass;
			m_invMassB = m_bodyB.m_invMass;
			m_invIA = m_bodyA.m_invI;
			m_invIB = m_bodyB.m_invI;

			Vec2 cA = data.positions[m_indexA].c;
			float aA = data.positions[m_indexA].a;
			Vec2 vA = data.velocities[m_indexA].v;
			float wA = data.velocities[m_indexA].w;

			Vec2 cB = data.positions[m_indexB].c;
			float aB = data.positions[m_indexB].a;
			Vec2 vB = data.velocities[m_indexB].v;
			float wB = data.velocities[m_indexB].w;

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

			m_rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
			m_rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
			m_u = cB + m_rB - cA - m_rA;

			// Handle singularity.
			float length = m_u.Length();
			if (length >Settings._linearSlop)
			{
				m_u *= 1.0f / length;
			}
			else
			{
				m_u.Set(0.0f, 0.0f);
			}

			float crAu = Utilities.Cross(m_rA, m_u);
			float crBu = Utilities.Cross(m_rB, m_u);
			float invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;

			// Compute the effective mass matrix.
			m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

			if (m_frequencyHz > 0.0f)
			{
				float C = length - m_length;

				// Frequency
				float omega = 2.0f * (float)Math.PI * m_frequencyHz;

				// Damping coefficient
				float d = 2.0f * m_mass * m_dampingRatio * omega;

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

				// magic formulas
				float h = data.step.dt;
				m_gamma = h * (d + h * k);
				m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
				m_bias = C * h * k * m_gamma;

				invMass += m_gamma;
				m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
			}
			else
			{
				m_gamma = 0.0f;
				m_bias = 0.0f;
			}

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

				Vec2 P = m_impulse * m_u;
				vA -= m_invMassA * P;
				wA -= m_invIA * Utilities.Cross(m_rA, P);
				vB += m_invMassB * P;
				wB += m_invIB * Utilities.Cross(m_rB, P);
			}
			else
			{
				m_impulse = 0.0f;
			}

			data.velocities[m_indexA].v = vA;
			data.velocities[m_indexA].w = wA;
			data.velocities[m_indexB].v = vB;
			data.velocities[m_indexB].w = wB;
		}
Esempio n. 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 angularError = 0.0f;
			float positionError = 0.0f;

			bool fixedRotation = (m_invIA + m_invIB == 0.0f);

			// Solve angular limit constraint.
			if (m_enableLimit && m_limitState != LimitState.e_inactiveLimit && fixedRotation == false)
			{
				float angle = aB - aA - m_referenceAngle;
				float limitImpulse = 0.0f;

				if (m_limitState ==LimitState.e_equalLimits)
				{
					// Prevent large angular corrections
					float C = Utilities.Clamp(angle - m_lowerAngle, -Settings._maxAngularCorrection, Settings._maxAngularCorrection);
					limitImpulse = -m_motorMass * C;
					angularError = Math.Abs(C);
				}
				else if (m_limitState == LimitState.e_atLowerLimit)
				{
					float C = angle - m_lowerAngle;
					angularError = -C;

					// Prevent large angular corrections and allow some slop.
					C = Utilities.Clamp(C + Settings._angularSlop, -Settings._maxAngularCorrection, 0.0f);
					limitImpulse = -m_motorMass * C;
				}
				else if (m_limitState == LimitState.e_atUpperLimit)
				{
					float C = angle - m_upperAngle;
					angularError = C;

					// Prevent large angular corrections and allow some slop.
					C = Utilities.Clamp(C - Settings._angularSlop, 0.0f, Settings._maxAngularCorrection);
					limitImpulse = -m_motorMass * C;
				}

				aA -= m_invIA * limitImpulse;
				aB += m_invIB * limitImpulse;
			}

			// Solve point-to-point constraint.
			{
				qA.Set(aA);
				qB.Set(aB);
				Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
				Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);

				Vec2 C = cB + rB - cA - rA;
				positionError = C.Length();

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

				Mat22 K;
				K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y;
				K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y;
				K.ey.X = K.ex.Y;
				K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X;

				Vec2 impulse = -K.Solve(C);

				cA -= mA * impulse;
				aA -= iA * Utilities.Cross(rA, impulse);

				cB += mB * impulse;
				aB += iB * Utilities.Cross(rB, impulse);
			}

			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 positionError <=Settings._linearSlop && angularError <= Settings._angularSlop;
		}
Esempio n. 7
0
		internal override void InitVelocityConstraints(SolverData data) {
			m_indexA = m_bodyA.m_islandIndex;
			m_indexB = m_bodyB.m_islandIndex;
			m_localCenterA = m_bodyA.m_sweep.localCenter;
			m_localCenterB = m_bodyB.m_sweep.localCenter;
			m_invMassA = m_bodyA.m_invMass;
			m_invMassB = m_bodyB.m_invMass;
			m_invIA = m_bodyA.m_invI;
			m_invIB = m_bodyB.m_invI;

			Vec2 cA = data.positions[m_indexA].c;
			float aA = data.positions[m_indexA].a;
			Vec2 vA = data.velocities[m_indexA].v;
			float wA = data.velocities[m_indexA].w;

			Vec2 cB = data.positions[m_indexB].c;
			float aB = data.positions[m_indexB].a;
			Vec2 vB = data.velocities[m_indexB].v;
			float wB = data.velocities[m_indexB].w;

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

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

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

			// Compute motor Jacobian and effective mass.
			{
				m_axis = Utilities.Mul(qA, m_localXAxisA);
				m_a1 = Utilities.Cross(d + rA, m_axis);
				m_a2 = Utilities.Cross(rB, m_axis);

				m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
				if (m_motorMass > 0.0f)
				{
					m_motorMass = 1.0f / m_motorMass;
				}
			}

			// Prismatic constraint.
			{
				m_perp = Utilities.Mul(qA, m_localYAxisA);

				m_s1 = Utilities.Cross(d + rA, m_perp);
				m_s2 = Utilities.Cross(rB, m_perp);

				float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
				float k12 = iA * m_s1 + iB * m_s2;
				float k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
				float k22 = iA + iB;
				if (k22 == 0.0f)
				{
					// For bodies with fixed rotation.
					k22 = 1.0f;
				}
				float k23 = iA * m_a1 + iB * m_a2;
				float k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;

				m_K.ex.Set(k11, k12, k13);
				m_K.ey.Set(k12, k22, k23);
				m_K.ez.Set(k13, k23, k33);
			}

			// Compute motor and limit terms.
			if (m_enableLimit)
			{
				float jointTranslation = Utilities.Dot(m_axis, d);
				if (Math.Abs(m_upperTranslation - m_lowerTranslation) < 2.0f *Settings._linearSlop)
				{
					m_limitState = LimitState.e_equalLimits;
				}
				else if (jointTranslation <= m_lowerTranslation)
				{
					if (m_limitState != LimitState.e_atLowerLimit)
					{
						m_limitState = LimitState.e_atLowerLimit;
						m_impulse.Z = 0.0f;
					}
				}
				else if (jointTranslation >= m_upperTranslation)
				{
					if (m_limitState != LimitState.e_atUpperLimit)
					{
						m_limitState = LimitState.e_atUpperLimit;
						m_impulse.Z = 0.0f;
					}
				}
				else
				{
					m_limitState = LimitState.e_inactiveLimit;
					m_impulse.Z = 0.0f;
				}
			}
			else
			{
				m_limitState = LimitState.e_inactiveLimit;
				m_impulse.Z = 0.0f;
			}

			if (m_enableMotor == false)
			{
				m_motorImpulse = 0.0f;
			}

			if (data.step.warmStarting)
			{
				// Account for variable time step.
				m_impulse *= data.step.dtRatio;
				m_motorImpulse *= data.step.dtRatio;

				Vec2 P = m_impulse.X * m_perp + (m_motorImpulse + m_impulse.Z) * m_axis;
				float LA = m_impulse.X * m_s1 + m_impulse.Y + (m_motorImpulse + m_impulse.Z) * m_a1;
				float LB = m_impulse.X * m_s2 + m_impulse.Y + (m_motorImpulse + m_impulse.Z) * m_a2;

				vA -= mA * P;
				wA -= iA * LA;

				vB += mB * P;
				wB += iB * LB;
			}
			else
			{
				m_impulse.SetZero();
				m_motorImpulse = 0.0f;
			}

			data.velocities[m_indexA].v = vA;
			data.velocities[m_indexA].w = wA;
			data.velocities[m_indexB].v = vB;
			data.velocities[m_indexB].w = wB;
		}
Esempio n. 8
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;
			Vec2 cC = data.positions[m_indexC].c;
			float aC = data.positions[m_indexC].a;
			Vec2 cD = data.positions[m_indexD].c;
			float aD = data.positions[m_indexD].a;

			Rot qA = new Rot(aA);
			Rot qB = new Rot(aB);
			Rot qC= new Rot(aC);
			Rot qD = new Rot(aD);

			float linearError = 0.0f;

			float coordinateA, coordinateB;

			Vec2 JvAC = new Vec2();
			Vec2 JvBD = new Vec2();
			float JwA, JwB, JwC, JwD;
			float mass = 0.0f;

			if (m_typeA == JointType.e_revoluteJoint)
			{
				JvAC.SetZero();
				JwA = 1.0f;
				JwC = 1.0f;
				mass += m_iA + m_iC;

				coordinateA = aA - aC - m_referenceAngleA;
			}
			else
			{
				Vec2 u = Utilities.Mul(qC, m_localAxisC);
				Vec2 rC = Utilities.Mul(qC, m_localAnchorC - m_lcC);
				Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_lcA);
				JvAC = u;
				JwC = Utilities.Cross(rC, u);
				JwA = Utilities.Cross(rA, u);
				mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;

				Vec2 pC = m_localAnchorC - m_lcC;
				Vec2 pA = Utilities.MulT(qC, rA + (cA - cC));
				coordinateA = Utilities.Dot(pA - pC, m_localAxisC);
			}

			if (m_typeB == JointType.e_revoluteJoint)
			{
				JvBD.SetZero();
				JwB = m_ratio;
				JwD = m_ratio;
				mass += m_ratio * m_ratio * (m_iB + m_iD);

				coordinateB = aB - aD - m_referenceAngleB;
			}
			else
			{
				Vec2 u = Utilities.Mul(qD, m_localAxisD);
				Vec2 rD = Utilities.Mul(qD, m_localAnchorD - m_lcD);
				Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_lcB);
				JvBD = m_ratio * u;
				JwD = m_ratio * Utilities.Cross(rD, u);
				JwB = m_ratio * Utilities.Cross(rB, u);
				mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;

				Vec2 pD = m_localAnchorD - m_lcD;
				Vec2 pB = Utilities.MulT(qD, rB + (cB - cD));
				coordinateB = Utilities.Dot(pB - pD, m_localAxisD);
			}

			float C = (coordinateA + m_ratio * coordinateB) - m_constant;

			float impulse = 0.0f;
			if (mass > 0.0f)
			{
				impulse = -C / mass;
			}

			cA += m_mA * impulse * JvAC;
			aA += m_iA * impulse * JwA;
			cB += m_mB * impulse * JvBD;
			aB += m_iB * impulse * JwB;
			cC -= m_mC * impulse * JvAC;
			aC -= m_iC * impulse * JwC;
			cD -= m_mD * impulse * JvBD;
			aD -= m_iD * impulse * JwD;

			data.positions[m_indexA].c = cA;
			data.positions[m_indexA].a = aA;
			data.positions[m_indexB].c = cB;
			data.positions[m_indexB].a = aB;
			data.positions[m_indexC].c = cC;
			data.positions[m_indexC].a = aC;
			data.positions[m_indexD].c = cD;
			data.positions[m_indexD].a = aD;

			// TODO_ERIN not implemented
			return linearError <Settings._linearSlop;
		}
Esempio n. 9
0
        internal override void InitVelocityConstraints(SolverData data)
        {
            m_indexA = m_bodyA.m_islandIndex;
            m_indexB = m_bodyB.m_islandIndex;
            m_indexC = m_bodyC.m_islandIndex;
            m_indexD = m_bodyD.m_islandIndex;
            m_lcA    = m_bodyA.m_sweep.localCenter;
            m_lcB    = m_bodyB.m_sweep.localCenter;
            m_lcC    = m_bodyC.m_sweep.localCenter;
            m_lcD    = m_bodyD.m_sweep.localCenter;
            m_mA     = m_bodyA.m_invMass;
            m_mB     = m_bodyB.m_invMass;
            m_mC     = m_bodyC.m_invMass;
            m_mD     = m_bodyD.m_invMass;
            m_iA     = m_bodyA.m_invI;
            m_iB     = m_bodyB.m_invI;
            m_iC     = m_bodyC.m_invI;
            m_iD     = m_bodyD.m_invI;

            float aA = data.positions[m_indexA].a;
            Vec2  vA = data.velocities[m_indexA].v;
            float wA = data.velocities[m_indexA].w;

            float aB = data.positions[m_indexB].a;
            Vec2  vB = data.velocities[m_indexB].v;
            float wB = data.velocities[m_indexB].w;

            float aC = data.positions[m_indexC].a;
            Vec2  vC = data.velocities[m_indexC].v;
            float wC = data.velocities[m_indexC].w;

            float aD = data.positions[m_indexD].a;
            Vec2  vD = data.velocities[m_indexD].v;
            float wD = data.velocities[m_indexD].w;

            Rot qA = new Rot(aA);
            Rot qB = new Rot(aB);
            Rot qC = new Rot(aC);
            Rot qD = new Rot(aD);

            m_mass = 0.0f;

            if (m_typeA == JointType.e_revoluteJoint)
            {
                m_JvAC.SetZero();
                m_JwA   = 1.0f;
                m_JwC   = 1.0f;
                m_mass += m_iA + m_iC;
            }
            else
            {
                Vec2 u  = Utilities.Mul(qC, m_localAxisC);
                Vec2 rC = Utilities.Mul(qC, m_localAnchorC - m_lcC);
                Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_lcA);
                m_JvAC  = u;
                m_JwC   = Utilities.Cross(rC, u);
                m_JwA   = Utilities.Cross(rA, u);
                m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA;
            }

            if (m_typeB == JointType.e_revoluteJoint)
            {
                m_JvBD.SetZero();
                m_JwB   = m_ratio;
                m_JwD   = m_ratio;
                m_mass += m_ratio * m_ratio * (m_iB + m_iD);
            }
            else
            {
                Vec2 u  = Utilities.Mul(qD, m_localAxisD);
                Vec2 rD = Utilities.Mul(qD, m_localAnchorD - m_lcD);
                Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_lcB);
                m_JvBD  = m_ratio * u;
                m_JwD   = m_ratio * Utilities.Cross(rD, u);
                m_JwB   = m_ratio * Utilities.Cross(rB, u);
                m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB;
            }

            // Compute effective mass.
            m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f;

            if (data.step.warmStarting)
            {
                vA += (m_mA * m_impulse) * m_JvAC;
                wA += m_iA * m_impulse * m_JwA;
                vB += (m_mB * m_impulse) * m_JvBD;
                wB += m_iB * m_impulse * m_JwB;
                vC -= (m_mC * m_impulse) * m_JvAC;
                wC -= m_iC * m_impulse * m_JwC;
                vD -= (m_mD * m_impulse) * m_JvBD;
                wD -= m_iD * m_impulse * m_JwD;
            }
            else
            {
                m_impulse = 0.0f;
            }

            data.velocities[m_indexA].v = vA;
            data.velocities[m_indexA].w = wA;
            data.velocities[m_indexB].v = vB;
            data.velocities[m_indexB].w = wB;
            data.velocities[m_indexC].v = vC;
            data.velocities[m_indexC].w = wC;
            data.velocities[m_indexD].v = vD;
            data.velocities[m_indexD].w = wD;
        }
Esempio n. 10
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;
            Vec2  cC = data.positions[m_indexC].c;
            float aC = data.positions[m_indexC].a;
            Vec2  cD = data.positions[m_indexD].c;
            float aD = data.positions[m_indexD].a;

            Rot qA = new Rot(aA);
            Rot qB = new Rot(aB);
            Rot qC = new Rot(aC);
            Rot qD = new Rot(aD);

            float linearError = 0.0f;

            float coordinateA, coordinateB;

            Vec2  JvAC = new Vec2();
            Vec2  JvBD = new Vec2();
            float JwA, JwB, JwC, JwD;
            float mass = 0.0f;

            if (m_typeA == JointType.e_revoluteJoint)
            {
                JvAC.SetZero();
                JwA   = 1.0f;
                JwC   = 1.0f;
                mass += m_iA + m_iC;

                coordinateA = aA - aC - m_referenceAngleA;
            }
            else
            {
                Vec2 u  = Utilities.Mul(qC, m_localAxisC);
                Vec2 rC = Utilities.Mul(qC, m_localAnchorC - m_lcC);
                Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_lcA);
                JvAC  = u;
                JwC   = Utilities.Cross(rC, u);
                JwA   = Utilities.Cross(rA, u);
                mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;

                Vec2 pC = m_localAnchorC - m_lcC;
                Vec2 pA = Utilities.MulT(qC, rA + (cA - cC));
                coordinateA = Utilities.Dot(pA - pC, m_localAxisC);
            }

            if (m_typeB == JointType.e_revoluteJoint)
            {
                JvBD.SetZero();
                JwB   = m_ratio;
                JwD   = m_ratio;
                mass += m_ratio * m_ratio * (m_iB + m_iD);

                coordinateB = aB - aD - m_referenceAngleB;
            }
            else
            {
                Vec2 u  = Utilities.Mul(qD, m_localAxisD);
                Vec2 rD = Utilities.Mul(qD, m_localAnchorD - m_lcD);
                Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_lcB);
                JvBD  = m_ratio * u;
                JwD   = m_ratio * Utilities.Cross(rD, u);
                JwB   = m_ratio * Utilities.Cross(rB, u);
                mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;

                Vec2 pD = m_localAnchorD - m_lcD;
                Vec2 pB = Utilities.MulT(qD, rB + (cB - cD));
                coordinateB = Utilities.Dot(pB - pD, m_localAxisD);
            }

            float C = (coordinateA + m_ratio * coordinateB) - m_constant;

            float impulse = 0.0f;

            if (mass > 0.0f)
            {
                impulse = -C / mass;
            }

            cA += m_mA * impulse * JvAC;
            aA += m_iA * impulse * JwA;
            cB += m_mB * impulse * JvBD;
            aB += m_iB * impulse * JwB;
            cC -= m_mC * impulse * JvAC;
            aC -= m_iC * impulse * JwC;
            cD -= m_mD * impulse * JvBD;
            aD -= m_iD * impulse * JwD;

            data.positions[m_indexA].c = cA;
            data.positions[m_indexA].a = aA;
            data.positions[m_indexB].c = cB;
            data.positions[m_indexB].a = aB;
            data.positions[m_indexC].c = cC;
            data.positions[m_indexC].a = aC;
            data.positions[m_indexD].c = cD;
            data.positions[m_indexD].a = aD;

            // TODO_ERIN not implemented
            return(linearError < Settings._linearSlop);
        }
Esempio n. 11
0
        internal override void InitVelocityConstraints(SolverData data)
        {
            m_indexA       = m_bodyA.m_islandIndex;
            m_indexB       = m_bodyB.m_islandIndex;
            m_localCenterA = m_bodyA.m_sweep.localCenter;
            m_localCenterB = m_bodyB.m_sweep.localCenter;
            m_invMassA     = m_bodyA.m_invMass;
            m_invMassB     = m_bodyB.m_invMass;
            m_invIA        = m_bodyA.m_invI;
            m_invIB        = m_bodyB.m_invI;

            Vec2  cA = data.positions[m_indexA].c;
            float aA = data.positions[m_indexA].a;
            Vec2  vA = data.velocities[m_indexA].v;
            float wA = data.velocities[m_indexA].w;

            Vec2  cB = data.positions[m_indexB].c;
            float aB = data.positions[m_indexB].a;
            Vec2  vB = data.velocities[m_indexB].v;
            float wB = data.velocities[m_indexB].w;

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

            m_rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
            m_rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
            m_u  = cB + m_rB - cA - m_rA;

            // Handle singularity.
            float length = m_u.Length();

            if (length > Settings._linearSlop)
            {
                m_u *= 1.0f / length;
            }
            else
            {
                m_u.Set(0.0f, 0.0f);
            }

            float crAu    = Utilities.Cross(m_rA, m_u);
            float crBu    = Utilities.Cross(m_rB, m_u);
            float invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;

            // Compute the effective mass matrix.
            m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

            if (m_frequencyHz > 0.0f)
            {
                float C = length - m_length;

                // Frequency
                float omega = 2.0f * (float)Math.PI * m_frequencyHz;

                // Damping coefficient
                float d = 2.0f * m_mass * m_dampingRatio * omega;

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

                // magic formulas
                float h = data.step.dt;
                m_gamma = h * (d + h * k);
                m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
                m_bias  = C * h * k * m_gamma;

                invMass += m_gamma;
                m_mass   = invMass != 0.0f ? 1.0f / invMass : 0.0f;
            }
            else
            {
                m_gamma = 0.0f;
                m_bias  = 0.0f;
            }

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

                Vec2 P = m_impulse * m_u;
                vA -= m_invMassA * P;
                wA -= m_invIA * Utilities.Cross(m_rA, P);
                vB += m_invMassB * P;
                wB += m_invIB * Utilities.Cross(m_rB, P);
            }
            else
            {
                m_impulse = 0.0f;
            }

            data.velocities[m_indexA].v = vA;
            data.velocities[m_indexA].w = wA;
            data.velocities[m_indexB].v = vB;
            data.velocities[m_indexB].w = wB;
        }
Esempio n. 12
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);
        }
Esempio n. 13
0
        internal override void InitVelocityConstraints(SolverData data)
        {
            m_indexA       = m_bodyA.m_islandIndex;
            m_indexB       = m_bodyB.m_islandIndex;
            m_localCenterA = m_bodyA.m_sweep.localCenter;
            m_localCenterB = m_bodyB.m_sweep.localCenter;
            m_invMassA     = m_bodyA.m_invMass;
            m_invMassB     = m_bodyB.m_invMass;
            m_invIA        = m_bodyA.m_invI;
            m_invIB        = m_bodyB.m_invI;

            Vec2  cA = data.positions[m_indexA].c;
            float aA = data.positions[m_indexA].a;
            Vec2  vA = data.velocities[m_indexA].v;
            float wA = data.velocities[m_indexA].w;

            Vec2  cB = data.positions[m_indexB].c;
            float aB = data.positions[m_indexB].a;
            Vec2  vB = data.velocities[m_indexB].v;
            float wB = data.velocities[m_indexB].w;

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

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

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

            // Compute motor Jacobian and effective mass.
            {
                m_axis = Utilities.Mul(qA, m_localXAxisA);
                m_a1   = Utilities.Cross(d + rA, m_axis);
                m_a2   = Utilities.Cross(rB, m_axis);

                m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
                if (m_motorMass > 0.0f)
                {
                    m_motorMass = 1.0f / m_motorMass;
                }
            }

            // Prismatic constraint.
            {
                m_perp = Utilities.Mul(qA, m_localYAxisA);

                m_s1 = Utilities.Cross(d + rA, m_perp);
                m_s2 = Utilities.Cross(rB, m_perp);

                float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
                float k12 = iA * m_s1 + iB * m_s2;
                float k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
                float k22 = iA + iB;
                if (k22 == 0.0f)
                {
                    // For bodies with fixed rotation.
                    k22 = 1.0f;
                }
                float k23 = iA * m_a1 + iB * m_a2;
                float k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;

                m_K.ex.Set(k11, k12, k13);
                m_K.ey.Set(k12, k22, k23);
                m_K.ez.Set(k13, k23, k33);
            }

            // Compute motor and limit terms.
            if (m_enableLimit)
            {
                float jointTranslation = Utilities.Dot(m_axis, d);
                if (Math.Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * Settings._linearSlop)
                {
                    m_limitState = LimitState.e_equalLimits;
                }
                else if (jointTranslation <= m_lowerTranslation)
                {
                    if (m_limitState != LimitState.e_atLowerLimit)
                    {
                        m_limitState = LimitState.e_atLowerLimit;
                        m_impulse.Z  = 0.0f;
                    }
                }
                else if (jointTranslation >= m_upperTranslation)
                {
                    if (m_limitState != LimitState.e_atUpperLimit)
                    {
                        m_limitState = LimitState.e_atUpperLimit;
                        m_impulse.Z  = 0.0f;
                    }
                }
                else
                {
                    m_limitState = LimitState.e_inactiveLimit;
                    m_impulse.Z  = 0.0f;
                }
            }
            else
            {
                m_limitState = LimitState.e_inactiveLimit;
                m_impulse.Z  = 0.0f;
            }

            if (m_enableMotor == false)
            {
                m_motorImpulse = 0.0f;
            }

            if (data.step.warmStarting)
            {
                // Account for variable time step.
                m_impulse      *= data.step.dtRatio;
                m_motorImpulse *= data.step.dtRatio;

                Vec2  P  = m_impulse.X * m_perp + (m_motorImpulse + m_impulse.Z) * m_axis;
                float LA = m_impulse.X * m_s1 + m_impulse.Y + (m_motorImpulse + m_impulse.Z) * m_a1;
                float LB = m_impulse.X * m_s2 + m_impulse.Y + (m_motorImpulse + m_impulse.Z) * m_a2;

                vA -= mA * P;
                wA -= iA * LA;

                vB += mB * P;
                wB += iB * LB;
            }
            else
            {
                m_impulse.SetZero();
                m_motorImpulse = 0.0f;
            }

            data.velocities[m_indexA].v = vA;
            data.velocities[m_indexA].w = wA;
            data.velocities[m_indexB].v = vB;
            data.velocities[m_indexB].w = wB;
        }
Esempio n. 14
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);

			Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
			Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);

			// Get the pulley axes.
			Vec2 uA = cA + rA - m_groundAnchorA;
			Vec2 uB = cB + rB - m_groundAnchorB;

			float lengthA = uA.Length();
			float lengthB = uB.Length();

			if (lengthA > 10.0f *Settings._linearSlop)
			{
				uA *= 1.0f / lengthA;
			}
			else
			{
				uA.SetZero();
			}

			if (lengthB > 10.0f *Settings._linearSlop)
			{
				uB *= 1.0f / lengthB;
			}
			else
			{
				uB.SetZero();
			}

			// Compute effective mass.
			float ruA = Utilities.Cross(rA, uA);
			float ruB = Utilities.Cross(rB, uB);

			float mA = m_invMassA + m_invIA * ruA * ruA;
			float mB = m_invMassB + m_invIB * ruB * ruB;

			float mass = mA + m_ratio * m_ratio * mB;

			if (mass > 0.0f)
			{
				mass = 1.0f / mass;
			}

			float C = m_constant - lengthA - m_ratio * lengthB;
			float linearError = Math.Abs(C);

			float impulse = -mass * C;

			Vec2 PA = -impulse * uA;
			Vec2 PB = -m_ratio * impulse * uB;

			cA += m_invMassA * PA;
			aA += m_invIA * Utilities.Cross(rA, PA);
			cB += m_invMassB * PB;
			aB += m_invIB * Utilities.Cross(rB, PB);

			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;
		}
Esempio n. 15
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;
		}
Esempio n. 16
0
		internal override void InitVelocityConstraints(SolverData data){
			m_indexA = m_bodyA.m_islandIndex;
			m_indexB = m_bodyB.m_islandIndex;
			m_indexC = m_bodyC.m_islandIndex;
			m_indexD = m_bodyD.m_islandIndex;
			m_lcA = m_bodyA.m_sweep.localCenter;
			m_lcB = m_bodyB.m_sweep.localCenter;
			m_lcC = m_bodyC.m_sweep.localCenter;
			m_lcD = m_bodyD.m_sweep.localCenter;
			m_mA = m_bodyA.m_invMass;
			m_mB = m_bodyB.m_invMass;
			m_mC = m_bodyC.m_invMass;
			m_mD = m_bodyD.m_invMass;
			m_iA = m_bodyA.m_invI;
			m_iB = m_bodyB.m_invI;
			m_iC = m_bodyC.m_invI;
			m_iD = m_bodyD.m_invI;

			float aA = data.positions[m_indexA].a;
			Vec2 vA = data.velocities[m_indexA].v;
			float wA = data.velocities[m_indexA].w;

			float aB = data.positions[m_indexB].a;
			Vec2 vB = data.velocities[m_indexB].v;
			float wB = data.velocities[m_indexB].w;

			float aC = data.positions[m_indexC].a;
			Vec2 vC = data.velocities[m_indexC].v;
			float wC = data.velocities[m_indexC].w;

			float aD = data.positions[m_indexD].a;
			Vec2 vD = data.velocities[m_indexD].v;
			float wD = data.velocities[m_indexD].w;

			Rot qA = new Rot(aA);
			Rot qB = new Rot(aB);
			Rot qC= new Rot(aC);
			Rot qD= new Rot(aD);

			m_mass = 0.0f;

			if (m_typeA == JointType.e_revoluteJoint)
			{
				m_JvAC.SetZero();
				m_JwA = 1.0f;
				m_JwC = 1.0f;
				m_mass += m_iA + m_iC;
			}
			else
			{
				Vec2 u = Utilities.Mul(qC, m_localAxisC);
				Vec2 rC = Utilities.Mul(qC, m_localAnchorC - m_lcC);
				Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_lcA);
				m_JvAC = u;
				m_JwC = Utilities.Cross(rC, u);
				m_JwA = Utilities.Cross(rA, u);
				m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA;
			}

			if (m_typeB == JointType.e_revoluteJoint)
			{
				m_JvBD.SetZero();
				m_JwB = m_ratio;
				m_JwD = m_ratio;
				m_mass += m_ratio * m_ratio * (m_iB + m_iD);
			}
			else
			{
				Vec2 u = Utilities.Mul(qD, m_localAxisD);
				Vec2 rD = Utilities.Mul(qD, m_localAnchorD - m_lcD);
				Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_lcB);
				m_JvBD = m_ratio * u;
				m_JwD = m_ratio * Utilities.Cross(rD, u);
				m_JwB = m_ratio * Utilities.Cross(rB, u);
				m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB;
			}

			// Compute effective mass.
			m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f;

			if (data.step.warmStarting)
			{
				vA += (m_mA * m_impulse) * m_JvAC;
				wA += m_iA * m_impulse * m_JwA;
				vB += (m_mB * m_impulse) * m_JvBD;
				wB += m_iB * m_impulse * m_JwB;
				vC -= (m_mC * m_impulse) * m_JvAC;
				wC -= m_iC * m_impulse * m_JwC;
				vD -= (m_mD * m_impulse) * m_JvBD;
				wD -= m_iD * m_impulse * m_JwD;
			}
			else
			{
				m_impulse = 0.0f;
			}

			data.velocities[m_indexA].v = vA;
			data.velocities[m_indexA].w = wA;
			data.velocities[m_indexB].v = vB;
			data.velocities[m_indexB].w = wB;
			data.velocities[m_indexC].v = vC;
			data.velocities[m_indexC].w = wC;
			data.velocities[m_indexD].v = vD;
			data.velocities[m_indexD].w = wD;
		}
Esempio n. 17
0
		/// Multiply two rotations: q * r
		public static  Rot Mul(Rot q, Rot r)
		{
			// [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
			// [qs  qc]   [rs  rc]   [qs*rc+qc*rs -qs*rs+qc*rc]
			// s = qs * rc + qc * rs
			// c = qc * rc - qs * rs
			Rot qr;
			qr.s = q.s * r.c + q.c * r.s;
			qr.c = q.c * r.c - q.s * r.s;
			return qr;
		}
Esempio n. 18
0
		internal override bool SolvePositionConstraints(SolverData data){
			if (m_frequencyHz > 0.0f)
			{
				// There is no position correction for soft distance constraints.
				return true;
			}

			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);

			Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
			Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
			Vec2 u = cB + rB - cA - rA;

			float length = u.Normalize();
			float C = length - m_length;
			C = Utilities.Clamp(C, -Settings._maxLinearCorrection, Settings._maxLinearCorrection);

			float impulse = -m_mass * C;
			Vec2 P = impulse * u;

			cA -= m_invMassA * P;
			aA -= m_invIA * Utilities.Cross(rA, P);
			cB += m_invMassB * P;
			aB += m_invIB * Utilities.Cross(rB, P);

			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 Math.Abs(C) <Settings._linearSlop;
		}
Esempio n. 19
0
		/// Transpose multiply two rotations: qT * r
		public static  Rot MulT(Rot q, Rot r)
		{
			// [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
			// [-qs qc]   [rs  rc]   [-qs*rc+qc*rs qs*rs+qc*rc]
			// s = qc * rs - qs * rc
			// c = qc * rc + qs * rs
			Rot qr;
			qr.s = q.c * r.s - q.s * r.c;
			qr.c = q.c * r.c + q.s * r.s;
			return qr;
		}
Esempio n. 20
0
		internal override void InitVelocityConstraints(SolverData data){
			m_indexA = m_bodyA.m_islandIndex;
			m_indexB = m_bodyB.m_islandIndex;
			m_localCenterA = m_bodyA.m_sweep.localCenter;
			m_localCenterB = m_bodyB.m_sweep.localCenter;
			m_invMassA = m_bodyA.m_invMass;
			m_invMassB = m_bodyB.m_invMass;
			m_invIA = m_bodyA.m_invI;
			m_invIB = m_bodyB.m_invI;

			float aA = data.positions[m_indexA].a;
			Vec2 vA = data.velocities[m_indexA].v;
			float wA = data.velocities[m_indexA].w;

			float aB = data.positions[m_indexB].a;
			Vec2 vB = data.velocities[m_indexB].v;
			float wB = data.velocities[m_indexB].w;

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

			m_rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
			m_rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);

			// 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 = m_invMassA, mB = m_invMassB;
			float iA = m_invIA, iB = m_invIB;

			bool fixedRotation = (iA + iB == 0.0f);

			m_mass.ex.X = mA + mB + m_rA.Y * m_rA.Y * iA + m_rB.Y * m_rB.Y * iB;
			m_mass.ey.X = -m_rA.Y * m_rA.X * iA - m_rB.Y * m_rB.X * iB;
			m_mass.ez.X = -m_rA.Y * iA - m_rB.Y * iB;
			m_mass.ex.Y = m_mass.ey.X;
			m_mass.ey.Y = mA + mB + m_rA.X * m_rA.X * iA + m_rB.X * m_rB.X * iB;
			m_mass.ez.Y = m_rA.X * iA + m_rB.X * iB;
			m_mass.ex.Z = m_mass.ez.X;
			m_mass.ey.Z = m_mass.ez.Y;
			m_mass.ez.Z = iA + iB;

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

			if (m_enableMotor == false || fixedRotation)
			{
				m_motorImpulse = 0.0f;
			}

			if (m_enableLimit && fixedRotation == false)
			{
				float jointAngle = aB - aA - m_referenceAngle;
				if (Math.Abs(m_upperAngle - m_lowerAngle) < 2.0f * Settings._angularSlop)
				{
					m_limitState = LimitState.e_equalLimits;
				}
				else if (jointAngle <= m_lowerAngle)
				{
					if (m_limitState != LimitState.e_atLowerLimit)
					{
						m_impulse.Z = 0.0f;
					}
					m_limitState = LimitState.e_atLowerLimit;
				}
				else if (jointAngle >= m_upperAngle)
				{
					if (m_limitState != LimitState.e_atUpperLimit)
					{
						m_impulse.Z = 0.0f;
					}
					m_limitState = LimitState.e_atUpperLimit;
				}
				else
				{
					m_limitState = LimitState.e_inactiveLimit;
					m_impulse.Z = 0.0f;
				}
			}
			else
			{
				m_limitState = LimitState.e_inactiveLimit;
			}

			if (data.step.warmStarting)
			{
				// Scale impulses to support a variable time step.
				m_impulse *= data.step.dtRatio;
				m_motorImpulse *= data.step.dtRatio;

				Vec2 P = new Vec2(m_impulse.X, m_impulse.Y);

				vA -= mA * P;
				wA -= iA * (Utilities.Cross(m_rA, P) + m_motorImpulse + m_impulse.Z);

				vB += mB * P;
				wB += iB * (Utilities.Cross(m_rB, P) + m_motorImpulse + m_impulse.Z);
			}
			else
			{
				m_impulse.SetZero();
				m_motorImpulse = 0.0f;
			}

			data.velocities[m_indexA].v = vA;
			data.velocities[m_indexA].w = wA;
			data.velocities[m_indexB].v = vB;
			data.velocities[m_indexB].w = wB;
		}
Esempio n. 21
0
		/// Inverse rotate a vector
		public static  Vec2 MulT(Rot q, Vec2 v)
		{
			return new Vec2(q.c * v.X + q.s * v.Y, -q.s * v.X + q.c * v.Y);
		}
Esempio n. 22
0
		internal override void InitVelocityConstraints(SolverData data){
			m_indexA = m_bodyA.m_islandIndex;
			m_indexB = m_bodyB.m_islandIndex;
			m_localCenterA = m_bodyA.m_sweep.localCenter;
			m_localCenterB = m_bodyB.m_sweep.localCenter;
			m_invMassA = m_bodyA.m_invMass;
			m_invMassB = m_bodyB.m_invMass;
			m_invIA = m_bodyA.m_invI;
			m_invIB = m_bodyB.m_invI;

			Vec2 cA = data.positions[m_indexA].c;
			float aA = data.positions[m_indexA].a;
			Vec2 vA = data.velocities[m_indexA].v;
			float wA = data.velocities[m_indexA].w;

			Vec2 cB = data.positions[m_indexB].c;
			float aB = data.positions[m_indexB].a;
			Vec2 vB = data.velocities[m_indexB].v;
			float wB = data.velocities[m_indexB].w;

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

			// Compute the effective mass matrix.
			m_rA = Utilities.Mul(qA, -m_localCenterA);
			m_rB = Utilities.Mul(qB, -m_localCenterB);

			// 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 = m_invMassA, mB = m_invMassB;
			float iA = m_invIA, iB = m_invIB;

			Mat22 K;
			K.ex.X = mA + mB + iA * m_rA.Y * m_rA.Y + iB * m_rB.Y * m_rB.Y;
			K.ex.Y = -iA * m_rA.X * m_rA.Y - iB * m_rB.X * m_rB.Y;
			K.ey.X = K.ex.Y;
			K.ey.Y = mA + mB + iA * m_rA.X * m_rA.X + iB * m_rB.X * m_rB.X;

			m_linearMass = K.GetInverse();

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

			m_linearError = cB + m_rB - cA - m_rA - Utilities.Mul(qA, m_linearOffset);
			m_angularError = aB - aA - m_angularOffset;

			if (data.step.warmStarting)
			{
				// Scale impulses to support a variable time step.
				m_linearImpulse *= data.step.dtRatio;
				m_angularImpulse *= data.step.dtRatio;

				Vec2 P = new Vec2(m_linearImpulse.X, m_linearImpulse.Y);
				vA -= mA * P;
				wA -= iA * (Utilities.Cross(m_rA, P) + m_angularImpulse);
				vB += mB * P;
				wB += iB * (Utilities.Cross(m_rB, P) + m_angularImpulse);
			}
			else
			{
				m_linearImpulse.SetZero();
				m_angularImpulse = 0.0f;
			}

			data.velocities[m_indexA].v = vA;
			data.velocities[m_indexA].w = wA;
			data.velocities[m_indexB].v = vB;
			data.velocities[m_indexB].w = wB;
		}
Esempio n. 23
0
		/// The default constructor does nothing.
		public Transform(object ignore) {
			p = new Vec2();
			q = new Rot();
		}
Esempio n. 24
0
		internal override void InitVelocityConstraints(SolverData data){
			m_indexA = m_bodyA.m_islandIndex;
			m_indexB = m_bodyB.m_islandIndex;
			m_localCenterA = m_bodyA.m_sweep.localCenter;
			m_localCenterB = m_bodyB.m_sweep.localCenter;
			m_invMassA = m_bodyA.m_invMass;
			m_invMassB = m_bodyB.m_invMass;
			m_invIA = m_bodyA.m_invI;
			m_invIB = m_bodyB.m_invI;

			Vec2 cA = data.positions[m_indexA].c;
			float aA = data.positions[m_indexA].a;
			Vec2 vA = data.velocities[m_indexA].v;
			float wA = data.velocities[m_indexA].w;

			Vec2 cB = data.positions[m_indexB].c;
			float aB = data.positions[m_indexB].a;
			Vec2 vB = data.velocities[m_indexB].v;
			float wB = data.velocities[m_indexB].w;

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

			m_rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
			m_rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
			m_u = cB + m_rB - cA - m_rA;

			m_length = m_u.Length();

			float C = m_length - m_maxLength;
			if (C > 0.0f)
			{
				m_state = LimitState.e_atUpperLimit;
			}
			else
			{
				m_state = LimitState.e_inactiveLimit;
			}

			if (m_length >Settings._linearSlop)
			{
				m_u *= 1.0f / m_length;
			}
			else
			{
				m_u.SetZero();
				m_mass = 0.0f;
				m_impulse = 0.0f;
				return;
			}

			// Compute effective mass.
			float crA = Utilities.Cross(m_rA, m_u);
			float crB = Utilities.Cross(m_rB, m_u);
			float invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB;

			m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

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

				Vec2 P = m_impulse * m_u;
				vA -= m_invMassA * P;
				wA -= m_invIA * Utilities.Cross(m_rA, P);
				vB += m_invMassB * P;
				wB += m_invIB * Utilities.Cross(m_rB, P);
			}
			else
			{
				m_impulse = 0.0f;
			}

			data.velocities[m_indexA].v = vA;
			data.velocities[m_indexA].w = wA;
			data.velocities[m_indexB].v = vB;
			data.velocities[m_indexB].w = wB;
		}
Esempio n. 25
0
		/// Initialize using a position vector and a rotation.
		public Transform(Vec2 position, Rot rotation){
			this.p = position;
			this.q = rotation;
		}
Esempio n. 26
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);

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

			Vec2 ay = Utilities.Mul(qA, m_localYAxisA);

			float sAy = Utilities.Cross(d + rA, ay);
			float sBy = Utilities.Cross(rB, ay);

			float C = Utilities.Dot(d, ay);

			float k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;

			float impulse;
			if (k != 0.0f)
			{
				impulse = - C / k;
			}
			else
			{
				impulse = 0.0f;
			}

			Vec2 P = impulse * ay;
			float LA = impulse * sAy;
			float LB = impulse * sBy;

			cA -= m_invMassA * P;
			aA -= m_invIA * LA;
			cB += m_invMassB * P;
			aB += m_invIB * 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 Math.Abs(C) <=Settings._linearSlop;
		}
Esempio n. 27
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;

			Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
			Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);

			float positionError, angularError;

			Mat33 K;
			K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
			K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
			K.ez.X = -rA.Y * iA - rB.Y * iB;
			K.ex.Y = K.ey.X;
			K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
			K.ez.Y = rA.X * iA + rB.X * iB;
			K.ex.Z = K.ez.X;
			K.ey.Z = K.ez.Y;
			K.ez.Z = iA + iB;

			if (m_frequencyHz > 0.0f)
			{
				Vec2 C1 =  cB + rB - cA - rA;

				positionError = C1.Length();
				angularError = 0.0f;

				Vec2 P = -K.Solve22(C1);

				cA -= mA * P;
				aA -= iA * Utilities.Cross(rA, P);

				cB += mB * P;
				aB += iB * Utilities.Cross(rB, P);
			}
			else
			{
				Vec2 C1 =  cB + rB - cA - rA;
				float C2 = aB - aA - m_referenceAngle;

				positionError = C1.Length();
				angularError = Math.Abs(C2);

				Vec3 C = new Vec3(C1.X, C1.Y, C2);
	
				Vec3 impulse = -K.Solve33(C);
				Vec2 P = new Vec2(impulse.X, impulse.Y);

				cA -= mA * P;
				aA -= iA * (Utilities.Cross(rA, P) + impulse.Z);

				cB += mB * P;
				aB += iB * (Utilities.Cross(rB, P) + impulse.Z);
			}

			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 positionError <=Settings._linearSlop && angularError <= Settings._angularSlop;
		}