Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <iostream>
- #include <vector>
- #include <set>
- #include <utility>
- #include <cmath>
- using std::vector;
- using std::set;
- using std::pair;
- struct Node;
- struct Edge {
- Node* next;
- double cost;
- Edge(Node* next = nullptr, double cost = 0)
- : next(next)
- , cost(cost)
- {}
- };
- struct Node {
- bool visited = false;
- double cost = 1e+10;
- vector<Edge> edges;
- };
- struct Road;
- struct City {
- vector<Road*> roads;
- double posx, posy;
- };
- struct Road {
- Node begin;
- Node end;
- City* from;
- City* to;
- double length;
- double dx, dy;
- void init(City* from_, City* to_) {
- from = from_;
- to = to_;
- from->roads.push_back(this);
- dx = to->posx - from->posx;
- dy = to->posy - from->posy;
- length = sqrt(dx * dx + dy * dy);
- begin.edges.emplace_back(&end, length);
- }
- };
- class Queue {
- public:
- Node* pop() {
- const auto& it = queue_.begin();
- Node* result = it->second;
- queue_.erase(it);
- return result;
- }
- bool empty() const {
- return queue_.empty();
- }
- void insert(Node* node, double cost) {
- queue_.erase(std::make_pair(node->cost, node));
- node->cost = cost;
- queue_.insert(std::make_pair(node->cost, node));
- }
- private:
- set<pair<double, Node*>> queue_;
- };
- const double PI = atan(1) * 4;
- double GetAngle(Road const& from, Road const& to) {
- double dot = from.dx * to.dx + from.dy * to.dy;
- double cross = abs(from.dx * to.dy - from.dy * to.dx);
- double len = from.length * to.length;
- dot /= len;
- cross /= len;
- double angle;
- if (abs(dot) < cross) {
- angle = acos(dot);
- } else if (dot > 0) {
- angle = asin(cross);
- } else {
- angle = PI - asin(cross);
- }
- return angle * 180 / PI;
- }
- void dijkstra(Node* start) {
- Queue queue;
- queue.insert(start, 0);
- while (!queue.empty()) {
- Node* node = queue.pop();
- node->visited = true;
- for (auto& edge : node->edges) {
- if (!edge.next->visited && node->cost + edge.cost < edge.next->cost) {
- queue.insert(edge.next, node->cost + edge.cost);
- }
- }
- }
- }
- int main() {
- std::ios_base::sync_with_stdio(false);
- int numCities, numRoads, idxStart, idxEnd;
- double turnCost;
- std::cin >> numCities >> numRoads >> idxStart >> idxEnd >> turnCost;
- --idxStart;
- --idxEnd;
- vector<City> cities(numCities);
- vector<Road> roads(numRoads);
- for (auto& city : cities) {
- std::cin >> city.posx >> city.posy;
- }
- for (auto& road : roads) {
- int from, to;
- std::cin >> from >> to;
- --from;
- --to;
- road.init(&cities[from], &cities[to]);
- }
- Node start, end;
- for (auto& road : roads) {
- for (auto to : road.to->roads) {
- road.end.edges.emplace_back(&to->begin, GetAngle(road, *to) * turnCost);
- }
- if (road.from == &cities[idxStart]) {
- start.edges.emplace_back(&road.begin, 0);
- }
- if (road.to == &cities[idxEnd]) {
- road.end.edges.emplace_back(&end, 0);
- }
- }
- dijkstra(&start);
- if (end.visited) {
- std::cout.precision(6);
- std::cout << std::fixed << end.cost;
- } else {
- std::cout << -1;
- }
- return 0;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement