두빈스 경로 (Dubins Path) - 2

RSL 경로는 시작점 \(\mathbf{p}_1\) 에서 오른쪽 원을 타고 우회전한 다음 직진하고 끝점 \(\mathbf{p}_2\) 에 도착할 때까지 왼쪽에 접한 원에서 다시 좌회전하는 것으로 구성된다. 아래 그림에는 원호에서 직선으로의 전환점인 풀아웃(pull-out) 지점 \(\mathbf{q}_1\) 과 직선에서 원호로 전환점인 휠오버(wheel-over) 지점 \(\mathbf{q}_2\) 와 이를 연결하는 직선을 각각 보여준다.

 

 

풀아웃 지점 \(\mathbf{q}_1\) 과 휠오버 지점 \(\mathbf{q}_2\) 는 다음과 같이 계산할 수 있다.

 

\[ \begin{align} \mathbf{q}_1 &= \mathbf{c}_1 + ( \mathbf{q}^\prime_1 - \mathbf{c}_1 ) \cos \psi + \mathbf{e}_{rot,1} \times (\mathbf{q}^\prime_1 -\mathbf{c}_1 ) \sin \psi \tag{1} \\ \\ \mathbf{q}_2 &= \mathbf{c}_2+( \mathbf{q}^\prime_2 - \mathbf{c}_2 ) \cos \psi - \mathbf{e}_{rot,2} \times (\mathbf{q}^\prime_2- \mathbf{c}_2 ) \sin \psi \end{align} \]

여기서

 

\[ \begin{align} \mathbf{q}_1^\prime &= \mathbf{c}_1-R \ \mathbf{e}_{rot,1} \times \mathbf{e}_c \tag{2} \\ \\ \mathbf{q}_2^\prime &= \mathbf{c}_2-R \ \mathbf{e}_{rot,2} \times \mathbf{e}_c \\ \\ \psi &= \sin^{-1} \left( \frac{2R}{‖ \mathbf{c}_2- \mathbf{c}_1 ‖_2} \right) \end{align} \]

이고, \(\mathbf{e}_{rot,1}=- \mathbf{e}_n\), \(\mathbf{e}_{rot,2}= \mathbf{e}_n\), \(\mathbf{c}_1= \mathbf{c}_{r,1}\), \(\mathbf{c}_2=\mathbf{c}_{l,2}\) 이다.

두 지점 \(\mathbf{p}_1\) 에서 \(\mathbf{q}_1\) 까지 이동하는 회전 각도 \(\theta_1\) 은 다음과 같이 계산할 수 있다.

 

\[ \begin{align} & \phi_1= \cos^{-1} \left( \frac{(\mathbf{p}_1-\mathbf{c}_1 ) \cdot (\mathbf{q}_1^\prime- \mathbf{c}_1 )}{R^2} \right) \tag{3} \\ \\ & \theta_1= \begin{cases} \phi_1+ \psi, & \mbox{if } \mathbf{e}_1 \cdot \mathbf{e}_{d1} \ge 0 \\ 2 \pi -\phi_1+ \psi , & \mbox{else} \end{cases} \end{align} \]

여기서 \(\mathbf{e}_{d1}\) 은 \(\mathbf{p}_1\) 에서 \(\mathbf{q}_1^\prime\) 로 향하는 단위벡터다.

 

\[ \begin{align} \mathbf{e}_{d1}= \frac{ \mathbf{q}_1^\prime- \mathbf{p}_1}{‖ \mathbf{q}_1^\prime- \mathbf{p}_1 ‖_2} \tag{4} \end{align} \]

비슷한 방법으로 \(\mathbf{q}_2\) 에서 \(\mathbf{p}_2\) 까지 이동하는 회전 각도 \(\theta_2\) 는 다음과 같이 계산할 수 있다.

 

\[ \begin{align} & \phi_2= \cos^{-1} \left( \frac{(\mathbf{p}_2-\mathbf{c}_2 ) \cdot (\mathbf{q}_2^\prime- \mathbf{c}_2 )}{R^2} \right) \tag{5} \\ \\ & \theta_2= \begin{cases} \phi_2+ \psi, & \mbox{if } \mathbf{e}_2 \cdot \mathbf{e}_{d2} \ge 0 \\ 2 \pi -\phi_2+ \psi , & \mbox{else} \end{cases} \end{align} \]

여기서 \(\mathbf{e}_{d2}\) 는 \(\mathbf{q}_2^\prime\) 에서 \(\mathbf{p}_2\) 로 향하는 단위벡터다.

 

\[ \begin{align} \mathbf{e}_{d2}= \frac{ \mathbf{p}_2- \mathbf{q}_2^\prime}{‖ \mathbf{p}_2- \mathbf{q}_2^\prime ‖_2} \tag{6} \end{align} \]

LSR 경로는 시작점 \(\mathbf{p}_1\) 에서 왼쪽 원을 타고 좌회전한 다음 직진하고 끝점 \(\mathbf{p}_2\) 에 도착할 때까지 오른쪽에 접한 원에서 다시 우회전하는 것으로 구성된다. 아래 그림에는 원호에서 직선으로의 전환점인 풀아웃(pull-out) 지점 \(\mathbf{q}_1\) 과 직선에서 원호로 전환점인 휠오버(wheel-over) 지점 \(\mathbf{q}_2\) 와 이를 연결하는 직선을 각각 보여준다.

 

 

풀아웃 \(\mathbf{q}_1\), 휠오버 지점 \(\mathbf{q}_2\) 및 회전 각도 \(\theta_1, \ \theta_2\) 는 다음과 같이 치환하면 RSL의 경로와 동일한 방정식 (1) \(\sim\) (6)으로 계산할 수 있다.

 

\[ \begin{align} & \mathbf{e}_{rot,1}= \mathbf{e}_n \tag{7} \\ \\ & \mathbf{e}_{rot,2}=- \mathbf{e}_n \\ \\ & \mathbf{c}_1= \mathbf{c}_{l,1} \\ \\ & \mathbf{c}_2= \mathbf{c}_{r,2} \end{align} \]

RSL과 LSR의 경로를 따르는 두 지점 \(\mathbf{p}_1\) 과 \(\mathbf{p}_2\) 사이의 거리 \(s_f\) 는 원호의 길이와 직선의 길이를 합산하여 계산한다.

 

\[ \begin{align} s_f=R \theta_1+‖ \mathbf{q}_2- \mathbf{q}_1 ‖_2+R \theta_2 \tag{8} \end{align} \]

RSL과 LSR 경로의 풀아웃 \(\mathbf{q}_1\), 휠오버 \(\mathbf{q}_2\) 및 회전 각도 \(\theta_1, \ \theta_2\) 와 시작점 \(\mathbf{p}_1\) 에서 끝점 \(\mathbf{p}_2\) 까지의 이동 거리를 구하는 매트랩 (Matlab) 코드는 다음과 같다.

 

% RSL ---------------------------------------------
function param = RSL(right_c1, left_c2, p1, p2, e1, e2, R, en)

c1 = right_c1; % right
c2 = left_c2; % left

e_rot1 = -en; % right circle rotation axis
e_rot2 = en; % left circle rotation axis

[dist, q1, q2, the1, the2] = oneSother(c1, c2, p1, p2, e1, e2, R, e_rot1, e_rot2);

param.dist = dist;
param.q1 = q1;
param.q2 = q2;
param.c1 = c1;
param.c2 = c2;
param.the1 = the1;
param.the2 = the2;
param.rot1 = e_rot1;
param.rot2 = e_rot2;

end


% LSR ---------------------------------------------
function param = LSR(left_c1, right_c2, p1, p2, e1, e2, R, en)

