Ejemplo n.º 1
0
 private void DrawMarkerSquare()
 {
     if (null != spatial_system_.JumpPointFinder)
     {
         int ct = spatial_system_.JumpPointFinder.VisitedCells.Count;
         for (int i = 0; i < ct; ++i)
         {
             CellPos cell = spatial_system_.JumpPointFinder.VisitedCells[i];
             Vector3 pos  = cell_manager_.GetCellCenter(cell.row, cell.col);
             DrawMarker((int)pos.x, (int)(map_height_ - pos.z), (int)cell_width_);
         }
     }
     if (null != triangles)
     {
         for (int i = 0; i < triangles.Count; ++i)
         {
             TriangleNode tri = triangles[i];
             Vector3      pt1 = tri.Points[0];
             Vector3      pt2 = tri.Points[1];
             Vector3      pt3 = tri.Points[2];
             Point        p1  = new Point((int)pt1.x, (int)(map_height_ - pt1.z));
             Point        p2  = new Point((int)pt2.x, (int)(map_height_ - pt2.z));
             Point        p3  = new Point((int)pt3.x, (int)(map_height_ - pt3.z));
             graphics_.Graphics.DrawLine(triangle_pen, p1, p2);
             graphics_.Graphics.DrawLine(triangle_pen, p2, p3);
             graphics_.Graphics.DrawLine(triangle_pen, p3, p1);
             graphics_.Graphics.DrawString((i + 1).ToString(), SystemFonts.DefaultFont, triangle_brush, (int)tri.Position.x, (int)(map_height_ - tri.Position.z));
             //graphics_.Graphics.FillPolygon(triangle_brush, new Point[] { p1, p2, p3, p1 });
         }
     }
 }
Ejemplo n.º 2
0
        // Compare parent and children, if only one is relevant, return it, if both are revelant choose maximum
        public static TriangleNode ChooseSuitableChild(Triangle triangle, int i, int j)
        {
            var          currentParent     = FindParent(triangle, i, j);
            var          currentLeftChild  = FindLeftChild(triangle, i, j);
            var          currentRightChild = FindRightChild(triangle, i, j);
            TriangleNode result            = null;

            if (IsValidTraversal(currentParent, currentLeftChild))
            {
                result = currentLeftChild;
            }

            if (IsValidTraversal(currentParent, currentRightChild))
            {
                if (result != null)
                {
                    result = currentRightChild.NodeSum > currentLeftChild.NodeSum ? currentRightChild : currentLeftChild;
                }
                else
                {
                    result = currentRightChild;
                }
            }
            return(result);
        }
 internal TnsPathNode(TriangleNode node)
 {
     triangle_node   = node;
     prev            = null;
     cost            = 0;
     from_start_cost = 0;
 }
Ejemplo n.º 4
0
    static void Main()
    {
        TriangleNode Root   = LoadTree();
        int          MaxSum = GetMaxSum(Root);

        Console.Write("The best path sums up to {0}. Press enter to exit...", MaxSum);
        Console.Read();
    }
        private float CalcHeuristic(TriangleNode triNode)
        {
            double xDelta = Math.Abs(m_EndNode.Position.x - triNode.Position.x);
            double yDelta = Math.Abs(m_EndNode.Position.z - triNode.Position.z);

            return((float)Math.Sqrt(xDelta * xDelta + yDelta * yDelta));

            //return Math.Abs(m_EndNode.Position.X - triNode.Position.X) + Math.Abs(m_EndNode.Position.Z - triNode.Position.Z);
        }
Ejemplo n.º 6
0
 private static TriangleNode MaxOf(TriangleNode a, TriangleNode b)
 {
     if (null != a && null != b)
     {
         return(a.Value > b.Value ? a : b);
     }
     else
     {
         return(a ?? b);
     }
 }
Ejemplo n.º 7
0
 private static bool IsValidTraversal(TriangleNode parent, TriangleNode child)
 {
     if (parent.NodeValue % 2 == child.NodeValue % 2)
     {
         return(false);
     }
     else
     {
         return(true);
     }
 }
Ejemplo n.º 8
0
 private static TriangleNode[] LineToNodes(string line, TriangleNode[] lineBelow = null)
 {
     int[]          vals     = ParseLine(line);
     TriangleNode[] newNodes = new TriangleNode[vals.Length];
     for (int i = 0; i < vals.Length; i++)
     {
         var left  = lineBelow?[i];
         var right = lineBelow?[i + 1];
         newNodes[i] = new TriangleNode(vals[i], left, right);
     }
     return(newNodes);
 }
