//     ____   ______ __
//    / __ \ / ____// /
//   / /_/ // /    / /
//  / ____// /___ / /___   PixInsight Class Library
// /_/     \____//_____/   PCL 2.4.23
// ----------------------------------------------------------------------------
// pcl/Math.h - Released 2022-03-12T18:59:29Z
// ----------------------------------------------------------------------------
// This file is part of the PixInsight Class Library (PCL).
// PCL is a multiplatform C++ framework for development of PixInsight modules.
//
// Copyright (c) 2003-2022 Pleiades Astrophoto S.L. All Rights Reserved.
//
// Redistribution and use in both source and binary forms, with or without
// modification, is permitted provided that the following conditions are met:
//
// 1. All redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. All redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//
// 3. Neither the names "PixInsight" and "Pleiades Astrophoto", nor the names
//    of their contributors, may be used to endorse or promote products derived
//    from this software without specific prior written permission. For written
//    permission, please contact info@pixinsight.com.
//
// 4. All products derived from this software, in any form whatsoever, must
//    reproduce the following acknowledgment in the end-user documentation
//    and/or other materials provided with the product:
//
//    "This product is based on software from the PixInsight project, developed
//    by Pleiades Astrophoto and its contributors (https://pixinsight.com/)."
//
//    Alternatively, if that is where third-party acknowledgments normally
//    appear, this acknowledgment must be reproduced in the product itself.
//
// THIS SOFTWARE IS PROVIDED BY PLEIADES ASTROPHOTO AND ITS CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL PLEIADES ASTROPHOTO OR ITS
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, BUSINESS
// INTERRUPTION; PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; AND LOSS OF USE,
// DATA OR PROFITS) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
// ----------------------------------------------------------------------------

#ifndef __PCL_Math_h
#define __PCL_Math_h

/// \file pcl/Math.h

#include <pcl/Defs.h>
#include <pcl/Diagnostics.h>

#include <pcl/Memory.h>
#include <pcl/Selection.h>
#include <pcl/Sort.h>
#include <pcl/Utility.h>

#include <cmath>
#include <cstdlib>
#include <limits>

#ifdef _MSC_VER
#  include <intrin.h> // for __cpuid()
#endif

#if defined( __x86_64__ ) || defined( _M_X64 ) || defined( __PCL_MACOSX )
#  define __PCL_HAVE_SSE2  1
#  include <emmintrin.h>
#endif

/*
 * sincos() is only available as a GNU extension:
 *
 * http://man7.org/linux/man-pages/man3/sincos.3.html
 *
 * Unfortunately, it is not part of the C++ standard library because of the
 * anachronic dependency on errno.
 */
#if !defined( _MSC_VER ) && !defined( __clang__ ) && defined( __GNUC__ )
#  define __PCL_HAVE_SINCOS 1
#endif

#ifdef __PCL_QT_INTERFACE
#  include <QtWidgets/QtWidgets>
#endif

/*
 * Number of histogram bins used by fast histogram-based median calculation
 * algorithm implementations.
 */
#define __PCL_MEDIAN_HISTOGRAM_LENGTH  8192

