protected Mat33 newInstance() { return new Mat33(); } };
/** * Solve A * x = b, where b is a column vector. This is more efficient than computing the inverse * in one-shot cases. * * @param b * @return */ public final Vec2 solve22(Vec2 b) { Vec2 x = new Vec2(); solve22ToOut(b, x); return x; }
/** * Solve A * x = b, where b is a column vector. This is more efficient than computing the inverse * in one-shot cases. * * @param b * @return */ public final Vec3 solve33(Vec3 b) { Vec3 x = new Vec3(); solve33ToOut(b, x); return x; }
m_mass.solve33ToOut(Cdot, impulse); impulse.negateLocal(); final 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; final 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; Vec2.crossToOutUnsafe(wB, m_rB, Cdot); Cdot.addLocal(vB).subLocal(vA).subLocal(temp); m_mass.solve22ToOut(Cdot.negateLocal(), impulse); // just leave negated
Mat33.mul22ToOutUnsafe(m_mass, Cdot1, impulse1); impulse1.negateLocal(); Mat33.mulToOutUnsafe(m_mass, Cdot, impulse); impulse.negateLocal(); m_impulse.addLocal(impulse);
K.getInverse22(m_mass); 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;
m_K.solve33ToOut(Cdot.negateLocal(), df); b.set(Cdot1).negateLocal().subLocal(temp); m_K.solve22ToOut(b, f2r); f2r.addLocal(f1.x, f1.y); m_impulse.x = f2r.x; m_K.solve22ToOut(Cdot1.negateLocal(), df); Cdot1.negateLocal();
Mat33.mul22ToOutUnsafe(m_mass, Cdot1, impulse1); impulse1.negateLocal(); Mat33.mulToOutUnsafe(m_mass, Cdot, impulse); impulse.negateLocal(); m_impulse.addLocal(impulse);
K.getInverse22(m_mass); 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;
angularError = 0.0f; K.solve22ToOut(C1, P); P.negateLocal(); C.set(C1.x, C1.y, C2); K.solve33ToOut(C, impulse); impulse.negateLocal(); P.set(impulse.x, impulse.y);
Mat33.mul22ToOutUnsafe(m_mass, Cdot1, impulse1); impulse1.negateLocal(); Mat33.mulToOutUnsafe(m_mass, Cdot, impulse); impulse.negateLocal(); m_impulse.addLocal(impulse);
K.getInverse22(m_mass); 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;
/** * Solve A * x = b, where b is a column vector. This is more efficient than computing the inverse * in one-shot cases. * * @param b * @return */ public final Vec3 solve33(Vec3 b) { Vec3 x = new Vec3(); solve33ToOut(b, x); return x; }
protected PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def) { super(argWorld, def); m_localAnchorA = new Vec2(def.localAnchorA); m_localAnchorB = new Vec2(def.localAnchorB); m_localXAxisA = new Vec2(def.localAxisA); m_localXAxisA.normalize(); m_localYAxisA = new Vec2(); Vec2.crossToOutUnsafe(1f, m_localXAxisA, m_localYAxisA); m_referenceAngle = def.referenceAngle; m_impulse = new Vec3(); m_motorMass = 0.0f; m_motorImpulse = 0.0f; m_lowerTranslation = def.lowerTranslation; m_upperTranslation = def.upperTranslation; m_maxMotorForce = def.maxMotorForce; m_motorSpeed = def.motorSpeed; m_enableLimit = def.enableLimit; m_enableMotor = def.enableMotor; m_limitState = LimitState.INACTIVE; m_K = new Mat33(); m_axis = new Vec2(); m_perp = new Vec2(); }
/** * Solve A * x = b, where b is a column vector. This is more efficient than computing the inverse * in one-shot cases. * * @param b * @return */ public final Vec2 solve22(Vec2 b) { Vec2 x = new Vec2(); solve22ToOut(b, x); return x; }
m_mass.solve33ToOut(Cdot, impulse); impulse.negateLocal(); final 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; final 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; Vec2.crossToOutUnsafe(wB, m_rB, Cdot); Cdot.addLocal(vB).subLocal(vA).subLocal(temp); m_mass.solve22ToOut(Cdot.negateLocal(), impulse); // just leave negated
Mat33.mul22ToOutUnsafe(m_mass, Cdot1, impulse1); impulse1.negateLocal(); Mat33.mulToOutUnsafe(m_mass, Cdot, impulse); impulse.negateLocal(); m_impulse.addLocal(impulse);
K.getInverse22(m_mass); 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;
C.z = C2; K.solve33ToOut(C.negateLocal(), impulse); pool.pushVec3(1); pool.pushMat33(1);
protected Mat33 newInstance() { return new Mat33(); } };