本文整理汇总了C++中qB函数的典型用法代码示例。如果您正苦于以下问题：C++ qB函数的具体用法？C++ qB怎么用？C++ qB使用的例子？那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了qB函数的10个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: qA
bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data)
{
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Vec2 cC = data.positions[m_indexC].c;
float32 aC = data.positions[m_indexC].a;
b2Vec2 cD = data.positions[m_indexD].c;
float32 aD = data.positions[m_indexD].a;
b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
float32 linearError = 0.0f;
float32 coordinateA, coordinateB;
b2Vec2 JvAC, JvBD;
float32 JwA, JwB, JwC, JwD;
float32 mass = 0.0f;
if (m_typeA == e_revoluteJoint)
{
JvAC.SetZero();
JwA = 1.0f;
JwC = 1.0f;
mass += m_iA + m_iC;
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
b2Vec2 u = b2Mul(qC, m_localAxisC);
b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
JvAC = u;
JwC = b2Cross(rC, u);
JwA = b2Cross(rA, u);
mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;
b2Vec2 pC = m_localAnchorC - m_lcC;
b2Vec2 pA = b2MulT(qC, rA + (cA - cC));
coordinateA = b2Dot(pA - pC, m_localAxisC);
}
if (m_typeB == 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
{
b2Vec2 u = b2Mul(qD, m_localAxisD);
b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
JvBD = m_ratio * u;
JwD = m_ratio * b2Cross(rD, u);
JwB = m_ratio * b2Cross(rB, u);
mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;
b2Vec2 pD = m_localAnchorD - m_lcD;
b2Vec2 pB = b2MulT(qD, rB + (cB - cD));
coordinateB = b2Dot(pB - pD, m_localAxisD);
}
float32 C = (coordinateA + m_ratio * coordinateB) - m_constant;
float32 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 < b2_linearSlop;
}示例2: qA
void b2RopeJoint::InitVelocityConstraints(const b2SolverData& 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;
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
b2Rot qA(aA), qB(aB);
m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
m_u = cB + m_rB - cA - m_rA;
m_length = m_u.Length();
float32 C = m_length - m_maxLength;
if (C > 0.0f)
{
m_state = e_atUpperLimit;
}
else
{
m_state = e_inactiveLimit;
}
if (m_length > b2_linearSlop)
{
m_u *= 1.0f / m_length;
}
else
{
m_u.SetZero();
m_mass = 0.0f;
m_impulse = 0.0f;
return;
}
// Compute effective mass.
float32 crA = b2Cross(m_rA, m_u);
float32 crB = b2Cross(m_rB, m_u);
float32 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 * b2Cross(m_rA, P);
vB += m_invMassB * P;
wB += m_invIB * 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;
}示例3: qA
bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data)
{
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Rot qA(aA), qB(aB);
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
// Compute fresh Jacobians
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 d = cB + rB - cA - rA;
b2Vec2 axis = b2Mul(qA, m_localXAxisA);
float32 a1 = b2Cross(d + rA, axis);
float32 a2 = b2Cross(rB, axis);
b2Vec2 perp = b2Mul(qA, m_localYAxisA);
float32 s1 = b2Cross(d + rA, perp);
float32 s2 = b2Cross(rB, perp);
b2Vec3 impulse;
b2Vec2 C1;
C1.x = b2Dot(perp, d);
C1.y = aB - aA - m_referenceAngle;
float32 linearError = b2Abs(C1.x);
float32 angularError = b2Abs(C1.y);
bool active = false;
float32 C2 = 0.0f;
if (m_enableLimit)
{
float32 translation = b2Dot(axis, d);
if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
{
// Prevent large angular corrections
C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
linearError = b2Max(linearError, b2Abs(translation));
active = true;
}
else if (translation <= m_lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
linearError = b2Max(linearError, m_lowerTranslation - translation);
active = true;
}
else if (translation >= m_upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
linearError = b2Max(linearError, translation - m_upperTranslation);
active = true;
}
}
if (active)
{
float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float32 k12 = iA * s1 + iB * s2;
float32 k13 = iA * s1 * a1 + iB * s2 * a2;
float32 k22 = iA + iB;
if (k22 == 0.0f)
{
// For fixed rotation
k22 = 1.0f;
}
float32 k23 = iA * a1 + iB * a2;
float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
b2Mat33 K;
K.ex.Set(k11, k12, k13);
K.ey.Set(k12, k22, k23);
K.ez.Set(k13, k23, k33);
b2Vec3 C;
C.x = C1.x;
C.y = C1.y;
C.z = C2;
impulse = K.Solve33(-C);
}
else
{
float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float32 k12 = iA * s1 + iB * s2;
float32 k22 = iA + iB;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
b2Mat22 K;
K.ex.Set(k11, k12);
K.ey.Set(k12, k22);
//.........这里部分代码省略.........
示例4: qA
void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& 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;
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
b2Rot qA(aA), qB(aB);
m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
m_u = cB + m_rB - cA - m_rA;
// Handle singularity.
float32 length = m_u.Length();
if (length > b2_linearSlop)
{
m_u *= 1.0f / length;
}
else
{
m_u.Set(0.0f, 0.0f);
}
float32 crAu = b2Cross(m_rA, m_u);
float32 crBu = b2Cross(m_rB, m_u);
float32 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)
{
float32 C = length - m_length;
// Frequency
float32 omega = 2.0f * b2_pi * m_frequencyHz;
// Damping coefficient
float32 d = 2.0f * m_mass * m_dampingRatio * omega;
// Spring stiffness
float32 k = m_mass * omega * omega;
// magic formulas
float32 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 * b2Cross(m_rA, P);
vB += m_invMassB * P;
wB += m_invIB * 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;
}示例5: qA
bool b2WeldJoint::SolvePositionConstraints(const 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(aA), qB(aB);
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
float positionError, angularError;
b2Mat33 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)
{
b2Vec2 C1 = cB + rB - cA - rA;
positionError = C1.Length();
angularError = 0.0f;
b2Vec2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * b2Cross(rA, P);
cB += mB * P;
aB += iB * b2Cross(rB, P);
}
else
{
b2Vec2 C1 = cB + rB - cA - rA;
float C2 = aB - aA - m_referenceAngle;
positionError = C1.Length();
angularError = b2Abs(C2);
b2Vec3 C(C1.x, C1.y, C2);
b2Vec3 impulse = -K.Solve33(C);
b2Vec2 P(impulse.x, impulse.y);
cA -= mA * P;
aA -= iA * (b2Cross(rA, P) + impulse.z);
cB += mB * P;
aB += iB * (b2Cross(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 <= b2_linearSlop && angularError <= b2_angularSlop;
}示例6: qA
void b2MotorJoint::InitVelocityConstraints(const b2SolverData& 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;
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
b2Rot qA(aA), qB(aB);
// Compute the effective mass matrix.
m_rA = b2Mul(qA, -m_localCenterA);
m_rB = b2Mul(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]
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
b2Mat22 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 - b2Mul(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;
b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
vA -= mA * P;
wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse);
vB += mB * P;
wB += iB * (b2Cross(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;
}示例7: qA
void b2WheelJoint::InitVelocityConstraints(const b2SolverData& 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;
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
b2Rot qA(aA), qB(aB);
// Compute the effective masses.
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 d = cB + rB - cA - rA;
// Point to line constraint
{
m_ay = b2Mul(qA, m_localYAxisA);
m_sAy = b2Cross(d + rA, m_ay);
m_sBy = b2Cross(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 = b2Mul(qA, m_localXAxisA);
m_sAx = b2Cross(d + rA, m_ax);
m_sBx = b2Cross(rB, m_ax);
float32 invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx;
if (invMass > 0.0f)
{
m_springMass = 1.0f / invMass;
float32 C = b2Dot(d, m_ax);
// Frequency
float32 omega = 2.0f * b2_pi * m_frequencyHz;
// Damping coefficient
float32 d = 2.0f * m_springMass * m_dampingRatio * omega;
// Spring stiffness
float32 k = m_springMass * omega * omega;
// magic formulas
float32 h = data.step.dt;
m_gamma = h * (d + 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)
{
//.........这里部分代码省略.........
示例8: qA
void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& 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;
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
b2Rot qA(aA), qB(aB);
m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
// Get the pulley axes.
m_uA = cA + m_rA - m_groundAnchorA;
m_uB = cB + m_rB - m_groundAnchorB;
float32 lengthA = m_uA.Length();
float32 lengthB = m_uB.Length();
if (lengthA > 10.0f * b2_linearSlop)
{
m_uA *= 1.0f / lengthA;
}
else
{
m_uA.SetZero();
}
if (lengthB > 10.0f * b2_linearSlop)
{
m_uB *= 1.0f / lengthB;
}
else
{
m_uB.SetZero();
}
// Compute effective mass.
float32 ruA = b2Cross(m_rA, m_uA);
float32 ruB = b2Cross(m_rB, m_uB);
float32 mA = m_invMassA + m_invIA * ruA * ruA;
float32 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 * b2Cross(m_rA, PA);
vB += m_invMassB * PB;
wB += m_invIB * 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;
}示例9: qA
void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& 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;
float32 aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
b2Rot qA(aA), qB(aB);
m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = 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]
float32 mA = m_invMassA, mB = m_invMassB;
float32 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)
{
float32 jointAngle = aB - aA - m_referenceAngle;
if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
{
m_limitState = e_equalLimits;
}
else if (jointAngle <= m_lowerAngle)
{
if (m_limitState != e_atLowerLimit)
{
m_impulse.z = 0.0f;
}
m_limitState = e_atLowerLimit;
}
else if (jointAngle >= m_upperAngle)
{
if (m_limitState != e_atUpperLimit)
{
m_impulse.z = 0.0f;
}
m_limitState = e_atUpperLimit;
}
else
{
m_limitState = e_inactiveLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_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;
b2Vec2 P(m_impulse.x, m_impulse.y);
//.........这里部分代码省略.........
示例10: qB
void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data)
{
m_indexB = m_bodyB->m_islandIndex;
m_localCenterB = m_bodyB->m_sweep.localCenter;
m_invMassB = m_bodyB->m_invMass;
m_invIB = m_bodyB->m_invI;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
b2Rot qB(aB);
float32 mass = m_bodyB->GetMass();
// Frequency
float32 omega = 2.0f * b2_pi * m_frequencyHz;
// Damping coefficient
float32 d = 2.0f * mass * m_dampingRatio * omega;
// Spring stiffness
float32 k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
float32 h = data.step.dt;
b2Assert(d + h * k > b2_epsilon);
m_gamma = h * (d + h * k);
if (m_gamma != 0.0f)
{
m_gamma = 1.0f / m_gamma;
}
m_beta = h * k * m_gamma;
// Compute the effective mass matrix.
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
b2Mat22 K;
K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
K.ex.y = -m_invIB * m_rB.x * m_rB.y;
K.ey.x = K.ex.y;
K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;
m_mass = K.GetInverse();
m_C = cB + m_rB - m_targetA;
m_C *= m_beta;
// Cheat with some damping
wB *= 0.98f;
if (data.step.warmStarting)
{
m_impulse *= data.step.dtRatio;
vB += m_invMassB * m_impulse;
wB += m_invIB * b2Cross(m_rB, m_impulse);
}
else
{
m_impulse.SetZero();
}
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}