namespace pcl
{

// ----------------------------------------------------------------------------

/*!
 * \defgroup hw_identification_functions Hardware Identification Routines
 */

/*!
 * Returns an integer representing the highest set of Streaming SIMD Extensions
 * instructions (SSE) supported by the running processor. This function is a
 * portable wrapper to the CPUID x86 instruction.
 *
 * The returned value can be one of:
 *
 * <pre>
 *  0 : No SSE instructions supported
 *  1 : SSE instructions set supported
 *  2 : SSE2 instructions set supported
 *  3 : SSE3 instructions set supported
 * 41 : SSE4.1 instructions set supported
 * 42 : SSE4.2 instructions set supported
 * </pre>
 *
 * \ingroup hw_identification_functions
 */
inline int MaxSSEInstructionSetSupported() noexcept
{
   int32 edxFlags = 0;
   int32 ecxFlags = 0;

#ifdef _MSC_VER
   int cpuInfo[ 4 ];
   __cpuid( cpuInfo, 1 );
   edxFlags = cpuInfo[3];
   ecxFlags = cpuInfo[2];
#else
   asm volatile( "mov $0x00000001, %%eax\n\t"
                 "cpuid\n\t"
                 "mov %%edx, %0\n\t"
                 "mov %%ecx, %1\n"
                  : "=r" (edxFlags), "=r" (ecxFlags)  // output operands
                  :                                   // input operands
                  : "%eax", "%ebx", "%ecx", "%edx" ); // clobbered registers
#endif

   if ( ecxFlags & (1u << 20) ) // SSE4.2
      return 42;
   if ( ecxFlags & (1u << 19) ) // SSE4.1
      return 41;
   if ( ecxFlags & 1u )         // SSE3
      return 3;
   if ( edxFlags & (1u << 26) ) // SSE2
      return 2;
   if ( edxFlags & (1u << 25) ) // SSE
      return 1;
   return 0;
}

// ----------------------------------------------------------------------------

/*!
 * \defgroup fpclassification_functions Floating Point Number Classification
 */

#define __PCL_FLOAT_SGNMASK   0x80000000
#define __PCL_FLOAT_EXPMASK   0x7f800000
#define __PCL_FLOAT_SIGMASK   0x007fffff

/*!
 * Returns true iff the specified 32-bit floating point number is \e finite.
 * A number is finite if it is neither NaN (Not a Number) nor positive or
 * negative infinity.
 *
 * \ingroup fpclassification_functions
 * \sa IsNaN( float ), IsInfinity( float )
 */
inline bool IsFinite( float x ) noexcept
{
   union { float f; uint32 u; } v = { x };
   return (v.u & __PCL_FLOAT_EXPMASK) != __PCL_FLOAT_EXPMASK;
}

/*!
 * Returns true iff the specified 32-bit floating point number is \e NaN. A NaN
 * (Not A Number) is a special value in the IEEE 754 standard used to represent
 * an undefined or unrepresentable value, such as the result of invalid
 * operations like 0/0, or real operations with complex results as the square
 * root of a negative number.
 *
 * \ingroup fpclassification_functions
 * \sa IsFinite( float ), IsInfinity( float )
 */
inline bool IsNaN( float x ) noexcept
{
   union { float f; uint32 u; } v = { x };
   return (v.u & __PCL_FLOAT_EXPMASK) == __PCL_FLOAT_EXPMASK &&
          (v.u & __PCL_FLOAT_SIGMASK) != 0;
}

/*!
 * Returns a nonzero integer value if the specified 32-bit floating point
 * number is an \e infinity.
 *
 * This function returns:
 *
 * <pre>
 * +1 if \a x is <em>positive infinity</em>.
 * -1 if \a x is <em>negative infinity</em>.
 *  0 if \a x is either NaN or a finite value.
 * </pre>
 *
 * \ingroup fpclassification_functions
 * \sa IsFinite( float ), IsNaN( float )
 */
inline int IsInfinity( float x ) noexcept
{
   union { float f; uint32 u; } v = { x };
   if ( (v.u & __PCL_FLOAT_EXPMASK) == __PCL_FLOAT_EXPMASK &&
        (v.u & __PCL_FLOAT_SIGMASK) == 0 )
      return ((v.u & __PCL_FLOAT_SGNMASK) == 0) ? +1 : -1;
   return 0;
}

/*!
 * Returns true iff the specified 32-bit floating point number is a negative
 * zero.
 *
 * \ingroup fpclassification_functions
 */
inline bool IsNegativeZero( float x ) noexcept
{
   union { float f; uint32 u; } v = { x };
   return v.u == __PCL_FLOAT_SGNMASK;
}

#define __PCL_DOUBLE_SGNMASK  0x80000000
#define __PCL_DOUBLE_EXPMASK  0x7ff00000
#define __PCL_DOUBLE_SIGMASK  0x000fffff

/*!
 * Returns true iff the specified 64-bit floating point number is \e finite.
 * A number is finite if it is neither NaN (Not a Number) nor positive or
 * negative infinity.
 *
 * \ingroup fpclassification_functions
 * \sa IsNaN( double ), IsInfinity( double )
 */
inline bool IsFinite( double x ) noexcept
{
   union { double d; uint32 u[2]; } v = { x };
   return (v.u[1] & __PCL_DOUBLE_EXPMASK) != __PCL_DOUBLE_EXPMASK;
}

/*!
 * Returns true iff the specified 64-bit floating point number is \e NaN. A NaN
 * (Not A Number) is a special value in the IEEE 754 standard used to represent
 * an undefined or unrepresentable value, such as the result of invalid
 * operations like 0/0, or real operations with complex results as the square
 * root of a negative number.
 *
 * \ingroup fpclassification_functions
 * \sa IsFinite( double ), IsInfinity( double )
 */
inline bool IsNaN( double x ) noexcept
{
   union { double d; uint32 u[2]; } v = { x };
   return (v.u[1] & __PCL_DOUBLE_EXPMASK) == __PCL_DOUBLE_EXPMASK &&
          (v.u[0] != 0 || (v.u[1] & __PCL_DOUBLE_SIGMASK) != 0);
}

/*!
 * Returns a nonzero integer value if the specified 64-bit floating point
 * number is an \e infinity.
 *
 * This function returns:
 *
 * <pre>
 * +1 if \a x is <em>positive infinity</em>.
 * -1 if \a x is <em>negative infinity</em>.
 *  0 if \a x is either NaN or a finite value.
 * </pre>
 *
 * \ingroup fpclassification_functions
 * \sa IsFinite( double ), IsNaN( double )
 */
inline int IsInfinity( double x ) noexcept
{
   union { double d; uint32 u[2]; } v = { x };
   if ( v.u[0] == 0 &&
       (v.u[1] & __PCL_DOUBLE_SIGMASK) == 0 &&
       (v.u[1] & __PCL_DOUBLE_EXPMASK) == __PCL_DOUBLE_EXPMASK )
      return ((v.u[1] & __PCL_DOUBLE_SGNMASK) == 0) ? +1 : -1;
   return 0;
}

/*!
 * Returns true iff the specified 64-bit floating point number is a negative
 * zero.
 *
 * \ingroup fpclassification_functions
 */
inline bool IsNegativeZero( double x ) noexcept
{
   union { double d; uint32 u[2]; } v = { x };
   return v.u[1] == __PCL_DOUBLE_SGNMASK &&
          v.u[0] == 0;
}

// ----------------------------------------------------------------------------

/*!
 * \defgroup mathematical_functions Mathematical Functions
 */

/*!
 * Absolute value of \a x.
 * \ingroup mathematical_functions
 */
inline float Abs( float x ) noexcept
{
   return std::fabs( x );
}

/*!
 * \ingroup mathematical_functions
 */
inline double Abs( double x ) noexcept
{
   return std::fabs( x );
}

/*!
 * \ingroup mathematical_functions
 */
inline long double Abs( long double x ) noexcept
{
   return std::fabs( x );
}

/*!
 * \ingroup mathematical_functions
 */
inline signed int Abs( signed int x ) noexcept
{
   return ::abs( x );
}

/*!
 * \ingroup mathematical_functions
 */
#if defined( __PCL_MACOSX ) && defined( __clang__ ) // turn off warning due to broken cstdlib in Xcode
_Pragma("clang diagnostic push")
_Pragma("clang diagnostic ignored \"-Wabsolute-value\"")
#endif
inline signed long Abs( signed long x ) noexcept
{
   return ::abs( x );
}
#if defined( __PCL_MACOSX ) && defined( __clang__ )
_Pragma("clang diagnostic pop")
#endif

/*!
 * \ingroup mathematical_functions
 */
#if defined( _MSC_VER )
inline __int64 Abs( __int64 x ) noexcept
{
   return (x < 0) ? -x : +x;
}
#elif defined( __PCL_MACOSX ) && defined( __clang__ )
inline constexpr signed long long Abs( signed long long x ) noexcept
{
   return (x < 0) ? -x : +x;
}
#else
inline signed long long Abs( signed long long x ) noexcept
{
   return ::abs( x );
}
#endif

/*!
 * \ingroup mathematical_functions
 */
inline signed short Abs( signed short x ) noexcept
{
   return (signed short)::abs( int( x ) );
}

/*!
 * \ingroup mathematical_functions
 */
inline signed char Abs( signed char x ) noexcept
{
   return (signed char)::abs( int( x ) );
}

/*!
 * \ingroup mathematical_functions
 */
inline wchar_t Abs( wchar_t x ) noexcept
{
   return (wchar_t)::abs( int( x ) );
}

/*!
 * \ingroup mathematical_functions
 */
inline constexpr unsigned int Abs( unsigned int x ) noexcept
{
   return x;
}

/*!
 * \ingroup mathematical_functions
 */
inline constexpr unsigned long Abs( unsigned long x ) noexcept
{
   return x;
}

/*!
 * \ingroup mathematical_functions
 */
#ifdef _MSC_VER
inline unsigned __int64 Abs( unsigned __int64 x ) noexcept
{
   return x;
}
#else
inline constexpr unsigned long long Abs( unsigned long long x ) noexcept
{
   return x;
}
#endif

/*!
 * \ingroup mathematical_functions
 */
inline constexpr unsigned short Abs( unsigned short x ) noexcept
{
   return x;
}

/*!
 * \ingroup mathematical_functions
 */
inline constexpr unsigned char Abs( unsigned char x ) noexcept
{
   return x;
}

// ----------------------------------------------------------------------------

/*!
 * The pi constant: 3.141592...
 * \ingroup mathematical_functions
 */
inline constexpr long double Pi() noexcept
{
   return (long double)( 0.31415926535897932384626433832795029e+01L );
}

/*!
 * Twice the pi constant: 0.6283185...
 * \ingroup mathematical_functions
 */
inline constexpr long double TwoPi() noexcept
{
   return (long double)( 0.62831853071795864769252867665590058e+01L );
}

// ----------------------------------------------------------------------------

/*!
 * Merges a complex angle given by degrees and arcminutes into single degrees.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Angle( int d, T m ) noexcept
{
   return d + m/60;
}

/*!
 * Merges a complex angle given by degrees, arcminutes and arcseconds into
 * single degrees.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Angle( int d, int m, T s ) noexcept
{
   return Angle( d, m + s/60 );
}

// ----------------------------------------------------------------------------

/*!
 * Inverse cosine function (arccosine).
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T ArcCos( T x ) noexcept
{
   PCL_PRECONDITION( T( -1 ) <= x && x <= T( 1 ) )
   return std::acos( x );
}

// ----------------------------------------------------------------------------

/*!
 * Inverse sine function (arcsine).
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T ArcSin( T x ) noexcept
{
   PCL_PRECONDITION( T( -1 ) <= x && x <= T( 1 ) )
   return std::asin( x );
}

// ----------------------------------------------------------------------------

/*!
 * Inverse tangent function (arctangent).
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T ArcTan( T x ) noexcept
{
   return std::atan( x );
}

// ----------------------------------------------------------------------------

/*!
 * Arctangent of y/x, result in the proper quadrant.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T ArcTan( T y, T x ) noexcept
{
   return std::atan2( y, x );
}

// ----------------------------------------------------------------------------

/*!
 * Arctangent of y/x, proper quadrant, result in the interval [0,2pi).
 * \ingroup mathematical_functions
 */
template <typename T> inline T ArcTan2Pi( T y, T x ) noexcept
{
   T r = ArcTan( y, x );
   if ( r < 0 )
      r = static_cast<T>( r + TwoPi() );
   return r;
}

// ----------------------------------------------------------------------------

/*!
 * The ceil function: lowest integer >= x.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Ceil( T x ) noexcept
{
   return std::ceil( x );
}

// ----------------------------------------------------------------------------

/*!
 * Cosine function.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Cos( T x ) noexcept
{
   return std::cos( x );
}

// ----------------------------------------------------------------------------

/*!
 * Hyperbolic Cosine function.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Cosh( T x ) noexcept
{
   return std::cosh( x );
}

// ----------------------------------------------------------------------------

/*!
 * Cotangent of x, equal to Cos(x)/Sin(x) or 1/Tan(x).
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Cotan( T x ) noexcept
{
   return T( 1 )/std::tan( x );
}

// ----------------------------------------------------------------------------

/*!
 * Conversion from radians to degrees.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Deg( T x ) noexcept
{
   return static_cast<T>( 0.572957795130823208767981548141051700441964e+02L * x );
}

// ----------------------------------------------------------------------------

/*!
 * The exponential function e**x.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Exp( T x ) noexcept
{
   return std::exp( x );
}

// ----------------------------------------------------------------------------

/*!
 * The floor function: highest integer <= x.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Floor( T x ) noexcept
{
   return std::floor( x );
}

// ----------------------------------------------------------------------------

/*!
 * Fractional part of x.
 * The returned value is within (-1,+1), and has the same sign as x.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Frac( T x ) noexcept
{
   return std::modf( x, &x );
}

// ----------------------------------------------------------------------------

/*!
 * Calculates base-2 mantissa and exponent.
 * The arguments \a m and \a p receive the values of the base-2 mantissa and
 * exponent, respectively, such that: 0.5 <= m < 1.0, x = m * 2**p
 * \ingroup mathematical_functions
 */
template <typename T> inline void Frexp( T x, T& m, int& p ) noexcept
{
   m = std::frexp( x, &p );
}

// ----------------------------------------------------------------------------

/*!
 * Haversine function:
 *
 * <tt>Hav( x ) = (1 - Cos( x ))/2</tt>
 *
 * The haversine is useful to work with tiny angles.
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Hav( T x ) noexcept
{
   return (1 - Cos( x ))/2;
}

// ----------------------------------------------------------------------------

/*!
 * Calculates m * 2**p.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Ldexp( T m, int p ) noexcept
{
   return std::ldexp( m, p );
}

// ----------------------------------------------------------------------------

/*!
 * Natural (base e) logarithm of \a x.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Ln( T x ) noexcept
{
   PCL_PRECONDITION( x >= 0 )
   return std::log( x );
}

// ----------------------------------------------------------------------------

struct PCL_CLASS FactorialCache
{
   constexpr static int s_cacheSize = 127;
   static const double  s_lut[ s_cacheSize+1 ];
};

/*!
 * The factorial of \a n &ge; 0.
 *
 * A static lookup table is used to speed up for \a n <= 127.
 *
 * \ingroup mathematical_functions
 */
inline double Factorial( int n ) noexcept
{
   PCL_PRECONDITION( n >= 0 )
   if ( n <= FactorialCache::s_cacheSize )
      return FactorialCache::s_lut[n];
   double x = FactorialCache::s_lut[FactorialCache::s_cacheSize];
   for ( int m = FactorialCache::s_cacheSize; ++m <= n; )
      x *= m;
   return x;
}

/*!
 * The natural logarithm of the factorial of \a n &ge; 0.
 *
 * For \a n <= 127 computes the natural logarithm of the factorial function
 * directly. For \a n > 127 computes a series approximation, so that the
 * function won't overflow even for very large arguments.
 *
 * \ingroup mathematical_functions
 */
inline double LnFactorial( int n ) noexcept
{
   PCL_PRECONDITION( n >= 0 )
   if ( n <= FactorialCache::s_cacheSize )
      return Ln( FactorialCache::s_lut[n] );
   double x = n + 1;
// return (x - 0.5)*Ln( x ) - x + 0.5*Ln( TwoPi() ) + 1/12/x - 1/(360*x*x*x);
   return (x - 0.5)*Ln( x ) - x + 0.91893853320467267 + 1/12/x - 1/(360*x*x*x);
}

/*!
 * \class Fact
 * \brief Factorial function.
 *
 * We use a static lookup table to speed up for \a n <= 127.
 *
 * Example of use:
 *
 * \code double factorialOfEight = Fact<double>()( 8 ); // = 40320 \endcode
 *
 * The implementation of this class is thread-safe.
 *
 * \deprecated This class is deprecated and subject to removal in a future
 * version of PCL. For newly produced code, use the pcl::Factorial() and
 * pcl::LnFactorial() functions instead.
 *
 * \ingroup mathematical_functions
 */
template <typename T> struct PCL_CLASS Fact : public FactorialCache
{
   /*!
    * Returns the factorial of \a n &ge; 0.
    */
   T operator()( int n ) const noexcept
   {
      PCL_PRECONDITION( n >= 0 )
      if ( n <= s_cacheSize )
         return T( s_lut[n] );
      double x = s_lut[s_cacheSize];
      for ( int m = s_cacheSize; ++m <= n; )
         x *= m;
      return T( x );
   }

   /*!
    * Returns the natural logarithm of the factorial of \a n &ge; 0. For
    * \a n <= 127 computes the natural logarithm of the factorial function
    * directly. For \a n > 127 computes a series approximation, so that the
    * function won't overflow even for very large arguments.
    */
   T Ln( int n ) const noexcept
   {
      PCL_PRECONDITION( n >= 0 )
      if ( n <= s_cacheSize )
         return T( pcl::Ln( s_lut[n] ) );
      double x = n + 1;
//    return T( (x - 0.5)*pcl::Ln( x ) - x + 0.5*pcl::Ln( TwoPi() ) + 1/12/x - 1/(360*x*x*x) );
      return T( (x - 0.5)*pcl::Ln( x ) - x + 0.91893853320467267 + 1/12/x - 1/(360*x*x*x) );
   }
};

// ----------------------------------------------------------------------------

/*!
 * Natural (base e) logarithm of two.
 * \ingroup mathematical_functions
 */
inline constexpr long double Ln2() noexcept
{
   return (long double)( 0.6931471805599453094172321214581766e+00L );
}

// ----------------------------------------------------------------------------

/*!
 * Base 10 Logarithm of x.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Log( T x ) noexcept
{
   PCL_PRECONDITION( x >= 0 )
   return std::log10( x );
}

// ----------------------------------------------------------------------------

/*!
 * Base 10 Logarithm of two.
 * \ingroup mathematical_functions
 */
inline constexpr long double Log2() noexcept
{
   // Use the relation:
   //    log10(2) = ln(2)/ln(10)
   return (long double)( 0.3010299956639811952137388947244930416265e+00L );
}

// ----------------------------------------------------------------------------

/*!
 * Base 2 Logarithm of x.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Log2( T x ) noexcept
{
   // Use the relation:
   //    log2(x) = ln(x)/ln(2)
   PCL_PRECONDITION( x >= 0 )
   return std::log( x )/Ln2();
}

// ----------------------------------------------------------------------------

/*!
 * Base 2 Logarithm of e.
 * \ingroup mathematical_functions
 */
inline constexpr long double Log2e() noexcept
{
   // Use the relation:
   //    log2(e) = 1/ln(2)
   return (long double)( 0.14426950408889634073599246810018920709799e+01L );
}

// ----------------------------------------------------------------------------

/*!
 * Base 2 Logarithm of ten.
 * \ingroup mathematical_functions
 */
inline constexpr long double Log2T() noexcept
{
   // Use the relation:
   //    log2(10) = 1/log(2)
   return (long double)( 0.33219280948873623478703194294893900118996e+01L );
}

// ----------------------------------------------------------------------------

/*!
 * Base \a n logarithm of \a x.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T LogN( T n, T x ) noexcept
{
   PCL_PRECONDITION( x >= 0 )
   return std::log( x )/std::log( n );
}

// ----------------------------------------------------------------------------

/*!
 * Remainder of x/y.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Mod( T x, T y ) noexcept
{
   return std::fmod( x, y );
}

// ----------------------------------------------------------------------------

/*!
 * Horner's algorithm to evaluate the polynomial function with the specified
 * array \a c of \a n + 1 coefficients:
 *
 * <pre>
 * y = c[0] + c[1]*x + c[2]*x**2 + ... + c[n]*x**n
 * </pre>
 *
 * The array \a c of coefficients must provide contiguous storage for at least
 * \a n + 1 values of type T. The degree \a n must be >= 0; otherwise this
 * function invokes undefined behavior.
 *
 * \ingroup mathematical_functions
 */
template <typename T, typename C> inline T Poly( T x, C c, int n ) noexcept
{
   PCL_PRECONDITION( n >= 0 )
   T y;
   for ( c += n, y = *c; n > 0; --n )
   {
      y *= x;
      y += *--c;
   }
   return y;
}

/*!
 * Horner's algorithm to evaluate the polynomial function:
 *
 * <pre>
 * y = c[0] + c[1]*x + c[2]*x**2 + ... + c[n]*x**n
 * </pre>
 *
 * The specified coefficients initializer list \a c must not be empty;
 * otherwise this function invokes undefined behavior.
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline T Poly( T x, std::initializer_list<T> c ) noexcept
{
   PCL_PRECONDITION( c.size() > 0 )
   return Poly( x, c.begin(), int( c.size() )-1 );
}

// ----------------------------------------------------------------------------

/*!
 * Sign function:
 *
 * <pre>
 * -1 if x &le; 0
 *  0 if x = 0
 * +1 if x &ge; 0
 * </pre>
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr int Sign( T x ) noexcept
{
   return (x < 0) ? -1 : ((x > 0) ? +1 : 0);
}

// ----------------------------------------------------------------------------

/*!
 * Sign character:
 *
 * <pre>
 * '-' if x &le; 0
 * ' ' if x = 0
 * '+' if x &ge; 0
 * </pre>
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr char SignChar( T x ) noexcept
{
   return (x < 0) ? '-' : ((x > 0) ? '+' : ' ');
}

// ----------------------------------------------------------------------------

/*!
 * Sine function
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Sin( T x ) noexcept
{
   return std::sin( x );
}

// ----------------------------------------------------------------------------

/*!
 * Hyperbolic Sine function.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Sinh( T x ) noexcept
{
   return std::sinh( x );
}

// ----------------------------------------------------------------------------

#ifdef __PCL_HAVE_SINCOS

inline void __pcl_sincos__( float x, float& sx, float& cx ) noexcept
{
   ::sincosf( x, &sx, &cx );
}

inline void __pcl_sincos__( double x, double& sx, double& cx ) noexcept
{
   ::sincos( x, &sx, &cx );
}

inline void __pcl_sincos__( long double x, long double& sx, long double& cx ) noexcept
{
   ::sincosl( x, &sx, &cx );
}

#endif

/*!
 * Sine and cosine of \a x.
 *
 * The arguments \a sx and \a cx will receive, respectively, the values of the
 * sine and cosine of x.
 *
 * If supported by the underlying standard math library, this function is
 * roughly twice as fast as calling Sin() and Cos() separately. For code that
 * spend a significant amount of time calculating sines and cosines, this
 * optimization is critical.
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline void SinCos( T x, T& sx, T& cx ) noexcept
{
#ifdef __PCL_HAVE_SINCOS
   __pcl_sincos__( x, sx, cx );
#else
   sx = std::sin( x );
   cx = std::cos( x );
#endif
}

// ----------------------------------------------------------------------------

/*!
 * Integer and fractional parts of \a x.
 *
 * The arguments \a i and \a f receive, respectively, the integer (truncated)
 * part and the fractional part of \a x. It holds that \a x = \a i + \a f, i.e.
 * both \a i and \a f have the same sign as \a x.
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline void Split( T x, T& i, T& f ) noexcept
{
   f = std::modf( x, &i );
}

// ----------------------------------------------------------------------------

/*!
 * Square root function.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Sqrt( T x ) noexcept
{
   return sqrt( x );
}

// ----------------------------------------------------------------------------

/*!
 * Tangent function.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Tan( T x ) noexcept
{
   return std::tan( x );
}

// ----------------------------------------------------------------------------

/*!
 * Hyperbolic Tangent function.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Tanh( T x ) noexcept
{
   return std::tanh( x );
}

// ----------------------------------------------------------------------------

/*!
 * Truncated integer part of \a x.
 * \ingroup mathematical_functions
 */
template <typename T> inline T Trunc( T x ) noexcept
{
   (void)std::modf( x, &x );
   return x;
}

// ----------------------------------------------------------------------------

#ifdef __PCL_HAVE_SSE2

inline int __pcl_trunci__( float x ) noexcept
{
   return _mm_cvtt_ss2si( _mm_load_ss( &x ) );
}

inline int __pcl_trunci__( double x ) noexcept
{
   return _mm_cvttsd_si32( _mm_load_sd( &x ) );
}

inline int __pcl_trunci__( long double x ) noexcept
{
   return int( x );
}

#endif

/*!
 * TruncInt function: Truncated integer part of \a x as a 32-bit signed
 * integer.
 *
 * \note This is a performance-critical routine. It has been implemented using
 * high-performance, low-level techniques that may include inline assembly code
 * and/or compiler intrinsic functions.
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline int TruncInt( T x ) noexcept
{
   PCL_PRECONDITION( x >= int_min && x <= int_max )
#ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
   return int( x );
#else
# ifdef __PCL_HAVE_SSE2
   return __pcl_trunci__( x );
# else
   return int( x );
# endif
#endif
}

/*!
 * TruncI function: Truncated integer part of \a x as a 32-bit signed integer.
 * to a 32-bit signed integer.
 *
 * \deprecated Use TruncInt() in newly produced code.
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline int TruncI( T x ) noexcept
{
   return TruncInt( x );
}

#define TruncInt32( x ) TruncInt( x )
#define TruncI32( x )   TruncInt( x )

// ----------------------------------------------------------------------------

#ifdef __PCL_HAVE_SSE2

#if defined( __x86_64__ ) || defined( _M_X64 )

inline int64 __pcl_trunci64__( float x ) noexcept
{
   return _mm_cvttss_si64( _mm_load_ss( &x ) );
}

inline int64 __pcl_trunci64__( double x ) noexcept
{
   return _mm_cvttsd_si64( _mm_load_sd( &x ) );
}

#else

inline int64 __pcl_trunci64__( float x ) noexcept
{
   return int64( _mm_cvtt_ss2si( _mm_load_ss( &x ) ) );
}

inline int64 __pcl_trunci64__( double x ) noexcept
{
   return int64( x );
}

#endif

inline int64 __pcl_trunci64__( long double x ) noexcept
{
   return int64( x );
}

#endif // __PCL_HAVE_SSE2

/*!
 * TruncInt64 function: Truncated integer part of \a x as a 64-bit signed
 * integer.
 *
 * \note This is a performance-critical routine. It has been implemented using
 * high-performance, low-level techniques that may include inline assembly code
 * and/or compiler intrinsic functions.
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline int64 TruncInt64( T x ) noexcept
{
   PCL_PRECONDITION( x >= int64_min && x <= int64_max )
#ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES
   return int64( Trunc( x ) );
#else
# ifdef __PCL_HAVE_SSE2
   return __pcl_trunci64__( x );
# else
   return int64( Trunc( x ) );
# endif
#endif
}

/*!
 * TruncI64 function: Truncated integer part of \a x as a 64-bit signed
 * integer.
 *
 * \deprecated Use TruncInt64() in newly produced code.
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline int64 TruncI64( T x ) noexcept
{
   return TruncInt64( x );
}

// ----------------------------------------------------------------------------

/*!
 * General power function: \a x raised to \a y.
 *
 * When you know some of the arguments in advance, faster alternatives are:
 *
 * \li Use <tt>Pow10I\<T\>()( y )</tt> when x == 10 and y is an integer
 * \li Use <tt>PowI( x, y )</tt> when x != 10 and y is an integer
 * \li Use <tt>Pow10( y )</tt> when x == 10 and y is not an integer
 *
 * Otherwise, you can also use: <tt>Pow2( y*Log2( x ) )</tt>
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Pow( T x, T y ) noexcept
{
   PCL_PRECONDITION( y < T( int_max ) )
   return std::pow( x, y );
}

// ----------------------------------------------------------------------------

/*!
 * \class Pow10I
 * \brief Exponential function 10**n, \a n being a signed integer.
 *
 * Example of use:
 *
 * \code double x = Pow10I<double>()( 5 ); // x = 10^5 \endcode
 *
 * \ingroup mathematical_functions
 */
template <typename T> struct PCL_CLASS Pow10I
{
   T operator ()( int n ) const noexcept
   {
      // Use fast table lookups and squaring up to |n| <= 50.
      static const T lut[] =
      {
#define ___( x ) static_cast<T>( x )
         ___( 1.0e+00 ), ___( 1.0e+01 ), ___( 1.0e+02 ), ___( 1.0e+03 ), ___( 1.0e+04 ),
         ___( 1.0e+05 ), ___( 1.0e+06 ), ___( 1.0e+07 ), ___( 1.0e+08 ), ___( 1.0e+09 ),
         ___( 1.0e+10 ), ___( 1.0e+11 ), ___( 1.0e+12 ), ___( 1.0e+13 ), ___( 1.0e+14 ),
         ___( 1.0e+15 ), ___( 1.0e+16 ), ___( 1.0e+17 ), ___( 1.0e+18 ), ___( 1.0e+19 ),
         ___( 1.0e+20 ), ___( 1.0e+21 ), ___( 1.0e+22 ), ___( 1.0e+23 ), ___( 1.0e+24 ),
         ___( 1.0e+25 ), ___( 1.0e+26 ), ___( 1.0e+27 ), ___( 1.0e+28 ), ___( 1.0e+29 ),
         ___( 1.0e+30 ), ___( 1.0e+31 ), ___( 1.0e+32 ), ___( 1.0e+33 ), ___( 1.0e+34 ),
         ___( 1.0e+35 ), ___( 1.0e+36 ), ___( 1.0e+37 ), ___( 1.0e+38 ), ___( 1.0e+39 ),
         ___( 1.0e+40 ), ___( 1.0e+41 ), ___( 1.0e+42 ), ___( 1.0e+43 ), ___( 1.0e+44 ),
         ___( 1.0e+45 ), ___( 1.0e+46 ), ___( 1.0e+47 ), ___( 1.0e+48 ), ___( 1.0e+49 )
#undef ___
      };
      static const int N = ItemsInArray( lut );
      int i = ::abs( n );
      double x;
      if ( i < N )
         x = lut[i];
      else
      {
         x = lut[N-1];
         while ( (i -= N-1) >= N )
            x *= x;
         if ( i != 0 )
            x *= lut[i];
      }
      return T( (n >= 0) ? x : 1/x );
   }
};

// ----------------------------------------------------------------------------

/*!
 * The exponential function 10**x.
 * \ingroup mathematical_functions
 */
template <typename T> inline T Pow10( T x ) noexcept
{
   int i = TruncInt( x );
   return (i == x) ? Pow10I<T>()( i ) : T( std::pow( 10, x ) );
}

// ----------------------------------------------------------------------------

/*!
 * Bitwise rotate left function: Rotates \a x to the left by \a n bits.
 *
 * The template argument T must be an unsigned arithmetic type (uint8, uint16,
 * uint32 or uint64). The bit count \a n must be smaller than the number of
 * bits required to store an instance of T; for example, if T is uint32, \a n
 * must be in the range [0,31].
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline T RotL( T x, uint32 n ) noexcept
{
   static_assert( std::is_unsigned<T>::value,
                  "RotL() can only be used for unsigned integer scalar types" );
   const uint8 mask = 8*sizeof( x ) - 1;
   const uint8 r = uint8( n & mask );
   return (x << r) | (x >> ((-r) & mask));
   // Or equivalently, but less optimized:
   //return (x << r) | (x >> (1+mask-r));
}

/*!
 * Bitwise rotate right function: Rotates \a x to the right by \a n bits.
 *
 * The template argument T must be an unsigned arithmetic type (uint8, uint16,
 * uint32 or uint64). The bit count \a n must be smaller than the number of
 * bits required to store an instance of T; for example, if T is uint32, \a n
 * must be in the range [0,31].
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline T RotR( T x, uint32 n ) noexcept
{
   static_assert( std::is_unsigned<T>::value,
                  "RotR() can only be used for unsigned integer scalar types" );
   const uint8 mask = 8*sizeof( x ) - 1;
   const uint8 r = uint8( n & mask );
   return (x >> r) | (x << ((-r) & mask));
   // Or equivalently, but less optimized:
   //return (x >> r) | (x << (1+mask-r));
}

// ----------------------------------------------------------------------------

/*!
 * Round function: x rounded to the nearest integer (double precision version).
 *
 * \note This is a performance-critical routine. It has been implemented using
 * high-performance, low-level techniques that may include inline assembly code
 * and/or compiler intrinsic functions.
 *
 * \ingroup mathematical_functions
 */
inline double Round( double x ) noexcept
{
#ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES

   return floor( x + 0.5 );

#else

#  ifdef _MSC_VER
#    ifdef _M_X64
   return double( _mm_cvtsd_si64( _mm_load_sd( &x ) ) );
#    else
   __asm fld      x
   __asm frndint
#    endif
#  else
   double r;
   asm volatile( "frndint\n": "=t" (r) : "0" (x) );
   return r;
#  endif

#endif
}

/*!
 * Round function: x rounded to the nearest integer (single precision version).
 *
 * \note This is a performance-critical routine. It has been implemented using
 * high-performance, low-level techniques that may include inline assembly code
 * and/or compiler intrinsic functions.
 *
 * \ingroup mathematical_functions
 */
inline float Round( float x ) noexcept
{
#ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES

   return floorf( x + 0.5F );

#else

#  ifdef _MSC_VER
#    ifdef _M_X64
   return float( _mm_cvtss_si32( _mm_load_ss( &x ) ) );
#    else
   __asm fld      x
   __asm frndint
#    endif
#  else
   float r;
   asm volatile( "frndint\n": "=t" (r) : "0" (x) );
   return r;
#  endif

#endif
}

/*!
 * Round function: x rounded to the nearest integer (long double version).
 *
 * \note This is a performance-critical routine. It has been implemented using
 * high-performance, low-level techniques that may include inline assembly code
 * and/or compiler intrinsic functions.
 *
 * \ingroup mathematical_functions
 */
inline long double Round( long double x ) noexcept
{
#ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES

   return floorl( x + 0.5L );

#else

#  ifdef _MSC_VER
#    ifdef _M_X64
   double _x = x;
   return (long double)_mm_cvtsd_si64( _mm_load_sd( &_x ) );
#    else
   __asm fld      x
   __asm frndint
#    endif
#  else
   long double r;
   asm volatile( "frndint\n": "=t" (r) : "0" (x) );
   return r;
#  endif

#endif
}

// ----------------------------------------------------------------------------

/*!
 * RoundInt function: Rounds \a x to the nearest integer and converts the
 * result to a 32-bit signed integer.
 *
 * This function follows the Banker's rounding rule: a perfect half is always
 * rounded to the nearest even digit. Some examples:
 *
 * <pre>
 * RoundInt( 0.5 ) -> 0
 * RoundInt( 1.5 ) -> 2
 * RoundInt( 2.5 ) -> 2
 * RoundInt( 3.5 ) -> 4
 * </pre>
 *
 * By contrast, arithmetic rounding rounds a perfect half to the nearest digit,
 * either odd or even. For example:
 *
 * <pre>
 * RoundIntArithmetic( 0.5 ) -> 1
 * RoundIntArithmetic( 1.5 ) -> 2
 * RoundIntArithmetic( 2.5 ) -> 3
 * RoundIntArithmetic( 3.5 ) -> 4
 * </pre>
 *
 * Banker's rounding (also known as Gaussian rounding) is statistically more
 * accurate than the usual arithmetic rounding, but it causes aliasing problems
 * in some specific algorithms that depend critically on uniform rounding, such
 * as nearest neighbor upsampling.
 *
 * \note This is a performance-critical routine. It has been implemented using
 * high-performance, low-level techniques that may include inline assembly code
 * and/or compiler intrinsic functions.
 *
 * \sa RoundIntArithmetic(), RoundIntBanker()
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline int RoundInt( T x ) noexcept
{
   PCL_PRECONDITION( x >= int_min && x <= int_max )

#ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES

   return int( Round( x ) );

#else

   volatile union { double d; int32 i; } v = { x + 6755399441055744.0 };
   return v.i; // ### NB: Assuming little-endian architecture.

/*
   ### Deprecated code - The above code based on magic numbers is faster and
                         more portable across platforms and compilers.

   // Default FPU rounding mode assumed to be nearest integer.
   int r;

#  ifdef _MSC_VER
   __asm fld      x
   __asm fistp    dword ptr r
#  else
   asm volatile( "fistpl %0\n" : "=m" (r) : "t" (x) : "st" );
#  endif

   return r;
*/

#endif
}

/*!
 * RoundI function: Rounds \a x to the nearest integer and converts the result
 * to a 32-bit signed integer.
 *
 * \deprecated Use RoundInt() in newly produced code.
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline int RoundI( T x ) noexcept
{
   return RoundInt( x );
}

/*!
 * Rounds \a x to the nearest integer using the Banker's rounding rule, and
 * converts the result to a 32-bit signed integer.
 *
 * This function is a convenience synonym for RoundInt().
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline int RoundIntBanker( T x ) noexcept
{
   return RoundInt( x );
}

/*!
 * Rounds \a x to the nearest integer using the arithmetic rounding rule, and
 * converts the result to a 32-bit signed integer.
 *
 * Arithmetic rounding rounds a perfect half to the nearest digit, either odd
 * or even. For example:
 *
 * <pre>
 * RoundIntArithmetic( 0.5 ) -> 1
 * RoundIntArithmetic( 1.5 ) -> 2
 * RoundIntArithmetic( 2.5 ) -> 3
 * RoundIntArithmetic( 3.5 ) -> 4
 * </pre>
 *
 * See the RoundInt() function for more information on rounding rules.
 *
 * \note This is a performance-critical routine. It has been implemented using
 * high-performance, low-level techniques that may include inline assembly code
 * and/or compiler intrinsic functions.
 *
 * \sa RoundInt(), RoundIntBanker()
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline int RoundIntArithmetic( T x ) noexcept
{
   PCL_PRECONDITION( x >= int_min && x <= int_max )

   int i = TruncInt( x );
   if ( i < 0 )
   {
      if ( x - i <= T( -0.5 ) )
         return i-1;
   }
   else
   {
      if ( x - i >= T( +0.5 ) )
         return i+1;
   }
   return i;
}

// ----------------------------------------------------------------------------

/*!
 * RoundInt64 function: Rounds \a x to the nearest integer and converts the
 * result to a 64-bit signed integer.
 *
 * Since the default IEEE 754 rounding mode follows Banker's rounding rule,
 * this is what you should expect when calling this function. See the
 * documentation for RoundInt() for more information on rounding modes.
 *
 * \note This is a performance-critical routine. It has been implemented using
 * high-performance, low-level techniques that may include inline assembly code
 * and/or compiler intrinsic functions.
 *
 * \ingroup mathematical_functions
 */
inline int64 RoundInt64( double x ) noexcept
{
#ifdef __PCL_NO_PERFORMANCE_CRITICAL_MATH_ROUTINES

   return int64( Round( x ) );

#else

   // ### N.B.: Default FPU rounding mode assumed to be nearest integer.

#  ifdef _MSC_VER
#    ifdef _M_X64
   return _mm_cvtsd_si64( _mm_load_sd( &x ) );
#    else
   int64 r;
   __asm fld      x
   __asm fistp    qword ptr r
   return r;
#    endif
#  else
   int64 r;
   asm volatile( "fistpll %0\n" : "=m" (r) : "t" (x) : "st" );
   return r;
#  endif

#endif
}

/*!
 * RoundI64 function: Rounds \a x to the nearest integer and converts the
 * result to a 64-bit signed integer.
 *
 * \deprecated Use RoundInt64() in newly produced code.
 */
inline int64 RoundI64( double x ) noexcept
{
   return RoundInt64( x );
}

/*!
 * Rounds \a x to the nearest integer using the arithmetic rounding rule, and
 * converts the result to a 64-bit signed integer.
 *
 * Arithmetic rounding rounds a perfect half to the nearest digit, either odd
 * or even. For example:
 *
 * <pre>
 * RoundIntArithmetic( 0.5 ) -> 1
 * RoundIntArithmetic( 1.5 ) -> 2
 * RoundIntArithmetic( 2.5 ) -> 3
 * RoundIntArithmetic( 3.5 ) -> 4
 * </pre>
 *
 * See the RoundInt() function for more information on rounding rules.
 *
 * \note This is a performance-critical routine. It has been implemented using
 * high-performance, low-level techniques that may include inline assembly code
 * and/or compiler intrinsic functions.
 *
 * \sa RoundInt(), RoundIntBanker()
 *
 * \ingroup mathematical_functions
 */
inline int64 RoundInt64Arithmetic( double x ) noexcept
{
   int64 i = TruncInt64( x );
   if ( i < 0 )
   {
      if ( x - i <= -0.5 )
         return i-1;
   }
   else
   {
      if ( x - i >= +0.5 )
         return i+1;
   }
   return i;
}

// ----------------------------------------------------------------------------

/*!
 * General rounding function: \a x rounded to \a n fractional digits.
 * \ingroup mathematical_functions
 */
template <typename T> inline T Round( T x, int n ) noexcept
{
   PCL_PRECONDITION( n >= 0 )
   T p = Pow10I<T>()( n ); return Round( p*x )/p;
}

// ----------------------------------------------------------------------------

/*!
 * The exponential function 2**x.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Pow2( T x ) noexcept
{
   // Use the relation:
   //    2**x = e**(x*ln(2))
   return Exp( x*Ln2() );
}

// ----------------------------------------------------------------------------

/*!
 * \class Pow2I
 * \brief Exponential function 2**n, \a n being a signed integer.
 *
 * Example of use:
 *
 * \code float x = Pow2I<float>()( -2 ); // x = 1/4 \endcode
 *
 * \ingroup mathematical_functions
 */
template <typename T> struct PCL_CLASS Pow2I
{
   T operator ()( int n ) const noexcept
   {
      // We shift left a single bit in 31-bit chunks.
      int i = ::abs( n ), p;
      double x = uint32( 1 ) << (p = Min( i, 31 ));
      while ( (i -= p) != 0 )
         x *= uint32( 1 ) << (p = Min( i, 31 ));
      return T( (n >= 0) ? x : 1/x );
   }
};

// ----------------------------------------------------------------------------

/*!
 * The exponential function x**n, where \a n is a signed integer.
 * \ingroup mathematical_functions
 */
template <typename T> inline T PowI( T x, int n ) noexcept
{
   if ( n == 0 )
      return T( 1 );

   T r = x;
   int i = ::abs( n );
   if ( i > 1 )
   {
      do
         r *= r;
      while ( (i >>= 1) > 1 );
      if ( n & 1 )
         r *= x;
   }
   return (n > 0) ? r : T( 1/r );
}

/*!
 * Returns \a x raised to the 4th power.
 * \ingroup mathematical_functions
 */
template <typename T> inline T PowI4( T x ) noexcept
{
   x *= x; return x*x;
}

/*!
 * Returns \a x raised to the 6th power.
 * \ingroup mathematical_functions
 */
template <typename T> inline T PowI6( T x ) noexcept
{
   x *= x*x; return x*x;
}

/*!
 * Returns \a x raised to the 8th power.
 * \ingroup mathematical_functions
 */
template <typename T> inline T PowI8( T x ) noexcept
{
   x *= x*x*x; return x*x;
}

/*!
 * Returns \a x raised to the 10th power.
 * \ingroup mathematical_functions
 */
template <typename T> inline T PowI10( T x ) noexcept
{
   x *= x*x*x*x; return x*x;
}

/*!
 * Returns \a x raised to the 12th power.
 * \ingroup mathematical_functions
 */
template <typename T> inline T PowI12( T x ) noexcept
{
   x *= x*x*x*x*x; return x*x;
}

// ----------------------------------------------------------------------------

/*!
 * Inverse hyperbolic sine function.
 *
 * <tt>ArcSinh( x ) = Ln( x + Sqrt( 1 + x**2 ) )</tt>
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T ArcSinh( T x ) noexcept
{
#ifndef __PCL_NO_STD_INV_HYP_TRIG_FUNCTIONS
   return std::asinh( x );
#else
   return Ln( x + Sqrt( 1 + x*x ) );
#endif
}

// ----------------------------------------------------------------------------

/*!
 * Inverse hyperbolic cosine function.
 *
 * <tt>ArcCosh( x ) = 2*Ln( Sqrt( (x + 1)/2 ) + Sqrt( (x - 1)/2 ) )</tt>
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T ArcCosh( T x ) noexcept
{
#ifndef __PCL_NO_STD_INV_HYP_TRIG_FUNCTIONS
   return std::acosh( x );
#else
   return 2*Ln( Sqrt( (x + 1)/2 ) + Sqrt( (x - 1)/2 ) );
#endif
}

// ----------------------------------------------------------------------------

/*!
 * Inverse hyperbolic tangent function.
 *
 * <tt>ArcTanh( x ) = (Ln( 1 + x ) - Ln( 1 - x ))/2</tt>
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T ArcTanh( T x ) noexcept
{
#ifndef __PCL_NO_STD_INV_HYP_TRIG_FUNCTIONS
   return std::atanh( x );
#else
   return (Ln( 1 + x ) - Ln( 1 - x ))/2;
#endif
}

// ----------------------------------------------------------------------------

/*!
 * Inverse haversine (archaversine) function:
 *
 * <tt>ArcHav( x ) = 2*ArcSin( Sqrt( x ) )</tt>
 *
 * The haversine is useful to work with tiny angles.
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T ArcHav( T x ) noexcept
{
   return 2*ArcSin( Sqrt( x ) );
}

// ----------------------------------------------------------------------------

/*!
 * Conversion from degrees to radians.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Rad( T x ) noexcept
{
   return static_cast<T>( 0.174532925199432957692369076848861272222e-01L * x );
}

// ----------------------------------------------------------------------------

/*!
 * Conversion from radians to arcminutes.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T RadMin( T x ) noexcept
{
   return Deg( x )*60;
}

// ----------------------------------------------------------------------------

/*!
 * Conversion from radians to arcseconds.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T RadSec( T x ) noexcept
{
   return Deg( x )*3600;
}

// ----------------------------------------------------------------------------

/*!
 * Conversion from arcminutes to radians.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T MinRad( T x ) noexcept
{
   return Rad( x/60 );
}

// ----------------------------------------------------------------------------

/*!
 * Conversion from arcseconds to radians.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T SecRad( T x ) noexcept
{
   return Rad( x/3600 );
}

/*!
 * Conversion from arcseconds to radians (a synonym for SecRad()).
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T AsRad( T x ) noexcept
{
   return SecRad( x );
}

// ----------------------------------------------------------------------------

/*!
 * Conversion from milliarcseconds (mas) to radians.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T MasRad( T x ) noexcept
{
   return Rad( x/3600000 );
}

// ----------------------------------------------------------------------------

/*!
 * Conversion from microarcseconds (uas) to radians.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T UasRad( T x ) noexcept
{
   return Rad( x/3600000000 );
}

// ----------------------------------------------------------------------------

/*!
 * An angle in radians reduced to the [-pi,+pi] range.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T ModPi( T x ) noexcept
{
   x = Mod( x + static_cast<T>( Pi() ), static_cast<T>( TwoPi() ) ) - static_cast<T>( Pi() );
   return (x < -static_cast<T>( Pi() )) ? x + static_cast<T>( TwoPi() ) : x;
}

// ----------------------------------------------------------------------------

/*!
 * An angle in radians reduced to the (-2pi,+2pi) range, that is, the remainder
 * of x/(2*pi).
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Mod2Pi( T x ) noexcept
{
   return Mod( x, static_cast<T>( TwoPi() ) );
}

// ----------------------------------------------------------------------------

/*!
 * An angle in radians normalized to the [0,2pi) range.
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Norm2Pi( T x ) noexcept
{
   return ((x = Mod2Pi( x )) < 0) ? x + static_cast<T>( TwoPi() ) : x;
}

// ----------------------------------------------------------------------------

/*!
 * Rotates a point on the plane.
 *
 * \param[out] x,y   %Point coordinates. On output, these variables will
 *                   receive the corresponding rotated coordinates.
 *
 * \param sa, ca     Sine and cosine of the rotation angle.
 *
 * \param xc,yc      Coordinates of the center of rotation.
 *
 * \ingroup mathematical_functions
 */
template <typename T, typename T1, typename T2>
inline void Rotate( T& x, T& y, T1 sa, T1 ca, T2 xc, T2 yc ) noexcept
{
   T1 dx = T1( x ) - T1( xc );
   T1 dy = T1( y ) - T1( yc );
   x = T( T1( xc ) + ca*dx - sa*dy );
   y = T( T1( yc ) + sa*dx + ca*dy );
}

/*!
 * Rotates a point on the plane.
 *
 * This is a template instantiation of Rotate( T&, T&, T1, T1, T2, T2 ) for
 * the \c int type.
 *
 * Rotated coordinates are rounded to the nearest integers.
 *
 * \ingroup mathematical_functions
 */
template <typename T1, typename T2>
inline void Rotate( int& x, int& y, T1 sa, T1 ca, T2 xc, T2 yc ) noexcept
{
   T1 dx = T1( x ) - T1( xc );
   T1 dy = T1( y ) - T1( yc );
   x = RoundInt( T1( xc ) + ca*dx - sa*dy );
   y = RoundInt( T1( yc ) + sa*dx + ca*dy );
}

/*!
 * Rotates a point on the plane.
 *
 * This is a template instantiation of Rotate( T&, T&, T1, T1, T2, T2 ) for
 * the \c long type.
 *
 * Rotated coordinates are rounded to the nearest integers.
 *
 * \ingroup mathematical_functions
 */
template <typename T1, typename T2>
inline void Rotate( long& x, long& y, T1 sa, T1 ca, T2 xc, T2 yc ) noexcept
{
   T1 dx = T1( x ) - T1( xc );
   T1 dy = T1( y ) - T1( yc );
   x = (long)RoundInt( T1( xc ) + ca*dx - sa*dy );
   y = (long)RoundInt( T1( yc ) + sa*dx + ca*dy );
}

/*!
 * Rotates a point on the plane.
 *
 * This is a template instantiation of Rotate( T&, T&, T1, T1, T2, T2 ) for
 * the \c int64 type.
 *
 * Rotated coordinates are rounded to the nearest integers.
 *
 * \ingroup mathematical_functions
 */
template <typename T1, typename T2>
inline void Rotate( int64& x, int64& y, T1 sa, T1 ca, T2 xc, T2 yc ) noexcept
{
   T1 dx = T1( x ) - T1( xc );
   T1 dy = T1( y ) - T1( yc );
   x = RoundInt64( T1( xc ) + ca*dx - sa*dy );
   y = RoundInt64( T1( yc ) + sa*dx + ca*dy );
}

/*!
 * Rotates a point on the plane.
 *
 * \param[out] x,y   %Point coordinates. On output, these variables will
 *                   receive the corresponding rotated coordinates.
 *
 * \param a          Rotation angle in radians.
 *
 * \param xc,yc      Coordinates of the center of rotation.
 *
 * Instantiations for integer types round rotated coordinated to the nearest
 * integers.
 *
 * \ingroup mathematical_functions
 */
template <typename T, typename T1, typename T2>
inline void Rotate( T& x, T& y, T1 a, T2 xc, T2 yc ) noexcept
{
   T1 sa, ca; SinCos( a, sa, ca ); Rotate( x, y, sa, ca, xc, yc );
}

// ----------------------------------------------------------------------------

template <typename T, typename P, typename N> inline
N __pcl_norm__( const T* i, const T* j, const P& p, N* ) noexcept
{
   PCL_PRECONDITION( p > P( 0 ) )
   N n = N( 0 );
   for ( ; i < j; ++i )
      n += Pow( Abs( N( *i ) ), N( p ) );
   return Pow( n, N( 1/p ) );
}

/*!
 * Computes the norm of the elements in the sequence [i,j). For any real p > 0,
 * the norm N is given by:
 *
 * <pre>
 * N = sum( abs( x )^p )^(1/p)
 * </pre>
 *
 * for all x in [i,j).
 *
 * \ingroup mathematical_functions
 * \sa L1Norm(), L2Norm()
 */
template <typename T, typename P> inline T Norm( const T* i, const T* j, const P& p ) noexcept
{
   return __pcl_norm__( i, j, p, (T*)( 0 ) );
}

template <typename P> inline double Norm( const float* i, const float* j, const P& p ) noexcept
{
   return __pcl_norm__( i, j, p, (double*)( 0 ) );
}
template <typename P> inline double Norm( const int* i, const int* j, const P& p ) noexcept
{
   return __pcl_norm__( i, j, p, (double*)( 0 ) );
}
template <typename P> inline double Norm( const unsigned* i, const unsigned* j, const P& p ) noexcept
{
   return __pcl_norm__( i, j, p, (double*)( 0 ) );
}
template <typename P> inline double Norm( const int8* i, const int8* j, const P& p ) noexcept
{
   return __pcl_norm__( i, j, p, (double*)( 0 ) );
}
template <typename P> inline double Norm( const uint8* i, const uint8* j, const P& p ) noexcept
{
   return __pcl_norm__( i, j, p, (double*)( 0 ) );
}
template <typename P> inline double Norm( const int16* i, const int16* j, const P& p ) noexcept
{
   return __pcl_norm__( i, j, p, (double*)( 0 ) );
}
template <typename P> inline double Norm( const uint16* i, const uint16* j, const P& p ) noexcept
{
   return __pcl_norm__( i, j, p, (double*)( 0 ) );
}
template <typename P> inline double Norm( const int64* i, const int64* j, const P& p ) noexcept
{
   return __pcl_norm__( i, j, p, (double*)( 0 ) );
}
template <typename P> inline double Norm( const uint64* i, const uint64* j, const P& p ) noexcept
{
   return __pcl_norm__( i, j, p, (double*)( 0 ) );
}

// ----------------------------------------------------------------------------

template <typename T, typename N> inline
N __pcl_l1norm__( const T* i, const T* j, N* ) noexcept
{
   N n = N( 0 );
   for ( ; i < j; ++i )
      n += N( Abs( *i ) );
   return n;
}

/*!
 * Computes the L1 norm (or Manhattan norm) of the elements in the sequence
 * [i,j). The L1 norm is the sum of the absolute values of the elements.
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline T L1Norm( const T* i, const T* j ) noexcept
{
   return __pcl_l1norm__( i, j, (T*)( 0 ) );
}

inline double L1Norm( const float* i, const float* j ) noexcept
{
   return __pcl_l1norm__( i, j, (double*)( 0 ) );
}
inline double L1Norm( const int* i, const int* j ) noexcept
{
   return __pcl_l1norm__( i, j, (double*)( 0 ) );
}
inline double L1Norm( const unsigned* i, const unsigned* j ) noexcept
{
   return __pcl_l1norm__( i, j, (double*)( 0 ) );
}
inline double L1Norm( const int8* i, const int8* j ) noexcept
{
   return __pcl_l1norm__( i, j, (double*)( 0 ) );
}
inline double L1Norm( const uint8* i, const uint8* j ) noexcept
{
   return __pcl_l1norm__( i, j, (double*)( 0 ) );
}
inline double L1Norm( const int16* i, const int16* j ) noexcept
{
   return __pcl_l1norm__( i, j, (double*)( 0 ) );
}
inline double L1Norm( const uint16* i, const uint16* j ) noexcept
{
   return __pcl_l1norm__( i, j, (double*)( 0 ) );
}
inline double L1Norm( const int64* i, const int64* j ) noexcept
{
   return __pcl_l1norm__( i, j, (double*)( 0 ) );
}
inline double L1Norm( const uint64* i, const uint64* j ) noexcept
{
   return __pcl_l1norm__( i, j, (double*)( 0 ) );
}

// ----------------------------------------------------------------------------

template <typename T, typename N> inline
N __pcl_l2norm__( const T* i, const T* j, N* ) noexcept
{
   N n = N( 0 );
   for ( ; i < j; ++i )
      n += N( *i ) * N( *i );
   return Sqrt( n );
}

/*!
 * Computes the L2 norm (or Euclidean norm) of the elements in the sequence
 * [i,j). The L2 norm is the square root of the sum of squared elements.
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline T L2Norm( const T* i, const T* j ) noexcept
{
   return __pcl_l2norm__( i, j, (T*)( 0 ) );
}

inline double L2Norm( const float* i, const float* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}
inline double L2Norm( const int* i, const int* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}
inline double L2Norm( const unsigned* i, const unsigned* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}
inline double L2Norm( const int8* i, const int8* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}
inline double L2Norm( const uint8* i, const uint8* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}
inline double L2Norm( const int16* i, const int16* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}
inline double L2Norm( const uint16* i, const uint16* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}
inline double L2Norm( const int64* i, const int64* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}
inline double L2Norm( const uint64* i, const uint64* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}

// ----------------------------------------------------------------------------

/*!
 * Computes the L2 norm (or Euclidean norm) of the elements in the sequence
 * [i,j). This function is a synonym for L2Norm().
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline T Norm( const T* i, const T* j ) noexcept
{
   return L2Norm( i, j );
}

inline double Norm( const float* i, const float* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}
inline double Norm( const int* i, const int* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}
inline double Norm( const unsigned* i, const unsigned* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}
inline double Norm( const int8* i, const int8* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}
inline double Norm( const uint8* i, const uint8* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}
inline double Norm( const int16* i, const int16* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}
inline double Norm( const uint16* i, const uint16* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}
inline double Norm( const int64* i, const int64* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}
inline double Norm( const uint64* i, const uint64* j ) noexcept
{
   return __pcl_l2norm__( i, j, (double*)( 0 ) );
}

// ----------------------------------------------------------------------------

/*!
 * Computes the Julian date (JD) corresponding to a time point expressed as a
 * date and a day fraction, providing the result by its separate integer and
 * fractional parts.
 *
 * \param[out] jdi   On output, the integer part of the resulting JD.
 *
 * \param[out] jdf   On output, the fractional part of the resulting JD.
 *
 * \param year       The year of the date. Positive and negative years are
 *                   supported. Years are counted arithmetically: the year zero
 *                   is the year before the year +1, that is, what historians
 *                   call the year 1 B.C.
 *
 * \param month      The month of the date. Usually in the [1,12] range but can
 *                   be any integer number.
 *
 * \param day        The day of the date. Usually in the [1,31] range but can
 *                   be any integer number.
 *
 * \param dayf       The day fraction. The default value is zero, which
 *                   computes the JD at zero hours. Usually in the [0,1) range
 *                   but can be any real number.
 *
 * This routine, as well as JDToCalendarTime(), implement modified versions of
 * the original algorithms due to Jean Meeus. Our modifications allow for
 * negative Julian dates, which extends the range of allowed dates to the past
 * considerably. We developed these modifications in the context of large-scale
 * solar system ephemeris calculations.
 *
 * The computed value is the JD corresponding to the specified date and day
 * fraction, which is equal to the sum of the \a jdi and \a jdf variables.
 *
 * \b References
 *
 * Meeus, Jean (1991), <em>Astronomical Algorithms</em>, Willmann-Bell, Inc.,
 * chapter 7.
 *
 * \ingroup mathematical_functions
 */
void PCL_FUNC CalendarTimeToJD( int& jdi, double& jdf, int year, int month, int day, double dayf = 0 ) noexcept;

/*!
 * Computes the Julian date (JD) corresponding to a time point expressed as a
 * date and a day fraction.
 *
 * \param year    The year of the date. Positive and negative years are
 *                supported. Years are counted arithmetically: the year zero is
 *                the year before the year +1, that is, what historians call
 *                the year 1 B.C.
 *
 * \param month   The month of the date. Usually in the [1,12] range but can be
 *                any integer number.
 *
 * \param day     The day of the date. Usually in the [1,31] range but can be
 *                any integer number.
 *
 * \param dayf    The day fraction. The default value is zero, which computes
 *                the JD at zero hours. Usually in the [0,1) range but can be
 *                any real number.
 *
 * This routine, as well as JDToCalendarTime(), implement modified versions of
 * the original algorithms due to Jean Meeus. Our modifications allow for
 * negative Julian dates, which extends the range of allowed dates to the past
 * considerably. We developed these modifications in the context of large-scale
 * solar system ephemeris calculations.
 *
 * The returned value is the JD corresponding to the specified date and day
 * fraction.
 *
 * Because of the numerical precision of the double type (IEEE 64-bit floating
 * point), this routine can return JD values accurate only to within one
 * millisecond.
 *
 * \b References
 *
 * Meeus, Jean (1991), <em>Astronomical Algorithms</em>, Willmann-Bell, Inc.,
 * chapter 7.
 *
 * \ingroup mathematical_functions
 */
inline double CalendarTimeToJD( int year, int month, int day, double dayf = 0 ) noexcept
{
   int jdi;
   double jdf;
   CalendarTimeToJD( jdi, jdf, year, month, day, dayf );
   return jdi + jdf;
}

/*!
 * Computes the date and day fraction corresponding to a time point expressed
 * as a Julian date (JD), specified by its separate integer and fractional
 * parts.
 *
 * \param[out] year  On output, this variable receives the year of the
 *                   resulting date.
 *
 * \param[out] month On output, this variable receives the month of the
 *                   resulting date in the range [1,12].
 *
 * \param[out] day   On output, this variable receives the day of the
 *                   resulting date in the range [1,31]. Different month day
 *                   counts and leap years are taken into account, so the
 *                   returned day corresponds to an existing calendar date.
 *
 * \param[out] dayf  On output, this variable receives the day fraction for the
 *                   specified time point, in the [0,1) range.
 *
 * \param jdi        The integer part of the input Julian date.
 *
 * \param jdf        The fractional part of the input Julian date.
 *
 * The input time point must be equal to the sum of \a jdi and \a jdf.
 *
 * For more information about the implemented algorithms and references, see
 * the documentation for CalendarTimeToJD().
 *
 * \ingroup mathematical_functions
 */
void PCL_FUNC JDToCalendarTime( int& year, int& month, int& day, double& dayf, int jdi, double jdf ) noexcept;

/*!
 * Computes the date and day fraction corresponding to a time point expressed
 * as a Julian date (JD).
 *
 * \param[out] year  On output, this variable receives the year of the
 *                   resulting date.
 *
 * \param[out] month On output, this variable receives the month of the
 *                   resulting date in the range [1,12].
 *
 * \param[out] day   On output, this variable receives the day of the
 *                   resulting date in the range [1,31]. Different month day
 *                   counts and leap years are taken into account, so the
 *                   returned day corresponds to an existing calendar date.
 *
 * \param[out] dayf  On output, this variable receives the day fraction for the
 *                   specified time point, in the [0,1) range.
 *
 * \param jd         The input time point as a Julian date.
 *
 * Because of the numerical precision of the double type (IEEE 64-bit floating
 * point), this routine can handle JD values accurate only to within one
 * millisecond.
 *
 * For more information about the implemented algorithms and references, see
 * the documentation for CalendarTimeToJD().
 *
 * \ingroup mathematical_functions
 */
inline void JDToCalendarTime( int& year, int& month, int& day, double& dayf, double jd ) noexcept
{
   JDToCalendarTime( year, month, day, dayf, TruncInt( jd ), Frac( jd ) );
}

// ----------------------------------------------------------------------------

/*!
 * Conversion of a decimal scalar \a d to the equivalent sexagesimal decimal
 * components \a sign, \a s1, \a s2 and \a s3, such that:
 *
 * <pre>
 * d = sign * (s1 + (s2 + s3/60)/60)
 * </pre>
 *
 * with the following constraints:
 *
 * <pre>
 * sign = -1 iff d &lt; 0
 * sign = +1 iff d &ge; 0
 * 0 &le; s1
 * 0 &le; s2 &lt; 60
 * 0 &le; s3 &lt; 60
 * </pre>
 *
 * \ingroup mathematical_functions
 */
template <typename S1, typename S2, typename S3, typename D>
inline void DecimalToSexagesimal( int& sign, S1& s1, S2& s2, S3& s3, const D& d ) noexcept
{
   double t1 = Abs( d );
   double t2 = Frac( t1 )*60;
   double t3 = Frac( t2 )*60;
   sign = (d < 0) ? -1 : +1;
   s1 = S1( TruncInt( t1 ) );
   s2 = S2( TruncInt( t2 ) );
   s3 = S3( t3 );
}

/*!
 * Conversion of the sexagesimal decimal components \a sign, \a s1, \a s2 and
 * \a s3 to their equivalent decimal scalar. The returned value is equal to:
 *
 * <pre>
 * ((sign < 0) ? -1 : +1)*(Abs( s1 ) + (s2 + s3/60)/60);
 * </pre>
 *
 * \ingroup mathematical_functions
 */
template <typename S1, typename S2, typename S3>
inline double SexagesimalToDecimal( int sign, const S1& s1, const S2& s2 = S2( 0 ), const S3& s3 = S3( 0 ) ) noexcept
{
   double d = Abs( s1 ) + (s2 + s3/60)/60;
   return (sign < 0) ? -d : d;
}

// ----------------------------------------------------------------------------

/*!
 * \defgroup statistical_functions Statistical Functions
 */

/*!
 * Returns the sum of elements in a sequence [i,j).
 *
 * For empty sequences, this function returns zero.
 *
 * This is a straight implementation of a floating point sum, which is subject
 * to severe roundoff errors if the number of summed elements is large. One way
 * to improve on this problem is to sort the input set by decreasing order of
 * absolute value \e before calling this function. A much better solution, but
 * computationally expensive, is a compensated summation algorithm such as
 * Kahan summation, which we have implemented in the StableSum() routine.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double Sum( const T* __restrict__ i, const T* __restrict__ j ) noexcept
{
   double sum = 0;
   while ( i < j )
      sum += double( *i++ );
   return sum;
}

/*!
 * Computes the sum of elements in a sequence [i,j) using a numerically stable
 * summation algorithm to minimize roundoff error.
 *
 * For empty sequences, this function returns zero.
 *
 * In the current PCL versions, this function implements the Kahan summation
 * algorithm to reduce roundoff error to the machine's floating point error.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double StableSum( const T* __restrict__ i, const T* __restrict__ j ) noexcept
{
   double sum = 0;
   double eps = 0;
   while ( i < j )
   {
      double y = double( *i++ ) - eps;
      double t = sum + y;
      eps = (t - sum) - y;
      sum = t;
   }
   return sum;
}

/*!
 * Returns the sum of the absolute values of the elements in a sequence [i,j).
 *
 * For empty sequences, this function returns zero.
 *
 * See the remarks made for the Sum() function, which are equally applicable in
 * this case. See StableModulus() for a (slow) numerically stable version of
 * this function.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double Modulus( const T* __restrict__ i, const T* __restrict__ j ) noexcept
{
   double S = 0;
   while ( i < j )
      S += Abs( double( *i++ ) );
   return S;
}

/*!
 * Computes the sum of the absolute values of the elements in a sequence [i,j)
 * using a numerically stable summation algorithm to minimize roundoff error.
 *
 * For empty sequences, this function returns zero.
 *
 * In the current PCL versions, this function implements the Kahan summation
 * algorithm to reduce roundoff error to the machine's floating point error.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double StableModulus( const T* __restrict__ i, const T* __restrict__ j ) noexcept
{
   double sum = 0;
   double eps = 0;
   while ( i < j )
   {
      double y = Abs( double( *i++ ) ) - eps;
      double t = sum + y;
      eps = (t - sum) - y;
      sum = t;
   }
   return sum;
}

/*!
 * Returns the sum of the squares of the elements in a sequence [i,j).
 *
 * For empty sequences, this function returns zero.
 *
 * See the remarks made for the Sum() function, which are equally applicable in
 * this case. See StableSumOfSquares() for a (slow) numerically stable version
 * of this function.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double SumOfSquares( const T* __restrict__ i, const T* __restrict__ j ) noexcept
{
   double Q = 0;
   while ( i < j )
   {
      double f = double( *i++ );
      Q += f*f;
   }
   return Q;
}

/*!
 * Computes the sum of the squares of the elements in a sequence [i,j) using a
 * numerically stable summation algorithm to minimize roundoff error.
 *
 * For empty sequences, this function returns zero.
 *
 * In the current PCL versions, this function implements the Kahan summation
 * algorithm to reduce roundoff error to the machine's floating point error.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double StableSumOfSquares( const T* __restrict__ i, const T* __restrict__ j ) noexcept
{
   double sum = 0;
   double eps = 0;
   while ( i < j )
   {
      double f = double( *i++ );
      double y = f*f - eps;
      double t = sum + y;
      eps = (t - sum) - y;
      sum = t;
   }
   return sum;
}

/*!
 * Returns the arithmetic mean of a sequence [i,j).
 *
 * For empty sequences, this function returns zero.
 *
 * See the remarks made for the Sum() function, which are equally applicable in
 * this case. See StableMean() for a (slow) numerically stable version of this
 * function.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double Mean( const T* __restrict__ i, const T* __restrict__ j ) noexcept
{
   distance_type n = j - i;
   if ( n < 1 )
      return 0;
   return Sum( i, j )/n;
}

/*!
 * Computes the arithmetic mean of a sequence [i,j) using a numerically stable
 * summation algorithm to minimize roundoff error.
 *
 * For empty sequences, this function returns zero.
 *
 * In the current PCL versions, this function implements the Kahan summation
 * algorithm to reduce roundoff error to the machine's floating point error.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double StableMean( const T* __restrict__ i, const T* __restrict__ j ) noexcept
{
   distance_type n = j - i;
   if ( n < 1 )
      return 0;
   return StableSum( i, j )/n;
}

/*!
 * Returns the variance of a sequence [i,j) with respect to the specified
 * \a center value.
 *
 * For sequences of less than two elements, this function returns zero.
 *
 * This implementation uses a two-pass compensated summation algorithm to
 * minimize roundoff errors (see the references).
 *
 * \b References
 *
 * William H. Press et al., <em>Numerical Recipes in C: The Art of Scientific
 * Computing, Second Edition</em> (1997 reprint) Cambridge University Press,
 * page 613.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double Variance( const T* __restrict__ i, const T* __restrict__ j, double center ) noexcept
{
   distance_type n = j - i;
   if ( n < 2 )
      return 0;
   double var = 0, eps = 0;
   do
   {
      double d = double( *i++ ) - center;
      var += d*d;
      eps += d;
   }
   while ( i < j );
   return (var - eps*eps/n)/(n - 1);
}

/*!
 * Returns the variance from the mean of a sequence [i,j).
 *
 * For sequences of less than two elements, this function returns zero.
 *
 * This implementation uses a two-pass compensated summation algorithm to
 * minimize roundoff errors (see References).
 *
 * \b References
 *
 * William H. Press et al., <em>Numerical Recipes in C: The Art of Scientific
 * Computing, Second Edition</em> (1997 reprint) Cambridge University Press,
 * page 613.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double Variance( const T* __restrict__ i, const T* __restrict__ j ) noexcept
{
   distance_type n = j - i;
   if ( n < 2 )
      return 0;
   double m = 0;
   for ( const T* f = i; f < j; ++f )
      m += double( *f );
   m /= n;
   double var = 0, eps = 0;
   do
   {
      double d = double( *i++ ) - m;
      var += d*d;
      eps += d;
   }
   while ( i < j );
   return (var - eps*eps/n)/(n - 1);
}

/*!
 * Returns the standard deviation of a sequence [i,j) with respect to the
 * specified \a center value.
 *
 * For sequences of less than two elements, this function returns zero.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double StdDev( const T* __restrict__ i, const T* __restrict__ j, double center ) noexcept
{
   return Sqrt( Variance( i, j, center ) );
}

/*!
 * Returns the standard deviation from the mean of a sequence [i,j).
 *
 * For sequences of less than two elements, this function returns zero.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double StdDev( const T* __restrict__ i, const T* __restrict__ j ) noexcept
{
   return Sqrt( Variance( i, j ) );
}

/*!
 * Returns the median value of a sequence [i,j).
 *
 * For scalar data types the following algorithms are used:
 *
 * \li Hard-coded, fast selection networks for small sequences of 32 or less
 * elements.
 *
 * \li A quick selection algorithm for sequences of up to about 2M elements.
 * The actual limit has been determined empirically and can vary across PCL
 * versions. This single-threaded algorithm can use up to 16 MiB of additional
 * memory allocated dynamically (for 8-byte types such as \c double).
 *
 * \li A parallelized, fast histogram-based algorithm for sequences larger than
 * the limit described above. This algorithm has negligible additional memory
 * space requirements.
 *
 * For non-scalar data types, this function requires the following type
 * conversion operator publicly defined for the type T:
 *
 * \code T::operator double() const; \endcode
 *
 * This operator will be used to generate a temporary dynamic array of
 * \c double values with the length of the input sequence, which will be used
 * to compute the median with the algorithms enumerated above.
 *
 * \b References (selection networks)
 *
 * \li Knuth, D. E., <em>The Art of Computer Programming, volume 3:
 * Sorting and Searching,</em> Addison Wesley, 1973.
 *
 * \li Hillis, W. D., <em>Co-evolving parasites improve simulated
 * evolution as an optimization procedure.</em> Langton, C. et al. (Eds.),
 * Artificial Life II. Addison Wesley, 1992.
 *
 * \li Hugues Juille, <em>Evolution of Non-Deterministic Incremental
 * Algorithms as a New Approach for Search in State Spaces,</em> 1995
 *
 * \b References (quick select algorithm)
 *
 * \li William H. Press et al., <em>Numerical Recipes 3rd Edition: The Art of
 * Scientific Computing,</em> Cambridge University Press, 2007, Section 8.5.
 *
 * \li Robert Sedgewick, Kevin Wayne, <em>Algorithms, 4th Edition,</em>
 * Addison-Wesley Professional, 2011, pp 345-347.
 *
 * \ingroup statistical_functions
 */
template <class T> inline double Median( const T* __restrict__ i, const T* __restrict__ j )
{
   distance_type n = j - i;
   if ( n < 1 )
      return 0;
   if ( n == 1 )
      return double( *i );
   double* d = new double[ n ];
   double* __restrict__ t = d;
   do
      *t++ = double( *i++ );
   while ( i < j );
   double m = double( *pcl::Select( d, t, n >> 1 ) );
   if ( (n & 1) == 0 )
      m = (m + double( *pcl::Select( d, t, (n >> 1)-1 ) ))/2;
   delete [] d;
   return m;
}

double PCL_FUNC Median( const unsigned char* __restrict__ i, const unsigned char* __restrict__ j );
double PCL_FUNC Median( const signed char* __restrict__ i, const signed char* __restrict__ j );
double PCL_FUNC Median( const wchar_t* __restrict__ i, const wchar_t* __restrict__ j );
double PCL_FUNC Median( const unsigned short* __restrict__ i, const unsigned short* __restrict__ j );
double PCL_FUNC Median( const signed short* __restrict__ i, const signed short* __restrict__ j );
double PCL_FUNC Median( const unsigned int* __restrict__ i, const unsigned int* __restrict__ j );
double PCL_FUNC Median( const signed int* __restrict__ i, const signed int* __restrict__ j );
double PCL_FUNC Median( const unsigned long* __restrict__ i, const unsigned long* __restrict__ j );
double PCL_FUNC Median( const signed long* __restrict__ i, const signed long* __restrict__ j );
double PCL_FUNC Median( const unsigned long long* __restrict__ i, const unsigned long long* __restrict__ j );
double PCL_FUNC Median( const signed long long* __restrict__ i, const signed long long* __restrict__ j );
double PCL_FUNC Median( const float* __restrict__ i, const float* __restrict__ j );
double PCL_FUNC Median( const double* __restrict__ i, const double* __restrict__ j );
double PCL_FUNC Median( const long double* __restrict__ i, const long double* __restrict__ j );

#define CMPXCHG( a, b )  \
   if ( i[b] < i[a] ) pcl::Swap( i[a], i[b] )

#define MEAN( a, b ) \
   (double( a ) + double( b ))/2

/*!
 * Returns the median value of a sequence [i,j), altering the existing order of
 * elements in the input sequence.
 *
 * This function is intended for sequences of non-scalar objects where the
 * order of elements is irrelevant, and hence generation of a working duplicate
 * is unnecessary. The following type conversion operator must be publicly
 * defined for the type T:
 *
 * \code T::operator double() const; \endcode
 *
 * The following algorithms are used:
 *
 * \li Hard-coded, fast selection networks for sequences of 9 or less elements.
 *
 * \li A quick selection algorithm for sequences larger than 9 elements.
 *
 * \note This is a \e destructive median calculation algorithm: it alters the
 * existing order of items in the input [i,j) sequence.
 *
 * \b References (selection networks)
 *
 * \li Knuth, D. E., <em>The Art of Computer Programming, volume 3:
 * Sorting and Searching,</em> Addison Wesley, 1973.
 *
 * \li Hillis, W. D., <em>Co-evolving parasites improve simulated
 * evolution as an optimization procedure.</em> Langton, C. et al. (Eds.),
 * Artificial Life II. Addison Wesley, 1992.
 *
 * \li Hugues Juille, <em>Evolution of Non-Deterministic Incremental
 * Algorithms as a New Approach for Search in State Spaces,</em> 1995
 *
 * \b References (quick select algorithm)
 *
 * \li William H. Press et al., <em>Numerical Recipes 3rd Edition: The Art of
 * Scientific Computing,</em> Cambridge University Press, 2007, Section 8.5.
 *
 * \li Robert Sedgewick, Kevin Wayne, <em>Algorithms, 4th Edition,</em>
 * Addison-Wesley Professional, 2011, pp 345-347.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double MedianDestructive( T* __restrict__ i, T* __restrict__ j ) noexcept
{
   distance_type n = j - i;
   if ( n < 1 )
      return 0;

   switch ( n )
   {
   case  1: // !?
      return i[0];
   case  2:
      return MEAN( i[0], i[1] );
   case  3:
      CMPXCHG( 0, 1 ); CMPXCHG( 1, 2 );
      return pcl::Max( i[0], i[1] );
   case  4:
      CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
      CMPXCHG( 1, 3 );
      return MEAN( i[1], i[2] );
   case  5:
      CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 0, 3 );
      CMPXCHG( 1, 4 ); CMPXCHG( 1, 2 ); CMPXCHG( 2, 3 );
      return pcl::Max( i[1], i[2] );
   case  6:
      CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
      CMPXCHG( 1, 3 ); CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 );
      CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 1, 4 );
      CMPXCHG( 2, 4 ); CMPXCHG( 3, 5 ); CMPXCHG( 3, 4 );
      return MEAN( i[2], i[3] );
   case  7:
      CMPXCHG( 0, 5 ); CMPXCHG( 0, 3 ); CMPXCHG( 1, 6 );
      CMPXCHG( 2, 4 ); CMPXCHG( 0, 1 ); CMPXCHG( 3, 5 );
      CMPXCHG( 2, 6 ); CMPXCHG( 2, 3 ); CMPXCHG( 3, 6 );
      CMPXCHG( 4, 5 ); CMPXCHG( 1, 4 ); CMPXCHG( 1, 3 );
      return pcl::Min( i[3], i[4] );
   case  8:
      CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 2, 6 );
      CMPXCHG( 3, 7 ); CMPXCHG( 0, 2 ); CMPXCHG( 1, 3 );
      CMPXCHG( 4, 6 ); CMPXCHG( 5, 7 ); CMPXCHG( 2, 4 );
      CMPXCHG( 3, 5 ); CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 );
      CMPXCHG( 4, 5 ); CMPXCHG( 6, 7 ); CMPXCHG( 1, 4 );
      CMPXCHG( 3, 6 );
      return MEAN( i[3], i[4] );
   case  9:
      CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
      CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 6, 7 );
      CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
      CMPXCHG( 0, 3 ); CMPXCHG( 5, 8 ); CMPXCHG( 4, 7 );
      CMPXCHG( 3, 6 ); CMPXCHG( 1, 4 ); CMPXCHG( 2, 5 );
      CMPXCHG( 4, 7 ); CMPXCHG( 4, 2 ); CMPXCHG( 6, 4 );
      return pcl::Min( i[2], i[4] );
   default:
      {
         double m = double( *pcl::Select( i, j, n >> 1 ) );
         if ( n & 1 )
            return m;
         return MEAN( m, double( *pcl::Select( i, j, (n >> 1)-1 ) ) );
      }
   }
}

#undef CMPXCHG

#define CMPXCHG( a, b )  \
   if ( p( i[b], i[a] ) ) pcl::Swap( i[a], i[b] )

/*!
 * Returns the median value of a sequence [i,j), altering the existing order of
 * elements in the input sequence.
 *
 * Element comparison is given by a binary predicate \a p such that p( a, b )
 * is true for any pair a, b of elements such that a precedes b.
 *
 * We use fast, hard-coded selection networks for sequences of 9 or less
 * elements, and a quick selection algorithm for larger sets.
 *
 * See the documentation of MedianDestructive( T*, T* ) for more information
 * and references.
 *
 * \ingroup statistical_functions
 */
template <typename T, class BP> inline double MedianDestructive( T* __restrict__ i, T* __restrict__ j, BP p ) noexcept
{
   distance_type n = j - i;
   if ( n < 1 )
      return 0;

   switch ( n )
   {
   case  1: // !?
      return i[0];
   case  2:
      return MEAN( i[0], i[1] );
   case  3:
      CMPXCHG( 0, 1 ); CMPXCHG( 1, 2 );
      return pcl::Max( i[0], i[1] );
   case  4:
      CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
      CMPXCHG( 1, 3 );
      return MEAN( i[1], i[2] );
   case  5:
      CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 0, 3 );
      CMPXCHG( 1, 4 ); CMPXCHG( 1, 2 ); CMPXCHG( 2, 3 );
      return pcl::Max( i[1], i[2] );
   case  6:
      CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 ); CMPXCHG( 0, 2 );
      CMPXCHG( 1, 3 ); CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 );
      CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 1, 4 );
      CMPXCHG( 2, 4 ); CMPXCHG( 3, 5 ); CMPXCHG( 3, 4 );
      return MEAN( i[2], i[3] );
   case  7:
      CMPXCHG( 0, 5 ); CMPXCHG( 0, 3 ); CMPXCHG( 1, 6 );
      CMPXCHG( 2, 4 ); CMPXCHG( 0, 1 ); CMPXCHG( 3, 5 );
      CMPXCHG( 2, 6 ); CMPXCHG( 2, 3 ); CMPXCHG( 3, 6 );
      CMPXCHG( 4, 5 ); CMPXCHG( 1, 4 ); CMPXCHG( 1, 3 );
      return pcl::Min( i[3], i[4] );
   case  8:
      CMPXCHG( 0, 4 ); CMPXCHG( 1, 5 ); CMPXCHG( 2, 6 );
      CMPXCHG( 3, 7 ); CMPXCHG( 0, 2 ); CMPXCHG( 1, 3 );
      CMPXCHG( 4, 6 ); CMPXCHG( 5, 7 ); CMPXCHG( 2, 4 );
      CMPXCHG( 3, 5 ); CMPXCHG( 0, 1 ); CMPXCHG( 2, 3 );
      CMPXCHG( 4, 5 ); CMPXCHG( 6, 7 ); CMPXCHG( 1, 4 );
      CMPXCHG( 3, 6 );
      return MEAN( i[3], i[4] );
   case  9:
      CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
      CMPXCHG( 0, 1 ); CMPXCHG( 3, 4 ); CMPXCHG( 6, 7 );
      CMPXCHG( 1, 2 ); CMPXCHG( 4, 5 ); CMPXCHG( 7, 8 );
      CMPXCHG( 0, 3 ); CMPXCHG( 5, 8 ); CMPXCHG( 4, 7 );
      CMPXCHG( 3, 6 ); CMPXCHG( 1, 4 ); CMPXCHG( 2, 5 );
      CMPXCHG( 4, 7 ); CMPXCHG( 4, 2 ); CMPXCHG( 6, 4 );
      return pcl::Min( i[2], i[4] );
   default:
      {
         double m = double( *pcl::Select( i, j, n >> 1, p ) );
         if ( n & 1 )
            return m;
         return MEAN( m, double( *pcl::Select( i, j, (n >> 1)-1, p ) ) );
      }
   }
}

#undef CMPXCHG
#undef MEAN

/*!
 * Returns the k-th order statistic of a sequence [i,j).
 *
 * For scalar data types the following algorithms are used:
 *
 * \li A quick selection algorithm for sequences of up to about 2M elements.
 * The actual limit has been determined empirically and can vary across PCL
 * versions. This single-threaded algorithm can use up to 16 MiB of additional
 * memory allocated dynamically (for 8-byte types such as \c double).
 *
 * \li A parallelized, fast histogram-based algorithm for sequences larger than
 * the limit described above. This algorithm has negligible additional memory
 * space requirements.
 *
 * For non-scalar data types, this function requires the following type
 * conversion operator publicly defined for the type T:
 *
 * \code T::operator double() const; \endcode
 *
 * This operator will be used to generate a temporary dynamic array of
 * \c double values with the length of the input sequence, which will be used
 * to compute the median with the quick selection algorithm.
 *
 * \b References (quick select algorithm)
 *
 * \li William H. Press et al., <em>Numerical Recipes 3rd Edition: The Art of
 * Scientific Computing,</em> Cambridge University Press, 2007, Section 8.5.
 *
 * \li Robert Sedgewick, Kevin Wayne, <em>Algorithms, 4th Edition,</em>
 * Addison-Wesley Professional, 2011, pp 345-347.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double OrderStatistic( const T* __restrict__ i, const T* __restrict__ j, distance_type k )
{
   distance_type n = j - i;
   if ( n < 1 || k < 0 || k >= n )
      return 0;
   if ( n == 1 )
      return double( *i );
   double* d = new double[ n ];
   double* t = d;
   do
      *t++ = double( *i++ );
   while ( i < j );
   double s = *pcl::Select( d, t, k );
   delete [] d;
   return s;
}

double PCL_FUNC OrderStatistic( const unsigned char* __restrict__ i, const unsigned char* __restrict__ j, distance_type k );
double PCL_FUNC OrderStatistic( const signed char* __restrict__ i, const signed char* __restrict__ j, distance_type k );
double PCL_FUNC OrderStatistic( const wchar_t* __restrict__ i, const wchar_t* __restrict__ j, distance_type k );
double PCL_FUNC OrderStatistic( const unsigned short* __restrict__ i, const unsigned short* __restrict__ j, distance_type k );
double PCL_FUNC OrderStatistic( const signed short* __restrict__ i, const signed short* __restrict__ j, distance_type k );
double PCL_FUNC OrderStatistic( const unsigned int* __restrict__ i, const unsigned int* __restrict__ j, distance_type k );
double PCL_FUNC OrderStatistic( const signed int* __restrict__ i, const signed int* __restrict__ j, distance_type k );
double PCL_FUNC OrderStatistic( const unsigned long* __restrict__ i, const unsigned long* __restrict__ j, distance_type k );
double PCL_FUNC OrderStatistic( const signed long* __restrict__ i, const signed long* __restrict__ j, distance_type k );
double PCL_FUNC OrderStatistic( const unsigned long long* __restrict__ i, const unsigned long long* __restrict__ j, distance_type k );
double PCL_FUNC OrderStatistic( const signed long long* __restrict__ i, const signed long long* __restrict__ j, distance_type k );
double PCL_FUNC OrderStatistic( const float* __restrict__ i, const float* __restrict__ j, distance_type k );
double PCL_FUNC OrderStatistic( const double* __restrict__ i, const double* __restrict__ j, distance_type k );
double PCL_FUNC OrderStatistic( const long double* __restrict__ i, const long double* __restrict__ j, distance_type k );

/*!
 * Returns the k-th order statistic of a sequence [i,j), altering the existing
 * order of elements in the input sequence.
 *
 * This function is intended for sequences of non-scalar objects where the
 * order of elements is irrelevant, and hence generation of a working duplicate
 * is unnecessary. The following type conversion operator must be publicly
 * defined for the type T:
 *
 * <\code> T::operator double() const; \endcode
 *
 * The quick selection algorithm is used to find the k-th element in the
 * ordered sequence.
 *
 * \note This is a \e destructive algorithm: it alters the existing order of
 * items in the input [i,j) sequence.
 *
 * \b References (quick select algorithm)
 *
 * \li William H. Press et al., <em>Numerical Recipes 3rd Edition: The Art of
 * Scientific Computing,</em> Cambridge University Press, 2007, Section 8.5.
 *
 * \li Robert Sedgewick, Kevin Wayne, <em>Algorithms, 4th Edition,</em>
 * Addison-Wesley Professional, 2011, pp 345-347.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double OrderStatisticDestructive( T* __restrict__ i, T* __restrict__ j, distance_type k ) noexcept
{
   distance_type n = j - i;
   if ( n < 1 || k < 0 || k >= n )
      return 0;
   if ( n == 1 )
      return double( *i );
   return double( *pcl::Select( i, j, k ) );
}

/*!
 * Returns the k-th order statistic of a sequence [i,j), altering the existing
 * order of elements in the input sequence.
 *
 * Element comparison is given by a binary predicate \a p such that p( a, b )
 * is true for any pair a, b of elements such that a precedes b.
 *
 * See the documentation of OrderStatisticDestructive( T*, T*, distance_type )
 * for more information and references.
 *
 * \ingroup statistical_functions
 */
template <typename T, class BP> inline double OrderStatisticDestructive( const T* __restrict__ i, const T* __restrict__ j, distance_type k, BP p ) noexcept
{
   distance_type n = j - i;
   if ( n < 1 || k < 0 || k >= n )
      return 0;
   if ( n == 1 )
      return double( *i );
   return double( *pcl::Select( i, j, k, p ) );
}

/*!
 * Computes the two-sided, asymmetric trimmed mean of a sequence [i,j).
 *
 * The returned value is the arithmetic mean of a sequence [I+l,J-h-1], where
 * [I,J) is the input sequence [i,j) sorted in ascending order.
 *
 * Let n = j-i be the length of the input sequence. For empty sequences
 * (n &le; 0) or completely truncated sequences (l+h >= n), this function
 * returns zero. Otherwise the returned value is the arithmetic mean of the
 * nonrejected n-l-h elements in the sorted sequence, as described above.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double TrimmedMean( const T* __restrict__ i, const T* __restrict__ j, distance_type l = 1, distance_type h = 1 )
{
   distance_type n = j - i;
   if ( n < 1 )
      return 0;
   if ( l+h < 1 )
      return Sum( i, j )/n;
   if ( l+h >= n )
      return 0;
   for ( double s = 0,
         t0 = OrderStatistic( i, j, l ),
         t1 = OrderStatistic( i, j, n-h-1 ); ; )
   {
      double x = double( *i );
      if ( x >= t0 )
         if ( x <= t1 )
            s += x;
      if ( ++i == j )
         return s/(n - l - h);
   }
}

/*!
 * Computes the two-sided, asymmetric trimmed mean of a sequence [i,j),
 * possibly altering the existing order of elements in the input sequence.
 *
 * The returned value is the arithmetic mean of a sequence [I+l,J-h-1], where
 * [I,J) represents the input sequence [i,j) sorted in ascending order.
 *
 * Let n = j-i be the length of the input sequence. For empty sequences
 * (n &le; 0) or completely truncated sequences (l+h >= n), this function
 * returns zero. Otherwise the returned value is the arithmetic mean of the
 * nonrejected n-l-h elements in the sorted sequence, as described above.
 *
 * \note This is a \e destructive trimmed mean calculation algorithm: it may
 * alter the existing order of items in the input [i,j) sequence.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double TrimmedMeanDestructive( T* __restrict__ i, T* __restrict__ j, distance_type l = 1, distance_type h = 1 ) noexcept
{
   distance_type n = j - i;
   if ( n < 1 )
      return 0;
   if ( l+h < 1 )
      return Sum( i, j )/n;
   if ( l+h >= n )
      return 0;
   for ( double s = 0,
         t0 = OrderStatisticDestructive( i, j, l ),
         t1 = OrderStatisticDestructive( i, j, n-h-1 ); ; )
   {
      double x = double( *i );
      if ( x >= t0 )
         if ( x <= t1 )
            s += x;
      if ( ++i == j )
         return s/(n - l - h);
   }
}

/*!
 * Computes the two-sided, asymmetric trimmed mean of squares of a sequence
 * [i,j).
 *
 * The returned value is the arithmetic mean of squares of a sequence
 * [I+l,J-h-1], where [I,J) represents the input sequence [i,j) sorted in
 * ascending order.
 *
 * Let n = j-i be the length of the input sequence. For empty sequences
 * (n &le; 0) or completely truncated sequences (l+h >= n), this function
 * returns zero. Otherwise the returned value is the arithmetic mean of the
 * squared nonrejected n-l-h elements in the sorted sequence, as described
 * above.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double TrimmedMeanOfSquares( const T* __restrict__ i, const T* __restrict__ j, distance_type l = 1, distance_type h = 1 )
{
   distance_type n = j - i;
   if ( n < 1 )
      return 0;
   if ( l+h < 1 )
      return Sum( i, j )/n;
   if ( l+h >= n )
      return 0;
   for ( double s = 0,
         t0 = OrderStatistic( i, j, l ),
         t1 = OrderStatistic( i, j, n-h-1 ); ; )
   {
      double x = double( *i );
      if ( x >= t0 )
         if ( x <= t1 )
            s += x*x;
      if ( ++i == j )
         return s/(n - l - h);
   }
}

/*!
 * Computes the two-sided, asymmetric trimmed mean of squares of a sequence
 * [i,j), possibly altering the existing order of elements in the input
 * sequence.
 *
 * The returned value is the arithmetic mean of squares of a sequence
 * [I+l,J-h-1], where [I,J) represents the input sequence [i,j) sorted in
 * ascending order.
 *
 * Let n = j-i be the length of the input sequence. For empty sequences
 * (n &le; 0) or completely truncated sequences (l+h >= n), this function
 * returns zero. Otherwise the returned value is the arithmetic mean of the
 * squared nonrejected n-l-h elements in the sorted sequence, as described
 * above.
 *
 * \note This is a \e destructive trimmed mean of squares calculation
 * algorithm: it may alter the existing order of items in the input [i,j)
 * sequence.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double TrimmedMeanOfSquaresDestructive( T* __restrict__ i, T* __restrict__ j, distance_type l = 1, distance_type h = 1 ) noexcept
{
   distance_type n = j - i;
   if ( n < 1 )
      return 0;
   if ( l+h < 1 )
      return Sum( i, j )/n;
   if ( l+h >= n )
      return 0;
   for ( double s = 0,
         t0 = OrderStatisticDestructive( i, j, l ),
         t1 = OrderStatisticDestructive( i, j, n-h-1 ); ; )
   {
      double x = double( *i );
      if ( x >= t0 )
         if ( x <= t1 )
            s += x*x;
      if ( ++i == j )
         return s/(n - l - h);
   }
}

/*!
 * Returns the average absolute deviation of the values in a sequence [i,j)
 * with respect to the specified \a center value.
 *
 * When the median of the sequence is used as the center value, this function
 * returns the average absolute deviation from the median, which is a
 * well-known estimator of dispersion.
 *
 * For sequences of less than two elements, this function returns zero.
 *
 * See the remarks made for the Sum() function, which are equally applicable in
 * this case. See StableAvgDev() for a (slow) numerically stable version of
 * this function.
 *
 * \note To make the average absolute deviation about the median consistent
 * with the standard deviation of a normal distribution, it must be
 * multiplied by the constant 1.2533.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double AvgDev( const T* __restrict__ i, const T* __restrict__ j, double center ) noexcept
{
   distance_type n = j - i;
   if ( n < 2 )
      return 0;
   double d = 0;
   do
      d += Abs( double( *i++ ) - center );
   while ( i < j );
   return d/n;
}

/*!
 * Returns the average absolute deviation of the values in a sequence [i,j)
 * with respect to the specified \a center value, using a numerically stable
 * summation algorithm to minimize roundoff error.
 *
 * When the median of the sequence is used as the center value, this function
 * returns the average absolute deviation from the median, which is a
 * well-known estimator of dispersion.
 *
 * For sequences of less than two elements, this function returns zero.
 *
 * In the current PCL versions, this function implements the Kahan summation
 * algorithm to reduce roundoff error to the machine's floating point error.
 *
 * \note To make the average absolute deviation about the median consistent
 * with the standard deviation of a normal distribution, it must be
 * multiplied by the constant 1.2533.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double StableAvgDev( const T* __restrict__ i, const T* __restrict__ j, double center ) noexcept
{
   distance_type n = j - i;
   if ( n < 2 )
      return 0;
   double sum = 0;
   double eps = 0;
   do
   {
      double y = Abs( double( *i++ ) - center ) - eps;
      double t = sum + y;
      eps = (t - sum) - y;
      sum = t;
   }
   while ( i < j );
   return sum/n;
}

/*!
 * Returns the average absolute deviation from the median of the values in a
 * sequence [i,j).
 *
 * The average absolute deviation from the median is a well-known estimator of
 * dispersion.
 *
 * For sequences of less than two elements, this function returns zero.
 *
 * See the remarks made for the Sum() function, which are equally applicable in
 * this case. See StableAvgDev() for a (slow) numerically stable version of
 * this function.
 *
 * \note To make the average absolute deviation about the median consistent
 * with the standard deviation of a normal distribution, it must be
 * multiplied by the constant 1.2533.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double AvgDev( const T* __restrict__ i, const T* __restrict__ j )
{
   distance_type n = j - i;
   if ( n < 2 )
      return 0;
   double m = Median( i, j );
   double d = 0;
   do
      d += Abs( double( *i++ ) - m );
   while ( i < j );
   return d/n;
}

/*!
 * Computes the average absolute deviation from the median of the values in a
 * sequence [i,j) using a numerically stable summation algorithm to minimize
 * roundoff error.
 *
 * The average absolute deviation from the median is a well-known estimator of
 * dispersion.
 *
 * For sequences of less than two elements, this function returns zero.
 *
 * In the current PCL versions, this function implements the Kahan summation
 * algorithm to reduce roundoff error to the machine's floating point error.
 *
 * \note To make the average absolute deviation about the median consistent
 * with the standard deviation of a normal distribution, it must be
 * multiplied by the constant 1.2533.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double StableAvgDev( const T* __restrict__ i, const T* __restrict__ j )
{
   return pcl::StableAvgDev( i, j, pcl::Median( i, j ) );
}

/*!
 * \struct TwoSidedEstimate
 * \brief Two-sided descriptive statistical estimate.
 *
 * This POD structure is returned by functions implementing two-sided scale
 * estimators. Given a sample X = {x_0,...,x_n-1} and a reference center value
 * m (typically, the median of X), a two-sided scale estimate is computed as
 * two separate components: A <em>low estimate</em> for all x in X such that
 * x &le; m, and a <em>high estimate</em> for all x in X such that x > m.
 *
 * Two-sided scale estimates are important in normalization for accurate
 * outlier rejection and sample distribution characterization, especially for
 * skewed or asymmetric distributions.
 *
 * \ingroup statistical_functions
 * \sa TwoSidedAvgDev(), TwoSidedMAD(), TwoSidedBiweightMidvariance()
 */
struct TwoSidedEstimate
{
   double low = 0;  //!< Low estimate component.
   double high = 0; //!< High estimate component.

   /*!
    * Default constructor. Both components are initialized to zero.
    */
   TwoSidedEstimate() = default;

   /*!
    * Copy constructor.
    */
   TwoSidedEstimate( const TwoSidedEstimate& ) = default;

   /*!
    * Move constructor.
    */
   TwoSidedEstimate( TwoSidedEstimate&& ) = default;

   /*!
    * Copy assignment operator.
    */
   TwoSidedEstimate& operator =( const TwoSidedEstimate& ) = default;

   /*!
    * Move assignment operator.
    */
   TwoSidedEstimate& operator =( TwoSidedEstimate&& ) = default;

   /*!
    * Constructor from separate low and high components.
    */
   template <typename T1, typename T2>
   TwoSidedEstimate( const T1& l, const T2& h )
      : low( double( l ) )
      , high( double( h ) )
   {
   }

   /*!
    * Constructor from a unique component value \a x, which is assigned to both
    * the low and high estimate components.
    */
   template <typename T>
   TwoSidedEstimate( const T& x )
   {
      low = high = double( x );
   }

   /*!
    * Returns true iff this two-sided scale estimate is valid. A two-sided
    * scale estimate is valid if both the low and high components are finite,
    * positive and nonzero with respect to the machine epsilon for the type
    * \c double.
    */
   bool IsValid() const noexcept
   {
      return IsFinite( low ) && low > std::numeric_limits<double>::epsilon()
          && IsFinite( high ) && high > std::numeric_limits<double>::epsilon();
   }

   /*!
    * Returns the sum of the low and high estimate components.
    */
   double Total() const noexcept
   {
      return low + high;
   }

   /*!
    * Returns the arithmetic mean of the low and high estimate components if
    * both are nonzero. Otherwise returns the maximum component.
    */
   double Mean() const noexcept
   {
      if ( low != 0 )
      {
         if ( high != 0 )
            return (low + high)/2;
         return low;
      }
      return high;
   }

   /*!
    * Conversion to scalar. Equivalent to Mean().
    */
   explicit operator double() const noexcept
   {
      return Mean();
   }

   /*!
    * Assignment-multiplication by a scalar. Returns a reference to this
    * object.
    */
   TwoSidedEstimate& operator *=( double x ) noexcept
   {
      low *= x;
      high *= x;
      return *this;
   }

   /*!
    * Assignment-division by a scalar. Returns a reference to this object.
    */
   TwoSidedEstimate& operator /=( double x ) noexcept
   {
      low /= x;
      high /= x;
      return *this;
   }

   /*!
    * Assignment-division by a two-sided estimate. Returns a reference to this
    * object.
    */
   TwoSidedEstimate& operator /=( const TwoSidedEstimate& e ) noexcept
   {
      low /= e.low;
      high /= e.high;
      return *this;
   }

   /*!
    * Returns the result of multiplying this two-sided estimate by a scalar.
    */
   TwoSidedEstimate operator *( double x ) const noexcept
   {
      return { low*x, high*x };
   }

   /*!
    * Returns the result of dividing this two-sided estimate by a scalar.
    */
   TwoSidedEstimate operator /( double x ) const noexcept
   {
      return { low/x, high/x };
   }

   /*!
    * Returns the result of the component wise division of this two-sided
    * estimate by another two-sided estimate.
    */
   TwoSidedEstimate operator /( const TwoSidedEstimate& e ) const noexcept
   {
      return { low/e.low, high/e.high };
   }
};

/*!
 * Returns the component wise square root of a two-sided estimate.
 * \ingroup statistical_functions
 */
inline TwoSidedEstimate Sqrt( const TwoSidedEstimate& e ) noexcept
{
   return { Sqrt( e.low ), Sqrt( e.high ) };
}

/*!
 * Returns the component wise exponent function of a two-sided estimate.
 * \ingroup statistical_functions
 */
template <typename T> inline TwoSidedEstimate Pow( const TwoSidedEstimate& e, T x ) noexcept
{
   double x_ = double( x );
   return { Pow( e.low, x_ ), Pow( e.high, x_ ) };
}

/*!
 * Returns the two-sided average absolute deviation of the values in a sequence
 * [i,j) with respect to the specified \a center value.
 *
 * When the median of the sequence is used as the center value, this function
 * returns the average absolute deviation from the median, which is a
 * well-known estimator of dispersion.
 *
 * For sequences of less than two elements, this function returns zero.
 *
 * See the remarks made for the Sum() function, which are equally applicable in
 * this case. See StableAvgDev() for a (slow) numerically stable version of
 * this function.
 *
 * \note To make the average absolute deviation about the median consistent
 * with the standard deviation of a normal distribution, it must be
 * multiplied by the constant 1.2533.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline TwoSidedEstimate TwoSidedAvgDev( const T* __restrict__ i, const T* __restrict__ j, double center ) noexcept
{
   double dl = 0, dh = 0;
   distance_type nl = 0, nh = 0;
   while ( i < j )
   {
      double x = double( *i++ );
      if ( x <= center )
      {
         dl += center - x;
         ++nl;
      }
      else
      {
         dh += x - center;
         ++nh;
      }
   }
   return { (nl > 1) ? dl/nl : 0.0,
            (nh > 1) ? dh/nh : 0.0 };
}

/*!
 * Returns the two-sided average absolute deviation from the median of the
 * values in a sequence [i,j).
 *
 * The average absolute deviation from the median is a well-known estimator of
 * dispersion.
 *
 * For sequences of less than two elements, this function returns zero.
 *
 * See the remarks made for the Sum() function, which are equally applicable in
 * this case. See StableAvgDev() for a (slow) numerically stable version of
 * this function.
 *
 * \note To make the average absolute deviation about the median consistent
 * with the standard deviation of a normal distribution, it must be
 * multiplied by the constant 1.2533.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline TwoSidedEstimate TwoSidedAvgDev( const T* __restrict__ i, const T* __restrict__ j )
{
   return pcl::TwoSidedAvgDev( i, j, pcl::Median( i, j ) );
}

/*!
 * Returns the median absolute deviation (MAD) of the values in a sequence
 * [i,j) with respect to the specified \a center value.
 *
 * The MAD is a well-known robust estimator of scale.
 *
 * For sequences of less than two elements, this function returns zero.
 *
 * \note To make the MAD estimator consistent with the standard deviation of
 * a normal distribution, its result must be multiplied by the constant 1.4826.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double MAD( const T* __restrict__ i, const T* __restrict__ j, double center )
{
   distance_type n = j - i;
   if ( n < 2 )
      return 0;
   double* d = new double[ n ];
   double* p = d;
   do
      *p++ = Abs( double( *i++ ) - center );
   while ( i < j );
   double m = pcl::Median( d, d+n );
   delete [] d;
   return m;
}

double PCL_FUNC MAD( const unsigned char* __restrict__ i, const unsigned char* __restrict__ j, double center );
double PCL_FUNC MAD( const signed char* __restrict__ i, const signed char* __restrict__ j, double center );
double PCL_FUNC MAD( const wchar_t* __restrict__ i, const wchar_t* __restrict__ j, double center );
double PCL_FUNC MAD( const unsigned short* __restrict__ i, const unsigned short* __restrict__ j, double center );
double PCL_FUNC MAD( const signed short* __restrict__ i, const signed short* __restrict__ j, double center );
double PCL_FUNC MAD( const unsigned int* __restrict__ i, const unsigned int* __restrict__ j, double center );
double PCL_FUNC MAD( const signed int* __restrict__ i, const signed int* __restrict__ j, double center );
double PCL_FUNC MAD( const unsigned long* __restrict__ i, const unsigned long* __restrict__ j, double center );
double PCL_FUNC MAD( const signed long* __restrict__ i, const signed long* __restrict__ j, double center );
double PCL_FUNC MAD( const unsigned long long* __restrict__ i, const unsigned long long* __restrict__ j, double center );
double PCL_FUNC MAD( const signed long long* __restrict__ i, const signed long long* __restrict__ j, double center );
double PCL_FUNC MAD( const float* __restrict__ i, const float* __restrict__ j, double center );
double PCL_FUNC MAD( const double* __restrict__ i, const double* __restrict__ j, double center );
double PCL_FUNC MAD( const long double* __restrict__ i, const long double* __restrict__ j, double center );

/*!
 * Returns the median absolute deviation from the median (MAD) for the values
 * in a sequence [i,j).
 *
 * The MAD is a well-known robust estimator of scale.
 *
 * For sequences of less than two elements, this function returns zero.
 *
 * \note To make the MAD estimator consistent with the standard deviation of
 * a normal distribution, its result must be multiplied by the constant 1.4826.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline double MAD( const T* __restrict__ i, const T* __restrict__ j )
{
   return pcl::MAD( i, j, pcl::Median( i, j ) );
}

/*!
 * Returns the two-sided median absolute deviation (MAD) of the values in a
 * sequence [i,j) with respect to the specified \a center value.
 *
 * The MAD is a well-known robust estimator of scale.
 *
 * For sequences of less than two elements, this function returns zero.
 *
 * \note To make the MAD estimator consistent with the standard deviation of
 * a normal distribution, its result must be multiplied by the constant 1.4826.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline TwoSidedEstimate TwoSidedMAD( const T* __restrict__ i, const T* __restrict__ j, double center )
{
   distance_type n = j - i;
   if ( n < 2 )
      return 0;
   double* d = new double[ n ];
   double* __restrict__ p = d;
   double* __restrict__ q = d + n;
   do
   {
      double x = double( *i++ );
      if ( x <= center )
         *p++ = center - x;
      else
         *--q = x - center;
   }
   while( i < j );
   double l = pcl::Median( d, p );
   double h = pcl::Median( q, d+n );
   delete [] d;
   return { l, h };
}

TwoSidedEstimate PCL_FUNC TwoSidedMAD( const unsigned char* __restrict__ i, const unsigned char* __restrict__ j, double center );
TwoSidedEstimate PCL_FUNC TwoSidedMAD( const signed char* __restrict__ i, const signed char* __restrict__ j, double center );
TwoSidedEstimate PCL_FUNC TwoSidedMAD( const wchar_t* __restrict__ i, const wchar_t* __restrict__ j, double center );
TwoSidedEstimate PCL_FUNC TwoSidedMAD( const unsigned short* __restrict__ i, const unsigned short* __restrict__ j, double center );
TwoSidedEstimate PCL_FUNC TwoSidedMAD( const signed short* __restrict__ i, const signed short* __restrict__ j, double center );
TwoSidedEstimate PCL_FUNC TwoSidedMAD( const unsigned int* __restrict__ i, const unsigned int* __restrict__ j, double center );
TwoSidedEstimate PCL_FUNC TwoSidedMAD( const signed int* __restrict__ i, const signed int* __restrict__ j, double center );
TwoSidedEstimate PCL_FUNC TwoSidedMAD( const unsigned long* __restrict__ i, const unsigned long* __restrict__ j, double center );
TwoSidedEstimate PCL_FUNC TwoSidedMAD( const signed long* __restrict__ i, const signed long* __restrict__ j, double center );
TwoSidedEstimate PCL_FUNC TwoSidedMAD( const unsigned long long* __restrict__ i, const unsigned long long* __restrict__ j, double center );
TwoSidedEstimate PCL_FUNC TwoSidedMAD( const signed long long* __restrict__ i, const signed long long* __restrict__ j, double center );
TwoSidedEstimate PCL_FUNC TwoSidedMAD( const float* __restrict__ i, const float* __restrict__ j, double center );
TwoSidedEstimate PCL_FUNC TwoSidedMAD( const double* __restrict__ i, const double* __restrict__ j, double center );
TwoSidedEstimate PCL_FUNC TwoSidedMAD( const long double* __restrict__ i, const long double* __restrict__ j, double center );

/*!
 * Returns the two-sided median absolute deviation from the median (MAD) for
 * the values in a sequence [i,j).
 *
 * The MAD is a well-known robust estimator of scale.
 *
 * For sequences of less than two elements, this function returns zero.
 *
 * \note To make the MAD estimator consistent with the standard deviation of
 * a normal distribution, its result must be multiplied by the constant 1.4826.
 *
 * \ingroup statistical_functions
 */
