home *** CD-ROM | disk | FTP | other *** search
/ 404 Jogos / CLJG.iso / Diversos / Beez.swf / scripts / Box2D / Collision / b2Distance.as < prev    next >
Encoding:
Text File  |  2008-09-03  |  13.3 KB  |  379 lines
  1. package Box2D.Collision
  2. {
  3.    import Box2D.Collision.Shapes.*;
  4.    import Box2D.Common.*;
  5.    import Box2D.Common.Math.*;
  6.    
  7.    public class b2Distance
  8.    {
  9.       
  10.       private static var s_p2s:Array = [new b2Vec2(),new b2Vec2(),new b2Vec2()];
  11.       
  12.       private static var s_p1s:Array = [new b2Vec2(),new b2Vec2(),new b2Vec2()];
  13.       
  14.       private static var s_points:Array = [new b2Vec2(),new b2Vec2(),new b2Vec2()];
  15.       
  16.       private static var gPoint:b2Point = new b2Point();
  17.       
  18.       public static var g_GJK_Iterations:int = 0;
  19.        
  20.       
  21.       public function b2Distance()
  22.       {
  23.          super();
  24.       }
  25.       
  26.       public static function InPoints(w:b2Vec2, points:Array, pointCount:int) : Boolean
  27.       {
  28.          var points_i:b2Vec2 = null;
  29.          var dX:Number = NaN;
  30.          var dY:Number = NaN;
  31.          var mX:Number = NaN;
  32.          var mY:Number = NaN;
  33.          var k_tolerance:Number = 100 * Number.MIN_VALUE;
  34.          for(var i:int = 0; i < pointCount; i++)
  35.          {
  36.             points_i = points[i];
  37.             dX = Math.abs(w.x - points_i.x);
  38.             dY = Math.abs(w.y - points_i.y);
  39.             mX = Math.max(Math.abs(w.x),Math.abs(points_i.x));
  40.             mY = Math.max(Math.abs(w.y),Math.abs(points_i.y));
  41.             if(dX < k_tolerance * (mX + 1) && dY < k_tolerance * (mY + 1))
  42.             {
  43.                return true;
  44.             }
  45.          }
  46.          return false;
  47.       }
  48.       
  49.       public static function DistanceGeneric(x1:b2Vec2, x2:b2Vec2, shape1:*, xf1:b2XForm, shape2:*, xf2:b2XForm) : Number
  50.       {
  51.          var tVec:b2Vec2 = null;
  52.          var vX:Number = NaN;
  53.          var vY:Number = NaN;
  54.          var w1:b2Vec2 = null;
  55.          var w2:b2Vec2 = null;
  56.          var wX:Number = NaN;
  57.          var wY:Number = NaN;
  58.          var vw:Number = NaN;
  59.          var maxSqr:Number = NaN;
  60.          var i:int = 0;
  61.          var p1s:Array = s_p1s;
  62.          var p2s:Array = s_p2s;
  63.          var points:Array = s_points;
  64.          var pointCount:int = 0;
  65.          x1.SetV(shape1.GetFirstVertex(xf1));
  66.          x2.SetV(shape2.GetFirstVertex(xf2));
  67.          var vSqr:Number = 0;
  68.          var maxIterations:int = 20;
  69.          for(var iter:int = 0; iter < maxIterations; iter++)
  70.          {
  71.             vX = x2.x - x1.x;
  72.             vY = x2.y - x1.y;
  73.             w1 = shape1.Support(xf1,vX,vY);
  74.             w2 = shape2.Support(xf2,-vX,-vY);
  75.             vSqr = vX * vX + vY * vY;
  76.             wX = w2.x - w1.x;
  77.             wY = w2.y - w1.y;
  78.             vw = vX * wX + vY * wY;
  79.             if(vSqr - (vX * wX + vY * wY) <= 0.01 * vSqr)
  80.             {
  81.                if(pointCount == 0)
  82.                {
  83.                   x1.SetV(w1);
  84.                   x2.SetV(w2);
  85.                }
  86.                g_GJK_Iterations = iter;
  87.                return Math.sqrt(vSqr);
  88.             }
  89.             switch(pointCount)
  90.             {
  91.                case 0:
  92.                   tVec = p1s[0];
  93.                   tVec.SetV(w1);
  94.                   tVec = p2s[0];
  95.                   tVec.SetV(w2);
  96.                   tVec = points[0];
  97.                   tVec.x = wX;
  98.                   tVec.y = wY;
  99.                   x1.SetV(p1s[0]);
  100.                   x2.SetV(p2s[0]);
  101.                   pointCount++;
  102.                   break;
  103.                case 1:
  104.                   tVec = p1s[1];
  105.                   tVec.SetV(w1);
  106.                   tVec = p2s[1];
  107.                   tVec.SetV(w2);
  108.                   tVec = points[1];
  109.                   tVec.x = wX;
  110.                   tVec.y = wY;
  111.                   pointCount = ProcessTwo(x1,x2,p1s,p2s,points);
  112.                   break;
  113.                case 2:
  114.                   tVec = p1s[2];
  115.                   tVec.SetV(w1);
  116.                   tVec = p2s[2];
  117.                   tVec.SetV(w2);
  118.                   tVec = points[2];
  119.                   tVec.x = wX;
  120.                   tVec.y = wY;
  121.                   pointCount = ProcessThree(x1,x2,p1s,p2s,points);
  122.             }
  123.             if(pointCount == 3)
  124.             {
  125.                g_GJK_Iterations = iter;
  126.                return 0;
  127.             }
  128.             maxSqr = -Number.MAX_VALUE;
  129.             for(i = 0; i < pointCount; i++)
  130.             {
  131.                tVec = points[i];
  132.                maxSqr = b2Math.b2Max(maxSqr,tVec.x * tVec.x + tVec.y * tVec.y);
  133.             }
  134.             if(pointCount == 3 || vSqr <= 100 * Number.MIN_VALUE * maxSqr)
  135.             {
  136.                g_GJK_Iterations = iter;
  137.                vX = x2.x - x1.x;
  138.                vY = x2.y - x1.y;
  139.                vSqr = vX * vX + vY * vY;
  140.                return Math.sqrt(vSqr);
  141.             }
  142.          }
  143.          g_GJK_Iterations = maxIterations;
  144.          return Math.sqrt(vSqr);
  145.       }
  146.       
  147.       public static function DistanceCC(x1:b2Vec2, x2:b2Vec2, circle1:b2CircleShape, xf1:b2XForm, circle2:b2CircleShape, xf2:b2XForm) : Number
  148.       {
  149.          var tMat:b2Mat22 = null;
  150.          var tVec:b2Vec2 = null;
  151.          var dLen:Number = NaN;
  152.          var distance:Number = NaN;
  153.          tMat = xf1.R;
  154.          tVec = circle1.m_localPosition;
  155.          var p1X:Number = xf1.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
  156.          var p1Y:Number = xf1.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
  157.          tMat = xf2.R;
  158.          tVec = circle2.m_localPosition;
  159.          var p2X:Number = xf2.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
  160.          var p2Y:Number = xf2.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
  161.          var dX:Number = p2X - p1X;
  162.          var dY:Number = p2Y - p1Y;
  163.          var dSqr:Number = dX * dX + dY * dY;
  164.          var r1:Number = circle1.m_radius - b2Settings.b2_toiSlop;
  165.          var r2:Number = circle2.m_radius - b2Settings.b2_toiSlop;
  166.          var r:Number = r1 + r2;
  167.          if(dSqr > r * r)
  168.          {
  169.             dLen = Math.sqrt(dX * dX + dY * dY);
  170.             dX /= dLen;
  171.             dY /= dLen;
  172.             distance = dLen - r;
  173.             x1.x = p1X + r1 * dX;
  174.             x1.y = p1Y + r1 * dY;
  175.             x2.x = p2X - r2 * dX;
  176.             x2.y = p2Y - r2 * dY;
  177.             return distance;
  178.          }
  179.          if(dSqr > Number.MIN_VALUE * Number.MIN_VALUE)
  180.          {
  181.             dLen = Math.sqrt(dX * dX + dY * dY);
  182.             dX /= dLen;
  183.             dY /= dLen;
  184.             x1.x = p1X + r1 * dX;
  185.             x1.y = p1Y + r1 * dY;
  186.             x2.x = x1.x;
  187.             x2.y = x1.y;
  188.             return 0;
  189.          }
  190.          x1.x = p1X;
  191.          x1.y = p1Y;
  192.          x2.x = x1.x;
  193.          x2.y = x1.y;
  194.          return 0;
  195.       }
  196.       
  197.       public static function ProcessThree(x1:b2Vec2, x2:b2Vec2, p1s:Array, p2s:Array, points:Array) : int
  198.       {
  199.          var points_0:b2Vec2 = null;
  200.          var points_1:b2Vec2 = null;
  201.          var points_2:b2Vec2 = null;
  202.          var p1s_0:b2Vec2 = null;
  203.          var p1s_1:b2Vec2 = null;
  204.          var p1s_2:b2Vec2 = null;
  205.          var p2s_0:b2Vec2 = null;
  206.          var p2s_1:b2Vec2 = null;
  207.          var lambda:Number = NaN;
  208.          points_0 = points[0];
  209.          points_1 = points[1];
  210.          points_2 = points[2];
  211.          p1s_0 = p1s[0];
  212.          p1s_1 = p1s[1];
  213.          p1s_2 = p1s[2];
  214.          p2s_0 = p2s[0];
  215.          p2s_1 = p2s[1];
  216.          var p2s_2:b2Vec2 = p2s[2];
  217.          var aX:Number = points_0.x;
  218.          var aY:Number = points_0.y;
  219.          var bX:Number = points_1.x;
  220.          var bY:Number = points_1.y;
  221.          var cX:Number = points_2.x;
  222.          var cY:Number = points_2.y;
  223.          var abX:Number = bX - aX;
  224.          var abY:Number = bY - aY;
  225.          var acX:Number = cX - aX;
  226.          var acY:Number = cY - aY;
  227.          var bcX:Number = cX - bX;
  228.          var bcY:Number = cY - bY;
  229.          var sn:Number = -(aX * abX + aY * abY);
  230.          var sd:Number = bX * abX + bY * abY;
  231.          var tn:Number = -(aX * acX + aY * acY);
  232.          var td:Number = cX * acX + cY * acY;
  233.          var un:Number = -(bX * bcX + bY * bcY);
  234.          var ud:Number = cX * bcX + cY * bcY;
  235.          if(td <= 0 && ud <= 0)
  236.          {
  237.             x1.SetV(p1s_2);
  238.             x2.SetV(p2s_2);
  239.             p1s_0.SetV(p1s_2);
  240.             p2s_0.SetV(p2s_2);
  241.             points_0.SetV(points_2);
  242.             return 1;
  243.          }
  244.          var n:Number = abX * acY - abY * acX;
  245.          var vc:Number = n * (aX * bY - aY * bX);
  246.          var va:Number = n * (bX * cY - bY * cX);
  247.          if(va <= 0 && un >= 0 && ud >= 0 && un + ud > 0)
  248.          {
  249.             lambda = un / (un + ud);
  250.             x1.x = p1s_1.x + lambda * (p1s_2.x - p1s_1.x);
  251.             x1.y = p1s_1.y + lambda * (p1s_2.y - p1s_1.y);
  252.             x2.x = p2s_1.x + lambda * (p2s_2.x - p2s_1.x);
  253.             x2.y = p2s_1.y + lambda * (p2s_2.y - p2s_1.y);
  254.             p1s_0.SetV(p1s_2);
  255.             p2s_0.SetV(p2s_2);
  256.             points_0.SetV(points_2);
  257.             return 2;
  258.          }
  259.          var vb:Number = n * (cX * aY - cY * aX);
  260.          if(vb <= 0 && tn >= 0 && td >= 0 && tn + td > 0)
  261.          {
  262.             lambda = tn / (tn + td);
  263.             x1.x = p1s_0.x + lambda * (p1s_2.x - p1s_0.x);
  264.             x1.y = p1s_0.y + lambda * (p1s_2.y - p1s_0.y);
  265.             x2.x = p2s_0.x + lambda * (p2s_2.x - p2s_0.x);
  266.             x2.y = p2s_0.y + lambda * (p2s_2.y - p2s_0.y);
  267.             p1s_1.SetV(p1s_2);
  268.             p2s_1.SetV(p2s_2);
  269.             points_1.SetV(points_2);
  270.             return 2;
  271.          }
  272.          var denom:Number = va + vb + vc;
  273.          denom = 1 / denom;
  274.          var u:Number = va * denom;
  275.          var v:Number = vb * denom;
  276.          var w:Number = 1 - u - v;
  277.          x1.x = u * p1s_0.x + v * p1s_1.x + w * p1s_2.x;
  278.          x1.y = u * p1s_0.y + v * p1s_1.y + w * p1s_2.y;
  279.          x2.x = u * p2s_0.x + v * p2s_1.x + w * p2s_2.x;
  280.          x2.y = u * p2s_0.y + v * p2s_1.y + w * p2s_2.y;
  281.          return 3;
  282.       }
  283.       
  284.       public static function DistancePC(x1:b2Vec2, x2:b2Vec2, polygon:b2PolygonShape, xf1:b2XForm, circle:b2CircleShape, xf2:b2XForm) : Number
  285.       {
  286.          var tMat:b2Mat22 = null;
  287.          var tVec:b2Vec2 = null;
  288.          var dX:Number = NaN;
  289.          var dY:Number = NaN;
  290.          var dLen:Number = NaN;
  291.          var point:b2Point = gPoint;
  292.          tVec = circle.m_localPosition;
  293.          tMat = xf2.R;
  294.          point.p.x = xf2.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
  295.          point.p.y = xf2.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
  296.          var distance:Number = DistanceGeneric(x1,x2,polygon,xf1,point,b2Math.b2XForm_identity);
  297.          var r:Number = circle.m_radius - b2Settings.b2_toiSlop;
  298.          if(distance > r)
  299.          {
  300.             distance -= r;
  301.             dX = x2.x - x1.x;
  302.             dY = x2.y - x1.y;
  303.             dLen = Math.sqrt(dX * dX + dY * dY);
  304.             dX /= dLen;
  305.             dY /= dLen;
  306.             x2.x -= r * dX;
  307.             x2.y -= r * dY;
  308.          }
  309.          else
  310.          {
  311.             distance = 0;
  312.             x2.x = x1.x;
  313.             x2.y = x1.y;
  314.          }
  315.          return distance;
  316.       }
  317.       
  318.       public static function Distance(x1:b2Vec2, x2:b2Vec2, shape1:b2Shape, xf1:b2XForm, shape2:b2Shape, xf2:b2XForm) : Number
  319.       {
  320.          var type1:int = shape1.m_type;
  321.          var type2:int = shape2.m_type;
  322.          if(type1 == b2Shape.e_circleShape && type2 == b2Shape.e_circleShape)
  323.          {
  324.             return DistanceCC(x1,x2,shape1 as b2CircleShape,xf1,shape2 as b2CircleShape,xf2);
  325.          }
  326.          if(type1 == b2Shape.e_polygonShape && type2 == b2Shape.e_circleShape)
  327.          {
  328.             return DistancePC(x1,x2,shape1 as b2PolygonShape,xf1,shape2 as b2CircleShape,xf2);
  329.          }
  330.          if(type1 == b2Shape.e_circleShape && type2 == b2Shape.e_polygonShape)
  331.          {
  332.             return DistancePC(x2,x1,shape2 as b2PolygonShape,xf2,shape1 as b2CircleShape,xf1);
  333.          }
  334.          if(type1 == b2Shape.e_polygonShape && type2 == b2Shape.e_polygonShape)
  335.          {
  336.             return DistanceGeneric(x1,x2,shape1 as b2PolygonShape,xf1,shape2 as b2PolygonShape,xf2);
  337.          }
  338.          return 0;
  339.       }
  340.       
  341.       public static function ProcessTwo(x1:b2Vec2, x2:b2Vec2, p1s:Array, p2s:Array, points:Array) : int
  342.       {
  343.          var p1s_1:b2Vec2 = null;
  344.          var p2s_0:b2Vec2 = null;
  345.          var p2s_1:b2Vec2 = null;
  346.          var lambda:Number = NaN;
  347.          var points_0:b2Vec2 = points[0];
  348.          var points_1:b2Vec2 = points[1];
  349.          var p1s_0:b2Vec2 = p1s[0];
  350.          p1s_1 = p1s[1];
  351.          p2s_0 = p2s[0];
  352.          p2s_1 = p2s[1];
  353.          var rX:Number = -points_1.x;
  354.          var rY:Number = -points_1.y;
  355.          var dX:Number = points_0.x - points_1.x;
  356.          var dY:Number = points_0.y - points_1.y;
  357.          var length:Number = Math.sqrt(dX * dX + dY * dY);
  358.          dX /= length;
  359.          dY /= length;
  360.          lambda = rX * dX + rY * dY;
  361.          if(lambda <= 0 || length < Number.MIN_VALUE)
  362.          {
  363.             x1.SetV(p1s_1);
  364.             x2.SetV(p2s_1);
  365.             p1s_0.SetV(p1s_1);
  366.             p2s_0.SetV(p2s_1);
  367.             points_0.SetV(points_1);
  368.             return 1;
  369.          }
  370.          lambda /= length;
  371.          x1.x = p1s_1.x + lambda * (p1s_0.x - p1s_1.x);
  372.          x1.y = p1s_1.y + lambda * (p1s_0.y - p1s_1.y);
  373.          x2.x = p2s_1.x + lambda * (p2s_0.x - p2s_1.x);
  374.          x2.y = p2s_1.y + lambda * (p2s_0.y - p2s_1.y);
  375.          return 2;
  376.       }
  377.    }
  378. }
  379.