Esempio n. 1
0
        private static List <SupportTriangle> AddTriangle(
            List <Edge> edge,
            List <SupportTriangle> triangles,
            Support p,
            Vector3d centroid)
        {
            var result = new List <SupportTriangle>(triangles);

            for (int i = 0; i < edge.Count; i++)
            {
                var normal = GeometryUtils.CalculateTriangleNormal(edge[i].A.s, edge[i].B.s, p.s);

                var tri = new SupportTriangle(
                    edge[i].A,
                    edge[i].B,
                    p,
                    0.0,
                    0.0,
                    normal);

                tri = TurnClockWiseNormal(tri, centroid);
                result.Add(tri);
            }

            return(result);
        }
Esempio n. 2
0
        private Vector3d GetMinDistance(
            ref List <SupportTriangle> triangles,
            Vector3d point,
            ref int minTriangleIndex)
        {
            var    result = new Vector3d();
            var    distanceBuf = new Vector3d();
            double s = 0; double t = 0;

            var buffer = new SupportTriangle();

            double minDistance = double.MaxValue;

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

                if (!GeometryUtils.TestCollinearity(
                        buffer.A.s,
                        buffer.B.s,
                        buffer.C.s))
                {
                    distanceBuf = GeometryUtils.GetPointTriangleIntersection(
                        buffer.A.s,
                        buffer.B.s,
                        buffer.C.s,
                        point,
                        ref s,
                        ref t).Value;

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


                triangles[i] = buffer;

                double distance = Vector3d.Length(distanceBuf);

                if (distance < minDistance)
                {
                    minDistance      = distance;
                    minTriangleIndex = i;
                    result           = distanceBuf;
                }
            }

            return(result);
        }
Esempio n. 3
0
        private static SupportTriangle AddTriangle(
            Support a,
            Support b,
            Support c)
        {
            var normal = GeometryUtils.CalculateTriangleNormal(a.s, b.s, c.s);

            var epaTriangle = new SupportTriangle(
                a,
                b,
                c,
                0.0,
                0.0,
                normal);

            return(epaTriangle);
        }
Esempio n. 4
0
        public static void GetVertexFromMinkowsky(
            SupportTriangle triangle,
            VertexProperties[] vertexShape1,
            VertexProperties[] vertexShape2,
            ref CollisionPointOutput 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 }));
        }
Esempio n. 5
0
        public static SupportTriangle TurnClockWiseNormal(
            SupportTriangle triangle,
            Vector3d v)
        {
            Vector3d centroidDiff = triangle.A.s - v;
            Vector3d normal       = triangle.Normal;

            if (Vector3d.Dot(triangle.Normal, centroidDiff) < -1E-10)
            {
                normal = triangle.Normal * -1.0;
            }

            var tr = new SupportTriangle(
                triangle.A,
                triangle.B,
                triangle.C,
                triangle.S,
                triangle.T,
                normal);

            return(tr);
        }