Package org.jbox2d.dynamics

Examples of org.jbox2d.dynamics.Body


    return m_motorImpulse;
  }
 
  @Override
  public void initVelocityConstraints(TimeStep step) {
    Body b1 = m_bodyA;
    Body b2 = m_bodyB;
   
    m_localCenterA.set(b1.getLocalCenter());
    m_localCenterB.set(b2.getLocalCenter());
   
    Transform xf1 = b1.getTransform();
    Transform xf2 = b2.getTransform();
   
    // Compute the effective masses.
   
    final Vec2 temp = pool.popVec2();
    final Vec2 r1 = pool.popVec2();
View Full Code Here


  }
 
  @Override
  public boolean solvePositionConstraints(float baumgarte) {
   
    Body b1 = m_bodyA;
    Body b2 = m_bodyB;
   
    Vec2 c1 = b1.m_sweep.c;
    float a1 = b1.m_sweep.a;
   
    Vec2 c2 = b2.m_sweep.c;
    float a2 = b2.m_sweep.a;

   
    // Solve linear limit constraint.
    float linearError = 0.0f, angularError = 0.0f;
    boolean active = false;
    float C2 = 0.0f;
   
    final Mat22 R1 = pool.popMat22();
    final Mat22 R2 = pool.popMat22();
    R1.set(a1);
    R2.set(a2);
   
    final Vec2 temp = pool.popVec2();
    final Vec2 r1 = pool.popVec2();
    final Vec2 r2 = pool.popVec2();
    final Vec2 d = pool.popVec2();
   
    r1.set(m_localAnchor1).subLocal(m_localCenterA);
    r2.set(m_localAnchor2).subLocal(m_localCenterB);
    Mat22.mulToOut(R1, r1, r1);
    Mat22.mulToOut(R2, r2, r2);
    d.set(c2).addLocal(r2).subLocal(c1).subLocal(r1);
   
    if (m_enableLimit) {
      Mat22.mulToOut(R1, m_localXAxis1, m_axis);
     
      temp.set(d).addLocal(r1);
      m_a1 = Vec2.cross(temp, m_axis);
      m_a2 = Vec2.cross(r2, m_axis);
     
      float translation = Vec2.dot(m_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.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 = 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 = translation - m_upperTranslation;
        active = true;
      }
    }
   
    Mat22.mulToOut(R1, m_localYAxis1, m_perp);
   
    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;
     
      float k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
      float k12 = i1 * m_s1 + i2 * m_s2;
      float k22 = i1 + i2;
     
      m_K.col1.set(k11, k12, 0.0f);
      m_K.col2.set(k12, k22, 0.0f);
     
      m_K.solve22ToOut(C1.negateLocal(), temp);
      C1.negateLocal();
     
      impulse.x = temp.x;
      impulse.y = temp.y;
      impulse.z = 0.0f;
    }
   
    final Vec2 P = pool.popVec2();
    temp.set(m_perp).mulLocal(impulse.x);
    P.set(m_axis).mulLocal(impulse.z).addLocal(temp);
    float L1 = impulse.x * m_s1 + impulse.y + impulse.z * m_a1;
    float L2 = impulse.x * m_s2 + impulse.y + impulse.z * m_a2;
   
    temp.set(P).mulLocal(m_invMassA);
    c1.subLocal(temp);
    a1 -= m_invIA * L1;
   
    temp.set(P).mulLocal(m_invMassB);
    c2.addLocal(temp);
    a2 += m_invIB * L2;
   
    // TODO_ERIN remove need for this.
    b1.m_sweep.c.set(c1);
    b1.m_sweep.a = a1;

    b2.m_sweep.c.set(c2);
    b2.m_sweep.a = a2;
    b1.synchronizeTransform();
    b2.synchronizeTransform();
   
    pool.pushVec2(6);
    pool.pushVec3(1);
    pool.pushMat22(2);
   
