Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Correction for curves generated on the radius of curvature. #11

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
+25 −15
Open

Correction for curves generated on the radius of curvature. #11

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
10980ee
removed tracking for .so files
jacobjj Jul 24, 2019
ec6502b
added same circle check, for clockwise and anti-clockwise motion
jacobjj Jul 25, 2019
abf2613
Correct solutions for points on radius of curvature
jacobjj Jul 25, 2019
34c9495
removed redundant defs
jacobjj Aug 12, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,3 +3,4 @@ _build
build
dist
MANIFEST
**/*.so
39 changes: 24 additions & 15 deletions dubins/src/dubins.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,7 @@ typedef struct
int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3]);
int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho);


/**
* Floating point modulus suitable for rings
*
Expand Down Expand Up @@ -286,6 +287,7 @@ int dubins_extract_subpath( DubinsPath* path, double t, DubinsPath* newpath )

int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho)
{
// Add testing for curves that can be reached with a single right or left curve.
double dx, dy, D, d, theta, alpha, beta;
if( rho <= 0.0 ) {
return EDUBBADRHO;
Expand Down Expand Up @@ -313,23 +315,25 @@ int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], dou
in->cb = cos(beta);
in->c_ab = cos(alpha - beta);
in->d_sq = d * d;

return EDUBOK;
}

int dubins_LSL(DubinsIntermediateResults* in, double out[3])
{
double tmp0, tmp1, p_sq;

tmp0 = in->d + in->sa - in->sb;
p_sq = 2 + in->d_sq - (2*in->c_ab) + (2 * in->d * (in->sa - in->sb));

if(p_sq >= 0) {
tmp1 = atan2( (in->cb - in->ca), tmp0 );
out[0] = mod2pi(tmp1 - in->alpha);
out[1] = sqrt(p_sq);
out[2] = mod2pi(in->beta - tmp1);
return EDUBOK;
if (p_sq<1e-6) {
out[0] = mod2pi(in->beta - in->alpha);
out[1] = 0;
out[2] = 0;
return EDUBOK;
} else if (p_sq >= 0) {
tmp1 = atan2((in->cb - in->ca), tmp0);
out[0] = mod2pi(tmp1 - in->alpha);
out[1] = sqrt(p_sq);
out[2] = mod2pi(in->beta - tmp1);
return EDUBOK;
}
return EDUBNOPATH;
}
Expand All @@ -339,12 +343,17 @@ int dubins_RSR(DubinsIntermediateResults* in, double out[3])
{
double tmp0 = in->d - in->sa + in->sb;
double p_sq = 2 + in->d_sq - (2 * in->c_ab) + (2 * in->d * (in->sb - in->sa));
if( p_sq >= 0 ) {
double tmp1 = atan2( (in->ca - in->cb), tmp0 );
out[0] = mod2pi(in->alpha - tmp1);
out[1] = sqrt(p_sq);
out[2] = mod2pi(tmp1 -in->beta);
return EDUBOK;
if (p_sq<1e-6) {
out[0] = mod2pi(in->alpha - in->beta);
out[1] = 0;
out[2] = 0;
return EDUBOK;
} else if (p_sq >= 0) {
double tmp1 = atan2((in->ca - in->cb), tmp0);
out[0] = mod2pi(in->alpha - tmp1);
out[1] = sqrt(p_sq);
out[2] = mod2pi(tmp1 - in->beta);
return EDUBOK;
}
return EDUBNOPATH;
}
Expand Down