public override void SolveVelocityConstraints(b2SolverData data) { b2Vec2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; // Cdot = v + cross(w, r) b2Vec2 Cdot = vB + b2Math.b2Cross(wB, m_rB); b2Vec2 impulse = b2Math.b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse)); b2Vec2 oldImpulse = m_impulse; m_impulse += impulse; float maxImpulse = data.step.dt * m_maxForce; if (m_impulse.LengthSquared() > maxImpulse * maxImpulse) { m_impulse *= maxImpulse / m_impulse.Length(); } impulse = m_impulse - oldImpulse; vB += m_invMassB * impulse; wB += m_invIB * b2Math.b2Cross(m_rB, impulse); data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
public static float b2Distance(b2Vec2 a, b2Vec2 b) { b2Vec2 c = a - b; return(c.Length()); }
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); m_u = cB + m_rB - cA - m_rA; // Handle singularity. float length = m_u.Length(); if (length > b2Settings.b2_linearSlop) { m_u *= 1.0f / length; } else { m_u.Set(0.0f, 0.0f); } float crAu = b2Math.b2Cross(m_rA, m_u); float crBu = b2Math.b2Cross(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; b2Vec2 P = m_impulse * m_u; vA -= m_invMassA * P; wA -= m_invIA * b2Math.b2Cross(m_rA, P); vB += m_invMassB * P; wB += m_invIB * b2Math.b2Cross(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; }
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); // 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 * b2Settings.b2_linearSlop) { m_uA *= 1.0f / lengthA; } else { m_uA.SetZero(); } if (lengthB > 10.0f * b2Settings.b2_linearSlop) { m_uB *= 1.0f / lengthB; } else { m_uB.SetZero(); } // Compute effective mass. float ruA = b2Math.b2Cross(m_rA, m_uA); float ruB = b2Math.b2Cross(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. b2Vec2 PA = -(m_impulse) * m_uA; b2Vec2 PB = (-m_ratio * m_impulse) * m_uB; vA += m_invMassA * PA; wA += m_invIA * b2Math.b2Cross(m_rA, PA); vB += m_invMassB * PB; wB += m_invIB * b2Math.b2Cross(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; }
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); 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 = b2LimitState.e_atUpperLimit; } else { m_state = b2LimitState.e_inactiveLimit; } if (m_length > b2Settings.b2_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 = b2Math.b2Cross(m_rA, m_u); float crB = b2Math.b2Cross(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; b2Vec2 P = m_impulse * m_u; vA -= m_invMassA * P; wA -= m_invIA * b2Math.b2Cross(m_rA, P); vB += m_invMassB * P; wB += m_invIB * b2Math.b2Cross(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; }