void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data) { b2Vec2 vA = data.velocities[m_indexA].v; float32 wA = data.velocities[m_indexA].w; b2Vec2 vB = data.velocities[m_indexB].v; float32 wB = data.velocities[m_indexB].w; float32 mA = m_invMassA, mB = m_invMassB; float32 iA = m_invIA, iB = m_invIB; if (m_frequencyHz > 0.0f) { float32 Cdot2 = wB - wA; float32 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 + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); b2Vec2 impulse1 = -b2Mul22(m_mass, Cdot1); m_impulse.x += impulse1.x; m_impulse.y += impulse1.y; b2Vec2 P = impulse1; vA -= mA * P; wA -= iA * b2Cross(m_rA, P); vB += mB * P; wB += iB * b2Cross(m_rB, P); } else { b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); float32 Cdot2 = wB - wA; b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); b2Vec3 impulse = -b2Mul(m_mass, Cdot); m_impulse += impulse; b2Vec2 P(impulse.x, impulse.y); vA -= mA * P; wA -= iA * (b2Cross(m_rA, P) + impulse.z); vB += mB * P; wB += iB * (b2Cross(m_rB, P) + impulse.z); } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data) { b2Vec2 vA = data.velocities[m_indexA].v; float32 wA = data.velocities[m_indexA].w; b2Vec2 vB = data.velocities[m_indexB].v; float32 wB = data.velocities[m_indexB].w; float32 mA = m_invMassA, mB = m_invMassB; float32 iA = m_invIA, iB = m_invIB; // Solve linear motor constraint. if (m_enableMotor && m_limitState != e_equalLimits) { float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; float32 impulse = m_motorMass * (m_motorSpeed - Cdot); float32 oldImpulse = m_motorImpulse; float32 maxImpulse = data.step.dt * m_maxMotorForce; m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_motorImpulse - oldImpulse; b2Vec2 P = impulse * m_axis; float32 LA = impulse * m_a1; float32 LB = impulse * m_a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } b2Vec2 Cdot1; Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA; Cdot1.y = wB - wA; if (m_enableLimit && m_limitState != e_inactiveLimit) { // Solve prismatic and limit constraint in block form. float32 Cdot2; Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); b2Vec3 f1 = m_impulse; b2Vec3 df = m_K.Solve33(-Cdot); m_impulse += df; if (m_limitState == e_atLowerLimit) { m_impulse.z = b2Max(m_impulse.z, 0.0f); } else if (m_limitState == e_atUpperLimit) { m_impulse.z = b2Min(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) * b2Vec2(m_K.ez.x, m_K.ez.y); b2Vec2 f2r = m_K.Solve22(b) + 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; float32 LA = df.x * m_s1 + df.y + df.z * m_a1; float32 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; float32 LA = df.x * m_s1 + df.y; float32 LB = df.x * m_s2 + df.y; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; b2Vec2 Cdot10 = Cdot1; Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA; Cdot1.y = wB - wA; if (b2Abs(Cdot1.x) > 0.01f || b2Abs(Cdot1.y) > 0.01f) { b2Vec2 test = 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; }