Examples of org.jbox2d.common.Vec3

org.jbox2d.common.Vec3
@author Daniel Murphy

    
    temp.set(d).addLocal(r1);
    m_s1 = Vec2.cross(temp, m_perp);
    m_s2 = Vec2.cross(r2, m_perp);
    
    final Vec3 impulse = pool.popVec3();
    final Vec2 C1 = pool.popVec2();
    
    C1.x = Vec2.dot(m_perp, d);
    C1.y = a2 - a1 - m_refAngle;
    
    linearError = MathUtils.max(linearError, MathUtils.abs(C1.x));
    angularError = MathUtils.abs(C1.y);
    
    if (active) {
      float m1 = m_invMassA, m2 = m_invMassB;
      float i1 = m_invIA, i2 = m_invIB;
      
      float k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
      float k12 = i1 * m_s1 + i2 * m_s2;
      float k13 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
      float k22 = i1 + i2;
      float k23 = i1 * m_a1 + i2 * m_a2;
      float k33 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
      
      m_K.col1.set(k11, k12, k13);
      m_K.col2.set(k12, k22, k23);
      m_K.col3.set(k13, k23, k33);
      
      final Vec3 C = pool.popVec3();
      C.x = C1.x;
      C.y = C1.y;
      C.z = C2;
      
      m_K.solve33ToOut(C.negateLocal(), impulse);
      pool.pushVec3(1);
    }
    else {
      float m1 = m_invMassA, m2 = m_invMassB;
      float i1 = m_invIA, i2 = m_invIB;

View Full Code Here

      // Solve prismatic and limit constraint in block form.
      float Cdot2;
      temp.set(v2).subLocal(v1);
      Cdot2 = Vec2.dot(m_axis, temp) + m_a2 * w2 - m_a1 * w1;
      
      final Vec3 Cdot = pool.popVec3();
      Cdot.set(Cdot1.x, Cdot1.y, Cdot2);
      Cdot.negateLocal();
      
      final Vec3 f1 = pool.popVec3();
      f1.set(m_impulse);
      
      final Vec3 df = pool.popVec3();
      m_K.solve33ToOut(Cdot, df);
      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.col3.x, m_K.col3.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

      Mat22.mulToOut(b2.getTransform().R, r2, r2);
      // Vec2 r1 = b2Mul(b1.getTransform().R, m_localAnchor1 - b1.getLocalCenter());
      // Vec2 r2 = b2Mul(b2.getTransform().R, m_localAnchor2 - b2.getLocalCenter());
      
      final Vec2 Cdot1 = pool.popVec2();
      final Vec3 Cdot = pool.popVec3();
      
      // Solve point-to-point constraint
      Vec2.crossToOut(w1, r1, temp);
      Vec2.crossToOut(w2, r2, Cdot1);
      Cdot1.addLocal(v2).subLocal(v1).subLocal(temp);
      float Cdot2 = w2 - w1;
      Cdot.set(Cdot1.x, Cdot1.y, Cdot2);
      // Vec2 Cdot1 = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
      // float Cdot2 = w2 - w1;
      // b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
      
      Vec3 impulse = pool.popVec3();
      m_mass.solve33ToOut(Cdot.negateLocal(), impulse);
      // Cdot.negateLocal(); just leave negated, we don't use later
      
      if (m_limitState == LimitState.EQUAL) {
        m_impulse.addLocal(impulse);

View Full Code Here

    super(argWorld, def);
    m_localAnchorA = new Vec2(def.localAnchorA);
    m_localAnchorB = new Vec2(def.localAnchorB);
    m_referenceAngle = def.referenceAngle;


    m_impulse = new Vec3();
    m_impulse.setZero();
    
    m_mass = new Mat33();
  }

View Full Code Here

    Vec2.crossToOut(wA, rA, temp);
    Vec2.crossToOut(wB, rB, Cdot1);
    Cdot1.addLocal(vB).subLocal(vA).subLocal(temp);
    float Cdot2 = wB - wA;
    
    final Vec3 Cdot = pool.popVec3();
    Cdot.set(Cdot1.x, Cdot1.y, Cdot2);


    final Vec3 impulse = pool.popVec3();
    m_mass.solve33ToOut(Cdot.negateLocal(), impulse); // just leave Cdot negated..
    m_impulse.addLocal(impulse);


    final Vec2 P = pool.popVec2();
    P.set(impulse.x, impulse.y);

View Full Code Here

    m_mass.col3.y = rA.x * iA + rB.x * iB;
    m_mass.col1.z = m_mass.col3.x;
    m_mass.col2.z = m_mass.col3.y;
    m_mass.col3.z = iA + iB;


    final Vec3 C = pool.popVec3();
    final Vec3 impulse = pool.popVec3();
    C.set(C1.x, C1.y, C2);


    m_mass.solve33ToOut(C.negateLocal(), impulse); // just leave c negated..


    final Vec2 P = pool.popVec2();

View Full Code Here


  public OrderedStackVec3(int argStackSize, int argContainerSize) {
    super(argStackSize, argContainerSize);
    pool = new Vec3[argStackSize];
    for (int i = 0; i < argStackSize; i++) {
      pool[i] = new Vec3();
    }
    container = new Vec3[argContainerSize];
    for (int i = 0; i < argContainerSize; i++) {
      container[i] = new Vec3();
    }
  }

View Full Code Here

      Vec3[] sk = new Vec3[argSize];
      for (int i = 0; i < argOld.length; i++) {
        sk[i] = argOld[i];
      }
      for (int i = argOld.length; i < argSize; i++) {
        sk[i] = new Vec3();
      }
      return sk;
    } else {
      Vec3[] sk = new Vec3[argSize];
      for (int i = 0; i < argSize; i++) {
        sk[i] = new Vec3();
      }
      return sk;
    }
  }

View Full Code Here

    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;

View Full Code Here

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


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


      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

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.