Main.cpp 6.11 KB
Newer Older
1

stonneau's avatar
stonneau committed
2
3
4
#include "spline/cubic_function.h"
#include "spline/exact_cubic.h"
#include "spline/bezier_curve.h"
5
6
7
8
9
10
11
12
13

#include <string>
#include <iostream>
#include <cmath>

using namespace std;

namespace spline
{
14
15
16
17
18
19
20
21
typedef Eigen::Vector3d point_t;
typedef cubic_function<double, double, 3, true, point_t> cubic_function_t;
typedef exact_cubic   <double, double, 3, true, point_t> exact_cubic_t;
typedef bezier_curve  <double, double, 3, true, point_t> bezier_curve_t;
typedef typename std::pair<double, point_t> Waypoint;
typedef typename std::vector<Waypoint> T_Waypoint;

bool QuasiEqual(const double a, const double b, const float margin)
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;

35
bool operator ==(const point_t& a, const point_t& b)
36
37
38
{
	return QuasiEqual(a.x(), b.x(), margin) && QuasiEqual(a.y(), b.y(), margin) && QuasiEqual(a.z(), b.z(), margin);
}
39

40
41
42
43
} // namespace spline

using namespace spline;

44
ostream& operator<<(ostream& os, const point_t& pt)
45
46
47
48
49
{
    os << "(" << pt.x() << ", " << pt.y() << ", " << pt.z() << ")";
    return os;
}

50
void ComparePoints(const point_t& pt1, const point_t& pt2, const std::string& errmsg, bool& error)
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 ");
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);
68

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);
73
74
	ComparePoints(x0, res1, errMsg + "(0) ", error);
	
75
76
77
	point_t x1(9,15,19);
	
	res1 =cf(1); 
78
79
	ComparePoints(x1, res1, errMsg + "(1) ", error);
	
80
81
	point_t x2(3.125,5.25,7.125);
	res1 =cf(0.5);
82
83
	ComparePoints(x2, res1, errMsg + "(0.5) ", error);

84
85
	cubic_function_t cf2(a, b, c, d, 0.5, 1);
	res1 = cf2(0.5); 
86
	ComparePoints(x0, res1, errMsg + "x3 ", error);
87
88
89
90
91
92
93
94
95
96
	error = true;	
	try
	{
		cf2(0.4);
	}
	catch(...)
	{
		error = false;
	}
	if(error)
97
98
99
	{
		std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range value\n";
	}
100
101
102
103
104
105
106
107
108
109
	error = true;	
	try
	{
		cf2(1.1);
	}
	catch(...)
	{
		error = false;
	}
	if(error)
110
111
112
	{
		std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n";
	}
stonneau's avatar
stonneau committed
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";
	}
123
124
}

125
/*bezier_curve Function tests*/
126
127
128
129

void BezierCurveTest(bool& error)
{
	std::string errMsg("In test BezierCurveTest ; unexpected result for x ");
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);
134

135
	std::vector<point_t> params;
136
137
138
139
	params.push_back(a);
	params.push_back(b);

	// 2d curve
140
141
142
143
	bezier_curve_t cf(params.begin(), params.end());
	point_t res1;
	res1 = cf(0); 
	point_t x20 = a ;
144
145
	ComparePoints(x20, res1, errMsg + "2(0) ", error);
	
146
147
	point_t x21 = b;
	res1 = cf(1); 
148
149
150
151
	ComparePoints(x21, res1, errMsg + "2(1) ", error);
	
	//3d curve
	params.push_back(c);
152
153
	bezier_curve_t cf3(params.begin(), params.end());
	res1 = cf3(0); 
154
155
	ComparePoints(a, res1, errMsg + "3(0) ", error);

156
	res1 = cf3(1); 
157
158
159
160
	ComparePoints(c, res1, errMsg + "3(1) ", error);

	//4d curve
	params.push_back(d);
161
162
	bezier_curve_t cf4(params.begin(), params.end(), 0.4, 2);
	res1 = cf4(0.4); 
163
	ComparePoints(a, res1, errMsg + "3(0) ", error);
164
165
	
	res1 = cf4(2); 
166
167
	ComparePoints(d, res1, errMsg + "3(1) ", error);

168
	try
169
	{
170
		cf(-0.4);
171
	}
172
	catch(...)
173
	{
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";
192
	}
stonneau's avatar
stonneau committed
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";
	}
203
204
}

205
206
207
208
/*Exact Cubic Function tests*/
void ExactCubicNoErrorTest(bool& error)
{
	spline::T_Waypoint waypoints;
209
	for(double i = 0; i <= 1; i = i + 0.2)
210
	{
211
		waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
212
	}
213
214
215
	exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
	point_t res1;
	try
216
	{
217
218
		exactCubic(0);
		exactCubic(1);
219
	}
220
	catch(...)
221
222
	{
		error = true;
223
		std::cout << "Evaluation of ExactCubicNoErrorTest error\n";
224
	}
225
226
	error = true;
	try
227
	{
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";
237
	}
stonneau's avatar
stonneau committed
238
	if(exactCubic.max() != 1)
239
240
241
242
	{
		error = true;
		std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
	}
stonneau's avatar
stonneau committed
243
	if(exactCubic.min() != 0)
244
245
246
247
	{
		error = true;
		std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
	}
248
249
250
251
252
}

void ExactCubicPointsCrossedTest(bool& error)
{
	spline::T_Waypoint waypoints;
253
	for(double i = 0; i <= 1; i = i + 0.2)
254
	{
255
		waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
256
	}
257
258
259
	exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
	point_t res1;
	for(double i = 0; i <= 1; i = i + 0.2)
260
	{
261
		res1 = exactCubic(i);
262
		std::string errmsg("Error While checking that given wayPoints are crossed (expected / obtained)");
263
		ComparePoints(point_t(i,i,i), res1, errmsg, error);
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
274
	//BezierCurveTest(error);
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;
	}
}
286