home *** CD-ROM | disk | FTP | other *** search
/ 404 Jogos / CLJG.iso / Diversos / Beez.swf / scripts / Box2D / Dynamics / Contacts / b2ContactSolver.as < prev    next >
Encoding:
Text File  |  2008-09-03  |  17.3 KB  |  452 lines
  1. package Box2D.Dynamics.Contacts
  2. {
  3.    import Box2D.Collision.b2Manifold;
  4.    import Box2D.Collision.b2ManifoldPoint;
  5.    import Box2D.Common.Math.b2Mat22;
  6.    import Box2D.Common.Math.b2Math;
  7.    import Box2D.Common.Math.b2Vec2;
  8.    import Box2D.Common.b2Settings;
  9.    import Box2D.Dynamics.b2Body;
  10.    import Box2D.Dynamics.b2TimeStep;
  11.    
  12.    public class b2ContactSolver
  13.    {
  14.        
  15.       
  16.       public var m_constraintCount:int;
  17.       
  18.       public var m_constraints:Array;
  19.       
  20.       public var m_allocator:*;
  21.       
  22.       public var m_step:b2TimeStep;
  23.       
  24.       public function b2ContactSolver(step:b2TimeStep, contacts:Array, contactCount:int, allocator:*)
  25.       {
  26.          var contact:b2Contact = null;
  27.          var i:int = 0;
  28.          var tVec:b2Vec2 = null;
  29.          var tMat:b2Mat22 = null;
  30.          var b1:b2Body = null;
  31.          var b2:b2Body = null;
  32.          var manifoldCount:int = 0;
  33.          var manifolds:Array = null;
  34.          var friction:Number = NaN;
  35.          var restitution:Number = NaN;
  36.          var v1X:Number = NaN;
  37.          var v1Y:Number = NaN;
  38.          var v2X:Number = NaN;
  39.          var v2Y:Number = NaN;
  40.          var w1:Number = NaN;
  41.          var w2:Number = NaN;
  42.          var j:int = 0;
  43.          var manifold:b2Manifold = null;
  44.          var normalX:Number = NaN;
  45.          var normalY:Number = NaN;
  46.          var c:b2ContactConstraint = null;
  47.          var k:uint = 0;
  48.          var cp:b2ManifoldPoint = null;
  49.          var ccp:b2ContactConstraintPoint = null;
  50.          var tX:Number = NaN;
  51.          var tY:Number = NaN;
  52.          var r1X:Number = NaN;
  53.          var r1Y:Number = NaN;
  54.          var r2X:Number = NaN;
  55.          var r2Y:Number = NaN;
  56.          var r1Sqr:Number = NaN;
  57.          var r2Sqr:Number = NaN;
  58.          var rn1:Number = NaN;
  59.          var rn2:Number = NaN;
  60.          var kNormal:Number = NaN;
  61.          var kEqualized:Number = NaN;
  62.          var tangentX:Number = NaN;
  63.          var tangentY:Number = NaN;
  64.          var rt1:Number = NaN;
  65.          var rt2:Number = NaN;
  66.          var kTangent:Number = NaN;
  67.          var vRel:Number = NaN;
  68.          m_step = new b2TimeStep();
  69.          m_constraints = new Array();
  70.          super();
  71.          m_step.dt = step.dt;
  72.          m_step.inv_dt = step.inv_dt;
  73.          m_step.maxIterations = step.maxIterations;
  74.          m_allocator = allocator;
  75.          m_constraintCount = 0;
  76.          for(i = 0; i < contactCount; i++)
  77.          {
  78.             contact = contacts[i];
  79.             m_constraintCount += contact.m_manifoldCount;
  80.          }
  81.          for(i = 0; i < m_constraintCount; i++)
  82.          {
  83.             m_constraints[i] = new b2ContactConstraint();
  84.          }
  85.          var count:int = 0;
  86.          for(i = 0; i < contactCount; i++)
  87.          {
  88.             contact = contacts[i];
  89.             b1 = contact.m_shape1.m_body;
  90.             b2 = contact.m_shape2.m_body;
  91.             manifoldCount = contact.m_manifoldCount;
  92.             manifolds = contact.GetManifolds();
  93.             friction = contact.m_friction;
  94.             restitution = contact.m_restitution;
  95.             v1X = b1.m_linearVelocity.x;
  96.             v1Y = b1.m_linearVelocity.y;
  97.             v2X = b2.m_linearVelocity.x;
  98.             v2Y = b2.m_linearVelocity.y;
  99.             w1 = b1.m_angularVelocity;
  100.             w2 = b2.m_angularVelocity;
  101.             for(j = 0; j < manifoldCount; j++)
  102.             {
  103.                manifold = manifolds[j];
  104.                normalX = manifold.normal.x;
  105.                normalY = manifold.normal.y;
  106.                c = m_constraints[count];
  107.                c.body1 = b1;
  108.                c.body2 = b2;
  109.                c.manifold = manifold;
  110.                c.normal.x = normalX;
  111.                c.normal.y = normalY;
  112.                c.pointCount = manifold.pointCount;
  113.                c.friction = friction;
  114.                c.restitution = restitution;
  115.                for(k = 0; k < c.pointCount; k++)
  116.                {
  117.                   cp = manifold.points[k];
  118.                   ccp = c.points[k];
  119.                   ccp.normalImpulse = cp.normalImpulse;
  120.                   ccp.tangentImpulse = cp.tangentImpulse;
  121.                   ccp.separation = cp.separation;
  122.                   ccp.positionImpulse = 0;
  123.                   ccp.localAnchor1.SetV(cp.localPoint1);
  124.                   ccp.localAnchor2.SetV(cp.localPoint2);
  125.                   tMat = b1.m_xf.R;
  126.                   r1X = cp.localPoint1.x - b1.m_sweep.localCenter.x;
  127.                   r1Y = cp.localPoint1.y - b1.m_sweep.localCenter.y;
  128.                   tX = tMat.col1.x * r1X + tMat.col2.x * r1Y;
  129.                   r1Y = tMat.col1.y * r1X + tMat.col2.y * r1Y;
  130.                   r1X = tX;
  131.                   ccp.r1.Set(r1X,r1Y);
  132.                   tMat = b2.m_xf.R;
  133.                   r2X = cp.localPoint2.x - b2.m_sweep.localCenter.x;
  134.                   r2Y = cp.localPoint2.y - b2.m_sweep.localCenter.y;
  135.                   tX = tMat.col1.x * r2X + tMat.col2.x * r2Y;
  136.                   r2Y = tMat.col1.y * r2X + tMat.col2.y * r2Y;
  137.                   r2X = tX;
  138.                   ccp.r2.Set(r2X,r2Y);
  139.                   r1Sqr = r1X * r1X + r1Y * r1Y;
  140.                   r2Sqr = r2X * r2X + r2Y * r2Y;
  141.                   rn1 = r1X * normalX + r1Y * normalY;
  142.                   rn2 = r2X * normalX + r2Y * normalY;
  143.                   kNormal = b1.m_invMass + b2.m_invMass;
  144.                   kNormal += b1.m_invI * (r1Sqr - rn1 * rn1) + b2.m_invI * (r2Sqr - rn2 * rn2);
  145.                   ccp.normalMass = 1 / kNormal;
  146.                   kEqualized = b1.m_mass * b1.m_invMass + b2.m_mass * b2.m_invMass;
  147.                   kEqualized += b1.m_mass * b1.m_invI * (r1Sqr - rn1 * rn1) + b2.m_mass * b2.m_invI * (r2Sqr - rn2 * rn2);
  148.                   ccp.equalizedMass = 1 / kEqualized;
  149.                   tangentX = normalY;
  150.                   tangentY = -normalX;
  151.                   rt1 = r1X * tangentX + r1Y * tangentY;
  152.                   rt2 = r2X * tangentX + r2Y * tangentY;
  153.                   kTangent = b1.m_invMass + b2.m_invMass;
  154.                   kTangent += b1.m_invI * (r1Sqr - rt1 * rt1) + b2.m_invI * (r2Sqr - rt2 * rt2);
  155.                   ccp.tangentMass = 1 / kTangent;
  156.                   ccp.velocityBias = 0;
  157.                   if(ccp.separation > 0)
  158.                   {
  159.                      ccp.velocityBias = -60 * ccp.separation;
  160.                   }
  161.                   tX = v2X + -w2 * r2Y - v1X - -w1 * r1Y;
  162.                   tY = v2Y + w2 * r2X - v1Y - w1 * r1X;
  163.                   vRel = c.normal.x * tX + c.normal.y * tY;
  164.                   if(vRel < -b2Settings.b2_velocityThreshold)
  165.                   {
  166.                      ccp.velocityBias += -c.restitution * vRel;
  167.                   }
  168.                }
  169.                count++;
  170.             }
  171.          }
  172.       }
  173.       
  174.       public function InitVelocityConstraints(step:b2TimeStep) : void
  175.       {
  176.          var tVec:b2Vec2 = null;
  177.          var tVec2:b2Vec2 = null;
  178.          var tMat:b2Mat22 = null;
  179.          var c:b2ContactConstraint = null;
  180.          var b1:b2Body = null;
  181.          var b2:b2Body = null;
  182.          var invMass1:Number = NaN;
  183.          var invI1:Number = NaN;
  184.          var invMass2:Number = NaN;
  185.          var invI2:Number = NaN;
  186.          var normalX:Number = NaN;
  187.          var normalY:Number = NaN;
  188.          var tangentX:Number = NaN;
  189.          var tangentY:Number = NaN;
  190.          var tX:Number = NaN;
  191.          var j:int = 0;
  192.          var tCount:int = 0;
  193.          var ccp:b2ContactConstraintPoint = null;
  194.          var PX:Number = NaN;
  195.          var PY:Number = NaN;
  196.          var ccp2:b2ContactConstraintPoint = null;
  197.          for(var i:int = 0; i < m_constraintCount; i++)
  198.          {
  199.             c = m_constraints[i];
  200.             b1 = c.body1;
  201.             b2 = c.body2;
  202.             invMass1 = b1.m_invMass;
  203.             invI1 = b1.m_invI;
  204.             invMass2 = b2.m_invMass;
  205.             invI2 = b2.m_invI;
  206.             normalX = c.normal.x;
  207.             normalY = c.normal.y;
  208.             tangentX = normalY;
  209.             tangentY = -normalX;
  210.             if(step.warmStarting)
  211.             {
  212.                tCount = c.pointCount;
  213.                for(j = 0; j < tCount; j++)
  214.                {
  215.                   ccp = c.points[j];
  216.                   ccp.normalImpulse *= step.dtRatio;
  217.                   ccp.tangentImpulse *= step.dtRatio;
  218.                   PX = ccp.normalImpulse * normalX + ccp.tangentImpulse * tangentX;
  219.                   PY = ccp.normalImpulse * normalY + ccp.tangentImpulse * tangentY;
  220.                   b1.m_angularVelocity -= invI1 * (ccp.r1.x * PY - ccp.r1.y * PX);
  221.                   b1.m_linearVelocity.x -= invMass1 * PX;
  222.                   b1.m_linearVelocity.y -= invMass1 * PY;
  223.                   b2.m_angularVelocity += invI2 * (ccp.r2.x * PY - ccp.r2.y * PX);
  224.                   b2.m_linearVelocity.x += invMass2 * PX;
  225.                   b2.m_linearVelocity.y += invMass2 * PY;
  226.                }
  227.             }
  228.             else
  229.             {
  230.                tCount = c.pointCount;
  231.                for(j = 0; j < tCount; j++)
  232.                {
  233.                   ccp2 = c.points[j];
  234.                   ccp2.normalImpulse = 0;
  235.                   ccp2.tangentImpulse = 0;
  236.                }
  237.             }
  238.          }
  239.       }
  240.       
  241.       public function SolvePositionConstraints(baumgarte:Number) : Boolean
  242.       {
  243.          var tMat:b2Mat22 = null;
  244.          var tVec:b2Vec2 = null;
  245.          var c:b2ContactConstraint = null;
  246.          var b1:b2Body = null;
  247.          var b2:b2Body = null;
  248.          var b1_sweep_c:b2Vec2 = null;
  249.          var b1_sweep_a:Number = NaN;
  250.          var b2_sweep_c:b2Vec2 = null;
  251.          var b2_sweep_a:Number = NaN;
  252.          var invMass1:Number = NaN;
  253.          var invI1:Number = NaN;
  254.          var invMass2:Number = NaN;
  255.          var invI2:Number = NaN;
  256.          var normalX:Number = NaN;
  257.          var normalY:Number = NaN;
  258.          var tCount:int = 0;
  259.          var j:int = 0;
  260.          var ccp:b2ContactConstraintPoint = null;
  261.          var r1X:Number = NaN;
  262.          var r1Y:Number = NaN;
  263.          var r2X:Number = NaN;
  264.          var r2Y:Number = NaN;
  265.          var tX:Number = NaN;
  266.          var p1X:Number = NaN;
  267.          var p1Y:Number = NaN;
  268.          var p2X:Number = NaN;
  269.          var p2Y:Number = NaN;
  270.          var dpX:Number = NaN;
  271.          var dpY:Number = NaN;
  272.          var separation:Number = NaN;
  273.          var C:Number = NaN;
  274.          var dImpulse:Number = NaN;
  275.          var impulse0:Number = NaN;
  276.          var impulseX:Number = NaN;
  277.          var impulseY:Number = NaN;
  278.          var minSeparation:Number = 0;
  279.          for(var i:int = 0; i < m_constraintCount; i++)
  280.          {
  281.             c = m_constraints[i];
  282.             b1 = c.body1;
  283.             b2 = c.body2;
  284.             b1_sweep_c = b1.m_sweep.c;
  285.             b1_sweep_a = b1.m_sweep.a;
  286.             b2_sweep_c = b2.m_sweep.c;
  287.             b2_sweep_a = b2.m_sweep.a;
  288.             invMass1 = b1.m_mass * b1.m_invMass;
  289.             invI1 = b1.m_mass * b1.m_invI;
  290.             invMass2 = b2.m_mass * b2.m_invMass;
  291.             invI2 = b2.m_mass * b2.m_invI;
  292.             normalX = c.normal.x;
  293.             normalY = c.normal.y;
  294.             tCount = c.pointCount;
  295.             for(j = 0; j < tCount; j++)
  296.             {
  297.                ccp = c.points[j];
  298.                tMat = b1.m_xf.R;
  299.                tVec = b1.m_sweep.localCenter;
  300.                r1X = ccp.localAnchor1.x - tVec.x;
  301.                r1Y = ccp.localAnchor1.y - tVec.y;
  302.                tX = tMat.col1.x * r1X + tMat.col2.x * r1Y;
  303.                r1Y = tMat.col1.y * r1X + tMat.col2.y * r1Y;
  304.                r1X = tX;
  305.                tMat = b2.m_xf.R;
  306.                tVec = b2.m_sweep.localCenter;
  307.                r2X = ccp.localAnchor2.x - tVec.x;
  308.                r2Y = ccp.localAnchor2.y - tVec.y;
  309.                tX = tMat.col1.x * r2X + tMat.col2.x * r2Y;
  310.                r2Y = tMat.col1.y * r2X + tMat.col2.y * r2Y;
  311.                r2X = tX;
  312.                p1X = b1_sweep_c.x + r1X;
  313.                p1Y = b1_sweep_c.y + r1Y;
  314.                p2X = b2_sweep_c.x + r2X;
  315.                p2Y = b2_sweep_c.y + r2Y;
  316.                dpX = p2X - p1X;
  317.                dpY = p2Y - p1Y;
  318.                separation = dpX * normalX + dpY * normalY + ccp.separation;
  319.                minSeparation = b2Math.b2Min(minSeparation,separation);
  320.                C = baumgarte * b2Math.b2Clamp(separation + b2Settings.b2_linearSlop,-b2Settings.b2_maxLinearCorrection,0);
  321.                dImpulse = -ccp.equalizedMass * C;
  322.                impulse0 = ccp.positionImpulse;
  323.                ccp.positionImpulse = b2Math.b2Max(impulse0 + dImpulse,0);
  324.                dImpulse = ccp.positionImpulse - impulse0;
  325.                impulseX = dImpulse * normalX;
  326.                impulseY = dImpulse * normalY;
  327.                b1_sweep_c.x -= invMass1 * impulseX;
  328.                b1_sweep_c.y -= invMass1 * impulseY;
  329.                b1_sweep_a -= invI1 * (r1X * impulseY - r1Y * impulseX);
  330.                b1.m_sweep.a = b1_sweep_a;
  331.                b1.SynchronizeTransform();
  332.                b2_sweep_c.x += invMass2 * impulseX;
  333.                b2_sweep_c.y += invMass2 * impulseY;
  334.                b2_sweep_a += invI2 * (r2X * impulseY - r2Y * impulseX);
  335.                b2.m_sweep.a = b2_sweep_a;
  336.                b2.SynchronizeTransform();
  337.             }
  338.          }
  339.          return minSeparation >= -1.5 * b2Settings.b2_linearSlop;
  340.       }
  341.       
  342.       public function SolveVelocityConstraints() : void
  343.       {
  344.          var j:int = 0;
  345.          var ccp:b2ContactConstraintPoint = null;
  346.          var r1X:Number = NaN;
  347.          var r1Y:Number = NaN;
  348.          var r2X:Number = NaN;
  349.          var r2Y:Number = NaN;
  350.          var dvX:Number = NaN;
  351.          var dvY:Number = NaN;
  352.          var vn:Number = NaN;
  353.          var vt:Number = NaN;
  354.          var lambda_n:Number = NaN;
  355.          var lambda_t:Number = NaN;
  356.          var newImpulse_n:Number = NaN;
  357.          var newImpulse_t:Number = NaN;
  358.          var PX:Number = NaN;
  359.          var PY:Number = NaN;
  360.          var tMat:b2Mat22 = null;
  361.          var tVec:b2Vec2 = null;
  362.          var c:b2ContactConstraint = null;
  363.          var b1:b2Body = null;
  364.          var b2:b2Body = null;
  365.          var w1:Number = NaN;
  366.          var w2:Number = NaN;
  367.          var v1:b2Vec2 = null;
  368.          var v2:b2Vec2 = null;
  369.          var invMass1:Number = NaN;
  370.          var invI1:Number = NaN;
  371.          var invMass2:Number = NaN;
  372.          var invI2:Number = NaN;
  373.          var normalX:Number = NaN;
  374.          var normalY:Number = NaN;
  375.          var tangentX:Number = NaN;
  376.          var tangentY:Number = NaN;
  377.          var friction:Number = NaN;
  378.          var tX:Number = NaN;
  379.          var tCount:int = 0;
  380.          var maxFriction:Number = NaN;
  381.          for(var i:int = 0; i < m_constraintCount; i++)
  382.          {
  383.             c = m_constraints[i];
  384.             b1 = c.body1;
  385.             b2 = c.body2;
  386.             w1 = b1.m_angularVelocity;
  387.             w2 = b2.m_angularVelocity;
  388.             v1 = b1.m_linearVelocity;
  389.             v2 = b2.m_linearVelocity;
  390.             invMass1 = b1.m_invMass;
  391.             invI1 = b1.m_invI;
  392.             invMass2 = b2.m_invMass;
  393.             invI2 = b2.m_invI;
  394.             normalX = c.normal.x;
  395.             normalY = c.normal.y;
  396.             tangentX = normalY;
  397.             tangentY = -normalX;
  398.             friction = c.friction;
  399.             tCount = c.pointCount;
  400.             for(j = 0; j < tCount; j++)
  401.             {
  402.                ccp = c.points[j];
  403.                dvX = v2.x + -w2 * ccp.r2.y - v1.x - -w1 * ccp.r1.y;
  404.                dvY = v2.y + w2 * ccp.r2.x - v1.y - w1 * ccp.r1.x;
  405.                vn = dvX * normalX + dvY * normalY;
  406.                lambda_n = -ccp.normalMass * (vn - ccp.velocityBias);
  407.                vt = dvX * tangentX + dvY * tangentY;
  408.                lambda_t = ccp.tangentMass * -vt;
  409.                newImpulse_n = b2Math.b2Max(ccp.normalImpulse + lambda_n,0);
  410.                lambda_n = newImpulse_n - ccp.normalImpulse;
  411.                maxFriction = friction * ccp.normalImpulse;
  412.                newImpulse_t = b2Math.b2Clamp(ccp.tangentImpulse + lambda_t,-maxFriction,maxFriction);
  413.                lambda_t = newImpulse_t - ccp.tangentImpulse;
  414.                PX = lambda_n * normalX + lambda_t * tangentX;
  415.                PY = lambda_n * normalY + lambda_t * tangentY;
  416.                v1.x -= invMass1 * PX;
  417.                v1.y -= invMass1 * PY;
  418.                w1 -= invI1 * (ccp.r1.x * PY - ccp.r1.y * PX);
  419.                v2.x += invMass2 * PX;
  420.                v2.y += invMass2 * PY;
  421.                w2 += invI2 * (ccp.r2.x * PY - ccp.r2.y * PX);
  422.                ccp.normalImpulse = newImpulse_n;
  423.                ccp.tangentImpulse = newImpulse_t;
  424.             }
  425.             b1.m_angularVelocity = w1;
  426.             b2.m_angularVelocity = w2;
  427.          }
  428.       }
  429.       
  430.       public function FinalizeVelocityConstraints() : void
  431.       {
  432.          var c:b2ContactConstraint = null;
  433.          var m:b2Manifold = null;
  434.          var j:int = 0;
  435.          var point1:b2ManifoldPoint = null;
  436.          var point2:b2ContactConstraintPoint = null;
  437.          for(var i:int = 0; i < m_constraintCount; i++)
  438.          {
  439.             c = m_constraints[i];
  440.             m = c.manifold;
  441.             for(j = 0; j < c.pointCount; j++)
  442.             {
  443.                point1 = m.points[j];
  444.                point2 = c.points[j];
  445.                point1.normalImpulse = point2.normalImpulse;
  446.                point1.tangentImpulse = point2.tangentImpulse;
  447.             }
  448.          }
  449.       }
  450.    }
  451. }
  452.