13 #ifndef eli_geom_intersect_minimum_distance_curve_hpp
14 #define eli_geom_intersect_minimum_distance_curve_hpp
21 #include "eli/code_eli.hpp"
35 template<
template<
typename,
unsigned short,
typename>
class curve__,
typename data__,
unsigned short dim__,
typename tol__ >
43 template <
typename curve__>
47 typename curve__::point_type
pt;
49 typename curve__::data_type
operator()(
const typename curve__::data_type &t)
const
51 typename curve__::data_type tt(t);
53 if ( !(tt>=pc->get_t0()) )
55 std::cout <<
"Minimum distance curve g_functor, tt less than minimum. tt: " << tt <<
" t0: " << pc->get_t0() << std::endl;
58 if ( !(tt<=pc->get_tmax()) )
60 std::cout <<
"Minimum distance curve g_functor, tt greater than maximum. tt: " << tt <<
" tmax: " << pc->get_tmax() << std::endl;
64 assert((tt>=pc->get_t0()) && (tt<=pc->get_tmax()));
66 return (pc->f(tt)-
pt).dot(pc->fp(tt));
70 template <
typename curve__>
74 typename curve__::point_type
pt;
76 typename curve__::data_type
operator()(
const typename curve__::data_type &t)
const
78 typename curve__::data_type tt(t);
80 if ( !(tt>=pc->get_t0()) )
82 std::cout <<
"Minimum distance curve gp_functor, tt less than minimum. tt: " << tt <<
" t0: " << pc->get_t0() << std::endl;
85 if ( !(tt<=pc->get_tmax()) )
87 std::cout <<
"Minimum distance curve gp_functor, tt greater than maximum. tt: " << tt <<
" tmax: " << pc->get_tmax() << std::endl;
91 assert((tt>=pc->get_t0()) && (tt<=pc->get_tmax()));
93 typename curve__::point_type fp(pc->fp(tt));
94 typename curve__::data_type rtn(fp.dot(fp)+pc->fpp(tt).dot(pc->f(tt)-
pt));
95 typename curve__::tolerance_type tol;
97 if (tol.approximately_equal(rtn, 0))
103 if (t>=pc->get_tmax())
105 rtn=(g(pc->get_tmax())-g(static_cast<typename curve__::data_type>(pc->get_tmax()-.01)))/
static_cast<typename curve__::data_type
>(0.01);
107 else if (t<=pc->get_t0())
109 rtn=(g(pc->get_t0()+
static_cast<typename curve__::data_type
>(0.01))-g(pc->get_t0()))/
static_cast<typename curve__::data_type
>(0.01);
113 rtn=(g(t+static_cast<typename curve__::data_type>(0.01))-g(t))/static_cast<typename curve__::data_type>(0.01);
122 template<
typename curve__>
123 typename curve__::data_type
minimum_distance(
typename curve__::data_type &t,
const curve__ &c,
const typename curve__::point_type &pt,
const typename curve__::data_type &t0)
128 typename curve__::data_type dist0, dist;
129 typename curve__::tolerance_type tol;
159 assert((t>=c.get_t0()) && (t<=c.get_tmax()));
173 template<
typename curve__>
174 typename curve__::data_type
minimum_distance(
typename curve__::data_type &t,
const curve__ &c,
const typename curve__::point_type &pt)
176 typename curve__::tolerance_type tol;
177 std::list<std::pair<typename curve__::data_type, typename curve__::data_type>> tinit;
178 typename std::list<std::pair<typename curve__::data_type, typename curve__::data_type>>::iterator it;
179 std::pair<typename curve__::data_type, typename curve__::data_type> cand_pair;
182 typename curve__::data_type dist, tt, dd, tspan;
185 typename curve__::index_type i, n;
189 tspan = c.get_tmax()-c.get_t0();
192 typename curve__::data_type dt;
204 dist = std::numeric_limits<typename curve__::data_type>::max();
205 for (i = 0; i < n; i++)
215 if( tt >= c.get_tmax() )
222 typename curve__::data_type t0(t);
228 template<
typename first__,
typename second__>
229 bool pairfirstcompare(
const std::pair < first__, second__ > &a,
const std::pair < first__, second__ > &b )
231 return ( a.first < b.first );
234 template<
template<
typename,
unsigned short,
typename>
class curve__,
typename data__,
unsigned short dim__,
typename tol__>
240 typedef typename piecewise_type::curve_type curve_type;
241 typedef typename piecewise_type::data_type data_type;
242 typedef typename piecewise_type::bounding_box_type bounding_box_type;
244 typedef typename piecewise_type::segment_collection_type::const_iterator segit;
246 typedef std::vector< std::pair<data_type,segit> > dvec;
253 bounding_box_type bb_local;
259 minbbdist.push_back(std::make_pair(dbbmin, seg));
263 std::sort( minbbdist.begin(), minbbdist.end(), pairfirstcompare<data_type, segit> );
266 data_type dmin(std::numeric_limits<data_type>::max());
267 typename dvec::const_iterator it;
268 for (it=minbbdist.begin(); it!=minbbdist.end(); ++it)
271 if(it->first < dmin )
273 segit seg = it->second;
275 curve_type c(seg->second);
282 data_type tstart(seg->first);
void set_periodic_condition(const data_type &dmin, const data_type &dmax)
Definition: newton_raphson_constrained_method.hpp:58
curve::piecewise< curve1__, data1__, dim1__, tol1__ >::data_type minimum_distance(typename curve::piecewise< curve1__, data1__, dim1__, tol1__ >::data_type &t, const curve::piecewise< curve1__, data1__, dim1__, tol1__ > &pc, const typename curve::piecewise< curve1__, data1__, dim1__, tol1__ >::point_type &pt)
Derived1__::Scalar distance(const Eigen::MatrixBase< Derived1__ > &p1, const Eigen::MatrixBase< Derived2__ > &p2)
Definition: distance.hpp:33
curve__::data_type operator()(const typename curve__::data_type &t) const
Definition: minimum_distance_curve.hpp:49
curve__::point_type pt
Definition: minimum_distance_curve.hpp:74
data__ data_type
Definition: piecewise.hpp:276
curve__::data_type operator()(const typename curve__::data_type &t) const
Definition: minimum_distance_curve.hpp:76
void set_upper_condition(const data_type &d, end_condition_usage ec)
Definition: newton_raphson_constrained_method.hpp:93
Definition: piecewise.hpp:244
void get_bounding_box(bounding_box_type &bb) const
Definition: piecewise.hpp:456
Definition: newton_raphson_constrained_method.hpp:30
void set_max_iteration(const iteration_type &mi)
Definition: iterative_root_base.hpp:207
const curve__ * pc
Definition: minimum_distance_curve.hpp:73
segment_collection_type segments
Definition: piecewise.hpp:1858
void set_absolute_tolerance(const tolerance_type &abs_tol)
Definition: iterative_root_base.hpp:194
Definition: minimum_distance_curve.hpp:71
curve__::point_type pt
Definition: minimum_distance_curve.hpp:47
int find_root(data_type &root, const f__ &fun, const g__ &fprime, const data_type &f0) const
Definition: newton_raphson_method.hpp:59
void set_lower_condition(const data_type &d, end_condition_usage ec)
Definition: newton_raphson_constrained_method.hpp:72
const curve__ * pc
Definition: minimum_distance_curve.hpp:46
bool pairfirstcompare(const std::pair< first__, second__ > &a, const std::pair< first__, second__ > &b)
Definition: minimum_distance_curve.hpp:229
curve_type::point_type point_type
Definition: piecewise.hpp:272
Definition: minimum_distance_curve.hpp:44
void set_initial_guess(const data_type &xg)
Definition: newton_raphson_method.hpp:48
data_type get_delta_t(const typename segment_collection_type::iterator &it) const
Definition: piecewise.hpp:1939