pbox2d 0.6.0-java → 0.8.0-java

data/src/org/jbox2d/dynamics/joints/RopeJoint.java

@@ -0,0 +1,276 @@
+package org.jbox2d.dynamics.joints;
+
+import org.jbox2d.common.MathUtils;
+import org.jbox2d.common.Rot;
+import org.jbox2d.common.Settings;
+import org.jbox2d.common.Vec2;
+import org.jbox2d.dynamics.SolverData;
+import org.jbox2d.pooling.IWorldPool;
+
+/**
+ * A rope joint enforces a maximum distance between two points on two bodies. It has no other
+ * effect. Warning: if you attempt to change the maximum length during the simulation you will get
+ * some non-physical behavior. A model that would allow you to dynamically modify the length would
+ * have some sponginess, so I chose not to implement it that way. See DistanceJoint if you want to
+ * dynamically control length.
+ * 
+ * @author Daniel Murphy
+ */
+public class RopeJoint extends Joint {
+  // Solver shared
+  private final Vec2 m_localAnchorA = new Vec2();
+  private final Vec2 m_localAnchorB = new Vec2();
+  private float m_maxLength;
+  private float m_length;
+  private float m_impulse;
+
+  // Solver temp
+  private int m_indexA;
+  private int m_indexB;
+  private final Vec2 m_u = new Vec2();
+  private final Vec2 m_rA = new Vec2();
+  private final Vec2 m_rB = new Vec2();
+  private final Vec2 m_localCenterA = new Vec2();
+  private final Vec2 m_localCenterB = new Vec2();
+  private float m_invMassA;
+  private float m_invMassB;
+  private float m_invIA;
+  private float m_invIB;
+  private float m_mass;
+  private LimitState m_state;
+
+  protected RopeJoint(IWorldPool worldPool, RopeJointDef def) {
+    super(worldPool, def);
+    m_localAnchorA.set(def.localAnchorA);
+    m_localAnchorB.set(def.localAnchorB);
+
+    m_maxLength = def.maxLength;
+
+    m_mass = 0.0f;
+    m_impulse = 0.0f;
+    m_state = LimitState.INACTIVE;
+    m_length = 0.0f;
+  }
+
+  @Override
+  public void initVelocityConstraints(final SolverData data) {
+    m_indexA = m_bodyA.m_islandIndex;
+    m_indexB = m_bodyB.m_islandIndex;
+    m_localCenterA.set(m_bodyA.m_sweep.localCenter);
+    m_localCenterB.set(m_bodyB.m_sweep.localCenter);
+    m_invMassA = m_bodyA.m_invMass;
+    m_invMassB = m_bodyB.m_invMass;
+    m_invIA = m_bodyA.m_invI;
+    m_invIB = m_bodyB.m_invI;
+
+    Vec2 cA = data.positions[m_indexA].c;
+    float aA = data.positions[m_indexA].a;
+    Vec2 vA = data.velocities[m_indexA].v;
+    float wA = data.velocities[m_indexA].w;
+
+    Vec2 cB = data.positions[m_indexB].c;
+    float aB = data.positions[m_indexB].a;
+    Vec2 vB = data.velocities[m_indexB].v;
+    float wB = data.velocities[m_indexB].w;
+
+    final Rot qA = pool.popRot();
+    final Rot qB = pool.popRot();
+    final Vec2 temp = pool.popVec2();
+
+    qA.set(aA);
+    qB.set(aB);
+
+    // Compute the effective masses.
+    Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), m_rA);
+    Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);
+
+    m_u.set(cB).addLocal(m_rB).subLocal(cA).subLocal(m_rA);
+
+    m_length = m_u.length();
+
+    float C = m_length - m_maxLength;
+    if (C > 0.0f) {
+      m_state = LimitState.AT_UPPER;
+    } else {
+      m_state = LimitState.INACTIVE;
+    }
+
+    if (m_length > Settings.linearSlop) {
+      m_u.mulLocal(1.0f / m_length);
+    } else {
+      m_u.setZero();
+      m_mass = 0.0f;
+      m_impulse = 0.0f;
+      return;
+    }
+
+    // Compute effective mass.
+    float crA = Vec2.cross(m_rA, m_u);
+    float crB = Vec2.cross(m_rB, m_u);
+    float invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB;
+
+    m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
+
+    if (data.step.warmStarting) {
+      // Scale the impulse to support a variable time step.
+      m_impulse *= data.step.dtRatio;
+
+      float Px = m_impulse * m_u.x;
+      float Py = m_impulse * m_u.y;
+      vA.x -= m_invMassA * Px;
+      vA.y -= m_invMassA * Py;
+      wA -= m_invIA * (m_rA.x * Py - m_rA.y * Px);
+
+      vB.x += m_invMassB * Px;
+      vB.y += m_invMassB * Py;
+      wB += m_invIB * (m_rB.x * Py - m_rB.y * Px);
+    } else {
+      m_impulse = 0.0f;
+    }
+
+    pool.pushRot(2);
+    pool.pushVec2(1);
+
+    // data.velocities[m_indexA].v = vA;
+    data.velocities[m_indexA].w = wA;
+    // data.velocities[m_indexB].v = vB;
+    data.velocities[m_indexB].w = wB;
+  }
+
+  @Override
+  public void solveVelocityConstraints(final SolverData data) {
+    Vec2 vA = data.velocities[m_indexA].v;
+    float wA = data.velocities[m_indexA].w;
+    Vec2 vB = data.velocities[m_indexB].v;
+    float wB = data.velocities[m_indexB].w;
+
+    // Cdot = dot(u, v + cross(w, r))
+    Vec2 vpA = pool.popVec2();
+    Vec2 vpB = pool.popVec2();
+    Vec2 temp = pool.popVec2();
+
+    Vec2.crossToOutUnsafe(wA, m_rA, vpA);
+    vpA.addLocal(vA);
+    Vec2.crossToOutUnsafe(wB, m_rB, vpB);
+    vpB.addLocal(vB);
+
+    float C = m_length - m_maxLength;
+    float Cdot = Vec2.dot(m_u, temp.set(vpB).subLocal(vpA));
+
+    // Predictive constraint.
+    if (C < 0.0f) {
+      Cdot += data.step.inv_dt * C;
+    }
+
+    float impulse = -m_mass * Cdot;
+    float oldImpulse = m_impulse;
+    m_impulse = MathUtils.min(0.0f, m_impulse + impulse);
+    impulse = m_impulse - oldImpulse;
+
+    float Px = impulse * m_u.x;
+    float Py = impulse * m_u.y;
+    vA.x -= m_invMassA * Px;
+    vA.y -= m_invMassA * Py;
+    wA -= m_invIA * (m_rA.x * Py - m_rA.y * Px);
+    vB.x += m_invMassB * Px;
+    vB.y += m_invMassB * Py;
+    wB += m_invIB * (m_rB.x * Py - m_rB.y * Px);
+
+    pool.pushVec2(3);
+
+    // data.velocities[m_indexA].v = vA;
+    data.velocities[m_indexA].w = wA;
+    // data.velocities[m_indexB].v = vB;
+    data.velocities[m_indexB].w = wB;
+  }
+
+  @Override
+  public boolean solvePositionConstraints(final SolverData data) {
+    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;
+
+    final Rot qA = pool.popRot();
+    final Rot qB = pool.popRot();
+    final Vec2 u = pool.popVec2();
+    final Vec2 rA = pool.popVec2();
+    final Vec2 rB = pool.popVec2();
+    final Vec2 temp = pool.popVec2();
+
+    qA.set(aA);
+    qB.set(aB);
+
+    // Compute the effective masses.
+    Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
+    Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
+    u.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
+
+    float length = u.normalize();
+    float C = length - m_maxLength;
+
+    C = MathUtils.clamp(C, 0.0f, Settings.maxLinearCorrection);
+
+    float impulse = -m_mass * C;
+    float Px = impulse * u.x;
+    float Py = impulse * u.y;
+
+    cA.x -= m_invMassA * Px;
+    cA.y -= m_invMassA * Py;
+    aA -= m_invIA * (rA.x * Py - rA.y * Px);
+    cB.x += m_invMassB * Px;
+    cB.y += m_invMassB * Py;
+    aB += m_invIB * (rB.x * Py - rB.y * Px);
+
+    pool.pushRot(2);
+    pool.pushVec2(4);
+
+    // data.positions[m_indexA].c = cA;
+    data.positions[m_indexA].a = aA;
+    // data.positions[m_indexB].c = cB;
+    data.positions[m_indexB].a = aB;
+
+    return length - m_maxLength < Settings.linearSlop;
+  }
+
+  @Override
+  public void getAnchorA(Vec2 argOut) {
+    m_bodyA.getWorldPointToOut(m_localAnchorA, argOut);
+  }
+
+  @Override
+  public void getAnchorB(Vec2 argOut) {
+    m_bodyB.getWorldPointToOut(m_localAnchorB, argOut);
+  }
+
+  @Override
+  public void getReactionForce(float inv_dt, Vec2 argOut) {
+    argOut.set(m_u).mulLocal(inv_dt).mulLocal(m_impulse);
+  }
+
+  @Override
+  public float getReactionTorque(float inv_dt) {
+    return 0f;
+  }
+
+  public Vec2 getLocalAnchorA() {
+    return m_localAnchorA;
+  }
+
+  public Vec2 getLocalAnchorB() {
+    return m_localAnchorB;
+  }
+
+  public float getMaxLength() {
+    return m_maxLength;
+  }
+
+  public void setMaxLength(float maxLength) {
+    this.m_maxLength = maxLength;
+  }
+
+  public LimitState getLimitState() {
+    return m_state;
+  }
+
+}

data/src/org/jbox2d/dynamics/joints/RopeJointDef.java

@@ -0,0 +1,34 @@
+package org.jbox2d.dynamics.joints;
+
+import org.jbox2d.common.Vec2;
+
+/**
+ * Rope joint definition. This requires two body anchor points and a maximum lengths. Note: by
+ * default the connected objects will not collide. see collideConnected in b2JointDef.
+ * 
+ * @author Daniel Murphy
+ */
+public class RopeJointDef extends JointDef {
+
+  /**
+   * The local anchor point relative to bodyA's origin.
+   */
+  public final Vec2 localAnchorA = new Vec2();
+
+  /**
+   * The local anchor point relative to bodyB's origin.
+   */
+  public final Vec2 localAnchorB = new Vec2();
+
+  /**
+   * The maximum length of the rope. Warning: this must be larger than b2_linearSlop or the joint
+   * will have no effect.
+   */
+  public float maxLength;
+
+  public RopeJointDef() {
+    super(JointType.ROPE);
+    localAnchorA.set(-1.0f, 0.0f);
+    localAnchorB.set(1.0f, 0.0f);
+  }
+}