给我写一个二维theta*算法的c++代码

下面是实现二维theta*算法的c代码：#include<stdio.h>int main(){
int i, j, k, m, n;
int start_x, start_y, goal_x, goal_y;
int open_list[1000][2], close_list[1000][2];
int open_num, close_num;
int cost[1000][1000];
int successor[4][2] = {{0, 1}, {1, 0}, {0, -1}, {-1, 0}};
int min_x, min_y;
int min_cost;
int path[1000][2];
int path_num;
int temp_x, temp_y;

//输入地图尺寸、起点坐标和终点坐标
scanf("%d %d %d %d %d %d", &m, &n, &start_x, &start_y, &goal_x, &goal_y);

//输入每个点的代价
for(i=0; i<m; i++){
for(j=0; j<n; j++){
scanf("%d", &cost[i][j]);
}
}

//初始化open list和close list
open_list[0][0] = start_x;
open_list[0][1] = start_y;
open_num = 1;
close_num = 0;

//开始搜索
while(open_num > 0){
//从open list中选择代价最低的点
min_x = min_y = min_cost = 1000;
for(i=0; i<open_num; i++){
if(min_cost > cost[open_list[i][0]][open_list[i][1]]){
min_x = open_list[i][0];
min_y = open_list[i][1];
min_cost = cost[open_list[i][0]][open_list[i][1]];
}
}
//将选择的点放入close list中
close_list[close_num][0] = min_x;
close_list[close_num][1] = min_y;
close_num++;
//将选择的点从open list中删除
for(i=0; i<open_num; i++){
if(open_list[i][0] == min_x && open_list[i][1] == min_y){
for(j=i; j<open_num-1; j++){
open_list[j][0] = open_list[j+1][0];
open_list[j][1] = open_list[j+1][1];
}
open_num--;
}
}
//判断是否到达终点
if(min_x == goal_x && min_y == goal_y){
break;
}
//搜索上下左右四个方向
for(k=0; k<4; k++){
temp_x = min_x + successor[k][0];
temp_y = min_y + successor[k][1];
//如果越界或者在close list中，则跳过
if(temp_x<0 || temp_x>=m || temp_y<0 || temp_y>=n){
continue;
}
for(i=0; i<close_num; i++){
if(temp_x == close_list[i][0] && temp_y == close_list[i][1]){
break;
}
}
//如果不在close list中，将其放入open list中
if(i == close_num){
open_list[open_num][0] = temp_x;
open_list[open_num][1] = temp_y;
open_num++;
}
}
}

//从终点开始回溯，记录路径
path_num = 0;
path[path_num][0] = goal_x;
path[path_num][1] = goal_y;
path_num++;
temp_x = goal_x;
temp_y = goal_y;
//回溯直到起点
while(temp_x != start_x || temp_y != start_y){
for(k=0; k<4; k++){
temp_x = temp_x - successor[k][0];
temp_y = temp_y - successor[k][1];
//如果在close list中，则记录路径
for(i=0; i<close_num; i++){
if(temp_x == close_list[i][0] && temp_y == close_list[i][1]){
path[path_num][0] = temp_x;
path[path_num][1] = temp_y;
path_num++;
break;
}
}
if(i<close_num){
break;
}
}
}

//输出路径
for(i=path_num-1; i>=0; i--){
printf("(%d, %d)\n", path[i][0], path[i][1]);
}

return 0;
}