Ejemplo n.º 9
0
        /// <summary>
        /// Key part of the algorithm. Flips edges if they need to be flipped,
        /// and recurses.
        ///
        /// pi is the newly inserted point, creating a new triangle ti0.
        /// The adjacent triangle opposite pi in ti0 is ti1. The edge separating
        /// the two triangles is li0 and li1.
        ///
        /// Checks if the (li0, li1) edge needs to be flipped. If it does,
        /// creates two new triangles, and recurses to check if the newly
        /// created triangles need flipping.
        /// <summary>
        void LegalizeEdge(int ti0, int ti1, int pi, int li0, int li1)
        {
            // ti1 might not be a leaf node (ti0 is guaranteed to be, it was
            // just created), so find the current correct leaf.
            ti1 = LeafWithEdge(ti1, li0, li1);

            var t0 = triangles[ti0];
            var t1 = triangles[ti1];
            var qi = t1.OtherPoint(li0, li1);

            if (!LegalEdge(pi, qi, li0, li1))
            {
                var ti2 = triangles.Count;
                var ti3 = ti2 + 1;

                var t2 = new TriangleNode(pi, li0, qi);
                var t3 = new TriangleNode(pi, qi, li1);

                t2.A0 = t1.Opposite(li1);
                t2.A1 = ti3;
                t2.A2 = t0.Opposite(li1);

                t3.A0 = t1.Opposite(li0);
                t3.A1 = t0.Opposite(li0);
                t3.A2 = ti2;

                triangles.Add(t2);
                triangles.Add(t3);

                var nt0 = triangles[ti0];
                var nt1 = triangles[ti1];

                nt0.C0 = ti2;
                nt0.C1 = ti3;

                nt1.C0 = ti2;
                nt1.C1 = ti3;

                triangles[ti0] = nt0;
                triangles[ti1] = nt1;

                if (t2.A0 != -1)
                {
                    LegalizeEdge(ti2, t2.A0, pi, li0, qi);
                }
                if (t3.A0 != -1)
                {
                    LegalizeEdge(ti3, t3.A0, pi, qi, li1);
                }
            }
        }
        private void FindNeighbors(TnsPathNode node)
        {
            m_Neighbors.Clear();
            TriangleNode triNode = node.triangle_node;

            for (int i = 0; i < 3; ++i)
            {
                TriangleNode neighbor = triNode.Neighbors[i];
                if (null != neighbor)
                {
                    m_Neighbors.Add(neighbor);
                }
            }
        }
        private int GetSideIndex(TriangleNode cur, TriangleNode other)
        {
            int ret = -1;

            for (int ix = 0; ix < 3; ++ix)
            {
                if (cur.Neighbors[ix] == other)
                {
                    ret = ix;
                    break;
                }
            }
            return(ret);
        }
Ejemplo n.º 12
0
    // Navigates the tree and returns the maximum sum
    static int GetMaxSum(TriangleNode Curr)
    {
        bool LeafNode = Curr.Left == null && Curr.Right == null;
        bool Memoized = Curr.MaxSum != -1;

        if (LeafNode)
        {
            Curr.MaxSum = Curr.Value;
        }
        else if (!Memoized)
        {
            Curr.MaxSum = Max(GetMaxSum(Curr.Left), GetMaxSum(Curr.Right)) + Curr.Value;
        }
        return(Curr.MaxSum);
    }
        private void IdentifySuccessors(TnsPathNode node)
        {
            FindNeighbors(node);

            int ct = m_Neighbors.Count;

            for (int i = 0; i < ct; ++i)
            {
                TriangleNode triNode = m_Neighbors[i];
                if (m_CloseSet.ContainsKey(triNode))
                {
                    continue;
                }
                Vector3 pos1 = node.triangle_node.Position;
                Vector3 pos2 = triNode.Position;
                float   d    = Geometry.Distance(pos1, pos2);
                float   ng   = node.from_start_cost + d;

                bool        isOpened = true;
                TnsPathNode nextNode;
                if (!m_OpenNodes.TryGetValue(triNode, out nextNode))
                {
                    nextNode = new TnsPathNode(triNode);
                    isOpened = false;
                }
                if (!isOpened || ng < nextNode.from_start_cost)
                {
                    nextNode.from_start_cost = ng;
                    nextNode.cost            = nextNode.from_start_cost + CalcHeuristic(triNode);
                    nextNode.prev            = node;

                    if (!isOpened)
                    {
                        m_OpenQueue.Push(nextNode);
                        m_OpenNodes.Add(triNode, nextNode);
                    }
                    else
                    {
                        int index = m_OpenQueue.IndexOf(nextNode);
                        m_OpenQueue.Update(index, nextNode);
                    }
                }
            }
        }
        private bool IsLineAcross(Vector3 sp, Vector3 ep, TriangleNode start, TriangleNode end)
        {
            bool         across    = true;
            TriangleNode current   = start;
            List <int>   cacheList = new List <int>();

            do
            {
                int acrossIndex = -1;
                do
                {
                    TriangleNode assit = current.Neighbors[0];
                    if (null != assit && !cacheList.Contains(assit.Id) && IntersectLine(sp, ep, assit))
                    {
                        acrossIndex = 0;
                        break;
                    }
                    assit = current.Neighbors[1];
                    if (null != assit && !cacheList.Contains(assit.Id) && IntersectLine(sp, ep, assit))
                    {
                        acrossIndex = 1;
                        break;
                    }
                    assit = current.Neighbors[2];
                    if (null != assit && !cacheList.Contains(assit.Id) && IntersectLine(sp, ep, assit))
                    {
                        acrossIndex = 2;
                        break;
                    }
                } while (false);
                if (acrossIndex < 0)
                {
                    across = false;
                    break;
                }
                else
                {
                    cacheList.Add(current.Id);
                    current = current.Neighbors[acrossIndex];
                }
            } while (current.Id != end.Id);

            return(across);
        }
Ejemplo n.º 15
0
        private static TriangleNode BuildNodeStructure(List <string> data)
        {
            TriangleNode[] lineBelow = LineToNodes(data.Last(), null);
            TriangleNode   topNode   = null;

            for (int lineIdx = data.Count() - 2; lineIdx >= 0; lineIdx--)
            {
                TriangleNode[] thisLine = LineToNodes(data[lineIdx], lineBelow);
                if (lineIdx == 0)
                {
                    topNode = thisLine[0];
                }
                else
                {
                    lineBelow = thisLine;
                }
            }
            return(topNode);
        }
Ejemplo n.º 16
0
            public TriangleNode(int value, TriangleNode child1, TriangleNode child2)
            {
                Value      = value;
                LeftChild  = child1;
                RightChild = child2;

                int maxChildVal = 0;

                if (null != LeftChild && null != RightChild)
                {
                    maxChildVal = LeftChild.MaxPathValue > RightChild.MaxPathValue
                        ? LeftChild.MaxPathValue : RightChild.MaxPathValue;
                }
                else
                {
                    maxChildVal = LeftChild?.MaxPathValue ?? RightChild?.MaxPathValue ?? 0;
                }
                MaxPathValue = Value + maxChildVal;
            }
Ejemplo n.º 17
0
    // Loads the tree from the file in the specified directory
    static TriangleNode LoadTree()
    {
        TriangleNode        Root      = new TriangleNode(-1);
        List <TriangleNode> PrevNodes = new List <TriangleNode>(); // each line of nodes needs to know the line before it

        foreach (string Line in File.ReadAllLines(ProgramDataDir))
        {
            int[] NodeValues = Array.ConvertAll(Line.Split(' '), int.Parse);
            if (Root.Value == -1)
            {
                Root.Value = NodeValues[0];
                PrevNodes.Add(Root);
            }
            else
            {
                List <TriangleNode> TempPrevNodes = new List <TriangleNode>();
                for (int i = 0; i < PrevNodes.Count; i++)
                {
                    TriangleNode ParentNode    = PrevNodes[i];
                    TriangleNode ChildNodeLeft = null;
                    if (i == 0)
                    {
                        ChildNodeLeft = new TriangleNode(NodeValues[0]);
                        TempPrevNodes.Add(ChildNodeLeft);
                    }
                    else
                    {
                        ChildNodeLeft = TempPrevNodes[i];
                    }
                    TriangleNode ChildNodeRight = new TriangleNode(NodeValues[i + 1]);
                    ParentNode.Left  = ChildNodeLeft;
                    ParentNode.Right = ChildNodeRight;
                    TempPrevNodes.Add(ChildNodeRight);
                }
                PrevNodes = TempPrevNodes;
            }
        }
        return(Root);
    }
Ejemplo n.º 18
0
        public void TriangleNode()
        {
            var tc = new TestCore(new Configuration());

            tc.Init();

            var triangle = new TriangleNode()
            {
                Color    = new Color(255, 0, 0),
                Point1   = new Vector2F(100f, 100f),
                Point2   = new Vector2F(200f, 200f),
                Point3   = new Vector2F(100f, 200f),
                Position = new Vector2F(100, 100),
            };

            Engine.AddNode(triangle);

            tc.LoopBody((c) =>
            {
                triangle.Point2 += new Vector2F(1, 1);
            }, null);

            tc.End();
        }
Ejemplo n.º 19
0
        // Populate the triangle structure nodes from text file, return empty array if exception
        public Triangle PopulateFromFile(string fileName)
        {
            if (string.IsNullOrEmpty(fileName))
            {
                throw new ArgumentException("Error! File Name is incorrect.");
            }

            List <TriangleNode[]> nodes = new List <TriangleNode[]>();

            using (StreamReader reader = new StreamReader(fileName))
            {
                var line = "";
                while ((line = reader.ReadLine()) != null)
                {
                    var            valuesList = line.Split(" ");
                    TriangleNode[] lineNodes  = new TriangleNode[valuesList.Length];

                    for (var i = 0; i < valuesList.Length; i++)
                    {
                        try
                        {
                            var numberValue = int.Parse(valuesList[i]);
                            lineNodes[i] = new TriangleNode(numberValue);
                        }
                        catch (FormatException ex)
                        {
                            Console.WriteLine("Error! Wrong file format. Error: " + ex.Message);
                            nodes.Clear();
                            return(new Triangle(nodes.ToArray()));
                        }
                    }
                    nodes.Add(lineNodes);
                }
            }
            return(new Triangle(nodes.ToArray()));
        }
        public bool CanPass(Vector3 start, Vector3 target)
        {
            bool         pass = false;
            TriangleNode stNode = null, edNode = null;

            m_KdTree.Query(start, m_SparseDistance * 2, (float distSqr, TriangleNode node) =>
            {
                if (0 <= Geometry.PointInTriangle(start, node.Points[0], node.Points[1], node.Points[2]))
                {
                    stNode = node;
                    return(false);
                }
                return(true);
            });
            m_KdTree.Query(target, m_SparseDistance * 2, (float distSqr, TriangleNode node) =>
            {
                if (0 <= Geometry.PointInTriangle(target, node.Points[0], node.Points[1], node.Points[2]))
                {
                    edNode = node;
                    return(false);
                }
                return(true);
            });
            if (null != stNode && null != edNode)
            {
                if (stNode == edNode)
                {
                    pass = true;
                }
                else
                {
                    pass = IsLineAcross(start, target, stNode, edNode);
                }
            }
            return(pass);
        }
Ejemplo n.º 21
0
        static void Main(string[] args)
        {
            // Altseed2 を初期化します。
            Engine.Initialize("ShapeNode", 640, 480);

            // 円を描画するノード
            var circle = new CircleNode()
            {
                Color    = new Color(255, 100, 100),
                Radius   = 30f,
                Position = new Vector2F(100f, 300f),
                VertNum  = 30
            };

            // 円弧を描画するノード
            var arc = new ArcNode()
            {
                Color       = new Color(100, 255, 100),
                Radius      = 25f,
                Position    = new Vector2F(300f, 100f),
                StartDegree = 30f,
                EndDegree   = 150f,
                VertNum     = 30
            };

            // 直線を描画するノード
            var line = new LineNode()
            {
                Color     = new Color(100, 100, 255),
                Point1    = new Vector2F(200f, 150f),
                Point2    = new Vector2F(400f, 350f),
                Thickness = 5f
            };

            // 短形を描画するノード
            var rectangle = new RectangleNode()
            {
                Color         = new Color(255, 255, 100),
                Position      = new Vector2F(300f, 400f),
                RectangleSize = new Vector2F(50f, 50f)
            };

            // 三角形を描画するノード
            var triangle = new TriangleNode()
            {
                Color  = new Color(255, 100, 255),
                Point1 = new Vector2F(50f, 50f),
                Point2 = new Vector2F(100f, 50f),
                Point3 = new Vector2F(50f, 100f),
            };

            // エンジンにノードを追加します。
            Engine.AddNode(circle);
            Engine.AddNode(arc);
            Engine.AddNode(line);
            Engine.AddNode(rectangle);
            Engine.AddNode(triangle);

            // メインループ。
            // Altseed のウインドウが閉じられると終了します。
            while (Engine.DoEvents())
            {
                // Altseed を更新します。
                Engine.Update();
            }

            // Altseed の終了処理をします。
            Engine.Terminate();
        }
Ejemplo n.º 22
0
        public static IList <TriangleNode> BuildTriangleFromFile(string sFilePath, bool useFileScale, out float width, out float height)
        {
            width  = 0;
            height = 0;
            string sFileName = sFilePath.Replace("obs", "navmesh");

            if (!File.Exists(sFileName))
            {
                return(null);
            }
            Dictionary <int, TriangleNode> TriangleNodeMap  = new Dictionary <int, TriangleNode>();
            List <TriangleNode>            TriangleNodeList = new List <TriangleNode>();

            try
            {
                using (FileStream fs = new FileStream(sFileName, FileMode.Open))
                {
                    using (BinaryReader binReader = new BinaryReader(fs))
                    {
                        // 读取版本号
                        string fileVersion = new string(binReader.ReadChars(NAVMESH_VERSION.Length));
                        if (fileVersion != NAVMESH_VERSION)
                        {
                            return(null);
                        }
                        width  = binReader.ReadInt32();
                        height = binReader.ReadInt32();
                        float scale = binReader.ReadSingle();
                        if (!useFileScale)
                        {
                            scale = 1.0f;
                        }

                        // 读取导航三角形数量
                        int navCount = binReader.ReadInt32();
                        for (int i = 0; i < navCount; i++)
                        {
                            TriangleNode node = new TriangleNode();
                            node.Read(binReader, scale);
                            TriangleNodeList.Add(node);
                            TriangleNodeMap.Add(node.Id, node);
                        }
                        binReader.Close();
                    }
                    fs.Close();
                }

                //build Neighbors node//顺时针
                int ct = TriangleNodeList.Count;
                for (int i = 0; i < ct; ++i)
                {
                    TriangleNode node    = TriangleNodeList[i];
                    TriangleNode outnode = null;
                    if (TriangleNodeMap.TryGetValue(node.NeighborsId[0], out outnode))
                    {
                        node.Neighbors[2] = outnode;
                    }
                    if (TriangleNodeMap.TryGetValue(node.NeighborsId[1], out outnode))
                    {
                        node.Neighbors[1] = outnode;
                    }
                    if (TriangleNodeMap.TryGetValue(node.NeighborsId[2], out outnode))
                    {
                        node.Neighbors[0] = outnode;
                    }
                    node.CalcGeometryInfo();
                }
            }
            catch
            {
            }
            finally
            {
            }
            if (TriangleNodeList.Count == 0)
            {
                return(null);
            }
            //排序//
            TriangleNodeList.Sort((TriangleNode node1, TriangleNode node2) =>
            {
                if (node1.Position.x < node2.Position.x)
                {
                    return(-1); //左值小于右值,返回-1,为升序,如果返回1,就是降序
                }
                else if (node1.Position.x == node2.Position.x)
                {//左值等于右值,返回0
                    if (node1.Position.z == node2.Position.z)
                    {
                        return(0);
                    }
                    else if (node1.Position.z < node2.Position.z)
                    {
                        return(-1);
                    }
                    else
                    {
                        return(1);
                    }
                }
                else
                {
                    return(1);//左值大于右值,返回1,为升序,如果返回-1,就是降序
                }
            });

            IList <TriangleNode> ret = TriangleNodeList.ToArray();

            TriangleNodeList.Clear();
            TriangleNodeMap.Clear();
            return(ret);
        }
        public List <Vector3> FindPath(Vector3 start, Vector3 target, Vector3[] addToFirst, Vector3[] addToLast)
        {
            m_OpenQueue.Clear();
            m_OpenNodes.Clear();
            m_CloseSet.Clear();

            m_VisitedNodes.Clear();

            TriangleNode stNode = null, edNode = null;

            m_CacheNodesForStart.Clear();
            m_CacheNodesForEnd.Clear();

            m_KdTree.Query(start, m_SparseDistance * 2, (float distSqr, TriangleNode node) =>
            {
                if (0 <= Geometry.PointInTriangle(start, node.Points[0], node.Points[1], node.Points[2]))
                {
                    stNode = node;
                    return(false);
                }
                else
                {
                    m_CacheNodesForStart.Add(node);
                }
                return(true);
            });
            m_KdTree.Query(target, m_SparseDistance * 2, (float distSqr, TriangleNode node) =>
            {
                if (0 <= Geometry.PointInTriangle(target, node.Points[0], node.Points[1], node.Points[2]))
                {
                    edNode = node;
                    return(false);
                }
                else
                {
                    m_CacheNodesForEnd.Add(node);
                }
                return(true);
            });

            Vector3 nearstStartPt  = new Vector3();
            bool    useNearstStart = false;

            if (null == stNode)
            {
                TriangleNode nearstNode    = null;
                float        nearstDistSqr = float.MaxValue;
                for (int ix = 0; ix < m_CacheNodesForStart.Count; ++ix)
                {
                    TriangleNode node = m_CacheNodesForStart[ix];
                    for (int i = 0; i < 3; ++i)
                    {
                        if (node.Neighbors[i] == null)
                        {
                            Vector3 pt;
                            float   dSqr = Geometry.PointToLineSegmentDistanceSquare(start, node.Points[i], node.Points[(i + 1) % 3], out pt);
                            if (dSqr < nearstDistSqr)
                            {
                                nearstNode    = node;
                                nearstDistSqr = dSqr;
                                nearstStartPt = pt;
                            }
                        }
                    }
                }
                if (null != nearstNode)
                {
                    stNode         = nearstNode;
                    useNearstStart = true;
                }
            }
            Vector3 nearstEndPt  = new Vector3();
            bool    useNearstEnd = false;

            if (null == edNode)
            {
                TriangleNode nearstNode    = null;
                float        nearstDistSqr = float.MaxValue;
                for (int ix = 0; ix < m_CacheNodesForEnd.Count; ++ix)
                {
                    TriangleNode node = m_CacheNodesForEnd[ix];
                    for (int i = 0; i < 3; ++i)
                    {
                        if (node.Neighbors[i] == null)
                        {
                            Vector3 pt;
                            float   dSqr = Geometry.PointToLineSegmentDistanceSquare(target, node.Points[i], node.Points[(i + 1) % 3], out pt);
                            if (dSqr < nearstDistSqr)
                            {
                                nearstNode    = node;
                                nearstDistSqr = dSqr;
                                nearstEndPt   = pt;
                            }
                        }
                    }
                }
                if (null != nearstNode)
                {
                    edNode       = nearstNode;
                    useNearstEnd = true;
                }
            }
            if (null == stNode || null == edNode)
            {
                return(new List <Vector3>());
            }
            m_StartNode = stNode;
            m_EndNode   = edNode;
            TnsPathNode startNode = new TnsPathNode(stNode);

            startNode.cost            = 0;
            startNode.from_start_cost = 0;

            m_OpenQueue.Push(startNode);
            m_OpenNodes.Add(startNode.triangle_node, startNode);

            TnsPathNode curNode = null;

            while (m_OpenQueue.Count > 0)
            {
                curNode = m_OpenQueue.Pop();
                if (m_RecordVisitedNodes)
                {
                    m_VisitedNodes.Add(curNode.triangle_node);

                    //LogSystem.Debug("cost:{0} from_start_cost:{1}", curNode.cost, curNode.from_start_cost);
                }
                m_OpenNodes.Remove(curNode.triangle_node);
                m_CloseSet.Add(curNode.triangle_node, true);

                if (curNode.triangle_node == edNode)
                {
                    break;
                }

                IdentifySuccessors(curNode);
            }

            List <Vector3> path = new List <Vector3>();

            if (curNode.triangle_node == edNode)
            {
                //建立路径邻接信息与路点三角形的穿出边信息
                TnsPathNode next = null;
                while (curNode != null)
                {
                    TnsPathNode prev = curNode.prev;
                    if (null != prev)
                    {
                        int ptIx = GetSideIndex(curNode.triangle_node, prev.triangle_node);
                        if (ptIx >= 0)
                        {
                            ptIx = (ptIx + 2) % 3;
                            curNode.new_point_index = ptIx;
                        }
                        if (null != next)
                        {
                            int sideIx = GetSideIndex(curNode.triangle_node, next.triangle_node);
                            if (sideIx == ptIx)
                            {
                                curNode.edge_flags = 0x01;
                            }
                            else
                            {
                                curNode.edge_flags = 0x02;
                            }
                        }
                        prev.next = curNode;
                    }
                    next    = curNode;
                    curNode = prev;
                }
                //拐点法化简与平滑路径
                if (null != addToFirst && addToFirst.Length > 0)
                {
                    path.AddRange(addToFirst);
                }
                path.Add(start);
                Vector3 curPnt;
                Vector3 targetPnt;
                if (useNearstStart)
                {
                    path.Add(nearstStartPt);
                    curPnt = nearstStartPt;
                }
                else
                {
                    curPnt = start;
                }
                if (useNearstEnd)
                {
                    targetPnt = nearstEndPt;
                }
                else
                {
                    targetPnt = target;
                }
                curNode = startNode;
                while (null != curNode)
                {
                    TnsPathNode nextNode;
                    curPnt = GetNextWayPoint(curPnt, targetPnt, curNode, out nextNode);
                    int ct = path.Count;
                    if (Geometry.DistanceSquare(path[ct - 1], curPnt) > m_SparseDistance * m_SparseDistance)
                    {
                        path.Add(curPnt);
                    }
                    curNode = nextNode;
                }
                if (useNearstEnd)
                {
                    path.Add(nearstEndPt);
                }
                /// path.Add(target);
                if (null != addToLast && addToLast.Length > 0)
                {
                    path.AddRange(addToLast);
                }
            }
            return(path);
        }
 private bool IntersectLine(Vector3 start, Vector3 target, TriangleNode node)
 {
     return(null != node && Geometry.IntersectLine(start, target, node.Points[0], node.Points[1], node.Points[2]));
 }
