Why do I keep getting TLE for POJ 1984 -- Navigation Nightmare? - algorithm

Question

#include <iostream>
#include <vector>
#include <cassert>
#include <queue>
using namespace std;
struct vectors{
int x;
int y;
bool visited;
vectors(int x = 0, int y = 0){
this -> x = x;
this -> y = y;
visited = false;
}
bool isNotFound(){
return visited == false;
}
void setZero(){
x = 0;
y = 0;
}
vectors operator+(vectors others){
return vectors(x + others.x, y + others.y);
}
vectors operator*(int others){
return vectors(x * others, y * others);
}
};
struct node{
int adjFarm;
int length;
char direction;
node(int f, int l, char d){
adjFarm = f;
length = l;
direction = d;
}
};
int num_villages;
char inverse_direction(char d){
assert(d == 'N' || d == 'E' || d == 'W' || d == 'S' );
switch(d){
case 'S': return 'N';
case 'N': return 'S';
case 'E': return 'W';
case 'W': return 'E';
}
}
void AddToPaths(int farm1, int farm2, char direction, int length, vector< vector<node> > &pathGraph){
pathGraph.at(farm1).push_back(node(farm2, length, direction));
pathGraph.at(farm2).push_back(node(farm1, length, inverse_direction(direction)));
}
vectors directionVector(int d){
switch(d){
case 'N': return vectors(0, 1);
case 'E': return vectors(1, 0);
case 'W': return vectors(- 1, 0);
case 'S': return vectors(0, - 1);
}
}
void print_coords(vector< vectors > coords){
for(int i = 1; i < num_villages + 1; i ++){
cout << "farm: " << i << " coordinates x at farm: " << coords.at(i).x << " coordinated y at farm: " << coords.at(i).y << endl;
}
}
void update(char direction, int newFarm, int length, int currentFarm, vector <vectors> &coords){
vectors directions = directionVector(direction);
coords.at(newFarm) = coords.at(currentFarm) + directions * length;
coords.at(newFarm).visited = true;
}
void computeCoords(vector <vectors> &coords, vector< vector<node> > &pathGraph){
queue <int> frontier;
frontier.push(1);
coords.at(1).visited = true;
while(!frontier.empty()){
int currentFarm = frontier.front();
frontier.pop();
for(int i = 0; i < pathGraph.at(currentFarm).size(); i ++){
node options = pathGraph.at(currentFarm).at(i);
if(coords.at(options.adjFarm).isNotFound()){
update(options.direction, options.adjFarm, options.length, currentFarm, coords);
frontier.push(options.adjFarm);
}
}
}
}
struct UnionFind{
vector<int> L;
UnionFind(int num_villages){
L.resize(num_villages + 1);
for(int i = 1; i <= num_villages; i ++){
L.at(i) = i;
}
}
int find(int x){
if(x == L.at(x)) return x;
int root = find(L.at(x));
L.at(x) = root;
return root;
}
int Union(int x, int y){
int root1 = find(x);
int root2 = find(y);
L.at(y) = root1;
}
};
int pos;
int query(int start, int destination, int order, UnionFind &reachables, vector<vectors> &coords, vector<vector<int> > &source_destination_order){
while(pos <= order){
reachables.Union(reachables.find(source_destination_order.at(pos).at(0)), reachables.find(source_destination_order.at(pos).at(1)));
pos ++;
}
if(reachables.find(start) == reachables.find(destination)){
return abs(coords.at(start).x - coords.at(destination).x) + abs(coords.at(start).y - coords.at(destination).y);
}
else{
return -1;
}
}
int main(void){
int num_roads;
cin >> num_villages;
cin >> num_roads;
vector <vector<node> > pathGraph(num_villages + 1);
vector <vectors > coords(num_villages + 1);
vector <vector <int> > source_destination_order(num_villages + 1, vector<int> (2));
//Adding inforamtion about the farms
int farm1;
int farm2;
char direction;
int length;
int source;
for(int i = 0; i < num_roads; i ++){
cin >> farm1;
cin >> farm2;
cin >> length;
cin >> direction;
AddToPaths(farm1, farm2, direction, length, pathGraph);
source_destination_order.at(i + 1).at(0) = farm1;
source_destination_order.at(i + 1).at(1) = farm2;
}
computeCoords(coords, pathGraph);
int numQueries;
cin >> numQueries;
int start;
int destination;
int order;
int result;
UnionFind reachables(num_villages);
pos = 1;
for(int i = 0; i < numQueries; i ++){
cin >> start;
cin >> destination;
cin >> order;
result = query(start, destination, order, reachables, coords, source_destination_order);
cout << result << endl;
}
}
I tried to create an undirected acyclic graph with the farms as the vertices and the roads as the edges, and then use BFS to compute the coordinates of each farm relative to the first farm. Afterwards I used the union find structure to create disjoint sets of farms that are reachable from each other at the time of the query. However, it seems like my code takes too long to run, how should I fix it?