dynamics/joints/PrismaticJoint.java

/*******************************************************************************
 * 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;
  }
}