示例#1
0
        public static List <Point3d> RunAstar(Point3d person, Point3d interest, List <Mesh> obstacles)
        {
            //Run solver

            //Declare Lists
            List <A_star>  open       = new List <A_star>();
            List <A_star>  closed     = new List <A_star>();
            List <Point3d> Pointpaths = new List <Point3d>();
            List <Point3d> finalPath  = new List <Point3d>();
            List <A_star>  children   = new List <A_star>();

            //Declare var
            Point3d position;
            Boolean run = true;

            //Movement list
            List <Point3d> move = new List <Point3d>();

            move.Add(new Point3d(0, -1, 0));
            move.Add(new Point3d(0, 1, 0));
            move.Add(new Point3d(-1, 0, 0));
            move.Add(new Point3d(1, 0, 0));
            move.Add(new Point3d(-1, -1, 0));
            move.Add(new Point3d(-1, 1, 0));
            move.Add(new Point3d(1, -1, 0));
            move.Add(new Point3d(1, 1, 0));

            //Declare start and end

            A_star start = new A_star(null, person);
            A_star end   = new A_star(null, interest);


            //Add startpoint to open list
            open.Add(start);

            // Join obstacles in one Mesh
            Mesh obsMesh = new Mesh();

            foreach (Mesh mesh in obstacles)
            {
                obsMesh.Append(mesh);
            }

            //Pathfinding
            while (run)
            {
                //Get current node
                A_star currentNode  = open[0];
                int    currentIndex = 0;

                for (int i = 0; i < open.Count; i++)
                {
                    if (open[i].f < currentNode.f)
                    {
                        currentNode  = open[i];
                        currentIndex = i;
                    }
                }



                //Remove current node from open list and add to closed
                open.RemoveAt(currentIndex);
                closed.Add(currentNode);


                //If at endpoint, traceback the parents
                position = currentNode.Position;
                Pointpaths.Add(position);


                if (currentNode.Position.DistanceTo(interest) < 2)
                {
                    A_star tracer = currentNode;
                    while (tracer.g > 0)
                    {
                        finalPath.Add(tracer.Position);
                        tracer = tracer.Parent;
                    }
                    //Set Outputs
                    finalPath.Add(start.Position);
                    finalPath.Reverse();
                    finalPath.Add(end.Position);
                    return(finalPath);
                }


                //Generate children
                children.Clear();
                foreach (Point3d new_position in move)
                {
                    Point3d child_position = new Point3d(currentNode.Position.X + new_position.X, currentNode.Position.Y + new_position.Y, currentNode.Position.Z + new_position.Z);

                    //Check if new point is in obstacle

                    if (obsMesh.IsPointInside(child_position, 0, false))
                    {
                        continue;
                    }

                    A_star new_node = new A_star(currentNode, child_position);
                    children.Add(new_node);
                }


                //Find best child
                foreach (A_star child in children)
                {
                    Boolean breakout = false;
                    foreach (A_star close in closed)
                    {
                        if (child.Position.DistanceTo(close.Position) < 1)
                        {
                            breakout = true;
                            break;
                        }
                    }

                    if (breakout == true)
                    {
                        continue;
                    }

                    child.g = currentNode.g + 1;
                    child.h = child.Position.DistanceTo(end.Position);
                    child.f = child.g + child.h;

                    for (int i = 0; i < open.Count; i++)
                    {
                        double dist = child.Position.DistanceTo(open[i].Position);
                        if (child.Position.DistanceTo(open[i].Position) < 1)
                        {
                            if (child.g > open[i].g)
                            {
                                breakout = true;
                                break;
                            }
                            else
                            {
                                open.RemoveAt(i);
                            }
                        }
                    }

                    if (breakout == true)
                    {
                        continue;
                    }

                    open.Add(child);


                    if (open.Count > 10000)
                    {
                        run = false;
                    }
                }
            }
            return(finalPath);
        }
示例#2
0
文件: A_Star3D.cs 项目: Esrup/Antflow
        public static Results RunAstar3d(Point3d person, Point3d interest, List <Mesh> obstacles, List <Mesh> Floors, double slope, double Person_Height)
        {
            //Run solver

            //Declare Lists
            List <A_star>  open       = new List <A_star>();
            List <A_star>  closed     = new List <A_star>();
            List <Point3d> Pointpaths = new List <Point3d>();
            List <Point3d> finalPath  = new List <Point3d>();
            List <A_star>  children   = new List <A_star>();
            List <Point3d> closedlist = new List <Point3d>();



            //Declare var
            Point3d position;
            Boolean run = true;

            //Movement list
            List <Point3d> move = new List <Point3d>();

            move.Add(new Point3d(0, -1, 0));
            move.Add(new Point3d(0, 1, 0));
            move.Add(new Point3d(-1, 0, 0));
            move.Add(new Point3d(1, 0, 0));
            move.Add(new Point3d(-1, -1, 0));
            move.Add(new Point3d(-1, 1, 0));
            move.Add(new Point3d(1, -1, 0));
            move.Add(new Point3d(1, 1, 0));


            ////Interest on mesh
            List <Point3d> intList = new List <Point3d>
            {
                interest
            };

            try
            {
                Point3d[] templist = Rhino.Geometry.Intersect.Intersection.ProjectPointsToMeshes(Floors, intList, new Vector3d(0, 0, -1), 0);
                double    mindist  = double.MaxValue;
                Point3d   temp_pos = interest;
                for (int i = 0; i < templist.Length; i++)
                {
                    double dist = templist[i].DistanceTo(interest);
                    if (dist < mindist)
                    {
                        temp_pos = templist[i];
                        mindist  = dist;
                    }
                }
                interest = temp_pos;
            }
            catch (Exception)
            {
                foreach (A_star node in closed)
                {
                    closedlist.Add(node.Position);
                }

                return(new Results {
                    Path = finalPath, Closed = closedlist, Error = "Interest not placed above a floor"
                });;
            }

            ////Start on mesh
            List <Point3d> startList = new List <Point3d>
            {
                person
            };

            try
            {
                Point3d[] templist = Rhino.Geometry.Intersect.Intersection.ProjectPointsToMeshes(Floors, startList, new Vector3d(0, 0, -1), 0);
                double    mindist  = double.MaxValue;
                Point3d   temp_pos = person;
                for (int i = 0; i < templist.Length; i++)
                {
                    double dist = templist[i].DistanceTo(person);
                    if (dist < mindist)
                    {
                        temp_pos = templist[i];
                        mindist  = dist;
                    }
                }
                person = temp_pos;
            }
            catch (Exception)
            {
                foreach (A_star node in closed)
                {
                    closedlist.Add(node.Position);
                }
                return(new Results {
                    Path = finalPath, Closed = closedlist, Error = "Person not placed above a floor"
                });;
            }



            //Declare start and end

            A_star start = new A_star(null, person);
            A_star end   = new A_star(null, interest);


            //Add startpoint to open list
            open.Add(start);

            // Join obstacles in one Mesh
            Mesh obsMesh = new Mesh();

            foreach (Mesh mesh in obstacles)
            {
                obsMesh.Append(mesh);
            }

            //Pathfinding
            while (run)
            {
                //Get current node
                if (open.Count == 0)
                {
                    break;
                }

                A_star currentNode  = open[0];
                int    currentIndex = 0;

                for (int i = 0; i < open.Count; i++)
                {
                    if (open[i].f < currentNode.f)
                    {
                        currentNode  = open[i];
                        currentIndex = i;
                    }
                }



                //Remove current node from open list and add to closed
                open.RemoveAt(currentIndex);
                closed.Add(currentNode);


                //If at endpoint, traceback the parents
                position = currentNode.Position;
                Pointpaths.Add(position);


                if (currentNode.Position.DistanceTo(interest) < 2)
                {
                    //Check for final obstacle
                    Vector3d vectorToFinal = interest - currentNode.Position;
                    Ray3d    ray           = new Ray3d(currentNode.Position, vectorToFinal);
                    double   rayT          = Rhino.Geometry.Intersect.Intersection.MeshRay(obsMesh, ray);
                    if (rayT < 0 | ray.PointAt(rayT).DistanceTo(currentNode.Position) > 2)
                    {
                        A_star tracer = currentNode;
                        while (tracer.g > 0)
                        {
                            finalPath.Add(tracer.Position);
                            tracer = tracer.Parent;
                        }
                        //Set Outputs
                        finalPath.Add(start.Position);
                        finalPath.Reverse();
                        finalPath.Add(end.Position);


                        foreach (A_star node in closed)
                        {
                            closedlist.Add(node.Position);
                        }
                        return(new Results {
                            Path = finalPath, Closed = closedlist
                        });
                    }
                }


                //Generate children
                children.Clear();
                foreach (Point3d new_position in move)
                {
                    List <Point3d> stupidlist = new List <Point3d>();

                    Point3d child_position = new Point3d(currentNode.Position.X + new_position.X, currentNode.Position.Y + new_position.Y, currentNode.Position.Z + new_position.Z);
                    child_position.Z = child_position.Z + Person_Height;
                    stupidlist.Add(child_position);
                    //Project point to terrain
                    //Try z positive
                    try
                    {
                        Point3d[] child_positions = Rhino.Geometry.Intersect.Intersection.ProjectPointsToMeshes(Floors, stupidlist, new Vector3d(0, 0, -1), 0);
                        double    mindist         = double.MaxValue;
                        Point3d   temp_child      = child_position;
                        for (int i = 0; i < child_positions.Length; i++)
                        {
                            double dist = child_positions[i].DistanceTo(child_position);
                            if (dist < mindist)
                            {
                                temp_child = child_positions[i];
                                mindist    = dist;
                            }
                        }
                        child_position = temp_child;
                    }
                    catch (Exception)
                    {
                        continue;
                    }


                    //Check if new point is in obstacle

                    if (obsMesh.IsPointInside(child_position, 0, false))
                    {
                        continue;
                    }

                    //Check if max sloping
                    if (slope != 0)
                    {
                        double terrain_slope = (child_position.Z - currentNode.Position.Z) / Math.Sqrt(Math.Pow(child_position.Y - currentNode.Position.Y, 2) + Math.Pow(child_position.X - currentNode.Position.X, 2));
                        if ((Math.Atan(terrain_slope) * 180 / Math.PI) > slope)
                        {
                            continue;
                        }
                    }
                    A_star new_node = new A_star(currentNode, child_position);
                    children.Add(new_node);
                }


                //Find best child
                foreach (A_star child in children)
                {
                    Boolean breakout = false;
                    foreach (A_star close in closed)
                    {
                        if (child.Position.DistanceTo(close.Position) < 1)
                        {
                            breakout = true;
                            break;
                        }
                    }

                    if (breakout == true)
                    {
                        continue;
                    }

                    double height_dif = child.Position.Z - interest.Z;

                    child.g = currentNode.g + 1;
                    child.h = child.Position.DistanceTo(end.Position) + (Math.Abs(height_dif) * 100);
                    child.f = child.g + child.h;

                    for (int i = 0; i < open.Count; i++)
                    {
                        double dist = child.Position.DistanceTo(open[i].Position);
                        if (child.Position.DistanceTo(open[i].Position) < 1)
                        {
                            if (child.g > open[i].g)
                            {
                                breakout = true;
                                break;
                            }
                            else
                            {
                                open.RemoveAt(i);
                            }
                        }
                    }

                    if (breakout == true)
                    {
                        continue;
                    }

                    open.Add(child);


                    if (open.Count > 100000)
                    {
                        run = false;
                    }
                }
            }

            foreach (A_star node in closed)
            {
                closedlist.Add(node.Position);
            }
            return(new Results {
                Path = finalPath, Closed = closedlist, Error = $"Exceeded count threshold of {open.Count}"
            });
        }
示例#3
0
 public A_star(A_star parent, Point3d position)
 {
     Parent   = parent;
     Position = position;
 }
示例#4
0
        public static List <Point3d> RunAstarTerrain(Point3d person, Point3d interest, List <Mesh> obstacles, List <Mesh> terrain, double slope)
        {
            //Run solver

            //Declare Lists
            List <A_star>  open       = new List <A_star>();
            List <A_star>  closed     = new List <A_star>();
            List <Point3d> Pointpaths = new List <Point3d>();
            List <Point3d> finalPath  = new List <Point3d>();
            List <A_star>  children   = new List <A_star>();



            //Declare var
            Point3d position;
            Boolean run = true;

            //Movement list
            List <Point3d> move = new List <Point3d>();

            move.Add(new Point3d(0, -1, 0));
            move.Add(new Point3d(0, 1, 0));
            move.Add(new Point3d(-1, 0, 0));
            move.Add(new Point3d(1, 0, 0));
            move.Add(new Point3d(-1, -1, 0));
            move.Add(new Point3d(-1, 1, 0));
            move.Add(new Point3d(1, -1, 0));
            move.Add(new Point3d(1, 1, 0));


            //Interest on mesh
            List <Point3d> intList = new List <Point3d>();

            intList.Add(interest);
            try
            {
                interest = Rhino.Geometry.Intersect.Intersection.ProjectPointsToMeshes(terrain, intList, new Vector3d(0, 0, 1), 0)[0];
            }
            catch (Exception)
            {
                try
                {
                    interest = Rhino.Geometry.Intersect.Intersection.ProjectPointsToMeshes(terrain, intList, new Vector3d(0, 0, -1), 0)[0];
                }
                catch (Exception)
                {
                    return(null);
                }
            }



            //Declare start and end

            A_star start = new A_star(null, person);
            A_star end   = new A_star(null, interest);


            //Add startpoint to open list
            open.Add(start);

            // Join obstacles in one Mesh
            Mesh obsMesh = new Mesh();

            foreach (Mesh mesh in obstacles)
            {
                obsMesh.Append(mesh);
            }

            //Pathfinding
            while (run)
            {
                //Get current node
                if (open.Count == 0)
                {
                    break;
                }

                A_star currentNode  = open[0];
                int    currentIndex = 0;

                for (int i = 0; i < open.Count; i++)
                {
                    if (open[i].f < currentNode.f)
                    {
                        currentNode  = open[i];
                        currentIndex = i;
                    }
                }



                //Remove current node from open list and add to closed
                open.RemoveAt(currentIndex);
                closed.Add(currentNode);


                //If at endpoint, traceback the parents
                position = currentNode.Position;
                Pointpaths.Add(position);


                if (currentNode.Position.DistanceTo(interest) < 2)
                {
                    A_star tracer = currentNode;
                    while (tracer.g > 0)
                    {
                        finalPath.Add(tracer.Position);
                        tracer = tracer.Parent;
                    }
                    //Set Outputs
                    finalPath.Add(start.Position);
                    finalPath.Reverse();
                    finalPath.Add(end.Position);
                    return(finalPath);
                }


                //Generate children
                children.Clear();
                foreach (Point3d new_position in move)
                {
                    List <Point3d> stupidlist = new List <Point3d>();

                    Point3d child_position = new Point3d(currentNode.Position.X + new_position.X, currentNode.Position.Y + new_position.Y, currentNode.Position.Z + new_position.Z);
                    stupidlist.Add(child_position);
                    //Project point to terrain
                    //Try z positive
                    try
                    {
                        child_position = Rhino.Geometry.Intersect.Intersection.ProjectPointsToMeshes(terrain, stupidlist, new Vector3d(0, 0, 1), 0)[0];
                    }
                    catch (Exception)
                    {
                        try
                        {
                            //Try z Negative
                            child_position = Rhino.Geometry.Intersect.Intersection.ProjectPointsToMeshes(terrain, stupidlist, new Vector3d(0, 0, -1), 0)[0];
                        }
                        catch (Exception)
                        {
                            continue;
                        }
                    }


                    //Check if new point is in obstacle

                    if (obsMesh.IsPointInside(child_position, 0, false))
                    {
                        continue;
                    }

                    //Check if max sloping
                    if (slope != 0)
                    {
                        double terrain_slope = (child_position.Z - currentNode.Position.Z) / Math.Sqrt(Math.Pow(child_position.Y - currentNode.Position.Y, 2) + Math.Pow(child_position.X - currentNode.Position.X, 2));
                        if (terrain_slope * 100 > slope)
                        {
                            continue;
                        }
                    }
                    A_star new_node = new A_star(currentNode, child_position);
                    children.Add(new_node);
                }


                //Find best child
                foreach (A_star child in children)
                {
                    Boolean breakout = false;
                    foreach (A_star close in closed)
                    {
                        if (child.Position.DistanceTo(close.Position) < 1)
                        {
                            breakout = true;
                            break;
                        }
                    }

                    if (breakout == true)
                    {
                        continue;
                    }

                    child.g = currentNode.g + 1;
                    child.h = child.Position.DistanceTo(end.Position);
                    child.f = child.g + child.h;

                    for (int i = 0; i < open.Count; i++)
                    {
                        double dist = child.Position.DistanceTo(open[i].Position);
                        if (child.Position.DistanceTo(open[i].Position) < 1)
                        {
                            if (child.g > open[i].g)
                            {
                                breakout = true;
                                break;
                            }
                            else
                            {
                                open.RemoveAt(i);
                            }
                        }
                    }

                    if (breakout == true)
                    {
                        continue;
                    }

                    open.Add(child);


                    if (open.Count > 10000)
                    {
                        run = false;
                    }
                }
            }
            return(finalPath);
        }