# Energy-Optimized 3D Path Planning for Unmanned Aerial Vehicles
## 10 Future Works
```
1: if \(x>=\mathit{obstacle[0]}\) and \(x<=\mathit{obstacle[1]}\) and \(y>=\mathit{obstacle[2]}\) and \(y<=\mathit{obstacle[3]}\) and \(z>=\mathit{obstacle[4]}\) and \(z<=\mathit{obstacle[5]}\)
2: return False
3:endif
4: return True
```  
**Algorithm A1** isFree (for obstacles)  
```
1: # Calculate the heuristic cost between two points
2: distance \(=\mathrm{abs}(x1-x2)+\mathrm{abs}(y1-y2)+\mathrm{abs}(z1-z2)\)
3: # Calculate the angle between the direction of the wind and the line connecting the two points
4: \(\mathit{wind\_angle}=\arctan(y2-y1,x2-x1)-\mathit{wind\_direction}\)
5: # Calculate the component of the wind speed in the direction of the line connecting the two points
6: \(\mathit{wind\_component}=\mathit{wind\_speed}^{*}\cos(\mathit{wind\_angle})\)
7: # Return the total heuristic cost, taking into account the wind speed and direction
8: return distance + wind\(\mathit{component}\)
```  
**Algorithm A2** heuristic(x1, y1, z1, x2, y2, z2, wind\_speed, wind\_direction)```
1:# Create a list to store the visited nodes
2:visited = []
3:# Create a list of the remaining nodes to be explored
4:remaining = [start]
5:# Keep looping until there are no more nodes to explore
6:whileremaining
7: # Sort the remaining nodes by their heuristic cost
8:remaining.sort(key=lambda \(x\): map.heuristic(\(x\)[0], \(x\)[1], \(x\)[2], goal[0], goal[1], goal[2], \(\mathit{wind\_speed,\mathit{wind\_direction}}\)))
9: # Get the node with the lowest heuristic cost
10:current = remaining.pop(0)
11:print(current)
12:# Check if we have reached the goal
13:ifcurrent == goal
14:# Return the list of visited nodes if the goal is reached
15:returnvisited
16:endif
17:# Check the neighboring nodes of the current node
18:for\(x\) in [current[0] \(-\) 1, current[0] \(+\) 1]
19:for\(y\) in [current[1] \(-\) 1, current[1] \(+\) 1]
20:for\(z\) in [current[2] \(-\) 1, current[2] \(+\) 1]
21:# Check if the neighboring node is free of obstacles and has not been visited
22:ifmap.isFree(\(x\), \(y\), \(z\)) and (\(x\), \(y\), \(z\)) not in visited
23:# Add the node to the list of remaining nodes to be explored
24:remaining.append((\(x\), \(y\), \(z\)))
25:endif
26:forend
27:forend
28:forend
29:Add the current node to the list of visited nodes
30:visited.append(current)
31:# Return an empty list if the goal could not be reached
32:return[]
33:# Define the start and goal coordinates
34:start = (0, 0, 0)
35:start2 = (0, 6, 0)
36:start3 = (8, \(-\)2, 2)
37:goal = (13, 15, 13)
38:goal2 = (13, 13, 13)
39:goal3 = (15, 15, 15)
40:# Define the wind speed and direction
41:\(\mathit{wind\_speed1}\) = 0
42:\(\mathit{wind\_direction1}\) = math.pi / 2
43:\(\mathit{wind\_speed2}\) = 2
44:\(\mathit{wind\_direction2}\) = math.pi / 3
45:# Perform the A* search, taking into account the wind speed and direction
46:\(\mathit{visited}\) = aStarSearch(map, start, goal, \(\mathit{wind\_speed1}\), \(\mathit{wind\_direction1}\))
47:\(\mathit{visited2}\) = aStarSearch(map, start2, goal2, \(\mathit{wind\_speed1}\), \(\mathit{wind\_direction2}\))
48:\(\mathit{visited3}\) = aStarSearch(map, start3, goal3, \(\mathit{wind\_speed2}\), \(\mathit{wind\_direction2}\))
49:# Plot the visited nodes
50:\(\mathit{ax\_plot}\)([\(x\)[0]] for \(x\) in visited], [\(x\)[1]] for \(x\) in visited], [\(x\)[2] for \(x\) in visited], \"ro-\")
51:\(\mathit{ax\_plot}\)([\(x\)[0]] for \(x\) in visited]2, [\(x\)[1] for \(x\) in visited], [\(x\)[2]] for \(x\) in visited], \"go-\")
52:\(\mathit{ax\_plot}\)([\(x\)[0]] for \(x\) in visited]3, [\(x\)[1] for \(x\) in visited], [\(x\)[2] for \(x\) in visited], \"yo-\")
```

**Algorithm A3** aStarSearch(map, start, goal, wind_speed, wind_direction)

## Appendix B

The generalized pseudocode of the whole process.