Main.cpp 6.11 KB
 stonneau committed Jun 19, 2013 1 `````` `````` stonneau committed Jun 27, 2013 2 3 4 ``````#include "spline/cubic_function.h" #include "spline/exact_cubic.h" #include "spline/bezier_curve.h" `````` stonneau committed Jun 19, 2013 5 6 7 8 9 10 11 12 13 `````` #include #include #include using namespace std; namespace spline { `````` stonneau committed Jun 27, 2013 14 15 16 17 18 19 20 21 ``````typedef Eigen::Vector3d point_t; typedef cubic_function cubic_function_t; typedef exact_cubic exact_cubic_t; typedef bezier_curve bezier_curve_t; typedef typename std::pair Waypoint; typedef typename std::vector T_Waypoint; bool QuasiEqual(const double a, const double b, const float margin) `````` stonneau committed Jun 19, 2013 22 23 24 25 26 27 28 29 30 31 32 33 34 ``````{ if ((a <= 0 && b <= 0) || (a >= 0 && b>= 0)) { return (abs(a-b)) <= margin; } else { return abs(a) + abs(b) <= margin; } } const float margin = 0.01f; `````` stonneau committed Jun 27, 2013 35 ``````bool operator ==(const point_t& a, const point_t& b) `````` stonneau committed Jun 19, 2013 36 37 38 ``````{ return QuasiEqual(a.x(), b.x(), margin) && QuasiEqual(a.y(), b.y(), margin) && QuasiEqual(a.z(), b.z(), margin); } `````` stonneau committed Jun 27, 2013 39 `````` `````` stonneau committed Jun 19, 2013 40 41 42 43 ``````} // namespace spline using namespace spline; `````` stonneau committed Jun 27, 2013 44 ``````ostream& operator<<(ostream& os, const point_t& pt) `````` stonneau committed Jun 19, 2013 45 46 47 48 49 ``````{ os << "(" << pt.x() << ", " << pt.y() << ", " << pt.z() << ")"; return os; } `````` stonneau committed Jun 27, 2013 50 ``````void ComparePoints(const point_t& pt1, const point_t& pt2, const std::string& errmsg, bool& error) `````` stonneau committed Jun 19, 2013 51 52 53 54 55 56 57 58 59 60 61 62 63 ``````{ if(!(pt1 == pt2)) { error = true; std::cout << errmsg << pt1 << " ; " << pt2 << "\n"; } } /*Cubic Function tests*/ void CubicFunctionTest(bool& error) { std::string errMsg("In test CubicFunctionTest ; unexpected result for x "); `````` stonneau committed Jun 27, 2013 64 65 66 67 `````` point_t a(1,2,3); point_t b(2,3,4); point_t c(3,4,5); point_t d(3,6,7); `````` stonneau committed Jun 19, 2013 68 `````` `````` stonneau committed Jun 27, 2013 69 70 71 72 `````` cubic_function_t cf(a, b, c, d, 0, 1); point_t res1; res1 =cf(0); point_t x0(1,2,3); `````` stonneau committed Jun 19, 2013 73 74 `````` ComparePoints(x0, res1, errMsg + "(0) ", error); `````` stonneau committed Jun 27, 2013 75 76 77 `````` point_t x1(9,15,19); res1 =cf(1); `````` stonneau committed Jun 19, 2013 78 79 `````` ComparePoints(x1, res1, errMsg + "(1) ", error); `````` stonneau committed Jun 27, 2013 80 81 `````` point_t x2(3.125,5.25,7.125); res1 =cf(0.5); `````` stonneau committed Jun 19, 2013 82 83 `````` ComparePoints(x2, res1, errMsg + "(0.5) ", error); `````` stonneau committed Jun 27, 2013 84 85 `````` cubic_function_t cf2(a, b, c, d, 0.5, 1); res1 = cf2(0.5); `````` stonneau committed Jun 19, 2013 86 `````` ComparePoints(x0, res1, errMsg + "x3 ", error); `````` stonneau committed Jun 27, 2013 87 88 89 90 91 92 93 94 95 96 `````` error = true; try { cf2(0.4); } catch(...) { error = false; } if(error) `````` stonneau committed Jun 19, 2013 97 98 99 `````` { std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range value\n"; } `````` stonneau committed Jun 27, 2013 100 101 102 103 104 105 106 107 108 109 `````` error = true; try { cf2(1.1); } catch(...) { error = false; } if(error) `````` stonneau committed Jun 19, 2013 110 111 112 `````` { std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n"; } `````` stonneau committed Jun 27, 2013 113 114 115 116 117 118 119 120 121 122 `````` if(cf.max() != 1) { error = true; std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n"; } if(cf.min() != 0) { error = true; std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n"; } `````` stonneau committed Jun 19, 2013 123 124 ``````} `````` stonneau committed Jun 27, 2013 125 ``````/*bezier_curve Function tests*/ `````` stonneau committed Jun 27, 2013 126 127 128 129 `````` void BezierCurveTest(bool& error) { std::string errMsg("In test BezierCurveTest ; unexpected result for x "); `````` stonneau committed Jun 27, 2013 130 131 132 133 `````` point_t a(1,2,3); point_t b(2,3,4); point_t c(3,4,5); point_t d(3,6,7); `````` stonneau committed Jun 27, 2013 134 `````` `````` stonneau committed Jun 27, 2013 135 `````` std::vector params; `````` stonneau committed Jun 27, 2013 136 137 138 139 `````` params.push_back(a); params.push_back(b); // 2d curve `````` stonneau committed Jun 27, 2013 140 141 142 143 `````` bezier_curve_t cf(params.begin(), params.end()); point_t res1; res1 = cf(0); point_t x20 = a ; `````` stonneau committed Jun 27, 2013 144 145 `````` ComparePoints(x20, res1, errMsg + "2(0) ", error); `````` stonneau committed Jun 27, 2013 146 147 `````` point_t x21 = b; res1 = cf(1); `````` stonneau committed Jun 27, 2013 148 149 150 151 `````` ComparePoints(x21, res1, errMsg + "2(1) ", error); //3d curve params.push_back(c); `````` stonneau committed Jun 27, 2013 152 153 `````` bezier_curve_t cf3(params.begin(), params.end()); res1 = cf3(0); `````` stonneau committed Jun 27, 2013 154 155 `````` ComparePoints(a, res1, errMsg + "3(0) ", error); `````` stonneau committed Jun 27, 2013 156 `````` res1 = cf3(1); `````` stonneau committed Jun 27, 2013 157 158 159 160 `````` ComparePoints(c, res1, errMsg + "3(1) ", error); //4d curve params.push_back(d); `````` stonneau committed Jun 27, 2013 161 162 `````` bezier_curve_t cf4(params.begin(), params.end(), 0.4, 2); res1 = cf4(0.4); `````` stonneau committed Jun 27, 2013 163 `````` ComparePoints(a, res1, errMsg + "3(0) ", error); `````` stonneau committed Jun 27, 2013 164 165 `````` res1 = cf4(2); `````` stonneau committed Jun 27, 2013 166 167 `````` ComparePoints(d, res1, errMsg + "3(1) ", error); `````` stonneau committed Jun 27, 2013 168 `````` try `````` stonneau committed Jun 27, 2013 169 `````` { `````` stonneau committed Jun 27, 2013 170 `````` cf(-0.4); `````` stonneau committed Jun 27, 2013 171 `````` } `````` stonneau committed Jun 27, 2013 172 `````` catch(...) `````` stonneau committed Jun 27, 2013 173 `````` { `````` stonneau committed Jun 27, 2013 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 `````` error = false; } if(error) { std::cout << "Evaluation of bezier cf error, -0.4 should be an out of range value\n"; } error = true; try { cf(1.1); } catch(...) { error = false; } if(error) { std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range value\n"; `````` stonneau committed Jun 27, 2013 192 `````` } `````` stonneau committed Jun 27, 2013 193 194 195 196 197 198 199 200 201 202 `````` if(cf.max() != 1) { error = true; std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n"; } if(cf.min() != 0) { error = true; std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n"; } `````` stonneau committed Jun 27, 2013 203 204 ``````} `````` stonneau committed Jun 19, 2013 205 206 207 208 ``````/*Exact Cubic Function tests*/ void ExactCubicNoErrorTest(bool& error) { spline::T_Waypoint waypoints; `````` stonneau committed Jun 27, 2013 209 `````` for(double i = 0; i <= 1; i = i + 0.2) `````` stonneau committed Jun 19, 2013 210 `````` { `````` stonneau committed Jun 27, 2013 211 `````` waypoints.push_back(std::make_pair(i,point_t(i,i,i))); `````` stonneau committed Jun 19, 2013 212 `````` } `````` stonneau committed Jun 27, 2013 213 214 215 `````` exact_cubic_t exactCubic(waypoints.begin(), waypoints.end()); point_t res1; try `````` stonneau committed Jun 19, 2013 216 `````` { `````` stonneau committed Jun 27, 2013 217 218 `````` exactCubic(0); exactCubic(1); `````` stonneau committed Jun 19, 2013 219 `````` } `````` stonneau committed Jun 27, 2013 220 `````` catch(...) `````` stonneau committed Jun 19, 2013 221 222 `````` { error = true; `````` stonneau committed Jun 27, 2013 223 `````` std::cout << "Evaluation of ExactCubicNoErrorTest error\n"; `````` stonneau committed Jun 19, 2013 224 `````` } `````` stonneau committed Jun 27, 2013 225 226 `````` error = true; try `````` stonneau committed Jun 19, 2013 227 `````` { `````` stonneau committed Jun 27, 2013 228 229 230 231 232 233 234 235 236 `````` exactCubic(1.2); } catch(...) { error = false; } if(error) { std::cout << "Evaluation of exactCubic cf error, 1.2 should be an out of range value\n"; `````` stonneau committed Jun 19, 2013 237 `````` } `````` stonneau committed Jun 27, 2013 238 `````` if(exactCubic.max() != 1) `````` stonneau committed Jun 27, 2013 239 240 241 242 `````` { error = true; std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n"; } `````` stonneau committed Jun 27, 2013 243 `````` if(exactCubic.min() != 0) `````` stonneau committed Jun 27, 2013 244 245 246 247 `````` { error = true; std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n"; } `````` stonneau committed Jun 19, 2013 248 249 250 251 252 ``````} void ExactCubicPointsCrossedTest(bool& error) { spline::T_Waypoint waypoints; `````` stonneau committed Jun 27, 2013 253 `````` for(double i = 0; i <= 1; i = i + 0.2) `````` stonneau committed Jun 19, 2013 254 `````` { `````` stonneau committed Jun 27, 2013 255 `````` waypoints.push_back(std::make_pair(i,point_t(i,i,i))); `````` stonneau committed Jun 19, 2013 256 `````` } `````` stonneau committed Jun 27, 2013 257 258 259 `````` exact_cubic_t exactCubic(waypoints.begin(), waypoints.end()); point_t res1; for(double i = 0; i <= 1; i = i + 0.2) `````` stonneau committed Jun 19, 2013 260 `````` { `````` stonneau committed Jun 27, 2013 261 `````` res1 = exactCubic(i); `````` stonneau committed Jun 19, 2013 262 `````` std::string errmsg("Error While checking that given wayPoints are crossed (expected / obtained)"); `````` stonneau committed Jun 27, 2013 263 `````` ComparePoints(point_t(i,i,i), res1, errmsg, error); `````` stonneau committed Jun 19, 2013 264 265 266 267 268 269 270 271 272 273 `````` } } int main(int argc, char *argv[]) { std::cout << "performing tests... \n"; bool error = false; CubicFunctionTest(error); ExactCubicNoErrorTest(error); ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed `````` stonneau committed Jun 27, 2013 274 `````` //BezierCurveTest(error); `````` stonneau committed Jun 19, 2013 275 276 277 278 279 280 281 282 283 284 285 `````` if(error) { std::cout << "There were some errors\n"; return -1; } else { std::cout << "no errors found \n"; return 0; } } `````` stonneau committed Jun 27, 2013 286