void* work_B2A(void* param){
usleep(rand() % 1000000);
int i = *(int *) param + WORK_THREAD;
sem_wait(&mutex_B2A);
sem_wait(&mutex_BM);
printf("thread %d BtoA: A<---M<-*-B\n", i);
print_road();
usleep(rand() % 1000000);
sem_post(&mutex_BM);
printf("thread %d BtoA: at M\n", i);
print_road();
usleep(rand() % 1000000);
sem_wait(&mutex_AM);
printf("thread %d BtoA: A<-*-M<---B\n", i);
print_road();
usleep(rand() % 1000000);
sem_post(&mutex_AM);
sem_post(&mutex_B2A);
printf("thread %d BtoA: at A\n", i);
print_road();
}
void* work_A2B(void* param){
usleep(rand() % 1000000);
int i = *(int *) param;
int semvalue;
sem_wait(&mutex_A2B);
sem_wait(&mutex_AM);
printf("thread %d AtoB: A-*->M--->B\n", i);
print_road();
usleep(rand() % 1000000);
sem_post(&mutex_AM);
printf("thread %d AtoB: at M\n", i);
print_road();
usleep(rand() % 1000000);
sem_wait(&mutex_BM);
printf("thread %d AtoB: A--->M-*->B\n", i);
print_road();
usleep(rand() % 1000000);
sem_post(&mutex_BM);
sem_post(&mutex_A2B);
printf("thread %d AtoB: at B\n", i);
print_road();
return NULL;
}
void bidirection_breadth_search(bi_node **s,
int m, int n, bi_node beg, bi_node end)
{//矩阵s有m行n列，行下标从0到m-1，列下标从0到n-1
//visit_beg记录beg队列访问过的点
//visit_end记录end队列访问过的点
int **visit_beg = new int*[MAX];
int **visit_end = new int*[MAX];
for(int i = 0; i < m; ++ i){
visit_beg[i] = new int[MAX];
memset(visit_beg[i], 0, MAX * sizeof(int));
visit_end[i] = new int[MAX];
memset(visit_end[i], 0, MAX * sizeof(int));
}
//q_beg是从起点bfs的队列
//q_end是从终点bfs的队列
deque<bi_node> q_beg, q_end;
//起点终点分别进入两个队列
q_beg.push_back(beg), q_end.push_back(end);
visit_beg[beg.b_y][beg.b_x] = 1, visit_end[end.b_y][end.b_x] = 1;
//meet_pos返回q_beg和q_end中相遇的点
//meet_pos.first是q_beg队列中的相遇点
//meet_pos.second是q_end队列中的相遇点
//这两个点是相邻的，即这两个点相遇
pair<bi_node, bi_node> meet_pos;
while(1){
if(q_beg.size() > q_end.size()){
//扩展q_end，检查要加入q_end的点是否已存在于q_beg中
//最后一个参数 2 标志被扩展的队列是q_end
meet_pos = expand_queue(q_end, q_beg, s, m, n, visit_end, 2);
if(meet_pos != pair<bi_node, bi_node>(*q_end.end(), *q_end.end()))
//找到了相遇点
break;
}
else{
//扩展q_beg，检查要加入q_beg的点是否已存在于q_end中
//最后一个参数 1 标志被扩展的队列是q_beg
meet_pos = expand_queue(q_beg, q_end, s, m, n, visit_beg, 1);
if(meet_pos != pair<bi_node, bi_node>(*q_beg.end(), *q_beg.end()))
break;
}
}
//若找到相遇的两点，则传入print_road函数输出路径
print_road(meet_pos, s);
}
bool Gps_algorithm::find_best_road(int from_id,int to_id)
{
clear();
//qDebug()<<"in_al";
if (from_id == to_id)
return false;
before_line[from_id] = -2;
cost[from_id] = 0;
priority_queue<Node> q;
while(!q.empty())
{
q.pop();
}
q.push(Node(from_id,0));
while (!q.empty())
{
Node now = q.top();
be_search[now.point_id] = true;
q.pop();
if (now.point_id == to_id)
{
qDebug()<<"find_road_success\n";
int t = to_id;
after_line[t] = -2;
while (before_line[t]!=-2)
{
after_line[map->line_vector[before_line[t]].Begin_point_id] = before_line[t];
t = map->line_vector[before_line[t]].Begin_point_id;
if (t == from_id)
break;
}
print_road();
return true;
}
Gps_point& p = map->point_vector[now.point_id];
for (int i = 0;i < p.lines_id.size(); ++i)
{
int line_id = p.lines_id[i];
int from_id = map->line_vector[line_id].Begin_point_id;
int to_id = map->line_vector[line_id].End_point_id;
if (be_search[to_id] == false &&
cost[to_id] > (cost[from_id] + road_cost(line_id)))
{
cost[to_id] = cost[from_id] + road_cost(line_id);
before_line[to_id]=line_id;
q.push(Node(to_id,cost[to_id]));
}
}
}
return false;
}
