home *** CD-ROM | disk | FTP | other *** search
/ 404 Jogos / CLJG.iso / Diversos / Beez.swf / scripts / Box2D / Dynamics / Joints / b2RevoluteJoint.as < prev    next >
Encoding:
Text File  |  2008-09-03  |  15.9 KB  |  439 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 b2RevoluteJoint extends b2Joint
  11.    {
  12.       
  13.       public static var tImpulse:b2Vec2 = new b2Vec2();
  14.        
  15.       
  16.       public var m_limitForce:Number;
  17.       
  18.       public var m_pivotMass:b2Mat22;
  19.       
  20.       public var m_motorForce:Number;
  21.       
  22.       public var m_enableLimit:Boolean;
  23.       
  24.       public var m_limitState:int;
  25.       
  26.       public var m_motorMass:Number;
  27.       
  28.       public var m_localAnchor1:b2Vec2;
  29.       
  30.       public var m_localAnchor2:b2Vec2;
  31.       
  32.       private var K1:b2Mat22;
  33.       
  34.       private var K2:b2Mat22;
  35.       
  36.       private var K3:b2Mat22;
  37.       
  38.       private var K:b2Mat22;
  39.       
  40.       public var m_pivotForce:b2Vec2;
  41.       
  42.       public var m_enableMotor:Boolean;
  43.       
  44.       public var m_referenceAngle:Number;
  45.       
  46.       public var m_limitPositionImpulse:Number;
  47.       
  48.       public var m_motorSpeed:Number;
  49.       
  50.       public var m_upperAngle:Number;
  51.       
  52.       public var m_lowerAngle:Number;
  53.       
  54.       public var m_maxMotorTorque:Number;
  55.       
  56.       public function b2RevoluteJoint(def:b2RevoluteJointDef)
  57.       {
  58.          K = new b2Mat22();
  59.          K1 = new b2Mat22();
  60.          K2 = new b2Mat22();
  61.          K3 = new b2Mat22();
  62.          m_localAnchor1 = new b2Vec2();
  63.          m_localAnchor2 = new b2Vec2();
  64.          m_pivotForce = new b2Vec2();
  65.          m_pivotMass = new b2Mat22();
  66.          super(def);
  67.          m_localAnchor1.SetV(def.localAnchor1);
  68.          m_localAnchor2.SetV(def.localAnchor2);
  69.          m_referenceAngle = def.referenceAngle;
  70.          m_pivotForce.Set(0,0);
  71.          m_motorForce = 0;
  72.          m_limitForce = 0;
  73.          m_limitPositionImpulse = 0;
  74.          m_lowerAngle = def.lowerAngle;
  75.          m_upperAngle = def.upperAngle;
  76.          m_maxMotorTorque = def.maxMotorTorque;
  77.          m_motorSpeed = def.motorSpeed;
  78.          m_enableLimit = def.enableLimit;
  79.          m_enableMotor = def.enableMotor;
  80.       }
  81.       
  82.       override public function SolveVelocityConstraints(step:b2TimeStep) : void
  83.       {
  84.          var tMat:b2Mat22 = null;
  85.          var tX:Number = NaN;
  86.          var oldLimitForce:Number = NaN;
  87.          var PY:Number = NaN;
  88.          var motorCdot:Number = NaN;
  89.          var motorForce:Number = NaN;
  90.          var oldMotorForce:Number = NaN;
  91.          var limitCdot:Number = NaN;
  92.          var limitForce:Number = NaN;
  93.          var b1:b2Body = m_body1;
  94.          var b2:b2Body = m_body2;
  95.          tMat = b1.m_xf.R;
  96.          var r1X:Number = m_localAnchor1.x - b1.m_sweep.localCenter.x;
  97.          var r1Y:Number = m_localAnchor1.y - b1.m_sweep.localCenter.y;
  98.          tX = tMat.col1.x * r1X + tMat.col2.x * r1Y;
  99.          r1Y = tMat.col1.y * r1X + tMat.col2.y * r1Y;
  100.          r1X = tX;
  101.          tMat = b2.m_xf.R;
  102.          var r2X:Number = m_localAnchor2.x - b2.m_sweep.localCenter.x;
  103.          var r2Y:Number = m_localAnchor2.y - b2.m_sweep.localCenter.y;
  104.          tX = tMat.col1.x * r2X + tMat.col2.x * r2Y;
  105.          r2Y = tMat.col1.y * r2X + tMat.col2.y * r2Y;
  106.          r2X = tX;
  107.          var pivotCdotX:Number = b2.m_linearVelocity.x + -b2.m_angularVelocity * r2Y - b1.m_linearVelocity.x - -b1.m_angularVelocity * r1Y;
  108.          var pivotCdotY:Number = b2.m_linearVelocity.y + b2.m_angularVelocity * r2X - b1.m_linearVelocity.y - b1.m_angularVelocity * r1X;
  109.          var pivotForceX:Number = -step.inv_dt * (m_pivotMass.col1.x * pivotCdotX + m_pivotMass.col2.x * pivotCdotY);
  110.          var pivotForceY:Number = -step.inv_dt * (m_pivotMass.col1.y * pivotCdotX + m_pivotMass.col2.y * pivotCdotY);
  111.          m_pivotForce.x += pivotForceX;
  112.          m_pivotForce.y += pivotForceY;
  113.          var PX:Number = step.dt * pivotForceX;
  114.          PY = step.dt * pivotForceY;
  115.          b1.m_linearVelocity.x -= b1.m_invMass * PX;
  116.          b1.m_linearVelocity.y -= b1.m_invMass * PY;
  117.          b1.m_angularVelocity -= b1.m_invI * (r1X * PY - r1Y * PX);
  118.          b2.m_linearVelocity.x += b2.m_invMass * PX;
  119.          b2.m_linearVelocity.y += b2.m_invMass * PY;
  120.          b2.m_angularVelocity += b2.m_invI * (r2X * PY - r2Y * PX);
  121.          if(m_enableMotor && m_limitState != e_equalLimits)
  122.          {
  123.             motorCdot = b2.m_angularVelocity - b1.m_angularVelocity - m_motorSpeed;
  124.             motorForce = -step.inv_dt * m_motorMass * motorCdot;
  125.             oldMotorForce = m_motorForce;
  126.             m_motorForce = b2Math.b2Clamp(m_motorForce + motorForce,-m_maxMotorTorque,m_maxMotorTorque);
  127.             motorForce = m_motorForce - oldMotorForce;
  128.             b1.m_angularVelocity -= b1.m_invI * step.dt * motorForce;
  129.             b2.m_angularVelocity += b2.m_invI * step.dt * motorForce;
  130.          }
  131.          if(m_enableLimit && m_limitState != e_inactiveLimit)
  132.          {
  133.             limitCdot = b2.m_angularVelocity - b1.m_angularVelocity;
  134.             limitForce = -step.inv_dt * m_motorMass * limitCdot;
  135.             if(m_limitState == e_equalLimits)
  136.             {
  137.                m_limitForce += limitForce;
  138.             }
  139.             else if(m_limitState == e_atLowerLimit)
  140.             {
  141.                oldLimitForce = m_limitForce;
  142.                m_limitForce = b2Math.b2Max(m_limitForce + limitForce,0);
  143.                limitForce = m_limitForce - oldLimitForce;
  144.             }
  145.             else if(m_limitState == e_atUpperLimit)
  146.             {
  147.                oldLimitForce = m_limitForce;
  148.                m_limitForce = b2Math.b2Min(m_limitForce + limitForce,0);
  149.                limitForce = m_limitForce - oldLimitForce;
  150.             }
  151.             b1.m_angularVelocity -= b1.m_invI * step.dt * limitForce;
  152.             b2.m_angularVelocity += b2.m_invI * step.dt * limitForce;
  153.          }
  154.       }
  155.       
  156.       override public function GetAnchor1() : b2Vec2
  157.       {
  158.          return m_body1.GetWorldPoint(m_localAnchor1);
  159.       }
  160.       
  161.       override public function GetAnchor2() : b2Vec2
  162.       {
  163.          return m_body2.GetWorldPoint(m_localAnchor2);
  164.       }
  165.       
  166.       public function GetUpperLimit() : Number
  167.       {
  168.          return m_upperAngle;
  169.       }
  170.       
  171.       public function GetLowerLimit() : Number
  172.       {
  173.          return m_lowerAngle;
  174.       }
  175.       
  176.       public function EnableMotor(flag:Boolean) : void
  177.       {
  178.          m_enableMotor = flag;
  179.       }
  180.       
  181.       public function GetMotorSpeed() : Number
  182.       {
  183.          return m_motorSpeed;
  184.       }
  185.       
  186.       override public function GetReactionForce() : b2Vec2
  187.       {
  188.          return m_pivotForce;
  189.       }
  190.       
  191.       override public function SolvePositionConstraints() : Boolean
  192.       {
  193.          var oldLimitImpulse:Number = NaN;
  194.          var limitC:Number = NaN;
  195.          var tMat:b2Mat22 = null;
  196.          var angle:Number = NaN;
  197.          var limitImpulse:Number = NaN;
  198.          var b1:b2Body = m_body1;
  199.          var b2:b2Body = m_body2;
  200.          var positionError:Number = 0;
  201.          tMat = b1.m_xf.R;
  202.          var r1X:Number = m_localAnchor1.x - b1.m_sweep.localCenter.x;
  203.          var r1Y:Number = m_localAnchor1.y - b1.m_sweep.localCenter.y;
  204.          var tX:Number = tMat.col1.x * r1X + tMat.col2.x * r1Y;
  205.          r1Y = tMat.col1.y * r1X + tMat.col2.y * r1Y;
  206.          r1X = tX;
  207.          tMat = b2.m_xf.R;
  208.          var r2X:Number = m_localAnchor2.x - b2.m_sweep.localCenter.x;
  209.          var r2Y:Number = m_localAnchor2.y - b2.m_sweep.localCenter.y;
  210.          tX = tMat.col1.x * r2X + tMat.col2.x * r2Y;
  211.          r2Y = tMat.col1.y * r2X + tMat.col2.y * r2Y;
  212.          r2X = tX;
  213.          var p1X:Number = b1.m_sweep.c.x + r1X;
  214.          var p1Y:Number = b1.m_sweep.c.y + r1Y;
  215.          var p2X:Number = b2.m_sweep.c.x + r2X;
  216.          var p2Y:Number = b2.m_sweep.c.y + r2Y;
  217.          var ptpCX:Number = p2X - p1X;
  218.          var ptpCY:Number = p2Y - p1Y;
  219.          positionError = Math.sqrt(ptpCX * ptpCX + ptpCY * ptpCY);
  220.          var invMass1:Number = b1.m_invMass;
  221.          var invMass2:Number = b2.m_invMass;
  222.          var invI1:Number = b1.m_invI;
  223.          var invI2:Number = b2.m_invI;
  224.          K1.col1.x = invMass1 + invMass2;
  225.          K1.col2.x = 0;
  226.          K1.col1.y = 0;
  227.          K1.col2.y = invMass1 + invMass2;
  228.          K2.col1.x = invI1 * r1Y * r1Y;
  229.          K2.col2.x = -invI1 * r1X * r1Y;
  230.          K2.col1.y = -invI1 * r1X * r1Y;
  231.          K2.col2.y = invI1 * r1X * r1X;
  232.          K3.col1.x = invI2 * r2Y * r2Y;
  233.          K3.col2.x = -invI2 * r2X * r2Y;
  234.          K3.col1.y = -invI2 * r2X * r2Y;
  235.          K3.col2.y = invI2 * r2X * r2X;
  236.          K.SetM(K1);
  237.          K.AddM(K2);
  238.          K.AddM(K3);
  239.          K.Solve(tImpulse,-ptpCX,-ptpCY);
  240.          var impulseX:Number = tImpulse.x;
  241.          var impulseY:Number = tImpulse.y;
  242.          b1.m_sweep.c.x -= b1.m_invMass * impulseX;
  243.          b1.m_sweep.c.y -= b1.m_invMass * impulseY;
  244.          b1.m_sweep.a -= b1.m_invI * (r1X * impulseY - r1Y * impulseX);
  245.          b2.m_sweep.c.x += b2.m_invMass * impulseX;
  246.          b2.m_sweep.c.y += b2.m_invMass * impulseY;
  247.          b2.m_sweep.a += b2.m_invI * (r2X * impulseY - r2Y * impulseX);
  248.          b1.SynchronizeTransform();
  249.          b2.SynchronizeTransform();
  250.          var angularError:Number = 0;
  251.          if(m_enableLimit && m_limitState != e_inactiveLimit)
  252.          {
  253.             angle = b2.m_sweep.a - b1.m_sweep.a - m_referenceAngle;
  254.             limitImpulse = 0;
  255.             if(m_limitState == e_equalLimits)
  256.             {
  257.                limitC = b2Math.b2Clamp(angle,-b2Settings.b2_maxAngularCorrection,b2Settings.b2_maxAngularCorrection);
  258.                limitImpulse = -m_motorMass * limitC;
  259.                angularError = b2Math.b2Abs(limitC);
  260.             }
  261.             else if(m_limitState == e_atLowerLimit)
  262.             {
  263.                limitC = angle - m_lowerAngle;
  264.                angularError = b2Math.b2Max(0,-limitC);
  265.                limitC = b2Math.b2Clamp(limitC + b2Settings.b2_angularSlop,-b2Settings.b2_maxAngularCorrection,0);
  266.                limitImpulse = -m_motorMass * limitC;
  267.                oldLimitImpulse = m_limitPositionImpulse;
  268.                m_limitPositionImpulse = b2Math.b2Max(m_limitPositionImpulse + limitImpulse,0);
  269.                limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
  270.             }
  271.             else if(m_limitState == e_atUpperLimit)
  272.             {
  273.                limitC = angle - m_upperAngle;
  274.                angularError = b2Math.b2Max(0,limitC);
  275.                limitC = b2Math.b2Clamp(limitC - b2Settings.b2_angularSlop,0,b2Settings.b2_maxAngularCorrection);
  276.                limitImpulse = -m_motorMass * limitC;
  277.                oldLimitImpulse = m_limitPositionImpulse;
  278.                m_limitPositionImpulse = b2Math.b2Min(m_limitPositionImpulse + limitImpulse,0);
  279.                limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
  280.             }
  281.             b1.m_sweep.a -= b1.m_invI * limitImpulse;
  282.             b2.m_sweep.a += b2.m_invI * limitImpulse;
  283.             b1.SynchronizeTransform();
  284.             b2.SynchronizeTransform();
  285.          }
  286.          return positionError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop;
  287.       }
  288.       
  289.       public function SetMotorSpeed(speed:Number) : void
  290.       {
  291.          m_motorSpeed = speed;
  292.       }
  293.       
  294.       public function GetJointSpeed() : Number
  295.       {
  296.          return m_body2.m_angularVelocity - m_body1.m_angularVelocity;
  297.       }
  298.       
  299.       public function SetMaxMotorTorque(torque:Number) : void
  300.       {
  301.          m_maxMotorTorque = torque;
  302.       }
  303.       
  304.       public function GetJointAngle() : Number
  305.       {
  306.          return m_body2.m_sweep.a - m_body1.m_sweep.a - m_referenceAngle;
  307.       }
  308.       
  309.       public function GetMotorTorque() : Number
  310.       {
  311.          return m_motorForce;
  312.       }
  313.       
  314.       override public function InitVelocityConstraints(step:b2TimeStep) : void
  315.       {
  316.          var b1:b2Body = null;
  317.          var b2:b2Body = null;
  318.          var tMat:b2Mat22 = null;
  319.          var tX:Number = NaN;
  320.          var r1Y:Number = NaN;
  321.          var jointAngle:Number = NaN;
  322.          b1 = m_body1;
  323.          b2 = m_body2;
  324.          tMat = b1.m_xf.R;
  325.          var r1X:Number = m_localAnchor1.x - b1.m_sweep.localCenter.x;
  326.          r1Y = m_localAnchor1.y - b1.m_sweep.localCenter.y;
  327.          tX = tMat.col1.x * r1X + tMat.col2.x * r1Y;
  328.          r1Y = tMat.col1.y * r1X + tMat.col2.y * r1Y;
  329.          r1X = tX;
  330.          tMat = b2.m_xf.R;
  331.          var r2X:Number = m_localAnchor2.x - b2.m_sweep.localCenter.x;
  332.          var r2Y:Number = m_localAnchor2.y - b2.m_sweep.localCenter.y;
  333.          tX = tMat.col1.x * r2X + tMat.col2.x * r2Y;
  334.          r2Y = tMat.col1.y * r2X + tMat.col2.y * r2Y;
  335.          r2X = tX;
  336.          var invMass1:Number = b1.m_invMass;
  337.          var invMass2:Number = b2.m_invMass;
  338.          var invI1:Number = b1.m_invI;
  339.          var invI2:Number = b2.m_invI;
  340.          K1.col1.x = invMass1 + invMass2;
  341.          K1.col2.x = 0;
  342.          K1.col1.y = 0;
  343.          K1.col2.y = invMass1 + invMass2;
  344.          K2.col1.x = invI1 * r1Y * r1Y;
  345.          K2.col2.x = -invI1 * r1X * r1Y;
  346.          K2.col1.y = -invI1 * r1X * r1Y;
  347.          K2.col2.y = invI1 * r1X * r1X;
  348.          K3.col1.x = invI2 * r2Y * r2Y;
  349.          K3.col2.x = -invI2 * r2X * r2Y;
  350.          K3.col1.y = -invI2 * r2X * r2Y;
  351.          K3.col2.y = invI2 * r2X * r2X;
  352.          K.SetM(K1);
  353.          K.AddM(K2);
  354.          K.AddM(K3);
  355.          K.Invert(m_pivotMass);
  356.          m_motorMass = 1 / (invI1 + invI2);
  357.          if(m_enableMotor == false)
  358.          {
  359.             m_motorForce = 0;
  360.          }
  361.          if(m_enableLimit)
  362.          {
  363.             jointAngle = b2.m_sweep.a - b1.m_sweep.a - m_referenceAngle;
  364.             if(b2Math.b2Abs(m_upperAngle - m_lowerAngle) < 2 * b2Settings.b2_angularSlop)
  365.             {
  366.                m_limitState = e_equalLimits;
  367.             }
  368.             else if(jointAngle <= m_lowerAngle)
  369.             {
  370.                if(m_limitState != e_atLowerLimit)
  371.                {
  372.                   m_limitForce = 0;
  373.                }
  374.                m_limitState = e_atLowerLimit;
  375.             }
  376.             else if(jointAngle >= m_upperAngle)
  377.             {
  378.                if(m_limitState != e_atUpperLimit)
  379.                {
  380.                   m_limitForce = 0;
  381.                }
  382.                m_limitState = e_atUpperLimit;
  383.             }
  384.             else
  385.             {
  386.                m_limitState = e_inactiveLimit;
  387.                m_limitForce = 0;
  388.             }
  389.          }
  390.          else
  391.          {
  392.             m_limitForce = 0;
  393.          }
  394.          if(step.warmStarting)
  395.          {
  396.             b1.m_linearVelocity.x -= step.dt * invMass1 * m_pivotForce.x;
  397.             b1.m_linearVelocity.y -= step.dt * invMass1 * m_pivotForce.y;
  398.             b1.m_angularVelocity -= step.dt * invI1 * (r1X * m_pivotForce.y - r1Y * m_pivotForce.x + m_motorForce + m_limitForce);
  399.             b2.m_linearVelocity.x += step.dt * invMass2 * m_pivotForce.x;
  400.             b2.m_linearVelocity.y += step.dt * invMass2 * m_pivotForce.y;
  401.             b2.m_angularVelocity += step.dt * invI2 * (r2X * m_pivotForce.y - r2Y * m_pivotForce.x + m_motorForce + m_limitForce);
  402.          }
  403.          else
  404.          {
  405.             m_pivotForce.SetZero();
  406.             m_motorForce = 0;
  407.             m_limitForce = 0;
  408.          }
  409.          m_limitPositionImpulse = 0;
  410.       }
  411.       
  412.       public function EnableLimit(flag:Boolean) : void
  413.       {
  414.          m_enableLimit = flag;
  415.       }
  416.       
  417.       override public function GetReactionTorque() : Number
  418.       {
  419.          return m_limitForce;
  420.       }
  421.       
  422.       public function IsLimitEnabled() : Boolean
  423.       {
  424.          return m_enableLimit;
  425.       }
  426.       
  427.       public function IsMotorEnabled() : Boolean
  428.       {
  429.          return m_enableMotor;
  430.       }
  431.       
  432.       public function SetLimits(lower:Number, upper:Number) : void
  433.       {
  434.          m_lowerAngle = lower;
  435.          m_upperAngle = upper;
  436.       }
  437.    }
  438. }
  439.