예제 #1
0
        /// <summary>
        /// Execute Expanding Polytope Algorithm (EPA).
        /// </summary>
        /// <param name="objectA">Object a.</param>
        /// <param name="objectB">Object b.</param>
        /// <param name="startTriangles">Start triangles.</param>
        public EPAOutput Execute(
            VertexProperties[] vertexObjA,
            VertexProperties[] vertexObjB,
            List <SupportTriangle> startTriangles,
            Vector3d centroid)
        {
            EngineCollisionPoint epaCollisionPoint = ExecuteEngine(
                vertexObjA,
                vertexObjB,
                startTriangles,
                centroid);

            var distance = Vector3d.Length(epaCollisionPoint.Dist);

            var collisionPoint = new CollisionPoint(
                epaCollisionPoint.A,
                epaCollisionPoint.B,
                epaCollisionPoint.Normal,
                distance,
                true);

            return(new EPAOutput(
                       distance,
                       collisionPoint));
        }
예제 #2
0
        public static void GetVertexFromMinkowsky(
            SupportTriangle triangle,
            VertexProperties[] vertexShape1,
            VertexProperties[] vertexShape2,
            ref EngineCollisionPoint collisionPoint)
        {
            Vector3d a1  = vertexShape1[triangle.A.a].Vertex;
            Vector3d ba1 = vertexShape1[triangle.B.a].Vertex - a1;
            Vector3d ca1 = vertexShape1[triangle.C.a].Vertex - a1;

            Vector3d a2  = vertexShape2[triangle.A.b].Vertex;
            Vector3d ba2 = vertexShape2[triangle.B.b].Vertex - a2;
            Vector3d ca2 = vertexShape2[triangle.C.b].Vertex - a2;

            collisionPoint.SetA(
                new VertexProperties(a1 + (ba1 * triangle.S) + (ca1 * triangle.T),
                                     new int?[] { vertexShape1[triangle.A.a].ID, vertexShape1[triangle.B.a].ID, vertexShape1[triangle.C.a].ID }));
            collisionPoint.SetB(
                new VertexProperties(a2 + (ba2 * triangle.S) + (ca2 * triangle.T),
                                     new int?[] { vertexShape2[triangle.A.b].ID, vertexShape2[triangle.B.b].ID, vertexShape2[triangle.C.b].ID }));
        }
예제 #3
0
        /// <summary>
        /// Ritorna la distanza se distanza 0.0f allora vi è intersezione o compenetrazione tra gli oggetti,
        /// se distanza 0.0 allora i due oggetti non collidono
        /// </summary>
        /// <returns>The GJK algorithm.</returns>
        /// <param name="shape1">Shape1.</param>
        /// <param name="shape2">Shape2.</param>
        /// <param name="cp">Cp.</param>
        /// <param name="isIntersection">If set to <c>true</c> is itersection.</param>
        private double ExecuteGJKAlgorithm(
            VertexProperties[] vertexShape1,
            VertexProperties[] vertexShape2,
            ref Vector3d collisionNormal,
            ref CollisionPoint cp,
            ref List <SupportTriangle> triangles,
            ref Vector3d centroid,
            ref bool isIntersection)
        {
            double minDistance      = double.MaxValue;
            int    minTriangleIndex = -1;
            var    result           = new EngineCollisionPoint();
            var    oldDirection     = new Vector3d();
            var    simplex          = new Simplex();

            //Primo punto del simplex
            simplex.Support.Add(GetFarthestPoint(vertexShape1, vertexShape2, vertexShape2.Length / 2));

            //Secondo punto del simplex
            Vector3d direction = Vector3d.Normalize(simplex.Support[0].s * -1.0);

            if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, direction)))
            {
                return(-1.0);
            }

            //Terzo punto del simplex
            direction = Vector3d.Normalize(GetDirectionOnSimplex2(simplex));
            if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, direction)))
            {
                return(-1.0);
            }

            //Quarto punto del simplex
            direction = Vector3d.Normalize(GeometryUtils.CalculateTriangleNormal(
                                               simplex.Support[0].s,
                                               simplex.Support[1].s,
                                               simplex.Support[2].s));

            if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, direction)))
            {
                simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, -1.0 * direction));
            }

            //Costruisco il poliedro
            centroid = Helper.SetStartTriangle(
                ref triangles,
                simplex.Support.ToArray());

            //Verifico che l'origine sia contenuta nel poliedro
            if (Helper.IsInConvexPoly(origin, triangles))
            {
                isIntersection = true;
                return(-1.0);
            }

            Vector3d triangleDistance = GetMinDistance(ref triangles, origin, ref minTriangleIndex);

            result.SetDist(triangleDistance);
            result.SetNormal(Vector3d.Normalize(triangleDistance));
            Helper.GetVertexFromMinkowsky(triangles[minTriangleIndex], vertexShape1, vertexShape2, ref result);

            minDistance = triangleDistance.Length();

            for (int i = 0; i < MaxIterations; i++)
            {
                direction = -1.0 * triangleDistance.Normalize();

                if (Vector3d.Length(direction) < constTolerance)
                {
                    direction = origin - centroid;
                }

                if (direction == oldDirection)
                {
                    break;
                }

                oldDirection = direction;

                if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, direction)))
                {
                    for (int j = 0; j < triangles.Count; j++)
                    {
                        direction = triangles[j].Normal;
                        if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, direction)))
                        {
                            if (simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, -1.0 * direction)))
                            {
                                break;
                            }

                            continue;
                        }
                        break;
                    }
                }

                triangles = Helper.AddPointToConvexPolygon(triangles, simplex.Support[simplex.Support.Count - 1], centroid);

                //Verifico che l'origine sia contenuta nel poliedro
                if (Helper.IsInConvexPoly(origin, triangles))
                {
                    isIntersection = true;
                    return(-1.0);
                }

                triangleDistance = GetMinDistance(ref triangles, origin, ref minTriangleIndex);

                double mod = triangleDistance.Length();

                if (mod < minDistance)
                {
                    result.SetDist(triangleDistance);
                    result.SetNormal(triangles[minTriangleIndex].Normal);

                    Helper.GetVertexFromMinkowsky(triangles[minTriangleIndex], vertexShape1, vertexShape2, ref result);

                    minDistance = mod;
                }
            }

            collisionNormal = -1.0 * result.Normal;

            cp = new CollisionPoint(
                result.A,
                result.B,
                collisionNormal,
                0.0,
                false);

            return(minDistance);
        }
