Пример #1
0
 public void Clear()
 {
     vectors.Clear();
     pathPoints.Clear();
     startTri       = null;
     lastPointAdded = null;
     lastEdge       = null;
 }
Пример #2
0
        /**
         * Store all edge crossing points between the start and end indices. If the path
         * crosses exactly the start or end points (which is quite likely), store the
         * edges in order of crossing in the EdgePoint data structure.
         * <p/>
         * Edge crossings are calculated as intersections with the plane from the start,
         * end and up vectors.
         */
        private void CalculateEdgeCrossings(int startIndex, int endIndex, LVector3 startPoint, LVector3 endPoint)
        {
            if (startIndex >= numEdges() || endIndex >= numEdges())
            {
                return;
            }

            crossingPlane.set(startPoint, tmp1.set(startPoint).Add(V3_UP), endPoint);

            EdgePoint previousLast = lastPointAdded;

            var       edge = getEdge(endIndex);
            EdgePoint end  = new EdgePoint(endPoint, edge.toNode);

            for (int i = startIndex; i < endIndex; i++)
            {
                edge = getEdge(i);

                if (edge.rightVertex.Equals(startPoint) || edge.leftVertex.Equals(startPoint))
                {
                    previousLast.toNode = edge.toNode;
                    if (!previousLast.connectingEdges.Contains(edge))
                    {
                        previousLast.connectingEdges.Add(edge);
                    }
                }
                else if (edge.leftVertex.Equals(endPoint) || edge.rightVertex.Equals(endPoint))
                {
                    if (!end.connectingEdges.Contains(edge))
                    {
                        end.connectingEdges.Add(edge);
                    }
                }
                else if (IntersectSegmentPlane(edge.leftVertex, edge.rightVertex, crossingPlane, tmp1))
                {
                    if (i != startIndex || i == 0)
                    {
                        lastPointAdded.toNode = edge.fromNode;
                        EdgePoint crossing = new EdgePoint(tmp1, edge.toNode);
                        crossing.connectingEdges.Add(edge);
                        addPoint(crossing);
                    }
                }
            }

            if (endIndex < numEdges() - 1)
            {
                end.connectingEdges.Add(getEdge(endIndex));
            }

            if (!lastPointAdded.Equals(end))
            {
                addPoint(end);
            }
        }
Пример #3
0
        /**
         * Calculate the shortest
         * point path through the path triangles, using the Simple Stupid Funnel
         * Algorithm.
         *
         * @return
         */
        private void CalculateEdgePoints(bool calculateCrossPoint)
        {
            TriangleEdge edge = getEdge(0);

            addPoint(start, edge.fromNode);
            lastPointAdded.fromNode = edge.fromNode;

            Funnel funnel = new Funnel();

            funnel.pivot = (start);               // 起点为漏斗点
            funnel.setPlanes(funnel.pivot, edge); // 设置第一对平面

            int leftIndex   = 0;                  // 左顶点索引
            int rightIndex  = 0;                  // 右顶点索引
            int lastRestart = 0;

            for (int i = 1; i < numEdges(); ++i)
            {
                edge = getEdge(i); // 下一条边

                var leftPlaneLeftDP   = funnel.sideLeftPlane(edge.leftVertex);
                var leftPlaneRightDP  = funnel.sideLeftPlane(edge.rightVertex);
                var rightPlaneLeftDP  = funnel.sideRightPlane(edge.leftVertex);
                var rightPlaneRightDP = funnel.sideRightPlane(edge.rightVertex);

                // 右顶点在右平面里面
                if (rightPlaneRightDP != PlaneSide.Front)
                {
                    // 右顶点在左平面里面
                    if (leftPlaneRightDP != PlaneSide.Front)
                    {
                        // Tighten the funnel. 缩小漏斗
                        funnel.setRightPlane(funnel.pivot, edge.rightVertex);
                        rightIndex = i;
                    }
                    else
                    {
                        // Right over left, insert left to path and restart scan from portal left point.
                        // 右顶点在左平面外面,设置左顶点为漏斗顶点和路径点,从新已该漏斗开始扫描
                        if (calculateCrossPoint)
                        {
                            CalculateEdgeCrossings(lastRestart, leftIndex, funnel.pivot, funnel.leftPortal);
                        }
                        else
                        {
                            vectors.Add(funnel.leftPortal);
                        }

                        funnel.pivot = (funnel.leftPortal);
                        i            = leftIndex;
                        rightIndex   = i;
                        if (i < numEdges() - 1)
                        {
                            lastRestart = i;
                            funnel.setPlanes(funnel.pivot, getEdge(i + 1));
                            continue;
                        }

                        break;
                    }
                }

                // 左顶点在左平面里面
                if (leftPlaneLeftDP != PlaneSide.Front)
                {
                    // 左顶点在右平面里面
                    if (rightPlaneLeftDP != PlaneSide.Front)
                    {
                        // Tighten the funnel.
                        funnel.setLeftPlane(funnel.pivot, edge.leftVertex);
                        leftIndex = i;
                    }
                    else
                    {
                        // Left over right, insert right to path and restart scan from portal right
                        // point.
                        if (calculateCrossPoint)
                        {
                            CalculateEdgeCrossings(lastRestart, rightIndex, funnel.pivot, funnel.rightPortal);
                        }
                        else
                        {
                            vectors.Add(funnel.rightPortal);
                        }

                        funnel.pivot = (funnel.rightPortal);
                        i            = rightIndex;
                        leftIndex    = i;
                        if (i < numEdges() - 1)
                        {
                            lastRestart = i;
                            funnel.setPlanes(funnel.pivot, getEdge(i + 1));
                            continue;
                        }

                        break;
                    }
                }
            }

            if (calculateCrossPoint)
            {
                CalculateEdgeCrossings(lastRestart, numEdges() - 1, funnel.pivot, end);
            }
            else
            {
                vectors.Add(end);
            }

            for (int i = 1; i < pathPoints.Count; i++)
            {
                EdgePoint p = pathPoints.get(i);
                p.fromNode = pathPoints.get(i - 1).toNode;
            }

            return;
        }
Пример #4
0
 private void addPoint(EdgePoint edgePoint)
 {
     vectors.Add(edgePoint.point);
     pathPoints.Add(edgePoint);
     lastPointAdded = edgePoint;
 }