Пример #1
0
 public int GetDistanceSquared(Point3D point)
 {
     int dx = this.X - point.X;
     int dy = this.Y - point.Y;
     int dz = this.Z - point.Z;
     return (dx * dx) + (dy * dy) + (dz * dz);
 }
Пример #2
0
        /// <summary>
        /// Method that switfly finds the best path from start to end. Doesn't reverse outcome
        /// </summary>
        /// <returns>The end breadcrump where each .next is a step back)</returns>
        private static BreadCrumb FindPathReversed(World world, Point3D start, Point3D end)
        {
            MinHeap<BreadCrumb> openList = new MinHeap<BreadCrumb>(256);
            BreadCrumb[, ,] brWorld = new BreadCrumb[world.Right, world.Top, world.Back];
            BreadCrumb node;
            Point3D tmp;
            int cost;
            int diff;

            int count;

            BreadCrumb current = new BreadCrumb(start);
            current.cost = 0;

            BreadCrumb finish = new BreadCrumb(end);
            if (current.position.X < 0 || current.position.X > brWorld.GetUpperBound(0) || current.position.Y < 0 || current.position.Y > brWorld.GetUpperBound(0)) return null;
            brWorld[current.position.X, current.position.Y, current.position.Z] = current;
            openList.Add(current);

            count = 0;

            while (openList.Count > 0)
            {
                //Find best item and switch it to the 'closedList'
                current = openList.ExtractFirst();
                current.onClosedList = true;

                //Find neighbours
                for (int i = 0; i < surrounding.Length; i++)
                {
                    tmp = current.position + surrounding[i];
                    if (world.PositionIsFree(tmp) || (tmp.X==end.X && tmp.Y==end.Y && tmp.Z==end.Z))
                    {
                        //Check if we've already examined a neighbour, if not create a new node for it.
                        if (brWorld[tmp.X, tmp.Y, tmp.Z] == null)
                        {
                            node = new BreadCrumb(tmp);
                            brWorld[tmp.X, tmp.Y, tmp.Z] = node;
                        }
                        else
                        {
                            node = brWorld[tmp.X, tmp.Y, tmp.Z];
                        }

                        //If the node is not on the 'closedList' check it's new score, keep the best
                        if (!node.onClosedList)
                        {
                            diff = 0;
                            if (current.position.X != node.position.X)
                            {
                                diff += 1;
                            }
                            if (current.position.Y != node.position.Y)
                            {
                                diff += 1;
                            }
                            if (current.position.Z != node.position.Z)
                            {
                                diff += 1;
                            }
                            cost = current.cost + diff + node.position.GetDistanceSquared(end);

                            if (cost < node.cost)
                            {
                                node.cost = cost;
                                node.next = current;
                            }

                            //If the node wasn't on the openList yet, add it
                            if (!node.onOpenList)
                            {
                                //Check to see if we're done
                                if (node.Equals(finish))
                                {
                                    node.next = current;
                                    return node;
                                }
                                node.onOpenList = true;
                                openList.Add(node);
                            }
                        }
                    }
                }

                count++;
                if (count > 1000) return null;
            }
            return null; //no path found
        }
Пример #3
0
 /// <summary>
 /// Method that switfly finds the best path from start to end.
 /// </summary>
 /// <returns>The starting breadcrumb traversable via .next to the end or null if there is no path</returns>        
 public static BreadCrumb FindPath(World world, Point3D start, Point3D end)
 {
     //note we just flip start and end here so you don't have to.
     return FindPathReversed(world, end, start);
 }
Пример #4
0
 /// <summary>
 /// Checks if a position is free or marked (and legal)
 /// </summary>        
 /// <returns>true if the position is free</returns>
 public bool PositionIsFree(Point3D position)
 {
     return position.X >= 0 && position.X < worldBlocked.GetLength(0) &&
         position.Y >= 0 && position.Y < worldBlocked.GetLength(1) &&
         position.Z >= 0 && position.Z < worldBlocked.GetLength(2) &&
         !worldBlocked[position.X, position.Y, position.Z];
 }
Пример #5
0
 /// <summary>
 /// Mark positions in the world als blocked (true) or unblocked (false)
 /// </summary>
 /// <param name="value">use true if you wan't to block the value</param>
 public void MarkPosition(Point3D position, bool value)
 {
     worldBlocked[position.X, position.Y, position.Z] = value;
 }