예제 #4
0
        /// <summary>
        /// Executes the EPA engine.
        /// </summary>
        /// <param name="shape1">Shape1.</param>
        /// <param name="shape2">Shape2.</param>
        /// <param name="startPoint">Start point.</param>
        private EngineCollisionPoint ExecuteEngine(
            VertexProperties[] vertexShapeA,
            VertexProperties[] vertexShapeB,
            List <SupportTriangle> triangles,
            Vector3d centroid)
        {
            var epaCollisionPoint = new EngineCollisionPoint();

            double s = 0.0;
            double t = 0.0;

            var direction    = new Vector3d();
            var oldDirection = new Vector3d();
            var vDistance    = new Vector3d();

            SupportTriangle epaBuffer;

            if (triangles.Count > 0)
            {
                for (int k = 0; k < MaxIterations; k++)
                {
                    double minDistance = double.MaxValue;

                    for (int i = 0; i < triangles.Count; i++)
                    {
                        epaBuffer = triangles [i];

                        if (!GeometryUtils.TestCollinearity(
                                epaBuffer.A.s,
                                epaBuffer.B.s,
                                epaBuffer.C.s))
                        {
                            vDistance = GeometryUtils.GetPointTriangleIntersection(
                                epaBuffer.A.s,
                                epaBuffer.B.s,
                                epaBuffer.C.s,
                                origin,
                                ref s,
                                ref t).Value;

                            epaBuffer.SetValueS(s);
                            epaBuffer.SetValueT(t);
                        }
                        else
                        {
                            continue;
                        }

                        triangles [i] = epaBuffer;

                        double distance = Vector3d.Length(vDistance);

                        if (distance < minDistance)
                        {
                            minDistance = distance;

                            direction = vDistance;
                            epaCollisionPoint.SetDist(vDistance);

                            epaCollisionPoint.SetNormal(triangles[i].Normal);

                            Helper.GetVertexFromMinkowsky(
                                triangles[i],
                                vertexShapeA,
                                vertexShapeB,
                                ref epaCollisionPoint);
                        }
                    }

                    //L'origine risiede su uno dei bordi del triangolo
                    if (Vector3d.Length(direction) < constTolerance)
                    {
                        direction = origin - centroid;
                    }

                    if (direction == oldDirection)
                    {
                        break;
                    }

                    Support vt = Helper.GetMinkowskiFarthestPoint(
                        vertexShapeA,
                        vertexShapeB,
                        direction.Normalize());

                    triangles = Helper.AddPointToConvexPolygon(
                        triangles,
                        vt,
                        centroid);

                    oldDirection = direction;
                }
            }
            triangles.Clear();

            return(epaCollisionPoint);
        }