예제 #1
0
        /// <summary>
        /// Checks for colision between <paramref name="A"/> and <paramref name="B"/>
        /// </summary>
        /// <param name="A">Convex object</param>
        /// <param name="B">Convex object</param>
        /// <param name="W">Initial simplex</param>
        public GJKOutput ProximityGJK(Polyline A, Polyline B, Simplex W)
        {
            Vector    v = new Vector(1, 0);
            GJKOutput result;
            double    delta = 0;
            Vector    w     = new Vector(A.points.First().X, A.points.First().Y);
            Simplex   old   = new Simplex();
            int       i     = 0;
            bool      start = true;

            while (Vector.Multiply(v, v) - delta > 0.1 || (v.X == 0 && v.Y == 0))
            {
                i++;
                if (!start)
                {
                    v = ClosestPoint(W);
                }
                Vector supportA = SupportHC(A, v);
                Vector supportB = SupportHC(B, -v);

                w = supportA - supportB;
                ShapePoint sp = new ShapePoint();
                sp.S1             = supportA;
                sp.S2             = supportB;
                sp.MinkowskiPoint = w;
                W = BestSimplex(W, sp);

                if (W.count == 3 || (w.X == 0 && w.Y == 0) || (i > 10000 && old == W))
                {
                    Pair <Vector, double> epa = computeEPA(A, B, W);
                    result.Collision = true;
                    result.Distance  = epa.Second;

                    result.closestFeatures      = new Pair <Vector, Vector>();
                    result.touchingVector       = new Vector();
                    result.touchingVectorPoints = new Pair <Vector, Vector>();
                    return(result);
                }
                if (Vector.Multiply(v, w) > 0 && !start)
                {
                    delta = Math.Max(delta, ((Vector.Multiply(v, w) * Vector.Multiply(v, w)) / Vector.Multiply(v, v)));
                }
                start = false;
                old   = W;
            }
            result.Collision            = false;
            result.Distance             = v.Length;
            result.closestFeatures      = getClosestFeatures(W);
            result.touchingVector       = w;
            result.touchingVectorPoints = getTouchingVectorPoints(W, w);
            return(result);
        }
예제 #2
0
 public Simplex()
 {
     A = new ShapePoint();
     B = new ShapePoint();
     C = new ShapePoint();
 }
예제 #3
0
        /// <summary>
        /// Form new simplex and test in which external Voronoi region the origin lies.
        /// </summary>
        /// <param name="W">Simplex</param>
        /// <param name="w">New point in CSO surface</param>
        /// <returns>New smallest simplex <paramref name="W"/> containing <paramref name="w"/> and closest point to origin.</returns>
        public Simplex BestSimplex(Simplex W, ShapePoint w)
        {
            Simplex result = new Simplex();

            switch (W.count)
            {
            case 0: {
                result.A     = w;
                result.count = 1;
                break;
            }

            case 1: {
                Simplex line = new Simplex();
                line.A     = w;
                line.B     = W.A;
                line.count = 2;
                Vector closest = ClosestToSegment(new Vector(0, 0), w.MinkowskiPoint, W.A.MinkowskiPoint);
                if (closest == w.MinkowskiPoint)
                {
                    result.A     = w;
                    result.count = 1;
                }
                else
                {
                    result.A     = W.A;
                    result.B     = w;
                    result.count = 2;
                }
                break;
            }

            case 2: {
                Vector  d     = new Vector(0, 0) - w.MinkowskiPoint;    // d.negate(); AO = O - A; O = origin (0, 0)
                Vector  e1    = W.A.MinkowskiPoint - w.MinkowskiPoint;  // AB = B - A
                Vector  e2    = W.B.MinkowskiPoint - w.MinkowskiPoint;
                Vector3 e1_3D = new Vector3((float)e1.X, (float)e1.Y, 0);
                Vector3 e2_3D = new Vector3((float)e2.X, (float)e2.Y, 0);
                Vector3 u1    = Vector3.Cross(e1_3D, Vector3.Cross(e1_3D, e2_3D));
                Vector3 v1    = Vector3.Cross(Vector3.Cross(e1_3D, e2_3D), e2_3D);
                if (Vector.Multiply(d, e1) < 0 && Vector.Multiply(d, e2) < 0)
                {
                    result.A     = w;
                    result.count = 1;
                }
                else if (Vector.Multiply(d, e1) > 0 && (d.X * u1.X + d.Y * u1.Y + 0 * u1.Z) > 0)
                {
                    result.A     = W.A;
                    result.B     = w;
                    result.count = 2;
                }
                else if (Vector.Multiply(d, e2) > 0 && (d.X * v1.X + d.Y * v1.Y + 0 * v1.Z) > 0)
                {
                    result.A     = W.B;
                    result.B     = w;
                    result.count = 2;
                }
                else if ((d.X * u1.X + d.Y * u1.Y + 0 * u1.Z < 0) && (d.X * v1.X + d.Y * v1.Y + 0 * v1.Z < 0))
                {
                    result.A     = W.A;
                    result.B     = W.B;
                    result.C     = w;
                    result.count = 3;
                }
                else
                {
                    result = W;
                }
                break;
            }
            }
            return(result);
        }