diff --git a/include/spline/bezier_curve.h b/include/spline/bezier_curve.h index e02e4fa0092037a598967aa84ef8fefe2cb9feb6..85b06a74521a48537d2a2a037862a778e05dbc58 100644 --- a/include/spline/bezier_curve.h +++ b/include/spline/bezier_curve.h @@ -72,7 +72,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> num_t nT = (t - minBound_) / (maxBound_ - minBound_); if(Safe &! (0 <= nT && nT <= 1)) { - throw; // TODO + //throw; // TODO } else { diff --git a/include/spline/cubic_function.h b/include/spline/cubic_function.h index 8705375b63765d3e64f421306650adba74b75f02..c5acdf6b50d6038dd0040b0768c1c7b5932519e4 100644 --- a/include/spline/cubic_function.h +++ b/include/spline/cubic_function.h @@ -40,7 +40,7 @@ struct cubic_function : public curve_abc<Time, Numeric, Dim, Safe, Point> cubic_function(point_t const& a, point_t const& b, point_t const& c, point_t const &d, time_t min, time_t max) :a_(a), b_(b), c_(c), d_(d), t_min_(min), t_max_(max) { - if(t_min_ >= t_max_ && Safe) + if(t_min_ > t_max_ && Safe) { std::out_of_range("TODO"); } diff --git a/include/spline/exact_cubic.h b/include/spline/exact_cubic.h index 3c2e6413d4d08ec33e8cde469970fcb65e844f6f..6958f048f850f5358507e6cbedcd3772cbfe4172 100644 --- a/include/spline/exact_cubic.h +++ b/include/spline/exact_cubic.h @@ -146,7 +146,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point> virtual point_t operator()(time_t t) const { - if(Safe && (t < subSplines_.front()->t_min_ || t > subSplines_.back()->t_max_)){throw std::out_of_range("TODO");} + if(Safe && (t < subSplines_.front()->t_min_ || t > subSplines_.back()->t_max_)){throw std::out_of_range("TODO");} for(CIT_cubic it = subSplines_.begin(); it != subSplines_.end(); ++ it) { if(t >= ((*it)->t_min_) && t <= ((*it)->t_max_)) diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp index 3fef0ca27ea228bb8b04394e7fde678195f6d5aa..0a120f686d77d9fbee904cb2ad5943bf05abb696 100644 --- a/src/tests/spline_test/Main.cpp +++ b/src/tests/spline_test/Main.cpp @@ -18,6 +18,13 @@ typedef bezier_curve <double, double, 3, true, point_t> bezier_curve_t; typedef std::pair<double, point_t> Waypoint; typedef std::vector<Waypoint> T_Waypoint; + +typedef Eigen::Matrix<double,1,1> point_one; +typedef cubic_function<double, double, 1, true, point_one> cubic_function_one; +typedef exact_cubic <double, double, 1, true, point_one> exact_cubic_one; +typedef std::pair<double, point_one> WaypointOne; +typedef std::vector<WaypointOne> T_WaypointOne; + bool QuasiEqual(const double a, const double b, const float margin) { if ((a <= 0 && b <= 0) || (a >= 0 && b>= 0)) @@ -56,6 +63,15 @@ void ComparePoints(const point_t& pt1, const point_t& pt2, const std::string& er } } +void ComparePoints(const point_one& pt1, const point_one& pt2, const std::string& errmsg, bool& error) +{ + if(!(pt1 == pt2)) + { + error = true; + std::cout << errmsg << pt1 << " ; " << pt2 << "\n"; + } +} + /*Cubic Function tests*/ void CubicFunctionTest(bool& error) @@ -247,6 +263,44 @@ void ExactCubicNoErrorTest(bool& error) } } + +/*Exact Cubic Function tests*/ +void ExactCubicTwoPointsTest(bool& error) +{ + spline::T_Waypoint waypoints; + for(double i = 0; i < 2; ++i) + { + waypoints.push_back(std::make_pair(i,point_t(i,i,i))); + } + exact_cubic_t exactCubic(waypoints.begin(), waypoints.end()); + + point_t res1 = exactCubic(0); + std::string errmsg("in ExactCubic 2 points Error While checking that given wayPoints are crossed (expected / obtained)"); + ComparePoints(point_t(0,0,0), res1, errmsg, error); + + res1 = exactCubic(1); + ComparePoints(point_t(1,1,1), res1, errmsg, error); +} + +void ExactCubicOneDimTest(bool& error) +{ + spline::T_WaypointOne waypoints; + point_one zero; zero(0,0) = 9; + point_one one; one(0,0) = 14; + point_one two; two(0,0) = 25; + waypoints.push_back(std::make_pair(0., zero)); + waypoints.push_back(std::make_pair(1., one)); + waypoints.push_back(std::make_pair(2., two)); + exact_cubic_one exactCubic(waypoints.begin(), waypoints.end()); + + point_one res1 = exactCubic(0); + std::string errmsg("in ExactCubicOneDim Error While checking that given wayPoints are crossed (expected / obtained)"); + ComparePoints(zero, res1, errmsg, error); + + res1 = exactCubic(1); + ComparePoints(one, res1, errmsg, error); +} + void ExactCubicPointsCrossedTest(bool& error) { spline::T_Waypoint waypoints; @@ -271,6 +325,8 @@ int main(int argc, char *argv[]) CubicFunctionTest(error); ExactCubicNoErrorTest(error); ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed + ExactCubicTwoPointsTest(error); + ExactCubicOneDimTest(error); //BezierCurveTest(error); if(error) {