首页 > 试题广场 >

路径规划

[编程题]路径规划
  • 热度指数:39 时间限制:C/C++ 2秒,其他语言4秒 空间限制:C/C++ 256M,其他语言512M
  • 算法知识视频讲解
路径规划对于自动驾驶来说是非常重要的一环,它决定了自动驾驶的车辆如何在道路上行驶。现在给出一个城市的地图,请规划出最快从起点到达终点的路线。
地图中的路都平行于X轴或Y轴,所有的路是双向通行的。路与路的交叉点有交通灯限制通行,所有的交通灯都是统一周期控制的。
交通灯共有三种不同的状态。第一种是只能右转,维持T1秒;第二种是可以直行左转和右转,维持T2秒;第三种是只能直行和右转,维持T3秒。
三种状态按顺序交替循环,即[0, T1), [T1 + T2 + T3, 2T1 + T2 + T3), [2T1 + 2T2 + 2T3, 3T1 + 2T2 + 2T3)时只能右转,[T1, T1 + T2), [2T1 + T2 + T3, 2T1 + 2T2 + T3), [3T1 + 2T2 + 2T3, 3T1 + 3T2 + 2T3)时可以直行左转和右转,[T1 + T2, T1 + T2 + T3), [2T1 + 2T2 + T3, 2T1 + 2T2 + 2T3), [3T1 + 3T2 + 2T3, 3T1 + 3T2 + 3T3)时可以直行和右转,如此类推。
路的中间和路口都无法掉头。假设车的速度为1单位长度/s,在起点处车可以自己选择启动的方向,给出起点和终点的坐标,问从起点开到终点最少需要的时间。

输入描述:
第一行输入道路的条数N。

然后N行,每行4个整数X1 Y1 X2 Y2,表示每条道路两个端点的坐标。输入保证每条道路平行于X轴或Y轴,每条路的长度都大于0,,道路之间不会有长度大于0的重合。

然后一行4个整数SX SY TX TY,表示起点和终点的坐标。保证起点和终点位于道路上。

最后一行3个整数T0 T1 T2,表示交通灯三种状态的持续时间。


输出描述:
输出到达终点的最少时间。数据保证可以从起点走到终点。
示例1

输入

2
0 2 4 2
2 0 2 4
1 2 2 1
1 1 1

输出

2

说明

只要直接往东走1,右转之后往南走1就可以到达终点。
示例2

输入

4
0 0 0 2
0 1 1 1
1 0 1 2
1 2 2 2
0 0 2 2
1 1 1

输出

6

说明

先往北走1,再右转往东走1,需要等2秒才能左转,之后再往北走1和右转往东走1就可以到达终点,总共需要时间6秒。

备注:
60%的数据,,所有坐标
100%的数据,,所有坐标

基本思路是建立离散矩阵,然后DFS+回溯得到答案,DFS思路包含了按照当前规则走或者等一个周期,但是只通过了2/10组,原因应该处在等待时候出的问题。仅供参考

#include <iostream>
#include <string>
#include <vector>
#include <sstream>
#include <unordered_map>
#include <unordered_set>
#include <set>
#include <algorithm>

#define _DEBUG__

using namespace std;

class node {
public:
    int x = -1;
    int y = -1;
    string name;
    bool vist = false;
    node* north = nullptr;
    node* south = nullptr;
    node* west = nullptr;
    node* east = nullptr;
};

enum dir {north, south, east, west};

