06-图4. Saving James Bond - Hard Version (30)

该题的关键是题干最后一句：如果有不同的路线都是最短路，那么输出第一跳最短的路线。解决方法是把湖心小岛周围一圈的节点（即距离湖心小岛小于D的鳄鱼）看作起点，对这些起点依次调用无权最短路算法，找出路径最短（且相同）的那些起点，然后比较从湖心小岛到它们的距离，输出最小的距离所在的路径即可。

#include <iostream>
#include <vector>
#include <queue>
#include <stack>
#include <cmath>

#define fuck cout << endl;
using namespace std;

struct Coordinate
{
    int x;
    int y;
};

bool operator==(Coordinate& a, Coordinate& b)
{
    return a.x == b.x && a.y == b.y;
}

float DistanceOfPoints(const Coordinate& a, const Coordinate& b)
{
    return sqrtf(pow(a.x - b.x, 2) + pow(a.y - b.y, 2));
}

void JudgePosition(const int& D, Coordinate* crocodile, const int& i, bool* isCloseToEdge, bool* isCloseToCenter)
{
    // 靠近湖岸
    if (crocodile[i].x >= 50 - D || crocodile[i].x <= -50 + D ||
        crocodile[i].y >= 50 - D || crocodile[i].y <= -50 + D)
    {
        isCloseToEdge[i] = true;
    }
    else
    {
        isCloseToEdge[i] = false;
    }
    // 靠近湖心小岛
    if ( sqrtf(pow(crocodile[i].x, 2) + pow(crocodile[i].y, 2)) <= 7.5 + D)
    {
        isCloseToCenter[i] = true;
    }
    else
    {
        isCloseToCenter[i] = false;
    }
}

bool IsCloseToEdge(const int& D, const Coordinate& crocodile)
{
    return (crocodile.x >= 50 - D || crocodile.x <= -50 + D ||
            crocodile.y >= 50 - D || crocodile.y <= -50 + D);
}

bool IsCloseToCenter(const int& D, const Coordinate& crocodile)
{
    return (sqrtf(pow(crocodile.x, 2) + pow(crocodile.y, 2)) <= 7.5 + D);
}

vector<int> UnweightedShortestWay(const int& s, vector<vector<int> >& graph, vector<Coordinate>& crocodile)
{
    Coordinate O;
    O.x = 0;
    O.y = 0;

    int minPath         = 9999;
    int minFirstHeap    = 9999;
    vector<int> bestPath;

    // 一共s个鳄鱼，s是湖心小岛，s+1是岸边
    for (int i = 0; i < graph[s].size(); i++)
    {
        bool isInitialFind  = true;

        float currentHeapDistance = DistanceOfPoints(O, crocodile[graph[s][i]]);

        vector<int> currentPath(s + 2, -1);

        vector<int> distance(s + 2, -1);
        distance[graph[s][i]] = 0;

        queue<int> nodeQueue;
        nodeQueue.push(graph[s][i]);

        while(!nodeQueue.empty())
        {
            bool isFind = false;
            int lastNode = nodeQueue.front();
            nodeQueue.pop();
            for (int j = 0; j < graph[lastNode].size(); j++)
            {
                // graph[lastNode][j]是和lastNode相连的节点的编号
                int currentNode = graph[lastNode][j];
                if (distance[currentNode] == -1)
                {
                    distance[currentNode] = distance[lastNode] + 1;
                    currentPath[currentNode] = lastNode;
                    if (currentNode == s + 1)
                    {
                        if (isInitialFind)
                        {
                            isInitialFind = false;
                            //minPath = distance[currentNode];
                        }
                        if (distance[currentNode] < minPath)
                        {
                            minPath = distance[currentNode];
                            minFirstHeap = currentHeapDistance;
                            bestPath = currentPath;
                        }
                        else if (distance[currentNode] == minPath)
                        {
                            if (currentHeapDistance < minFirstHeap)
                            {
                                minFirstHeap = currentHeapDistance;
                                bestPath = currentPath;
                            }
                        }
                    }
                    nodeQueue.push(currentNode);
                    if (!isInitialFind)
                    {
                        isFind = true;
                        break;
                    }
                }
            }
            if (isFind)
            {
                break;
            }
        }
    }
    return bestPath;
}

int main()
{
    int N;
    int D;
    scanf("%d %d", &N, &D);
    int nodeCount = N + 2;
    vector<vector<int> > graph(nodeCount);
    vector<Coordinate> crocodile(nodeCount);

    for (int i = 0; i < N; i++)
    {
        scanf("%d %d", &crocodile[i].x, &crocodile[i].y);
    }
    crocodile[N].x = 0;
    crocodile[N].y = 0;
    crocodile[N + 1].x = -1;
    crocodile[N + 1].y = -1;
    // 一共N个鳄鱼，N是湖心小岛，N+1是岸边
    for (int i = 0; i < N; i++)
    {
        if (IsCloseToCenter(D, crocodile[i]))
        {
            graph[i].push_back(N);
            graph[N].push_back(i);
        }
        if (IsCloseToEdge(D, crocodile[i]))
        {
            graph[i].push_back(N + 1);
            graph[N + 1].push_back(i);
        }
        for (int j = i + 1; j < N; j++)
        {
            if (DistanceOfPoints(crocodile[i], crocodile[j]) <= D)
            {
                graph[i].push_back(j);
                graph[j].push_back(i);
            }
        }
    }
    // 如果湖心小岛和岸边距离小于D
    if (50 - 7.5 <= D)
    {
        graph[N].push_back(N + 1);
        graph[N + 1].push_back(N);
        printf("1\n");
        return 0;
    }

    vector<vector<int> > path;

    vector<int> bestPath = UnweightedShortestWay(N, graph, crocodile);

    if (graph[N].empty() || graph[N + 1].empty())
    {
        printf("0\n");
        return 0;
    }
    int i = N + 1;
    int n = 0;
    stack<Coordinate> pathStack;
    while (bestPath[i] != -1)
    {
        i = bestPath[i];
        pathStack.push(crocodile[i]);
        n++;
    }

    printf("%d\n", pathStack.size() + 1);

    while (!pathStack.empty())
    {
        printf("%d %d\n", pathStack.top().x, pathStack.top().y);
        pathStack.pop();
    }
    return 0;
}