Ejemplo n.º 25
0
        /// <summary>
        /// Run the algorithm
        /// </summary>
        void RunBowyerWatson()
        {
            // For each point, find the containing triangle, split it into three
            // new triangles, call LegalizeEdge on all edges opposite the newly
            // inserted points
            for (int i = 0; i < verts.Count; i++)
            {
                var pi = i;

                if (pi == highest)
                {
                    continue;
                }

                // Index of the containing triangle
                var ti = FindTriangleNode(pi);

                var t = triangles[ti];

                // The points of the containing triangle in CCW order
                var p0 = t.P0;
                var p1 = t.P1;
                var p2 = t.P2;

                // Indices of the newly created triangles.
                var nti0 = triangles.Count;
                var nti1 = nti0 + 1;
                var nti2 = nti0 + 2;

                // The new triangles! All in CCW order
                var nt0 = new TriangleNode(pi, p0, p1);
                var nt1 = new TriangleNode(pi, p1, p2);
                var nt2 = new TriangleNode(pi, p2, p0);


                // Setting the adjacency triangle references.  Only way to make
                // sure you do this right is by drawing the triangles up on a
                // piece of paper.
                nt0.A0 = t.A2;
                nt1.A0 = t.A0;
                nt2.A0 = t.A1;

                nt0.A1 = nti1;
                nt1.A1 = nti2;
                nt2.A1 = nti0;

                nt0.A2 = nti2;
                nt1.A2 = nti0;
                nt2.A2 = nti1;

                // The new triangles are the children of the old one.
                t.C0 = nti0;
                t.C1 = nti1;
                t.C2 = nti2;

                triangles[ti] = t;

                triangles.Add(nt0);
                triangles.Add(nt1);
                triangles.Add(nt2);

                if (nt0.A0 != -1)
                {
                    LegalizeEdge(nti0, nt0.A0, pi, p0, p1);
                }

                if (nt1.A0 != -1)
                {
                    LegalizeEdge(nti1, nt1.A0, pi, p1, p2);
                }

                if (nt2.A0 != -1)
                {
                    LegalizeEdge(nti2, nt2.A0, pi, p2, p0);
                }
            }
        }
Ejemplo n.º 26
0
 private void MergeMemory(TriangleNode parent, TriangleNode child)
 {
     parent.NodeMemory.AddRange(child.NodeMemory);
 }
Ejemplo n.º 27
0
        public void Basic()
        {
            Engine.Initialize("Basic", 800, 600, new Configuration());

            var world = new World(new RectF(-100, -100, 1000, 1000), new Vector2F(0, 10));
            var floor = new RectangleNode();

            floor.RectangleSize = new Vector2F(800, 50);
            Engine.AddNode(floor);

            var floorCollider = new PhysicsRectangleColliderNode(world);

            floorCollider.Position            = new Vector2F(0.0f, 550.0f);
            floorCollider.RectangleSize       = floor.RectangleSize;
            floorCollider.PhysicsColliderType = PhysicsColliderType.Static;
            floorCollider.Restitution         = 0.2f;
            floor.AddChildNode(floorCollider);

            var sprite = new RectangleNode();

            sprite.Position       = new Vector2F(400, 80);
            sprite.RectangleSize  = new Vector2F(50, 50);
            sprite.CenterPosition = sprite.RectangleSize / 2;
            sprite.Angle          = 40;
            Engine.AddNode(sprite);

            var collider = new PhysicsRectangleColliderNode(world);

            collider.CenterPosition      = sprite.CenterPosition;
            collider.Position            = sprite.Position;
            collider.Angle               = sprite.Angle;
            collider.RectangleSize       = sprite.RectangleSize;
            collider.PhysicsColliderType = PhysicsColliderType.Dynamic;
            collider.Restitution         = 0.2f;
            sprite.AddChildNode(collider);

            var text = new TextNode();

            text.Font = Font.LoadDynamicFont("../TestData/Font/mplus-1m-regular.ttf", 50);
            Engine.AddNode(text);

            int count = 0;

            while (Engine.DoEvents())
            {
                if (count % 30 == 0)
                {
                    var sprite1 = new RectangleNode();
                    sprite1.Position       = new Vector2F(400, 80);
                    sprite1.RectangleSize  = new Vector2F(50, 50);
                    sprite1.CenterPosition = sprite1.RectangleSize / 2;
                    sprite1.Angle          = 40;
                    Engine.AddNode(sprite1);

                    var collider1 = new PhysicsRectangleColliderNode(world);
                    collider1.CenterPosition      = sprite1.CenterPosition;
                    collider1.Position            = sprite1.Position;
                    collider1.Angle               = sprite1.Angle;
                    collider1.RectangleSize       = sprite1.RectangleSize;
                    collider1.PhysicsColliderType = PhysicsColliderType.Dynamic;
                    collider1.Restitution         = 1f;
                    sprite1.AddChildNode(collider1);

                    var triangle = new TriangleNode();
                    triangle.Position = new Vector2F(300, 80);
                    triangle.Point1   = new Vector2F(50, 50);
                    triangle.Point2   = new Vector2F(100, 50);
                    triangle.Point3   = new Vector2F(0, 0);
                    triangle.Angle    = 40;
                    Engine.AddNode(triangle);

                    var collider2 = new PhysicsPolygonColliderNode(world);
                    collider2.Position = triangle.Position;
                    collider2.Angle    = triangle.Angle;
                    collider2.AddVertex(new Vector2F(50, 50));
                    collider2.AddVertex(new Vector2F(0, 0));
                    collider2.AddVertex(new Vector2F(100, 50));
                    collider2.PhysicsColliderType = PhysicsColliderType.Dynamic;
                    collider2.Restitution         = 0f;
                    triangle.AddChildNode(collider2);
                }

                if (count++ > 4000)
                {
                    break;
                }
                //text.Text = collider.Velocity.ToString();
                world.Update();
                Engine.Update();
            }

            Engine.Terminate();
        }
