home *** CD-ROM | disk | FTP | other *** search
/ 404 Jogos / CLJG.iso / Diversos / Beez.swf / scripts / Box2D / Dynamics / Joints / b2GearJoint.as < prev    next >
Encoding:
Text File  |  2008-09-03  |  9.0 KB  |  258 lines
  1. package Box2D.Dynamics.Joints
  2. {
  3.    import Box2D.Common.Math.b2Mat22;
  4.    import Box2D.Common.Math.b2Vec2;
  5.    import Box2D.Common.b2Settings;
  6.    import Box2D.Dynamics.b2Body;
  7.    import Box2D.Dynamics.b2TimeStep;
  8.    
  9.    public class b2GearJoint extends b2Joint
  10.    {
  11.        
  12.       
  13.       public var m_ground2:b2Body;
  14.       
  15.       public var m_groundAnchor1:b2Vec2;
  16.       
  17.       public var m_groundAnchor2:b2Vec2;
  18.       
  19.       public var m_localAnchor1:b2Vec2;
  20.       
  21.       public var m_localAnchor2:b2Vec2;
  22.       
  23.       public var m_ratio:Number;
  24.       
  25.       public var m_revolute2:b2RevoluteJoint;
  26.       
  27.       public var m_force:Number;
  28.       
  29.       public var m_mass:Number;
  30.       
  31.       public var m_prismatic2:b2PrismaticJoint;
  32.       
  33.       public var m_ground1:b2Body;
  34.       
  35.       public var m_revolute1:b2RevoluteJoint;
  36.       
  37.       public var m_prismatic1:b2PrismaticJoint;
  38.       
  39.       public var m_constant:Number;
  40.       
  41.       public var m_J:b2Jacobian;
  42.       
  43.       public function b2GearJoint(def:b2GearJointDef)
  44.       {
  45.          var coordinate1:Number = NaN;
  46.          var coordinate2:Number = NaN;
  47.          m_groundAnchor1 = new b2Vec2();
  48.          m_groundAnchor2 = new b2Vec2();
  49.          m_localAnchor1 = new b2Vec2();
  50.          m_localAnchor2 = new b2Vec2();
  51.          m_J = new b2Jacobian();
  52.          super(def);
  53.          var type1:int = def.joint1.m_type;
  54.          var type2:int = def.joint2.m_type;
  55.          m_revolute1 = null;
  56.          m_prismatic1 = null;
  57.          m_revolute2 = null;
  58.          m_prismatic2 = null;
  59.          m_ground1 = def.joint1.m_body1;
  60.          m_body1 = def.joint1.m_body2;
  61.          if(type1 == b2Joint.e_revoluteJoint)
  62.          {
  63.             m_revolute1 = def.joint1 as b2RevoluteJoint;
  64.             m_groundAnchor1.SetV(m_revolute1.m_localAnchor1);
  65.             m_localAnchor1.SetV(m_revolute1.m_localAnchor2);
  66.             coordinate1 = m_revolute1.GetJointAngle();
  67.          }
  68.          else
  69.          {
  70.             m_prismatic1 = def.joint1 as b2PrismaticJoint;
  71.             m_groundAnchor1.SetV(m_prismatic1.m_localAnchor1);
  72.             m_localAnchor1.SetV(m_prismatic1.m_localAnchor2);
  73.             coordinate1 = m_prismatic1.GetJointTranslation();
  74.          }
  75.          m_ground2 = def.joint2.m_body1;
  76.          m_body2 = def.joint2.m_body2;
  77.          if(type2 == b2Joint.e_revoluteJoint)
  78.          {
  79.             m_revolute2 = def.joint2 as b2RevoluteJoint;
  80.             m_groundAnchor2.SetV(m_revolute2.m_localAnchor1);
  81.             m_localAnchor2.SetV(m_revolute2.m_localAnchor2);
  82.             coordinate2 = m_revolute2.GetJointAngle();
  83.          }
  84.          else
  85.          {
  86.             m_prismatic2 = def.joint2 as b2PrismaticJoint;
  87.             m_groundAnchor2.SetV(m_prismatic2.m_localAnchor1);
  88.             m_localAnchor2.SetV(m_prismatic2.m_localAnchor2);
  89.             coordinate2 = m_prismatic2.GetJointTranslation();
  90.          }
  91.          m_ratio = def.ratio;
  92.          m_constant = coordinate1 + m_ratio * coordinate2;
  93.          m_force = 0;
  94.       }
  95.       
  96.       override public function GetAnchor1() : b2Vec2
  97.       {
  98.          return m_body1.GetWorldPoint(m_localAnchor1);
  99.       }
  100.       
  101.       override public function GetAnchor2() : b2Vec2
  102.       {
  103.          return m_body2.GetWorldPoint(m_localAnchor2);
  104.       }
  105.       
  106.       override public function InitVelocityConstraints(step:b2TimeStep) : void
  107.       {
  108.          var ugX:Number = NaN;
  109.          var ugY:Number = NaN;
  110.          var rX:Number = NaN;
  111.          var rY:Number = NaN;
  112.          var tMat:b2Mat22 = null;
  113.          var tVec:b2Vec2 = null;
  114.          var crug:Number = NaN;
  115.          var tX:Number = NaN;
  116.          var P:Number = NaN;
  117.          var g1:b2Body = m_ground1;
  118.          var g2:b2Body = m_ground2;
  119.          var b1:b2Body = m_body1;
  120.          var b2:b2Body = m_body2;
  121.          var K:Number = 0;
  122.          m_J.SetZero();
  123.          if(m_revolute1)
  124.          {
  125.             m_J.angular1 = -1;
  126.             K += b1.m_invI;
  127.          }
  128.          else
  129.          {
  130.             tMat = g1.m_xf.R;
  131.             tVec = m_prismatic1.m_localXAxis1;
  132.             ugX = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
  133.             ugY = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;
  134.             tMat = b1.m_xf.R;
  135.             rX = m_localAnchor1.x - b1.m_sweep.localCenter.x;
  136.             rY = m_localAnchor1.y - b1.m_sweep.localCenter.y;
  137.             tX = tMat.col1.x * rX + tMat.col2.x * rY;
  138.             rY = tMat.col1.y * rX + tMat.col2.y * rY;
  139.             rX = tX;
  140.             crug = rX * ugY - rY * ugX;
  141.             m_J.linear1.Set(-ugX,-ugY);
  142.             m_J.angular1 = -crug;
  143.             K += b1.m_invMass + b1.m_invI * crug * crug;
  144.          }
  145.          if(m_revolute2)
  146.          {
  147.             m_J.angular2 = -m_ratio;
  148.             K += m_ratio * m_ratio * b2.m_invI;
  149.          }
  150.          else
  151.          {
  152.             tMat = g2.m_xf.R;
  153.             tVec = m_prismatic2.m_localXAxis1;
  154.             ugX = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
  155.             ugY = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;
  156.             tMat = b2.m_xf.R;
  157.             rX = m_localAnchor2.x - b2.m_sweep.localCenter.x;
  158.             rY = m_localAnchor2.y - b2.m_sweep.localCenter.y;
  159.             tX = tMat.col1.x * rX + tMat.col2.x * rY;
  160.             rY = tMat.col1.y * rX + tMat.col2.y * rY;
  161.             rX = tX;
  162.             crug = rX * ugY - rY * ugX;
  163.             m_J.linear2.Set(-m_ratio * ugX,-m_ratio * ugY);
  164.             m_J.angular2 = -m_ratio * crug;
  165.             K += m_ratio * m_ratio * (b2.m_invMass + b2.m_invI * crug * crug);
  166.          }
  167.          m_mass = 1 / K;
  168.          if(step.warmStarting)
  169.          {
  170.             P = step.dt * m_force;
  171.             b1.m_linearVelocity.x += b1.m_invMass * P * m_J.linear1.x;
  172.             b1.m_linearVelocity.y += b1.m_invMass * P * m_J.linear1.y;
  173.             b1.m_angularVelocity += b1.m_invI * P * m_J.angular1;
  174.             b2.m_linearVelocity.x += b2.m_invMass * P * m_J.linear2.x;
  175.             b2.m_linearVelocity.y += b2.m_invMass * P * m_J.linear2.y;
  176.             b2.m_angularVelocity += b2.m_invI * P * m_J.angular2;
  177.          }
  178.          else
  179.          {
  180.             m_force = 0;
  181.          }
  182.       }
  183.       
  184.       override public function GetReactionTorque() : Number
  185.       {
  186.          var tMat:b2Mat22 = m_body2.m_xf.R;
  187.          var rX:Number = m_localAnchor1.x - m_body2.m_sweep.localCenter.x;
  188.          var rY:Number = m_localAnchor1.y - m_body2.m_sweep.localCenter.y;
  189.          var tX:Number = tMat.col1.x * rX + tMat.col2.x * rY;
  190.          rY = tMat.col1.y * rX + tMat.col2.y * rY;
  191.          rX = tX;
  192.          return m_force * m_J.angular2 - (rX * (m_force * m_J.linear2.y) - rY * (m_force * m_J.linear2.x));
  193.       }
  194.       
  195.       override public function GetReactionForce() : b2Vec2
  196.       {
  197.          return new b2Vec2(m_force * m_J.linear2.x,m_force * m_J.linear2.y);
  198.       }
  199.       
  200.       override public function SolvePositionConstraints() : Boolean
  201.       {
  202.          var coordinate1:Number = NaN;
  203.          var coordinate2:Number = NaN;
  204.          var linearError:Number = 0;
  205.          var b1:b2Body = m_body1;
  206.          var b2:b2Body = m_body2;
  207.          if(m_revolute1)
  208.          {
  209.             coordinate1 = m_revolute1.GetJointAngle();
  210.          }
  211.          else
  212.          {
  213.             coordinate1 = m_prismatic1.GetJointTranslation();
  214.          }
  215.          if(m_revolute2)
  216.          {
  217.             coordinate2 = m_revolute2.GetJointAngle();
  218.          }
  219.          else
  220.          {
  221.             coordinate2 = m_prismatic2.GetJointTranslation();
  222.          }
  223.          var C:Number = m_constant - (coordinate1 + m_ratio * coordinate2);
  224.          var impulse:Number = -m_mass * C;
  225.          b1.m_sweep.c.x += b1.m_invMass * impulse * m_J.linear1.x;
  226.          b1.m_sweep.c.y += b1.m_invMass * impulse * m_J.linear1.y;
  227.          b1.m_sweep.a += b1.m_invI * impulse * m_J.angular1;
  228.          b2.m_sweep.c.x += b2.m_invMass * impulse * m_J.linear2.x;
  229.          b2.m_sweep.c.y += b2.m_invMass * impulse * m_J.linear2.y;
  230.          b2.m_sweep.a += b2.m_invI * impulse * m_J.angular2;
  231.          b1.SynchronizeTransform();
  232.          b2.SynchronizeTransform();
  233.          return linearError < b2Settings.b2_linearSlop;
  234.       }
  235.       
  236.       public function GetRatio() : Number
  237.       {
  238.          return m_ratio;
  239.       }
  240.       
  241.       override public function SolveVelocityConstraints(step:b2TimeStep) : void
  242.       {
  243.          var b1:b2Body = m_body1;
  244.          var b2:b2Body = m_body2;
  245.          var Cdot:Number = m_J.Compute(b1.m_linearVelocity,b1.m_angularVelocity,b2.m_linearVelocity,b2.m_angularVelocity);
  246.          var force:Number = -step.inv_dt * m_mass * Cdot;
  247.          m_force += force;
  248.          var P:Number = step.dt * force;
  249.          b1.m_linearVelocity.x += b1.m_invMass * P * m_J.linear1.x;
  250.          b1.m_linearVelocity.y += b1.m_invMass * P * m_J.linear1.y;
  251.          b1.m_angularVelocity += b1.m_invI * P * m_J.angular1;
  252.          b2.m_linearVelocity.x += b2.m_invMass * P * m_J.linear2.x;
  253.          b2.m_linearVelocity.y += b2.m_invMass * P * m_J.linear2.y;
  254.          b2.m_angularVelocity += b2.m_invI * P * m_J.angular2;
  255.       }
  256.    }
  257. }
  258.