\section{aversive\_\-10-03-12/modules/devices/robot/obstacle\_\-avoidance/obstacle\_\-avoidance.c File Reference}
\label{obstacle__avoidance_8c}\index{aversive\_\-10-03-12/modules/devices/robot/obstacle\_\-avoidance/obstacle\_\-avoidance.c@{aversive\_\-10-03-12/modules/devices/robot/obstacle\_\-avoidance/obstacle\_\-avoidance.c}}
{\tt \#include $<$aversive.h$>$}\par
{\tt \#include $<$aversive/error.h$>$}\par
{\tt \#include $<$math.h$>$}\par
{\tt \#include $<$stdio.h$>$}\par
{\tt \#include $<$stdlib.h$>$}\par
{\tt \#include $<$string.h$>$}\par
{\tt \#include $<$obstacle\_\-avoidance.h$>$}\par
\subsection*{Defines}
\begin{CompactItemize}
\item 
\#define {\bf debug\_\-printf}(args...)
\item 
\#define {\bf MAX\_\-COEF}~5000
\item 
\#define {\bf IS\_\-IN\_\-PLAYGROUND}(pt)
\end{CompactItemize}
\subsection*{Functions}
\begin{CompactItemize}
\item 
void {\bf oa\_\-init} (void)
\item 
void {\bf oa\_\-start\_\-end\_\-points} (int32\_\-t st\_\-x, int32\_\-t st\_\-y, int32\_\-t en\_\-x, int32\_\-t en\_\-y)
\item 
{\bf oa\_\-poly\_\-t} $\ast$ {\bf oa\_\-new\_\-poly} (uint8\_\-t size)
\item 
void {\bf oa\_\-poly\_\-set\_\-point} ({\bf oa\_\-poly\_\-t} $\ast$pol, int32\_\-t x, int32\_\-t y, uint8\_\-t {\bf i})
\item 
{\bf oa\_\-point\_\-t} $\ast$ {\bf oa\_\-get\_\-path} (void)
\item 
void {\bf pts2line} (const {\bf oa\_\-ext\_\-point\_\-t} $\ast$p1, const {\bf oa\_\-ext\_\-point\_\-t} $\ast$p2, {\bf oa\_\-line\_\-t} $\ast$l)
\item 
uint8\_\-t {\bf intersect\_\-line} (const {\bf oa\_\-line\_\-t} $\ast$l1, const {\bf oa\_\-line\_\-t} $\ast$l2, {\bf oa\_\-ext\_\-point\_\-t} $\ast$p)
\item 
uint8\_\-t {\bf intersect\_\-segment} (const {\bf oa\_\-ext\_\-point\_\-t} $\ast$s1, const {\bf oa\_\-ext\_\-point\_\-t} $\ast$s2, const {\bf oa\_\-ext\_\-point\_\-t} $\ast$t1, const {\bf oa\_\-ext\_\-point\_\-t} $\ast$t2, {\bf oa\_\-ext\_\-point\_\-t} $\ast$p)
\item 
uint8\_\-t {\bf is\_\-point\_\-in\_\-poly} ({\bf oa\_\-poly\_\-t} $\ast$pol, int16\_\-t x, int16\_\-t y)
\item 
uint8\_\-t {\bf is\_\-crossing\_\-poly} ({\bf oa\_\-ext\_\-point\_\-t} p1, {\bf oa\_\-ext\_\-point\_\-t} p2, {\bf oa\_\-poly\_\-t} $\ast$pol)
\item 
uint8\_\-t {\bf calc\_\-rays} ({\bf oa\_\-poly\_\-t} $\ast$polys, uint8\_\-t npolys, uint8\_\-t $\ast$rays)
\item 
void {\bf calc\_\-rays\_\-weight} ({\bf oa\_\-poly\_\-t} $\ast$polys, uint8\_\-t npolys, uint8\_\-t $\ast$rays, uint8\_\-t ray\_\-n, uint16\_\-t $\ast$weight)
\item 
void {\bf dijkstra} (uint8\_\-t start\_\-p, uint8\_\-t start)
\item 
int8\_\-t {\bf get\_\-path} ({\bf oa\_\-poly\_\-t} $\ast$polys, uint8\_\-t $\ast$rays)
\item 
int8\_\-t {\bf oa\_\-process} (void)
\end{CompactItemize}


\subsection{Define Documentation}
\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!debug\_\-printf@{debug\_\-printf}}
\index{debug\_\-printf@{debug\_\-printf}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[debug\_\-printf]{\setlength{\rightskip}{0pt plus 5cm}\#define debug\_\-printf(args...)}\label{obstacle__avoidance_8c_acea5c89f299b0d2ae3db3be26d54114}




Definition at line 34 of file obstacle\_\-avoidance.c.\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!IS\_\-IN\_\-PLAYGROUND@{IS\_\-IN\_\-PLAYGROUND}}
\index{IS\_\-IN\_\-PLAYGROUND@{IS\_\-IN\_\-PLAYGROUND}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[IS\_\-IN\_\-PLAYGROUND]{\setlength{\rightskip}{0pt plus 5cm}\#define IS\_\-IN\_\-PLAYGROUND(pt)}\label{obstacle__avoidance_8c_b6c77d5fee2df9f45f9ef08685c6bb20}


\textbf{Value:}

\begin{Code}\begin{verbatim}( (pt).x >= PLAYGROUND_X_MIN && \
                (pt).x<=PLAYGROUND_X_MAX && \
                (pt).y >= PLAYGROUND_Y_MIN && (pt).y<=PLAYGROUND_Y_MAX )
\end{verbatim}
\end{Code}


Definition at line 443 of file obstacle\_\-avoidance.c.

Referenced by calc\_\-rays().\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!MAX\_\-COEF@{MAX\_\-COEF}}
\index{MAX\_\-COEF@{MAX\_\-COEF}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[MAX\_\-COEF]{\setlength{\rightskip}{0pt plus 5cm}\#define MAX\_\-COEF~5000}\label{obstacle__avoidance_8c_edac08b0ebac13e1c539527f43d5216a}




Definition at line 170 of file obstacle\_\-avoidance.c.

Referenced by pts2line().

\subsection{Function Documentation}
\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!calc\_\-rays@{calc\_\-rays}}
\index{calc\_\-rays@{calc\_\-rays}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[calc\_\-rays]{\setlength{\rightskip}{0pt plus 5cm}uint8\_\-t calc\_\-rays ({\bf oa\_\-poly\_\-t} $\ast$ {\em polys}, \/  uint8\_\-t {\em npolys}, \/  uint8\_\-t $\ast$ {\em rays})}\label{obstacle__avoidance_8c_03b678d07017f0636ebdbe5c6fc14e4a}




Definition at line 465 of file obstacle\_\-avoidance.c.

References debug\_\-printf, is\_\-crossing\_\-poly(), IS\_\-IN\_\-PLAYGROUND, \_\-poly::l, parse\_\-symbols::l, and obstacle\_\-avoidance::ray\_\-n.

Referenced by oa\_\-process().\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!calc\_\-rays\_\-weight@{calc\_\-rays\_\-weight}}
\index{calc\_\-rays\_\-weight@{calc\_\-rays\_\-weight}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[calc\_\-rays\_\-weight]{\setlength{\rightskip}{0pt plus 5cm}void calc\_\-rays\_\-weight ({\bf oa\_\-poly\_\-t} $\ast$ {\em polys}, \/  uint8\_\-t {\em npolys}, \/  uint8\_\-t $\ast$ {\em rays}, \/  uint8\_\-t {\em ray\_\-n}, \/  uint16\_\-t $\ast$ {\em weight})}\label{obstacle__avoidance_8c_556dbbd17fd1787fd0205dbeb5a1086d}




Definition at line 565 of file obstacle\_\-avoidance.c.

Referenced by oa\_\-process().\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!dijkstra@{dijkstra}}
\index{dijkstra@{dijkstra}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[dijkstra]{\setlength{\rightskip}{0pt plus 5cm}void dijkstra (uint8\_\-t {\em start\_\-p}, \/  uint8\_\-t {\em start})}\label{obstacle__avoidance_8c_2fc39dba8603c2414f825455aac4a419}




Definition at line 593 of file obstacle\_\-avoidance.c.

References \_\-poly::l, MAX\_\-POLY, \_\-ext\_\-point::p, obstacle\_\-avoidance::polys, \_\-ext\_\-point::pt, \_\-poly::pts, obstacle\_\-avoidance::ray\_\-n, obstacle\_\-avoidance::rays, obstacle\_\-avoidance::u, \_\-ext\_\-point::valid, \_\-ext\_\-point::weight, and obstacle\_\-avoidance::weight.

Referenced by oa\_\-process().\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!get\_\-path@{get\_\-path}}
\index{get\_\-path@{get\_\-path}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[get\_\-path]{\setlength{\rightskip}{0pt plus 5cm}int8\_\-t get\_\-path ({\bf oa\_\-poly\_\-t} $\ast$ {\em polys}, \/  uint8\_\-t $\ast$ {\em rays})}\label{obstacle__avoidance_8c_3097cd72d0e659d452c90c10a7692a79}




Definition at line 654 of file obstacle\_\-avoidance.c.

References DEBUG, E\_\-OA, MAX\_\-CHKPOINTS, \_\-ext\_\-point::p, \_\-ext\_\-point::pt, \_\-poly::pts, obstacle\_\-avoidance::res, obstacle\_\-avoidance::u, \_\-ext\_\-point::x, \_\-point::x, \_\-ext\_\-point::y, and \_\-point::y.

Referenced by oa\_\-process().\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!intersect\_\-line@{intersect\_\-line}}
\index{intersect\_\-line@{intersect\_\-line}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[intersect\_\-line]{\setlength{\rightskip}{0pt plus 5cm}uint8\_\-t intersect\_\-line (const {\bf oa\_\-line\_\-t} $\ast$ {\em l1}, \/  const {\bf oa\_\-line\_\-t} $\ast$ {\em l2}, \/  {\bf oa\_\-ext\_\-point\_\-t} $\ast$ {\em p})}\label{obstacle__avoidance_8c_b06e1bf18e9eb84c18f577ea8956b10e}




Definition at line 208 of file obstacle\_\-avoidance.c.

References \_\-line::a, \_\-line::b, \_\-line::c, debug\_\-printf, \_\-ext\_\-point::x, and \_\-ext\_\-point::y.

Referenced by intersect\_\-segment().\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!intersect\_\-segment@{intersect\_\-segment}}
\index{intersect\_\-segment@{intersect\_\-segment}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[intersect\_\-segment]{\setlength{\rightskip}{0pt plus 5cm}uint8\_\-t intersect\_\-segment (const {\bf oa\_\-ext\_\-point\_\-t} $\ast$ {\em s1}, \/  const {\bf oa\_\-ext\_\-point\_\-t} $\ast$ {\em s2}, \/  const {\bf oa\_\-ext\_\-point\_\-t} $\ast$ {\em t1}, \/  const {\bf oa\_\-ext\_\-point\_\-t} $\ast$ {\em t2}, \/  {\bf oa\_\-ext\_\-point\_\-t} $\ast$ {\em p})}\label{obstacle__avoidance_8c_03530cbd0de2619e8b4637d8b63066f0}




Definition at line 269 of file obstacle\_\-avoidance.c.

References debug\_\-printf, intersect\_\-line(), pts2line(), \_\-ext\_\-point::x, and \_\-ext\_\-point::y.

Referenced by is\_\-crossing\_\-poly().\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!is\_\-crossing\_\-poly@{is\_\-crossing\_\-poly}}
\index{is\_\-crossing\_\-poly@{is\_\-crossing\_\-poly}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[is\_\-crossing\_\-poly]{\setlength{\rightskip}{0pt plus 5cm}uint8\_\-t is\_\-crossing\_\-poly ({\bf oa\_\-ext\_\-point\_\-t} {\em p1}, \/  {\bf oa\_\-ext\_\-point\_\-t} {\em p2}, \/  {\bf oa\_\-poly\_\-t} $\ast$ {\em pol})}\label{obstacle__avoidance_8c_2e2568225a42ae7e2ff0c9d96ed73b74}




Definition at line 377 of file obstacle\_\-avoidance.c.

References debug\_\-printf, intersect\_\-segment(), \_\-poly::l, \_\-poly::pts, \_\-ext\_\-point::x, and \_\-ext\_\-point::y.

Referenced by calc\_\-rays().\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!is\_\-point\_\-in\_\-poly@{is\_\-point\_\-in\_\-poly}}
\index{is\_\-point\_\-in\_\-poly@{is\_\-point\_\-in\_\-poly}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[is\_\-point\_\-in\_\-poly]{\setlength{\rightskip}{0pt plus 5cm}uint8\_\-t is\_\-point\_\-in\_\-poly ({\bf oa\_\-poly\_\-t} $\ast$ {\em pol}, \/  int16\_\-t {\em x}, \/  int16\_\-t {\em y})}\label{obstacle__avoidance_8c_33e3c882ea312186a0a263d572f506a1}


return 

Definition at line 361 of file obstacle\_\-avoidance.c.

References \_\-ext\_\-point::x, and \_\-ext\_\-point::y.\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!oa\_\-get\_\-path@{oa\_\-get\_\-path}}
\index{oa\_\-get\_\-path@{oa\_\-get\_\-path}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[oa\_\-get\_\-path]{\setlength{\rightskip}{0pt plus 5cm}{\bf oa\_\-point\_\-t}$\ast$ oa\_\-get\_\-path (void)}\label{obstacle__avoidance_8c_47b3874400276592eacaa416aade0dc5}


get the result after a call to \doxyref{oa\_\-process()}{p.}{obstacle__avoidance_8c_40b4deae6ef3d9969c6d51908b16427e} 

Definition at line 112 of file obstacle\_\-avoidance.c.

References obstacle\_\-avoidance::res, and obstacle\_\-avoidance::u.\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!oa\_\-init@{oa\_\-init}}
\index{oa\_\-init@{oa\_\-init}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[oa\_\-init]{\setlength{\rightskip}{0pt plus 5cm}void oa\_\-init (void)}\label{obstacle__avoidance_8c_22d755b846db66d149dbdc994e886815}


Init the oa structure. Note: In the algorithm, the first polygon is a dummy one, and is used to represent the START and END points (so it has 2 vertices) 

Definition at line 42 of file obstacle\_\-avoidance.c.

References \_\-poly::l, oa\_\-start\_\-end\_\-points(), obstacle\_\-avoidance::points, obstacle\_\-avoidance::polys, and \_\-poly::pts.\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!oa\_\-new\_\-poly@{oa\_\-new\_\-poly}}
\index{oa\_\-new\_\-poly@{oa\_\-new\_\-poly}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[oa\_\-new\_\-poly]{\setlength{\rightskip}{0pt plus 5cm}{\bf oa\_\-poly\_\-t}$\ast$ oa\_\-new\_\-poly (uint8\_\-t {\em size})}\label{obstacle__avoidance_8c_a3f707513be2570f65a5fac7b1d1d3e1}


Create a new obstacle polygon. Return NULL on \doxyref{error}{p.}{structerror}. 

Definition at line 84 of file obstacle\_\-avoidance.c.

References obstacle\_\-avoidance::cur\_\-poly\_\-idx, obstacle\_\-avoidance::cur\_\-pt\_\-idx, \_\-poly::l, MAX\_\-POLY, MAX\_\-PTS, obstacle\_\-avoidance::points, obstacle\_\-avoidance::polys, and \_\-poly::pts.\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!oa\_\-poly\_\-set\_\-point@{oa\_\-poly\_\-set\_\-point}}
\index{oa\_\-poly\_\-set\_\-point@{oa\_\-poly\_\-set\_\-point}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[oa\_\-poly\_\-set\_\-point]{\setlength{\rightskip}{0pt plus 5cm}void oa\_\-poly\_\-set\_\-point ({\bf oa\_\-poly\_\-t} $\ast$ {\em pol}, \/  int32\_\-t {\em x}, \/  int32\_\-t {\em y}, \/  uint8\_\-t {\em i})}\label{obstacle__avoidance_8c_230340c441b04e8947dc7efa59650b0b}


Add a point to the polygon. 

Definition at line 101 of file obstacle\_\-avoidance.c.

References DEBUG, E\_\-OA, \_\-poly::pts, \_\-ext\_\-point::valid, \_\-ext\_\-point::weight, \_\-ext\_\-point::x, and \_\-ext\_\-point::y.\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!oa\_\-process@{oa\_\-process}}
\index{oa\_\-process@{oa\_\-process}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[oa\_\-process]{\setlength{\rightskip}{0pt plus 5cm}int8\_\-t oa\_\-process (void)}\label{obstacle__avoidance_8c_40b4deae6ef3d9969c6d51908b16427e}


process the path from start to end. Return 0 on sucess. 

Definition at line 687 of file obstacle\_\-avoidance.c.

References calc\_\-rays(), calc\_\-rays\_\-weight(), obstacle\_\-avoidance::cur\_\-poly\_\-idx, DEBUG, dijkstra(), E\_\-OA, get\_\-path(), obstacle\_\-avoidance::polys, \_\-poly::pts, obstacle\_\-avoidance::ray\_\-n, obstacle\_\-avoidance::rays, obstacle\_\-avoidance::u, obstacle\_\-avoidance::weight, \_\-ext\_\-point::x, and \_\-ext\_\-point::y.\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!oa\_\-start\_\-end\_\-points@{oa\_\-start\_\-end\_\-points}}
\index{oa\_\-start\_\-end\_\-points@{oa\_\-start\_\-end\_\-points}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[oa\_\-start\_\-end\_\-points]{\setlength{\rightskip}{0pt plus 5cm}void oa\_\-start\_\-end\_\-points (int32\_\-t {\em st\_\-x}, \/  int32\_\-t {\em st\_\-y}, \/  int32\_\-t {\em en\_\-x}, \/  int32\_\-t {\em en\_\-y})}\label{obstacle__avoidance_8c_04bcd822062597b78e8a6ef0f7263ebc}


Set the start and destination point. Return 0 on sucess 

Definition at line 56 of file obstacle\_\-avoidance.c.

References obstacle\_\-avoidance::cur\_\-poly\_\-idx, obstacle\_\-avoidance::cur\_\-pt\_\-idx, obstacle\_\-avoidance::points, \_\-ext\_\-point::valid, \_\-ext\_\-point::weight, \_\-ext\_\-point::x, and \_\-ext\_\-point::y.

Referenced by oa\_\-init().\index{obstacle\_\-avoidance.c@{obstacle\_\-avoidance.c}!pts2line@{pts2line}}
\index{pts2line@{pts2line}!obstacle_avoidance.c@{obstacle\_\-avoidance.c}}
\subsubsection[pts2line]{\setlength{\rightskip}{0pt plus 5cm}void pts2line (const {\bf oa\_\-ext\_\-point\_\-t} $\ast$ {\em p1}, \/  const {\bf oa\_\-ext\_\-point\_\-t} $\ast$ {\em p2}, \/  {\bf oa\_\-line\_\-t} $\ast$ {\em l})}\label{obstacle__avoidance_8c_7f77cea4a2bb40da116b13152ffe859a}




Definition at line 183 of file obstacle\_\-avoidance.c.

References \_\-line::a, ABS, \_\-line::b, \_\-line::c, MAX\_\-COEF, \_\-ext\_\-point::x, and \_\-ext\_\-point::y.

Referenced by intersect\_\-segment().