internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _indexB = BodyB.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _localCenterB = BodyB._sweep.LocalCenter; _invMassA = BodyA._invMass; _invMassB = BodyB._invMass; _invIA = BodyA._invI; _invIB = BodyB._invI; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); // 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 = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Mat33 K = new Mat33(); 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 (FrequencyHz > 0.0f) { K.GetInverse22(ref _mass); float invM = iA + iB; float m = invM > 0.0f ? 1.0f / invM : 0.0f; float C = aB - aA - ReferenceAngle; // Frequency float omega = 2.0f * MathHelper.Pi * FrequencyHz; // Damping coefficient float d = 2.0f * m * DampingRatio * omega; // Spring stiffness float k = m * omega * omega; // magic formulas float h = data.step.dt; _gamma = h * (d + h * k); _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; _bias = C * h * k * _gamma; invM += _gamma; _mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f; } else if (K.ez.Z == 0.0f) { K.GetInverse22(ref _mass); _gamma = 0.0f; _bias = 0.0f; } else { K.GetSymInverse33(ref _mass); _gamma = 0.0f; _bias = 0.0f; } if (data.step.warmStarting) { // Scale impulses to support a variable time step. _impulse *= data.step.dtRatio; Vector2 P = new Vector2(_impulse.X, _impulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(ref _rA, ref P) + _impulse.Z); vB += mB * P; wB += iB * (MathUtils.Cross(ref _rB, ref P) + _impulse.Z); } else { _impulse = Vector3.Zero; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _indexB = BodyB.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _localCenterB = BodyB._sweep.LocalCenter; _invMassA = BodyA._invMass; _invMassB = BodyB._invMass; _invIA = BodyA._invI; _invIB = BodyB._invI; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Rot qA = new Rot(aA), qB = new Rot(aB); _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); _rB = MathUtils.Mul(qB, LocalAnchorB - _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 = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Mat33 K = new Mat33(); 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 (FrequencyHz > 0.0f) { K.GetInverse22(ref _mass); float invM = iA + iB; float m = invM > 0.0f ? 1.0f / invM : 0.0f; float C = aB - aA - ReferenceAngle; // Frequency float omega = 2.0f * Settings.Pi * FrequencyHz; // Damping coefficient float d = 2.0f * m * DampingRatio * omega; // Spring stiffness float k = m * omega * omega; // magic formulas float h = data.step.dt; _gamma = h * (d + h * k); _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; _bias = C * h * k * _gamma; invM += _gamma; _mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f; } else { K.GetSymInverse33(ref _mass); _gamma = 0.0f; _bias = 0.0f; } if (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _impulse *= data.step.dtRatio; Vector2 P = new Vector2(_impulse.X, _impulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.Z); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + _impulse.Z); } else { _impulse = Vector3.Zero; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _indexB = BodyB.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _localCenterB = BodyB._sweep.LocalCenter; _invMassA = BodyA._invMass; _invMassB = BodyB._invMass; _invIA = BodyA._invI; _invIB = BodyB._invI; FP aA = data.positions[_indexA].a; FPVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; FP aB = data.positions[_indexB].a; FPVector2 vB = data.velocities[_indexB].v; FP wB = data.velocities[_indexB].w; Rot qA = new Rot(aA), qB = new Rot(aB); _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); _rB = MathUtils.Mul(qB, LocalAnchorB - _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] FP mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; Mat33 K = new Mat33(); 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 (FrequencyHz > 0.0f) { K.GetInverse22(ref _mass); FP invM = iA + iB; FP m = invM > 0.0f ? 1.0f / invM : 0.0f; FP C = aB - aA - ReferenceAngle; // Frequency FP omega = 2.0f * Settings.Pi * FrequencyHz; // Damping coefficient FP d = 2.0f * m * DampingRatio * omega; // Spring stiffness FP k = m * omega * omega; // magic formulas FP h = data.step.dt; _gamma = h * (d + h * k); _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; _bias = C * h * k * _gamma; invM += _gamma; _mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f; } else { K.GetSymInverse33(ref _mass); _gamma = 0.0f; _bias = 0.0f; } if (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _impulse *= data.step.dtRatio; FPVector2 P = new FPVector2(_impulse.x, _impulse.y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.z); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + _impulse.z); } else { _impulse = FPVector.zero; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
internal override void InitVelocityConstraints() { m_LocalCenterA = _bodyA.centerOfMass; m_LocalCenterB = _bodyB.centerOfMass; m_InvMassA = _bodyA._invMass; m_InvMassB = _bodyB._invMass; m_InvIA = _bodyA._invI; m_InvIB = _bodyB._invI; float aA = _bodyA.rotation * Mathf.Deg2Rad; Vector2 vA = _bodyA.velocity; float wA = _bodyA.angularVelocity * Mathf.Deg2Rad; float aB = _bodyB.rotation * Mathf.Deg2Rad; Vector2 vB = _bodyB.velocity; float wB = _bodyB.angularVelocity * Mathf.Deg2Rad; Rot qA = new Rot(aA), qB = new Rot(aB); m_RA = MathUtils.Mul(qA, _localAnchorA - m_LocalCenterA); m_RB = MathUtils.Mul(qB, _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; Mat33 K = new Mat33(); K.ex.x = mA + mB + m_RA.y * m_RA.y * iA + m_RB.y * m_RB.y * iB; K.ey.x = -m_RA.y * m_RA.x * iA - m_RB.y * m_RB.x * iB; K.ez.x = -m_RA.y * iA - m_RB.y * iB; K.ex.y = K.ey.x; K.ey.y = mA + mB + m_RA.x * m_RA.x * iA + m_RB.x * m_RB.x * iB; K.ez.y = m_RA.x * iA + m_RB.x * iB; K.ex.z = K.ez.x; K.ey.z = K.ez.y; K.ez.z = iA + iB; if (frequency > 0.0f) { K.GetInverse22(ref m_Mass); float invM = iA + iB; float m = invM > 0.0f ? 1.0f / invM : 0.0f; float C = aB - aA - referenceAngle; // Frequency float omega = 2.0f * Mathf.PI * frequency; // Damping coefficient float d = 2.0f * m * dampingRatio * omega; // Spring stiffness float k = m * omega * omega; // magic formulas float h = Time.deltaTime; 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.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f; } else { K.GetSymInverse33(ref m_Mass); m_Gamma = 0.0f; m_Bias = 0.0f; } // TODO: Scale impulses to support a variable time step. // m_Impulse *= data.step.dtRatio; Vector2 P = new Vector2(m_Impulse.x, m_Impulse.y); vA -= mA * P; wA -= iA * (MathUtils.Cross(m_RA, P) + m_Impulse.z); vB += mB * P; wB += iB * (MathUtils.Cross(m_RB, P) + m_Impulse.z); if (!_bodyA.isKinematic) { _bodyA.velocity = vA; if (!_bodyA.fixedAngle) { _bodyA.angularVelocity = wA * Mathf.Rad2Deg; } } if (!_bodyB.isKinematic) { _bodyB.velocity = vB; if (!_bodyB.fixedAngle) { _bodyB.angularVelocity = wB * Mathf.Rad2Deg; } } }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _indexB = BodyB.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _localCenterB = BodyB._sweep.LocalCenter; _invMassA = BodyA._invMass; _invMassB = BodyB._invMass; _invIA = BodyA._invI; _invIB = BodyB._invI; GGame.Math.Fix64 aA = data.Positions[_indexA].A; Vector2 vA = data.Velocities[_indexA].V; GGame.Math.Fix64 wA = data.Velocities[_indexA].W; GGame.Math.Fix64 aB = data.Positions[_indexB].A; Vector2 vB = data.Velocities[_indexB].V; GGame.Math.Fix64 wB = data.Velocities[_indexB].W; Rot qA = new Rot(aA), qB = new Rot(aB); _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); _rB = MathUtils.Mul(qB, LocalAnchorB - _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] GGame.Math.Fix64 mA = _invMassA, mB = _invMassB; GGame.Math.Fix64 iA = _invIA, iB = _invIB; Mat33 K = new Mat33(); 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 (FrequencyHz > 0.0f) { K.GetInverse22(ref _mass); GGame.Math.Fix64 invM = iA + iB; GGame.Math.Fix64 m = invM > 0.0f ? 1.0f / invM : 0.0f; GGame.Math.Fix64 C = aB - aA - ReferenceAngle; // Frequency GGame.Math.Fix64 omega = 2.0f * Settings.Pi * FrequencyHz; // Damping coefficient GGame.Math.Fix64 d = 2.0f * m * DampingRatio * omega; // Spring stiffness GGame.Math.Fix64 k = m * omega * omega; // magic formulas GGame.Math.Fix64 h = data.Step.dt; _gamma = h * (d + h * k); _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; _bias = C * h * k * _gamma; invM += _gamma; _mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f; } else if (K.ez.Z == 0.0f) { K.GetInverse22(ref _mass); _gamma = 0.0f; _bias = 0.0f; } else { K.GetSymInverse33(ref _mass); _gamma = 0.0f; _bias = 0.0f; } if (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _impulse *= data.Step.dtRatio; Vector2 P = new Vector2(_impulse.X, _impulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.Z); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + _impulse.Z); } else { _impulse = Vector3.Zero; } data.Velocities[_indexA].V = vA; data.Velocities[_indexA].W = wA; data.Velocities[_indexB].V = vB; data.Velocities[_indexB].W = wB; }
internal override void InitVelocityConstraints(ref SolverData data) { m_indexA = BodyA.IslandIndex; m_indexB = BodyB.IslandIndex; m_localCenterA = BodyA.Sweep.LocalCenter; m_localCenterB = BodyB.Sweep.LocalCenter; m_invMassA = BodyA.InvMass; m_invMassB = BodyB.InvMass; m_invIA = BodyA.InvI; m_invIB = BodyB.InvI; //FVector2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; FVector2 vA = data.velocities[m_indexA].v; float wA = data.velocities[m_indexA].w; //FVector2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; FVector2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; Rot qA = new Rot(aA), qB = new Rot(aB); m_rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA); m_rB = MathUtils.Mul(qB, 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; Mat33 K = new Mat33(); K.ex.X = mA + mB + m_rA.Y * m_rA.Y * iA + m_rB.Y * m_rB.Y * iB; K.ey.X = -m_rA.Y * m_rA.X * iA - m_rB.Y * m_rB.X * iB; K.ez.X = -m_rA.Y * iA - m_rB.Y * iB; K.ex.Y = K.ey.X; K.ey.Y = mA + mB + m_rA.X * m_rA.X * iA + m_rB.X * m_rB.X * iB; K.ez.Y = m_rA.X * iA + m_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) { K.GetInverse22(ref _mass); float invM = iA + iB; float m = invM > 0.0f ? 1.0f / invM : 0.0f; float C = aB - aA - ReferenceAngle; // Frequency float omega = 2.0f * Settings.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; _mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f; } else { K.GetSymInverse33(ref _mass); m_gamma = 0.0f; m_bias = 0.0f; } if (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _impulse *= data.step.dtRatio; FVector2 P = new FVector2(_impulse.X, _impulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(m_rA, P) + _impulse.Z); vB += mB * P; wB += iB * (MathUtils.Cross(m_rB, P) + _impulse.Z); } else { _impulse = FVector3.Zero; } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
/// <seealso cref="Joint.initVelocityConstraints(TimeStep)"></seealso> public override void InitVelocityConstraints(SolverData data) { m_indexA = BodyA.IslandIndex; m_indexB = BodyB.IslandIndex; m_localCenterA.Set(BodyA.Sweep.LocalCenter); m_localCenterB.Set(BodyB.Sweep.LocalCenter); m_invMassA = BodyA.InvMass; m_invMassB = BodyB.InvMass; m_invIA = BodyA.InvI; m_invIB = BodyB.InvI; // Vec2 cA = data.positions[m_indexA].c; float aA = data.Positions[m_indexA].A; Vec2 vA = data.Velocities[m_indexA].V; float wA = data.Velocities[m_indexA].W; // Vec2 cB = data.positions[m_indexB].c; float aB = data.Positions[m_indexB].A; Vec2 vB = data.Velocities[m_indexB].V; float wB = data.Velocities[m_indexB].W; Rot qA = Pool.PopRot(); Rot qB = Pool.PopRot(); Vec2 temp = Pool.PopVec2(); qA.Set(aA); qB.Set(aB); // Compute the effective masses. Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(m_localCenterA), m_rA); Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(m_localCenterB), m_rB); // 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; Mat33 K = Pool.PopMat33(); K.Ex.X = mA + mB + m_rA.Y * m_rA.Y * iA + m_rB.Y * m_rB.Y * iB; K.Ey.X = (-m_rA.Y) * m_rA.X * iA - m_rB.Y * m_rB.X * iB; K.Ez.X = (-m_rA.Y) * iA - m_rB.Y * iB; K.Ex.Y = K.Ey.X; K.Ey.Y = mA + mB + m_rA.X * m_rA.X * iA + m_rB.X * m_rB.X * iB; K.Ez.Y = m_rA.X * iA + m_rB.X * iB; K.Ex.Z = K.Ez.X; K.Ey.Z = K.Ez.Y; K.Ez.Z = iA + iB; if (Frequency > 0.0f) { 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 * MathUtils.PI * Frequency; // Damping coefficient float d = 2.0f * 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.Ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f; } else { K.GetSymInverse33(m_mass); m_gamma = 0.0f; m_bias = 0.0f; } if (data.Step.WarmStarting) { Vec2 P = Pool.PopVec2(); // Scale impulses to support a variable time step. m_impulse.MulLocal(data.Step.DtRatio); P.Set(m_impulse.X, m_impulse.Y); vA.X -= mA * P.X; vA.Y -= mA * P.Y; wA -= iA * (Vec2.Cross(m_rA, P) + m_impulse.Z); vB.X += mB * P.X; vB.Y += mB * P.Y; wB += iB * (Vec2.Cross(m_rB, P) + m_impulse.Z); Pool.PushVec2(1); } else { m_impulse.SetZero(); } data.Velocities[m_indexA].V.Set(vA); data.Velocities[m_indexA].W = wA; data.Velocities[m_indexB].V.Set(vB); data.Velocities[m_indexB].W = wB; Pool.PushVec2(1); Pool.PushRot(2); Pool.PushMat33(1); }