PCL
Math.h
Go to the documentation of this file.
1 // ____ ______ __
2 // / __ \ / ____// /
3 // / /_/ // / / /
4 // / ____// /___ / /___ PixInsight Class Library
5 // /_/ \____//_____/ PCL 2.8.5
6 // ----------------------------------------------------------------------------
7 // pcl/Math.h - Released 2024-12-28T16:53:48Z
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 __PCL_AVX2
70 # ifdef __PCL_WINDOWS
71 # include <emmintrin.h>
72 # include <intrin.h>
73 # else
74 # include <xmmintrin.h>
75 # include <immintrin.h>
76 # endif
77 # define __PCL_HAVE_SSE2 1
78 #else
79 # ifdef _MSC_VER
80 # include <intrin.h> // for __cpuid()
81 # endif
82 # if defined( __x86_64__ ) || defined( _M_X64 )
83 # define __PCL_HAVE_SSE2 1
84 # include <emmintrin.h>
85 # endif
86 #endif
87 
88 /*
89  * sincos() is only available as a GNU extension:
90  *
91  * http://man7.org/linux/man-pages/man3/sincos.3.html
92  *
93  * Unfortunately, it is not part of the C++ standard library because of the
94  * anachronic dependency on errno.
95  */
96 #if !defined( _MSC_VER ) && !defined( __clang__ ) && defined( __GNUC__ )
97 # define __PCL_HAVE_SINCOS 1
98 #endif
99 
100 #ifdef __PCL_QT_INTERFACE
101 # include <QtWidgets/QtWidgets>
102 #endif
103 
104 /*
105  * Number of histogram bins used by fast histogram-based median calculation
106  * algorithm implementations.
107  */
108 #define __PCL_MEDIAN_HISTOGRAM_LENGTH 8192
109 
110 /*
111  * The minimum array length for efficient fast histogram generation. This is an
112  * empirical value to be replaced by automatic benchmarks.
113  */
114 #define __PCL_MEDIAN_HISTOGRAM_OVERHEAD (64*1024)
115 
116 namespace pcl
117 {
118 
119 // ----------------------------------------------------------------------------
120 
143 inline int MaxSSEInstructionSetSupported() noexcept
144 {
145  int32 edxFlags = 0;
146  int32 ecxFlags = 0;
147 
148 #ifdef _MSC_VER
149  int cpuInfo[ 4 ];
150  __cpuid( cpuInfo, 1 );
151  edxFlags = cpuInfo[3];
152  ecxFlags = cpuInfo[2];
153 #else
154  asm volatile( "mov $0x00000001, %%eax\n\t"
155  "cpuid\n\t"
156  "mov %%edx, %0\n\t"
157  "mov %%ecx, %1\n"
158  : "=r" (edxFlags), "=r" (ecxFlags) // output operands
159  : // input operands
160  : "%eax", "%ebx", "%ecx", "%edx" ); // clobbered registers
161 #endif
162 
163  if ( ecxFlags & (1u << 20) ) // SSE4.2
164  return 42;
165  if ( ecxFlags & (1u << 19) ) // SSE4.1
166  return 41;
167  if ( ecxFlags & 1u ) // SSE3
168  return 3;
169  if ( edxFlags & (1u << 26) ) // SSE2
170  return 2;
171  if ( edxFlags & (1u << 25) ) // SSE
172  return 1;
173  return 0;
174 }
175 
176 // ----------------------------------------------------------------------------
177 
182 #define __PCL_FLOAT_SGNMASK 0x80000000
183 #define __PCL_FLOAT_EXPMASK 0x7f800000
184 #define __PCL_FLOAT_SIGMASK 0x007fffff
185 
194 inline bool IsFinite( float x ) noexcept
195 {
196  union { float f; uint32 u; } v = { x };
197  return (v.u & __PCL_FLOAT_EXPMASK) != __PCL_FLOAT_EXPMASK;
198 }
199 
210 inline bool IsNaN( float x ) noexcept
211 {
212  union { float f; uint32 u; } v = { x };
213  return (v.u & __PCL_FLOAT_EXPMASK) == __PCL_FLOAT_EXPMASK &&
214  (v.u & __PCL_FLOAT_SIGMASK) != 0;
215 }
216 
232 inline int IsInfinity( float x ) noexcept
233 {
234  union { float f; uint32 u; } v = { x };
235  if ( (v.u & __PCL_FLOAT_EXPMASK) == __PCL_FLOAT_EXPMASK &&
236  (v.u & __PCL_FLOAT_SIGMASK) == 0 )
237  return ((v.u & __PCL_FLOAT_SGNMASK) == 0) ? +1 : -1;
238  return 0;
239 }
240 
247 inline bool IsNegativeZero( float x ) noexcept
248 {
249  union { float f; uint32 u; } v = { x };
250  return v.u == __PCL_FLOAT_SGNMASK;
251 }
252 
253 #define __PCL_DOUBLE_SGNMASK 0x80000000
254 #define __PCL_DOUBLE_EXPMASK 0x7ff00000
255 #define __PCL_DOUBLE_SIGMASK 0x000fffff
256 
265 inline bool IsFinite( 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 }
270 
281 inline bool IsNaN( double x ) noexcept
282 {
283  union { double d; uint32 u[2]; } v = { x };
284  return (v.u[1] & __PCL_DOUBLE_EXPMASK) == __PCL_DOUBLE_EXPMASK &&
285  (v.u[0] != 0 || (v.u[1] & __PCL_DOUBLE_SIGMASK) != 0);
286 }
287 
303 inline int IsInfinity( double x ) noexcept
304 {
305  union { double d; uint32 u[2]; } v = { x };
306  if ( v.u[0] == 0 &&
307  (v.u[1] & __PCL_DOUBLE_SIGMASK) == 0 &&
308  (v.u[1] & __PCL_DOUBLE_EXPMASK) == __PCL_DOUBLE_EXPMASK )
309  return ((v.u[1] & __PCL_DOUBLE_SGNMASK) == 0) ? +1 : -1;
310  return 0;
311 }
312 
319 inline bool IsNegativeZero( double x ) noexcept
320 {
321  union { double d; uint32 u[2]; } v = { x };
322  return v.u[1] == __PCL_DOUBLE_SGNMASK &&
323  v.u[0] == 0;
324 }
325 
326 // ----------------------------------------------------------------------------
327 
328 #ifdef __PCL_AVX2
329 
330 /*
331  * Efficient horizontal SSE/AVX vector sum routines.
332  *
333  * See StackOverflow articles:
334  *
335  * Fastest way to do horizontal SSE vector sum (or other reduction)
336  * https://stackoverflow.com/questions/6996764/
337  * fastest-way-to-do-horizontal-sse-vector-sum-or-other-reduction
338  *
339  * Get sum of values stored in __m256d with SSE/AVX
340  * https://stackoverflow.com/questions/49941645/
341  * get-sum-of-values-stored-in-m256d-with-sse-avx
342  */
343 
344 inline float __pcl_hsum128_ps( __m128 v )
345 {
346  __m128 shuf = _mm_movehdup_ps( v ); // broadcast elements 3,1 to 2,0
347  __m128 sums = _mm_add_ps( v, shuf );
348  shuf = _mm_movehl_ps( shuf, sums );
349  sums = _mm_add_ss( sums, shuf );
350  return _mm_cvtss_f32( sums );
351 }
352 
353 inline float __pcl_hsum256_ps( __m256 v )
354 {
355  __m128 vlow = _mm256_castps256_ps128( v );
356  __m128 vhigh = _mm256_extractf128_ps( v, 1 );
357  vlow = _mm_add_ps( vlow, vhigh );
358  return __pcl_hsum128_ps( vlow );
359 }
360 
361 inline double __pcl_hsum128_pd( __m128d v )
362 {
363  __m128 undef = _mm_undefined_ps();
364  __m128 shuftmp = _mm_movehl_ps( undef, _mm_castpd_ps( v ) ); // there is no movhl_pd
365  __m128d shuf = _mm_castps_pd( shuftmp );
366  return _mm_cvtsd_f64( _mm_add_sd( v, shuf ) );
367 }
368 
369 inline double __pcl_hsum256_pd( __m256d v )
370 {
371  __m128d vlow = _mm256_castpd256_pd128( v );
372  __m128d vhigh = _mm256_extractf128_pd( v, 1 );
373  vlow = _mm_add_pd( vlow, vhigh );
374  __m128d high64 = _mm_unpackhi_pd( vlow, vlow );
375  return _mm_cvtsd_f64( _mm_add_sd( vlow, high64 ) );
376 }
377 
378 #endif
379 
380 // ----------------------------------------------------------------------------
381 
390 inline float Abs( float x ) noexcept
391 {
392  return std::fabs( x );
393 }
394 
398 inline double Abs( double x ) noexcept
399 {
400  return std::fabs( x );
401 }
402 
406 inline long double Abs( long double x ) noexcept
407 {
408  return std::fabs( x );
409 }
410 
414 inline signed int Abs( signed int x ) noexcept
415 {
416  return ::abs( x );
417 }
418 
422 #if defined( __PCL_MACOSX ) && defined( __clang__ ) // turn off warning due to broken cstdlib in Xcode
423 _Pragma("clang diagnostic push")
424 _Pragma("clang diagnostic ignored \"-Wabsolute-value\"")
425 #endif
426 inline signed long Abs( signed long x ) noexcept
427 {
428  return ::abs( x );
429 }
430 #if defined( __PCL_MACOSX ) && defined( __clang__ )
431 _Pragma("clang diagnostic pop")
432 #endif
433 
437 #if defined( _MSC_VER )
438 inline __int64 Abs( __int64 x ) noexcept
439 {
440  return (x < 0) ? -x : +x;
441 }
442 #elif defined( __PCL_MACOSX ) && defined( __clang__ )
443 inline constexpr signed long long Abs( signed long long x ) noexcept
444 {
445  return (x < 0) ? -x : +x;
446 }
447 #else
448 inline signed long long Abs( signed long long x ) noexcept
449 {
450  return ::abs( x );
451 }
452 #endif
453 
457 inline signed short Abs( signed short x ) noexcept
458 {
459  return (signed short)::abs( int( x ) );
460 }
461 
465 inline signed char Abs( signed char x ) noexcept
466 {
467  return (signed char)::abs( int( x ) );
468 }
469 
473 inline wchar_t Abs( wchar_t x ) noexcept
474 {
475  return (wchar_t)::abs( int( x ) );
476 }
477 
481 inline constexpr unsigned int Abs( unsigned int x ) noexcept
482 {
483  return x;
484 }
485 
489 inline constexpr unsigned long Abs( unsigned long x ) noexcept
490 {
491  return x;
492 }
493 
497 #ifdef _MSC_VER
498 inline unsigned __int64 Abs( unsigned __int64 x ) noexcept
499 {
500  return x;
501 }
502 #else
503 inline constexpr unsigned long long Abs( unsigned long long x ) noexcept
504 {
505  return x;
506 }
507 #endif
508 
512 inline constexpr unsigned short Abs( unsigned short x ) noexcept
513 {
514  return x;
515 }
516 
520 inline constexpr unsigned char Abs( unsigned char x ) noexcept
521 {
522  return x;
523 }
524 
525 // ----------------------------------------------------------------------------
526 
531 inline constexpr long double Pi() noexcept
532 {
533  return (long double)( 0.31415926535897932384626433832795029e+01L );
534 }
535 
540 inline constexpr long double TwoPi() noexcept
541 {
542  return (long double)( 0.62831853071795864769252867665590058e+01L );
543 }
544 
545 // ----------------------------------------------------------------------------
546 
552 template <typename T> inline constexpr T Angle( int d, T m ) noexcept
553 {
554  return d + m/60;
555 }
556 
562 template <typename T> inline constexpr T Angle( int d, int m, T s ) noexcept
563 {
564  return Angle( d, m + s/60 );
565 }
566 
567 // ----------------------------------------------------------------------------
568 
573 template <typename T> inline constexpr T ArcCos( T x ) noexcept
574 {
575  PCL_PRECONDITION( T( -1 ) <= x && x <= T( 1 ) )
576  return std::acos( x );
577 }
578 
579 // ----------------------------------------------------------------------------
580 
585 template <typename T> inline constexpr T ArcSin( T x ) noexcept
586 {
587  PCL_PRECONDITION( T( -1 ) <= x && x <= T( 1 ) )
588  return std::asin( x );
589 }
590 
591 // ----------------------------------------------------------------------------
592 
597 template <typename T> inline constexpr T ArcTan( T x ) noexcept
598 {
599  return std::atan( x );
600 }
601 
602 // ----------------------------------------------------------------------------
603 
608 template <typename T> inline constexpr T ArcTan( T y, T x ) noexcept
609 {
610  return std::atan2( y, x );
611 }
612 
613 // ----------------------------------------------------------------------------
614 
619 template <typename T> inline T ArcTan2Pi( T y, T x ) noexcept
620 {
621  T r = ArcTan( y, x );
622  if ( r < 0 )
623  r = static_cast<T>( r + TwoPi() );
624  return r;
625 }
626 
627 // ----------------------------------------------------------------------------
628 
633 template <typename T> inline constexpr T Ceil( T x ) noexcept
634 {
635  return std::ceil( x );
636 }
637 
638 // ----------------------------------------------------------------------------
639 
644 template <typename T> inline constexpr T Cos( T x ) noexcept
645 {
646  return std::cos( x );
647 }
648 
649 // ----------------------------------------------------------------------------
650 
655 template <typename T> inline constexpr T Cosh( T x ) noexcept
656 {
657  return std::cosh( x );
658 }
659 
660 // ----------------------------------------------------------------------------
661 
666 template <typename T> inline constexpr T Cotan( T x ) noexcept
667 {
668  return T( 1 )/std::tan( x );
669 }
670 
671 // ----------------------------------------------------------------------------
672 
677 template <typename T> inline constexpr T Deg( T x ) noexcept
678 {
679  return static_cast<T>( 0.572957795130823208767981548141051700441964e+02L * x );
680 }
681 
682 // ----------------------------------------------------------------------------
683 
688 template <typename T> inline constexpr T Exp( T x ) noexcept
689 {
690  return std::exp( x );
691 }
692 
693 // ----------------------------------------------------------------------------
694 
699 template <typename T> inline constexpr T Floor( T x ) noexcept
700 {
701  return std::floor( x );
702 }
703 
704 // ----------------------------------------------------------------------------
705 
711 template <typename T> inline constexpr T Frac( T x ) noexcept
712 {
713  return std::modf( x, &x );
714 }
715 
716 // ----------------------------------------------------------------------------
717 
724 template <typename T> inline void Frexp( T x, T& m, int& p ) noexcept
725 {
726  m = std::frexp( x, &p );
727 }
728 
729 // ----------------------------------------------------------------------------
730 
740 template <typename T> inline constexpr T Hav( T x ) noexcept
741 {
742  return (1 - Cos( x ))/2;
743 }
744 
745 // ----------------------------------------------------------------------------
746 
751 template <typename T> inline constexpr T Ldexp( T m, int p ) noexcept
752 {
753  return std::ldexp( m, p );
754 }
755 
756 // ----------------------------------------------------------------------------
757 
762 template <typename T> inline constexpr T Ln( T x ) noexcept
763 {
764  PCL_PRECONDITION( x >= 0 )
765  return std::log( x );
766 }
767 
768 // ----------------------------------------------------------------------------
769 
770 struct PCL_CLASS FactorialCache
771 {
772  constexpr static int s_cacheSize = 127;
773  static const double s_lut[ s_cacheSize+1 ];
774 };
775 
783 inline double Factorial( int n ) noexcept
784 {
785  PCL_PRECONDITION( n >= 0 )
786  if ( n <= FactorialCache::s_cacheSize )
787  return FactorialCache::s_lut[n];
788  double x = FactorialCache::s_lut[FactorialCache::s_cacheSize];
789  for ( int m = FactorialCache::s_cacheSize; ++m <= n; )
790  x *= m;
791  return x;
792 }
793 
803 inline double LnFactorial( int n ) noexcept
804 {
805  PCL_PRECONDITION( n >= 0 )
806  if ( n <= FactorialCache::s_cacheSize )
807  return Ln( FactorialCache::s_lut[n] );
808  double x = n + 1;
809 // return (x - 0.5)*Ln( x ) - x + 0.5*Ln( TwoPi() ) + 1/12/x - 1/(360*x*x*x);
810  return (x - 0.5)*Ln( x ) - x + 0.91893853320467267 + 1/12/x - 1/(360*x*x*x);
811 }
812 
831 template <typename T> struct PCL_CLASS Fact : public FactorialCache
832 {
836  T operator()( int n ) const noexcept
837  {
838  PCL_PRECONDITION( n >= 0 )
839  if ( n <= s_cacheSize )
840  return T( s_lut[n] );
841  double x = s_lut[s_cacheSize];
842  for ( int m = s_cacheSize; ++m <= n; )
843  x *= m;
844  return T( x );
845  }
846 
853  T Ln( int n ) const noexcept
854  {
855  PCL_PRECONDITION( n >= 0 )
856  if ( n <= s_cacheSize )
857  return T( pcl::Ln( s_lut[n] ) );
858  double x = n + 1;
859 // return T( (x - 0.5)*pcl::Ln( x ) - x + 0.5*pcl::Ln( TwoPi() ) + 1/12/x - 1/(360*x*x*x) );
860  return T( (x - 0.5)*pcl::Ln( x ) - x + 0.91893853320467267 + 1/12/x - 1/(360*x*x*x) );
861  }
862 };
863 
864 // ----------------------------------------------------------------------------
865 
870 inline constexpr long double Ln2() noexcept
871 {
872  return (long double)( 0.6931471805599453094172321214581766e+00L );
873 }
874 
875 // ----------------------------------------------------------------------------
876 
881 template <typename T> inline constexpr T Log( T x ) noexcept
882 {
883  PCL_PRECONDITION( x >= 0 )
884  return std::log10( x );
885 }
886 
887 // ----------------------------------------------------------------------------
888 
893 inline constexpr long double Log2() noexcept
894 {
895  // Use the relation:
896  // log10(2) = ln(2)/ln(10)
897  return (long double)( 0.3010299956639811952137388947244930416265e+00L );
898 }
899 
900 // ----------------------------------------------------------------------------
901 
906 template <typename T> inline constexpr T Log2( T x ) noexcept
907 {
908  // Use the relation:
909  // log2(x) = ln(x)/ln(2)
910  PCL_PRECONDITION( x >= 0 )
911  return std::log( x )/Ln2();
912 }
913 
914 // ----------------------------------------------------------------------------
915 
920 inline constexpr long double Log2e() noexcept
921 {
922  // Use the relation:
923  // log2(e) = 1/ln(2)
924  return (long double)( 0.14426950408889634073599246810018920709799e+01L );
925 }
926 
927 // ----------------------------------------------------------------------------
928 
933 inline constexpr long double Log2T() noexcept
934 {
935  // Use the relation:
936  // log2(10) = 1/log(2)
937  return (long double)( 0.33219280948873623478703194294893900118996e+01L );
938 }
939 
940 // ----------------------------------------------------------------------------
941 
946 template <typename T> inline constexpr T LogN( T n, T x ) noexcept
947 {
948  PCL_PRECONDITION( x >= 0 )
949  return std::log( x )/std::log( n );
950 }
951 
952 // ----------------------------------------------------------------------------
953 
958 template <typename T> inline constexpr T Mod( T x, T y ) noexcept
959 {
960  return std::fmod( x, y );
961 }
962 
963 // ----------------------------------------------------------------------------
964 
979 template <typename T, typename C> inline T Poly( T x, C c, int n ) noexcept
980 {
981  PCL_PRECONDITION( n >= 0 )
982  T y;
983  for ( c += n, y = *c; n > 0; --n )
984  {
985  y *= x;
986  y += *--c;
987  }
988  return y;
989 }
990 
1003 template <typename T> inline T Poly( T x, std::initializer_list<T> c ) noexcept
1004 {
1005  PCL_PRECONDITION( c.size() > 0 )
1006  return Poly( x, c.begin(), int( c.size() )-1 );
1007 }
1008 
1009 // ----------------------------------------------------------------------------
1010 
1022 template <typename T> inline constexpr int Sign( T x ) noexcept
1023 {
1024  return (x < 0) ? -1 : ((x > 0) ? +1 : 0);
1025 }
1026 
1027 // ----------------------------------------------------------------------------
1028 
1040 template <typename T> inline constexpr char SignChar( T x ) noexcept
1041 {
1042  return (x < 0) ? '-' : ((x > 0) ? '+' : ' ');
1043 }
1044 
1045 // ----------------------------------------------------------------------------
1046 
1051 template <typename T> inline constexpr T Sin( T x ) noexcept
1052 {
1053  return std::sin( x );
1054 }
1055 
1056 // ----------------------------------------------------------------------------
1057 
1062 template <typename T> inline constexpr T Sinh( T x ) noexcept
1063 {
1064  return std::sinh( x );
1065 }
1066 
1067 // ----------------------------------------------------------------------------
1068 
1069 #ifdef __PCL_HAVE_SINCOS
1070 
1071 inline void __pcl_sincos( float x, float& sx, float& cx ) noexcept
1072 {
1073  ::sincosf( x, &sx, &cx );
1074 }
1075 
1076 inline void __pcl_sincos( double x, double& sx, double& cx ) noexcept
1077 {
1078  ::sincos( x, &sx, &cx );
1079 }
1080 
1081 inline void __pcl_sincos( long double x, long double& sx, long double& cx ) noexcept
1082 {
1083  ::sincosl( x, &sx, &cx );
1084 }
1085 
1086 #endif
1087 
1101 template <typename T> inline void SinCos( T x, T& sx, T& cx ) noexcept
1102 {
1103 #ifdef __PCL_HAVE_SINCOS
1104  __pcl_sincos( x, sx, cx );
1105 #else
1106  sx = std::sin( x );
1107  cx = std::cos( x );
1108 #endif
1109 }
1110 
1111 // ----------------------------------------------------------------------------
1112 
1122 template <typename T> inline void Split( T x, T& i, T& f ) noexcept
1123 {
1124  f = std::modf( x, &i );
1125 }
1126 
1127 // ----------------------------------------------------------------------------
1128 
1133 template <typename T> inline constexpr T Sqrt( T x ) noexcept
1134 {
1135  return sqrt( x );
1136 }
1137 
1138 // ----------------------------------------------------------------------------
1139 
1144 template <typename T> inline constexpr T Tan( T x ) noexcept
1145 {
1146  return std::tan( x );
1147 }
1148 
1149 // ----------------------------------------------------------------------------
1150 
1155 template <typename T> inline constexpr T Tanh( T x ) noexcept
1156 {
1157  return std::tanh( x );
1158 }
1159 
1160 // ----------------------------------------------------------------------------
1161 
1166 template <typename T> inline T Trunc( T x ) noexcept
1167 {
1168  (void)std::modf( x, &x );
1169  return x;
1170 }
1171 
1172 // ----------------------------------------------------------------------------
1173 
1174 template <typename T> inline int __pcl_trunc_i32( T x ) noexcept
1175 {
1176  return int( x );
1177 }
1178 
1179 #ifdef __PCL_HAVE_SSE2
1180 
1181 inline int __pcl_trunc_i32( float x ) noexcept
1182 {
1183  return _mm_cvtt_ss2si( _mm_load_ss( &x ) );
1184 }
1185 
1186 inline int __pcl_trunc_i32( double x ) noexcept
1187 {
1188  return _mm_cvttsd_si32( _mm_load_sd( &x ) );
1189 }
1190 
1191 #endif
1192 
1203 template <typename T> inline int TruncInt( T x ) noexcept
1204 {
1205  PCL_PRECONDITION( x >= int_min && x <= int_max )
1206 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1207  return int( x );
1208 #else
1209  return __pcl_trunc_i32( x );
1210 #endif
1211 }
1212 
1221 template <typename T> inline int TruncI( T x ) noexcept
1222 {
1223  return TruncInt( x );
1224 }
1225 
1226 #define TruncInt32( x ) TruncInt( x )
1227 #define TruncI32( x ) TruncInt( x )
1228 
1229 // ----------------------------------------------------------------------------
1230 
1231 template <typename T> inline int64 __pcl_trunc_i64( T x ) noexcept
1232 {
1233  return int64( x );
1234 }
1235 
1236 #ifdef __PCL_HAVE_SSE2
1237 
1238 inline int64 __pcl_trunc_i64( float x ) noexcept
1239 {
1240  return _mm_cvttss_si64( _mm_load_ss( &x ) );
1241 }
1242 
1243 inline int64 __pcl_trunc_i64( double x ) noexcept
1244 {
1245  return _mm_cvttsd_si64( _mm_load_sd( &x ) );
1246 }
1247 
1248 #endif // __PCL_HAVE_SSE2
1249 
1260 template <typename T> inline int64 TruncInt64( T x ) noexcept
1261 {
1262  PCL_PRECONDITION( x >= int64_min && x <= int64_max )
1263 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1264  return int64( x );
1265 #else
1266  return __pcl_trunc_i64( x );
1267 #endif
1268 }
1269 
1278 template <typename T> inline int64 TruncI64( T x ) noexcept
1279 {
1280  return TruncInt64( x );
1281 }
1282 
1283 // ----------------------------------------------------------------------------
1284 
1298 template <typename T> inline constexpr T Pow( T x, T y ) noexcept
1299 {
1300  PCL_PRECONDITION( y < T( int_max ) )
1301  return std::pow( x, y );
1302 }
1303 
1304 // ----------------------------------------------------------------------------
1305 
1316 template <typename T> struct PCL_CLASS Pow10I
1317 {
1318  T operator ()( int n ) const noexcept
1319  {
1320  // Use fast table lookups and squaring up to |n| <= 50.
1321  static const T lut[] =
1322  {
1323 #define ___( x ) static_cast<T>( x )
1324  ___( 1.0e+00 ), ___( 1.0e+01 ), ___( 1.0e+02 ), ___( 1.0e+03 ), ___( 1.0e+04 ),
1325  ___( 1.0e+05 ), ___( 1.0e+06 ), ___( 1.0e+07 ), ___( 1.0e+08 ), ___( 1.0e+09 ),
1326  ___( 1.0e+10 ), ___( 1.0e+11 ), ___( 1.0e+12 ), ___( 1.0e+13 ), ___( 1.0e+14 ),
1327  ___( 1.0e+15 ), ___( 1.0e+16 ), ___( 1.0e+17 ), ___( 1.0e+18 ), ___( 1.0e+19 ),
1328  ___( 1.0e+20 ), ___( 1.0e+21 ), ___( 1.0e+22 ), ___( 1.0e+23 ), ___( 1.0e+24 ),
1329  ___( 1.0e+25 ), ___( 1.0e+26 ), ___( 1.0e+27 ), ___( 1.0e+28 ), ___( 1.0e+29 ),
1330  ___( 1.0e+30 ), ___( 1.0e+31 ), ___( 1.0e+32 ), ___( 1.0e+33 ), ___( 1.0e+34 ),
1331  ___( 1.0e+35 ), ___( 1.0e+36 ), ___( 1.0e+37 ), ___( 1.0e+38 ), ___( 1.0e+39 ),
1332  ___( 1.0e+40 ), ___( 1.0e+41 ), ___( 1.0e+42 ), ___( 1.0e+43 ), ___( 1.0e+44 ),
1333  ___( 1.0e+45 ), ___( 1.0e+46 ), ___( 1.0e+47 ), ___( 1.0e+48 ), ___( 1.0e+49 )
1334 #undef ___
1335  };
1336  static const int N = ItemsInArray( lut );
1337  int i = ::abs( n );
1338  double x;
1339  if ( i < N )
1340  x = lut[i];
1341  else
1342  {
1343  x = lut[N-1];
1344  while ( (i -= N-1) >= N )
1345  x *= x;
1346  if ( i != 0 )
1347  x *= lut[i];
1348  }
1349  return T( (n >= 0) ? x : 1/x );
1350  }
1351 };
1352 
1353 // ----------------------------------------------------------------------------
1354 
1359 template <typename T> inline T Pow10( T x ) noexcept
1360 {
1361  int i = TruncInt( x );
1362  return (i == x) ? Pow10I<T>()( i ) : T( std::pow( 10, x ) );
1363 }
1364 
1365 // ----------------------------------------------------------------------------
1366 
1377 template <typename T> inline T RotL( T x, uint32 n ) noexcept
1378 {
1379  static_assert( std::is_unsigned<T>::value,
1380  "RotL() can only be used for unsigned integer scalar types" );
1381  const uint8 mask = 8*sizeof( x ) - 1;
1382  const uint8 r = uint8( n & mask );
1383  return (x << r) | (x >> ((-r) & mask));
1384  // Or equivalently, but less optimized:
1385  //return (x << r) | (x >> (1+mask-r));
1386 }
1387 
1398 template <typename T> inline T RotR( T x, uint32 n ) noexcept
1399 {
1400  static_assert( std::is_unsigned<T>::value,
1401  "RotR() can only be used for unsigned integer scalar types" );
1402  const uint8 mask = 8*sizeof( x ) - 1;
1403  const uint8 r = uint8( n & mask );
1404  return (x >> r) | (x << ((-r) & mask));
1405  // Or equivalently, but less optimized:
1406  //return (x >> r) | (x << (1+mask-r));
1407 }
1408 
1409 // ----------------------------------------------------------------------------
1410 
1420 inline double Round( double x ) noexcept
1421 {
1422 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1423 
1424  return floor( x + 0.5 );
1425 
1426 #else
1427 
1428 # ifdef _MSC_VER
1429 # ifdef _M_X64
1430  return double( _mm_cvtsd_si64( _mm_load_sd( &x ) ) );
1431 # else
1432  __asm fld x
1433  __asm frndint
1434 # endif
1435 # else
1436  double r;
1437  asm volatile( "frndint\n": "=t" (r) : "0" (x) );
1438  return r;
1439 # endif
1440 
1441 #endif
1442 }
1443 
1453 inline float Round( float x ) noexcept
1454 {
1455 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1456 
1457  return floorf( x + 0.5F );
1458 
1459 #else
1460 
1461 # ifdef _MSC_VER
1462 # ifdef _M_X64
1463  return float( _mm_cvtss_si32( _mm_load_ss( &x ) ) );
1464 # else
1465  __asm fld x
1466  __asm frndint
1467 # endif
1468 # else
1469  float r;
1470  asm volatile( "frndint\n": "=t" (r) : "0" (x) );
1471  return r;
1472 # endif
1473 
1474 #endif
1475 }
1476 
1486 inline long double Round( long double x ) noexcept
1487 {
1488 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1489 
1490  return floorl( x + 0.5L );
1491 
1492 #else
1493 
1494 # ifdef _MSC_VER
1495 # ifdef _M_X64
1496  double _x = x;
1497  return (long double)_mm_cvtsd_si64( _mm_load_sd( &_x ) );
1498 # else
1499  __asm fld x
1500  __asm frndint
1501 # endif
1502 # else
1503  long double r;
1504  asm volatile( "frndint\n": "=t" (r) : "0" (x) );
1505  return r;
1506 # endif
1507 
1508 #endif
1509 }
1510 
1511 // ----------------------------------------------------------------------------
1512 
1550 template <typename T> inline int RoundInt( T x ) noexcept
1551 {
1552  PCL_PRECONDITION( x >= int_min && x <= int_max )
1553 
1554 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1555 
1556  return int( Round( x ) );
1557 
1558 #else
1559 
1560  static const double magic = 6755399441055744.0;
1561  volatile union { double d; int32 i; } v = { double( x ) + magic };
1562  return v.i; // ### NB: Assuming little-endian architecture.
1563 
1564 #endif
1565 }
1566 
1575 template <typename T> inline int RoundI( T x ) noexcept
1576 {
1577  return RoundInt( x );
1578 }
1579 
1588 template <typename T> inline int RoundIntBanker( T x ) noexcept
1589 {
1590  return RoundInt( x );
1591 }
1592 
1617 template <typename T> inline int RoundIntArithmetic( T x ) noexcept
1618 {
1619  PCL_PRECONDITION( x >= int_min && x <= int_max )
1620 
1621  int i = TruncInt( x );
1622  if ( i < 0 )
1623  {
1624  if ( x - i <= T( -0.5 ) )
1625  --i;
1626  }
1627  else
1628  {
1629  if ( x - i >= T( +0.5 ) )
1630  ++i;
1631  }
1632  return i;
1633 }
1634 
1635 // ----------------------------------------------------------------------------
1636 
1651 inline int64 RoundInt64( double x ) noexcept
1652 {
1653 #ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
1654 
1655  return int64( Round( x ) );
1656 
1657 #else
1658 
1659  // ### N.B.: Default FPU rounding mode assumed to be nearest integer.
1660 
1661 # ifdef _MSC_VER
1662 # ifdef _M_X64
1663  return _mm_cvtsd_si64( _mm_load_sd( &x ) );
1664 # else
1665  int64 r;
1666  __asm fld x
1667  __asm fistp qword ptr r
1668  return r;
1669 # endif
1670 # else
1671  int64 r;
1672  asm volatile( "fistpll %0\n" : "=m" (r) : "t" (x) : "st" );
1673  return r;
1674 # endif
1675 
1676 #endif
1677 }
1678 
1685 inline int64 RoundI64( double x ) noexcept
1686 {
1687  return RoundInt64( x );
1688 }
1689 
1714 inline int64 RoundInt64Arithmetic( double x ) noexcept
1715 {
1716  int64 i = TruncInt64( x );
1717  if ( i < 0 )
1718  {
1719  if ( x - i <= -0.5 )
1720  return i-1;
1721  }
1722  else
1723  {
1724  if ( x - i >= +0.5 )
1725  return i+1;
1726  }
1727  return i;
1728 }
1729 
1730 // ----------------------------------------------------------------------------
1731 
1736 template <typename T> inline T Round( T x, int n ) noexcept
1737 {
1738  PCL_PRECONDITION( n >= 0 )
1739  T p = Pow10I<T>()( n ); return Round( p*x )/p;
1740 }
1741 
1742 // ----------------------------------------------------------------------------
1743 
1748 template <typename T> inline constexpr T Pow2( T x ) noexcept
1749 {
1750  // Use the relation:
1751  // 2**x = e**(x*ln(2))
1752  return Exp( x*Ln2() );
1753 }
1754 
1755 // ----------------------------------------------------------------------------
1756 
1767 template <typename T> struct PCL_CLASS Pow2I
1768 {
1769  T operator ()( int n ) const noexcept
1770  {
1771  // We shift left a single bit in 31-bit chunks.
1772  int i = ::abs( n ), p;
1773  double x = uint32( 1 ) << (p = Min( i, 31 ));
1774  while ( (i -= p) != 0 )
1775  x *= uint32( 1 ) << (p = Min( i, 31 ));
1776  return T( (n >= 0) ? x : 1/x );
1777  }
1778 };
1779 
1780 // ----------------------------------------------------------------------------
1781 
1786 template <typename T> inline T PowI( T x, int n ) noexcept
1787 {
1788  if ( n == 0 )
1789  return T( 1 );
1790 
1791  T r = x;
1792  int i = ::abs( n );
1793  if ( i > 1 )
1794  {
1795  do
1796  r *= r;
1797  while ( (i >>= 1) > 1 );
1798  if ( n & 1 )
1799  r *= x;
1800  }
1801  return (n > 0) ? r : T( 1/r );
1802 }
1803 
1808 template <typename T> inline T PowI4( T x ) noexcept
1809 {
1810  x *= x; return x*x;
1811 }
1812 
1817 template <typename T> inline T PowI6( T x ) noexcept
1818 {
1819  x *= x*x; return x*x;
1820 }
1821 
1826 template <typename T> inline T PowI8( T x ) noexcept
1827 {
1828  x *= x*x*x; return x*x;
1829 }
1830 
1835 template <typename T> inline T PowI10( T x ) noexcept
1836 {
1837  x *= x*x*x*x; return x*x;
1838 }
1839 
1844 template <typename T> inline T PowI12( T x ) noexcept
1845 {
1846  x *= x*x*x*x*x; return x*x;
1847 }
1848 
1849 // ----------------------------------------------------------------------------
1850 
1858 template <typename T> inline constexpr T ArcSinh( T x ) noexcept
1859 {
1860 #ifndef __PCL_NO_STD_INV_HYP_TRIG_FUNCTIONS
1861  return std::asinh( x );
1862 #else
1863  return Ln( x + Sqrt( 1 + x*x ) );
1864 #endif
1865 }
1866 
1867 // ----------------------------------------------------------------------------
1868 
1876 template <typename T> inline constexpr T ArcCosh( T x ) noexcept
1877 {
1878 #ifndef __PCL_NO_STD_INV_HYP_TRIG_FUNCTIONS
1879  return std::acosh( x );
1880 #else
1881  return 2*Ln( Sqrt( (x + 1)/2 ) + Sqrt( (x - 1)/2 ) );
1882 #endif
1883 }
1884 
1885 // ----------------------------------------------------------------------------
1886 
1894 template <typename T> inline constexpr T ArcTanh( T x ) noexcept
1895 {
1896 #ifndef __PCL_NO_STD_INV_HYP_TRIG_FUNCTIONS
1897  return std::atanh( x );
1898 #else
1899  return (Ln( 1 + x ) - Ln( 1 - x ))/2;
1900 #endif
1901 }
1902 
1903 // ----------------------------------------------------------------------------
1904 
1914 template <typename T> inline constexpr T ArcHav( T x ) noexcept
1915 {
1916  return 2*ArcSin( Sqrt( x ) );
1917 }
1918 
1919 // ----------------------------------------------------------------------------
1920 
1925 template <typename T> inline constexpr T Rad( T x ) noexcept
1926 {
1927  return static_cast<T>( 0.174532925199432957692369076848861272222e-01L * x );
1928 }
1929 
1930 // ----------------------------------------------------------------------------
1931 
1936 template <typename T> inline constexpr T RadMin( T x ) noexcept
1937 {
1938  return Deg( x )*60;
1939 }
1940 
1941 // ----------------------------------------------------------------------------
1942 
1947 template <typename T> inline constexpr T RadSec( T x ) noexcept
1948 {
1949  return Deg( x )*3600;
1950 }
1951 
1952 // ----------------------------------------------------------------------------
1953 
1958 template <typename T> inline constexpr T MinRad( T x ) noexcept
1959 {
1960  return Rad( x/60 );
1961 }
1962 
1963 // ----------------------------------------------------------------------------
1964 
1969 template <typename T> inline constexpr T SecRad( T x ) noexcept
1970 {
1971  return Rad( x/3600 );
1972 }
1973 
1978 template <typename T> inline constexpr T AsRad( T x ) noexcept
1979 {
1980  return SecRad( x );
1981 }
1982 
1983 // ----------------------------------------------------------------------------
1984 
1989 template <typename T> inline constexpr T MasRad( T x ) noexcept
1990 {
1991  return Rad( x/3600000 );
1992 }
1993 
1994 // ----------------------------------------------------------------------------
1995 
2000 template <typename T> inline constexpr T UasRad( T x ) noexcept
2001 {
2002  return Rad( x/3600000000 );
2003 }
2004 
2005 // ----------------------------------------------------------------------------
2006 
2011 template <typename T> inline constexpr T ModPi( T x ) noexcept
2012 {
2013  x = Mod( x + static_cast<T>( Pi() ), static_cast<T>( TwoPi() ) ) - static_cast<T>( Pi() );
2014  return (x < -static_cast<T>( Pi() )) ? x + static_cast<T>( TwoPi() ) : x;
2015 }
2016 
2017 // ----------------------------------------------------------------------------
2018 
2024 template <typename T> inline constexpr T Mod2Pi( T x ) noexcept
2025 {
2026  return Mod( x, static_cast<T>( TwoPi() ) );
2027 }
2028 
2029 // ----------------------------------------------------------------------------
2030 
2035 template <typename T> inline constexpr T Norm2Pi( T x ) noexcept
2036 {
2037  return ((x = Mod2Pi( x )) < 0) ? x + static_cast<T>( TwoPi() ) : x;
2038 }
2039 
2040 // ----------------------------------------------------------------------------
2041 
2054 template <typename T, typename T1, typename T2>
2055 inline void Rotate( T& x, T& y, T1 sa, T1 ca, T2 xc, T2 yc ) noexcept
2056 {
2057  T1 dx = T1( x ) - T1( xc );
2058  T1 dy = T1( y ) - T1( yc );
2059  x = T( T1( xc ) + ca*dx - sa*dy );
2060  y = T( T1( yc ) + sa*dx + ca*dy );
2061 }
2062 
2073 template <typename T1, typename T2>
2074 inline void Rotate( int& x, int& y, T1 sa, T1 ca, T2 xc, T2 yc ) noexcept
2075 {
2076  T1 dx = T1( x ) - T1( xc );
2077  T1 dy = T1( y ) - T1( yc );
2078  x = RoundInt( T1( xc ) + ca*dx - sa*dy );
2079  y = RoundInt( T1( yc ) + sa*dx + ca*dy );
2080 }
2081 
2092 template <typename T1, typename T2>
2093 inline void Rotate( long& x, long& y, T1 sa, T1 ca, T2 xc, T2 yc ) noexcept
2094 {
2095  T1 dx = T1( x ) - T1( xc );
2096  T1 dy = T1( y ) - T1( yc );
2097  x = (long)RoundInt( T1( xc ) + ca*dx - sa*dy );
2098  y = (long)RoundInt( T1( yc ) + sa*dx + ca*dy );
2099 }
2100 
2111 template <typename T1, typename T2>
2112 inline void Rotate( int64& x, int64& y, T1 sa, T1 ca, T2 xc, T2 yc ) noexcept
2113 {
2114  T1 dx = T1( x ) - T1( xc );
2115  T1 dy = T1( y ) - T1( yc );
2116  x = RoundInt64( T1( xc ) + ca*dx - sa*dy );
2117  y = RoundInt64( T1( yc ) + sa*dx + ca*dy );
2118 }
2119 
2135 template <typename T, typename T1, typename T2>
2136 inline void Rotate( T& x, T& y, T1 a, T2 xc, T2 yc ) noexcept
2137 {
2138  T1 sa, ca; SinCos( a, sa, ca ); Rotate( x, y, sa, ca, xc, yc );
2139 }
2140 
2141 // ----------------------------------------------------------------------------
2142 
2143 template <typename T, typename P, typename N> inline
2144 N __pcl_norm( const T* i, const T* j, const P& p, N* ) noexcept
2145 {
2146  PCL_PRECONDITION( p > P( 0 ) )
2147  N n = N( 0 );
2148  for ( ; i < j; ++i )
2149  n += Pow( Abs( N( *i ) ), N( p ) );
2150  return Pow( n, N( 1/p ) );
2151 }
2152 
2166 template <typename T, typename P> inline T Norm( const T* i, const T* j, const P& p ) noexcept
2167 {
2168  return __pcl_norm( i, j, p, (T*)( 0 ) );
2169 }
2170 
2171 template <typename P> inline double Norm( const float* i, const float* j, const P& p ) noexcept
2172 {
2173  return __pcl_norm( i, j, p, (double*)( 0 ) );
2174 }
2175 template <typename P> inline double Norm( const int* i, const int* j, const P& p ) noexcept
2176 {
2177  return __pcl_norm( i, j, p, (double*)( 0 ) );
2178 }
2179 template <typename P> inline double Norm( const unsigned* i, const unsigned* j, const P& p ) noexcept
2180 {
2181  return __pcl_norm( i, j, p, (double*)( 0 ) );
2182 }
2183 template <typename P> inline double Norm( const int8* i, const int8* j, const P& p ) noexcept
2184 {
2185  return __pcl_norm( i, j, p, (double*)( 0 ) );
2186 }
2187 template <typename P> inline double Norm( const uint8* i, const uint8* j, const P& p ) noexcept
2188 {
2189  return __pcl_norm( i, j, p, (double*)( 0 ) );
2190 }
2191 template <typename P> inline double Norm( const int16* i, const int16* j, const P& p ) noexcept
2192 {
2193  return __pcl_norm( i, j, p, (double*)( 0 ) );
2194 }
2195 template <typename P> inline double Norm( const uint16* i, const uint16* j, const P& p ) noexcept
2196 {
2197  return __pcl_norm( i, j, p, (double*)( 0 ) );
2198 }
2199 template <typename P> inline double Norm( const int64* i, const int64* j, const P& p ) noexcept
2200 {
2201  return __pcl_norm( i, j, p, (double*)( 0 ) );
2202 }
2203 template <typename P> inline double Norm( const uint64* i, const uint64* j, const P& p ) noexcept
2204 {
2205  return __pcl_norm( i, j, p, (double*)( 0 ) );
2206 }
2207 
2208 // ----------------------------------------------------------------------------
2209 
2210 template <typename T, typename N> inline
2211 N __pcl_l1norm( const T* i, const T* j, N* ) noexcept
2212 {
2213  N n = N( 0 );
2214  for ( ; i < j; ++i )
2215  n += N( Abs( *i ) );
2216  return n;
2217 }
2218 
2225 template <typename T> inline T L1Norm( const T* i, const T* j ) noexcept
2226 {
2227  return __pcl_l1norm( i, j, (T*)( 0 ) );
2228 }
2229 
2230 inline double L1Norm( const float* i, const float* j ) noexcept
2231 {
2232  return __pcl_l1norm( i, j, (double*)( 0 ) );
2233 }
2234 inline double L1Norm( const int* i, const int* j ) noexcept
2235 {
2236  return __pcl_l1norm( i, j, (double*)( 0 ) );
2237 }
2238 inline double L1Norm( const unsigned* i, const unsigned* j ) noexcept
2239 {
2240  return __pcl_l1norm( i, j, (double*)( 0 ) );
2241 }
2242 inline double L1Norm( const int8* i, const int8* j ) noexcept
2243 {
2244  return __pcl_l1norm( i, j, (double*)( 0 ) );
2245 }
2246 inline double L1Norm( const uint8* i, const uint8* j ) noexcept
2247 {
2248  return __pcl_l1norm( i, j, (double*)( 0 ) );
2249 }
2250 inline double L1Norm( const int16* i, const int16* j ) noexcept
2251 {
2252  return __pcl_l1norm( i, j, (double*)( 0 ) );
2253 }
2254 inline double L1Norm( const uint16* i, const uint16* j ) noexcept
2255 {
2256  return __pcl_l1norm( i, j, (double*)( 0 ) );
2257 }
2258 inline double L1Norm( const int64* i, const int64* j ) noexcept
2259 {
2260  return __pcl_l1norm( i, j, (double*)( 0 ) );
2261 }
2262 inline double L1Norm( const uint64* i, const uint64* j ) noexcept
2263 {
2264  return __pcl_l1norm( i, j, (double*)( 0 ) );
2265 }
2266 
2267 // ----------------------------------------------------------------------------
2268 
2269 template <typename T, typename N> inline
2270 N __pcl_l2norm( const T* i, const T* j, N* ) noexcept
2271 {
2272  N n = N( 0 );
2273  for ( ; i < j; ++i )
2274  n += N( *i ) * N( *i );
2275  return Sqrt( n );
2276 }
2277 
2284 template <typename T> inline T L2Norm( const T* i, const T* j ) noexcept
2285 {
2286  return __pcl_l2norm( i, j, (T*)( 0 ) );
2287 }
2288 
2289 inline double L2Norm( const float* i, const float* j ) noexcept
2290 {
2291  return __pcl_l2norm( i, j, (double*)( 0 ) );
2292 }
2293 inline double L2Norm( const int* i, const int* j ) noexcept
2294 {
2295  return __pcl_l2norm( i, j, (double*)( 0 ) );
2296 }
2297 inline double L2Norm( const unsigned* i, const unsigned* j ) noexcept
2298 {
2299  return __pcl_l2norm( i, j, (double*)( 0 ) );
2300 }
2301 inline double L2Norm( const int8* i, const int8* j ) noexcept
2302 {
2303  return __pcl_l2norm( i, j, (double*)( 0 ) );
2304 }
2305 inline double L2Norm( const uint8* i, const uint8* j ) noexcept
2306 {
2307  return __pcl_l2norm( i, j, (double*)( 0 ) );
2308 }
2309 inline double L2Norm( const int16* i, const int16* j ) noexcept
2310 {
2311  return __pcl_l2norm( i, j, (double*)( 0 ) );
2312 }
2313 inline double L2Norm( const uint16* i, const uint16* j ) noexcept
2314 {
2315  return __pcl_l2norm( i, j, (double*)( 0 ) );
2316 }
2317 inline double L2Norm( const int64* i, const int64* j ) noexcept
2318 {
2319  return __pcl_l2norm( i, j, (double*)( 0 ) );
2320 }
2321 inline double L2Norm( const uint64* i, const uint64* j ) noexcept
2322 {
2323  return __pcl_l2norm( i, j, (double*)( 0 ) );
2324 }
2325 
2326 // ----------------------------------------------------------------------------
2327 
2334 template <typename T> inline T Norm( const T* i, const T* j ) noexcept
2335 {
2336  return L2Norm( i, j );
2337 }
2338 
2339 inline double Norm( const float* i, const float* j ) noexcept
2340 {
2341  return __pcl_l2norm( i, j, (double*)( 0 ) );
2342 }
2343 inline double Norm( const int* i, const int* j ) noexcept
2344 {
2345  return __pcl_l2norm( i, j, (double*)( 0 ) );
2346 }
2347 inline double Norm( const unsigned* i, const unsigned* j ) noexcept
2348 {
2349  return __pcl_l2norm( i, j, (double*)( 0 ) );
2350 }
2351 inline double Norm( const int8* i, const int8* j ) noexcept
2352 {
2353  return __pcl_l2norm( i, j, (double*)( 0 ) );
2354 }
2355 inline double Norm( const uint8* i, const uint8* j ) noexcept
2356 {
2357  return __pcl_l2norm( i, j, (double*)( 0 ) );
2358 }
2359 inline double Norm( const int16* i, const int16* j ) noexcept
2360 {
2361  return __pcl_l2norm( i, j, (double*)( 0 ) );
2362 }
2363 inline double Norm( const uint16* i, const uint16* j ) noexcept
2364 {
2365  return __pcl_l2norm( i, j, (double*)( 0 ) );
2366 }
2367 inline double Norm( const int64* i, const int64* j ) noexcept
2368 {
2369  return __pcl_l2norm( i, j, (double*)( 0 ) );
2370 }
2371 inline double Norm( const uint64* i, const uint64* j ) noexcept
2372 {
2373  return __pcl_l2norm( i, j, (double*)( 0 ) );
2374 }
2375 
2376 // ----------------------------------------------------------------------------
2377 
2418 void PCL_FUNC CalendarTimeToJD( int& jdi, double& jdf, int year, int month, int day, double dayf = 0 ) noexcept;
2419 
2459 inline double CalendarTimeToJD( int year, int month, int day, double dayf = 0 ) noexcept
2460 {
2461  int jdi;
2462  double jdf;
2463  CalendarTimeToJD( jdi, jdf, year, month, day, dayf );
2464  return jdi + jdf;
2465 }
2466 
2497 void PCL_FUNC JDToCalendarTime( int& year, int& month, int& day, double& dayf, int jdi, double jdf ) noexcept;
2498 
2528 inline void JDToCalendarTime( int& year, int& month, int& day, double& dayf, double jd ) noexcept
2529 {
2530  JDToCalendarTime( year, month, day, dayf, TruncInt( jd ), Frac( jd ) );
2531 }
2532 
2533 // ----------------------------------------------------------------------------
2534 
2555 template <typename S1, typename S2, typename S3, typename D>
2556 inline void DecimalToSexagesimal( int& sign, S1& s1, S2& s2, S3& s3, const D& d ) noexcept
2557 {
2558  double t1 = Abs( d );
2559  double t2 = Frac( t1 )*60;
2560  double t3 = Frac( t2 )*60;
2561  sign = (d < 0) ? -1 : +1;
2562  s1 = S1( TruncInt( t1 ) );
2563  s2 = S2( TruncInt( t2 ) );
2564  s3 = S3( t3 );
2565 }
2566 
2577 template <typename S1, typename S2, typename S3>
2578 inline double SexagesimalToDecimal( int sign, const S1& s1, const S2& s2 = S2( 0 ), const S3& s3 = S3( 0 ) ) noexcept
2579 {
2580  double d = Abs( s1 ) + (s2 + s3/60)/60;
2581  return (sign < 0) ? -d : d;
2582 }
2583 
2584 // ----------------------------------------------------------------------------
2585 
2604 template <typename T> inline double Sum( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2605 {
2606  double sum = 0;
2607  while ( i < j )
2608  sum += double( *i++ );
2609  return sum;
2610 }
2611 
2623 template <typename T> inline double StableSum( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2624 {
2625  double sum = 0;
2626  double eps = 0;
2627  while ( i < j )
2628  {
2629  double y = double( *i++ ) - eps;
2630  double t = sum + y;
2631  eps = (t - sum) - y;
2632  sum = t;
2633  }
2634  return sum;
2635 }
2636 
2648 template <typename T> inline double Modulus( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2649 {
2650  double S = 0;
2651  while ( i < j )
2652  S += Abs( double( *i++ ) );
2653  return S;
2654 }
2655 
2667 template <typename T> inline double StableModulus( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2668 {
2669  double sum = 0;
2670  double eps = 0;
2671  while ( i < j )
2672  {
2673  double y = Abs( double( *i++ ) ) - eps;
2674  double t = sum + y;
2675  eps = (t - sum) - y;
2676  sum = t;
2677  }
2678  return sum;
2679 }
2680 
2692 template <typename T> inline double SumOfSquares( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2693 {
2694  double Q = 0;
2695  while ( i < j )
2696  {
2697  double f = double( *i++ );
2698  Q += f*f;
2699  }
2700  return Q;
2701 }
2702 
2714 template <typename T> inline double StableSumOfSquares( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2715 {
2716  double sum = 0;
2717  double eps = 0;
2718  while ( i < j )
2719  {
2720  double f = double( *i++ );
2721  double y = f*f - eps;
2722  double t = sum + y;
2723  eps = (t - sum) - y;
2724  sum = t;
2725  }
2726  return sum;
2727 }
2728 
2740 template <typename T> inline double Mean( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2741 {
2742  distance_type n = j - i;
2743  if ( n < 1 )
2744  return 0;
2745  return Sum( i, j )/n;
2746 }
2747 
2759 template <typename T> inline double StableMean( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2760 {
2761  distance_type n = j - i;
2762  if ( n < 1 )
2763  return 0;
2764  return StableSum( i, j )/n;
2765 }
2766 
2784 template <typename T> inline double Variance( const T* __restrict__ i, const T* __restrict__ j, double center ) noexcept
2785 {
2786  distance_type n = j - i;
2787  if ( n < 2 )
2788  return 0;
2789  double var = 0, eps = 0;
2790  do
2791  {
2792  double d = double( *i++ ) - center;
2793  var += d*d;
2794  eps += d;
2795  }
2796  while ( i < j );
2797  return (var - eps*eps/n)/(n - 1);
2798 }
2799 
2816 template <typename T> inline double Variance( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2817 {
2818  distance_type n = j - i;
2819  if ( n < 2 )
2820  return 0;
2821  double m = 0;
2822  for ( const T* f = i; f < j; ++f )
2823  m += double( *f );
2824  m /= n;
2825  double var = 0, eps = 0;
2826  do
2827  {
2828  double d = double( *i++ ) - m;
2829  var += d*d;
2830  eps += d;
2831  }
2832  while ( i < j );
2833  return (var - eps*eps/n)/(n - 1);
2834 }
2835 
2844 template <typename T> inline double StdDev( const T* __restrict__ i, const T* __restrict__ j, double center ) noexcept
2845 {
2846  return Sqrt( Variance( i, j, center ) );
2847 }
2848 
2856 template <typename T> inline double StdDev( const T* __restrict__ i, const T* __restrict__ j ) noexcept
2857 {
2858  return Sqrt( Variance( i, j ) );
2859 }
2860 
2917 template <class T> inline double Median( const T* __restrict__ i, const T* __restrict__ j, double eps = 0 )
2918 {
2919  distance_type n = j - i;
2920  if ( n < 1 )
2921  return 0;
2922  if ( n == 1 )
2923  return double( *i );
2924  double* d = new double[ n ];
2925  double* __restrict__ t = d;
2926  do
2927  *t++ = double( *i++ );
2928  while ( i < j );
2929  double m = double( *pcl::Select( d, t, n >> 1 ) );
2930  if ( (n & 1) == 0 )
2931  m = (m + double( *pcl::Select( d, t, (n >> 1)-1 ) ))/2;
2932  delete [] d;
2933  return m;
2934 }
2935 
2936 double PCL_FUNC Median( const unsigned char* __restrict__ i, const unsigned char* __restrict__ j, double eps = 0 );
2937 double PCL_FUNC Median( const signed char* __restrict__ i, const signed char* __restrict__ j, double eps = 0 );
2938 double PCL_FUNC Median( const wchar_t* __restrict__ i, const wchar_t* __restrict__ j, double eps = 0 );
2939 double PCL_FUNC Median( const unsigned short* __restrict__ i, const unsigned short* __restrict__ j, double eps = 0 );
2940 double PCL_FUNC Median( const signed short* __restrict__ i, const signed short* __restrict__ j, double eps = 0 );
2941 double PCL_FUNC Median( const unsigned int* __restrict__ i, const unsigned int* __restrict__ j, double eps = 0 );
2942 double PCL_FUNC Median( const signed int* __restrict__ i, const signed int* __restrict__ j, double eps = 0 );
2943 double PCL_FUNC Median( const unsigned long* __restrict__ i, const unsigned long* __restrict__ j, double eps = 0 );
2944 double PCL_FUNC Median( const signed long* __restrict__ i, const signed long* __restrict__ j, double eps = 0 );
2945 double PCL_FUNC Median( const unsigned long long* __restrict__ i, const unsigned long long* __restrict__ j, double eps = 0 );
2946 double PCL_FUNC Median( const signed long long* __restrict__ i, const signed long long* __restrict__ j, double eps = 0 );
2947 double PCL_FUNC Median( const float* __restrict__ i, const float* __restrict__ j, double eps = 0 );
2948 double PCL_FUNC Median( const double* __restrict__ i, const double* __restrict__ j, double eps = 0 );
2949 double PCL_FUNC Median( const long double* __restrict__ i, const long double* __restrict__ j, double eps = 0 );
2950 
2951 #define __PCL_SMALL_MEDIAN_MAX_LENGTH 32
2952 
2953 double PCL_FUNC SmallMedianDestructive( unsigned char* __restrict__ i, unsigned char* __restrict__ j );
2954 double PCL_FUNC SmallMedianDestructive( signed char* __restrict__ i, signed char* __restrict__ j );
2955 double PCL_FUNC SmallMedianDestructive( wchar_t* __restrict__ i, wchar_t* __restrict__ j );
2956 double PCL_FUNC SmallMedianDestructive( unsigned short* __restrict__ i, unsigned short* __restrict__ j );
2957 double PCL_FUNC SmallMedianDestructive( signed short* __restrict__ i, signed short* __restrict__ j );
2958 double PCL_FUNC SmallMedianDestructive( unsigned int* __restrict__ i, unsigned int* __restrict__ j );
2959 double PCL_FUNC SmallMedianDestructive( signed int* __restrict__ i, signed int* __restrict__ j );
2960 double PCL_FUNC SmallMedianDestructive( unsigned long* __restrict__ i, unsigned long* __restrict__ j );
2961 double PCL_FUNC SmallMedianDestructive( signed long* __restrict__ i, signed long* __restrict__ j );
2962 double PCL_FUNC SmallMedianDestructive( unsigned long long* __restrict__ i, unsigned long long* __restrict__ j );
2963 double PCL_FUNC SmallMedianDestructive( signed long long* __restrict__ i, signed long long* __restrict__ j );
2964 double PCL_FUNC SmallMedianDestructive( float* __restrict__ i, float* __restrict__ j );
2965 double PCL_FUNC SmallMedianDestructive( double* __restrict__ i, double* __restrict__ j );
2966 double PCL_FUNC SmallMedianDestructive( long double* __restrict__ i, long double* __restrict__ j );
2967 
2968 #define CMPXCHG( a, b ) \
2969  if ( i[b] < i[a] ) pcl::Swap( i[a], i[b] )
2970 
2971 #define MEAN( a, b ) \
2972  (double( a ) + double( b ))/2
2973 
3016 template <typename T> inline double MedianDestructive( T* __restrict__ i, T* __restrict__ j ) noexcept
3017 {
3018  distance_type n = j - i;
3019  if ( n < 1 )
3020  return 0;
3021 
3022  switch ( n )
3023  {
3024  case 1: // !?
3025  return i[0];
3026  case 2:
3027  return MEAN( i[0], i[1] );
3028  case 3:
3029  CMPXCHG( 0, 1 ); CMPXCHG( 1, 2 );
3030  return pcl::Max( i[0], i[1] );
3031  case 4:
3032  CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
3033  CMPXCHG( 1, 3 );
3034  return MEAN( i[1], i[2] );
3035  case 5:
3036  CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 0, 3 );
3037  CMPXCHG( 1, 4 ); CMPXCHG( 1, 2 ); CMPXCHG( 2, 3 );
3038  return pcl::Max( i[1], i[2] );
3039  case 6:
3040  CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
3041  CMPXCHG( 1, 3 ); CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 );
3042  CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 1, 4 );
3043  CMPXCHG( 2, 4 ); CMPXCHG( 3, 5 ); CMPXCHG( 3, 4 );
3044  return MEAN( i[2], i[3] );
3045  case 7:
3046  CMPXCHG( 0, 5 ); CMPXCHG( 0, 3 ); CMPXCHG( 1, 6 );
3047  CMPXCHG( 2, 4 ); CMPXCHG( 0, 1 ); CMPXCHG( 3, 5 );
3048  CMPXCHG( 2, 6 ); CMPXCHG( 2, 3 ); CMPXCHG( 3, 6 );
3049  CMPXCHG( 4, 5 ); CMPXCHG( 1, 4 ); CMPXCHG( 1, 3 );
3050  return pcl::Min( i[3], i[4] );
3051  case 8:
3052  CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 2, 6 );
3053  CMPXCHG( 3, 7 ); CMPXCHG( 0, 2 ); CMPXCHG( 1, 3 );
3054  CMPXCHG( 4, 6 ); CMPXCHG( 5, 7 ); CMPXCHG( 2, 4 );
3055  CMPXCHG( 3, 5 ); CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 );
3056  CMPXCHG( 4, 5 ); CMPXCHG( 6, 7 ); CMPXCHG( 1, 4 );
3057  CMPXCHG( 3, 6 );
3058  return MEAN( i[3], i[4] );
3059  case 9:
3060  CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
3061  CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 6, 7 );
3062  CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
3063  CMPXCHG( 0, 3 ); CMPXCHG( 5, 8 ); CMPXCHG( 4, 7 );
3064  CMPXCHG( 3, 6 ); CMPXCHG( 1, 4 ); CMPXCHG( 2, 5 );
3065  CMPXCHG( 4, 7 ); CMPXCHG( 4, 2 ); CMPXCHG( 6, 4 );
3066  return pcl::Min( i[2], i[4] );
3067  default:
3068  {
3069  double m = double( *pcl::Select( i, j, n >> 1 ) );
3070  if ( n & 1 )
3071  return m;
3072  return MEAN( m, double( *pcl::Select( i, j, (n >> 1)-1 ) ) );
3073  }
3074  }
3075 }
3076 
3077 #undef CMPXCHG
3078 
3079 #define CMPXCHG( a, b ) \
3080  if ( p( i[b], i[a] ) ) pcl::Swap( i[a], i[b] )
3081 
3097 template <typename T, class BP> inline double MedianDestructive( T* __restrict__ i, T* __restrict__ j, BP p ) noexcept
3098 {
3099  distance_type n = j - i;
3100  if ( n < 1 )
3101  return 0;
3102 
3103  switch ( n )
3104  {
3105  case 1: // !?
3106  return i[0];
3107  case 2:
3108  return MEAN( i[0], i[1] );
3109  case 3:
3110  CMPXCHG( 0, 1 ); CMPXCHG( 1, 2 );
3111  return pcl::Max( i[0], i[1] );
3112  case 4:
3113  CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
3114  CMPXCHG( 1, 3 );
3115  return MEAN( i[1], i[2] );
3116  case 5:
3117  CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 0, 3 );
3118  CMPXCHG( 1, 4 ); CMPXCHG( 1, 2 ); CMPXCHG( 2, 3 );
3119  return pcl::Max( i[1], i[2] );
3120  case 6:
3121  CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
3122  CMPXCHG( 1, 3 ); CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 );
3123  CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 1, 4 );
3124  CMPXCHG( 2, 4 ); CMPXCHG( 3, 5 ); CMPXCHG( 3, 4 );
3125  return MEAN( i[2], i[3] );
3126  case 7:
3127  CMPXCHG( 0, 5 ); CMPXCHG( 0, 3 ); CMPXCHG( 1, 6 );
3128  CMPXCHG( 2, 4 ); CMPXCHG( 0, 1 ); CMPXCHG( 3, 5 );
3129  CMPXCHG( 2, 6 ); CMPXCHG( 2, 3 ); CMPXCHG( 3, 6 );
3130  CMPXCHG( 4, 5 ); CMPXCHG( 1, 4 ); CMPXCHG( 1, 3 );
3131  return pcl::Min( i[3], i[4] );
3132  case 8:
3133  CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 2, 6 );
3134  CMPXCHG( 3, 7 ); CMPXCHG( 0, 2 ); CMPXCHG( 1, 3 );
3135  CMPXCHG( 4, 6 ); CMPXCHG( 5, 7 ); CMPXCHG( 2, 4 );
3136  CMPXCHG( 3, 5 ); CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 );
3137  CMPXCHG( 4, 5 ); CMPXCHG( 6, 7 ); CMPXCHG( 1, 4 );
3138  CMPXCHG( 3, 6 );
3139  return MEAN( i[3], i[4] );
3140  case 9:
3141  CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
3142  CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 6, 7 );
3143  CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
3144  CMPXCHG( 0, 3 ); CMPXCHG( 5, 8 ); CMPXCHG( 4, 7 );
3145  CMPXCHG( 3, 6 ); CMPXCHG( 1, 4 ); CMPXCHG( 2, 5 );
3146  CMPXCHG( 4, 7 ); CMPXCHG( 4, 2 ); CMPXCHG( 6, 4 );
3147  return pcl::Min( i[2], i[4] );
3148  default:
3149  {
3150  double m = double( *pcl::Select( i, j, n >> 1, p ) );
3151  if ( n & 1 )
3152  return m;
3153  return MEAN( m, double( *pcl::Select( i, j, (n >> 1)-1, p ) ) );
3154  }
3155  }
3156 }
3157 
3158 #undef CMPXCHG
3159 #undef MEAN
3160 
3202 template <typename T> inline double OrderStatistic( const T* __restrict__ i, const T* __restrict__ j, distance_type k, double eps = 0 )
3203 {
3204  distance_type n = j - i;
3205  if ( n < 1 || k < 0 || k >= n )
3206  return 0;
3207  if ( n == 1 )
3208  return double( *i );
3209  double* d = new double[ n ];
3210  double* t = d;
3211  do
3212  *t++ = double( *i++ );
3213  while ( i < j );
3214  double s = *pcl::Select( d, t, k );
3215  delete [] d;
3216  return s;
3217 }
3218 
3219 double PCL_FUNC OrderStatistic( const unsigned char* __restrict__ i, const unsigned char* __restrict__ j, distance_type k, double eps = 0 );
3220 double PCL_FUNC OrderStatistic( const signed char* __restrict__ i, const signed char* __restrict__ j, distance_type k, double eps = 0 );
3221 double PCL_FUNC OrderStatistic( const wchar_t* __restrict__ i, const wchar_t* __restrict__ j, distance_type k, double eps = 0 );
3222 double PCL_FUNC OrderStatistic( const unsigned short* __restrict__ i, const unsigned short* __restrict__ j, distance_type k, double eps = 0 );
3223 double PCL_FUNC OrderStatistic( const signed short* __restrict__ i, const signed short* __restrict__ j, distance_type k, double eps = 0 );
3224 double PCL_FUNC OrderStatistic( const unsigned int* __restrict__ i, const unsigned int* __restrict__ j, distance_type k, double eps = 0 );
3225 double PCL_FUNC OrderStatistic( const signed int* __restrict__ i, const signed int* __restrict__ j, distance_type k, double eps = 0 );
3226 double PCL_FUNC OrderStatistic( const unsigned long* __restrict__ i, const unsigned long* __restrict__ j, distance_type k, double eps = 0 );
3227 double PCL_FUNC OrderStatistic( const signed long* __restrict__ i, const signed long* __restrict__ j, distance_type k, double eps = 0 );
3228 double PCL_FUNC OrderStatistic( const unsigned long long* __restrict__ i, const unsigned long long* __restrict__ j, distance_type k, double eps = 0 );
3229 double PCL_FUNC OrderStatistic( const signed long long* __restrict__ i, const signed long long* __restrict__ j, distance_type k, double eps = 0 );
3230 double PCL_FUNC OrderStatistic( const float* __restrict__ i, const float* __restrict__ j, distance_type k, double eps = 0 );
3231 double PCL_FUNC OrderStatistic( const double* __restrict__ i, const double* __restrict__ j, distance_type k, double eps = 0 );
3232 double PCL_FUNC OrderStatistic( const long double* __restrict__ i, const long double* __restrict__ j, distance_type k, double eps = 0 );
3233 
3261 template <typename T> inline double OrderStatisticDestructive( T* __restrict__ i, T* __restrict__ j, distance_type k ) noexcept
3262 {
3263  distance_type n = j - i;
3264  if ( n < 1 || k < 0 || k >= n )
3265  return 0;
3266  if ( n == 1 )
3267  return double( *i );
3268  return double( *pcl::Select( i, j, k ) );
3269 }
3270 
3283 template <typename T, class BP> inline double OrderStatisticDestructive( const T* __restrict__ i, const T* __restrict__ j, distance_type k, BP p ) noexcept
3284 {
3285  distance_type n = j - i;
3286  if ( n < 1 || k < 0 || k >= n )
3287  return 0;
3288  if ( n == 1 )
3289  return double( *i );
3290  return double( *pcl::Select( i, j, k, p ) );
3291 }
3292 
3306 template <typename T> inline double TrimmedMean( const T* __restrict__ i, const T* __restrict__ j, distance_type l = 1, distance_type h = 1 )
3307 {
3308  distance_type n = j - i;
3309  if ( n < 1 )
3310  return 0;
3311  if ( l+h < 1 )
3312  return Sum( i, j )/n;
3313  if ( l+h >= n )
3314  return 0;
3315  for ( double s = 0,
3316  t0 = OrderStatistic( i, j, l ),
3317  t1 = OrderStatistic( i, j, n-h-1 ); ; )
3318  {
3319  double x = double( *i );
3320  if ( x >= t0 )
3321  if ( x <= t1 )
3322  s += x;
3323  if ( ++i == j )
3324  return s/(n - l - h);
3325  }
3326 }
3327 
3345 template <typename T> inline double TrimmedMeanDestructive( T* __restrict__ i, T* __restrict__ j, distance_type l = 1, distance_type h = 1 ) noexcept
3346 {
3347  distance_type n = j - i;
3348  if ( n < 1 )
3349  return 0;
3350  if ( l+h < 1 )
3351  return Sum( i, j )/n;
3352  if ( l+h >= n )
3353  return 0;
3354  for ( double s = 0,
3355  t0 = OrderStatisticDestructive( i, j, l ),
3356  t1 = OrderStatisticDestructive( i, j, n-h-1 ); ; )
3357  {
3358  double x = double( *i );
3359  if ( x >= t0 )
3360  if ( x <= t1 )
3361  s += x;
3362  if ( ++i == j )
3363  return s/(n - l - h);
3364  }
3365 }
3366 
3383 template <typename T> inline double TrimmedMeanOfSquares( const T* __restrict__ i, const T* __restrict__ j, distance_type l = 1, distance_type h = 1 )
3384 {
3385  distance_type n = j - i;
3386  if ( n < 1 )
3387  return 0;
3388  if ( l+h < 1 )
3389  return Sum( i, j )/n;
3390  if ( l+h >= n )
3391  return 0;
3392  for ( double s = 0,
3393  t0 = OrderStatistic( i, j, l ),
3394  t1 = OrderStatistic( i, j, n-h-1 ); ; )
3395  {
3396  double x = double( *i );
3397  if ( x >= t0 )
3398  if ( x <= t1 )
3399  s += x*x;
3400  if ( ++i == j )
3401  return s/(n - l - h);
3402  }
3403 }
3404 
3426 template <typename T> inline double TrimmedMeanOfSquaresDestructive( T* __restrict__ i, T* __restrict__ j, distance_type l = 1, distance_type h = 1 ) noexcept
3427 {
3428  distance_type n = j - i;
3429  if ( n < 1 )
3430  return 0;
3431  if ( l+h < 1 )
3432  return Sum( i, j )/n;
3433  if ( l+h >= n )
3434  return 0;
3435  for ( double s = 0,
3436  t0 = OrderStatisticDestructive( i, j, l ),
3437  t1 = OrderStatisticDestructive( i, j, n-h-1 ); ; )
3438  {
3439  double x = double( *i );
3440  if ( x >= t0 )
3441  if ( x <= t1 )
3442  s += x*x;
3443  if ( ++i == j )
3444  return s/(n - l - h);
3445  }
3446 }
3447 
3468 template <typename T> inline double AvgDev( const T* __restrict__ i, const T* __restrict__ j, double center ) noexcept
3469 {
3470  distance_type n = j - i;
3471  if ( n < 2 )
3472  return 0;
3473  double d = 0;
3474  do
3475  d += Abs( double( *i++ ) - center );
3476  while ( i < j );
3477  return d/n;
3478 }
3479 
3500 template <typename T> inline double StableAvgDev( const T* __restrict__ i, const T* __restrict__ j, double center ) noexcept
3501 {
3502  distance_type n = j - i;
3503  if ( n < 2 )
3504  return 0;
3505  double sum = 0;
3506  double eps = 0;
3507  do
3508  {
3509  double y = Abs( double( *i++ ) - center ) - eps;
3510  double t = sum + y;
3511  eps = (t - sum) - y;
3512  sum = t;
3513  }
3514  while ( i < j );
3515  return sum/n;
3516 }
3517 
3537 template <typename T> inline double AvgDev( const T* __restrict__ i, const T* __restrict__ j )
3538 {
3539  distance_type n = j - i;
3540  if ( n < 2 )
3541  return 0;
3542  double m = Median( i, j );
3543  double d = 0;
3544  do
3545  d += Abs( double( *i++ ) - m );
3546  while ( i < j );
3547  return d/n;
3548 }
3549 
3569 template <typename T> inline double StableAvgDev( const T* __restrict__ i, const T* __restrict__ j )
3570 {
3571  return pcl::StableAvgDev( i, j, pcl::Median( i, j ) );
3572 }
3573 
3591 struct TwoSidedEstimate
3592 {
3593  double low = 0;
3594  double high = 0;
3595 
3599  TwoSidedEstimate() = default;
3600 
3604  TwoSidedEstimate( const TwoSidedEstimate& ) = default;
3605 
3609  TwoSidedEstimate( TwoSidedEstimate&& ) = default;
3610 
3614  TwoSidedEstimate& operator =( const TwoSidedEstimate& ) = default;
3615 
3619  TwoSidedEstimate& operator =( TwoSidedEstimate&& ) = default;
3620 
3624  template <typename T1, typename T2>
3625  TwoSidedEstimate( const T1& l, const T2& h )
3626  : low( double( l ) )
3627  , high( double( h ) )
3628  {
3629  }
3630 
3635  template <typename T>
3636  TwoSidedEstimate( const T& x )
3637  {
3638  low = high = double( x );
3639  }
3640 
3647  bool IsValid() const noexcept
3648  {
3649  return IsFinite( low ) && low > std::numeric_limits<double>::epsilon()
3650  && IsFinite( high ) && high > std::numeric_limits<double>::epsilon();
3651  }
3652 
3656  double Total() const noexcept
3657  {
3658  return low + high;
3659  }
3660 
3665  double Mean() const noexcept
3666  {
3667  if ( low != 0 )
3668  {
3669  if ( high != 0 )
3670  return (low + high)/2;
3671  return low;
3672  }
3673  return high;
3674  }
3675 
3679  explicit operator double() const noexcept
3680  {
3681  return Mean();
3682  }
3683 
3688  TwoSidedEstimate& operator *=( double x ) noexcept
3689  {
3690  low *= x;
3691  high *= x;
3692  return *this;
3693  }
3694 
3698  TwoSidedEstimate& operator /=( double x ) noexcept
3699  {
3700  low /= x;
3701  high /= x;
3702  return *this;
3703  }
3704 
3709  TwoSidedEstimate& operator /=( const TwoSidedEstimate& e ) noexcept
3710  {
3711  low /= e.low;
3712  high /= e.high;
3713  return *this;
3714  }
3715 
3719  TwoSidedEstimate operator *( double x ) const noexcept
3720  {
3721  return { low*x, high*x };
3722  }
3723 
3727  TwoSidedEstimate operator /( double x ) const noexcept
3728  {
3729  return { low/x, high/x };
3730  }
3731 
3736  TwoSidedEstimate operator /( const TwoSidedEstimate& e ) const noexcept
3737  {
3738  return { low/e.low, high/e.high };
3739  }
3740 };
3741 
3746 inline TwoSidedEstimate Sqrt( const TwoSidedEstimate& e ) noexcept
3747 {
3748  return { Sqrt( e.low ), Sqrt( e.high ) };
3749 }
3750 
3755 template <typename T> inline TwoSidedEstimate Pow( const TwoSidedEstimate& e, T x ) noexcept
3756 {
3757  double x_ = double( x );
3758  return { Pow( e.low, x_ ), Pow( e.high, x_ ) };
3759 }
3760 
3781 template <typename T> inline TwoSidedEstimate TwoSidedAvgDev( const T* __restrict__ i, const T* __restrict__ j, double center ) noexcept
3782 {
3783  double dl = 0, dh = 0;
3784  distance_type nl = 0, nh = 0;
3785  while ( i < j )
3786  {
3787  double x = double( *i++ );
3788  if ( x <= center )
3789  {
3790  dl += center - x;
3791  ++nl;
3792  }
3793  else
3794  {
3795  dh += x - center;
3796  ++nh;
3797  }
3798  }
3799  return { (nl > 1) ? dl/nl : 0.0,
3800  (nh > 1) ? dh/nh : 0.0 };
3801 }
3802 
3822 template <typename T> inline TwoSidedEstimate TwoSidedAvgDev( const T* __restrict__ i, const T* __restrict__ j )
3823 {
3824  return pcl::TwoSidedAvgDev( i, j, pcl::Median( i, j ) );
3825 }
3826 
3844 template <typename T> inline double MAD( const T* __restrict__ i, const T* __restrict__ j, double center, double eps = 0 )
3845 {
3846  distance_type n = j - i;
3847  if ( n < 2 )
3848  return 0;
3849  double* d = new double[ n ];
3850  double* p = d;
3851  do
3852  *p++ = Abs( double( *i++ ) - center );
3853  while ( i < j );
3854  double m = pcl::Median( d, d+n );
3855  delete [] d;
3856  return m;
3857 }
3858 
3859 double PCL_FUNC MAD( const unsigned char* __restrict__ i, const unsigned char* __restrict__ j, double center, double eps = 0 );
3860 double PCL_FUNC MAD( const signed char* __restrict__ i, const signed char* __restrict__ j, double center, double eps = 0 );
3861 double PCL_FUNC MAD( const wchar_t* __restrict__ i, const wchar_t* __restrict__ j, double center, double eps = 0 );
3862 double PCL_FUNC MAD( const unsigned short* __restrict__ i, const unsigned short* __restrict__ j, double center, double eps = 0 );
3863 double PCL_FUNC MAD( const signed short* __restrict__ i, const signed short* __restrict__ j, double center, double eps = 0 );
3864 double PCL_FUNC MAD( const unsigned int* __restrict__ i, const unsigned int* __restrict__ j, double center, double eps = 0 );
3865 double PCL_FUNC MAD( const signed int* __restrict__ i, const signed int* __restrict__ j, double center, double eps = 0 );
3866 double PCL_FUNC MAD( const unsigned long* __restrict__ i, const unsigned long* __restrict__ j, double center, double eps = 0 );
3867 double PCL_FUNC MAD( const signed long* __restrict__ i, const signed long* __restrict__ j, double center, double eps = 0 );
3868 double PCL_FUNC MAD( const unsigned long long* __restrict__ i, const unsigned long long* __restrict__ j, double center, double eps = 0 );
3869 double PCL_FUNC MAD( const signed long long* __restrict__ i, const signed long long* __restrict__ j, double center, double eps = 0 );
3870 double PCL_FUNC MAD( const float* __restrict__ i, const float* __restrict__ j, double center, double eps = 0 );
3871 double PCL_FUNC MAD( const double* __restrict__ i, const double* __restrict__ j, double center, double eps = 0 );
3872 double PCL_FUNC MAD( const long double* __restrict__ i, const long double* __restrict__ j, double center, double eps = 0 );
3873 
3891 template <typename T> inline double MAD( const T* __restrict__ i, const T* __restrict__ j, double eps = 0 )
3892 {
3893  return pcl::MAD( i, j, pcl::Median( i, j, eps ), eps );
3894 }
3895 
3913 template <typename T> inline TwoSidedEstimate TwoSidedMAD( const T* __restrict__ i, const T* __restrict__ j, double center, double eps = 0 )
3914 {
3915  distance_type n = j - i;
3916  if ( n < 2 )
3917  return 0;
3918  double* d = new double[ n ];
3919  double* __restrict__ p = d;
3920  double* __restrict__ q = d + n;
3921  do
3922  {
3923  double x = double( *i++ );
3924  if ( x <= center )
3925  *p++ = center - x;
3926  else
3927  *--q = x - center;
3928  }
3929  while( i < j );
3930  double l = pcl::Median( d, p );
3931  double h = pcl::Median( q, d+n );
3932  delete [] d;
3933  return { l, h };
3934 }
3935 
3936 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const unsigned char* __restrict__ i, const unsigned char* __restrict__ j, double center, double eps = 0 );
3937 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const signed char* __restrict__ i, const signed char* __restrict__ j, double center, double eps = 0 );
3938 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const wchar_t* __restrict__ i, const wchar_t* __restrict__ j, double center, double eps = 0 );
3939 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const unsigned short* __restrict__ i, const unsigned short* __restrict__ j, double center, double eps = 0 );
3940 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const signed short* __restrict__ i, const signed short* __restrict__ j, double center, double eps = 0 );
3941 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const unsigned int* __restrict__ i, const unsigned int* __restrict__ j, double center, double eps = 0 );
3942 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const signed int* __restrict__ i, const signed int* __restrict__ j, double center, double eps = 0 );
3943 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const unsigned long* __restrict__ i, const unsigned long* __restrict__ j, double center, double eps = 0 );
3944 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const signed long* __restrict__ i, const signed long* __restrict__ j, double center, double eps = 0 );
3945 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const unsigned long long* __restrict__ i, const unsigned long long* __restrict__ j, double center, double eps = 0 );
3946 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const signed long long* __restrict__ i, const signed long long* __restrict__ j, double center, double eps = 0 );
3947 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const float* __restrict__ i, const float* __restrict__ j, double center, double eps = 0 );
3948 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const double* __restrict__ i, const double* __restrict__ j, double center, double eps = 0 );
3949 TwoSidedEstimate PCL_FUNC TwoSidedMAD( const long double* __restrict__ i, const long double* __restrict__ j, double center, double eps = 0 );
3950 
3968 template <typename T> inline TwoSidedEstimate TwoSidedMAD( const T* __restrict__ i, const T* __restrict__ j, double eps = 0 )
3969 {
3970  return pcl::TwoSidedMAD( i, j, pcl::Median( i, j, eps ), eps );
3971 }
3972 
4004 template <typename T> double Sn( T* __restrict__ x, T* __restrict__ xn )
4005 {
4006  /*
4007  * N.B.: In the code below, lines commented with an asterisk (*) have been
4008  * modified with respect to the FORTRAN original to account for zero-based
4009  * array indices.
4010  */
4011 
4012  distance_type n = xn - x;
4013  if ( n < 2 )
4014  return 0;
4015 
4016  pcl::Sort( x, xn );
4017 
4018  double* a2 = new double[ n ];
4019  a2[0] = double( x[n >> 1] ) - double( x[0] ); // *
4020 
4021  distance_type nh = (n + 1) >> 1;
4022 
4023  for ( distance_type i = 2; i <= nh; ++i )
4024  {
4025  distance_type nA = i-1;
4026  distance_type nB = n - i;
4027  distance_type diff = nB - nA;
4028  distance_type leftA = 1;
4029  distance_type leftB = 1;
4030  distance_type rightA = nB;
4031  distance_type Amin = (diff >> 1) + 1;
4032  distance_type Amax = (diff >> 1) + nA;
4033 
4034  while ( leftA < rightA )
4035  {
4036  distance_type length = rightA - leftA + 1;
4037  distance_type even = (length & 1) == 0;
4038  distance_type half = (length - 1) >> 1;
4039  distance_type tryA = leftA + half;
4040  distance_type tryB = leftB + half;
4041 
4042  if ( tryA < Amin )
4043  leftA = tryA + even;
4044  else
4045  {
4046  if ( tryA > Amax )
4047  {
4048  rightA = tryA;
4049  leftB = tryB + even;
4050  }
4051  else
4052  {
4053  double medA = double( x[i-1] ) - double( x[i-2 - tryA + Amin] ); // *
4054  double medB = double( x[tryB + i-1] ) - double( x[i-1] ); // *
4055  if ( medA >= medB )
4056  {
4057  rightA = tryA;
4058  leftB = tryB + even;
4059  }
4060  else
4061  leftA = tryA + even;
4062  }
4063  }
4064  }
4065 
4066  if ( leftA > Amax )
4067  a2[i-1] = double( x[leftB + i-1] ) - double( x[i-1] ); // *
4068  else
4069  {
4070  double medA = double( x[i-1] ) - double( x[i-2 - leftA + Amin] ); // *
4071  double medB = double( x[leftB + i-1] ) - double( x[i-1] );
4072  a2[i-1] = pcl::Min( medA, medB ); // *
4073  }
4074  }
4075 
4076  for ( distance_type i = nh + 1; i < n; ++i )
4077  {
4078  distance_type nA = n - i;
4079  distance_type nB = i - 1;
4080  distance_type diff = nB - nA;
4081  distance_type leftA = 1;
4082  distance_type leftB = 1;
4083  distance_type rightA = nB;
4084  distance_type Amin = (diff >> 1) + 1;
4085  distance_type Amax = (diff >> 1) + nA;
4086 
4087  while ( leftA < rightA )
4088  {
4089  distance_type length = rightA - leftA + 1;
4090  distance_type even = (length & 1) == 0;
4091  distance_type half = (length - 1) >> 1;
4092  distance_type tryA = leftA + half;
4093  distance_type tryB = leftB + half;
4094 
4095  if ( tryA < Amin)
4096  leftA = tryA + even;
4097  else
4098  {
4099  if ( tryA > Amax )
4100  {
4101  rightA = tryA;
4102  leftB = tryB + even;
4103  }
4104  else
4105  {
4106  double medA = double( x[i + tryA - Amin] ) - double( x[i-1] ); // *
4107  double medB = double( x[i-1] ) - double( x[i-1 - tryB] ); // *
4108  if ( medA >= medB )
4109  {
4110  rightA = tryA;
4111  leftB = tryB + even;
4112  }
4113  else
4114  leftA = tryA + even;
4115  }
4116  }
4117  }
4118 
4119  if ( leftA > Amax )
4120  a2[i-1] = double( x[i-1] ) - double( x[i-1 - leftB] ); // *
4121  else
4122  {
4123  double medA = double( x[i + leftA - Amin] ) - double( x[i-1] ); // *
4124  double medB = double( x[i-1] ) - double( x[i-1 - leftB] ); // *
4125  a2[i-1] = pcl::Min( medA, medB ); // *
4126  }
4127  }
4128 
4129  a2[n-1] = double( x[n-1] ) - double( x[nh-1] ); // *
4130 
4131  /*
4132  * Correction for a finite sample
4133  */
4134  double cn;
4135  switch ( n )
4136  {
4137  case 2: cn = 0.743; break;
4138  case 3: cn = 1.851; break;
4139  case 4: cn = 0.954; break;
4140  case 5: cn = 1.351; break;
4141  case 6: cn = 0.993; break;
4142  case 7: cn = 1.198; break;
4143  case 8: cn = 1.005; break;
4144  case 9: cn = 1.131; break;
4145  default: cn = (n & 1) ? n/(n - 0.9) : 1.0; break;
4146  }
4147 
4148  double sn = cn * *pcl::Select( a2, a2+n, nh-1 );
4149 
4150  delete [] a2;
4151  return sn;
4152 }
4153 
4164 inline double __pcl_whimed( double* a, distance_type* iw, distance_type n,
4165  double* acand, distance_type* iwcand )
4166 {
4167  distance_type wtotal = 0;
4168  for ( distance_type i = 0; i < n; ++i )
4169  wtotal += iw[i];
4170 
4171  for ( distance_type nn = n, wrest = 0; ; )
4172  {
4173  double trial = *pcl::Select( a, a+nn, nn >> 1 ); // *
4174 
4175  distance_type wleft = 0;
4176  distance_type wmid = 0;
4177  for ( distance_type i = 0; i < nn; ++i )
4178  if ( a[i] < trial )
4179  wleft += iw[i];
4180  else if ( a[i] == trial )
4181  wmid += iw[i];
4182 
4183  if ( 2*(wrest + wleft) > wtotal )
4184  {
4185  distance_type kcand = 0;
4186  for ( distance_type i = 0; i < nn; ++i )
4187  if ( a[i] < trial )
4188  {
4189  acand[kcand] = a[i];
4190  iwcand[kcand] = iw[i];
4191  ++kcand;
4192  }
4193  nn = kcand;
4194  }
4195  else
4196  {
4197  if ( 2*(wrest + wleft + wmid) > wtotal )
4198  return trial;
4199 
4200  distance_type kcand = 0;
4201  for ( distance_type i = 0; i < nn; ++i )
4202  if ( a[i] > trial )
4203  {
4204  acand[kcand] = a[i];
4205  iwcand[kcand] = iw[i];
4206  ++kcand;
4207  }
4208  nn = kcand;
4209  wrest += wleft + wmid;
4210  }
4211 
4212  for ( distance_type i = 0; i < nn; ++i )
4213  {
4214  a[i] = acand[i];
4215  iw[i] = iwcand[i];
4216  }
4217  }
4218 }
4219 
4250 template <typename T> double Qn( T* __restrict__ x, T* __restrict__ xn )
4251 {
4252  distance_type n = xn - x;
4253  if ( n < 2 )
4254  return 0;
4255 
4256  double* y = new double[ n ];
4257  double* work = new double[ n ];
4258  double* acand = new double[ n ];
4259  distance_type* iwcand = new distance_type[ n ];
4260  distance_type* left = new distance_type[ n ];
4261  distance_type* right = new distance_type[ n ];
4262  distance_type* P = new distance_type[ n ];
4263  distance_type* Q = new distance_type[ n ];
4264  distance_type* weight = new distance_type[ n ];
4265 
4266  distance_type h = (n >> 1) + 1;
4267  distance_type k = (h*(h - 1)) >> 1;
4268  for ( distance_type i = 0; i < n; ++i )
4269  {
4270  y[i] = double( x[i] );
4271  left[i] = n - i + 1; // *
4272  right[i] = (i <= h) ? n : n - i + h; // N.B. The original code is "right[i] = n"
4273  }
4274 
4275  pcl::Sort( y, y+n );
4276 
4277  distance_type nL = (n*(n + 1)) >> 1;
4278  distance_type nR = n*n;
4279  distance_type knew = k + nL;
4280 
4281  bool found = false;
4282  double qn;
4283 
4284  while ( nR-nL > n )
4285  {
4286  distance_type j = 0; // *
4287  for ( distance_type i = 1; i < n; ++i ) // *
4288  if ( left[i] <= right[i] )
4289  {
4290  weight[j] = right[i] - left[i] + 1;
4291  work[j] = double( y[i] ) - y[n - left[i] - (weight[j] >> 1)];
4292  ++j;
4293  }
4294  qn = __pcl_whimed( work, weight, j, acand, iwcand );
4295 
4296  for ( distance_type i = n-1, j = 0; i >= 0; --i ) // *
4297  {
4298  while ( j < n && double( y[i] ) - y[n-j-1] < qn )
4299  ++j;
4300  P[i] = j;
4301  }
4302 
4303  for ( distance_type i = 0, j = n+1; i < n; ++i ) // *
4304  {
4305  while ( double( y[i] ) - y[n-j+1] > qn )
4306  --j;
4307  Q[i] = j;
4308  }
4309 
4310  double sumP = 0;
4311  double sumQ = 0;
4312  for ( distance_type i = 0; i < n; ++i )
4313  {
4314  sumP += P[i];
4315  sumQ += Q[i] - 1;
4316  }
4317 
4318  if ( knew <= sumP )
4319  {
4320  for ( distance_type i = 0; i < n; ++i )
4321  right[i] = P[i];
4322  nR = sumP;
4323  }
4324  else if ( knew > sumQ )
4325  {
4326  for ( distance_type i = 0; i < n; ++i )
4327  left[i] = Q[i];
4328  nL = sumQ;
4329  }
4330  else
4331  {
4332  found = true;
4333  break;
4334  }
4335  }
4336 
4337  if ( !found )
4338  {
4339  distance_type j = 0;
4340  for ( distance_type i = 1; i < n; ++i )
4341  for ( distance_type jj = left[i]; jj <= right[i]; ++jj, ++j )
4342  work[j] = double( y[i] ) - y[n-jj]; // *
4343  qn = *pcl::Select( work, work+j, knew-nL-1 ); // *
4344  }
4345 
4346  /*
4347  * Correction for a finite sample
4348  */
4349  double dn;
4350  switch ( n )
4351  {
4352  case 2: dn = 0.399; break;
4353  case 3: dn = 0.994; break;
4354  case 4: dn = 0.512; break;
4355  case 5: dn = 0.844; break;
4356  case 6: dn = 0.611; break;
4357  case 7: dn = 0.857; break;
4358  case 8: dn = 0.669; break;
4359  case 9: dn = 0.872; break;
4360  default: dn = (n & 1) ? n/(n + 1.4) : n/(n + 3.8); break;
4361  }
4362  qn *= dn;
4363 
4364  delete [] y;
4365  delete [] work;
4366  delete [] acand;
4367  delete [] iwcand;
4368  delete [] left;
4369  delete [] right;
4370  delete [] P;
4371  delete [] Q;
4372  delete [] weight;
4373 
4374  return qn;
4375 }
4376 
4420 template <typename T>
4421 double BiweightMidvariance( const T* __restrict__ x, const T* __restrict__ xn, double center,
4422  double sigma, int k = 9, bool reducedLength = false ) noexcept
4423 {
4424  distance_type n = xn - x;
4425  if ( n < 2 )
4426  return 0;
4427 
4428  double kd = k * sigma;
4429  if ( kd < 0 || 1 + kd == 1 )
4430  return 0;
4431 
4432  double num = 0, den = 0;
4433  distance_type nr = 0;
4434  for ( ; x < xn; ++x )
4435  {
4436  double xc = double( *x ) - center;
4437  double y = xc/kd;
4438  if ( Abs( y ) < 1 )
4439  {
4440  double y2 = y*y;
4441  double y21 = 1 - y2;
4442  num += xc*xc * y21*y21*y21*y21;
4443  den += y21 * (1 - 5*y2);
4444  ++nr;
4445  }
4446  }
4447 
4448  den *= den;
4449  return (1 + den != 1) ? (reducedLength ? nr : n)*num/den : 0.0;
4450 }
4451 
4483 template <typename T>
4484 TwoSidedEstimate TwoSidedBiweightMidvariance( const T* __restrict__ x, const T* __restrict__ xn, double center,
4485  const TwoSidedEstimate& sigma, int k = 9, bool reducedLength = false ) noexcept
4486 {
4487  double kd0 = k * sigma.low;
4488  double kd1 = k * sigma.high;
4489  if ( kd0 < 0 || 1 + kd0 == 1 || kd1 < 0 || 1 + kd1 == 1 )
4490  return 0;
4491 
4492  double num0 = 0, den0 = 0, num1 = 0, den1 = 0;
4493  size_type n0 = 0, n1 = 0, nr0 = 0, nr1 = 0;
4494  for ( ; x < xn; ++x )
4495  {
4496  double xc = double( *x ) - center;
4497  bool low = xc <= 0;
4498  if ( low )
4499  ++n0;
4500  else
4501  ++n1;
4502 
4503  double y = xc/(low ? kd0 : kd1);
4504  if ( pcl::Abs( y ) < 1 )
4505  {
4506  double y2 = y*y;
4507  double y21 = 1 - y2;
4508  double num = xc*xc * y21*y21*y21*y21;
4509  double den = y21 * (1 - 5*y2);
4510  if ( low )
4511  {
4512  num0 += num;
4513  den0 += den;
4514  ++nr0;
4515  }
4516  else
4517  {
4518  num1 += num;
4519  den1 += den;
4520  ++nr1;
4521  }
4522  }
4523  }
4524 
4525  den0 *= den0;
4526  den1 *= den1;
4527  return { (n0 >= 2 && 1 + den0 != 1) ? (reducedLength ? nr0 : n0)*num0/den0 : 0.0,
4528  (n1 >= 2 && 1 + den1 != 1) ? (reducedLength ? nr1 : n1)*num1/den1 : 0.0 };
4529 }
4530 
4559 template <typename T>
4560 double BendMidvariance( const T* __restrict__ x, const T* __restrict__ xn, double center, double beta = 0.2 )
4561 {
4562  distance_type n = xn - x;
4563  if ( n < 2 )
4564  return 0;
4565 
4566  beta = Range( beta, 0.0, 0.5 );
4567  distance_type m = Floor( (1 - beta)*n + 0.5 );
4568 
4569  double* w = new double[ n ];
4570  for ( distance_type i = 0; i < n; ++i )
4571  w[i] = Abs( double( x[i] ) - center );
4572  double wb = *pcl::Select( w, w+n, m );
4573  delete [] w;
4574  if ( 1 + wb == 1 )
4575  return 0;
4576 
4577  double num = 0;
4578  distance_type den = 0;
4579  for ( ; x < xn; ++x )
4580  {
4581  double y = (double( *x ) - center)/wb;
4582  double f = Max( -1.0, Min( 1.0, y ) );
4583  num += f*f;
4584  if ( Abs( y ) < 1 )
4585  ++den;
4586  }
4587 
4588  den *= den;
4589  return (1 + den != 1) ? n*wb*wb*num/den : 0.0;
4590 }
4591 
4592 // ----------------------------------------------------------------------------
4593 
4620 inline double IncompleteBeta( double a, double b, double x, double eps = 1.0e-8 ) noexcept
4621 {
4622  if ( x < 0 || x > 1 )
4623  return std::numeric_limits<double>::infinity();
4624 
4625  /*
4626  * The continued fraction converges nicely for x < (a+1)/(a+b+2)
4627  */
4628  if ( x > (a + 1)/(a + b + 2) )
4629  return 1 - IncompleteBeta( b, a, 1 - x ); // Use the fact that beta is symmetric
4630 
4631  /*
4632  * Find the first part before the continued fraction.
4633  */
4634  double lbeta_ab = lgamma( a ) + lgamma( b ) - lgamma( a + b );
4635  double front = Exp( Ln( x )*a + Ln( 1 - x )*b - lbeta_ab )/a;
4636 
4637  /*
4638  * Use Lentz's algorithm to evaluate the continued fraction.
4639  */
4640  const double tiny = 1.0e-30;
4641  double f = 1, c = 1, d = 0;
4642  for ( int i = 0; i <= 200; ++i )
4643  {
4644  int m = i >> 1;
4645  double numerator;
4646  if ( i & 1 )
4647  numerator = -((a + m)*(a + b + m)*x)/((a + 2*m)*(a + 2*m + 1)); // Odd term
4648  else if ( i > 0 )
4649  numerator = (m*(b - m)*x)/((a + 2*m - 1)*(a + 2*m)); // Even term
4650  else
4651  numerator = 1; // First numerator is 1.0
4652 
4653  /*
4654  * Do an iteration of Lentz's algorithm.
4655  */
4656  d = 1 + numerator*d;
4657  if ( Abs( d ) < tiny )
4658  d = tiny;
4659  d = 1/d;
4660  c = 1 + numerator/c;
4661  if ( Abs( c ) < tiny )
4662  c = tiny;
4663  double cd = c*d;
4664  f *= cd;
4665  if ( Abs( 1 - cd ) < eps )
4666  return front*(f - 1);
4667  }
4668 
4669  // Needed more loops, did not converge.
4670  return std::numeric_limits<double>::infinity();
4671 }
4672 
4673 // ----------------------------------------------------------------------------
4674 
4679 template <typename T> inline constexpr T Erf( T x ) noexcept
4680 {
4681  return std::erf( x );
4682 }
4683 
4696 template <typename T> inline constexpr T ErfInv( T x ) noexcept
4697 {
4698  if ( x < -1 || x > 1 )
4699  return std::numeric_limits<T>::quiet_NaN();
4700  if ( x == 1 )
4701  return std::numeric_limits<T>::infinity();
4702  if ( x == -1 )
4703  return -std::numeric_limits<T>::infinity();
4704 
4705  const long double A0 = 1.1975323115670912564578e0L;
4706  const long double A1 = 4.7072688112383978012285e1L;
4707  const long double A2 = 6.9706266534389598238465e2L;
4708  const long double A3 = 4.8548868893843886794648e3L;
4709  const long double A4 = 1.6235862515167575384252e4L;
4710  const long double A5 = 2.3782041382114385731252e4L;
4711  const long double A6 = 1.1819493347062294404278e4L;
4712  const long double A7 = 8.8709406962545514830200e2L;
4713 
4714  const long double B0 = 1.0000000000000000000e0L;
4715  const long double B1 = 4.2313330701600911252e1L;
4716  const long double B2 = 6.8718700749205790830e2L;
4717  const long double B3 = 5.3941960214247511077e3L;
4718  const long double B4 = 2.1213794301586595867e4L;
4719  const long double B5 = 3.9307895800092710610e4L;
4720  const long double B6 = 2.8729085735721942674e4L;
4721  const long double B7 = 5.2264952788528545610e3L;
4722 
4723  const long double C0 = 1.42343711074968357734e0L;
4724  const long double C1 = 4.63033784615654529590e0L;
4725  const long double C2 = 5.76949722146069140550e0L;
4726  const long double C3 = 3.64784832476320460504e0L;
4727  const long double C4 = 1.27045825245236838258e0L;
4728  const long double C5 = 2.41780725177450611770e-1L;
4729  const long double C6 = 2.27238449892691845833e-2L;
4730  const long double C7 = 7.74545014278341407640e-4L;
4731 
4732  const long double D0 = 1.4142135623730950488016887e0L;
4733  const long double D1 = 2.9036514445419946173133295e0L;
4734  const long double D2 = 2.3707661626024532365971225e0L;
4735  const long double D3 = 9.7547832001787427186894837e-1L;
4736  const long double D4 = 2.0945065210512749128288442e-1L;
4737  const long double D5 = 2.1494160384252876777097297e-2L;
4738  const long double D6 = 7.7441459065157709165577218e-4L;
4739  const long double D7 = 1.4859850019840355905497876e-9L;
4740 
4741  const long double E0 = 6.65790464350110377720e0L;
4742  const long double E1 = 5.46378491116411436990e0L;
4743  const long double E2 = 1.78482653991729133580e0L;
4744  const long double E3 = 2.96560571828504891230e-1L;
4745  const long double E4 = 2.65321895265761230930e-2L;
4746  const long double E5 = 1.24266094738807843860e-3L;
4747  const long double E6 = 2.71155556874348757815e-5L;
4748  const long double E7 = 2.01033439929228813265e-7L;
4749 
4750  const long double F0 = 1.414213562373095048801689e0L;
4751  const long double F1 = 8.482908416595164588112026e-1L;
4752  const long double F2 = 1.936480946950659106176712e-1L;
4753  const long double F3 = 2.103693768272068968719679e-2L;
4754  const long double F4 = 1.112800997078859844711555e-3L;
4755  const long double F5 = 2.611088405080593625138020e-5L;
4756  const long double F6 = 2.010321207683943062279931e-7L;
4757  const long double F7 = 2.891024605872965461538222e-15L;
4758 
4759  long double abs_x = Abs( x );
4760 
4761  if ( abs_x <= 0.85L )
4762  {
4763  long double r = 0.180625L - 0.25L * x*x;
4764  long double num = (((((((A7*r + A6)*r + A5)*r + A4)*r + A3)*r + A2)*r + A1)*r + A0);
4765  long double den = (((((((B7*r + B6)*r + B5)*r + B4)*r + B3)*r + B2)*r + B1)*r + B0);
4766  return T( x * num/den );
4767  }
4768 
4769  long double r = Sqrt( Ln2() - Ln( 1 - abs_x ) );
4770  long double num = 0, den = 0;
4771  if ( r <= 5 )
4772  {
4773  r = r - 1.6L;
4774  num = (((((((C7*r + C6)*r + C5)*r + C4)*r + C3)*r + C2)*r + C1)*r + C0);
4775  den = (((((((D7*r + D6)*r + D5)*r + D4)*r + D3)*r + D2)*r + D1)*r + D0);
4776  }
4777  else
4778  {
4779  r = r - 5.0L;
4780  num = (((((((E7*r + E6)*r + E5)*r + E4)*r + E3)*r + E2)*r + E1)*r + E0);
4781  den = (((((((F7*r + F6)*r + F5)*r + F4)*r + F3)*r + F2)*r + F1)*r + F0);
4782  }
4783 
4784  return T( std::copysign( num/den, x ) );
4785 }
4786 
4787 // ----------------------------------------------------------------------------
4788 
4830 inline uint64 Hash64( const void* data, size_type size, uint64 seed = 0 ) noexcept
4831 {
4832 #define PRIME64_1 11400714785074694791ULL
4833 #define PRIME64_2 14029467366897019727ULL
4834 #define PRIME64_3 1609587929392839161ULL
4835 #define PRIME64_4 9650029242287828579ULL
4836 #define PRIME64_5 2870177450012600261ULL
4837 
4838  const uint8* p = (const uint8*)data;
4839  const uint8* end = p + size;
4840  uint64 h64;
4841 
4842  if ( seed == 0 )
4843  seed = size;
4844 
4845  if ( size >= 32 )
4846  {
4847  const uint8* limit = end - 32;
4848  uint64 v1 = seed + PRIME64_1 + PRIME64_2;
4849  uint64 v2 = seed + PRIME64_2;
4850  uint64 v3 = seed + 0;
4851  uint64 v4 = seed - PRIME64_1;
4852 
4853  do
4854  {
4855  v1 += *(uint64*)p * PRIME64_2;
4856  p += 8;
4857  v1 = RotL( v1, 31 );
4858  v1 *= PRIME64_1;
4859  v2 += *(uint64*)p * PRIME64_2;
4860  p += 8;
4861  v2 = RotL( v2, 31 );
4862  v2 *= PRIME64_1;
4863  v3 += *(uint64*)p * PRIME64_2;
4864  p += 8;
4865  v3 = RotL( v3, 31 );
4866  v3 *= PRIME64_1;
4867  v4 += *(uint64*)p * PRIME64_2;
4868  p += 8;
4869  v4 = RotL( v4, 31 );
4870  v4 *= PRIME64_1;
4871  }
4872  while ( p <= limit );
4873 
4874  h64 = RotL( v1, 1 ) + RotL( v2, 7 ) + RotL( v3, 12 ) + RotL( v4, 18 );
4875 
4876  v1 *= PRIME64_2;
4877  v1 = RotL( v1, 31 );
4878  v1 *= PRIME64_1;
4879  h64 ^= v1;
4880  h64 = h64 * PRIME64_1 + PRIME64_4;
4881 
4882  v2 *= PRIME64_2;
4883  v2 = RotL( v2, 31 );
4884  v2 *= PRIME64_1;
4885  h64 ^= v2;
4886  h64 = h64 * PRIME64_1 + PRIME64_4;
4887 
4888  v3 *= PRIME64_2;
4889  v3 = RotL( v3, 31 );
4890  v3 *= PRIME64_1;
4891  h64 ^= v3;
4892  h64 = h64 * PRIME64_1 + PRIME64_4;
4893 
4894  v4 *= PRIME64_2;
4895  v4 = RotL( v4, 31 );
4896  v4 *= PRIME64_1;
4897  h64 ^= v4;
4898  h64 = h64 * PRIME64_1 + PRIME64_4;
4899  }
4900  else
4901  {
4902  h64 = seed + PRIME64_5;
4903  }
4904 
4905  h64 += size;
4906 
4907  while ( p+8 <= end )
4908  {
4909  uint64 k1 = *(uint64*)p;
4910  k1 *= PRIME64_2;
4911  k1 = RotL( k1, 31 );
4912  k1 *= PRIME64_1;
4913  h64 ^= k1;
4914  h64 = RotL( h64, 27 ) * PRIME64_1 + PRIME64_4;
4915  p += 8;
4916  }
4917 
4918  if ( p+4 <= end )
4919  {
4920  h64 ^= (uint64)(*(uint32*)p) * PRIME64_1;
4921  h64 = RotL( h64, 23 ) * PRIME64_2 + PRIME64_3;
4922  p += 4;
4923  }
4924 
4925  while ( p < end )
4926  {
4927  h64 ^= *p * PRIME64_5;
4928  h64 = RotL( h64, 11 ) * PRIME64_1;
4929  ++p;
4930  }
4931 
4932  h64 ^= h64 >> 33;
4933  h64 *= PRIME64_2;
4934  h64 ^= h64 >> 29;
4935  h64 *= PRIME64_3;
4936  h64 ^= h64 >> 32;
4937 
4938  return h64;
4939 
4940 #undef PRIME64_1
4941 #undef PRIME64_2
4942 #undef PRIME64_3
4943 #undef PRIME64_4
4944 #undef PRIME64_5
4945 }
4946 
4984 inline uint32 Hash32( const void* data, size_type size, uint32 seed = 0 ) noexcept
4985 {
4986 #define PRIME32_1 2654435761U
4987 #define PRIME32_2 2246822519U
4988 #define PRIME32_3 3266489917U
4989 #define PRIME32_4 668265263U
4990 #define PRIME32_5 374761393U
4991 
4992  const uint8* p = (const uint8*)data;
4993  const uint8* end = p + size;
4994  uint32 h32;
4995 
4996  if ( seed == 0 )
4997  seed = uint32( size );
4998 
4999  if ( size >= 16 )
5000  {
5001  const uint8* limit = end - 16;
5002  uint32 v1 = seed + PRIME32_1 + PRIME32_2;
5003  uint32 v2 = seed + PRIME32_2;
5004  uint32 v3 = seed + 0;
5005  uint32 v4 = seed - PRIME32_1;
5006 
5007  do
5008  {
5009  v1 += *(uint32*)p * PRIME32_2;
5010  v1 = RotL( v1, 13 );
5011  v1 *= PRIME32_1;
5012  p += 4;
5013  v2 += *(uint32*)p * PRIME32_2;
5014  v2 = RotL( v2, 13 );
5015  v2 *= PRIME32_1;
5016  p += 4;
5017  v3 += *(uint32*)p * PRIME32_2;
5018  v3 = RotL( v3, 13 );
5019  v3 *= PRIME32_1;
5020  p += 4;
5021  v4 += *(uint32*)p * PRIME32_2;
5022  v4 = RotL( v4, 13 );
5023  v4 *= PRIME32_1;
5024  p += 4;
5025  }
5026  while ( p <= limit );
5027 
5028  h32 = RotL( v1, 1 ) + RotL( v2, 7 ) + RotL( v3, 12 ) + RotL( v4, 18 );
5029  }
5030  else
5031  {
5032  h32 = seed + PRIME32_5;
5033  }
5034 
5035  h32 += (uint32)size;
5036 
5037  while ( p+4 <= end )
5038  {
5039  h32 += *(uint32*)p * PRIME32_3;
5040  h32 = RotL( h32, 17 ) * PRIME32_4 ;
5041  p+=4;
5042  }
5043 
5044  while ( p < end )
5045  {
5046  h32 += *p * PRIME32_5;
5047  h32 = RotL( h32, 11 ) * PRIME32_1 ;
5048  ++p;
5049  }
5050 
5051  h32 ^= h32 >> 15;
5052  h32 *= PRIME32_2;
5053  h32 ^= h32 >> 13;
5054  h32 *= PRIME32_3;
5055  h32 ^= h32 >> 16;
5056 
5057  return h32;
5058 
5059 #undef PRIME32_1
5060 #undef PRIME32_2
5061 #undef PRIME32_3
5062 #undef PRIME32_4
5063 #undef PRIME32_5
5064 }
5065 
5066 // ----------------------------------------------------------------------------
5067 
5068 } // pcl
5069 
5070 #endif // __PCL_Math_h
5071 
5072 // ----------------------------------------------------------------------------
5073 // EOF pcl/Math.h - Released 2024-12-28T16:53:48Z
pcl::Pow
Complex< T1 > Pow(const Complex< T1 > &c, T2 x) noexcept
Definition: Complex.h:761
pcl::Sqrt
Complex< T > Sqrt(const Complex< T > &c) noexcept
Definition: Complex.h:688
pcl::Exp
Complex< T > Exp(const Complex< T > &c) noexcept
Definition: Complex.h:728
pcl::Log
Complex< T > Log(const Complex< T > &c) noexcept
Definition: Complex.h:749
pcl::Ln
Complex< T > Ln(const Complex< T > &c) noexcept
Definition: Complex.h:739
pcl::Abs
T Abs(const Complex< T > &c) noexcept
Definition: Complex.h:443
pcl::Round
Complex< T > Round(const Complex< T > &c) noexcept
Definition: Complex.h:952
pcl::Sinh
Complex< T > Sinh(const Complex< T > &c) noexcept
Definition: Complex.h:841
pcl::Tanh
Complex< T > Tanh(const Complex< T > &c) noexcept
Definition: Complex.h:863
pcl::Sin
Complex< T > Sin(const Complex< T > &c) noexcept
Definition: Complex.h:809
pcl::Cosh
Complex< T > Cosh(const Complex< T > &c) noexcept
Definition: Complex.h:852
pcl::Cos
Complex< T > Cos(const Complex< T > &c) noexcept
Definition: Complex.h:820
pcl::Tan
Complex< T > Tan(const Complex< T > &c) noexcept
Definition: Complex.h:831
pcl::IsNegativeZero
bool IsNegativeZero(float x) noexcept
Definition: Math.h:247
pcl::IsFinite
bool IsFinite(float x) noexcept
Definition: Math.h:194
pcl::IsInfinity
int IsInfinity(float x) noexcept
Definition: Math.h:232
pcl::IsNaN
bool IsNaN(float x) noexcept
Definition: Math.h:210
pcl::Hash64
uint64 Hash64(const void *data, size_type size, uint64 seed=0) noexcept
Definition: Math.h:4830
pcl::Hash32
uint32 Hash32(const void *data, size_type size, uint32 seed=0) noexcept
Definition: Math.h:4984
pcl::MaxSSEInstructionSetSupported
int MaxSSEInstructionSetSupported() noexcept
Definition: Math.h:143
int64_max
#define int64_max
Definition: Defs.h:888
int64_min
#define int64_min
Definition: Defs.h:882
pcl::RadSec
constexpr T RadSec(T x) noexcept
Definition: Math.h:1947
pcl::SinCos
void SinCos(T x, T &sx, T &cx) noexcept
Definition: Math.h:1101
pcl::L1Norm
T L1Norm(const T *i, const T *j) noexcept
Definition: Math.h:2225
pcl::Mod2Pi
constexpr T Mod2Pi(T x) noexcept
Definition: Math.h:2024
pcl::SignChar
constexpr char SignChar(T x) noexcept
Definition: Math.h:1040
pcl::AsRad
constexpr T AsRad(T x) noexcept
Definition: Math.h:1978
pcl::ArcTan
constexpr T ArcTan(T x) noexcept
Definition: Math.h:597
pcl::Log2T
constexpr long double Log2T() noexcept
Definition: Math.h:933
pcl::LogN
constexpr T LogN(T n, T x) noexcept
Definition: Math.h:946
pcl::Norm2Pi
constexpr T Norm2Pi(T x) noexcept
Definition: Math.h:2035
pcl::RotL
T RotL(T x, uint32 n) noexcept
Definition: Math.h:1377
pcl::ModPi
constexpr T ModPi(T x) noexcept
Definition: Math.h:2011
pcl::Sign
constexpr int Sign(T x) noexcept
Definition: Math.h:1022
pcl::Rotate
void Rotate(T &x, T &y, T1 sa, T1 ca, T2 xc, T2 yc) noexcept
Definition: Math.h:2055
pcl::Hav
constexpr T Hav(T x) noexcept
Definition: Math.h:740
pcl::CalendarTimeToJD
void PCL_FUNC CalendarTimeToJD(int &jdi, double &jdf, int year, int month, int day, double dayf=0) noexcept
pcl::Frexp
void Frexp(T x, T &m, int &p) noexcept
Definition: Math.h:724
pcl::ArcCosh
constexpr T ArcCosh(T x) noexcept
Definition: Math.h:1876
pcl::Frac
constexpr T Frac(T x) noexcept
Definition: Math.h:711
pcl::SecRad
constexpr T SecRad(T x) noexcept
Definition: Math.h:1969
pcl::Pow10
T Pow10(T x) noexcept
Definition: Math.h:1359
pcl::Ceil
constexpr T Ceil(T x) noexcept
Definition: Math.h:633
pcl::ArcTanh
constexpr T ArcTanh(T x) noexcept
Definition: Math.h:1894
pcl::TwoPi
constexpr long double TwoPi() noexcept
Definition: Math.h:540
pcl::Split
void Split(T x, T &i, T &f) noexcept
Definition: Math.h:1122
pcl::MasRad
constexpr T MasRad(T x) noexcept
Definition: Math.h:1989
pcl::ArcTan2Pi
T ArcTan2Pi(T y, T x) noexcept
Definition: Math.h:619
pcl::LnFactorial
double LnFactorial(int n) noexcept
Definition: Math.h:803
pcl::Cotan
constexpr T Cotan(T x) noexcept
Definition: Math.h:666
pcl::RoundIntArithmetic
int RoundIntArithmetic(T x) noexcept
Definition: Math.h:1617
pcl::PowI4
T PowI4(T x) noexcept
Definition: Math.h:1808
pcl::RoundInt64
int64 RoundInt64(double x) noexcept
Definition: Math.h:1651
pcl::UasRad
constexpr T UasRad(T x) noexcept
Definition: Math.h:2000
pcl::Trunc
T Trunc(T x) noexcept
Definition: Math.h:1166
pcl::TruncI64
int64 TruncI64(T x) noexcept
Definition: Math.h:1278
pcl::SexagesimalToDecimal
double SexagesimalToDecimal(int sign, const S1 &s1, const S2 &s2=S2(0), const S3 &s3=S3(0)) noexcept
Definition: Math.h:2578
pcl::PowI12
T PowI12(T x) noexcept
Definition: Math.h:1844
pcl::JDToCalendarTime
void PCL_FUNC JDToCalendarTime(int &year, int &month, int &day, double &dayf, int jdi, double jdf) noexcept
pcl::RadMin
constexpr T RadMin(T x) noexcept
Definition: Math.h:1936
pcl::PowI
T PowI(T x, int n) noexcept
Definition: Math.h:1786
pcl::ArcSinh
constexpr T ArcSinh(T x) noexcept
Definition: Math.h:1858
pcl::Log2
constexpr long double Log2() noexcept
Definition: Math.h:893
pcl::Ldexp
constexpr T Ldexp(T m, int p) noexcept
Definition: Math.h:751
pcl::ArcSin
constexpr T ArcSin(T x) noexcept
Definition: Math.h:585
pcl::Rad
constexpr T Rad(T x) noexcept
Definition: Math.h:1925
pcl::Ln2
constexpr long double Ln2() noexcept
Definition: Math.h:870
pcl::Factorial
double Factorial(int n) noexcept
Definition: Math.h:783
pcl::ErfInv
constexpr T ErfInv(T x) noexcept
Definition: Math.h:4696
pcl::Angle
constexpr T Angle(int d, T m) noexcept
Definition: Math.h:552
pcl::DecimalToSexagesimal
void DecimalToSexagesimal(int &sign, S1 &s1, S2 &s2, S3 &s3, const D &d) noexcept
Definition: Math.h:2556
pcl::Erf
constexpr T Erf(T x) noexcept
Definition: Math.h:4679
pcl::PowI8
T PowI8(T x) noexcept
Definition: Math.h:1826
pcl::Poly
T Poly(T x, C c, int n) noexcept
Definition: Math.h:979
pcl::ArcCos
constexpr T ArcCos(T x) noexcept
Definition: Math.h:573
pcl::TruncInt64
int64 TruncInt64(T x) noexcept
Definition: Math.h:1260
pcl::RoundInt
int RoundInt(T x) noexcept
Definition: Math.h:1550
pcl::Norm
T Norm(const T *i, const T *j, const P &p) noexcept
Definition: Math.h:2166
pcl::Mod
constexpr T Mod(T x, T y) noexcept
Definition: Math.h:958
pcl::ArcHav
constexpr T ArcHav(T x) noexcept
Definition: Math.h:1914
pcl::MinRad
constexpr T MinRad(T x) noexcept
Definition: Math.h:1958
pcl::TruncInt
int TruncInt(T x) noexcept
Definition: Math.h:1203
pcl::L2Norm
T L2Norm(const T *i, const T *j) noexcept
Definition: Math.h:2284
pcl::Floor
constexpr T Floor(T x) noexcept
Definition: Math.h:699
pcl::Deg
constexpr T Deg(T x) noexcept
Definition: Math.h:677
pcl::RoundInt64Arithmetic
int64 RoundInt64Arithmetic(double x) noexcept
Definition: Math.h:1714
pcl::Pi
constexpr long double Pi() noexcept
Definition: Math.h:531
pcl::PowI6
T PowI6(T x) noexcept
Definition: Math.h:1817
pcl::PowI10
T PowI10(T x) noexcept
Definition: Math.h:1835
pcl::RoundIntBanker
int RoundIntBanker(T x) noexcept
Definition: Math.h:1588
pcl::RoundI
int RoundI(T x) noexcept
Definition: Math.h:1575
pcl::RotR
T RotR(T x, uint32 n) noexcept
Definition: Math.h:1398
pcl::TruncI
int TruncI(T x) noexcept
Definition: Math.h:1221
pcl::Pow2
constexpr T Pow2(T x) noexcept
Definition: Math.h:1748
pcl::Log2e
constexpr long double Log2e() noexcept
Definition: Math.h:920
pcl::int8
signed char int8
Definition: Defs.h:636
pcl::uint64
unsigned long long uint64
Definition: Defs.h:682
pcl::uint16
unsigned short uint16
Definition: Defs.h:654
pcl::uint8
unsigned char uint8
Definition: Defs.h:642
pcl::int16
signed short int16
Definition: Defs.h:648
pcl::int64
signed long long int64
Definition: Defs.h:676
pcl::int32
signed int int32
Definition: Defs.h:660
pcl::uint32
unsigned int uint32
Definition: Defs.h:666
pcl::Select
RI Select(RI i, RI j, distance_type k)
Definition: Selection.h:165
pcl::distance_type
ptrdiff_t distance_type
Definition: Defs.h:615
pcl::size_type
size_t size_type
Definition: Defs.h:609
pcl::Sort
void Sort(BI i, BI j)
Definition: Sort.h:520
pcl::IncompleteBeta
double IncompleteBeta(double a, double b, double x, double eps=1.0e-8) noexcept
Definition: Math.h:4620
pcl::TrimmedMeanDestructive
double TrimmedMeanDestructive(T *__restrict__ i, T *__restrict__ j, distance_type l=1, distance_type h=1) noexcept
Definition: Math.h:3345
pcl::Qn
double Qn(T *__restrict__ x, T *__restrict__ xn)
Definition: Math.h:4250
pcl::BendMidvariance
double BendMidvariance(const T *__restrict__ x, const T *__restrict__ xn, double center, double beta=0.2)
Definition: Math.h:4560
pcl::StableModulus
double StableModulus(const T *__restrict__ i, const T *__restrict__ j) noexcept
Definition: Math.h:2667
pcl::MAD
double MAD(const T *__restrict__ i, const T *__restrict__ j, double center, double eps=0)
Definition: Math.h:3844
pcl::SumOfSquares
double SumOfSquares(const T *__restrict__ i, const T *__restrict__ j) noexcept
Definition: Math.h:2692
pcl::StableMean
double StableMean(const T *__restrict__ i, const T *__restrict__ j) noexcept
Definition: Math.h:2759
pcl::TrimmedMean
double TrimmedMean(const T *__restrict__ i, const T *__restrict__ j, distance_type l=1, distance_type h=1)
Definition: Math.h:3306
pcl::MedianDestructive
double MedianDestructive(T *__restrict__ i, T *__restrict__ j) noexcept
Definition: Math.h:3016
pcl::TwoSidedMAD
TwoSidedEstimate TwoSidedMAD(const T *__restrict__ i, const T *__restrict__ j, double center, double eps=0)
Definition: Math.h:3913
pcl::StableSum
double StableSum(const T *__restrict__ i, const T *__restrict__ j) noexcept
Definition: Math.h:2623
pcl::Variance
double Variance(const T *__restrict__ i, const T *__restrict__ j, double center) noexcept
Definition: Math.h:2784
pcl::TrimmedMeanOfSquaresDestructive
double TrimmedMeanOfSquaresDestructive(T *__restrict__ i, T *__restrict__ j, distance_type l=1, distance_type h=1) noexcept
Definition: Math.h:3426
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:4484
pcl::Sum
double Sum(const T *__restrict__ i, const T *__restrict__ j) noexcept
Definition: Math.h:2604
pcl::StableSumOfSquares
double StableSumOfSquares(const T *__restrict__ i, const T *__restrict__ j) noexcept
Definition: Math.h:2714
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:4421
pcl::StdDev
double StdDev(const T *__restrict__ i, const T *__restrict__ j, double center) noexcept
Definition: Math.h:2844
pcl::TrimmedMeanOfSquares
double TrimmedMeanOfSquares(const T *__restrict__ i, const T *__restrict__ j, distance_type l=1, distance_type h=1)
Definition: Math.h:3383
pcl::AvgDev
double AvgDev(const T *__restrict__ i, const T *__restrict__ j, double center) noexcept
Definition: Math.h:3468
pcl::Median
double Median(const T *__restrict__ i, const T *__restrict__ j, double eps=0)
Definition: Math.h:2917
pcl::OrderStatisticDestructive
double OrderStatisticDestructive(T *__restrict__ i, T *__restrict__ j, distance_type k) noexcept
Definition: Math.h:3261
pcl::OrderStatistic
double OrderStatistic(const T *__restrict__ i, const T *__restrict__ j, distance_type k, double eps=0)
Definition: Math.h:3202
pcl::Mean
double Mean(const T *__restrict__ i, const T *__restrict__ j) noexcept
Definition: Math.h:2740
pcl::StableAvgDev
double StableAvgDev(const T *__restrict__ i, const T *__restrict__ j, double center) noexcept
Definition: Math.h:3500
pcl::TwoSidedAvgDev
TwoSidedEstimate TwoSidedAvgDev(const T *__restrict__ i, const T *__restrict__ j, double center) noexcept
Definition: Math.h:3781
pcl::Modulus
double Modulus(const T *__restrict__ i, const T *__restrict__ j) noexcept
Definition: Math.h:2648
pcl::Sn
double Sn(T *__restrict__ x, T *__restrict__ xn)
Definition: Math.h:4004
pcl::Min
constexpr const T & Min(const T &a, const T &b) noexcept
Definition: Utility.h:90
ItemsInArray
#define ItemsInArray(a)
Definition: Utility.h:223
pcl::Range
constexpr const T & Range(const T &x, const T &a, const T &b) noexcept
Definition: Utility.h:190
pcl::Max
constexpr const T & Max(const T &a, const T &b) noexcept
Definition: Utility.h:119
pcl
PCL root namespace.
Definition: AbstractImage.h:77
pcl::RoundI64
int64 RoundI64(double x) noexcept
Definition: Math.h:1685
pcl::Fact
Factorial function.
Definition: Math.h:832
pcl::Pow10I
Exponential function 10**n, n being a signed integer.
Definition: Math.h:1317
pcl::Pow2I
Exponential function 2**n, n being a signed integer.
Definition: Math.h:1768
pcl::TwoSidedEstimate
Two-sided descriptive statistical estimate.
Definition: Math.h:3592
pcl::TwoSidedEstimate::Total
double Total() const noexcept
Definition: Math.h:3656
pcl::TwoSidedEstimate::TwoSidedEstimate
TwoSidedEstimate(TwoSidedEstimate &&)=default
pcl::TwoSidedEstimate::IsValid
bool IsValid() const noexcept
Definition: Math.h:3647
pcl::TwoSidedEstimate::TwoSidedEstimate
TwoSidedEstimate(const T1 &l, const T2 &h)
Definition: Math.h:3625
pcl::TwoSidedEstimate::operator*=
TwoSidedEstimate & operator*=(double x) noexcept
Definition: Math.h:3688
pcl::TwoSidedEstimate::operator/
TwoSidedEstimate operator/(double x) const noexcept
Definition: Math.h:3727
pcl::TwoSidedEstimate::high
double high
High estimate component.
Definition: Math.h:3594
pcl::TwoSidedEstimate::TwoSidedEstimate
TwoSidedEstimate(const T &x)
Definition: Math.h:3636
pcl::TwoSidedEstimate::Mean
double Mean() const noexcept
Definition: Math.h:3665
pcl::TwoSidedEstimate::TwoSidedEstimate
TwoSidedEstimate(const TwoSidedEstimate &)=default
pcl::TwoSidedEstimate::low
double low
Low estimate component.
Definition: Math.h:3593
pcl::TwoSidedEstimate::operator*
TwoSidedEstimate operator*(double x) const noexcept
Definition: Math.h:3719
pcl::TwoSidedEstimate::operator=
TwoSidedEstimate & operator=(const TwoSidedEstimate &)=default
pcl::TwoSidedEstimate::TwoSidedEstimate
TwoSidedEstimate()=default
pcl::TwoSidedEstimate::operator/=
TwoSidedEstimate & operator/=(double x) noexcept
Definition: Math.h:3698