internal override void SolveVelocityConstraints(ref SolverData data) { Vector2 vA = data.Velocities[_indexA].V; float wA = data.Velocities[_indexA].W; Vector2 vB = data.Velocities[_indexB].V; float wB = data.Velocities[_indexB].W; float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; // Solve linear motor constraint. if (_enableMotor && _limitState != LimitState.Equal) { float Cdot = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA; float impulse = _motorMass * (_motorSpeed - Cdot); float oldImpulse = MotorImpulse; float maxImpulse = data.Step.dt * _maxMotorForce; MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse); impulse = MotorImpulse - oldImpulse; Vector2 P = impulse * _axis; float LA = impulse * _a1; float LB = impulse * _a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } Vector2 Cdot1 = new Vector2(); Cdot1.X = Vector2.Dot(_perp, vB - vA) + _s2 * wB - _s1 * wA; Cdot1.Y = wB - wA; if (_enableLimit && _limitState != LimitState.Inactive) { // Solve prismatic and limit constraint in block form. float Cdot2; Cdot2 = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA; Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); Vector3 f1 = _impulse; Vector3 df = _K.Solve33(-Cdot); _impulse += df; if (_limitState == LimitState.AtLower) { _impulse.Z = Math.Max(_impulse.Z, 0.0f); } else if (_limitState == LimitState.AtUpper) { _impulse.Z = Math.Min(_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) Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.ez.X, _K.ez.Y); Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y); _impulse.X = f2r.X; _impulse.Y = f2r.Y; df = _impulse - f1; Vector2 P = df.X * _perp + df.Z * _axis; float LA = df.X * _s1 + df.Y + df.Z * _a1; float LB = df.X * _s2 + df.Y + df.Z * _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. Vector2 df = _K.Solve22(-Cdot1); _impulse.X += df.X; _impulse.Y += df.Y; Vector2 P = df.X * _perp; float LA = df.X * _s1 + df.Y; float LB = df.X * _s2 + df.Y; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } data.Velocities[_indexA].V = vA; data.Velocities[_indexA].W = wA; data.Velocities[_indexB].V = vB; data.Velocities[_indexB].W = wB; }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); float positionError, angularError; 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) { Vector2 C1 = cB + rB - cA - rA; positionError = C1.Length(); angularError = 0.0f; Vector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(rA, P); cB += mB * P; aB += iB * MathUtils.Cross(rB, P); } else { Vector2 C1 = cB + rB - cA - rA; float C2 = aB - aA - ReferenceAngle; positionError = C1.Length(); angularError = Math.Abs(C2); Vector3 C = new Vector3(C1.X, C1.Y, C2); Vector3 impulse = -K.Solve33(C); Vector2 P = new Vector2(impulse.X, impulse.Y); cA -= mA * P; aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z); cB += mB * P; aB += iB * (MathUtils.Cross(rB, P) + impulse.Z); } data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; }
internal override void SolveVelocityConstraints(ref SolverData data) { Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; bool fixedRotation = (iA + iB == 0.0f); // Solve motor constraint. if (_enableMotor && _limitState != LimitState.Equal && fixedRotation == false) { float Cdot = wB - wA - _motorSpeed; float impulse = _motorMass * (-Cdot); float oldImpulse = _motorImpulse; float maxImpulse = data.step.dt * _maxMotorTorque; _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } // Solve limit constraint. if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false) { Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); float Cdot2 = wB - wA; Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); Vector3 impulse = -_mass.Solve33(Cdot); if (_limitState == LimitState.Equal) { _impulse += impulse; } else if (_limitState == LimitState.AtLower) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse < 0.0f) { Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y); Vector2 reduced = _mass.Solve22(rhs); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } else { _impulse += impulse; } } else if (_limitState == LimitState.AtUpper) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse > 0.0f) { Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y); Vector2 reduced = _mass.Solve22(rhs); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } else { _impulse += impulse; } } 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 { // Solve point-to-point constraint Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); Vector2 impulse = _mass.Solve22(-Cdot); _impulse.X += impulse.X; _impulse.Y += impulse.Y; vA -= mA * impulse; wA -= iA * MathUtils.Cross(_rA, impulse); vB += mB * impulse; wB += iB * MathUtils.Cross(_rB, impulse); } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
internal override void SolveVelocityConstraints(ref TimeStep step) { Body b1 = BodyA; Body b2 = BodyB; Vector2 v1 = b1.LinearVelocityInternal; float w1 = b1.AngularVelocityInternal; Vector2 v2 = b2.LinearVelocityInternal; float w2 = b2.AngularVelocityInternal; // Solve linear motor constraint. if (_enableMotor && _limitState != LimitState.Equal) { float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; float impulse = _motorMass * (_motorSpeed - Cdot); float oldImpulse = _motorImpulse; float maxImpulse = step.dt * _maxMotorForce; _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; Vector2 P = impulse * _axis; float L1 = impulse * _a1; float L2 = impulse * _a2; v1 -= InvMassA * P; w1 -= InvIA * L1; v2 += InvMassB * P; w2 += InvIB * L2; } Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1); if (_enableLimit && _limitState != LimitState.Inactive) { // Solve prismatic and limit constraint in block form. float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); Vector3 f1 = _impulse; Vector3 df = _K.Solve33(-Cdot); _impulse += df; if (_limitState == LimitState.AtLower) { _impulse.Z = Math.Max(_impulse.Z, 0.0f); } else if (_limitState == LimitState.AtUpper) { _impulse.Z = Math.Min(_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) Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.Col3.X, _K.Col3.Y); Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y); _impulse.X = f2r.X; _impulse.Y = f2r.Y; df = _impulse - f1; Vector2 P = df.X * _perp + df.Z * _axis; float L1 = df.X * _s1 + df.Y + df.Z * _a1; float L2 = df.X * _s2 + df.Y + df.Z * _a2; v1 -= InvMassA * P; w1 -= InvIA * L1; v2 += InvMassB * P; w2 += InvIB * L2; } else { // Limit is inactive, just solve the prismatic constraint in block form. Vector2 df = _K.Solve22(-Cdot1); _impulse.X += df.X; _impulse.Y += df.Y; Vector2 P = df.X * _perp; float L1 = df.X * _s1 + df.Y; float L2 = df.X * _s2 + df.Y; v1 -= InvMassA * P; w1 -= InvIA * L1; v2 += InvMassB * P; w2 += InvIB * L2; } b1.LinearVelocityInternal = v1; b1.AngularVelocityInternal = w1; b2.LinearVelocityInternal = v2; b2.AngularVelocityInternal = w2; }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.Positions[_indexA].C; float aA = data.Positions[_indexA].A; Vector2 cB = data.Positions[_indexB].C; float aB = data.Positions[_indexB].A; Rot qA = new Rot(aA), qB = new Rot(aB); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); float positionError, angularError; 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) { Vector2 C1 = cB + rB - cA - rA; positionError = C1.Length(); angularError = 0.0f; Vector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(rA, P); cB += mB * P; aB += iB * MathUtils.Cross(rB, P); } else { Vector2 C1 = cB + rB - cA - rA; float C2 = aB - aA - ReferenceAngle; positionError = C1.Length(); angularError = Math.Abs(C2); Vector3 C = new Vector3(C1.X, C1.Y, C2); Vector3 impulse; if (K.ez.Z > 0.0f) { impulse = -K.Solve33(C); } else { Vector2 impulse2 = -K.Solve22(C1); impulse = new Vector3(impulse2.X, impulse2.Y, 0.0f); } Vector2 P = new Vector2(impulse.X, impulse.Y); cA -= mA * P; aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z); cB += mB * P; aB += iB * (MathUtils.Cross(rB, P) + impulse.Z); } data.Positions[_indexA].C = cA; data.Positions[_indexA].A = aA; data.Positions[_indexB].C = cB; data.Positions[_indexB].A = aB; return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
internal override void SolveVelocityConstraints(ref TimeStep step) { Body b1 = BodyA; Vector2 v1 = b1.LinearVelocityInternal; float w1 = b1.AngularVelocityInternal; Vector2 v2 = Vector2.Zero; const float w2 = 0; float m1 = b1.InvMass; float i1 = b1.InvI; // Solve motor constraint. if (_enableMotor && _limitState != LimitState.Equal) { float Cdot = w2 - w1 - _motorSpeed; float impulse = _motorMass * (-Cdot); float oldImpulse = _motorImpulse; float maxImpulse = step.dt * _maxMotorTorque; _motorImpulse = MathHelper.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; w1 -= i1 * impulse; } // Solve limit constraint. if (_enableLimit && _limitState != LimitState.Inactive) { Transform xf1; b1.GetTransform(out xf1); Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); Vector2 r2 = _worldAnchor; // Solve point-to-point constraint Vector2 Cdot1 = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1); float Cdot2 = w2 - w1; Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); Vector3 impulse = _mass.Solve33(-Cdot); if (_limitState == LimitState.Equal) { _impulse += impulse; } else if (_limitState == LimitState.AtLower) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse < 0.0f) { Vector2 reduced = _mass.Solve22(-Cdot1); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } } else if (_limitState == LimitState.AtUpper) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse > 0.0f) { Vector2 reduced = _mass.Solve22(-Cdot1); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } } Vector2 P = new Vector2(impulse.X, impulse.Y); v1 -= m1 * P; w1 -= i1 * (MathUtils.Cross(r1, P) + impulse.Z); } else { Transform xf1; b1.GetTransform(out xf1); Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); Vector2 r2 = _worldAnchor; // Solve point-to-point constraint Vector2 Cdot = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1); Vector2 impulse = _mass.Solve22(-Cdot); _impulse.X += impulse.X; _impulse.Y += impulse.Y; v1 -= m1 * impulse; w1 -= i1 * MathUtils.Cross(r1, impulse); } b1.LinearVelocityInternal = v1; b1.AngularVelocityInternal = w1; }
internal override void SolveVelocityConstraints(ref TimeStep step) { Body b1 = BodyA; Body b2 = BodyB; Vector2 v1 = b1.LinearVelocityInternal; float w1 = b1.AngularVelocityInternal; Vector2 v2 = b2.LinearVelocityInternal; float w2 = b2.AngularVelocityInternal; float m1 = b1.InvMass, m2 = b2.InvMass; float i1 = b1.InvI, i2 = b2.InvI; // Solve motor constraint. if (_enableMotor && _limitState != LimitState.Equal) { float Cdot = w2 - w1 - _motorSpeed; float impulse = _motorMass * (-Cdot); float oldImpulse = _motorImpulse; float maxImpulse = step.dt * _maxMotorTorque; _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; w1 -= i1 * impulse; w2 += i2 * impulse; } // Solve limit constraint. if (_enableLimit && _limitState != LimitState.Inactive) { /*Transform xf1, xf2; * b1.GetTransform(out xf1); * b2.GetTransform(out xf2);*/ Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter); Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter); // Solve point-to-point constraint MathUtils.Cross(w2, ref r2, out _tmpVector2); MathUtils.Cross(w1, ref r1, out _tmpVector1); Vector2 Cdot1 = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1; float Cdot2 = w2 - w1; Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); Vector3 impulse = _mass.Solve33(-Cdot); if (_limitState == LimitState.Equal) { _impulse += impulse; } else if (_limitState == LimitState.AtLower) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse < 0.0f) { Vector2 reduced = _mass.Solve22(-Cdot1); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } } else if (_limitState == LimitState.AtUpper) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse > 0.0f) { Vector2 reduced = _mass.Solve22(-Cdot1); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } } Vector2 P = new Vector2(impulse.X, impulse.Y); v1 -= m1 * P; MathUtils.Cross(ref r1, ref P, out _tmpFloat1); w1 -= i1 * (/* r1 x P */ _tmpFloat1 + impulse.Z); v2 += m2 * P; MathUtils.Cross(ref r2, ref P, out _tmpFloat1); w2 += i2 * (/* r2 x P */ _tmpFloat1 + impulse.Z); } else { /*Transform xf1, xf2; * b1.GetTransform(out xf1); * b2.GetTransform(out xf2);*/ _tmpVector1 = LocalAnchorA - b1.LocalCenter; _tmpVector2 = LocalAnchorB - b2.LocalCenter; Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, ref _tmpVector1); Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, ref _tmpVector2); // Solve point-to-point constraint MathUtils.Cross(w2, ref r2, out _tmpVector2); MathUtils.Cross(w1, ref r1, out _tmpVector1); Vector2 Cdot = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1; Vector2 impulse = _mass.Solve22(-Cdot); _impulse.X += impulse.X; _impulse.Y += impulse.Y; v1 -= m1 * impulse; MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1); w1 -= i1 * /* r1 x impulse */ _tmpFloat1; v2 += m2 * impulse; MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1); w2 += i2 * /* r2 x impulse */ _tmpFloat1; } b1.LinearVelocityInternal = v1; b1.AngularVelocityInternal = w1; b2.LinearVelocityInternal = v2; b2.AngularVelocityInternal = w2; }
internal override void SolveVelocityConstraints(TimeStep step) { Body b1 = _body1; Body b2 = _body2; Vector2 v1 = b1._linearVelocity; float w1 = b1._angularVelocity; Vector2 v2 = b2._linearVelocity; float w2 = b2._angularVelocity; // Solve linear motor constraint. if (_enableMotor && _limitState != LimitState.EqualLimits) { float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; float impulse = _motorMass * (_motorSpeed - Cdot); float oldImpulse = _motorImpulse; float maxImpulse = step.Dt * _maxMotorForce; _motorImpulse = Mathf.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; Vector2 P = impulse * _axis; float L1 = impulse * _a1; float L2 = impulse * _a2; v1 -= _invMass1 * P; w1 -= _invI1 * L1; v2 += _invMass2 * P; w2 += _invI2 * L2; } Vector2 Cdot1; Cdot1.x = Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1; Cdot1.y = w2 - w1; if (_enableLimit && _limitState != LimitState.InactiveLimit) { // Solve prismatic and limit constraint in block form. float Cdot2; Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2); Vector3 f1 = _impulse; Vector3 df = _K.Solve33(-Cdot); _impulse += df; if (_limitState == LimitState.AtLowerLimit) { _impulse.z = Mathf.Max(_impulse.z, 0.0f); } else if (_limitState == LimitState.AtUpperLimit) { _impulse.z = Mathf.Min(_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) Vector2 b = -Cdot1 - (_impulse.z - f1.z) * new Vector2(_K.Col3.x, _K.Col3.y); Vector2 f2r = _K.Solve22(b) + new Vector2(f1.x, f1.y); _impulse.x = f2r.x; _impulse.y = f2r.y; df = _impulse - f1; Vector2 P = df.x * _perp + df.z * _axis; float L1 = df.x * _s1 + df.y + df.z * _a1; float L2 = df.x * _s2 + df.y + df.z * _a2; v1 -= _invMass1 * P; w1 -= _invI1 * L1; v2 += _invMass2 * P; w2 += _invI2 * L2; } else { // Limit is inactive, just solve the prismatic constraint in block form. Vector2 df = _K.Solve22(-Cdot1); _impulse.x += df.x; _impulse.y += df.y; Vector2 P = df.x * _perp; float L1 = df.x * _s1 + df.y; float L2 = df.x * _s2 + df.y; v1 -= _invMass1 * P; w1 -= _invI1 * L1; v2 += _invMass2 * P; w2 += _invI2 * L2; } b1._linearVelocity = v1; b1._angularVelocity = w1; b2._linearVelocity = v2; b2._angularVelocity = w2; }
internal override bool SolvePositionConstraints(ref SolverData data) { TSVector2 cA = data.positions[_indexA].c; FP aA = data.positions[_indexA].a; TSVector2 cB = data.positions[_indexB].c; FP aB = data.positions[_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); FP mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; TSVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); TSVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); FP positionError, angularError; 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) { TSVector2 C1 = cB + rB - cA - rA; positionError = C1.magnitude; angularError = 0.0f; TSVector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(rA, P); cB += mB * P; aB += iB * MathUtils.Cross(rB, P); } else { TSVector2 C1 = cB + rB - cA - rA; FP C2 = aB - aA - ReferenceAngle; positionError = C1.magnitude; angularError = FP.Abs(C2); TSVector C = new TSVector(C1.x, C1.y, C2); TSVector impulse = K.Solve33(C) * -1; TSVector2 P = new TSVector2(impulse.x, impulse.y); cA -= mA * P; aA -= iA * (MathUtils.Cross(rA, P) + impulse.z); cB += mB * P; aB += iB * (MathUtils.Cross(rB, P) + impulse.z); } data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }