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); TSVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); TSVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); // Get the pulley axes. TSVector2 uA = cA + rA - WorldAnchorA; TSVector2 uB = cB + rB - WorldAnchorB; FP lengthA = uA.magnitude; FP lengthB = uB.magnitude; if (lengthA > 10.0f * Settings.LinearSlop) { uA *= 1.0f / lengthA; } else { uA = TSVector2.zero; } if (lengthB > 10.0f * Settings.LinearSlop) { uB *= 1.0f / lengthB; } else { uB = TSVector2.zero; } // Compute effective mass. FP ruA = MathUtils.Cross(rA, uA); FP ruB = MathUtils.Cross(rB, uB); FP mA = _invMassA + _invIA * ruA * ruA; FP mB = _invMassB + _invIB * ruB * ruB; FP mass = mA + Ratio * Ratio * mB; if (mass > 0.0f) { mass = 1.0f / mass; } FP C = Constant - lengthA - Ratio * lengthB; FP linearError = FP.Abs(C); FP impulse = -mass * C; TSVector2 PA = -impulse * uA; TSVector2 PB = -Ratio * impulse * uB; cA += _invMassA * PA; aA += _invIA * MathUtils.Cross(rA, PA); cB += _invMassB * PB; aB += _invIB * MathUtils.Cross(rB, PB); data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(linearError < Settings.LinearSlop); }
internal override bool SolvePositionConstraints(ref SolverData data) { //no position solving for this joint return(true); }
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; TSVector2 cA = data.positions[_indexA].c; FP aA = data.positions[_indexA].a; TSVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; TSVector2 cB = data.positions[_indexB].c; FP aB = data.positions[_indexB].a; TSVector2 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); // Get the pulley axes. _uA = cA + _rA - WorldAnchorA; _uB = cB + _rB - WorldAnchorB; FP lengthA = _uA.magnitude; FP lengthB = _uB.magnitude; if (lengthA > 10.0f * Settings.LinearSlop) { _uA *= 1.0f / lengthA; } else { _uA = TSVector2.zero; } if (lengthB > 10.0f * Settings.LinearSlop) { _uB *= 1.0f / lengthB; } else { _uB = TSVector2.zero; } // Compute effective mass. FP ruA = MathUtils.Cross(_rA, _uA); FP ruB = MathUtils.Cross(_rB, _uB); FP mA = _invMassA + _invIA * ruA * ruA; FP mB = _invMassB + _invIB * ruB * ruB; _mass = mA + Ratio * Ratio * mB; if (_mass > 0.0f) { _mass = 1.0f / _mass; } if (Settings.EnableWarmstarting) { // Scale impulses to support variable time steps. _impulse *= data.step.dtRatio; // Warm starting. TSVector2 PA = -(_impulse) * _uA; TSVector2 PB = (-Ratio * _impulse) * _uB; vA += _invMassA * PA; wA += _invIA * MathUtils.Cross(_rA, PA); vB += _invMassB * PB; wB += _invIB * MathUtils.Cross(_rB, PB); } else { _impulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }