Beispiel #1
0
        public Path CreatePath(Spot from, Spot to, Location realDst,
                               float minHowClose, bool AllowInvented,
                               ILocationHeuristics locationHeuristics)
        {
            Spot newTo = search(from, to, realDst, minHowClose, AllowInvented,
                                locationHeuristics);

            if (newTo != null)
            {
                if (newTo.GetDistanceTo(to) <= minHowClose)
                {
                    List <Spot> path = FollowTraceBack(from, newTo);
                    return(new Path(path));
                }
            }
            return(null);
        }
        public List <Spot> GetAllSpots()
        {
            List <Spot> l = new List <Spot>();

            for (int x = 0; x < CHUNK_SIZE; x++)
            {
                for (int y = 0; y < CHUNK_SIZE; y++)
                {
                    Spot s = spots[x, y];
                    while (s != null)
                    {
                        l.Add(s);
                        s = s.next;
                    }
                }
            }
            return(l);
        }
Beispiel #3
0
        private void ScoreSpot(Spot spotLinkedToCurrent, Spot destinationSpot, int currentSearchID, ILocationHeuristics locationHeuristics, PriorityQueue <Spot, float> prioritySpotQueue)
        {
            switch (searchScoreSpot)
            {
            case eSearchScoreSpot.A_Star:
                ScoreSpot_A_Star(spotLinkedToCurrent, destinationSpot, currentSearchID, locationHeuristics, prioritySpotQueue);
                break;

            case eSearchScoreSpot.A_Star_With_Model_Avoidance:
                ScoreSpot_A_Star_With_Model_And_Gradient_Avoidance(spotLinkedToCurrent, destinationSpot, currentSearchID, locationHeuristics, prioritySpotQueue);
                break;

            case eSearchScoreSpot.OriginalPather:
            default:
                ScoreSpot_Pather(spotLinkedToCurrent, destinationSpot, currentSearchID, locationHeuristics, prioritySpotQueue);
                break;
            }
        }
        // return old spot at conflicting poision
        // or the same as passed the function if all was ok
        public Spot AddSpot(Spot s)
        {
            Spot old = GetSpot(s.X, s.Y, s.Z);

            if (old != null)
            {
                return(old);
            }
            int x, y;

            s.chunk = this;

            LocalCoords(s.X, s.Y, out x, out y);

            s.next      = spots[x, y];
            spots[x, y] = s;
            modified    = true;
            return(s);
        }
Beispiel #5
0
        private Path CreatePath(Spot from, Spot to, float minHowClose, ILocationHeuristics locationHeuristics)
        {
            Spot newTo = Search(from, to, minHowClose, locationHeuristics);

            if (newTo != null)
            {
                if (newTo.GetDistanceTo(to) <= MaximumAllowedRangeFromTarget)
                {
                    List <Spot> path = FollowTraceBack(from, newTo);
                    LastPath = new Path(path);
                    return(LastPath);
                }
                else
                {
                    logger.WriteLine($"Closest spot is too far from target. {newTo.GetDistanceTo(to)}>{MaximumAllowedRangeFromTarget}");
                    return(null);
                }
            }
            return(null);
        }
Beispiel #6
0
        public void ScoreSpot_A_Star(Spot spotLinkedToCurrent, Spot destinationSpot, int currentSearchID, ILocationHeuristics locationHeuristics, PriorityQueue <Spot, float> prioritySpotQueue)
        {
            //score spot
            float G_Score = currentSearchSpot.traceBackDistance + currentSearchSpot.GetDistanceTo(spotLinkedToCurrent); //  the movement cost to move from the starting point A to a given square on the grid, following the path generated to get there.
            float H_Score = spotLinkedToCurrent.GetDistanceTo2D(destinationSpot) * heuristicsFactor;                    // the estimated movement cost to move from that given square on the grid to the final destination, point B. This is often referred to as the heuristic, which can be a bit confusing. The reason why it is called that is because it is a guess. We really don�t know the actual distance until we find the path, because all sorts of things can be in the way (walls, water, etc.). You are given one way to calculate H in this tutorial, but there are many others that you can find in other articles on the web.
            float F_Score = G_Score + H_Score;

            if (spotLinkedToCurrent.IsFlagSet(Spot.FLAG_WATER))
            {
                F_Score += 30;
            }

            if (!spotLinkedToCurrent.SearchScoreIsSet(currentSearchID) || F_Score < spotLinkedToCurrent.SearchScoreGet(currentSearchID))
            {
                // shorter path to here found
                spotLinkedToCurrent.traceBack         = currentSearchSpot;
                spotLinkedToCurrent.traceBackDistance = G_Score;
                spotLinkedToCurrent.SearchScoreSet(currentSearchID, F_Score);
                prioritySpotQueue.Enqueue(spotLinkedToCurrent, -F_Score);
            }
        }
Beispiel #7
0
        public List <Spot> FindAllSpots(Location l, float max_d)
        {
            List <Spot> sl = new List <Spot>();

            int d = 0;

            while ((float)d <= max_d + 0.1f)
            {
                for (int i = -d; i <= d; i++)
                {
                    float x_up = l.X + (float)d;
                    float x_dn = l.X - (float)d;
                    float y_up = l.Y + (float)d;
                    float y_dn = l.Y - (float)d;

                    Spot s0 = GetSpot2D(x_up, l.Y + i);
                    Spot s2 = GetSpot2D(x_dn, l.Y + i);

                    Spot   s1 = GetSpot2D(l.X + i, y_dn);
                    Spot   s3 = GetSpot2D(l.X + i, y_up);
                    Spot[] sv = { s0, s1, s2, s3 };
                    foreach (Spot s in sv)
                    {
                        Spot ss = s;
                        while (ss != null)
                        {
                            float di = ss.GetDistanceTo(l);
                            if (di < max_d)
                            {
                                sl.Add(ss);
                            }
                            ss = ss.next;
                        }
                    }
                }
                d++;
            }
            return(sl);
        }
Beispiel #8
0
        public void MarkBlockedAt(Location loc)
        {
            Spot s = new Spot(loc);

            s = AddSpot(s);
            s.SetFlag(Spot.FLAG_BLOCKED, true);
            // Find all paths leading though this one

            List <Spot> sl = FindAllSpots(loc, 5.0f);

            foreach (Spot sp in sl)
            {
                List <Location> paths = sp.GetPaths();
                foreach (Location to in paths)
                {
                    if (LineCrosses(sp.location, to, loc))
                    {
                        sp.RemovePathTo(to);
                    }
                }
            }
        }
Beispiel #9
0
        private Spot Search(Spot fromSpot, Spot destinationSpot, float minHowClose, ILocationHeuristics locationHeuristics)
        {
            currentSearchStartSpot = fromSpot;
            searchID++;
            int currentSearchID = searchID;
            //searchProgress = new SearchProgress(fromSpot, destinationSpot, searchID);

            // lowest first queue
            PriorityQueue <Spot, float> prioritySpotQueue = new PriorityQueue <Spot, float>(); // (new SpotSearchComparer(dst, score)); ;

            prioritySpotQueue.Enqueue(fromSpot, -fromSpot.GetDistanceTo(destinationSpot) * heuristicsFactor);

            fromSpot.SearchScoreSet(currentSearchID, 0.0f);
            fromSpot.traceBack         = null;
            fromSpot.traceBackDistance = 0;

            // A* -ish algorithm
            while (prioritySpotQueue.Count != 0 && SearchEnabled)
            {
                if (sleepMSBetweenSpots != 0)
                {
                    Thread.Sleep(sleepMSBetweenSpots);
                }                                                                    // slow down the pathing

                float prio;
                currentSearchSpot = prioritySpotQueue.Dequeue(out prio); // .Value;

                // force the world to be loaded
                TriangleCollection tc = triangleWorld.GetChunkAt(currentSearchSpot.X, currentSearchSpot.Y);

                if (currentSearchSpot.SearchIsClosed(currentSearchID))
                {
                    continue;
                }
                currentSearchSpot.SearchClose(currentSearchID);

                //update status
                //if (!searchProgress.CheckProgress(currentSearchSpot)) { break; }

                // are we there?
                if (currentSearchSpot.location.GetDistanceTo(destinationSpot.location) <= minHowClose)
                {
                    return(currentSearchSpot); // got there
                }

                //Find spots to link to
                CreateSpotsAroundSpot(currentSearchSpot);

                //score each spot around the current search spot and add them to the queue
                foreach (Spot spotLinkedToCurrent in currentSearchSpot.GetPathsToSpots(this))
                {
                    if (spotLinkedToCurrent != null && !spotLinkedToCurrent.IsBlocked() && !spotLinkedToCurrent.SearchIsClosed(currentSearchID))
                    {
                        ScoreSpot(spotLinkedToCurrent, destinationSpot, currentSearchID, locationHeuristics, prioritySpotQueue);
                    }
                }
            }

            //we ran out of spots to search
            //searchProgress.LogStatus("  search failed. ");

            return(null); // :(
        }
Beispiel #10
0
 public bool IsUnderwaterOrInAir(Spot s)
 {
     return(IsUnderwaterOrInAir(s.GetLocation()));
 }
Beispiel #11
0
        public void CreateSpotsAroundSpot(Spot currentSearchSpot, bool mapped)
        {
            if (!mapped)
            {
                //mark as mapped
                currentSearchSpot.SetFlag(Spot.FLAG_MPQ_MAPPED, true);

                float PI = (float)Math.PI;

                //loop through the spots in a circle around the current search spot
                for (float radianAngle = 0; radianAngle < PI * 2; radianAngle += PI / 8)
                {
                    //calculate the location of the spot at the angle
                    float nx = currentSearchSpot.X + (float)Math.Sin(radianAngle) * WantedStepLength; // *0.8f;
                    float ny = currentSearchSpot.Y + (float)Math.Cos(radianAngle) * WantedStepLength; // *0.8f;

                    PeekSpot = new Spot(nx, ny, currentSearchSpot.Z);

                    //find the spot at this location, stop if there is one already
                    if (GetSpot(nx, ny, currentSearchSpot.Z) != null)
                    {
                        continue;
                    }                                                               //found a spot so don't create a new one

                    //see if there is a close spot, stop if there is
                    if (FindClosestSpot(new Location(nx, ny, currentSearchSpot.Z), MinStepLength) != null)
                    {
                        continue;
                    } // TODO: this is slow

                    // check we can stand at this new location
                    float new_z;
                    int   flags;
                    if (!triangleWorld.FindStandableAt(nx, ny, currentSearchSpot.Z - WantedStepLength * .75f, currentSearchSpot.Z + WantedStepLength * .75f, out new_z, out flags, toonHeight, toonSize))
                    {
                        continue;
                    }

                    //see if a spot already exists at this location
                    if (FindClosestSpot(new Location(nx, ny, new_z), MinStepLength) != null)
                    {
                        continue;
                    }

                    //if the step is blocked then stop
                    if (triangleWorld.IsStepBlocked(currentSearchSpot.X, currentSearchSpot.Y, currentSearchSpot.Z, nx, ny, new_z, toonHeight, toonSize, null))
                    {
                        continue;
                    }

                    //create a new spot and connect it
                    Spot newSpot = AddAndConnectSpot(new Spot(nx, ny, new_z));
                    //PeekSpot = newSpot;

                    //check flags return by triangleWorld.FindStandableA
                    if ((flags & ChunkedTriangleCollection.TriangleFlagDeepWater) != 0)
                    {
                        newSpot.SetFlag(Spot.FLAG_WATER, true);
                    }
                    if (((flags & ChunkedTriangleCollection.TriangleFlagModel) != 0) || ((flags & ChunkedTriangleCollection.TriangleFlagObject) != 0))
                    {
                        newSpot.SetFlag(Spot.FLAG_INDOORS, true);
                    }
                    if (triangleWorld.IsCloseToModel(newSpot.X, newSpot.Y, newSpot.Z, IsCloseToModelRange))
                    {
                        newSpot.SetFlag(Spot.FLAG_CLOSETOMODEL, true);
                    }
                }
            }
        }
Beispiel #12
0
 public void CreateSpotsAroundSpot(Spot currentSearchSpot)
 {
     CreateSpotsAroundSpot(currentSearchSpot, currentSearchSpot.IsFlagSet(Spot.FLAG_MPQ_MAPPED));
 }
Beispiel #13
0
 public void AddPathTo(Spot s)
 {
     AddPathTo(s.X, s.Y, s.Z);
 }
Beispiel #14
0
 public bool IsSet(Spot s)
 {
     return(data.ContainsKey(s));
 }
Beispiel #15
0
        private Spot Search(Spot fromSpot, Spot destinationSpot, float minHowClose, ILocationHeuristics locationHeuristics)
        {
            var searchDuration = new Stopwatch();

            searchDuration.Start();
            var timeSinceProgress = new Stopwatch();

            timeSinceProgress.Start();

            var closest = 99999f;

            ClosestSpot = null;

            currentSearchStartSpot = fromSpot;
            searchID++;
            int currentSearchID = searchID;
            //searchProgress = new SearchProgress(fromSpot, destinationSpot, searchID);

            // lowest first queue
            PriorityQueue <Spot, float> prioritySpotQueue = new PriorityQueue <Spot, float>(); // (new SpotSearchComparer(dst, score)); ;

            prioritySpotQueue.Enqueue(fromSpot, -fromSpot.GetDistanceTo(destinationSpot) * heuristicsFactor);

            fromSpot.SearchScoreSet(currentSearchID, 0.0f);
            fromSpot.traceBack         = null;
            fromSpot.traceBackDistance = 0;

            // A* -ish algorithm
            while (prioritySpotQueue.Count != 0 && SearchEnabled)
            {
                if (sleepMSBetweenSpots != 0)
                {
                    Thread.Sleep(sleepMSBetweenSpots);
                }                                                                    // slow down the pathing

                float prio;
                currentSearchSpot = prioritySpotQueue.Dequeue(out prio); // .Value;

                // force the world to be loaded
                TriangleCollection tc = triangleWorld.GetChunkAt(currentSearchSpot.X, currentSearchSpot.Y);

                if (currentSearchSpot.SearchIsClosed(currentSearchID))
                {
                    continue;
                }
                currentSearchSpot.SearchClose(currentSearchID);

                //update status
                //if (!searchProgress.CheckProgress(currentSearchSpot)) { break; }

                // are we there?

                var distance = currentSearchSpot.location.GetDistanceTo(destinationSpot.location);

                if (distance <= minHowClose)
                {
                    return(currentSearchSpot); // got there
                }

                if (distance < closest)
                {
                    // spamming as hell
                    //logger.WriteLine($"Closet spot is {distance} from the target");
                    closest     = distance;
                    ClosestSpot = currentSearchSpot;
                    PeekSpot    = ClosestSpot;
                    timeSinceProgress.Reset();
                    timeSinceProgress.Start();
                }

                if (timeSinceProgress.Elapsed.TotalSeconds > ProgressTimeoutSeconds || searchDuration.Elapsed.TotalSeconds > TimeoutSeconds)
                {
                    logger.WriteLine("search failed, 10 seconds since last progress, returning the closest spot.");
                    return(ClosestSpot);
                }

                //Find spots to link to
                CreateSpotsAroundSpot(currentSearchSpot);

                //score each spot around the current search spot and add them to the queue
                foreach (Spot spotLinkedToCurrent in currentSearchSpot.GetPathsToSpots(this))
                {
                    if (spotLinkedToCurrent != null && !spotLinkedToCurrent.IsBlocked() && !spotLinkedToCurrent.SearchIsClosed(currentSearchID))
                    {
                        ScoreSpot(spotLinkedToCurrent, destinationSpot, currentSearchID, locationHeuristics, prioritySpotQueue);
                    }
                }
            }

            //we ran out of spots to search
            //searchProgress.LogStatus("  search failed. ");

            if (ClosestSpot != null && closest < MaximumAllowedRangeFromTarget)
            {
                logger.WriteLine("search failed, returning the closest spot.");
                return(ClosestSpot);
            }
            return(null);
        }
Beispiel #16
0
 public float GetDistanceTo2D(Spot s)
 {
     return((float)Math.Sqrt((Math.Pow((double)(this.X - s.X), 2.0) + Math.Pow((double)(this.Y - s.Y), 2.0))));
 }
        // Per spot:
        // uint32 magic
        // uint32 reserved;
        // uint32 flags;
        // float x;
        // float y;
        // float z;
        // uint32 no_paths
        //   for each path
        //     float x;
        //     float y;
        //     float z;

        public bool Load(string baseDir)
        {
            string fileName    = FileName();
            string filenamebin = baseDir + fileName;

            System.IO.Stream       stream = null;
            System.IO.BinaryReader file   = null;
            int n_spots = 0;
            int n_steps = 0;

            try
            {
                stream = System.IO.File.OpenRead(filenamebin);
                if (stream != null)
                {
                    file = new System.IO.BinaryReader(stream);
                    if (file != null)
                    {
                        uint magic = file.ReadUInt32();
                        if (magic == FILE_MAGIC)
                        {
                            uint type;
                            while ((type = file.ReadUInt32()) != FILE_ENDMAGIC)
                            {
                                n_spots++;
                                uint  reserved = file.ReadUInt32();
                                uint  flags    = file.ReadUInt32();
                                float x        = file.ReadSingle();
                                float y        = file.ReadSingle();
                                float z        = file.ReadSingle();
                                uint  n_paths  = file.ReadUInt32();
                                if (x != 0 && y != 0)
                                {
                                    Spot s = new Spot(x, y, z);
                                    s.flags = flags;

                                    for (uint i = 0; i < n_paths; i++)
                                    {
                                        n_steps++;
                                        float sx = file.ReadSingle();
                                        float sy = file.ReadSingle();
                                        float sz = file.ReadSingle();
                                        s.AddPathTo(sx, sy, sz);
                                    }
                                    AddSpot(s);
                                }
                            }
                        }
                    }
                }
            }
            catch (System.IO.FileNotFoundException e)
            {
                logger.Debug(e.Message);
            }
            catch (System.IO.DirectoryNotFoundException e)
            {
                logger.Debug(e.Message);
            }
            catch (Exception e)
            {
                logger.Debug(e.Message);
            }

            if (file != null)
            {
                file.Close();
            }
            if (stream != null)
            {
                stream.Close();
            }

            Log("Loaded " + fileName + " " + n_spots + " spots " + n_steps + " steps");

            modified = false;
            return(false);
        }
Beispiel #18
0
        // return null if failed or the last spot in the path found

        private Spot search(Spot src, Spot dst,
                            Location realDst,
                            float minHowClose, bool AllowInvented,
                            ILocationHeuristics locationHeuristics)
        {
            searchID++;
            int      count            = 0;
            int      prevCount        = 0;
            int      currentSearchID  = searchID;
            float    heuristicsFactor = 1.3f;
            DateTime pre      = DateTime.Now;
            DateTime lastSpam = pre;

            // lowest first queue
            var q = new PriorityQueue <Spot, float>(); // (new SpotSearchComparer(dst, score)); ;

            q.Enqueue(src, -src.GetDistanceTo(dst) * heuristicsFactor);
            Spot BestSpot = null;

            //Set<Spot> closed      = new Set<Spot>();
            //SpotData<float> score = new SpotData<float>();

            src.SearchScoreSet(currentSearchID, 0.0f);
            src.traceBack = null;

            // A* -ish algorithm

            while (q.Count != 0) // && count < 100000)
            {
                float prio;
                Spot  spot = q.Dequeue(out prio); // .Value;
                //q.Remove(spot);


                if (spot.SearchIsClosed(currentSearchID))
                {
                    continue;
                }
                spot.SearchClose(currentSearchID);

                if (count % 100 == 0)
                {
                    TimeSpan span = DateTime.Now.Subtract(lastSpam);
                    if (span.Seconds != 0 && BestSpot != null)
                    {
                        Thread.Sleep(50); // give glider a chance to stop us
                        int t = span.Seconds * 1000 + span.Milliseconds;
                        if (t == 0)
                        {
                            Log("searching.... " + (count + 1) + " d: " + BestSpot.location.GetDistanceTo(realDst));
                        }
                        else
                        {
                            Log("searching.... " + (count + 1) + " d: " + BestSpot.location.GetDistanceTo(realDst) + " " +
                                (count - prevCount) * 1000 / t + " steps/s");
                        }
                        lastSpam  = DateTime.Now;
                        prevCount = count;
                    }
                }
                count++;


                if (spot.Equals(dst) || spot.location.GetDistanceTo(realDst) <= minHowClose)
                {
                    TimeSpan ts = DateTime.Now.Subtract(pre);
                    int      t  = ts.Seconds * 1000 + ts.Milliseconds;

                    /*if(t == 0)
                     *  Log("  search found the way there. " + count);
                     * else
                     *  Log("  search found the way there. " + count + " " + (count * 1000) / t + " steps/s");
                     */
                    return(spot); // got there
                }

                if (BestSpot == null ||
                    spot.location.GetDistanceTo(realDst) < BestSpot.location.GetDistanceTo(realDst))
                {
                    BestSpot = spot;
                }
                {
                    TimeSpan ts = DateTime.Now.Subtract(pre);
                    if (ts.Seconds > 15)
                    {
                        Log("too long search, aborting");
                        break;
                    }
                }

                float src_score = spot.SearchScoreGet(currentSearchID);

                //PathGraph.Log("inspect: " + c + " score " + s);


                int         new_found = 0;
                List <Spot> ll        = spot.GetPathsToSpots(this);
                foreach (Spot to in ll)
                {
                    //Spot to = GetSpot(l);

                    if (to != null && !to.IsBlocked() && !to.SearchIsClosed(currentSearchID))
                    {
                        float old_score = 1E30f;

                        float new_score = src_score + spot.GetDistanceTo(to) + TurnCost(spot, to);
                        if (locationHeuristics != null)
                        {
                            new_score += locationHeuristics.Score(spot.X, spot.Y, spot.Z);
                        }
                        if (to.GetFlag(Spot.FLAG_WATER))
                        {
                            new_score += 30;
                        }

                        if (to.SearchScoreIsSet(currentSearchID))
                        {
                            old_score = to.SearchScoreGet(currentSearchID);
                        }

                        if (new_score < old_score)
                        {
                            // shorter path to here found
                            to.traceBack = spot;
                            //if (q.Contains(to))
                            //   q.Remove(to); // very sloppy to not dequeue it
                            to.SearchScoreSet(currentSearchID, new_score);
                            q.Enqueue(to, -(new_score + to.GetDistanceTo(dst) * heuristicsFactor));
                            new_found++;
                        }
                    }
                }

                //hmm search the triangles :p
                if (!spot.GetFlag(Spot.FLAG_MPQ_MAPPED))
                {
                    var PI = (float)Math.PI;

                    spot.SetFlag(Spot.FLAG_MPQ_MAPPED, true);
                    for (float a = 0; a < PI * 2; a += PI / 8)
                    {
                        float nx = spot.X + (float)Math.Sin(a) * WantedStepLength; // *0.8f;
                        float ny = spot.Y + (float)Math.Cos(a) * WantedStepLength; // *0.8f;
                        Spot  s  = GetSpot(nx, ny, spot.Z);
                        if (s == null)
                        {
                            s = FindClosestSpot(new Location(nx, ny, spot.Z), MinStepLength); // TODO: this is slow
                        }
                        if (s != null)
                        {
                            // hmm, they should already be connected
                        }
                        else
                        {
                            float new_z;
                            int   flags;
                            // gogo find a new one
                            //PathGraph.Log("gogo brave new world");
                            if (!triangleWorld.FindStandableAt(nx, ny,
                                                               spot.Z - WantedStepLength * .75f,
                                                               spot.Z + WantedStepLength * .75f,
                                                               out new_z, out flags, toonHeight, toonSize))
                            {
                                //Spot blocked = new Spot(nx, ny, spot.Z);
                                //blocked.SetFlag(Spot.FLAG_BLOCKED, true);
                                //AddSpot(blocked);
                            }
                            else
                            {
                                s = FindClosestSpot(new Location(nx, ny, new_z), MinStepLength);
                                if (s == null)
                                {
                                    if (!triangleWorld.IsStepBlocked(spot.X, spot.Y, spot.Z, nx, ny, new_z,
                                                                     toonHeight, toonSize, null))
                                    {
                                        var  n  = new Spot(nx, ny, new_z);
                                        Spot to = AddAndConnectSpot(n);
                                        if ((flags & ChunkedTriangleCollection.TriangleFlagDeepWater) != 0)
                                        {
                                            to.SetFlag(Spot.FLAG_WATER, true);
                                        }
                                        if (((flags & ChunkedTriangleCollection.TriangleFlagModel) != 0) ||
                                            ((flags & ChunkedTriangleCollection.TriangleFlagObject) != 0))
                                        {
                                            to.SetFlag(Spot.FLAG_INDOORS, true);
                                        }
                                        if (to != n || to.SearchIsClosed(currentSearchID))
                                        {
                                            // PathGraph.Log("/sigh");
                                        }
                                        else
                                        {
                                            // There should be a path from source to this one now
                                            if (spot.HasPathTo(to.location))
                                            {
                                                float old_score = 1E30f;

                                                float new_score = src_score + spot.GetDistanceTo(to) +
                                                                  TurnCost(spot, to);
                                                if (locationHeuristics != null)
                                                {
                                                    new_score += locationHeuristics.Score(spot.X, spot.Y, spot.Z);
                                                }


                                                if (to.GetFlag(Spot.FLAG_WATER))
                                                {
                                                    new_score += 30;
                                                }

                                                if (to.SearchScoreIsSet(currentSearchID))
                                                {
                                                    old_score = to.SearchScoreGet(currentSearchID);
                                                }

                                                if (new_score < old_score)
                                                {
                                                    // shorter path to here found
                                                    to.traceBack = spot;
                                                    //if (q.Contains(to))
                                                    //    q.Remove(to);
                                                    to.SearchScoreSet(currentSearchID, new_score);
                                                    q.Enqueue(to, -(new_score + to.GetDistanceTo(dst) * heuristicsFactor));
                                                    new_found++;
                                                }
                                            }
                                            else
                                            {
                                                // woot! I added a new one and it is not connected!?!?
                                                //PathGraph.Log("/cry");
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }
            {
                TimeSpan ts = DateTime.Now.Subtract(pre);
                int      t  = ts.Seconds * 1000 + ts.Milliseconds;
                if (t == 0)
                {
                    t = 1;
                }
                Log("  search failed. " + (count * 1000) / t + " steps/s");
            }
            return(BestSpot); // :(
        }