Skip to content

Commit

Permalink
added Dubins3p range angles
Browse files Browse the repository at this point in the history
  • Loading branch information
@ebertolazzi
ebertolazzi committed Jun 28, 2024
1 parent 03e42f0 commit b2ab74d
Show file tree
Hide file tree
Showing 12 changed files with 222 additions and 48 deletions.
29 changes: 29 additions & 0 deletions src/Clothoids/Dubins3p.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -218,6 +218,35 @@ namespace G2lib {
void build( Dubins const & );
void build( Dubins3p const & );

//!
//! Construct a Dubins3p solution
//!
//! \param[in] xi initial position x-coordinate
//! \param[in] yi initial position y-coordinate
//! \param[in] thetai initial angle
//! \param[in] xm intermediate position x-coordinate
//! \param[in] ym intermediate position y-coordinate
//! \param[in] xf final position x-coordinate
//! \param[in] yf final position y-coordinate
//! \param[in] thetaf final angle
//! \param[in] kmax max curvature
//! \param[in] angles angles of possibile dicontinuity
//! \return number of point computed
//!
integer
get_range_angles(
real_type xi,
real_type yi,
real_type thetai,
real_type xm,
real_type ym,
real_type xf,
real_type yf,
real_type thetaf,
real_type k_max,
real_type angles[]
) const;

void
bbox(
real_type & xmin,
Expand Down
8 changes: 4 additions & 4 deletions src/Dubins.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -932,7 +932,7 @@ return m_C2.FUN(s)
real_type t15 { 16 * cb };
real_type t16 { (t10 * t7 - 48) * sb };

for ( real_type s{-1}; s != 1; s = 1 ) {
for ( real_type s{-1}; s <= 1; s += 2 ) {

real_type t5 { s * d };
real_type t6 { t5 * sb };
Expand All @@ -958,7 +958,7 @@ return m_C2.FUN(s)
}
{ // case CSC+
real_type t{ d*d/2-1 };
for ( real_type s{-1}; s != 1; s = 1 ) {
for ( real_type s{-1}; s <= 1; s += 2 ) {
real_type tmp { s*d*sb + t };
real_type A { tmp - cb };
real_type B { 2*(s*d+sb) };
Expand Down Expand Up @@ -1020,7 +1020,7 @@ return m_C2.FUN(s)
real_type t15 { 16 * ca };
real_type t16 { (t10 * t5 - 48) * sa };

for ( real_type s{-1}; s != 1; s = 1 ) {
for ( real_type s{-1}; s <= 1; s += 2 ) {

real_type t8 { s * d };
real_type t9 { t8 * sa } ;
Expand All @@ -1046,7 +1046,7 @@ return m_C2.FUN(s)
}
{ // case CSC+
real_type t{ d*d/2-1 };
for ( real_type s{-1}; s != 1; s = 1 ) {
for ( real_type s{-1}; s <= 1; s += 2 ) {
real_type tmp { s*d*sa + t };
real_type A { tmp - ca };
real_type B { 2*(s*d+sa) };
Expand Down
22 changes: 22 additions & 0 deletions src/Dubins3p.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,7 @@ namespace G2lib {
real_type thetaf,
real_type k_max
) {
m_evaluation = 0;
real_type thetam{0};
m_Dubins0.build( xi, yi, thetai, xm, ym, thetam, k_max );
m_Dubins1.build( xm, ym, thetam, xf, yf, thetaf, k_max );
Expand Down Expand Up @@ -640,6 +641,27 @@ return m_Dubins1.FUN(s)

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

integer
Dubins3p::get_range_angles(
real_type xi,
real_type yi,
real_type thetai,
real_type xm,
real_type ym,
real_type xf,
real_type yf,
real_type thetaf,
real_type k_max,
real_type angles[]
) const {
integer npts{0};
npts += m_Dubins0.get_range_angles_end ( xi, yi, thetai, xm, ym, k_max, angles );
npts += m_Dubins1.get_range_angles_begin ( xm, ym, xf, yf, thetaf, k_max, angles + npts );
return npts;
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

ostream_type &
operator << ( ostream_type & stream, Dubins3p const & bi ) {
stream
Expand Down
52 changes: 34 additions & 18 deletions src/Dubins3p_pattern.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,8 @@ namespace G2lib {
bool use_trichotomy
) {

m_evaluation = 0;

typedef struct Dubins3p_data {
Dubins D0{"temporary Dubins A"};
Dubins D1{"temporary Dubins B"};
Expand All @@ -55,19 +57,14 @@ namespace G2lib {
len = rhs.len;
}

bool
compare( Dubins3p_data const & D ) const {
return D0.solution_type() == D.D0.solution_type() &&
D1.solution_type() == D.D1.solution_type();
}
//bool
//compare( Dubins3p_data const & D ) const {
// return D0.solution_type() == D.D0.solution_type() &&
// D1.solution_type() == D.D1.solution_type();
//}

} Dubins3p_data;

integer NSEG{ integer(std::floor(Utils::m_2pi / m_sample_angle)) };

vector<Dubins3p_data> DB(NSEG);
Dubins3p_data L, C, R;

auto eval3p = [this,xi,yi,thetai,xm,ym,xf,yf,thetaf,k_max]( Dubins3p_data & D3P ) -> void {
D3P.D0.build( xi, yi, thetai, xm, ym, D3P.thetam, k_max );
D3P.D1.build( xm, ym, D3P.thetam, xf, yf, thetaf, k_max );
Expand Down Expand Up @@ -112,15 +109,34 @@ namespace G2lib {
}
};

// select angles
integer NSEG{ integer(std::floor(Utils::m_2pi / m_sample_angle)) };
vector<real_type> angles;
angles.reserve( NSEG + 12 );
for ( integer i{0}; i < NSEG; ++i ) angles.push_back( i*m_sample_angle );
{
real_type ang[12];
integer npts = this->get_range_angles( xi, yi, thetai, xm, ym, xf, yf, thetaf, k_max, ang );
for ( integer i{0}; i < npts; ++i ) {
// Find the position to insert the new value using lower_bound
auto it = std::lower_bound(angles.begin(), angles.end(), ang[i]);
angles.insert(it,ang[i]);
}
}

vector<Dubins3p_data> DB(angles.size());
Dubins3p_data L, C, R;

// initialize and find min
integer imin{0};
DB[0].thetam = 0;
m_evaluation = 0;
eval3p( DB[0] );
for ( integer i{1}; i < NSEG; ++i ) {
DB[i].thetam = i*m_sample_angle;
eval3p( DB[i] );
if ( DB[i].len < DB[imin].len ) imin = i;
integer imin{0};
real_type lmin{Utils::Inf<real_type>()};
NSEG = 0;
for ( real_type a : angles ) {
Dubins3p_data & db{ DB[NSEG] };
db.thetam = a;
eval3p( db );
if ( db.len < lmin ) { imin = NSEG; lmin = db.len; }
++NSEG;
}

// select interval
Expand Down
2 changes: 1 addition & 1 deletion src_tests/testDubins3p1.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ main() {
// Pattern trichotomy
G2lib::Dubins3p DB_PT{"D3P_pattern_trichotomy"};
DB_PT.set_tolerance(0.1*m_pi/180.0);
DB_PT.set_sample_angle(m_2pi/16);
DB_PT.set_sample_angle(m_2pi/8);

// Ellipse
G2lib::Dubins3p DB_EL{"D3P_ellipse"};
Expand Down
4 changes: 4 additions & 0 deletions toolbox/lib/Dubins3p.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,6 +131,10 @@ function sample_points( self, npts )
Dubins3pMexWrapper( 'sample_points', self.objectHandle, npts );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function angles = get_range_angles( self, xi, yi, thetai, xm, ym, xf, yf, thetaf, k_max )
angles = Dubins3pMexWrapper( 'get_range_angles', self.objectHandle, xi, yi, thetai, xm, ym, xf, yf, thetaf, k_max);
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%> Plot the Dubins
%>
%> **Usage:**
Expand Down
29 changes: 29 additions & 0 deletions toolbox/src/Clothoids/Dubins3p.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -218,6 +218,35 @@ namespace G2lib {
void build( Dubins const & );
void build( Dubins3p const & );

//!
//! Construct a Dubins3p solution
//!
//! \param[in] xi initial position x-coordinate
//! \param[in] yi initial position y-coordinate
//! \param[in] thetai initial angle
//! \param[in] xm intermediate position x-coordinate
//! \param[in] ym intermediate position y-coordinate
//! \param[in] xf final position x-coordinate
//! \param[in] yf final position y-coordinate
//! \param[in] thetaf final angle
//! \param[in] kmax max curvature
//! \param[in] angles angles of possibile dicontinuity
//! \return number of point computed
//!
integer
get_range_angles(
real_type xi,
real_type yi,
real_type thetai,
real_type xm,
real_type ym,
real_type xf,
real_type yf,
real_type thetaf,
real_type k_max,
real_type angles[]
) const;

void
bbox(
real_type & xmin,
Expand Down
8 changes: 4 additions & 4 deletions toolbox/src/Dubins.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -932,7 +932,7 @@ return m_C2.FUN(s)
real_type t15 { 16 * cb };
real_type t16 { (t10 * t7 - 48) * sb };

for ( real_type s{-1}; s != 1; s = 1 ) {
for ( real_type s{-1}; s <= 1; s += 2 ) {

real_type t5 { s * d };
real_type t6 { t5 * sb };
Expand All @@ -958,7 +958,7 @@ return m_C2.FUN(s)
}
{ // case CSC+
real_type t{ d*d/2-1 };
for ( real_type s{-1}; s != 1; s = 1 ) {
for ( real_type s{-1}; s <= 1; s += 2 ) {
real_type tmp { s*d*sb + t };
real_type A { tmp - cb };
real_type B { 2*(s*d+sb) };
Expand Down Expand Up @@ -1020,7 +1020,7 @@ return m_C2.FUN(s)
real_type t15 { 16 * ca };
real_type t16 { (t10 * t5 - 48) * sa };

for ( real_type s{-1}; s != 1; s = 1 ) {
for ( real_type s{-1}; s <= 1; s += 2 ) {

real_type t8 { s * d };
real_type t9 { t8 * sa } ;
Expand All @@ -1046,7 +1046,7 @@ return m_C2.FUN(s)
}
{ // case CSC+
real_type t{ d*d/2-1 };
for ( real_type s{-1}; s != 1; s = 1 ) {
for ( real_type s{-1}; s <= 1; s += 2 ) {
real_type tmp { s*d*sa + t };
real_type A { tmp - ca };
real_type B { 2*(s*d+sa) };
Expand Down
21 changes: 21 additions & 0 deletions toolbox/src/Dubins3p.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -640,6 +640,27 @@ return m_Dubins1.FUN(s)

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

integer
Dubins3p::get_range_angles(
real_type xi,
real_type yi,
real_type thetai,
real_type xm,
real_type ym,
real_type xf,
real_type yf,
real_type thetaf,
real_type k_max,
real_type angles[]
) const {
integer npts{0};
npts += m_Dubins0.get_range_angles_end ( xi, yi, thetai, xm, ym, k_max, angles );
npts += m_Dubins1.get_range_angles_begin ( xm, ym, xf, yf, thetaf, k_max, angles + npts );
return npts;
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

ostream_type &
operator << ( ostream_type & stream, Dubins3p const & bi ) {
stream
Expand Down
50 changes: 32 additions & 18 deletions toolbox/src/Dubins3p_pattern.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,19 +55,14 @@ namespace G2lib {
len = rhs.len;
}

bool
compare( Dubins3p_data const & D ) const {
return D0.solution_type() == D.D0.solution_type() &&
D1.solution_type() == D.D1.solution_type();
}
//bool
//compare( Dubins3p_data const & D ) const {
// return D0.solution_type() == D.D0.solution_type() &&
// D1.solution_type() == D.D1.solution_type();
//}

} Dubins3p_data;

integer NSEG{ integer(std::floor(Utils::m_2pi / m_sample_angle)) };

vector<Dubins3p_data> DB(NSEG);
Dubins3p_data L, C, R;

auto eval3p = [this,xi,yi,thetai,xm,ym,xf,yf,thetaf,k_max]( Dubins3p_data & D3P ) -> void {
D3P.D0.build( xi, yi, thetai, xm, ym, D3P.thetam, k_max );
D3P.D1.build( xm, ym, D3P.thetam, xf, yf, thetaf, k_max );
Expand Down Expand Up @@ -112,15 +107,34 @@ namespace G2lib {
}
};

// select angles
integer NSEG{ integer(std::floor(Utils::m_2pi / m_sample_angle)) };
vector<real_type> angles;
angles.reserve( NSEG + 12 );
for ( integer i{0}; i < NSEG; ++i ) angles.push_back( i*m_sample_angle );
{
real_type ang[12];
integer npts = this->get_range_angles( xi, yi, thetai, xm, ym, xf, yf, thetaf, k_max, ang );
for ( integer i{0}; i < npts; ++i ) {
// Find the position to insert the new value using lower_bound
auto it = std::lower_bound(angles.begin(), angles.end(), ang[i]);
angles.insert(it,ang[i]);
}
}

vector<Dubins3p_data> DB(angles.size());
Dubins3p_data L, C, R;

// initialize and find min
integer imin{0};
DB[0].thetam = 0;
m_evaluation = 0;
eval3p( DB[0] );
for ( integer i{1}; i < NSEG; ++i ) {
DB[i].thetam = i*m_sample_angle;
eval3p( DB[i] );
if ( DB[i].len < DB[imin].len ) imin = i;
integer imin{0};
real_type lmin{Utils::Inf<real_type>()};
NSEG = 0;
for ( real_type a : angles ) {
Dubins3p_data & db{ DB[NSEG] };
db.thetam = a;
eval3p( db );
if ( db.len < lmin ) { imin = NSEG; lmin = db.len; }
++NSEG;
}

// select interval
Expand Down
Loading

0 comments on commit b2ab74d

Please sign in to comment.