PCL
Math.h
Go to the documentation of this file.
1 // ____ ______ __
2 // / __ \ / ____// /
3 // / /_/ // / / /
4 // / ____// /___ / /___ PixInsight Class Library
5 // /_/ \____//_____/ PCL 2.6.5
6 // ----------------------------------------------------------------------------
7 // pcl/Math.h - Released 2024-01-13T15:47:58Z
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-2024 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 (https://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 /*
95  * Number of histogram bins used by fast histogram-based median calculation
96  * algorithm implementations.
97  */
98 #define __PCL_MEDIAN_HISTOGRAM_LENGTH 8192
99 
100 namespace pcl
101 {
102 
103 // ----------------------------------------------------------------------------
104 
127 inline int MaxSSEInstructionSetSupported() noexcept
128 {
129  int32 edxFlags = 0;
130  int32 ecxFlags = 0;
131 
132 #ifdef _MSC_VER
133  int cpuInfo[ 4 ];
134  __cpuid( cpuInfo, 1 );
135  edxFlags = cpuInfo[3];
136  ecxFlags = cpuInfo[2];
137 #else
138  asm volatile( "mov $0x00000001, %%eax\n\t"
139  "cpuid\n\t"
140  "mov %%edx, %0\n\t"
141  "mov %%ecx, %1\n"
142  : "=r" (edxFlags), "=r" (ecxFlags) // output operands
143  : // input operands
144  : "%eax", "%ebx", "%ecx", "%edx" ); // clobbered registers
145 #endif
146 
147  if ( ecxFlags & (1u << 20) ) // SSE4.2
148  return 42;
149  if ( ecxFlags & (1u << 19) ) // SSE4.1
150  return 41;
151  if ( ecxFlags & 1u ) // SSE3
152  return 3;
153  if ( edxFlags & (1u << 26) ) // SSE2
154  return 2;
155  if ( edxFlags & (1u << 25) ) // SSE
156  return 1;
157  return 0;
158 }
159 
160 // ----------------------------------------------------------------------------
161 
166 #define __PCL_FLOAT_SGNMASK 0x80000000
167 #define __PCL_FLOAT_EXPMASK 0x7f800000
168 #define __PCL_FLOAT_SIGMASK 0x007fffff
169 
178 inline bool IsFinite( float x ) noexcept
179 {
180  union { float f; uint32 u; } v = { x };
181  return (v.u & __PCL_FLOAT_EXPMASK) != __PCL_FLOAT_EXPMASK;
182 }
183 
194 inline bool IsNaN( float x ) noexcept
195 {
196  union { float f; uint32 u; } v = { x };
197  return (v.u & __PCL_FLOAT_EXPMASK) == __PCL_FLOAT_EXPMASK &&
198  (v.u & __PCL_FLOAT_SIGMASK) != 0;
199 }
200 
216 inline int IsInfinity( float x ) noexcept
217 {
218  union { float f; uint32 u; } v = { x };
219  if ( (v.u & __PCL_FLOAT_EXPMASK) == __PCL_FLOAT_EXPMASK &&
220  (v.u & __PCL_FLOAT_SIGMASK) == 0 )
221  return ((v.u & __PCL_FLOAT_SGNMASK) == 0) ? +1 : -1;
222  return 0;
223 }
224 
231 inline bool IsNegativeZero( float x ) noexcept
232 {
233  union { float f; uint32 u; } v = { x };
234  return v.u == __PCL_FLOAT_SGNMASK;
235 }
236 
237 #define __PCL_DOUBLE_SGNMASK 0x80000000
238 #define __PCL_DOUBLE_EXPMASK 0x7ff00000
239 #define __PCL_DOUBLE_SIGMASK 0x000fffff
240 
249 inline bool IsFinite( double x ) noexcept
250 {
251  union { double d; uint32 u[2]; } v = { x };
252  return (v.u[1] & __PCL_DOUBLE_EXPMASK) != __PCL_DOUBLE_EXPMASK;
253 }
254 
265 inline bool IsNaN( double x ) noexcept
266 {
267  union { double d; uint32 u[2]; } v = { x };
268  return (v.u[1] & __PCL_DOUBLE_EXPMASK) == __PCL_DOUBLE_EXPMASK &&
269  (v.u[0] != 0 || (v.u[1] & __PCL_DOUBLE_SIGMASK) != 0);
270 }
271 
287 inline int IsInfinity( double x ) noexcept
288 {
289  union { double d; uint32 u[2]; } v = { x };
290  if ( v.u[0] == 0 &&
291  (v.u[1] & __PCL_DOUBLE_SIGMASK) == 0 &&
292  (v.u[1] & __PCL_DOUBLE_EXPMASK) == __PCL_DOUBLE_EXPMASK )
293  return ((v.u[1] & __PCL_DOUBLE_SGNMASK) == 0) ? +1 : -1;
294  return 0;
295 }
296 
303 inline bool IsNegativeZero( double x ) noexcept
304 {
305  union { double d; uint32 u[2]; } v = { x };
306  return v.u[1] == __PCL_DOUBLE_SGNMASK &&
307  v.u[0] == 0;
308 }
309 
310 // ----------------------------------------------------------------------------
311 
320 inline float Abs( float x ) noexcept
321 {
322  return std::fabs( x );
323 }
324 
328 inline double Abs( double x ) noexcept
329 {
330  return std::fabs( x );
331 }
332 
336 inline long double Abs( long double x ) noexcept
337 {
338  return std::fabs( x );
339 }
340 
344 inline signed int Abs( signed int x ) noexcept
345 {
346  return ::abs( x );
347 }
348 
352 #if defined( __PCL_MACOSX ) && defined( __clang__ ) // turn off warning due to broken cstdlib in Xcode
353 _Pragma("clang diagnostic push")
354 _Pragma("clang diagnostic ignored \"-Wabsolute-value\"")
355 #endif
356 inline signed long Abs( signed long x ) noexcept
357 {
358  return ::abs( x );
359 }
360 #if defined( __PCL_MACOSX ) && defined( __clang__ )
361 _Pragma("clang diagnostic pop")
362 #endif
363 
367 #if defined( _MSC_VER )
368 inline __int64 Abs( __int64 x ) noexcept
369 {
370  return (x < 0) ? -x : +x;
371 }
372 #elif defined( __PCL_MACOSX ) && defined( __clang__ )
373 inline constexpr signed long long Abs( signed long long x ) noexcept
374 {
375  return (x < 0) ? -x : +x;
376 }
377 #else
378 inline signed long long Abs( signed long long x ) noexcept
379 {
380  return ::abs( x );
381 }
382 #endif
383 
387 inline signed short Abs( signed short x ) noexcept
388 {
389  return (signed short)::abs( int( x ) );
390 }
391 
395 inline signed char Abs( signed char x ) noexcept
396 {
397  return (signed char)::abs( int( x ) );
398 }
399 
403 inline wchar_t Abs( wchar_t x ) noexcept
404 {
405  return (wchar_t)::abs( int( x ) );
406 }
407 
411 inline constexpr unsigned int Abs( unsigned int x ) noexcept
412 {
413  return x;
414 }
415 
419 inline constexpr unsigned long Abs( unsigned long x ) noexcept
420 {
421  return x;
422 }
423 
427 #ifdef _MSC_VER
428 inline unsigned __int64 Abs( unsigned __int64 x ) noexcept
429 {
430  return x;
431 }
432 #else
433 inline constexpr unsigned long long Abs( unsigned long long x ) noexcept
434 {
435  return x;
436 }
437 #endif
438 
442 inline constexpr unsigned short Abs( unsigned short x ) noexcept
443 {
444  return x;
445 }
446 
450 inline constexpr unsigned char Abs( unsigned char x ) noexcept
451 {
452  return x;
453 }
454 
455 // ----------------------------------------------------------------------------
456 
461 inline constexpr long double Pi() noexcept
462 {
463  return (long double)( 0.31415926535897932384626433832795029e+01L );
464 }
465 
470 inline constexpr long double TwoPi() noexcept
471 {
472  return (long double)( 0.62831853071795864769252867665590058e+01L );
473 }
474 
475 // ----------------------------------------------------------------------------
476 
481 template <typename T> inline constexpr T Angle( int d, T m ) noexcept
482 {
483  return d + m/60;
484 }
485 
491 template <typename T> inline constexpr T Angle( int d, int m, T s ) noexcept
492 {
493  return Angle( d, m + s/60 );
494 }
495 
496 // ----------------------------------------------------------------------------
497 
502 template <typename T> inline constexpr T ArcCos( T x ) noexcept
503 {
504  PCL_PRECONDITION( T( -1 ) <= x && x <= T( 1 ) )
505  return std::acos( x );
506 }
507 
508 // ----------------------------------------------------------------------------
509 
514 template <typename T> inline constexpr T ArcSin( T x ) noexcept
515 {
516  PCL_PRECONDITION( T( -1 ) <= x && x <= T( 1 ) )
517  return std::asin( x );
518 }
519 
520 // ----------------------------------------------------------------------------
521 
526 template <typename T> inline constexpr T ArcTan( T x ) noexcept
527 {
528  return std::atan( x );
529 }
530 
531 // ----------------------------------------------------------------------------
532 
537 template <typename T> inline constexpr T ArcTan( T y, T x ) noexcept
538 {
539  return std::atan2( y, x );
540 }
541 
542 // ----------------------------------------------------------------------------
543 
548 template <typename T> inline T ArcTan2Pi( T y, T x ) noexcept
549 {
550  T r = ArcTan( y, x );
551  if ( r < 0 )
552  r = static_cast<T>( r + TwoPi() );
553  return r;
554 }
555 
556 // ----------------------------------------------------------------------------
557 
562 template <typename T> inline constexpr T Ceil( T x ) noexcept
563 {
564  return std::ceil( x );
565 }
566 
567 // ----------------------------------------------------------------------------
568 
573 template <typename T> inline constexpr T Cos( T x ) noexcept
574 {
575  return std::cos( x );
576 }
577 
578 // ----------------------------------------------------------------------------
579 
584 template <typename T> inline constexpr T Cosh( T x ) noexcept
585 {
586  return std::cosh( x );
587 }
588 
589 // ----------------------------------------------------------------------------
590 
595 template <typename T> inline constexpr T Cotan( T x ) noexcept
596 {
597  return T( 1 )/std::tan( x );
598 }
599 
600 // ----------------------------------------------------------------------------
601 
606 template <typename T> inline constexpr T Deg( T x ) noexcept
607 {
608  return static_cast<T>( 0.572957795130823208767981548141051700441964e+02L * x );
609 }
610 
611 // ----------------------------------------------------------------------------
612 
617 template <typename T> inline constexpr T Exp( T x ) noexcept
618 {
619  return std::exp( x );
620 }
621 
622 // ----------------------------------------------------------------------------
623 
628 template <typename T> inline constexpr T Floor( T x ) noexcept
629 {
630  return std::floor( x );
631 }
632 
633 // ----------------------------------------------------------------------------
634 
640 template <typename T> inline constexpr T Frac( T x ) noexcept
641 {
642  return std::modf( x, &x );
643 }
644 
645 // ----------------------------------------------------------------------------
646 
653 template <typename T> inline void Frexp( T x, T& m, int& p ) noexcept
654 {
655  m = std::frexp( x, &p );
656 }
657 
658 // ----------------------------------------------------------------------------
659 
669 template <typename T> inline constexpr T Hav( T x ) noexcept
670 {
671  return (1 - Cos( x ))/2;
672 }
673 
674 // ----------------------------------------------------------------------------
675 
680 template <typename T> inline constexpr T Ldexp( T m, int p ) noexcept
681 {
682  return std::ldexp( m, p );
683 }
684 
685 // ----------------------------------------------------------------------------
686 
691 template <typename T> inline constexpr T Ln( T x ) noexcept
692 {
693  PCL_PRECONDITION( x >= 0 )
694  return std::log( x );
695 }
696 
697 // ----------------------------------------------------------------------------
698 
699 struct PCL_CLASS FactorialCache
700 {
701  constexpr static int s_cacheSize = 127;
702  static const double s_lut[ s_cacheSize+1 ];
703 };
704 
712 inline double Factorial( int n ) noexcept
713 {
714  PCL_PRECONDITION( n >= 0 )
715  if ( n <= FactorialCache::s_cacheSize )
716  return FactorialCache::s_lut[n];
717  double x = FactorialCache::s_lut[FactorialCache::s_cacheSize];
718  for ( int m = FactorialCache::s_cacheSize; ++m <= n; )
719  x *= m;
720  return x;
721 }
722 
732 inline double LnFactorial( int n ) noexcept
733 {
734  PCL_PRECONDITION( n >= 0 )
735  if ( n <= FactorialCache::s_cacheSize )
736  return Ln( FactorialCache::s_lut[n] );
737  double x = n + 1;
738 // return (x - 0.5)*Ln( x ) - x + 0.5*Ln( TwoPi() ) + 1/12/x - 1/(360*x*x*x);
739  return (x - 0.5)*Ln( x ) - x + 0.91893853320467267 + 1/12/x - 1/(360*x*x*x);
740 }
741 
760 template <typename T> struct PCL_CLASS Fact : public FactorialCache
761 {
765  T operator()( int n ) const noexcept
766  {
767  PCL_PRECONDITION( n >= 0 )
768  if ( n <= s_cacheSize )
769  return T( s_lut[n] );
770  double x = s_lut[s_cacheSize];
771  for ( int m = s_cacheSize; ++m <= n; )
772  x *= m;
773  return T( x );
774  }
775 
782  T Ln( int n ) const noexcept
783  {
784  PCL_PRECONDITION( n >= 0 )
785  if ( n <= s_cacheSize )
786  return T( pcl::Ln( s_lut[n] ) );
787  double x = n + 1;
788 // return T( (x - 0.5)*pcl::Ln( x ) - x + 0.5*pcl::Ln( TwoPi() ) + 1/12/x - 1/(360*x*x*x) );
789  return T( (x - 0.5)*pcl::Ln( x ) - x + 0.91893853320467267 + 1/12/x - 1/(360*x*x*x) );
790  }
791 };
792 
793 // ----------------------------------------------------------------------------
794 
799 inline constexpr long double Ln2() noexcept
800 {
801  return (long double)( 0.6931471805599453094172321214581766e+00L );
802 }
803 
804 // ----------------------------------------------------------------------------
805 
810 template <typename T> inline constexpr T Log( T x ) noexcept
811 {
812  PCL_PRECONDITION( x >= 0 )
813  return std::log10( x );
814 }
815 
816 // ----------------------------------------------------------------------------
817 
822 inline constexpr long double Log2() noexcept
823 {
824  // Use the relation:
825  // log10(2) = ln(2)/ln(10)
826  return (long double)( 0.3010299956639811952137388947244930416265e+00L );
827 }
828 
829 // ----------------------------------------------------------------------------
830 
835 template <typename T> inline constexpr T Log2( T x ) noexcept
836 {
837  // Use the relation:
838  // log2(x) = ln(x)/ln(2)
839  PCL_PRECONDITION( x >= 0 )
840  return std::log( x )/Ln2();
841 }
842 
843 // ----------------------------------------------------------------------------
844 
849 inline constexpr long double Log2e() noexcept
850 {
851  // Use the relation:
852  // log2(e) = 1/ln(2)
853  return (long double)( 0.14426950408889634073599246810018920709799e+01L );
854 }
855 
856 // ----------------------------------------------------------------------------
857 
862 inline constexpr long double Log2T() noexcept
863 {
864  // Use the relation:
865  // log2(10) = 1/log(2)
866  return (long double)( 0.33219280948873623478703194294893900118996e+01L );
867 }
868 
869 // ----------------------------------------------------------------------------
870 
875 template <typename T> inline constexpr T LogN( T n, T x ) noexcept
876 {
877  PCL_PRECONDITION( x >= 0 )
878  return std::log( x )/std::log( n );
879 }
880 
881 // ----------------------------------------------------------------------------
882 
887 template <typename T> inline constexpr T Mod( T x, T y ) noexcept
888 {
889  return std::fmod( x, y );
890 }
891 
892 // ----------------------------------------------------------------------------
893 
908 template <typename T, typename C> inline T Poly( T x, C c, int n ) noexcept
909 {
910  PCL_PRECONDITION( n >= 0 )
911  T y;
912  for ( c += n, y = *c; n > 0; --n )
913  {
914  y *= x;
915  y += *--c;
916  }
917  return y;
918 }
919 
932 template <typename T> inline T Poly( T x, std::initializer_list<T> c ) noexcept
933 {
934  PCL_PRECONDITION( c.size() > 0 )
935  return Poly( x, c.begin(), int( c.size() )-1 );
936 }
937 
938 // ----------------------------------------------------------------------------
939 
951 template <typename T> inline constexpr int Sign( T x ) noexcept
952 {
953  return (x < 0) ? -1 : ((x > 0) ? +1 : 0);
954 }
955 
956 // ----------------------------------------------------------------------------
957 
969 template <typename T> inline constexpr char SignChar( T x ) noexcept
970 {
971  return (x < 0) ? '-' : ((x > 0) ? '+' : ' ');
972 }
973 
974 // ----------------------------------------------------------------------------
975 
980 template <typename T> inline constexpr T Sin( T x ) noexcept
981 {
982  return std::sin( x );
983 }
984 
985 // ----------------------------------------------------------------------------
986 
991 template <typename T> inline constexpr T Sinh( T x ) noexcept
992 {
993  return std::sinh( x );
994 }
995 
996 // ----------------------------------------------------------------------------
997 
998 #ifdef __PCL_HAVE_SINCOS
999 
1000 inline void __pcl_sincos__( float x, float& sx, float& cx ) noexcept
1001 {
1002  ::sincosf( x, &sx, &cx );
1003 }
1004 
1005 inline void __pcl_sincos__( double x, double& sx, double& cx ) noexcept
1006 {
1007  ::sincos( x, &sx, &cx );
1008 }
1009 
1010 inline void __pcl_sincos__( long double x, long double& sx, long double& cx ) noexcept
1011 {
1012  ::sincosl( x, &sx, &cx );
1013 }
1014 
1015 #endif
1016 
1030 template <typename T> inline void SinCos( T x, T& sx, T& cx ) noexcept
1031 {
1032 #ifdef __PCL_HAVE_SINCOS
1033  __pcl_sincos__( x, sx, cx );
1034 #else
1035  sx = std::sin( x );
1036  cx = std::cos( x );
1037 #endif
1038 }
1039 
1040 // ----------------------------------------------------------------------------
1041 
1051 template <typename T> inline void Split( T x, T& i, T& f ) noexcept
1052 {
1053  f = std::modf( x, &i );
1054 }
1055 
1056 // ----------------------------------------------------------------------------
1057 
1062 template <typename T> inline constexpr T Sqrt( T x ) noexcept
1063 {
1064  return sqrt( x );
1065 }
1066 
1067 // ----------------------------------------------------------------------------
1068 
1073 template <typename T> inline constexpr T Tan( T x ) noexcept
1074 {
1075  return std::tan( x );
1076 }
1077 
1078 // ----------------------------------------------------------------------------
1079 
1084 template <typename T> inline constexpr T Tanh( T x ) noexcept
1085 {
1086  return std::tanh( x );
1087 }
1088 
1089 // ----------------------------------------------------------------------------
1090 
1095 template <typename T> inline T Trunc( T x ) noexcept
1096 {
1097  (void)std::modf( x, &x );
1098  return x;
1099 }
1100 
1101 // ----------------------------------------------------------------------------
1102 
1103 #ifdef __PCL_HAVE_SSE2
1104 
1105 inline int __pcl_trunci__( float x ) noexcept
1106 {
1107  return _mm_cvtt_ss2si( _mm_load_ss( &x ) );
1108 }
1109 
1110 inline int __pcl_trunci__( double x ) noexcept
1111 {
1112  return _mm_cvttsd_si32( _mm_load_sd( &x ) );
1113 }
1114 
1115 inline int __pcl_trunci__( long double x ) noexcept
1116 {
1117  return int( x );
1118 }
1119 
1120 #endif
1121 
1132 template <typename T> inline int TruncInt( T x ) noexcept
1133 {
1134  PCL_PRECONDITION( x >= int_min && x <= int_max )
1135 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1136  return int( x );
1137 #else
1138 # ifdef __PCL_HAVE_SSE2
1139  return __pcl_trunci__( x );
1140 # else
1141  return int( x );
1142 # endif
1143 #endif
1144 }
1145 
1154 template <typename T> inline int TruncI( T x ) noexcept
1155 {
1156  return TruncInt( x );
1157 }
1158 
1159 #define TruncInt32( x ) TruncInt( x )
1160 #define TruncI32( x ) TruncInt( x )
1161 
1162 // ----------------------------------------------------------------------------
1163 
1164 #ifdef __PCL_HAVE_SSE2
1165 
1166 #if defined( __x86_64__ ) || defined( _M_X64 )
1167 
1168 inline int64 __pcl_trunci64__( float x ) noexcept
1169 {
1170  return _mm_cvttss_si64( _mm_load_ss( &x ) );
1171 }
1172 
1173 inline int64 __pcl_trunci64__( double x ) noexcept
1174 {
1175  return _mm_cvttsd_si64( _mm_load_sd( &x ) );
1176 }
1177 
1178 #else
1179 
1180 inline int64 __pcl_trunci64__( float x ) noexcept
1181 {
1182  return int64( _mm_cvtt_ss2si( _mm_load_ss( &x ) ) );
1183 }
1184 
1185 inline int64 __pcl_trunci64__( double x ) noexcept
1186 {
1187  return int64( x );
1188 }
1189 
1190 #endif
1191 
1192 inline int64 __pcl_trunci64__( long double x ) noexcept
1193 {
1194  return int64( x );
1195 }
1196 
1197 #endif // __PCL_HAVE_SSE2
1198 
1209 template <typename T> inline int64 TruncInt64( T x ) noexcept
1210 {
1211  PCL_PRECONDITION( x >= int64_min && x <= int64_max )
1212 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1213  return int64( Trunc( x ) );
1214 #else
1215 # ifdef __PCL_HAVE_SSE2
1216  return __pcl_trunci64__( x );
1217 # else
1218  return int64( Trunc( x ) );
1219 # endif
1220 #endif
1221 }
1222 
1231 template <typename T> inline int64 TruncI64( T x ) noexcept
1232 {
1233  return TruncInt64( x );
1234 }
1235 
1236 // ----------------------------------------------------------------------------
1237 
1251 template <typename T> inline constexpr T Pow( T x, T y ) noexcept
1252 {
1253  PCL_PRECONDITION( y < T( int_max ) )
1254  return std::pow( x, y );
1255 }
1256 
1257 // ----------------------------------------------------------------------------
1258 
1269 template <typename T> struct PCL_CLASS Pow10I
1270 {
1271  T operator ()( int n ) const noexcept
1272  {
1273  // Use fast table lookups and squaring up to |n| <= 50.
1274  static const T lut[] =
1275  {
1276 #define ___( x ) static_cast<T>( x )
1277  ___( 1.0e+00 ), ___( 1.0e+01 ), ___( 1.0e+02 ), ___( 1.0e+03 ), ___( 1.0e+04 ),
1278  ___( 1.0e+05 ), ___( 1.0e+06 ), ___( 1.0e+07 ), ___( 1.0e+08 ), ___( 1.0e+09 ),
1279  ___( 1.0e+10 ), ___( 1.0e+11 ), ___( 1.0e+12 ), ___( 1.0e+13 ), ___( 1.0e+14 ),
1280  ___( 1.0e+15 ), ___( 1.0e+16 ), ___( 1.0e+17 ), ___( 1.0e+18 ), ___( 1.0e+19 ),
1281  ___( 1.0e+20 ), ___( 1.0e+21 ), ___( 1.0e+22 ), ___( 1.0e+23 ), ___( 1.0e+24 ),
1282  ___( 1.0e+25 ), ___( 1.0e+26 ), ___( 1.0e+27 ), ___( 1.0e+28 ), ___( 1.0e+29 ),
1283  ___( 1.0e+30 ), ___( 1.0e+31 ), ___( 1.0e+32 ), ___( 1.0e+33 ), ___( 1.0e+34 ),
1284  ___( 1.0e+35 ), ___( 1.0e+36 ), ___( 1.0e+37 ), ___( 1.0e+38 ), ___( 1.0e+39 ),
1285  ___( 1.0e+40 ), ___( 1.0e+41 ), ___( 1.0e+42 ), ___( 1.0e+43 ), ___( 1.0e+44 ),
1286  ___( 1.0e+45 ), ___( 1.0e+46 ), ___( 1.0e+47 ), ___( 1.0e+48 ), ___( 1.0e+49 )
1287 #undef ___
1288  };
1289  static const int N = ItemsInArray( lut );
1290  int i = ::abs( n );
1291  double x;
1292  if ( i < N )
1293  x = lut[i];
1294  else
1295  {
1296  x = lut[N-1];
1297  while ( (i -= N-1) >= N )
1298  x *= x;
1299  if ( i != 0 )
1300  x *= lut[i];
1301  }
1302  return T( (n >= 0) ? x : 1/x );
1303  }
1304 };
1305 
1306 // ----------------------------------------------------------------------------
1307 
1312 template <typename T> inline T Pow10( T x ) noexcept
1313 {
1314  int i = TruncInt( x );
1315  return (i == x) ? Pow10I<T>()( i ) : T( std::pow( 10, x ) );
1316 }
1317 
1318 // ----------------------------------------------------------------------------
1319 
1330 template <typename T> inline T RotL( T x, uint32 n ) noexcept
1331 {
1332  static_assert( std::is_unsigned<T>::value,
1333  "RotL() can only be used for unsigned integer scalar types" );
1334  const uint8 mask = 8*sizeof( x ) - 1;
1335  const uint8 r = uint8( n & mask );
1336  return (x << r) | (x >> ((-r) & mask));
1337  // Or equivalently, but less optimized:
1338  //return (x << r) | (x >> (1+mask-r));
1339 }
1340 
1351 template <typename T> inline T RotR( T x, uint32 n ) noexcept
1352 {
1353  static_assert( std::is_unsigned<T>::value,
1354  "RotR() can only be used for unsigned integer scalar types" );
1355  const uint8 mask = 8*sizeof( x ) - 1;
1356  const uint8 r = uint8( n & mask );
1357  return (x >> r) | (x << ((-r) & mask));
1358  // Or equivalently, but less optimized:
1359  //return (x >> r) | (x << (1+mask-r));
1360 }
1361 
1362 // ----------------------------------------------------------------------------
1363 
1373 inline double Round( double x ) noexcept
1374 {
1375 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1376 
1377  return floor( x + 0.5 );
1378 
1379 #else
1380 
1381 # ifdef _MSC_VER
1382 # ifdef _M_X64
1383  return double( _mm_cvtsd_si64( _mm_load_sd( &x ) ) );
1384 # else
1385  __asm fld x
1386  __asm frndint
1387 # endif
1388 # else
1389  double r;
1390  asm volatile( "frndint\n": "=t" (r) : "0" (x) );
1391  return r;
1392 # endif
1393 
1394 #endif
1395 }
1396 
1406 inline float Round( float x ) noexcept
1407 {
1408 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1409 
1410  return floorf( x + 0.5F );
1411 
1412 #else
1413 
1414 # ifdef _MSC_VER
1415 # ifdef _M_X64
1416  return float( _mm_cvtss_si32( _mm_load_ss( &x ) ) );
1417 # else
1418  __asm fld x
1419  __asm frndint
1420 # endif
1421 # else
1422  float r;
1423  asm volatile( "frndint\n": "=t" (r) : "0" (x) );
1424  return r;
1425 # endif
1426 
1427 #endif
1428 }
1429 
1439 inline long double Round( long double x ) noexcept
1440 {
1441 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1442 
1443  return floorl( x + 0.5L );
1444 
1445 #else
1446 
1447 # ifdef _MSC_VER
1448 # ifdef _M_X64
1449  double _x = x;
1450  return (long double)_mm_cvtsd_si64( _mm_load_sd( &_x ) );
1451 # else
1452  __asm fld x
1453  __asm frndint
1454 # endif
1455 # else
1456  long double r;
1457  asm volatile( "frndint\n": "=t" (r) : "0" (x) );
1458  return r;
1459 # endif
1460 
1461 #endif
1462 }
1463 
1464 // ----------------------------------------------------------------------------
1465 
1503 template <typename T> inline int RoundInt( T x ) noexcept
1504 {
1505  PCL_PRECONDITION( x >= int_min && x <= int_max )
1506 
1507 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1508 
1509  return int( Round( x ) );
1510 
1511 #else
1512 
1513  volatile union { double d; int32 i; } v = { x + 6755399441055744.0 };
1514  return v.i; // ### NB: Assuming little-endian architecture.
1515 
1516 /*
1517  ### Deprecated code - The above code based on magic numbers is faster and
1518  more portable across platforms and compilers.
1519 
1520  // Default FPU rounding mode assumed to be nearest integer.
1521  int r;
1522 
1523 # ifdef _MSC_VER
1524  __asm fld x
1525  __asm fistp dword ptr r
1526 # else
1527  asm volatile( "fistpl %0\n" : "=m" (r) : "t" (x) : "st" );
1528 # endif
1529 
1530  return r;
1531 */
1532 
1533 #endif
1534 }
1535 
1544 template <typename T> inline int RoundI( T x ) noexcept
1545 {
1546  return RoundInt( x );
1547 }
1548 
1557 template <typename T> inline int RoundIntBanker( T x ) noexcept
1558 {
1559  return RoundInt( x );
1560 }
1561 
1586 template <typename T> inline int RoundIntArithmetic( T x ) noexcept
1587 {
1588  PCL_PRECONDITION( x >= int_min && x <= int_max )
1589 
1590  int i = TruncInt( x );
1591  if ( i < 0 )
1592  {
1593  if ( x - i <= T( -0.5 ) )
1594  return i-1;
1595  }
1596  else
1597  {
1598  if ( x - i >= T( +0.5 ) )
1599  return i+1;
1600  }
1601  return i;
1602 }
1603 
1604 // ----------------------------------------------------------------------------
1605 
1620 inline int64 RoundInt64( double x ) noexcept
1621 {
1622 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1623 
1624  return int64( Round( x ) );
1625 
1626 #else
1627 
1628  // ### N.B.: Default FPU rounding mode assumed to be nearest integer.
1629 
1630 # ifdef _MSC_VER
1631 # ifdef _M_X64
1632  return _mm_cvtsd_si64( _mm_load_sd( &x ) );
1633 # else
1634  int64 r;
1635  __asm fld x
1636  __asm fistp qword ptr r
1637  return r;
1638 # endif
1639 # else
1640  int64 r;
1641  asm volatile( "fistpll %0\n" : "=m" (r) : "t" (x) : "st" );
1642  return r;
1643 # endif
1644 
1645 #endif
1646 }
1647 
1654 inline int64 RoundI64( double x ) noexcept
1655 {
1656  return RoundInt64( x );
1657 }
1658 
1683 inline int64 RoundInt64Arithmetic( double x ) noexcept
1684 {
1685  int64 i = TruncInt64( x );
1686  if ( i < 0 )
1687  {
1688  if ( x - i <= -0.5 )
1689  return i-1;
1690  }
1691  else
1692  {
1693  if ( x - i >= +0.5 )
1694  return i+1;
1695  }
1696  return i;
1697 }
1698 
1699 // ----------------------------------------------------------------------------
1700 
1705 template <typename T> inline T Round( T x, int n ) noexcept
1706 {
1707  PCL_PRECONDITION( n >= 0 )
1708  T p = Pow10I<T>()( n ); return Round( p*x )/p;
1709 }
1710 
1711 // ----------------------------------------------------------------------------
1712 
1717 template <typename T> inline constexpr T Pow2( T x ) noexcept
1718 {
1719  // Use the relation:
1720  // 2**x = e**(x*ln(2))
1721  return Exp( x*Ln2() );
1722 }
1723 
1724 // ----------------------------------------------------------------------------
1725 
1736 template <typename T> struct PCL_CLASS Pow2I
1737 {
1738  T operator ()( int n ) const noexcept
1739  {
1740  // We shift left a single bit in 31-bit chunks.
1741  int i = ::abs( n ), p;
1742  double x = uint32( 1 ) << (p = Min( i, 31 ));
1743  while ( (i -= p) != 0 )
1744  x *= uint32( 1 ) << (p = Min( i, 31 ));
1745  return T( (n >= 0) ? x : 1/x );
1746  }
1747 };
1748 
1749 // ----------------------------------------------------------------------------
1750 
1755 template <typename T> inline T PowI( T x, int n ) noexcept
1756 {
1757  if ( n == 0 )
1758  return T( 1 );
1759 
1760  T r = x;
1761  int i = ::abs( n );
1762  if ( i > 1 )
1763  {
1764  do
1765  r *= r;
1766  while ( (i >>= 1) > 1 );
1767  if ( n & 1 )
1768  r *= x;
1769  }
1770  return (n > 0) ? r : T( 1/r );
1771 }
1772 
1777 template <typename T> inline T PowI4( T x ) noexcept
1778 {
1779  x *= x; return x*x;
1780 }
1781 
1786 template <typename T> inline T PowI6( T x ) noexcept
1787 {
1788  x *= x*x; return x*x;
1789 }
1790 
1795 template <typename T> inline T PowI8( T x ) noexcept
1796 {
1797  x *= x*x*x; return x*x;
1798 }
1799 
1804 template <typename T> inline T PowI10( T x ) noexcept
1805 {
1806  x *= x*x*x*x; return x*x;
1807 }
1808 
1813 template <typename T> inline T PowI12( T x ) noexcept
1814 {
1815  x *= x*x*x*x*x; return x*x;
1816 }
1817 
1818 // ----------------------------------------------------------------------------
1819 
1827 template <typename T> inline constexpr T ArcSinh( T x ) noexcept
1828 {
1829 #ifndef __PCL_NO_STD_INV_HYP_TRIG_FUNCTIONS
1830  return std::asinh( x );
1831 #else
1832  return Ln( x + Sqrt( 1 + x*x ) );
1833 #endif
1834 }
1835 
1836 // ----------------------------------------------------------------------------
1837 
1845 template <typename T> inline constexpr T ArcCosh( T x ) noexcept
1846 {
1847 #ifndef __PCL_NO_STD_INV_HYP_TRIG_FUNCTIONS
1848  return std::acosh( x );
1849 #else
1850  return 2*Ln( Sqrt( (x + 1)/2 ) + Sqrt( (x - 1)/2 ) );
1851 #endif
1852 }
1853 
1854 // ----------------------------------------------------------------------------
1855 
1863 template <typename T> inline constexpr T ArcTanh( T x ) noexcept
1864 {
1865 #ifndef __PCL_NO_STD_INV_HYP_TRIG_FUNCTIONS
1866  return std::atanh( x );
1867 #else
1868  return (Ln( 1 + x ) - Ln( 1 - x ))/2;
1869 #endif
1870 }
1871 
1872 // ----------------------------------------------------------------------------
1873 
1883 template <typename T> inline constexpr T ArcHav( T x ) noexcept
1884 {
1885  return 2*ArcSin( Sqrt( x ) );
1886 }
1887 
1888 // ----------------------------------------------------------------------------
1889 
1894 template <typename T> inline constexpr T Rad( T x ) noexcept
1895 {
1896  return static_cast<T>( 0.174532925199432957692369076848861272222e-01L * x );
1897 }
1898 
1899 // ----------------------------------------------------------------------------
1900 
1905 template <typename T> inline constexpr T RadMin( T x ) noexcept
1906 {
1907  return Deg( x )*60;
1908 }
1909 
1910 // ----------------------------------------------------------------------------
1911 
1916 template <typename T> inline constexpr T RadSec( T x ) noexcept
1917 {
1918  return Deg( x )*3600;
1919 }
1920 
1921 // ----------------------------------------------------------------------------
1922 
1927 template <typename T> inline constexpr T MinRad( T x ) noexcept
1928 {
1929  return Rad( x/60 );
1930 }
1931 
1932 // ----------------------------------------------------------------------------
1933 
1938 template <typename T> inline constexpr T SecRad( T x ) noexcept
1939 {
1940  return Rad( x/3600 );
1941 }
1942 
1947 template <typename T> inline constexpr T AsRad( T x ) noexcept
1948 {
1949  return SecRad( x );
1950 }
1951 
1952 // ----------------------------------------------------------------------------
1953 
1958 template <typename T> inline constexpr T MasRad( T x ) noexcept
1959 {
1960  return Rad( x/3600000 );
1961 }
1962 
1963 // ----------------------------------------------------------------------------
1964 
1969 template <typename T> inline constexpr T UasRad( T x ) noexcept
1970 {
1971  return Rad( x/3600000000 );
1972 }
1973 
1974 // ----------------------------------------------------------------------------
1975 
1980 template <typename T> inline constexpr T ModPi( T x ) noexcept
1981 {
1982  x = Mod( x + static_cast<T>( Pi() ), static_cast<T>( TwoPi() ) ) - static_cast<T>( Pi() );
1983  return (x < -static_cast<T>( Pi() )) ? x + static_cast<T>( TwoPi() ) : x;
1984 }
1985 
1986 // ----------------------------------------------------------------------------
1987 
1993 template <typename T> inline constexpr T Mod2Pi( T x ) noexcept
1994 {
1995  return Mod( x, static_cast<T>( TwoPi() ) );
1996 }
1997 
1998 // ----------------------------------------------------------------------------
1999 
2004 template <typename T> inline constexpr T Norm2Pi( T x ) noexcept
2005 {
2006  return ((x = Mod2Pi( x )) < 0) ? x + static_cast<T>( TwoPi() ) : x;
2007 }
2008 
2009 // ----------------------------------------------------------------------------
2010 
2023 template <typename T, typename T1, typename T2>
2024 inline void Rotate( T& x, T& y, T1 sa, T1 ca, T2 xc, T2 yc ) noexcept
2025 {
2026  T1 dx = T1( x ) - T1( xc );
2027  T1 dy = T1( y ) - T1( yc );
2028  x = T( T1( xc ) + ca*dx - sa*dy );
2029  y = T( T1( yc ) + sa*dx + ca*dy );
2030 }
2031 
2042 template <typename T1, typename T2>
2043 inline void Rotate( int& x, int& y, T1 sa, T1 ca, T2 xc, T2 yc ) noexcept
2044 {
2045  T1 dx = T1( x ) - T1( xc );
2046  T1 dy = T1( y ) - T1( yc );
2047  x = RoundInt( T1( xc ) + ca*dx - sa*dy );
2048  y = RoundInt( T1( yc ) + sa*dx + ca*dy );
2049 }
2050 
2061 template <typename T1, typename T2>
2062 inline void Rotate( long& x, long& y, T1 sa, T1 ca, T2 xc, T2 yc ) noexcept
2063 {
2064  T1 dx = T1( x ) - T1( xc );
2065  T1 dy = T1( y ) - T1( yc );
2066  x = (long)RoundInt( T1( xc ) + ca*dx - sa*dy );
2067  y = (long)RoundInt( T1( yc ) + sa*dx + ca*dy );
2068 }
2069 
2080 template <typename T1, typename T2>
2081 inline void Rotate( int64& x, int64& y, T1 sa, T1 ca, T2 xc, T2 yc ) noexcept
2082 {
2083  T1 dx = T1( x ) - T1( xc );
2084  T1 dy = T1( y ) - T1( yc );
2085  x = RoundInt64( T1( xc ) + ca*dx - sa*dy );
2086  y = RoundInt64( T1( yc ) + sa*dx + ca*dy );
2087 }
2088 
2104 template <typename T, typename T1, typename T2>
2105 inline void Rotate( T& x, T& y, T1 a, T2 xc, T2 yc ) noexcept
2106 {
2107  T1 sa, ca; SinCos( a, sa, ca ); Rotate( x, y, sa, ca, xc, yc );
2108 }
2109 
2110 // ----------------------------------------------------------------------------
2111 
2112 template <typename T, typename P, typename N> inline
2113 N __pcl_norm__( const T* i, const T* j, const P& p, N* ) noexcept
2114 {
2115  PCL_PRECONDITION( p > P( 0 ) )
2116  N n = N( 0 );
2117  for ( ; i < j; ++i )
2118  n += Pow( Abs( N( *i ) ), N( p ) );
2119  return Pow( n, N( 1/p ) );
2120 }
2121 
2135 template <typename T, typename P> inline T Norm( const T* i, const T* j, const P& p ) noexcept
2136 {
2137  return __pcl_norm__( i, j, p, (T*)( 0 ) );
2138 }
2139 
2140 template <typename P> inline double Norm( const float* i, const float* j, const P& p ) noexcept
2141 {
2142  return __pcl_norm__( i, j, p, (double*)( 0 ) );
2143 }
2144 template <typename P> inline double Norm( const int* i, const int* j, const P& p ) noexcept
2145 {
2146  return __pcl_norm__( i, j, p, (double*)( 0 ) );
2147 }
2148 template <typename P> inline double Norm( const unsigned* i, const unsigned* j, const P& p ) noexcept
2149 {
2150  return __pcl_norm__( i, j, p, (double*)( 0 ) );
2151 }
2152 template <typename P> inline double Norm( const int8* i, const int8* j, const P& p ) noexcept
2153 {
2154  return __pcl_norm__( i, j, p, (double*)( 0 ) );
2155 }
2156 template <typename P> inline double Norm( const uint8* i, const uint8* j, const P& p ) noexcept
2157 {
2158  return __pcl_norm__( i, j, p, (double*)( 0 ) );
2159 }
2160 template <typename P> inline double Norm( const int16* i, const int16* j, const P& p ) noexcept
2161 {
2162  return __pcl_norm__( i, j, p, (double*)( 0 ) );
2163 }
2164 template <typename P> inline double Norm( const uint16* i, const uint16* j, const P& p ) noexcept
2165 {
2166  return __pcl_norm__( i, j, p, (double*)( 0 ) );
2167 }
2168 template <typename P> inline double Norm( const int64* i, const int64* j, const P& p ) noexcept
2169 {
2170  return __pcl_norm__( i, j, p, (double*)( 0 ) );
2171 }
2172 template <typename P> inline double Norm( const uint64* i, const uint64* j, const P& p ) noexcept
2173 {
2174  return __pcl_norm__( i, j, p, (double*)( 0 ) );
2175 }
2176 
2177 // ----------------------------------------------------------------------------
2178 
2179 template <typename T, typename N> inline
2180 N __pcl_l1norm__( const T* i, const T* j, N* ) noexcept
2181 {
2182  N n = N( 0 );
2183  for ( ; i < j; ++i )
2184  n += N( Abs( *i ) );
2185  return n;
2186 }
2187 
2194 template <typename T> inline T L1Norm( const T* i, const T* j ) noexcept
2195 {
2196  return __pcl_l1norm__( i, j, (T*)( 0 ) );
2197 }
2198 
2199 inline double L1Norm( const float* i, const float* j ) noexcept
2200 {
2201  return __pcl_l1norm__( i, j, (double*)( 0 ) );
2202 }
2203 inline double L1Norm( const int* i, const int* j ) noexcept
2204 {
2205  return __pcl_l1norm__( i, j, (double*)( 0 ) );
2206 }
2207 inline double L1Norm( const unsigned* i, const unsigned* j ) noexcept
2208 {
2209  return __pcl_l1norm__( i, j, (double*)( 0 ) );
2210 }
2211 inline double L1Norm( const int8* i, const int8* j ) noexcept
2212 {
2213  return __pcl_l1norm__( i, j, (double*)( 0 ) );
2214 }
2215 inline double L1Norm( const uint8* i, const uint8* j ) noexcept
2216 {
2217  return __pcl_l1norm__( i, j, (double*)( 0 ) );
2218 }
2219 inline double L1Norm( const int16* i, const int16* j ) noexcept
2220 {
2221  return __pcl_l1norm__( i, j, (double*)( 0 ) );
2222 }
2223 inline double L1Norm( const uint16* i, const uint16* j ) noexcept
2224 {
2225  return __pcl_l1norm__( i, j, (double*)( 0 ) );
2226 }
2227 inline double L1Norm( const int64* i, const int64* j ) noexcept
2228 {
2229  return __pcl_l1norm__( i, j, (double*)( 0 ) );
2230 }
2231 inline double L1Norm( const uint64* i, const uint64* j ) noexcept
2232 {
2233  return __pcl_l1norm__( i, j, (double*)( 0 ) );
2234 }
2235 
2236 // ----------------------------------------------------------------------------
2237 
2238 template <typename T, typename N> inline
2239 N __pcl_l2norm__( const T* i, const T* j, N* ) noexcept
2240 {
2241  N n = N( 0 );
2242  for ( ; i < j; ++i )
2243  n += N( *i ) * N( *i );
2244  return Sqrt( n );
2245 }
2246 
2253 template <typename T> inline T L2Norm( const T* i, const T* j ) noexcept
2254 {
2255  return __pcl_l2norm__( i, j, (T*)( 0 ) );
2256 }
2257 
2258 inline double L2Norm( const float* i, const float* j ) noexcept
2259 {
2260  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2261 }
2262 inline double L2Norm( const int* i, const int* j ) noexcept
2263 {
2264  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2265 }
2266 inline double L2Norm( const unsigned* i, const unsigned* j ) noexcept
2267 {
2268  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2269 }
2270 inline double L2Norm( const int8* i, const int8* j ) noexcept
2271 {
2272  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2273 }
2274 inline double L2Norm( const uint8* i, const uint8* j ) noexcept
2275 {
2276  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2277 }
2278 inline double L2Norm( const int16* i, const int16* j ) noexcept
2279 {
2280  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2281 }
2282 inline double L2Norm( const uint16* i, const uint16* j ) noexcept
2283 {
2284  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2285 }
2286 inline double L2Norm( const int64* i, const int64* j ) noexcept
2287 {
2288  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2289 }
2290 inline double L2Norm( const uint64* i, const uint64* j ) noexcept
2291 {
2292  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2293 }
2294 
2295 // ----------------------------------------------------------------------------
2296 
2303 template <typename T> inline T Norm( const T* i, const T* j ) noexcept
2304 {
2305  return L2Norm( i, j );
2306 }
2307 
2308 inline double Norm( const float* i, const float* j ) noexcept
2309 {
2310  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2311 }
2312 inline double Norm( const int* i, const int* j ) noexcept
2313 {
2314  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2315 }
2316 inline double Norm( const unsigned* i, const unsigned* j ) noexcept
2317 {
2318  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2319 }
2320 inline double Norm( const int8* i, const int8* j ) noexcept
2321 {
2322  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2323 }
2324 inline double Norm( const uint8* i, const uint8* j ) noexcept
2325 {
2326  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2327 }
2328 inline double Norm( const int16* i, const int16* j ) noexcept
2329 {
2330  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2331 }
2332 inline double Norm( const uint16* i, const uint16* j ) noexcept
2333 {
2334  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2335 }
2336 inline double Norm( const int64* i, const int64* j ) noexcept
2337 {
2338  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2339 }
2340 inline double Norm( const uint64* i, const uint64* j ) noexcept
2341 {
2342  return __pcl_l2norm__( i, j, (double*)( 0 ) );
2343 }
2344 
2345 // ----------------------------------------------------------------------------
2346 
2387 void PCL_FUNC CalendarTimeToJD( int& jdi, double& jdf, int year, int month, int day, double dayf = 0 ) noexcept;
2388 
2428 inline double CalendarTimeToJD( int year, int month, int day, double dayf = 0 ) noexcept
2429 {
2430  int jdi;
2431  double jdf;
2432  CalendarTimeToJD( jdi, jdf, year, month, day, dayf );
2433  return jdi + jdf;
2434 }
2435 
2466 void PCL_FUNC JDToCalendarTime( int& year, int& month, int& day, double& dayf, int jdi, double jdf ) noexcept;
2467 
2497 inline void JDToCalendarTime( int& year, int& month, int& day, double& dayf, double jd ) noexcept
2498 {
2499  JDToCalendarTime( year, month, day, dayf, TruncInt( jd ), Frac( jd ) );
2500 }
2501 
2502 // ----------------------------------------------------------------------------
2503 
2524 template <typename S1, typename S2, typename S3, typename D>
2525 inline void DecimalToSexagesimal( int& sign, S1& s1, S2& s2, S3& s3, const D& d ) noexcept
2526 {
2527  double t1 = Abs( d );
2528  double t2 = Frac( t1 )*60;
2529  double t3 = Frac( t2 )*60;
2530  sign = (d < 0) ? -1 : +1;
2531  s1 = S1( TruncInt( t1 ) );
2532  s2 = S2( TruncInt( t2 ) );
2533  s3 = S3( t3 );
2534 }
2535 
2546 template <typename S1, typename S2, typename S3>
2547 inline double SexagesimalToDecimal( int sign, const S1& s1, const S2& s2 = S2( 0 ), const S3& s3 = S3( 0 ) ) noexcept
2548 {
2549  double d = Abs( s1 ) + (s2 + s3/60)/60;
2550  return (sign < 0) ? -d : d;
2551 }
2552 
2553 // ----------------------------------------------------------------------------
2554 
2573 template <typename T> inline double Sum( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2574 {
2575  double sum = 0;
2576  while ( i < j )
2577  sum += double( *i++ );
2578  return sum;
2579 }
2580 
2592 template <typename T> inline double StableSum( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2593 {
2594  double sum = 0;
2595  double eps = 0;
2596  while ( i < j )
2597  {
2598  double y = double( *i++ ) - eps;
2599  double t = sum + y;
2600  eps = (t - sum) - y;
2601  sum = t;
2602  }
2603  return sum;
2604 }
2605 
2617 template <typename T> inline double Modulus( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2618 {
2619  double S = 0;
2620  while ( i < j )
2621  S += Abs( double( *i++ ) );
2622  return S;
2623 }
2624 
2636 template <typename T> inline double StableModulus( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2637 {
2638  double sum = 0;
2639  double eps = 0;
2640  while ( i < j )
2641  {
2642  double y = Abs( double( *i++ ) ) - eps;
2643  double t = sum + y;
2644  eps = (t - sum) - y;
2645  sum = t;
2646  }
2647  return sum;
2648 }
2649 
2661 template <typename T> inline double SumOfSquares( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2662 {
2663  double Q = 0;
2664  while ( i < j )
2665  {
2666  double f = double( *i++ );
2667  Q += f*f;
2668  }
2669  return Q;
2670 }
2671 
2683 template <typename T> inline double StableSumOfSquares( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2684 {
2685  double sum = 0;
2686  double eps = 0;
2687  while ( i < j )
2688  {
2689  double f = double( *i++ );
2690  double y = f*f - eps;
2691  double t = sum + y;
2692  eps = (t - sum) - y;
2693  sum = t;
2694  }
2695  return sum;
2696 }
2697 
2709 template <typename T> inline double Mean( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2710 {
2711  distance_type n = j - i;
2712  if ( n < 1 )
2713  return 0;
2714  return Sum( i, j )/n;
2715 }
2716 
2728 template <typename T> inline double StableMean( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2729 {
2730  distance_type n = j - i;
2731  if ( n < 1 )
2732  return 0;
2733  return StableSum( i, j )/n;
2734 }
2735 
2753 template <typename T> inline double Variance( const T* __restrict__ i, const T* __restrict__ j, double center ) noexcept
2754 {
2755  distance_type n = j - i;
2756  if ( n < 2 )
2757  return 0;
2758  double var = 0, eps = 0;
2759  do
2760  {
2761  double d = double( *i++ ) - center;
2762  var += d*d;
2763  eps += d;
2764  }
2765  while ( i < j );
2766  return (var - eps*eps/n)/(n - 1);
2767 }
2768 
2785 template <typename T> inline double Variance( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2786 {
2787  distance_type n = j - i;
2788  if ( n < 2 )
2789  return 0;
2790  double m = 0;
2791  for ( const T* f = i; f < j; ++f )
2792  m += double( *f );
2793  m /= n;
2794  double var = 0, eps = 0;
2795  do
2796  {
2797  double d = double( *i++ ) - m;
2798  var += d*d;
2799  eps += d;
2800  }
2801  while ( i < j );
2802  return (var - eps*eps/n)/(n - 1);
2803 }
2804 
2813 template <typename T> inline double StdDev( const T* __restrict__ i, const T* __restrict__ j, double center ) noexcept
2814 {
2815  return Sqrt( Variance( i, j, center ) );
2816 }
2817 
2825 template <typename T> inline double StdDev( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2826 {
2827  return Sqrt( Variance( i, j ) );
2828 }
2829 
2878 template <class T> inline double Median( const T* __restrict__ i, const T* __restrict__ j )
2879 {
2880  distance_type n = j - i;
2881  if ( n < 1 )
2882  return 0;
2883  if ( n == 1 )
2884  return double( *i );
2885  double* d = new double[ n ];
2886  double* __restrict__ t = d;
2887  do
2888  *t++ = double( *i++ );
2889  while ( i < j );
2890  double m = double( *pcl::Select( d, t, n >> 1 ) );
2891  if ( (n & 1) == 0 )
2892  m = (m + double( *pcl::Select( d, t, (n >> 1)-1 ) ))/2;
2893  delete [] d;
2894  return m;
2895 }
2896 
2897 double PCL_FUNC Median( const unsigned char* __restrict__ i, const unsigned char* __restrict__ j );
2898 double PCL_FUNC Median( const signed char* __restrict__ i, const signed char* __restrict__ j );
2899 double PCL_FUNC Median( const wchar_t* __restrict__ i, const wchar_t* __restrict__ j );
2900 double PCL_FUNC Median( const unsigned short* __restrict__ i, const unsigned short* __restrict__ j );
2901 double PCL_FUNC Median( const signed short* __restrict__ i, const signed short* __restrict__ j );
2902 double PCL_FUNC Median( const unsigned int* __restrict__ i, const unsigned int* __restrict__ j );
2903 double PCL_FUNC Median( const signed int* __restrict__ i, const signed int* __restrict__ j );
2904 double PCL_FUNC Median( const unsigned long* __restrict__ i, const unsigned long* __restrict__ j );
2905 double PCL_FUNC Median( const signed long* __restrict__ i, const signed long* __restrict__ j );
2906 double PCL_FUNC Median( const unsigned long long* __restrict__ i, const unsigned long long* __restrict__ j );
2907 double PCL_FUNC Median( const signed long long* __restrict__ i, const signed long long* __restrict__ j );
2908 double PCL_FUNC Median( const float* __restrict__ i, const float* __restrict__ j );
2909 double PCL_FUNC Median( const double* __restrict__ i, const double* __restrict__ j );
2910 double PCL_FUNC Median( const long double* __restrict__ i, const long double* __restrict__ j );
2911 
2912 #define CMPXCHG( a, b ) \
2913  if ( i[b] < i[a] ) pcl::Swap( i[a], i[b] )
2914 
2915 #define MEAN( a, b ) \
2916  (double( a ) + double( b ))/2
2917 
2960 template <typename T> inline double MedianDestructive( T* __restrict__ i, T* __restrict__ j ) noexcept
2961 {
2962  distance_type n = j - i;
2963  if ( n < 1 )
2964  return 0;
2965 
2966  switch ( n )
2967  {
2968  case 1: // !?
2969  return i[0];
2970  case 2:
2971  return MEAN( i[0], i[1] );
2972  case 3:
2973  CMPXCHG( 0, 1 ); CMPXCHG( 1, 2 );
2974  return pcl::Max( i[0], i[1] );
2975  case 4:
2976  CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
2977  CMPXCHG( 1, 3 );
2978  return MEAN( i[1], i[2] );
2979  case 5:
2980  CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 0, 3 );
2981  CMPXCHG( 1, 4 ); CMPXCHG( 1, 2 ); CMPXCHG( 2, 3 );
2982  return pcl::Max( i[1], i[2] );
2983  case 6:
2984  CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
2985  CMPXCHG( 1, 3 ); CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 );
2986  CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 1, 4 );
2987  CMPXCHG( 2, 4 ); CMPXCHG( 3, 5 ); CMPXCHG( 3, 4 );
2988  return MEAN( i[2], i[3] );
2989  case 7:
2990  CMPXCHG( 0, 5 ); CMPXCHG( 0, 3 ); CMPXCHG( 1, 6 );
2991  CMPXCHG( 2, 4 ); CMPXCHG( 0, 1 ); CMPXCHG( 3, 5 );
2992  CMPXCHG( 2, 6 ); CMPXCHG( 2, 3 ); CMPXCHG( 3, 6 );
2993  CMPXCHG( 4, 5 ); CMPXCHG( 1, 4 ); CMPXCHG( 1, 3 );
2994  return pcl::Min( i[3], i[4] );
2995  case 8:
2996  CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 2, 6 );
2997  CMPXCHG( 3, 7 ); CMPXCHG( 0, 2 ); CMPXCHG( 1, 3 );
2998  CMPXCHG( 4, 6 ); CMPXCHG( 5, 7 ); CMPXCHG( 2, 4 );
2999  CMPXCHG( 3, 5 ); CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 );
3000  CMPXCHG( 4, 5 ); CMPXCHG( 6, 7 ); CMPXCHG( 1, 4 );
3001  CMPXCHG( 3, 6 );
3002  return MEAN( i[3], i[4] );
3003  case 9:
3004  CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
3005  CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 6, 7 );
3006  CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
3007  CMPXCHG( 0, 3 ); CMPXCHG( 5, 8 ); CMPXCHG( 4, 7 );
3008  CMPXCHG( 3, 6 ); CMPXCHG( 1, 4 ); CMPXCHG( 2, 5 );
3009  CMPXCHG( 4, 7 ); CMPXCHG( 4, 2 ); CMPXCHG( 6, 4 );
3010  return pcl::Min( i[2], i[4] );
3011  default:
3012  {
3013  double m = double( *pcl::Select( i, j, n >> 1 ) );
3014  if ( n & 1 )
3015  return m;
3016  return MEAN( m, double( *pcl::Select( i, j, (n >> 1)-1 ) ) );
3017  }
3018  }
3019 }
3020 
3021 #undef CMPXCHG
3022 
3023 #define CMPXCHG( a, b ) \
3024  if ( p( i[b], i[a] ) ) pcl::Swap( i[a], i[b] )
3025 
3041 template <typename T, class BP> inline double MedianDestructive( T* __restrict__ i, T* __restrict__ j, BP p ) noexcept
3042 {
3043  distance_type n = j - i;
3044  if ( n < 1 )
3045  return 0;
3046 
3047  switch ( n )
3048  {
3049  case 1: // !?
3050  return i[0];
3051  case 2:
3052  return MEAN( i[0], i[1] );
3053  case 3:
3054  CMPXCHG( 0, 1 ); CMPXCHG( 1, 2 );
3055  return pcl::Max( i[0], i[1] );
3056  case 4:
3057  CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
3058  CMPXCHG( 1, 3 );
3059  return MEAN( i[1], i[2] );
3060  case 5:
3061  CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 0, 3 );
3062  CMPXCHG( 1, 4 ); CMPXCHG( 1, 2 ); CMPXCHG( 2, 3 );
3063  return pcl::Max( i[1], i[2] );
3064  case 6:
3065  CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
3066  CMPXCHG( 1, 3 ); CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 );
3067  CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 1, 4 );
3068  CMPXCHG( 2, 4 ); CMPXCHG( 3, 5 ); CMPXCHG( 3, 4 );
3069  return MEAN( i[2], i[3] );
3070  case 7:
3071  CMPXCHG( 0, 5 ); CMPXCHG( 0, 3 ); CMPXCHG( 1, 6 );
3072  CMPXCHG( 2, 4 ); CMPXCHG( 0, 1 ); CMPXCHG( 3, 5 );
3073  CMPXCHG( 2, 6 ); CMPXCHG( 2, 3 ); CMPXCHG( 3, 6 );
3074  CMPXCHG( 4, 5 ); CMPXCHG( 1, 4 ); CMPXCHG( 1, 3 );
3075  return pcl::Min( i[3], i[4] );
3076  case 8:
3077  CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 2, 6 );
3078  CMPXCHG( 3, 7 ); CMPXCHG( 0, 2 ); CMPXCHG( 1, 3 );
3079  CMPXCHG( 4, 6 ); CMPXCHG( 5, 7 ); CMPXCHG( 2, 4 );
3080  CMPXCHG( 3, 5 ); CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 );
3081  CMPXCHG( 4, 5 ); CMPXCHG( 6, 7 ); CMPXCHG( 1, 4 );
3082  CMPXCHG( 3, 6 );
3083  return MEAN( i[3], i[4] );
3084  case 9:
3085  CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
3086  CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 6, 7 );
3087  CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
3088  CMPXCHG( 0, 3 ); CMPXCHG( 5, 8 ); CMPXCHG( 4, 7 );
3089  CMPXCHG( 3, 6 ); CMPXCHG( 1, 4 ); CMPXCHG( 2, 5 );
3090  CMPXCHG( 4, 7 ); CMPXCHG( 4, 2 ); CMPXCHG( 6, 4 );
3091  return pcl::Min( i[2], i[4] );
3092  default:
3093  {
3094  double m = double( *pcl::Select( i, j, n >> 1, p ) );
3095  if ( n & 1 )
3096  return m;
3097  return MEAN( m, double( *pcl::Select( i, j, (n >> 1)-1, p ) ) );
3098  }
3099  }
3100 }
3101 
3102 #undef CMPXCHG
3103 #undef MEAN
3104 
3138 template <typename T> inline double OrderStatistic( const T* __restrict__ i, const T* __restrict__ j, distance_type k )
3139 {
3140  distance_type n = j - i;
3141  if ( n < 1 || k < 0 || k >= n )
3142  return 0;
3143  if ( n == 1 )
3144  return double( *i );
3145  double* d = new double[ n ];
3146  double* t = d;
3147  do
3148  *t++ = double( *i++ );
3149  while ( i < j );
3150  double s = *pcl::Select( d, t, k );
3151  delete [] d;
3152  return s;
3153 }
3154 
3155 double PCL_FUNC OrderStatistic( const unsigned char* __restrict__ i, const unsigned char* __restrict__ j, distance_type k );
3156 double PCL_FUNC OrderStatistic( const signed char* __restrict__ i, const signed char* __restrict__ j, distance_type k );
3157 double PCL_FUNC OrderStatistic( const wchar_t* __restrict__ i, const wchar_t* __restrict__ j, distance_type k );
3158 double PCL_FUNC OrderStatistic( const unsigned short* __restrict__ i, const unsigned short* __restrict__ j, distance_type k );
3159 double PCL_FUNC OrderStatistic( const signed short* __restrict__ i, const signed short* __restrict__ j, distance_type k );
3160 double PCL_FUNC OrderStatistic( const unsigned int* __restrict__ i, const unsigned int* __restrict__ j, distance_type k );
3161 double PCL_FUNC OrderStatistic( const signed int* __restrict__ i, const signed int* __restrict__ j, distance_type k );
3162 double PCL_FUNC OrderStatistic( const unsigned long* __restrict__ i, const unsigned long* __restrict__ j, distance_type k );
3163 double PCL_FUNC OrderStatistic( const signed long* __restrict__ i, const signed long* __restrict__ j, distance_type k );
3164 double PCL_FUNC OrderStatistic( const unsigned long long* __restrict__ i, const unsigned long long* __restrict__ j, distance_type k );
3165 double PCL_FUNC OrderStatistic( const signed long long* __restrict__ i, const signed long long* __restrict__ j, distance_type k );
3166 double PCL_FUNC OrderStatistic( const float* __restrict__ i, const float* __restrict__ j, distance_type k );
3167 double PCL_FUNC OrderStatistic( const double* __restrict__ i, const double* __restrict__ j, distance_type k );
3168 double PCL_FUNC OrderStatistic( const long double* __restrict__ i, const long double* __restrict__ j, distance_type k );
3169 
3197 template <typename T> inline double OrderStatisticDestructive( T* __restrict__ i, T* __restrict__ j, distance_type k ) noexcept
3198 {
3199  distance_type n = j - i;
3200  if ( n < 1 || k < 0 || k >= n )
3201  return 0;
3202  if ( n == 1 )
3203  return double( *i );
3204  return double( *pcl::Select( i, j, k ) );
3205 }
3206 
3219 template <typename T, class BP> inline double OrderStatisticDestructive( const T* __restrict__ i, const T* __restrict__ j, distance_type k, BP p ) noexcept
3220 {
3221  distance_type n = j - i;
3222  if ( n < 1 || k < 0 || k >= n )
3223  return 0;
3224  if ( n == 1 )
3225  return double( *i );
3226  return double( *pcl::Select( i, j, k, p ) );
3227 }
3228 
3242 template <typename T> inline double TrimmedMean( const T* __restrict__ i, const T* __restrict__ j, distance_type l = 1, distance_type h = 1 )
3243 {
3244  distance_type n = j - i;
3245  if ( n < 1 )
3246  return 0;
3247  if ( l+h < 1 )
3248  return Sum( i, j )/n;
3249  if ( l+h >= n )
3250  return 0;
3251  for ( double s = 0,
3252  t0 = OrderStatistic( i, j, l ),
3253  t1 = OrderStatistic( i, j, n-h-1 ); ; )
3254  {
3255  double x = double( *i );
3256  if ( x >= t0 )
3257  if ( x <= t1 )
3258  s += x;
3259  if ( ++i == j )
3260  return s/(n - l - h);
3261  }
3262 }
3263 
3281 template <typename T> inline double TrimmedMeanDestructive( T* __restrict__ i, T* __restrict__ j, distance_type l = 1, distance_type h = 1 ) noexcept
3282 {
3283  distance_type n = j - i;
3284  if ( n < 1 )
3285  return 0;
3286  if ( l+h < 1 )
3287  return Sum( i, j )/n;
3288  if ( l+h >= n )
3289  return 0;
3290  for ( double s = 0,
3291  t0 = OrderStatisticDestructive( i, j, l ),
3292  t1 = OrderStatisticDestructive( i, j, n-h-1 ); ; )
3293  {
3294  double x = double( *i );
3295  if ( x >= t0 )
3296  if ( x <= t1 )
3297  s += x;
3298  if ( ++i == j )
3299  return s/(n - l - h);
3300  }
3301 }
3302 
3319 template <typename T> inline double TrimmedMeanOfSquares( const T* __restrict__ i, const T* __restrict__ j, distance_type l = 1, distance_type h = 1 )
3320 {
3321  distance_type n = j - i;
3322  if ( n < 1 )
3323  return 0;
3324  if ( l+h < 1 )
3325  return Sum( i, j )/n;
3326  if ( l+h >= n )
3327  return 0;
3328  for ( double s = 0,
3329  t0 = OrderStatistic( i, j, l ),
3330  t1 = OrderStatistic( i, j, n-h-1 ); ; )
3331  {
3332  double x = double( *i );
3333  if ( x >= t0 )
3334  if ( x <= t1 )
3335  s += x*x;
3336  if ( ++i == j )
3337  return s/(n - l - h);
3338  }
3339 }
3340 
3362 template <typename T> inline double TrimmedMeanOfSquaresDestructive( T* __restrict__ i, T* __restrict__ j, distance_type l = 1, distance_type h = 1 ) noexcept
3363 {
3364  distance_type n = j - i;
3365  if ( n < 1 )
3366  return 0;
3367  if ( l+h < 1 )
3368  return Sum( i, j )/n;
3369  if ( l+h >= n )
3370  return 0;
3371  for ( double s = 0,
3372  t0 = OrderStatisticDestructive( i, j, l ),
3373  t1 = OrderStatisticDestructive( i, j, n-h-1 ); ; )
3374  {
3375  double x = double( *i );
3376  if ( x >= t0 )
3377  if ( x <= t1 )
3378  s += x*x;
3379  if ( ++i == j )
3380  return s/(n - l - h);
3381  }
3382 }
3383 
3404 template <typename T> inline double AvgDev( const T* __restrict__ i, const T* __restrict__ j, double center ) noexcept
3405 {
3406  distance_type n = j - i;
3407  if ( n < 2 )
3408  return 0;
3409  double d = 0;
3410  do
3411  d += Abs( double( *i++ ) - center );
3412  while ( i < j );
3413  return d/n;
3414 }
3415 
3436 template <typename T> inline double StableAvgDev( const T* __restrict__ i, const T* __restrict__ j, double center ) noexcept
3437 {
3438  distance_type n = j - i;
3439  if ( n < 2 )
3440  return 0;
3441  double sum = 0;
3442  double eps = 0;
3443  do
3444  {
3445  double y = Abs( double( *i++ ) - center ) - eps;
3446  double t = sum + y;
3447  eps = (t - sum) - y;
3448  sum = t;
3449  }
3450  while ( i < j );
3451  return sum/n;
3452 }
3453 
3473 template <typename T> inline double AvgDev( const T* __restrict__ i, const T* __restrict__ j )
3474 {
3475  distance_type n = j - i;
3476  if ( n < 2 )
3477  return 0;
3478  double m = Median( i, j );
3479  double d = 0;
3480  do
3481  d += Abs( double( *i++ ) - m );
3482  while ( i < j );
3483  return d/n;
3484 }
3485 
3505 template <typename T> inline double StableAvgDev( const T* __restrict__ i, const T* __restrict__ j )
3506 {
3507  return pcl::StableAvgDev( i, j, pcl::Median( i, j ) );
3508 }
3509 
3527 struct TwoSidedEstimate
3528 {
3529  double low = 0;
3530  double high = 0;
3531 
3535  TwoSidedEstimate() = default;
3536 
3540  TwoSidedEstimate( const TwoSidedEstimate& ) = default;
3541 
3545  TwoSidedEstimate( TwoSidedEstimate&& ) = default;
3546 
3550  TwoSidedEstimate& operator =( const TwoSidedEstimate& ) = default;
3551 
3555  TwoSidedEstimate& operator =( TwoSidedEstimate&& ) = default;
3556 
3560  template <typename T1, typename T2>
3561  TwoSidedEstimate( const T1& l, const T2& h )
3562  : low( double( l ) )
3563  , high( double( h ) )
3564  {
3565  }
3566 
3571  template <typename T>
3572  TwoSidedEstimate( const T& x )
3573  {
3574  low = high = double( x );
3575  }
3576 
3583  bool IsValid() const noexcept
3584  {
3585  return IsFinite( low ) && low > std::numeric_limits<double>::epsilon()
3586  && IsFinite( high ) && high > std::numeric_limits<double>::epsilon();
3587  }
3588 
3592  double Total() const noexcept
3593  {
3594  return low + high;
3595  }
3596 
3601  double Mean() const noexcept
3602  {
3603  if ( low != 0 )
3604  {
3605  if ( high != 0 )
3606  return (low + high)/2;
3607  return low;
3608  }
3609  return high;
3610  }
3611 
3615  explicit operator double() const noexcept
3616  {
3617  return Mean();
3618  }
3619 
3624  TwoSidedEstimate& operator *=( double x ) noexcept
3625  {
3626  low *= x;
3627  high *= x;
3628  return *this;
3629  }
3630 
3634  TwoSidedEstimate& operator /=( double x ) noexcept
3635  {
3636  low /= x;
3637  high /= x;
3638  return *this;
3639  }
3640 
3645  TwoSidedEstimate& operator /=( const TwoSidedEstimate& e ) noexcept
3646  {
3647  low /= e.low;
3648  high /= e.high;
3649  return *this;
3650  }
3651 
3655  TwoSidedEstimate operator *( double x ) const noexcept
3656  {
3657  return { low*x, high*x };
3658  }
3659 
3663  TwoSidedEstimate operator /( double x ) const noexcept
3664  {
3665  return { low/x, high/x };
3666  }
3667 
3672  TwoSidedEstimate operator /( const TwoSidedEstimate& e ) const noexcept
3673  {
3674  return { low/e.low, high/e.high };
3675  }
3676 };
3677 
3682 inline TwoSidedEstimate Sqrt( const TwoSidedEstimate& e ) noexcept
3683 {
3684  return { Sqrt( e.low ), Sqrt( e.high ) };
3685 }
3686 
3691 template <typename T> inline TwoSidedEstimate Pow( const TwoSidedEstimate& e, T x ) noexcept
3692 {
3693  double x_ = double( x );
3694  return { Pow( e.low, x_ ), Pow( e.high, x_ ) };
3695 }
3696 
3717 template <typename T> inline TwoSidedEstimate TwoSidedAvgDev( const T* __restrict__ i, const T* __restrict__ j, double center ) noexcept
3718 {
3719  double dl = 0, dh = 0;
3720  distance_type nl = 0, nh = 0;
3721  while ( i < j )
3722  {
3723  double x = double( *i++ );
3724  if ( x <= center )
3725  {
3726  dl += center - x;
3727  ++nl;
3728  }
3729  else
3730  {
3731  dh += x - center;
3732  ++nh;
3733  }
3734  }
3735  return { (nl > 1) ? dl/nl : 0.0,
3736  (nh > 1) ? dh/nh : 0.0 };
3737 }
3738 
3758 template <typename T> inline TwoSidedEstimate TwoSidedAvgDev( const T* __restrict__ i, const T* __restrict__ j )
3759 {
3760  return pcl::TwoSidedAvgDev( i, j, pcl::Median( i, j ) );
3761 }
3762 
3776 template <typename T> inline double MAD( const T* __restrict__ i, const T* __restrict__ j, double center )
3777 {
3778  distance_type n = j - i;
3779  if ( n < 2 )
3780  return 0;
3781  double* d = new double[ n ];
3782  double* p = d;
3783  do
3784  *p++ = Abs( double( *i++ ) - center );
3785  while ( i < j );
3786  double m = pcl::Median( d, d+n );
3787  delete [] d;
3788  return m;
3789 }
3790 
3791 double PCL_FUNC MAD( const unsigned char* __restrict__ i, const unsigned char* __restrict__ j, double center );
3792 double PCL_FUNC MAD( const signed char* __restrict__ i, const signed char* __restrict__ j, double center );
3793 double PCL_FUNC MAD( const wchar_t* __restrict__ i, const wchar_t* __restrict__ j, double center );
3794 double PCL_FUNC MAD( const unsigned short* __restrict__ i, const unsigned short* __restrict__ j, double center );
3795 double PCL_FUNC MAD( const signed short* __restrict__ i, const signed short* __restrict__ j, double center );
3796 double PCL_FUNC MAD( const unsigned int* __restrict__ i, const unsigned int* __restrict__ j, double center );
3797 double PCL_FUNC MAD( const signed int* __restrict__ i, const signed int* __restrict__ j, double center );
3798 double PCL_FUNC MAD( const unsigned long* __restrict__ i, const unsigned long* __restrict__ j, double center );
3799 double PCL_FUNC MAD( const signed long* __restrict__ i, const signed long* __restrict__ j, double center );
3800 double PCL_FUNC MAD( const unsigned long long* __restrict__ i, const unsigned long long* __restrict__ j, double center );
3801 double PCL_FUNC MAD( const signed long long* __restrict__ i, const signed long long* __restrict__ j, double center );
3802 double PCL_FUNC MAD( const float* __restrict__ i, const float* __restrict__ j, double center );
3803 double PCL_FUNC MAD( const double* __restrict__ i, const double* __restrict__ j, double center );
3804 double PCL_FUNC MAD( const long double* __restrict__ i, const long double* __restrict__ j, double center );
3805 
3819 template <typename T> inline double MAD( const T* __restrict__ i, const T* __restrict__ j )
3820 {
3821  return pcl::MAD( i, j, pcl::Median( i, j ) );
3822 }
3823 
3837 template <typename T> inline TwoSidedEstimate TwoSidedMAD( const T* __restrict__ i, const T* __restrict__ j, double center )
3838 {
3839  distance_type n = j - i;
3840  if ( n < 2 )
3841  return 0;
3842  double* d = new double[ n ];
3843  double* __restrict__ p = d;
3844  double* __restrict__ q = d + n;
3845  do
3846  {
3847  double x = double( *i++ );
3848  if ( x <= center )
3849  *p++ = center - x;
3850  else
3851  *--q = x - center;
3852  }
3853  while( i < j );
3854  double l = pcl::Median( d, p );
3855  double h = pcl::Median( q, d+n );
3856  delete [] d;
3857  return { l, h };
3858 }
3859 
3860 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const unsigned char* __restrict__ i, const unsigned char* __restrict__ j, double center );
3861 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const signed char* __restrict__ i, const signed char* __restrict__ j, double center );
3862 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const wchar_t* __restrict__ i, const wchar_t* __restrict__ j, double center );
3863 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const unsigned short* __restrict__ i, const unsigned short* __restrict__ j, double center );
3864 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const signed short* __restrict__ i, const signed short* __restrict__ j, double center );
3865 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const unsigned int* __restrict__ i, const unsigned int* __restrict__ j, double center );
3866 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const signed int* __restrict__ i, const signed int* __restrict__ j, double center );
3867 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const unsigned long* __restrict__ i, const unsigned long* __restrict__ j, double center );
3868 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const signed long* __restrict__ i, const signed long* __restrict__ j, double center );
3869 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const unsigned long long* __restrict__ i, const unsigned long long* __restrict__ j, double center );
3870 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const signed long long* __restrict__ i, const signed long long* __restrict__ j, double center );
3871 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const float* __restrict__ i, const float* __restrict__ j, double center );
3872 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const double* __restrict__ i, const double* __restrict__ j, double center );
3873 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const long double* __restrict__ i, const long double* __restrict__ j, double center );
3874 
3888 template <typename T> inline TwoSidedEstimate TwoSidedMAD( const T* __restrict__ i, const T* __restrict__ j )
3889 {
3890  return pcl::TwoSidedMAD( i, j, pcl::Median( i, j ) );
3891 }
3892 
3924 template <typename T> double Sn( T* __restrict__ x, T* __restrict__ xn )
3925 {
3926  /*
3927  * N.B.: In the code below, lines commented with an asterisk (*) have been
3928  * modified with respect to the FORTRAN original to account for zero-based
3929  * array indices.
3930  */
3931 
3932  distance_type n = xn - x;
3933  if ( n < 2 )
3934  return 0;
3935 
3936  pcl::Sort( x, xn );
3937 
3938  double* a2 = new double[ n ];
3939  a2[0] = double( x[n >> 1] ) - double( x[0] ); // *
3940 
3941  distance_type nh = (n + 1) >> 1;
3942 
3943  for ( distance_type i = 2; i <= nh; ++i )
3944  {
3945  distance_type nA = i-1;
3946  distance_type nB = n - i;
3947  distance_type diff = nB - nA;
3948  distance_type leftA = 1;
3949  distance_type leftB = 1;
3950  distance_type rightA = nB;
3951  distance_type Amin = (diff >> 1) + 1;
3952  distance_type Amax = (diff >> 1) + nA;
3953 
3954  while ( leftA < rightA )
3955  {
3956  distance_type length = rightA - leftA + 1;
3957  distance_type even = (length & 1) == 0;
3958  distance_type half = (length - 1) >> 1;
3959  distance_type tryA = leftA + half;
3960  distance_type tryB = leftB + half;
3961 
3962  if ( tryA < Amin )
3963  leftA = tryA + even;
3964  else
3965  {
3966  if ( tryA > Amax )
3967  {
3968  rightA = tryA;
3969  leftB = tryB + even;
3970  }
3971  else
3972  {
3973  double medA = double( x[i-1] ) - double( x[i-2 - tryA + Amin] ); // *
3974  double medB = double( x[tryB + i-1] ) - double( x[i-1] ); // *
3975  if ( medA >= medB )
3976  {
3977  rightA = tryA;
3978  leftB = tryB + even;
3979  }
3980  else
3981  leftA = tryA + even;
3982  }
3983  }
3984  }
3985 
3986  if ( leftA > Amax )
3987  a2[i-1] = double( x[leftB + i-1] ) - double( x[i-1] ); // *
3988  else
3989  {
3990  double medA = double( x[i-1] ) - double( x[i-2 - leftA + Amin] ); // *
3991  double medB = double( x[leftB + i-1] ) - double( x[i-1] );
3992  a2[i-1] = pcl::Min( medA, medB ); // *
3993  }
3994  }
3995 
3996  for ( distance_type i = nh + 1; i < n; ++i )
3997  {
3998  distance_type nA = n - i;
3999  distance_type nB = i - 1;
4000  distance_type diff = nB - nA;
4001  distance_type leftA = 1;
4002  distance_type leftB = 1;
4003  distance_type rightA = nB;
4004  distance_type Amin = (diff >> 1) + 1;
4005  distance_type Amax = (diff >> 1) + nA;
4006 
4007  while ( leftA < rightA )
4008  {
4009  distance_type length = rightA - leftA + 1;
4010  distance_type even = (length & 1) == 0;
4011  distance_type half = (length - 1) >> 1;
4012  distance_type tryA = leftA + half;
4013  distance_type tryB = leftB + half;
4014 
4015  if ( tryA < Amin)
4016  leftA = tryA + even;
4017  else
4018  {
4019  if ( tryA > Amax )
4020  {
4021  rightA = tryA;
4022  leftB = tryB + even;
4023  }
4024  else
4025  {
4026  double medA = double( x[i + tryA - Amin] ) - double( x[i-1] ); // *
4027  double medB = double( x[i-1] ) - double( x[i-1 - tryB] ); // *
4028  if ( medA >= medB )
4029  {
4030  rightA = tryA;
4031  leftB = tryB + even;
4032  }
4033  else
4034  leftA = tryA + even;
4035  }
4036  }
4037  }
4038 
4039  if ( leftA > Amax )
4040  a2[i-1] = double( x[i-1] ) - double( x[i-1 - leftB] ); // *
4041  else
4042  {
4043  double medA = double( x[i + leftA - Amin] ) - double( x[i-1] ); // *
4044  double medB = double( x[i-1] ) - double( x[i-1 - leftB] ); // *
4045  a2[i-1] = pcl::Min( medA, medB ); // *
4046  }
4047  }
4048 
4049  a2[n-1] = double( x[n-1] ) - double( x[nh-1] ); // *
4050 
4051  /*
4052  * Correction for a finite sample
4053  */
4054  double cn;
4055  switch ( n )
4056  {
4057  case 2: cn = 0.743; break;
4058  case 3: cn = 1.851; break;
4059  case 4: cn = 0.954; break;
4060  case 5: cn = 1.351; break;
4061  case 6: cn = 0.993; break;
4062  case 7: cn = 1.198; break;
4063  case 8: cn = 1.005; break;
4064  case 9: cn = 1.131; break;
4065  default: cn = (n & 1) ? n/(n - 0.9) : 1.0; break;
4066  }
4067 
4068  double sn = cn * *pcl::Select( a2, a2+n, nh-1 );
4069 
4070  delete [] a2;
4071  return sn;
4072 }
4073 
4084 inline double __pcl_whimed__( double* a, distance_type* iw, distance_type n,
4085  double* acand, distance_type* iwcand )
4086 {
4087  distance_type wtotal = 0;
4088  for ( distance_type i = 0; i < n; ++i )
4089  wtotal += iw[i];
4090 
4091  for ( distance_type nn = n, wrest = 0; ; )
4092  {
4093  double trial = *pcl::Select( a, a+nn, nn >> 1 ); // *
4094 
4095  distance_type wleft = 0;
4096  distance_type wmid = 0;
4097  for ( distance_type i = 0; i < nn; ++i )
4098  if ( a[i] < trial )
4099  wleft += iw[i];
4100  else if ( a[i] == trial )
4101  wmid += iw[i];
4102 
4103  if ( 2*(wrest + wleft) > wtotal )
4104  {
4105  distance_type kcand = 0;
4106  for ( distance_type i = 0; i < nn; ++i )
4107  if ( a[i] < trial )
4108  {
4109  acand[kcand] = a[i];
4110  iwcand[kcand] = iw[i];
4111  ++kcand;
4112  }
4113  nn = kcand;
4114  }
4115  else
4116  {
4117  if ( 2*(wrest + wleft + wmid) > wtotal )
4118  return trial;
4119 
4120  distance_type kcand = 0;
4121  for ( distance_type i = 0; i < nn; ++i )
4122  if ( a[i] > trial )
4123  {
4124  acand[kcand] = a[i];
4125  iwcand[kcand] = iw[i];
4126  ++kcand;
4127  }
4128  nn = kcand;
4129  wrest += wleft + wmid;
4130  }
4131 
4132  for ( distance_type i = 0; i < nn; ++i )
4133  {
4134  a[i] = acand[i];
4135  iw[i] = iwcand[i];
4136  }
4137  }
4138 }
4139 
4170 template <typename T> double Qn( T* __restrict__ x, T* __restrict__ xn )
4171 {
4172  distance_type n = xn - x;
4173  if ( n < 2 )
4174  return 0;
4175 
4176  double* y = new double[ n ];
4177  double* work = new double[ n ];
4178  double* acand = new double[ n ];
4179  distance_type* iwcand = new distance_type[ n ];
4180  distance_type* left = new distance_type[ n ];
4181  distance_type* right = new distance_type[ n ];
4182  distance_type* P = new distance_type[ n ];
4183  distance_type* Q = new distance_type[ n ];
4184  distance_type* weight = new distance_type[ n ];
4185 
4186  distance_type h = (n >> 1) + 1;
4187  distance_type k = (h*(h - 1)) >> 1;
4188  for ( distance_type i = 0; i < n; ++i )
4189  {
4190  y[i] = double( x[i] );
4191  left[i] = n - i + 1; // *
4192  right[i] = (i <= h) ? n : n - i + h; // N.B. The original code is "right[i] = n"
4193  }
4194 
4195  pcl::Sort( y, y+n );
4196 
4197  distance_type nL = (n*(n + 1)) >> 1;
4198  distance_type nR = n*n;
4199  distance_type knew = k + nL;
4200 
4201  bool found = false;
4202  double qn;
4203 
4204  while ( nR-nL > n )
4205  {
4206  distance_type j = 0; // *
4207  for ( distance_type i = 1; i < n; ++i ) // *
4208  if ( left[i] <= right[i] )
4209  {
4210  weight[j] = right[i] - left[i] + 1;
4211  work[j] = double( y[i] ) - y[n - left[i] - (weight[j] >> 1)];
4212  ++j;
4213  }
4214  qn = __pcl_whimed__( work, weight, j, acand, iwcand );
4215 
4216  for ( distance_type i = n-1, j = 0; i >= 0; --i ) // *
4217  {
4218  while ( j < n && double( y[i] ) - y[n-j-1] < qn )
4219  ++j;
4220  P[i] = j;
4221  }
4222 
4223  for ( distance_type i = 0, j = n+1; i < n; ++i ) // *
4224  {
4225  while ( double( y[i] ) - y[n-j+1] > qn )
4226  --j;
4227  Q[i] = j;
4228  }
4229 
4230  double sumP = 0;
4231  double sumQ = 0;
4232  for ( distance_type i = 0; i < n; ++i )
4233  {
4234  sumP += P[i];
4235  sumQ += Q[i] - 1;
4236  }
4237 
4238  if ( knew <= sumP )
4239  {
4240  for ( distance_type i = 0; i < n; ++i )
4241  right[i] = P[i];
4242  nR = sumP;
4243  }
4244  else if ( knew > sumQ )
4245  {
4246  for ( distance_type i = 0; i < n; ++i )
4247  left[i] = Q[i];
4248  nL = sumQ;
4249  }
4250  else
4251  {
4252  found = true;
4253  break;
4254  }
4255  }
4256 
4257  if ( !found )
4258  {
4259  distance_type j = 0;
4260  for ( distance_type i = 1; i < n; ++i )
4261  for ( distance_type jj = left[i]; jj <= right[i]; ++jj, ++j )
4262  work[j] = double( y[i] ) - y[n-jj]; // *
4263  qn = *pcl::Select( work, work+j, knew-nL-1 ); // *
4264  }
4265 
4266  /*
4267  * Correction for a finite sample
4268  */
4269  double dn;
4270  switch ( n )
4271  {
4272  case 2: dn = 0.399; break;
4273  case 3: dn = 0.994; break;
4274  case 4: dn = 0.512; break;
4275  case 5: dn = 0.844; break;
4276  case 6: dn = 0.611; break;
4277  case 7: dn = 0.857; break;
4278  case 8: dn = 0.669; break;
4279  case 9: dn = 0.872; break;
4280  default: dn = (n & 1) ? n/(n + 1.4) : n/(n + 3.8); break;
4281  }
4282  qn *= dn;
4283 
4284  delete [] y;
4285  delete [] work;
4286  delete [] acand;
4287  delete [] iwcand;
4288  delete [] left;
4289  delete [] right;
4290  delete [] P;
4291  delete [] Q;
4292  delete [] weight;
4293 
4294  return qn;
4295 }
4296 
4340 template <typename T>
4341 double BiweightMidvariance( const T* __restrict__ x, const T* __restrict__ xn, double center,
4342  double sigma, int k = 9, bool reducedLength = false ) noexcept
4343 {
4344  distance_type n = xn - x;
4345  if ( n < 2 )
4346  return 0;
4347 
4348  double kd = k * sigma;
4349  if ( kd < 0 || 1 + kd == 1 )
4350  return 0;
4351 
4352  double num = 0, den = 0;
4353  distance_type nr = 0;
4354  for ( ; x < xn; ++x )
4355  {
4356  double xc = double( *x ) - center;
4357  double y = xc/kd;
4358  if ( Abs( y ) < 1 )
4359  {
4360  double y2 = y*y;
4361  double y21 = 1 - y2;
4362  num += xc*xc * y21*y21*y21*y21;
4363  den += y21 * (1 - 5*y2);
4364  ++nr;
4365  }
4366  }
4367 
4368  den *= den;
4369  return (1 + den != 1) ? (reducedLength ? nr : n)*num/den : 0.0;
4370 }
4371 
4403 template <typename T>
4404 TwoSidedEstimate TwoSidedBiweightMidvariance( const T* __restrict__ x, const T* __restrict__ xn, double center,
4405  const TwoSidedEstimate& sigma, int k = 9, bool reducedLength = false ) noexcept
4406 {
4407  double kd0 = k * sigma.low;
4408  double kd1 = k * sigma.high;
4409  if ( kd0 < 0 || 1 + kd0 == 1 || kd1 < 0 || 1 + kd1 == 1 )
4410  return 0;
4411 
4412  double num0 = 0, den0 = 0, num1 = 0, den1 = 0;
4413  size_type n0 = 0, n1 = 0, nr0 = 0, nr1 = 0;
4414  for ( ; x < xn; ++x )
4415  {
4416  double xc = double( *x ) - center;
4417  bool low = xc <= 0;
4418  if ( low )
4419  ++n0;
4420  else
4421  ++n1;
4422 
4423  double y = xc/(low ? kd0 : kd1);
4424  if ( pcl::Abs( y ) < 1 )
4425  {
4426  double y2 = y*y;
4427  double y21 = 1 - y2;
4428  double num = xc*xc * y21*y21*y21*y21;
4429  double den = y21 * (1 - 5*y2);
4430  if ( low )
4431  {
4432  num0 += num;
4433  den0 += den;
4434  ++nr0;
4435  }
4436  else
4437  {
4438  num1 += num;
4439  den1 += den;
4440  ++nr1;
4441  }
4442  }
4443  }
4444 
4445  den0 *= den0;
4446  den1 *= den1;
4447  return { (n0 >= 2 && 1 + den0 != 1) ? (reducedLength ? nr0 : n0)*num0/den0 : 0.0,
4448  (n1 >= 2 && 1 + den1 != 1) ? (reducedLength ? nr1 : n1)*num1/den1 : 0.0 };
4449 }
4450 
4479 template <typename T>
4480 double BendMidvariance( const T* __restrict__ x, const T* __restrict__ xn, double center, double beta = 0.2 )
4481 {
4482  distance_type n = xn - x;
4483  if ( n < 2 )
4484  return 0;
4485 
4486  beta = Range( beta, 0.0, 0.5 );
4487  distance_type m = Floor( (1 - beta)*n + 0.5 );
4488 
4489  double* w = new double[ n ];
4490  for ( distance_type i = 0; i < n; ++i )
4491  w[i] = Abs( double( x[i] ) - center );
4492  double wb = *pcl::Select( w, w+n, m );
4493  delete [] w;
4494  if ( 1 + wb == 1 )
4495  return 0;
4496 
4497  double num = 0;
4498  distance_type den = 0;
4499  for ( ; x < xn; ++x )
4500  {
4501  double y = (double( *x ) - center)/wb;
4502  double f = Max( -1.0, Min( 1.0, y ) );
4503  num += f*f;
4504  if ( Abs( y ) < 1 )
4505  ++den;
4506  }
4507 
4508  den *= den;
4509  return (1 + den != 1) ? n*wb*wb*num/den : 0.0;
4510 }
4511 
4512 // ----------------------------------------------------------------------------
4513 
4540 inline double IncompleteBeta( double a, double b, double x, double eps = 1.0e-8 ) noexcept
4541 {
4542  if ( x < 0 || x > 1 )
4543  return std::numeric_limits<double>::infinity();
4544 
4545  /*
4546  * The continued fraction converges nicely for x < (a+1)/(a+b+2)
4547  */
4548  if ( x > (a + 1)/(a + b + 2) )
4549  return 1 - IncompleteBeta( b, a, 1 - x ); // Use the fact that beta is symmetric
4550 
4551  /*
4552  * Find the first part before the continued fraction.
4553  */
4554  double lbeta_ab = lgamma( a ) + lgamma( b ) - lgamma( a + b );
4555  double front = Exp( Ln( x )*a + Ln( 1 - x )*b - lbeta_ab )/a;
4556 
4557  /*
4558  * Use Lentz's algorithm to evaluate the continued fraction.
4559  */
4560  const double tiny = 1.0e-30;
4561  double f = 1, c = 1, d = 0;
4562  for ( int i = 0; i <= 200; ++i )
4563  {
4564  int m = i >> 1;
4565  double numerator;
4566  if ( i & 1 )
4567  numerator = -((a + m)*(a + b + m)*x)/((a + 2*m)*(a + 2*m + 1)); // Odd term
4568  else if ( i > 0 )
4569  numerator = (m*(b - m)*x)/((a + 2*m - 1)*(a + 2*m)); // Even term
4570  else
4571  numerator = 1; // First numerator is 1.0
4572 
4573  /*
4574  * Do an iteration of Lentz's algorithm.
4575  */
4576  d = 1 + numerator*d;
4577  if ( Abs( d ) < tiny )
4578  d = tiny;
4579  d = 1/d;
4580  c = 1 + numerator/c;
4581  if ( Abs( c ) < tiny )
4582  c = tiny;
4583  double cd = c*d;
4584  f *= cd;
4585  if ( Abs( 1 - cd ) < eps )
4586  return front*(f - 1);
4587  }
4588 
4589  // Needed more loops, did not converge.
4590  return std::numeric_limits<double>::infinity();
4591 }
4592 
4593 // ----------------------------------------------------------------------------
4594 
4599 template <typename T> inline constexpr T Erf( T x ) noexcept
4600 {
4601  return std::erf( x );
4602 }
4603 
4616 template <typename T> inline constexpr T ErfInv( T x ) noexcept
4617 {
4618  if ( x < -1 || x > 1 )
4619  return std::numeric_limits<T>::quiet_NaN();
4620  if ( x == 1 )
4621  return std::numeric_limits<T>::infinity();
4622  if ( x == -1 )
4623  return -std::numeric_limits<T>::infinity();
4624 
4625  const long double A0 = 1.1975323115670912564578e0L;
4626  const long double A1 = 4.7072688112383978012285e1L;
4627  const long double A2 = 6.9706266534389598238465e2L;
4628  const long double A3 = 4.8548868893843886794648e3L;
4629  const long double A4 = 1.6235862515167575384252e4L;
4630  const long double A5 = 2.3782041382114385731252e4L;
4631  const long double A6 = 1.1819493347062294404278e4L;
4632  const long double A7 = 8.8709406962545514830200e2L;
4633 
4634  const long double B0 = 1.0000000000000000000e0L;
4635  const long double B1 = 4.2313330701600911252e1L;
4636  const long double B2 = 6.8718700749205790830e2L;
4637  const long double B3 = 5.3941960214247511077e3L;
4638  const long double B4 = 2.1213794301586595867e4L;
4639  const long double B5 = 3.9307895800092710610e4L;
4640  const long double B6 = 2.8729085735721942674e4L;
4641  const long double B7 = 5.2264952788528545610e3L;
4642 
4643  const long double C0 = 1.42343711074968357734e0L;
4644  const long double C1 = 4.63033784615654529590e0L;
4645  const long double C2 = 5.76949722146069140550e0L;
4646  const long double C3 = 3.64784832476320460504e0L;
4647  const long double C4 = 1.27045825245236838258e0L;
4648  const long double C5 = 2.41780725177450611770e-1L;
4649  const long double C6 = 2.27238449892691845833e-2L;
4650  const long double C7 = 7.74545014278341407640e-4L;
4651 
4652  const long double D0 = 1.4142135623730950488016887e0L;
4653  const long double D1 = 2.9036514445419946173133295e0L;
4654  const long double D2 = 2.3707661626024532365971225e0L;
4655  const long double D3 = 9.7547832001787427186894837e-1L;
4656  const long double D4 = 2.0945065210512749128288442e-1L;
4657  const long double D5 = 2.1494160384252876777097297e-2L;
4658  const long double D6 = 7.7441459065157709165577218e-4L;
4659  const long double D7 = 1.4859850019840355905497876e-9L;
4660 
4661  const long double E0 = 6.65790464350110377720e0L;
4662  const long double E1 = 5.46378491116411436990e0L;
4663  const long double E2 = 1.78482653991729133580e0L;
4664  const long double E3 = 2.96560571828504891230e-1L;
4665  const long double E4 = 2.65321895265761230930e-2L;
4666  const long double E5 = 1.24266094738807843860e-3L;
4667  const long double E6 = 2.71155556874348757815e-5L;
4668  const long double E7 = 2.01033439929228813265e-7L;
4669 
4670  const long double F0 = 1.414213562373095048801689e0L;
4671  const long double F1 = 8.482908416595164588112026e-1L;
4672  const long double F2 = 1.936480946950659106176712e-1L;
4673  const long double F3 = 2.103693768272068968719679e-2L;
4674  const long double F4 = 1.112800997078859844711555e-3L;
4675  const long double F5 = 2.611088405080593625138020e-5L;
4676  const long double F6 = 2.010321207683943062279931e-7L;
4677  const long double F7 = 2.891024605872965461538222e-15L;
4678 
4679  long double abs_x = Abs( x );
4680 
4681  if ( abs_x <= 0.85L )
4682  {
4683  long double r = 0.180625L - 0.25L * x*x;
4684  long double num = (((((((A7*r + A6)*r + A5)*r + A4)*r + A3)*r + A2)*r + A1)*r + A0);
4685  long double den = (((((((B7*r + B6)*r + B5)*r + B4)*r + B3)*r + B2)*r + B1)*r + B0);
4686  return T( x * num/den );
4687  }
4688 
4689  long double r = Sqrt( Ln2() - Ln( 1 - abs_x ) );
4690  long double num = 0, den = 0;
4691  if ( r <= 5 )
4692  {
4693  r = r - 1.6L;
4694  num = (((((((C7*r + C6)*r + C5)*r + C4)*r + C3)*r + C2)*r + C1)*r + C0);
4695  den = (((((((D7*r + D6)*r + D5)*r + D4)*r + D3)*r + D2)*r + D1)*r + D0);
4696  }
4697  else
4698  {
4699  r = r - 5.0L;
4700  num = (((((((E7*r + E6)*r + E5)*r + E4)*r + E3)*r + E2)*r + E1)*r + E0);
4701  den = (((((((F7*r + F6)*r + F5)*r + F4)*r + F3)*r + F2)*r + F1)*r + F0);
4702  }
4703 
4704  return T( std::copysign( num/den, x ) );
4705 }
4706 
4707 // ----------------------------------------------------------------------------
4708 
4750 inline uint64 Hash64( const void* data, size_type size, uint64 seed = 0 ) noexcept
4751 {
4752 #define PRIME64_1 11400714785074694791ULL
4753 #define PRIME64_2 14029467366897019727ULL
4754 #define PRIME64_3 1609587929392839161ULL
4755 #define PRIME64_4 9650029242287828579ULL
4756 #define PRIME64_5 2870177450012600261ULL
4757 
4758  const uint8* p = (const uint8*)data;
4759  const uint8* end = p + size;
4760  uint64 h64;
4761 
4762  if ( seed == 0 )
4763  seed = size;
4764 
4765  if ( size >= 32 )
4766  {
4767  const uint8* limit = end - 32;
4768  uint64 v1 = seed + PRIME64_1 + PRIME64_2;
4769  uint64 v2 = seed + PRIME64_2;
4770  uint64 v3 = seed + 0;
4771  uint64 v4 = seed - PRIME64_1;
4772 
4773  do
4774  {
4775  v1 += *(uint64*)p * PRIME64_2;
4776  p += 8;
4777  v1 = RotL( v1, 31 );
4778  v1 *= PRIME64_1;
4779  v2 += *(uint64*)p * PRIME64_2;
4780  p += 8;
4781  v2 = RotL( v2, 31 );
4782  v2 *= PRIME64_1;
4783  v3 += *(uint64*)p * PRIME64_2;
4784  p += 8;
4785  v3 = RotL( v3, 31 );
4786  v3 *= PRIME64_1;
4787  v4 += *(uint64*)p * PRIME64_2;
4788  p += 8;
4789  v4 = RotL( v4, 31 );
4790  v4 *= PRIME64_1;
4791  }
4792  while ( p <= limit );
4793 
4794  h64 = RotL( v1, 1 ) + RotL( v2, 7 ) + RotL( v3, 12 ) + RotL( v4, 18 );
4795 
4796  v1 *= PRIME64_2;
4797  v1 = RotL( v1, 31 );
4798  v1 *= PRIME64_1;
4799  h64 ^= v1;
4800  h64 = h64 * PRIME64_1 + PRIME64_4;
4801 
4802  v2 *= PRIME64_2;
4803  v2 = RotL( v2, 31 );
4804  v2 *= PRIME64_1;
4805  h64 ^= v2;
4806  h64 = h64 * PRIME64_1 + PRIME64_4;
4807 
4808  v3 *= PRIME64_2;
4809  v3 = RotL( v3, 31 );
4810  v3 *= PRIME64_1;
4811  h64 ^= v3;
4812  h64 = h64 * PRIME64_1 + PRIME64_4;
4813 
4814  v4 *= PRIME64_2;
4815  v4 = RotL( v4, 31 );
4816  v4 *= PRIME64_1;
4817  h64 ^= v4;
4818  h64 = h64 * PRIME64_1 + PRIME64_4;
4819  }
4820  else
4821  {
4822  h64 = seed + PRIME64_5;
4823  }
4824 
4825  h64 += size;
4826 
4827  while ( p+8 <= end )
4828  {
4829  uint64 k1 = *(uint64*)p;
4830  k1 *= PRIME64_2;
4831  k1 = RotL( k1, 31 );
4832  k1 *= PRIME64_1;
4833  h64 ^= k1;
4834  h64 = RotL( h64, 27 ) * PRIME64_1 + PRIME64_4;
4835  p += 8;
4836  }
4837 
4838  if ( p+4 <= end )
4839  {
4840  h64 ^= (uint64)(*(uint32*)p) * PRIME64_1;
4841  h64 = RotL( h64, 23 ) * PRIME64_2 + PRIME64_3;
4842  p += 4;
4843  }
4844 
4845  while ( p < end )
4846  {
4847  h64 ^= *p * PRIME64_5;
4848  h64 = RotL( h64, 11 ) * PRIME64_1;
4849  ++p;
4850  }
4851 
4852  h64 ^= h64 >> 33;
4853  h64 *= PRIME64_2;
4854  h64 ^= h64 >> 29;
4855  h64 *= PRIME64_3;
4856  h64 ^= h64 >> 32;
4857 
4858  return h64;
4859 
4860 #undef PRIME64_1
4861 #undef PRIME64_2
4862 #undef PRIME64_3
4863 #undef PRIME64_4
4864 #undef PRIME64_5
4865 }
4866 
4904 inline uint32 Hash32( const void* data, size_type size, uint32 seed = 0 ) noexcept
4905 {
4906 #define PRIME32_1 2654435761U
4907 #define PRIME32_2 2246822519U
4908 #define PRIME32_3 3266489917U
4909 #define PRIME32_4 668265263U
4910 #define PRIME32_5 374761393U
4911 
4912  const uint8* p = (const uint8*)data;
4913  const uint8* end = p + size;
4914  uint32 h32;
4915 
4916  if ( seed == 0 )
4917  seed = uint32( size );
4918 
4919  if ( size >= 16 )
4920  {
4921  const uint8* limit = end - 16;
4922  uint32 v1 = seed + PRIME32_1 + PRIME32_2;
4923  uint32 v2 = seed + PRIME32_2;
4924  uint32 v3 = seed + 0;
4925  uint32 v4 = seed - PRIME32_1;
4926 
4927  do
4928  {
4929  v1 += *(uint32*)p * PRIME32_2;
4930  v1 = RotL( v1, 13 );
4931  v1 *= PRIME32_1;
4932  p += 4;
4933  v2 += *(uint32*)p * PRIME32_2;
4934  v2 = RotL( v2, 13 );
4935  v2 *= PRIME32_1;
4936  p += 4;
4937  v3 += *(uint32*)p * PRIME32_2;
4938  v3 = RotL( v3, 13 );
4939  v3 *= PRIME32_1;
4940  p += 4;
4941  v4 += *(uint32*)p * PRIME32_2;
4942  v4 = RotL( v4, 13 );
4943  v4 *= PRIME32_1;
4944  p += 4;
4945  }
4946  while ( p <= limit );
4947 
4948  h32 = RotL( v1, 1 ) + RotL( v2, 7 ) + RotL( v3, 12 ) + RotL( v4, 18 );
4949  }
4950  else
4951  {
4952  h32 = seed + PRIME32_5;
4953  }
4954 
4955  h32 += (uint32)size;
4956 
4957  while ( p+4 <= end )
4958  {
4959  h32 += *(uint32*)p * PRIME32_3;
4960  h32 = RotL( h32, 17 ) * PRIME32_4 ;
4961  p+=4;
4962  }
4963 
4964  while ( p < end )
4965  {
4966  h32 += *p * PRIME32_5;
4967  h32 = RotL( h32, 11 ) * PRIME32_1 ;
4968  ++p;
4969  }
4970 
4971  h32 ^= h32 >> 15;
4972  h32 *= PRIME32_2;
4973  h32 ^= h32 >> 13;
4974  h32 *= PRIME32_3;
4975  h32 ^= h32 >> 16;
4976 
4977  return h32;
4978 
4979 #undef PRIME32_1
4980 #undef PRIME32_2
4981 #undef PRIME32_3
4982 #undef PRIME32_4
4983 #undef PRIME32_5
4984 }
4985 
4986 // ----------------------------------------------------------------------------
4987 
4988 } // pcl
4989 
4990 #endif // __PCL_Math_h
4991 
4992 // ----------------------------------------------------------------------------
4993 // EOF pcl/Math.h - Released 2024-01-13T15:47:58Z
pcl::Norm2Pi
constexpr T Norm2Pi(T x) noexcept
Definition: Math.h:2004
pcl::L1Norm
T L1Norm(const T *i, const T *j) noexcept
Definition: Math.h:2194
pcl::Select
RI Select(RI i, RI j, distance_type k)
Definition: Selection.h:165
pcl::SexagesimalToDecimal
double SexagesimalToDecimal(int sign, const S1 &s1, const S2 &s2=S2(0), const S3 &s3=S3(0)) noexcept
Definition: Math.h:2547
pcl::RotL
T RotL(T x, uint32 n) noexcept
Definition: Math.h:1330
pcl::Sinh
Complex< T > Sinh(const Complex< T > &c) noexcept
Definition: Complex.h:827
pcl
PCL root namespace.
Definition: AbstractImage.h:76
pcl::Norm
T Norm(const T *i, const T *j, const P &p) noexcept
Definition: Math.h:2135
pcl::RoundI64
int64 RoundI64(double x) noexcept
Definition: Math.h:1654
pcl::AvgDev
double AvgDev(const T *__restrict__ i, const T *__restrict__ j, double center) noexcept
Definition: Math.h:3404
pcl::IncompleteBeta
double IncompleteBeta(double a, double b, double x, double eps=1.0e-8) noexcept
Definition: Math.h:4540
pcl::Floor
constexpr T Floor(T x) noexcept
Definition: Math.h:628
Selection.h
pcl::CalendarTimeToJD
void PCL_FUNC CalendarTimeToJD(int &jdi, double &jdf, int year, int month, int day, double dayf=0) noexcept
pcl::Range
constexpr const T & Range(const T &x, const T &a, const T &b) noexcept
Definition: Utility.h:190
pcl::ArcTanh
constexpr T ArcTanh(T x) noexcept
Definition: Math.h:1863
pcl::TrimmedMeanOfSquaresDestructive
double TrimmedMeanOfSquaresDestructive(T *__restrict__ i, T *__restrict__ j, distance_type l=1, distance_type h=1) noexcept
Definition: Math.h:3362
pcl::TwoSidedEstimate::operator/=
TwoSidedEstimate & operator/=(double x) noexcept
Definition: Math.h:3634
pcl::Sign
constexpr int Sign(T x) noexcept
Definition: Math.h:951
pcl::StableSum
double StableSum(const T *__restrict__ i, const T *__restrict__ j) noexcept
Definition: Math.h:2592
pcl::TwoSidedEstimate::low
double low
Low estimate component.
Definition: Math.h:3529
pcl::Modulus
double Modulus(const T *__restrict__ i, const T *__restrict__ j) noexcept
Definition: Math.h:2617
pcl::int16
signed short int16
Definition: Defs.h:651
pcl::ArcHav
constexpr T ArcHav(T x) noexcept
Definition: Math.h:1883
ItemsInArray
#define ItemsInArray(a)
Definition: Utility.h:223
pcl::TwoSidedEstimate::high
double high
High estimate component.
Definition: Math.h:3530
pcl::Frac
constexpr T Frac(T x) noexcept
Definition: Math.h:640
pcl::Cos
Complex< T > Cos(const Complex< T > &c) noexcept
Definition: Complex.h:806
pcl::TwoSidedAvgDev
TwoSidedEstimate TwoSidedAvgDev(const T *__restrict__ i, const T *__restrict__ j, double center) noexcept
Definition: Math.h:3717
pcl::ErfInv
constexpr T ErfInv(T x) noexcept
Definition: Math.h:4616
pcl::Mean
double Mean(const T *__restrict__ i, const T *__restrict__ j) noexcept
Definition: Math.h:2709
pcl::RoundInt64
int64 RoundInt64(double x) noexcept
Definition: Math.h:1620
Utility.h
pcl::PowI8
T PowI8(T x) noexcept
Definition: Math.h:1795
pcl::JDToCalendarTime
void PCL_FUNC JDToCalendarTime(int &year, int &month, int &day, double &dayf, int jdi, double jdf) noexcept
pcl::Max
constexpr const T & Max(const T &a, const T &b) noexcept
Definition: Utility.h:119
pcl::RoundInt
int RoundInt(T x) noexcept
Definition: Math.h:1503
pcl::PowI4
T PowI4(T x) noexcept
Definition: Math.h:1777
pcl::Hash64
uint64 Hash64(const void *data, size_type size, uint64 seed=0) noexcept
Definition: Math.h:4750
pcl::StdDev
double StdDev(const T *__restrict__ i, const T *__restrict__ j, double center) noexcept
Definition: Math.h:2813
pcl::Round
Complex< T > Round(const Complex< T > &c) noexcept
Definition: Complex.h:938
pcl::LogN
constexpr T LogN(T n, T x) noexcept
Definition: Math.h:875
pcl::RoundIntArithmetic
int RoundIntArithmetic(T x) noexcept
Definition: Math.h:1586
pcl::Qn
double Qn(T *__restrict__ x, T *__restrict__ xn)
Definition: Math.h:4170
pcl::Angle
constexpr T Angle(int d, T m) noexcept
Definition: Math.h:481
pcl::RoundInt64Arithmetic
int64 RoundInt64Arithmetic(double x) noexcept
Definition: Math.h:1683
pcl::Ln
Complex< T > Ln(const Complex< T > &c) noexcept
Definition: Complex.h:725
pcl::TruncI
int TruncI(T x) noexcept
Definition: Math.h:1154
pcl::uint32
unsigned int uint32
Definition: Defs.h:669
pcl::Mod
constexpr T Mod(T x, T y) noexcept
Definition: Math.h:887
pcl::int8
signed char int8
Definition: Defs.h:639
pcl::uint16
unsigned short uint16
Definition: Defs.h:657
pcl::Factorial
double Factorial(int n) noexcept
Definition: Math.h:712
pcl::IsInfinity
int IsInfinity(float x) noexcept
Definition: Math.h:216
pcl::IsFinite
bool IsFinite(float x) noexcept
Definition: Math.h:178
pcl::ArcCosh
constexpr T ArcCosh(T x) noexcept
Definition: Math.h:1845
pcl::TwoSidedMAD
TwoSidedEstimate TwoSidedMAD(const T *__restrict__ i, const T *__restrict__ j, double center)
Definition: Math.h:3837
pcl::TrimmedMeanOfSquares
double TrimmedMeanOfSquares(const T *__restrict__ i, const T *__restrict__ j, distance_type l=1, distance_type h=1)
Definition: Math.h:3319
pcl::PowI6
T PowI6(T x) noexcept
Definition: Math.h:1786
pcl::RotR
T RotR(T x, uint32 n) noexcept
Definition: Math.h:1351
pcl::StableSumOfSquares
double StableSumOfSquares(const T *__restrict__ i, const T *__restrict__ j) noexcept
Definition: Math.h:2683
pcl::Ln2
constexpr long double Ln2() noexcept
Definition: Math.h:799
pcl::Pow
Complex< T1 > Pow(const Complex< T1 > &c, T2 x) noexcept
Definition: Complex.h:747
pcl::Frexp
void Frexp(T x, T &m, int &p) noexcept
Definition: Math.h:653
pcl::Log2e
constexpr long double Log2e() noexcept
Definition: Math.h:849
pcl::size_type
size_t size_type
Definition: Defs.h:612
pcl::Tanh
Complex< T > Tanh(const Complex< T > &c) noexcept
Definition: Complex.h:849
pcl::Sn
double Sn(T *__restrict__ x, T *__restrict__ xn)
Definition: Math.h:3924
pcl::StableAvgDev
double StableAvgDev(const T *__restrict__ i, const T *__restrict__ j, double center) noexcept
Definition: Math.h:3436
pcl::L2Norm
T L2Norm(const T *i, const T *j) noexcept
Definition: Math.h:2253
pcl::ModPi
constexpr T ModPi(T x) noexcept
Definition: Math.h:1980
pcl::TwoPi
constexpr long double TwoPi() noexcept
Definition: Math.h:470
pcl::uint64
unsigned long long uint64
Definition: Defs.h:685
pcl::ArcTan
constexpr T ArcTan(T x) noexcept
Definition: Math.h:526
pcl::MinRad
constexpr T MinRad(T x) noexcept
Definition: Math.h:1927
pcl::LnFactorial
double LnFactorial(int n) noexcept
Definition: Math.h:732
pcl::uint8
unsigned char uint8
Definition: Defs.h:645
pcl::distance_type
ptrdiff_t distance_type
Definition: Defs.h:618
pcl::TwoSidedEstimate::TwoSidedEstimate
TwoSidedEstimate(const T &x)
Definition: Math.h:3572
pcl::MasRad
constexpr T MasRad(T x) noexcept
Definition: Math.h:1958
pcl::Tan
Complex< T > Tan(const Complex< T > &c) noexcept
Definition: Complex.h:817
pcl::SinCos
void SinCos(T x, T &sx, T &cx) noexcept
Definition: Math.h:1030
pcl::Sum
double Sum(const T *__restrict__ i, const T *__restrict__ j) noexcept
Definition: Math.h:2573
pcl::Erf
constexpr T Erf(T x) noexcept
Definition: Math.h:4599
pcl::TruncInt64
int64 TruncInt64(T x) noexcept
Definition: Math.h:1209
pcl::PowI12
T PowI12(T x) noexcept
Definition: Math.h:1813
pcl::Median
double Median(const T *__restrict__ i, const T *__restrict__ j)
Definition: Math.h:2878
pcl::Sqrt
Complex< T > Sqrt(const Complex< T > &c) noexcept
Definition: Complex.h:674
pcl::ArcSin
constexpr T ArcSin(T x) noexcept
Definition: Math.h:514
pcl::Mod2Pi
constexpr T Mod2Pi(T x) noexcept
Definition: Math.h:1993
pcl::Log2
constexpr long double Log2() noexcept
Definition: Math.h:822
pcl::Hav
constexpr T Hav(T x) noexcept
Definition: Math.h:669
pcl::Pow10I
Exponential function 10**n, n being a signed integer.
Definition: Math.h:1269
pcl::TruncI64
int64 TruncI64(T x) noexcept
Definition: Math.h:1231
pcl::DecimalToSexagesimal
void DecimalToSexagesimal(int &sign, S1 &s1, S2 &s2, S3 &s3, const D &d) noexcept
Definition: Math.h:2525
pcl::OrderStatistic
double OrderStatistic(const T *__restrict__ i, const T *__restrict__ j, distance_type k)
Definition: Math.h:3138
pcl::OrderStatisticDestructive
double OrderStatisticDestructive(T *__restrict__ i, T *__restrict__ j, distance_type k) noexcept
Definition: Math.h:3197
pcl::PowI10
T PowI10(T x) noexcept
Definition: Math.h:1804
pcl::TwoSidedEstimate::Mean
double Mean() const noexcept
Definition: Math.h:3601
pcl::TwoSidedEstimate::operator=
TwoSidedEstimate & operator=(const TwoSidedEstimate &)=default
pcl::Pow10
T Pow10(T x) noexcept
Definition: Math.h:1312
pcl::Hash32
uint32 Hash32(const void *data, size_type size, uint32 seed=0) noexcept
Definition: Math.h:4904
pcl::BendMidvariance
double BendMidvariance(const T *__restrict__ x, const T *__restrict__ xn, double center, double beta=0.2)
Definition: Math.h:4480
pcl::Cotan
constexpr T Cotan(T x) noexcept
Definition: Math.h:595
pcl::TwoSidedBiweightMidvariance
TwoSidedEstimate TwoSidedBiweightMidvariance(const T *__restrict__ x, const T *__restrict__ xn, double center, const TwoSidedEstimate &sigma, int k=9, bool reducedLength=false) noexcept
Definition: Math.h:4404
pcl::Deg
constexpr T Deg(T x) noexcept
Definition: Math.h:606
pcl::TwoSidedEstimate::TwoSidedEstimate
TwoSidedEstimate(const T1 &l, const T2 &h)
Definition: Math.h:3561
pcl::IsNegativeZero
bool IsNegativeZero(float x) noexcept
Definition: Math.h:231
pcl::ArcSinh
constexpr T ArcSinh(T x) noexcept
Definition: Math.h:1827
pcl::IsNaN
bool IsNaN(float x) noexcept
Definition: Math.h:194
pcl::SecRad
constexpr T SecRad(T x) noexcept
Definition: Math.h:1938
pcl::Cosh
Complex< T > Cosh(const Complex< T > &c) noexcept
Definition: Complex.h:838
pcl::StableModulus
double StableModulus(const T *__restrict__ i, const T *__restrict__ j) noexcept
Definition: Math.h:2636
pcl::Min
constexpr const T & Min(const T &a, const T &b) noexcept
Definition: Utility.h:90
pcl::Pow2I
Exponential function 2**n, n being a signed integer.
Definition: Math.h:1736
pcl::RoundIntBanker
int RoundIntBanker(T x) noexcept
Definition: Math.h:1557
pcl::Sort
void Sort(BI i, BI j)
Definition: Sort.h:520
pcl::TwoSidedEstimate::Total
double Total() const noexcept
Definition: Math.h:3592
pcl::int32
signed int int32
Definition: Defs.h:663
pcl::Log2T
constexpr long double Log2T() noexcept
Definition: Math.h:862
pcl::Split
void Split(T x, T &i, T &f) noexcept
Definition: Math.h:1051
int64_max
#define int64_max
Definition: Defs.h:891
pcl::StableMean
double StableMean(const T *__restrict__ i, const T *__restrict__ j) noexcept
Definition: Math.h:2728
pcl::TwoSidedEstimate::operator*=
TwoSidedEstimate & operator*=(double x) noexcept
Definition: Math.h:3624
pcl::Pi
constexpr long double Pi() noexcept
Definition: Math.h:461
Sort.h
pcl::Exp
Complex< T > Exp(const Complex< T > &c) noexcept
Definition: Complex.h:714
pcl::TwoSidedEstimate
Two-sided descriptive statistical estimate.
Definition: Math.h:3527
pcl::SumOfSquares
double SumOfSquares(const T *__restrict__ i, const T *__restrict__ j) noexcept
Definition: Math.h:2661
pcl::Fact
Factorial function.
Definition: Math.h:760
pcl::MaxSSEInstructionSetSupported
int MaxSSEInstructionSetSupported() noexcept
Definition: Math.h:127
pcl::PowI
T PowI(T x, int n) noexcept
Definition: Math.h:1755
pcl::TruncInt
int TruncInt(T x) noexcept
Definition: Math.h:1132
pcl::SignChar
constexpr char SignChar(T x) noexcept
Definition: Math.h:969
pcl::TwoSidedEstimate::TwoSidedEstimate
TwoSidedEstimate()=default
pcl::MAD
double MAD(const T *__restrict__ i, const T *__restrict__ j, double center)
Definition: Math.h:3776
pcl::ArcTan2Pi
T ArcTan2Pi(T y, T x) noexcept
Definition: Math.h:548
pcl::Rad
constexpr T Rad(T x) noexcept
Definition: Math.h:1894
pcl::RoundI
int RoundI(T x) noexcept
Definition: Math.h:1544
pcl::TrimmedMean
double TrimmedMean(const T *__restrict__ i, const T *__restrict__ j, distance_type l=1, distance_type h=1)
Definition: Math.h:3242
pcl::Sin
Complex< T > Sin(const Complex< T > &c) noexcept
Definition: Complex.h:795
pcl::Abs
T Abs(const Complex< T > &c) noexcept
Definition: Complex.h:429
pcl::Pow2
constexpr T Pow2(T x) noexcept
Definition: Math.h:1717
pcl::int64
signed long long int64
Definition: Defs.h:679
pcl::Log
Complex< T > Log(const Complex< T > &c) noexcept
Definition: Complex.h:735
Defs.h
pcl::TrimmedMeanDestructive
double TrimmedMeanDestructive(T *__restrict__ i, T *__restrict__ j, distance_type l=1, distance_type h=1) noexcept
Definition: Math.h:3281
pcl::BiweightMidvariance
double BiweightMidvariance(const T *__restrict__ x, const T *__restrict__ xn, double center, double sigma, int k=9, bool reducedLength=false) noexcept
Definition: Math.h:4341
pcl::UasRad
constexpr T UasRad(T x) noexcept
Definition: Math.h:1969
pcl::ArcCos
constexpr T ArcCos(T x) noexcept
Definition: Math.h:502
pcl::MedianDestructive
double MedianDestructive(T *__restrict__ i, T *__restrict__ j) noexcept
Definition: Math.h:2960
pcl::Trunc
T Trunc(T x) noexcept
Definition: Math.h:1095
pcl::RadMin
constexpr T RadMin(T x) noexcept
Definition: Math.h:1905
pcl::TwoSidedEstimate::IsValid
bool IsValid() const noexcept
Definition: Math.h:3583
pcl::TwoSidedEstimate::operator*
TwoSidedEstimate operator*(double x) const noexcept
Definition: Math.h:3655
pcl::Poly
T Poly(T x, C c, int n) noexcept
Definition: Math.h:908
pcl::Ceil
constexpr T Ceil(T x) noexcept
Definition: Math.h:562
pcl::Ldexp
constexpr T Ldexp(T m, int p) noexcept
Definition: Math.h:680
int64_min
#define int64_min
Definition: Defs.h:885
pcl::AsRad
constexpr T AsRad(T x) noexcept
Definition: Math.h:1947
pcl::RadSec
constexpr T RadSec(T x) noexcept
Definition: Math.h:1916
pcl::Variance
double Variance(const T *__restrict__ i, const T *__restrict__ j, double center) noexcept
Definition: Math.h:2753
pcl::TwoSidedEstimate::operator/
TwoSidedEstimate operator/(double x) const noexcept
Definition: Math.h:3663
pcl::Rotate
void Rotate(T &x, T &y, T1 sa, T1 ca, T2 xc, T2 yc) noexcept
Definition: Math.h:2024