home *** CD-ROM | disk | FTP | other *** search
/ 404 Jogos / CLJG.iso / Diversos / pup_idol.swf / scripts / Box2D / Dynamics / Joints / b2RevoluteJoint.as < prev    next >
Encoding:
Text File  |  2008-08-07  |  15.8 KB  |  386 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.    import Box2D.Dynamics.b2World;
  10.    
  11.    public class b2RevoluteJoint extends b2Joint
  12.    {
  13.       
  14.       public static var tImpulse:b2Vec2 = new b2Vec2();
  15.        
  16.       
  17.       public var m_enableLimit:Boolean;
  18.       
  19.       public var m_limitState:int;
  20.       
  21.       public var m_ptpMass:b2Mat22;
  22.       
  23.       public var m_motorMass:Number;
  24.       
  25.       public var m_localAnchor1:b2Vec2;
  26.       
  27.       public var m_localAnchor2:b2Vec2;
  28.       
  29.       private var K1:b2Mat22;
  30.       
  31.       private var K2:b2Mat22;
  32.       
  33.       private var K:b2Mat22;
  34.       
  35.       public var m_limitImpulse:Number;
  36.       
  37.       private var K3:b2Mat22;
  38.       
  39.       public var m_motorImpulse:Number;
  40.       
  41.       public var m_enableMotor:Boolean;
  42.       
  43.       public var m_limitPositionImpulse:Number;
  44.       
  45.       public var m_motorSpeed:Number;
  46.       
  47.       public var m_upperAngle:Number;
  48.       
  49.       public var m_lowerAngle:Number;
  50.       
  51.       public var m_maxMotorTorque:Number;
  52.       
  53.       public var m_ptpImpulse:b2Vec2;
  54.       
  55.       public var m_intialAngle:Number;
  56.       
  57.       public function b2RevoluteJoint(param1:b2RevoluteJointDef)
  58.       {
  59.          var _loc2_:b2Mat22 = null;
  60.          var _loc3_:Number = NaN;
  61.          var _loc4_:Number = NaN;
  62.          K = new b2Mat22();
  63.          K1 = new b2Mat22();
  64.          K2 = new b2Mat22();
  65.          K3 = new b2Mat22();
  66.          m_localAnchor1 = new b2Vec2();
  67.          m_localAnchor2 = new b2Vec2();
  68.          m_ptpImpulse = new b2Vec2();
  69.          m_ptpMass = new b2Mat22();
  70.          super(param1);
  71.          _loc2_ = m_body1.m_R;
  72.          _loc3_ = param1.anchorPoint.x - m_body1.m_position.x;
  73.          _loc4_ = param1.anchorPoint.y - m_body1.m_position.y;
  74.          m_localAnchor1.x = _loc3_ * _loc2_.col1.x + _loc4_ * _loc2_.col1.y;
  75.          m_localAnchor1.y = _loc3_ * _loc2_.col2.x + _loc4_ * _loc2_.col2.y;
  76.          _loc2_ = m_body2.m_R;
  77.          _loc3_ = param1.anchorPoint.x - m_body2.m_position.x;
  78.          _loc4_ = param1.anchorPoint.y - m_body2.m_position.y;
  79.          m_localAnchor2.x = _loc3_ * _loc2_.col1.x + _loc4_ * _loc2_.col1.y;
  80.          m_localAnchor2.y = _loc3_ * _loc2_.col2.x + _loc4_ * _loc2_.col2.y;
  81.          m_intialAngle = m_body2.m_rotation - m_body1.m_rotation;
  82.          m_ptpImpulse.Set(0,0);
  83.          m_motorImpulse = 0;
  84.          m_limitImpulse = 0;
  85.          m_limitPositionImpulse = 0;
  86.          m_lowerAngle = param1.lowerAngle;
  87.          m_upperAngle = param1.upperAngle;
  88.          m_maxMotorTorque = param1.motorTorque;
  89.          m_motorSpeed = param1.motorSpeed;
  90.          m_enableLimit = param1.enableLimit;
  91.          m_enableMotor = param1.enableMotor;
  92.       }
  93.       
  94.       override public function GetAnchor1() : b2Vec2
  95.       {
  96.          var _loc1_:b2Mat22 = m_body1.m_R;
  97.          return new b2Vec2(m_body1.m_position.x + (_loc1_.col1.x * m_localAnchor1.x + _loc1_.col2.x * m_localAnchor1.y),m_body1.m_position.y + (_loc1_.col1.y * m_localAnchor1.x + _loc1_.col2.y * m_localAnchor1.y));
  98.       }
  99.       
  100.       override public function GetAnchor2() : b2Vec2
  101.       {
  102.          var _loc1_:b2Mat22 = m_body2.m_R;
  103.          return new b2Vec2(m_body2.m_position.x + (_loc1_.col1.x * m_localAnchor2.x + _loc1_.col2.x * m_localAnchor2.y),m_body2.m_position.y + (_loc1_.col1.y * m_localAnchor2.x + _loc1_.col2.y * m_localAnchor2.y));
  104.       }
  105.       
  106.       override public function PrepareVelocitySolver() : void
  107.       {
  108.          var _loc1_:b2Body = null;
  109.          var _loc2_:b2Body = null;
  110.          var _loc3_:b2Mat22 = null;
  111.          var _loc12_:Number = NaN;
  112.          _loc1_ = m_body1;
  113.          _loc2_ = m_body2;
  114.          _loc3_ = _loc1_.m_R;
  115.          var _loc4_:Number = _loc3_.col1.x * m_localAnchor1.x + _loc3_.col2.x * m_localAnchor1.y;
  116.          var _loc5_:Number = _loc3_.col1.y * m_localAnchor1.x + _loc3_.col2.y * m_localAnchor1.y;
  117.          _loc3_ = _loc2_.m_R;
  118.          var _loc6_:Number = _loc3_.col1.x * m_localAnchor2.x + _loc3_.col2.x * m_localAnchor2.y;
  119.          var _loc7_:Number = _loc3_.col1.y * m_localAnchor2.x + _loc3_.col2.y * m_localAnchor2.y;
  120.          var _loc8_:Number = _loc1_.m_invMass;
  121.          var _loc9_:Number = _loc2_.m_invMass;
  122.          var _loc10_:Number = _loc1_.m_invI;
  123.          var _loc11_:Number = _loc2_.m_invI;
  124.          K1.col1.x = _loc8_ + _loc9_;
  125.          K1.col2.x = 0;
  126.          K1.col1.y = 0;
  127.          K1.col2.y = _loc8_ + _loc9_;
  128.          K2.col1.x = _loc10_ * _loc5_ * _loc5_;
  129.          K2.col2.x = -_loc10_ * _loc4_ * _loc5_;
  130.          K2.col1.y = -_loc10_ * _loc4_ * _loc5_;
  131.          K2.col2.y = _loc10_ * _loc4_ * _loc4_;
  132.          K3.col1.x = _loc11_ * _loc7_ * _loc7_;
  133.          K3.col2.x = -_loc11_ * _loc6_ * _loc7_;
  134.          K3.col1.y = -_loc11_ * _loc6_ * _loc7_;
  135.          K3.col2.y = _loc11_ * _loc6_ * _loc6_;
  136.          K.SetM(K1);
  137.          K.AddM(K2);
  138.          K.AddM(K3);
  139.          K.Invert(m_ptpMass);
  140.          m_motorMass = 1 / (_loc10_ + _loc11_);
  141.          if(m_enableMotor == false)
  142.          {
  143.             m_motorImpulse = 0;
  144.          }
  145.          if(m_enableLimit)
  146.          {
  147.             _loc12_ = _loc2_.m_rotation - _loc1_.m_rotation - m_intialAngle;
  148.             if(b2Math.b2Abs(m_upperAngle - m_lowerAngle) < 2 * b2Settings.b2_angularSlop)
  149.             {
  150.                m_limitState = e_equalLimits;
  151.             }
  152.             else if(_loc12_ <= m_lowerAngle)
  153.             {
  154.                if(m_limitState != e_atLowerLimit)
  155.                {
  156.                   m_limitImpulse = 0;
  157.                }
  158.                m_limitState = e_atLowerLimit;
  159.             }
  160.             else if(_loc12_ >= m_upperAngle)
  161.             {
  162.                if(m_limitState != e_atUpperLimit)
  163.                {
  164.                   m_limitImpulse = 0;
  165.                }
  166.                m_limitState = e_atUpperLimit;
  167.             }
  168.             else
  169.             {
  170.                m_limitState = e_inactiveLimit;
  171.                m_limitImpulse = 0;
  172.             }
  173.          }
  174.          else
  175.          {
  176.             m_limitImpulse = 0;
  177.          }
  178.          if(b2World.s_enableWarmStarting)
  179.          {
  180.             _loc1_.m_linearVelocity.x -= _loc8_ * m_ptpImpulse.x;
  181.             _loc1_.m_linearVelocity.y -= _loc8_ * m_ptpImpulse.y;
  182.             _loc1_.m_angularVelocity -= _loc10_ * (_loc4_ * m_ptpImpulse.y - _loc5_ * m_ptpImpulse.x + m_motorImpulse + m_limitImpulse);
  183.             _loc2_.m_linearVelocity.x += _loc9_ * m_ptpImpulse.x;
  184.             _loc2_.m_linearVelocity.y += _loc9_ * m_ptpImpulse.y;
  185.             _loc2_.m_angularVelocity += _loc11_ * (_loc6_ * m_ptpImpulse.y - _loc7_ * m_ptpImpulse.x + m_motorImpulse + m_limitImpulse);
  186.          }
  187.          else
  188.          {
  189.             m_ptpImpulse.SetZero();
  190.             m_motorImpulse = 0;
  191.             m_limitImpulse = 0;
  192.          }
  193.          m_limitPositionImpulse = 0;
  194.       }
  195.       
  196.       override public function GetReactionForce(param1:Number) : b2Vec2
  197.       {
  198.          var _loc2_:b2Vec2 = m_ptpImpulse.Copy();
  199.          _loc2_.Multiply(param1);
  200.          return _loc2_;
  201.       }
  202.       
  203.       override public function SolvePositionConstraints() : Boolean
  204.       {
  205.          var _loc1_:Number = NaN;
  206.          var _loc2_:Number = NaN;
  207.          var _loc6_:b2Mat22 = null;
  208.          var _loc24_:Number = NaN;
  209.          var _loc25_:Number = NaN;
  210.          var _loc3_:b2Body = m_body1;
  211.          var _loc4_:b2Body = m_body2;
  212.          var _loc5_:Number = 0;
  213.          var _loc7_:Number = (_loc6_ = _loc3_.m_R).col1.x * m_localAnchor1.x + _loc6_.col2.x * m_localAnchor1.y;
  214.          var _loc8_:Number = _loc6_.col1.y * m_localAnchor1.x + _loc6_.col2.y * m_localAnchor1.y;
  215.          var _loc9_:Number = (_loc6_ = _loc4_.m_R).col1.x * m_localAnchor2.x + _loc6_.col2.x * m_localAnchor2.y;
  216.          var _loc10_:Number = _loc6_.col1.y * m_localAnchor2.x + _loc6_.col2.y * m_localAnchor2.y;
  217.          var _loc11_:Number = _loc3_.m_position.x + _loc7_;
  218.          var _loc12_:Number = _loc3_.m_position.y + _loc8_;
  219.          var _loc13_:Number = _loc4_.m_position.x + _loc9_;
  220.          var _loc14_:Number = _loc4_.m_position.y + _loc10_;
  221.          var _loc15_:Number = _loc13_ - _loc11_;
  222.          var _loc16_:Number = _loc14_ - _loc12_;
  223.          _loc5_ = Math.sqrt(_loc15_ * _loc15_ + _loc16_ * _loc16_);
  224.          var _loc17_:Number = _loc3_.m_invMass;
  225.          var _loc18_:Number = _loc4_.m_invMass;
  226.          var _loc19_:Number = _loc3_.m_invI;
  227.          var _loc20_:Number = _loc4_.m_invI;
  228.          K1.col1.x = _loc17_ + _loc18_;
  229.          K1.col2.x = 0;
  230.          K1.col1.y = 0;
  231.          K1.col2.y = _loc17_ + _loc18_;
  232.          K2.col1.x = _loc19_ * _loc8_ * _loc8_;
  233.          K2.col2.x = -_loc19_ * _loc7_ * _loc8_;
  234.          K2.col1.y = -_loc19_ * _loc7_ * _loc8_;
  235.          K2.col2.y = _loc19_ * _loc7_ * _loc7_;
  236.          K3.col1.x = _loc20_ * _loc10_ * _loc10_;
  237.          K3.col2.x = -_loc20_ * _loc9_ * _loc10_;
  238.          K3.col1.y = -_loc20_ * _loc9_ * _loc10_;
  239.          K3.col2.y = _loc20_ * _loc9_ * _loc9_;
  240.          K.SetM(K1);
  241.          K.AddM(K2);
  242.          K.AddM(K3);
  243.          K.Solve(tImpulse,-_loc15_,-_loc16_);
  244.          var _loc21_:Number = tImpulse.x;
  245.          var _loc22_:Number = tImpulse.y;
  246.          _loc3_.m_position.x -= _loc3_.m_invMass * _loc21_;
  247.          _loc3_.m_position.y -= _loc3_.m_invMass * _loc22_;
  248.          _loc3_.m_rotation -= _loc3_.m_invI * (_loc7_ * _loc22_ - _loc8_ * _loc21_);
  249.          _loc3_.m_R.Set(_loc3_.m_rotation);
  250.          _loc4_.m_position.x += _loc4_.m_invMass * _loc21_;
  251.          _loc4_.m_position.y += _loc4_.m_invMass * _loc22_;
  252.          _loc4_.m_rotation += _loc4_.m_invI * (_loc9_ * _loc22_ - _loc10_ * _loc21_);
  253.          _loc4_.m_R.Set(_loc4_.m_rotation);
  254.          var _loc23_:Number = 0;
  255.          if(m_enableLimit && m_limitState != e_inactiveLimit)
  256.          {
  257.             _loc24_ = _loc4_.m_rotation - _loc3_.m_rotation - m_intialAngle;
  258.             _loc25_ = 0;
  259.             if(m_limitState == e_equalLimits)
  260.             {
  261.                _loc2_ = b2Math.b2Clamp(_loc24_,-b2Settings.b2_maxAngularCorrection,b2Settings.b2_maxAngularCorrection);
  262.                _loc25_ = -m_motorMass * _loc2_;
  263.                _loc23_ = b2Math.b2Abs(_loc2_);
  264.             }
  265.             else if(m_limitState == e_atLowerLimit)
  266.             {
  267.                _loc2_ = _loc24_ - m_lowerAngle;
  268.                _loc23_ = b2Math.b2Max(0,-_loc2_);
  269.                _loc2_ = b2Math.b2Clamp(_loc2_ + b2Settings.b2_angularSlop,-b2Settings.b2_maxAngularCorrection,0);
  270.                _loc25_ = -m_motorMass * _loc2_;
  271.                _loc1_ = m_limitPositionImpulse;
  272.                m_limitPositionImpulse = b2Math.b2Max(m_limitPositionImpulse + _loc25_,0);
  273.                _loc25_ = m_limitPositionImpulse - _loc1_;
  274.             }
  275.             else if(m_limitState == e_atUpperLimit)
  276.             {
  277.                _loc2_ = _loc24_ - m_upperAngle;
  278.                _loc23_ = b2Math.b2Max(0,_loc2_);
  279.                _loc2_ = b2Math.b2Clamp(_loc2_ - b2Settings.b2_angularSlop,0,b2Settings.b2_maxAngularCorrection);
  280.                _loc25_ = -m_motorMass * _loc2_;
  281.                _loc1_ = m_limitPositionImpulse;
  282.                m_limitPositionImpulse = b2Math.b2Min(m_limitPositionImpulse + _loc25_,0);
  283.                _loc25_ = m_limitPositionImpulse - _loc1_;
  284.             }
  285.             _loc3_.m_rotation -= _loc3_.m_invI * _loc25_;
  286.             _loc3_.m_R.Set(_loc3_.m_rotation);
  287.             _loc4_.m_rotation += _loc4_.m_invI * _loc25_;
  288.             _loc4_.m_R.Set(_loc4_.m_rotation);
  289.          }
  290.          return _loc5_ <= b2Settings.b2_linearSlop && _loc23_ <= b2Settings.b2_angularSlop;
  291.       }
  292.       
  293.       public function SetMotorSpeed(param1:Number) : void
  294.       {
  295.          m_motorSpeed = param1;
  296.       }
  297.       
  298.       public function GetJointSpeed() : Number
  299.       {
  300.          return m_body2.m_angularVelocity - m_body1.m_angularVelocity;
  301.       }
  302.       
  303.       public function SetMotorTorque(param1:Number) : void
  304.       {
  305.          m_maxMotorTorque = param1;
  306.       }
  307.       
  308.       public function GetJointAngle() : Number
  309.       {
  310.          return m_body2.m_rotation - m_body1.m_rotation;
  311.       }
  312.       
  313.       public function GetMotorTorque(param1:Number) : Number
  314.       {
  315.          return param1 * m_motorImpulse;
  316.       }
  317.       
  318.       override public function GetReactionTorque(param1:Number) : Number
  319.       {
  320.          return param1 * m_limitImpulse;
  321.       }
  322.       
  323.       override public function SolveVelocityConstraints(param1:b2TimeStep) : void
  324.       {
  325.          var _loc4_:b2Mat22 = null;
  326.          var _loc9_:Number = NaN;
  327.          var _loc14_:Number = NaN;
  328.          var _loc15_:Number = NaN;
  329.          var _loc16_:Number = NaN;
  330.          var _loc17_:Number = NaN;
  331.          var _loc18_:Number = NaN;
  332.          var _loc2_:b2Body = m_body1;
  333.          var _loc3_:b2Body = m_body2;
  334.          var _loc5_:Number = (_loc4_ = _loc2_.m_R).col1.x * m_localAnchor1.x + _loc4_.col2.x * m_localAnchor1.y;
  335.          var _loc6_:Number = _loc4_.col1.y * m_localAnchor1.x + _loc4_.col2.y * m_localAnchor1.y;
  336.          var _loc7_:Number = (_loc4_ = _loc3_.m_R).col1.x * m_localAnchor2.x + _loc4_.col2.x * m_localAnchor2.y;
  337.          var _loc8_:Number = _loc4_.col1.y * m_localAnchor2.x + _loc4_.col2.y * m_localAnchor2.y;
  338.          var _loc10_:Number = _loc3_.m_linearVelocity.x + -_loc3_.m_angularVelocity * _loc8_ - _loc2_.m_linearVelocity.x - -_loc2_.m_angularVelocity * _loc6_;
  339.          var _loc11_:Number = _loc3_.m_linearVelocity.y + _loc3_.m_angularVelocity * _loc7_ - _loc2_.m_linearVelocity.y - _loc2_.m_angularVelocity * _loc5_;
  340.          var _loc12_:Number = -(m_ptpMass.col1.x * _loc10_ + m_ptpMass.col2.x * _loc11_);
  341.          var _loc13_:Number = -(m_ptpMass.col1.y * _loc10_ + m_ptpMass.col2.y * _loc11_);
  342.          m_ptpImpulse.x += _loc12_;
  343.          m_ptpImpulse.y += _loc13_;
  344.          _loc2_.m_linearVelocity.x -= _loc2_.m_invMass * _loc12_;
  345.          _loc2_.m_linearVelocity.y -= _loc2_.m_invMass * _loc13_;
  346.          _loc2_.m_angularVelocity -= _loc2_.m_invI * (_loc5_ * _loc13_ - _loc6_ * _loc12_);
  347.          _loc3_.m_linearVelocity.x += _loc3_.m_invMass * _loc12_;
  348.          _loc3_.m_linearVelocity.y += _loc3_.m_invMass * _loc13_;
  349.          _loc3_.m_angularVelocity += _loc3_.m_invI * (_loc7_ * _loc13_ - _loc8_ * _loc12_);
  350.          if(m_enableMotor && m_limitState != e_equalLimits)
  351.          {
  352.             _loc14_ = _loc3_.m_angularVelocity - _loc2_.m_angularVelocity - m_motorSpeed;
  353.             _loc15_ = -m_motorMass * _loc14_;
  354.             _loc16_ = m_motorImpulse;
  355.             m_motorImpulse = b2Math.b2Clamp(m_motorImpulse + _loc15_,-param1.dt * m_maxMotorTorque,param1.dt * m_maxMotorTorque);
  356.             _loc15_ = m_motorImpulse - _loc16_;
  357.             _loc2_.m_angularVelocity -= _loc2_.m_invI * _loc15_;
  358.             _loc3_.m_angularVelocity += _loc3_.m_invI * _loc15_;
  359.          }
  360.          if(m_enableLimit && m_limitState != e_inactiveLimit)
  361.          {
  362.             _loc17_ = _loc3_.m_angularVelocity - _loc2_.m_angularVelocity;
  363.             _loc18_ = -m_motorMass * _loc17_;
  364.             if(m_limitState == e_equalLimits)
  365.             {
  366.                m_limitImpulse += _loc18_;
  367.             }
  368.             else if(m_limitState == e_atLowerLimit)
  369.             {
  370.                _loc9_ = m_limitImpulse;
  371.                m_limitImpulse = b2Math.b2Max(m_limitImpulse + _loc18_,0);
  372.                _loc18_ = m_limitImpulse - _loc9_;
  373.             }
  374.             else if(m_limitState == e_atUpperLimit)
  375.             {
  376.                _loc9_ = m_limitImpulse;
  377.                m_limitImpulse = b2Math.b2Min(m_limitImpulse + _loc18_,0);
  378.                _loc18_ = m_limitImpulse - _loc9_;
  379.             }
  380.             _loc2_.m_angularVelocity -= _loc2_.m_invI * _loc18_;
  381.             _loc3_.m_angularVelocity += _loc3_.m_invI * _loc18_;
  382.          }
  383.       }
  384.    }
  385. }
  386.