home *** CD-ROM | disk | FTP | other *** search
/ 404 Jogos / CLJG.iso / Diversos / Beez.swf / scripts / Box2D / Dynamics / Joints / b2PrismaticJoint.as < prev    next >
Encoding:
Text File  |  2008-09-03  |  22.8 KB  |  542 lines
  1. package Box2D.Dynamics.Joints
  2. {
  3.    import Box2D.Common.Math.b2Mat22;
  4.    import Box2D.Common.Math.b2Math;
  5.    import Box2D.Common.Math.b2Vec2;
  6.    import Box2D.Common.b2Settings;
  7.    import Box2D.Dynamics.b2Body;
  8.    import Box2D.Dynamics.b2TimeStep;
  9.    
  10.    public class b2PrismaticJoint extends b2Joint
  11.    {
  12.        
  13.       
  14.       public var m_limitForce:Number;
  15.       
  16.       public var m_lowerTranslation:Number;
  17.       
  18.       public var m_localXAxis1:b2Vec2;
  19.       
  20.       public var m_refAngle:Number;
  21.       
  22.       public var m_torque:Number;
  23.       
  24.       public var m_motorForce:Number;
  25.       
  26.       public var m_enableLimit:Boolean;
  27.       
  28.       public var m_angularMass:Number;
  29.       
  30.       public var m_maxMotorForce:Number;
  31.       
  32.       public var m_localYAxis1:b2Vec2;
  33.       
  34.       public var m_force:Number;
  35.       
  36.       public var m_motorMass:Number;
  37.       
  38.       public var m_upperTranslation:Number;
  39.       
  40.       public var m_localAnchor1:b2Vec2;
  41.       
  42.       public var m_localAnchor2:b2Vec2;
  43.       
  44.       public var m_limitState:int;
  45.       
  46.       public var m_linearMass:Number;
  47.       
  48.       public var m_motorJacobian:b2Jacobian;
  49.       
  50.       public var m_limitPositionImpulse:Number;
  51.       
  52.       public var m_motorSpeed:Number;
  53.       
  54.       public var m_enableMotor:Boolean;
  55.       
  56.       public var m_linearJacobian:b2Jacobian;
  57.       
  58.       public function b2PrismaticJoint(def:b2PrismaticJointDef)
  59.       {
  60.          var tMat:b2Mat22 = null;
  61.          var tX:Number = NaN;
  62.          var tY:Number = NaN;
  63.          m_localAnchor1 = new b2Vec2();
  64.          m_localAnchor2 = new b2Vec2();
  65.          m_localXAxis1 = new b2Vec2();
  66.          m_localYAxis1 = new b2Vec2();
  67.          m_linearJacobian = new b2Jacobian();
  68.          m_motorJacobian = new b2Jacobian();
  69.          super(def);
  70.          m_localAnchor1.SetV(def.localAnchor1);
  71.          m_localAnchor2.SetV(def.localAnchor2);
  72.          m_localXAxis1.SetV(def.localAxis1);
  73.          m_localYAxis1.x = -m_localXAxis1.y;
  74.          m_localYAxis1.y = m_localXAxis1.x;
  75.          m_refAngle = def.referenceAngle;
  76.          m_linearJacobian.SetZero();
  77.          m_linearMass = 0;
  78.          m_force = 0;
  79.          m_angularMass = 0;
  80.          m_torque = 0;
  81.          m_motorJacobian.SetZero();
  82.          m_motorMass = 0;
  83.          m_motorForce = 0;
  84.          m_limitForce = 0;
  85.          m_limitPositionImpulse = 0;
  86.          m_lowerTranslation = def.lowerTranslation;
  87.          m_upperTranslation = def.upperTranslation;
  88.          m_maxMotorForce = def.maxMotorForce;
  89.          m_motorSpeed = def.motorSpeed;
  90.          m_enableLimit = def.enableLimit;
  91.          m_enableMotor = def.enableMotor;
  92.       }
  93.       
  94.       override public function SolveVelocityConstraints(step:b2TimeStep) : void
  95.       {
  96.          var oldLimitForce:Number = NaN;
  97.          var motorCdot:Number = NaN;
  98.          var motorForce:Number = NaN;
  99.          var oldMotorForce:Number = NaN;
  100.          var limitCdot:Number = NaN;
  101.          var limitForce:Number = NaN;
  102.          var b1:b2Body = m_body1;
  103.          var b2:b2Body = m_body2;
  104.          var invMass1:Number = b1.m_invMass;
  105.          var invMass2:Number = b2.m_invMass;
  106.          var invI1:Number = b1.m_invI;
  107.          var invI2:Number = b2.m_invI;
  108.          var linearCdot:Number = m_linearJacobian.Compute(b1.m_linearVelocity,b1.m_angularVelocity,b2.m_linearVelocity,b2.m_angularVelocity);
  109.          var force:Number = -step.inv_dt * m_linearMass * linearCdot;
  110.          m_force += force;
  111.          var P:Number = step.dt * force;
  112.          b1.m_linearVelocity.x += invMass1 * P * m_linearJacobian.linear1.x;
  113.          b1.m_linearVelocity.y += invMass1 * P * m_linearJacobian.linear1.y;
  114.          b1.m_angularVelocity += invI1 * P * m_linearJacobian.angular1;
  115.          b2.m_linearVelocity.x += invMass2 * P * m_linearJacobian.linear2.x;
  116.          b2.m_linearVelocity.y += invMass2 * P * m_linearJacobian.linear2.y;
  117.          b2.m_angularVelocity += invI2 * P * m_linearJacobian.angular2;
  118.          var angularCdot:Number = b2.m_angularVelocity - b1.m_angularVelocity;
  119.          var torque:Number = -step.inv_dt * m_angularMass * angularCdot;
  120.          m_torque += torque;
  121.          var L:Number = step.dt * torque;
  122.          b1.m_angularVelocity -= invI1 * L;
  123.          b2.m_angularVelocity += invI2 * L;
  124.          if(m_enableMotor && m_limitState != e_equalLimits)
  125.          {
  126.             motorCdot = m_motorJacobian.Compute(b1.m_linearVelocity,b1.m_angularVelocity,b2.m_linearVelocity,b2.m_angularVelocity) - m_motorSpeed;
  127.             motorForce = -step.inv_dt * m_motorMass * motorCdot;
  128.             oldMotorForce = m_motorForce;
  129.             m_motorForce = b2Math.b2Clamp(m_motorForce + motorForce,-m_maxMotorForce,m_maxMotorForce);
  130.             motorForce = m_motorForce - oldMotorForce;
  131.             P = step.dt * motorForce;
  132.             b1.m_linearVelocity.x += invMass1 * P * m_motorJacobian.linear1.x;
  133.             b1.m_linearVelocity.y += invMass1 * P * m_motorJacobian.linear1.y;
  134.             b1.m_angularVelocity += invI1 * P * m_motorJacobian.angular1;
  135.             b2.m_linearVelocity.x += invMass2 * P * m_motorJacobian.linear2.x;
  136.             b2.m_linearVelocity.y += invMass2 * P * m_motorJacobian.linear2.y;
  137.             b2.m_angularVelocity += invI2 * P * m_motorJacobian.angular2;
  138.          }
  139.          if(m_enableLimit && m_limitState != e_inactiveLimit)
  140.          {
  141.             limitCdot = m_motorJacobian.Compute(b1.m_linearVelocity,b1.m_angularVelocity,b2.m_linearVelocity,b2.m_angularVelocity);
  142.             limitForce = -step.inv_dt * m_motorMass * limitCdot;
  143.             if(m_limitState == e_equalLimits)
  144.             {
  145.                m_limitForce += limitForce;
  146.             }
  147.             else if(m_limitState == e_atLowerLimit)
  148.             {
  149.                oldLimitForce = m_limitForce;
  150.                m_limitForce = b2Math.b2Max(m_limitForce + limitForce,0);
  151.                limitForce = m_limitForce - oldLimitForce;
  152.             }
  153.             else if(m_limitState == e_atUpperLimit)
  154.             {
  155.                oldLimitForce = m_limitForce;
  156.                m_limitForce = b2Math.b2Min(m_limitForce + limitForce,0);
  157.                limitForce = m_limitForce - oldLimitForce;
  158.             }
  159.             P = step.dt * limitForce;
  160.             b1.m_linearVelocity.x += invMass1 * P * m_motorJacobian.linear1.x;
  161.             b1.m_linearVelocity.y += invMass1 * P * m_motorJacobian.linear1.y;
  162.             b1.m_angularVelocity += invI1 * P * m_motorJacobian.angular1;
  163.             b2.m_linearVelocity.x += invMass2 * P * m_motorJacobian.linear2.x;
  164.             b2.m_linearVelocity.y += invMass2 * P * m_motorJacobian.linear2.y;
  165.             b2.m_angularVelocity += invI2 * P * m_motorJacobian.angular2;
  166.          }
  167.       }
  168.       
  169.       override public function GetAnchor1() : b2Vec2
  170.       {
  171.          return m_body1.GetWorldPoint(m_localAnchor1);
  172.       }
  173.       
  174.       override public function GetAnchor2() : b2Vec2
  175.       {
  176.          return m_body2.GetWorldPoint(m_localAnchor2);
  177.       }
  178.       
  179.       public function GetUpperLimit() : Number
  180.       {
  181.          return m_upperTranslation;
  182.       }
  183.       
  184.       public function GetLowerLimit() : Number
  185.       {
  186.          return m_lowerTranslation;
  187.       }
  188.       
  189.       public function EnableMotor(flag:Boolean) : void
  190.       {
  191.          m_enableMotor = flag;
  192.       }
  193.       
  194.       public function GetJointTranslation() : Number
  195.       {
  196.          var tMat:b2Mat22 = null;
  197.          var b1:b2Body = m_body1;
  198.          var b2:b2Body = m_body2;
  199.          var p1:b2Vec2 = b1.GetWorldPoint(m_localAnchor1);
  200.          var p2:b2Vec2 = b2.GetWorldPoint(m_localAnchor2);
  201.          var dX:Number = p2.x - p1.x;
  202.          var dY:Number = p2.y - p1.y;
  203.          var axis:b2Vec2 = b1.GetWorldVector(m_localXAxis1);
  204.          return axis.x * dX + axis.y * dY;
  205.       }
  206.       
  207.       public function GetMotorSpeed() : Number
  208.       {
  209.          return m_motorSpeed;
  210.       }
  211.       
  212.       override public function GetReactionForce() : b2Vec2
  213.       {
  214.          var tMat:b2Mat22 = m_body1.m_xf.R;
  215.          var ax1X:Number = m_limitForce * (tMat.col1.x * m_localXAxis1.x + tMat.col2.x * m_localXAxis1.y);
  216.          var ax1Y:Number = m_limitForce * (tMat.col1.y * m_localXAxis1.x + tMat.col2.y * m_localXAxis1.y);
  217.          var ay1X:Number = m_force * (tMat.col1.x * m_localYAxis1.x + tMat.col2.x * m_localYAxis1.y);
  218.          var ay1Y:Number = m_force * (tMat.col1.y * m_localYAxis1.x + tMat.col2.y * m_localYAxis1.y);
  219.          return new b2Vec2(m_limitForce * ax1X + m_force * ay1X,m_limitForce * ax1Y + m_force * ay1Y);
  220.       }
  221.       
  222.       override public function SolvePositionConstraints() : Boolean
  223.       {
  224.          var limitC:Number = NaN;
  225.          var oldLimitImpulse:Number = NaN;
  226.          var tMat:b2Mat22 = null;
  227.          var tX:Number = NaN;
  228.          var ax1X:Number = NaN;
  229.          var ax1Y:Number = NaN;
  230.          var translation:Number = NaN;
  231.          var limitImpulse:Number = NaN;
  232.          var b1:b2Body = m_body1;
  233.          var b2:b2Body = m_body2;
  234.          var invMass1:Number = b1.m_invMass;
  235.          var invMass2:Number = b2.m_invMass;
  236.          var invI1:Number = b1.m_invI;
  237.          var invI2:Number = b2.m_invI;
  238.          tMat = b1.m_xf.R;
  239.          var r1X:Number = m_localAnchor1.x - b1.m_sweep.localCenter.x;
  240.          var r1Y:Number = m_localAnchor1.y - b1.m_sweep.localCenter.y;
  241.          tX = tMat.col1.x * r1X + tMat.col2.x * r1Y;
  242.          r1Y = tMat.col1.y * r1X + tMat.col2.y * r1Y;
  243.          r1X = tX;
  244.          tMat = b2.m_xf.R;
  245.          var r2X:Number = m_localAnchor2.x - b2.m_sweep.localCenter.x;
  246.          var r2Y:Number = m_localAnchor2.y - b2.m_sweep.localCenter.y;
  247.          tX = tMat.col1.x * r2X + tMat.col2.x * r2Y;
  248.          r2Y = tMat.col1.y * r2X + tMat.col2.y * r2Y;
  249.          r2X = tX;
  250.          var p1X:Number = b1.m_sweep.c.x + r1X;
  251.          var p1Y:Number = b1.m_sweep.c.y + r1Y;
  252.          var p2X:Number = b2.m_sweep.c.x + r2X;
  253.          var p2Y:Number = b2.m_sweep.c.y + r2Y;
  254.          var dX:Number = p2X - p1X;
  255.          var dY:Number = p2Y - p1Y;
  256.          tMat = b1.m_xf.R;
  257.          var ay1X:Number = tMat.col1.x * m_localYAxis1.x + tMat.col2.x * m_localYAxis1.y;
  258.          var ay1Y:Number = tMat.col1.y * m_localYAxis1.x + tMat.col2.y * m_localYAxis1.y;
  259.          var linearC:Number = ay1X * dX + ay1Y * dY;
  260.          linearC = b2Math.b2Clamp(linearC,-b2Settings.b2_maxLinearCorrection,b2Settings.b2_maxLinearCorrection);
  261.          var linearImpulse:Number = -m_linearMass * linearC;
  262.          b1.m_sweep.c.x += invMass1 * linearImpulse * m_linearJacobian.linear1.x;
  263.          b1.m_sweep.c.y += invMass1 * linearImpulse * m_linearJacobian.linear1.y;
  264.          b1.m_sweep.a += invI1 * linearImpulse * m_linearJacobian.angular1;
  265.          b2.m_sweep.c.x += invMass2 * linearImpulse * m_linearJacobian.linear2.x;
  266.          b2.m_sweep.c.y += invMass2 * linearImpulse * m_linearJacobian.linear2.y;
  267.          b2.m_sweep.a += invI2 * linearImpulse * m_linearJacobian.angular2;
  268.          var positionError:Number = b2Math.b2Abs(linearC);
  269.          var angularC:Number = b2.m_sweep.a - b1.m_sweep.a - m_refAngle;
  270.          angularC = b2Math.b2Clamp(angularC,-b2Settings.b2_maxAngularCorrection,b2Settings.b2_maxAngularCorrection);
  271.          var angularImpulse:Number = -m_angularMass * angularC;
  272.          b1.m_sweep.a -= b1.m_invI * angularImpulse;
  273.          b2.m_sweep.a += b2.m_invI * angularImpulse;
  274.          b1.SynchronizeTransform();
  275.          b2.SynchronizeTransform();
  276.          var angularError:Number = b2Math.b2Abs(angularC);
  277.          if(m_enableLimit && m_limitState != e_inactiveLimit)
  278.          {
  279.             tMat = b1.m_xf.R;
  280.             r1X = m_localAnchor1.x - b1.m_sweep.localCenter.x;
  281.             r1Y = m_localAnchor1.y - b1.m_sweep.localCenter.y;
  282.             tX = tMat.col1.x * r1X + tMat.col2.x * r1Y;
  283.             r1Y = tMat.col1.y * r1X + tMat.col2.y * r1Y;
  284.             r1X = tX;
  285.             tMat = b2.m_xf.R;
  286.             r2X = m_localAnchor2.x - b2.m_sweep.localCenter.x;
  287.             r2Y = m_localAnchor2.y - b2.m_sweep.localCenter.y;
  288.             tX = tMat.col1.x * r2X + tMat.col2.x * r2Y;
  289.             r2Y = tMat.col1.y * r2X + tMat.col2.y * r2Y;
  290.             r2X = tX;
  291.             p1X = b1.m_sweep.c.x + r1X;
  292.             p1Y = b1.m_sweep.c.y + r1Y;
  293.             p2X = b2.m_sweep.c.x + r2X;
  294.             p2Y = b2.m_sweep.c.y + r2Y;
  295.             dX = p2X - p1X;
  296.             dY = p2Y - p1Y;
  297.             tMat = b1.m_xf.R;
  298.             ax1X = tMat.col1.x * m_localXAxis1.x + tMat.col2.x * m_localXAxis1.y;
  299.             ax1Y = tMat.col1.y * m_localXAxis1.x + tMat.col2.y * m_localXAxis1.y;
  300.             translation = ax1X * dX + ax1Y * dY;
  301.             limitImpulse = 0;
  302.             if(m_limitState == e_equalLimits)
  303.             {
  304.                limitC = b2Math.b2Clamp(translation,-b2Settings.b2_maxLinearCorrection,b2Settings.b2_maxLinearCorrection);
  305.                limitImpulse = -m_motorMass * limitC;
  306.                positionError = b2Math.b2Max(positionError,b2Math.b2Abs(angularC));
  307.             }
  308.             else if(m_limitState == e_atLowerLimit)
  309.             {
  310.                limitC = translation - m_lowerTranslation;
  311.                positionError = b2Math.b2Max(positionError,-limitC);
  312.                limitC = b2Math.b2Clamp(limitC + b2Settings.b2_linearSlop,-b2Settings.b2_maxLinearCorrection,0);
  313.                limitImpulse = -m_motorMass * limitC;
  314.                oldLimitImpulse = m_limitPositionImpulse;
  315.                m_limitPositionImpulse = b2Math.b2Max(m_limitPositionImpulse + limitImpulse,0);
  316.                limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
  317.             }
  318.             else if(m_limitState == e_atUpperLimit)
  319.             {
  320.                limitC = translation - m_upperTranslation;
  321.                positionError = b2Math.b2Max(positionError,limitC);
  322.                limitC = b2Math.b2Clamp(limitC - b2Settings.b2_linearSlop,0,b2Settings.b2_maxLinearCorrection);
  323.                limitImpulse = -m_motorMass * limitC;
  324.                oldLimitImpulse = m_limitPositionImpulse;
  325.                m_limitPositionImpulse = b2Math.b2Min(m_limitPositionImpulse + limitImpulse,0);
  326.                limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
  327.             }
  328.             b1.m_sweep.c.x += invMass1 * limitImpulse * m_motorJacobian.linear1.x;
  329.             b1.m_sweep.c.y += invMass1 * limitImpulse * m_motorJacobian.linear1.y;
  330.             b1.m_sweep.a += invI1 * limitImpulse * m_motorJacobian.angular1;
  331.             b2.m_sweep.c.x += invMass2 * limitImpulse * m_motorJacobian.linear2.x;
  332.             b2.m_sweep.c.y += invMass2 * limitImpulse * m_motorJacobian.linear2.y;
  333.             b2.m_sweep.a += invI2 * limitImpulse * m_motorJacobian.angular2;
  334.             b1.SynchronizeTransform();
  335.             b2.SynchronizeTransform();
  336.          }
  337.          return positionError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop;
  338.       }
  339.       
  340.       public function SetMotorSpeed(speed:Number) : void
  341.       {
  342.          m_motorSpeed = speed;
  343.       }
  344.       
  345.       public function GetJointSpeed() : Number
  346.       {
  347.          var tMat:b2Mat22 = null;
  348.          var b1:b2Body = m_body1;
  349.          var b2:b2Body = m_body2;
  350.          tMat = b1.m_xf.R;
  351.          var r1X:Number = m_localAnchor1.x - b1.m_sweep.localCenter.x;
  352.          var r1Y:Number = m_localAnchor1.y - b1.m_sweep.localCenter.y;
  353.          var tX:Number = tMat.col1.x * r1X + tMat.col2.x * r1Y;
  354.          r1Y = tMat.col1.y * r1X + tMat.col2.y * r1Y;
  355.          r1X = tX;
  356.          tMat = b2.m_xf.R;
  357.          var r2X:Number = m_localAnchor2.x - b2.m_sweep.localCenter.x;
  358.          var r2Y:Number = m_localAnchor2.y - b2.m_sweep.localCenter.y;
  359.          tX = tMat.col1.x * r2X + tMat.col2.x * r2Y;
  360.          r2Y = tMat.col1.y * r2X + tMat.col2.y * r2Y;
  361.          r2X = tX;
  362.          var p1X:Number = b1.m_sweep.c.x + r1X;
  363.          var p1Y:Number = b1.m_sweep.c.y + r1Y;
  364.          var p2X:Number = b2.m_sweep.c.x + r2X;
  365.          var p2Y:Number = b2.m_sweep.c.y + r2Y;
  366.          var dX:Number = p2X - p1X;
  367.          var dY:Number = p2Y - p1Y;
  368.          var axis:b2Vec2 = b1.GetWorldVector(m_localXAxis1);
  369.          var v1:b2Vec2 = b1.m_linearVelocity;
  370.          var v2:b2Vec2 = b2.m_linearVelocity;
  371.          var w1:Number = b1.m_angularVelocity;
  372.          var w2:Number = b2.m_angularVelocity;
  373.          return dX * (-w1 * axis.y) + dY * (w1 * axis.x) + (axis.x * (v2.x + -w2 * r2Y - v1.x - -w1 * r1Y) + axis.y * (v2.y + w2 * r2X - v1.y - w1 * r1X));
  374.       }
  375.       
  376.       override public function InitVelocityConstraints(step:b2TimeStep) : void
  377.       {
  378.          var tMat:b2Mat22 = null;
  379.          var tX:Number = NaN;
  380.          var ax1X:Number = NaN;
  381.          var ax1Y:Number = NaN;
  382.          var dX:Number = NaN;
  383.          var dY:Number = NaN;
  384.          var jointTranslation:Number = NaN;
  385.          var P1X:Number = NaN;
  386.          var P1Y:Number = NaN;
  387.          var P2X:Number = NaN;
  388.          var P2Y:Number = NaN;
  389.          var L1:Number = NaN;
  390.          var L2:Number = NaN;
  391.          var b1:b2Body = m_body1;
  392.          var b2:b2Body = m_body2;
  393.          tMat = b1.m_xf.R;
  394.          var r1X:Number = m_localAnchor1.x - b1.m_sweep.localCenter.x;
  395.          var r1Y:Number = m_localAnchor1.y - b1.m_sweep.localCenter.y;
  396.          tX = tMat.col1.x * r1X + tMat.col2.x * r1Y;
  397.          r1Y = tMat.col1.y * r1X + tMat.col2.y * r1Y;
  398.          r1X = tX;
  399.          tMat = b2.m_xf.R;
  400.          var r2X:Number = m_localAnchor2.x - b2.m_sweep.localCenter.x;
  401.          var r2Y:Number = m_localAnchor2.y - b2.m_sweep.localCenter.y;
  402.          tX = tMat.col1.x * r2X + tMat.col2.x * r2Y;
  403.          r2Y = tMat.col1.y * r2X + tMat.col2.y * r2Y;
  404.          r2X = tX;
  405.          var invMass1:Number = b1.m_invMass;
  406.          var invMass2:Number = b2.m_invMass;
  407.          var invI1:Number = b1.m_invI;
  408.          var invI2:Number = b2.m_invI;
  409.          tMat = b1.m_xf.R;
  410.          var ay1X:Number = tMat.col1.x * m_localYAxis1.x + tMat.col2.x * m_localYAxis1.y;
  411.          var ay1Y:Number = tMat.col1.y * m_localYAxis1.x + tMat.col2.y * m_localYAxis1.y;
  412.          var eX:Number = b2.m_sweep.c.x + r2X - b1.m_sweep.c.x;
  413.          var eY:Number = b2.m_sweep.c.y + r2Y - b1.m_sweep.c.y;
  414.          m_linearJacobian.linear1.x = -ay1X;
  415.          m_linearJacobian.linear1.y = -ay1Y;
  416.          m_linearJacobian.linear2.x = ay1X;
  417.          m_linearJacobian.linear2.y = ay1Y;
  418.          m_linearJacobian.angular1 = -(eX * ay1Y - eY * ay1X);
  419.          m_linearJacobian.angular2 = r2X * ay1Y - r2Y * ay1X;
  420.          m_linearMass = invMass1 + invI1 * m_linearJacobian.angular1 * m_linearJacobian.angular1 + invMass2 + invI2 * m_linearJacobian.angular2 * m_linearJacobian.angular2;
  421.          m_linearMass = 1 / m_linearMass;
  422.          m_angularMass = invI1 + invI2;
  423.          if(m_angularMass > Number.MIN_VALUE)
  424.          {
  425.             m_angularMass = 1 / m_angularMass;
  426.          }
  427.          if(m_enableLimit || m_enableMotor)
  428.          {
  429.             tMat = b1.m_xf.R;
  430.             ax1X = tMat.col1.x * m_localXAxis1.x + tMat.col2.x * m_localXAxis1.y;
  431.             ax1Y = tMat.col1.y * m_localXAxis1.x + tMat.col2.y * m_localXAxis1.y;
  432.             m_motorJacobian.linear1.x = -ax1X;
  433.             m_motorJacobian.linear1.y = -ax1Y;
  434.             m_motorJacobian.linear2.x = ax1X;
  435.             m_motorJacobian.linear2.y = ax1Y;
  436.             m_motorJacobian.angular1 = -(eX * ax1Y - eY * ax1X);
  437.             m_motorJacobian.angular2 = r2X * ax1Y - r2Y * ax1X;
  438.             m_motorMass = invMass1 + invI1 * m_motorJacobian.angular1 * m_motorJacobian.angular1 + invMass2 + invI2 * m_motorJacobian.angular2 * m_motorJacobian.angular2;
  439.             m_motorMass = 1 / m_motorMass;
  440.             if(m_enableLimit)
  441.             {
  442.                dX = eX - r1X;
  443.                dY = eY - r1Y;
  444.                jointTranslation = ax1X * dX + ax1Y * dY;
  445.                if(b2Math.b2Abs(m_upperTranslation - m_lowerTranslation) < 2 * b2Settings.b2_linearSlop)
  446.                {
  447.                   m_limitState = e_equalLimits;
  448.                }
  449.                else if(jointTranslation <= m_lowerTranslation)
  450.                {
  451.                   if(m_limitState != e_atLowerLimit)
  452.                   {
  453.                      m_limitForce = 0;
  454.                   }
  455.                   m_limitState = e_atLowerLimit;
  456.                }
  457.                else if(jointTranslation >= m_upperTranslation)
  458.                {
  459.                   if(m_limitState != e_atUpperLimit)
  460.                   {
  461.                      m_limitForce = 0;
  462.                   }
  463.                   m_limitState = e_atUpperLimit;
  464.                }
  465.                else
  466.                {
  467.                   m_limitState = e_inactiveLimit;
  468.                   m_limitForce = 0;
  469.                }
  470.             }
  471.          }
  472.          if(m_enableMotor == false)
  473.          {
  474.             m_motorForce = 0;
  475.          }
  476.          if(m_enableLimit == false)
  477.          {
  478.             m_limitForce = 0;
  479.          }
  480.          if(step.warmStarting)
  481.          {
  482.             P1X = step.dt * (m_force * m_linearJacobian.linear1.x + (m_motorForce + m_limitForce) * m_motorJacobian.linear1.x);
  483.             P1Y = step.dt * (m_force * m_linearJacobian.linear1.y + (m_motorForce + m_limitForce) * m_motorJacobian.linear1.y);
  484.             P2X = step.dt * (m_force * m_linearJacobian.linear2.x + (m_motorForce + m_limitForce) * m_motorJacobian.linear2.x);
  485.             P2Y = step.dt * (m_force * m_linearJacobian.linear2.y + (m_motorForce + m_limitForce) * m_motorJacobian.linear2.y);
  486.             L1 = step.dt * (m_force * m_linearJacobian.angular1 - m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular1);
  487.             L2 = step.dt * (m_force * m_linearJacobian.angular2 + m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular2);
  488.             b1.m_linearVelocity.x += invMass1 * P1X;
  489.             b1.m_linearVelocity.y += invMass1 * P1Y;
  490.             b1.m_angularVelocity += invI1 * L1;
  491.             b2.m_linearVelocity.x += invMass2 * P2X;
  492.             b2.m_linearVelocity.y += invMass2 * P2Y;
  493.             b2.m_angularVelocity += invI2 * L2;
  494.          }
  495.          else
  496.          {
  497.             m_force = 0;
  498.             m_torque = 0;
  499.             m_limitForce = 0;
  500.             m_motorForce = 0;
  501.          }
  502.          m_limitPositionImpulse = 0;
  503.       }
  504.       
  505.       public function GetMotorForce() : Number
  506.       {
  507.          return m_motorForce;
  508.       }
  509.       
  510.       public function EnableLimit(flag:Boolean) : void
  511.       {
  512.          m_enableLimit = flag;
  513.       }
  514.       
  515.       public function SetMaxMotorForce(force:Number) : void
  516.       {
  517.          m_maxMotorForce = force;
  518.       }
  519.       
  520.       override public function GetReactionTorque() : Number
  521.       {
  522.          return m_torque;
  523.       }
  524.       
  525.       public function IsLimitEnabled() : Boolean
  526.       {
  527.          return m_enableLimit;
  528.       }
  529.       
  530.       public function IsMotorEnabled() : Boolean
  531.       {
  532.          return m_enableMotor;
  533.       }
  534.       
  535.       public function SetLimits(lower:Number, upper:Number) : void
  536.       {
  537.          m_lowerTranslation = lower;
  538.          m_upperTranslation = upper;
  539.       }
  540.    }
  541. }
  542.