<html><head><meta name="color-scheme" content="light dark"></head><body><pre style="word-wrap: break-word; white-space: pre-wrap;">%-------------------------------------------------------------------
\subsubsection{Closest Point of Approach Calculations}
\label{cpadef}

The IvP functions produced by contact-related behaviors are defined
over the domain of possible heading and speed choices. The utility
assigned to a point in this domain (a heading-speed pair) depends in
part on the calculated closest point of approach (CPA) between the
candidate maneuver leg, and the contact leg formed from the contact's
position and trajectory.  Figure \ref{ipf_raw_cpa} shows the
relationship $cpa({\mcrs},v)$ between CPA and candidate maneuvers
$({\mcrs},v)$, where ${\mcrs}$=heading and $v$=speed, for a given
relative position between ownship and a given contact vehicle and
trajectory. The IvP function generated by the AvoidCollision behavior
applies a further user-defined utility function to the CPA calculation
for a candidate maneuver, $f(cpa({\mcrs}, v))$. The form of $f()$ is
determined by configuration parameters specific to the individual 
behavior.

\vspace{0.1in}
\begin{figure}[H]
  \centering 
  \includegraphics[width=0.92\textwidth]{figures/ipf_raw_cpa.png}
  \caption{{\bf The closest point of approach mapping:} The function
    on the right indicates the relative change in calculated closest
    point of approach between ownship and contact position and
    trajectory shown on the left.}
\label{ipf_raw_cpa}
\end{figure}

For contact related behaviors, an important quality of a candidate
action {\dpt}, is the {\em {closest point of approach (CPA)}} between
two vehicles during a candidate leg. A behavior producing an objective
function with CPA as a component of its utility function needs to
perform many variations of this calculation on each new call to
generate an IvP objective function.  The algorithm is given here, 
highlighting a few areas where caching may be exploited to improve
efficiency.

\pskip

Our own current position is known and given by ($\oslon$, $\oslat$),
and the other vehicle's current position and trajectory is given by
($\cnlon$, $\cnlat$, $\cncrs$, $\cnspd$). To compute the CPA distance
for a given {\dpt}, first the time {\ttol} when the minimum distance
between two vehicles occurs is computed. The distance between the two
vehicles at the current time can by determined by the Pythagorean
theorem. Generally, for any given time $t$ (where the current time is
$t=0$), and assuming the other vehicle stays on a constant trajectory,
the distance between the two vehicles for any chosen {\dpt} is given
by:

\begin{equation*}
\label{disteq}
dist^2(\mcrs, \mspd, \mtol) = k_2{\mtol}^2 + k_1{\mtol} + k_0,
\end{equation*}

\noindent
where

\vspace{0.18in}
\begin{tabular}{rl} 
\wikitablehint{border=0}
$k_2 = \wikiMathLarge $ &amp; $\ktwoa \wikiMathLarge $ \\
                        &amp; $\ktwob \wikiMathLarge $
\end{tabular}

\vspace{0.12in}
\begin{tabular}{rl} 
\wikitablehint{border=0}
$k_1 = \wikiMathLarge $ &amp; $ \konea \wikiMathLarge $ \\
                        &amp; $ \koneb \wikiMathLarge $
\end{tabular}

\vspace{0.12in}
\begin{tabular}{rl} 
\wikitablehint{border=0}
$k_0 = \wikiMathLarge $ &amp; $ \kzero \wikiMathLarge $ 
\end{tabular}

\vspace{0.15in}
\noindent
The stationary point is obtained by taking the first derivative with
respect to \tol:

$$dist^{2}(\mcrs, \mspd, \mtol)' = 2k_2{\mtol} + k_1.$$

\vspace{0.15in}
\noindent
Since there is no maximum distance, this stationary point always
represents the closest point of approach, and therefore:

$${\displaystyle {{\mtol}' = \frac{-k_1}{2k_2}}}.$$

\vspace{0.15in}
\noindent
The value of {\ttol} may be in the past, i.e., less than zero, if the
two vehicles are currently opening range. Or {\ttol} may be well beyond
$t$, the time length of the candidate maneuver {\dpt}. Therefore the
value of {\ttol} is clipped by $[0, t]$. Furthermore {\ttol} is zero
when the two vehicles have the same heading and speed (the only
condition where $k_2$ is zero). The actual CPA value is then obtained
by plugging {\ttol} back into first equation above. 

\begin{equation*}
CPA(\mcrs, \mspd, \mtol) = \sqrt{k_2{\mttol}^2 + k_1{\mttol} + k_0}.
\end{equation*}

\vspace{0.15in}

As mentioned before, this calculation is a common component in the
underlying utility function for behaviors dealing with relative
vehicle motion. A behavior, within a single iteration of the control
cycle, will perform a sequence of calculations on different {\dpt}
values. However, all calculations have the same values of current
vehicle position ($\oslon$, $\oslat$), and current position and
trajectory of the other vehicle ($\cnlon$, $\cnlat$, $\cncrs$,
$\cnspd$).  To make this overall sequence of calculations faster, all
terms in $k_0$, $k_1$, and $k_2$ above comprised exclusively of
$\oslon$, $\oslat$, $\cnlon$, $\cnlat$, $\cncrs$, $\cnspd$ are
calculated once and cached for later calculations.

</pre></body></html>