Visual Servoing Platform version 3.5.0
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#if defined(_WIN32)
54// Define _USE_MATH_DEFINES before including <math.h> to expose these macro
55// definitions for common math constants. These are placed under an #ifdef
56// since these commonly-defined names are not part of the C or C++ standards
57# define _USE_MATH_DEFINES
58#endif
59#include <math.h>
60#include <vector>
61
62#if defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN) || defined(VISP_HAVE_FUNC_ISINF) || \
63 defined(VISP_HAVE_FUNC_STD_ISINF) || defined(VISP_HAVE_FUNC_STD_ROUND)
64#include <cmath>
65#endif
66
67#if defined(_WIN32) // Not defined in Microsoft math.h
68
69#ifndef M_PI
70#define M_PI 3.14159265358979323846
71#endif
72
73#ifndef M_PI_2
74#define M_PI_2 (M_PI / 2.0)
75#endif
76
77#ifndef M_PI_4
78#define M_PI_4 (M_PI / 4.0)
79#endif
80
81#endif
82
83#include <visp3/core/vpImagePoint.h>
84
94class VISP_EXPORT vpMath
95{
96public:
103 static inline double deg(double rad) { return (rad * 180.0) / M_PI; }
104
110 static inline double rad(double deg) { return (deg * M_PI) / 180.0; }
111
116 static inline double sqr(double x) { return x * x; }
117
118 // factorial of x
119 static inline double fact(unsigned int x);
120
121 // combinaison
122 static inline long double comb(unsigned int n, unsigned int p);
123
124 // round x to the nearest integer
125 static inline int round(double x);
126
127 // return the sign of x (+-1)
128 static inline int(sign)(double x);
129
130 // test if a number equals 0 (with threshold value)
131 static inline bool nul(double x, double s = 0.001);
132
133 // test if two numbers are equals (with a user defined threshold)
134 static inline bool equal(double x, double y, double s = 0.001);
135
136 // test if a number is greater than another (with a user defined threshold)
137 static inline bool greater(double x, double y, double s = 0.001);
138
145 template <class Type> static Type maximum(const Type &a, const Type &b) { return (a > b) ? a : b; }
146
153 template <class Type> static Type minimum(const Type &a, const Type &b) { return (a < b) ? a : b; }
154
160 template <class Type> static Type abs(const Type &x) { return (x < 0) ? -x : x; }
161
162 // sinus cardinal
163 static double sinc(double x);
164 static double sinc(double sinx, double x);
165 static double mcosc(double cosx, double x);
166 static double msinc(double sinx, double x);
167
168 // sigmoid
169 static inline double sigmoid(double x, double x0 = 0., double x1 = 1., double n = 12.);
170
177 template <class Type> static void swap(Type &a, Type &b)
178 {
179 Type tmp = b;
180 b = a;
181 a = tmp;
182 }
183
184 static bool isNaN(double value);
185 static bool isNaN(float value);
186 static bool isInf(double value);
187 static bool isInf(float value);
188
189 static double lineFitting(const std::vector<vpImagePoint>& imPts, double& a, double& b, double& c);
190
191 template <typename _Tp> static inline _Tp saturate(unsigned char v) { return _Tp(v); }
192 template <typename _Tp> static inline _Tp saturate(char v) { return _Tp(v); }
193 template <typename _Tp> static inline _Tp saturate(unsigned short v) { return _Tp(v); }
194 template <typename _Tp> static inline _Tp saturate(short v) { return _Tp(v); }
195 template <typename _Tp> static inline _Tp saturate(unsigned v) { return _Tp(v); }
196 template <typename _Tp> static inline _Tp saturate(int v) { return _Tp(v); }
197 template <typename _Tp> static inline _Tp saturate(float v) { return _Tp(v); }
198 template <typename _Tp> static inline _Tp saturate(double v) { return _Tp(v); }
199
200 static double getMean(const std::vector<double> &v);
201 static double getMedian(const std::vector<double> &v);
202 static double getStdev(const std::vector<double> &v, bool useBesselCorrection = false);
203
204 static int modulo(int a, int n);
205
206private:
207 static const double ang_min_sinc;
208 static const double ang_min_mc;
209};
210
211// Begining of the inline functions definition
212
217double vpMath::fact(unsigned int x)
218{
219 if ((x == 1) || (x == 0))
220 return 1;
221 return x * fact(x - 1);
222}
223
232long double vpMath::comb(unsigned int n, unsigned int p)
233{
234 if (n == p)
235 return 1;
236 return fact(n) / (fact(n - p) * fact(p));
237}
238
247int vpMath::round(double x)
248{
249#if defined(VISP_HAVE_FUNC_ROUND)
250 //:: to design the global namespace and avoid to call recursively
251 // vpMath::round
252 return (int)::round(x);
253#elif defined(VISP_HAVE_FUNC_STD_ROUND)
254 return (int)std::round(x);
255#else
256 return (x > 0.0) ? ((int)floor(x + 0.5)) : ((int)ceil(x - 0.5));
257#endif
258}
259
267int ( vpMath::sign ) (double x)
268{
269 if (fabs(x) < std::numeric_limits<double>::epsilon())
270 return 0;
271 else {
272 if (x < 0)
273 return -1;
274 else
275 return 1;
276 }
277}
278
286bool vpMath::nul(double x, double s) { return (fabs(x) < s); }
287
295bool vpMath::equal(double x, double y, double s) { return (nul(x - y, s)); }
296
304bool vpMath::greater(double x, double y, double s) { return (x > (y - s)); }
305
317double vpMath::sigmoid(double x, double x0, double x1, double n)
318{
319 if (x < x0)
320 return 0.;
321 else if (x > x1)
322 return 1.;
323 double l0 = 1. / (1. + exp(0.5 * n));
324 double l1 = 1. / (1. + exp(-0.5 * n));
325 return (1. / (1. + exp(-n * ((x - x0) / (x1 - x0) - 0.5))) - l0) / (l1 - l0);
326}
327
328// unsigned char
329template <> inline unsigned char vpMath::saturate<unsigned char>(char v)
330{
331 // On big endian arch like powerpc, char implementation is unsigned
332 // with CHAR_MIN=0, CHAR_MAX=255 and SCHAR_MIN=-128, SCHAR_MAX=127
333 // leading to (int)(char -127) = 129.
334 // On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to
335 // (int)(char -127) = -127.
336 if (std::numeric_limits<char>::is_signed)
337 return (unsigned char)(((std::max))((int)v, 0));
338 else
339 return (unsigned char)((unsigned int)v > SCHAR_MAX ? 0 : v);
340}
341
342template <> inline unsigned char vpMath::saturate<unsigned char>(unsigned short v)
343{
344 return (unsigned char)((std::min))((unsigned int)v, (unsigned int)UCHAR_MAX);
345}
346
347template <> inline unsigned char vpMath::saturate<unsigned char>(int v)
348{
349 return (unsigned char)((unsigned int)v <= UCHAR_MAX ? v : v > 0 ? UCHAR_MAX : 0);
350}
351
352template <> inline unsigned char vpMath::saturate<unsigned char>(short v) { return saturate<unsigned char>((int)v); }
353
354template <> inline unsigned char vpMath::saturate<unsigned char>(unsigned int v)
355{
356 return (unsigned char)((std::min))(v, (unsigned int)UCHAR_MAX);
357}
358
359template <> inline unsigned char vpMath::saturate<unsigned char>(float v)
360{
361 int iv = vpMath::round(v);
362 return saturate<unsigned char>(iv);
363}
364
365template <> inline unsigned char vpMath::saturate<unsigned char>(double v)
366{
367 int iv = vpMath::round(v);
368 return saturate<unsigned char>(iv);
369}
370
371// char
372template <> inline char vpMath::saturate<char>(unsigned char v) { return (char)((std::min))((int)v, SCHAR_MAX); }
373
374template <> inline char vpMath::saturate<char>(unsigned short v)
375{
376 return (char)((std::min))((unsigned int)v, (unsigned int)SCHAR_MAX);
377}
378
379template <> inline char vpMath::saturate<char>(int v)
380{
381 return (char)((unsigned int)(v - SCHAR_MIN) <= (unsigned int)UCHAR_MAX ? v : v > 0 ? SCHAR_MAX : SCHAR_MIN);
382}
383
384template <> inline char vpMath::saturate<char>(short v) { return saturate<char>((int)v); }
385
386template <> inline char vpMath::saturate<char>(unsigned int v)
387{
388 return (char)((std::min))(v, (unsigned int)SCHAR_MAX);
389}
390
391template <> inline char vpMath::saturate<char>(float v)
392{
393 int iv = vpMath::round(v);
394 return saturate<char>(iv);
395}
396
397template <> inline char vpMath::saturate<char>(double v)
398{
399 int iv = vpMath::round(v);
400 return saturate<char>(iv);
401}
402
403// unsigned short
404template <> inline unsigned short vpMath::saturate<unsigned short>(char v)
405{
406 // On big endian arch like powerpc, char implementation is unsigned
407 // with CHAR_MIN=0, CHAR_MAX=255 and SCHAR_MIN=-128, SCHAR_MAX=127
408 // leading to (int)(char -127) = 129.
409 // On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to
410 // (int)(char -127) = -127.
411 if (std::numeric_limits<char>::is_signed)
412 return (unsigned char)(((std::max))((int)v, 0));
413 else
414 return (unsigned char)((unsigned int)v > SCHAR_MAX ? 0 : v);
415}
416
417template <> inline unsigned short vpMath::saturate<unsigned short>(short v)
418{
419 return (unsigned short)((std::max))((int)v, 0);
420}
421
422template <> inline unsigned short vpMath::saturate<unsigned short>(int v)
423{
424 return (unsigned short)((unsigned int)v <= (unsigned int)USHRT_MAX ? v : v > 0 ? USHRT_MAX : 0);
425}
426
427template <> inline unsigned short vpMath::saturate<unsigned short>(unsigned int v)
428{
429 return (unsigned short)((std::min))(v, (unsigned int)USHRT_MAX);
430}
431
432template <> inline unsigned short vpMath::saturate<unsigned short>(float v)
433{
434 int iv = vpMath::round(v);
435 return vpMath::saturate<unsigned short>(iv);
436}
437
438template <> inline unsigned short vpMath::saturate<unsigned short>(double v)
439{
440 int iv = vpMath::round(v);
441 return vpMath::saturate<unsigned short>(iv);
442}
443
444// short
445template <> inline short vpMath::saturate<short>(unsigned short v) { return (short)((std::min))((int)v, SHRT_MAX); }
446template <> inline short vpMath::saturate<short>(int v)
447{
448 return (short)((unsigned int)(v - SHRT_MIN) <= (unsigned int)USHRT_MAX ? v : v > 0 ? SHRT_MAX : SHRT_MIN);
449}
450template <> inline short vpMath::saturate<short>(unsigned int v)
451{
452 return (short)((std::min))(v, (unsigned int)SHRT_MAX);
453}
454template <> inline short vpMath::saturate<short>(float v)
455{
456 int iv = vpMath::round(v);
457 return vpMath::saturate<short>(iv);
458}
459template <> inline short vpMath::saturate<short>(double v)
460{
461 int iv = vpMath::round(v);
462 return vpMath::saturate<short>(iv);
463}
464
465// int
466template <> inline int vpMath::saturate<int>(float v) { return vpMath::round(v); }
467
468template <> inline int vpMath::saturate<int>(double v) { return vpMath::round(v); }
469
470// unsigned int
471// (Comment from OpenCV) we intentionally do not clip negative numbers, to
472// make -1 become 0xffffffff etc.
473template <> inline unsigned int vpMath::saturate<unsigned int>(float v) { return (unsigned int)vpMath::round(v); }
474
475template <> inline unsigned int vpMath::saturate<unsigned int>(double v) { return (unsigned int)vpMath::round(v); }
476
477#endif
Provides simple mathematics computation tools that are not available in the C mathematics library (ma...
Definition: vpMath.h:95
static _Tp saturate(char v)
Definition: vpMath.h:192
static void swap(Type &a, Type &b)
Definition: vpMath.h:177
static int() sign(double x)
static double fact(unsigned int x)
Definition: vpMath.h:217
static double rad(double deg)
Definition: vpMath.h:110
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
static _Tp saturate(short v)
Definition: vpMath.h:194
static _Tp saturate(double v)
Definition: vpMath.h:198
static double sqr(double x)
Definition: vpMath.h:116
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:295
static _Tp saturate(unsigned short v)
Definition: vpMath.h:193
static Type abs(const Type &x)
Definition: vpMath.h:160
static _Tp saturate(float v)
Definition: vpMath.h:197
static double sigmoid(double x, double x0=0., double x1=1., double n=12.)
Definition: vpMath.h:317
static bool nul(double x, double s=0.001)
Definition: vpMath.h:286
static _Tp saturate(unsigned v)
Definition: vpMath.h:195
static int round(double x)
Definition: vpMath.h:247
static _Tp saturate(int v)
Definition: vpMath.h:196
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:153
static long double comb(unsigned int n, unsigned int p)
Definition: vpMath.h:232
static double deg(double rad)
Definition: vpMath.h:103
static _Tp saturate(unsigned char v)
Definition: vpMath.h:191
static bool greater(double x, double y, double s=0.001)
Definition: vpMath.h:304