/// Get the symmetric inverse of this matrix as a 3-by-3. /// Returns the zero matrix if singular. public b2Mat33 GetSymInverse33(b2Mat33 M) { b2Vec3 cross = b2Math.b2Cross(ey, ez); float det = b2Math.b2Dot(ex, cross); if (det != 0.0f) { det = 1.0f / det; } float a11 = ex.x, a12 = ey.x, a13 = ez.x; float a22 = ey.y, a23 = ez.y; float a33 = ez.z; M.ex.x = det * (a22 * a33 - a23 * a23); M.ex.y = det * (a13 * a23 - a12 * a33); M.ex.z = det * (a12 * a23 - a13 * a22); M.ey.x = M.ex.y; M.ey.y = det * (a11 * a33 - a13 * a13); M.ey.z = det * (a13 * a12 - a11 * a23); M.ez.x = M.ex.z; M.ez.y = M.ey.z; M.ez.z = det * (a11 * a22 - a12 * a12); return(M); }
/// Solve A * x = b, where b is a column vector. This is more efficient /// than computing the inverse in one-shot cases. public b2Vec3 Solve33(b2Vec3 b) { float det = b2Math.b2Dot(_ex, b2Math.b2Cross(_ey, _ez)); if (det != 0.0f) { det = 1.0f / det; } b2Vec3 x = new b2Vec3(); x.x = det * b2Math.b2Dot(b, b2Math.b2Cross(_ey, _ez)); x.y = det * b2Math.b2Dot(_ex, b2Math.b2Cross(b, _ez)); x.z = det * b2Math.b2Dot(_ex, b2Math.b2Cross(_ey, b)); return x; }
/// Solve A * x = b, where b is a column vector. This is more efficient /// than computing the inverse in one-shot cases. public b2Vec3 Solve33(b2Vec3 b) { float det = b2Math.b2Dot(ex, b2Math.b2Cross(ey, ez)); if (det != 0.0f) { det = 1.0f / det; } b2Vec3 x = new b2Vec3(); x.x = det * b2Math.b2Dot(b, b2Math.b2Cross(ey, ez)); x.y = det * b2Math.b2Dot(ex, b2Math.b2Cross(b, ez)); x.z = det * b2Math.b2Dot(ex, b2Math.b2Cross(ey, b)); return(x); }
/// Solve A * x = b, where b is a column vector. This is more efficient /// than computing the inverse in one-shot cases. public b2Vec3 Solve33(b2Vec3 b) { b2Vec3 cx = b2Math.b2Cross(ref ey, ref ez); float det = b2Math.b2Dot(ref ex, ref cx); if (det != 0.0f) { det = 1.0f / det; } b2Vec3 x = new b2Vec3(); cx = b2Math.b2Cross(ref ey, ref ez); x.x = det * b2Math.b2Dot(ref b, ref cx); cx = b2Math.b2Cross(ref b, ref ez); x.y = det * b2Math.b2Dot(ref ex, ref cx); cx = b2Math.b2Cross(ref ey, ref b); x.z = det * b2Math.b2Dot(ref ex, ref cx); return x; }
/// Solve A * x = b, where b is a column vector. This is more efficient /// than computing the inverse in one-shot cases. public b2Vec3 Solve33(b2Vec3 b) { b2Vec3 cx = b2Math.b2Cross(ref ey, ref ez); float det = b2Math.b2Dot(ref ex, ref cx); if (det != 0.0f) { det = 1.0f / det; } b2Vec3 x = new b2Vec3(); cx = b2Math.b2Cross(ref ey, ref ez); x.x = det * b2Math.b2Dot(ref b, ref cx); cx = b2Math.b2Cross(ref b, ref ez); x.y = det * b2Math.b2Dot(ref ex, ref cx); cx = b2Math.b2Cross(ref ey, ref b); x.z = det * b2Math.b2Dot(ref ex, ref cx); return(x); }
public static b2Vec3 b2Cross(b2Vec3 a, b2Vec3 b) { return(new b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x)); }
public static float b2Dot(ref b2Vec3 a, ref b2Vec3 b) { return(a.x * b.x + a.y * b.y + a.z * b.z); }
public override bool SolvePositionConstraints(b2SolverData data) { b2Vec2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; b2Vec2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; b2Rot qA = new b2Rot(aA); b2Rot qB = new b2Rot(aB); float mA = InvertedMassA, mB = InvertedMassB; float iA = InvertedIA, iB = InvertedIB; // Compute fresh Jacobians b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA); b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB); b2Vec2 d = cB + rB - cA - rA; b2Vec2 axis = b2Math.b2Mul(qA, m_localXAxisA); float a1 = b2Math.b2Cross(d + rA, axis); float a2 = b2Math.b2Cross(rB, axis); b2Vec2 perp = b2Math.b2Mul(qA, m_localYAxisA); float s1 = b2Math.b2Cross(d + rA, perp); float s2 = b2Math.b2Cross(rB, perp); b2Vec3 impulse; b2Vec2 C1 = new b2Vec2(); C1.x = b2Math.b2Dot(perp, d); C1.y = aB - aA - m_referenceAngle; float linearError = b2Math.b2Abs(C1.x); float angularError = b2Math.b2Abs(C1.y); bool active = false; float C2 = 0.0f; if (m_enableLimit) { float translation = b2Math.b2Dot(axis, d); if (b2Math.b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2Settings.b2_linearSlop) { // Prevent large angular corrections C2 = b2Math.b2Clamp(translation, -b2Settings.b2_maxLinearCorrection, b2Settings.b2_maxLinearCorrection); linearError = Math.Max(linearError, b2Math.b2Abs(translation)); active = true; } else if (translation <= m_lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = b2Math.b2Clamp(translation - m_lowerTranslation + b2Settings.b2_linearSlop, -b2Settings.b2_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 = b2Math.b2Clamp(translation - m_upperTranslation - b2Settings.b2_linearSlop, 0.0f, b2Settings.b2_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; b2Mat33 K = new b2Mat33( new b2Vec3(k11, k12, k13), new b2Vec3(k12, k22, k23), new b2Vec3(k13, k23, k33)); b2Vec3 C = new b2Vec3(C1.x, C1.y, 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; } b2Mat22 K = new b2Mat22(); K.ex.Set(k11, k12); K.ey.Set(k12, k22); b2Vec2 impulse1 = K.Solve(-C1); impulse = new b2Vec3(); impulse.x = impulse1.x; impulse.y = impulse1.y; impulse.z = 0.0f; } b2Vec2 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 <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop; }
public static b2Vec3 b2Cross(ref b2Vec3 a, ref b2Vec3 b) { return new b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); }
public override void InitVelocityConstraints(b2SolverData data) { m_indexA = m_bodyA.IslandIndex; m_indexB = m_bodyB.IslandIndex; m_localCenterA = m_bodyA.Sweep.localCenter; m_localCenterB = m_bodyB.Sweep.localCenter; m_invMassA = m_bodyA.InvertedMass; m_invMassB = m_bodyB.InvertedMass; m_invIA = m_bodyA.InvertedI; m_invIB = m_bodyB.InvertedI; b2Vec2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; float wA = data.velocities[m_indexA].w; b2Vec2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; b2Rot qA = new b2Rot(aA); b2Rot qB = new b2Rot(aB); m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA); m_rB = b2Math.b2Mul(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); b2Vec3 ex = new b2Vec3(); b2Vec3 ey = new b2Vec3(); b2Vec3 ez = new b2Vec3(); ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; ez.x = -m_rA.y * iA - m_rB.y * iB; ex.y = ey.x; ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; ez.y = m_rA.x * iA + m_rB.x * iB; ex.z = ez.x; ey.z = ez.y; ez.z = iA + iB; m_mass = new b2Mat33(ex, ey, ez); 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 (b2Math.b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2Settings.b2_angularSlop) { m_limitState = b2LimitState.e_equalLimits; } else if (jointAngle <= m_lowerAngle) { if (m_limitState != b2LimitState.e_atLowerLimit) { m_impulse.z = 0.0f; } m_limitState = b2LimitState.e_atLowerLimit; } else if (jointAngle >= m_upperAngle) { if (m_limitState != b2LimitState.e_atUpperLimit) { m_impulse.z = 0.0f; } m_limitState = b2LimitState.e_atUpperLimit; } else { m_limitState = b2LimitState.e_inactiveLimit; m_impulse.z = 0.0f; } } else { m_limitState = b2LimitState.e_inactiveLimit; } if (data.step.warmStarting) { // Scale impulses to support a variable time step. m_impulse *= data.step.dtRatio; m_motorImpulse *= data.step.dtRatio; b2Vec2 P = new b2Vec2(m_impulse.x, m_impulse.y); vA -= mA * P; wA -= iA * (b2Math.b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z); vB += mB * P; wB += iB * (b2Math.b2Cross(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; }
public override bool SolvePositionConstraints(b2SolverData data) { b2Vec2 cA = m_bodyA.InternalPosition.c; float aA = m_bodyA.InternalPosition.a; b2Vec2 cB = m_bodyB.InternalPosition.c; float aB = m_bodyB.InternalPosition.a; b2Rot qA = new b2Rot(aA); b2Rot qB = new b2Rot(aB); float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA); b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB); float positionError, angularError; b2Vec3 ex = new b2Vec3(); b2Vec3 ey = new b2Vec3(); b2Vec3 ez = new b2Vec3(); ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; ez.x = -rA.y * iA - rB.y * iB; ex.y = ey.x; ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; ez.y = rA.x * iA + rB.x * iB; ex.z = ez.x; ey.z = ez.y; ez.z = iA + iB; b2Mat33 K = new b2Mat33(ex, ey, ez); if (m_frequencyHz > 0.0f) { b2Vec2 C1 = cB + rB - cA - rA; positionError = C1.Length; angularError = 0.0f; b2Vec2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * b2Math.b2Cross(rA, P); cB += mB * P; aB += iB * b2Math.b2Cross(rB, P); } else { b2Vec2 C1 = cB + rB - cA - rA; float C2 = aB - aA - m_referenceAngle; positionError = C1.Length; angularError = b2Math.b2Abs(C2); b2Vec3 C = new b2Vec3(C1.x, C1.y, C2); b2Vec3 impulse = -K.Solve33(C); b2Vec2 P = new b2Vec2(impulse.x, impulse.y); cA -= mA * P; aA -= iA * (b2Math.b2Cross(rA, P) + impulse.z); cB += mB * P; aB += iB * (b2Math.b2Cross(rB, P) + impulse.z); } m_bodyA.InternalPosition.c = cA; m_bodyA.InternalPosition.a = aA; m_bodyB.InternalPosition.c = cB; m_bodyB.InternalPosition.a = aB; return positionError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop; }
/* public b2Mat33() { ex = new b2Vec3(); ey = new b2Vec3(); ez = new b2Vec3(); } */ /// ruct this matrix using columns. public b2Mat33(b2Vec3 c1, b2Vec3 c2, b2Vec3 c3) { ex = c1; ey = c2; ez = c3; }
/// Negate this vector. public static b2Vec3 operator -(b2Vec3 b) { b2Vec3 v = new b2Vec3(-b.x, -b.y, -b.z); return(v); }
/* * public b2Mat33() * { * ex = new b2Vec3(); * ey = new b2Vec3(); * ez = new b2Vec3(); * } */ /// ruct this matrix using columns. public b2Mat33(b2Vec3 c1, b2Vec3 c2, b2Vec3 c3) { ex = c1; ey = c2; ez = c3; }
/* * public b2Mat33() * { * _ex = new b2Vec3(); * _ey = new b2Vec3(); * _ez = new b2Vec3(); * } */ /// ruct this matrix using columns. public b2Mat33(b2Vec3 c1, b2Vec3 c2, b2Vec3 c3) { _ex = c1; _ey = c2; _ez = c3; }
public static b2Vec3 b2Mul(b2Mat33 A, b2Vec3 v) { return(v.x * A.ex + v.y * A.ey + v.z * A.ez); }
/* public b2Mat33() { _ex = new b2Vec3(); _ey = new b2Vec3(); _ez = new b2Vec3(); } */ /// ruct this matrix using columns. public b2Mat33(b2Vec3 c1, b2Vec3 c2, b2Vec3 c3) { _ex = c1; _ey = c2; _ez = c3; }
/// Negate this vector. public static b2Vec3 operator -(b2Vec3 b) { b2Vec3 v = new b2Vec3(-b.x, -b.y, -b.z); return v; }
public override void SolveVelocityConstraints(b2SolverData data) { b2Vec2 vA = m_bodyA.InternalVelocity.v; float wA = m_bodyA.InternalVelocity.w; b2Vec2 vB = m_bodyB.InternalVelocity.v; float wB = m_bodyB.InternalVelocity.w; float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; if (m_frequencyHz > 0.0f) { float Cdot2 = wB - wA; float impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z); m_impulse.z += impulse2; wA -= iA * impulse2; wB += iB * impulse2; b2Vec2 Cdot1 = vB + b2Math.b2Cross(wB, ref m_rB) - vA - b2Math.b2Cross(wA, ref m_rA); b2Vec2 impulse1 = -b2Math.b2Mul22(m_mass, Cdot1); m_impulse.x += impulse1.x; m_impulse.y += impulse1.y; b2Vec2 P = impulse1; vA -= mA * P; wA -= iA * b2Math.b2Cross(ref m_rA, ref P); vB += mB * P; wB += iB * b2Math.b2Cross(ref m_rB, ref P); } else { b2Vec2 Cdot1 = vB + b2Math.b2Cross(wB, ref m_rB) - vA - b2Math.b2Cross(wA, ref m_rA); float Cdot2 = wB - wA; b2Vec3 Cdot = new b2Vec3(Cdot1.x, Cdot1.y, Cdot2); b2Vec3 impulse = -b2Math.b2Mul(m_mass, Cdot); m_impulse += impulse; b2Vec2 P = b2Vec2.Zero; P.Set(impulse.x, impulse.y); vA -= mA * P; wA -= iA * (b2Math.b2Cross(ref m_rA, ref P) + impulse.z); vB += mB * P; wB += iB * (b2Math.b2Cross(ref m_rB, ref P) + impulse.z); } m_bodyA.InternalVelocity.v = vA; m_bodyA.InternalVelocity.w = wA; m_bodyB.InternalVelocity.v = vB; m_bodyB.InternalVelocity.w = wB; }
public override void InitVelocityConstraints(b2SolverData data) { m_indexA = m_bodyA.IslandIndex; m_indexB = m_bodyB.IslandIndex; m_localCenterA = m_bodyA.Sweep.localCenter; m_localCenterB = m_bodyB.Sweep.localCenter; InvertedMassA = m_bodyA.InvertedMass; InvertedMassB = m_bodyB.InvertedMass; InvertedIA = m_bodyA.InvertedI; InvertedIB = m_bodyB.InvertedI; b2Vec2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; float wA = data.velocities[m_indexA].w; b2Vec2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; b2Rot qA = new b2Rot(aA); b2Rot qB = new b2Rot(aB); // Compute the effective masses. b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA); b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB); b2Vec2 d = (cB - cA) + rB - rA; float mA = InvertedMassA, mB = InvertedMassB; float iA = InvertedIA, iB = InvertedIB; // Compute motor Jacobian and effective mass. { m_axis = b2Math.b2Mul(qA, m_localXAxisA); m_a1 = b2Math.b2Cross(d + rA, m_axis); m_a2 = b2Math.b2Cross(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 = b2Math.b2Mul(qA, m_localYAxisA); m_s1 = b2Math.b2Cross(d + rA, m_perp); m_s2 = b2Math.b2Cross(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 = b2Math.b2Dot(m_axis, d); if (b2Math.b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2Settings.b2_linearSlop) { m_limitState = b2LimitState.e_equalLimits; } else if (jointTranslation <= m_lowerTranslation) { if (m_limitState != b2LimitState.e_atLowerLimit) { m_limitState = b2LimitState.e_atLowerLimit; m_impulse.z = 0.0f; } } else if (jointTranslation >= m_upperTranslation) { if (m_limitState != b2LimitState.e_atUpperLimit) { m_limitState = b2LimitState.e_atUpperLimit; m_impulse.z = 0.0f; } } else { m_limitState = b2LimitState.e_inactiveLimit; m_impulse.z = 0.0f; } } else { m_limitState = b2LimitState.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; b2Vec2 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; }
public override void InitVelocityConstraints(b2SolverData data) { m_indexA = m_bodyA.IslandIndex; m_indexB = m_bodyB.IslandIndex; m_localCenterA = m_bodyA.Sweep.localCenter; m_localCenterB = m_bodyB.Sweep.localCenter; m_invMassA = m_bodyA.InvertedMass; m_invMassB = m_bodyB.InvertedMass; m_invIA = m_bodyA.InvertedI; m_invIB = m_bodyB.InvertedI; b2Vec2 cA = m_bodyA.InternalPosition.c; float aA = m_bodyA.InternalPosition.a; b2Vec2 vA = m_bodyA.InternalVelocity.v; float wA = m_bodyA.InternalVelocity.w; b2Vec2 cB = m_bodyB.InternalPosition.c; float aB = m_bodyB.InternalPosition.a; b2Vec2 vB = m_bodyB.InternalVelocity.v; float wB = m_bodyB.InternalVelocity.w; b2Rot qA = new b2Rot(aA); b2Rot qB = new b2Rot(aB); m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA); m_rB = b2Math.b2Mul(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; b2Vec3 ex = new b2Vec3(); b2Vec3 ey = new b2Vec3(); b2Vec3 ez = new b2Vec3(); ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; ez.x = -m_rA.y * iA - m_rB.y * iB; ex.y = ey.x; ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; ez.y = m_rA.x * iA + m_rB.x * iB; ex.z = ez.x; ey.z = ez.y; ez.z = iA + iB; b2Mat33 K = new b2Mat33(ex, ey, ez); if (m_frequencyHz > 0.0f) { m_mass = K.GetInverse22(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 * b2Settings.b2_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.ezz = invM != 0.0f ? 1.0f / invM : 0.0f; } else { m_mass = K.GetSymInverse33(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; b2Vec2 P = new b2Vec2(m_impulse.x, m_impulse.y); vA -= mA * P; wA -= iA * (b2Math.b2Cross(m_rA, P) + m_impulse.z); vB += mB * P; wB += iB * (b2Math.b2Cross(m_rB, P) + m_impulse.z); } else { m_impulse.SetZero(); } m_bodyA.InternalVelocity.v = vA; m_bodyA.InternalVelocity.w = wA; m_bodyB.InternalVelocity.v = vB; m_bodyB.InternalVelocity.w = wB; }
public override void SolveVelocityConstraints(b2SolverData data) { b2Vec2 vA = data.velocities[m_indexA].v; float wA = data.velocities[m_indexA].w; b2Vec2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; float mA = InvertedMassA, mB = InvertedMassB; float iA = InvertedIA, iB = InvertedIB; // Solve linear motor constraint. if (m_enableMotor && m_limitState != b2LimitState.e_equalLimits) { float Cdot = b2Math.b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; float impulse = m_motorMass * (m_motorSpeed - Cdot); float oldImpulse = m_motorImpulse; float maxImpulse = data.step.dt * m_maxMotorForce; m_motorImpulse = b2Math.b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_motorImpulse - oldImpulse; b2Vec2 P = impulse * m_axis; float LA = impulse * m_a1; float LB = impulse * m_a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } b2Vec2 Cdot1 = new b2Vec2(); Cdot1.x = b2Math.b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA; Cdot1.y = wB - wA; if (m_enableLimit && m_limitState != b2LimitState.e_inactiveLimit) { // Solve prismatic and limit constraint in block form. float Cdot2; Cdot2 = b2Math.b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; b2Vec3 Cdot = new b2Vec3(Cdot1.x, Cdot1.y, Cdot2); b2Vec3 f1 = m_impulse; b2Vec3 df = m_K.Solve33(-Cdot); m_impulse += df; if (m_limitState == b2LimitState.e_atLowerLimit) { m_impulse.z = Math.Max(m_impulse.z, 0.0f); } else if (m_limitState == b2LimitState.e_atUpperLimit) { m_impulse.z = Math.Min(m_impulse.z, 0.0f); } // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * (new b2Vec2(m_K.ez.x, m_K.ez.y)); b2Vec2 f2r = m_K.Solve22(b) + (new b2Vec2(f1.x, f1.y)); m_impulse.x = f2r.x; m_impulse.y = f2r.y; df = m_impulse - f1; b2Vec2 P = df.x * m_perp + df.z * m_axis; float LA = df.x * m_s1 + df.y + df.z * m_a1; float LB = df.x * m_s2 + df.y + df.z * m_a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } else { // Limit is inactive, just solve the prismatic constraint in block form. b2Vec2 df = m_K.Solve22(-Cdot1); m_impulse.x += df.x; m_impulse.y += df.y; b2Vec2 P = df.x * m_perp; float LA = df.x * m_s1 + df.y; float LB = df.x * m_s2 + df.y; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; b2Vec2 Cdot10 = Cdot1; Cdot1.x = b2Math.b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA; Cdot1.y = wB - wA; if (b2Math.b2Abs(Cdot1.x) > 0.01f || b2Math.b2Abs(Cdot1.y) > 0.01f) { b2Vec2 test = b2Math.b2Mul22(m_K, df); Cdot1.x += 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; }
public override void SolveVelocityConstraints(b2SolverData data) { b2Vec2 vA = data.velocities[m_indexA].v; float wA = data.velocities[m_indexA].w; b2Vec2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; bool fixedRotation = (iA + iB == 0.0f); // Solve motor constraint. if (m_enableMotor && m_limitState != b2LimitState.e_equalLimits && fixedRotation == false) { float Cdot = wB - wA - m_motorSpeed; float impulse = -m_motorMass * Cdot; float oldImpulse = m_motorImpulse; float maxImpulse = data.step.dt * m_maxMotorTorque; m_motorImpulse = b2Math.b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_motorImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } // Solve limit constraint. if (m_enableLimit && m_limitState != b2LimitState.e_inactiveLimit && fixedRotation == false) { b2Vec2 Cdot1 = vB + b2Math.b2Cross(wB, m_rB) - vA - b2Math.b2Cross(wA, m_rA); float Cdot2 = wB - wA; b2Vec3 Cdot = new b2Vec3(Cdot1.x, Cdot1.y, Cdot2); b2Vec3 impulse = -m_mass.Solve33(Cdot); if (m_limitState == b2LimitState.e_equalLimits) { m_impulse += impulse; } else if (m_limitState == b2LimitState.e_atLowerLimit) { float newImpulse = m_impulse.z + impulse.z; if (newImpulse < 0.0f) { b2Vec2 rhs = -Cdot1 + m_impulse.z * (new b2Vec2(m_mass.ez.x, m_mass.ez.y)); b2Vec2 reduced = m_mass.Solve22(rhs); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -m_impulse.z; m_impulse.x += reduced.x; m_impulse.y += reduced.y; m_impulse.z = 0.0f; } else { m_impulse += impulse; } } else if (m_limitState == b2LimitState.e_atUpperLimit) { float newImpulse = m_impulse.z + impulse.z; if (newImpulse > 0.0f) { b2Vec2 rhs = -Cdot1 + m_impulse.z * (new b2Vec2(m_mass.ez.x, m_mass.ez.y)); b2Vec2 reduced = m_mass.Solve22(rhs); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -m_impulse.z; m_impulse.x += reduced.x; m_impulse.y += reduced.y; m_impulse.z = 0.0f; } else { m_impulse += impulse; } } b2Vec2 P = new b2Vec2(impulse.x, impulse.y); vA -= mA * P; wA -= iA * (b2Math.b2Cross(m_rA, P) + impulse.z); vB += mB * P; wB += iB * (b2Math.b2Cross(m_rB, P) + impulse.z); } else { // Solve point-to-point constraint b2Vec2 Cdot = vB + b2Math.b2Cross(wB, m_rB) - vA - b2Math.b2Cross(wA, m_rA); b2Vec2 impulse = m_mass.Solve22(-Cdot); m_impulse.x += impulse.x; m_impulse.y += impulse.y; vA -= mA * impulse; wA -= iA * b2Math.b2Cross(m_rA, impulse); vB += mB * impulse; wB += iB * b2Math.b2Cross(m_rB, impulse); } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
public static float b2Dot(b2Vec3 a, b2Vec3 b) { return a.x * b.x + a.y * b.y + a.z * b.z; }