class solution {
public:
    void setPoint(vector<int>& data) {
        pointData = data;
        start = to_string(data[0]) + ' ' + to_string(data[1]);
        end = to_string(data[2]) + ' ' + to_string(data[3]);
        newNode(data[0], data[1]);
        newNode(data[2], data[3]);
        return;
    }
    void setTime(int x, int y, int z) {
        t1 = x, t2 = y, t3 = z;
        return;
    }
    void establishNode(vector<vector<int>>& roadData) {//建立地图
        int roadNum = roadData.size();
        for (int i = 0; i < roadNum; i++) {
            if (roadData[i][0] == roadData[i][2]) {//竖线
                set<int> yCord;
                yCord.emplace(roadData[i][1]);
                yCord.emplace(roadData[i][3]);
                if (pointData[0] == roadData[i][0] && roadData[i][1] <= pointData[1] && pointData[1] <= roadData[i][3]) {//起点与竖线相交
                    yCord.emplace(pointData[1]);
                }
                if (pointData[2] == roadData[i][0] && roadData[i][1] <= pointData[3] && pointData[3] <= roadData[i][3]) {//重点与竖线相交
                    yCord.emplace(pointData[3]);
                }
                vector<int> yLine = roadData[i];
                for (int j = 0; j < roadNum; j++) {
                    if (i == j) {
                        continue;
                    }
                    if (roadData[j][0] == roadData[j][2]) {//平行
                        continue;
                    }
                    vector<int> xLine = roadData[j];
                    if (xLine[1] < yLine[1] || xLine[1] > yLine[3] || xLine[0] > yLine[0] || xLine[2] < yLine[0]) {//没有交点
                        continue;
                    }
                    yCord.emplace(xLine[1]);
                }
                for (auto it = next(yCord.begin()); it != yCord.end(); it++) {//南北建立邻居节点
                    node* south = newNode(roadData[i][0], *prev(it));
                    node* north = newNode(roadData[i][0], *it);
                    south->north = north;
                    north->south = south;
                }
            }
            else {//横线
                set<int> xCord;
                xCord.emplace(roadData[i][0]);
                xCord.emplace(roadData[i][2]);
                if (pointData[1] == roadData[i][1] && roadData[i][0] <= pointData[0] && pointData[0] <= roadData[i][2]) {//起点与横线相交
                    xCord.emplace(pointData[0]);
                }
                if (pointData[3] == roadData[i][1] && roadData[i][0] <= pointData[2] && pointData[2] <= roadData[i][2]) {//重点与横线相交
                    xCord.emplace(pointData[2]);
                }
                vector<int> xLine = roadData[i];
                for (int j = 0; j < roadNum; j++) {
                    if (i == j) {
                        continue;
                    }
                    if (roadData[j][1] == roadData[j][3]) {//平行
                        continue;
                    }
                    vector<int> yLine = roadData[j];
                    if (xLine[1] < yLine[1] || xLine[1] > yLine[3] || xLine[0] > yLine[0] || xLine[2] < yLine[0]) {//没有交点
                        continue;
                    }
                    xCord.emplace(yLine[0]);
                }
                for (auto it = next(xCord.begin()); it != xCord.end(); it++) {//南北建立邻居节点
                    node* west = newNode(*prev(it), roadData[i][1]);
                    node* east = newNode(*it, roadData[i][1]);
                    west->east = east;
                    east->west = west;
                }
            }
        }
        return;
    }
    int exec(void) {
        int res = INT_MAX;
        curMap[start]->vist = true;
        if (curMap[start]->north) {
            explore(curMap[start]->north->y - curMap[start]->y, res, curMap[start]->north, north);
        }
        if (curMap[start]->south) {
            explore(curMap[start]->y - curMap[start]->south->y, res, curMap[start]->south, south);
        }
        if (curMap[start]->east) {
            explore(curMap[start]->east->x - curMap[start]->x, res, curMap[start]->east, east);
        }
        if (curMap[start]->west) {
            explore(curMap[start]->x - curMap[start]->west->x, res, curMap[start]->west, west);
        }
        return res;
    }
    void explore(int currentTime, int& leastTime, node* currentNode, dir lastDirection) {
        if (currentNode->name == end) {
            leastTime = min(leastTime, currentTime);
            return;
        }
        currentNode->vist = true;
        //无论如何都能向右探索,当前立即右转
        if (lastDirection == north && currentNode->east != nullptr && !currentNode->east->vist) {//当前北转东
            explore(currentTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
        }
        else if (lastDirection == east && currentNode->south != nullptr && !currentNode->south->vist) {
            explore(currentTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
        }
        else if (lastDirection == south && currentNode->west != nullptr && !currentNode->west->vist) {
            explore(currentTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, west);
        }
        else if (lastDirection == west && currentNode->north != nullptr && !currentNode->north->vist) {
            explore(currentTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
        }
        int timePeriod = currentTime % (t1 + t2 + t3);
        if (timePeriod < t1) {//当前只能右转,等到t2直行或左转或右转,或者等到t3直行或右转
            //等到t2
            int fakeTime = currentTime + t1 - timePeriod;
            //继续直行
            if (lastDirection == north && currentNode->north != nullptr && !currentNode->north->vist) {
                explore(fakeTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
            }
            else if (lastDirection == east && currentNode->east != nullptr && !currentNode->east->vist) {
                explore(fakeTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
            }
            else if (lastDirection == south && currentNode->south != nullptr && !currentNode->south->vist) {
                explore(fakeTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
            }
            else if (lastDirection == west && currentNode->west != nullptr && !currentNode->west->vist) {
                explore(fakeTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, north);
            }
            //左转
            if (lastDirection == north && currentNode->west != nullptr && !currentNode->west->vist) {
                explore(fakeTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, west);
            }
            else if (lastDirection == west && currentNode->south != nullptr && !currentNode->south->vist) {
                explore(fakeTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
            }
            else if (lastDirection == south && currentNode->east != nullptr && !currentNode->east->vist) {
                explore(fakeTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
            }
            else if (lastDirection == east && currentNode->north != nullptr && !currentNode->north->vist) {
                explore(fakeTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
            }
            //右转
            if (lastDirection == north && currentNode->east != nullptr && !currentNode->east->vist) {//当前北转东
                explore(fakeTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
            }
            else if (lastDirection == east && currentNode->south != nullptr && !currentNode->south->vist) {
                explore(fakeTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
            }
            else if (lastDirection == south && currentNode->west != nullptr && !currentNode->west->vist) {
                explore(fakeTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, west);
            }
            else if (lastDirection == west && currentNode->north != nullptr && !currentNode->north->vist) {
                explore(fakeTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
            }
            //等到t3
            fakeTime = currentTime + t1 - timePeriod + t2;
            //直行
            if (lastDirection == north && currentNode->north != nullptr && !currentNode->north->vist) {
                explore(fakeTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
            }
            else if (lastDirection == east && currentNode->east != nullptr && !currentNode->east->vist) {
                explore(fakeTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
            }
            else if (lastDirection == south && currentNode->south != nullptr && !currentNode->south->vist) {
                explore(fakeTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
            }
            else if (lastDirection == west && currentNode->west != nullptr && !currentNode->west->vist) {
                explore(fakeTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, north);
            }
            //右转
            if (lastDirection == north && currentNode->east != nullptr && !currentNode->east->vist) {//当前北转东
                explore(fakeTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
            }
            else if (lastDirection == east && currentNode->south != nullptr && !currentNode->south->vist) {
                explore(fakeTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
            }
            else if (lastDirection == south && currentNode->west != nullptr && !currentNode->west->vist) {
                explore(fakeTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, west);
            }
            else if (lastDirection == west && currentNode->north != nullptr && !currentNode->north->vist) {
                explore(fakeTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
            }
        }
        else if (t1 <= timePeriod  && timePeriod < t1 + t2) {//当前随便走,也可以等到t3期间直行右转或者等到t1右转
            //当前期间
            int fakeTime = currentTime;
            //继续直行
            if (lastDirection == north && currentNode->north != nullptr && !currentNode->north->vist) {
                explore(fakeTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
            }
            else if (lastDirection == east && currentNode->east != nullptr && !currentNode->east->vist) {
                explore(fakeTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
            }
            else if (lastDirection == south && currentNode->south != nullptr && !currentNode->south->vist) {
                explore(fakeTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
            }
            else if (lastDirection == west && currentNode->west != nullptr && !currentNode->west->vist) {
                explore(fakeTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, north);
            }
            //左转
            if (lastDirection == north && currentNode->west != nullptr && !currentNode->west->vist) {
                explore(fakeTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, west);
            }
            else if (lastDirection == west && currentNode->south != nullptr && !currentNode->south->vist) {
                explore(fakeTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
            }
            else if (lastDirection == south && currentNode->east != nullptr && !currentNode->east->vist) {
                explore(fakeTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
            }
            else if (lastDirection == east && currentNode->north != nullptr && !currentNode->north->vist) {
                explore(fakeTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
            }
            //等到t3期间
            fakeTime = currentTime + t1 + t2 - timePeriod;
            //直行
            if (lastDirection == north && currentNode->north != nullptr && !currentNode->north->vist) {
                explore(fakeTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
            }
            else if (lastDirection == east && currentNode->east != nullptr && !currentNode->east->vist) {
                explore(fakeTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
            }
            else if (lastDirection == south && currentNode->south != nullptr && !currentNode->south->vist) {
                explore(fakeTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
            }
            else if (lastDirection == west && currentNode->west != nullptr && !currentNode->west->vist) {
                explore(fakeTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, north);
            }
            //右转
            if (lastDirection == north && currentNode->east != nullptr && !currentNode->east->vist) {//当前北转东
                explore(fakeTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
            }
            else if (lastDirection == east && currentNode->south != nullptr && !currentNode->south->vist) {
                explore(fakeTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
            }
            else if (lastDirection == south && currentNode->west != nullptr && !currentNode->west->vist) {
                explore(fakeTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, west);
            }
            else if (lastDirection == west && currentNode->north != nullptr && !currentNode->north->vist) {
                explore(fakeTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
            }
            //等到t1期间
            fakeTime = currentTime + t1 + t2 + t3 - timePeriod;
            //右转
            if (lastDirection == north && currentNode->east != nullptr && !currentNode->east->vist) {//当前北转东
                explore(fakeTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
            }
            else if (lastDirection == east && currentNode->south != nullptr && !currentNode->south->vist) {
                explore(fakeTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
            }
            else if (lastDirection == south && currentNode->west != nullptr && !currentNode->west->vist) {
                explore(fakeTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, west);
            }
            else if (lastDirection == west && currentNode->north != nullptr && !currentNode->north->vist) {
                explore(fakeTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
            }
        }
        else {//当前只能直行或右转,等几秒种到t2左转右转直行,或者等到t1右转
            int fakeTime = currentTime;
            //直行
            if (lastDirection == north && currentNode->north != nullptr && !currentNode->north->vist) {
                explore(fakeTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
            }
            else if (lastDirection == east && currentNode->east != nullptr && !currentNode->east->vist) {
                explore(fakeTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
            }
            else if (lastDirection == south && currentNode->south != nullptr && !currentNode->south->vist) {
                explore(fakeTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
            }
            else if (lastDirection == west && currentNode->west != nullptr && !currentNode->west->vist) {
                explore(fakeTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, north);
            }
            //右转
            if (lastDirection == north && currentNode->east != nullptr && !currentNode->east->vist) {//当前北转东
                explore(fakeTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
            }
            else if (lastDirection == east && currentNode->south != nullptr && !currentNode->south->vist) {
                explore(fakeTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
            }
            else if (lastDirection == south && currentNode->west != nullptr && !currentNode->west->vist) {
                explore(fakeTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, west);
            }
            else if (lastDirection == west && currentNode->north != nullptr && !currentNode->north->vist) {
                explore(fakeTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
            }
            //等到t1
            fakeTime = currentTime + t1 + t2 + t3 - timePeriod;
            //右转
            if (lastDirection == north && currentNode->east != nullptr && !currentNode->east->vist) {//当前北转东
                explore(fakeTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
            }
            else if (lastDirection == east && currentNode->south != nullptr && !currentNode->south->vist) {
                explore(fakeTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
            }
            else if (lastDirection == south && currentNode->west != nullptr && !currentNode->west->vist) {
                explore(fakeTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, west);
            }
            else if (lastDirection == west && currentNode->north != nullptr && !currentNode->north->vist) {
                explore(fakeTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
            }
            //等到t2
            int fakeTime = currentTime + t1 + t2 + t3 - timePeriod + t1;
            //直行
            if (lastDirection == north && currentNode->north != nullptr && !currentNode->north->vist) {
                explore(fakeTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
            }
            else if (lastDirection == east && currentNode->east != nullptr && !currentNode->east->vist) {
                explore(fakeTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
            }
            else if (lastDirection == south && currentNode->south != nullptr && !currentNode->south->vist) {
                explore(fakeTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
            }
            else if (lastDirection == west && currentNode->west != nullptr && !currentNode->west->vist) {
                explore(fakeTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, north);
            }
            //左转
            if (lastDirection == north && currentNode->west != nullptr && !currentNode->west->vist) {
                explore(fakeTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, west);
            }
            else if (lastDirection == west && currentNode->south != nullptr && !currentNode->south->vist) {
                explore(fakeTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
            }
            else if (lastDirection == south && currentNode->east != nullptr && !currentNode->east->vist) {
                explore(fakeTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
            }
            else if (lastDirection == east && currentNode->north != nullptr && !currentNode->north->vist) {
                explore(fakeTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
            }
            //右转
            if (lastDirection == north && currentNode->east != nullptr && !currentNode->east->vist) {//当前北转东
                explore(fakeTime + currentNode->east->x - currentNode->x, leastTime, currentNode->east, east);
            }
            else if (lastDirection == east && currentNode->south != nullptr && !currentNode->south->vist) {
                explore(fakeTime + currentNode->y - currentNode->south->y, leastTime, currentNode->south, south);
            }
            else if (lastDirection == south && currentNode->west != nullptr && !currentNode->west->vist) {
                explore(fakeTime + currentNode->x - currentNode->west->x, leastTime, currentNode->west, west);
            }
            else if (lastDirection == west && currentNode->north != nullptr && !currentNode->north->vist) {
                explore(fakeTime + currentNode->north->y - currentNode->y, leastTime, currentNode->north, north);
            }
        }
        currentNode->vist = false;
        return;
    }
private:
    string start, end;
    vector<int> pointData;
    unordered_map<string, node*> curMap;
    int t1, t2, t3;
    node* newNode(int x, int y) {
        string name = to_string(x) + ' ' + to_string(y);
        if (curMap.count(name)) {
            return curMap[name];
        }
        node* cur = new node;
        cur->x = x;
        cur->y = y;
        cur->name = name;
        curMap[name] = cur;
        return cur;
    }
};

int main(int argc, char* argv[]) {
#ifndef _DEBUG_
    int roadNum;
    string tmp;
    vector<vector<int>> roadData;
    vector<int> data(4, 0);
    getline(cin, tmp);
    stringstream ss(tmp);
    ss >> roadNum;
    for (int i = 0; i < roadNum; i++) {
        getline(cin, tmp);
        stringstream ss(tmp);
        ss >> data[0] >> data[1] >> data[2] >> data[3];
        roadData.push_back(data);
    }
    cin >> data[0] >> data[1] >> data[2] >> data[3];
    int t1, t2, t3;
    cin >>  t1 >> t2 >> t3;
#else
    vector<vector<int>> roadData = { {0, 0, 0, 2}, {0, 1, 1, 1}, {1, 0, 1, 2}, {1, 2, 2, 2} };
    int t1 = 1, t2 = 1, t3 = 1;
    vector<int> data = { 0, 0, 2, 2 };
#endif
    solution mySolution;
    mySolution.setPoint(data);
    mySolution.establishNode(roadData);
    mySolution.setTime(t1, t2, t3);
    int res = mySolution.exec();
    cout << res << endl;
    return 0;
}
编辑于 2021-08-23 16:15:22 回复(0)