home *** CD-ROM | disk | FTP | other *** search
/ 404 Jogos / CLJG.iso / Diversos / Beez.swf / scripts / Box2D / Dynamics / Joints / b2DistanceJoint.as next >
Encoding:
Text File  |  2008-09-03  |  8.2 KB  |  227 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 b2DistanceJoint extends b2Joint
  11.    {
  12.        
  13.       
  14.       public var m_localAnchor1:b2Vec2;
  15.       
  16.       public var m_localAnchor2:b2Vec2;
  17.       
  18.       public var m_bias:Number;
  19.       
  20.       public var m_gamma:Number;
  21.       
  22.       public var m_u:b2Vec2;
  23.       
  24.       public var m_mass:Number;
  25.       
  26.       public var m_impulse:Number;
  27.       
  28.       public var m_dampingRatio:Number;
  29.       
  30.       public var m_frequencyHz:Number;
  31.       
  32.       public var m_length:Number;
  33.       
  34.       public function b2DistanceJoint(def:b2DistanceJointDef)
  35.       {
  36.          var tMat:b2Mat22 = null;
  37.          var tX:Number = NaN;
  38.          var tY:Number = NaN;
  39.          m_localAnchor1 = new b2Vec2();
  40.          m_localAnchor2 = new b2Vec2();
  41.          m_u = new b2Vec2();
  42.          super(def);
  43.          m_localAnchor1.SetV(def.localAnchor1);
  44.          m_localAnchor2.SetV(def.localAnchor2);
  45.          m_length = def.length;
  46.          m_frequencyHz = def.frequencyHz;
  47.          m_dampingRatio = def.dampingRatio;
  48.          m_impulse = 0;
  49.          m_gamma = 0;
  50.          m_bias = 0;
  51.          m_inv_dt = 0;
  52.       }
  53.       
  54.       override public function GetAnchor1() : b2Vec2
  55.       {
  56.          return m_body1.GetWorldPoint(m_localAnchor1);
  57.       }
  58.       
  59.       override public function GetAnchor2() : b2Vec2
  60.       {
  61.          return m_body2.GetWorldPoint(m_localAnchor2);
  62.       }
  63.       
  64.       override public function InitVelocityConstraints(step:b2TimeStep) : void
  65.       {
  66.          var tMat:b2Mat22 = null;
  67.          var tX:Number = NaN;
  68.          var b1:b2Body = null;
  69.          var b2:b2Body = null;
  70.          var r1X:Number = NaN;
  71.          var r2X:Number = NaN;
  72.          var C:Number = NaN;
  73.          var omega:Number = NaN;
  74.          var d:Number = NaN;
  75.          var k:Number = NaN;
  76.          var PX:Number = NaN;
  77.          var PY:Number = NaN;
  78.          m_inv_dt = step.inv_dt;
  79.          b1 = m_body1;
  80.          b2 = m_body2;
  81.          tMat = b1.m_xf.R;
  82.          r1X = m_localAnchor1.x - b1.m_sweep.localCenter.x;
  83.          var r1Y:Number = m_localAnchor1.y - b1.m_sweep.localCenter.y;
  84.          tX = tMat.col1.x * r1X + tMat.col2.x * r1Y;
  85.          r1Y = tMat.col1.y * r1X + tMat.col2.y * r1Y;
  86.          r1X = tX;
  87.          tMat = b2.m_xf.R;
  88.          r2X = m_localAnchor2.x - b2.m_sweep.localCenter.x;
  89.          var r2Y:Number = m_localAnchor2.y - b2.m_sweep.localCenter.y;
  90.          tX = tMat.col1.x * r2X + tMat.col2.x * r2Y;
  91.          r2Y = tMat.col1.y * r2X + tMat.col2.y * r2Y;
  92.          r2X = tX;
  93.          m_u.x = b2.m_sweep.c.x + r2X - b1.m_sweep.c.x - r1X;
  94.          m_u.y = b2.m_sweep.c.y + r2Y - b1.m_sweep.c.y - r1Y;
  95.          var length:Number = Math.sqrt(m_u.x * m_u.x + m_u.y * m_u.y);
  96.          if(length > b2Settings.b2_linearSlop)
  97.          {
  98.             m_u.Multiply(1 / length);
  99.          }
  100.          else
  101.          {
  102.             m_u.SetZero();
  103.          }
  104.          var cr1u:Number = r1X * m_u.y - r1Y * m_u.x;
  105.          var cr2u:Number = r2X * m_u.y - r2Y * m_u.x;
  106.          var invMass:Number = b1.m_invMass + b1.m_invI * cr1u * cr1u + b2.m_invMass + b2.m_invI * cr2u * cr2u;
  107.          m_mass = 1 / invMass;
  108.          if(m_frequencyHz > 0)
  109.          {
  110.             C = length - m_length;
  111.             omega = 2 * Math.PI * m_frequencyHz;
  112.             d = 2 * m_mass * m_dampingRatio * omega;
  113.             k = m_mass * omega * omega;
  114.             m_gamma = 1 / (step.dt * (d + step.dt * k));
  115.             m_bias = C * step.dt * k * m_gamma;
  116.             m_mass = 1 / (invMass + m_gamma);
  117.          }
  118.          if(step.warmStarting)
  119.          {
  120.             m_impulse *= step.dtRatio;
  121.             PX = m_impulse * m_u.x;
  122.             PY = m_impulse * m_u.y;
  123.             b1.m_linearVelocity.x -= b1.m_invMass * PX;
  124.             b1.m_linearVelocity.y -= b1.m_invMass * PY;
  125.             b1.m_angularVelocity -= b1.m_invI * (r1X * PY - r1Y * PX);
  126.             b2.m_linearVelocity.x += b2.m_invMass * PX;
  127.             b2.m_linearVelocity.y += b2.m_invMass * PY;
  128.             b2.m_angularVelocity += b2.m_invI * (r2X * PY - r2Y * PX);
  129.          }
  130.          else
  131.          {
  132.             m_impulse = 0;
  133.          }
  134.       }
  135.       
  136.       override public function GetReactionTorque() : Number
  137.       {
  138.          return 0;
  139.       }
  140.       
  141.       override public function GetReactionForce() : b2Vec2
  142.       {
  143.          var F:b2Vec2 = new b2Vec2();
  144.          F.SetV(m_u);
  145.          F.Multiply(m_inv_dt * m_impulse);
  146.          return F;
  147.       }
  148.       
  149.       override public function SolvePositionConstraints() : Boolean
  150.       {
  151.          var tMat:b2Mat22 = null;
  152.          if(m_frequencyHz > 0)
  153.          {
  154.             return true;
  155.          }
  156.          var b1:b2Body = m_body1;
  157.          var b2:b2Body = m_body2;
  158.          tMat = b1.m_xf.R;
  159.          var r1X:Number = m_localAnchor1.x - b1.m_sweep.localCenter.x;
  160.          var r1Y:Number = m_localAnchor1.y - b1.m_sweep.localCenter.y;
  161.          var tX:Number = tMat.col1.x * r1X + tMat.col2.x * r1Y;
  162.          r1Y = tMat.col1.y * r1X + tMat.col2.y * r1Y;
  163.          r1X = tX;
  164.          tMat = b2.m_xf.R;
  165.          var r2X:Number = m_localAnchor2.x - b2.m_sweep.localCenter.x;
  166.          var r2Y:Number = m_localAnchor2.y - b2.m_sweep.localCenter.y;
  167.          tX = tMat.col1.x * r2X + tMat.col2.x * r2Y;
  168.          r2Y = tMat.col1.y * r2X + tMat.col2.y * r2Y;
  169.          r2X = tX;
  170.          var dX:Number = b2.m_sweep.c.x + r2X - b1.m_sweep.c.x - r1X;
  171.          var dY:Number = b2.m_sweep.c.y + r2Y - b1.m_sweep.c.y - r1Y;
  172.          var length:Number = Math.sqrt(dX * dX + dY * dY);
  173.          dX /= length;
  174.          dY /= length;
  175.          var C:Number = length - m_length;
  176.          C = b2Math.b2Clamp(C,-b2Settings.b2_maxLinearCorrection,b2Settings.b2_maxLinearCorrection);
  177.          var impulse:Number = -m_mass * C;
  178.          m_u.Set(dX,dY);
  179.          var PX:Number = impulse * m_u.x;
  180.          var PY:Number = impulse * m_u.y;
  181.          b1.m_sweep.c.x -= b1.m_invMass * PX;
  182.          b1.m_sweep.c.y -= b1.m_invMass * PY;
  183.          b1.m_sweep.a -= b1.m_invI * (r1X * PY - r1Y * PX);
  184.          b2.m_sweep.c.x += b2.m_invMass * PX;
  185.          b2.m_sweep.c.y += b2.m_invMass * PY;
  186.          b2.m_sweep.a += b2.m_invI * (r2X * PY - r2Y * PX);
  187.          b1.SynchronizeTransform();
  188.          b2.SynchronizeTransform();
  189.          return b2Math.b2Abs(C) < b2Settings.b2_linearSlop;
  190.       }
  191.       
  192.       override public function SolveVelocityConstraints(step:b2TimeStep) : void
  193.       {
  194.          var tMat:b2Mat22 = null;
  195.          var b1:b2Body = m_body1;
  196.          var b2:b2Body = m_body2;
  197.          tMat = b1.m_xf.R;
  198.          var r1X:Number = m_localAnchor1.x - b1.m_sweep.localCenter.x;
  199.          var r1Y:Number = m_localAnchor1.y - b1.m_sweep.localCenter.y;
  200.          var tX:Number = tMat.col1.x * r1X + tMat.col2.x * r1Y;
  201.          r1Y = tMat.col1.y * r1X + tMat.col2.y * r1Y;
  202.          r1X = tX;
  203.          tMat = b2.m_xf.R;
  204.          var r2X:Number = m_localAnchor2.x - b2.m_sweep.localCenter.x;
  205.          var r2Y:Number = m_localAnchor2.y - b2.m_sweep.localCenter.y;
  206.          tX = tMat.col1.x * r2X + tMat.col2.x * r2Y;
  207.          r2Y = tMat.col1.y * r2X + tMat.col2.y * r2Y;
  208.          r2X = tX;
  209.          var v1X:Number = b1.m_linearVelocity.x + -b1.m_angularVelocity * r1Y;
  210.          var v1Y:Number = b1.m_linearVelocity.y + b1.m_angularVelocity * r1X;
  211.          var v2X:Number = b2.m_linearVelocity.x + -b2.m_angularVelocity * r2Y;
  212.          var v2Y:Number = b2.m_linearVelocity.y + b2.m_angularVelocity * r2X;
  213.          var Cdot:Number = m_u.x * (v2X - v1X) + m_u.y * (v2Y - v1Y);
  214.          var impulse:Number = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
  215.          m_impulse += impulse;
  216.          var PX:Number = impulse * m_u.x;
  217.          var PY:Number = impulse * m_u.y;
  218.          b1.m_linearVelocity.x -= b1.m_invMass * PX;
  219.          b1.m_linearVelocity.y -= b1.m_invMass * PY;
  220.          b1.m_angularVelocity -= b1.m_invI * (r1X * PY - r1Y * PX);
  221.          b2.m_linearVelocity.x += b2.m_invMass * PX;
  222.          b2.m_linearVelocity.y += b2.m_invMass * PY;
  223.          b2.m_angularVelocity += b2.m_invI * (r2X * PY - r2Y * PX);
  224.       }
  225.    }
  226. }
  227.