% Copyright (C) 2016-2021 by Paul Gaborit % % This file may be distributed and/or modified % % 1. under the LaTeX Project Public License and/or % % 2. under the GNU Public License. \usetikzlibrary{calc} \RequirePackage{etoolbox} \RequirePackage{etextools} \RequirePackage{xfp} \newbool{dubinspathreverse} \def\ifpgfmathcond#1{% \pgfmathparse{(#1)?1:0}% \ifnumequal{\pgfmathresult}{1}% } \def\anglebetween#1#2#3{%macro, s, t \path let \p{s}=(#2), \p{t}=(#3), \n{angle}={atan2(\y{t}-\y{s},\x{t}-\x{s})} in \pgfextra { \pgfmathsetmacro#1{\n{angle}} \aftergroup@def#1 }; } \def\distancebetween#1#2#3{%macro, s, t \path let \p{s}=(#2), \p{t}=(#3), %\n{dist}={veclen(\x{t}-\x{s},\y{t}-\y{s}))} \n{dist}={\fpeval{sqrt((\x{t}-\x{s})*(\x{t}-\x{s})+(\y{t}-\y{s})*(\y{t}-\y{s}))}} in \pgfextra { %\typeout{n{dist}:\n{dist}} \pgfmathsetmacro#1{\n{dist}} \aftergroup@def#1 }; } \newcommand\dubinspath@rsr{% s, sa, t, ta, as, len, at, r let \p{tr}=([shift={(\dbp@angb-90:\dbp@radius pt)}]\dbp@b), \n1={\dbp@anga+90}, \n2={\dbp@angb+90}, \n3={\n2+\dbp@lastangle} in arc(\n1:\n1-\dbp@firstangle:\dbp@rradius pt) -- ([shift={(\p{tr})}]\n3:\dbp@rradius pt) arc(\n3:\n2:\dbp@rradius pt) } \newcommand\dubinspath@lsl{% s, sa, t, ta, as, len, at, r let \p{tl}=([shift={(\dbp@angb+90:\dbp@radius pt)}]\dbp@b), \n1={\dbp@anga-90},\n2={\n1+\dbp@firstangle}, \n3={\dbp@angb-90},\n4={\n3-\dbp@lastangle} in arc(\n1:\n2:\dbp@lradius pt) -- ([shift={(\p{tl})}]\n4:\dbp@lradius pt) arc(\n4:\n3:\dbp@lradius pt) } \newcommand\dubinspath@rsl{% s, sa, t, ta, as, len, at, r let \p{tl}=([shift={(\dbp@angb+90:\dbp@radius pt)}]\dbp@b), \n1={\dbp@anga+90},\n2={\n1-\dbp@firstangle}, \n3={\dbp@angb-90},\n4={\n3-\dbp@lastangle} in arc(\n1:\n2:\dbp@rradius pt) -- ([shift={(\p{tl})}]\n4:\dbp@lradius pt) arc(\n4:\n3:\dbp@lradius pt) } \newcommand\dubinspath@lsr{% s, sa, t, ta, as, len, at, r let \p{tr}=([shift={(\dbp@angb-90:\dbp@radius pt)}]\dbp@b), \n1={\dbp@anga-90},\n2={\n1+\dbp@firstangle}, \n3={\dbp@angb+90},\n4={\n3+\dbp@lastangle} in arc(\n1:\n2:\dbp@lradius pt) -- ([shift={(\p{tr})}]\n4:\dbp@rradius pt) arc(\n4:\n3:\dbp@rradius pt) } \newcommand\dubinspath@lrl{% s, sa, t, ta, as, ai, at, r let \n1={\dbp@anga-90},\n2={\n1+\dbp@firstangle}, \n3={\dbp@angb-90},\n4={\n3-\dbp@lastangle} in arc(\n1:\n2:\dbp@lradius pt) arc(\n2+180:\n2+180-\dbp@midparam:\dbp@rradius pt) arc(\n4:\n3:\dbp@lradius pt) } \newcommand\dubinspath@rlr{% s, sa, t, ta, as, ai, at, r let \n1={\dbp@anga+90},\n2={\n1-\dbp@firstangle}, \n3={\dbp@angb+90},\n4={\n3+\dbp@lastangle} in arc(\n1:\n2:\dbp@rradius pt) arc(\n2+180:\n2+180+\dbp@midparam:\dbp@lradius pt) arc(\n4:\n3:\dbp@rradius pt) } \newcommand\dubinspath@rev@lsl{\dubinspath@rsr} \newcommand\dubinspath@rev@rsr{\dubinspath@lsl} \newcommand\dubinspath@rev@lsr{\dubinspath@lsr} \newcommand\dubinspath@rev@rsl{\dubinspath@rsl} \newcommand\dubinspath@rev@lrl{\dubinspath@rlr} \newcommand\dubinspath@rev@rlr{\dubinspath@lrl} \newcommand\dubinspath[1]{% \pgfextra{ \dubinspathset{#1} \ifbool{dubinspathreverse}{ \edef\newa{\dbp@b} \edef\newb{\dbp@a} \pgfmathsetmacro\newanga{180+\dbp@angb} \pgfmathsetmacro\newangb{180+\dbp@anga} \edef\newfirstangle{\dbp@lastangle} \edef\newlastangle{\dbp@firstangle} \edef\newmethod{rev@\dbp@method} \edef\newlradius{\dbp@rradius} \edef\newrradius{\dbp@lradius} \dubinspathset{ start point=\newa, end point=\newb, start angle=\newanga, end angle=\newangb, first angle=\newfirstangle, last angle=\newlastangle, left and right minimum radii=\newlradius pt and \newrradius pt, method=\newmethod, } }{} %\typeout{dbp@lradius:\dbp@lradius} %\typeout{dbp@rradius:\dbp@rradius} } \csname dubinspath@\dbp@method\endcsname% } % solution % \xdef\dubinspathmethod{} % type of dubins path % \xdef\dubinspathlength{} % lenght (pt) % \xdef\dubinspathfirstangle{} % first angle % \xdef\dubinspathmiddleparam{} % intermediate angle or straight distance % \xdef\dubinspathlastangle{} % last angle \def\dubinspathstore#1#2{% \expandafter\xdef\csname dubinspath@store@#1@#2\endcsname% {\csname dubinspath#2\endcsname}% } \def\dubinspathget#1#2{% \csname dubinspath@store@#1@#2\endcsname% } % \def\dubinspathsetparams#1#2#3#4#5{% method, length, fisrt angle, middle param, last angle \edef\dubinspathmethod{#1} \edef\dubinspathlength{#2} \edef\dubinspathfirstangle{#3} \edef\dubinspathmiddleparam{#4} \edef\dubinspathlastangle{#5} \ifdef{\dbp@storename}{ \foreach \p in {method,length,firstangle,middleparam,lastangle}{ \dubinspathstore{\dbp@storename}{\p} } }{} } \def\dubinspathupdateparams#1#2#3#4#5{ \ifpgfmathcond{#2<\dubinspathlength}{ \dubinspathsetparams{#1}{#2}{#3}{#4}{#5} }{} } \tikzset{ dubins path/.is family, dubins path, start point/.store in=\dbp@a, start angle/.store in=\dbp@anga, end point/.store in=\dbp@b, end angle/.store in=\dbp@angb, store/.store in=\dbp@storename, use store/.style={ method=\dubinspathget{#1}{method}, first angle=\dubinspathget{#1}{firstangle}, last angle=\dubinspathget{#1}{lastangle}, middle param=\dubinspathget{#1}{middleparam}, }, minimum radius/.code={ \pgfmathsetmacro\dbp@radius{#1} \pgfmathsetmacro\dbp@rradius{#1} \pgfmathsetmacro\dbp@lradius{#1} }, left and right minimum radii/.code args={#1 and #2}{ \pgfmathsetmacro\dbp@lradius{#1} \pgfmathsetmacro\dbp@rradius{#2} \pgfmathsetmacro\dbp@radius{(\dbp@lradius + \dbp@rradius)/2} }, method/.store in=\dbp@method, first angle/.store in=\dbp@firstangle, last angle/.store in=\dbp@lastangle, middle param/.store in=\dbp@midparam, reverse/.is if=dubinspathreverse, } \newcommand\dubinspathset[1]{\tikzset{dubins path,#1}} \newcommand\dubinspathcalc[1]{% \begingroup \dubinspathset{#1} \tikzset{ declare function={ angtodist(\a,\r)={abs(\a)*.01745329*\r}; modangr(\a,\b)={ (Mod(\a,360)=2*\radius)?1:0} \ifnumequal{\rslok}{1}{ \anglebetween\rslarbl{\p{ar}}{\p{bl}} \pgfmathsetmacro\rslanglesup{ asin(\radius/\rsldarbl*2)} \pgfmathsetmacro\rslangone{Mod(\anga-\rslarbl+\rslanglesup,360)} \pgfmathsetmacro\rslangtwo{Mod(\angb-\rslarbl+\rslanglesup,360)} \pgfmathsetmacro\rsllen{veclen(\rsldarbl,\radius)} \pgfmathsetmacro\rsldist{ angtodist(\rslangone,\radius) +\rsllen +angtodist(\rslangtwo,\radius) } \dubinspathupdateparams{rsl}{\rsldist}{\rslangone}{\rsllen}{\rslangtwo} }{} % LSR (al et br) %\typeout{LSR begin} \distancebetween\lsrdalbr{\p{al}}{\p{br}} \pgfmathtruncatemacro\lsrok{(\lsrdalbr>=2*\radius)?1:0} \ifnumequal{\lsrok}{1}{ \anglebetween\lsralbr{\p{al}}{\p{br}} \pgfmathsetmacro\lsranglesup{ asin(\radius/\lsrdalbr*2)} \pgfmathsetmacro\lsrangone{Mod(\lsralbr+\lsranglesup-\anga,360)} \pgfmathsetmacro\lsrangtwo{Mod(\lsralbr+\lsranglesup-\angb,360)} \pgfmathsetmacro\lsrlen{veclen(\lsrdalbr,\radius)} \pgfmathsetmacro\lsrdist{ angtodist(\lsrangone,\radius) +\lsrlen +angtodist(\lsrangtwo,\radius) } \dubinspathupdateparams{lsr}{\lsrdist}{\lsrangone}{\lsrlen}{\lsrangtwo} }{} % LRL (al et bl) %\typeout{LRL begin} \distancebetween\lrldalbl{\p{al}}{\p{bl}} \pgfmathtruncatemacro\lrlok{(\lrldalbl<=4*\radius)?1:0} \ifnumequal{\lrlok}{1}{ % \debug{LRL} \anglebetween\lrlalbl{\p{al}}{\p{bl}} \pgfmathsetmacro\lrlangsup{acos(\lrldalbl/\radius/4)} \pgfmathsetmacro\lrlangone{ modangl(\lrlalbl+\lrlangsup,\anga-90)-(\anga-90)} \pgfmathsetmacro\lrlangtwo{% (\angb-90)-modangr(\lrlalbl+180-\lrlangsup,\angb-90)} \pgfmathsetmacro\lrlangthree{360-2*(90-\lrlangsup)} \pgfmathsetmacro\lrldist{ angtodist(\lrlangone,\radius) +angtodist(\lrlangthree,\radius) +angtodist(\lrlangtwo,\radius) } \dubinspathupdateparams{lrl}{\lrldist}{\lrlangone}{\lrlangthree}{\lrlangtwo} }{} % RLR (ar et br) %\typeout{RLR begin} \distancebetween\rlrdarbr{\p{ar}}{\p{br}} \pgfmathtruncatemacro\rlrok{(\rlrdarbr<=4*\radius)?1:0} \ifnumequal{\rlrok}{1}{ \anglebetween\rlrarbr{\p{ar}}{\p{br}} \pgfmathsetmacro\rlrangsup{acos(\rlrdarbr/\radius/4)} \pgfmathsetmacro\rlrangone{ (\anga+90)-modangr(\rlrarbr-\rlrangsup,\anga+90)} \pgfmathsetmacro\rlrangtwo{% modangl(\rlrarbr+180+\rlrangsup,\angb+90)-(\angb+90)} \pgfmathsetmacro\rlrangthree{360-2*(90-\rlrangsup)} \pgfmathsetmacro\rlrdist{ angtodist(\rlrangone,\radius) +angtodist(\rlrangthree,\radius) +angtodist(\rlrangtwo,\radius) } \dubinspathupdateparams{rlr}{\rlrdist}{\rlrangone}{\rlrangthree}{\rlrangtwo} }{} \endpgfinterruptpath }; \endgroup } %%% Local Variables: %%% mode: latex %%% TeX-master: t %%% End: