PCL
Math.h
Go to the documentation of this file.
1 // ____ ______ __
2 // / __ \ / ____// /
3 // / /_/ // / / /
4 // / ____// /___ / /___ PixInsight Class Library
5 // /_/ \____//_____/ PCL 2.1.19
6 // ----------------------------------------------------------------------------
7 // pcl/Math.h - Released 2019-11-07T10:59:34Z
8 // ----------------------------------------------------------------------------
9 // This file is part of the PixInsight Class Library (PCL).
10 // PCL is a multiplatform C++ framework for development of PixInsight modules.
11 //
12 // Copyright (c) 2003-2019 Pleiades Astrophoto S.L. All Rights Reserved.
13 //
14 // Redistribution and use in both source and binary forms, with or without
15 // modification, is permitted provided that the following conditions are met:
16 //
17 // 1. All redistributions of source code must retain the above copyright
18 // notice, this list of conditions and the following disclaimer.
19 //
20 // 2. All redistributions in binary form must reproduce the above copyright
21 // notice, this list of conditions and the following disclaimer in the
22 // documentation and/or other materials provided with the distribution.
23 //
24 // 3. Neither the names "PixInsight" and "Pleiades Astrophoto", nor the names
25 // of their contributors, may be used to endorse or promote products derived
26 // from this software without specific prior written permission. For written
27 // permission, please contact info@pixinsight.com.
28 //
29 // 4. All products derived from this software, in any form whatsoever, must
30 // reproduce the following acknowledgment in the end-user documentation
31 // and/or other materials provided with the product:
32 //
33 // "This product is based on software from the PixInsight project, developed
34 // by Pleiades Astrophoto and its contributors (http://pixinsight.com/)."
35 //
36 // Alternatively, if that is where third-party acknowledgments normally
37 // appear, this acknowledgment must be reproduced in the product itself.
38 //
39 // THIS SOFTWARE IS PROVIDED BY PLEIADES ASTROPHOTO AND ITS CONTRIBUTORS
40 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
41 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
42 // PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL PLEIADES ASTROPHOTO OR ITS
43 // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
44 // EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, BUSINESS
45 // INTERRUPTION; PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; AND LOSS OF USE,
46 // DATA OR PROFITS) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
47 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
48 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
49 // POSSIBILITY OF SUCH DAMAGE.
50 // ----------------------------------------------------------------------------
51 
52 #ifndef __PCL_Math_h
53 #define __PCL_Math_h
54 
56 
57 #include <pcl/Defs.h>
58 #include <pcl/Diagnostics.h>
59 
60 #include <pcl/Memory.h>
61 #include <pcl/Selection.h>
62 #include <pcl/Sort.h>
63 #include <pcl/Utility.h>
64 
65 #include <cmath>
66 #include <cstdlib>
67 #include <limits>
68 
69 #ifdef _MSC_VER
70 # include <intrin.h> // for __cpuid()
71 #endif
72 
73 #if defined( __x86_64__ ) || defined( _M_X64 ) || defined( __PCL_MACOSX )
74 # define __PCL_HAVE_SSE2 1
75 # include <emmintrin.h>
76 #endif
77 
78 /*
79  * sincos() is only available as a GNU extension:
80  *
81  * http://man7.org/linux/man-pages/man3/sincos.3.html
82  *
83  * Unfortunately, it is not part of the C++ standard library because of the
84  * anachronic dependency on errno.
85  */
86 #if !defined( _MSC_VER ) && !defined( __clang__ ) && defined( __GNUC__ )
87 # define __PCL_HAVE_SINCOS 1
88 #endif
89 
90 #ifdef __PCL_QT_INTERFACE
91 # include <QtWidgets/QtWidgets>
92 #endif
93 
94 namespace pcl
95 {
96 
97 // ----------------------------------------------------------------------------
98 
120 {
121  int32 edxFlags = 0;
122  int32 ecxFlags = 0;
123 
124 #ifdef _MSC_VER
125  int cpuInfo[ 4 ];
126  __cpuid( cpuInfo, 1 );
127  edxFlags = cpuInfo[3];
128  ecxFlags = cpuInfo[2];
129 #else
130  asm volatile( "mov $0x00000001, %%eax\n\t"
131  "cpuid\n\t"
132  "mov %%edx, %0\n\t"
133  "mov %%ecx, %1\n"
134  : "=r" (edxFlags), "=r" (ecxFlags) // output operands
135  : // input operands
136  : "%eax", "%ebx", "%ecx", "%edx" ); // clobbered registers
137 #endif
138 
139  if ( ecxFlags & (1u << 20) ) // SSE4.2
140  return 42;
141  if ( ecxFlags & (1u << 19) ) // SSE4.1
142  return 41;
143  if ( ecxFlags & 1u ) // SSE3
144  return 3;
145  if ( edxFlags & (1u << 26) ) // SSE2
146  return 2;
147  if ( edxFlags & (1u << 25) ) // SSE
148  return 1;
149  return 0;
150 }
151 
152 // ----------------------------------------------------------------------------
153 
158 #define __PCL_FLOAT_SGNMASK 0x80000000
159 #define __PCL_FLOAT_EXPMASK 0x7f800000
160 #define __PCL_FLOAT_SIGMASK 0x007fffff
161 
170 inline bool IsFinite( float x )
171 {
172  union { float f; uint32 u; } v = { x };
173  return (v.u & __PCL_FLOAT_EXPMASK) != __PCL_FLOAT_EXPMASK;
174 }
175 
186 inline bool IsNaN( float x )
187 {
188  union { float f; uint32 u; } v = { x };
189  return (v.u & __PCL_FLOAT_EXPMASK) == __PCL_FLOAT_EXPMASK &&
190  (v.u & __PCL_FLOAT_SIGMASK) != 0;
191 }
192 
205 inline int IsInfinity( float x )
206 {
207  union { float f; uint32 u; } v = { x };
208  if ( (v.u & __PCL_FLOAT_EXPMASK) == __PCL_FLOAT_EXPMASK &&
209  (v.u & __PCL_FLOAT_SIGMASK) == 0 )
210  return ((v.u & __PCL_FLOAT_SGNMASK) == 0) ? +1 : -1;
211  return 0;
212 }
213 
214 #define __PCL_DOUBLE_SGNMASK 0x80000000
215 #define __PCL_DOUBLE_EXPMASK 0x7ff00000
216 #define __PCL_DOUBLE_SIGMASK 0x000fffff
217 
226 inline bool IsFinite( double x )
227 {
228  union { double d; uint32 u[2]; } v = { x };
229  return (v.u[1] & __PCL_DOUBLE_EXPMASK) != __PCL_DOUBLE_EXPMASK;
230 }
231 
242 inline bool IsNaN( double x )
243 {
244  union { double d; uint32 u[2]; } v = { x };
245  return (v.u[1] & __PCL_DOUBLE_EXPMASK) == __PCL_DOUBLE_EXPMASK &&
246  (v.u[0] != 0 || (v.u[1] & __PCL_DOUBLE_SIGMASK) != 0);
247 }
248 
261 inline int IsInfinity( double x )
262 {
263  union { double d; uint32 u[2]; } v = { x };
264  if ( v.u[0] == 0 &&
265  (v.u[1] & __PCL_DOUBLE_SIGMASK) == 0 &&
266  (v.u[1] & __PCL_DOUBLE_EXPMASK) == __PCL_DOUBLE_EXPMASK )
267  return ((v.u[1] & __PCL_DOUBLE_SGNMASK) == 0) ? +1 : -1;
268  return 0;
269 }
270 
271 // ----------------------------------------------------------------------------
272 
281 inline float Abs( float x )
282 {
283  return std::fabs( x );
284 }
285 
289 inline double Abs( double x )
290 {
291  return std::fabs( x );
292 }
293 
297 inline long double Abs( long double x )
298 {
299  return std::fabs( x );
300 }
301 
305 inline signed int Abs( signed int x )
306 {
307  return ::abs( x );
308 }
309 
313 #if defined( __PCL_MACOSX ) && defined( __clang__ ) // turn off warning due to broken cstdlib in Xcode
314 _Pragma("clang diagnostic push")
315 _Pragma("clang diagnostic ignored \"-Wabsolute-value\"")
316 #endif
317 inline signed long Abs( signed long x )
318 {
319  return ::abs( x );
320 }
321 #if defined( __PCL_MACOSX ) && defined( __clang__ )
322 _Pragma("clang diagnostic pop")
323 #endif
324 
328 #if defined( _MSC_VER )
329 inline __int64 Abs( __int64 x )
330 {
331  return (x < 0) ? -x : +x;
332 }
333 #elif defined( __PCL_MACOSX ) && defined( __clang__ )
334 inline constexpr signed long long Abs( signed long long x )
335 {
336  return (x < 0) ? -x : +x;
337 }
338 #else
339 inline signed long long Abs( signed long long x )
340 {
341  return ::abs( x );
342 }
343 #endif
344 
348 inline signed short Abs( signed short x )
349 {
350  return (signed short)::abs( int( x ) );
351 }
352 
356 inline signed char Abs( signed char x )
357 {
358  return (signed char)::abs( int( x ) );
359 }
360 
364 inline wchar_t Abs( wchar_t x )
365 {
366  return (wchar_t)::abs( int( x ) );
367 }
368 
372 inline constexpr unsigned int Abs( unsigned int x )
373 {
374  return x;
375 }
376 
380 inline constexpr unsigned long Abs( unsigned long x )
381 {
382  return x;
383 }
384 
388 #ifdef _MSC_VER
389 inline unsigned __int64 Abs( unsigned __int64 x )
390 {
391  return x;
392 }
393 #else
394 inline constexpr unsigned long long Abs( unsigned long long x )
395 {
396  return x;
397 }
398 #endif
399 
403 inline constexpr unsigned short Abs( unsigned short x )
404 {
405  return x;
406 }
407 
411 inline constexpr unsigned char Abs( unsigned char x )
412 {
413  return x;
414 }
415 
416 // ----------------------------------------------------------------------------
417 
422 inline constexpr long double Pi()
423 {
424  return (long double)( 0.31415926535897932384626433832795029e+01L );
425 }
426 
431 inline constexpr long double TwoPi()
432 {
433  return (long double)( 0.62831853071795864769252867665590058e+01L );
434 }
435 
436 // ----------------------------------------------------------------------------
437 
442 template <typename T> inline constexpr T Angle( int d, T m )
443 {
444  return d + m/60;
445 }
446 
452 template <typename T> inline constexpr T Angle( int d, int m, T s )
453 {
454  return Angle( d, m + s/60 );
455 }
456 
457 // ----------------------------------------------------------------------------
458 
463 template <typename T> inline constexpr T ArcCos( T x )
464 {
465  PCL_PRECONDITION( T( -1 ) <= x && x <= T( 1 ) )
466  return std::acos( x );
467 }
468 
469 // ----------------------------------------------------------------------------
470 
475 template <typename T> inline constexpr T ArcSin( T x )
476 {
477  PCL_PRECONDITION( T( -1 ) <= x && x <= T( 1 ) )
478  return std::asin( x );
479 }
480 
481 // ----------------------------------------------------------------------------
482 
487 template <typename T> inline constexpr T ArcTan( T x )
488 {
489  return std::atan( x );
490 }
491 
492 // ----------------------------------------------------------------------------
493 
498 template <typename T> inline constexpr T ArcTan( T y, T x )
499 {
500  return std::atan2( y, x );
501 }
502 
503 // ----------------------------------------------------------------------------
504 
509 template <typename T> inline T ArcTan2Pi( T y, T x )
510 {
511  T r = ArcTan( y, x );
512  if ( r < 0 )
513  r = static_cast<T>( r + TwoPi() );
514  return r;
515 }
516 
517 // ----------------------------------------------------------------------------
518 
523 template <typename T> inline constexpr T Ceil( T x )
524 {
525  return std::ceil( x );
526 }
527 
528 // ----------------------------------------------------------------------------
529 
534 template <typename T> inline constexpr T Cos( T x )
535 {
536  return std::cos( x );
537 }
538 
539 // ----------------------------------------------------------------------------
540 
545 template <typename T> inline constexpr T Cosh( T x )
546 {
547  return std::cosh( x );
548 }
549 
550 // ----------------------------------------------------------------------------
551 
556 template <typename T> inline constexpr T Cotan( T x )
557 {
558  return T( 1 )/std::tan( x );
559 }
560 
561 // ----------------------------------------------------------------------------
562 
567 template <typename T> inline constexpr T Deg( T x )
568 {
569  return static_cast<T>( 0.572957795130823208767981548141051700441964e+02L * x );
570 }
571 
572 // ----------------------------------------------------------------------------
573 
578 template <typename T> inline constexpr T Exp( T x )
579 {
580  return std::exp( x );
581 }
582 
583 // ----------------------------------------------------------------------------
584 
589 template <typename T> inline constexpr T Floor( T x )
590 {
591  return std::floor( x );
592 }
593 
594 // ----------------------------------------------------------------------------
595 
601 template <typename T> inline constexpr T Frac( T x )
602 {
603  return std::modf( x, &x );
604 }
605 
606 // ----------------------------------------------------------------------------
607 
614 template <typename T> inline void Frexp( T x, T& m, int& p )
615 {
616  m = std::frexp( x, &p );
617 }
618 
619 // ----------------------------------------------------------------------------
620 
630 template <typename T> inline constexpr T Hav( T x )
631 {
632  return (1 - Cos( x ))/2;
633 }
634 
635 // ----------------------------------------------------------------------------
636 
641 template <typename T> inline constexpr T Ldexp( T m, int p )
642 {
643  return std::ldexp( m, p );
644 }
645 
646 // ----------------------------------------------------------------------------
647 
652 template <typename T> inline constexpr T Ln( T x )
653 {
654  return std::log( x );
655 }
656 
657 // ----------------------------------------------------------------------------
658 
671 template <typename T> struct PCL_CLASS Fact
672 {
676  T operator()( int n ) const
677  {
678  static T f[ 61 ] = { T( 1 ), T( 1 ), T( 2 ), T( 6 ), T( 24 ), T( 120 ) };
679  static int last = 5;
680  PCL_PRECONDITION( 0 <= n )
681  if ( last < n )
682  {
683  int m = Min( n, 60 );
684  T x = f[last];
685  while ( last < m )
686  {
687  x *= ++last;
688  f[last] = x;
689  }
690  while ( m < n )
691  x *= ++m;
692  return x;
693  }
694  return f[n];
695  }
696 
703  T Ln( int n ) const
704  {
705  if ( n <= 60 )
706  return Ln( operator()( n ) );
707  double x = n + 1;
708  return (x - 0.5)*pcl::Ln( x ) - x + 0.5*pcl::Ln( TwoPi() ) + 1/12/x - 1/(360*x*x*x);
709  }
710 };
711 
712 // ----------------------------------------------------------------------------
713 
718 inline constexpr long double Ln2()
719 {
720  return (long double)( 0.6931471805599453094172321214581766e+00L );
721 }
722 
723 // ----------------------------------------------------------------------------
724 
729 template <typename T> inline constexpr T Log( T x )
730 {
731  return std::log10( x );
732 }
733 
734 // ----------------------------------------------------------------------------
735 
740 inline constexpr long double Log2()
741 {
742  // Use the relation:
743  // log10(2) = ln(2)/ln(10)
744  return (long double)( 0.3010299956639811952137388947244930416265e+00L );
745 }
746 
747 // ----------------------------------------------------------------------------
748 
753 template <typename T> inline constexpr T Log2( T x )
754 {
755  // Use the relation:
756  // log2(x) = ln(x)/ln(2)
757  return std::log( x )/Ln2();
758 }
759 
760 // ----------------------------------------------------------------------------
761 
766 inline constexpr long double Log2e()
767 {
768  // Use the relation:
769  // log2(e) = 1/ln(2)
770  return (long double)( 0.14426950408889634073599246810018920709799e+01L );
771 }
772 
773 // ----------------------------------------------------------------------------
774 
779 inline constexpr long double Log2T()
780 {
781  // Use the relation:
782  // log2(10) = 1/log(2)
783  return (long double)( 0.33219280948873623478703194294893900118996e+01L );
784 }
785 
786 // ----------------------------------------------------------------------------
787 
792 template <typename T> inline constexpr T LogN( T n, T x )
793 {
794  return std::log( x )/std::log( n );
795 }
796 
797 // ----------------------------------------------------------------------------
798 
803 template <typename T> inline constexpr T Mod( T x, T y )
804 {
805  return std::fmod( x, y );
806 }
807 
808 // ----------------------------------------------------------------------------
809 
822 template <typename T, typename C> inline T Poly( T x, C c, int n )
823 {
824  PCL_PRECONDITION( n >= 0 )
825  T y;
826  for ( c += n, y = *c; n > 0; --n )
827  {
828  y *= x;
829  y += *--c;
830  }
831  return y;
832 }
833 
844 template <typename T> inline T Poly( T x, std::initializer_list<T> c )
845 {
846  PCL_PRECONDITION( c.size() > 0 )
847  return Poly( x, c.begin(), int( c.size() )-1 );
848 }
849 
850 // ----------------------------------------------------------------------------
851 
861 template <typename T> inline constexpr int Sign( T x )
862 {
863  return (x < 0) ? -1 : ((x > 0) ? +1 : 0);
864 }
865 
866 // ----------------------------------------------------------------------------
867 
877 template <typename T> inline constexpr char SignChar( T x )
878 {
879  return (x < 0) ? '-' : ((x > 0) ? '+' : ' ');
880 }
881 
882 // ----------------------------------------------------------------------------
883 
888 template <typename T> inline constexpr T Sin( T x )
889 {
890  return std::sin( x );
891 }
892 
893 // ----------------------------------------------------------------------------
894 
899 template <typename T> inline constexpr T Sinh( T x )
900 {
901  return std::sinh( x );
902 }
903 
904 // ----------------------------------------------------------------------------
905 
906 #ifdef __PCL_HAVE_SINCOS
907 
908 inline void __pcl_sincos__( float x, float& sx, float& cx )
909 {
910  ::sincosf( x, &sx, &cx );
911 }
912 
913 inline void __pcl_sincos__( double x, double& sx, double& cx )
914 {
915  ::sincos( x, &sx, &cx );
916 }
917 
918 inline void __pcl_sincos__( long double x, long double& sx, long double& cx )
919 {
920  ::sincosl( x, &sx, &cx );
921 }
922 
923 #endif
924 
938 template <typename T> inline void SinCos( T x, T& sx, T& cx )
939 {
940 #ifdef __PCL_HAVE_SINCOS
941  __pcl_sincos__( x, sx, cx );
942 #else
943  sx = std::sin( x );
944  cx = std::cos( x );
945 #endif
946 }
947 
948 // ----------------------------------------------------------------------------
949 
959 template <typename T> inline void Split( T x, T& i, T& f )
960 {
961  f = std::modf( x, &i );
962 }
963 
964 // ----------------------------------------------------------------------------
965 
970 template <typename T> inline constexpr T Sqrt( T x )
971 {
972  return std::sqrt( x );
973 }
974 
975 // ----------------------------------------------------------------------------
976 
981 template <typename T> inline constexpr T Tan( T x )
982 {
983  return std::tan( x );
984 }
985 
986 // ----------------------------------------------------------------------------
987 
992 template <typename T> inline constexpr T Tanh( T x )
993 {
994  return std::tanh( x );
995 }
996 
997 // ----------------------------------------------------------------------------
998 
1003 template <typename T> inline T Trunc( T x )
1004 {
1005  (void)std::modf( x, &x );
1006  return x;
1007 }
1008 
1009 // ----------------------------------------------------------------------------
1010 
1011 #ifdef __PCL_HAVE_SSE2
1012 
1013 inline int __pcl_trunci__( float x )
1014 {
1015  return _mm_cvtt_ss2si( _mm_load_ss( &x ) );
1016 }
1017 
1018 inline int __pcl_trunci__( double x )
1019 {
1020  return _mm_cvttsd_si32( _mm_load_sd( &x ) );
1021 }
1022 
1023 #endif
1024 
1035 template <typename T> inline int TruncInt( T x )
1036 {
1037  PCL_PRECONDITION( x >= int_min && x <= int_max )
1038 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1039  return int( x );
1040 #else
1041 # ifdef __PCL_HAVE_SSE2
1042  return __pcl_trunci__( x );
1043 # else
1044  return int( x );
1045 # endif
1046 #endif
1047 }
1048 
1057 template <typename T> inline int TruncI( T x )
1058 {
1059  return TruncInt( x );
1060 }
1061 
1062 #define TruncInt32( x ) TruncInt( x )
1063 #define TruncI32( x ) TruncInt( x )
1064 
1065 // ----------------------------------------------------------------------------
1066 
1067 #ifdef __PCL_HAVE_SSE2
1068 
1069 #if defined( __x86_64__ ) || defined( _M_X64 )
1070 
1071 inline int64 __pcl_trunci64__( float x )
1072 {
1073  return _mm_cvttss_si64( _mm_load_ss( &x ) );
1074 }
1075 
1076 inline int64 __pcl_trunci64__( double x )
1077 {
1078  return _mm_cvttsd_si64( _mm_load_sd( &x ) );
1079 }
1080 
1081 #else
1082 
1083 inline int64 __pcl_trunci64__( float x )
1084 {
1085  return int64( _mm_cvtt_ss2si( _mm_load_ss( &x ) ) );
1086 }
1087 
1088 inline int64 __pcl_trunci64__( double x )
1089 {
1090  return int64( x );
1091 }
1092 
1093 #endif
1094 
1095 #endif // __PCL_HAVE_SSE2
1096 
1107 template <typename T> inline int64 TruncInt64( T x )
1108 {
1109  PCL_PRECONDITION( x >= int64_min && x <= int64_max )
1110 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1111  return int64( Trunc( x ) );
1112 #else
1113 # ifdef __PCL_HAVE_SSE2
1114  return __pcl_trunci64__( x );
1115 # else
1116  return int64( Trunc( x ) );
1117 # endif
1118 #endif
1119 }
1120 
1129 template <typename T> inline int64 TruncI64( T x )
1130 {
1131  return TruncInt64( x );
1132 }
1133 
1134 // ----------------------------------------------------------------------------
1135 
1149 template <typename T> inline constexpr T Pow( T x, T y )
1150 {
1151  PCL_PRECONDITION( y < T( int_max ) )
1152  return std::pow( x, y );
1153 }
1154 
1155 // ----------------------------------------------------------------------------
1156 
1167 template <typename T> struct PCL_CLASS Pow10I
1168 {
1169  T operator ()( int n ) const
1170  {
1171  // Use fast table lookups and squaring up to |n| <= 50.
1172  static const T lut[] =
1173  {
1174 #define ___( x ) static_cast<T>( x )
1175  ___( 1.0e+00 ), ___( 1.0e+01 ), ___( 1.0e+02 ), ___( 1.0e+03 ), ___( 1.0e+04 ),
1176  ___( 1.0e+05 ), ___( 1.0e+06 ), ___( 1.0e+07 ), ___( 1.0e+08 ), ___( 1.0e+09 ),
1177  ___( 1.0e+10 ), ___( 1.0e+11 ), ___( 1.0e+12 ), ___( 1.0e+13 ), ___( 1.0e+14 ),
1178  ___( 1.0e+15 ), ___( 1.0e+16 ), ___( 1.0e+17 ), ___( 1.0e+18 ), ___( 1.0e+19 ),
1179  ___( 1.0e+20 ), ___( 1.0e+21 ), ___( 1.0e+22 ), ___( 1.0e+23 ), ___( 1.0e+24 ),
1180  ___( 1.0e+25 ), ___( 1.0e+26 ), ___( 1.0e+27 ), ___( 1.0e+28 ), ___( 1.0e+29 ),
1181  ___( 1.0e+30 ), ___( 1.0e+31 ), ___( 1.0e+32 ), ___( 1.0e+33 ), ___( 1.0e+34 ),
1182  ___( 1.0e+35 ), ___( 1.0e+36 ), ___( 1.0e+37 ), ___( 1.0e+38 ), ___( 1.0e+39 ),
1183  ___( 1.0e+40 ), ___( 1.0e+41 ), ___( 1.0e+42 ), ___( 1.0e+43 ), ___( 1.0e+44 ),
1184  ___( 1.0e+45 ), ___( 1.0e+46 ), ___( 1.0e+47 ), ___( 1.0e+48 ), ___( 1.0e+49 )
1185 #undef ___
1186  };
1187  static const int N = ItemsInArray( lut );
1188  int i = ::abs( n );
1189  double x;
1190  if ( i < N )
1191  x = lut[i];
1192  else
1193  {
1194  x = lut[N-1];
1195  while ( (i -= N-1) >= N )
1196  x *= x;
1197  if ( i != 0 )
1198  x *= lut[i];
1199  }
1200  return T( (n >= 0) ? x : 1/x );
1201  }
1202 };
1203 
1204 // ----------------------------------------------------------------------------
1205 
1210 template <typename T> inline T Pow10( T x )
1211 {
1212  int i = TruncInt( x );
1213  return (i == x) ? Pow10I<T>()( i ) : T( std::pow( 10, x ) );
1214 }
1215 
1216 // ----------------------------------------------------------------------------
1217 
1228 template <typename T> inline T RotL( T x, uint32 n )
1229 {
1230  static_assert( std::is_unsigned<T>::value,
1231  "RotL() can only be used for unsigned scalar types" );
1232  const uint8 mask = 8*sizeof( x ) - 1;
1233  const uint8 r = uint8( n & mask );
1234  return (x << r) | (x >> ((-r) & mask));
1235  // Or, equivalently but less optimized:
1236  //return (x << r) | (x >> (1+mask-r));
1237 }
1238 
1249 template <typename T> inline T RotR( T x, uint32 n )
1250 {
1251  static_assert( std::is_unsigned<T>::value,
1252  "RotR() can only be used for unsigned scalar types" );
1253  const uint8 mask = 8*sizeof( x ) - 1;
1254  const uint8 r = uint8( n & mask );
1255  return (x >> r) | (x << ((-r) & mask));
1256  // Or, equivalently but less optimized:
1257  //return (x >> r) | (x << (1+mask-r));
1258 }
1259 
1260 // ----------------------------------------------------------------------------
1261 
1271 inline double Round( double x )
1272 {
1273 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1274 
1275  return floor( x + 0.5 );
1276 
1277 #else
1278 
1279 # ifdef _MSC_VER
1280 # ifdef _M_X64
1281  return double( _mm_cvtsd_si64( _mm_load_sd( &x ) ) );
1282 # else
1283  __asm fld x
1284  __asm frndint
1285 # endif
1286 # else
1287  double r;
1288  asm volatile( "frndint\n": "=t" (r) : "0" (x) );
1289  return r;
1290 # endif
1291 
1292 #endif
1293 }
1294 
1304 inline float Round( float x )
1305 {
1306 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1307 
1308  return floorf( x + 0.5F );
1309 
1310 #else
1311 
1312 # ifdef _MSC_VER
1313 # ifdef _M_X64
1314  return float( _mm_cvtss_si32( _mm_load_ss( &x ) ) );
1315 # else
1316  __asm fld x
1317  __asm frndint
1318 # endif
1319 # else
1320  float r;
1321  asm volatile( "frndint\n": "=t" (r) : "0" (x) );
1322  return r;
1323 # endif
1324 
1325 #endif
1326 }
1327 
1337 inline long double Round( long double x )
1338 {
1339 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1340 
1341  return floorl( x + 0.5L );
1342 
1343 #else
1344 
1345 # ifdef _MSC_VER
1346 # ifdef _M_X64
1347  double _x = x;
1348  return (long double)_mm_cvtsd_si64( _mm_load_sd( &_x ) );
1349 # else
1350  __asm fld x
1351  __asm frndint
1352 # endif
1353 # else
1354  long double r;
1355  asm volatile( "frndint\n": "=t" (r) : "0" (x) );
1356  return r;
1357 # endif
1358 
1359 #endif
1360 }
1361 
1362 // ----------------------------------------------------------------------------
1363 
1397 template <typename T> inline int RoundInt( T x )
1398 {
1399  PCL_PRECONDITION( x >= int_min && x <= int_max )
1400 
1401 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1402 
1403  return int( Round( x ) );
1404 
1405 #else
1406 
1407  volatile union { double d; int32 i; } v = { x + 6755399441055744.0 };
1408  return v.i; // ### NB: Assuming little-endian architecture, i.e. Intel.
1409 
1410 /*
1411  ### Deprecated code - The above code based on magic numbers is faster and
1412  more portable across platforms and compilers.
1413 
1414  // ### N.B.: Default FPU rounding mode assumed to be nearest integer.
1415  int r;
1416 
1417 # ifdef _MSC_VER
1418  __asm fld x
1419  __asm fistp dword ptr r
1420 # else
1421  asm volatile( "fistpl %0\n" : "=m" (r) : "t" (x) : "st" );
1422 # endif
1423 
1424  return r;
1425 */
1426 
1427 #endif
1428 }
1429 
1438 template <typename T> inline int RoundI( T x )
1439 {
1440  return RoundInt( x );
1441 }
1442 
1451 template <typename T> inline int RoundIntBanker( T x )
1452 {
1453  return RoundInt( x );
1454 }
1455 
1478 template <typename T> inline int RoundIntArithmetic( T x )
1479 {
1480  PCL_PRECONDITION( x >= int_min && x <= int_max )
1481 
1482  int i = TruncInt( x );
1483  if ( i < 0 )
1484  {
1485  if ( x - i <= T( -0.5 ) )
1486  return i-1;
1487  }
1488  else
1489  {
1490  if ( x - i >= T( +0.5 ) )
1491  return i+1;
1492  }
1493  return i;
1494 }
1495 
1496 // ----------------------------------------------------------------------------
1497 
1512 inline int64 RoundInt64( double x )
1513 {
1514 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1515 
1516  return int64( Round( x ) );
1517 
1518 #else
1519 
1520  // ### N.B.: Default FPU rounding mode assumed to be nearest integer.
1521 
1522 # ifdef _MSC_VER
1523 # ifdef _M_X64
1524  return _mm_cvtsd_si64( _mm_load_sd( &x ) );
1525 # else
1526  int64 r;
1527  __asm fld x
1528  __asm fistp qword ptr r
1529  return r;
1530 # endif
1531 # else
1532  int64 r;
1533  asm volatile( "fistpll %0\n" : "=m" (r) : "t" (x) : "st" );
1534  return r;
1535 # endif
1536 
1537 #endif
1538 }
1539 
1546 inline int64 RoundI64( double x )
1547 {
1548  return RoundInt64( x );
1549 }
1550 
1573 inline int64 RoundInt64Arithmetic( double x )
1574 {
1575  int64 i = TruncInt64( x );
1576  if ( i < 0 )
1577  {
1578  if ( x - i <= -0.5 )
1579  return i-1;
1580  }
1581  else
1582  {
1583  if ( x - i >= +0.5 )
1584  return i+1;
1585  }
1586  return i;
1587 }
1588 
1589 // ----------------------------------------------------------------------------
1590 
1595 template <typename T> inline T Round( T x, int n )
1596 {
1597  PCL_PRECONDITION( n >= 0 )
1598  T p = Pow10I<T>()( n ); return Round( p*x )/p;
1599 }
1600 
1601 // ----------------------------------------------------------------------------
1602 
1607 template <typename T> inline constexpr T Pow2( T x )
1608 {
1609  // Use the relation:
1610  // 2**x = e**(x*ln(2))
1611  return Exp( x*Ln2() );
1612 }
1613 
1614 // ----------------------------------------------------------------------------
1615 
1626 template <typename T> struct PCL_CLASS Pow2I
1627 {
1628  T operator ()( int n ) const
1629  {
1630  // We shift left a single bit in 31-bit chunks.
1631  int i = ::abs( n ), p;
1632  double x = uint32( 1 ) << (p = Min( i, 31 ));
1633  while ( (i -= p) != 0 )
1634  x *= uint32( 1 ) << (p = Min( i, 31 ));
1635  return T( (n >= 0) ? x : 1/x );
1636  }
1637 };
1638 
1639 // ----------------------------------------------------------------------------
1640 
1645 template <typename T> inline T PowI( T x, int n )
1646 {
1647  if ( n == 0 )
1648  return 1;
1649 
1650  int i = Abs( n );
1651  T r = x;
1652  if ( i > 1 )
1653  {
1654  do
1655  r *= r;
1656  while ( (i >>= 1) >= 2 );
1657 
1658  if ( (n & 1) != 0 )
1659  r *= x;
1660  }
1661  return (n >= 0) ? r : 1/r;
1662 }
1663 
1664 // ----------------------------------------------------------------------------
1665 
1673 template <typename T> inline constexpr T ArcSinh( T x )
1674 {
1675  return Ln( x + Sqrt( 1 + x*x ) );
1676 }
1677 
1678 // ----------------------------------------------------------------------------
1679 
1687 template <typename T> inline constexpr T ArcCosh( T x )
1688 {
1689  return 2*Ln( Sqrt( (x + 1)/2 ) + Sqrt( (x - 1)/2 ) );
1690 }
1691 
1692 // ----------------------------------------------------------------------------
1693 
1701 template <typename T> inline constexpr T ArcTanh( T x )
1702 {
1703  return (Ln( 1 + x ) - Ln( 1 - x ))/2;
1704 }
1705 
1706 // ----------------------------------------------------------------------------
1707 
1717 template <typename T> inline constexpr T ArcHav( T x )
1718 {
1719  return 2*ArcSin( Sqrt( x ) );
1720 }
1721 
1722 // ----------------------------------------------------------------------------
1723 
1728 template <typename T> inline constexpr T Rad( T x )
1729 {
1730  return static_cast<T>( 0.174532925199432957692369076848861272222e-01L * x );
1731 }
1732 
1733 // ----------------------------------------------------------------------------
1734 
1739 template <typename T> inline constexpr T RadMin( T x )
1740 {
1741  return Deg( x )*60;
1742 }
1743 
1744 // ----------------------------------------------------------------------------
1745 
1750 template <typename T> inline constexpr T RadSec( T x )
1751 {
1752  return Deg( x )*3600;
1753 }
1754 
1755 // ----------------------------------------------------------------------------
1756 
1761 template <typename T> inline constexpr T MinRad( T x )
1762 {
1763  return Rad( x/60 );
1764 }
1765 
1766 // ----------------------------------------------------------------------------
1767 
1772 template <typename T> inline constexpr T SecRad( T x )
1773 {
1774  return Rad( x/3600 );
1775 }
1776 
1781 template <typename T> inline constexpr T AsRad( T x )
1782 {
1783  return SecRad( x );
1784 }
1785 
1786 // ----------------------------------------------------------------------------
1787 
1792 template <typename T> inline constexpr T MasRad( T x )
1793 {
1794  return Rad( x/3600000 );
1795 }
1796 
1797 // ----------------------------------------------------------------------------
1798 
1803 template <typename T> inline constexpr T UasRad( T x )
1804 {
1805  return Rad( x/3600000000 );
1806 }
1807 
1808 // ----------------------------------------------------------------------------
1809 
1815 template <typename T> inline constexpr T Mod2Pi( T x )
1816 {
1817  return Mod( x, static_cast<T>( TwoPi() ) );
1818 }
1819 
1820 // ----------------------------------------------------------------------------
1821 
1826 template <typename T> inline constexpr T Norm2Pi( T x )
1827 {
1828  return ((x = Mod2Pi( x )) < 0) ? x + static_cast<T>( TwoPi() ) : x;
1829 }
1830 
1831 // ----------------------------------------------------------------------------
1832 
1845 template <typename T, typename T1, typename T2>
1846 inline void Rotate( T& x, T& y, T1 sa, T1 ca, T2 xc, T2 yc )
1847 {
1848  T1 dx = T1( x ) - T1( xc );
1849  T1 dy = T1( y ) - T1( yc );
1850  x = T( T1( xc ) + ca*dx + sa*dy );
1851  y = T( T1( yc ) - sa*dx + ca*dy );
1852 }
1853 
1864 template <typename T1, typename T2>
1865 inline void Rotate( int& x, int& y, T1 sa, T1 ca, T2 xc, T2 yc )
1866 {
1867  T1 dx = T1( x ) - T1( xc );
1868  T1 dy = T1( y ) - T1( yc );
1869  x = RoundInt( T1( xc ) + ca*dx + sa*dy );
1870  y = RoundInt( T1( yc ) - sa*dx + ca*dy );
1871 }
1872 
1883 template <typename T1, typename T2>
1884 inline void Rotate( long& x, long& y, T1 sa, T1 ca, T2 xc, T2 yc )
1885 {
1886  T1 dx = T1( x ) - T1( xc );
1887  T1 dy = T1( y ) - T1( yc );
1888  x = (long)RoundInt( T1( xc ) + ca*dx + sa*dy );
1889  y = (long)RoundInt( T1( yc ) - sa*dx + ca*dy );
1890 }
1891 
1902 template <typename T1, typename T2>
1903 inline void Rotate( int64& x, int64& y, T1 sa, T1 ca, T2 xc, T2 yc )
1904 {
1905  T1 dx = T1( x ) - T1( xc );
1906  T1 dy = T1( y ) - T1( yc );
1907  x = RoundInt64( T1( xc ) + ca*dx + sa*dy );
1908  y = RoundInt64( T1( yc ) - sa*dx + ca*dy );
1909 }
1910 
1926 template <typename T, typename T1, typename T2>
1927 inline void Rotate( T& x, T& y, T1 a, T2 xc, T2 yc )
1928 {
1929  T1 sa, ca; SinCos( a, sa, ca ); Rotate( x, y, sa, ca, xc, yc );
1930 }
1931 
1932 // ----------------------------------------------------------------------------
1933 
1945 template <typename T> inline double Norm( const T* i, const T* j, double p )
1946 {
1947  PCL_PRECONDITION( p > 0 )
1948  double N = 0;
1949  for ( ; i < j; ++i )
1950  N += Pow( Abs( double( *i ) ), p );
1951  return Pow( N, 1/p );
1952 }
1953 
1960 template <typename T> inline double L1Norm( const T* i, const T* j )
1961 {
1962  double N = 0;
1963  for ( ; i < j; ++i )
1964  N += Abs( *i );
1965  return N;
1966 }
1967 
1974 template <typename T> inline double L2Norm( const T* i, const T* j )
1975 {
1976  double N = 0;
1977  for ( ; i < j; ++i )
1978  N += double( *i ) * *i;
1979  return Sqrt( N );
1980 }
1981 
1988 template <typename T> inline double Norm( const T* i, const T* j )
1989 {
1990  return L2Norm( i, j );
1991 }
1992 
1993 // ----------------------------------------------------------------------------
1994 
2035 void PCL_FUNC ComplexTimeToJD( int& jdi, double& jdf, int year, int month, int day, double dayf = 0 );
2036 
2076 inline double ComplexTimeToJD( int year, int month, int day, double dayf = 0 )
2077 {
2078  int jdi;
2079  double jdf;
2080  ComplexTimeToJD( jdi, jdf, year, month, day, dayf );
2081  return jdi + jdf;
2082 }
2083 
2114 void PCL_FUNC JDToComplexTime( int& year, int& month, int& day, double& dayf, int jdi, double jdf );
2115 
2145 inline void JDToComplexTime( int& year, int& month, int& day, double& dayf, double jd )
2146 {
2147  JDToComplexTime( year, month, day, dayf, TruncInt( jd ), Frac( jd ) );
2148 }
2149 
2150 // ----------------------------------------------------------------------------
2151 
2168 template <typename S1, typename S2, typename S3, typename D>
2169 inline void DecimalToSexagesimal( int& sign, S1& s1, S2& s2, S3& s3, const D& d )
2170 {
2171  double t1 = Abs( d );
2172  double t2 = Frac( t1 )*60;
2173  double t3 = Frac( t2 )*60;
2174  sign = (d < 0) ? -1 : +1;
2175  s1 = S1( TruncInt( t1 ) );
2176  s2 = S2( TruncInt( t2 ) );
2177  s3 = S3( t3 );
2178 }
2179 
2188 template <typename S1, typename S2, typename S3>
2189 inline double SexagesimalToDecimal( int sign, const S1& s1, const S2& s2 = S2( 0 ), const S3& s3 = S3( 0 ) )
2190 {
2191  double d = Abs( s1 ) + (s2 + s3/60)/60;
2192  return (sign < 0) ? -d : d;
2193 }
2194 
2195 // ----------------------------------------------------------------------------
2196 
2215 template <typename T> inline double Sum( const T* i, const T* j )
2216 {
2217  double sum = 0;
2218  for ( ; i < j; ++i )
2219  sum += double( *i );
2220  return sum;
2221 }
2222 
2234 template <typename T> inline double StableSum( const T* i, const T* j )
2235 {
2236  double sum = 0;
2237  double eps = 0;
2238  for ( ; i < j; ++i )
2239  {
2240  double y = double( *i ) - eps;
2241  double t = sum + y;
2242  eps = (t - sum) - y;
2243  sum = t;
2244  }
2245  return sum;
2246 }
2247 
2259 template <typename T> inline double Modulus( const T* i, const T* j )
2260 {
2261  double S = 0;
2262  for ( ; i < j; ++i )
2263  S += Abs( double( *i ) );
2264  return S;
2265 }
2266 
2278 template <typename T> inline double StableModulus( const T* i, const T* j )
2279 {
2280  double sum = 0;
2281  double eps = 0;
2282  for ( ; i < j; ++i )
2283  {
2284  double y = Abs( double( *i ) ) - eps;
2285  double t = sum + y;
2286  eps = (t - sum) - y;
2287  sum = t;
2288  }
2289  return sum;
2290 }
2291 
2303 template <typename T> inline double SumOfSquares( const T* i, const T* j )
2304 {
2305  double Q = 0;
2306  for ( ; i < j; ++i )
2307  {
2308  double f = double( *i );
2309  Q += f*f;
2310  }
2311  return Q;
2312 }
2313 
2325 template <typename T> inline double StableSumOfSquares( const T* i, const T* j )
2326 {
2327  double sum = 0;
2328  double eps = 0;
2329  for ( ; i < j; ++i )
2330  {
2331  double f = double( *i );
2332  double y = f*f - eps;
2333  double t = sum + y;
2334  eps = (t - sum) - y;
2335  sum = t;
2336  }
2337  return sum;
2338 }
2339 
2351 template <typename T> inline double Mean( const T* i, const T* j )
2352 {
2353  distance_type n = j - i;
2354  if ( n < 1 )
2355  return 0;
2356  return Sum( i, j )/n;
2357 }
2358 
2370 template <typename T> inline double StableMean( const T* i, const T* j )
2371 {
2372  distance_type n = j - i;
2373  if ( n < 1 )
2374  return 0;
2375  return StableSum( i, j )/n;
2376 }
2377 
2395 template <typename T> inline double Variance( const T* i, const T* j, double center )
2396 {
2397  distance_type n = j - i;
2398  if ( n < 2 )
2399  return 0;
2400  double var = 0, eps = 0;
2401  for ( ; i < j; ++i )
2402  {
2403  double d = double( *i ) - center;
2404  var += d*d;
2405  eps += d;
2406  }
2407  return (var - eps*eps/n)/(n - 1);
2408 }
2409 
2426 template <typename T> inline double Variance( const T* i, const T* j )
2427 {
2428  distance_type n = j - i;
2429  if ( n < 2 )
2430  return 0;
2431  double m = 0;
2432  for ( const T* f = i; f < j; ++f )
2433  m += double( *f );
2434  m /= n;
2435  double var = 0, eps = 0;
2436  for ( const T* f = i; f < j; ++f )
2437  {
2438  double d = double( *f ) - m;
2439  var += d*d;
2440  eps += d;
2441  }
2442  return (var - eps*eps/n)/(n - 1);
2443 }
2444 
2453 template <typename T> inline double StdDev( const T* i, const T* j, double center )
2454 {
2455  return Sqrt( Variance( i, j, center ) );
2456 }
2457 
2465 template <typename T> inline double StdDev( const T* i, const T* j )
2466 {
2467  return Sqrt( Variance( i, j ) );
2468 }
2469 
2470 #define CMPXCHG( a, b ) \
2471  if ( i[b] < i[a] ) pcl::Swap( i[a], i[b] )
2472 
2473 #define MEAN( a, b ) \
2474  (double( a ) + double( b ))/2
2475 
2514 template <typename T> inline double Median( T* i, T* j )
2515 {
2516  distance_type n = j - i;
2517  if ( n < 1 )
2518  return 0;
2519 
2520  switch ( n )
2521  {
2522  case 1: // !?
2523  return i[0];
2524  case 2:
2525  return MEAN( i[0], i[1] );
2526  case 3:
2527  CMPXCHG( 0, 1 ); CMPXCHG( 1, 2 );
2528  return pcl::Max( i[0], i[1] );
2529  case 4:
2530  CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
2531  CMPXCHG( 1, 3 );
2532  return MEAN( i[1], i[2] );
2533  case 5:
2534  CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 0, 3 );
2535  CMPXCHG( 1, 4 ); CMPXCHG( 1, 2 ); CMPXCHG( 2, 3 );
2536  return pcl::Max( i[1], i[2] );
2537  case 6:
2538  CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
2539  CMPXCHG( 1, 3 ); CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 );
2540  CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 1, 4 );
2541  CMPXCHG( 2, 4 ); CMPXCHG( 3, 5 ); CMPXCHG( 3, 4 );
2542  return MEAN( i[2], i[3] );
2543  case 7:
2544  CMPXCHG( 0, 5 ); CMPXCHG( 0, 3 ); CMPXCHG( 1, 6 );
2545  CMPXCHG( 2, 4 ); CMPXCHG( 0, 1 ); CMPXCHG( 3, 5 );
2546  CMPXCHG( 2, 6 ); CMPXCHG( 2, 3 ); CMPXCHG( 3, 6 );
2547  CMPXCHG( 4, 5 ); CMPXCHG( 1, 4 ); CMPXCHG( 1, 3 );
2548  return pcl::Min( i[3], i[4] );
2549  case 8:
2550  CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 2, 6 );
2551  CMPXCHG( 3, 7 ); CMPXCHG( 0, 2 ); CMPXCHG( 1, 3 );
2552  CMPXCHG( 4, 6 ); CMPXCHG( 5, 7 ); CMPXCHG( 2, 4 );
2553  CMPXCHG( 3, 5 ); CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 );
2554  CMPXCHG( 4, 5 ); CMPXCHG( 6, 7 ); CMPXCHG( 1, 4 );
2555  CMPXCHG( 3, 6 );
2556  return MEAN( i[3], i[4] );
2557  case 9:
2558  CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
2559  CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 6, 7 );
2560  CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
2561  CMPXCHG( 0, 3 ); CMPXCHG( 5, 8 ); CMPXCHG( 4, 7 );
2562  CMPXCHG( 3, 6 ); CMPXCHG( 1, 4 ); CMPXCHG( 2, 5 );
2563  CMPXCHG( 4, 7 ); CMPXCHG( 4, 2 ); CMPXCHG( 6, 4 );
2564  return pcl::Min( i[2], i[4] );
2565  default:
2566  {
2567  distance_type n2 = distance_type( n >> 1 );
2568  if ( n & 1 )
2569  return *pcl::Select( i, j, n2 );
2570  return MEAN( *pcl::Select( i, j, n2 ), *pcl::Select( i, j, n2-1 ) );
2571  }
2572  }
2573 }
2574 
2575 #undef CMPXCHG
2576 
2577 double PCL_FUNC Median( unsigned char* i, unsigned char* j );
2578 double PCL_FUNC Median( signed char* i, signed char* j );
2579 double PCL_FUNC Median( wchar_t* i, wchar_t* j );
2580 double PCL_FUNC Median( unsigned short* i, unsigned short* j );
2581 double PCL_FUNC Median( signed short* i, signed short* j );
2582 double PCL_FUNC Median( unsigned int* i, unsigned int* j );
2583 double PCL_FUNC Median( signed int* i, signed int* j );
2584 double PCL_FUNC Median( unsigned long* i, unsigned long* j );
2585 double PCL_FUNC Median( signed long* i, signed long* j );
2586 double PCL_FUNC Median( unsigned long long* i, unsigned long long* j );
2587 double PCL_FUNC Median( signed long long* i, signed long long* j );
2588 double PCL_FUNC Median( float* i, float* j );
2589 double PCL_FUNC Median( double* i, double* j );
2590 double PCL_FUNC Median( long double* i, long double* j );
2591 
2592 #define CMPXCHG( a, b ) \
2593  if ( p( i[b], i[a] ) ) pcl::Swap( i[a], i[b] )
2594 
2607 template <typename T, class BP> inline double Median( T* i, T* j, BP p )
2608 {
2609  distance_type n = j - i;
2610  if ( n < 1 )
2611  return 0;
2612 
2613  switch ( n )
2614  {
2615  case 1: // !?
2616  return i[0];
2617  case 2:
2618  return MEAN( i[0], i[1] );
2619  case 3:
2620  CMPXCHG( 0, 1 ); CMPXCHG( 1, 2 );
2621  return pcl::Max( i[0], i[1] );
2622  case 4:
2623  CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
2624  CMPXCHG( 1, 3 );
2625  return MEAN( i[1], i[2] );
2626  case 5:
2627  CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 0, 3 );
2628  CMPXCHG( 1, 4 ); CMPXCHG( 1, 2 ); CMPXCHG( 2, 3 );
2629  return pcl::Max( i[1], i[2] );
2630  case 6:
2631  CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
2632  CMPXCHG( 1, 3 ); CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 );
2633  CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 1, 4 );
2634  CMPXCHG( 2, 4 ); CMPXCHG( 3, 5 ); CMPXCHG( 3, 4 );
2635  return MEAN( i[2], i[3] );
2636  case 7:
2637  CMPXCHG( 0, 5 ); CMPXCHG( 0, 3 ); CMPXCHG( 1, 6 );
2638  CMPXCHG( 2, 4 ); CMPXCHG( 0, 1 ); CMPXCHG( 3, 5 );
2639  CMPXCHG( 2, 6 ); CMPXCHG( 2, 3 ); CMPXCHG( 3, 6 );
2640  CMPXCHG( 4, 5 ); CMPXCHG( 1, 4 ); CMPXCHG( 1, 3 );
2641  return pcl::Min( i[3], i[4] );
2642  case 8:
2643  CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 2, 6 );
2644  CMPXCHG( 3, 7 ); CMPXCHG( 0, 2 ); CMPXCHG( 1, 3 );
2645  CMPXCHG( 4, 6 ); CMPXCHG( 5, 7 ); CMPXCHG( 2, 4 );
2646  CMPXCHG( 3, 5 ); CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 );
2647  CMPXCHG( 4, 5 ); CMPXCHG( 6, 7 ); CMPXCHG( 1, 4 );
2648  CMPXCHG( 3, 6 );
2649  return MEAN( i[3], i[4] );
2650  case 9:
2651  CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
2652  CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 6, 7 );
2653  CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
2654  CMPXCHG( 0, 3 ); CMPXCHG( 5, 8 ); CMPXCHG( 4, 7 );
2655  CMPXCHG( 3, 6 ); CMPXCHG( 1, 4 ); CMPXCHG( 2, 5 );
2656  CMPXCHG( 4, 7 ); CMPXCHG( 4, 2 ); CMPXCHG( 6, 4 );
2657  return pcl::Min( i[2], i[4] );
2658  default:
2659  {
2660  distance_type n2 = distance_type( n >> 1 );
2661  if ( n & 1 )
2662  return *pcl::Select( i, j, n2, p );
2663  return MEAN( *pcl::Select( i, j, n2, p ), *pcl::Select( i, j, n2-1, p ) );
2664  }
2665  }
2666 }
2667 
2668 #undef CMPXCHG
2669 #undef MEAN
2670 
2680 template <typename T> inline double NondestructiveMedian( const T* i, const T* j )
2681 {
2682  distance_type n = j - i;
2683  if ( n < 2 )
2684  return 0;
2685  double* d = new double[ n ];
2686  double* p = d;
2687  for ( const T* f = i; f < j; ++f, ++p )
2688  *p = double( *f );
2689  double m = pcl::Median( d, d+n );
2690  delete [] d;
2691  return m;
2692 }
2693 
2705 template <typename T, class BP> inline double NondestructiveMedian( const T* i, const T* j, BP p )
2706 {
2707  distance_type n = j - i;
2708  if ( n < 2 )
2709  return 0;
2710  double* d = new double[ n ];
2711  double* t = d;
2712  for ( const T* f = i; f < j; ++f, ++t )
2713  *t = double( *f );
2714  double m = pcl::Median( d, d+n, p );
2715  delete [] d;
2716  return m;
2717 }
2718 
2739 template <typename T> inline double AvgDev( const T* i, const T* j, double center )
2740 {
2741  distance_type n = j - i;
2742  if ( n < 2 )
2743  return 0;
2744  double d = 0;
2745  for ( ; i < j; ++i )
2746  d += Abs( double( *i ) - center );
2747  return d/n;
2748 }
2749 
2770 template <typename T> inline double StableAvgDev( const T* i, const T* j, double center )
2771 {
2772  distance_type n = j - i;
2773  if ( n < 2 )
2774  return 0;
2775  double sum = 0;
2776  double eps = 0;
2777  for ( ; i < j; ++i )
2778  {
2779  double y = Abs( double( *i ) - center ) - eps;
2780  double t = sum + y;
2781  eps = (t - sum) - y;
2782  sum = t;
2783  }
2784  return sum/n;
2785 }
2786 
2806 template <typename T> inline double AvgDev( const T* i, const T* j )
2807 {
2808  distance_type n = j - i;
2809  if ( n < 2 )
2810  return 0;
2811  T* t = new T[ n ];
2812  pcl::Copy( t, i, j );
2813  double m = Median( t, t+n );
2814  delete [] t;
2815  double d = 0;
2816  for ( ; i < j; ++i )
2817  d += Abs( double( *i ) - m );
2818  return d/n;
2819 }
2820 
2840 template <typename T> inline double StableAvgDev( const T* i, const T* j )
2841 {
2842  distance_type n = j - i;
2843  if ( n < 2 )
2844  return 0;
2845  T* t = new T[ n ];
2846  pcl::Copy( t, i, j );
2847  double m = Median( t, t+n );
2848  delete [] t;
2849  return StableAvgDev( i, j, m );
2850 }
2851 
2865 template <typename T> inline double MAD( const T* i, const T* j, double center )
2866 {
2867  distance_type n = j - i;
2868  if ( n < 2 )
2869  return 0;
2870  double* d = new double[ n ];
2871  double* p = d;
2872  for ( const T* f = i; f < j; ++f, ++p )
2873  *p = Abs( double( *f ) - center );
2874  double m = pcl::Median( d, d+n );
2875  delete [] d;
2876  return m;
2877 }
2878 
2892 template <typename T> inline double MAD( const T* i, const T* j )
2893 {
2894  distance_type n = j - i;
2895  if ( n < 2 )
2896  return 0;
2897  double* d = new double[ n ];
2898  double* p = d;
2899  for ( const T* f = i; f < j; ++f, ++p )
2900  *p = double( *f );
2901  double m = pcl::Median( d, d+n );
2902  p = d;
2903  for ( const T* f = i; f < j; ++f, ++p )
2904  *p = Abs( double( *f ) - m );
2905  m = pcl::Median( d, d+n );
2906  delete [] d;
2907  return m;
2908 }
2909 
2939 template <typename T> double Sn( T* x, T* xn )
2940 {
2941  /*
2942  * N.B.: In the code below, lines commented with an asterisk (*) have been
2943  * modified with respect to the FORTRAN original to account for zero-based
2944  * array indices.
2945  */
2946 
2947  distance_type n = xn - x;
2948  if ( n < 2 )
2949  return 0;
2950 
2951  pcl::Sort( x, xn );
2952 
2953  double* a2 = new double[ n ];
2954  a2[0] = double( x[n >> 1] ) - double( x[0] ); // *
2955 
2956  distance_type nh = (n + 1) >> 1;
2957 
2958  for ( distance_type i = 2; i <= nh; ++i )
2959  {
2960  distance_type nA = i-1;
2961  distance_type nB = n - i;
2962  distance_type diff = nB - nA;
2963  distance_type leftA = 1;
2964  distance_type leftB = 1;
2965  distance_type rightA = nB;
2966  distance_type Amin = (diff >> 1) + 1;
2967  distance_type Amax = (diff >> 1) + nA;
2968 
2969  while ( leftA < rightA )
2970  {
2971  distance_type length = rightA - leftA + 1;
2972  distance_type even = (length & 1) == 0;
2973  distance_type half = (length - 1) >> 1;
2974  distance_type tryA = leftA + half;
2975  distance_type tryB = leftB + half;
2976 
2977  if ( tryA < Amin )
2978  leftA = tryA + even;
2979  else
2980  {
2981  if ( tryA > Amax )
2982  {
2983  rightA = tryA;
2984  leftB = tryB + even;
2985  }
2986  else
2987  {
2988  double medA = double( x[i-1] ) - double( x[i-2 - tryA + Amin] ); // *
2989  double medB = double( x[tryB + i-1] ) - double( x[i-1] ); // *
2990  if ( medA >= medB )
2991  {
2992  rightA = tryA;
2993  leftB = tryB + even;
2994  }
2995  else
2996  leftA = tryA + even;
2997  }
2998  }
2999  }
3000 
3001  if ( leftA > Amax )
3002  a2[i-1] = double( x[leftB + i-1] ) - double( x[i-1] ); // *
3003  else
3004  {
3005  double medA = double( x[i-1] ) - double( x[i-2 - leftA + Amin] ); // *
3006  double medB = double( x[leftB + i-1] ) - double( x[i-1] );
3007  a2[i-1] = pcl::Min( medA, medB ); // *
3008  }
3009  }
3010 
3011  for ( distance_type i = nh + 1; i < n; ++i )
3012  {
3013  distance_type nA = n - i;
3014  distance_type nB = i - 1;
3015  distance_type diff = nB - nA;
3016  distance_type leftA = 1;
3017  distance_type leftB = 1;
3018  distance_type rightA = nB;
3019  distance_type Amin = (diff >> 1) + 1;
3020  distance_type Amax = (diff >> 1) + nA;
3021 
3022  while ( leftA < rightA )
3023  {
3024  distance_type length = rightA - leftA + 1;
3025  distance_type even = (length & 1) == 0;
3026  distance_type half = (length - 1) >> 1;
3027  distance_type tryA = leftA + half;
3028  distance_type tryB = leftB + half;
3029 
3030  if ( tryA < Amin)
3031  leftA = tryA + even;
3032  else
3033  {
3034  if ( tryA > Amax )
3035  {
3036  rightA = tryA;
3037  leftB = tryB + even;
3038  }
3039  else
3040  {
3041  double medA = double( x[i + tryA - Amin] ) - double( x[i-1] ); // *
3042  double medB = double( x[i-1] ) - double( x[i-1 - tryB] ); // *
3043  if ( medA >= medB )
3044  {
3045  rightA = tryA;
3046  leftB = tryB + even;
3047  }
3048  else
3049  leftA = tryA + even;
3050  }
3051  }
3052  }
3053 
3054  if ( leftA > Amax )
3055  a2[i-1] = double( x[i-1] ) - double( x[i-1 - leftB] ); // *
3056  else
3057  {
3058  double medA = double( x[i + leftA - Amin] ) - double( x[i-1] ); // *
3059  double medB = double( x[i-1] ) - double( x[i-1 - leftB] ); // *
3060  a2[i-1] = pcl::Min( medA, medB ); // *
3061  }
3062  }
3063 
3064  a2[n-1] = double( x[n-1] ) - double( x[nh-1] ); // *
3065 
3066  /*
3067  * Correction for a finite sample
3068  */
3069  double cn;
3070  switch ( n )
3071  {
3072  case 2: cn = 0.743; break;
3073  case 3: cn = 1.851; break;
3074  case 4: cn = 0.954; break;
3075  case 5: cn = 1.351; break;
3076  case 6: cn = 0.993; break;
3077  case 7: cn = 1.198; break;
3078  case 8: cn = 1.005; break;
3079  case 9: cn = 1.131; break;
3080  default: cn = (n & 1) ? n/(n - 0.9) : 1.0; break;
3081  }
3082 
3083  double sn = cn * *pcl::Select( a2, a2+n, nh-1 );
3084 
3085  delete [] a2;
3086  return sn;
3087 }
3088 
3099 inline double __pcl_whimed__( double* a, distance_type* iw, distance_type n,
3100  double* acand, distance_type* iwcand )
3101 {
3102  distance_type wtotal = 0;
3103  for ( distance_type i = 0; i < n; ++i )
3104  wtotal += iw[i];
3105 
3106  for ( distance_type nn = n, wrest = 0; ; )
3107  {
3108  double trial = *pcl::Select( a, a+nn, nn >> 1 ); // *
3109 
3110  distance_type wleft = 0;
3111  distance_type wmid = 0;
3112  distance_type wright = 0;
3113  for ( distance_type i = 0; i < nn; ++i )
3114  if ( a[i] < trial )
3115  wleft += iw[i];
3116  else if ( a[i] > trial )
3117  wright += iw[i];
3118  else
3119  wmid += iw[i];
3120 
3121  if ( 2*(wrest + wleft) > wtotal )
3122  {
3123  distance_type kcand = 0;
3124  for ( distance_type i = 0; i < nn; ++i )
3125  if ( a[i] < trial )
3126  {
3127  acand[kcand] = a[i];
3128  iwcand[kcand] = iw[i];
3129  ++kcand;
3130  }
3131  nn = kcand;
3132  }
3133  else
3134  {
3135  if ( 2*(wrest + wleft + wmid) > wtotal )
3136  return trial;
3137 
3138  distance_type kcand = 0;
3139  for ( distance_type i = 0; i < nn; ++i )
3140  if ( a[i] > trial )
3141  {
3142  acand[kcand] = a[i];
3143  iwcand[kcand] = iw[i];
3144  ++kcand;
3145  }
3146  nn = kcand;
3147  wrest += wleft + wmid;
3148  }
3149 
3150  for ( distance_type i = 0; i < nn; ++i )
3151  {
3152  a[i] = acand[i];
3153  iw[i] = iwcand[i];
3154  }
3155  }
3156 }
3157 
3186 template <typename T> double Qn( T* x, T* xn )
3187 {
3188  distance_type n = xn - x;
3189  if ( n < 2 )
3190  return 0;
3191 
3192  double* y = new double[ n ];
3193  double* work = new double[ n ];
3194  double* acand = new double[ n ];
3195  distance_type* iwcand = new distance_type[ n ];
3196  distance_type* left = new distance_type[ n ];
3197  distance_type* right = new distance_type[ n ];
3198  distance_type* P = new distance_type[ n ];
3199  distance_type* Q = new distance_type[ n ];
3200  distance_type* weight = new distance_type[ n ];
3201 
3202  distance_type h = (n >> 1) + 1;
3203  distance_type k = (h*(h - 1)) >> 1;
3204  for ( distance_type i = 0; i < n; ++i )
3205  {
3206  y[i] = double( x[i] );
3207  left[i] = n - i + 1; // *
3208  right[i] = (i <= h) ? n : n - i + h; // N.B. The original code is "right[i] = n"
3209  }
3210 
3211  pcl::Sort( y, y+n );
3212 
3213  distance_type nL = (n*(n + 1)) >> 1;
3214  distance_type nR = n*n;
3215  distance_type knew = k + nL;
3216 
3217  bool found = false;
3218  double qn;
3219 
3220  while ( nR-nL > n )
3221  {
3222  distance_type j = 0; // *
3223  for ( distance_type i = 1; i < n; ++i ) // *
3224  if ( left[i] <= right[i] )
3225  {
3226  weight[j] = right[i] - left[i] + 1;
3227  work[j] = double( y[i] ) - y[n - left[i] - (weight[j] >> 1)];
3228  ++j;
3229  }
3230  qn = __pcl_whimed__( work, weight, j, acand, iwcand );
3231 
3232  for ( distance_type i = n-1, j = 0; i >= 0; --i ) // *
3233  {
3234  while ( j < n && double( y[i] ) - y[n-j-1] < qn )
3235  ++j;
3236  P[i] = j;
3237  }
3238 
3239  for ( distance_type i = 0, j = n+1; i < n; ++i ) // *
3240  {
3241  while ( double( y[i] ) - y[n-j+1] > qn )
3242  --j;
3243  Q[i] = j;
3244  }
3245 
3246  double sumP = 0;
3247  double sumQ = 0;
3248  for ( distance_type i = 0; i < n; ++i )
3249  {
3250  sumP += P[i];
3251  sumQ += Q[i] - 1;
3252  }
3253 
3254  if ( knew <= sumP )
3255  {
3256  for ( distance_type i = 0; i < n; ++i )
3257  right[i] = P[i];
3258  nR = sumP;
3259  }
3260  else if ( knew > sumQ )
3261  {
3262  for ( distance_type i = 0; i < n; ++i )
3263  left[i] = Q[i];
3264  nL = sumQ;
3265  }
3266  else
3267  {
3268  found = true;
3269  break;
3270  }
3271  }
3272 
3273  if ( !found )
3274  {
3275  distance_type j = 0;
3276  for ( distance_type i = 1; i < n; ++i )
3277  for ( distance_type jj = left[i]; jj <= right[i]; ++jj, ++j )
3278  work[j] = double( y[i] ) - y[n-jj]; // *
3279  qn = *pcl::Select( work, work+j, knew-nL-1 ); // *
3280  }
3281 
3282  /*
3283  * Correction for a finite sample
3284  */
3285  double dn;
3286  switch ( n )
3287  {
3288  case 2: dn = 0.399; break;
3289  case 3: dn = 0.994; break;
3290  case 4: dn = 0.512; break;
3291  case 5: dn = 0.844; break;
3292  case 6: dn = 0.611; break;
3293  case 7: dn = 0.857; break;
3294  case 8: dn = 0.669; break;
3295  case 9: dn = 0.872; break;
3296  default: dn = (n & 1) ? n/(n + 1.4) : n/(n + 3.8); break;
3297  }
3298  qn *= dn;
3299 
3300  delete [] y;
3301  delete [] work;
3302  delete [] acand;
3303  delete [] iwcand;
3304  delete [] left;
3305  delete [] right;
3306  delete [] P;
3307  delete [] Q;
3308  delete [] weight;
3309 
3310  return qn;
3311 }
3312 
3342 template <typename T>
3343 double BiweightMidvariance( const T* x, const T* xn, double center, double sigma, int k = 9 )
3344 {
3345  distance_type n = xn - x;
3346  if ( n < 2 )
3347  return 0;
3348 
3349  double kd = k * sigma;
3350  if ( kd < 0 || 1 + kd == 1 )
3351  return 0;
3352 
3353  double num = 0, den = 0;
3354  for ( ; x < xn; ++x )
3355  {
3356  double xc = double( *x ) - center;
3357  double y = xc/kd;
3358  if ( Abs( y ) < 1 )
3359  {
3360  double y2 = y*y;
3361  double y21 = 1 - y2;
3362  num += xc*xc * y21*y21*y21*y21;
3363  den += y21 * (1 - 5*y2);
3364  }
3365  }
3366 
3367  den *= den;
3368  if ( 1 + den == 1 )
3369  return 0;
3370  return n*num/den;
3371 }
3372 
3401 template <typename T>
3402 double BendMidvariance( const T* x, const T* xn, double center, double beta = 0.2 )
3403 {
3404  distance_type n = xn - x;
3405  if ( n < 2 )
3406  return 0;
3407 
3408  beta = Range( beta, 0.0, 0.5 );
3409  distance_type m = Floor( (1 - beta)*n + 0.5 );
3410 
3411  double* w = new double[ n ];
3412  for ( distance_type i = 0; i < n; ++i )
3413  w[i] = Abs( double( x[i] ) - center );
3414  double wb = *pcl::Select( w, w+n, m );
3415  delete [] w;
3416  if ( 1 + wb == 1 )
3417  return 0;
3418 
3419  double num = 0;
3420  distance_type den = 0;
3421  for ( ; x < xn; ++x )
3422  {
3423  double y = (double( *x ) - center)/wb;
3424  double f = Max( -1.0, Min( 1.0, y ) );
3425  num += f*f;
3426  if ( Abs( y ) < 1 )
3427  ++den;
3428  }
3429 
3430  if ( den == 0 )
3431  return 0;
3432  return n*wb*wb*num/den/den;
3433 }
3434 
3435 // ----------------------------------------------------------------------------
3436 
3461 inline double IncompleteBeta( double a, double b, double x, double eps = 1.0e-8 )
3462 {
3463  if ( x < 0 || x > 1 )
3464  return std::numeric_limits<double>::infinity();
3465 
3466  /*
3467  * The continued fraction converges nicely for x < (a+1)/(a+b+2)
3468  */
3469  if ( x > (a + 1)/(a + b + 2) )
3470  return 1 - IncompleteBeta( b, a, 1 - x ); // Use the fact that beta is symmetrical
3471 
3472  /*
3473  * Find the first part before the continued fraction.
3474  */
3475  double lbeta_ab = lgamma( a ) + lgamma( b ) - lgamma( a + b );
3476  double front = Exp( Ln( x )*a + Ln( 1 - x )*b - lbeta_ab )/a;
3477 
3478  /*
3479  * Use Lentz's algorithm to evaluate the continued fraction.
3480  */
3481  const double tiny = 1.0e-30;
3482  double f = 1, c = 1, d = 0;
3483  for ( int i = 0; i <= 200; ++i )
3484  {
3485  int m = i >> 1;
3486  double numerator;
3487  if ( i & 1 )
3488  numerator = -((a + m)*(a + b + m)*x)/((a + 2*m)*(a + 2*m + 1)); // Odd term
3489  else if ( i > 0 )
3490  numerator = (m*(b - m)*x)/((a + 2*m - 1)*(a + 2*m)); // Even term
3491  else
3492  numerator = 1; // First numerator is 1.0
3493 
3494  /*
3495  * Do an iteration of Lentz's algorithm.
3496  */
3497  d = 1 + numerator*d;
3498  if ( Abs( d ) < tiny )
3499  d = tiny;
3500  d = 1/d;
3501  c = 1 + numerator/c;
3502  if ( Abs( c ) < tiny )
3503  c = tiny;
3504  double cd = c*d;
3505  f *= cd;
3506  if ( Abs( 1 - cd ) < eps )
3507  return front*(f - 1);
3508  }
3509 
3510  // Needed more loops, did not converge.
3511  return std::numeric_limits<double>::infinity();
3512 }
3513 
3514 // ----------------------------------------------------------------------------
3515 
3551 inline uint64 Hash64( const void* data, size_type size, uint64 seed = 0 )
3552 {
3553 #define PRIME64_1 11400714785074694791ULL
3554 #define PRIME64_2 14029467366897019727ULL
3555 #define PRIME64_3 1609587929392839161ULL
3556 #define PRIME64_4 9650029242287828579ULL
3557 #define PRIME64_5 2870177450012600261ULL
3558 
3559  const uint8* p = (const uint8*)data;
3560  const uint8* end = p + size;
3561  uint64 h64;
3562 
3563  if ( seed == 0 )
3564  seed = size;
3565 
3566  if ( size >= 32 )
3567  {
3568  const uint8* limit = end - 32;
3569  uint64 v1 = seed + PRIME64_1 + PRIME64_2;
3570  uint64 v2 = seed + PRIME64_2;
3571  uint64 v3 = seed + 0;
3572  uint64 v4 = seed - PRIME64_1;
3573 
3574  do
3575  {
3576  v1 += *(uint64*)p * PRIME64_2;
3577  p += 8;
3578  v1 = RotL( v1, 31 );
3579  v1 *= PRIME64_1;
3580  v2 += *(uint64*)p * PRIME64_2;
3581  p += 8;
3582  v2 = RotL( v2, 31 );
3583  v2 *= PRIME64_1;
3584  v3 += *(uint64*)p * PRIME64_2;
3585  p += 8;
3586  v3 = RotL( v3, 31 );
3587  v3 *= PRIME64_1;
3588  v4 += *(uint64*)p * PRIME64_2;
3589  p += 8;
3590  v4 = RotL( v4, 31 );
3591  v4 *= PRIME64_1;
3592  }
3593  while ( p <= limit );
3594 
3595  h64 = RotL( v1, 1 ) + RotL( v2, 7 ) + RotL( v3, 12 ) + RotL( v4, 18 );
3596 
3597  v1 *= PRIME64_2;
3598  v1 = RotL( v1, 31 );
3599  v1 *= PRIME64_1;
3600  h64 ^= v1;
3601  h64 = h64 * PRIME64_1 + PRIME64_4;
3602 
3603  v2 *= PRIME64_2;
3604  v2 = RotL( v2, 31 );
3605  v2 *= PRIME64_1;
3606  h64 ^= v2;
3607  h64 = h64 * PRIME64_1 + PRIME64_4;
3608 
3609  v3 *= PRIME64_2;
3610  v3 = RotL( v3, 31 );
3611  v3 *= PRIME64_1;
3612  h64 ^= v3;
3613  h64 = h64 * PRIME64_1 + PRIME64_4;
3614 
3615  v4 *= PRIME64_2;
3616  v4 = RotL( v4, 31 );
3617  v4 *= PRIME64_1;
3618  h64 ^= v4;
3619  h64 = h64 * PRIME64_1 + PRIME64_4;
3620  }
3621  else
3622  {
3623  h64 = seed + PRIME64_5;
3624  }
3625 
3626  h64 += size;
3627 
3628  while ( p+8 <= end )
3629  {
3630  uint64 k1 = *(uint64*)p;
3631  k1 *= PRIME64_2;
3632  k1 = RotL( k1, 31 );
3633  k1 *= PRIME64_1;
3634  h64 ^= k1;
3635  h64 = RotL( h64, 27 ) * PRIME64_1 + PRIME64_4;
3636  p += 8;
3637  }
3638 
3639  if ( p+4 <= end )
3640  {
3641  h64 ^= (uint64)(*(uint32*)p) * PRIME64_1;
3642  h64 = RotL( h64, 23 ) * PRIME64_2 + PRIME64_3;
3643  p += 4;
3644  }
3645 
3646  while ( p < end )
3647  {
3648  h64 ^= *p * PRIME64_5;
3649  h64 = RotL( h64, 11 ) * PRIME64_1;
3650  ++p;
3651  }
3652 
3653  h64 ^= h64 >> 33;
3654  h64 *= PRIME64_2;
3655  h64 ^= h64 >> 29;
3656  h64 *= PRIME64_3;
3657  h64 ^= h64 >> 32;
3658 
3659  return h64;
3660 
3661 #undef PRIME64_1
3662 #undef PRIME64_2
3663 #undef PRIME64_3
3664 #undef PRIME64_4
3665 #undef PRIME64_5
3666 }
3667 
3699 inline uint32 Hash32( const void* data, size_type size, uint32 seed = 0 )
3700 {
3701 #define PRIME32_1 2654435761U
3702 #define PRIME32_2 2246822519U
3703 #define PRIME32_3 3266489917U
3704 #define PRIME32_4 668265263U
3705 #define PRIME32_5 374761393U
3706 
3707  const uint8* p = (const uint8*)data;
3708  const uint8* end = p + size;
3709  uint32 h32;
3710 
3711  if ( seed == 0 )
3712  seed = uint32( size );
3713 
3714  if ( size >= 16 )
3715  {
3716  const uint8* limit = end - 16;
3717  uint32 v1 = seed + PRIME32_1 + PRIME32_2;
3718  uint32 v2 = seed + PRIME32_2;
3719  uint32 v3 = seed + 0;
3720  uint32 v4 = seed - PRIME32_1;
3721 
3722  do
3723  {
3724  v1 += *(uint32*)p * PRIME32_2;
3725  v1 = RotL( v1, 13 );
3726  v1 *= PRIME32_1;
3727  p += 4;
3728  v2 += *(uint32*)p * PRIME32_2;
3729  v2 = RotL( v2, 13 );
3730  v2 *= PRIME32_1;
3731  p += 4;
3732  v3 += *(uint32*)p * PRIME32_2;
3733  v3 = RotL( v3, 13 );
3734  v3 *= PRIME32_1;
3735  p += 4;
3736  v4 += *(uint32*)p * PRIME32_2;
3737  v4 = RotL( v4, 13 );
3738  v4 *= PRIME32_1;
3739  p += 4;
3740  }
3741  while ( p <= limit );
3742 
3743  h32 = RotL( v1, 1 ) + RotL( v2, 7 ) + RotL( v3, 12 ) + RotL( v4, 18 );
3744  }
3745  else
3746  {
3747  h32 = seed + PRIME32_5;
3748  }
3749 
3750  h32 += (uint32)size;
3751 
3752  while ( p+4 <= end )
3753  {
3754  h32 += *(uint32*)p * PRIME32_3;
3755  h32 = RotL( h32, 17 ) * PRIME32_4 ;
3756  p+=4;
3757  }
3758 
3759  while ( p < end )
3760  {
3761  h32 += *p * PRIME32_5;
3762  h32 = RotL( h32, 11 ) * PRIME32_1 ;
3763  ++p;
3764  }
3765 
3766  h32 ^= h32 >> 15;
3767  h32 *= PRIME32_2;
3768  h32 ^= h32 >> 13;
3769  h32 *= PRIME32_3;
3770  h32 ^= h32 >> 16;
3771 
3772  return h32;
3773 
3774 #undef PRIME32_1
3775 #undef PRIME32_2
3776 #undef PRIME32_3
3777 #undef PRIME32_4
3778 #undef PRIME32_5
3779 }
3780 
3781 // ----------------------------------------------------------------------------
3782 
3783 } // pcl
3784 
3785 #endif // __PCL_Math_h
3786 
3787 // ----------------------------------------------------------------------------
3788 // EOF pcl/Math.h - Released 2019-11-07T10:59:34Z
Complex< T > Log(const Complex< T > &c)
Definition: Complex.h:726
double L1Norm(const T *i, const T *j)
Definition: Math.h:1960
double Mean(const T *i, const T *j)
Definition: Math.h:2351
int MaxSSEInstructionSetSupported()
Definition: Math.h:119
uint64 Hash64(const void *data, size_type size, uint64 seed=0)
Definition: Math.h:3551
constexpr T Frac(T x)
Definition: Math.h:601
constexpr long double Log2T()
Definition: Math.h:779
constexpr T ArcTan(T x)
Definition: Math.h:487
constexpr T RadSec(T x)
Definition: Math.h:1750
int64 RoundI64(double x)
Definition: Math.h:1546
double StableAvgDev(const T *i, const T *j, double center)
Definition: Math.h:2770
unsigned char uint8
Definition: Defs.h:576
constexpr T AsRad(T x)
Definition: Math.h:1781
int64 TruncInt64(T x)
Definition: Math.h:1107
int RoundInt(T x)
Definition: Math.h:1397
double Median(T *i, T *j)
Definition: Math.h:2514
PCL root namespace.
Definition: AbstractImage.h:76
double Norm(const T *i, const T *j, double p)
Definition: Math.h:1945
double Qn(T *x, T *xn)
Definition: Math.h:3186
constexpr T MasRad(T x)
Definition: Math.h:1792
int64 RoundInt64Arithmetic(double x)
Definition: Math.h:1573
double NondestructiveMedian(const T *i, const T *j)
Definition: Math.h:2680
constexpr T ArcSin(T x)
Definition: Math.h:475
constexpr T Pow2(T x)
Definition: Math.h:1607
constexpr T SecRad(T x)
Definition: Math.h:1772
Factorial function.
Definition: Math.h:671
Complex< T > Sqrt(const Complex< T > &c)
Definition: Complex.h:665
bool IsFinite(float x)
Definition: Math.h:170
double BiweightMidvariance(const T *x, const T *xn, double center, double sigma, int k=9)
Definition: Math.h:3343
T Pow10(T x)
Definition: Math.h:1210
void Sort(BI i, BI j)
Definition: Sort.h:534
Complex< T > Round(const Complex< T > &c)
Definition: Complex.h:929
constexpr int Sign(T x)
Definition: Math.h:861
Exponential function 10**n, n being a signed integer.
Definition: Math.h:1167
double Sum(const T *i, const T *j)
Definition: Math.h:2215
constexpr T Rad(T x)
Definition: Math.h:1728
constexpr T Ceil(T x)
Definition: Math.h:523
void Split(T x, T &i, T &f)
Definition: Math.h:959
constexpr T LogN(T n, T x)
Definition: Math.h:792
constexpr T ArcCosh(T x)
Definition: Math.h:1687
double StableMean(const T *i, const T *j)
Definition: Math.h:2370
T RotR(T x, uint32 n)
Definition: Math.h:1249
constexpr const T & Range(const T &x, const T &a, const T &b)
Definition: Utility.h:190
int TruncI(T x)
Definition: Math.h:1057
size_t size_type
Definition: Defs.h:543
constexpr T Mod2Pi(T x)
Definition: Math.h:1815
double Variance(const T *i, const T *j, double center)
Definition: Math.h:2395
constexpr T RadMin(T x)
Definition: Math.h:1739
int64 RoundInt64(double x)
Definition: Math.h:1512
RI Select(RI i, RI j, distance_type k)
Definition: Selection.h:165
T Abs(const Complex< T > &c)
Definition: Complex.h:420
constexpr T ArcCos(T x)
Definition: Math.h:463
double StableSum(const T *i, const T *j)
Definition: Math.h:2234
unsigned long long uint64
Definition: Defs.h:616
double MAD(const T *i, const T *j, double center)
Definition: Math.h:2865
bool IsNaN(float x)
Definition: Math.h:186
constexpr const T & Max(const T &a, const T &b)
Definition: Utility.h:119
int RoundI(T x)
Definition: Math.h:1438
constexpr T Angle(int d, T m)
Definition: Math.h:442
int RoundIntArithmetic(T x)
Definition: Math.h:1478
double StableSumOfSquares(const T *i, const T *j)
Definition: Math.h:2325
int TruncInt(T x)
Definition: Math.h:1035
int RoundIntBanker(T x)
Definition: Math.h:1451
constexpr T Floor(T x)
Definition: Math.h:589
constexpr const T & Min(const T &a, const T &b)
Definition: Utility.h:90
void Rotate(T &x, T &y, T1 sa, T1 ca, T2 xc, T2 yc)
Definition: Math.h:1846
constexpr long double Ln2()
Definition: Math.h:718
Complex< T > Sinh(const Complex< T > &c)
Definition: Complex.h:818
void PCL_FUNC JDToComplexTime(int &year, int &month, int &day, double &dayf, int jdi, double jdf)
void PCL_FUNC ComplexTimeToJD(int &jdi, double &jdf, int year, int month, int day, double dayf=0)
constexpr T Norm2Pi(T x)
Definition: Math.h:1826
constexpr T MinRad(T x)
Definition: Math.h:1761
constexpr long double Pi()
Definition: Math.h:422
int64 TruncI64(T x)
Definition: Math.h:1129
signed int int32
Definition: Defs.h:594
constexpr T Cotan(T x)
Definition: Math.h:556
void DecimalToSexagesimal(int &sign, S1 &s1, S2 &s2, S3 &s3, const D &d)
Definition: Math.h:2169
double IncompleteBeta(double a, double b, double x, double eps=1.0e-8)
Definition: Math.h:3461
constexpr T Mod(T x, T y)
Definition: Math.h:803
uint32 Hash32(const void *data, size_type size, uint32 seed=0)
Definition: Math.h:3699
T RotL(T x, uint32 n)
Definition: Math.h:1228
double L2Norm(const T *i, const T *j)
Definition: Math.h:1974
Complex< T > Cosh(const Complex< T > &c)
Definition: Complex.h:829
T PowI(T x, int n)
Definition: Math.h:1645
int IsInfinity(float x)
Definition: Math.h:205
constexpr T Deg(T x)
Definition: Math.h:567
double BendMidvariance(const T *x, const T *xn, double center, double beta=0.2)
Definition: Math.h:3402
T Poly(T x, C c, int n)
Definition: Math.h:822
constexpr T Ldexp(T m, int p)
Definition: Math.h:641
double SexagesimalToDecimal(int sign, const S1 &s1, const S2 &s2=S2(0), const S3 &s3=S3(0))
Definition: Math.h:2189
constexpr T Hav(T x)
Definition: Math.h:630
#define int64_max
Definition: Defs.h:822
double Sn(T *x, T *xn)
Definition: Math.h:2939
T ArcTan2Pi(T y, T x)
Definition: Math.h:509
void SinCos(T x, T &sx, T &cx)
Definition: Math.h:938
Complex< T > Ln(const Complex< T > &c)
Definition: Complex.h:716
constexpr T ArcSinh(T x)
Definition: Math.h:1673
Complex< T > Sin(const Complex< T > &c)
Definition: Complex.h:786
Complex< T > Cos(const Complex< T > &c)
Definition: Complex.h:797
double Modulus(const T *i, const T *j)
Definition: Math.h:2259
#define ItemsInArray(a)
Definition: Utility.h:223
constexpr T ArcHav(T x)
Definition: Math.h:1717
Complex< T > Exp(const Complex< T > &c)
Definition: Complex.h:705
ptrdiff_t distance_type
Definition: Defs.h:549
Exponential function 2**n, n being a signed integer.
Definition: Math.h:1626
constexpr long double Log2()
Definition: Math.h:740
double SumOfSquares(const T *i, const T *j)
Definition: Math.h:2303
constexpr long double TwoPi()
Definition: Math.h:431
Complex< T > Tan(const Complex< T > &c)
Definition: Complex.h:808
constexpr char SignChar(T x)
Definition: Math.h:877
double StableModulus(const T *i, const T *j)
Definition: Math.h:2278
unsigned int uint32
Definition: Defs.h:600
signed long long int64
Definition: Defs.h:610
T Trunc(T x)
Definition: Math.h:1003
constexpr T ArcTanh(T x)
Definition: Math.h:1701
constexpr T UasRad(T x)
Definition: Math.h:1803
Complex< T1 > Pow(const Complex< T1 > &c, T2 x)
Definition: Complex.h:738
Complex< T > Tanh(const Complex< T > &c)
Definition: Complex.h:840
double StdDev(const T *i, const T *j, double center)
Definition: Math.h:2453
#define int64_min
Definition: Defs.h:816
constexpr long double Log2e()
Definition: Math.h:766
double AvgDev(const T *i, const T *j, double center)
Definition: Math.h:2739
void Frexp(T x, T &m, int &p)
Definition: Math.h:614