RevoluteJoint.java :  » Game » JBox2D-2.0.1 » org » jbox2d » dynamics » joints » Java Open Source

Java Open Source » Game » JBox2D 2.0.1 
JBox2D 2.0.1 » org » jbox2d » dynamics » joints » RevoluteJoint.java
/*
 * JBox2D - A Java Port of Erin Catto's Box2D
 * 
 * JBox2D homepage: http://jbox2d.sourceforge.net/ 
 * Box2D homepage: http://www.box2d.org
 * 
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * 
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

package org.jbox2d.dynamics.joints;

import org.jbox2d.common.Mat22;
import org.jbox2d.common.MathUtils;
import org.jbox2d.common.Settings;
import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;
import org.jbox2d.dynamics.TimeStep;
import org.jbox2d.dynamics.World;


//Updated to rev. 56->108 of b2RevoluteJoint.cpp/.h

//Point-to-point constraint
//C = p2 - p1
//Cdot = v2 - v1
//   = v2 + cross(w2, r2) - v1 - cross(w1, r1)
//J = [-I -r1_skew I r2_skew ]
//Identity used:
//w k % (rx i + ry j) = w * (-ry i + rx j)

//Motor constraint
//Cdot = w2 - w1
//J = [0 0 -1 0 0 1]
//K = invI1 + invI2

public class RevoluteJoint extends Joint {
  public Vec2 m_localAnchor1;  // relative
  public Vec2 m_localAnchor2;
  public Vec2 m_pivotForce;
  public float m_motorForce;
  public float m_limitForce;
  public float m_limitPositionImpulse;

  public Mat22 m_pivotMass;    // effective mass for point-to-point constraint.
  public float m_motorMass;  // effective mass for motor/limit angular constraint.
  
  public boolean m_enableMotor;
  public float m_maxMotorTorque;
  public float m_motorSpeed;

  public boolean m_enableLimit;
  public float m_referenceAngle;
  public float m_lowerAngle;
  public float m_upperAngle;
  public LimitState m_limitState;

    public RevoluteJoint(RevoluteJointDef def) {
        super(def);
        m_localAnchor1 = def.localAnchor1.clone();
      m_localAnchor2 = def.localAnchor2.clone();
      m_referenceAngle = def.referenceAngle;

      m_pivotForce = new Vec2(0.0f, 0.0f);
      m_motorForce = 0.0f;
      m_limitForce = 0.0f;
      m_limitPositionImpulse = 0.0f;
      m_pivotMass = new Mat22();

      m_lowerAngle = def.lowerAngle;
      m_upperAngle = def.upperAngle;
      m_maxMotorTorque = def.maxMotorTorque;
      m_motorSpeed = def.motorSpeed;
      m_enableLimit = def.enableLimit;
      m_enableMotor = def.enableMotor;
    }

    @Override
    public void initVelocityConstraints(TimeStep step) {
      Body b1 = m_body1;
      Body b2 = m_body2;

      // Compute the effective mass matrix.
      Vec2 r1 = Mat22.mul(b1.m_xf.R, m_localAnchor1.sub(b1.getLocalCenter()));
      Vec2 r2 = Mat22.mul(b2.m_xf.R, m_localAnchor2.sub(b2.getLocalCenter()));

      // K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
      //      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
      //        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
      float invMass1 = b1.m_invMass, invMass2 = b2.m_invMass;
      float invI1 = b1.m_invI, invI2 = b2.m_invI;

      Mat22 K1 = new Mat22();
      K1.col1.x = invMass1 + invMass2;  K1.col2.x = 0.0f;
      K1.col1.y = 0.0f;          K1.col2.y = invMass1 + invMass2;

      Mat22 K2 = new Mat22();
      K2.col1.x =  invI1 * r1.y * r1.y;  K2.col2.x = -invI1 * r1.x * r1.y;
      K2.col1.y = -invI1 * r1.x * r1.y;  K2.col2.y =  invI1 * r1.x * r1.x;

      Mat22 K3 = new Mat22();
      K3.col1.x =  invI2 * r2.y * r2.y;  K3.col2.x = -invI2 * r2.x * r2.y;
      K3.col1.y = -invI2 * r2.x * r2.y;  K3.col2.y =  invI2 * r2.x * r2.x;

      Mat22 K = K1.addLocal(K2).addLocal(K3);
      m_pivotMass = K.invert();

      m_motorMass = 1.0f / (invI1 + invI2);

      if (m_enableMotor == false) {
        m_motorForce = 0.0f;
      }

      if (m_enableLimit) {
        float jointAngle = b2.m_sweep.a - b1.m_sweep.a - m_referenceAngle;
        if (Math.abs(m_upperAngle - m_lowerAngle) < 2.0f * Settings.angularSlop) {
          m_limitState = LimitState.EQUAL_LIMITS;
        } else if (jointAngle <= m_lowerAngle) {
          if (m_limitState != LimitState.AT_LOWER_LIMIT) {
            m_limitForce = 0.0f;
          }
          m_limitState = LimitState.AT_LOWER_LIMIT;
        } else if (jointAngle >= m_upperAngle) {
          if (m_limitState != LimitState.AT_UPPER_LIMIT) {
            m_limitForce = 0.0f;
          }
          m_limitState = LimitState.AT_UPPER_LIMIT;
        }else {
          m_limitState = LimitState.INACTIVE_LIMIT;
          m_limitForce = 0.0f;
        }
      } else {
        m_limitForce = 0.0f;
      }

      if (step.warmStarting) {
        b1.m_linearVelocity.x -= step.dt * invMass1 * m_pivotForce.x;
        b1.m_linearVelocity.y -= step.dt * invMass1 * m_pivotForce.y;
        b1.m_angularVelocity -= step.dt * invI1 * (Vec2.cross(r1, m_pivotForce) + m_motorForce + m_limitForce);

        b2.m_linearVelocity.x += step.dt * invMass2 * m_pivotForce.x;
        b2.m_linearVelocity.y += step.dt * invMass2 * m_pivotForce.y;
        b2.m_angularVelocity += step.dt * invI2 * (Vec2.cross(r2, m_pivotForce) + m_motorForce + m_limitForce);
      } else {
        m_pivotForce.setZero();
        m_motorForce = 0.0f;
        m_limitForce = 0.0f;
      }

      m_limitPositionImpulse = 0.0f;
    }

    @Override
    public void solveVelocityConstraints(TimeStep step) {
      Body b1 = m_body1;
      Body b2 = m_body2;

      Vec2 r1 = Mat22.mul(b1.m_xf.R, m_localAnchor1.sub(b1.getLocalCenter()));
      Vec2 r2 = Mat22.mul(b2.m_xf.R, m_localAnchor2.sub(b2.getLocalCenter()));

      // Solve point-to-point constraint
      Vec2 pivotCdot = b2.m_linearVelocity.add( Vec2.cross(b2.m_angularVelocity, r2).subLocal(b1.m_linearVelocity).subLocal(Vec2.cross(b1.m_angularVelocity, r1)));
      Vec2 pivotForce = Mat22.mul(m_pivotMass, pivotCdot).mulLocal(-step.inv_dt);
      m_pivotForce.addLocal(pivotForce);

      Vec2 P = pivotForce.mul(step.dt);
      b1.m_linearVelocity.x -= b1.m_invMass * P.x;
      b1.m_linearVelocity.y -= b1.m_invMass * P.y;
      b1.m_angularVelocity -= b1.m_invI * Vec2.cross(r1, P);

      b2.m_linearVelocity.x += b2.m_invMass * P.x;
      b2.m_linearVelocity.y += b2.m_invMass * P.y;
      b2.m_angularVelocity += b2.m_invI * Vec2.cross(r2, P);

      if (m_enableMotor && m_limitState != LimitState.EQUAL_LIMITS) {
        float motorCdot = b2.m_angularVelocity - b1.m_angularVelocity - m_motorSpeed;
        float motorForce = -step.inv_dt * m_motorMass * motorCdot;
        float oldMotorForce = m_motorForce;
        m_motorForce = MathUtils.clamp(m_motorForce + motorForce, -m_maxMotorTorque, m_maxMotorTorque);
        motorForce = m_motorForce - oldMotorForce;

        float P2 = step.dt * motorForce;
        b1.m_angularVelocity -= b1.m_invI * P2;
        b2.m_angularVelocity += b2.m_invI * P2;
      }

      if (m_enableLimit && m_limitState != LimitState.INACTIVE_LIMIT) {
        float limitCdot = b2.m_angularVelocity - b1.m_angularVelocity;
        float limitForce = -step.inv_dt * m_motorMass * limitCdot;

        if (m_limitState == LimitState.EQUAL_LIMITS) {
          m_limitForce += limitForce;
        } else if (m_limitState == LimitState.AT_LOWER_LIMIT) {
          float oldLimitForce = m_limitForce;
          m_limitForce = Math.max(m_limitForce + limitForce, 0.0f);
          limitForce = m_limitForce - oldLimitForce;
        } else if (m_limitState == LimitState.AT_UPPER_LIMIT) {
          float oldLimitForce = m_limitForce;
          m_limitForce = Math.min(m_limitForce + limitForce, 0.0f);
          limitForce = m_limitForce - oldLimitForce;
        }

        float P2 = step.dt * limitForce;
        b1.m_angularVelocity -= b1.m_invI * P2;
        b2.m_angularVelocity += b2.m_invI * P2;
      }
    }

    @Override
    public boolean solvePositionConstraints() {
        Body b1 = m_body1;
        Body b2 = m_body2;
        
        float positionError = 0f;

        // Solve point-to-point position error.
        Vec2 r1 = Mat22.mul(b1.m_xf.R, m_localAnchor1.sub(b1.getLocalCenter()));
      Vec2 r2 = Mat22.mul(b2.m_xf.R, m_localAnchor2.sub(b2.getLocalCenter()));

      Vec2 p1 = b1.m_sweep.c.add(r1);
      Vec2 p2 = b2.m_sweep.c.add(r2);
      Vec2 ptpC = p2.sub(p1);

      positionError = ptpC.length();

      // Prevent overly large corrections.
      //public b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection);
      //ptpC = b2Clamp(ptpC, -dpMax, dpMax);

      float invMass1 = b1.m_invMass, invMass2 = b2.m_invMass;
      float invI1 = b1.m_invI, invI2 = b2.m_invI;

        Mat22 K1 = new Mat22();
        K1.col1.x = invMass1 + invMass2;    K1.col2.x = 0.0f;
        K1.col1.y = 0.0f;                   K1.col2.y = invMass1 + invMass2;
        
        Mat22 K2 = new Mat22();
        K2.col1.x =  invI1 * r1.y * r1.y;   K2.col2.x = -invI1 * r1.x * r1.y;
        K2.col1.y = -invI1 * r1.x * r1.y;   K2.col2.y =  invI1 * r1.x * r1.x;

        Mat22 K3 = new Mat22();
        K3.col1.x =  invI2 * r2.y * r2.y;   K3.col2.x = -invI2 * r2.x * r2.y;
        K3.col1.y = -invI2 * r2.x * r2.y;   K3.col2.y =  invI2 * r2.x * r2.x;

        Mat22 K = K1.add(K2).add(K3);
        Vec2 impulse = K.solve(ptpC.negate());

      b1.m_sweep.c.x -= b1.m_invMass * impulse.x;
      b1.m_sweep.c.y -= b1.m_invMass * impulse.y;
      b1.m_sweep.a -= b1.m_invI * Vec2.cross(r1, impulse);

      b2.m_sweep.c.x += b2.m_invMass * impulse.x;
      b2.m_sweep.c.y += b2.m_invMass * impulse.y;
      b2.m_sweep.a += b2.m_invI * Vec2.cross(r2, impulse);

      b1.synchronizeTransform();
      b2.synchronizeTransform();

      // Handle limits.
      float angularError = 0.0f;


      if (m_enableLimit && m_limitState != LimitState.INACTIVE_LIMIT) {
        float angle = b2.m_sweep.a - b1.m_sweep.a - m_referenceAngle;
        float limitImpulse = 0.0f;

        if (m_limitState == LimitState.EQUAL_LIMITS) {
          // Prevent large angular corrections
          float limitC = MathUtils.clamp(angle, -Settings.maxAngularCorrection, Settings.maxAngularCorrection);
          limitImpulse = -m_motorMass * limitC;
          angularError = Math.abs(limitC);
        } else if (m_limitState == LimitState.AT_LOWER_LIMIT) {
          float limitC = angle - m_lowerAngle;
          angularError = Math.max(0.0f, -limitC);

          // Prevent large angular corrections and allow some slop.
          limitC = MathUtils.clamp(limitC + Settings.angularSlop, -Settings.maxAngularCorrection, 0.0f);
          limitImpulse = -m_motorMass * limitC;
          float oldLimitImpulse = m_limitPositionImpulse;
          m_limitPositionImpulse = Math.max(m_limitPositionImpulse + limitImpulse, 0.0f);
          limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
        } else if (m_limitState == LimitState.AT_UPPER_LIMIT) {
          float limitC = angle - m_upperAngle;
          angularError = Math.max(0.0f, limitC);

          // Prevent large angular corrections and allow some slop.
          limitC = MathUtils.clamp(limitC - Settings.angularSlop, 0.0f, Settings.maxAngularCorrection);
          limitImpulse = -m_motorMass * limitC;
          float oldLimitImpulse = m_limitPositionImpulse;
          m_limitPositionImpulse = Math.min(m_limitPositionImpulse + limitImpulse, 0.0f);
          limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
        }

        b1.m_sweep.a -= b1.m_invI * limitImpulse;
        b2.m_sweep.a += b2.m_invI * limitImpulse;

        b1.synchronizeTransform();
        b2.synchronizeTransform();
      }

      return positionError <= Settings.linearSlop && angularError <= Settings.angularSlop;
    }

    public Vec2 getAnchor1() {
      return m_body1.getWorldPoint(m_localAnchor1);
    }

    public Vec2 getAnchor2() {
      return m_body2.getWorldPoint(m_localAnchor2);
    }

    public Vec2 getReactionForce() {
      return m_pivotForce;
    }

    public float getReactionTorque() {
      return m_limitForce;
    }

    public float getJointAngle() {
      Body b1 = m_body1;
      Body b2 = m_body2;
      return b2.m_sweep.a - b1.m_sweep.a - m_referenceAngle;
    }

    public float getJointSpeed() {
      Body b1 = m_body1;
      Body b2 = m_body2;
      return b2.m_angularVelocity - b1.m_angularVelocity;
    }

    public boolean isMotorEnabled() {
      return m_enableMotor;
    }

    public void enableMotor(boolean flag) {
      m_enableMotor = flag;
    }

    public float getMotorTorque() {
      return m_motorForce;
    }

    public void setMotorSpeed(float speed) {
      m_motorSpeed = speed;
    }

    public void setMaxMotorTorque(float torque) {
      m_maxMotorTorque = torque;
    }

    public boolean isLimitEnabled() {
      return m_enableLimit;
    }

    public void enableLimit(boolean flag) {
      m_enableLimit = flag;
    }

    public float getLowerLimit() {
      return m_lowerAngle;
    }

    public float getUpperLimit() {
      return m_upperAngle;
    }

    public void setLimits(float lower, float upper) {
      assert(lower <= upper);
      m_lowerAngle = lower;
      m_upperAngle = upper;
    }
}
java2s.com  | Contact Us | Privacy Policy
Copyright 2009 - 12 Demo Source and Support. All rights reserved.
All other trademarks are property of their respective owners.