Package org.jbox2d.dynamics.joints

Source Code of org.jbox2d.dynamics.joints.PrismaticJoint

/*******************************************************************************
* Copyright (c) 2011, 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.
*     * Neither the name of the <organization> nor the
*       names of its contributors may be used to endorse or promote products
*       derived from this software without specific prior written permission.
*
* 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 DANIEL MURPHY 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.Settings;
import org.jbox2d.common.Transform;
import org.jbox2d.common.Vec2;
import org.jbox2d.common.Vec3;
import org.jbox2d.dynamics.Body;
import org.jbox2d.dynamics.TimeStep;
import org.jbox2d.pooling.IWorldPool;

public class PrismaticJoint extends Joint {
 
  public final Vec2 m_localAnchor1;
  public final Vec2 m_localAnchor2;
  public final Vec2 m_localXAxis1;
  public final Vec2 m_localYAxis1;
  public float m_refAngle;
 
  public final Vec2 m_axis, m_perp;
  public float m_s1, m_s2;
  public float m_a1, m_a2;
 
  public final Mat33 m_K;
  public final Vec3 m_impulse;
 
  public float m_motorMass; // effective mass for motor/limit translational constraint.
  public float m_motorImpulse;
 
  public float m_lowerTranslation;
  public float m_upperTranslation;
  public float m_maxMotorForce;
  public float m_motorSpeed;
 
  public boolean m_enableLimit;
  public boolean m_enableMotor;
  public LimitState m_limitState;
 
  public PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def) {
    super(argWorld, def);
    m_localAnchor1 = new Vec2(def.localAnchorA);
    m_localAnchor2 = new Vec2(def.localAnchorB);
    m_localXAxis1 = new Vec2(def.localAxis1);
    m_localYAxis1 = new Vec2();
    Vec2.crossToOut(1f, m_localXAxis1, m_localYAxis1);
    m_refAngle = 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();
  }
 
  @Override
  public void getAnchorA(Vec2 argOut) {
    m_bodyA.getWorldPointToOut(m_localAnchor1, argOut);
  }
 
  @Override
  public void getAnchorB(Vec2 argOut) {
    m_bodyB.getWorldPointToOut(m_localAnchor2, 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 getJointTranslation() {
    Body b1 = m_bodyA;
    Body b2 = m_bodyB;
   
    Vec2 p1 = pool.popVec2();
    Vec2 p2 = pool.popVec2();
    Vec2 axis = pool.popVec2();
   
    b1.getWorldPointToOut(m_localAnchor1, p1);
    b2.getWorldPointToOut(m_localAnchor2, p2);
    p2.subLocal(p1);
    b1.getWorldVectorToOut(m_localXAxis1, axis);
   
    float translation = Vec2.dot(p2, axis);
   
    pool.pushVec2(3);
    return translation;
  }
 
  // / Get the current joint translation speed, usually in meters per second.
  public float getJointSpeed() {
    Body b1 = m_bodyA;
    Body b2 = m_bodyB;
   
    Vec2[] pc = pool.popVec2(9);
    Vec2 temp = pc[0];
    Vec2 r1 = pc[1];
    Vec2 r2 = pc[2];
    Vec2 p1 = pc[3];
    Vec2 p2 = pc[4];
    Vec2 d = pc[5];
    Vec2 axis = pc[6];
    Vec2 temp2 = pc[7];
    Vec2 temp3 = pc[8];
   
    temp.set(m_localAnchor1).subLocal(b1.getLocalCenter());
    Mat22.mulToOut(b1.getTransform().R, temp, r1);
   
    temp.set(m_localAnchor2).subLocal(b2.getLocalCenter());
    Mat22.mulToOut(b2.getTransform().R, temp, r2);
   
    p1.set(b1.m_sweep.c).addLocal(r1);
    p2.set(b2.m_sweep.c).addLocal(r2);
   
    d.set(p2).subLocal(p1);
    b1.getWorldVectorToOut(m_localXAxis1, axis);
   
    Vec2 v1 = b1.m_linearVelocity;
    Vec2 v2 = b2.m_linearVelocity;
    float w1 = b1.m_angularVelocity;
    float w2 = b2.m_angularVelocity;
   
   
    Vec2.crossToOut(w1, axis, temp);
    Vec2.crossToOut(w2, r2, temp2);
    Vec2.crossToOut(w1, r1, temp3);
   
    temp2.addLocal(v2).subLocal(v1).subLocal(temp3);
    float speed = Vec2.dot(d, temp) + Vec2.dot(axis, temp2);
   
    pool.pushVec2(9);
   
    return speed;
  }
 
  // / Is the joint limit enabled?
  public boolean isLimitEnabled() {
    return m_enableLimit;
  }
 
  // / Enable/disable the joint limit.
  public void enableLimit(boolean flag) {
    m_bodyA.setAwake(true);
    m_bodyB.setAwake(true);
    m_enableLimit = flag;
  }
 
  // / Get the lower joint limit, usually in meters.
  public float getLowerLimit() {
    return m_lowerTranslation;
  }
 
  // / Get the upper joint limit, usually in meters.
  public float getUpperLimit() {
    return m_upperTranslation;
  }
 
  // / Set the joint limits, usually in meters.
  public void setLimits(float lower, float upper) {
    assert (lower <= upper);
    m_bodyA.setAwake(true);
    m_bodyB.setAwake(true);
    m_lowerTranslation = lower;
    m_upperTranslation = upper;
  }
 
  // / Is the joint motor enabled?
  public boolean isMotorEnabled() {
    return m_enableMotor;
  }
 
  // / Enable/disable the joint motor.
  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.
  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.
  public float getMotorSpeed() {
    return m_motorSpeed;
  }
 
  // / Set the maximum motor force, usually in N.
  public void setMaxMotorForce(float force) {
    m_bodyA.setAwake(true);
    m_bodyB.setAwake(true);
    m_maxMotorForce = force;
  }
 
  // / Get the current motor force, usually in N.
  public float getMotorForce() {
    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();
    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(xf1.R, r1, r1);
    Mat22.mulToOut(xf2.R, r2, r2);
   
    d.set(b2.m_sweep.c).addLocal(r2).subLocal(b1.m_sweep.c).subLocal(r1);
   
    m_invMassA = b1.m_invMass;
    m_invIA = b1.m_invI;
    m_invMassB = b2.m_invMass;
    m_invIB = b2.m_invI;
   
    // Compute motor Jacobian and effective mass.
    {
      Mat22.mulToOut(xf1.R, m_localXAxis1, m_axis);
      temp.set(d).addLocal(r1);
      m_a1 = Vec2.cross(temp, m_axis);
      m_a2 = Vec2.cross(r2, m_axis);
     
      m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2;
      if (m_motorMass > Settings.EPSILON) {
        m_motorMass = 1.0f / m_motorMass;
      }
    }
   
    // Prismatic constraint.
    {
      Mat22.mulToOut(xf1.R, m_localYAxis1, m_perp);
     
      temp.set(d).addLocal(r1);
      m_s1 = Vec2.cross(temp, m_perp);
      m_s2 = Vec2.cross(r2, m_perp);
     
      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);
    }
   
    // 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 (step.warmStarting) {
      // Account for variable time step.
      m_impulse.mulLocal(step.dtRatio);
      m_motorImpulse *= 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 L1 = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
      float L2 = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
     
      temp.set(P).mulLocal(m_invMassA);
      b1.m_linearVelocity.subLocal(temp);
      b1.m_angularVelocity -= m_invIA * L1;
     
      temp.set(P).mulLocal(m_invMassB);
      b2.m_linearVelocity.addLocal(temp);
      b2.m_angularVelocity += m_invIB * L2;
     
      pool.pushVec2(1);
    }
    else {
      m_impulse.setZero();
      m_motorImpulse = 0.0f;
    }
    pool.pushVec2(4);
  }
 
  @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);
   
    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;
   
    final Vec2 temp = pool.popVec2();
   
    // Solve linear motor constraint.
    if (m_enableMotor && m_limitState != LimitState.EQUAL) {
      temp.set(v2).subLocal(v1);
      float Cdot = Vec2.dot(m_axis, temp) + m_a2 * w2 - m_a1 * w1;
      float impulse = m_motorMass * (m_motorSpeed - Cdot);
      float oldImpulse = m_motorImpulse;
      float maxImpulse = 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 L1 = impulse * m_a1;
      float L2 = impulse * m_a2;
     
      temp.set(P).mulLocal(m_invMassA);
      v1.subLocal(temp);
      w1 -= m_invIA * L1;
     
      temp.set(P).mulLocal(m_invMassB);
      v2.addLocal(temp);
      w2 += m_invIB * L2;
     
      pool.pushVec2(1);
    }
   
    final Vec2 Cdot1 = pool.popVec2();
    temp.set(v2).subLocal(v1);
    Cdot1.x = Vec2.dot(m_perp, temp) + m_s2 * w2 - m_s1 * w1;
    Cdot1.y = w2 - w1;
    //System.out.println(Cdot1);
   
    if (m_enableLimit && m_limitState != LimitState.INACTIVE) {
      // 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);
     
      float L1 = df.x * m_s1 + df.y + df.z * m_a1;
      float L2 = df.x * m_s2 + df.y + df.z * m_a2;
     
      temp.set(P).mulLocal(m_invMassA);
      v1.subLocal(temp);
      w1 -= m_invIA * L1;
     
      temp.set(P).mulLocal(m_invMassB);
      v2.addLocal(temp);
      w2 += m_invIB * L2;
     
      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 L1 = df.x * m_s1 + df.y;
      float L2 = df.x * m_s2 + df.y;
     
      temp.set(P).mulLocal(m_invMassA);
      v1.subLocal(temp);
      w1 -= m_invIA * L1;
     
      temp.set(P).mulLocal(m_invMassB);
      v2.addLocal(temp);
      w2 += m_invIB * L2;
     
      pool.pushVec2(2);
    }
   
    b1.m_linearVelocity.set(v1);
    b1.m_angularVelocity = w1;
    b2.m_linearVelocity.set(v2);
    b2.m_angularVelocity = w2;
   
    pool.pushVec2(2);
  }
}
TOP

Related Classes of org.jbox2d.dynamics.joints.PrismaticJoint

TOP
Copyright © 2018 www.massapi.com. 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.