public override bool solvePositionConstraints(SolverData data) { Rot qA = pool.popRot(); Rot qB = pool.popRot(); Vec2 rA = pool.popVec2(); Vec2 rB = pool.popVec2(); Vec2 d = pool.popVec2(); Vec2 axis = pool.popVec2(); Vec2 perp = pool.popVec2(); Vec2 temp = pool.popVec2(); Vec2 C1 = pool.popVec2(); Vec3 impulse = pool.popVec3(); Vec2 cA = data.positions[m_indexA].c; double aA = data.positions[m_indexA].a; Vec2 cB = data.positions[m_indexB].c; double aB = data.positions[m_indexB].a; qA.set(aA); qB.set(aB); double mA = m_invMassA, mB = m_invMassB; double iA = m_invIA, iB = m_invIB; // Compute fresh Jacobians Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA); Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB); d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA); Rot.mulToOutUnsafe(qA, m_localXAxisA, axis); double a1 = Vec2.cross(temp.set(d).addLocal(rA), axis); double a2 = Vec2.cross(rB, axis); Rot.mulToOutUnsafe(qA, m_localYAxisA, perp); double s1 = Vec2.cross(temp.set(d).addLocal(rA), perp); double s2 = Vec2.cross(rB, perp); C1.x = Vec2.dot(perp, d); C1.y = aB - aA - m_referenceAngle; double linearError = MathUtils.abs(C1.x); double angularError = MathUtils.abs(C1.y); bool active = false; double C2 = 0.0d; if (m_enableLimit) { double translation = Vec2.dot(axis, d); if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0d * Settings.linearSlop) { // Prevent large angular corrections C2 = MathUtils.clamp(translation, -Settings.maxLinearCorrection, Settings.maxLinearCorrection); linearError = MathUtils.max(linearError, MathUtils.abs(translation)); active = true; } else if (translation <= m_lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.clamp(translation - m_lowerTranslation + Settings.linearSlop, -Settings.maxLinearCorrection, 0.0d); linearError = MathUtils.max(linearError, m_lowerTranslation - translation); active = true; } else if (translation >= m_upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.clamp(translation - m_upperTranslation - Settings.linearSlop, 0.0d, Settings.maxLinearCorrection); linearError = MathUtils.max(linearError, translation - m_upperTranslation); active = true; } } if (active) { double k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; double k12 = iA * s1 + iB * s2; double k13 = iA * s1 * a1 + iB * s2 * a2; double k22 = iA + iB; if (k22 == 0.0d) { // For fixed rotation k22 = 1.0d; } double k23 = iA * a1 + iB * a2; double k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; Mat33 K = pool.popMat33(); K.ex.set(k11, k12, k13); K.ey.set(k12, k22, k23); K.ez.set(k13, k23, k33); Vec3 C = pool.popVec3(); C.x = C1.x; C.y = C1.y; C.z = C2; K.solve33ToOut(C.negateLocal(), impulse); pool.pushVec3(1); pool.pushMat33(1); } else { double k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; double k12 = iA * s1 + iB * s2; double k22 = iA + iB; if (k22 == 0.0d) { k22 = 1.0d; } Mat22 K = pool.popMat22(); K.ex.set(k11, k12); K.ey.set(k12, k22); // temp is impulse1 K.solveToOut(C1.negateLocal(), temp); C1.negateLocal(); impulse.x = temp.x; impulse.y = temp.y; impulse.z = 0.0d; pool.pushMat22(1); } double Px = impulse.x * perp.x + impulse.z * axis.x; double Py = impulse.x * perp.y + impulse.z * axis.y; double LA = impulse.x * s1 + impulse.y + impulse.z * a1; double LB = impulse.x * s2 + impulse.y + impulse.z * a2; cA.x -= mA * Px; cA.y -= mA * Py; aA -= iA * LA; cB.x += mB * Px; cB.y += mB * Py; aB += iB * LB; // data.positions[m_indexA].c.set(cA); data.positions[m_indexA].a = aA; // data.positions[m_indexB].c.set(cB); data.positions[m_indexB].a = aB; pool.pushVec2(7); pool.pushVec3(1); pool.pushRot(2); return(linearError <= Settings.linearSlop && angularError <= Settings.angularSlop); }
public override void solveVelocityConstraints(SolverData data) { Vec2 vA = data.velocities[m_indexA].v; double wA = data.velocities[m_indexA].w; Vec2 vB = data.velocities[m_indexB].v; double wB = data.velocities[m_indexB].w; double mA = m_invMassA, mB = m_invMassB; double iA = m_invIA, iB = m_invIB; bool fixedRotation = (iA + iB == 0.0d); // Solve motor constraint. if (m_enableMotor && m_limitState != LimitState.EQUAL && fixedRotation == false) { double Cdot = wB - wA - m_motorSpeed; double impulse = -m_motorMass * Cdot; double oldImpulse = m_motorImpulse; double maxImpulse = data.step.dt * m_maxMotorTorque; m_motorImpulse = MathUtils.clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_motorImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } Vec2 temp = pool.popVec2(); // Solve limit constraint. if (m_enableLimit && m_limitState != LimitState.INACTIVE && fixedRotation == false) { Vec2 Cdot1 = pool.popVec2(); Vec3 Cdot = pool.popVec3(); // Solve point-to-point constraint Vec2.crossToOutUnsafe(wA, m_rA, temp); Vec2.crossToOutUnsafe(wB, m_rB, Cdot1); Cdot1.addLocal(vB).subLocal(vA).subLocal(temp); double Cdot2 = wB - wA; Cdot.set(Cdot1.x, Cdot1.y, Cdot2); Vec3 impulse = pool.popVec3(); m_mass.solve33ToOut(Cdot, impulse); impulse.negateLocal(); if (m_limitState == LimitState.EQUAL) { m_impulse.addLocal(impulse); } else if (m_limitState == LimitState.AT_LOWER) { double newImpulse = m_impulse.z + impulse.z; if (newImpulse < 0.0d) { Vec2 rhs = pool.popVec2(); rhs.set(m_mass.ez.x, m_mass.ez.y).mulLocal(m_impulse.z).subLocal(Cdot1); m_mass.solve22ToOut(rhs, temp); impulse.x = temp.x; impulse.y = temp.y; impulse.z = -m_impulse.z; m_impulse.x += temp.x; m_impulse.y += temp.y; m_impulse.z = 0.0d; pool.pushVec2(1); } else { m_impulse.addLocal(impulse); } } else if (m_limitState == LimitState.AT_UPPER) { double newImpulse = m_impulse.z + impulse.z; if (newImpulse > 0.0d) { Vec2 rhs = pool.popVec2(); rhs.set(m_mass.ez.x, m_mass.ez.y).mulLocal(m_impulse.z).subLocal(Cdot1); m_mass.solve22ToOut(rhs, temp); impulse.x = temp.x; impulse.y = temp.y; impulse.z = -m_impulse.z; m_impulse.x += temp.x; m_impulse.y += temp.y; m_impulse.z = 0.0d; pool.pushVec2(1); } else { m_impulse.addLocal(impulse); } } Vec2 P = pool.popVec2(); P.set(impulse.x, impulse.y); vA.x -= mA * P.x; vA.y -= mA * P.y; wA -= iA * (Vec2.cross(m_rA, P) + impulse.z); vB.x += mB * P.x; vB.y += mB * P.y; wB += iB * (Vec2.cross(m_rB, P) + impulse.z); pool.pushVec2(2); pool.pushVec3(2); } else { // Solve point-to-point constraint Vec2 Cdot = pool.popVec2(); Vec2 impulse = pool.popVec2(); Vec2.crossToOutUnsafe(wA, m_rA, temp); Vec2.crossToOutUnsafe(wB, m_rB, Cdot); Cdot.addLocal(vB).subLocal(vA).subLocal(temp); m_mass.solve22ToOut(Cdot.negateLocal(), impulse); // just leave negated m_impulse.x += impulse.x; m_impulse.y += impulse.y; vA.x -= mA * impulse.x; vA.y -= mA * impulse.y; wA -= iA * Vec2.cross(m_rA, impulse); vB.x += mB * impulse.x; vB.y += mB * impulse.y; wB += iB * Vec2.cross(m_rB, impulse); pool.pushVec2(2); } // 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); }
public override void solveVelocityConstraints(SolverData data) { Vec2 vA = data.velocities[m_indexA].v; double wA = data.velocities[m_indexA].w; Vec2 vB = data.velocities[m_indexB].v; double wB = data.velocities[m_indexB].w; double mA = m_invMassA, mB = m_invMassB; double iA = m_invIA, iB = m_invIB; Vec2 temp = pool.popVec2(); // Solve linear motor constraint. if (m_enableMotor && m_limitState != LimitState.EQUAL) { temp.set(vB).subLocal(vA); double Cdot = Vec2.dot(m_axis, temp) + m_a2 * wB - m_a1 * wA; double impulse = m_motorMass * (m_motorSpeed - Cdot); double oldImpulse = m_motorImpulse; double maxImpulse = data.step.dt * m_maxMotorForce; m_motorImpulse = MathUtils.clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_motorImpulse - oldImpulse; Vec2 P = pool.popVec2(); P.set(m_axis).mulLocal(impulse); double LA = impulse * m_a1; double LB = impulse * m_a2; vA.x -= mA * P.x; vA.y -= mA * P.y; wA -= iA * LA; vB.x += mB * P.x; vB.y += mB * P.y; wB += iB * LB; pool.pushVec2(1); } Vec2 Cdot1 = pool.popVec2(); temp.set(vB).subLocal(vA); Cdot1.x = Vec2.dot(m_perp, temp) + m_s2 * wB - m_s1 * wA; Cdot1.y = wB - wA; // System.out.println(Cdot1); if (m_enableLimit && m_limitState != LimitState.INACTIVE) { // Solve prismatic and limit constraint in block form. double Cdot2; temp.set(vB).subLocal(vA); Cdot2 = Vec2.dot(m_axis, temp) + m_a2 * wB - m_a1 * wA; Vec3 Cdot = pool.popVec3(); Cdot.set(Cdot1.x, Cdot1.y, Cdot2); Vec3 f1 = pool.popVec3(); Vec3 df = pool.popVec3(); f1.set(m_impulse); m_K.solve33ToOut(Cdot.negateLocal(), df); // Cdot.negateLocal(); not used anymore m_impulse.addLocal(df); if (m_limitState == LimitState.AT_LOWER) { m_impulse.z = MathUtils.max(m_impulse.z, 0.0d); } else if (m_limitState == LimitState.AT_UPPER) { m_impulse.z = MathUtils.min(m_impulse.z, 0.0d); } // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + // f1(1:2) Vec2 b = pool.popVec2(); Vec2 f2r = pool.popVec2(); temp.set(m_K.ez.x, m_K.ez.y).mulLocal(m_impulse.z - f1.z); b.set(Cdot1).negateLocal().subLocal(temp); m_K.solve22ToOut(b, f2r); f2r.addLocal(f1.x, f1.y); m_impulse.x = f2r.x; m_impulse.y = f2r.y; df.set(m_impulse).subLocal(f1); Vec2 P = pool.popVec2(); temp.set(m_axis).mulLocal(df.z); P.set(m_perp).mulLocal(df.x).addLocal(temp); double LA = df.x * m_s1 + df.y + df.z * m_a1; double LB = df.x * m_s2 + df.y + df.z * m_a2; vA.x -= mA * P.x; vA.y -= mA * P.y; wA -= iA * LA; vB.x += mB * P.x; vB.y += mB * P.y; wB += iB * LB; pool.pushVec2(3); pool.pushVec3(3); } else { // Limit is inactive, just solve the prismatic constraint in block form. Vec2 df = pool.popVec2(); m_K.solve22ToOut(Cdot1.negateLocal(), df); Cdot1.negateLocal(); m_impulse.x += df.x; m_impulse.y += df.y; Vec2 P = pool.popVec2(); P.set(m_perp).mulLocal(df.x); double LA = df.x * m_s1 + df.y; double LB = df.x * m_s2 + df.y; vA.x -= mA * P.x; vA.y -= mA * P.y; wA -= iA * LA; vB.x += mB * P.x; vB.y += mB * P.y; wB += iB * LB; pool.pushVec2(2); } // 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(2); }
public override bool solvePositionConstraints(SolverData data) { Vec2 cA = data.positions[m_indexA].c; double aA = data.positions[m_indexA].a; Vec2 cB = data.positions[m_indexB].c; double aB = data.positions[m_indexB].a; Rot qA = pool.popRot(); Rot qB = pool.popRot(); Vec2 temp = pool.popVec2(); Vec2 rA = pool.popVec2(); Vec2 rB = pool.popVec2(); qA.set(aA); qB.set(aB); double mA = m_invMassA, mB = m_invMassB; double iA = m_invIA, iB = m_invIB; Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA); Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB); double positionError, angularError; Mat33 K = pool.popMat33(); Vec2 C1 = pool.popVec2(); Vec2 P = pool.popVec2(); 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.0d) { C1.set(cB).addLocal(rB).subLocal(cA).subLocal(rA); positionError = C1.length(); angularError = 0.0d; K.solve22ToOut(C1, P); P.negateLocal(); cA.x -= mA * P.x; cA.y -= mA * P.y; aA -= iA * Vec2.cross(rA, P); cB.x += mB * P.x; cB.y += mB * P.y; aB += iB * Vec2.cross(rB, P); } else { C1.set(cB).addLocal(rB).subLocal(cA).subLocal(rA); double C2 = aB - aA - m_referenceAngle; positionError = C1.length(); angularError = MathUtils.abs(C2); Vec3 C = pool.popVec3(); Vec3 impulse = pool.popVec3(); C.set(C1.x, C1.y, C2); K.solve33ToOut(C, impulse); impulse.negateLocal(); P.set(impulse.x, impulse.y); cA.x -= mA * P.x; cA.y -= mA * P.y; aA -= iA * (Vec2.cross(rA, P) + impulse.z); cB.x += mB * P.x; cB.y += mB * P.y; aB += iB * (Vec2.cross(rB, P) + impulse.z); pool.pushVec3(2); } // data.positions[m_indexA].c.set(cA); data.positions[m_indexA].a = aA; // data.positions[m_indexB].c.set(cB); data.positions[m_indexB].a = aB; pool.pushVec2(5); pool.pushRot(2); pool.pushMat33(1); return(positionError <= Settings.linearSlop && angularError <= Settings.angularSlop); }