Skip to content

Commit

Permalink
resample points for pattern search
Browse files Browse the repository at this point in the history
  • Loading branch information
@ebertolazzi
ebertolazzi committed Jun 29, 2024
1 parent b2ab74d commit e78464e
Show file tree
Hide file tree
Showing 22 changed files with 964 additions and 562 deletions.
53 changes: 41 additions & 12 deletions src/Clothoids/Dubins3p.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -219,19 +219,19 @@ namespace G2lib {
void build( Dubins3p const & );

//!
//! Construct a Dubins3p solution
//! Get possible point of discontinuity for the length
//!
//! \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
//! \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[out] angles angles of possibile dicontinuity
//! \return number of point computed
//!
integer
get_range_angles(
Expand All @@ -247,6 +247,35 @@ namespace G2lib {
real_type angles[]
) const;

//!
//! Get sample point for length minimization
//!
//! \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[out] angles sample points
//!
void
get_sample_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 tolerance,
vector<real_type> & angles
) const;

void
bbox(
real_type & xmin,
Expand Down
93 changes: 64 additions & 29 deletions src/Dubins.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -893,6 +893,11 @@ return m_C2.FUN(s)

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

#define OFFSET_ROOT \
real_type p, dp; \
Q.eval( x, p, dp ); \
x += 1e-4 * (dp > 0?1:-1)

integer
Dubins::get_range_angles_begin(
real_type x0,
Expand Down Expand Up @@ -932,45 +937,59 @@ 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 += 2 ) {
real_type s_x_d[2]{d,-d};
for ( integer i{0}; i < 2; ++i ) {

real_type t5 { s * d };
real_type t5 { s_x_d[i] };
real_type t6 { t5 * sb };
real_type t13 { t5 * (t2-t7) };

real_type A { -t10 * (t6 - cb) + 4*d * ((t8+t3) * s - t9) + 26 + t11 - t12 };
real_type A { -t10 * (t6 - cb) + 4* ((t8+t3) * t5 - d*t9) + 26 + t11 - t12 };
real_type B { t14 * (t13 + t3) + t16 + t5 * (40-t15) };
real_type C { d * ((t14 * t7 - 16) * sb * s -2 * d * t7) + 12*t2 + 4*t7 * (t2 + 1) + 52 };
real_type C { (t14 * t7 - 16) * sb * t5 - 2 * (d*d) * t7 + 12*t2 + 4*t7 * (t2 + 1) + 52 };
real_type D { t14 * (t13 - t3) + t16 + t5 * (t15 + 40) };
real_type E { -t10 * (t6 + cb) - 4*d * ((t3-t8) * s - t9) + 26 + t11 - t12 };
real_type E { -t10 * (t6 + cb) - 4* ((t3-t8) * t5 - d*t9) + 26 + t11 - t12 };

bool reciprocal{ std::abs(A) >= std::abs(E) };
if ( reciprocal ) {
std::swap( A, E );
std::swap( B, D );
}
Quartic Q( A, B, C, D, E );

real_type X[4];
integer nr{ Q.get_real_roots( X ) };
for ( integer ir{0}; ir < nr; ++ir ) {
// convert to angle
real_type theta{ 2*atan(X[ir]) + th };
real_type y{ X[ir] };
real_type x{ 1 };
if ( reciprocal ) std::swap( y, x );
real_type theta{ 2*atan2(y,x)+th };
minus_pi_pi( theta );
angles[npts++] = theta;
}
}
}
{ // case CSC+
real_type t{ d*d/2-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) };
real_type C { tmp + cb };
real_type s_x_d[2]{d,-d};
for ( integer i{0}; i < 2; ++i ) {
real_type tmp { s_x_d[i]*sb + t };
real_type A { tmp - cb };
real_type B { 2*(s_x_d[i]+sb) };
real_type C { tmp + cb };

Quadratic Q( A, B, C );
bool reciprocal{ std::abs(A) >= std::abs(C) };
if ( reciprocal ) std::swap( A, C );

Quadratic Q( A, B, C );
real_type X[2];
integer nr{ Q.get_real_roots( X ) };
for ( integer ir{0}; ir < nr; ++ir ) {
// convert to angle
real_type theta{ 2*atan(X[ir]) + th };
real_type y{ X[ir] };
real_type x{ 1 };
if ( reciprocal ) std::swap( y, x );
real_type theta{ 2*atan2(y,x)+th };
minus_pi_pi( theta );
angles[npts++] = theta;
}
Expand Down Expand Up @@ -1020,45 +1039,61 @@ 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 += 2 ) {
real_type s_x_d[2]{d,-d};
for ( integer i{0}; i < 2; ++i ) {

real_type t8 { s * d };
real_type t9 { t8 * sa } ;
real_type t8 { s_x_d[i] };
real_type t9 { t8 * sa };
real_type t13 { t8 * (t2-t5) };

real_type A { -4*d * ((t6 + t3) * s + t7) + t10 * (t9 + ca) + 26 + t11 - t12 };
real_type A { t10 * (t9 + ca) + t11 - t12 - 4 * ((t6 + t3) * t8 + d*t7) + 26 };
real_type B { t14 * (t13 - t3) + t16 + t8 * (t15 - 40) };
real_type C { d * (-2*d * t5 + (t14 * t5 + 16) * sa * s) + 12 * t2 + 4*t5 * (t2 + 1) + 52 };
real_type C { -2*(d*d) * t5 + (t14 * t5 + 16) * sa * t8 + 12 * t2 + 4*t5 * (t2 + 1) + 52 };
real_type D { t14 * (t13 + t3) + t16 + t8 * (-t15 - 40) };
real_type E { t10 * (t9 - ca) + t11 - t12 - 4 * d * ((t6 - t3) * s - t7) + 26 };
real_type E { t10 * (t9 - ca) + t11 - t12 - 4 * ((t6 - t3) * t8 - d*t7) + 26 };

Quartic Q( A, B, C, D, E );
bool reciprocal{ std::abs(A) >= std::abs(E) };
if ( reciprocal ) {
std::swap( A, E );
std::swap( B, D );
}

Quartic Q( A, B, C, D, E );
real_type X[4];
integer nr{ Q.get_real_roots( X ) };
for ( integer ir{0}; ir < nr; ++ir ) {
// convert to angle
real_type theta{ 2*atan(X[ir]) + th };
real_type y{ X[ir] };
real_type x{ 1 };
if ( reciprocal ) std::swap( y, x );
real_type theta{ 2*atan2(y,x)+th };
minus_pi_pi( theta );
angles[npts++] = theta;
}
}
}
{ // case CSC+
real_type t{ d*d/2-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) };
real_type C { tmp + ca };
real_type s_x_d[2]{d,-d};
for ( integer i{0}; i < 2; ++i ) {

Quadratic Q( A, B, C );
real_type tmp { s_x_d[i]*sa + t };
real_type A { tmp - ca };
real_type B { 2*(s_x_d[i]+sa) };
real_type C { tmp + ca };

bool reciprocal{ std::abs(A) >= std::abs(C) };
if ( reciprocal ) std::swap( A, C );

Quadratic Q( A, B, C );
real_type X[2];
integer nr{ Q.get_real_roots( X ) };
for ( integer ir{0}; ir < nr; ++ir ) {
// convert to angle
real_type theta{ 2*atan(X[ir]) + th };
real_type y{ X[ir] };
real_type x{ 1 };
if ( reciprocal ) std::swap( y, x );
real_type theta{ 2*atan2(y,x)+th };
minus_pi_pi( theta );
angles[npts++] = theta;
}
Expand Down
20 changes: 14 additions & 6 deletions src/Dubins3p.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -657,6 +657,13 @@ return m_Dubins1.FUN(s)
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 );
// put in [0,2pi]
for ( integer i{0}; i < npts; ++i ) {
real_type & a{angles[i]};
while ( a < 0 ) a += Utils::m_2pi;
while ( a > Utils::m_2pi ) a -= Utils::m_2pi;
}
std::sort( angles, angles + npts );
return npts;
}

Expand All @@ -665,12 +672,13 @@ return m_Dubins1.FUN(s)
ostream_type &
operator << ( ostream_type & stream, Dubins3p const & bi ) {
stream
<< "tolerance = " << bi.m_tolerance
<< "max_evaluation = " << bi.m_max_evaluation
<< "evaluation = " << bi.m_evaluation
<< "Dubins0\n" << bi.m_Dubins0
<< "Dubins1\n" << bi.m_Dubins1
<< "\n";
<< "tolerance = " << bi.m_tolerance << '\n'
<< "max_evaluation = " << bi.m_max_evaluation << '\n'
<< "evaluation = " << bi.m_evaluation << '\n'
<< "length = " << bi.length() << '\n'
<< "theta_M = " << bi.theta3_begin() << "\n\n"
<< "Dubins0\n" << bi.m_Dubins0
<< "Dubins1\n" << bi.m_Dubins1;
return stream;
}

Expand Down
Loading

0 comments on commit e78464e

Please sign in to comment.