46 #include <visp3/core/vpMath.h>
47 #include <visp3/core/vpColVector.h>
48 #include <visp3/core/vpRxyzVector.h>
50 #include <visp3/core/vpHomogeneousMatrix.h>
52 #if defined _MSC_VER && _MSC_VER >= 1200
53 #pragma warning(disable : 4723)
62 static const unsigned long __nan[2] = { 0xffffffff, 0x7fffffff };
63 #define NAN (*(const float *)__nan)
71 std::cerr <<
"Fail: IsNaN(0.0)=" <<
vpMath::isNaN(0.0) <<
" / should be false" << std::endl;
75 double num = 1.0, den = 0.0;
77 std::cerr <<
"Fail: IsNaN(1.0/0.0)=" <<
vpMath::isNaN(num / den) <<
" / should be false" << std::endl;
82 std::cerr <<
"Fail: IsNaN(NAN)=" <<
vpMath::isNaN(NAN) <<
" / should be true" << std::endl;
88 std::cerr <<
"Fail: IsNaN(0.0/0.0)=" <<
vpMath::isNaN(num / den) <<
" / should be true" << std::endl;
92 if (!
vpMath::isNaN(std::numeric_limits<double>::quiet_NaN())) {
93 std::cerr <<
"Fail: IsNaN(quiet_NaN)=" <<
vpMath::isNaN(std::numeric_limits<double>::quiet_NaN())
94 <<
" / should be true" << std::endl;
98 if (!
vpMath::isNaN(std::numeric_limits<float>::quiet_NaN())) {
99 std::cerr <<
"Fail: IsNaN(quiet_NaN)=" <<
vpMath::isNaN(std::numeric_limits<float>::quiet_NaN())
100 <<
" / should be true" << std::endl;
104 if (!
vpMath::isNaN(std::numeric_limits<double>::signaling_NaN())) {
105 std::cerr <<
"Fail: IsNaN(signaling_NaN)=" <<
vpMath::isNaN(std::numeric_limits<double>::signaling_NaN())
106 <<
" / should be true" << std::endl;
110 if (!
vpMath::isNaN(std::numeric_limits<float>::signaling_NaN())) {
111 std::cerr <<
"Fail: IsNaN(signaling_NaN)=" <<
vpMath::isNaN(std::numeric_limits<float>::signaling_NaN())
112 <<
" / should be true" << std::endl;
116 if (
vpMath::isNaN(std::numeric_limits<double>::infinity())) {
117 std::cerr <<
"Fail: IsNaN(infinity)=" <<
vpMath::isNaN(std::numeric_limits<double>::infinity())
118 <<
" / should be false" << std::endl;
123 std::cerr <<
"Fail: IsNaN(infinity)=" <<
vpMath::isNaN(std::numeric_limits<float>::infinity())
124 <<
" / should be false" << std::endl;
128 if (
vpMath::isNaN(1.0 / std::numeric_limits<double>::epsilon())) {
129 std::cerr <<
"Fail: IsNaN(1.0/epsilon)=" <<
vpMath::isNaN(1.0 / std::numeric_limits<double>::epsilon())
130 <<
" / should be false" << std::endl;
134 if (!
vpMath::isNaN(std::numeric_limits<double>::infinity() - std::numeric_limits<double>::infinity())) {
135 std::cerr <<
"Fail: IsNaN(infinity - infinity)="
136 <<
vpMath::isNaN(std::numeric_limits<double>::infinity() - std::numeric_limits<double>::infinity())
137 <<
" / should be true" << std::endl;
141 float a = 0.0f, b = 0.0f;
143 std::cerr <<
"Fail: IsNaN(0.0f/0.0f)=" <<
vpMath::isNaN(a / b) <<
" / should be true" << std::endl;
146 std::cout <<
"vpMath::isNaN is Ok !" << std::endl;
150 std::cerr <<
"Fail: vpMath::isInf(NAN)=" <<
vpMath::isInf(NAN) <<
" / should be false" << std::endl;
155 std::cerr <<
"Fail: vpMath::isInf(1.0/0.0)=" <<
vpMath::isInf(1.0 / a) <<
" / should be true" << std::endl;
160 std::cerr <<
"Fail: vpMath::isInf(0.0)=" <<
vpMath::isInf(0.0) <<
" / should be false" << std::endl;
165 std::cerr <<
"Fail: vpMath::isInf(exp(800.))=" <<
vpMath::isInf(exp(800.)) <<
" / should be true" << std::endl;
170 std::cerr <<
"Fail: vpMath::isInf(DBL_MIN/2.0)=" <<
vpMath::isInf(DBL_MIN / 2.0) <<
" / should be false"
174 std::cout <<
"vpMath::isInf is Ok !" << std::endl;
178 std::cerr <<
"Fail: vpMath::isFinite(NAN)=" <<
vpMath::isFinite(NAN) <<
" / should be false" << std::endl;
183 std::cerr <<
"Fail: vpMath::isFinite(1.0/0.0)=" <<
vpMath::isFinite(1.0 / a) <<
" / should be false" << std::endl;
188 std::cerr <<
"Fail: vpMath::isFinite(0.0)=" <<
vpMath::isFinite(0.0) <<
" / should be true" << std::endl;
193 std::cerr <<
"Fail: vpMath::isFinite(exp(800.))=" <<
vpMath::isFinite(exp(800.)) <<
" / should be false" << std::endl;
198 std::cerr <<
"Fail: vpMath::isFinite(DBL_MIN/2.0)=" <<
vpMath::isFinite(DBL_MIN / 2.0) <<
" / should be true"
204 std::cerr <<
"Fail: vpMath::isFinite(DBL_MAX)=" <<
vpMath::isFinite(std::numeric_limits<float>::max()) <<
" / should be true"
208 std::cout <<
"vpMath::isFinite is Ok !" << std::endl;
212 std::cerr <<
"Fail: vpMath::isNumber(\"123\")=" <<
vpMath::isNumber(
"123") <<
" / should be false" << std::endl;
216 std::cerr <<
"Fail: vpMath::isNumber(\"string\")=" <<
vpMath::isNumber(
"string") <<
" / should be true" << std::endl;
220 std::cerr <<
"Fail: vpMath::isNumber(\"123string\")=" <<
vpMath::isNumber(
"123string") <<
" / should be true" << std::endl;
223 std::cout <<
"vpMath::isNumber is Ok !" << std::endl;
227 std::cerr <<
"Fail: vpMath::round(2.3)=" <<
vpMath::round(2.3) <<
" / should be 2" << std::endl;
232 std::cerr <<
"Fail: vpMath::round(3.8)=" <<
vpMath::round(3.8) <<
" / should be 4" << std::endl;
237 std::cerr <<
"Fail: vpMath::round(5.5)=" <<
vpMath::round(5.5) <<
" / should be 6" << std::endl;
242 std::cerr <<
"Fail: vpMath::round(-2.3)=" <<
vpMath::round(-2.3) <<
" / should be -2" << std::endl;
247 std::cerr <<
"Fail: vpMath::round(-3.8)=" <<
vpMath::round(-3.8) <<
" / should be -4" << std::endl;
252 std::cerr <<
"Fail: vpMath::round(-5.5)=" <<
vpMath::round(-5.5) <<
" / should be -6" << std::endl;
257 std::cerr <<
"Fail: vpMath::round(0.0)=" <<
vpMath::round(0.0) <<
" / should be 0" << std::endl;
260 std::cout <<
"vpMath::round is Ok !" << std::endl;
264 char char_value = -127;
265 unsigned char uchar_value = vpMath::saturate<unsigned char>(char_value);
266 if (uchar_value != 0) {
267 std::cerr <<
"Fail: vpMath::saturate<unsigned char>(-127)=" << uchar_value <<
" / should be 0" << std::endl;
271 unsigned short ushort_value = 60000;
272 uchar_value = vpMath::saturate<unsigned char>(ushort_value);
273 if (uchar_value != UCHAR_MAX) {
274 std::cerr <<
"Fail: vpMath::saturate<unsigned char>(60000)=" << uchar_value <<
" / should be " << UCHAR_MAX
279 int int_value = 70000;
280 uchar_value = vpMath::saturate<unsigned char>(int_value);
281 if (uchar_value != UCHAR_MAX) {
282 std::cerr <<
"Fail: vpMath::saturate<unsigned char>(70000)=" << uchar_value <<
" / should be " << UCHAR_MAX
288 uchar_value = vpMath::saturate<unsigned char>(int_value);
289 if (uchar_value != 0) {
290 std::cerr <<
"Fail: vpMath::saturate<unsigned char>(-70000)=" << uchar_value <<
" / should be 0" << std::endl;
294 short short_value = 30000;
295 uchar_value = vpMath::saturate<unsigned char>(short_value);
296 if (uchar_value != UCHAR_MAX) {
297 std::cerr <<
"Fail: vpMath::saturate<unsigned char>(30000)=" << uchar_value <<
" / should be " << UCHAR_MAX
302 short_value = -30000;
303 uchar_value = vpMath::saturate<unsigned char>(short_value);
304 if (uchar_value != 0) {
305 std::cerr <<
"Fail: vpMath::saturate<unsigned char>(-30000)=" << uchar_value <<
" / should be 0" << std::endl;
309 unsigned int uint_value = 10000;
310 uchar_value = vpMath::saturate<unsigned char>(uint_value);
311 if (uchar_value != UCHAR_MAX) {
312 std::cerr <<
"Fail: vpMath::saturate<unsigned char>(10000)=" << uchar_value <<
" / should be " << UCHAR_MAX
317 float float_value = 10000.1f;
318 uchar_value = vpMath::saturate<unsigned char>(float_value);
319 if (uchar_value != UCHAR_MAX) {
320 std::cerr <<
"Fail: vpMath::saturate<unsigned char>(10000.1f)=" << uchar_value <<
" / should be " << UCHAR_MAX
325 float_value = -10000.1f;
326 uchar_value = vpMath::saturate<unsigned char>(float_value);
327 if (uchar_value != 0) {
328 std::cerr <<
"Fail: vpMath::saturate<unsigned char>(-10000.1f)=" << uchar_value <<
" / should be 0" << std::endl;
332 double double_value = 10000.1;
333 uchar_value = vpMath::saturate<unsigned char>(double_value);
334 if (uchar_value != UCHAR_MAX) {
335 std::cerr <<
"Fail: vpMath::saturate<unsigned char>(10000.0)=" << uchar_value <<
" / should be " << UCHAR_MAX
340 double_value = -10000.1;
341 uchar_value = vpMath::saturate<unsigned char>(double_value);
342 if (uchar_value != 0) {
343 std::cerr <<
"Fail: vpMath::saturate<unsigned char>(-10000.0)=" << uchar_value <<
" / should be 0" << std::endl;
346 std::cout <<
"vpMath::saturate<unsigned char>() is Ok !" << std::endl;
350 char_value = vpMath::saturate<char>(uchar_value);
351 if (char_value != SCHAR_MAX) {
352 std::cerr <<
"Fail: vpMath::saturate<char>(255)=" << char_value <<
" / should be " << SCHAR_MAX << std::endl;
356 ushort_value = 60000;
357 char_value = vpMath::saturate<char>(ushort_value);
358 if (char_value != SCHAR_MAX) {
359 std::cerr <<
"Fail: vpMath::saturate<char>(60000)=" << char_value <<
" / should be " << SCHAR_MAX << std::endl;
364 char_value = vpMath::saturate<char>(int_value);
365 if (char_value != SCHAR_MAX) {
366 std::cerr <<
"Fail: vpMath::saturate<char>(70000)=" << char_value <<
" / should be " << SCHAR_MAX << std::endl;
371 char_value = vpMath::saturate<char>(int_value);
372 if (char_value != (
char)SCHAR_MIN) {
373 std::cerr <<
"Fail: vpMath::saturate<char>(-70000)=" << char_value <<
" / should be " << SCHAR_MIN << std::endl;
378 char_value = vpMath::saturate<char>(short_value);
379 if (char_value != SCHAR_MAX) {
380 std::cerr <<
"Fail: vpMath::saturate<char>(30000)=" << char_value <<
" / should be " << SCHAR_MAX << std::endl;
384 short_value = -30000;
385 char_value = vpMath::saturate<char>(short_value);
386 if (char_value != (
char)SCHAR_MIN) {
387 std::cerr <<
"Fail: vpMath::saturate<char>(-30000)=" << char_value <<
" / should be " << SCHAR_MIN << std::endl;
392 char_value = vpMath::saturate<char>(uint_value);
393 if (char_value != SCHAR_MAX) {
394 std::cerr <<
"Fail: vpMath::saturate<char>(10000)=" << char_value <<
" / should be " << SCHAR_MAX << std::endl;
398 float_value = 10000.1f;
399 char_value = vpMath::saturate<char>(float_value);
400 if (char_value != SCHAR_MAX) {
401 std::cerr <<
"Fail: vpMath::saturate<char>(10000.1f)=" << char_value <<
" / should be " << SCHAR_MAX << std::endl;
405 float_value = -10000.1f;
406 char_value = vpMath::saturate<char>(float_value);
407 if (char_value != (
char)SCHAR_MIN) {
408 std::cerr <<
"Fail: vpMath::saturate<char>(-10000.1f)=" << char_value <<
" / should be " << SCHAR_MIN << std::endl;
412 double_value = 10000.1;
413 char_value = vpMath::saturate<char>(double_value);
414 if (char_value != SCHAR_MAX) {
415 std::cerr <<
"Fail: vpMath::saturate<char>(10000.1)=" << char_value <<
" / should be " << SCHAR_MAX << std::endl;
419 double_value = -10000.1;
420 char_value = vpMath::saturate<char>(double_value);
421 if (char_value != (
char)SCHAR_MIN) {
422 std::cerr <<
"Fail: vpMath::saturate<char>(-10000.1)=" << char_value <<
" / should be " << SCHAR_MIN << std::endl;
425 std::cout <<
"vpMath::saturate<char>() is Ok !" << std::endl;
429 ushort_value = vpMath::saturate<unsigned short>(char_value);
430 if (ushort_value != 0) {
431 std::cerr <<
"Fail: vpMath::saturate<unsigned short>(-127)=" << ushort_value <<
" / should be 0" << std::endl;
435 short_value = -30000;
436 ushort_value = vpMath::saturate<unsigned short>(short_value);
437 if (ushort_value != 0) {
438 std::cerr <<
"Fail: vpMath::saturate<unsigned short>(-30000)=" << ushort_value <<
" / should be 0" << std::endl;
443 ushort_value = vpMath::saturate<unsigned short>(int_value);
444 if (ushort_value != USHRT_MAX) {
445 std::cerr <<
"Fail: vpMath::saturate<unsigned short>(70000)=" << ushort_value <<
" / should be " << USHRT_MAX
451 ushort_value = vpMath::saturate<unsigned short>(int_value);
452 if (ushort_value != 0) {
453 std::cerr <<
"Fail: vpMath::saturate<unsigned short>(-70000)=" << ushort_value <<
" / should be 0" << std::endl;
458 ushort_value = vpMath::saturate<unsigned short>(uint_value);
459 if (ushort_value != USHRT_MAX) {
460 std::cerr <<
"Fail: vpMath::saturate<unsigned short>(70000)=" << ushort_value <<
" / should be " << USHRT_MAX
465 float_value = 70000.1f;
466 ushort_value = vpMath::saturate<unsigned short>(float_value);
467 if (ushort_value != USHRT_MAX) {
468 std::cerr <<
"Fail: vpMath::saturate<unsigned short>(70000.1f)=" << ushort_value <<
" / should be " << USHRT_MAX
473 float_value = -10000.1f;
474 ushort_value = vpMath::saturate<unsigned short>(float_value);
475 if (ushort_value != 0) {
476 std::cerr <<
"Fail: vpMath::saturate<unsigned short>(-10000.1f)=" << ushort_value <<
" / should be 0" << std::endl;
480 double_value = 70000.1;
481 ushort_value = vpMath::saturate<unsigned short>(double_value);
482 if (ushort_value != USHRT_MAX) {
483 std::cerr <<
"Fail: vpMath::saturate<unsigned short>(70000.1)=" << ushort_value <<
" / should be " << USHRT_MAX
488 double_value = -10000.1;
489 ushort_value = vpMath::saturate<unsigned short>(double_value);
490 if (ushort_value != 0) {
491 std::cerr <<
"Fail: vpMath::saturate<unsigned short>(-10000.1)=" << ushort_value <<
" / should be 0" << std::endl;
494 std::cout <<
"vpMath::saturate<unsigned short>() is Ok !" << std::endl;
497 ushort_value = 60000;
498 short_value = vpMath::saturate<short>(ushort_value);
499 if (short_value != SHRT_MAX) {
500 std::cerr <<
"Fail: vpMath::saturate<short>(60000)=" << short_value <<
" / should be " << SHRT_MAX << std::endl;
505 short_value = vpMath::saturate<short>(int_value);
506 if (short_value != SHRT_MAX) {
507 std::cerr <<
"Fail: vpMath::saturate<short>(70000)=" << short_value <<
" / should be " << SHRT_MAX << std::endl;
512 short_value = vpMath::saturate<short>(int_value);
513 if (short_value != SHRT_MIN) {
514 std::cerr <<
"Fail: vpMath::saturate<short>(-70000)=" << short_value <<
" / should be " << SHRT_MIN << std::endl;
519 short_value = vpMath::saturate<short>(uint_value);
520 if (short_value != SHRT_MAX) {
521 std::cerr <<
"Fail: vpMath::saturate<short>(70000)=" << short_value <<
" / should be " << SHRT_MAX << std::endl;
525 float_value = 70000.1f;
526 short_value = vpMath::saturate<short>(float_value);
527 if (short_value != SHRT_MAX) {
528 std::cerr <<
"Fail: vpMath::saturate<short>(70000.1f)=" << short_value <<
" / should be " << SHRT_MAX << std::endl;
532 float_value = -70000.1f;
533 short_value = vpMath::saturate<short>(float_value);
534 if (short_value != SHRT_MIN) {
535 std::cerr <<
"Fail: vpMath::saturate<short>(-70000.1f)=" << short_value <<
" / should be " << SHRT_MIN << std::endl;
539 double_value = 70000.1;
540 short_value = vpMath::saturate<short>(double_value);
541 if (short_value != SHRT_MAX) {
542 std::cerr <<
"Fail: vpMath::saturate<short>(70000.1)=" << short_value <<
" / should be " << SHRT_MAX << std::endl;
546 double_value = -70000.1;
547 short_value = vpMath::saturate<short>(double_value);
548 if (short_value != SHRT_MIN) {
549 std::cerr <<
"Fail: vpMath::saturate<short>(70000.1)=" << short_value <<
" / should be " << SHRT_MIN << std::endl;
552 std::cout <<
"vpMath::saturate<short>() is Ok !" << std::endl;
556 std::vector<double> vectorOfDoubles(10);
557 vectorOfDoubles[0] = 8.1472;
558 vectorOfDoubles[1] = 9.0579;
559 vectorOfDoubles[2] = 1.2699;
560 vectorOfDoubles[3] = 9.1338;
561 vectorOfDoubles[4] = 6.3236;
562 vectorOfDoubles[5] = 0.9754;
563 vectorOfDoubles[6] = 2.7850;
564 vectorOfDoubles[7] = 5.4688;
565 vectorOfDoubles[8] = 9.5751;
566 vectorOfDoubles[9] = 9.6489;
570 std::cerr <<
"Problem with vpMath::getMean()=" << res << std::endl;
573 std::cout <<
"vpMath::getMean() is Ok !" << std::endl;
577 std::cerr <<
"Problem with vpMath::getStdev()=" << res << std::endl;
583 std::cerr <<
"Problem with vpMath::getStdev() with Bessel correction=" << res << std::endl;
586 std::cout <<
"vpMath::getStdev() is Ok !" << std::endl;
590 std::cerr <<
"Problem with vpMath::getMedian()=" << res << std::endl;
595 vectorOfDoubles.push_back(1.5761);
598 std::cerr <<
"Problem with vpMath::getMedian()=" << res << std::endl;
601 std::cout <<
"vpMath::getMedian() is Ok !" << std::endl;
604 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
606 const double lower { -10. }, upper { +10. };
607 std::vector<double> testing_values { 5., -15., 15. };
608 std::vector<double> expected_values { 5., -10., 10. };
610 for (
size_t i = 0u; i < testing_values.size(); i++) {
611 const double clamp_val =
vpMath::clamp(testing_values.at(i), lower, upper);
612 if (!
vpMath::equal(clamp_val, expected_values.at(i), 0.001)) {
613 std::cerr <<
"Problem with vpMath::clamp()=" << clamp_val << std::endl;
617 std::cout <<
"vpMath::clamp() is Ok !" << std::endl;
624 rxyz_deg_truth[0] = 10;
625 rxyz_deg_truth[1] = 20;
626 rxyz_deg_truth[2] = -30;
628 rxyz_rad_truth[0] =
vpMath::rad(rxyz_deg_truth[0]);
629 rxyz_rad_truth[1] =
vpMath::rad(rxyz_deg_truth[1]);
630 rxyz_rad_truth[2] =
vpMath::rad(rxyz_deg_truth[2]);
635 for (
unsigned int i = 0u; i < rxyz_deg_truth.size(); i++) {
637 std::cerr <<
"Problem with vpMath::deg(vpRotationVector) " << i <<
": " << rxyz_deg[i] << std::endl;
641 std::cout <<
"vpMath::deg(vpRxyzVector) is Ok !" << std::endl;
645 for (
unsigned int i = 0u; i < rxyz_deg_truth.size(); i++) {
647 std::cerr <<
"Problem with vpMath::deg(vpColVector) " << i <<
": " << rxyz_deg[i] << std::endl;
651 std::cout <<
"vpMath::deg(vpColVector) is Ok !" << std::endl;
655 for (
unsigned int i = 0u; i < rxyz_deg_truth.size(); i++) {
657 std::cerr <<
"Problem with vpMath::rad(vpColVector) " << i <<
": " << rxyz_rad[i] << std::endl;
661 std::cout <<
"vpMath::rad(vpColVector) is Ok !" << std::endl;
665 std::cout <<
"Test succeed" << std::endl;
Implementation of column vector and the associated operations.
static bool isNaN(double value)
static double rad(double deg)
static double getMedian(const std::vector< double > &v)
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
static bool equal(double x, double y, double threshold=0.001)
static T clamp(const T &v, const T &lower, const T &upper)
static int round(double x)
static bool isFinite(double value)
static double getMean(const std::vector< double > &v)
static bool isInf(double value)
static double deg(double rad)
static bool isNumber(const std::string &str)
Implementation of a rotation vector as Euler angle minimal representation.