Пример #1
0
        private double ExecuteGJKAlgorithm(
            VertexProperties[] vertexShape1,
            VertexProperties[] vertexShape2,
            ref CollisionPoint cp,
            ref List <SupportTriangle> triangles,
            ref Vector3d centroid,
            ref bool isIntersection)
        {
            double minDistance      = double.MaxValue;
            int    minTriangleIndex = -1;
            var    result           = new CollisionPointOutput();
            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;
                }
            }

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

            return(minDistance);
        }