Examples of org.jbox2d.common.Vec3

org.jbox2d.common.Vec3
@author Daniel Murphy

      // Solve prismatic and limit constraint in block form.
      float Cdot2;
      temp.set(vB).subLocal(vA);
      Cdot2 = Vec2.dot(m_axis, temp) + m_a2 * wB - m_a1 * wA;


      final Vec3 Cdot = pool.popVec3();
      Cdot.set(Cdot1.x, Cdot1.y, Cdot2);


      final Vec3 f1 = pool.popVec3();
      final 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.0f);
      } else if (m_limitState == LimitState.AT_UPPER) {
        m_impulse.z = MathUtils.min(m_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)
      final Vec2 b = pool.popVec2();
      final 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);


      temp.set(f1.x, f1.y);
      m_K.solve22ToOut(b, f2r);
      f2r.addLocal(temp);
      m_impulse.x = f2r.x;
      m_impulse.y = f2r.y;


      df.set(m_impulse).subLocal(f1);


      final Vec2 P = pool.popVec2();
      temp.set(m_axis).mulLocal(df.z);
      P.set(m_perp).mulLocal(df.x).addLocal(temp);

View Full Code Here

    final Vec2 axis = pool.popVec2();
    final Vec2 perp = pool.popVec2();
    final Vec2 temp = pool.popVec2();
    final Vec2 C1 = pool.popVec2();


    final Vec3 impulse = pool.popVec3();


    Vec2 cA = data.positions[m_indexA].c;
    float aA = data.positions[m_indexA].a;
    Vec2 cB = data.positions[m_indexB].c;
    float aB = data.positions[m_indexB].a;


    qA.set(aA);
    qB.set(aB);


    float mA = m_invMassA, mB = m_invMassB;
    float 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);
    float a1 = Vec2.cross(temp.set(d).addLocal(rA), axis);
    float a2 = Vec2.cross(rB, axis);
    Rot.mulToOutUnsafe(qA, m_localYAxisA, perp);


    float s1 = Vec2.cross(temp.set(d).addLocal(rA), perp);
    float s2 = Vec2.cross(rB, perp);


    C1.x = Vec2.dot(perp, d);
    C1.y = aB - aA - m_referenceAngle;


    float linearError = MathUtils.abs(C1.x);
    float angularError = MathUtils.abs(C1.y);


    boolean active = false;
    float C2 = 0.0f;
    if (m_enableLimit) {
      float translation = Vec2.dot(axis, d);
      if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0f * 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.0f);
        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.0f, Settings.maxLinearCorrection);
        linearError = MathUtils.max(linearError, translation - m_upperTranslation);
        active = true;
      }
    }


    if (active) {
      float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
      float k12 = iA * s1 + iB * s2;
      float k13 = iA * s1 * a1 + iB * s2 * a2;
      float k22 = iA + iB;
      if (k22 == 0.0f) {
        // For fixed rotation
        k22 = 1.0f;
      }
      float k23 = iA * a1 + iB * a2;
      float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;


      final Mat33 K = pool.popMat33();
      K.ex.set(k11, k12, k13);
      K.ey.set(k12, k22, k23);
      K.ez.set(k13, k23, k33);


      final 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 {
      float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
      float k12 = iA * s1 + iB * s2;

View Full Code Here

      }
    }, argSize, new Vec2[argContainerSize]);
    vec3s = new OrderedStack<Vec3>(new Generator<Vec3>() {
      @Override
      public Vec3 gen () {
        return new Vec3();
      }
    }, argSize, new Vec3[argContainerSize]);
    mats = new OrderedStack<Mat22>(new Generator<Mat22>() {
      @Override
      public Mat22 gen () {

View Full Code Here

        return new Vec2();
      }
    };
    vec3s = new OrderedStack<Vec3>(argSize, argContainerSize) {
      protected Vec3 newInstance () {
        return new Vec3();
      }
    };
    mats = new OrderedStack<Mat22>(argSize, argContainerSize) {
      protected Mat22 newInstance () {
        return new Mat22();

View Full Code Here


    mat.ex.set(3, 19, -5);
    mat.ey.set(-1, 1, 4);
    mat.ez.set(-10, 4, 4);


    Vec3 b = new Vec3(4, 1, 2);
    assertEquals(new Vec3(0.096f, 1.1013334f, -.48133332f), mat.solve33(b));


    Vec2 b2 = new Vec2(4, 1);
    assertEquals(new Vec2(0.22727273f, -3.318182f), mat.solve22(b2));
  }

View Full Code Here

    Vec2 b2 = new Vec2(4, 1);
    assertEquals(new Vec2(0.22727273f, -3.318182f), mat.solve22(b2));
  }


  public void testVec3() {
    Vec3 v1 = new Vec3();
    Vec3 v2 = new Vec3();


    assertEquals(new Vec3(1, -15, 36), Vec3.cross(v1.set(9, 3, 1), v2.set(3, 5, 2)));
  }

View Full Code Here

0 1 2 3

TOP

Related Classes of org.jbox2d.common.Vec3

org.jbox2d.dynamics.joints.PrismaticJoint

org.jbox2d.dynamics.joints.RevoluteJoint

org.jbox2d.dynamics.joints.WeldJoint

org.jbox2d.pooling.normal.DefaultWorldPool

org.jbox2d.pooling.OrderedStackVec3

org.jbox2d.utests.MathTest

All source code are property of their respective owners. Java is a trademark of Sun Microsystems, Inc and owned by ORACLE Inc. Contact coftware#gmail.com.