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); }