# Fast Path Planning of Autonomous Vehicles in 3D Environments
## 2 Assumptions and Definitions
Also, the length of \(r_{c}\) is depicted at the bottom of this figure. Each edge in \(I\) is plotted with dashed line segments. See that each edge length is shorter than \(r_{c}\) and that each edge is a collision-free path. A virtual sensor \(n_{i}\) is _adjacent_ to another sensor, say \(n_{j}\), in the case where \(L(\mathbf{n}_{i},\mathbf{n}_{j})\) is a collision-free path and \(\|\mathbf{n}_{i}-\mathbf{n}_{j}\|<r_{c}\). Each edge, say \(e\in E(I)\), is weighted by \(w(e):e\to Z^{+}\). The weight of \(\{n_{i},n_{j}\}\) is set as \(\|\mathbf{n}_{i}-\mathbf{n}_{j}\|\). In simulations, \(R\) can move along any edge in \(E(I)\) infinitely fast. Therefore, we say that \(R\)_jumps_ from one sensor in \(I\) to another sensor in \(I\) instantly. Utilizing the definition of \(E(I)\), \(R\) with radius \(r\) does not collide with an obstacle as it moves along an edge in \(E(I)\). Hence, \(I\) is a collision-free topological map for the agent \(R\). By accessing the weight for every edge in \(I\), \(R\) can find the shortest path from one sensor to every other sensor. Let \(\mathbf{g}\) define the 3D coordinates of the goal. \(S_{n}\), the footprint boundary of a sensor \(n\), meets \(L(\mathbf{n},\mathbf{g})\) at one point. This point is called the _goal-closest point_, since it is closest to the goal among all points on \(S_{n}\). See Figure 2 for an illustration. An _open surface_ of a sensor \(n\) defines the set of points on \(S_{n}\), such that each point in this set is not inside an obstacle. A _frontier_ of \(n\) is the subset of an open surface, such that every point on the frontier is not inside the footprint of any other sensor. We next introduce how to generate _frontier points_ that are utilized to discretize a frontier with discrete points. We first generate \(q\) points on the footprint boundary of a sensor \(n\) so that they are evenly spaced on the footprint boundary. As \(q\) increases, we obtain densely spaced points on a footprint boundary. For instance, we address how to generate \(q=36*36\) points on a footprint boundary. We rotate a vector \([r_{s},0,0]^{T}\) by \(\phi\in\{\pi/18,2*\pi/18,\ldots,2*\pi\}\) centered at the \(z\)-axis. The matrix representing this rotation is  
\[\mathbf{r}(\phi)=\left( \begin{array}{ccc}\cos(\phi)&-\sin(\phi)&0\\ \sin(\phi)&\cos(\phi)&0\\ 0&0&1\end{array} \right). \tag{1}\]  
Thereafter, we rotate the vector \(\mathbf{r}(\phi)*[r_{s},0,0]^{T}\) by \(\theta\in\{\pi/18,2*\pi/18,\ldots,2*\pi\}\) centered at the \(y\)-axis. The matrix representing this rotation is  
\[\mathbf{r}(\theta)=\left( \begin{array}{ccc}\cos(\theta)&0&\sin(\theta)\\ 0&1&0\\ -\sin(\theta)&0&\cos(\theta)\end{array} \right). \tag{2}\]  
The resulting vector after two rotations is \(fp(\phi,\theta)=\mathbf{r}(\theta)*\mathbf{r}(\phi)*[r_{s},0,0]^{T}\). The vector \(fp(\phi,\theta)\) represents the relative position of a points on a footprint boundary with respect to \(\mathbf{n}\) (3D coordinates of sensor \(n\)). Since \(\phi\) and \(\theta\) are chosen from 36 values respectively, \(q=36*36\) points exist on a footprint boundary. By adding \(\mathbf{n}\) to \(fp(\phi,\theta)\), we generate the 3D coordinates of the \(j\)-th frontier point (\(j\in\{1,2,\ldots,q\}\)) of \(n\). The goal-closest point and these \(q\) points are termed the _candidate frontier points_ of \(n\). Among these \(q+1\) candidate frontier points, we select a set of _frontier points_, \(f(n)\), on a frontier satisfying the following conditions:  
1. A point in \(f(n)\) is not inside the footprint of any other sensor. 2. The line segment connecting \(n\) and a point in \(f(n)\) is a collision-free path for the agent \(R\). Figure 2: \(S_{n}\) meets \(L(\mathbf{n},\mathbf{g})\) at one point, called the goal-closest point. 3. The distance between every point in \(f(n)\) and an obstacle boundary is larger than the agent radius \(r\). In our path plan strategy, \(R\) utilizes a frontier point as a \"way-point\" for reaching the goal.