View Full Code Here

    return linearError <= Settings.linearSlop && angularError <= Settings.angularSlop;
  }
 
  @Override
  public void solveVelocityConstraints(TimeStep step) {
    Body b1 = m_bodyA;
    Body b2 = m_bodyB;
   
    Vec2 v1 = b1.m_linearVelocity;
    float w1 = b1.m_angularVelocity;
    Vec2 v2 = b2.m_linearVelocity;
    float w2 = b2.m_angularVelocity;
View Full Code Here

  /**
   * @see org.jbox2d.dynamics.joints.Joint#initVelocityConstraints(org.jbox2d.dynamics.TimeStep)
   */
  @Override
  public void initVelocityConstraints(TimeStep step) {
    Body g1 = m_ground1;
    Body g2 = m_ground2;
    Body b1 = m_bodyA;
    Body b2 = m_bodyB;
   
    float K = 0.0f;
    m_J.setZero();
   
    if (m_revolute1 != null) {
      m_J.angularA = -1.0f;
      K += b1.m_invI;
    }
    else {
      final Vec2 ug = pool.popVec2();
      final Vec2 r = pool.popVec2();
      Mat22.mulToOut(g1.getTransform().R, m_prismatic1.m_localXAxis1, ug);
     
      r.set(m_localAnchor1).subLocal(b1.getLocalCenter());
      Mat22.mulToOut(b1.getTransform().R, r, r);
      float crug = Vec2.cross(r, ug);
      m_J.linearA.set(ug).negateLocal();
      m_J.angularA = -crug;
      K += b1.m_invMass + b1.m_invI * crug * crug;
      pool.pushVec2(2);
    }
   
    if (m_revolute2 != null) {
      m_J.angularB = -m_ratio;
      K += m_ratio * m_ratio * b2.m_invI;
    }
    else {
      final Vec2 ug = pool.popVec2();
      final Vec2 r = pool.popVec2();
     
      Mat22.mulToOut(g2.getTransform().R, m_prismatic2.m_localXAxis1, ug);
     
      r.set(m_localAnchor2).subLocal(b2.getLocalCenter());
      Mat22.mulToOut(b2.getTransform().R, r, r);
      float crug = Vec2.cross(r, ug);
      m_J.linearB.set(ug).mulLocal(-m_ratio);
      m_J.angularB = -m_ratio * crug;
      K += m_ratio * m_ratio * (b2.m_invMass + b2.m_invI * crug * crug);
     
 
View Full Code Here

  /**
   * @see org.jbox2d.dynamics.joints.Joint#solveVelocityConstraints(org.jbox2d.dynamics.TimeStep)
   */
  @Override
  public void solveVelocityConstraints(TimeStep step) {
    Body b1 = m_bodyA;
    Body b2 = m_bodyB;
   
    float Cdot = m_J.compute(b1.m_linearVelocity, b1.m_angularVelocity, b2.m_linearVelocity, b2.m_angularVelocity);
   
    float impulse = m_mass * (-Cdot);
    m_impulse += impulse;
View Full Code Here

   */
  @Override
  public boolean solvePositionConstraints(float baumgarte) {
    float linearError = 0.0f;
   
    Body b1 = m_bodyA;
    Body b2 = m_bodyB;
   
    float coordinate1, coordinate2;
    if (m_revolute1 != null) {
      coordinate1 = m_revolute1.getJointAngle();
    }
    else {
      coordinate1 = m_prismatic1.getJointTranslation();
    }
   
    if (m_revolute2 != null) {
      coordinate2 = m_revolute2.getJointAngle();
    }
    else {
      coordinate2 = m_prismatic2.getJointTranslation();
    }
   
    float C = m_constant - (coordinate1 + m_ratio * coordinate2);
   
    float impulse = m_mass * (-C);
   
    final Vec2 temp = pool.popVec2();
    temp.set(m_J.linearA).mulLocal(b1.m_invMass).mulLocal(impulse);
    b1.m_sweep.c.addLocal(temp);
    b1.m_sweep.a += b1.m_invI * impulse * m_J.angularA;
   
    temp.set(m_J.linearB).mulLocal(b2.m_invMass).mulLocal(impulse);
    b2.m_sweep.c.addLocal(temp);
    b2.m_sweep.a += b2.m_invI * impulse * m_J.angularB;
   
    b1.synchronizeTransform();
    b2.synchronizeTransform();
   
    pool.pushVec2(1);
    // TODO_ERIN not implemented
    return linearError < Settings.linearSlop;
  }
View Full Code Here

  public PistonWorld() {}

  @Override
  public void setupWorld(World world) {
    this.world = world;
    Body ground = null;
    {
      BodyDef bd = new BodyDef();
      ground = world.createBody(bd);

      PolygonShape shape = new PolygonShape();
      shape.setAsBox(5.0f, 100.0f);
      bd = new BodyDef();
      bd.type = BodyType.STATIC;
      FixtureDef sides = new FixtureDef();
      sides.shape = shape;
      sides.density = 0;
      sides.friction = 0;
      sides.restitution = .8f;
      sides.filter.categoryBits = 4;
      sides.filter.maskBits = 2;

      bd.position.set(-10.01f, 50.0f);
      Body bod = world.createBody(bd);
      bod.createFixture(sides);
      bd.position.set(10.01f, 50.0f);
      bod = world.createBody(bd);
      bod.createFixture(sides);
    }

    // turney
    {
      CircleShape cd;
      FixtureDef fd = new FixtureDef();
      BodyDef bd = new BodyDef();
      bd.type = BodyType.DYNAMIC;
      int numPieces = 5;
      float radius = 4f;
      bd.position = new Vec2(0.0f, 25.0f);
      Body body = world.createBody(bd);
      for (int i = 0; i < numPieces; i++) {
        cd = new CircleShape();
        cd.m_radius = .5f;
        fd.shape = cd;
        fd.density = 25;
        fd.friction = .1f;
        fd.restitution = .9f;
        float xPos = radius * (float) Math.cos(2f * Math.PI * (i / (float) (numPieces)));
        float yPos = radius * (float) Math.sin(2f * Math.PI * (i / (float) (numPieces)));
        cd.m_p.set(xPos, yPos);

        body.createFixture(fd);
      }

      RevoluteJointDef rjd = new RevoluteJointDef();
      rjd.initialize(body, ground, body.getPosition());
      rjd.motorSpeed = MathUtils.PI;
      rjd.maxMotorTorque = 1000000.0f;
      rjd.enableMotor = true;
      world.createJoint(rjd);
    }

    {
      Body prevBody = ground;

      // Define crank.
      {
        PolygonShape shape = new PolygonShape();
        shape.setAsBox(0.5f, 2.0f);

        BodyDef bd = new BodyDef();
        bd.type = BodyType.DYNAMIC;
        bd.position.set(0.0f, 7.0f);
        Body body = world.createBody(bd);
        body.createFixture(shape, 2.0f);

        RevoluteJointDef rjd = new RevoluteJointDef();
        rjd.initialize(prevBody, body, new Vec2(0.0f, 5.0f));
        rjd.motorSpeed = 1.0f * MathUtils.PI;
        rjd.maxMotorTorque = 20000;
        rjd.enableMotor = true;
        m_joint1 = (RevoluteJoint) world.createJoint(rjd);

        prevBody = body;
      }

      // Define follower.
      {
        PolygonShape shape = new PolygonShape();
        shape.setAsBox(0.5f, 4.0f);

        BodyDef bd = new BodyDef();
        bd.type = BodyType.DYNAMIC;
        bd.position.set(0.0f, 13.0f);
        Body body = world.createBody(bd);
        body.createFixture(shape, 2.0f);

        RevoluteJointDef rjd = new RevoluteJointDef();
        rjd.initialize(prevBody, body, new Vec2(0.0f, 9.0f));
        rjd.enableMotor = false;
        world.createJoint(rjd);

        prevBody = body;
      }

      // Define piston
      {
        PolygonShape shape = new PolygonShape();
        shape.setAsBox(7f, 2f);

        BodyDef bd = new BodyDef();
        bd.type = BodyType.DYNAMIC;
        bd.position.set(0.0f, 17.0f);
        Body body = world.createBody(bd);
        FixtureDef piston = new FixtureDef();
        piston.shape = shape;
        piston.density = 2;
        piston.filter.categoryBits = 1;
        piston.filter.maskBits = 2;
        body.createFixture(piston);

        RevoluteJointDef rjd = new RevoluteJointDef();
        rjd.initialize(prevBody, body, new Vec2(0.0f, 17.0f));
        world.createJoint(rjd);

        PrismaticJointDef pjd = new PrismaticJointDef();
        pjd.initialize(ground, body, new Vec2(0.0f, 17.0f), new Vec2(0.0f, 1.0f));

        pjd.maxMotorForce = 1000.0f;
        pjd.enableMotor = true;

        m_joint2 = (PrismaticJoint) world.createJoint(pjd);
      }

      // Create a payload
      {
        PolygonShape sd = new PolygonShape();
        BodyDef bd = new BodyDef();
        bd.type = BodyType.DYNAMIC;
        FixtureDef fixture = new FixtureDef();
        Body body;
        for (int i = 0; i < 100; ++i) {
          sd.setAsBox(0.4f, 0.3f);
          bd.position.set(-1.0f, 23.0f + i);

          bd.bullet = false;
          body = world.createBody(bd);
          fixture.shape = sd;
          fixture.density = .1f;
          fixture.filter.categoryBits = 2;
          fixture.filter.maskBits = 1 | 4 | 2;
          body.createFixture(fixture);
        }

        CircleShape cd = new CircleShape();
        cd.m_radius = 0.36f;
        for (int i = 0; i < 100; ++i) {
          bd.position.set(1.0f, 23.0f + i);
          bd.bullet = false;
          fixture.shape = cd;
          fixture.density = 2f;
          fixture.filter.categoryBits = 2;
          fixture.filter.maskBits = 1 | 4 | 2;
          body = world.createBody(bd);
          body.createFixture(fixture);
        }

        float angle = 0.0f;
        float delta = MathUtils.PI / 3.0f;
        Vec2 vertices[] = new Vec2[6];
        for (int i = 0; i < 6; ++i) {
          vertices[i] = new Vec2(0.3f * MathUtils.cos(angle), 0.3f * MathUtils.sin(angle));
          angle += delta;
        }

        PolygonShape shape = new PolygonShape();
        shape.set(vertices, 6);

        for (int i = 0; i < 100; ++i) {
          bd.position.set(0f, 23.0f + i);
          bd.type = BodyType.DYNAMIC;
          bd.fixedRotation = true;
          bd.bullet = false;
          fixture.shape = shape;
          fixture.density = 1f;
          fixture.filter.categoryBits = 2;
          fixture.filter.maskBits = 1 | 4 | 2;
          body = world.createBody(bd);
          body.createFixture(fixture);
        }
      }
    }
  }
View Full Code Here

    // applies pressure between each particles in contact
    float velocityPerPressure = step.dt / (m_density * m_particleDiameter);
    for (int k = 0; k < m_bodyContactCount; k++) {
      ParticleBodyContact contact = m_bodyContactBuffer[k];
      int a = contact.index;
      Body b = contact.body;
      float w = contact.weight;
      float m = contact.mass;
      Vec2 n = contact.normal;
      Vec2 p = m_positionBuffer.data[a];
      float h = m_accumulationBuffer[a] + pressurePerWeight * w;
      final Vec2 f = tempVec;
      final float coef = velocityPerPressure * w * m * h;
      f.x = coef * n.x;
      f.y = coef * n.y;
      final Vec2 velData = m_velocityBuffer.data[a];
      final float particleInvMass = getParticleInvMass();
      velData.x -= particleInvMass * f.x;
      velData.y -= particleInvMass * f.y;
      b.applyLinearImpulse(f, p, true);
    }
    for (int k = 0; k < m_contactCount; k++) {
      ParticleContact contact = m_contactBuffer[k];
      int a = contact.indexA;
      int b = contact.indexB;
View Full Code Here

    // reduces normal velocity of each contact
    float damping = m_dampingStrength;
    for (int k = 0; k < m_bodyContactCount; k++) {
      final ParticleBodyContact contact = m_bodyContactBuffer[k];
      int a = contact.index;
      Body b = contact.body;
      float w = contact.weight;
      float m = contact.mass;
      Vec2 n = contact.normal;
      Vec2 p = m_positionBuffer.data[a];
      final float tempX = p.x - b.m_sweep.c.x;
      final float tempY = p.y - b.m_sweep.c.y;
      final Vec2 velA = m_velocityBuffer.data[a];
      // getLinearVelocityFromWorldPointToOut, with -= velA
      float vx = -b.m_angularVelocity * tempY + b.m_linearVelocity.x - velA.x;
      float vy = b.m_angularVelocity * tempX + b.m_linearVelocity.y - velA.y;
      // done
      float vn = vx * n.x + vy * n.y;
      if (vn < 0) {
        final Vec2 f = tempVec;
        f.x = damping * w * m * vn * n.x;
        f.y = damping * w * m * vn * n.y;
        final float invMass = getParticleInvMass();
        velA.x += invMass * f.x;
        velA.y += invMass * f.y;
        f.x = -f.x;
        f.y = -f.y;
        b.applyLinearImpulse(f, p, true);
      }
    }
    for (int k = 0; k < m_contactCount; k++) {
      final ParticleContact contact = m_contactBuffer[k];
      int a = contact.indexA;
View Full Code Here

    float viscousStrength = m_viscousStrength;
    for (int k = 0; k < m_bodyContactCount; k++) {
      final ParticleBodyContact contact = m_bodyContactBuffer[k];
      int a = contact.index;
      if ((m_flagsBuffer.data[a] & ParticleType.b2_viscousParticle) != 0) {
        Body b = contact.body;
        float w = contact.weight;
        float m = contact.mass;
        Vec2 p = m_positionBuffer.data[a];
        final Vec2 va = m_velocityBuffer.data[a];
        final float tempX = p.x - b.m_sweep.c.x;
        final float tempY = p.y - b.m_sweep.c.y;
        final float vx = -b.m_angularVelocity * tempY + b.m_linearVelocity.x - va.x;
        final float vy = b.m_angularVelocity * tempX + b.m_linearVelocity.y - va.y;
        final Vec2 f = tempVec;
        final float pInvMass = getParticleInvMass();
        f.x = viscousStrength * m * w * vx;
        f.y = viscousStrength * m * w * vy;
        va.x += pInvMass * f.x;
        va.y += pInvMass * f.y;
        f.x = -f.x;
        f.y = -f.y;
        b.applyLinearImpulse(f, p, true);
      }
    }
    for (int k = 0; k < m_contactCount; k++) {
      final ParticleContact contact = m_contactBuffer[k];
      if ((contact.flags & ParticleType.b2_viscousParticle) != 0) {
View Full Code Here

TOP

Related Classes of org.jbox2d.dynamics.Body

Copyright © 2018 www.massapicom. All rights reserved.
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.