Ejemplo n.º 28
0
        static void Main(string[] args)
        {
            Engine.Initialize("Phys2D", 960, 720);


            float             gravity     = 9.8f;
            Vector2F          speed       = new Vector2F(0f, 0f);
            Color             intersect   = new Color(200, 0, 0);
            Color             normalColor = new Color(100, 130, 180);
            List <CircleNode> collision   = new List <CircleNode>();

            //プレイヤー
            var player = new CircleNode();

            player.Radius   = 70;
            player.Position = new Vector2F(300, 100);
            player.Color    = normalColor;
            player.VertNum  = 100;
            //circle.ZOrder = 1;
            Engine.AddNode(player);

            //円の相手
            var other = new CircleNode();

            other.Radius   = 50;
            other.Position = new Vector2F(350, 300);
            other.Color    = normalColor;
            other.VertNum  = 100;
            Engine.AddNode(other);

            var other2 = new CircleNode();

            other2.Radius   = 50;
            other2.Position = new Vector2F(300, 50);
            other2.Color    = normalColor;
            other2.VertNum  = 100;
            Engine.AddNode(other2);

            //三角形の相手
            var triangle = new TriangleNode();

            triangle.Point1 = new Vector2F(400, 100);
            triangle.Point2 = new Vector2F(450, 200);
            triangle.Point3 = new Vector2F(500, 100);
            //triangle.Position = new Vector2F((triangle.Point1.X + triangle.Point2.X + triangle.Point3.X) / 3, (triangle.Point1.Y + triangle.Point2.Y + triangle.Point3.Y) / 3);
            //triangle.Position = new Vector2F((400 + 450 + 500) / 3, (100 + 200 + 100) / 3);
            triangle.Color = normalColor;
            //Engine.AddNode(triangle);

            var line = new RectangleNode();

            line.RectangleSize = new Vector2F(960, 2);
            line.Position      = new Vector2F(0, 400);
            line.Color         = new Color(255, 30, 30);
            Engine.AddNode(line);

            var lineL = new RectangleNode();

            lineL.RectangleSize = new Vector2F(2, 720 - 320);
            lineL.Position      = new Vector2F(200, 0);
            lineL.Color         = new Color(255, 30, 30);
            Engine.AddNode(lineL);

            var lineR = new RectangleNode();

            lineR.RectangleSize = new Vector2F(2, 720 - 320);
            lineR.Position      = new Vector2F(500, 0);
            lineR.Color         = new Color(255, 30, 30);
            Engine.AddNode(lineR);


            collision.Add(player);
            collision.Add(other);
            collision.Add(other2);

            while (Engine.DoEvents())
            {
                Engine.Update();

                //speed += new Vector2F(0f, gravity/100);
                //player.Position += new Vector2F(0f, 1f);
                for (int i = 0; i < collision.Count; i++)
                {
                    for (int j = 0; j < collision.Count; j++)
                    {
                        if (collision[i] == collision[j])
                        {
                            continue;
                        }
                        float length   = (collision[i].Position - collision[j].Position).Length;
                        float distance = collision[i].Radius + collision[j].Radius;
                        if (length <= distance)
                        {
                            //collision[i].Color = intersect;
                            //collision[j].Color = intersect;

                            //簡単な衝突応答
                            var dir = (collision[j].Position - collision[i].Position);
                            //中心間を結んだベクトルの方向に、めり込んだ分だけ移動
                            collision[i].Position -= dir.Normal * (distance - dir.Length);
                            collision[j].Position += dir.Normal * (distance - dir.Length);
                        }
                        else
                        {
                            collision[i].Color = normalColor;
                            collision[j].Color = normalColor;
                        }
                    }

                    CircleNode obj = collision[i];
                    obj.Position += new Vector2F(0f, 2f);
                    if (obj.Position.Y + obj.Radius > 400f)
                    {
                        obj.Position = new Vector2F(obj.Position.X, 400f - obj.Radius);
                    }
                    if (obj.Position.X - obj.Radius < 200f)
                    {
                        obj.Position = new Vector2F(200f + obj.Radius, obj.Position.Y);
                    }
                    if (obj.Position.X + obj.Radius > 500f)
                    {
                        obj.Position = new Vector2F(500f - obj.Radius, obj.Position.Y);
                    }
                }

                //円と三角形の衝突判定
                if ((triangle.Point1 - player.Position).Length < player.Radius ||
                    (triangle.Point2 - player.Position).Length < player.Radius ||
                    (triangle.Point3 - player.Position).Length < player.Radius)
                {
                    player.Color = intersect;
                }

                var AO        = player.Position - triangle.Point1;
                var BO        = player.Position - triangle.Point3;
                var CO        = player.Position - triangle.Point2;
                var AB        = triangle.Point3 - triangle.Point1;
                var BC        = triangle.Point2 - triangle.Point3;
                var CA        = triangle.Point1 - triangle.Point2;
                var center    = new Vector2F((triangle.Point1.X + triangle.Point2.X + triangle.Point3.X) / 3, (triangle.Point1.Y + triangle.Point2.Y + triangle.Point3.Y) / 3);
                var direction = center - player.Position;
                var dist1     = Math.Abs(Vector2F.Cross(AO, AB)) / AB.Length;
                var dist2     = Math.Abs(Vector2F.Cross(BO, BC)) / BC.Length;
                var dist3     = Math.Abs(Vector2F.Cross(CO, CA)) / CA.Length;
                if (dist1 <= player.Radius && Dot(AO, AB) * Dot(BO, AB) <= 0)
                {
                    player.Color     = intersect;
                    triangle.Point1 += direction.Normal * (player.Radius - dist1);
                    triangle.Point2 += direction.Normal * (player.Radius - dist1);
                    triangle.Point3 += direction.Normal * (player.Radius - dist1);
                }
                if (dist2 <= player.Radius && Dot(BO, BC) * Dot(CO, BC) <= 0)
                {
                    player.Color     = intersect;
                    triangle.Point1 += direction.Normal * (player.Radius - dist2);
                    triangle.Point2 += direction.Normal * (player.Radius - dist2);
                    triangle.Point3 += direction.Normal * (player.Radius - dist2);
                }
                if (dist3 <= player.Radius && Dot(CO, CA) * Dot(AO, CA) <= 0)
                {
                    player.Color     = intersect;
                    triangle.Point1 += direction.Normal * (player.Radius - dist3);
                    triangle.Point2 += direction.Normal * (player.Radius - dist3);
                    triangle.Point3 += direction.Normal * (player.Radius - dist3);
                }
                if (triangle.Point1.Y < 400 || triangle.Point2.Y < 400 || triangle.Point2.Y < 400)
                {
                    triangle.Point1 += new Vector2F(0f, 1f);
                    triangle.Point2 += new Vector2F(0f, 1f);
                    triangle.Point3 += new Vector2F(0f, 1f);
                }

                //if (Vector2F.Cross(AO, AB) <= 0 && Vector2F.Cross(BO, BC) <= 0 && Vector2F.Cross(CO, CA) <= 0) player.Color = intersect;

                // 移動
                if (Engine.Keyboard.GetKeyState(Key.Right) == ButtonState.Hold)
                {
                    player.Position += new Vector2F(5f, 0);
                }
                if (Engine.Keyboard.GetKeyState(Key.Left) == ButtonState.Hold)
                {
                    player.Position += new Vector2F(-5f, 0);
                }
                if (Engine.Keyboard.GetKeyState(Key.Up) == ButtonState.Hold)
                {
                    player.Position += new Vector2F(0, -5f);
                }
                if (Engine.Keyboard.GetKeyState(Key.Down) == ButtonState.Hold)
                {
                    player.Position += new Vector2F(0, 5f);
                }

                //終了
                if (Engine.Keyboard.GetKeyState(Key.Escape) == ButtonState.Push)
                {
                    break;
                }
            }

            Engine.Terminate();
        }
