private void Start()
{
// get the drone controller
m_Drone = GetComponent <DroneController>();
friends = GameObject.FindGameObjectsWithTag("Drone");
myIndex = -1;
for (int idx = 0; idx < friends.Length; idx++)
{
if (friends[idx].transform == m_Drone.transform)
{
myIndex = idx;
break;
}
}
terrain_manager = terrain_manager_game_object.GetComponent <TerrainManager>();
tInfo = terrain_manager.myInfo;
Vector3 start_pos = m_Drone.transform.position;
Vector3 goal_pos = my_goal_object.transform.position;
mHighResMap = new HighResMap(ref tInfo, 1);
AStar aster = new AStar(ref mHighResMap);
int i = mHighResMap.get_i_index(transform.position.x);
int j = mHighResMap.get_j_index(transform.position.z);
int iEnd = mHighResMap.get_i_index(goal_pos.x);
int jEnd = mHighResMap.get_j_index(goal_pos.z);
tPath = aster.ComputePath(i, j, iEnd, jEnd);
mDecision = new DroneDecisionSimple(ref m_Drone, ref friends, ref mHighResMap, tPath);
//status init
ifMoveable = false;
ifFinished = false;
}
public DroneDecisionSimple(ref DroneController m_Drone, ref GameObject[] friends, ref HighResMap highRes, List <Tuple <int, int> > pathII)
{
mPathII = pathII;
mHighResMap = highRes;
mDrone = m_Drone;
mFriends = friends;
mCurrentTargetIndex = 0;
foreach (var p in mPathII)
{
mPath.Add(new Vector3(mHighResMap.get_x_pos(p.Item1), 0f, mHighResMap.get_z_pos(p.Item2)));
}
printPath();
}
public void init(ref TerrainManager terrainManager, ref HighResMap hResMap, GameObject[] friends)
{
lock (padlock)
{
if (!solved)
{
mTerrainManager = terrainManager;
mInfo = mTerrainManager.myInfo;
mHighResMap = hResMap;
mFriends = friends;
playerStatus = new bool[100];
initOccupyMap();
solved = true;
}
}
}
private void init()
{
// get the drone controller
m_Drone = GetComponent <DroneController>();
terrain_manager = terrain_manager_game_object.GetComponent <TerrainManager>();
highResMap = new HighResMap(terrain_manager.myInfo, 2f);
highResMap.configurationSpace(); // 默认膨胀一圈
Vector3 start_pos = terrain_manager.myInfo.start_pos;
Vector3 goal_pos = terrain_manager.myInfo.goal_pos;
lastPosition = transform.position;
lastStopPosition = transform.position;
int start_i = highResMap.get_i_index(start_pos.x);
int start_j = highResMap.get_j_index(start_pos.z);
int goal_i = highResMap.get_i_index(goal_pos.x);
int goal_j = highResMap.get_j_index(goal_pos.z);
int x_N = highResMap.x_N;
int z_N = highResMap.z_N;
float[,] traversability = highResMap.traversability;
node_dic.Add(1, Tuple.Create(start_i, start_j));
for (int i = 0; i < x_N; i++)
{
for (int j = 0; j < z_N; j++)
{
if (traversability[i, j] == 0.0f)
{
if (i == start_i && j == start_j)
{
continue;
}
if (i == goal_i && j == goal_j)
{
continue;
}
n++;
node_dic.Add(n, Tuple.Create(i, j));
}
}
}
n += 1;
node_dic.Add(n, Tuple.Create(goal_i, goal_j));
// initialize node_dic2
foreach (KeyValuePair <int, Tuple <int, int> > kvp in node_dic)
{
node_dic2.Add(kvp.Value, kvp.Key);
}
// initialize virables
dis = new int[n + 1];
s = new bool[n + 1];
c = new int[n + 1, n + 1];
cc = new int[n + 1, n + 1];
v = 1;
for (int i = 0; i < n + 1; i++)
{
dis[i] = INF;
s[i] = false;
for (int j = 0; j < n + 1; j++)
{
c[i, j] = INF;
cc[i, j] = INF;
}
}
for (int i = 0; i < x_N; i++)
{
for (int j = 0; j < z_N; j++)
{
if (traversability[i, j] == 0.0f)
{
int current_node = node_dic2[Tuple.Create(i, j)];
// 上
if (j + 1 < z_N && traversability[i, j + 1] == 0)
{
int next_node = node_dic2[Tuple.Create(i, j + 1)];
this.setCandCC(current_node, next_node);
}
// 右
if (i + 1 < x_N && traversability[i + 1, j] == 0)
{
int next_node = node_dic2[Tuple.Create(i + 1, j)];
this.setCandCC(current_node, next_node);
}
// 右上
if (j + 1 < z_N && i + 1 < x_N && traversability[i + 1, j + 1] == 0)
{
int next_node = node_dic2[Tuple.Create(i + 1, j + 1)];
this.setCandCC(current_node, next_node, INF);
}
// 左上
if (i - 1 >= 0 && i + 1 < x_N && traversability[i - 1, j + 1] == 0)
{
int next_node = node_dic2[Tuple.Create(i - 1, j + 1)];
this.setCandCC(current_node, next_node, INF);
}
}
}
}
//for (int i = 0; i < n + 1; i++)
//{
// for (int j = 0; j < n + 1; j++)
// {
// Debug.Log(c[i, j]);
// }
// Debug.Log("-----------------------------------");
//}
}
public AStar(ref HighResMap highResMap)
{
mHighResMap = highResMap;
traversabilityMatrix = mHighResMap.traversability;
}
