Advertisement
Guest User

Untitled

a guest
May 29th, 2015
192
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 3.14 KB | None | 0 0
  1. #include <iostream>
  2. #include <vector>
  3. #include <set>
  4. #include <utility>
  5. #include <cmath>
  6. using std::vector;
  7. using std::set;
  8. using std::pair;
  9.  
  10. struct Node;
  11.  
  12. struct Edge {
  13.   Node* next;
  14.   double cost;
  15.   Edge(Node* next = nullptr, double cost = 0)
  16.     : next(next)
  17.     , cost(cost)
  18.   {}
  19. };
  20.  
  21. struct Node {
  22.   bool visited = false;
  23.   double cost = 1e+10;
  24.   vector<Edge> edges;
  25. };
  26.  
  27. struct Road;
  28.  
  29. struct City {
  30.   vector<Road*> roads;
  31.   double posx, posy;
  32. };
  33.  
  34. struct Road {
  35.   Node begin;
  36.   Node end;
  37.   City* from;
  38.   City* to;
  39.   double length;
  40.   double dx, dy;
  41.  
  42.   void init(City* from_, City* to_) {
  43.     from = from_;
  44.     to = to_;
  45.     from->roads.push_back(this);
  46.     dx = to->posx - from->posx;
  47.     dy = to->posy - from->posy;
  48.     length = sqrt(dx * dx + dy * dy);
  49.     begin.edges.emplace_back(&end, length);
  50.   }
  51. };
  52.  
  53. class Queue {
  54. public:
  55.   Node* pop() {
  56.     const auto& it = queue_.begin();
  57.     Node* result = it->second;
  58.     queue_.erase(it);
  59.     return result;
  60.   }
  61.  
  62.   bool empty() const {
  63.     return queue_.empty();
  64.   }
  65.  
  66.   void insert(Node* node, double cost) {
  67.     queue_.erase(std::make_pair(node->cost, node));
  68.     node->cost = cost;
  69.     queue_.insert(std::make_pair(node->cost, node));
  70.   }
  71.  
  72. private:
  73.   set<pair<double, Node*>> queue_;
  74. };
  75.  
  76. const double PI = atan(1) * 4;
  77.  
  78. double GetAngle(Road const& from, Road const& to) {
  79.   double dot = from.dx * to.dx + from.dy * to.dy;
  80.   double cross = abs(from.dx * to.dy - from.dy * to.dx);
  81.   double len = from.length * to.length;
  82.   dot /= len;
  83.   cross /= len;
  84.   double angle;
  85.   if (abs(dot) < cross) {
  86.     angle = acos(dot);
  87.   } else if (dot > 0) {
  88.     angle = asin(cross);
  89.   } else {
  90.     angle = PI - asin(cross);
  91.   }
  92.   return angle * 180 / PI;
  93. }
  94.  
  95. void dijkstra(Node* start) {
  96.   Queue queue;
  97.   queue.insert(start, 0);
  98.   while (!queue.empty()) {
  99.     Node* node = queue.pop();
  100.     node->visited = true;
  101.     for (auto& edge : node->edges) {
  102.       if (!edge.next->visited && node->cost + edge.cost < edge.next->cost) {
  103.         queue.insert(edge.next, node->cost + edge.cost);
  104.       }
  105.     }
  106.   }
  107. }
  108.  
  109. int main() {
  110.   std::ios_base::sync_with_stdio(false);
  111.  
  112.   int numCities, numRoads, idxStart, idxEnd;
  113.   double turnCost;
  114.   std::cin >> numCities >> numRoads >> idxStart >> idxEnd >> turnCost;
  115.   --idxStart;
  116.   --idxEnd;
  117.  
  118.   vector<City> cities(numCities);
  119.   vector<Road> roads(numRoads);
  120.  
  121.   for (auto& city : cities) {
  122.     std::cin >> city.posx >> city.posy;
  123.   }
  124.   for (auto& road : roads) {
  125.     int from, to;
  126.     std::cin >> from >> to;
  127.     --from;
  128.     --to;
  129.     road.init(&cities[from], &cities[to]);
  130.   }
  131.  
  132.   Node start, end;
  133.  
  134.   for (auto& road : roads) {
  135.     for (auto to : road.to->roads) {
  136.       road.end.edges.emplace_back(&to->begin, GetAngle(road, *to) * turnCost);
  137.     }
  138.     if (road.from == &cities[idxStart]) {
  139.       start.edges.emplace_back(&road.begin, 0);
  140.     }
  141.     if (road.to == &cities[idxEnd]) {
  142.       road.end.edges.emplace_back(&end, 0);
  143.     }
  144.   }
  145.  
  146.   dijkstra(&start);
  147.  
  148.   if (end.visited) {
  149.     std::cout.precision(6);
  150.     std::cout << std::fixed << end.cost;
  151.   } else {
  152.     std::cout << -1;
  153.   }
  154.  
  155.   return 0;
  156. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement