Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpMath.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Simple mathematical function not available in the C math library (math.h).
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
45 #ifndef vpMATH_HH
46 #define vpMATH_HH
47 
48 #include <visp3/core/vpConfig.h>
49 
50 #include <algorithm>
51 #include <climits>
52 #include <limits>
53 #include <math.h>
54 #include <vector>
55 
56 #if defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN) || defined(VISP_HAVE_FUNC_ISINF) || \
57  defined(VISP_HAVE_FUNC_STD_ISINF) || defined(VISP_HAVE_FUNC_STD_ROUND)
58 #include <cmath>
59 #endif
60 
61 #if defined(_WIN32) // Not defined in Microsoft math.h
62 
63 #ifndef M_PI
64 #define M_PI 3.14159265358979323846
65 #endif
66 
67 #ifndef M_PI_2
68 #define M_PI_2 (M_PI / 2.0)
69 #endif
70 
71 #ifndef M_PI_4
72 #define M_PI_4 (M_PI / 4.0)
73 #endif
74 
75 #endif
76 
86 class VISP_EXPORT vpMath
87 {
88 public:
95  static inline double deg(double rad) { return (rad * 180.0) / M_PI; }
96 
102  static inline double rad(double deg) { return (deg * M_PI) / 180.0; }
103 
108  static inline double sqr(double x) { return x * x; }
109 
110  // factorial of x
111  static inline double fact(unsigned int x);
112 
113  // combinaison
114  static inline long double comb(unsigned int n, unsigned int p);
115 
116  // round x to the nearest integer
117  static inline int round(const double x);
118 
119  // return the sign of x (+-1)
120  static inline int(sign)(double x);
121 
122  // test if a number equals 0 (with threshold value)
123  static inline bool nul(double x, double s = 0.001);
124 
125  // test if two numbers are equals (with a user defined threshold)
126  static inline bool equal(double x, double y, double s = 0.001);
127 
128  // test if a number is greater than another (with a user defined threshold)
129  static inline bool greater(double x, double y, double s = 0.001);
130 
137  template <class Type> static Type maximum(const Type &a, const Type &b) { return (a > b) ? a : b; }
138 
145  template <class Type> static Type minimum(const Type &a, const Type &b) { return (a < b) ? a : b; }
146 
152  template <class Type> static Type abs(const Type &x) { return (x < 0) ? -x : x; }
153 
154  // sinus cardinal
155  static double sinc(double x);
156  static double sinc(double sinx, double x);
157  static double mcosc(double cosx, double x);
158  static double msinc(double sinx, double x);
159 
160  // sigmoid
161  static inline double sigmoid(double x, double x0 = 0., double x1 = 1., double n = 12.);
162 
169  template <class Type> static void swap(Type &a, Type &b)
170  {
171  Type tmp = b;
172  b = a;
173  a = tmp;
174  }
175 
176  static bool isNaN(const double value);
177  static bool isInf(const double value);
178 
179  template <typename _Tp> static inline _Tp saturate(unsigned char v) { return _Tp(v); }
180  template <typename _Tp> static inline _Tp saturate(char v) { return _Tp(v); }
181  template <typename _Tp> static inline _Tp saturate(unsigned short v) { return _Tp(v); }
182  template <typename _Tp> static inline _Tp saturate(short v) { return _Tp(v); }
183  template <typename _Tp> static inline _Tp saturate(unsigned v) { return _Tp(v); }
184  template <typename _Tp> static inline _Tp saturate(int v) { return _Tp(v); }
185  template <typename _Tp> static inline _Tp saturate(float v) { return _Tp(v); }
186  template <typename _Tp> static inline _Tp saturate(double v) { return _Tp(v); }
187 
188  static double getMean(const std::vector<double> &v);
189  static double getMedian(const std::vector<double> &v);
190  static double getStdev(const std::vector<double> &v, const bool useBesselCorrection = false);
191 
192  static int modulo(const int a, const int n);
193 
194 private:
195  static const double ang_min_sinc;
196  static const double ang_min_mc;
197 };
198 
199 // Begining of the inline functions definition
200 
205 double vpMath::fact(unsigned int x)
206 {
207  if ((x == 1) || (x == 0))
208  return 1;
209  return x * fact(x - 1);
210 }
211 
220 long double vpMath::comb(unsigned int n, unsigned int p)
221 {
222  if (n == p)
223  return 1;
224  return fact(n) / (fact(n - p) * fact(p));
225 }
226 
235 int vpMath::round(const double x)
236 {
237 #if defined(VISP_HAVE_FUNC_ROUND)
238  //:: to design the global namespace and avoid to call recursively
239  // vpMath::round
240  return (int)::round(x);
241 #elif defined(VISP_HAVE_FUNC_STD_ROUND)
242  return (int)std::round(x);
243 #else
244  return (x > 0.0) ? ((int)floor(x + 0.5)) : ((int)ceil(x - 0.5));
245 #endif
246 }
247 
254 int
255 #ifndef DOXYGEN_SHOULD_SKIP_THIS
256  (
257 #endif
259 #ifndef DOXYGEN_SHOULD_SKIP_THIS
260  )
261 #endif
262  (double x)
263 {
264  if (fabs(x) < std::numeric_limits<double>::epsilon())
265  return 0;
266  else {
267  if (x < 0)
268  return -1;
269  else
270  return 1;
271  }
272 }
273 
281 bool vpMath::nul(double x, double s) { return (fabs(x) < s); }
282 
290 bool vpMath::equal(double x, double y, double s) { return (nul(x - y, s)); }
291 
299 bool vpMath::greater(double x, double y, double s) { return (x > (y - s)); }
300 
310 double vpMath::sigmoid(double x, double x0, double x1, double n)
311 {
312  if (x < x0)
313  return 0.;
314  else if (x > x1)
315  return 1.;
316  double l0 = 1. / (1. + exp(0.5 * n));
317  double l1 = 1. / (1. + exp(-0.5 * n));
318  return (1. / (1. + exp(-n * ((x - x0) / (x1 - x0) - 0.5))) - l0) / (l1 - l0);
319 }
320 
321 // unsigned char
322 template <> inline unsigned char vpMath::saturate<unsigned char>(char v)
323 {
324  // On big endian arch like powerpc, char implementation is unsigned
325  // with CHAR_MIN=0, CHAR_MAX=255 and SCHAR_MIN=-128, SCHAR_MAX=127
326  // leading to (int)(char -127) = 129.
327  // On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to
328  // (int)(char -127) = -127.
329  if (std::numeric_limits<char>::is_signed)
330  return (unsigned char)(((std::max))((int)v, 0));
331  else
332  return (unsigned char)((unsigned int)v > SCHAR_MAX ? 0 : v);
333 }
334 
335 template <> inline unsigned char vpMath::saturate<unsigned char>(unsigned short v)
336 {
337  return (unsigned char)((std::min))((unsigned int)v, (unsigned int)UCHAR_MAX);
338 }
339 
340 template <> inline unsigned char vpMath::saturate<unsigned char>(int v)
341 {
342  return (unsigned char)((unsigned int)v <= UCHAR_MAX ? v : v > 0 ? UCHAR_MAX : 0);
343 }
344 
345 template <> inline unsigned char vpMath::saturate<unsigned char>(short v) { return saturate<unsigned char>((int)v); }
346 
347 template <> inline unsigned char vpMath::saturate<unsigned char>(unsigned int v)
348 {
349  return (unsigned char)((std::min))(v, (unsigned int)UCHAR_MAX);
350 }
351 
352 template <> inline unsigned char vpMath::saturate<unsigned char>(float v)
353 {
354  int iv = vpMath::round(v);
355  return saturate<unsigned char>(iv);
356 }
357 
358 template <> inline unsigned char vpMath::saturate<unsigned char>(double v)
359 {
360  int iv = vpMath::round(v);
361  return saturate<unsigned char>(iv);
362 }
363 
364 // char
365 template <> inline char vpMath::saturate<char>(unsigned char v) { return (char)((std::min))((int)v, SCHAR_MAX); }
366 
367 template <> inline char vpMath::saturate<char>(unsigned short v)
368 {
369  return (char)((std::min))((unsigned int)v, (unsigned int)SCHAR_MAX);
370 }
371 
372 template <> inline char vpMath::saturate<char>(int v)
373 {
374  return (char)((unsigned int)(v - SCHAR_MIN) <= (unsigned int)UCHAR_MAX ? v : v > 0 ? SCHAR_MAX : SCHAR_MIN);
375 }
376 
377 template <> inline char vpMath::saturate<char>(short v) { return saturate<char>((int)v); }
378 
379 template <> inline char vpMath::saturate<char>(unsigned int v)
380 {
381  return (char)((std::min))(v, (unsigned int)SCHAR_MAX);
382 }
383 
384 template <> inline char vpMath::saturate<char>(float v)
385 {
386  int iv = vpMath::round(v);
387  return saturate<char>(iv);
388 }
389 
390 template <> inline char vpMath::saturate<char>(double v)
391 {
392  int iv = vpMath::round(v);
393  return saturate<char>(iv);
394 }
395 
396 // unsigned short
397 template <> inline unsigned short vpMath::saturate<unsigned short>(char v)
398 {
399  // On big endian arch like powerpc, char implementation is unsigned
400  // with CHAR_MIN=0, CHAR_MAX=255 and SCHAR_MIN=-128, SCHAR_MAX=127
401  // leading to (int)(char -127) = 129.
402  // On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to
403  // (int)(char -127) = -127.
404  if (std::numeric_limits<char>::is_signed)
405  return (unsigned char)(((std::max))((int)v, 0));
406  else
407  return (unsigned char)((unsigned int)v > SCHAR_MAX ? 0 : v);
408 }
409 
410 template <> inline unsigned short vpMath::saturate<unsigned short>(short v)
411 {
412  return (unsigned short)((std::max))((int)v, 0);
413 }
414 
415 template <> inline unsigned short vpMath::saturate<unsigned short>(int v)
416 {
417  return (unsigned short)((unsigned int)v <= (unsigned int)USHRT_MAX ? v : v > 0 ? USHRT_MAX : 0);
418 }
419 
420 template <> inline unsigned short vpMath::saturate<unsigned short>(unsigned int v)
421 {
422  return (unsigned short)((std::min))(v, (unsigned int)USHRT_MAX);
423 }
424 
425 template <> inline unsigned short vpMath::saturate<unsigned short>(float v)
426 {
427  int iv = vpMath::round(v);
428  return vpMath::saturate<unsigned short>(iv);
429 }
430 
431 template <> inline unsigned short vpMath::saturate<unsigned short>(double v)
432 {
433  int iv = vpMath::round(v);
434  return vpMath::saturate<unsigned short>(iv);
435 }
436 
437 // short
438 template <> inline short vpMath::saturate<short>(unsigned short v) { return (short)((std::min))((int)v, SHRT_MAX); }
439 template <> inline short vpMath::saturate<short>(int v)
440 {
441  return (short)((unsigned int)(v - SHRT_MIN) <= (unsigned int)USHRT_MAX ? v : v > 0 ? SHRT_MAX : SHRT_MIN);
442 }
443 template <> inline short vpMath::saturate<short>(unsigned int v)
444 {
445  return (short)((std::min))(v, (unsigned int)SHRT_MAX);
446 }
447 template <> inline short vpMath::saturate<short>(float v)
448 {
449  int iv = vpMath::round(v);
450  return vpMath::saturate<short>(iv);
451 }
452 template <> inline short vpMath::saturate<short>(double v)
453 {
454  int iv = vpMath::round(v);
455  return vpMath::saturate<short>(iv);
456 }
457 
458 // int
459 template <> inline int vpMath::saturate<int>(float v) { return vpMath::round(v); }
460 
461 template <> inline int vpMath::saturate<int>(double v) { return vpMath::round(v); }
462 
463 // unsigned int
464 // (Comment from OpenCV) we intentionally do not clip negative numbers, to
465 // make -1 become 0xffffffff etc.
466 template <> inline unsigned int vpMath::saturate<unsigned int>(float v) { return (unsigned int)vpMath::round(v); }
467 
468 template <> inline unsigned int vpMath::saturate<unsigned int>(double v) { return (unsigned int)vpMath::round(v); }
469 
470 #endif
static _Tp saturate(double v)
Definition: vpMath.h:186
static void swap(Type &a, Type &b)
Definition: vpMath.h:169
static _Tp saturate(char v)
Definition: vpMath.h:180
static double sigmoid(double x, double x0=0., double x1=1., double n=12.)
Definition: vpMath.h:310
static _Tp saturate(unsigned char v)
Definition: vpMath.h:179
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:290
Provides simple mathematics computation tools that are not available in the C mathematics library (ma...
Definition: vpMath.h:86
static _Tp saturate(unsigned short v)
Definition: vpMath.h:181
static double fact(unsigned int x)
Definition: vpMath.h:205
static int round(const double x)
Definition: vpMath.h:235
static Type abs(const Type &x)
Definition: vpMath.h:152
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
static _Tp saturate(short v)
Definition: vpMath.h:182
static bool nul(double x, double s=0.001)
Definition: vpMath.h:281
static double sqr(double x)
Definition: vpMath.h:108
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:145
static _Tp saturate(int v)
Definition: vpMath.h:184
static double rad(double deg)
Definition: vpMath.h:102
static _Tp saturate(unsigned v)
Definition: vpMath.h:183
static _Tp saturate(float v)
Definition: vpMath.h:185
static long double comb(unsigned int n, unsigned int p)
Definition: vpMath.h:220
static double deg(double rad)
Definition: vpMath.h:95
static bool greater(double x, double y, double s=0.001)
Definition: vpMath.h:299
static int() sign(double x)
Definition: vpMath.h:262