public b2PrismaticJoint(b2PrismaticJointDef def) : base(def) { m_localAnchorA = def.localAnchorA; m_localAnchorB = def.localAnchorB; m_localXAxisA = def.localAxisA; m_localXAxisA.Normalize(); m_localYAxisA = m_localXAxisA.NegUnitCross(); // b2Math.b2Cross(1.0f, m_localXAxisA); m_referenceAngle = def.referenceAngle; m_impulse.SetZero(); m_motorMass = 0.0f; m_motorImpulse = 0.0f; m_lowerTranslation = def.lowerTranslation; m_upperTranslation = def.upperTranslation; m_maxMotorForce = def.maxMotorForce; m_motorSpeed = def.motorSpeed; m_enableLimit = def.enableLimit; m_enableMotor = def.enableMotor; m_limitState = b2LimitState.e_inactiveLimit; m_axis.SetZero(); m_perp.SetZero(); }
public b2RopeJoint(b2RopeJointDef def) : base(def) { m_localAnchorA = def.localAnchorA; m_localAnchorB = def.localAnchorB; m_maxLength = def.maxLength; m_mass = 0.0f; m_impulse = 0.0f; m_state = b2LimitState.e_inactiveLimit; m_length = 0.0f; }
public b2RevoluteJoint(b2RevoluteJointDef def) : base(def) { m_localAnchorA = def.localAnchorA; m_localAnchorB = def.localAnchorB; m_referenceAngle = def.referenceAngle; m_impulse.SetZero(); m_motorImpulse = 0.0f; m_lowerAngle = def.lowerAngle; m_upperAngle = def.upperAngle; m_maxMotorTorque = def.maxMotorTorque; m_motorSpeed = def.motorSpeed; m_enableLimit = def.enableLimit; m_enableMotor = def.enableMotor; m_limitState = b2LimitState.e_inactiveLimit; }
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 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); 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; } 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; 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; }
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; 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; } m_bodyA.InternalVelocity.v = vA; m_bodyA.InternalVelocity.w = wA; m_bodyB.InternalVelocity.v = vB; m_bodyB.InternalVelocity.w = wB; }