template <typename T> inline TwoSidedEstimate TwoSidedMAD( const T* __restrict__ i, const T* __restrict__ j )
{
   return pcl::TwoSidedMAD( i, j, pcl::Median( i, j ) );
}

/*!
 * Returns the Sn scale estimator of Rousseeuw and Croux for a sequence [x,xn):
 *
 * <pre>
 * Sn = c * low_median( high_median( |x_i - x_j| ) )
 * </pre>
 *
 * where low_median() is the order statistic of rank (n + 1)/2, and
 * high_median() is the order statistic of rank n/2 + 1. n >= 2 is the number
 * of elements in the sequence: n = xn - x.
 *
 * This implementation is a direct C++ translation of the original FORTRAN
 * code by the authors (see References). The algorithm has O(n*log_2(n)) time
 * complexity and uses O(n) additional storage.
 *
 * The constant c = 1.1926 must be used to make the Sn estimator converge to
 * the standard deviation of a pure normal distribution. However, this
 * implementation does not apply it (it uses c=1 implicitly), for
 * consistency with other implementations of scale estimators.
 *
 * \b References
 *
 * P.J. Rousseeuw and C. Croux (1993), <em>Alternatives to the Median Absolute
 * Deviation,</em> Journal of the American Statistical Association, Vol. 88,
 * pp. 1273-1283.
 *
 * \note This is a \e destructive algorithm: it may alter the initial order of
 * items in the specified [x,xn) sequence.
 *
 * \ingroup statistical_functions
 */
template <typename T> double Sn( T* __restrict__ x, T* __restrict__ xn )
{
   /*
    * N.B.: In the code below, lines commented with an asterisk (*) have been
    * modified with respect to the FORTRAN original to account for zero-based
    * array indices.
    */

   distance_type n = xn - x;
   if ( n < 2 )
      return 0;

   pcl::Sort( x, xn );

   double* a2 = new double[ n ];
   a2[0] = double( x[n >> 1] ) - double( x[0] );                                 // *

   distance_type nh = (n + 1) >> 1;

   for ( distance_type i = 2; i <= nh; ++i )
   {
      distance_type nA = i-1;
      distance_type nB = n - i;
      distance_type diff = nB - nA;
      distance_type leftA = 1;
      distance_type leftB = 1;
      distance_type rightA = nB;
      distance_type Amin = (diff >> 1) + 1;
      distance_type Amax = (diff >> 1) + nA;

      while ( leftA < rightA )
      {
         distance_type length = rightA - leftA + 1;
         distance_type even = (length & 1) == 0;
         distance_type half = (length - 1) >> 1;
         distance_type tryA = leftA + half;
         distance_type tryB = leftB + half;

         if ( tryA < Amin )
            leftA = tryA + even;
         else
         {
            if ( tryA > Amax )
            {
               rightA = tryA;
               leftB = tryB + even;
            }
            else
            {
               double medA = double( x[i-1] ) - double( x[i-2 - tryA + Amin] );  // *
               double medB = double( x[tryB + i-1] ) - double( x[i-1] );         // *
               if ( medA >= medB )
               {
                  rightA = tryA;
                  leftB = tryB + even;
               }
               else
                  leftA = tryA + even;
            }
         }
      }

      if ( leftA > Amax )
         a2[i-1] = double( x[leftB + i-1] ) - double( x[i-1] );                  // *
      else
      {
         double medA = double( x[i-1] ) - double( x[i-2 - leftA + Amin] );       // *
         double medB = double( x[leftB + i-1] ) - double( x[i-1] );
         a2[i-1] = pcl::Min( medA, medB );                                       // *
      }
   }

   for ( distance_type i = nh + 1; i < n; ++i )
   {
      distance_type nA = n - i;
      distance_type nB = i - 1;
      distance_type diff = nB - nA;
      distance_type leftA = 1;
      distance_type leftB = 1;
      distance_type rightA = nB;
      distance_type Amin = (diff >> 1) + 1;
      distance_type Amax = (diff >> 1) + nA;

      while ( leftA < rightA )
      {
         distance_type length = rightA - leftA + 1;
         distance_type even = (length & 1) == 0;
         distance_type half = (length - 1) >> 1;
         distance_type tryA = leftA + half;
         distance_type tryB = leftB + half;

         if ( tryA < Amin)
            leftA = tryA + even;
         else
         {
            if ( tryA > Amax )
            {
               rightA = tryA;
               leftB = tryB + even;
            }
            else
            {
               double medA = double( x[i + tryA - Amin] ) - double( x[i-1] );    // *
               double medB = double( x[i-1] ) - double( x[i-1 - tryB] );         // *
               if ( medA >= medB )
               {
                  rightA = tryA;
                  leftB = tryB + even;
               }
               else
                  leftA = tryA + even;
            }
         }
      }

      if ( leftA > Amax )
         a2[i-1] = double( x[i-1] ) - double( x[i-1 - leftB] );                  // *
      else
      {
         double medA = double( x[i + leftA - Amin] ) - double( x[i-1] );         // *
         double medB = double( x[i-1] ) - double( x[i-1 - leftB] );              // *
         a2[i-1] = pcl::Min( medA, medB );            // *
      }
   }

   a2[n-1] = double( x[n-1] ) - double( x[nh-1] );                               // *

   /*
    * Correction for a finite sample
    */
   double cn;
   switch ( n )
   {
   case  2: cn = 0.743; break;
   case  3: cn = 1.851; break;
   case  4: cn = 0.954; break;
   case  5: cn = 1.351; break;
   case  6: cn = 0.993; break;
   case  7: cn = 1.198; break;
   case  8: cn = 1.005; break;
   case  9: cn = 1.131; break;
   default: cn = (n & 1) ? n/(n - 0.9) : 1.0; break;
   }

   double sn = cn * *pcl::Select( a2, a2+n, nh-1 );

   delete [] a2;
   return sn;
}

/*!
 * \internal
 * Auxiliary routine for Qn().
 *
 * Algorithm to compute the weighted high median in O(n) time.
 *
 * The weighted high median is defined as the smallest a[j] such that the sum
 * of the weights of all a[i] <= a[j] is strictly greater than half of the
 * total weight.
 */
inline double __pcl_whimed__( double* a, distance_type* iw, distance_type n,
                              double* acand, distance_type* iwcand )
{
   distance_type wtotal = 0;
   for ( distance_type i = 0; i < n; ++i )
      wtotal += iw[i];

   for ( distance_type nn = n, wrest = 0; ; )
   {
      double trial = *pcl::Select( a, a+nn, nn >> 1 ); // *

      distance_type wleft = 0;
      distance_type wmid = 0;
      distance_type wright = 0;
      for ( distance_type i = 0; i < nn; ++i )
         if ( a[i] < trial )
            wleft += iw[i];
         else if ( a[i] > trial )
            wright += iw[i];
         else
            wmid += iw[i];

      if ( 2*(wrest + wleft) > wtotal )
      {
         distance_type kcand = 0;
         for ( distance_type i = 0; i < nn; ++i )
            if ( a[i] < trial )
            {
               acand[kcand] = a[i];
               iwcand[kcand] = iw[i];
               ++kcand;
            }
          nn = kcand;
      }
      else
      {
         if ( 2*(wrest + wleft + wmid) > wtotal )
            return trial;

         distance_type kcand = 0;
         for ( distance_type i = 0; i < nn; ++i )
            if ( a[i] > trial )
            {
               acand[kcand] = a[i];
               iwcand[kcand] = iw[i];
               ++kcand;
            }
         nn = kcand;
         wrest += wleft + wmid;
      }

      for ( distance_type i = 0; i < nn; ++i )
      {
         a[i] = acand[i];
         iw[i] = iwcand[i];
      }
   }
}

/*!
 * Returns the Qn scale estimator of Rousseeuw and Croux for a sequence [x,xn):
 *
 * <pre>
 * Qn = c * first_quartile( |x_i - x_j| : i < j )
 * </pre>
 *
 * where first_quartile() is the order statistic of rank (n + 1)/4. n >= 2 is
 * the number of elements in the sequence: n = xn - x.
 *
 * This implementation is a C++ translation of the original FORTRAN code by the
 * authors (see References). The algorithm has O(n*log_2(n)) time complexity
 * and the implementation requires about O(9*n) additional storage.
 *
 * The constant c = 2.2219 must be used to make the Qn estimator converge to
 * the standard deviation of a pure normal distribution. However, this
 * implementation does not apply it (it uses c=1 implicitly), for consistency
 * with other implementations of scale estimators.
 *
 * \b References
 *
 * P.J. Rousseeuw and C. Croux (1993), <em>Alternatives to the Median Absolute
 * Deviation,</em> Journal of the American Statistical Association, Vol. 88,
 * pp. 1273-1283.
 *
 * \note This is a \e destructive algorithm: it may alter the initial order of
 * items in the specified [x,xn) sequence.
 *
 * \ingroup statistical_functions
 */
template <typename T> double Qn( T* __restrict__ x, T* __restrict__ xn )
{
   distance_type n = xn - x;
   if ( n < 2 )
      return 0;

   double*        y      = new double[ n ];
   double*        work   = new double[ n ];
   double*        acand  = new double[ n ];
   distance_type* iwcand = new distance_type[ n ];
   distance_type* left   = new distance_type[ n ];
   distance_type* right  = new distance_type[ n ];
   distance_type* P      = new distance_type[ n ];
   distance_type* Q      = new distance_type[ n ];
   distance_type* weight = new distance_type[ n ];

   distance_type h = (n >> 1) + 1;
   distance_type k = (h*(h - 1)) >> 1;
   for ( distance_type i = 0; i < n; ++i )
   {
      y[i] = double( x[i] );
      left[i] = n - i + 1;                                                       // *
      right[i] = (i <= h) ? n : n - i + h; // N.B. The original code is "right[i] = n"
   }

   pcl::Sort( y, y+n );

   distance_type nL = (n*(n + 1)) >> 1;
   distance_type nR = n*n;
   distance_type knew = k + nL;

   bool found = false;
   double qn;

   while ( nR-nL > n )
   {
      distance_type j = 0;                                                       // *
      for ( distance_type i = 1; i < n; ++i )                                    // *
         if ( left[i] <= right[i] )
         {
            weight[j] = right[i] - left[i] + 1;
            work[j] = double( y[i] ) - y[n - left[i] - (weight[j] >> 1)];
            ++j;
         }
      qn = __pcl_whimed__( work, weight, j, acand, iwcand );

      for ( distance_type i = n-1, j = 0; i >= 0; --i )                          // *
      {
         while ( j < n && double( y[i] ) - y[n-j-1] < qn )
            ++j;
         P[i] = j;
      }

      for ( distance_type i = 0, j = n+1; i < n; ++i )                           // *
      {
         while ( double( y[i] ) - y[n-j+1] > qn )
            --j;
         Q[i] = j;
      }

      double sumP = 0;
      double sumQ = 0;
      for ( distance_type i = 0; i < n; ++i )
      {
         sumP += P[i];
         sumQ += Q[i] - 1;
      }

      if ( knew <= sumP )
      {
         for ( distance_type i = 0; i < n; ++i )
            right[i] = P[i];
         nR = sumP;
      }
      else if ( knew > sumQ )
      {
         for ( distance_type i = 0; i < n; ++i )
            left[i] = Q[i];
         nL = sumQ;
      }
      else
      {
         found = true;
         break;
      }
   }

   if ( !found )
   {
      distance_type j = 0;
      for ( distance_type i = 1; i < n; ++i )
         for ( distance_type jj = left[i]; jj <= right[i]; ++jj, ++j )
            work[j] = double( y[i] ) - y[n-jj];                                  // *
      qn = *pcl::Select( work, work+j, knew-nL-1 );                              // *
   }

   /*
    * Correction for a finite sample
    */
   double dn;
   switch ( n )
   {
   case  2: dn = 0.399; break;
   case  3: dn = 0.994; break;
   case  4: dn = 0.512; break;
   case  5: dn = 0.844; break;
   case  6: dn = 0.611; break;
   case  7: dn = 0.857; break;
   case  8: dn = 0.669; break;
   case  9: dn = 0.872; break;
   default: dn = (n & 1) ? n/(n + 1.4) : n/(n + 3.8); break;
   }
   qn *= dn;

   delete [] y;
   delete [] work;
   delete [] acand;
   delete [] iwcand;
   delete [] left;
   delete [] right;
   delete [] P;
   delete [] Q;
   delete [] weight;

   return qn;
}

/*!
 * Returns a biweight midvariance (BWMV) for the elements in a sequence [x,xn).
 *
 * \param x, xn   Define a sequence of sample data points for which the BWMV
 *                estimator will be calculated.
 *
 * \param center  Reference center value. Normally, the median of the sample
 *                should be used.
 *
 * \param sigma   A reference estimate of dispersion. Normally, the median
 *                absolute deviation from the median (MAD) of the sample should
 *                be used.
 *
 * \param k       Rejection limit in sigma units. The default value is k=9.
 *
 * \param reducedLength    If true, reduce the sample size to exclude rejected
 *                elements. This tends to approximate the true dispersion of
 *                the data more accurately for relatively small samples, or
 *                samples with large amounts of outliers. Note that this
 *                departs from the standard definition of biweight midvariance,
 *                where the total number of data items is used to scale the
 *                variance estimate. If false, use the full sample size,
 *                including rejected elements, which gives a standard biweight
 *                midvariance estimate.
 *
 * The square root of the biweight midvariance is a robust estimator of scale.
 * It is an efficient estimator with respect to many statistical distributions
 * (about 87% Gaussian efficiency), and appears to have a breakdown point close
 * to 0.5 (the same as MAD).
 *
 * For sequences of less than two elements, this function returns zero.
 *
 * \note To make the BWMV estimator consistent with the standard deviation of
 * a normal distribution, its square root must be multiplied by the constant
 * 0.991.
 *
 * \b References
 *
 * Rand R. Wilcox (2017), <em>Introduction to Robust Estimation and Hypothesis
 * Testing, 4th Edition</em>, Elsevier Inc., Section 3.12.1.
 *
 * \ingroup statistical_functions
 */
template <typename T>
double BiweightMidvariance( const T* __restrict__ x, const T* __restrict__ xn, double center,
                            double sigma, int k = 9, bool reducedLength = false ) noexcept
{
   distance_type n = xn - x;
   if ( n < 2 )
      return 0;

   double kd = k * sigma;
   if ( kd < 0 || 1 + kd == 1 )
      return 0;

   double num = 0, den = 0;
   distance_type nr = 0;
   for ( ; x < xn; ++x )
   {
      double xc = double( *x ) - center;
      double y = xc/kd;
      if ( Abs( y ) < 1 )
      {
         double y2 = y*y;
         double y21 = 1 - y2;
         num += xc*xc * y21*y21*y21*y21;
         den += y21 * (1 - 5*y2);
         ++nr;
      }
   }

   den *= den;
   return (1 + den != 1) ? (reducedLength ? nr : n)*num/den : 0.0;
}

/*!
 * Returns a two-sided biweight midvariance (BWMV) for the elements in a
 * sequence [x,xn).
 *
 * \param x, xn   Define a sequence of sample data points for which the
 *                two-sided BWMV estimator will be calculated.
 *
 * \param center  Reference center value. Normally, the median of the sample
 *                should be used.
 *
 * \param sigma   A reference two-sided estimate of dispersion. Normally, the
 *                two-sided median absolute deviation from the median (MAD) of
 *                the sample should be used. See the TwoSidedMAD() function.
 *
 * \param k       Rejection limit in sigma units. The default value is k=9.
 *
 * \param reducedLength    If true, reduce the sample size to exclude rejected
 *                elements. Size reduction is performed separately for the low
 *                and high halves of the data. This tends to approximate the
 *                true dispersion of the data more accurately for relatively
 *                small samples, or samples with large amounts of outliers.
 *                Note that this departs from the standard definition of
 *                biweight midvariance, where the total number of data items is
 *                used to scale the variance estimate. If false, use the full
 *                sample size, including rejected elements, which gives a
 *                standard biweight midvariance estimate.
 *
 * See BiweightMidvariance() for more information and references.
 *
 * \ingroup statistical_functions
 */