c1 = left_c1; % left
c2 = right_c2; % right

e_rot1 = en; % left circle rotation axis
e_rot2 = -en; % right circle rotation axis

[dist, q1, q2, the1, the2] = oneSother(c1, c2, p1, p2, e1, e2, R, e_rot1, e_rot2);

param.dist = dist;
param.q1 = q1;
param.q2 = q2;
param.c1 = c1;
param.c2 = c2;
param.the1 = the1;
param.the2 = the2;
param.rot1 = e_rot1;
param.rot2 = e_rot2;

end


% common code in RSL and LSR --------------------------
function [dist, q1, q2, the1, the2] = oneSother(c1, c2, p1, p2, e1, e2, R, e_rot1, e_rot2)

e_c = (c2-c1)/norm(c2-c1);

q1_prime = c1 - R*cross(e_rot1, e_c);
e_d1 = (q1_prime-p1)/norm(q1_prime-p1);

q2_prime = c2 - R*cross(e_rot2, e_c);
e_d2 = (p2-q2_prime)/norm(p2-q2_prime);

psi = asin(2*R/norm(c2-c1));


the1 = acos((p1-c1)'*(q1_prime-c1)/R^2);
if e1'*e_d1 < 0
    the1 = 2*pi - the1 + psi;
else
    the1 = the1 + psi;
end


the2 = acos((p2-c2)'*(q2_prime-c2)/R^2);
if e2'*e_d2 < 0
    the2 = 2*pi - the2 + psi;
else
    the2 = the2 + psi;
end


q1 = c1 + (q1_prime-c1)*cos(psi) + cross(e_rot1, (q1_prime-c1))*sin(psi);
q2 = c2 + (q2_prime-c2)*cos(psi) - cross(e_rot2, (q2_prime-c2))*sin(psi);

dist = R*the1 + norm(q2-q1) + R*the2;

end

 

최종적으로 RSR, LSL, RSL, LSR 등 네 가지로 경로 중에서 최단거리 경로를 선택하면 두빈스 경로(Dubins path)가 된다. 두빈스 경로는 3차원 공간상에서 위치벡터 \(\mathbf{r}_p (l)\) 로 표현할 수 있다. 여기서 \(l\) 은 경로를 따라 이동한 거리를 나타내는 파라미터이다. 참고로 \(l=0\) 이면 \(\mathbf{r}_p (0)= \mathbf{p}_1\), \(l=s_f\) 이면 \(\mathbf{r}_p (s_f )= \mathbf{p}_2\) 이다.

 

 

식 (8)에 의하면 위치벡터 \(\mathbf{r}_p (l)\) 은 시작점에 접한 원호 상의 위치, 직선 상의 위치, 끝점에 접한 원호 상의 위치로 구분하여 계산할 수 있다.

 

\[ \begin{align} \mathbf{r}_p (l)= \begin{cases} \mathbf{c}_1+(\mathbf{p}_1- \mathbf{c}_1 ) \cos \left( \frac{l}{R} \right)+ \mathbf{e}_{rot,1} \times (\mathbf{p}_1- \mathbf{c}_1 ) \sin \left( \frac{l}{R} \right), & l \lt s_1 \\ \left( 1- \frac{l-s_1}{s_2-s_1 } \right) \mathbf{q}_1+ \frac{l-s_1}{s_2-s_1 } \mathbf{q}_2, & s_1 \le l \lt s_2 \\ \mathbf{c}_2+(\mathbf{q}_2-\mathbf{c}_2 ) \cos \left( \frac{l-s_2}{R} \right)+ \mathbf{e}_{rot,2 } \times (\mathbf{q}_2- \mathbf{c}_2 ) \sin \left( \frac{l-s_2}{R} \right), & s_2 \le l \le s_f \end{cases} \tag{9} \end{align} \]

여기서

 

\[ \begin{align} s_1 &= R \theta_1 \\ \\ s_2 &= s_1+‖ \mathbf{q}_2- \mathbf{q}_1 ‖_2 \\ \\ s_f & =s_2+R \theta_2 \end{align} \]

이다. 다음은 두빈스 경로상의 위치벡터를 구하는 매트랩 코드다.

 

l = linspace(0, dist, (pts+1)); % curve length as a parameter

rp = zeros(3, (pts+1));

s1 = R*path_param.the1; % traveled distance from p1 to q1 point
s2 = s1 + norm(q2-q1);  % distance to q2
s3 = dist;              % distance to p2

for k=1:(pts+1)
    if l(k) <= s1
        the_s = l(k)/R;
        rp(:,k) = c1 + (p1-c1)*cos(the_s) ...
                     + cross(rot1,(p1-c1))*sin(the_s);
    elseif l(k) < s2
        t = (l(k)-s1)/(s2-s1);
        rp(:,k) = (1-t)*q1 + t*q2;
    else
        the_s = (l(k)-s2)/R;
        rp(:,k) = c2 + (q2-c2)*cos(the_s) ...
                     + cross(rot2,(q2-c2))*sin(the_s); 
    end
end

 

다음 그림은 몇가지 조건에 대한 두빈스 경로의 예다.

 

 

다음은 두빈스 경로의 매트랩 전체 코드다.
test_dubins2d.m은 실행 코드다. 세 개의 벡터 \((\mathbf{p}_2- \mathbf{p}_1 )\), \(\mathbf{e}_1, \ \mathbf{e}_2\) 가 모두 동일 평면 상에 위치하도록 조정하며 이 평면의 수직벡터 \(\mathbf{e}_n\) 을 계산하는 기능도 포함한다.
dubins2d_param.m은 두빈스 경로를 생성하기 위한 주요 파라미터를 계산한다.
dubins2d_path.m은 이 파라미터를 바탕으로 두빈스 경로를 계산한다.

 

 

 

 

test_dubins2d.m

 

% Test Dubins 2D path generation
% need dubins2d_param.m, dubins2d_param.m
%
% coded by st.watermelon

clear all
close all

% data
p1 = [50 50 50]'; % starting point
p2 = [0 0 0]'; % end point
e1 = [-1 1 1]'; % direction vector at p1
e2 = [-1 1 -1]'; % direction vector at p2
R = 10; % circular arc restriction

%{
p1 = [50 0 0]'; % starting point
p2 = [0 0 0]'; % end point
e1 = [1 0 0]'; % direction vector at p1
e2 = [1 0 0]'; % direction vector at p2
R = 10; % circular arc restriction
%}

% check all vectors are in the same plane
% and compute normal vector to plane
[e1, e2, en] = chkdata(p1, p2, e1, e2);

pts = 100; % number of computing nodes
path2d = dubins2d_path(p1, p2, e1, e2, R, en, pts);

% plotting path
plot3(path2d.rp(:,1),path2d.rp(:,2), path2d.rp(:,3), 'r')
xlabel('x'), ylabel('y'), zlabel('z')
hold on
plot3(p1(1), p1(2), p1(3), 'ro', 'markersize', 10)
plot3(p2(1), p2(2), p2(3), 'r*', 'markersize', 10)


% plotting circles --------------------------------
c1 = path2d.c1;
c2 = path2d.c2;
rot1 = path2d.rot1;
rot2 = path2d.rot2;

s=0:0.01:2*pi;
for jj=1:length(s)
    ca1(:,jj) = (p1-c1)*cos(s(jj))+cross(rot1,(p1-c1))*sin(s(jj))+c1;
    ca2(:,jj) = (p2-c2)*cos(s(jj))+cross(rot2,(p2-c2))*sin(s(jj))+c2;
end
arc1=reshape(ca1,3,length(s));
arc2=reshape(ca2,3,length(s));
plot3(arc1(1,:),arc1(2,:),arc1(3,:),'k--')
plot3(arc2(1,:),arc2(2,:),arc2(3,:),'k--')
hold off



%%
function [e1, e2, en] = chkdata(p1, p2, e1, e2)

% check all vectors are in the same plane
% and compute normal vector to the plane

% change to column vector
if size(p1, 2) > 1
    p1 = p1';
end
if size(p2, 2) > 1
    p2 = p2';
end
if size(e1, 2) > 1
    e1 = e1';
end
if size(e2, 2) > 1
    e2 = e2';
end


ep = (p2-p1)/norm(p2-p1);
e1 = e1/norm(e1);
e2 = e2/norm(e2);
meas1 = ep'*e1;
meas2 = ep'*e2;

if abs(meas1) < abs(meas2)
    en = cross(ep, e1);
else
    en = cross(ep, e2);
end

if en'*en < 1e-6  % if all vectors are in parallel
    Pxyz = eye(3);
    chkx = abs(ep'*Pxyz(:,1));
    chky = abs(ep'*Pxyz(:,2));
    [mina, min_idx] = min([chkx chky]);
    en = cross(ep,Pxyz(:, min_idx));
end

en = en/norm(en);
if en'*[0 0 1]' < 0
    en = -en;
end

% compute direction vectors projected to the plane
e1 = e1 - (e1'*en)*en;
e2 = e2 - (e2'*en)*en;
e1 = e1/norm(e1);
e2 = e2/norm(e2);

end

 

 

dubins2d_param.m

 

function path_param = dubins2d_param(p1, p2, e1, e2, R, en)

% Parameters for construncting Dubins 2D Path 
% function path_param = dubins2d_param(p1, p2, e1, e2, R, en)
%    input: two points (p1, p2) and their associated moving direction (e1, e2)
%           circle radius R, normal vector to plane en
%    output: Dubins path info between p1 and p2
%            path_param.p1, .p2, .e1, .e2, .R, .en
%            path_param.c1, .c2 <-- circle
%            path_param.rot1, .rot2    <-- rotation direction
%            path_param.the1, .the2    <-- amouint of rotation at circle
%            path.param.q1, .q2  <-- departure and arrival points on circle
%            path_param.type, .dist <-- type and min_distance
%
% coded by st.watermelon
%


% calculating center points of 4 circles
[left_c1,right_c1,left_c2,right_c2] = circle_with_R(p1,p2,e1,e2,R,en);


% deafult collection
path_param.p1 = p1;
path_param.p2 = p2;
path_param.e1 = e1;
path_param.e2 = e2;
path_param.R = R;
path_param.en = en;

% compare 4 Dubins curve

param_rsr = RSR(right_c1, right_c2, p1, p2, e1, e2, R, en);
param_lsl = LSL(left_c1, left_c2, p1, p2, e1, e2, R, en);
param_rsl = RSL(right_c1, left_c2, p1, p2, e1, e2, R, en);
param_lsr = LSR(left_c1, right_c2, p1, p2, e1, e2, R, en);

% find minimum distance
[min_d, min_idx] = ...
    min([param_rsr.dist param_lsl.dist param_rsl.dist param_lsr.dist]);

switch min_idx
    case 1
        path_param.type = 'RSR';
        sel_param = param_rsr;

    case 2
        path_param.type = 'LSL';
        sel_param = param_lsl;

    case 3
        path_param.type = 'RSL';
        sel_param = param_rsl;

    case 4
        path_param.type = 'LSR';
        sel_param = param_lsr;
end

path_param.dist = sel_param.dist;
path_param.q1 = sel_param.q1;
path_param.q2 = sel_param.q2;
path_param.c1 = sel_param.c1;
path_param.c2 = sel_param.c2;
path_param.the1 = sel_param.the1;
path_param.the2 = sel_param.the2;
path_param.rot1 = sel_param.rot1;
path_param.rot2 = sel_param.rot2;


end

%%
% RSR --------------------------------------------
function param = RSR(right_c1, right_c2, p1, p2, e1, e2, R, en)

c1 = right_c1;
c2 = right_c2;
e_rot = -en;  % right circle rotation axis

[dist, q1, q2, the1, the2] = oneSone(c1, c2, p1, p2, e1, e2, R, e_rot);

param.dist = dist;
param.q1 = q1;
param.q2 = q2;
param.c1 = c1;
param.c2 = c2;
param.the1 = the1;
param.the2 = the2;
param.rot1 = e_rot;
param.rot2 = e_rot;

end


% LSL --------------------------------------------
function param = LSL(left_c1, left_c2, p1, p2, e1, e2, R, en)

c1 = left_c1;
c2 = left_c2;
e_rot = en;  % left circle rotation axis

[dist, q1, q2, the1, the2] = oneSone(c1, c2, p1, p2, e1, e2, R, e_rot);

param.dist = dist;
param.q1 = q1;
param.q2 = q2;
param.c1 = c1;
param.c2 = c2;
param.the1 = the1;
param.the2 = the2;
param.rot1 = e_rot;
param.rot2 = e_rot;

end


% common code in RSR and LSL --------------------------
function [dist, q1, q2, theta1, theta2] = oneSone(c1, c2, p1, p2, e1, e2, R, e_rot)

e_c = (c2-c1)/norm(c2-c1);

q1 = c1 - R*cross(e_rot,e_c);
e_d1 = (q1-p1)/norm(q1-p1);

q2 = c2 - R*cross(e_rot,e_c);
e_d2 = (p2-q2)/norm(p2-q2);

theta1 = acos((p1-c1)'*(q1-c1)/R^2);
if e1'*e_d1 < 0
    theta1 = 2*pi - theta1;
end

theta2 = acos((p2-c2)'*(q2-c2)/R^2);
if e2'*e_d2 < 0
    theta2 = 2*pi - theta2;
end

dist = R*theta1 + norm(q2-q1) + R*theta2;

end


%%
% RSL ---------------------------------------------
function param = RSL(right_c1, left_c2, p1, p2, e1, e2, R, en)

c1 = right_c1; % right
c2 = left_c2; % left

e_rot1 = -en; % right circle rotation axis
e_rot2 = en; % left circle rotation axis

[dist, q1, q2, the1, the2] = oneSother(c1, c2, p1, p2, e1, e2, R, e_rot1, e_rot2);

param.dist = dist;
param.q1 = q1;
param.q2 = q2;
param.c1 = c1;
param.c2 = c2;
param.the1 = the1;
param.the2 = the2;
param.rot1 = e_rot1;
param.rot2 = e_rot2;

end


% LSR ---------------------------------------------
function param = LSR(left_c1, right_c2, p1, p2, e1, e2, R, en)

c1 = left_c1; % left
c2 = right_c2; % right

e_rot1 = en; % left circle rotation axis
e_rot2 = -en; % right circle rotation axis

[dist, q1, q2, the1, the2] = oneSother(c1, c2, p1, p2, e1, e2, R, e_rot1, e_rot2);

param.dist = dist;
param.q1 = q1;
param.q2 = q2;
param.c1 = c1;
param.c2 = c2;
param.the1 = the1;
param.the2 = the2;
param.rot1 = e_rot1;
param.rot2 = e_rot2;

end


% common code in RSL and LSR --------------------------
function [dist, q1, q2, the1, the2] = oneSother(c1, c2, p1, p2, e1, e2, R, e_rot1, e_rot2)

e_c = (c2-c1)/norm(c2-c1);

q1_prime = c1 - R*cross(e_rot1, e_c);
e_d1 = (q1_prime-p1)/norm(q1_prime-p1);

q2_prime = c2 - R*cross(e_rot2, e_c);
e_d2 = (p2-q2_prime)/norm(p2-q2_prime);

psi = asin(2*R/norm(c2-c1));


the1 = acos((p1-c1)'*(q1_prime-c1)/R^2);
if e1'*e_d1 < 0
    the1 = 2*pi - the1 + psi;
else
    the1 = the1 + psi;
end


the2 = acos((p2-c2)'*(q2_prime-c2)/R^2);
if e2'*e_d2 < 0
    the2 = 2*pi - the2 + psi;
else
    the2 = the2 + psi;
end


q1 = c1 + (q1_prime-c1)*cos(psi) + cross(e_rot1, (q1_prime-c1))*sin(psi);
q2 = c2 + (q2_prime-c2)*cos(psi) - cross(e_rot2, (q2_prime-c2))*sin(psi);

dist = R*the1 + norm(q2-q1) + R*the2;

end 


%%
% -------------------------------------------------------------------
function [left_c1,right_c1,left_c2,right_c2] ...
                        = circle_with_R(p1,p2,e1,e2,R,en)

%    input: two positions p1, p2 (vectors) and directions e1, e2
% %         circle radius R, normal vector to plane en
%    output: centers of four circles
%            left_circle at p1, right circle at p1
%            left circle at p2, right circle at p2
%


% calculate the centers of the left and right circles at p1
% 'left' means 'rotation axis is the same to plane normal vector' 
left_c1 = p1 + R * cross(en, e1);
right_c1 = p1 - R * cross(en, e1);

% calculate the centers of the left and right circles at p2
left_c2 = p2 + R * cross(en, e2);
right_c2 = p2 - R * cross(en, e2);

end

 

dubins2d_path.m

 

function path2d = dubins2d_path(p1, p2, e1, e2, R, en, pts)

% Dubins 2D path
% need dubins2d_param.m
%
% function spath2d = dubins2d_path(p1, p2, e1, e2, R, en, pts)
%    input: two points (p1, p2) and their associated moving direction (e1, e2)
%           circle radius R, normal vector to plane en
%           pts : number of nodes i.e. l=linspace(0, dist, pts)
%    output: Dubins path between p1 and p2
%
% coded by st.watermelon


% confirm unit vector
e1 = e1/norm(e1);
e2 = e2/norm(e2);
en = en/norm(en);


% parameters for Dubins path generation
path_param = dubins2d_param(p1, p2, e1, e2, R, en);

dist = path_param.dist;
c1 = path_param.c1;
c2 = path_param.c2;
q1 = path_param.q1;
q2 = path_param.q2;
rot1 = path_param.rot1;
rot2 = path_param.rot2;


l = linspace(0, dist, (pts+1)); % curve length as a parameter

rp = zeros(3, (pts+1));

s1 = R*path_param.the1; % traveled distance from p1 to q1 point
s2 = s1 + norm(q2-q1);  % distance to q2
s3 = dist;              % distance to p2

for k=1:(pts+1)
    if l(k) <= s1
        the_s = l(k)/R;
        rp(:,k) = c1 + (p1-c1)*cos(the_s) ...
                     + cross(rot1,(p1-c1))*sin(the_s);
    elseif l(k) < s2
        t = (l(k)-s1)/(s2-s1);
        rp(:,k) = (1-t)*q1 + t*q2;
    else
        the_s = (l(k)-s2)/R;
        rp(:,k) = c2 + (q2-c2)*cos(the_s) ...
                     + cross(rot2,(q2-c2))*sin(the_s); 
    end
end

path2d.l = l';
path2d.rp = rp';
path2d.type = path_param.type;
path2d.dist = dist;

path2d.c1 = c1;
path2d.c2 = c2;
path2d.rot1 = rot1;
path2d.rot2 = rot2;
path2d.the1 = path_param.the1;
path2d.the2 = path_param.the2;
path2d.q1 = path_param.q1;
path2d.q2 = path_param.q2;

end