Ejemplo n.º 29
0
        /// <summary>
        /// Key part of the algorithm. Flips edges if they need to be flipped,
        /// and recurses.
        ///
        /// pi is the newly inserted point, creating a new triangle ti0.
        /// The adjacent triangle opposite pi in ti0 is ti1. The edge separating
        /// the two triangles is li0 and li1.
        ///
        /// Checks if the (li0, li1) edge needs to be flipped. If it does,
        /// creates two new triangles, and recurses to check if the newly
        /// created triangles need flipping.
        /// <summary>
        void LegalizeEdge(int ti0, int ti1, int pi, int li0, int li1)
        {
            // ti1 might not be a leaf node (ti0 is guaranteed to be, it was
            // just created), so find the current correct leaf.
            ti1 = LeafWithEdge(ti1, li0, li1);

            var t0 = triangles[ti0];
            var t1 = triangles[ti1];
            var qi = t1.OtherPoint(li0, li1);

            Debug.Assert(t0.HasEdge(li0, li1));
            Debug.Assert(t1.HasEdge(li0, li1));
            Debug.Assert(t0.IsLeaf);
            Debug.Assert(t1.IsLeaf);
            Debug.Assert(t0.P0 == pi || t0.P1 == pi || t0.P2 == pi);
            Debug.Assert(t1.P0 == qi || t1.P1 == qi || t1.P2 == qi);


            //var p = points[pi];
            //var q = points[qi];
            //var l0 = points[li0];
            //var l1 = points[li1];

            if (!LegalEdge(pi, qi, li0, li1))
            {
                var ti2 = triangles.Count;
                var ti3 = ti2 + 1;

                var t2 = new TriangleNode(pi, li0, qi);
                var t3 = new TriangleNode(pi, qi, li1);

                t2.A0 = t1.Opposite(li1);
                t2.A1 = ti3;
                t2.A2 = t0.Opposite(li1);

                t3.A0 = t1.Opposite(li0);
                t3.A1 = t0.Opposite(li0);
                t3.A2 = ti2;

                triangles.Add(t2);
                triangles.Add(t3);

                var nt0 = triangles[ti0];
                var nt1 = triangles[ti1];

                nt0.C0 = ti2;
                nt0.C1 = ti3;

                nt1.C0 = ti2;
                nt1.C1 = ti3;

                triangles[ti0] = nt0;
                triangles[ti1] = nt1;

                if (t2.A0 != -1)
                {
                    LegalizeEdge(ti2, t2.A0, pi, li0, qi);
                }
                if (t3.A0 != -1)
                {
                    LegalizeEdge(ti3, t3.A0, pi, qi, li1);
                }
            }
        }