PCL
Math.h
Go to the documentation of this file.
1 // ____ ______ __
2 // / __ \ / ____// /
3 // / /_/ // / / /
4 // / ____// /___ / /___ PixInsight Class Library
5 // /_/ \____//_____/ PCL 02.01.12.0947
6 // ----------------------------------------------------------------------------
7 // pcl/Math.h - Released 2019-04-30T16:30:41Z
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 // GCC 4.2 doesn't have sincos() on Mac OS X and FreeBSD
79 // MSVC doesn't have sincos() on Windows
80 #if !defined( __PCL_MACOSX ) && !defined( __PCL_FREEBSD ) && !defined( __PCL_WINDOWS )
81 # define __PCL_HAVE_SINCOS 1
82 #endif
83 
84 #ifdef __PCL_QT_INTERFACE
85 # include <QtWidgets/QtWidgets>
86 #endif
87 
88 namespace pcl
89 {
90 
91 // ----------------------------------------------------------------------------
92 
114 {
115  int32 edxFlags = 0;
116  int32 ecxFlags = 0;
117 
118 #ifdef _MSC_VER
119  int cpuInfo[ 4 ];
120  __cpuid( cpuInfo, 1 );
121  edxFlags = cpuInfo[3];
122  ecxFlags = cpuInfo[2];
123 #else
124  asm volatile( "mov $0x00000001, %%eax\n\t"
125  "cpuid\n\t"
126  "mov %%edx, %0\n\t"
127  "mov %%ecx, %1\n"
128  : "=r" (edxFlags), "=r" (ecxFlags) // output operands
129  : // input operands
130  : "%eax", "%ebx", "%ecx", "%edx" ); // clobbered registers
131 #endif
132 
133  if ( ecxFlags & (1u << 20) ) // SSE4.2
134  return 42;
135  if ( ecxFlags & (1u << 19) ) // SSE4.1
136  return 41;
137  if ( ecxFlags & 1u ) // SSE3
138  return 3;
139  if ( edxFlags & (1u << 26) ) // SSE2
140  return 2;
141  if ( edxFlags & (1u << 25) ) // SSE
142  return 1;
143  return 0;
144 }
145 
146 // ----------------------------------------------------------------------------
147 
152 #define __PCL_FLOAT_SGNMASK 0x80000000
153 #define __PCL_FLOAT_EXPMASK 0x7f800000
154 #define __PCL_FLOAT_SIGMASK 0x007fffff
155 
164 inline bool IsFinite( float x )
165 {
166  union { float f; uint32 u; } v = { x };
167  return (v.u & __PCL_FLOAT_EXPMASK) != __PCL_FLOAT_EXPMASK;
168 }
169 
180 inline bool IsNaN( float x )
181 {
182  union { float f; uint32 u; } v = { x };
183  return (v.u & __PCL_FLOAT_EXPMASK) == __PCL_FLOAT_EXPMASK &&
184  (v.u & __PCL_FLOAT_SIGMASK) != 0;
185 }
186 
199 inline int IsInfinity( float x )
200 {
201  union { float f; uint32 u; } v = { x };
202  if ( (v.u & __PCL_FLOAT_EXPMASK) == __PCL_FLOAT_EXPMASK &&
203  (v.u & __PCL_FLOAT_SIGMASK) == 0 )
204  return ((v.u & __PCL_FLOAT_SGNMASK) == 0) ? +1 : -1;
205  return 0;
206 }
207 
208 #define __PCL_DOUBLE_SGNMASK 0x80000000
209 #define __PCL_DOUBLE_EXPMASK 0x7ff00000
210 #define __PCL_DOUBLE_SIGMASK 0x000fffff
211 
220 inline bool IsFinite( double x )
221 {
222  union { double d; uint32 u[2]; } v = { x };
223  return (v.u[1] & __PCL_DOUBLE_EXPMASK) != __PCL_DOUBLE_EXPMASK;
224 }
225 
236 inline bool IsNaN( double x )
237 {
238  union { double d; uint32 u[2]; } v = { x };
239  return (v.u[1] & __PCL_DOUBLE_EXPMASK) == __PCL_DOUBLE_EXPMASK &&
240  (v.u[0] != 0 || (v.u[1] & __PCL_DOUBLE_SIGMASK) != 0);
241 }
242 
255 inline int IsInfinity( double x )
256 {
257  union { double d; uint32 u[2]; } v = { x };
258  if ( v.u[0] == 0 &&
259  (v.u[1] & __PCL_DOUBLE_SIGMASK) == 0 &&
260  (v.u[1] & __PCL_DOUBLE_EXPMASK) == __PCL_DOUBLE_EXPMASK )
261  return ((v.u[1] & __PCL_DOUBLE_SGNMASK) == 0) ? +1 : -1;
262  return 0;
263 }
264 
265 // ----------------------------------------------------------------------------
266 
275 inline float Abs( float x )
276 {
277  return std::fabs( x );
278 }
279 
283 inline double Abs( double x )
284 {
285  return std::fabs( x );
286 }
287 
291 inline long double Abs( long double x )
292 {
293  return std::fabs( x );
294 }
295 
299 inline signed int Abs( signed int x )
300 {
301  return ::abs( x );
302 }
303 
307 #if defined( __PCL_MACOSX ) && defined( __clang__ ) // turn off warning due to broken cstdlib in Xcode
308 _Pragma("clang diagnostic push")
309 _Pragma("clang diagnostic ignored \"-Wabsolute-value\"")
310 #endif
311 inline signed long Abs( signed long x )
312 {
313  return ::abs( x );
314 }
315 #if defined( __PCL_MACOSX ) && defined( __clang__ )
316 _Pragma("clang diagnostic pop")
317 #endif
318 
322 #if defined( _MSC_VER )
323 inline __int64 Abs( __int64 x )
324 {
325  return (x < 0) ? -x : +x;
326 }
327 #elif defined( __PCL_MACOSX ) && defined( __clang__ )
328 inline constexpr signed long long Abs( signed long long x )
329 {
330  return (x < 0) ? -x : +x;
331 }
332 #else
333 inline signed long long Abs( signed long long x )
334 {
335  return ::abs( x );
336 }
337 #endif
338 
342 inline signed short Abs( signed short x )
343 {
344  return (signed short)::abs( int( x ) );
345 }
346 
350 inline signed char Abs( signed char x )
351 {
352  return (signed char)::abs( int( x ) );
353 }
354 
358 inline wchar_t Abs( wchar_t x )
359 {
360  return (wchar_t)::abs( int( x ) );
361 }
362 
366 inline constexpr unsigned int Abs( unsigned int x )
367 {
368  return x;
369 }
370 
374 inline constexpr unsigned long Abs( unsigned long x )
375 {
376  return x;
377 }
378 
382 #ifdef _MSC_VER
383 inline unsigned __int64 Abs( unsigned __int64 x )
384 {
385  return x;
386 }
387 #else
388 inline constexpr unsigned long long Abs( unsigned long long x )
389 {
390  return x;
391 }
392 #endif
393 
397 inline constexpr unsigned short Abs( unsigned short x )
398 {
399  return x;
400 }
401 
405 inline constexpr unsigned char Abs( unsigned char x )
406 {
407  return x;
408 }
409 
410 // ----------------------------------------------------------------------------
411 
416 inline constexpr long double Pi()
417 {
418  return (long double)( 0.31415926535897932384626433832795029e+01L );
419 }
420 
425 inline constexpr long double TwoPi()
426 {
427  return (long double)( 0.62831853071795864769252867665590058e+01L );
428 }
429 
430 // ----------------------------------------------------------------------------
431 
436 template <typename T> inline constexpr T Angle( int d, T m )
437 {
438  return d + m/60;
439 }
440 
446 template <typename T> inline constexpr T Angle( int d, int m, T s )
447 {
448  return Angle( d, m + s/60 );
449 }
450 
451 // ----------------------------------------------------------------------------
452 
457 template <typename T> inline constexpr T ArcCos( T x )
458 {
459  PCL_PRECONDITION( T( -1 ) <= x && x <= T( 1 ) )
460  return std::acos( x );
461 }
462 
463 // ----------------------------------------------------------------------------
464 
469 template <typename T> inline constexpr T ArcSin( T x )
470 {
471  PCL_PRECONDITION( T( -1 ) <= x && x <= T( 1 ) )
472  return std::asin( x );
473 }
474 
475 // ----------------------------------------------------------------------------
476 
481 template <typename T> inline constexpr T ArcTan( T x )
482 {
483  return std::atan( x );
484 }
485 
486 // ----------------------------------------------------------------------------
487 
492 template <typename T> inline constexpr T ArcTan( T y, T x )
493 {
494  return std::atan2( y, x );
495 }
496 
497 // ----------------------------------------------------------------------------
498 
503 template <typename T> inline T ArcTan2Pi( T y, T x )
504 {
505  T r = ArcTan( y, x );
506  if ( r < 0 )
507  r = static_cast<T>( r + TwoPi() );
508  return r;
509 }
510 
511 // ----------------------------------------------------------------------------
512 
517 template <typename T> inline constexpr T Ceil( T x )
518 {
519  return std::ceil( x );
520 }
521 
522 // ----------------------------------------------------------------------------
523 
528 template <typename T> inline constexpr T Cos( T x )
529 {
530  return std::cos( x );
531 }
532 
533 // ----------------------------------------------------------------------------
534 
539 template <typename T> inline constexpr T Cosh( T x )
540 {
541  return std::cosh( x );
542 }
543 
544 // ----------------------------------------------------------------------------
545 
550 template <typename T> inline constexpr T Cotan( T x )
551 {
552  return T( 1 )/std::tan( x );
553 }
554 
555 // ----------------------------------------------------------------------------
556 
561 template <typename T> inline constexpr T Deg( T x )
562 {
563  return static_cast<T>( 0.572957795130823208767981548141051700441964e+02L * x );
564 }
565 
566 // ----------------------------------------------------------------------------
567 
572 template <typename T> inline constexpr T Exp( T x )
573 {
574  return std::exp( x );
575 }
576 
577 // ----------------------------------------------------------------------------
578 
591 template <typename T> struct PCL_CLASS Fact
592 {
593  T operator()( int n ) const
594  {
595  static T f[ 61 ] = { T( 1 ), T( 1 ), T( 2 ), T( 6 ), T( 24 ), T( 120 ) };
596  static int last = 5;
597  PCL_PRECONDITION( 0 <= n )
598  if ( last < n )
599  {
600  int m = Min( n, 60 );
601  T x = f[last];
602  while ( last < m )
603  {
604  x *= ++last;
605  f[last] = x;
606  }
607  while ( m < n )
608  x *= ++m;
609  return x;
610  }
611  return f[n];
612  }
613 };
614 
615 // ----------------------------------------------------------------------------
616 
621 template <typename T> inline constexpr T Floor( T x )
622 {
623  return std::floor( x );
624 }
625 
626 // ----------------------------------------------------------------------------
627 
633 template <typename T> inline constexpr T Frac( T x )
634 {
635  return std::modf( x, &x );
636 }
637 
638 // ----------------------------------------------------------------------------
639 
646 template <typename T> inline void Frexp( T x, T& m, int& p )
647 {
648  m = std::frexp( x, &p );
649 }
650 
651 // ----------------------------------------------------------------------------
652 
662 template <typename T> inline constexpr T Hav( T x )
663 {
664  return (1 - Cos( x ))/2;
665 }
666 
667 // ----------------------------------------------------------------------------
668 
673 template <typename T> inline constexpr T Ldexp( T m, int p )
674 {
675  return std::ldexp( m, p );
676 }
677 
678 // ----------------------------------------------------------------------------
679 
684 template <typename T> inline constexpr T Ln( T x )
685 {
686  return std::log( x );
687 }
688 
689 // ----------------------------------------------------------------------------
690 
695 inline constexpr long double Ln2()
696 {
697  return (long double)( 0.6931471805599453094172321214581766e+00L );
698 }
699 
700 // ----------------------------------------------------------------------------
701 
706 template <typename T> inline constexpr T Log( T x )
707 {
708  return std::log10( x );
709 }
710 
711 // ----------------------------------------------------------------------------
712 
717 inline constexpr long double Log2()
718 {
719  // Use the relation:
720  // log10(2) = ln(2)/ln(10)
721  return (long double)( 0.3010299956639811952137388947244930416265e+00L );
722 }
723 
724 // ----------------------------------------------------------------------------
725 
730 template <typename T> inline constexpr T Log2( T x )
731 {
732  // Use the relation:
733  // log2(x) = ln(x)/ln(2)
734  return std::log( x )/Ln2();
735 }
736 
737 // ----------------------------------------------------------------------------
738 
743 inline constexpr long double Log2e()
744 {
745  // Use the relation:
746  // log2(e) = 1/ln(2)
747  return (long double)( 0.14426950408889634073599246810018920709799e+01L );
748 }
749 
750 // ----------------------------------------------------------------------------
751 
756 inline constexpr long double Log2T()
757 {
758  // Use the relation:
759  // log2(10) = 1/log(2)
760  return (long double)( 0.33219280948873623478703194294893900118996e+01L );
761 }
762 
763 // ----------------------------------------------------------------------------
764 
769 template <typename T> inline constexpr T LogN( T n, T x )
770 {
771  return std::log( x )/std::log( n );
772 }
773 
774 // ----------------------------------------------------------------------------
775 
780 template <typename T> inline constexpr T Mod( T x, T y )
781 {
782  return std::fmod( x, y );
783 }
784 
785 // ----------------------------------------------------------------------------
786 
799 template <typename T, typename C> inline T Poly( T x, C c, int n )
800 {
801  PCL_PRECONDITION( n >= 0 )
802  T y;
803  for ( c += n, y = *c; n > 0; --n )
804  {
805  y *= x;
806  y += *--c;
807  }
808  return y;
809 }
810 
821 template <typename T> inline T Poly( T x, std::initializer_list<T> c )
822 {
823  PCL_PRECONDITION( c.size() > 0 )
824  return Poly( x, c.begin(), int( c.size() )-1 );
825 }
826 
827 // ----------------------------------------------------------------------------
828 
838 template <typename T> inline constexpr int Sign( T x )
839 {
840  return (x < 0) ? -1 : ((x > 0) ? +1 : 0);
841 }
842 
843 // ----------------------------------------------------------------------------
844 
854 template <typename T> inline constexpr char SignChar( T x )
855 {
856  return (x < 0) ? '-' : ((x > 0) ? '+' : ' ');
857 }
858 
859 // ----------------------------------------------------------------------------
860 
865 template <typename T> inline constexpr T Sin( T x )
866 {
867  return std::sin( x );
868 }
869 
870 // ----------------------------------------------------------------------------
871 
876 template <typename T> inline constexpr T Sinh( T x )
877 {
878  return std::sinh( x );
879 }
880 
881 // ----------------------------------------------------------------------------
882 
883 #ifdef __PCL_HAVE_SINCOS
884 
885 inline void __pcl_sincos__( float x, float& sx, float& cx )
886 {
887  ::sincosf( x, &sx, &cx );
888 }
889 
890 inline void __pcl_sincos__( double x, double& sx, double& cx )
891 {
892  ::sincos( x, &sx, &cx );
893 }
894 
895 inline void __pcl_sincos__( long double x, long double& sx, long double& cx )
896 {
897  ::sincosl( x, &sx, &cx );
898 }
899 
900 #endif
901 
915 template <typename T> inline void SinCos( T x, T& sx, T& cx )
916 {
917 #ifdef __PCL_HAVE_SINCOS
918  __pcl_sincos__( x, sx, cx );
919 #else
920  sx = std::sin( x );
921  cx = std::cos( x );
922 #endif
923 }
924 
925 // ----------------------------------------------------------------------------
926 
936 template <typename T> inline void Split( T x, T& i, T& f )
937 {
938  f = std::modf( x, &i );
939 }
940 
941 // ----------------------------------------------------------------------------
942 
947 template <typename T> inline constexpr T Sqrt( T x )
948 {
949  return std::sqrt( x );
950 }
951 
952 // ----------------------------------------------------------------------------
953 
958 template <typename T> inline constexpr T Tan( T x )
959 {
960  return std::tan( x );
961 }
962 
963 // ----------------------------------------------------------------------------
964 
969 template <typename T> inline constexpr T Tanh( T x )
970 {
971  return std::tanh( x );
972 }
973 
974 // ----------------------------------------------------------------------------
975 
980 template <typename T> inline T Trunc( T x )
981 {
982  (void)std::modf( x, &x );
983  return x;
984 }
985 
986 // ----------------------------------------------------------------------------
987 
988 #ifdef __PCL_HAVE_SSE2
989 
990 inline int __pcl_trunci__( float x )
991 {
992  return _mm_cvtt_ss2si( _mm_load_ss( &x ) );
993 }
994 
995 inline int __pcl_trunci__( double x )
996 {
997  return _mm_cvttsd_si32( _mm_load_sd( &x ) );
998 }
999 
1000 #endif
1001 
1012 template <typename T> inline int TruncInt( T x )
1013 {
1014  PCL_PRECONDITION( x >= int_min && x <= int_max )
1015 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1016  return int( x );
1017 #else
1018 # ifdef __PCL_HAVE_SSE2
1019  return __pcl_trunci__( x );
1020 # else
1021  return int( x );
1022 # endif
1023 #endif
1024 }
1025 
1034 template <typename T> inline int TruncI( T x )
1035 {
1036  return TruncInt( x );
1037 }
1038 
1039 #define TruncInt32( x ) TruncInt( x )
1040 #define TruncI32( x ) TruncInt( x )
1041 
1042 // ----------------------------------------------------------------------------
1043 
1044 #ifdef __PCL_HAVE_SSE2
1045 
1046 #if defined( __x86_64__ ) || defined( _M_X64 )
1047 
1048 inline int64 __pcl_trunci64__( float x )
1049 {
1050  return _mm_cvttss_si64( _mm_load_ss( &x ) );
1051 }
1052 
1053 inline int64 __pcl_trunci64__( double x )
1054 {
1055  return _mm_cvttsd_si64( _mm_load_sd( &x ) );
1056 }
1057 
1058 #else
1059 
1060 inline int64 __pcl_trunci64__( float x )
1061 {
1062  return int64( _mm_cvtt_ss2si( _mm_load_ss( &x ) ) );
1063 }
1064 
1065 inline int64 __pcl_trunci64__( double x )
1066 {
1067  return int64( x );
1068 }
1069 
1070 #endif
1071 
1072 #endif // __PCL_HAVE_SSE2
1073 
1084 template <typename T> inline int64 TruncInt64( T x )
1085 {
1086  PCL_PRECONDITION( x >= int64_min && x <= int64_max )
1087 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1088  return int64( Trunc( x ) );
1089 #else
1090 # ifdef __PCL_HAVE_SSE2
1091  return __pcl_trunci64__( x );
1092 # else
1093  return int64( Trunc( x ) );
1094 # endif
1095 #endif
1096 }
1097 
1106 template <typename T> inline int64 TruncI64( T x )
1107 {
1108  return TruncInt64( x );
1109 }
1110 
1111 // ----------------------------------------------------------------------------
1112 
1126 template <typename T> inline constexpr T Pow( T x, T y )
1127 {
1128  PCL_PRECONDITION( y < T( int_max ) )
1129  return std::pow( x, y );
1130 }
1131 
1132 // ----------------------------------------------------------------------------
1133 
1144 template <typename T> struct PCL_CLASS Pow10I
1145 {
1146  T operator ()( int n ) const
1147  {
1148  // Use fast table lookups and squaring up to |n| <= 50.
1149  static const T lut[] =
1150  {
1151 #define ___( x ) static_cast<T>( x )
1152  ___( 1.0e+00 ), ___( 1.0e+01 ), ___( 1.0e+02 ), ___( 1.0e+03 ), ___( 1.0e+04 ),
1153  ___( 1.0e+05 ), ___( 1.0e+06 ), ___( 1.0e+07 ), ___( 1.0e+08 ), ___( 1.0e+09 ),
1154  ___( 1.0e+10 ), ___( 1.0e+11 ), ___( 1.0e+12 ), ___( 1.0e+13 ), ___( 1.0e+14 ),
1155  ___( 1.0e+15 ), ___( 1.0e+16 ), ___( 1.0e+17 ), ___( 1.0e+18 ), ___( 1.0e+19 ),
1156  ___( 1.0e+20 ), ___( 1.0e+21 ), ___( 1.0e+22 ), ___( 1.0e+23 ), ___( 1.0e+24 ),
1157  ___( 1.0e+25 ), ___( 1.0e+26 ), ___( 1.0e+27 ), ___( 1.0e+28 ), ___( 1.0e+29 ),
1158  ___( 1.0e+30 ), ___( 1.0e+31 ), ___( 1.0e+32 ), ___( 1.0e+33 ), ___( 1.0e+34 ),
1159  ___( 1.0e+35 ), ___( 1.0e+36 ), ___( 1.0e+37 ), ___( 1.0e+38 ), ___( 1.0e+39 ),
1160  ___( 1.0e+40 ), ___( 1.0e+41 ), ___( 1.0e+42 ), ___( 1.0e+43 ), ___( 1.0e+44 ),
1161  ___( 1.0e+45 ), ___( 1.0e+46 ), ___( 1.0e+47 ), ___( 1.0e+48 ), ___( 1.0e+49 )
1162 #undef ___
1163  };
1164  static const int N = ItemsInArray( lut );
1165  int i = ::abs( n );
1166  double x;
1167  if ( i < N )
1168  x = lut[i];
1169  else
1170  {
1171  x = lut[N-1];
1172  while ( (i -= N-1) >= N )
1173  x *= x;
1174  if ( i != 0 )
1175  x *= lut[i];
1176  }
1177  return T( (n >= 0) ? x : 1/x );
1178  }
1179 };
1180 
1181 // ----------------------------------------------------------------------------
1182 
1187 template <typename T> inline T Pow10( T x )
1188 {
1189  int i = TruncInt( x );
1190  return (i == x) ? Pow10I<T>()( i ) : T( std::pow( 10, x ) );
1191 }
1192 
1193 // ----------------------------------------------------------------------------
1194 
1205 template <typename T> inline T RotL( T x, uint32 n )
1206 {
1207  static_assert( std::is_unsigned<T>::value,
1208  "RotL() can only be used for unsigned scalar types" );
1209  const uint8 mask = 8*sizeof( x ) - 1;
1210  const uint8 r = uint8( n & mask );
1211  return (x << r) | (x >> ((-r) & mask));
1212  // Or, equivalently but less optimized:
1213  //return (x << r) | (x >> (1+mask-r));
1214 }
1215 
1226 template <typename T> inline T RotR( T x, uint32 n )
1227 {
1228  static_assert( std::is_unsigned<T>::value,
1229  "RotR() can only be used for unsigned scalar types" );
1230  const uint8 mask = 8*sizeof( x ) - 1;
1231  const uint8 r = uint8( n & mask );
1232  return (x >> r) | (x << ((-r) & mask));
1233  // Or, equivalently but less optimized:
1234  //return (x >> r) | (x << (1+mask-r));
1235 }
1236 
1237 // ----------------------------------------------------------------------------
1238 
1248 inline double Round( double x )
1249 {
1250 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1251 
1252  return floor( x + 0.5 );
1253 
1254 #else
1255 
1256 # ifdef _MSC_VER
1257 # ifdef _M_X64
1258  return double( _mm_cvtsd_si64( _mm_load_sd( &x ) ) );
1259 # else
1260  __asm fld x
1261  __asm frndint
1262 # endif
1263 # else
1264  double r;
1265  asm volatile( "frndint\n": "=t" (r) : "0" (x) );
1266  return r;
1267 # endif
1268 
1269 #endif
1270 }
1271 
1281 inline float Round( float x )
1282 {
1283 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1284 
1285  return floorf( x + 0.5F );
1286 
1287 #else
1288 
1289 # ifdef _MSC_VER
1290 # ifdef _M_X64
1291  return float( _mm_cvtss_si32( _mm_load_ss( &x ) ) );
1292 # else
1293  __asm fld x
1294  __asm frndint
1295 # endif
1296 # else
1297  float r;
1298  asm volatile( "frndint\n": "=t" (r) : "0" (x) );
1299  return r;
1300 # endif
1301 
1302 #endif
1303 }
1304 
1314 inline long double Round( long double x )
1315 {
1316 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1317 
1318  return floorl( x + 0.5L );
1319 
1320 #else
1321 
1322 # ifdef _MSC_VER
1323 # ifdef _M_X64
1324  double _x = x;
1325  return (long double)_mm_cvtsd_si64( _mm_load_sd( &_x ) );
1326 # else
1327  __asm fld x
1328  __asm frndint
1329 # endif
1330 # else
1331  long double r;
1332  asm volatile( "frndint\n": "=t" (r) : "0" (x) );
1333  return r;
1334 # endif
1335 
1336 #endif
1337 }
1338 
1339 // ----------------------------------------------------------------------------
1340 
1374 template <typename T> inline int RoundInt( T x )
1375 {
1376  PCL_PRECONDITION( x >= int_min && x <= int_max )
1377 
1378 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1379 
1380  return int( Round( x ) );
1381 
1382 #else
1383 
1384  volatile union { double d; int32 i; } v = { x + 6755399441055744.0 };
1385  return v.i; // ### NB: Assuming little-endian architecture, i.e. Intel.
1386 
1387 /*
1388  ### Deprecated code - The above code based on magic numbers is faster and
1389  more portable across platforms and compilers.
1390 
1391  // ### N.B.: Default FPU rounding mode assumed to be nearest integer.
1392  int r;
1393 
1394 # ifdef _MSC_VER
1395  __asm fld x
1396  __asm fistp dword ptr r
1397 # else
1398  asm volatile( "fistpl %0\n" : "=m" (r) : "t" (x) : "st" );
1399 # endif
1400 
1401  return r;
1402 */
1403 
1404 #endif
1405 }
1406 
1415 template <typename T> inline int RoundI( T x )
1416 {
1417  return RoundInt( x );
1418 }
1419 
1428 template <typename T> inline int RoundIntBanker( T x )
1429 {
1430  return RoundInt( x );
1431 }
1432 
1455 template <typename T> inline int RoundIntArithmetic( T x )
1456 {
1457  PCL_PRECONDITION( x >= int_min && x <= int_max )
1458 
1459  int i = TruncInt( x );
1460  if ( i < 0 )
1461  {
1462  if ( x - i <= T( -0.5 ) )
1463  return i-1;
1464  }
1465  else
1466  {
1467  if ( x - i >= T( +0.5 ) )
1468  return i+1;
1469  }
1470  return i;
1471 }
1472 
1473 // ----------------------------------------------------------------------------
1474 
1489 inline int64 RoundInt64( double x )
1490 {
1491 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1492 
1493  return int64( Round( x ) );
1494 
1495 #else
1496 
1497  // ### N.B.: Default FPU rounding mode assumed to be nearest integer.
1498 
1499 # ifdef _MSC_VER
1500 # ifdef _M_X64
1501  return _mm_cvtsd_si64( _mm_load_sd( &x ) );
1502 # else
1503  int64 r;
1504  __asm fld x
1505  __asm fistp qword ptr r
1506  return r;
1507 # endif
1508 # else
1509  int64 r;
1510  asm volatile( "fistpll %0\n" : "=m" (r) : "t" (x) : "st" );
1511  return r;
1512 # endif
1513 
1514 #endif
1515 }
1516 
1523 inline int64 RoundI64( double x )
1524 {
1525  return RoundInt64( x );
1526 }
1527 
1550 inline int64 RoundInt64Arithmetic( double x )
1551 {
1552  int64 i = TruncInt64( x );
1553  if ( i < 0 )
1554  {
1555  if ( x - i <= -0.5 )
1556  return i-1;
1557  }
1558  else
1559  {
1560  if ( x - i >= +0.5 )
1561  return i+1;
1562  }
1563  return i;
1564 }
1565 
1566 // ----------------------------------------------------------------------------
1567 
1572 template <typename T> inline T Round( T x, int n )
1573 {
1574  PCL_PRECONDITION( n >= 0 )
1575  T p = Pow10I<T>()( n ); return Round( p*x )/p;
1576 }
1577 
1578 // ----------------------------------------------------------------------------
1579 
1584 template <typename T> inline constexpr T Pow2( T x )
1585 {
1586  // Use the relation:
1587  // 2**x = e**(x*ln(2))
1588  return Exp( x*Ln2() );
1589 }
1590 
1591 // ----------------------------------------------------------------------------
1592 
1603 template <typename T> struct PCL_CLASS Pow2I
1604 {
1605  T operator ()( int n ) const
1606  {
1607  // We shift left a single bit in 31-bit chunks.
1608  int i = ::abs( n ), p;
1609  double x = uint32( 1 ) << (p = Min( i, 31 ));
1610  while ( (i -= p) != 0 )
1611  x *= uint32( 1 ) << (p = Min( i, 31 ));
1612  return T( (n >= 0) ? x : 1/x );
1613  }
1614 };
1615 
1616 // ----------------------------------------------------------------------------
1617 
1622 template <typename T> inline T PowI( T x, int n )
1623 {
1624  if ( n == 0 )
1625  return 1;
1626 
1627  int i = Abs( n );
1628  T r = x;
1629  if ( i > 1 )
1630  {
1631  do
1632  r *= r;
1633  while ( (i >>= 1) >= 2 );
1634 
1635  if ( (n & 1) != 0 )
1636  r *= x;
1637  }
1638  return (n >= 0) ? r : 1/r;
1639 }
1640 
1641 // ----------------------------------------------------------------------------
1642 
1650 template <typename T> inline constexpr T ArcSinh( T x )
1651 {
1652  return Ln( x + Sqrt( 1 + x*x ) );
1653 }
1654 
1655 // ----------------------------------------------------------------------------
1656 
1664 template <typename T> inline constexpr T ArcCosh( T x )
1665 {
1666  return 2*Ln( Sqrt( (x + 1)/2 ) + Sqrt( (x - 1)/2 ) );
1667 }
1668 
1669 // ----------------------------------------------------------------------------
1670 
1678 template <typename T> inline constexpr T ArcTanh( T x )
1679 {
1680  return (Ln( 1 + x ) - Ln( 1 - x ))/2;
1681 }
1682 
1683 // ----------------------------------------------------------------------------
1684 
1694 template <typename T> inline constexpr T ArcHav( T x )
1695 {
1696  return 2*ArcSin( Sqrt( x ) );
1697 }
1698 
1699 // ----------------------------------------------------------------------------
1700 
1705 template <typename T> inline constexpr T Rad( T x )
1706 {
1707  return static_cast<T>( 0.174532925199432957692369076848861272222e-01L * x );
1708 }
1709 
1710 // ----------------------------------------------------------------------------
1711 
1716 template <typename T> inline constexpr T RadMin( T x )
1717 {
1718  return Deg( x )*60;
1719 }
1720 
1721 // ----------------------------------------------------------------------------
1722 
1727 template <typename T> inline constexpr T RadSec( T x )
1728 {
1729  return Deg( x )*3600;
1730 }
1731 
1732 // ----------------------------------------------------------------------------
1733 
1738 template <typename T> inline constexpr T MinRad( T x )
1739 {
1740  return Rad( x/60 );
1741 }
1742 
1743 // ----------------------------------------------------------------------------
1744 
1749 template <typename T> inline constexpr T SecRad( T x )
1750 {
1751  return Rad( x/3600 );
1752 }
1753 
1758 template <typename T> inline constexpr T AsRad( T x )
1759 {
1760  return SecRad( x );
1761 }
1762 
1763 // ----------------------------------------------------------------------------
1764 
1769 template <typename T> inline constexpr T MasRad( T x )
1770 {
1771  return Rad( x/3600000 );
1772 }
1773 
1774 // ----------------------------------------------------------------------------
1775 
1780 template <typename T> inline constexpr T UasRad( T x )
1781 {
1782  return Rad( x/3600000000 );
1783 }
1784 
1785 // ----------------------------------------------------------------------------
1786 
1792 template <typename T> inline constexpr T Mod2Pi( T x )
1793 {
1794  return Mod( x, static_cast<T>( TwoPi() ) );
1795 }
1796 
1797 // ----------------------------------------------------------------------------
1798 
1803 template <typename T> inline constexpr T Norm2Pi( T x )
1804 {
1805  return ((x = Mod2Pi( x )) < 0) ? x + static_cast<T>( TwoPi() ) : x;
1806 }
1807 
1808 // ----------------------------------------------------------------------------
1809 
1822 template <typename T, typename T1, typename T2>
1823 inline void Rotate( T& x, T& y, T1 sa, T1 ca, T2 xc, T2 yc )
1824 {
1825  T1 dx = T1( x ) - T1( xc );
1826  T1 dy = T1( y ) - T1( yc );
1827  x = T( T1( xc ) + ca*dx + sa*dy );
1828  y = T( T1( yc ) - sa*dx + ca*dy );
1829 }
1830 
1841 template <typename T1, typename T2>
1842 inline void Rotate( int& x, int& y, T1 sa, T1 ca, T2 xc, T2 yc )
1843 {
1844  T1 dx = T1( x ) - T1( xc );
1845  T1 dy = T1( y ) - T1( yc );
1846  x = RoundInt( T1( xc ) + ca*dx + sa*dy );
1847  y = RoundInt( T1( yc ) - sa*dx + ca*dy );
1848 }
1849 
1860 template <typename T1, typename T2>
1861 inline void Rotate( long& x, long& y, T1 sa, T1 ca, T2 xc, T2 yc )
1862 {
1863  T1 dx = T1( x ) - T1( xc );
1864  T1 dy = T1( y ) - T1( yc );
1865  x = (long)RoundInt( T1( xc ) + ca*dx + sa*dy );
1866  y = (long)RoundInt( T1( yc ) - sa*dx + ca*dy );
1867 }
1868 
1879 template <typename T1, typename T2>
1880 inline void Rotate( int64& x, int64& y, T1 sa, T1 ca, T2 xc, T2 yc )
1881 {
1882  T1 dx = T1( x ) - T1( xc );
1883  T1 dy = T1( y ) - T1( yc );
1884  x = RoundInt64( T1( xc ) + ca*dx + sa*dy );
1885  y = RoundInt64( T1( yc ) - sa*dx + ca*dy );
1886 }
1887 
1903 template <typename T, typename T1, typename T2>
1904 inline void Rotate( T& x, T& y, T1 a, T2 xc, T2 yc )
1905 {
1906  T1 sa, ca; SinCos( a, sa, ca ); Rotate( x, y, sa, ca, xc, yc );
1907 }
1908 
1909 // ----------------------------------------------------------------------------
1910 
1922 template <typename T> inline double Norm( const T* i, const T* j, double p )
1923 {
1924  PCL_PRECONDITION( p > 0 )
1925  double N = 0;
1926  for ( ; i < j; ++i )
1927  N += Pow( Abs( double( *i ) ), p );
1928  return Pow( N, 1/p );
1929 }
1930 
1937 template <typename T> inline double L1Norm( const T* i, const T* j )
1938 {
1939  double N = 0;
1940  for ( ; i < j; ++i )
1941  N += Abs( *i );
1942  return N;
1943 }
1944 
1951 template <typename T> inline double L2Norm( const T* i, const T* j )
1952 {
1953  double N = 0;
1954  for ( ; i < j; ++i )
1955  N += double( *i ) * *i;
1956  return Sqrt( N );
1957 }
1958 
1965 template <typename T> inline double Norm( const T* i, const T* j )
1966 {
1967  return L2Norm( i, j );
1968 }
1969 
1970 // ----------------------------------------------------------------------------
1971 
2012 void PCL_FUNC ComplexTimeToJD( int& jdi, double& jdf, int year, int month, int day, double dayf = 0 );
2013 
2053 inline double ComplexTimeToJD( int year, int month, int day, double dayf = 0 )
2054 {
2055  int jdi;
2056  double jdf;
2057  ComplexTimeToJD( jdi, jdf, year, month, day, dayf );
2058  return jdi + jdf;
2059 }
2060 
2091 void PCL_FUNC JDToComplexTime( int& year, int& month, int& day, double& dayf, int jdi, double jdf );
2092 
2122 inline void JDToComplexTime( int& year, int& month, int& day, double& dayf, double jd )
2123 {
2124  JDToComplexTime( year, month, day, dayf, TruncInt( jd ), Frac( jd ) );
2125 }
2126 
2127 // ----------------------------------------------------------------------------
2128 
2145 template <typename S1, typename S2, typename S3, typename D>
2146 inline void DecimalToSexagesimal( int& sign, S1& s1, S2& s2, S3& s3, const D& d )
2147 {
2148  double t1 = Abs( d );
2149  double t2 = Frac( t1 )*60;
2150  double t3 = Frac( t2 )*60;
2151  sign = (d < 0) ? -1 : +1;
2152  s1 = S1( TruncInt( t1 ) );
2153  s2 = S2( TruncInt( t2 ) );
2154  s3 = S3( t3 );
2155 }
2156 
2165 template <typename S1, typename S2, typename S3>
2166 inline double SexagesimalToDecimal( int sign, const S1& s1, const S2& s2 = S2( 0 ), const S3& s3 = S3( 0 ) )
2167 {
2168  double d = Abs( s1 ) + (s2 + s3/60)/60;
2169  return (sign < 0) ? -d : d;
2170 }
2171 
2172 // ----------------------------------------------------------------------------
2173 
2192 template <typename T> inline double Sum( const T* i, const T* j )
2193 {
2194  double sum = 0;
2195  for ( ; i < j; ++i )
2196  sum += double( *i );
2197  return sum;
2198 }
2199 
2211 template <typename T> inline double StableSum( const T* i, const T* j )
2212 {
2213  double sum = 0;
2214  double eps = 0;
2215  for ( ; i < j; ++i )
2216  {
2217  double y = double( *i ) - eps;
2218  double t = sum + y;
2219  eps = (t - sum) - y;
2220  sum = t;
2221  }
2222  return sum;
2223 }
2224 
2236 template <typename T> inline double Modulus( const T* i, const T* j )
2237 {
2238  double S = 0;
2239  for ( ; i < j; ++i )
2240  S += Abs( double( *i ) );
2241  return S;
2242 }
2243 
2255 template <typename T> inline double StableModulus( const T* i, const T* j )
2256 {
2257  double sum = 0;
2258  double eps = 0;
2259  for ( ; i < j; ++i )
2260  {
2261  double y = Abs( double( *i ) ) - eps;
2262  double t = sum + y;
2263  eps = (t - sum) - y;
2264  sum = t;
2265  }
2266  return sum;
2267 }
2268 
2280 template <typename T> inline double SumOfSquares( const T* i, const T* j )
2281 {
2282  double Q = 0;
2283  for ( ; i < j; ++i )
2284  Q += double( *i ) * *i;
2285  return Q;
2286 }
2287 
2299 template <typename T> inline double StableSumOfSquares( const T* i, const T* j )
2300 {
2301  double sum = 0;
2302  double eps = 0;
2303  for ( ; i < j; ++i )
2304  {
2305  double y = double( *i ) * *i - eps;
2306  double t = sum + y;
2307  eps = (t - sum) - y;
2308  sum = t;
2309  }
2310  return sum;
2311 }
2312 
2324 template <typename T> inline double Mean( const T* i, const T* j )
2325 {
2326  distance_type n = j - i;
2327  if ( n < 1 )
2328  return 0;
2329  return Sum( i, j )/n;
2330 }
2331 
2343 template <typename T> inline double StableMean( const T* i, const T* j )
2344 {
2345  distance_type n = j - i;
2346  if ( n < 1 )
2347  return 0;
2348  return StableSum( i, j )/n;
2349 }
2350 
2368 template <typename T> inline double Variance( const T* i, const T* j, double center )
2369 {
2370  distance_type n = j - i;
2371  if ( n < 2 )
2372  return 0;
2373  double var = 0, eps = 0;
2374  for ( ; i < j; ++i )
2375  {
2376  double d = double( *i ) - center;
2377  var += d*d;
2378  eps += d;
2379  }
2380  return (var - eps*eps/n)/(n - 1);
2381 }
2382 
2399 template <typename T> inline double Variance( const T* i, const T* j )
2400 {
2401  distance_type n = j - i;
2402  if ( n < 2 )
2403  return 0;
2404  double m = 0;
2405  for ( const T* f = i; f < j; ++f )
2406  m += double( *f );
2407  m /= n;
2408  double var = 0, eps = 0;
2409  for ( const T* f = i; f < j; ++f )
2410  {
2411  double d = double( *f ) - m;
2412  var += d*d;
2413  eps += d;
2414  }
2415  return (var - eps*eps/n)/(n - 1);
2416 }
2417 
2426 template <typename T> inline double StdDev( const T* i, const T* j, double center )
2427 {
2428  return Sqrt( Variance( i, j, center ) );
2429 }
2430 
2438 template <typename T> inline double StdDev( const T* i, const T* j )
2439 {
2440  return Sqrt( Variance( i, j ) );
2441 }
2442 
2443 #define CMPXCHG( a, b ) \
2444  if ( i[b] < i[a] ) pcl::Swap( i[a], i[b] )
2445 
2446 #define MEAN( a, b ) \
2447  (double( a ) + double( b ))/2
2448 
2487 template <typename T> inline double Median( T* i, T* j )
2488 {
2489  distance_type n = j - i;
2490  if ( n < 1 )
2491  return 0;
2492 
2493  switch ( n )
2494  {
2495  case 1: // !?
2496  return i[0];
2497  case 2:
2498  return MEAN( i[0], i[1] );
2499  case 3:
2500  CMPXCHG( 0, 1 ); CMPXCHG( 1, 2 );
2501  return pcl::Max( i[0], i[1] );
2502  case 4:
2503  CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
2504  CMPXCHG( 1, 3 );
2505  return MEAN( i[1], i[2] );
2506  case 5:
2507  CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 0, 3 );
2508  CMPXCHG( 1, 4 ); CMPXCHG( 1, 2 ); CMPXCHG( 2, 3 );
2509  return pcl::Max( i[1], i[2] );
2510  case 6:
2511  CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
2512  CMPXCHG( 1, 3 ); CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 );
2513  CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 1, 4 );
2514  CMPXCHG( 2, 4 ); CMPXCHG( 3, 5 ); CMPXCHG( 3, 4 );
2515  return MEAN( i[2], i[3] );
2516  case 7:
2517  CMPXCHG( 0, 5 ); CMPXCHG( 0, 3 ); CMPXCHG( 1, 6 );
2518  CMPXCHG( 2, 4 ); CMPXCHG( 0, 1 ); CMPXCHG( 3, 5 );
2519  CMPXCHG( 2, 6 ); CMPXCHG( 2, 3 ); CMPXCHG( 3, 6 );
2520  CMPXCHG( 4, 5 ); CMPXCHG( 1, 4 ); CMPXCHG( 1, 3 );
2521  return pcl::Min( i[3], i[4] );
2522  case 8:
2523  CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 2, 6 );
2524  CMPXCHG( 3, 7 ); CMPXCHG( 0, 2 ); CMPXCHG( 1, 3 );
2525  CMPXCHG( 4, 6 ); CMPXCHG( 5, 7 ); CMPXCHG( 2, 4 );
2526  CMPXCHG( 3, 5 ); CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 );
2527  CMPXCHG( 4, 5 ); CMPXCHG( 6, 7 ); CMPXCHG( 1, 4 );
2528  CMPXCHG( 3, 6 );
2529  return MEAN( i[3], i[4] );
2530  case 9:
2531  CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
2532  CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 6, 7 );
2533  CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
2534  CMPXCHG( 0, 3 ); CMPXCHG( 5, 8 ); CMPXCHG( 4, 7 );
2535  CMPXCHG( 3, 6 ); CMPXCHG( 1, 4 ); CMPXCHG( 2, 5 );
2536  CMPXCHG( 4, 7 ); CMPXCHG( 4, 2 ); CMPXCHG( 6, 4 );
2537  return pcl::Min( i[2], i[4] );
2538  default:
2539  {
2540  distance_type n2 = distance_type( n >> 1 );
2541  if ( n & 1 )
2542  return *pcl::Select( i, j, n2 );
2543  return MEAN( *pcl::Select( i, j, n2 ), *pcl::Select( i, j, n2-1 ) );
2544  }
2545  }
2546 }
2547 
2548 #undef CMPXCHG
2549 
2550 double PCL_FUNC Median( unsigned char* i, unsigned char* j );
2551 double PCL_FUNC Median( signed char* i, signed char* j );
2552 double PCL_FUNC Median( wchar_t* i, wchar_t* j );
2553 double PCL_FUNC Median( unsigned short* i, unsigned short* j );
2554 double PCL_FUNC Median( signed short* i, signed short* j );
2555 double PCL_FUNC Median( unsigned int* i, unsigned int* j );
2556 double PCL_FUNC Median( signed int* i, signed int* j );
2557 double PCL_FUNC Median( unsigned long* i, unsigned long* j );
2558 double PCL_FUNC Median( signed long* i, signed long* j );
2559 double PCL_FUNC Median( unsigned long long* i, unsigned long long* j );
2560 double PCL_FUNC Median( signed long long* i, signed long long* j );
2561 double PCL_FUNC Median( float* i, float* j );
2562 double PCL_FUNC Median( double* i, double* j );
2563 double PCL_FUNC Median( long double* i, long double* j );
2564 
2565 #define CMPXCHG( a, b ) \
2566  if ( p( i[b], i[a] ) ) pcl::Swap( i[a], i[b] )
2567 
2580 template <typename T, class BP> inline double Median( T* i, T* j, BP p )
2581 {
2582  distance_type n = j - i;
2583  if ( n < 1 )
2584  return 0;
2585 
2586  switch ( n )
2587  {
2588  case 1: // !?
2589  return i[0];
2590  case 2:
2591  return MEAN( i[0], i[1] );
2592  case 3:
2593  CMPXCHG( 0, 1 ); CMPXCHG( 1, 2 );
2594  return pcl::Max( i[0], i[1] );
2595  case 4:
2596  CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
2597  CMPXCHG( 1, 3 );
2598  return MEAN( i[1], i[2] );
2599  case 5:
2600  CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 0, 3 );
2601  CMPXCHG( 1, 4 ); CMPXCHG( 1, 2 ); CMPXCHG( 2, 3 );
2602  return pcl::Max( i[1], i[2] );
2603  case 6:
2604  CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
2605  CMPXCHG( 1, 3 ); CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 );
2606  CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 1, 4 );
2607  CMPXCHG( 2, 4 ); CMPXCHG( 3, 5 ); CMPXCHG( 3, 4 );
2608  return MEAN( i[2], i[3] );
2609  case 7:
2610  CMPXCHG( 0, 5 ); CMPXCHG( 0, 3 ); CMPXCHG( 1, 6 );
2611  CMPXCHG( 2, 4 ); CMPXCHG( 0, 1 ); CMPXCHG( 3, 5 );
2612  CMPXCHG( 2, 6 ); CMPXCHG( 2, 3 ); CMPXCHG( 3, 6 );
2613  CMPXCHG( 4, 5 ); CMPXCHG( 1, 4 ); CMPXCHG( 1, 3 );
2614  return pcl::Min( i[3], i[4] );
2615  case 8:
2616  CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 2, 6 );
2617  CMPXCHG( 3, 7 ); CMPXCHG( 0, 2 ); CMPXCHG( 1, 3 );
2618  CMPXCHG( 4, 6 ); CMPXCHG( 5, 7 ); CMPXCHG( 2, 4 );
2619  CMPXCHG( 3, 5 ); CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 );
2620  CMPXCHG( 4, 5 ); CMPXCHG( 6, 7 ); CMPXCHG( 1, 4 );
2621  CMPXCHG( 3, 6 );
2622  return MEAN( i[3], i[4] );
2623  case 9:
2624  CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
2625  CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 6, 7 );
2626  CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
2627  CMPXCHG( 0, 3 ); CMPXCHG( 5, 8 ); CMPXCHG( 4, 7 );
2628  CMPXCHG( 3, 6 ); CMPXCHG( 1, 4 ); CMPXCHG( 2, 5 );
2629  CMPXCHG( 4, 7 ); CMPXCHG( 4, 2 ); CMPXCHG( 6, 4 );
2630  return pcl::Min( i[2], i[4] );
2631  default:
2632  {
2633  distance_type n2 = distance_type( n >> 1 );
2634  if ( n & 1 )
2635  return *pcl::Select( i, j, n2, p );
2636  return MEAN( *pcl::Select( i, j, n2, p ), *pcl::Select( i, j, n2-1, p ) );
2637  }
2638  }
2639 }
2640 
2641 #undef CMPXCHG
2642 #undef MEAN
2643 
2653 template <typename T> inline double NondestructiveMedian( const T* i, const T* j )
2654 {
2655  distance_type n = j - i;
2656  if ( n < 2 )
2657  return 0;
2658  double* d = new double[ n ];
2659  double* p = d;
2660  for ( const T* f = i; f < j; ++f, ++p )
2661  *p = double( *f );
2662  double m = pcl::Median( d, d+n );
2663  delete [] d;
2664  return m;
2665 }
2666 
2678 template <typename T, class BP> inline double NondestructiveMedian( const T* i, const T* j, BP p )
2679 {
2680  distance_type n = j - i;
2681  if ( n < 2 )
2682  return 0;
2683  double* d = new double[ n ];
2684  double* t = d;
2685  for ( const T* f = i; f < j; ++f, ++t )
2686  *t = double( *f );
2687  double m = pcl::Median( d, d+n, p );
2688  delete [] d;
2689  return m;
2690 }
2691 
2712 template <typename T> inline double AvgDev( const T* i, const T* j, double center )
2713 {
2714  distance_type n = j - i;
2715  if ( n < 2 )
2716  return 0;
2717  double d = 0;
2718  for ( ; i < j; ++i )
2719  d += Abs( double( *i ) - center );
2720  return d/n;
2721 }
2722 
2743 template <typename T> inline double StableAvgDev( const T* i, const T* j, double center )
2744 {
2745  distance_type n = j - i;
2746  if ( n < 2 )
2747  return 0;
2748  double sum = 0;
2749  double eps = 0;
2750  for ( ; i < j; ++i )
2751  {
2752  double y = Abs( double( *i ) - center ) - eps;
2753  double t = sum + y;
2754  eps = (t - sum) - y;
2755  sum = t;
2756  }
2757  return sum/n;
2758 }
2759 
2779 template <typename T> inline double AvgDev( const T* i, const T* j )
2780 {
2781  distance_type n = j - i;
2782  if ( n < 2 )
2783  return 0;
2784  T* t = new T[ n ];
2785  pcl::Copy( t, i, j );
2786  double m = Median( t, t+n );
2787  delete [] t;
2788  double d = 0;
2789  for ( ; i < j; ++i )
2790  d += Abs( double( *i ) - m );
2791  return d/n;
2792 }
2793 
2813 template <typename T> inline double StableAvgDev( const T* i, const T* j )
2814 {
2815  distance_type n = j - i;
2816  if ( n < 2 )
2817  return 0;
2818  T* t = new T[ n ];
2819  pcl::Copy( t, i, j );
2820  double m = Median( t, t+n );
2821  delete [] t;
2822  return StableAvgDev( i, j, m );
2823 }
2824 
2838 template <typename T> inline double MAD( const T* i, const T* j, double center )
2839 {
2840  distance_type n = j - i;
2841  if ( n < 2 )
2842  return 0;
2843  double* d = new double[ n ];
2844  double* p = d;
2845  for ( const T* f = i; f < j; ++f, ++p )
2846  *p = Abs( double( *f ) - center );
2847  double m = pcl::Median( d, d+n );
2848  delete [] d;
2849  return m;
2850 }
2851 
2865 template <typename T> inline double MAD( const T* i, const T* j )
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 = double( *f );
2874  double m = pcl::Median( d, d+n );
2875  p = d;
2876  for ( const T* f = i; f < j; ++f, ++p )
2877  *p = Abs( *f - m );
2878  m = pcl::Median( d, d+n );
2879  delete [] d;
2880  return m;
2881 }
2882 
2912 template <typename T> double Sn( T* x, T* xn )
2913 {
2914  /*
2915  * N.B.: In the code below, lines commented with an asterisk (*) have been
2916  * modified with respect to the FORTRAN original to account for zero-based
2917  * array indices.
2918  */
2919 
2920  distance_type n = xn - x;
2921  if ( n < 2 )
2922  return 0;
2923 
2924  pcl::Sort( x, xn );
2925 
2926  T* a2 = new T[ n ];
2927  a2[0] = x[n >> 1] - x[0]; // *
2928 
2929  distance_type nh = (n + 1) >> 1;
2930 
2931  for ( distance_type i = 2; i <= nh; ++i )
2932  {
2933  distance_type nA = i-1;
2934  distance_type nB = n - i;
2935  distance_type diff = nB - nA;
2936  distance_type leftA = 1;
2937  distance_type leftB = 1;
2938  distance_type rightA = nB;
2939  distance_type Amin = (diff >> 1) + 1;
2940  distance_type Amax = (diff >> 1) + nA;
2941 
2942  while ( leftA < rightA )
2943  {
2944  distance_type length = rightA - leftA + 1;
2945  distance_type even = (length & 1) == 0;
2946  distance_type half = (length - 1) >> 1;
2947  distance_type tryA = leftA + half;
2948  distance_type tryB = leftB + half;
2949 
2950  if ( tryA < Amin )
2951  leftA = tryA + even;
2952  else
2953  {
2954  if ( tryA > Amax )
2955  {
2956  rightA = tryA;
2957  leftB = tryB + even;
2958  }
2959  else
2960  {
2961  T medA = x[i-1] - x[i-2 - tryA + Amin];// *
2962  T medB = x[tryB + i-1] - x[i-1]; // *
2963  if ( medA >= medB )
2964  {
2965  rightA = tryA;
2966  leftB = tryB + even;
2967  }
2968  else
2969  leftA = tryA + even;
2970  }
2971  }
2972  }
2973 
2974  if ( leftA > Amax )
2975  a2[i-1] = x[leftB + i-1] - x[i-1]; // *
2976  else
2977  {
2978  T medA = x[i-1] - x[i-2 - leftA + Amin]; // *
2979  T medB = x[leftB + i-1] - x[i-1];
2980  a2[i-1] = pcl::Min( medA, medB ); // *
2981  }
2982  }
2983 
2984  for ( distance_type i = nh + 1; i < n; ++i )
2985  {
2986  distance_type nA = n - i;
2987  distance_type nB = i - 1;
2988  distance_type diff = nB - nA;
2989  distance_type leftA = 1;
2990  distance_type leftB = 1;
2991  distance_type rightA = nB;
2992  distance_type Amin = (diff >> 1) + 1;
2993  distance_type Amax = (diff >> 1) + nA;
2994 
2995  while ( leftA < rightA )
2996  {
2997  distance_type length = rightA - leftA + 1;
2998  distance_type even = (length & 1) == 0;
2999  distance_type half = (length - 1) >> 1;
3000  distance_type tryA = leftA + half;
3001  distance_type tryB = leftB + half;
3002 
3003  if ( tryA < Amin)
3004  leftA = tryA + even;
3005  else
3006  {
3007  if ( tryA > Amax )
3008  {
3009  rightA = tryA;
3010  leftB = tryB + even;
3011  }
3012  else
3013  {
3014  T medA = x[i + tryA - Amin] - x[i-1]; // *
3015  T medB = x[i-1] - x[i-1 - tryB]; // *
3016  if ( medA >= medB )
3017  {
3018  rightA = tryA;
3019  leftB = tryB + even;
3020  }
3021  else
3022  leftA = tryA + even;
3023  }
3024  }
3025  }
3026 
3027  if ( leftA > Amax )
3028  a2[i-1] = x[i-1] - x[i-1 - leftB]; // *
3029  else
3030  {
3031  T medA = x[i + leftA - Amin] - x[i-1]; // *
3032  T medB = x[i-1] - x[i-1 - leftB]; // *
3033  a2[i-1] = pcl::Min( medA, medB ); // *
3034  }
3035  }
3036 
3037  a2[n-1] = x[n-1] - x[nh-1]; // *
3038 
3039  /*
3040  * Correction for a finite sample
3041  */
3042  double cn;
3043  switch ( n )
3044  {
3045  case 2: cn = 0.743; break;
3046  case 3: cn = 1.851; break;
3047  case 4: cn = 0.954; break;
3048  case 5: cn = 1.351; break;
3049  case 6: cn = 0.993; break;
3050  case 7: cn = 1.198; break;
3051  case 8: cn = 1.005; break;
3052  case 9: cn = 1.131; break;
3053  default: cn = (n & 1) ? n/(n - 0.9) : 1.0; break;
3054  }
3055 
3056  double sn = cn * double( *pcl::Select( a2, a2+n, nh-1 ) );
3057 
3058  delete [] a2;
3059  return sn;
3060 }
3061 
3072 inline double __pcl_whimed__( double* a, distance_type* iw, distance_type n,
3073  double* acand, distance_type* iwcand )
3074 {
3075  distance_type wtotal = 0;
3076  for ( distance_type i = 0; i < n; ++i )
3077  wtotal += iw[i];
3078 
3079  for ( distance_type nn = n, wrest = 0; ; )
3080  {
3081  double trial = *pcl::Select( a, a+nn, nn >> 1 ); // *
3082 
3083  distance_type wleft = 0;
3084  distance_type wmid = 0;
3085  distance_type wright = 0;
3086  for ( distance_type i = 0; i < nn; ++i )
3087  if ( a[i] < trial )
3088  wleft += iw[i];
3089  else if ( a[i] > trial )
3090  wright += iw[i];
3091  else
3092  wmid += iw[i];
3093 
3094  if ( 2*(wrest + wleft) > wtotal )
3095  {
3096  distance_type kcand = 0;
3097  for ( distance_type i = 0; i < nn; ++i )
3098  if ( a[i] < trial )
3099  {
3100  acand[kcand] = a[i];
3101  iwcand[kcand] = iw[i];
3102  ++kcand;
3103  }
3104  nn = kcand;
3105  }
3106  else
3107  {
3108  if ( 2*(wrest + wleft + wmid) > wtotal )
3109  return trial;
3110 
3111  distance_type kcand = 0;
3112  for ( distance_type i = 0; i < nn; ++i )
3113  if ( a[i] > trial )
3114  {
3115  acand[kcand] = a[i];
3116  iwcand[kcand] = iw[i];
3117  ++kcand;
3118  }
3119  nn = kcand;
3120  wrest += wleft + wmid;
3121  }
3122 
3123  for ( distance_type i = 0; i < nn; ++i )
3124  {
3125  a[i] = acand[i];
3126  iw[i] = iwcand[i];
3127  }
3128  }
3129 }
3130 
3159 template <typename T> double Qn( T* x, T* xn )
3160 {
3161  distance_type n = xn - x;
3162  if ( n < 2 )
3163  return 0;
3164 
3165  T* y = new T[ n ];
3166  double* work = new double[ n ];
3167  double* acand = new double[ n ];
3168  distance_type* iwcand = new distance_type[ n ];
3169  distance_type* left = new distance_type[ n ];
3170  distance_type* right = new distance_type[ n ];
3171  distance_type* P = new distance_type[ n ];
3172  distance_type* Q = new distance_type[ n ];
3173  distance_type* weight = new distance_type[ n ];
3174 
3175  distance_type h = (n >> 1) + 1;
3176  distance_type k = (h*(h - 1)) >> 1;
3177  for ( distance_type i = 0; i < n; ++i )
3178  {
3179  y[i] = x[i];
3180  left[i] = n - i + 1; // *
3181  right[i] = (i <= h) ? n : n - i + h; // the original code is "right[i] = n"
3182  }
3183 
3184  pcl::Sort( y, y+n );
3185 
3186  distance_type nL = (n*(n + 1)) >> 1;
3187  distance_type nR = n*n;
3188  distance_type knew = k + nL;
3189 
3190  bool found = false;
3191  double qn;
3192 
3193  while ( nR-nL > n )
3194  {
3195  distance_type j = 0; // *
3196  for ( distance_type i = 1; i < n; ++i ) // *
3197  if ( left[i] <= right[i] )
3198  {
3199  weight[j] = right[i] - left[i] + 1;
3200  work[j] = double( y[i] ) - y[n - left[i] - (weight[j] >> 1)];
3201  ++j;
3202  }
3203  qn = __pcl_whimed__( work, weight, j, acand, iwcand );
3204 
3205  for ( distance_type i = n-1, j = 0; i >= 0; --i ) // *
3206  {
3207  while ( j < n && double( y[i] ) - y[n-j-1] < qn )
3208  ++j;
3209  P[i] = j;
3210  }
3211 
3212  for ( distance_type i = 0, j = n+1; i < n; ++i ) // *
3213  {
3214  while ( double( y[i] ) - y[n-j+1] > qn )
3215  --j;
3216  Q[i] = j;
3217  }
3218 
3219  double sumP = 0;
3220  double sumQ = 0;
3221  for ( distance_type i = 0; i < n; ++i )
3222  {
3223  sumP += P[i];
3224  sumQ += Q[i] - 1;
3225  }
3226 
3227  if ( knew <= sumP )
3228  {
3229  for ( distance_type i = 0; i < n; ++i )
3230  right[i] = P[i];
3231  nR = sumP;
3232  }
3233  else if ( knew > sumQ )
3234  {
3235  for ( distance_type i = 0; i < n; ++i )
3236  left[i] = Q[i];
3237  nL = sumQ;
3238  }
3239  else
3240  {
3241  found = true;
3242  break;
3243  }
3244  }
3245 
3246  if ( !found )
3247  {
3248  distance_type j = 0;
3249  for ( distance_type i = 1; i < n; ++i )
3250  for ( distance_type jj = left[i]; jj <= right[i]; ++jj, ++j )
3251  work[j] = double( y[i] ) - y[n-jj]; // *
3252  qn = *pcl::Select( work, work+j, knew-nL-1 ); // *
3253  }
3254 
3255  /*
3256  * Correction for a finite sample
3257  */
3258  double dn;
3259  switch ( n )
3260  {
3261  case 2: dn = 0.399; break;
3262  case 3: dn = 0.994; break;
3263  case 4: dn = 0.512; break;
3264  case 5: dn = 0.844; break;
3265  case 6: dn = 0.611; break;
3266  case 7: dn = 0.857; break;
3267  case 8: dn = 0.669; break;
3268  case 9: dn = 0.872; break;
3269  default: dn = (n & 1) ? n/(n + 1.4) : n/(n + 3.8); break;
3270  }
3271  qn *= dn;
3272 
3273  delete [] y;
3274  delete [] work;
3275  delete [] acand;
3276  delete [] iwcand;
3277  delete [] left;
3278  delete [] right;
3279  delete [] P;
3280  delete [] Q;
3281  delete [] weight;
3282 
3283  return qn;
3284 }
3285 
3315 template <typename T>
3316 double BiweightMidvariance( const T* x, const T* xn, double center, double sigma, int k = 9 )
3317 {
3318  distance_type n = xn - x;
3319  if ( n < 2 )
3320  return 0;
3321 
3322  double kd = k * sigma;
3323  if ( kd < 0 || 1 + kd == 1 )
3324  return 0;
3325 
3326  double num = 0, den = 0;
3327  for ( ; x < xn; ++x )
3328  {
3329  double xc = *x - center;
3330  double y = xc/kd;
3331  if ( Abs( y ) < 1 )
3332  {
3333  double y2 = y*y;
3334  double y21 = 1 - y2;
3335  num += xc*xc * y21*y21*y21*y21;
3336  den += y21 * (1 - 5*y2);
3337  }
3338  }
3339 
3340  den *= den;
3341  if ( 1 + den == 1 )
3342  return 0;
3343  return n*num/den;
3344 }
3345 
3374 template <typename T>
3375 double BendMidvariance( const T* x, const T* xn, double center, double beta = 0.2 )
3376 {
3377  distance_type n = xn - x;
3378  if ( n < 2 )
3379  return 0;
3380 
3381  beta = Range( beta, 0.0, 0.5 );
3382  distance_type m = Floor( (1 - beta)*n + 0.5 );
3383 
3384  double* w = new double[ n ];
3385  for ( distance_type i = 0; i < n; ++i )
3386  w[i] = Abs( double( x[i] ) - center );
3387  double wb = *pcl::Select( w, w+n, m );
3388  delete [] w;
3389  if ( 1 + wb == 1 )
3390  return 0;
3391 
3392  double num = 0;
3393  distance_type den = 0;
3394  for ( ; x < xn; ++x )
3395  {
3396  double y = (double( *x ) - center)/wb;
3397  double f = Max( -1.0, Min( 1.0, y ) );
3398  num += f*f;
3399  if ( Abs( y ) < 1 )
3400  ++den;
3401  }
3402 
3403  if ( den == 0 )
3404  return 0;
3405  return n*wb*wb*num/den/den;
3406 }
3407 
3408 // ----------------------------------------------------------------------------
3409 
3445 inline uint64 Hash64( const void* data, size_type size, uint64 seed = 0 )
3446 {
3447 #define PRIME64_1 11400714785074694791ULL
3448 #define PRIME64_2 14029467366897019727ULL
3449 #define PRIME64_3 1609587929392839161ULL
3450 #define PRIME64_4 9650029242287828579ULL
3451 #define PRIME64_5 2870177450012600261ULL
3452 
3453  const uint8* p = (const uint8*)data;
3454  const uint8* end = p + size;
3455  uint64 h64;
3456 
3457  if ( seed == 0 )
3458  seed = size;
3459 
3460  if ( size >= 32 )
3461  {
3462  const uint8* limit = end - 32;
3463  uint64 v1 = seed + PRIME64_1 + PRIME64_2;
3464  uint64 v2 = seed + PRIME64_2;
3465  uint64 v3 = seed + 0;
3466  uint64 v4 = seed - PRIME64_1;
3467 
3468  do
3469  {
3470  v1 += *(uint64*)p * PRIME64_2;
3471  p += 8;
3472  v1 = RotL( v1, 31 );
3473  v1 *= PRIME64_1;
3474  v2 += *(uint64*)p * PRIME64_2;
3475  p += 8;
3476  v2 = RotL( v2, 31 );
3477  v2 *= PRIME64_1;
3478  v3 += *(uint64*)p * PRIME64_2;
3479  p += 8;
3480  v3 = RotL( v3, 31 );
3481  v3 *= PRIME64_1;
3482  v4 += *(uint64*)p * PRIME64_2;
3483  p += 8;
3484  v4 = RotL( v4, 31 );
3485  v4 *= PRIME64_1;
3486  }
3487  while ( p <= limit );
3488 
3489  h64 = RotL( v1, 1 ) + RotL( v2, 7 ) + RotL( v3, 12 ) + RotL( v4, 18 );
3490 
3491  v1 *= PRIME64_2;
3492  v1 = RotL( v1, 31 );
3493  v1 *= PRIME64_1;
3494  h64 ^= v1;
3495  h64 = h64 * PRIME64_1 + PRIME64_4;
3496 
3497  v2 *= PRIME64_2;
3498  v2 = RotL( v2, 31 );
3499  v2 *= PRIME64_1;
3500  h64 ^= v2;
3501  h64 = h64 * PRIME64_1 + PRIME64_4;
3502 
3503  v3 *= PRIME64_2;
3504  v3 = RotL( v3, 31 );
3505  v3 *= PRIME64_1;
3506  h64 ^= v3;
3507  h64 = h64 * PRIME64_1 + PRIME64_4;
3508 
3509  v4 *= PRIME64_2;
3510  v4 = RotL( v4, 31 );
3511  v4 *= PRIME64_1;
3512  h64 ^= v4;
3513  h64 = h64 * PRIME64_1 + PRIME64_4;
3514  }
3515  else
3516  {
3517  h64 = seed + PRIME64_5;
3518  }
3519 
3520  h64 += size;
3521 
3522  while ( p+8 <= end )
3523  {
3524  uint64 k1 = *(uint64*)p;
3525  k1 *= PRIME64_2;
3526  k1 = RotL( k1, 31 );
3527  k1 *= PRIME64_1;
3528  h64 ^= k1;
3529  h64 = RotL( h64, 27 ) * PRIME64_1 + PRIME64_4;
3530  p += 8;
3531  }
3532 
3533  if ( p+4 <= end )
3534  {
3535  h64 ^= (uint64)(*(uint32*)p) * PRIME64_1;
3536  h64 = RotL( h64, 23 ) * PRIME64_2 + PRIME64_3;
3537  p += 4;
3538  }
3539 
3540  while ( p < end )
3541  {
3542  h64 ^= *p * PRIME64_5;
3543  h64 = RotL( h64, 11 ) * PRIME64_1;
3544  ++p;
3545  }
3546 
3547  h64 ^= h64 >> 33;
3548  h64 *= PRIME64_2;
3549  h64 ^= h64 >> 29;
3550  h64 *= PRIME64_3;
3551  h64 ^= h64 >> 32;
3552 
3553  return h64;
3554 
3555 #undef PRIME64_1
3556 #undef PRIME64_2
3557 #undef PRIME64_3
3558 #undef PRIME64_4
3559 #undef PRIME64_5
3560 }
3561 
3593 inline uint32 Hash32( const void* data, size_type size, uint32 seed = 0 )
3594 {
3595 #define PRIME32_1 2654435761U
3596 #define PRIME32_2 2246822519U
3597 #define PRIME32_3 3266489917U
3598 #define PRIME32_4 668265263U
3599 #define PRIME32_5 374761393U
3600 
3601  const uint8* p = (const uint8*)data;
3602  const uint8* end = p + size;
3603  uint32 h32;
3604 
3605  if ( seed == 0 )
3606  seed = uint32( size );
3607 
3608  if ( size >= 16 )
3609  {
3610  const uint8* limit = end - 16;
3611  uint32 v1 = seed + PRIME32_1 + PRIME32_2;
3612  uint32 v2 = seed + PRIME32_2;
3613  uint32 v3 = seed + 0;
3614  uint32 v4 = seed - PRIME32_1;
3615 
3616  do
3617  {
3618  v1 += *(uint32*)p * PRIME32_2;
3619  v1 = RotL( v1, 13 );
3620  v1 *= PRIME32_1;
3621  p += 4;
3622  v2 += *(uint32*)p * PRIME32_2;
3623  v2 = RotL( v2, 13 );
3624  v2 *= PRIME32_1;
3625  p += 4;
3626  v3 += *(uint32*)p * PRIME32_2;
3627  v3 = RotL( v3, 13 );
3628  v3 *= PRIME32_1;
3629  p += 4;
3630  v4 += *(uint32*)p * PRIME32_2;
3631  v4 = RotL( v4, 13 );
3632  v4 *= PRIME32_1;
3633  p += 4;
3634  }
3635  while ( p <= limit );
3636 
3637  h32 = RotL( v1, 1 ) + RotL( v2, 7 ) + RotL( v3, 12 ) + RotL( v4, 18 );
3638  }
3639  else
3640  {
3641  h32 = seed + PRIME32_5;
3642  }
3643 
3644  h32 += (uint32)size;
3645 
3646  while ( p+4 <= end )
3647  {
3648  h32 += *(uint32*)p * PRIME32_3;
3649  h32 = RotL( h32, 17 ) * PRIME32_4 ;
3650  p+=4;
3651  }
3652 
3653  while ( p < end )
3654  {
3655  h32 += *p * PRIME32_5;
3656  h32 = RotL( h32, 11 ) * PRIME32_1 ;
3657  ++p;
3658  }
3659 
3660  h32 ^= h32 >> 15;
3661  h32 *= PRIME32_2;
3662  h32 ^= h32 >> 13;
3663  h32 *= PRIME32_3;
3664  h32 ^= h32 >> 16;
3665 
3666  return h32;
3667 
3668 #undef PRIME32_1
3669 #undef PRIME32_2
3670 #undef PRIME32_3
3671 #undef PRIME32_4
3672 #undef PRIME32_5
3673 }
3674 
3675 // ----------------------------------------------------------------------------
3676 
3677 } // pcl
3678 
3679 #endif // __PCL_Math_h
3680 
3681 // ----------------------------------------------------------------------------
3682 // EOF pcl/Math.h - Released 2019-04-30T16:30:41Z
Complex< T > Log(const Complex< T > &c)
Definition: Complex.h:726
double L1Norm(const T *i, const T *j)
Definition: Math.h:1937
double Mean(const T *i, const T *j)
Definition: Math.h:2324
int MaxSSEInstructionSetSupported()
Definition: Math.h:113
uint64 Hash64(const void *data, size_type size, uint64 seed=0)
Definition: Math.h:3445
constexpr T Frac(T x)
Definition: Math.h:633
constexpr long double Log2T()
Definition: Math.h:756
constexpr T ArcTan(T x)
Definition: Math.h:481
constexpr T RadSec(T x)
Definition: Math.h:1727
int64 RoundI64(double x)
Definition: Math.h:1523
double StableAvgDev(const T *i, const T *j, double center)
Definition: Math.h:2743
unsigned char uint8
Definition: Defs.h:578
constexpr T AsRad(T x)
Definition: Math.h:1758
int64 TruncInt64(T x)
Definition: Math.h:1084
int RoundInt(T x)
Definition: Math.h:1374
double Median(T *i, T *j)
Definition: Math.h:2487
PCL root namespace.
Definition: AbstractImage.h:76
double Norm(const T *i, const T *j, double p)
Definition: Math.h:1922
double Qn(T *x, T *xn)
Definition: Math.h:3159
constexpr T MasRad(T x)
Definition: Math.h:1769
int64 RoundInt64Arithmetic(double x)
Definition: Math.h:1550
double NondestructiveMedian(const T *i, const T *j)
Definition: Math.h:2653
constexpr T ArcSin(T x)
Definition: Math.h:469
constexpr T Pow2(T x)
Definition: Math.h:1584
constexpr T SecRad(T x)
Definition: Math.h:1749
Factorial function.
Definition: Math.h:591
Complex< T > Sqrt(const Complex< T > &c)
Definition: Complex.h:665
bool IsFinite(float x)
Definition: Math.h:164
double BiweightMidvariance(const T *x, const T *xn, double center, double sigma, int k=9)
Definition: Math.h:3316
T Pow10(T x)
Definition: Math.h:1187
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:838
Exponential function 10**n, n being a signed integer.
Definition: Math.h:1144
double Sum(const T *i, const T *j)
Definition: Math.h:2192
constexpr T Rad(T x)
Definition: Math.h:1705
constexpr T Ceil(T x)
Definition: Math.h:517
void Split(T x, T &i, T &f)
Definition: Math.h:936
constexpr T LogN(T n, T x)
Definition: Math.h:769
constexpr T ArcCosh(T x)
Definition: Math.h:1664
double StableMean(const T *i, const T *j)
Definition: Math.h:2343
T RotR(T x, uint32 n)
Definition: Math.h:1226
constexpr const T & Range(const T &x, const T &a, const T &b)
Definition: Utility.h:190
int TruncI(T x)
Definition: Math.h:1034
size_t size_type
Definition: Defs.h:545
constexpr T Mod2Pi(T x)
Definition: Math.h:1792
double Variance(const T *i, const T *j, double center)
Definition: Math.h:2368
constexpr T RadMin(T x)
Definition: Math.h:1716
int64 RoundInt64(double x)
Definition: Math.h:1489
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:457
double StableSum(const T *i, const T *j)
Definition: Math.h:2211
unsigned long long uint64
Definition: Defs.h:618
double MAD(const T *i, const T *j, double center)
Definition: Math.h:2838
bool IsNaN(float x)
Definition: Math.h:180
constexpr const T & Max(const T &a, const T &b)
Definition: Utility.h:119
int RoundI(T x)
Definition: Math.h:1415
constexpr T Angle(int d, T m)
Definition: Math.h:436
int RoundIntArithmetic(T x)
Definition: Math.h:1455
double StableSumOfSquares(const T *i, const T *j)
Definition: Math.h:2299
int TruncInt(T x)
Definition: Math.h:1012
int RoundIntBanker(T x)
Definition: Math.h:1428
constexpr T Floor(T x)
Definition: Math.h:621
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:1823
constexpr long double Ln2()
Definition: Math.h:695
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:1803
constexpr T MinRad(T x)
Definition: Math.h:1738
constexpr long double Pi()
Definition: Math.h:416
int64 TruncI64(T x)
Definition: Math.h:1106
signed int int32
Definition: Defs.h:596
constexpr T Cotan(T x)
Definition: Math.h:550
void DecimalToSexagesimal(int &sign, S1 &s1, S2 &s2, S3 &s3, const D &d)
Definition: Math.h:2146
constexpr T Mod(T x, T y)
Definition: Math.h:780
uint32 Hash32(const void *data, size_type size, uint32 seed=0)
Definition: Math.h:3593
T RotL(T x, uint32 n)
Definition: Math.h:1205
double L2Norm(const T *i, const T *j)
Definition: Math.h:1951
Complex< T > Cosh(const Complex< T > &c)
Definition: Complex.h:829
T PowI(T x, int n)
Definition: Math.h:1622
int IsInfinity(float x)
Definition: Math.h:199
constexpr T Deg(T x)
Definition: Math.h:561
double BendMidvariance(const T *x, const T *xn, double center, double beta=0.2)
Definition: Math.h:3375
T Poly(T x, C c, int n)
Definition: Math.h:799
constexpr T Ldexp(T m, int p)
Definition: Math.h:673
double SexagesimalToDecimal(int sign, const S1 &s1, const S2 &s2=S2(0), const S3 &s3=S3(0))
Definition: Math.h:2166
constexpr T Hav(T x)
Definition: Math.h:662
#define int64_max
Definition: Defs.h:824
double Sn(T *x, T *xn)
Definition: Math.h:2912
T ArcTan2Pi(T y, T x)
Definition: Math.h:503
void SinCos(T x, T &sx, T &cx)
Definition: Math.h:915
Complex< T > Ln(const Complex< T > &c)
Definition: Complex.h:716
constexpr T ArcSinh(T x)
Definition: Math.h:1650
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:2236
#define ItemsInArray(a)
Definition: Utility.h:223
constexpr T ArcHav(T x)
Definition: Math.h:1694
Complex< T > Exp(const Complex< T > &c)
Definition: Complex.h:705
ptrdiff_t distance_type
Definition: Defs.h:551
Exponential function 2**n, n being a signed integer.
Definition: Math.h:1603
constexpr long double Log2()
Definition: Math.h:717
double SumOfSquares(const T *i, const T *j)
Definition: Math.h:2280
constexpr long double TwoPi()
Definition: Math.h:425
Complex< T > Tan(const Complex< T > &c)
Definition: Complex.h:808
constexpr char SignChar(T x)
Definition: Math.h:854
double StableModulus(const T *i, const T *j)
Definition: Math.h:2255
unsigned int uint32
Definition: Defs.h:602
signed long long int64
Definition: Defs.h:612
T Trunc(T x)
Definition: Math.h:980
constexpr T ArcTanh(T x)
Definition: Math.h:1678
constexpr T UasRad(T x)
Definition: Math.h:1780
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:2426
#define int64_min
Definition: Defs.h:818
constexpr long double Log2e()
Definition: Math.h:743
double AvgDev(const T *i, const T *j, double center)
Definition: Math.h:2712
void Frexp(T x, T &m, int &p)
Definition: Math.h:646