pbox2d 0.6.0-java → 0.8.0-java

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

@@ -0,0 +1,808 @@
+/*******************************************************************************
+ * Copyright (c) 2013, Daniel Murphy
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ * 	* Redistributions of source code must retain the above copyright notice,
+ * 	  this list of conditions and the following disclaimer.
+ * 	* Redistributions in binary form must reproduce the above copyright notice,
+ * 	  this list of conditions and the following disclaimer in the documentation
+ * 	  and/or other materials provided with the distribution.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
+ * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ ******************************************************************************/
+package org.jbox2d.dynamics.joints;
+
+import org.jbox2d.common.Mat22;
+import org.jbox2d.common.Mat33;
+import org.jbox2d.common.MathUtils;
+import org.jbox2d.common.Rot;
+import org.jbox2d.common.Settings;
+import org.jbox2d.common.Vec2;
+import org.jbox2d.common.Vec3;
+import org.jbox2d.dynamics.Body;
+import org.jbox2d.dynamics.SolverData;
+import org.jbox2d.pooling.IWorldPool;
+
+//Linear constraint (point-to-line)
+//d = p2 - p1 = x2 + r2 - x1 - r1
+//C = dot(perp, d)
+//Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+//   = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
+//J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
+//
+//Angular constraint
+//C = a2 - a1 + a_initial
+//Cdot = w2 - w1
+//J = [0 0 -1 0 0 1]
+//
+//K = J * invM * JT
+//
+//J = [-a -s1 a s2]
+//  [0  -1  0  1]
+//a = perp
+//s1 = cross(d + r1, a) = cross(p2 - x1, a)
+//s2 = cross(r2, a) = cross(p2 - x2, a)
+
+
+//Motor/Limit linear constraint
+//C = dot(ax1, d)
+//Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
+//J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
+
+//Block Solver
+//We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
+//when the mass has poor distribution (leading to large torques about the joint anchor points).
+//
+//The Jacobian has 3 rows:
+//J = [-uT -s1 uT s2] // linear
+//  [0   -1   0  1] // angular
+//  [-vT -a1 vT a2] // limit
+//
+//u = perp
+//v = axis
+//s1 = cross(d + r1, u), s2 = cross(r2, u)
+//a1 = cross(d + r1, v), a2 = cross(r2, v)
+
+//M * (v2 - v1) = JT * df
+//J * v2 = bias
+//
+//v2 = v1 + invM * JT * df
+//J * (v1 + invM * JT * df) = bias
+//K * df = bias - J * v1 = -Cdot
+//K = J * invM * JT
+//Cdot = J * v1 - bias
+//
+//Now solve for f2.
+//df = f2 - f1
+//K * (f2 - f1) = -Cdot
+//f2 = invK * (-Cdot) + f1
+//
+//Clamp accumulated limit impulse.
+//lower: f2(3) = max(f2(3), 0)
+//upper: f2(3) = min(f2(3), 0)
+//
+//Solve for correct f2(1:2)
+//K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
+//                    = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
+//K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)
+//f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
+//
+//Now compute impulse to be applied:
+//df = f2 - f1
+
+/**
+ * A prismatic joint. This joint provides one degree of freedom: translation along an axis fixed in
+ * bodyA. Relative rotation is prevented. You can use a joint limit to restrict the range of motion
+ * and a joint motor to drive the motion or to model joint friction.
+ * 
+ * @author Daniel
+ */
+public class PrismaticJoint extends Joint {
+
+  // Solver shared
+  protected final Vec2 m_localAnchorA;
+  protected final Vec2 m_localAnchorB;
+  protected final Vec2 m_localXAxisA;
+  protected final Vec2 m_localYAxisA;
+  protected float m_referenceAngle;
+  private final Vec3 m_impulse;
+  private float m_motorImpulse;
+  private float m_lowerTranslation;
+  private float m_upperTranslation;
+  private float m_maxMotorForce;
+  private float m_motorSpeed;
+  private boolean m_enableLimit;
+  private boolean m_enableMotor;
+  private LimitState m_limitState;
+
+  // Solver temp
+  private int m_indexA;
+  private int m_indexB;
+  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 final Vec2 m_axis, m_perp;
+  private float m_s1, m_s2;
+  private float m_a1, m_a2;
+  private final Mat33 m_K;
+  private float m_motorMass; // effective mass for motor/limit translational constraint.
+
+  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();
+  }
+
+  public Vec2 getLocalAnchorA() {
+    return m_localAnchorA;
+  }
+
+  public Vec2 getLocalAnchorB() {
+    return m_localAnchorB;
+  }
+
+  @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) {
+    Vec2 temp = pool.popVec2();
+    temp.set(m_axis).mulLocal(m_motorImpulse + m_impulse.z);
+    argOut.set(m_perp).mulLocal(m_impulse.x).addLocal(temp).mulLocal(inv_dt);
+    pool.pushVec2(1);
+  }
+
+  @Override
+  public float getReactionTorque(float inv_dt) {
+    return inv_dt * m_impulse.y;
+  }
+
+  /**
+   * Get the current joint translation, usually in meters.
+   */
+  public float getJointSpeed() {
+    Body bA = m_bodyA;
+    Body bB = m_bodyB;
+
+    Vec2 temp = pool.popVec2();
+    Vec2 rA = pool.popVec2();
+    Vec2 rB = pool.popVec2();
+    Vec2 p1 = pool.popVec2();
+    Vec2 p2 = pool.popVec2();
+    Vec2 d = pool.popVec2();
+    Vec2 axis = pool.popVec2();
+    Vec2 temp2 = pool.popVec2();
+    Vec2 temp3 = pool.popVec2();
+
+    temp.set(m_localAnchorA).subLocal(bA.m_sweep.localCenter);
+    Rot.mulToOutUnsafe(bA.m_xf.q, temp, rA);
+
+    temp.set(m_localAnchorB).subLocal(bB.m_sweep.localCenter);
+    Rot.mulToOutUnsafe(bB.m_xf.q, temp, rB);
+
+    p1.set(bA.m_sweep.c).addLocal(rA);
+    p2.set(bB.m_sweep.c).addLocal(rB);
+
+    d.set(p2).subLocal(p1);
+    Rot.mulToOutUnsafe(bA.m_xf.q, m_localXAxisA, axis);
+
+    Vec2 vA = bA.m_linearVelocity;
+    Vec2 vB = bB.m_linearVelocity;
+    float wA = bA.m_angularVelocity;
+    float wB = bB.m_angularVelocity;
+
+
+    Vec2.crossToOutUnsafe(wA, axis, temp);
+    Vec2.crossToOutUnsafe(wB, rB, temp2);
+    Vec2.crossToOutUnsafe(wA, rA, temp3);
+
+    temp2.addLocal(vB).subLocal(vA).subLocal(temp3);
+    float speed = Vec2.dot(d, temp) + Vec2.dot(axis, temp2);
+
+    pool.pushVec2(9);
+
+    return speed;
+  }
+
+  public float getJointTranslation() {
+    Vec2 pA = pool.popVec2(), pB = pool.popVec2(), axis = pool.popVec2();
+    m_bodyA.getWorldPointToOut(m_localAnchorA, pA);
+    m_bodyB.getWorldPointToOut(m_localAnchorB, pB);
+    m_bodyA.getWorldVectorToOutUnsafe(m_localXAxisA, axis);
+    pB.subLocal(pA);
+    float translation = Vec2.dot(pB, axis);
+    pool.pushVec2(3);
+    return translation;
+  }
+
+  /**
+   * Is the joint limit enabled?
+   * 
+   * @return
+   */
+  public boolean isLimitEnabled() {
+    return m_enableLimit;
+  }
+
+  /**
+   * Enable/disable the joint limit.
+   * 
+   * @param flag
+   */
+  public void enableLimit(boolean flag) {
+    if (flag != m_enableLimit) {
+      m_bodyA.setAwake(true);
+      m_bodyB.setAwake(true);
+      m_enableLimit = flag;
+      m_impulse.z = 0.0f;
+    }
+  }
+
+  /**
+   * Get the lower joint limit, usually in meters.
+   * 
+   * @return
+   */
+  public float getLowerLimit() {
+    return m_lowerTranslation;
+  }
+
+  /**
+   * Get the upper joint limit, usually in meters.
+   * 
+   * @return
+   */
+  public float getUpperLimit() {
+    return m_upperTranslation;
+  }
+
+  /**
+   * Set the joint limits, usually in meters.
+   * 
+   * @param lower
+   * @param upper
+   */
+  public void setLimits(float lower, float upper) {
+    assert (lower <= upper);
+    if (lower != m_lowerTranslation || upper != m_upperTranslation) {
+      m_bodyA.setAwake(true);
+      m_bodyB.setAwake(true);
+      m_lowerTranslation = lower;
+      m_upperTranslation = upper;
+      m_impulse.z = 0.0f;
+    }
+  }
+
+  /**
+   * Is the joint motor enabled?
+   * 
+   * @return
+   */
+  public boolean isMotorEnabled() {
+    return m_enableMotor;
+  }
+
+  /**
+   * Enable/disable the joint motor.
+   * 
+   * @param flag
+   */
+  public void enableMotor(boolean flag) {
+    m_bodyA.setAwake(true);
+    m_bodyB.setAwake(true);
+    m_enableMotor = flag;
+  }
+
+  /**
+   * Set the motor speed, usually in meters per second.
+   * 
+   * @param speed
+   */
+  public void setMotorSpeed(float speed) {
+    m_bodyA.setAwake(true);
+    m_bodyB.setAwake(true);
+    m_motorSpeed = speed;
+  }
+
+  /**
+   * Get the motor speed, usually in meters per second.
+   * 
+   * @return
+   */
+  public float getMotorSpeed() {
+    return m_motorSpeed;
+  }
+
+  /**
+   * Set the maximum motor force, usually in N.
+   * 
+   * @param force
+   */
+  public void setMaxMotorForce(float force) {
+    m_bodyA.setAwake(true);
+    m_bodyB.setAwake(true);
+    m_maxMotorForce = force;
+  }
+
+  /**
+   * Get the current motor force, usually in N.
+   * 
+   * @param inv_dt
+   * @return
+   */
+  public float getMotorForce(float inv_dt) {
+    return m_motorImpulse * inv_dt;
+  }
+
+  public float getMaxMotorForce() {
+    return m_maxMotorForce;
+  }
+
+  public float getReferenceAngle() {
+    return m_referenceAngle;
+  }
+
+  public Vec2 getLocalAxisA() {
+    return m_localXAxisA;
+  }
+
+  @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 d = pool.popVec2();
+    final Vec2 temp = pool.popVec2();
+    final Vec2 rA = pool.popVec2();
+    final Vec2 rB = pool.popVec2();
+
+    qA.set(aA);
+    qB.set(aB);
+
+    // Compute the effective masses.
+    Rot.mulToOutUnsafe(qA, d.set(m_localAnchorA).subLocal(m_localCenterA), rA);
+    Rot.mulToOutUnsafe(qB, d.set(m_localAnchorB).subLocal(m_localCenterB), rB);
+    d.set(cB).subLocal(cA).addLocal(rB).subLocal(rA);
+
+    float mA = m_invMassA, mB = m_invMassB;
+    float iA = m_invIA, iB = m_invIB;
+
+    // Compute motor Jacobian and effective mass.
+    {
+      Rot.mulToOutUnsafe(qA, m_localXAxisA, m_axis);
+      temp.set(d).addLocal(rA);
+      m_a1 = Vec2.cross(temp, m_axis);
+      m_a2 = Vec2.cross(rB, m_axis);
+
+      m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
+      if (m_motorMass > 0.0f) {
+        m_motorMass = 1.0f / m_motorMass;
+      }
+    }
+
+    // Prismatic constraint.
+    {
+      Rot.mulToOutUnsafe(qA, m_localYAxisA, m_perp);
+
+      temp.set(d).addLocal(rA);
+      m_s1 = Vec2.cross(temp, m_perp);
+      m_s2 = Vec2.cross(rB, m_perp);
+
+      float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
+      float k12 = iA * m_s1 + iB * m_s2;
+      float k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
+      float k22 = iA + iB;
+      if (k22 == 0.0f) {
+        // For bodies with fixed rotation.
+        k22 = 1.0f;
+      }
+      float k23 = iA * m_a1 + iB * m_a2;
+      float k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
+
+      m_K.ex.set(k11, k12, k13);
+      m_K.ey.set(k12, k22, k23);
+      m_K.ez.set(k13, k23, k33);
+    }
+
+    // Compute motor and limit terms.
+    if (m_enableLimit) {
+
+      float jointTranslation = Vec2.dot(m_axis, d);
+      if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0f * Settings.linearSlop) {
+        m_limitState = LimitState.EQUAL;
+      } else if (jointTranslation <= m_lowerTranslation) {
+        if (m_limitState != LimitState.AT_LOWER) {
+          m_limitState = LimitState.AT_LOWER;
+          m_impulse.z = 0.0f;
+        }
+      } else if (jointTranslation >= m_upperTranslation) {
+        if (m_limitState != LimitState.AT_UPPER) {
+          m_limitState = LimitState.AT_UPPER;
+          m_impulse.z = 0.0f;
+        }
+      } else {
+        m_limitState = LimitState.INACTIVE;
+        m_impulse.z = 0.0f;
+      }
+    } else {
+      m_limitState = LimitState.INACTIVE;
+      m_impulse.z = 0.0f;
+    }
+
+    if (m_enableMotor == false) {
+      m_motorImpulse = 0.0f;
+    }
+
+    if (data.step.warmStarting) {
+      // Account for variable time step.
+      m_impulse.mulLocal(data.step.dtRatio);
+      m_motorImpulse *= data.step.dtRatio;
+
+      final Vec2 P = pool.popVec2();
+      temp.set(m_axis).mulLocal(m_motorImpulse + m_impulse.z);
+      P.set(m_perp).mulLocal(m_impulse.x).addLocal(temp);
+
+      float LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
+      float LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
+
+      vA.x -= mA * P.x;
+      vA.y -= mA * P.y;
+      wA -= iA * LA;
+
+      vB.x += mB * P.x;
+      vB.y += mB * P.y;
+      wB += iB * LB;
+
+      pool.pushVec2(1);
+    } else {
+      m_impulse.setZero();
+      m_motorImpulse = 0.0f;
+    }
+
+    // data.velocities[m_indexA].v.set(vA);
+    data.velocities[m_indexA].w = wA;
+    // data.velocities[m_indexB].v.set(vB);
+    data.velocities[m_indexB].w = wB;
+
+    pool.pushRot(2);
+    pool.pushVec2(4);
+  }
+
+  @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;
+
+    float mA = m_invMassA, mB = m_invMassB;
+    float iA = m_invIA, iB = m_invIB;
+
+    final Vec2 temp = pool.popVec2();
+
+    // Solve linear motor constraint.
+    if (m_enableMotor && m_limitState != LimitState.EQUAL) {
+      temp.set(vB).subLocal(vA);
+      float Cdot = Vec2.dot(m_axis, temp) + m_a2 * wB - m_a1 * wA;
+      float impulse = m_motorMass * (m_motorSpeed - Cdot);
+      float oldImpulse = m_motorImpulse;
+      float maxImpulse = data.step.dt * m_maxMotorForce;
+      m_motorImpulse = MathUtils.clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
+      impulse = m_motorImpulse - oldImpulse;
+
+      final Vec2 P = pool.popVec2();
+      P.set(m_axis).mulLocal(impulse);
+      float LA = impulse * m_a1;
+      float LB = impulse * m_a2;
+
+      vA.x -= mA * P.x;
+      vA.y -= mA * P.y;
+      wA -= iA * LA;
+
+      vB.x += mB * P.x;
+      vB.y += mB * P.y;
+      wB += iB * LB;
+
+      pool.pushVec2(1);
+    }
+
+    final Vec2 Cdot1 = pool.popVec2();
+    temp.set(vB).subLocal(vA);
+    Cdot1.x = Vec2.dot(m_perp, temp) + m_s2 * wB - m_s1 * wA;
+    Cdot1.y = wB - wA;
+    // System.out.println(Cdot1);
+
+    if (m_enableLimit && m_limitState != LimitState.INACTIVE) {
+      // 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);
+
+      float LA = df.x * m_s1 + df.y + df.z * m_a1;
+      float LB = df.x * m_s2 + df.y + df.z * m_a2;
+
+      vA.x -= mA * P.x;
+      vA.y -= mA * P.y;
+      wA -= iA * LA;
+
+      vB.x += mB * P.x;
+      vB.y += mB * P.y;
+      wB += iB * LB;
+
+      pool.pushVec2(3);
+      pool.pushVec3(3);
+    } else {
+      // Limit is inactive, just solve the prismatic constraint in block form.
+      final Vec2 df = pool.popVec2();
+      m_K.solve22ToOut(Cdot1.negateLocal(), df);
+      Cdot1.negateLocal();
+
+      m_impulse.x += df.x;
+      m_impulse.y += df.y;
+
+      final Vec2 P = pool.popVec2();
+      P.set(m_perp).mulLocal(df.x);
+      float LA = df.x * m_s1 + df.y;
+      float LB = df.x * m_s2 + df.y;
+
+      vA.x -= mA * P.x;
+      vA.y -= mA * P.y;
+      wA -= iA * LA;
+
+      vB.x += mB * P.x;
+      vB.y += mB * P.y;
+      wB += iB * LB;
+
+      pool.pushVec2(2);
+    }
+
+    // data.velocities[m_indexA].v.set(vA);
+    data.velocities[m_indexA].w = wA;
+    // data.velocities[m_indexB].v.set(vB);
+    data.velocities[m_indexB].w = wB;
+
+    pool.pushVec2(2);
+  }
+
+
+  @Override
+  public boolean solvePositionConstraints(final SolverData data) {
+
+    final Rot qA = pool.popRot();
+    final Rot qB = pool.popRot();
+    final Vec2 rA = pool.popVec2();
+    final Vec2 rB = pool.popVec2();
+    final Vec2 d = pool.popVec2();
+    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;
+      float k22 = iA + iB;
+      if (k22 == 0.0f) {
+        k22 = 1.0f;
+      }
+
+      final Mat22 K = pool.popMat22();
+      K.ex.set(k11, k12);
+      K.ey.set(k12, k22);
+
+      // temp is impulse1
+      K.solveToOut(C1.negateLocal(), temp);
+      C1.negateLocal();
+
+      impulse.x = temp.x;
+      impulse.y = temp.y;
+      impulse.z = 0.0f;
+
+      pool.pushMat22(1);
+    }
+
+    float Px = impulse.x * perp.x + impulse.z * axis.x;
+    float Py = impulse.x * perp.y + impulse.z * axis.y;
+    float LA = impulse.x * s1 + impulse.y + impulse.z * a1;
+    float LB = impulse.x * s2 + impulse.y + impulse.z * a2;
+
+    cA.x -= mA * Px;
+    cA.y -= mA * Py;
+    aA -= iA * LA;
+    cB.x += mB * Px;
+    cB.y += mB * Py;
+    aB += iB * LB;
+
+    // data.positions[m_indexA].c.set(cA);
+    data.positions[m_indexA].a = aA;
+    // data.positions[m_indexB].c.set(cB);
+    data.positions[m_indexB].a = aB;
+
+    pool.pushVec2(7);
+    pool.pushVec3(1);
+    pool.pushRot(2);
+
+    return linearError <= Settings.linearSlop && angularError <= Settings.angularSlop;
+  }
+}