template <typename T>
TwoSidedEstimate TwoSidedBiweightMidvariance( const T* __restrict__ x, const T* __restrict__ xn, double center,
                                              const TwoSidedEstimate& sigma, int k = 9, bool reducedLength = false ) noexcept
{
   double kd0 = k * sigma.low;
   double kd1 = k * sigma.high;
   if ( kd0 < 0 || 1 + kd0 == 1 || kd1 < 0 || 1 + kd1 == 1 )
      return 0;

   double num0 = 0, den0 = 0, num1 = 0, den1 = 0;
   size_type n0 = 0, n1 = 0, nr0 = 0, nr1 = 0;
   for ( ; x < xn; ++x )
   {
      double xc = double( *x ) - center;
      bool low = xc <= 0;
      if ( low )
         ++n0;
      else
         ++n1;

      double y = xc/(low ? kd0 : kd1);
      if ( pcl::Abs( y ) < 1 )
      {
         double y2 = y*y;
         double y21 = 1 - y2;
         double num = xc*xc * y21*y21*y21*y21;
         double den = y21 * (1 - 5*y2);
         if ( low )
         {
            num0 += num;
            den0 += den;
            ++nr0;
         }
         else
         {
            num1 += num;
            den1 += den;
            ++nr1;
         }
      }
   }

   den0 *= den0;
   den1 *= den1;
   return { (n0 >= 2 && 1 + den0 != 1) ? (reducedLength ? nr0 : n0)*num0/den0 : 0.0,
            (n1 >= 2 && 1 + den1 != 1) ? (reducedLength ? nr1 : n1)*num1/den1 : 0.0 };
}

/*!
 * Returns a percentage bend midvariance (PBMV) for the elements in a sequence
 * [x,xn).
 *
 * \param x, xn   Define a sequence of sample data points for which the PBWV
 *                estimator will be calculated.
 *
 * \param center  Reference center value. Normally, the median of the sample
 *                should be used.
 *
 * \param beta    Rejection parameter in the [0,0.5] range. Higher values
 *                improve robustness to outliers (i.e., increase the breakdown
 *                point of the estimator) at the expense of a lower efficiency.
 *                The default value is beta=0.2.
 *
 * The square root of the percentage bend midvariance is a robust estimator of
 * scale. With the default beta=0.2, its Gaussian efficiency is 67%. With
 * beta=0.1, its efficiency is 85% but its breakdown is only 0.1.
 *
 * For sequences of less than two elements, this function returns zero.
 *
 * \b References
 *
 * Rand R. Wilcox (2012), <em>Introduction to Robust Estimation and Hypothesis
 * Testing, 3rd Edition</em>, Elsevier Inc., Section 3.12.3.
 *
 * \ingroup statistical_functions
 */
template <typename T>
double BendMidvariance( const T* __restrict__ x, const T* __restrict__ xn, double center, double beta = 0.2 )
{
   distance_type n = xn - x;
   if ( n < 2 )
      return 0;

   beta = Range( beta, 0.0, 0.5 );
   distance_type m = Floor( (1 - beta)*n + 0.5 );

   double* w = new double[ n ];
   for ( distance_type i = 0; i < n; ++i )
      w[i] = Abs( double( x[i] ) - center );
   double wb = *pcl::Select( w, w+n, m );
   delete [] w;
   if ( 1 + wb == 1 )
      return 0;

   double num = 0;
   distance_type den = 0;
   for ( ; x < xn; ++x )
   {
      double y = (double( *x ) - center)/wb;
      double f = Max( -1.0, Min( 1.0, y ) );
      num += f*f;
      if ( Abs( y ) < 1 )
         ++den;
   }

   den *= den;
   return (1 + den != 1) ? n*wb*wb*num/den : 0.0;
}

// ----------------------------------------------------------------------------

/*!
 * \defgroup special_functions Special Functions
 */

/*!
 * Evaluation of the regularized incomplete beta function I_x( a, b ).
 *
 * \param a,b     The a and b parameters of the beta function being evaluated.
 *
 * \param x       Function evaluation point. Must be in the range [0,1].
 *
 * \param eps     Relative accuracy of the returned function evaluation. The
 *                default value is 1.0e-8.
 *
 * This implementation is adapted from original code by Lewis Van Winkle,
 * released under zlib license:
 *
 * <pre>
 * https://codeplea.com/incomplete-beta-function-c
 * https://github.com/codeplea/incbeta
 * </pre>
 *
 * Copyright (c) 2016, 2017 Lewis Van Winkle
 *
 * \ingroup special_functions
 */
inline double IncompleteBeta( double a, double b, double x, double eps = 1.0e-8 ) noexcept
{
   if ( x < 0 || x > 1 )
      return std::numeric_limits<double>::infinity();

   /*
    * The continued fraction converges nicely for x < (a+1)/(a+b+2)
    */
   if ( x > (a + 1)/(a + b + 2) )
      return 1 - IncompleteBeta( b, a, 1 - x ); // Use the fact that beta is symmetric

    /*
     * Find the first part before the continued fraction.
     */
    double lbeta_ab = lgamma( a ) + lgamma( b ) - lgamma( a + b );
    double front = Exp( Ln( x )*a + Ln( 1 - x )*b - lbeta_ab )/a;

    /*
     * Use Lentz's algorithm to evaluate the continued fraction.
     */
   const double tiny = 1.0e-30;
   double f = 1, c = 1, d = 0;
   for ( int i = 0; i <= 200; ++i )
   {
      int m = i >> 1;
      double numerator;
      if ( i & 1 )
         numerator = -((a + m)*(a + b + m)*x)/((a + 2*m)*(a + 2*m + 1)); // Odd term
      else if ( i > 0 )
         numerator = (m*(b - m)*x)/((a + 2*m - 1)*(a + 2*m)); // Even term
      else
         numerator = 1; // First numerator is 1.0

      /*
       * Do an iteration of Lentz's algorithm.
       */
      d = 1 + numerator*d;
      if ( Abs( d ) < tiny )
         d = tiny;
      d = 1/d;
      c = 1 + numerator/c;
      if ( Abs( c ) < tiny )
         c = tiny;
      double cd = c*d;
      f *= cd;
      if ( Abs( 1 - cd ) < eps )
         return front*(f - 1);
   }

   // Needed more loops, did not converge.
   return std::numeric_limits<double>::infinity();
}

// ----------------------------------------------------------------------------

/*!
 * The error function: erf( \a x ).
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T Erf( T x ) noexcept
{
   return std::erf( x );
}

/*!
 * The inverse error function: erf^-1( \a x ).
 *
 * Code adapted from erfinv implementation by Lakshay Garg:
 *
 * https://github.com/lakshayg/erfinv
 *
 * Copyright (c) 2017-2019 Lakshay Garg
 * Licensed under MIT license.
 *
 * \ingroup mathematical_functions
 */
template <typename T> inline constexpr T ErfInv( T x ) noexcept
{
  if ( x < -1 || x > 1 )
    return std::numeric_limits<T>::quiet_NaN();
  if ( x == 1 )
    return std::numeric_limits<T>::infinity();
  if ( x == -1 )
    return -std::numeric_limits<T>::infinity();

  const long double A0 = 1.1975323115670912564578e0L;
  const long double A1 = 4.7072688112383978012285e1L;
  const long double A2 = 6.9706266534389598238465e2L;
  const long double A3 = 4.8548868893843886794648e3L;
  const long double A4 = 1.6235862515167575384252e4L;
  const long double A5 = 2.3782041382114385731252e4L;
  const long double A6 = 1.1819493347062294404278e4L;
  const long double A7 = 8.8709406962545514830200e2L;

  const long double B0 = 1.0000000000000000000e0L;
  const long double B1 = 4.2313330701600911252e1L;
  const long double B2 = 6.8718700749205790830e2L;
  const long double B3 = 5.3941960214247511077e3L;
  const long double B4 = 2.1213794301586595867e4L;
  const long double B5 = 3.9307895800092710610e4L;
  const long double B6 = 2.8729085735721942674e4L;
  const long double B7 = 5.2264952788528545610e3L;

  const long double C0 = 1.42343711074968357734e0L;
  const long double C1 = 4.63033784615654529590e0L;
  const long double C2 = 5.76949722146069140550e0L;
  const long double C3 = 3.64784832476320460504e0L;
  const long double C4 = 1.27045825245236838258e0L;
  const long double C5 = 2.41780725177450611770e-1L;
  const long double C6 = 2.27238449892691845833e-2L;
  const long double C7 = 7.74545014278341407640e-4L;

  const long double D0 = 1.4142135623730950488016887e0L;
  const long double D1 = 2.9036514445419946173133295e0L;
  const long double D2 = 2.3707661626024532365971225e0L;
  const long double D3 = 9.7547832001787427186894837e-1L;
  const long double D4 = 2.0945065210512749128288442e-1L;
  const long double D5 = 2.1494160384252876777097297e-2L;
  const long double D6 = 7.7441459065157709165577218e-4L;
  const long double D7 = 1.4859850019840355905497876e-9L;

  const long double E0 = 6.65790464350110377720e0L;
  const long double E1 = 5.46378491116411436990e0L;
  const long double E2 = 1.78482653991729133580e0L;
  const long double E3 = 2.96560571828504891230e-1L;
  const long double E4 = 2.65321895265761230930e-2L;
  const long double E5 = 1.24266094738807843860e-3L;
  const long double E6 = 2.71155556874348757815e-5L;
  const long double E7 = 2.01033439929228813265e-7L;

  const long double F0 = 1.414213562373095048801689e0L;
  const long double F1 = 8.482908416595164588112026e-1L;
  const long double F2 = 1.936480946950659106176712e-1L;
  const long double F3 = 2.103693768272068968719679e-2L;
  const long double F4 = 1.112800997078859844711555e-3L;
  const long double F5 = 2.611088405080593625138020e-5L;
  const long double F6 = 2.010321207683943062279931e-7L;
  const long double F7 = 2.891024605872965461538222e-15L;

  long double abs_x = Abs( x );

  if ( abs_x <= 0.85L )
  {
    long double r = 0.180625L - 0.25L * x*x;
    long double num = (((((((A7*r + A6)*r + A5)*r + A4)*r + A3)*r + A2)*r + A1)*r + A0);
    long double den = (((((((B7*r + B6)*r + B5)*r + B4)*r + B3)*r + B2)*r + B1)*r + B0);
    return T( x * num/den );
  }

  long double r = Sqrt( Ln2() - Ln( 1 - abs_x ) );
  long double num = 0, den = 0;
  if ( r <= 5 )
  {
    r = r - 1.6L;
    num = (((((((C7*r + C6)*r + C5)*r + C4)*r + C3)*r + C2)*r + C1)*r + C0);
    den = (((((((D7*r + D6)*r + D5)*r + D4)*r + D3)*r + D2)*r + D1)*r + D0);
  }
  else
  {
    r = r - 5.0L;
    num = (((((((E7*r + E6)*r + E5)*r + E4)*r + E3)*r + E2)*r + E1)*r + E0);
    den = (((((((F7*r + F6)*r + F5)*r + F4)*r + F3)*r + F2)*r + F1)*r + F0);
  }

  return T( std::copysign( num/den, x ) );
}

// ----------------------------------------------------------------------------

/*!
 * \defgroup hash_functions Non-Cryptographic Hash Functions
 */

/*!
 * Computes a 64-bit non-cryptographic hash function.
 *
 * \param data    Address of the first byte of the input data block.
 *
 * \param size    Length in bytes of the input data block.
 *
 * \param seed    Optional seed value for initialization of the hash function.
 *                If \a seed is zero or is not specified, the seed will be set
 *                equal to the length of the data block.
 *
 * Returns a 64-bit hash value computed from the input data block.
 *
 * <pre>
 * Test vector: "The quick brown fox jumps over the lazy dog"
 * Hash64 checksum = 9a11f5e9468d7425
 *
 * Test vector: "" (empty string)\n
 * Hash64 checksum = ef46db3751d8e999
 * </pre>
 *
 * This function implements the xxHash algorithm by Yann Collet. Our code is an
 * adaptation of the original code by the author:
 *
 * <pre>
 * https://github.com/Cyan4973/xxHash
 * </pre>
 *
 * Copyright (C) 2012-2014, Yann Collet. \n
 * The original code has been released under the BSD 2-Clause License:
 *
 * <pre>
 * http://www.opensource.org/licenses/bsd-license.php
 * </pre>
 *
 * \ingroup hash_functions
 */
inline uint64 Hash64( const void* data, size_type size, uint64 seed = 0 ) noexcept
{
#define PRIME64_1 11400714785074694791ULL
#define PRIME64_2 14029467366897019727ULL
#define PRIME64_3  1609587929392839161ULL
#define PRIME64_4  9650029242287828579ULL
#define PRIME64_5  2870177450012600261ULL

   const uint8* p = (const uint8*)data;
   const uint8* end = p + size;
   uint64 h64;

   if ( seed == 0 )
      seed = size;

   if ( size >= 32 )
   {
      const uint8* limit = end - 32;
      uint64 v1 = seed + PRIME64_1 + PRIME64_2;
      uint64 v2 = seed + PRIME64_2;
      uint64 v3 = seed + 0;
      uint64 v4 = seed - PRIME64_1;

      do
      {
         v1 += *(uint64*)p * PRIME64_2;
         p += 8;
         v1 = RotL( v1, 31 );
         v1 *= PRIME64_1;
         v2 += *(uint64*)p * PRIME64_2;
         p += 8;
         v2 = RotL( v2, 31 );
         v2 *= PRIME64_1;
         v3 += *(uint64*)p * PRIME64_2;
         p += 8;
         v3 = RotL( v3, 31 );
         v3 *= PRIME64_1;
         v4 += *(uint64*)p * PRIME64_2;
         p += 8;
         v4 = RotL( v4, 31 );
         v4 *= PRIME64_1;
      }
      while ( p <= limit );

      h64 = RotL( v1, 1 ) + RotL( v2, 7 ) + RotL( v3, 12 ) + RotL( v4, 18 );

      v1 *= PRIME64_2;
      v1 = RotL( v1, 31 );
      v1 *= PRIME64_1;
      h64 ^= v1;
      h64 = h64 * PRIME64_1 + PRIME64_4;

      v2 *= PRIME64_2;
      v2 = RotL( v2, 31 );
      v2 *= PRIME64_1;
      h64 ^= v2;
      h64 = h64 * PRIME64_1 + PRIME64_4;

      v3 *= PRIME64_2;
      v3 = RotL( v3, 31 );
      v3 *= PRIME64_1;
      h64 ^= v3;
      h64 = h64 * PRIME64_1 + PRIME64_4;

      v4 *= PRIME64_2;
      v4 = RotL( v4, 31 );
      v4 *= PRIME64_1;
      h64 ^= v4;
      h64 = h64 * PRIME64_1 + PRIME64_4;
   }
   else
   {
      h64 = seed + PRIME64_5;
   }

   h64 += size;

   while ( p+8 <= end )
   {
      uint64 k1 = *(uint64*)p;
      k1 *= PRIME64_2;
      k1 = RotL( k1, 31 );
      k1 *= PRIME64_1;
      h64 ^= k1;
      h64 = RotL( h64, 27 ) * PRIME64_1 + PRIME64_4;
      p += 8;
   }

   if ( p+4 <= end )
   {
      h64 ^= (uint64)(*(uint32*)p) * PRIME64_1;
      h64 = RotL( h64, 23 ) * PRIME64_2 + PRIME64_3;
      p += 4;
   }

   while ( p < end )
   {
      h64 ^= *p * PRIME64_5;
      h64 = RotL( h64, 11 ) * PRIME64_1;
      ++p;
   }

   h64 ^= h64 >> 33;
   h64 *= PRIME64_2;
   h64 ^= h64 >> 29;
   h64 *= PRIME64_3;
   h64 ^= h64 >> 32;

   return h64;

#undef PRIME64_1
#undef PRIME64_2
#undef PRIME64_3
#undef PRIME64_4
#undef PRIME64_5
}

/*!
 * Computes a 32-bit non-cryptographic hash function.
 *
 * \param data    Address of the first byte of the input data block.
 *
 * \param size    Length in bytes of the input data block.
 *
 * \param seed    Optional seed value for initialization of the hash function.
 *                If \a seed is zero or is not specified, the seed will be set
 *                equal to the length of the data block.
 *
 * Returns a 32-bit hash value computed from the input data block.
 *
 * <pre>
 * Test vector: "The quick brown fox jumps over the lazy dog"\n
 * Hash32 checksum = 752cd1b8
 *
 * Test vector: "" (empty string)\n
 * Hash32 checksum = 2cc5d05
 * </pre>
 *
 * This function implements the xxHash algorithm by Yann Collet. Our code is an
 * adaptation of the original code by the author:
 *
 * <pre>
 * https://github.com/Cyan4973/xxHash
 * </pre>
 *
 * Copyright (C) 2012-2014, Yann Collet. \n
 * The original code has been released under the BSD 2-Clause License:
 *
 * <pre>
 * http://www.opensource.org/licenses/bsd-license.php
 * </pre>
 *
 * \ingroup hash_functions
 */
inline uint32 Hash32( const void* data, size_type size, uint32 seed = 0 ) noexcept
{
#define PRIME32_1 2654435761U
#define PRIME32_2 2246822519U
#define PRIME32_3 3266489917U
#define PRIME32_4  668265263U
#define PRIME32_5  374761393U

   const uint8* p = (const uint8*)data;
   const uint8* end = p + size;
   uint32 h32;

   if ( seed == 0 )
      seed = uint32( size );

   if ( size >= 16 )
   {
      const uint8* limit = end - 16;
      uint32 v1 = seed + PRIME32_1 + PRIME32_2;
      uint32 v2 = seed + PRIME32_2;
      uint32 v3 = seed + 0;
      uint32 v4 = seed - PRIME32_1;

      do
      {
         v1 += *(uint32*)p * PRIME32_2;
         v1 = RotL( v1, 13 );
         v1 *= PRIME32_1;
         p += 4;
         v2 += *(uint32*)p * PRIME32_2;
         v2 = RotL( v2, 13 );
         v2 *= PRIME32_1;
         p += 4;
         v3 += *(uint32*)p * PRIME32_2;
         v3 = RotL( v3, 13 );
         v3 *= PRIME32_1;
         p += 4;
         v4 += *(uint32*)p * PRIME32_2;
         v4 = RotL( v4, 13 );
         v4 *= PRIME32_1;
         p += 4;
      }
      while ( p <= limit );

      h32 = RotL( v1, 1 ) + RotL( v2, 7 ) + RotL( v3, 12 ) + RotL( v4, 18 );
   }
   else
   {
      h32  = seed + PRIME32_5;
   }

   h32 += (uint32)size;

   while ( p+4 <= end )
   {
      h32 += *(uint32*)p * PRIME32_3;
      h32  = RotL( h32, 17 ) * PRIME32_4 ;
      p+=4;
   }

   while ( p < end )
   {
      h32 += *p * PRIME32_5;
      h32 = RotL( h32, 11 ) * PRIME32_1 ;
      ++p;
   }

   h32 ^= h32 >> 15;
   h32 *= PRIME32_2;
   h32 ^= h32 >> 13;
   h32 *= PRIME32_3;
   h32 ^= h32 >> 16;

   return h32;

#undef PRIME32_1
#undef PRIME32_2
#undef PRIME32_3
#undef PRIME32_4
#undef PRIME32_5
}

// ----------------------------------------------------------------------------

} // pcl

#endif   // __PCL_Math_h

// ----------------------------------------------------------------------------
// EOF pcl/Math.h - Released 2022-03-12T18:59:29Z