root/as3/MultiTouchSprite/trunk/demo/lib/Box2D/Collision/b2Distance.as

/*
* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

package Box2D.Collision{
        
import Box2D.Common.Math.*;
import Box2D.Common.*;
import Box2D.Collision.Shapes.*;
import Box2D.Collision.*;

public class b2Distance
{

// GJK using Voronoi regions (Christer Ericson) and region selection
// optimizations (Casey Muratori).

// The origin is either in the region of points[1] or in the edge region. The origin is
// not in region of points[0] because that is the old point.
static public function ProcessTwo(x1:b2Vec2, x2:b2Vec2, p1s:Array, p2s:Array, points:Array):int
{
        var points_0:b2Vec2 = points[0];
        var points_1:b2Vec2 = points[1];
        var p1s_0:b2Vec2 = p1s[0];
        var p1s_1:b2Vec2 = p1s[1];
        var p2s_0:b2Vec2 = p2s[0];
        var p2s_1:b2Vec2 = p2s[1];
        
        // If in point[1] region
        //b2Vec2 r = -points[1];
        var rX:Number = -points_1.x;
        var rY:Number = -points_1.y;
        //b2Vec2 d = points[1] - points[0];
        var dX:Number = points_0.x - points_1.x;
        var dY:Number = points_0.y - points_1.y;
        //float32 length = d.Normalize();
        var length:Number = Math.sqrt(dX*dX + dY*dY);
        dX /= length;
        dY /= length;
        
        //float32 lambda = b2Dot(r, d);
        var lambda:Number = rX * dX + rY * dY;
        if (lambda <= 0.0 || length < Number.MIN_VALUE)
        {
                // The simplex is reduced to a point.
                //*p1Out = p1s[1];
                x1.SetV(p1s_1);
                //*p2Out = p2s[1];
                x2.SetV(p2s_1);
                //p1s[0] = p1s[1];
                p1s_0.SetV(p1s_1);
                //p2s[0] = p2s[1];
                p2s_0.SetV(p2s_1);
                points_0.SetV(points_1);
                return 1;
        }

        // Else in edge region
        lambda /= length;
        //*x1 = p1s[1] + lambda * (p1s[0] - p1s[1]);
        x1.x = p1s_1.x + lambda * (p1s_0.x - p1s_1.x);
        x1.y = p1s_1.y + lambda * (p1s_0.y - p1s_1.y);
        //*x2 = p2s[1] + lambda * (p2s[0] - p2s[1]);
        x2.x = p2s_1.x + lambda * (p2s_0.x - p2s_1.x);
        x2.y = p2s_1.y + lambda * (p2s_0.y - p2s_1.y);
        return 2;
}

// Possible regions:
// - points[2]
// - edge points[0]-points[2]
// - edge points[1]-points[2]
// - inside the triangle
static public function ProcessThree(x1:b2Vec2, x2:b2Vec2, p1s:Array, p2s:Array, points:Array):int
{
        var points_0:b2Vec2 = points[0];
        var points_1:b2Vec2 = points[1];
        var points_2:b2Vec2 = points[2];
        var p1s_0:b2Vec2 = p1s[0];
        var p1s_1:b2Vec2 = p1s[1];
        var p1s_2:b2Vec2 = p1s[2];
        var p2s_0:b2Vec2 = p2s[0];
        var p2s_1:b2Vec2 = p2s[1];
        var p2s_2:b2Vec2 = p2s[2];
        
        //b2Vec2 a = points[0];
        var aX:Number = points_0.x;
        var aY:Number = points_0.y;
        //b2Vec2 b = points[1];
        var bX:Number = points_1.x;
        var bY:Number = points_1.y;
        //b2Vec2 c = points[2];
        var cX:Number = points_2.x;
        var cY:Number = points_2.y;

        //b2Vec2 ab = b - a;
        var abX:Number = bX - aX;
        var abY:Number = bY - aY;
        //b2Vec2 ac = c - a;
        var acX:Number = cX - aX;
        var acY:Number = cY - aY;
        //b2Vec2 bc = c - b;
        var bcX:Number = cX - bX;
        var bcY:Number = cY - bY;

        //float32 sn = -b2Dot(a, ab), sd = b2Dot(b, ab);
        var sn:Number = -(aX * abX + aY * abY);
        var sd:Number = (bX * abX + bY * abY);
        //float32 tn = -b2Dot(a, ac), td = b2Dot(c, ac);
        var tn:Number = -(aX * acX + aY * acY);
        var td:Number = (cX * acX + cY * acY);
        //float32 un = -b2Dot(b, bc), ud = b2Dot(c, bc);
        var un:Number = -(bX * bcX + bY * bcY);
        var ud:Number = (cX * bcX + cY * bcY);

        // In vertex c region?
        if (td <= 0.0 && ud <= 0.0)
        {
                // Single point
                //*x1 = p1s[2];
                x1.SetV(p1s_2);
                //*x2 = p2s[2];
                x2.SetV(p2s_2);
                //p1s[0] = p1s[2];
                p1s_0.SetV(p1s_2);
                //p2s[0] = p2s[2];
                p2s_0.SetV(p2s_2);
                points_0.SetV(points_2);
                return 1;
        }

        // Should not be in vertex a or b region.
        //b2Settings.b2Assert(sn > 0.0 || tn > 0.0);
        //b2Settings.b2Assert(sd > 0.0 || un > 0.0);

        //float32 n = b2Cross(ab, ac);
        var n:Number = abX * acY - abY * acX;

        // Should not be in edge ab region.
        //float32 vc = n * b2Cross(a, b);
        var vc:Number = n * (aX * bY - aY * bX); 
        //b2Settings.b2Assert(vc > 0.0 || sn > 0.0 || sd > 0.0);
        var lambda:Number;
        
        // In edge bc region?
        //float32 va = n * b2Cross(b, c);
        var va:Number = n * (bX * cY - bY * cX); 
        if (va <= 0.0 && un >= 0.0 && ud >= 0.0 && (un+ud) > 0.0)
        {
                //b2Settings.b2Assert(un + ud > 0.0);
                
                //float32 lambda = un / (un + ud);
                lambda = un / (un + ud);
                //*x1 = p1s[1] + lambda * (p1s[2] - p1s[1]);
                x1.x = p1s_1.x + lambda * (p1s_2.x - p1s_1.x);
                x1.y = p1s_1.y + lambda * (p1s_2.y - p1s_1.y);
                //*x2 = p2s[1] + lambda * (p2s[2] - p2s[1]);
                x2.x = p2s_1.x + lambda * (p2s_2.x - p2s_1.x);
                x2.y = p2s_1.y + lambda * (p2s_2.y - p2s_1.y);
                //p1s[0] = p1s[2];
                p1s_0.SetV(p1s_2);
                //p2s[0] = p2s[2];
                p2s_0.SetV(p2s_2);
                //points[0] = points[2];
                points_0.SetV(points_2);
                return 2;
        }

        // In edge ac region?
        //float32 vb = n * b2Cross(c, a);
        var vb:Number = n * (cX * aY - cY * aX);
        if (vb <= 0.0 && tn >= 0.0 && td >= 0.0 && (tn+td) > 0.0)
        {
                //b2Settings.b2Assert(tn + td > 0.0);
                
                //float32 lambda = tn / (tn + td);
                lambda = tn / (tn + td);
                //*x1 = p1s[0] + lambda * (p1s[2] - p1s[0]);
                x1.x = p1s_0.x + lambda * (p1s_2.x - p1s_0.x);
                x1.y = p1s_0.y + lambda * (p1s_2.y - p1s_0.y);
                //*x2 = p2s[0] + lambda * (p2s[2] - p2s[0]);
                x2.x = p2s_0.x + lambda * (p2s_2.x - p2s_0.x);
                x2.y = p2s_0.y + lambda * (p2s_2.y - p2s_0.y);
                //p1s[1] = p1s[2];
                p1s_1.SetV(p1s_2);
                //p2s[1] = p2s[2];
                p2s_1.SetV(p2s_2);
                //points[1] = points[2];
                points_1.SetV(points_2);
                return 2;
        }

        // Inside the triangle, compute barycentric coordinates
        //float32 denom = va + vb + vc;
        var denom:Number = va + vb + vc;
        //b2Settings.b2Assert(denom > 0.0);
        denom = 1.0 / denom;
        //float32 u = va * denom;
        var u:Number = va * denom;
        //float32 v = vb * denom;
        var v:Number = vb * denom;
        //float32 w = 1.0f - u - v;
        var w:Number = 1.0 - u - v;
        //*x1 = u * p1s[0] + v * p1s[1] + w * p1s[2];
        x1.x = u * p1s_0.x + v * p1s_1.x + w * p1s_2.x;
        x1.y = u * p1s_0.y + v * p1s_1.y + w * p1s_2.y;
        //*x2 = u * p2s[0] + v * p2s[1] + w * p2s[2];
        x2.x = u * p2s_0.x + v * p2s_1.x + w * p2s_2.x;
        x2.y = u * p2s_0.y + v * p2s_1.y + w * p2s_2.y;
        return 3;
}

static public function InPoints(w:b2Vec2, points:Array, pointCount:int):Boolean
{
        const k_tolerance:Number = 100.0 * Number.MIN_VALUE;
        for (var i:int = 0; i < pointCount; ++i)
        {
                var points_i:b2Vec2 = points[i];
                //b2Vec2 d = b2Abs(w - points[i]);
                var dX:Number = Math.abs(w.x - points_i.x);
                var dY:Number = Math.abs(w.y - points_i.y);
                //b2Vec2 m = b2Max(b2Abs(w), b2Abs(points[i]));
                var mX:Number = Math.max(Math.abs(w.x), Math.abs(points_i.x));
                var mY:Number = Math.max(Math.abs(w.y), Math.abs(points_i.y));
                
                if (dX < k_tolerance * (mX + 1.0) &&
                        dY < k_tolerance * (mY + 1.0)){
                        return true;
                }
        }

        return false;
}

// 
static private var s_p1s:Array = [new b2Vec2(), new b2Vec2(), new b2Vec2()];
static private var s_p2s:Array = [new b2Vec2(), new b2Vec2(), new b2Vec2()];
static private var s_points:Array = [new b2Vec2(), new b2Vec2(), new b2Vec2()];
//

static public function DistanceGeneric(x1:b2Vec2, x2:b2Vec2, 
                                                                                shape1:*, xf1:b2XForm, 
                                                                                shape2:*, xf2:b2XForm):Number
{
        var tVec: b2Vec2;
        
        //b2Vec2 p1s[3], p2s[3];
        var p1s:Array = s_p1s;
        var p2s:Array = s_p2s;
        //b2Vec2 points[3];
        var points:Array = s_points;
        //int32 pointCount = 0;
        var pointCount:int = 0;

        //*x1 = shape1->GetFirstVertex(xf1);
        x1.SetV(shape1.GetFirstVertex(xf1));
        //*x2 = shape2->GetFirstVertex(xf2);
        x2.SetV(shape2.GetFirstVertex(xf2));

        var vSqr:Number = 0.0;
        const maxIterations:int = 20;
        for (var iter:int = 0; iter < maxIterations; ++iter)
        {
                //b2Vec2 v = *x2 - *x1;
                var vX:Number = x2.x - x1.x;
                var vY:Number = x2.y - x1.y;
                //b2Vec2 w1 = shape1->Support(xf1, v);
                var w1:b2Vec2 = shape1.Support(xf1, vX, vY);
                //b2Vec2 w2 = shape2->Support(xf2, -v);
                var w2:b2Vec2 = shape2.Support(xf2, -vX, -vY);
                //float32 vSqr = b2Dot(v, v);
                vSqr = (vX*vX + vY*vY);
                //b2Vec2 w = w2 - w1;
                var wX:Number = w2.x - w1.x;
                var wY:Number = w2.y - w1.y;
                //float32 vw = b2Dot(v, w);
                var vw:Number = (vX*wX + vY*wY);
                //if (vSqr - b2Dot(v, w) <= 0.01f * vSqr) // or w in points
                if (vSqr - vw <= 0.01 * vSqr) // or w in points
                {
                        if (pointCount == 0)
                        {
                                //*x1 = w1;
                                x1.SetV(w1);
                                //*x2 = w2;
                                x2.SetV(w2);
                        }
                        g_GJK_Iterations = iter;
                        return Math.sqrt(vSqr);
                }
                
                switch (pointCount)
                {
                case 0:
                        //p1s[0] = w1;
                        tVec = p1s[0];
                        tVec.SetV(w1);
                        //p2s[0] = w2;
                        tVec = p2s[0];
                        tVec.SetV(w2);
                        //points[0] = w;
                        tVec = points[0];
                        tVec.x = wX;
                        tVec.y = wY;
                        //*x1 = p1s[0];
                        x1.SetV(p1s[0]);
                        //*x2 = p2s[0];
                        x2.SetV(p2s[0]);
                        ++pointCount;
                        break;
                        
                case 1:
                        //p1s[1] = w1;
                        tVec = p1s[1];
                        tVec.SetV(w1);
                        //p2s[1] = w2;
                        tVec = p2s[1];
                        tVec.SetV(w2);
                        //points[1] = w;
                        tVec = points[1];
                        tVec.x = wX;
                        tVec.y = wY;
                        pointCount = ProcessTwo(x1, x2, p1s, p2s, points);
                        break;
                        
                case 2:
                        //p1s[2] = w1;
                        tVec = p1s[2];
                        tVec.SetV(w1);
                        //p2s[2] = w2;
                        tVec = p2s[2];
                        tVec.SetV(w2);
                        //points[2] = w;
                        tVec = points[2];
                        tVec.x = wX;
                        tVec.y = wY;
                        pointCount = ProcessThree(x1, x2, p1s, p2s, points);
                        break;
                }
                
                // If we have three points, then the origin is in the corresponding triangle.
                if (pointCount == 3)
                {
                        g_GJK_Iterations = iter;
                        return 0.0;
                }
                
                //float32 maxSqr = -FLT_MAX;
                var maxSqr:Number = -Number.MAX_VALUE;
                for (var i:int = 0; i < pointCount; ++i)
                {
                        //maxSqr = b2Math.b2Max(maxSqr, b2Dot(points[i], points[i]));
                        tVec = points[i];
                        maxSqr = b2Math.b2Max(maxSqr, (tVec.x*tVec.x + tVec.y*tVec.y));
                }
                
                if (pointCount == 3 || vSqr <= 100.0 * Number.MIN_VALUE * maxSqr)
                {
                        g_GJK_Iterations = iter;
                        //v = *x2 - *x1;
                        vX = x2.x - x1.x;
                        vY = x2.y - x1.y;
                        //vSqr = b2Dot(v, v);
                        vSqr = (vX*vX + vY*vY);
                        return Math.sqrt(vSqr);
                }
        }

        g_GJK_Iterations = maxIterations;
        return Math.sqrt(vSqr);
}


static public function DistanceCC(
        x1:b2Vec2, x2:b2Vec2,
        circle1:b2CircleShape, xf1:b2XForm,
        circle2:b2CircleShape, xf2:b2XForm) : Number
{
        var tMat:b2Mat22;
        var tVec:b2Vec2;
        //b2Vec2 p1 = b2Mul(xf1, circle1->m_localPosition);
        tMat = xf1.R;
        tVec = circle1.m_localPosition;
        var p1X:Number = xf1.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
        var p1Y:Number = xf1.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
        //b2Vec2 p2 = b2Mul(xf2, circle2->m_localPosition);
        tMat = xf2.R;
        tVec = circle2.m_localPosition;
        var p2X:Number = xf2.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
        var p2Y:Number = xf2.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);

        //b2Vec2 d = p2 - p1;
        var dX:Number = p2X - p1X;
        var dY:Number = p2Y - p1Y;
        var dSqr:Number = (dX*dX + dY*dY);
        var r1:Number = circle1.m_radius - b2Settings.b2_toiSlop;
        var r2:Number = circle2.m_radius - b2Settings.b2_toiSlop;
        var r:Number = r1 + r2;
        if (dSqr > r * r)
        {
                //var dLen:Number = d.Normalize();
                var dLen:Number = Math.sqrt(dSqr);
                dX /= dLen;
                dY /= dLen;
                var distance:Number = dLen - r;
                //*x1 = p1 + r1 * d;
                x1.x = p1X + r1 * dX;
                x1.y = p1Y + r1 * dY;
                //*x2 = p2 - r2 * d;
                x2.x = p2X - r2 * dX;
                x2.y = p2Y - r2 * dY;
                return distance;
        }
        else if (dSqr > Number.MIN_VALUE * Number.MIN_VALUE)
        {
                //d.Normalize();
                dLen = Math.sqrt(dSqr);
                dX /= dLen;
                dY /= dLen;
                //*x1 = p1 + r1 * d;
                x1.x = p1X + r1 * dX;
                x1.y = p1Y + r1 * dY;
                //*x2 = *x1;
                x2.x = x1.x;
                x2.y = x1.y;
                return 0.0;
        }

        //*x1 = p1;
        x1.x = p1X;
        x1.y = p1Y;
        //*x2 = *x1;
        x2.x = x1.x;
        x2.y = x1.y;
        return 0.0;
}



// GJK is more robust with polygon-vs-point than polygon-vs-circle.
// So we convert polygon-vs-circle to polygon-vs-point.
static private var gPoint:b2Point = new b2Point();
///
static public function DistancePC(
        x1:b2Vec2, x2:b2Vec2,
        polygon:b2PolygonShape, xf1:b2XForm,
        circle:b2CircleShape, xf2:b2XForm) : Number
{
        
        var tMat:b2Mat22;
        var tVec:b2Vec2;
        
        var point:b2Point = gPoint;
        //point.p = b2Mul(xf2, circle->m_localPosition);
        tVec = circle.m_localPosition;
        tMat = xf2.R;
        point.p.x = xf2.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
        point.p.y = xf2.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);

        // Create variation of function to replace template
        var distance:Number = DistanceGeneric(x1, x2, polygon, xf1, point, b2Math.b2XForm_identity);

        var r:Number = circle.m_radius - b2Settings.b2_toiSlop;

        if (distance > r)
        {
                distance -= r;
                //b2Vec2 d = *x2 - *x1;
                var dX:Number = x2.x - x1.x;
                var dY:Number = x2.y - x1.y;
                //d.Normalize();
                var dLen:Number = Math.sqrt(dX*dX + dY*dY);
                dX /= dLen;
                dY /= dLen;
                //*x2 -= r * d;
                x2.x -= r * dX;
                x2.y -= r * dY;
        }
        else
        {
                distance = 0.0;
                //*x2 = *x1;
                x2.x = x1.x;
                x2.y = x1.y;
        }
        
        return distance;
}


static public function Distance(x1:b2Vec2, x2:b2Vec2,
                                   shape1:b2Shape, xf1:b2XForm,
                                   shape2:b2Shape, xf2:b2XForm) : Number
{
        //b2ShapeType type1 = shape1->GetType();
        var type1:int = shape1.m_type;
        //b2ShapeType type2 = shape2->GetType();
        var type2:int = shape2.m_type;

        if (type1 == b2Shape.e_circleShape && type2 == b2Shape.e_circleShape)
        {
                //return DistanceCC(x1, x2, (b2CircleShape*)shape1, xf1, (b2CircleShape*)shape2, xf2);
                return DistanceCC(x1, x2, shape1 as b2CircleShape, xf1, shape2 as b2CircleShape, xf2);
        }
        
        if (type1 == b2Shape.e_polygonShape && type2 == b2Shape.e_circleShape)
        {
                //return DistancePC(x1, x2, (b2PolygonShape*)shape1, xf1, (b2CircleShape*)shape2, xf2);
                return DistancePC(x1, x2, shape1 as b2PolygonShape, xf1, shape2 as b2CircleShape, xf2);
        }

        if (type1 == b2Shape.e_circleShape && type2 == b2Shape.e_polygonShape)
        {
                return DistancePC(x2, x1, shape2 as b2PolygonShape, xf2, shape1 as b2CircleShape, xf1);
        }

        if (type1 == b2Shape.e_polygonShape && type2 == b2Shape.e_polygonShape)
        {
                return DistanceGeneric(x1, x2, shape1 as b2PolygonShape, xf1, shape2 as b2PolygonShape, xf2);
        }
        
        return 0.0;
}



static public var g_GJK_Iterations:int = 0;



};


}