diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 08b8084a0..0c04eeec3 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -65,7 +65,7 @@ function build() # Set to 2 cores so that Actions does not error out during resource provisioning. cmake --build build -j2 - $PYTHON -m pip install --user build/python + cmake --build build --target python-install } function test() @@ -73,6 +73,13 @@ function test() cd $GITHUB_WORKSPACE/python/gtsam/tests $PYTHON -m unittest discover -v cd $GITHUB_WORKSPACE + + cd $GITHUB_WORKSPACE/python/gtsam_unstable/tests + $PYTHON -m unittest discover -v + cd $GITHUB_WORKSPACE + + # cmake --build build --target python-test + # cmake --build build --target python-test-unstable } # select between build or test diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 91bc4e80a..520e94c09 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -18,6 +18,8 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} PYTHON_VERSION: ${{ matrix.python_version }} + BOOST_VERSION: 1.72.0 + BOOST_EXE: boost_1_72_0-msvc-14.2 strategy: fail-fast: true @@ -30,6 +32,7 @@ jobs: ubuntu-20.04-gcc-9-tbb, ubuntu-20.04-clang-9, macOS-11-xcode-13.4.1, + windows-2019-msbuild, ] build_type: [Release] @@ -56,6 +59,10 @@ jobs: compiler: xcode version: "13.4.1" + - name: windows-2019-msbuild + os: windows-2019 + platform: 64 + steps: - name: Checkout uses: actions/checkout@v3 @@ -97,29 +104,68 @@ jobs: echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV + - name: Setup msbuild (Windows) + if: runner.os == 'Windows' + uses: ilammy/msvc-dev-cmd@v1 + with: + arch: x${{matrix.platform}} + + - name: Setup python (Windows) + uses: actions/setup-python@v4 + if: runner.os == 'Windows' + with: + python-version: ${{ matrix.python_version }} + + - name: Install ninja (Windows) + if: runner.os == 'Windows' + shell: bash + run: | + choco install ninja + ninja --version + where ninja + + - name: Install Boost (Windows) + if: runner.os == 'Windows' + shell: powershell + run: | + # Snippet from: https://github.com/actions/virtual-environments/issues/2667 + $BOOST_PATH = "C:\hostedtoolcache\windows\Boost\$env:BOOST_VERSION\x86_64" + + # Use the prebuilt binary for Windows + $Url = "https://sourceforge.net/projects/boost/files/boost-binaries/$env:BOOST_VERSION/$env:BOOST_EXE-${{matrix.platform}}.exe" + (New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe") + Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=$BOOST_PATH" + + # Set the BOOST_ROOT variable + echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV + - name: Set GTSAM_WITH_TBB Flag if: matrix.flag == 'tbb' run: | echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" - - name: Set Swap Space + - name: Set Swap Space (Linux) if: runner.os == 'Linux' uses: pierotofy/set-swap-space@master with: swap-size-gb: 6 - - name: Install System Dependencies + - name: Install System Dependencies (Linux, macOS) + if: runner.os != 'Windows' run: | bash .github/scripts/python.sh -d - name: Install Python Dependencies + shell: bash run: python$PYTHON_VERSION -m pip install -r python/dev_requirements.txt - name: Build + shell: bash run: | bash .github/scripts/python.sh -b - name: Test + shell: bash run: | bash .github/scripts/python.sh -t diff --git a/CMakeLists.txt b/CMakeLists.txt index 16848a721..66b1804af 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,6 +72,7 @@ include(cmake/HandleCCache.cmake) # ccache include(cmake/HandleCPack.cmake) # CPack include(cmake/HandleEigen.cmake) # Eigen3 include(cmake/HandleMetis.cmake) # metis +include(cmake/HandleCephes.cmake) # cephes include(cmake/HandleMKL.cmake) # MKL include(cmake/HandleOpenMP.cmake) # OpenMP include(cmake/HandlePerfTools.cmake) # Google perftools diff --git a/cmake/HandleCephes.cmake b/cmake/HandleCephes.cmake new file mode 100644 index 000000000..9addddd60 --- /dev/null +++ b/cmake/HandleCephes.cmake @@ -0,0 +1,19 @@ +# ############################################################################## +# Cephes library + +# For both system or bundle version, a cmake target "cephes-gtsam-if" is defined +# (interface library) + + +add_subdirectory(${GTSAM_SOURCE_DIR}/gtsam/3rdparty/cephes) + +list(APPEND GTSAM_EXPORTED_TARGETS cephes-gtsam) + +add_library(cephes-gtsam-if INTERFACE) +target_link_libraries(cephes-gtsam-if INTERFACE cephes-gtsam) + +list(APPEND GTSAM_EXPORTED_TARGETS cephes-gtsam-if) +install( + TARGETS cephes-gtsam-if + EXPORT GTSAM-exports + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt new file mode 100644 index 000000000..5940d39d2 --- /dev/null +++ b/gtsam/3rdparty/cephes/CMakeLists.txt @@ -0,0 +1,122 @@ +cmake_minimum_required(VERSION 3.10) +enable_testing() +project( + cephes + DESCRIPTION "Cephes Mathematical Function Library" + VERSION 1.0.0 + LANGUAGES C) + +set(CEPHES_HEADER_FILES + cephes.h + cephes/dd_idefs.h + cephes/dd_real.h + cephes/dd_real_idefs.h + cephes/expn.h + cephes/igam.h + cephes/lanczos.h + cephes/mconf.h + cephes/polevl.h + cephes/sf_error.h) + +# Add header files +install(FILES ${CEPHES_HEADER_FILES} DESTINATION include/gtsam/3rdparty/cephes) + +set(CEPHES_SOURCES + cephes/airy.c + cephes/bdtr.c + cephes/besselpoly.c + cephes/beta.c + cephes/btdtr.c + cephes/cbrt.c + cephes/chbevl.c + cephes/chdtr.c + cephes/const.c + cephes/dawsn.c + cephes/dd_real.c + cephes/ellie.c + cephes/ellik.c + cephes/ellpe.c + cephes/ellpj.c + cephes/ellpk.c + cephes/erfinv.c + cephes/exp10.c + cephes/exp2.c + cephes/expn.c + cephes/fdtr.c + cephes/fresnl.c + cephes/gamma.c + cephes/gammasgn.c + cephes/gdtr.c + cephes/hyp2f1.c + cephes/hyperg.c + cephes/i0.c + cephes/i1.c + cephes/igam.c + cephes/igami.c + cephes/incbet.c + cephes/incbi.c + cephes/j0.c + cephes/j1.c + cephes/jv.c + cephes/k0.c + cephes/k1.c + cephes/kn.c + cephes/kolmogorov.c + cephes/lanczos.c + cephes/nbdtr.c + cephes/ndtr.c + cephes/ndtri.c + cephes/owens_t.c + cephes/pdtr.c + cephes/poch.c + cephes/psi.c + cephes/rgamma.c + cephes/round.c + cephes/sf_error.c + cephes/shichi.c + cephes/sici.c + cephes/sindg.c + cephes/sinpi.c + cephes/spence.c + cephes/stdtr.c + cephes/tandg.c + cephes/tukey.c + cephes/unity.c + cephes/yn.c + cephes/yv.c + cephes/zeta.c + cephes/zetac.c) + +# Add library source files +add_library(cephes-gtsam SHARED ${CEPHES_SOURCES}) + +# Add include directory (aka headers) +target_include_directories( + cephes-gtsam BEFORE PUBLIC $ + $) + +set_target_properties( + cephes-gtsam + PROPERTIES VERSION ${PROJECT_VERSION} + SOVERSION ${PROJECT_VERSION_MAJOR} + C_STANDARD 99) + +if(WIN32) + set_target_properties( + cephes-gtsam + PROPERTIES PREFIX "" + COMPILE_FLAGS /w + RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin") +endif() + +if(APPLE) + set_target_properties(cephes-gtsam PROPERTIES INSTALL_NAME_DIR + "${CMAKE_INSTALL_PREFIX}/lib") +endif() + +install( + TARGETS cephes-gtsam + EXPORT GTSAM-exports + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) diff --git a/gtsam/3rdparty/cephes/LICENSE.txt b/gtsam/3rdparty/cephes/LICENSE.txt new file mode 100644 index 000000000..6c2842a96 --- /dev/null +++ b/gtsam/3rdparty/cephes/LICENSE.txt @@ -0,0 +1,7 @@ +MIT License + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. \ No newline at end of file diff --git a/gtsam/3rdparty/cephes/README.md b/gtsam/3rdparty/cephes/README.md new file mode 100644 index 000000000..63e3a6c8c --- /dev/null +++ b/gtsam/3rdparty/cephes/README.md @@ -0,0 +1,22 @@ +# README + +This is a vendored version of the Cephes Mathematical Library. The source code can be found on [netlib.org](https://www.netlib.org/cephes/). + +The software is provided with an [MIT License](https://smath.com/en-US/view/CephesMathLibrary/license). + +## Original Readme + +Some software in this archive may be from the book _Methods and +Programs for Mathematical Functions_ (Prentice-Hall or Simon & Schuster +International, 1989) or from the Cephes Mathematical Library, a +commercial product. In either event, it is copyrighted by the author. +What you see here may be used freely but it comes with no support or +guarantee. + +The two known misprints in the book are repaired here in the +source listings for the gamma function and the incomplete beta +integral. + + +Stephen L. Moshier +moshier@na-net.ornl.gov diff --git a/gtsam/3rdparty/cephes/cephes.h b/gtsam/3rdparty/cephes/cephes.h new file mode 100644 index 000000000..ed53e521b --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes.h @@ -0,0 +1,169 @@ +#ifndef CEPHES_H +#define CEPHES_H + +#include "dllexport.h" + +#ifdef __cplusplus +extern "C" { +#endif + +CEPHES_EXTERN_EXPORT int airy(double x, double *ai, double *aip, double *bi, + double *bip); + +CEPHES_EXTERN_EXPORT double bdtrc(double k, int n, double p); +CEPHES_EXTERN_EXPORT double bdtr(double k, int n, double p); +CEPHES_EXTERN_EXPORT double bdtri(double k, int n, double y); + +CEPHES_EXTERN_EXPORT double besselpoly(double a, double lambda, double nu); + +CEPHES_EXTERN_EXPORT double beta(double a, double b); +CEPHES_EXTERN_EXPORT double lbeta(double a, double b); + +CEPHES_EXTERN_EXPORT double btdtr(double a, double b, double x); + +CEPHES_EXTERN_EXPORT double cbrt(double x); +CEPHES_EXTERN_EXPORT double chbevl(double x, double array[], int n); +CEPHES_EXTERN_EXPORT double chdtrc(double df, double x); +CEPHES_EXTERN_EXPORT double chdtr(double df, double x); +CEPHES_EXTERN_EXPORT double chdtri(double df, double y); +CEPHES_EXTERN_EXPORT double dawsn(double xx); + +CEPHES_EXTERN_EXPORT double ellie(double phi, double m); +CEPHES_EXTERN_EXPORT double ellik(double phi, double m); +CEPHES_EXTERN_EXPORT double ellpe(double x); + +CEPHES_EXTERN_EXPORT int ellpj(double u, double m, double *sn, double *cn, + double *dn, double *ph); +CEPHES_EXTERN_EXPORT double ellpk(double x); +CEPHES_EXTERN_EXPORT double exp10(double x); +CEPHES_EXTERN_EXPORT double exp2(double x); + +CEPHES_EXTERN_EXPORT double expn(int n, double x); + +CEPHES_EXTERN_EXPORT double fdtrc(double a, double b, double x); +CEPHES_EXTERN_EXPORT double fdtr(double a, double b, double x); +CEPHES_EXTERN_EXPORT double fdtri(double a, double b, double y); + +CEPHES_EXTERN_EXPORT int fresnl(double xxa, double *ssa, double *cca); +CEPHES_EXTERN_EXPORT double Gamma(double x); +CEPHES_EXTERN_EXPORT double lgam(double x); +CEPHES_EXTERN_EXPORT double lgam_sgn(double x, int *sign); +CEPHES_EXTERN_EXPORT double gammasgn(double x); + +CEPHES_EXTERN_EXPORT double gdtr(double a, double b, double x); +CEPHES_EXTERN_EXPORT double gdtrc(double a, double b, double x); +CEPHES_EXTERN_EXPORT double gdtri(double a, double b, double y); + +CEPHES_EXTERN_EXPORT double hyp2f1(double a, double b, double c, double x); +CEPHES_EXTERN_EXPORT double hyperg(double a, double b, double x); +CEPHES_EXTERN_EXPORT double threef0(double a, double b, double c, double x, + double *err); + +CEPHES_EXTERN_EXPORT double i0(double x); +CEPHES_EXTERN_EXPORT double i0e(double x); +CEPHES_EXTERN_EXPORT double i1(double x); +CEPHES_EXTERN_EXPORT double i1e(double x); +CEPHES_EXTERN_EXPORT double igamc(double a, double x); +CEPHES_EXTERN_EXPORT double igam(double a, double x); +CEPHES_EXTERN_EXPORT double igam_fac(double a, double x); +CEPHES_EXTERN_EXPORT double igamci(double a, double q); +CEPHES_EXTERN_EXPORT double igami(double a, double p); + +CEPHES_EXTERN_EXPORT double incbet(double aa, double bb, double xx); +CEPHES_EXTERN_EXPORT double incbi(double aa, double bb, double yy0); + +CEPHES_EXTERN_EXPORT double iv(double v, double x); +CEPHES_EXTERN_EXPORT double j0(double x); +CEPHES_EXTERN_EXPORT double y0(double x); +CEPHES_EXTERN_EXPORT double j1(double x); +CEPHES_EXTERN_EXPORT double y1(double x); + +CEPHES_EXTERN_EXPORT double jn(int n, double x); +CEPHES_EXTERN_EXPORT double jv(double n, double x); +CEPHES_EXTERN_EXPORT double k0(double x); +CEPHES_EXTERN_EXPORT double k0e(double x); +CEPHES_EXTERN_EXPORT double k1(double x); +CEPHES_EXTERN_EXPORT double k1e(double x); +CEPHES_EXTERN_EXPORT double kn(int nn, double x); + +CEPHES_EXTERN_EXPORT double nbdtrc(int k, int n, double p); +CEPHES_EXTERN_EXPORT double nbdtr(int k, int n, double p); +CEPHES_EXTERN_EXPORT double nbdtri(int k, int n, double p); + +CEPHES_EXTERN_EXPORT double ndtr(double a); +CEPHES_EXTERN_EXPORT double log_ndtr(double a); +CEPHES_EXTERN_EXPORT double erfc(double a); +CEPHES_EXTERN_EXPORT double erf(double x); +CEPHES_EXTERN_EXPORT double erfinv(double y); +CEPHES_EXTERN_EXPORT double erfcinv(double y); +CEPHES_EXTERN_EXPORT double ndtri(double y0); + +CEPHES_EXTERN_EXPORT double pdtrc(double k, double m); +CEPHES_EXTERN_EXPORT double pdtr(double k, double m); +CEPHES_EXTERN_EXPORT double pdtri(int k, double y); + +CEPHES_EXTERN_EXPORT double poch(double x, double m); + +CEPHES_EXTERN_EXPORT double psi(double x); + +CEPHES_EXTERN_EXPORT double rgamma(double x); +CEPHES_EXTERN_EXPORT double round(double x); + +CEPHES_EXTERN_EXPORT int shichi(double x, double *si, double *ci); +CEPHES_EXTERN_EXPORT int sici(double x, double *si, double *ci); + +CEPHES_EXTERN_EXPORT double radian(double d, double m, double s); +CEPHES_EXTERN_EXPORT double sindg(double x); +CEPHES_EXTERN_EXPORT double sinpi(double x); +CEPHES_EXTERN_EXPORT double cosdg(double x); +CEPHES_EXTERN_EXPORT double cospi(double x); + +CEPHES_EXTERN_EXPORT double spence(double x); + +CEPHES_EXTERN_EXPORT double stdtr(int k, double t); +CEPHES_EXTERN_EXPORT double stdtri(int k, double p); + +CEPHES_EXTERN_EXPORT double struve_h(double v, double x); +CEPHES_EXTERN_EXPORT double struve_l(double v, double x); +CEPHES_EXTERN_EXPORT double struve_power_series(double v, double x, int is_h, + double *err); +CEPHES_EXTERN_EXPORT double struve_asymp_large_z(double v, double z, int is_h, + double *err); +CEPHES_EXTERN_EXPORT double struve_bessel_series(double v, double z, int is_h, + double *err); + +CEPHES_EXTERN_EXPORT double yv(double v, double x); + +CEPHES_EXTERN_EXPORT double tandg(double x); +CEPHES_EXTERN_EXPORT double cotdg(double x); + +CEPHES_EXTERN_EXPORT double log1p(double x); +CEPHES_EXTERN_EXPORT double log1pmx(double x); +CEPHES_EXTERN_EXPORT double expm1(double x); +CEPHES_EXTERN_EXPORT double cosm1(double x); +CEPHES_EXTERN_EXPORT double lgam1p(double x); + +CEPHES_EXTERN_EXPORT double yn(int n, double x); +CEPHES_EXTERN_EXPORT double zeta(double x, double q); +CEPHES_EXTERN_EXPORT double zetac(double x); + +CEPHES_EXTERN_EXPORT double smirnov(int n, double d); +CEPHES_EXTERN_EXPORT double smirnovi(int n, double p); +CEPHES_EXTERN_EXPORT double smirnovp(int n, double d); +CEPHES_EXTERN_EXPORT double smirnovc(int n, double d); +CEPHES_EXTERN_EXPORT double smirnovci(int n, double p); +CEPHES_EXTERN_EXPORT double kolmogorov(double x); +CEPHES_EXTERN_EXPORT double kolmogi(double p); +CEPHES_EXTERN_EXPORT double kolmogp(double x); +CEPHES_EXTERN_EXPORT double kolmogc(double x); +CEPHES_EXTERN_EXPORT double kolmogci(double p); + +CEPHES_EXTERN_EXPORT double lanczos_sum_expg_scaled(double x); + +CEPHES_EXTERN_EXPORT double owens_t(double h, double a); + +#ifdef __cplusplus +} +#endif + +#endif /* CEPHES_H */ diff --git a/gtsam/3rdparty/cephes/cephes/airy.c b/gtsam/3rdparty/cephes/cephes/airy.c new file mode 100644 index 000000000..95e16a55f --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/airy.c @@ -0,0 +1,376 @@ +/* airy.c + * + * Airy function + * + * + * + * SYNOPSIS: + * + * double x, ai, aip, bi, bip; + * int airy(); + * + * airy( x, _&ai, _&aip, _&bi, _&bip ); + * + * + * + * DESCRIPTION: + * + * Solution of the differential equation + * + * y"(x) = xy. + * + * The function returns the two independent solutions Ai, Bi + * and their first derivatives Ai'(x), Bi'(x). + * + * Evaluation is by power series summation for small x, + * by rational minimax approximations for large x. + * + * + * + * ACCURACY: + * Error criterion is absolute when function <= 1, relative + * when function > 1, except * denotes relative error criterion. + * For large negative x, the absolute error increases as x^1.5. + * For large positive x, the relative error increases as x^1.5. + * + * Arithmetic domain function # trials peak rms + * IEEE -10, 0 Ai 10000 1.6e-15 2.7e-16 + * IEEE 0, 10 Ai 10000 2.3e-14* 1.8e-15* + * IEEE -10, 0 Ai' 10000 4.6e-15 7.6e-16 + * IEEE 0, 10 Ai' 10000 1.8e-14* 1.5e-15* + * IEEE -10, 10 Bi 30000 4.2e-15 5.3e-16 + * IEEE -10, 10 Bi' 30000 4.9e-15 7.3e-16 + * + */ + /* airy.c */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1989, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +static double c1 = 0.35502805388781723926; +static double c2 = 0.258819403792806798405; +static double sqrt3 = 1.732050807568877293527; +static double sqpii = 5.64189583547756286948E-1; + +extern double MACHEP; + +#ifdef UNK +#define MAXAIRY 25.77 +#endif +#ifdef IBMPC +#define MAXAIRY 103.892 +#endif +#ifdef MIEEE +#define MAXAIRY 103.892 +#endif + + +static double AN[8] = { + 3.46538101525629032477E-1, + 1.20075952739645805542E1, + 7.62796053615234516538E1, + 1.68089224934630576269E2, + 1.59756391350164413639E2, + 7.05360906840444183113E1, + 1.40264691163389668864E1, + 9.99999999999999995305E-1, +}; + +static double AD[8] = { + 5.67594532638770212846E-1, + 1.47562562584847203173E1, + 8.45138970141474626562E1, + 1.77318088145400459522E2, + 1.64234692871529701831E2, + 7.14778400825575695274E1, + 1.40959135607834029598E1, + 1.00000000000000000470E0, +}; + +static double APN[8] = { + 6.13759184814035759225E-1, + 1.47454670787755323881E1, + 8.20584123476060982430E1, + 1.71184781360976385540E2, + 1.59317847137141783523E2, + 6.99778599330103016170E1, + 1.39470856980481566958E1, + 1.00000000000000000550E0, +}; + +static double APD[8] = { + 3.34203677749736953049E-1, + 1.11810297306158156705E1, + 7.11727352147859965283E1, + 1.58778084372838313640E2, + 1.53206427475809220834E2, + 6.86752304592780337944E1, + 1.38498634758259442477E1, + 9.99999999999999994502E-1, +}; + +static double BN16[5] = { + -2.53240795869364152689E-1, + 5.75285167332467384228E-1, + -3.29907036873225371650E-1, + 6.44404068948199951727E-2, + -3.82519546641336734394E-3, +}; + +static double BD16[5] = { + /* 1.00000000000000000000E0, */ + -7.15685095054035237902E0, + 1.06039580715664694291E1, + -5.23246636471251500874E0, + 9.57395864378383833152E-1, + -5.50828147163549611107E-2, +}; + +static double BPPN[5] = { + 4.65461162774651610328E-1, + -1.08992173800493920734E0, + 6.38800117371827987759E-1, + -1.26844349553102907034E-1, + 7.62487844342109852105E-3, +}; + +static double BPPD[5] = { + /* 1.00000000000000000000E0, */ + -8.70622787633159124240E0, + 1.38993162704553213172E1, + -7.14116144616431159572E0, + 1.34008595960680518666E0, + -7.84273211323341930448E-2, +}; + +static double AFN[9] = { + -1.31696323418331795333E-1, + -6.26456544431912369773E-1, + -6.93158036036933542233E-1, + -2.79779981545119124951E-1, + -4.91900132609500318020E-2, + -4.06265923594885404393E-3, + -1.59276496239262096340E-4, + -2.77649108155232920844E-6, + -1.67787698489114633780E-8, +}; + +static double AFD[9] = { + /* 1.00000000000000000000E0, */ + 1.33560420706553243746E1, + 3.26825032795224613948E1, + 2.67367040941499554804E1, + 9.18707402907259625840E0, + 1.47529146771666414581E0, + 1.15687173795188044134E-1, + 4.40291641615211203805E-3, + 7.54720348287414296618E-5, + 4.51850092970580378464E-7, +}; + +static double AGN[11] = { + 1.97339932091685679179E-2, + 3.91103029615688277255E-1, + 1.06579897599595591108E0, + 9.39169229816650230044E-1, + 3.51465656105547619242E-1, + 6.33888919628925490927E-2, + 5.85804113048388458567E-3, + 2.82851600836737019778E-4, + 6.98793669997260967291E-6, + 8.11789239554389293311E-8, + 3.41551784765923618484E-10, +}; + +static double AGD[10] = { + /* 1.00000000000000000000E0, */ + 9.30892908077441974853E0, + 1.98352928718312140417E1, + 1.55646628932864612953E1, + 5.47686069422975497931E0, + 9.54293611618961883998E-1, + 8.64580826352392193095E-2, + 4.12656523824222607191E-3, + 1.01259085116509135510E-4, + 1.17166733214413521882E-6, + 4.91834570062930015649E-9, +}; + +static double APFN[9] = { + 1.85365624022535566142E-1, + 8.86712188052584095637E-1, + 9.87391981747398547272E-1, + 4.01241082318003734092E-1, + 7.10304926289631174579E-2, + 5.90618657995661810071E-3, + 2.33051409401776799569E-4, + 4.08718778289035454598E-6, + 2.48379932900442457853E-8, +}; + +static double APFD[9] = { + /* 1.00000000000000000000E0, */ + 1.47345854687502542552E1, + 3.75423933435489594466E1, + 3.14657751203046424330E1, + 1.09969125207298778536E1, + 1.78885054766999417817E0, + 1.41733275753662636873E-1, + 5.44066067017226003627E-3, + 9.39421290654511171663E-5, + 5.65978713036027009243E-7, +}; + +static double APGN[11] = { + -3.55615429033082288335E-2, + -6.37311518129435504426E-1, + -1.70856738884312371053E0, + -1.50221872117316635393E0, + -5.63606665822102676611E-1, + -1.02101031120216891789E-1, + -9.48396695961445269093E-3, + -4.60325307486780994357E-4, + -1.14300836484517375919E-5, + -1.33415518685547420648E-7, + -5.63803833958893494476E-10, +}; + +static double APGD[11] = { + /* 1.00000000000000000000E0, */ + 9.85865801696130355144E0, + 2.16401867356585941885E1, + 1.73130776389749389525E1, + 6.17872175280828766327E0, + 1.08848694396321495475E0, + 9.95005543440888479402E-2, + 4.78468199683886610842E-3, + 1.18159633322838625562E-4, + 1.37480673554219441465E-6, + 5.79912514929147598821E-9, +}; + +int airy(double x, double *ai, double *aip, double *bi, double *bip) +{ + double z, zz, t, f, g, uf, ug, k, zeta, theta; + int domflg; + + domflg = 0; + if (x > MAXAIRY) { + *ai = 0; + *aip = 0; + *bi = INFINITY; + *bip = INFINITY; + return (-1); + } + + if (x < -2.09) { + domflg = 15; + t = sqrt(-x); + zeta = -2.0 * x * t / 3.0; + t = sqrt(t); + k = sqpii / t; + z = 1.0 / zeta; + zz = z * z; + uf = 1.0 + zz * polevl(zz, AFN, 8) / p1evl(zz, AFD, 9); + ug = z * polevl(zz, AGN, 10) / p1evl(zz, AGD, 10); + theta = zeta + 0.25 * M_PI; + f = sin(theta); + g = cos(theta); + *ai = k * (f * uf - g * ug); + *bi = k * (g * uf + f * ug); + uf = 1.0 + zz * polevl(zz, APFN, 8) / p1evl(zz, APFD, 9); + ug = z * polevl(zz, APGN, 10) / p1evl(zz, APGD, 10); + k = sqpii * t; + *aip = -k * (g * uf + f * ug); + *bip = k * (f * uf - g * ug); + return (0); + } + + if (x >= 2.09) { /* cbrt(9) */ + domflg = 5; + t = sqrt(x); + zeta = 2.0 * x * t / 3.0; + g = exp(zeta); + t = sqrt(t); + k = 2.0 * t * g; + z = 1.0 / zeta; + f = polevl(z, AN, 7) / polevl(z, AD, 7); + *ai = sqpii * f / k; + k = -0.5 * sqpii * t / g; + f = polevl(z, APN, 7) / polevl(z, APD, 7); + *aip = f * k; + + if (x > 8.3203353) { /* zeta > 16 */ + f = z * polevl(z, BN16, 4) / p1evl(z, BD16, 5); + k = sqpii * g; + *bi = k * (1.0 + f) / t; + f = z * polevl(z, BPPN, 4) / p1evl(z, BPPD, 5); + *bip = k * t * (1.0 + f); + return (0); + } + } + + f = 1.0; + g = x; + t = 1.0; + uf = 1.0; + ug = x; + k = 1.0; + z = x * x * x; + while (t > MACHEP) { + uf *= z; + k += 1.0; + uf /= k; + ug *= z; + k += 1.0; + ug /= k; + uf /= k; + f += uf; + k += 1.0; + ug /= k; + g += ug; + t = fabs(uf / f); + } + uf = c1 * f; + ug = c2 * g; + if ((domflg & 1) == 0) + *ai = uf - ug; + if ((domflg & 2) == 0) + *bi = sqrt3 * (uf + ug); + + /* the deriviative of ai */ + k = 4.0; + uf = x * x / 2.0; + ug = z / 3.0; + f = uf; + g = 1.0 + ug; + uf /= 3.0; + t = 1.0; + + while (t > MACHEP) { + uf *= z; + ug /= k; + k += 1.0; + ug *= z; + uf /= k; + f += uf; + k += 1.0; + ug /= k; + uf /= k; + g += ug; + k += 1.0; + t = fabs(ug / g); + } + + uf = c1 * f; + ug = c2 * g; + if ((domflg & 4) == 0) + *aip = uf - ug; + if ((domflg & 8) == 0) + *bip = sqrt3 * (uf + ug); + return (0); +} diff --git a/gtsam/3rdparty/cephes/cephes/bdtr.c b/gtsam/3rdparty/cephes/cephes/bdtr.c new file mode 100644 index 000000000..29fcdf1af --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/bdtr.c @@ -0,0 +1,241 @@ +/* bdtr.c + * + * Binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, bdtr(); + * + * y = bdtr( k, n, p ); + * + * DESCRIPTION: + * + * Returns the sum of the terms 0 through k of the Binomial + * probability density: + * + * k + * -- ( n ) j n-j + * > ( ) p (1-p) + * -- ( j ) + * j=0 + * + * The terms are not summed directly; instead the incomplete + * beta integral is employed, according to the formula + * + * y = bdtr( k, n, p ) = incbet( n-k, k+1, 1-p ). + * + * The arguments must be positive, with p ranging from 0 to 1. + * + * ACCURACY: + * + * Tested at random points (a,b,p), with p between 0 and 1. + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * For p between 0.001 and 1: + * IEEE 0,100 100000 4.3e-15 2.6e-16 + * See also incbet.c. + * + * ERROR MESSAGES: + * + * message condition value returned + * bdtr domain k < 0 0.0 + * n < k + * x < 0, x > 1 + */ +/* bdtrc() + * + * Complemented binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, bdtrc(); + * + * y = bdtrc( k, n, p ); + * + * DESCRIPTION: + * + * Returns the sum of the terms k+1 through n of the Binomial + * probability density: + * + * n + * -- ( n ) j n-j + * > ( ) p (1-p) + * -- ( j ) + * j=k+1 + * + * The terms are not summed directly; instead the incomplete + * beta integral is employed, according to the formula + * + * y = bdtrc( k, n, p ) = incbet( k+1, n-k, p ). + * + * The arguments must be positive, with p ranging from 0 to 1. + * + * ACCURACY: + * + * Tested at random points (a,b,p). + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * For p between 0.001 and 1: + * IEEE 0,100 100000 6.7e-15 8.2e-16 + * For p between 0 and .001: + * IEEE 0,100 100000 1.5e-13 2.7e-15 + * + * ERROR MESSAGES: + * + * message condition value returned + * bdtrc domain x<0, x>1, n 1 + */ + +/* bdtr() */ + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +double bdtrc(double k, int n, double p) { + double dk, dn; + double fk = floor(k); + + if (isnan(p) || isnan(k)) { + return NAN; + } + + if (p < 0.0 || p > 1.0 || n < fk) { + sf_error("bdtrc", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (fk < 0) { + return 1.0; + } + + if (fk == n) { + return 0.0; + } + + dn = n - fk; + if (k == 0) { + if (p < .01) + dk = -expm1(dn * log1p(-p)); + else + dk = 1.0 - pow(1.0 - p, dn); + } else { + dk = fk + 1; + dk = incbet(dk, dn, p); + } + return dk; +} + +double bdtr(double k, int n, double p) { + double dk, dn; + double fk = floor(k); + + if (isnan(p) || isnan(k)) { + return NAN; + } + + if (p < 0.0 || p > 1.0 || fk < 0 || n < fk) { + sf_error("bdtr", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (fk == n) return 1.0; + + dn = n - fk; + if (fk == 0) { + dk = pow(1.0 - p, dn); + } else { + dk = fk + 1.; + dk = incbet(dn, dk, 1.0 - p); + } + return dk; +} + +double bdtri(double k, int n, double y) { + double p, dn, dk; + double fk = floor(k); + + if (isnan(k)) { + return NAN; + } + + if (y < 0.0 || y > 1.0 || fk < 0.0 || n <= fk) { + sf_error("bdtri", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + dn = n - fk; + + if (fk == n) return 1.0; + + if (fk == 0) { + if (y > 0.8) { + p = -expm1(log1p(y - 1.0) / dn); + } else { + p = 1.0 - pow(y, 1.0 / dn); + } + } else { + dk = fk + 1; + p = incbet(dn, dk, 0.5); + if (p > 0.5) + p = incbi(dk, dn, 1.0 - y); + else + p = 1.0 - incbi(dn, dk, y); + } + return p; +} diff --git a/gtsam/3rdparty/cephes/cephes/besselpoly.c b/gtsam/3rdparty/cephes/cephes/besselpoly.c new file mode 100644 index 000000000..a58fe2037 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/besselpoly.c @@ -0,0 +1,34 @@ +#include "mconf.h" + +#define EPS 1.0e-17 + +double besselpoly(double a, double lambda, double nu) { + + int m, factor=0; + double Sm, relerr, Sol; + double sum=0.0; + + /* Special handling for a = 0.0 */ + if (a == 0.0) { + if (nu == 0.0) return 1.0/(lambda + 1); + else return 0.0; + } + /* Special handling for negative and integer nu */ + if ((nu < 0) && (floor(nu)==nu)) { + nu = -nu; + factor = ((int) nu) % 2; + } + Sm = exp(nu*log(a))/(Gamma(nu+1)*(lambda+nu+1)); + m = 0; + do { + sum += Sm; + Sol = Sm; + Sm *= -a*a*(lambda+nu+1+2*m)/((nu+m+1)*(m+1)*(lambda+nu+1+2*m+2)); + m++; + relerr = fabs((Sm-Sol)/Sm); + } while (relerr > EPS && m < 1000); + if (!factor) + return sum; + else + return -sum; +} diff --git a/gtsam/3rdparty/cephes/cephes/beta.c b/gtsam/3rdparty/cephes/cephes/beta.c new file mode 100644 index 000000000..c0389deea --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/beta.c @@ -0,0 +1,258 @@ +/* beta.c + * + * Beta function + * + * + * + * SYNOPSIS: + * + * double a, b, y, beta(); + * + * y = beta( a, b ); + * + * + * + * DESCRIPTION: + * + * - - + * | (a) | (b) + * beta( a, b ) = -----------. + * - + * | (a+b) + * + * For large arguments the logarithm of the function is + * evaluated using lgam(), then exponentiated. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 30000 8.1e-14 1.1e-14 + * + * ERROR MESSAGES: + * + * message condition value returned + * beta overflow log(beta) > MAXLOG 0.0 + * a or b <0 integer 0.0 + * + */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +#define MAXGAM 171.624376956302725 + +extern double MAXLOG; + +#define ASYMP_FACTOR 1e6 + +static double lbeta_asymp(double a, double b, int *sgn); +static double lbeta_negint(int a, double b); +static double beta_negint(int a, double b); + +double beta(double a, double b) +{ + double y; + int sign = 1; + + if (a <= 0.0) { + if (a == floor(a)) { + if (a == (int)a) { + return beta_negint((int)a, b); + } + else { + goto overflow; + } + } + } + + if (b <= 0.0) { + if (b == floor(b)) { + if (b == (int)b) { + return beta_negint((int)b, a); + } + else { + goto overflow; + } + } + } + + if (fabs(a) < fabs(b)) { + y = a; a = b; b = y; + } + + if (fabs(a) > ASYMP_FACTOR * fabs(b) && a > ASYMP_FACTOR) { + /* Avoid loss of precision in lgam(a + b) - lgam(a) */ + y = lbeta_asymp(a, b, &sign); + return sign * exp(y); + } + + y = a + b; + if (fabs(y) > MAXGAM || fabs(a) > MAXGAM || fabs(b) > MAXGAM) { + int sgngam; + y = lgam_sgn(y, &sgngam); + sign *= sgngam; /* keep track of the sign */ + y = lgam_sgn(b, &sgngam) - y; + sign *= sgngam; + y = lgam_sgn(a, &sgngam) + y; + sign *= sgngam; + if (y > MAXLOG) { + goto overflow; + } + return (sign * exp(y)); + } + + y = Gamma(y); + a = Gamma(a); + b = Gamma(b); + if (y == 0.0) + goto overflow; + + if (fabs(fabs(a) - fabs(y)) > fabs(fabs(b) - fabs(y))) { + y = b / y; + y *= a; + } + else { + y = a / y; + y *= b; + } + + return (y); + +overflow: + sf_error("beta", SF_ERROR_OVERFLOW, NULL); + return (sign * INFINITY); +} + + +/* Natural log of |beta|. */ + +double lbeta(double a, double b) +{ + double y; + int sign; + + sign = 1; + + if (a <= 0.0) { + if (a == floor(a)) { + if (a == (int)a) { + return lbeta_negint((int)a, b); + } + else { + goto over; + } + } + } + + if (b <= 0.0) { + if (b == floor(b)) { + if (b == (int)b) { + return lbeta_negint((int)b, a); + } + else { + goto over; + } + } + } + + if (fabs(a) < fabs(b)) { + y = a; a = b; b = y; + } + + if (fabs(a) > ASYMP_FACTOR * fabs(b) && a > ASYMP_FACTOR) { + /* Avoid loss of precision in lgam(a + b) - lgam(a) */ + y = lbeta_asymp(a, b, &sign); + return y; + } + + y = a + b; + if (fabs(y) > MAXGAM || fabs(a) > MAXGAM || fabs(b) > MAXGAM) { + int sgngam; + y = lgam_sgn(y, &sgngam); + sign *= sgngam; /* keep track of the sign */ + y = lgam_sgn(b, &sgngam) - y; + sign *= sgngam; + y = lgam_sgn(a, &sgngam) + y; + sign *= sgngam; + return (y); + } + + y = Gamma(y); + a = Gamma(a); + b = Gamma(b); + if (y == 0.0) { + over: + sf_error("lbeta", SF_ERROR_OVERFLOW, NULL); + return (sign * INFINITY); + } + + if (fabs(fabs(a) - fabs(y)) > fabs(fabs(b) - fabs(y))) { + y = b / y; + y *= a; + } + else { + y = a / y; + y *= b; + } + + if (y < 0) { + y = -y; + } + + return (log(y)); +} + +/* + * Asymptotic expansion for ln(|B(a, b)|) for a > ASYMP_FACTOR*max(|b|, 1). + */ +static double lbeta_asymp(double a, double b, int *sgn) +{ + double r = lgam_sgn(b, sgn); + r -= b * log(a); + + r += b*(1-b)/(2*a); + r += b*(1-b)*(1-2*b)/(12*a*a); + r += - b*b*(1-b)*(1-b)/(12*a*a*a); + + return r; +} + + +/* + * Special case for a negative integer argument + */ + +static double beta_negint(int a, double b) +{ + int sgn; + if (b == (int)b && 1 - a - b > 0) { + sgn = ((int)b % 2 == 0) ? 1 : -1; + return sgn * beta(1 - a - b, b); + } + else { + sf_error("lbeta", SF_ERROR_OVERFLOW, NULL); + return INFINITY; + } +} + +static double lbeta_negint(int a, double b) +{ + double r; + if (b == (int)b && 1 - a - b > 0) { + r = lbeta(1 - a - b, b); + return r; + } + else { + sf_error("lbeta", SF_ERROR_OVERFLOW, NULL); + return INFINITY; + } +} diff --git a/gtsam/3rdparty/cephes/cephes/btdtr.c b/gtsam/3rdparty/cephes/cephes/btdtr.c new file mode 100644 index 000000000..fa115c7b7 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/btdtr.c @@ -0,0 +1,59 @@ + +/* btdtr.c + * + * Beta distribution + * + * + * + * SYNOPSIS: + * + * double a, b, x, y, btdtr(); + * + * y = btdtr( a, b, x ); + * + * + * + * DESCRIPTION: + * + * Returns the area from zero to x under the beta density + * function: + * + * + * x + * - - + * | (a+b) | | a-1 b-1 + * P(x) = ---------- | t (1-t) dt + * - - | | + * | (a) | (b) - + * 0 + * + * + * This function is identical to the incomplete beta + * integral function incbet(a, b, x). + * + * The complemented function is + * + * 1 - P(1-x) = incbet( b, a, x ); + * + * + * ACCURACY: + * + * See incbet.c. + * + */ + +/* btdtr() */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +double btdtr(double a, double b, double x) +{ + + return (incbet(a, b, x)); +} diff --git a/gtsam/3rdparty/cephes/cephes/cbrt.c b/gtsam/3rdparty/cephes/cephes/cbrt.c new file mode 100644 index 000000000..a83c07834 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/cbrt.c @@ -0,0 +1,117 @@ +/* cbrt.c + * + * Cube root + * + * + * + * SYNOPSIS: + * + * double x, y, cbrt(); + * + * y = cbrt( x ); + * + * + * + * DESCRIPTION: + * + * Returns the cube root of the argument, which may be negative. + * + * Range reduction involves determining the power of 2 of + * the argument. A polynomial of degree 2 applied to the + * mantissa, and multiplication by the cube root of 1, 2, or 4 + * approximates the root to within about 0.1%. Then Newton's + * iteration is used three times to converge to an accurate + * result. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,1e308 30000 1.5e-16 5.0e-17 + * + */ + /* cbrt.c */ + +/* + * Cephes Math Library Release 2.2: January, 1991 + * Copyright 1984, 1991 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + + +#include "mconf.h" + +static double CBRT2 = 1.2599210498948731647672; +static double CBRT4 = 1.5874010519681994747517; +static double CBRT2I = 0.79370052598409973737585; +static double CBRT4I = 0.62996052494743658238361; + +double cbrt(double x) +{ + int e, rem, sign; + double z; + + if (!cephes_isfinite(x)) + return x; + if (x == 0) + return (x); + if (x > 0) + sign = 1; + else { + sign = -1; + x = -x; + } + + z = x; + /* extract power of 2, leaving + * mantissa between 0.5 and 1 + */ + x = frexp(x, &e); + + /* Approximate cube root of number between .5 and 1, + * peak relative error = 9.2e-6 + */ + x = (((-1.3466110473359520655053e-1 * x + + 5.4664601366395524503440e-1) * x + - 9.5438224771509446525043e-1) * x + + 1.1399983354717293273738e0) * x + 4.0238979564544752126924e-1; + + /* exponent divided by 3 */ + if (e >= 0) { + rem = e; + e /= 3; + rem -= 3 * e; + if (rem == 1) + x *= CBRT2; + else if (rem == 2) + x *= CBRT4; + } + + + /* argument less than 1 */ + + else { + e = -e; + rem = e; + e /= 3; + rem -= 3 * e; + if (rem == 1) + x *= CBRT2I; + else if (rem == 2) + x *= CBRT4I; + e = -e; + } + + /* multiply by power of 2 */ + x = ldexp(x, e); + + /* Newton iteration */ + x -= (x - (z / (x * x))) * 0.33333333333333333333; + x -= (x - (z / (x * x))) * 0.33333333333333333333; + + if (sign < 0) + x = -x; + return (x); +} diff --git a/gtsam/3rdparty/cephes/cephes/chbevl.c b/gtsam/3rdparty/cephes/cephes/chbevl.c new file mode 100644 index 000000000..a0e9c5c52 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/chbevl.c @@ -0,0 +1,81 @@ +/* chbevl.c + * + * Evaluate Chebyshev series + * + * + * + * SYNOPSIS: + * + * int N; + * double x, y, coef[N], chebevl(); + * + * y = chbevl( x, coef, N ); + * + * + * + * DESCRIPTION: + * + * Evaluates the series + * + * N-1 + * - ' + * y = > coef[i] T (x/2) + * - i + * i=0 + * + * of Chebyshev polynomials Ti at argument x/2. + * + * Coefficients are stored in reverse order, i.e. the zero + * order term is last in the array. Note N is the number of + * coefficients, not the order. + * + * If coefficients are for the interval a to b, x must + * have been transformed to x -> 2(2x - b - a)/(b-a) before + * entering the routine. This maps x from (a, b) to (-1, 1), + * over which the Chebyshev polynomials are defined. + * + * If the coefficients are for the inverted interval, in + * which (a, b) is mapped to (1/b, 1/a), the transformation + * required is x -> 2(2ab/x - b - a)/(b-a). If b is infinity, + * this becomes x -> 4a/x - 1. + * + * + * + * SPEED: + * + * Taking advantage of the recurrence properties of the + * Chebyshev polynomials, the routine requires one more + * addition per loop than evaluating a nested polynomial of + * the same degree. + * + */ + /* chbevl.c */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1985, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" +#include + +double chbevl(double x, double array[], int n) +{ + double b0, b1, b2, *p; + int i; + + p = array; + b0 = *p++; + b1 = 0.0; + i = n - 1; + + do { + b2 = b1; + b1 = b0; + b0 = x * b1 - b2 + *p++; + } + while (--i); + + return (0.5 * (b0 - b2)); +} diff --git a/gtsam/3rdparty/cephes/cephes/chdtr.c b/gtsam/3rdparty/cephes/cephes/chdtr.c new file mode 100644 index 000000000..d576e7a8d --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/chdtr.c @@ -0,0 +1,186 @@ +/* chdtr.c + * + * Chi-square distribution + * + * + * + * SYNOPSIS: + * + * double df, x, y, chdtr(); + * + * y = chdtr( df, x ); + * + * + * + * DESCRIPTION: + * + * Returns the area under the left hand tail (from 0 to x) + * of the Chi square probability density function with + * v degrees of freedom. + * + * + * inf. + * - + * 1 | | v/2-1 -t/2 + * P( x | v ) = ----------- | t e dt + * v/2 - | | + * 2 | (v/2) - + * x + * + * where x is the Chi-square variable. + * + * The incomplete Gamma integral is used, according to the + * formula + * + * y = chdtr( v, x ) = igam( v/2.0, x/2.0 ). + * + * + * The arguments must both be positive. + * + * + * + * ACCURACY: + * + * See igam(). + * + * ERROR MESSAGES: + * + * message condition value returned + * chdtr domain x < 0 or v < 1 0.0 + */ + /* chdtrc() + * + * Complemented Chi-square distribution + * + * + * + * SYNOPSIS: + * + * double v, x, y, chdtrc(); + * + * y = chdtrc( v, x ); + * + * + * + * DESCRIPTION: + * + * Returns the area under the right hand tail (from x to + * infinity) of the Chi square probability density function + * with v degrees of freedom: + * + * + * inf. + * - + * 1 | | v/2-1 -t/2 + * P( x | v ) = ----------- | t e dt + * v/2 - | | + * 2 | (v/2) - + * x + * + * where x is the Chi-square variable. + * + * The incomplete Gamma integral is used, according to the + * formula + * + * y = chdtr( v, x ) = igamc( v/2.0, x/2.0 ). + * + * + * The arguments must both be positive. + * + * + * + * ACCURACY: + * + * See igamc(). + * + * ERROR MESSAGES: + * + * message condition value returned + * chdtrc domain x < 0 or v < 1 0.0 + */ + /* chdtri() + * + * Inverse of complemented Chi-square distribution + * + * + * + * SYNOPSIS: + * + * double df, x, y, chdtri(); + * + * x = chdtri( df, y ); + * + * + * + * + * DESCRIPTION: + * + * Finds the Chi-square argument x such that the integral + * from x to infinity of the Chi-square density is equal + * to the given cumulative probability y. + * + * This is accomplished using the inverse Gamma integral + * function and the relation + * + * x/2 = igamci( df/2, y ); + * + * + * + * + * ACCURACY: + * + * See igami.c. + * + * ERROR MESSAGES: + * + * message condition value returned + * chdtri domain y < 0 or y > 1 0.0 + * v < 1 + * + */ + +/* chdtr() */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +double chdtrc(double df, double x) +{ + + if (x < 0.0) + return 1.0; /* modified by T. Oliphant */ + return (igamc(df / 2.0, x / 2.0)); +} + + + +double chdtr(double df, double x) +{ + + if ((x < 0.0)) { /* || (df < 1.0) ) */ + sf_error("chdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + return (igam(df / 2.0, x / 2.0)); +} + + + +double chdtri(double df, double y) +{ + double x; + + if ((y < 0.0) || (y > 1.0)) { /* || (df < 1.0) ) */ + sf_error("chdtri", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + x = igamci(0.5 * df, y); + return (2.0 * x); +} diff --git a/gtsam/3rdparty/cephes/cephes/const.c b/gtsam/3rdparty/cephes/cephes/const.c new file mode 100644 index 000000000..8631554cc --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/const.c @@ -0,0 +1,129 @@ +/* const.c + * + * Globally declared constants + * + * + * + * SYNOPSIS: + * + * extern double nameofconstant; + * + * + * + * + * DESCRIPTION: + * + * This file contains a number of mathematical constants and + * also some needed size parameters of the computer arithmetic. + * The values are supplied as arrays of hexadecimal integers + * for IEEE arithmetic, and in a normal decimal scientific notation for + * other machines. The particular notation used is determined + * by a symbol (IBMPC, or UNK) defined in the include file + * mconf.h. + * + * The default size parameters are as follows. + * + * For UNK mode: + * MACHEP = 1.38777878078144567553E-17 2**-56 + * MAXLOG = 8.8029691931113054295988E1 log(2**127) + * MINLOG = -8.872283911167299960540E1 log(2**-128) + * + * For IEEE arithmetic (IBMPC): + * MACHEP = 1.11022302462515654042E-16 2**-53 + * MAXLOG = 7.09782712893383996843E2 log(2**1024) + * MINLOG = -7.08396418532264106224E2 log(2**-1022) + * + * The global symbols for mathematical constants are + * SQ2OPI = 7.9788456080286535587989E-1 sqrt( 2/pi ) + * LOGSQ2 = 3.46573590279972654709E-1 log(2)/2 + * THPIO4 = 2.35619449019234492885 3*pi/4 + * + * These lists are subject to change. + */ + +/* const.c */ + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +#ifdef UNK +double MACHEP = 1.11022302462515654042E-16; /* 2**-53 */ + +#ifdef DENORMAL +double MAXLOG = 7.09782712893383996732E2; /* log(DBL_MAX) */ + + /* double MINLOG = -7.44440071921381262314E2; *//* log(2**-1074) */ +double MINLOG = -7.451332191019412076235E2; /* log(2**-1075) */ +#else +double MAXLOG = 7.08396418532264106224E2; /* log 2**1022 */ +double MINLOG = -7.08396418532264106224E2; /* log 2**-1022 */ +#endif +double SQ2OPI = 7.9788456080286535587989E-1; /* sqrt( 2/pi ) */ +double LOGSQ2 = 3.46573590279972654709E-1; /* log(2)/2 */ +double THPIO4 = 2.35619449019234492885; /* 3*pi/4 */ + +#endif + +#ifdef IBMPC + /* 2**-53 = 1.11022302462515654042E-16 */ +unsigned short MACHEP[4] = { 0x0000, 0x0000, 0x0000, 0x3ca0 }; + +#ifdef DENORMAL + /* log(DBL_MAX) = 7.09782712893383996732224E2 */ +unsigned short MAXLOG[4] = { 0x39ef, 0xfefa, 0x2e42, 0x4086 }; + + /* log(2**-1074) = - -7.44440071921381262314E2 */ +/*unsigned short MINLOG[4] = {0x71c3,0x446d,0x4385,0xc087}; */ +unsigned short MINLOG[4] = { 0x3052, 0xd52d, 0x4910, 0xc087 }; +#else + /* log(2**1022) = 7.08396418532264106224E2 */ +unsigned short MAXLOG[4] = { 0xbcd2, 0xdd7a, 0x232b, 0x4086 }; + + /* log(2**-1022) = - 7.08396418532264106224E2 */ +unsigned short MINLOG[4] = { 0xbcd2, 0xdd7a, 0x232b, 0xc086 }; +#endif + /* 2**1024*(1-MACHEP) = 1.7976931348623158E308 */ +unsigned short SQ2OPI[4] = { 0x3651, 0x33d4, 0x8845, 0x3fe9 }; +unsigned short LOGSQ2[4] = { 0x39ef, 0xfefa, 0x2e42, 0x3fd6 }; +unsigned short THPIO4[4] = { 0x21d2, 0x7f33, 0xd97c, 0x4002 }; + +#endif + +#ifdef MIEEE + /* 2**-53 = 1.11022302462515654042E-16 */ +unsigned short MACHEP[4] = { 0x3ca0, 0x0000, 0x0000, 0x0000 }; + +#ifdef DENORMAL + /* log(2**1024) = 7.09782712893383996843E2 */ +unsigned short MAXLOG[4] = { 0x4086, 0x2e42, 0xfefa, 0x39ef }; + + /* log(2**-1074) = - -7.44440071921381262314E2 */ +/* unsigned short MINLOG[4] = {0xc087,0x4385,0x446d,0x71c3}; */ +unsigned short MINLOG[4] = { 0xc087, 0x4910, 0xd52d, 0x3052 }; +#else + /* log(2**1022) = 7.08396418532264106224E2 */ +unsigned short MAXLOG[4] = { 0x4086, 0x232b, 0xdd7a, 0xbcd2 }; + + /* log(2**-1022) = - 7.08396418532264106224E2 */ +unsigned short MINLOG[4] = { 0xc086, 0x232b, 0xdd7a, 0xbcd2 }; +#endif + /* 2**1024*(1-MACHEP) = 1.7976931348623158E308 */ +unsigned short SQ2OPI[4] = { 0x3fe9, 0x8845, 0x33d4, 0x3651 }; +unsigned short LOGSQ2[4] = { 0x3fd6, 0x2e42, 0xfefa, 0x39ef }; +unsigned short THPIO4[4] = { 0x4002, 0xd97c, 0x7f33, 0x21d2 }; + +#endif + +#ifndef UNK +extern unsigned short MACHEP[]; +extern unsigned short MAXLOG[]; +extern unsigned short UNDLOG[]; +extern unsigned short MINLOG[]; +extern unsigned short SQ2OPI[]; +extern unsigned short LOGSQ2[]; +extern unsigned short THPIO4[]; +#endif diff --git a/gtsam/3rdparty/cephes/cephes/dawsn.c b/gtsam/3rdparty/cephes/cephes/dawsn.c new file mode 100644 index 000000000..7049f191e --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/dawsn.c @@ -0,0 +1,160 @@ +/* dawsn.c + * + * Dawson's Integral + * + * + * + * SYNOPSIS: + * + * double x, y, dawsn(); + * + * y = dawsn( x ); + * + * + * + * DESCRIPTION: + * + * Approximates the integral + * + * x + * - + * 2 | | 2 + * dawsn(x) = exp( -x ) | exp( t ) dt + * | | + * - + * 0 + * + * Three different rational approximations are employed, for + * the intervals 0 to 3.25; 3.25 to 6.25; and 6.25 up. + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,10 10000 6.9e-16 1.0e-16 + * + * + */ + +/* dawsn.c */ + + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" +/* Dawson's integral, interval 0 to 3.25 */ +static double AN[10] = { + 1.13681498971755972054E-11, + 8.49262267667473811108E-10, + 1.94434204175553054283E-8, + 9.53151741254484363489E-7, + 3.07828309874913200438E-6, + 3.52513368520288738649E-4, + -8.50149846724410912031E-4, + 4.22618223005546594270E-2, + -9.17480371773452345351E-2, + 9.99999999999999994612E-1, +}; + +static double AD[11] = { + 2.40372073066762605484E-11, + 1.48864681368493396752E-9, + 5.21265281010541664570E-8, + 1.27258478273186970203E-6, + 2.32490249820789513991E-5, + 3.25524741826057911661E-4, + 3.48805814657162590916E-3, + 2.79448531198828973716E-2, + 1.58874241960120565368E-1, + 5.74918629489320327824E-1, + 1.00000000000000000539E0, +}; + +/* interval 3.25 to 6.25 */ +static double BN[11] = { + 5.08955156417900903354E-1, + -2.44754418142697847934E-1, + 9.41512335303534411857E-2, + -2.18711255142039025206E-2, + 3.66207612329569181322E-3, + -4.23209114460388756528E-4, + 3.59641304793896631888E-5, + -2.14640351719968974225E-6, + 9.10010780076391431042E-8, + -2.40274520828250956942E-9, + 3.59233385440928410398E-11, +}; + +static double BD[10] = { + /* 1.00000000000000000000E0, */ + -6.31839869873368190192E-1, + 2.36706788228248691528E-1, + -5.31806367003223277662E-2, + 8.48041718586295374409E-3, + -9.47996768486665330168E-4, + 7.81025592944552338085E-5, + -4.55875153252442634831E-6, + 1.89100358111421846170E-7, + -4.91324691331920606875E-9, + 7.18466403235734541950E-11, +}; + +/* 6.25 to infinity */ +static double CN[5] = { + -5.90592860534773254987E-1, + 6.29235242724368800674E-1, + -1.72858975380388136411E-1, + 1.64837047825189632310E-2, + -4.86827613020462700845E-4, +}; + +static double CD[5] = { + /* 1.00000000000000000000E0, */ + -2.69820057197544900361E0, + 1.73270799045947845857E0, + -3.93708582281939493482E-1, + 3.44278924041233391079E-2, + -9.73655226040941223894E-4, +}; + +extern double MACHEP; + +double dawsn(double xx) +{ + double x, y; + int sign; + + + sign = 1; + if (xx < 0.0) { + sign = -1; + xx = -xx; + } + + if (xx < 3.25) { + x = xx * xx; + y = xx * polevl(x, AN, 9) / polevl(x, AD, 10); + return (sign * y); + } + + + x = 1.0 / (xx * xx); + + if (xx < 6.25) { + y = 1.0 / xx + x * polevl(x, BN, 10) / (p1evl(x, BD, 10) * xx); + return (sign * 0.5 * y); + } + + + if (xx > 1.0e9) + return ((sign * 0.5) / xx); + + /* 6.25 to infinity */ + y = 1.0 / xx + x * polevl(x, CN, 4) / (p1evl(x, CD, 5) * xx); + return (sign * 0.5 * y); +} diff --git a/gtsam/3rdparty/cephes/cephes/dd_idefs.h b/gtsam/3rdparty/cephes/cephes/dd_idefs.h new file mode 100644 index 000000000..fec97c478 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/dd_idefs.h @@ -0,0 +1,198 @@ +/* + * include/dd_inline.h + * + * This work was supported by the Director, Office of Science, Division + * of Mathematical, Information, and Computational Sciences of the + * U.S. Department of Energy under contract numbers DE-AC03-76SF00098 and + * DE-AC02-05CH11231. + * + * Copyright (c) 2003-2009, The Regents of the University of California, + * through Lawrence Berkeley National Laboratory (subject to receipt of + * any required approvals from U.S. Dept. of Energy) All rights reserved. + * + * By downloading or using this software you are agreeing to the modified + * BSD license "BSD-LBNL-License.doc" (see LICENSE.txt). + */ +/* + * Contains small functions (suitable for inlining) in the double-double + * arithmetic package. + */ + +#ifndef _DD_IDEFS_H_ +#define _DD_IDEFS_H_ 1 + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define _DD_SPLITTER 134217729.0 // = 2^27 + 1 +#define _DD_SPLIT_THRESH 6.69692879491417e+299 // = 2^996 + +/* + ************************************************************************ + The basic routines taking double arguments, returning 1 (or 2) doubles + ************************************************************************ +*/ + +/* Computes fl(a+b) and err(a+b). Assumes |a| >= |b|. */ +static inline double +quick_two_sum(double a, double b, double *err) +{ + volatile double s = a + b; + volatile double c = s - a; + *err = b - c; + return s; +} + +/* Computes fl(a-b) and err(a-b). Assumes |a| >= |b| */ +static inline double +quick_two_diff(double a, double b, double *err) +{ + volatile double s = a - b; + volatile double c = a - s; + *err = c - b; + return s; +} + +/* Computes fl(a+b) and err(a+b). */ +static inline double +two_sum(double a, double b, double *err) +{ + volatile double s = a + b; + volatile double c = s - a; + volatile double d = b - c; + volatile double e = s - c; + *err = (a - e) + d; + return s; +} + +/* Computes fl(a-b) and err(a-b). */ +static inline double +two_diff(double a, double b, double *err) +{ + volatile double s = a - b; + volatile double c = s - a; + volatile double d = b + c; + volatile double e = s - c; + *err = (a - e) - d; + return s; +} + +/* Computes high word and lo word of a */ +static inline void +two_split(double a, double *hi, double *lo) +{ + volatile double temp, tempma; + if (a > _DD_SPLIT_THRESH || a < -_DD_SPLIT_THRESH) { + a *= 3.7252902984619140625e-09; // 2^-28 + temp = _DD_SPLITTER * a; + tempma = temp - a; + *hi = temp - tempma; + *lo = a - *hi; + *hi *= 268435456.0; // 2^28 + *lo *= 268435456.0; // 2^28 + } + else { + temp = _DD_SPLITTER * a; + tempma = temp - a; + *hi = temp - tempma; + *lo = a - *hi; + } +} + +/* Computes fl(a*b) and err(a*b). */ +static inline double +two_prod(double a, double b, double *err) +{ +#ifdef DD_FMS + volatile double p = a * b; + *err = DD_FMS(a, b, p); + return p; +#else + double a_hi, a_lo, b_hi, b_lo; + double p = a * b; + volatile double c, d; + two_split(a, &a_hi, &a_lo); + two_split(b, &b_hi, &b_lo); + c = a_hi * b_hi - p; + d = c + a_hi * b_lo + a_lo * b_hi; + *err = d + a_lo * b_lo; + return p; +#endif /* DD_FMA */ +} + +/* Computes fl(a*a) and err(a*a). Faster than the above method. */ +static inline double +two_sqr(double a, double *err) +{ +#ifdef DD_FMS + volatile double p = a * a; + *err = DD_FMS(a, a, p); + return p; +#else + double hi, lo; + volatile double c; + double q = a * a; + two_split(a, &hi, &lo); + c = hi * hi - q; + *err = (c + 2.0 * hi * lo) + lo * lo; + return q; +#endif /* DD_FMS */ +} + +static inline double +two_div(double a, double b, double *err) +{ + volatile double q1, q2; + double p1, p2; + double s, e; + + q1 = a / b; + + /* Compute a - q1 * b */ + p1 = two_prod(q1, b, &p2); + s = two_diff(a, p1, &e); + e -= p2; + + /* get next approximation */ + q2 = (s + e) / b; + + return quick_two_sum(q1, q2, err); +} + +/* Computes the nearest integer to d. */ +static inline double +two_nint(double d) +{ + if (d == floor(d)) { + return d; + } + return floor(d + 0.5); +} + +/* Computes the truncated integer. */ +static inline double +two_aint(double d) +{ + return (d >= 0.0 ? floor(d) : ceil(d)); +} + + +/* Compare a and b */ +static inline int +two_comp(const double a, const double b) +{ + /* Works for non-NAN inputs */ + return (a < b ? -1 : (a > b ? 1 : 0)); +} + + +#ifdef __cplusplus +} +#endif + +#endif /* _DD_IDEFS_H_ */ diff --git a/gtsam/3rdparty/cephes/cephes/dd_real.c b/gtsam/3rdparty/cephes/cephes/dd_real.c new file mode 100644 index 000000000..c37f57a7b --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/dd_real.c @@ -0,0 +1,587 @@ +/* + * src/double2.cc + * + * This work was supported by the Director, Office of Science, Division + * of Mathematical, Information, and Computational Sciences of the + * U.S. Department of Energy under contract numbers DE-AC03-76SF00098 and + * DE-AC02-05CH11231. + * + * Copyright (c) 2003-2009, The Regents of the University of California, + * through Lawrence Berkeley National Laboratory (subject to receipt of + * any required approvals from U.S. Dept. of Energy) All rights reserved. + * + * By downloading or using this software you are agreeing to the modified + * BSD license "BSD-LBNL-License.doc" (see LICENSE.txt). + */ +/* + * Contains implementation of non-inlined functions of double-double + * package. Inlined functions are found in dd_real_inline.h. + */ + +/* + * This code taken from v2.3.18 of the qd package. +*/ + + +#include +#include +#include +#include + +#include "dd_real.h" + +#define _DD_REAL_INIT(A, B) {{A, B}} + +const double DD_C_EPS = 4.93038065763132e-32; // 2^-104 +const double DD_C_MIN_NORMALIZED = 2.0041683600089728e-292; // = 2^(-1022 + 53) + +/* Compile-time initialization of const double2 structs */ + +const double2 DD_C_MAX = + _DD_REAL_INIT(1.79769313486231570815e+308, 9.97920154767359795037e+291); +const double2 DD_C_SAFE_MAX = + _DD_REAL_INIT(1.7976931080746007281e+308, 9.97920154767359795037e+291); +const int _DD_C_NDIGITS = 31; + +const double2 DD_C_ZERO = _DD_REAL_INIT(0.0, 0.0); +const double2 DD_C_ONE = _DD_REAL_INIT(1.0, 0.0); +const double2 DD_C_NEGONE = _DD_REAL_INIT(-1.0, 0.0); + +const double2 DD_C_2PI = + _DD_REAL_INIT(6.283185307179586232e+00, 2.449293598294706414e-16); +const double2 DD_C_PI = + _DD_REAL_INIT(3.141592653589793116e+00, 1.224646799147353207e-16); +const double2 DD_C_PI2 = + _DD_REAL_INIT(1.570796326794896558e+00, 6.123233995736766036e-17); +const double2 DD_C_PI4 = + _DD_REAL_INIT(7.853981633974482790e-01, 3.061616997868383018e-17); +const double2 DD_C_PI16 = + _DD_REAL_INIT(1.963495408493620697e-01, 7.654042494670957545e-18); +const double2 DD_C_3PI4 = + _DD_REAL_INIT(2.356194490192344837e+00, 9.1848509936051484375e-17); + +const double2 DD_C_E = + _DD_REAL_INIT(2.718281828459045091e+00, 1.445646891729250158e-16); +const double2 DD_C_LOG2 = + _DD_REAL_INIT(6.931471805599452862e-01, 2.319046813846299558e-17); +const double2 DD_C_LOG10 = + _DD_REAL_INIT(2.302585092994045901e+00, -2.170756223382249351e-16); + +#ifdef DD_C_NAN_IS_CONST +const double2 DD_C_NAN = _DD_REAL_INIT(NAN, NAN); +const double2 DD_C_INF = _DD_REAL_INIT(INFINITY, INFINITY); +const double2 DD_C_NEGINF = _DD_REAL_INIT(-INFINITY, -INFINITY); +#endif /* NAN */ + + +/* This routine is called whenever a fatal error occurs. */ +static volatile int errCount = 0; +void +dd_error(const char *msg) +{ + errCount++; + /* if (msg) { */ + /* fprintf(stderr, "ERROR %s\n", msg); */ + /* } */ +} + + +int +get_double_expn(double x) +{ + int i = 0; + double y; + if (x == 0.0) { + return INT_MIN; + } + if (isinf(x) || isnan(x)) { + return INT_MAX; + } + + y = fabs(x); + if (y < 1.0) { + while (y < 1.0) { + y *= 2.0; + i++; + } + return -i; + } else if (y >= 2.0) { + while (y >= 2.0) { + y *= 0.5; + i++; + } + return i; + } + return 0; +} + +/* ######################################################################## */ +/* # Exponentiation */ +/* ######################################################################## */ + +/* Computes the square root of the double-double number dd. + NOTE: dd must be a non-negative number. */ + +double2 +dd_sqrt(const double2 a) +{ + /* Strategy: Use Karp's trick: if x is an approximation + to sqrt(a), then + + sqrt(a) = a*x + [a - (a*x)^2] * x / 2 (approx) + + The approximation is accurate to twice the accuracy of x. + Also, the multiplication (a*x) and [-]*x can be done with + only half the precision. + */ + double x, ax; + + if (dd_is_zero(a)) + return DD_C_ZERO; + + if (dd_is_negative(a)) { + dd_error("(dd_sqrt): Negative argument."); + return DD_C_NAN; + } + + x = 1.0 / sqrt(a.x[0]); + ax = a.x[0] * x; + return dd_add_d_d(ax, dd_sub(a, dd_sqr_d(ax)).x[0] * (x * 0.5)); +} + +/* Computes the square root of a double in double-double precision. + NOTE: d must not be negative. */ + +double2 +dd_sqrt_d(double d) +{ + return dd_sqrt(dd_create_d(d)); +} + +/* Computes the n-th root of the double-double number a. + NOTE: n must be a positive integer. + NOTE: If n is even, then a must not be negative. */ + +double2 +dd_nroot(const double2 a, int n) +{ + /* Strategy: Use Newton iteration for the function + + f(x) = x^(-n) - a + + to find its root a^{-1/n}. The iteration is thus + + x' = x + x * (1 - a * x^n) / n + + which converges quadratically. We can then find + a^{1/n} by taking the reciprocal. + */ + double2 r, x; + + if (n <= 0) { + dd_error("(dd_nroot): N must be positive."); + return DD_C_NAN; + } + + if (n % 2 == 0 && dd_is_negative(a)) { + dd_error("(dd_nroot): Negative argument."); + return DD_C_NAN; + } + + if (n == 1) { + return a; + } + if (n == 2) { + return dd_sqrt(a); + } + + if (dd_is_zero(a)) + return DD_C_ZERO; + + /* Note a^{-1/n} = exp(-log(a)/n) */ + r = dd_abs(a); + x = dd_create_d(exp(-log(r.x[0]) / n)); + + /* Perform Newton's iteration. */ + x = dd_add( + x, dd_mul(x, dd_sub_d_dd(1.0, dd_div_dd_d(dd_mul(r, dd_npwr(x, n)), + DD_STATIC_CAST(double, n))))); + if (a.x[0] < 0.0) { + x = dd_neg(x); + } + return dd_inv(x); +} + +/* Computes the n-th power of a double-double number. + NOTE: 0^0 causes an error. */ + +double2 +dd_npwr(const double2 a, int n) +{ + double2 r = a; + double2 s = DD_C_ONE; + int N = abs(n); + if (N == 0) { + if (dd_is_zero(a)) { + dd_error("(dd_npwr): Invalid argument."); + return DD_C_NAN; + } + return DD_C_ONE; + } + + if (N > 1) { + /* Use binary exponentiation */ + while (N > 0) { + if (N % 2 == 1) { + s = dd_mul(s, r); + } + N /= 2; + if (N > 0) { + r = dd_sqr(r); + } + } + } + else { + s = r; + } + + /* Compute the reciprocal if n is negative. */ + if (n < 0) { + return dd_inv(s); + } + + return s; +} + +double2 +dd_npow(const double2 a, int n) +{ + return dd_npwr(a, n); +} + +double2 +dd_pow(const double2 a, const double2 b) +{ + return dd_exp(dd_mul(b, dd_log(a))); +} + +/* ######################################################################## */ +/* # Exp/Log functions */ +/* ######################################################################## */ + +static const double2 inv_fact[] = { + {{1.66666666666666657e-01, 9.25185853854297066e-18}}, + {{4.16666666666666644e-02, 2.31296463463574266e-18}}, + {{8.33333333333333322e-03, 1.15648231731787138e-19}}, + {{1.38888888888888894e-03, -5.30054395437357706e-20}}, + {{1.98412698412698413e-04, 1.72095582934207053e-22}}, + {{2.48015873015873016e-05, 2.15119478667758816e-23}}, + {{2.75573192239858925e-06, -1.85839327404647208e-22}}, + {{2.75573192239858883e-07, 2.37677146222502973e-23}}, + {{2.50521083854417202e-08, -1.44881407093591197e-24}}, + {{2.08767569878681002e-09, -1.20734505911325997e-25}}, + {{1.60590438368216133e-10, 1.25852945887520981e-26}}, + {{1.14707455977297245e-11, 2.06555127528307454e-28}}, + {{7.64716373181981641e-13, 7.03872877733453001e-30}}, + {{4.77947733238738525e-14, 4.39920548583408126e-31}}, + {{2.81145725434552060e-15, 1.65088427308614326e-31}} +}; +//static const int n_inv_fact = sizeof(inv_fact) / sizeof(inv_fact[0]); + +/* Exponential. Computes exp(x) in double-double precision. */ + +double2 +dd_exp(const double2 a) +{ + /* Strategy: We first reduce the size of x by noting that + + exp(kr + m * log(2)) = 2^m * exp(r)^k + + where m and k are integers. By choosing m appropriately + we can make |kr| <= log(2) / 2 = 0.347. Then exp(r) is + evaluated using the familiar Taylor series. Reducing the + argument substantially speeds up the convergence. */ + + const double k = 512.0; + const double inv_k = 1.0 / k; + double m; + double2 r, s, t, p; + int i = 0; + + if (a.x[0] <= -709.0) { + return DD_C_ZERO; + } + + if (a.x[0] >= 709.0) { + return DD_C_INF; + } + + if (dd_is_zero(a)) { + return DD_C_ONE; + } + + if (dd_is_one(a)) { + return DD_C_E; + } + + m = floor(a.x[0] / DD_C_LOG2.x[0] + 0.5); + r = dd_mul_pwr2(dd_sub(a, dd_mul_dd_d(DD_C_LOG2, m)), inv_k); + + p = dd_sqr(r); + s = dd_add(r, dd_mul_pwr2(p, 0.5)); + p = dd_mul(p, r); + t = dd_mul(p, inv_fact[0]); + do { + s = dd_add(s, t); + p = dd_mul(p, r); + ++i; + t = dd_mul(p, inv_fact[i]); + } while (fabs(dd_to_double(t)) > inv_k * DD_C_EPS && i < 5); + + s = dd_add(s, t); + + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(s, DD_C_ONE); + + return dd_ldexp(s, DD_STATIC_CAST(int, m)); +} + +double2 +dd_exp_d(const double a) +{ + return dd_exp(dd_create(a, 0)); +} + + +/* Logarithm. Computes log(x) in double-double precision. + This is a natural logarithm (i.e., base e). */ +double2 +dd_log(const double2 a) +{ + /* Strategy. The Taylor series for log converges much more + slowly than that of exp, due to the lack of the factorial + term in the denominator. Hence this routine instead tries + to determine the root of the function + + f(x) = exp(x) - a + + using Newton iteration. The iteration is given by + + x' = x - f(x)/f'(x) + = x - (1 - a * exp(-x)) + = x + a * exp(-x) - 1. + + Only one iteration is needed, since Newton's iteration + approximately doubles the number of digits per iteration. */ + double2 x; + + if (dd_is_one(a)) { + return DD_C_ZERO; + } + + if (a.x[0] <= 0.0) { + dd_error("(dd_log): Non-positive argument."); + return DD_C_NAN; + } + + x = dd_create_d(log(a.x[0])); /* Initial approximation */ + + /* x = x + a * exp(-x) - 1.0; */ + x = dd_add(x, dd_sub(dd_mul(a, dd_exp(dd_neg(x))), DD_C_ONE)); + return x; +} + + +double2 +dd_log1p(const double2 a) +{ + double2 ans; + double la, elam1, ll; + if (a.x[0] <= -1.0) { + return DD_C_NEGINF; + } + la = log1p(a.x[0]); + elam1 = expm1(la); + ll = log1p(a.x[1] / (1 + a.x[0])); + if (a.x[0] > 0) { + ll -= (elam1 - a.x[0])/(elam1+1); + } + ans = dd_add_d_d(la, ll); + return ans; +} + +double2 +dd_log10(const double2 a) +{ + return dd_div(dd_log(a), DD_C_LOG10); +} + +double2 +dd_log_d(double a) +{ + return dd_log(dd_create(a, 0)); +} + + +static const double2 expm1_numer[] = { + {{-0.028127670288085938, 1.46e-37}}, + {{0.5127815691121048, -4.248816580490825e-17}}, + {{-0.0632631785207471, 4.733650586348708e-18}}, + {{0.01470328560687425, -4.57569727474415e-20}}, + {{-0.0008675686051689528, 2.340010361165805e-20}}, + {{8.812635961829116e-05, 2.619804163788941e-21}}, + {{-2.596308786770631e-06, -1.6196413688647164e-22}}, + {{1.422669108780046e-07, 1.2956999470135368e-23}}, + {{-1.5995603306536497e-09, 5.185121944095551e-26}}, + {{4.526182006900779e-11, -1.9856249941108077e-27}} +}; + +static const double2 expm1_denom[] = { + {{1.0, 0.0}}, + {{-0.4544126470907431, -2.2553855773661143e-17}}, + {{0.09682713193619222, -4.961446925746919e-19}}, + {{-0.012745248725908178, -6.0676821249478945e-19}}, + {{0.001147361387158326, 1.3575817248483204e-20}}, + {{-7.370416847725892e-05, 3.720369981570573e-21}}, + {{3.4087499397791556e-06, -3.3067348191741576e-23}}, + {{-1.1114024704296196e-07, -3.313361038199987e-24}}, + {{2.3987051614110847e-09, 1.102474920537503e-25}}, + {{-2.947734185911159e-11, -9.4795654767864e-28}}, + {{1.32220659910223e-13, 6.440648413523595e-30}} +}; + +// +// Rational approximation of expm1(x) for -1/2 < x < 1/2 +// +static double2 +expm1_rational_approx(const double2 x) +{ + const double2 Y = dd_create(1.028127670288086, 0.0); + const double2 num = dd_polyeval(expm1_numer, 9, x); + const double2 den = dd_polyeval(expm1_denom, 10, x); + return dd_add(dd_mul(x, Y), dd_mul(x, dd_div(num, den))); +} + +// +// This is a translation of Boost's `expm1_imp` for quad precision +// for use with double2. +// + +#define LOG_MAX_VALUE 709.782712893384 + +double2 +dd_expm1(const double2 x) +{ + double2 a = dd_abs(x); + if (dd_hi(a) > 0.5) { + if (dd_hi(a) > LOG_MAX_VALUE) { + if (dd_hi(x) > 0) { + return DD_C_INF; + } + return DD_C_NEGONE; + } + return dd_sub_dd_d(dd_exp(x), 1.0); + } + return expm1_rational_approx(x); +} + + +double2 +dd_rand(void) +{ + static const double m_const = 4.6566128730773926e-10; /* = 2^{-31} */ + double m = m_const; + double2 r = DD_C_ZERO; + double d; + int i; + + /* Strategy: Generate 31 bits at a time, using lrand48 + random number generator. Shift the bits, and reapeat + 4 times. */ + + for (i = 0; i < 4; i++, m *= m_const) { + // d = lrand48() * m; + d = rand() * m; + r = dd_add_dd_d(r, d); + } + + return r; +} + +/* dd_polyeval(c, n, x) + Evaluates the given n-th degree polynomial at x. + The polynomial is given by the array of (n+1) coefficients. */ + +double2 +dd_polyeval(const double2 *c, int n, const double2 x) +{ + /* Just use Horner's method of polynomial evaluation. */ + double2 r = c[n]; + int i; + + for (i = n - 1; i >= 0; i--) { + r = dd_mul(r, x); + r = dd_add(r, c[i]); + } + + return r; +} + +/* dd_polyroot(c, n, x0) + Given an n-th degree polynomial, finds a root close to + the given guess x0. Note that this uses simple Newton + iteration scheme, and does not work for multiple roots. */ + +double2 +dd_polyroot(const double2 *c, int n, const double2 x0, int max_iter, + double thresh) +{ + double2 x = x0; + double2 f; + double2 *d = DD_STATIC_CAST(double2 *, calloc(sizeof(double2), n)); + int conv = 0; + int i; + double max_c = fabs(dd_to_double(c[0])); + double v; + + if (thresh == 0.0) { + thresh = DD_C_EPS; + } + + /* Compute the coefficients of the derivatives. */ + for (i = 1; i <= n; i++) { + v = fabs(dd_to_double(c[i])); + if (v > max_c) { + max_c = v; + } + d[i - 1] = dd_mul_dd_d(c[i], DD_STATIC_CAST(double, i)); + } + thresh *= max_c; + + /* Newton iteration. */ + for (i = 0; i < max_iter; i++) { + f = dd_polyeval(c, n, x); + + if (fabs(dd_to_double(f)) < thresh) { + conv = 1; + break; + } + x = dd_sub(x, (dd_div(f, dd_polyeval(d, n - 1, x)))); + } + free(d); + + if (!conv) { + dd_error("(dd_polyroot): Failed to converge."); + return DD_C_NAN; + } + + return x; +} diff --git a/gtsam/3rdparty/cephes/cephes/dd_real.h b/gtsam/3rdparty/cephes/cephes/dd_real.h new file mode 100644 index 000000000..4e09da143 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/dd_real.h @@ -0,0 +1,143 @@ +/* + * include/double2.h + * + * This work was supported by the Director, Office of Science, Division + * of Mathematical, Information, and Computational Sciences of the + * U.S. Department of Energy under contract numbers DE-AC03-76SF00098 and + * DE-AC02-05CH11231. + * + * Copyright (c) 2003-2009, The Regents of the University of California, + * through Lawrence Berkeley National Laboratory (subject to receipt of + * any required approvals from U.S. Dept. of Energy) All rights reserved. + * + * By downloading or using this software you are agreeing to the modified + * BSD license "BSD-LBNL-License.doc" (see LICENSE.txt). + */ +/* + * Double-double precision (>= 106-bit significand) floating point + * arithmetic package based on David Bailey's Fortran-90 double-double + * package, with some changes. See + * + * http://www.nersc.gov/~dhbailey/mpdist/mpdist.html + * + * for the original Fortran-90 version. + * + * Overall structure is similar to that of Keith Brigg's C++ double-double + * package. See + * + * http://www-epidem.plansci.cam.ac.uk/~kbriggs/doubledouble.html + * + * for more details. In particular, the fix for x86 computers is borrowed + * from his code. + * + * Yozo Hida + */ + +#ifndef _DD_REAL_H +#define _DD_REAL_H + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Some configuration defines */ + +/* If fast fused multiply-add is available, define to the correct macro for + using it. It is invoked as DD_FMA(a, b, c) to compute fl(a * b + c). + If correctly rounded multiply-add is not available (or if unsure), + keep it undefined. */ +#ifndef DD_FMA +#ifdef FP_FAST_FMA +#define DD_FMA(A, B, C) fma((A), (B), (C)) +#endif +#endif + +/* Same with fused multiply-subtract */ +#ifndef DD_FMS +#ifdef FP_FAST_FMA +#define DD_FMS(A, B, C) fma((A), (B), (-C)) +#endif +#endif + +#ifdef __cplusplus +#define DD_STATIC_CAST(T, X) (static_cast(X)) +#else +#define DD_STATIC_CAST(T, X) ((T)(X)) +#endif + +/* double2 struct definition, some external always-present double2 constants. +*/ +typedef struct double2 +{ + double x[2]; +} double2; + +extern const double DD_C_EPS; +extern const double DD_C_MIN_NORMALIZED; +extern const double2 DD_C_MAX; +extern const double2 DD_C_SAFE_MAX; +extern const int DD_C_NDIGITS; + +extern const double2 DD_C_2PI; +extern const double2 DD_C_PI; +extern const double2 DD_C_3PI4; +extern const double2 DD_C_PI2; +extern const double2 DD_C_PI4; +extern const double2 DD_C_PI16; +extern const double2 DD_C_E; +extern const double2 DD_C_LOG2; +extern const double2 DD_C_LOG10; +extern const double2 DD_C_ZERO; +extern const double2 DD_C_ONE; +extern const double2 DD_C_NEGONE; + +/* NAN definition in AIX's math.h doesn't make it qualify as constant literal. */ +#if defined(__STDC__) && defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L) && defined(NAN) && !defined(_AIX) +#define DD_C_NAN_IS_CONST +extern const double2 DD_C_NAN; +extern const double2 DD_C_INF; +extern const double2 DD_C_NEGINF; +#else +#define DD_C_NAN (dd_create(NAN, NAN)) +#define DD_C_INF (dd_create(INFINITY, INFINITY)) +#define DD_C_NEGINF (dd_create(-INFINITY, -INFINITY)) +#endif + + +/* Include the inline definitions of functions */ +#include "dd_real_idefs.h" + +/* Non-inline functions */ + +/********** Exponentiation **********/ +double2 dd_npwr(const double2 a, int n); + +/*********** Transcendental Functions ************/ +double2 dd_exp(const double2 a); +double2 dd_log(const double2 a); +double2 dd_expm1(const double2 a); +double2 dd_log1p(const double2 a); +double2 dd_log10(const double2 a); +double2 dd_log_d(double a); + +/* Returns the exponent of the double precision number. + Returns INT_MIN is x is zero, and INT_MAX if x is INF or NaN. */ +int get_double_expn(double x); + +/*********** Polynomial Functions ************/ +double2 dd_polyeval(const double2 *c, int n, const double2 x); + +/*********** Random number generator ************/ +extern double2 dd_rand(void); + + +#ifdef __cplusplus +} +#endif + + +#endif /* _DD_REAL_H */ diff --git a/gtsam/3rdparty/cephes/cephes/dd_real_idefs.h b/gtsam/3rdparty/cephes/cephes/dd_real_idefs.h new file mode 100644 index 000000000..d2b9ac1d6 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/dd_real_idefs.h @@ -0,0 +1,557 @@ +/* + * include/dd_inline.h + * + * This work was supported by the Director, Office of Science, Division + * of Mathematical, Information, and Computational Sciences of the + * U.S. Department of Energy under contract numbers DE-AC03-76SF00098 and + * DE-AC02-05CH11231. + * + * Copyright (c) 2003-2009, The Regents of the University of California, + * through Lawrence Berkeley National Laboratory (subject to receipt of + * any required approvals from U.S. Dept. of Energy) All rights reserved. + * + * By downloading or using this software you are agreeing to the modified + * BSD license "BSD-LBNL-License.doc" (see LICENSE.txt). + */ +/* + * Contains small functions (suitable for inlining) in the double-double + * arithmetic package. + */ + +#ifndef _DD_REAL_IDEFS_H_ +#define _DD_REAL_IDEFS_H_ 1 + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#include "dd_idefs.h" + +/* + ************************************************************************ + Now for the double2 routines + ************************************************************************ +*/ + +static inline double +dd_hi(const double2 a) +{ + return a.x[0]; +} + +static inline double +dd_lo(const double2 a) +{ + return a.x[1]; +} + +static inline int +dd_isfinite(const double2 a) +{ + return isfinite(a.x[0]); +} + +static inline int +dd_isinf(const double2 a) +{ + return isinf(a.x[0]); +} + +static inline int +dd_is_zero(const double2 a) +{ + return (a.x[0] == 0.0); +} + +static inline int +dd_is_one(const double2 a) +{ + return (a.x[0] == 1.0 && a.x[1] == 0.0); +} + +static inline int +dd_is_positive(const double2 a) +{ + return (a.x[0] > 0.0); +} + +static inline int +dd_is_negative(const double2 a) +{ + return (a.x[0] < 0.0); +} + +/* Cast to double. */ +static inline double +dd_to_double(const double2 a) +{ + return a.x[0]; +} + +/* Cast to int. */ +static inline int +dd_to_int(const double2 a) +{ + return DD_STATIC_CAST(int, a.x[0]); +} + +/*********** Equality and Other Comparisons ************/ +static inline int +dd_comp(const double2 a, const double2 b) +{ + int cmp = two_comp(a.x[0], b.x[0]); + if (cmp == 0) { + cmp = two_comp(a.x[1], b.x[1]); + } + return cmp; +} + +static inline int +dd_comp_dd_d(const double2 a, double b) +{ + int cmp = two_comp(a.x[0], b); + if (cmp == 0) { + cmp = two_comp(a.x[1], 0); + } + return cmp; +} + +static inline int +dd_comp_d_dd(double a, const double2 b) +{ + int cmp = two_comp(a, b.x[0]); + if (cmp == 0) { + cmp = two_comp(0.0, b.x[1]); + } + return cmp; +} + + +/*********** Creation ************/ +static inline double2 +dd_create(double hi, double lo) +{ + double2 ret = {{hi, lo}}; + return ret; +} + +static inline double2 +dd_zero(void) +{ + return DD_C_ZERO; +} + +static inline double2 +dd_create_d(double hi) +{ + double2 ret = {{hi, 0.0}}; + return ret; +} + +static inline double2 +dd_create_i(int hi) +{ + double2 ret = {{DD_STATIC_CAST(double, hi), 0.0}}; + return ret; +} + +static inline double2 +dd_create_dp(const double *d) +{ + double2 ret = {{d[0], d[1]}}; + return ret; +} + + +/*********** Unary Minus ***********/ +static inline double2 +dd_neg(const double2 a) +{ + double2 ret = {{-a.x[0], -a.x[1]}}; + return ret; +} + +/*********** Rounding ************/ +/* Round to Nearest integer */ +static inline double2 +dd_nint(const double2 a) +{ + double hi = two_nint(a.x[0]); + double lo; + + if (hi == a.x[0]) { + /* High word is an integer already. Round the low word.*/ + lo = two_nint(a.x[1]); + + /* Renormalize. This is needed if x[0] = some integer, x[1] = 1/2.*/ + hi = quick_two_sum(hi, lo, &lo); + } + else { + /* High word is not an integer. */ + lo = 0.0; + if (fabs(hi - a.x[0]) == 0.5 && a.x[1] < 0.0) { + /* There is a tie in the high word, consult the low word + to break the tie. */ + hi -= 1.0; /* NOTE: This does not cause INEXACT. */ + } + } + + return dd_create(hi, lo); +} + +static inline double2 +dd_floor(const double2 a) +{ + double hi = floor(a.x[0]); + double lo = 0.0; + + if (hi == a.x[0]) { + /* High word is integer already. Round the low word. */ + lo = floor(a.x[1]); + hi = quick_two_sum(hi, lo, &lo); + } + + return dd_create(hi, lo); +} + +static inline double2 +dd_ceil(const double2 a) +{ + double hi = ceil(a.x[0]); + double lo = 0.0; + + if (hi == a.x[0]) { + /* High word is integer already. Round the low word. */ + lo = ceil(a.x[1]); + hi = quick_two_sum(hi, lo, &lo); + } + + return dd_create(hi, lo); +} + +static inline double2 +dd_aint(const double2 a) +{ + return (a.x[0] >= 0.0) ? dd_floor(a) : dd_ceil(a); +} + +/* Absolute value */ +static inline double2 +dd_abs(const double2 a) +{ + return (a.x[0] < 0.0 ? dd_neg(a) : a); +} + +static inline double2 +dd_fabs(const double2 a) +{ + return dd_abs(a); +} + + +/*********** Normalizing ***********/ +/* double-double * (2.0 ^ expt) */ +static inline double2 +dd_ldexp(const double2 a, int expt) +{ + return dd_create(ldexp(a.x[0], expt), ldexp(a.x[1], expt)); +} + +static inline double2 +dd_frexp(const double2 a, int *expt) +{ +// r"""return b and l s.t. 0.5<=|b|<1 and 2^l == a +// 0.5<=|b[0]|<1.0 or |b[0]| == 1.0 and b[0]*b[1]<0 +// """ + int exponent; + double man = frexp(a.x[0], &exponent); + double b1 = ldexp(a.x[1], -exponent); + if (fabs(man) == 0.5 && man * b1 < 0) + { + man *=2; + b1 *= 2; + exponent -= 1; + } + *expt = exponent; + return dd_create(man, b1); +} + + +/*********** Additions ************/ +static inline double2 +dd_add_d_d(double a, double b) +{ + double s, e; + s = two_sum(a, b, &e); + return dd_create(s, e); +} + +static inline double2 +dd_add_dd_d(const double2 a, double b) +{ + double s1, s2; + s1 = two_sum(a.x[0], b, &s2); + s2 += a.x[1]; + s1 = quick_two_sum(s1, s2, &s2); + return dd_create(s1, s2); +} + +static inline double2 +dd_add_d_dd(double a, const double2 b) +{ + double s1, s2; + s1 = two_sum(a, b.x[0], &s2); + s2 += b.x[1]; + s1 = quick_two_sum(s1, s2, &s2); + return dd_create(s1, s2); +} + +static inline double2 +dd_ieee_add(const double2 a, const double2 b) +{ + /* This one satisfies IEEE style error bound, + due to K. Briggs and W. Kahan. */ + double s1, s2, t1, t2; + + s1 = two_sum(a.x[0], b.x[0], &s2); + t1 = two_sum(a.x[1], b.x[1], &t2); + s2 += t1; + s1 = quick_two_sum(s1, s2, &s2); + s2 += t2; + s1 = quick_two_sum(s1, s2, &s2); + return dd_create(s1, s2); +} + +static inline double2 +dd_sloppy_add(const double2 a, const double2 b) +{ + /* This is the less accurate version ... obeys Cray-style + error bound. */ + double s, e; + + s = two_sum(a.x[0], b.x[0], &e); + e += (a.x[1] + b.x[1]); + s = quick_two_sum(s, e, &e); + return dd_create(s, e); +} + +static inline double2 +dd_add(const double2 a, const double2 b) +{ + /* Always require IEEE-style error bounds */ + return dd_ieee_add(a, b); +} + +/*********** Subtractions ************/ +/* double-double = double - double */ +static inline double2 +dd_sub_d_d(double a, double b) +{ + double s, e; + s = two_diff(a, b, &e); + return dd_create(s, e); +} + +static inline double2 +dd_sub(const double2 a, const double2 b) +{ + return dd_ieee_add(a, dd_neg(b)); +} + +static inline double2 +dd_sub_dd_d(const double2 a, double b) +{ + double s1, s2; + s1 = two_sum(a.x[0], -b, &s2); + s2 += a.x[1]; + s1 = quick_two_sum(s1, s2, &s2); + return dd_create(s1, s2); +} + +static inline double2 +dd_sub_d_dd(double a, const double2 b) +{ + double s1, s2; + s1 = two_sum(a, -b.x[0], &s2); + s2 -= b.x[1]; + s1 = quick_two_sum(s1, s2, &s2); + return dd_create(s1, s2); +} + + +/*********** Multiplications ************/ +/* double-double = double * double */ +static inline double2 +dd_mul_d_d(double a, double b) +{ + double p, e; + p = two_prod(a, b, &e); + return dd_create(p, e); +} + +/* double-double * double, where double is a power of 2. */ +static inline double2 +dd_mul_pwr2(const double2 a, double b) +{ + return dd_create(a.x[0] * b, a.x[1] * b); +} + +static inline double2 +dd_mul(const double2 a, const double2 b) +{ + double p1, p2; + p1 = two_prod(a.x[0], b.x[0], &p2); + p2 += (a.x[0] * b.x[1] + a.x[1] * b.x[0]); + p1 = quick_two_sum(p1, p2, &p2); + return dd_create(p1, p2); +} + +static inline double2 +dd_mul_dd_d(const double2 a, double b) +{ + double p1, p2, e1, e2; + p1 = two_prod(a.x[0], b, &e1); + p2 = two_prod(a.x[1], b, &e2); + p1 = quick_two_sum(p1, e2 + p2 + e1, &e1); + return dd_create(p1, e1); +} + +static inline double2 +dd_mul_d_dd(double a, const double2 b) +{ + double p1, p2, e1, e2; + p1 = two_prod(a, b.x[0], &e1); + p2 = two_prod(a, b.x[1], &e2); + p1 = quick_two_sum(p1, e2 + p2 + e1, &e1); + return dd_create(p1, e1); +} + + +/*********** Divisions ************/ +static inline double2 +dd_sloppy_div(const double2 a, const double2 b) +{ + double s1, s2; + double q1, q2; + double2 r; + + q1 = a.x[0] / b.x[0]; /* approximate quotient */ + + /* compute this - q1 * dd */ + r = dd_sub(a, dd_mul_dd_d(b, q1)); + s1 = two_diff(a.x[0], r.x[0], &s2); + s2 -= r.x[1]; + s2 += a.x[1]; + + /* get next approximation */ + q2 = (s1 + s2) / b.x[0]; + + /* renormalize */ + r.x[0] = quick_two_sum(q1, q2, &r.x[1]); + return r; +} + +static inline double2 +dd_accurate_div(const double2 a, const double2 b) +{ + double q1, q2, q3; + double2 r; + + q1 = a.x[0] / b.x[0]; /* approximate quotient */ + + r = dd_sub(a, dd_mul_dd_d(b, q1)); + + q2 = r.x[0] / b.x[0]; + r = dd_sub(r, dd_mul_dd_d(b, q2)); + + q3 = r.x[0] / b.x[0]; + + q1 = quick_two_sum(q1, q2, &q2); + r = dd_add_dd_d(dd_create(q1, q2), q3); + return r; +} + +static inline double2 +dd_div(const double2 a, const double2 b) +{ + return dd_accurate_div(a, b); +} + +static inline double2 +dd_div_d_d(double a, double b) +{ + return dd_accurate_div(dd_create_d(a), dd_create_d(b)); +} + +static inline double2 +dd_div_dd_d(const double2 a, double b) +{ + return dd_accurate_div(a, dd_create_d(b)); +} + +static inline double2 +dd_div_d_dd(double a, const double2 b) +{ + return dd_accurate_div(dd_create_d(a), b); +} + +static inline double2 +dd_inv(const double2 a) +{ + return dd_div(DD_C_ONE, a); +} + + +/********** Remainder **********/ +static inline double2 +dd_drem(const double2 a, const double2 b) +{ + double2 n = dd_nint(dd_div(a, b)); + return dd_sub(a, dd_mul(n, b)); +} + +static inline double2 +dd_divrem(const double2 a, const double2 b, double2 *r) +{ + double2 n = dd_nint(dd_div(a, b)); + *r = dd_sub(a, dd_mul(n, b)); + return n; +} + +static inline double2 +dd_fmod(const double2 a, const double2 b) +{ + double2 n = dd_aint(dd_div(a, b)); + return dd_sub(a, dd_mul(b, n)); +} + +/*********** Squaring **********/ +static inline double2 +dd_sqr(const double2 a) +{ + double p1, p2; + double s1, s2; + p1 = two_sqr(a.x[0], &p2); + p2 += 2.0 * a.x[0] * a.x[1]; + p2 += a.x[1] * a.x[1]; + s1 = quick_two_sum(p1, p2, &s2); + return dd_create(s1, s2); +} + +static inline double2 +dd_sqr_d(double a) +{ + double p1, p2; + p1 = two_sqr(a, &p2); + return dd_create(p1, p2); +} + +#ifdef __cplusplus +} +#endif + +#endif /* _DD_REAL_IDEFS_H_ */ diff --git a/gtsam/3rdparty/cephes/cephes/ellie.c b/gtsam/3rdparty/cephes/cephes/ellie.c new file mode 100644 index 000000000..8a2823f3a --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ellie.c @@ -0,0 +1,282 @@ +/* ellie.c + * + * Incomplete elliptic integral of the second kind + * + * + * + * SYNOPSIS: + * + * double phi, m, y, ellie(); + * + * y = ellie( phi, m ); + * + * + * + * DESCRIPTION: + * + * Approximates the integral + * + * + * phi + * - + * | | + * | 2 + * E(phi_\m) = | sqrt( 1 - m sin t ) dt + * | + * | | + * - + * 0 + * + * of amplitude phi and modulus m, using the arithmetic - + * geometric mean algorithm. + * + * + * + * ACCURACY: + * + * Tested at random arguments with phi in [-10, 10] and m in + * [0, 1]. + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -10,10 150000 3.3e-15 1.4e-16 + */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987, 1993 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ +/* Copyright 2014, Eric W. Moore */ + +/* Incomplete elliptic integral of second kind */ + +#include "mconf.h" + +extern double MACHEP; + +static double ellie_neg_m(double phi, double m); + +double ellie(double phi, double m) +{ + double a, b, c, e, temp; + double lphi, t, E, denom, npio2; + int d, mod, sign; + + if (cephes_isnan(phi) || cephes_isnan(m)) + return NAN; + if (m > 1.0) + return NAN; + if (cephes_isinf(phi)) + return phi; + if (cephes_isinf(m)) + return -m; + if (m == 0.0) + return (phi); + lphi = phi; + npio2 = floor(lphi / M_PI_2); + if (fmod(fabs(npio2), 2.0) == 1.0) + npio2 += 1; + lphi = lphi - npio2 * M_PI_2; + if (lphi < 0.0) { + lphi = -lphi; + sign = -1; + } + else { + sign = 1; + } + a = 1.0 - m; + E = ellpe(m); + if (a == 0.0) { + temp = sin(lphi); + goto done; + } + if (a > 1.0) { + temp = ellie_neg_m(lphi, m); + goto done; + } + + if (lphi < 0.135) { + double m11= (((((-7.0/2816.0)*m + (5.0/1056.0))*m - (7.0/2640.0))*m + + (17.0/41580.0))*m - (1.0/155925.0))*m; + double m9 = ((((-5.0/1152.0)*m + (1.0/144.0))*m - (1.0/360.0))*m + + (1.0/5670.0))*m; + double m7 = ((-m/112.0 + (1.0/84.0))*m - (1.0/315.0))*m; + double m5 = (-m/40.0 + (1.0/30))*m; + double m3 = -m/6.0; + double p2 = lphi * lphi; + + temp = ((((m11*p2 + m9)*p2 + m7)*p2 + m5)*p2 + m3)*p2*lphi + lphi; + goto done; + } + t = tan(lphi); + b = sqrt(a); + /* Thanks to Brian Fitzgerald + * for pointing out an instability near odd multiples of pi/2. */ + if (fabs(t) > 10.0) { + /* Transform the amplitude */ + e = 1.0 / (b * t); + /* ... but avoid multiple recursions. */ + if (fabs(e) < 10.0) { + e = atan(e); + temp = E + m * sin(lphi) * sin(e) - ellie(e, m); + goto done; + } + } + c = sqrt(m); + a = 1.0; + d = 1; + e = 0.0; + mod = 0; + + while (fabs(c / a) > MACHEP) { + temp = b / a; + lphi = lphi + atan(t * temp) + mod * M_PI; + denom = 1 - temp * t * t; + if (fabs(denom) > 10*MACHEP) { + t = t * (1.0 + temp) / denom; + mod = (lphi + M_PI_2) / M_PI; + } + else { + t = tan(lphi); + mod = (int)floor((lphi - atan(t))/M_PI); + } + c = (a - b) / 2.0; + temp = sqrt(a * b); + a = (a + b) / 2.0; + b = temp; + d += d; + e += c * sin(lphi); + } + + temp = E / ellpk(1.0 - m); + temp *= (atan(t) + mod * M_PI) / (d * a); + temp += e; + + done: + + if (sign < 0) + temp = -temp; + temp += npio2 * E; + return (temp); +} + +/* N.B. This will evaluate its arguments multiple times. */ +#define MAX3(a, b, c) (a > b ? (a > c ? a : c) : (b > c ? b : c)) + +/* To calculate legendre's incomplete elliptical integral of the second kind for + * negative m, we use a power series in phi for small m*phi*phi, an asymptotic + * series in m for large m*phi*phi* and the relation to Carlson's symmetric + * integrals, R_F(x,y,z) and R_D(x,y,z). + * + * E(phi, m) = sin(phi) * R_F(cos(phi)^2, 1 - m * sin(phi)^2, 1.0) + * - m * sin(phi)^3 * R_D(cos(phi)^2, 1 - m * sin(phi)^2, 1.0) / 3 + * + * = R_F(c-1, c-m, c) - m * R_D(c-1, c-m, c) / 3 + * + * where c = csc(phi)^2. We use the second form of this for (approximately) + * phi > 1/(sqrt(DBL_MAX) ~ 1e-154, where csc(phi)^2 overflows. Elsewhere we + * use the first form, accounting for the smallness of phi. + * + * The algorithm used is described in Carlson, B. C. Numerical computation of + * real or complex elliptic integrals. (1994) https://arxiv.org/abs/math/9409227 + * Most variable names reflect Carlson's usage. + * + * In this routine, we assume m < 0 and 0 > phi > pi/2. + */ +double ellie_neg_m(double phi, double m) +{ + double x, y, z, x1, y1, z1, ret, Q; + double A0f, Af, Xf, Yf, Zf, E2f, E3f, scalef; + double A0d, Ad, seriesn, seriesd, Xd, Yd, Zd, E2d, E3d, E4d, E5d, scaled; + int n = 0; + double mpp = (m*phi)*phi; + + if (-mpp < 1e-6 && phi < -m) { + return phi + (mpp*phi*phi/30.0 - mpp*mpp/40.0 - mpp/6.0)*phi; + } + + if (-mpp > 1e6) { + double sm = sqrt(-m); + double sp = sin(phi); + double cp = cos(phi); + + double a = -cosm1(phi); + double b1 = log(4*sp*sm/(1+cp)); + double b = -(0.5 + b1) / 2.0 / m; + double c = (0.75 + cp/sp/sp - b1) / 16.0 / m / m; + return (a + b + c) * sm; + } + + if (phi > 1e-153 && m > -1e200) { + double s = sin(phi); + double csc2 = 1.0 / s / s; + scalef = 1.0; + scaled = m / 3.0; + x = 1.0 / tan(phi) / tan(phi); + y = csc2 - m; + z = csc2; + } + else { + scalef = phi; + scaled = mpp * phi / 3.0; + x = 1.0; + y = 1 - mpp; + z = 1.0; + } + + if (x == y && x == z) { + return (scalef + scaled/x)/sqrt(x); + } + + A0f = (x + y + z) / 3.0; + Af = A0f; + A0d = (x + y + 3.0*z) / 5.0; + Ad = A0d; + x1 = x; y1 = y; z1 = z; seriesd = 0.0; seriesn = 1.0; + /* Carlson gives 1/pow(3*r, 1.0/6.0) for this constant. if r == eps, + * it is ~338.38. */ + Q = 400.0 * MAX3(fabs(A0f-x), fabs(A0f-y), fabs(A0f-z)); + + while (Q > fabs(Af) && Q > fabs(Ad) && n <= 100) { + double sx = sqrt(x1); + double sy = sqrt(y1); + double sz = sqrt(z1); + double lam = sx*sy + sx*sz + sy*sz; + seriesd += seriesn / (sz * (z1 + lam)); + x1 = (x1 + lam) / 4.0; + y1 = (y1 + lam) / 4.0; + z1 = (z1 + lam) / 4.0; + Af = (x1 + y1 + z1) / 3.0; + Ad = (Ad + lam) / 4.0; + n += 1; + Q /= 4.0; + seriesn /= 4.0; + } + + Xf = (A0f - x) / Af / (1 << 2*n); + Yf = (A0f - y) / Af / (1 << 2*n); + Zf = -(Xf + Yf); + + E2f = Xf*Yf - Zf*Zf; + E3f = Xf*Yf*Zf; + + ret = scalef * (1.0 - E2f/10.0 + E3f/14.0 + E2f*E2f/24.0 + - 3.0*E2f*E3f/44.0) / sqrt(Af); + + Xd = (A0d - x) / Ad / (1 << 2*n); + Yd = (A0d - y) / Ad / (1 << 2*n); + Zd = -(Xd + Yd)/3.0; + + E2d = Xd*Yd - 6.0*Zd*Zd; + E3d = (3*Xd*Yd - 8.0*Zd*Zd)*Zd; + E4d = 3.0*(Xd*Yd - Zd*Zd)*Zd*Zd; + E5d = Xd*Yd*Zd*Zd*Zd; + + ret -= scaled * (1.0 - 3.0*E2d/14.0 + E3d/6.0 + 9.0*E2d*E2d/88.0 + - 3.0*E4d/22.0 - 9.0*E2d*E3d/52.0 + 3.0*E5d/26.0) + /(1 << 2*n) / Ad / sqrt(Ad); + ret -= 3.0 * scaled * seriesd; + return ret; +} + diff --git a/gtsam/3rdparty/cephes/cephes/ellik.c b/gtsam/3rdparty/cephes/cephes/ellik.c new file mode 100644 index 000000000..ee73e062a --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ellik.c @@ -0,0 +1,246 @@ +/* ellik.c + * + * Incomplete elliptic integral of the first kind + * + * + * + * SYNOPSIS: + * + * double phi, m, y, ellik(); + * + * y = ellik( phi, m ); + * + * + * + * DESCRIPTION: + * + * Approximates the integral + * + * + * + * phi + * - + * | | + * | dt + * F(phi | m) = | ------------------ + * | 2 + * | | sqrt( 1 - m sin t ) + * - + * 0 + * + * of amplitude phi and modulus m, using the arithmetic - + * geometric mean algorithm. + * + * + * + * + * ACCURACY: + * + * Tested at random points with m in [0, 1] and phi as indicated. + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -10,10 200000 7.4e-16 1.0e-16 + * + * + */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ +/* Copyright 2014, Eric W. Moore */ + +/* Incomplete elliptic integral of first kind */ + +#include "mconf.h" +extern double MACHEP; + +static double ellik_neg_m(double phi, double m); + +double ellik(double phi, double m) +{ + double a, b, c, e, temp, t, K, denom, npio2; + int d, mod, sign; + + if (cephes_isnan(phi) || cephes_isnan(m)) + return NAN; + if (m > 1.0) + return NAN; + if (cephes_isinf(phi) || cephes_isinf(m)) + { + if (cephes_isinf(m) && cephes_isfinite(phi)) + return 0.0; + else if (cephes_isinf(phi) && cephes_isfinite(m)) + return phi; + else + return NAN; + } + if (m == 0.0) + return (phi); + a = 1.0 - m; + if (a == 0.0) { + if (fabs(phi) >= (double)M_PI_2) { + sf_error("ellik", SF_ERROR_SINGULAR, NULL); + return (INFINITY); + } + /* DLMF 19.6.8, and 4.23.42 */ + return asinh(tan(phi)); + } + npio2 = floor(phi / M_PI_2); + if (fmod(fabs(npio2), 2.0) == 1.0) + npio2 += 1; + if (npio2 != 0.0) { + K = ellpk(a); + phi = phi - npio2 * M_PI_2; + } + else + K = 0.0; + if (phi < 0.0) { + phi = -phi; + sign = -1; + } + else + sign = 0; + if (a > 1.0) { + temp = ellik_neg_m(phi, m); + goto done; + } + b = sqrt(a); + t = tan(phi); + if (fabs(t) > 10.0) { + /* Transform the amplitude */ + e = 1.0 / (b * t); + /* ... but avoid multiple recursions. */ + if (fabs(e) < 10.0) { + e = atan(e); + if (npio2 == 0) + K = ellpk(a); + temp = K - ellik(e, m); + goto done; + } + } + a = 1.0; + c = sqrt(m); + d = 1; + mod = 0; + + while (fabs(c / a) > MACHEP) { + temp = b / a; + phi = phi + atan(t * temp) + mod * M_PI; + denom = 1.0 - temp * t * t; + if (fabs(denom) > 10*MACHEP) { + t = t * (1.0 + temp) / denom; + mod = (phi + M_PI_2) / M_PI; + } + else { + t = tan(phi); + mod = (int)floor((phi - atan(t))/M_PI); + } + c = (a - b) / 2.0; + temp = sqrt(a * b); + a = (a + b) / 2.0; + b = temp; + d += d; + } + + temp = (atan(t) + mod * M_PI) / (d * a); + + done: + if (sign < 0) + temp = -temp; + temp += npio2 * K; + return (temp); +} + +/* N.B. This will evaluate its arguments multiple times. */ +#define MAX3(a, b, c) (a > b ? (a > c ? a : c) : (b > c ? b : c)) + +/* To calculate legendre's incomplete elliptical integral of the first kind for + * negative m, we use a power series in phi for small m*phi*phi, an asymptotic + * series in m for large m*phi*phi* and the relation to Carlson's symmetric + * integral of the first kind. + * + * F(phi, m) = sin(phi) * R_F(cos(phi)^2, 1 - m * sin(phi)^2, 1.0) + * = R_F(c-1, c-m, c) + * + * where c = csc(phi)^2. We use the second form of this for (approximately) + * phi > 1/(sqrt(DBL_MAX) ~ 1e-154, where csc(phi)^2 overflows. Elsewhere we + * use the first form, accounting for the smallness of phi. + * + * The algorithm used is described in Carlson, B. C. Numerical computation of + * real or complex elliptic integrals. (1994) https://arxiv.org/abs/math/9409227 + * Most variable names reflect Carlson's usage. + * + * In this routine, we assume m < 0 and 0 > phi > pi/2. + */ +double ellik_neg_m(double phi, double m) +{ + double x, y, z, x1, y1, z1, A0, A, Q, X, Y, Z, E2, E3, scale; + int n = 0; + double mpp = (m*phi)*phi; + + if (-mpp < 1e-6 && phi < -m) { + return phi + (-mpp*phi*phi/30.0 + 3.0*mpp*mpp/40.0 + mpp/6.0)*phi; + } + + if (-mpp > 4e7) { + double sm = sqrt(-m); + double sp = sin(phi); + double cp = cos(phi); + + double a = log(4*sp*sm/(1+cp)); + double b = -(1 + cp/sp/sp - a) / 4 / m; + return (a + b) / sm; + } + + if (phi > 1e-153 && m > -1e305) { + double s = sin(phi); + double csc2 = 1.0 / (s*s); + scale = 1.0; + x = 1.0 / (tan(phi) * tan(phi)); + y = csc2 - m; + z = csc2; + } + else { + scale = phi; + x = 1.0; + y = 1 - m*scale*scale; + z = 1.0; + } + + if (x == y && x == z) { + return scale / sqrt(x); + } + + A0 = (x + y + z) / 3.0; + A = A0; + x1 = x; y1 = y; z1 = z; + /* Carlson gives 1/pow(3*r, 1.0/6.0) for this constant. if r == eps, + * it is ~338.38. */ + Q = 400.0 * MAX3(fabs(A0-x), fabs(A0-y), fabs(A0-z)); + + while (Q > fabs(A) && n <= 100) { + double sx = sqrt(x1); + double sy = sqrt(y1); + double sz = sqrt(z1); + double lam = sx*sy + sx*sz + sy*sz; + x1 = (x1 + lam) / 4.0; + y1 = (y1 + lam) / 4.0; + z1 = (z1 + lam) / 4.0; + A = (x1 + y1 + z1) / 3.0; + n += 1; + Q /= 4; + } + X = (A0 - x) / A / (1 << 2*n); + Y = (A0 - y) / A / (1 << 2*n); + Z = -(X + Y); + + E2 = X*Y - Z*Z; + E3 = X*Y*Z; + + return scale * (1.0 - E2/10.0 + E3/14.0 + E2*E2/24.0 + - 3.0*E2*E3/44.0) / sqrt(A); +} diff --git a/gtsam/3rdparty/cephes/cephes/ellpe.c b/gtsam/3rdparty/cephes/cephes/ellpe.c new file mode 100644 index 000000000..1ef8e0c12 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ellpe.c @@ -0,0 +1,108 @@ +/* ellpe.c + * + * Complete elliptic integral of the second kind + * + * + * + * SYNOPSIS: + * + * double m, y, ellpe(); + * + * y = ellpe( m ); + * + * + * + * DESCRIPTION: + * + * Approximates the integral + * + * + * pi/2 + * - + * | | 2 + * E(m) = | sqrt( 1 - m sin t ) dt + * | | + * - + * 0 + * + * Where m = 1 - m1, using the approximation + * + * P(x) - x log x Q(x). + * + * Though there are no singularities, the argument m1 is used + * internally rather than m for compatibility with ellpk(). + * + * E(1) = 1; E(0) = pi/2. + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 1 10000 2.1e-16 7.3e-17 + * + * + * ERROR MESSAGES: + * + * message condition value returned + * ellpe domain x<0, x>1 0.0 + * + */ + +/* ellpe.c */ + +/* Elliptic integral of second kind */ + +/* + * Cephes Math Library, Release 2.1: February, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + * + * Feb, 2002: altered by Travis Oliphant + * so that it is called with argument m + * (which gets immediately converted to m1 = 1-m) + */ + +#include "mconf.h" + +static double P[] = { + 1.53552577301013293365E-4, + 2.50888492163602060990E-3, + 8.68786816565889628429E-3, + 1.07350949056076193403E-2, + 7.77395492516787092951E-3, + 7.58395289413514708519E-3, + 1.15688436810574127319E-2, + 2.18317996015557253103E-2, + 5.68051945617860553470E-2, + 4.43147180560990850618E-1, + 1.00000000000000000299E0 +}; + +static double Q[] = { + 3.27954898576485872656E-5, + 1.00962792679356715133E-3, + 6.50609489976927491433E-3, + 1.68862163993311317300E-2, + 2.61769742454493659583E-2, + 3.34833904888224918614E-2, + 4.27180926518931511717E-2, + 5.85936634471101055642E-2, + 9.37499997197644278445E-2, + 2.49999999999888314361E-1 +}; + +double ellpe(double x) +{ + x = 1.0 - x; + if (x <= 0.0) { + if (x == 0.0) + return (1.0); + sf_error("ellpe", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (x > 1.0) { + return ellpe(1.0 - 1/x) * sqrt(x); + } + return (polevl(x, P, 10) - log(x) * (x * polevl(x, Q, 9))); +} diff --git a/gtsam/3rdparty/cephes/cephes/ellpj.c b/gtsam/3rdparty/cephes/cephes/ellpj.c new file mode 100644 index 000000000..6891a8244 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ellpj.c @@ -0,0 +1,154 @@ +/* ellpj.c + * + * Jacobian Elliptic Functions + * + * + * + * SYNOPSIS: + * + * double u, m, sn, cn, dn, phi; + * int ellpj(); + * + * ellpj( u, m, _&sn, _&cn, _&dn, _&phi ); + * + * + * + * DESCRIPTION: + * + * + * Evaluates the Jacobian elliptic functions sn(u|m), cn(u|m), + * and dn(u|m) of parameter m between 0 and 1, and real + * argument u. + * + * These functions are periodic, with quarter-period on the + * real axis equal to the complete elliptic integral + * ellpk(m). + * + * Relation to incomplete elliptic integral: + * If u = ellik(phi,m), then sn(u|m) = sin(phi), + * and cn(u|m) = cos(phi). Phi is called the amplitude of u. + * + * Computation is by means of the arithmetic-geometric mean + * algorithm, except when m is within 1e-9 of 0 or 1. In the + * latter case with m close to 1, the approximation applies + * only for phi < pi/2. + * + * ACCURACY: + * + * Tested at random points with u between 0 and 10, m between + * 0 and 1. + * + * Absolute error (* = relative error): + * arithmetic function # trials peak rms + * IEEE phi 10000 9.2e-16* 1.4e-16* + * IEEE sn 50000 4.1e-15 4.6e-16 + * IEEE cn 40000 3.6e-15 4.4e-16 + * IEEE dn 10000 1.3e-12 1.8e-14 + * + * Peak error observed in consistency check using addition + * theorem for sn(u+v) was 4e-16 (absolute). Also tested by + * the above relation to the incomplete elliptic integral. + * Accuracy deteriorates when u is large. + * + */ + +/* ellpj.c */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +/* Scipy changes: + * - 07-18-2016: improve evaluation of dn near quarter periods + */ + +#include "mconf.h" +extern double MACHEP; + +int ellpj(double u, double m, double *sn, double *cn, double *dn, double *ph) +{ + double ai, b, phi, t, twon, dnfac; + double a[9], c[9]; + int i; + + /* Check for special cases */ + if (m < 0.0 || m > 1.0 || cephes_isnan(m)) { + sf_error("ellpj", SF_ERROR_DOMAIN, NULL); + *sn = NAN; + *cn = NAN; + *ph = NAN; + *dn = NAN; + return (-1); + } + if (m < 1.0e-9) { + t = sin(u); + b = cos(u); + ai = 0.25 * m * (u - t * b); + *sn = t - ai * b; + *cn = b + ai * t; + *ph = u - ai; + *dn = 1.0 - 0.5 * m * t * t; + return (0); + } + if (m >= 0.9999999999) { + ai = 0.25 * (1.0 - m); + b = cosh(u); + t = tanh(u); + phi = 1.0 / b; + twon = b * sinh(u); + *sn = t + ai * (twon - u) / (b * b); + *ph = 2.0 * atan(exp(u)) - M_PI_2 + ai * (twon - u) / b; + ai *= t * phi; + *cn = phi - ai * (twon - u); + *dn = phi + ai * (twon + u); + return (0); + } + + /* A. G. M. scale. See DLMF 22.20(ii) */ + a[0] = 1.0; + b = sqrt(1.0 - m); + c[0] = sqrt(m); + twon = 1.0; + i = 0; + + while (fabs(c[i] / a[i]) > MACHEP) { + if (i > 7) { + sf_error("ellpj", SF_ERROR_OVERFLOW, NULL); + goto done; + } + ai = a[i]; + ++i; + c[i] = (ai - b) / 2.0; + t = sqrt(ai * b); + a[i] = (ai + b) / 2.0; + b = t; + twon *= 2.0; + } + + done: + /* backward recurrence */ + phi = twon * a[i] * u; + do { + t = c[i] * sin(phi) / a[i]; + b = phi; + phi = (asin(t) + phi) / 2.0; + } + while (--i); + + *sn = sin(phi); + t = cos(phi); + *cn = t; + dnfac = cos(phi - b); + /* See discussion after DLMF 22.20.5 */ + if (fabs(dnfac) < 0.1) { + *dn = sqrt(1 - m*(*sn)*(*sn)); + } + else { + *dn = t / dnfac; + } + *ph = phi; + return (0); +} diff --git a/gtsam/3rdparty/cephes/cephes/ellpk.c b/gtsam/3rdparty/cephes/cephes/ellpk.c new file mode 100644 index 000000000..3842a7403 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ellpk.c @@ -0,0 +1,124 @@ +/* ellpk.c + * + * Complete elliptic integral of the first kind + * + * + * + * SYNOPSIS: + * + * double m1, y, ellpk(); + * + * y = ellpk( m1 ); + * + * + * + * DESCRIPTION: + * + * Approximates the integral + * + * + * + * pi/2 + * - + * | | + * | dt + * K(m) = | ------------------ + * | 2 + * | | sqrt( 1 - m sin t ) + * - + * 0 + * + * where m = 1 - m1, using the approximation + * + * P(x) - log x Q(x). + * + * The argument m1 is used internally rather than m so that the logarithmic + * singularity at m = 1 will be shifted to the origin; this + * preserves maximum accuracy. + * + * K(0) = pi/2. + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,1 30000 2.5e-16 6.8e-17 + * + * ERROR MESSAGES: + * + * message condition value returned + * ellpk domain x<0, x>1 0.0 + * + */ + +/* ellpk.c */ + + +/* + * Cephes Math Library, Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +static double P[] = { + 1.37982864606273237150E-4, + 2.28025724005875567385E-3, + 7.97404013220415179367E-3, + 9.85821379021226008714E-3, + 6.87489687449949877925E-3, + 6.18901033637687613229E-3, + 8.79078273952743772254E-3, + 1.49380448916805252718E-2, + 3.08851465246711995998E-2, + 9.65735902811690126535E-2, + 1.38629436111989062502E0 +}; + +static double Q[] = { + 2.94078955048598507511E-5, + 9.14184723865917226571E-4, + 5.94058303753167793257E-3, + 1.54850516649762399335E-2, + 2.39089602715924892727E-2, + 3.01204715227604046988E-2, + 3.73774314173823228969E-2, + 4.88280347570998239232E-2, + 7.03124996963957469739E-2, + 1.24999999999870820058E-1, + 4.99999999999999999821E-1 +}; + +static double C1 = 1.3862943611198906188E0; /* log(4) */ + +extern double MACHEP; + +double ellpk(double x) +{ + + if (x < 0.0) { + sf_error("ellpk", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + if (x > 1.0) { + if (cephes_isinf(x)) { + return 0.0; + } + return ellpk(1/x)/sqrt(x); + } + + if (x > MACHEP) { + return (polevl(x, P, 10) - log(x) * polevl(x, Q, 10)); + } + else { + if (x == 0.0) { + sf_error("ellpk", SF_ERROR_SINGULAR, NULL); + return (INFINITY); + } + else { + return (C1 - 0.5 * log(x)); + } + } +} diff --git a/gtsam/3rdparty/cephes/cephes/erfinv.c b/gtsam/3rdparty/cephes/cephes/erfinv.c new file mode 100644 index 000000000..f7f49284c --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/erfinv.c @@ -0,0 +1,78 @@ +/* + * mconf configures NANS, INFINITYs etc. for cephes and includes some standard + * headers. Although erfinv and erfcinv are not defined in cephes, erf and erfc + * are. We want to keep the behaviour consistent for the inverse functions and + * so need to include mconf. + */ +#include "mconf.h" + +/* + * Inverse of the error function. + * + * Computes the inverse of the error function on the restricted domain + * -1 < y < 1. This restriction ensures the existence of a unique result + * such that erf(erfinv(y)) = y. + */ +double erfinv(double y) { + const double domain_lb = -1; + const double domain_ub = 1; + + const double thresh = 1e-7; + + /* + * For small arguments, use the Taylor expansion + * erf(y) = 2/\sqrt{\pi} (y - y^3 / 3 + O(y^5)), y\to 0 + * where we only retain the linear term. + * Otherwise, y + 1 loses precision for |y| << 1. + */ + if ((-thresh < y) && (y < thresh)){ + return y / M_2_SQRTPI; + } + if ((domain_lb < y) && (y < domain_ub)) { + return ndtri(0.5 * (y+1)) * M_SQRT1_2; + } + else if (y == domain_lb) { + return -INFINITY; + } + else if (y == domain_ub) { + return INFINITY; + } + else if (cephes_isnan(y)) { + sf_error("erfinv", SF_ERROR_DOMAIN, NULL); + return y; + } + else { + sf_error("erfinv", SF_ERROR_DOMAIN, NULL); + return NAN; + } +} + +/* + * Inverse of the complementary error function. + * + * Computes the inverse of the complimentary error function on the restricted + * domain 0 < y < 2. This restriction ensures the existence of a unique result + * such that erfc(erfcinv(y)) = y. + */ +double erfcinv(double y) { + const double domain_lb = 0; + const double domain_ub = 2; + + if ((domain_lb < y) && (y < domain_ub)) { + return -ndtri(0.5 * y) * M_SQRT1_2; + } + else if (y == domain_lb) { + return INFINITY; + } + else if (y == domain_ub) { + return -INFINITY; + } + else if (cephes_isnan(y)) { + sf_error("erfcinv", SF_ERROR_DOMAIN, NULL); + return y; + } + else { + sf_error("erfcinv", SF_ERROR_DOMAIN, NULL); + return NAN; + } +} diff --git a/gtsam/3rdparty/cephes/cephes/exp10.c b/gtsam/3rdparty/cephes/cephes/exp10.c new file mode 100644 index 000000000..0a71d3c52 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/exp10.c @@ -0,0 +1,115 @@ +/* exp10.c + * + * Base 10 exponential function + * (Common antilogarithm) + * + * + * + * SYNOPSIS: + * + * double x, y, exp10(); + * + * y = exp10( x ); + * + * + * + * DESCRIPTION: + * + * Returns 10 raised to the x power. + * + * Range reduction is accomplished by expressing the argument + * as 10**x = 2**n 10**f, with |f| < 0.5 log10(2). + * The Pade' form + * + * 1 + 2x P(x**2)/( Q(x**2) - P(x**2) ) + * + * is used to approximate 10**f. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -307,+307 30000 2.2e-16 5.5e-17 + * + * ERROR MESSAGES: + * + * message condition value returned + * exp10 underflow x < -MAXL10 0.0 + * exp10 overflow x > MAXL10 INFINITY + * + * IEEE arithmetic: MAXL10 = 308.2547155599167. + * + */ + +/* + * Cephes Math Library Release 2.2: January, 1991 + * Copyright 1984, 1991 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + + +#include "mconf.h" + +static double P[] = { + 4.09962519798587023075E-2, + 1.17452732554344059015E1, + 4.06717289936872725516E2, + 2.39423741207388267439E3, +}; + +static double Q[] = { + /* 1.00000000000000000000E0, */ + 8.50936160849306532625E1, + 1.27209271178345121210E3, + 2.07960819286001865907E3, +}; + +/* static double LOG102 = 3.01029995663981195214e-1; */ +static double LOG210 = 3.32192809488736234787e0; +static double LG102A = 3.01025390625000000000E-1; +static double LG102B = 4.60503898119521373889E-6; + +/* static double MAXL10 = 38.230809449325611792; */ +static double MAXL10 = 308.2547155599167; + +double exp10(double x) +{ + double px, xx; + short n; + + if (cephes_isnan(x)) + return (x); + if (x > MAXL10) { + return (INFINITY); + } + + if (x < -MAXL10) { /* Would like to use MINLOG but can't */ + sf_error("exp10", SF_ERROR_UNDERFLOW, NULL); + return (0.0); + } + + /* Express 10**x = 10**g 2**n + * = 10**g 10**( n log10(2) ) + * = 10**( g + n log10(2) ) + */ + px = floor(LOG210 * x + 0.5); + n = px; + x -= px * LG102A; + x -= px * LG102B; + + /* rational approximation for exponential + * of the fractional part: + * 10**x = 1 + 2x P(x**2)/( Q(x**2) - P(x**2) ) + */ + xx = x * x; + px = x * polevl(xx, P, 3); + x = px / (p1evl(xx, Q, 3) - px); + x = 1.0 + ldexp(x, 1); + + /* multiply by power of 2 */ + x = ldexp(x, n); + + return (x); +} diff --git a/gtsam/3rdparty/cephes/cephes/exp2.c b/gtsam/3rdparty/cephes/cephes/exp2.c new file mode 100644 index 000000000..14911f59c --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/exp2.c @@ -0,0 +1,108 @@ +/* exp2.c + * + * Base 2 exponential function + * + * + * + * SYNOPSIS: + * + * double x, y, exp2(); + * + * y = exp2( x ); + * + * + * + * DESCRIPTION: + * + * Returns 2 raised to the x power. + * + * Range reduction is accomplished by separating the argument + * into an integer k and fraction f such that + * x k f + * 2 = 2 2. + * + * A Pade' form + * + * 1 + 2x P(x**2) / (Q(x**2) - x P(x**2) ) + * + * approximates 2**x in the basic range [-0.5, 0.5]. + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -1022,+1024 30000 1.8e-16 5.4e-17 + * + * + * See exp.c for comments on error amplification. + * + * + * ERROR MESSAGES: + * + * message condition value returned + * exp underflow x < -MAXL2 0.0 + * exp overflow x > MAXL2 INFINITY + * + * For IEEE arithmetic, MAXL2 = 1024. + */ + + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1995 by Stephen L. Moshier + */ + + + +#include "mconf.h" + +static double P[] = { + 2.30933477057345225087E-2, + 2.02020656693165307700E1, + 1.51390680115615096133E3, +}; + +static double Q[] = { + /* 1.00000000000000000000E0, */ + 2.33184211722314911771E2, + 4.36821166879210612817E3, +}; + +#define MAXL2 1024.0 +#define MINL2 -1024.0 + +double exp2(double x) +{ + double px, xx; + short n; + + if (cephes_isnan(x)) + return (x); + if (x > MAXL2) { + return (INFINITY); + } + + if (x < MINL2) { + return (0.0); + } + + xx = x; /* save x */ + /* separate into integer and fractional parts */ + px = floor(x + 0.5); + n = px; + x = x - px; + + /* rational approximation + * exp2(x) = 1 + 2xP(xx)/(Q(xx) - P(xx)) + * where xx = x**2 + */ + xx = x * x; + px = x * polevl(xx, P, 2); + x = px / (p1evl(xx, Q, 2) - px); + x = 1.0 + ldexp(x, 1); + + /* scale by power of 2 */ + x = ldexp(x, n); + return (x); +} diff --git a/gtsam/3rdparty/cephes/cephes/expn.c b/gtsam/3rdparty/cephes/cephes/expn.c new file mode 100644 index 000000000..2a6ee14c0 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/expn.c @@ -0,0 +1,224 @@ +/* expn.c + * + * Exponential integral En + * + * + * + * SYNOPSIS: + * + * int n; + * double x, y, expn(); + * + * y = expn( n, x ); + * + * + * + * DESCRIPTION: + * + * Evaluates the exponential integral + * + * inf. + * - + * | | -xt + * | e + * E (x) = | ---- dt. + * n | n + * | | t + * - + * 1 + * + * + * Both n and x must be nonnegative. + * + * The routine employs either a power series, a continued + * fraction, or an asymptotic formula depending on the + * relative values of n and x. + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 10000 1.7e-15 3.6e-16 + * + */ + +/* expn.c */ + +/* Cephes Math Library Release 1.1: March, 1985 + * Copyright 1985 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 */ + +/* Sources + * [1] NIST, "The Digital Library of Mathematical Functions", dlmf.nist.gov + */ + +/* Scipy changes: + * - 09-10-2016: improved asymptotic expansion for large n + */ + +#include "mconf.h" +#include "polevl.h" +#include "expn.h" + +#define EUL 0.57721566490153286060 +#define BIG 1.44115188075855872E+17 +extern double MACHEP, MAXLOG; + +static double expn_large_n(int, double); + + +double expn(int n, double x) +{ + double ans, r, t, yk, xk; + double pk, pkm1, pkm2, qk, qkm1, qkm2; + double psi, z; + int i, k; + static double big = BIG; + + if (isnan(x)) { + return NAN; + } + else if (n < 0 || x < 0) { + sf_error("expn", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (x > MAXLOG) { + return (0.0); + } + + if (x == 0.0) { + if (n < 2) { + sf_error("expn", SF_ERROR_SINGULAR, NULL); + return (INFINITY); + } + else { + return (1.0 / (n - 1.0)); + } + } + + if (n == 0) { + return (exp(-x) / x); + } + + /* Asymptotic expansion for large n, DLMF 8.20(ii) */ + if (n > 50) { + ans = expn_large_n(n, x); + goto done; + } + + if (x > 1.0) { + goto cfrac; + } + + /* Power series expansion, DLMF 8.19.8 */ + psi = -EUL - log(x); + for (i = 1; i < n; i++) { + psi = psi + 1.0 / i; + } + + z = -x; + xk = 0.0; + yk = 1.0; + pk = 1.0 - n; + if (n == 1) { + ans = 0.0; + } else { + ans = 1.0 / pk; + } + do { + xk += 1.0; + yk *= z / xk; + pk += 1.0; + if (pk != 0.0) { + ans += yk / pk; + } + if (ans != 0.0) + t = fabs(yk / ans); + else + t = 1.0; + } while (t > MACHEP); + k = xk; + t = n; + r = n - 1; + ans = (pow(z, r) * psi / Gamma(t)) - ans; + goto done; + + /* Continued fraction, DLMF 8.19.17 */ + cfrac: + k = 1; + pkm2 = 1.0; + qkm2 = x; + pkm1 = 1.0; + qkm1 = x + n; + ans = pkm1 / qkm1; + + do { + k += 1; + if (k & 1) { + yk = 1.0; + xk = n + (k - 1) / 2; + } else { + yk = x; + xk = k / 2; + } + pk = pkm1 * yk + pkm2 * xk; + qk = qkm1 * yk + qkm2 * xk; + if (qk != 0) { + r = pk / qk; + t = fabs((ans - r) / r); + ans = r; + } else { + t = 1.0; + } + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + if (fabs(pk) > big) { + pkm2 /= big; + pkm1 /= big; + qkm2 /= big; + qkm1 /= big; + } + } while (t > MACHEP); + + ans *= exp(-x); + + done: + return (ans); +} + + +/* Asymptotic expansion for large n, DLMF 8.20(ii) */ +static double expn_large_n(int n, double x) +{ + int k; + double p = n; + double lambda = x/p; + double multiplier = 1/p/(lambda + 1)/(lambda + 1); + double fac = 1; + double res = 1; /* A[0] = 1 */ + double expfac, term; + + expfac = exp(-lambda*p)/(lambda + 1)/p; + if (expfac == 0) { + sf_error("expn", SF_ERROR_UNDERFLOW, NULL); + return 0; + } + + /* Do the k = 1 term outside the loop since A[1] = 1 */ + fac *= multiplier; + res += fac; + + for (k = 2; k < nA; k++) { + fac *= multiplier; + term = fac*polevl(lambda, A[k], Adegs[k]); + res += term; + if (fabs(term) < MACHEP*fabs(res)) { + break; + } + } + + return expfac*res; +} diff --git a/gtsam/3rdparty/cephes/cephes/expn.h b/gtsam/3rdparty/cephes/cephes/expn.h new file mode 100644 index 000000000..8ced02687 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/expn.h @@ -0,0 +1,19 @@ +/* This file was automatically generated by _precompute/expn_asy.py. + * Do not edit it manually! + */ +#define nA 13 +static const double A0[] = {1.00000000000000000}; +static const double A1[] = {1.00000000000000000}; +static const double A2[] = {-2.00000000000000000, 1.00000000000000000}; +static const double A3[] = {6.00000000000000000, -8.00000000000000000, 1.00000000000000000}; +static const double A4[] = {-24.0000000000000000, 58.0000000000000000, -22.0000000000000000, 1.00000000000000000}; +static const double A5[] = {120.000000000000000, -444.000000000000000, 328.000000000000000, -52.0000000000000000, 1.00000000000000000}; +static const double A6[] = {-720.000000000000000, 3708.00000000000000, -4400.00000000000000, 1452.00000000000000, -114.000000000000000, 1.00000000000000000}; +static const double A7[] = {5040.00000000000000, -33984.0000000000000, 58140.0000000000000, -32120.0000000000000, 5610.00000000000000, -240.000000000000000, 1.00000000000000000}; +static const double A8[] = {-40320.0000000000000, 341136.000000000000, -785304.000000000000, 644020.000000000000, -195800.000000000000, 19950.0000000000000, -494.000000000000000, 1.00000000000000000}; +static const double A9[] = {362880.000000000000, -3733920.00000000000, 11026296.0000000000, -12440064.0000000000, 5765500.00000000000, -1062500.00000000000, 67260.0000000000000, -1004.00000000000000, 1.00000000000000000}; +static const double A10[] = {-3628800.00000000000, 44339040.0000000000, -162186912.000000000, 238904904.000000000, -155357384.000000000, 44765000.0000000000, -5326160.00000000000, 218848.000000000000, -2026.00000000000000, 1.00000000000000000}; +static const double A11[] = {39916800.0000000000, -568356480.000000000, 2507481216.00000000, -4642163952.00000000, 4002695088.00000000, -1648384304.00000000, 314369720.000000000, -25243904.0000000000, 695038.000000000000, -4072.00000000000000, 1.00000000000000000}; +static const double A12[] = {-479001600.000000000, 7827719040.00000000, -40788301824.0000000, 92199790224.0000000, -101180433024.000000, 56041398784.0000000, -15548960784.0000000, 2051482776.00000000, -114876376.000000000, 2170626.00000000000, -8166.00000000000000, 1.00000000000000000}; +static const double *A[] = {A0, A1, A2, A3, A4, A5, A6, A7, A8, A9, A10, A11, A12}; +static const int Adegs[] = {0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}; diff --git a/gtsam/3rdparty/cephes/cephes/fdtr.c b/gtsam/3rdparty/cephes/cephes/fdtr.c new file mode 100644 index 000000000..9c119ed8f --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/fdtr.c @@ -0,0 +1,216 @@ +/* fdtr.c + * + * F distribution + * + * + * + * SYNOPSIS: + * + * double df1, df2; + * double x, y, fdtr(); + * + * y = fdtr( df1, df2, x ); + * + * DESCRIPTION: + * + * Returns the area from zero to x under the F density + * function (also known as Snedcor's density or the + * variance ratio density). This is the density + * of x = (u1/df1)/(u2/df2), where u1 and u2 are random + * variables having Chi square distributions with df1 + * and df2 degrees of freedom, respectively. + * + * The incomplete beta integral is used, according to the + * formula + * + * P(x) = incbet( df1/2, df2/2, (df1*x/(df2 + df1*x) ). + * + * + * The arguments a and b are greater than zero, and x is + * nonnegative. + * + * ACCURACY: + * + * Tested at random points (a,b,x). + * + * x a,b Relative error: + * arithmetic domain domain # trials peak rms + * IEEE 0,1 0,100 100000 9.8e-15 1.7e-15 + * IEEE 1,5 0,100 100000 6.5e-15 3.5e-16 + * IEEE 0,1 1,10000 100000 2.2e-11 3.3e-12 + * IEEE 1,5 1,10000 100000 1.1e-11 1.7e-13 + * See also incbet.c. + * + * + * ERROR MESSAGES: + * + * message condition value returned + * fdtr domain a<0, b<0, x<0 0.0 + * + */ + +/* fdtrc() + * + * Complemented F distribution + * + * + * + * SYNOPSIS: + * + * double df1, df2; + * double x, y, fdtrc(); + * + * y = fdtrc( df1, df2, x ); + * + * DESCRIPTION: + * + * Returns the area from x to infinity under the F density + * function (also known as Snedcor's density or the + * variance ratio density). + * + * + * inf. + * - + * 1 | | a-1 b-1 + * 1-P(x) = ------ | t (1-t) dt + * B(a,b) | | + * - + * x + * + * + * The incomplete beta integral is used, according to the + * formula + * + * P(x) = incbet( df2/2, df1/2, (df2/(df2 + df1*x) ). + * + * + * ACCURACY: + * + * Tested at random points (a,b,x) in the indicated intervals. + * x a,b Relative error: + * arithmetic domain domain # trials peak rms + * IEEE 0,1 1,100 100000 3.7e-14 5.9e-16 + * IEEE 1,5 1,100 100000 8.0e-15 1.6e-15 + * IEEE 0,1 1,10000 100000 1.8e-11 3.5e-13 + * IEEE 1,5 1,10000 100000 2.0e-11 3.0e-12 + * See also incbet.c. + * + * ERROR MESSAGES: + * + * message condition value returned + * fdtrc domain a<0, b<0, x<0 0.0 + * + */ + +/* fdtri() + * + * Inverse of F distribution + * + * + * + * SYNOPSIS: + * + * double df1, df2; + * double x, p, fdtri(); + * + * x = fdtri( df1, df2, p ); + * + * DESCRIPTION: + * + * Finds the F density argument x such that the integral + * from -infinity to x of the F density is equal to the + * given probability p. + * + * This is accomplished using the inverse beta integral + * function and the relations + * + * z = incbi( df2/2, df1/2, p ) + * x = df2 (1-z) / (df1 z). + * + * Note: the following relations hold for the inverse of + * the uncomplemented F distribution: + * + * z = incbi( df1/2, df2/2, p ) + * x = df2 z / (df1 (1-z)). + * + * ACCURACY: + * + * Tested at random points (a,b,p). + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * For p between .001 and 1: + * IEEE 1,100 100000 8.3e-15 4.7e-16 + * IEEE 1,10000 100000 2.1e-11 1.4e-13 + * For p between 10^-6 and 10^-3: + * IEEE 1,100 50000 1.3e-12 8.4e-15 + * IEEE 1,10000 50000 3.0e-12 4.8e-14 + * See also fdtrc.c. + * + * ERROR MESSAGES: + * + * message condition value returned + * fdtri domain p <= 0 or p > 1 NaN + * v < 1 + * + */ + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + + +#include "mconf.h" + + +double fdtrc(double a, double b, double x) +{ + double w; + + if ((a <= 0.0) || (b <= 0.0) || (x < 0.0)) { + sf_error("fdtrc", SF_ERROR_DOMAIN, NULL); + return NAN; + } + w = b / (b + a * x); + return incbet(0.5 * b, 0.5 * a, w); +} + + +double fdtr(double a, double b, double x) +{ + double w; + + if ((a <= 0.0) || (b <= 0.0) || (x < 0.0)) { + sf_error("fdtr", SF_ERROR_DOMAIN, NULL); + return NAN; + } + w = a * x; + w = w / (b + w); + return incbet(0.5 * a, 0.5 * b, w); +} + + +double fdtri(double a, double b, double y) +{ + double w, x; + + if ((a <= 0.0) || (b <= 0.0) || (y <= 0.0) || (y > 1.0)) { + sf_error("fdtri", SF_ERROR_DOMAIN, NULL); + return NAN; + } + y = 1.0 - y; + /* Compute probability for x = 0.5. */ + w = incbet(0.5 * b, 0.5 * a, 0.5); + /* If that is greater than y, then the solution w < .5. + * Otherwise, solve at 1-y to remove cancellation in (b - b*w). */ + if (w > y || y < 0.001) { + w = incbi(0.5 * b, 0.5 * a, y); + x = (b - b * w) / (a * w); + } + else { + w = incbi(0.5 * a, 0.5 * b, 1.0 - y); + x = b * w / (a * (1.0 - w)); + } + return x; +} diff --git a/gtsam/3rdparty/cephes/cephes/fresnl.c b/gtsam/3rdparty/cephes/cephes/fresnl.c new file mode 100644 index 000000000..50620fa2e --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/fresnl.c @@ -0,0 +1,219 @@ +/* fresnl.c + * + * Fresnel integral + * + * + * + * SYNOPSIS: + * + * double x, S, C; + * void fresnl(); + * + * fresnl( x, _&S, _&C ); + * + * + * DESCRIPTION: + * + * Evaluates the Fresnel integrals + * + * x + * - + * | | + * C(x) = | cos(pi/2 t**2) dt, + * | | + * - + * 0 + * + * x + * - + * | | + * S(x) = | sin(pi/2 t**2) dt. + * | | + * - + * 0 + * + * + * The integrals are evaluated by a power series for x < 1. + * For x >= 1 auxiliary functions f(x) and g(x) are employed + * such that + * + * C(x) = 0.5 + f(x) sin( pi/2 x**2 ) - g(x) cos( pi/2 x**2 ) + * S(x) = 0.5 - f(x) cos( pi/2 x**2 ) - g(x) sin( pi/2 x**2 ) + * + * + * + * ACCURACY: + * + * Relative error. + * + * Arithmetic function domain # trials peak rms + * IEEE S(x) 0, 10 10000 2.0e-15 3.2e-16 + * IEEE C(x) 0, 10 10000 1.8e-15 3.3e-16 + */ + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +/* S(x) for small x */ +static double sn[6] = { + -2.99181919401019853726E3, + 7.08840045257738576863E5, + -6.29741486205862506537E7, + 2.54890880573376359104E9, + -4.42979518059697779103E10, + 3.18016297876567817986E11, +}; + +static double sd[6] = { + /* 1.00000000000000000000E0, */ + 2.81376268889994315696E2, + 4.55847810806532581675E4, + 5.17343888770096400730E6, + 4.19320245898111231129E8, + 2.24411795645340920940E10, + 6.07366389490084639049E11, +}; + +/* C(x) for small x */ +static double cn[6] = { + -4.98843114573573548651E-8, + 9.50428062829859605134E-6, + -6.45191435683965050962E-4, + 1.88843319396703850064E-2, + -2.05525900955013891793E-1, + 9.99999999999999998822E-1, +}; + +static double cd[7] = { + 3.99982968972495980367E-12, + 9.15439215774657478799E-10, + 1.25001862479598821474E-7, + 1.22262789024179030997E-5, + 8.68029542941784300606E-4, + 4.12142090722199792936E-2, + 1.00000000000000000118E0, +}; + +/* Auxiliary function f(x) */ +static double fn[10] = { + 4.21543555043677546506E-1, + 1.43407919780758885261E-1, + 1.15220955073585758835E-2, + 3.45017939782574027900E-4, + 4.63613749287867322088E-6, + 3.05568983790257605827E-8, + 1.02304514164907233465E-10, + 1.72010743268161828879E-13, + 1.34283276233062758925E-16, + 3.76329711269987889006E-20, +}; + +static double fd[10] = { + /* 1.00000000000000000000E0, */ + 7.51586398353378947175E-1, + 1.16888925859191382142E-1, + 6.44051526508858611005E-3, + 1.55934409164153020873E-4, + 1.84627567348930545870E-6, + 1.12699224763999035261E-8, + 3.60140029589371370404E-11, + 5.88754533621578410010E-14, + 4.52001434074129701496E-17, + 1.25443237090011264384E-20, +}; + +/* Auxiliary function g(x) */ +static double gn[11] = { + 5.04442073643383265887E-1, + 1.97102833525523411709E-1, + 1.87648584092575249293E-2, + 6.84079380915393090172E-4, + 1.15138826111884280931E-5, + 9.82852443688422223854E-8, + 4.45344415861750144738E-10, + 1.08268041139020870318E-12, + 1.37555460633261799868E-15, + 8.36354435630677421531E-19, + 1.86958710162783235106E-22, +}; + +static double gd[11] = { + /* 1.00000000000000000000E0, */ + 1.47495759925128324529E0, + 3.37748989120019970451E-1, + 2.53603741420338795122E-2, + 8.14679107184306179049E-4, + 1.27545075667729118702E-5, + 1.04314589657571990585E-7, + 4.60680728146520428211E-10, + 1.10273215066240270757E-12, + 1.38796531259578871258E-15, + 8.39158816283118707363E-19, + 1.86958710162783236342E-22, +}; + +extern double MACHEP; + +int fresnl(double xxa, double *ssa, double *cca) +{ + double f, g, cc, ss, c, s, t, u; + double x, x2; + + if (cephes_isinf(xxa)) { + cc = 0.5; + ss = 0.5; + goto done; + } + + x = fabs(xxa); + x2 = x * x; + if (x2 < 2.5625) { + t = x2 * x2; + ss = x * x2 * polevl(t, sn, 5) / p1evl(t, sd, 6); + cc = x * polevl(t, cn, 5) / polevl(t, cd, 6); + goto done; + } + + if (x > 36974.0) { + /* + * http://functions.wolfram.com/GammaBetaErf/FresnelC/06/02/ + * http://functions.wolfram.com/GammaBetaErf/FresnelS/06/02/ + */ + cc = 0.5 + 1/(M_PI*x) * sin(M_PI*x*x/2); + ss = 0.5 - 1/(M_PI*x) * cos(M_PI*x*x/2); + goto done; + } + + + /* Asymptotic power series auxiliary functions + * for large argument + */ + x2 = x * x; + t = M_PI * x2; + u = 1.0 / (t * t); + t = 1.0 / t; + f = 1.0 - u * polevl(u, fn, 9) / p1evl(u, fd, 10); + g = t * polevl(u, gn, 10) / p1evl(u, gd, 11); + + t = M_PI_2 * x2; + c = cos(t); + s = sin(t); + t = M_PI * x; + cc = 0.5 + (f * s - g * c) / t; + ss = 0.5 - (f * c + g * s) / t; + + done: + if (xxa < 0.0) { + cc = -cc; + ss = -ss; + } + + *cca = cc; + *ssa = ss; + return (0); +} diff --git a/gtsam/3rdparty/cephes/cephes/gamma.c b/gtsam/3rdparty/cephes/cephes/gamma.c new file mode 100644 index 000000000..2a61defed --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/gamma.c @@ -0,0 +1,364 @@ +/* + * Gamma function + * + * + * + * SYNOPSIS: + * + * double x, y, Gamma(); + * + * y = Gamma( x ); + * + * + * + * DESCRIPTION: + * + * Returns Gamma function of the argument. The result is + * correctly signed. + * + * Arguments |x| <= 34 are reduced by recurrence and the function + * approximated by a rational function of degree 6/7 in the + * interval (2,3). Large arguments are handled by Stirling's + * formula. Large negative arguments are made positive using + * a reflection formula. + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -170,-33 20000 2.3e-15 3.3e-16 + * IEEE -33, 33 20000 9.4e-16 2.2e-16 + * IEEE 33, 171.6 20000 2.3e-15 3.2e-16 + * + * Error for arguments outside the test range will be larger + * owing to error amplification by the exponential function. + * + */ + +/* lgam() + * + * Natural logarithm of Gamma function + * + * + * + * SYNOPSIS: + * + * double x, y, lgam(); + * + * y = lgam( x ); + * + * + * + * DESCRIPTION: + * + * Returns the base e (2.718...) logarithm of the absolute + * value of the Gamma function of the argument. + * + * For arguments greater than 13, the logarithm of the Gamma + * function is approximated by the logarithmic version of + * Stirling's formula using a polynomial approximation of + * degree 4. Arguments between -33 and +33 are reduced by + * recurrence to the interval [2,3] of a rational approximation. + * The cosecant reflection formula is employed for arguments + * less than -33. + * + * Arguments greater than MAXLGM return INFINITY and an error + * message. MAXLGM = 2.556348e305 for IEEE arithmetic. + * + * + * + * ACCURACY: + * + * + * arithmetic domain # trials peak rms + * IEEE 0, 3 28000 5.4e-16 1.1e-16 + * IEEE 2.718, 2.556e305 40000 3.5e-16 8.3e-17 + * The error criterion was relative when the function magnitude + * was greater than one but absolute when it was less than one. + * + * The following test used the relative error criterion, though + * at certain points the relative error could be much higher than + * indicated. + * IEEE -200, -4 10000 4.8e-16 1.3e-16 + * + */ + +/* + * Cephes Math Library Release 2.2: July, 1992 + * Copyright 1984, 1987, 1989, 1992 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + + +#include "mconf.h" + +static double P[] = { + 1.60119522476751861407E-4, + 1.19135147006586384913E-3, + 1.04213797561761569935E-2, + 4.76367800457137231464E-2, + 2.07448227648435975150E-1, + 4.94214826801497100753E-1, + 9.99999999999999996796E-1 +}; + +static double Q[] = { + -2.31581873324120129819E-5, + 5.39605580493303397842E-4, + -4.45641913851797240494E-3, + 1.18139785222060435552E-2, + 3.58236398605498653373E-2, + -2.34591795718243348568E-1, + 7.14304917030273074085E-2, + 1.00000000000000000320E0 +}; + +#define MAXGAM 171.624376956302725 +static double LOGPI = 1.14472988584940017414; + +/* Stirling's formula for the Gamma function */ +static double STIR[5] = { + 7.87311395793093628397E-4, + -2.29549961613378126380E-4, + -2.68132617805781232825E-3, + 3.47222221605458667310E-3, + 8.33333333333482257126E-2, +}; + +#define MAXSTIR 143.01608 +static double SQTPI = 2.50662827463100050242E0; + +extern double MAXLOG; +static double stirf(double); + +/* Gamma function computed by Stirling's formula. + * The polynomial STIR is valid for 33 <= x <= 172. + */ +static double stirf(double x) +{ + double y, w, v; + + if (x >= MAXGAM) { + return (INFINITY); + } + w = 1.0 / x; + w = 1.0 + w * polevl(w, STIR, 4); + y = exp(x); + if (x > MAXSTIR) { /* Avoid overflow in pow() */ + v = pow(x, 0.5 * x - 0.25); + y = v * (v / y); + } + else { + y = pow(x, x - 0.5) / y; + } + y = SQTPI * y * w; + return (y); +} + + +double Gamma(double x) +{ + double p, q, z; + int i; + int sgngam = 1; + + if (!cephes_isfinite(x)) { + return x; + } + q = fabs(x); + + if (q > 33.0) { + if (x < 0.0) { + p = floor(q); + if (p == q) { + gamnan: + sf_error("Gamma", SF_ERROR_OVERFLOW, NULL); + return (INFINITY); + } + i = p; + if ((i & 1) == 0) + sgngam = -1; + z = q - p; + if (z > 0.5) { + p += 1.0; + z = q - p; + } + z = q * sin(M_PI * z); + if (z == 0.0) { + return (sgngam * INFINITY); + } + z = fabs(z); + z = M_PI / (z * stirf(q)); + } + else { + z = stirf(x); + } + return (sgngam * z); + } + + z = 1.0; + while (x >= 3.0) { + x -= 1.0; + z *= x; + } + + while (x < 0.0) { + if (x > -1.E-9) + goto small; + z /= x; + x += 1.0; + } + + while (x < 2.0) { + if (x < 1.e-9) + goto small; + z /= x; + x += 1.0; + } + + if (x == 2.0) + return (z); + + x -= 2.0; + p = polevl(x, P, 6); + q = polevl(x, Q, 7); + return (z * p / q); + + small: + if (x == 0.0) { + goto gamnan; + } + else + return (z / ((1.0 + 0.5772156649015329 * x) * x)); +} + + + +/* A[]: Stirling's formula expansion of log Gamma + * B[], C[]: log Gamma function between 2 and 3 + */ +static double A[] = { + 8.11614167470508450300E-4, + -5.95061904284301438324E-4, + 7.93650340457716943945E-4, + -2.77777777730099687205E-3, + 8.33333333333331927722E-2 +}; + +static double B[] = { + -1.37825152569120859100E3, + -3.88016315134637840924E4, + -3.31612992738871184744E5, + -1.16237097492762307383E6, + -1.72173700820839662146E6, + -8.53555664245765465627E5 +}; + +static double C[] = { + /* 1.00000000000000000000E0, */ + -3.51815701436523470549E2, + -1.70642106651881159223E4, + -2.20528590553854454839E5, + -1.13933444367982507207E6, + -2.53252307177582951285E6, + -2.01889141433532773231E6 +}; + +/* log( sqrt( 2*pi ) ) */ +static double LS2PI = 0.91893853320467274178; + +#define MAXLGM 2.556348e305 + + +/* Logarithm of Gamma function */ +double lgam(double x) +{ + int sign; + return lgam_sgn(x, &sign); +} + +double lgam_sgn(double x, int *sign) +{ + double p, q, u, w, z; + int i; + + *sign = 1; + + if (!cephes_isfinite(x)) + return x; + + if (x < -34.0) { + q = -x; + w = lgam_sgn(q, sign); + p = floor(q); + if (p == q) { + lgsing: + sf_error("lgam", SF_ERROR_SINGULAR, NULL); + return (INFINITY); + } + i = p; + if ((i & 1) == 0) + *sign = -1; + else + *sign = 1; + z = q - p; + if (z > 0.5) { + p += 1.0; + z = p - q; + } + z = q * sin(M_PI * z); + if (z == 0.0) + goto lgsing; + /* z = log(M_PI) - log( z ) - w; */ + z = LOGPI - log(z) - w; + return (z); + } + + if (x < 13.0) { + z = 1.0; + p = 0.0; + u = x; + while (u >= 3.0) { + p -= 1.0; + u = x + p; + z *= u; + } + while (u < 2.0) { + if (u == 0.0) + goto lgsing; + z /= u; + p += 1.0; + u = x + p; + } + if (z < 0.0) { + *sign = -1; + z = -z; + } + else + *sign = 1; + if (u == 2.0) + return (log(z)); + p -= 2.0; + x = x + p; + p = x * polevl(x, B, 5) / p1evl(x, C, 6); + return (log(z) + p); + } + + if (x > MAXLGM) { + return (*sign * INFINITY); + } + + q = (x - 0.5) * log(x) - x + LS2PI; + if (x > 1.0e8) + return (q); + + p = 1.0 / (x * x); + if (x >= 1000.0) + q += ((7.9365079365079365079365e-4 * p + - 2.7777777777777777777778e-3) * p + + 0.0833333333333333333333) / x; + else + q += polevl(p, A, 4) / x; + return (q); +} diff --git a/gtsam/3rdparty/cephes/cephes/gammasgn.c b/gtsam/3rdparty/cephes/cephes/gammasgn.c new file mode 100644 index 000000000..9d74318ff --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/gammasgn.c @@ -0,0 +1,25 @@ +#include "mconf.h" + +double gammasgn(double x) +{ + double fx; + + if (isnan(x)) { + return x; + } + if (x > 0) { + return 1.0; + } + else { + fx = floor(x); + if (x - fx == 0.0) { + return 0.0; + } + else if ((int)fx % 2) { + return -1.0; + } + else { + return 1.0; + } + } +} diff --git a/gtsam/3rdparty/cephes/cephes/gdtr.c b/gtsam/3rdparty/cephes/cephes/gdtr.c new file mode 100644 index 000000000..597c8d4d9 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/gdtr.c @@ -0,0 +1,132 @@ +/* gdtr.c + * + * Gamma distribution function + * + * + * + * SYNOPSIS: + * + * double a, b, x, y, gdtr(); + * + * y = gdtr( a, b, x ); + * + * + * + * DESCRIPTION: + * + * Returns the integral from zero to x of the Gamma probability + * density function: + * + * + * x + * b - + * a | | b-1 -at + * y = ----- | t e dt + * - | | + * | (b) - + * 0 + * + * The incomplete Gamma integral is used, according to the + * relation + * + * y = igam( b, ax ). + * + * + * ACCURACY: + * + * See igam(). + * + * ERROR MESSAGES: + * + * message condition value returned + * gdtr domain x < 0 0.0 + * + */ + /* gdtrc.c + * + * Complemented Gamma distribution function + * + * + * + * SYNOPSIS: + * + * double a, b, x, y, gdtrc(); + * + * y = gdtrc( a, b, x ); + * + * + * + * DESCRIPTION: + * + * Returns the integral from x to infinity of the Gamma + * probability density function: + * + * + * inf. + * b - + * a | | b-1 -at + * y = ----- | t e dt + * - | | + * | (b) - + * x + * + * The incomplete Gamma integral is used, according to the + * relation + * + * y = igamc( b, ax ). + * + * + * ACCURACY: + * + * See igamc(). + * + * ERROR MESSAGES: + * + * message condition value returned + * gdtrc domain x < 0 0.0 + * + */ + +/* gdtr() */ + + +/* + * Cephes Math Library Release 2.3: March,1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + + +double gdtr(double a, double b, double x) +{ + + if (x < 0.0) { + sf_error("gdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + return (igam(b, a * x)); +} + + +double gdtrc(double a, double b, double x) +{ + + if (x < 0.0) { + sf_error("gdtrc", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + return (igamc(b, a * x)); +} + + +double gdtri(double a, double b, double y) +{ + + if ((y < 0.0) || (y > 1.0) || (a <= 0.0) || (b < 0.0)) { + sf_error("gdtri", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + return (igamci(b, 1.0 - y) / a); +} diff --git a/gtsam/3rdparty/cephes/cephes/hyp2f1.c b/gtsam/3rdparty/cephes/cephes/hyp2f1.c new file mode 100644 index 000000000..7f0a84d02 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/hyp2f1.c @@ -0,0 +1,569 @@ +/* hyp2f1.c + * + * Gauss hypergeometric function F + * 2 1 + * + * + * SYNOPSIS: + * + * double a, b, c, x, y, hyp2f1(); + * + * y = hyp2f1( a, b, c, x ); + * + * + * DESCRIPTION: + * + * + * hyp2f1( a, b, c, x ) = F ( a, b; c; x ) + * 2 1 + * + * inf. + * - a(a+1)...(a+k) b(b+1)...(b+k) k+1 + * = 1 + > ----------------------------- x . + * - c(c+1)...(c+k) (k+1)! + * k = 0 + * + * Cases addressed are + * Tests and escapes for negative integer a, b, or c + * Linear transformation if c - a or c - b negative integer + * Special case c = a or c = b + * Linear transformation for x near +1 + * Transformation for x < -0.5 + * Psi function expansion if x > 0.5 and c - a - b integer + * Conditionally, a recurrence on c to make c-a-b > 0 + * + * x < -1 AMS 15.3.7 transformation applied (Travis Oliphant) + * valid for b,a,c,(b-a) != integer and (c-a),(c-b) != negative integer + * + * x >= 1 is rejected (unless special cases are present) + * + * The parameters a, b, c are considered to be integer + * valued if they are within 1.0e-14 of the nearest integer + * (1.0e-13 for IEEE arithmetic). + * + * ACCURACY: + * + * + * Relative error (-1 < x < 1): + * arithmetic domain # trials peak rms + * IEEE -1,7 230000 1.2e-11 5.2e-14 + * + * Several special cases also tested with a, b, c in + * the range -7 to 7. + * + * ERROR MESSAGES: + * + * A "partial loss of precision" message is printed if + * the internally estimated relative error exceeds 1^-12. + * A "singularity" message is printed on overflow or + * in cases not addressed (such as x < -1). + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1992, 2000 by Stephen L. Moshier + */ + +#include +#include +#include + +#include "mconf.h" + +#define EPS 1.0e-13 +#define EPS2 1.0e-10 + +#define ETHRESH 1.0e-12 + +#define MAX_ITERATIONS 10000 + +extern double MACHEP; + +/* hys2f1 and hyp2f1ra depend on each other, so we need this prototype */ +static double hyp2f1ra(double a, double b, double c, double x, double *loss); + +/* Defining power series expansion of Gauss hypergeometric function */ +/* The `loss` parameter estimates loss of significance */ +static double hys2f1(double a, double b, double c, double x, double *loss) { + double f, g, h, k, m, s, u, umax; + int i; + int ib, intflag = 0; + + if (fabs(b) > fabs(a)) { + /* Ensure that |a| > |b| ... */ + f = b; + b = a; + a = f; + } + + ib = round(b); + + if (fabs(b - ib) < EPS && ib <= 0 && fabs(b) < fabs(a)) { + /* .. except when `b` is a smaller negative integer */ + f = b; + b = a; + a = f; + intflag = 1; + } + + if ((fabs(a) > fabs(c) + 1 || intflag) && fabs(c - a) > 2 && fabs(a) > 2) { + /* |a| >> |c| implies that large cancellation error is to be expected. + * + * We try to reduce it with the recurrence relations + */ + return hyp2f1ra(a, b, c, x, loss); + } + + i = 0; + umax = 0.0; + f = a; + g = b; + h = c; + s = 1.0; + u = 1.0; + k = 0.0; + do { + if (fabs(h) < EPS) { + *loss = 1.0; + return INFINITY; + } + m = k + 1.0; + u = u * ((f + k) * (g + k) * x / ((h + k) * m)); + s += u; + k = fabs(u); /* remember largest term summed */ + if (k > umax) umax = k; + k = m; + if (++i > MAX_ITERATIONS) { /* should never happen */ + *loss = 1.0; + return (s); + } + } while (s == 0 || fabs(u / s) > MACHEP); + + /* return estimated relative error */ + *loss = (MACHEP * umax) / fabs(s) + (MACHEP * i); + + return (s); +} + +/* Apply transformations for |x| near 1 then call the power series */ +static double hyt2f1(double a, double b, double c, double x, double *loss) { + double p, q, r, s, t, y, w, d, err, err1; + double ax, id, d1, d2, e, y1; + int i, aid, sign; + + int ia, ib, neg_int_a = 0, neg_int_b = 0; + + ia = round(a); + ib = round(b); + + if (a <= 0 && fabs(a - ia) < EPS) { /* a is a negative integer */ + neg_int_a = 1; + } + + if (b <= 0 && fabs(b - ib) < EPS) { /* b is a negative integer */ + neg_int_b = 1; + } + + err = 0.0; + s = 1.0 - x; + if (x < -0.5 && !(neg_int_a || neg_int_b)) { + if (b > a) + y = pow(s, -a) * hys2f1(a, c - b, c, -x / s, &err); + + else + y = pow(s, -b) * hys2f1(c - a, b, c, -x / s, &err); + + goto done; + } + + d = c - a - b; + id = round(d); /* nearest integer to d */ + + if (x > 0.9 && !(neg_int_a || neg_int_b)) { + if (fabs(d - id) > EPS) { + int sgngam; + + /* test for integer c-a-b */ + /* Try the power series first */ + y = hys2f1(a, b, c, x, &err); + if (err < ETHRESH) goto done; + /* If power series fails, then apply AMS55 #15.3.6 */ + q = hys2f1(a, b, 1.0 - d, s, &err); + sign = 1; + w = lgam_sgn(d, &sgngam); + sign *= sgngam; + w -= lgam_sgn(c - a, &sgngam); + sign *= sgngam; + w -= lgam_sgn(c - b, &sgngam); + sign *= sgngam; + q *= sign * exp(w); + r = pow(s, d) * hys2f1(c - a, c - b, d + 1.0, s, &err1); + sign = 1; + w = lgam_sgn(-d, &sgngam); + sign *= sgngam; + w -= lgam_sgn(a, &sgngam); + sign *= sgngam; + w -= lgam_sgn(b, &sgngam); + sign *= sgngam; + r *= sign * exp(w); + y = q + r; + + q = fabs(q); /* estimate cancellation error */ + r = fabs(r); + if (q > r) r = q; + err += err1 + (MACHEP * r) / y; + + y *= gamma(c); + goto done; + } else { + /* Psi function expansion, AMS55 #15.3.10, #15.3.11, #15.3.12 + * + * Although AMS55 does not explicitly state it, this expansion fails + * for negative integer a or b, since the psi and Gamma functions + * involved have poles. + */ + + if (id >= 0.0) { + e = d; + d1 = d; + d2 = 0.0; + aid = id; + } else { + e = -d; + d1 = 0.0; + d2 = d; + aid = -id; + } + + ax = log(s); + + /* sum for t = 0 */ + y = psi(1.0) + psi(1.0 + e) - psi(a + d1) - psi(b + d1) - ax; + y /= gamma(e + 1.0); + + p = (a + d1) * (b + d1) * s / gamma(e + 2.0); /* Poch for t=1 */ + t = 1.0; + do { + r = psi(1.0 + t) + psi(1.0 + t + e) - psi(a + t + d1) - + psi(b + t + d1) - ax; + q = p * r; + y += q; + p *= s * (a + t + d1) / (t + 1.0); + p *= (b + t + d1) / (t + 1.0 + e); + t += 1.0; + if (t > MAX_ITERATIONS) { /* should never happen */ + sf_error("hyp2f1", SF_ERROR_SLOW, NULL); + *loss = 1.0; + return NAN; + } + } while (y == 0 || fabs(q / y) > EPS); + + if (id == 0.0) { + y *= gamma(c) / (gamma(a) * gamma(b)); + goto psidon; + } + + y1 = 1.0; + + if (aid == 1) goto nosum; + + t = 0.0; + p = 1.0; + for (i = 1; i < aid; i++) { + r = 1.0 - e + t; + p *= s * (a + t + d2) * (b + t + d2) / r; + t += 1.0; + p /= t; + y1 += p; + } + nosum: + p = gamma(c); + y1 *= gamma(e) * p / (gamma(a + d1) * gamma(b + d1)); + + y *= p / (gamma(a + d2) * gamma(b + d2)); + if ((aid & 1) != 0) y = -y; + + q = pow(s, id); /* s to the id power */ + if (id > 0.0) + y *= q; + else + y1 *= q; + + y += y1; + psidon: + goto done; + } + } + + /* Use defining power series if no special cases */ + y = hys2f1(a, b, c, x, &err); + +done: + *loss = err; + return (y); +} + +/* + 15.4.2 Abramowitz & Stegun. +*/ +static double hyp2f1_neg_c_equal_bc(double a, double b, double x) { + double k; + double collector = 1; + double sum = 1; + double collector_max = 1; + + if (!(fabs(b) < 1e5)) { + return NAN; + } + + for (k = 1; k <= -b; k++) { + collector *= (a + k - 1) * x / k; + collector_max = fmax(fabs(collector), collector_max); + sum += collector; + } + + if (1e-16 * (1 + collector_max / fabs(sum)) > 1e-7) { + return NAN; + } + + return sum; +} + +double hyp2f1(double a, double b, double c, double x) { + double d, d1, d2, e; + double p, q, r, s, y, ax; + double ia, ib, ic, id, err; + double t1; + int i, aid; + int neg_int_a = 0, neg_int_b = 0; + int neg_int_ca_or_cb = 0; + + err = 0.0; + ax = fabs(x); + s = 1.0 - x; + ia = round(a); /* nearest integer to a */ + ib = round(b); + + if (x == 0.0) { + return 1.0; + } + + d = c - a - b; + id = round(d); + + if ((a == 0 || b == 0) && c != 0) { + return 1.0; + } + + if (a <= 0 && fabs(a - ia) < EPS) { /* a is a negative integer */ + neg_int_a = 1; + } + + if (b <= 0 && fabs(b - ib) < EPS) { /* b is a negative integer */ + neg_int_b = 1; + } + + if (d <= -1 && !(fabs(d - id) > EPS && s < 0) && !(neg_int_a || neg_int_b)) { + return pow(s, d) * hyp2f1(c - a, c - b, c, x); + } + if (d <= 0 && x == 1 && !(neg_int_a || neg_int_b)) goto hypdiv; + + if (ax < 1.0 || x == -1.0) { + /* 2F1(a,b;b;x) = (1-x)**(-a) */ + if (fabs(b - c) < EPS) { /* b = c */ + if (neg_int_b) { + y = hyp2f1_neg_c_equal_bc(a, b, x); + } else { + y = pow(s, -a); /* s to the -a power */ + } + goto hypdon; + } + if (fabs(a - c) < EPS) { /* a = c */ + y = pow(s, -b); /* s to the -b power */ + goto hypdon; + } + } + + if (c <= 0.0) { + ic = round(c); /* nearest integer to c */ + if (fabs(c - ic) < EPS) { /* c is a negative integer */ + /* check if termination before explosion */ + if (neg_int_a && (ia > ic)) goto hypok; + if (neg_int_b && (ib > ic)) goto hypok; + goto hypdiv; + } + } + + if (neg_int_a || neg_int_b) /* function is a polynomial */ + goto hypok; + + t1 = fabs(b - a); + if (x < -2.0 && fabs(t1 - round(t1)) > EPS) { + /* This transform has a pole for b-a integer, and + * may produce large cancellation errors for |1/x| close 1 + */ + p = hyp2f1(a, 1 - c + a, 1 - b + a, 1.0 / x); + q = hyp2f1(b, 1 - c + b, 1 - a + b, 1.0 / x); + p *= pow(-x, -a); + q *= pow(-x, -b); + t1 = gamma(c); + s = t1 * gamma(b - a) / (gamma(b) * gamma(c - a)); + y = t1 * gamma(a - b) / (gamma(a) * gamma(c - b)); + return s * p + y * q; + } else if (x < -1.0) { + if (fabs(a) < fabs(b)) { + return pow(s, -a) * hyp2f1(a, c - b, c, x / (x - 1)); + } else { + return pow(s, -b) * hyp2f1(b, c - a, c, x / (x - 1)); + } + } + + if (ax > 1.0) /* series diverges */ + goto hypdiv; + + p = c - a; + ia = round(p); /* nearest integer to c-a */ + if ((ia <= 0.0) && (fabs(p - ia) < EPS)) /* negative int c - a */ + neg_int_ca_or_cb = 1; + + r = c - b; + ib = round(r); /* nearest integer to c-b */ + if ((ib <= 0.0) && (fabs(r - ib) < EPS)) /* negative int c - b */ + neg_int_ca_or_cb = 1; + + id = round(d); /* nearest integer to d */ + q = fabs(d - id); + + /* Thanks to Christian Burger + * for reporting a bug here. */ + if (fabs(ax - 1.0) < EPS) { /* |x| == 1.0 */ + if (x > 0.0) { + if (neg_int_ca_or_cb) { + if (d >= 0.0) + goto hypf; + else + goto hypdiv; + } + if (d <= 0.0) goto hypdiv; + y = gamma(c) * gamma(d) / (gamma(p) * gamma(r)); + goto hypdon; + } + if (d <= -1.0) goto hypdiv; + } + + /* Conditionally make d > 0 by recurrence on c + * AMS55 #15.2.27 + */ + if (d < 0.0) { + /* Try the power series first */ + y = hyt2f1(a, b, c, x, &err); + if (err < ETHRESH) goto hypdon; + /* Apply the recurrence if power series fails */ + err = 0.0; + aid = 2 - id; + e = c + aid; + d2 = hyp2f1(a, b, e, x); + d1 = hyp2f1(a, b, e + 1.0, x); + q = a + b + 1.0; + for (i = 0; i < aid; i++) { + r = e - 1.0; + y = (e * (r - (2.0 * e - q) * x) * d2 + (e - a) * (e - b) * x * d1) / + (e * r * s); + e = r; + d1 = d2; + d2 = y; + } + goto hypdon; + } + + if (neg_int_ca_or_cb) goto hypf; /* negative integer c-a or c-b */ + +hypok: + y = hyt2f1(a, b, c, x, &err); + +hypdon: + if (err > ETHRESH) { + sf_error("hyp2f1", SF_ERROR_LOSS, NULL); + /* printf( "Estimated err = %.2e\n", err ); */ + } + return (y); + + /* The transformation for c-a or c-b negative integer + * AMS55 #15.3.3 + */ +hypf: + y = pow(s, d) * hys2f1(c - a, c - b, c, x, &err); + goto hypdon; + + /* The alarm exit */ +hypdiv: + sf_error("hyp2f1", SF_ERROR_OVERFLOW, NULL); + return INFINITY; +} + +/* + * Evaluate hypergeometric function by two-term recurrence in `a`. + * + * This avoids some of the loss of precision in the strongly alternating + * hypergeometric series, and can be used to reduce the `a` and `b` parameters + * to smaller values. + * + * AMS55 #15.2.10 + */ +static double hyp2f1ra(double a, double b, double c, double x, double *loss) { + double f2, f1, f0; + int n; + double t, err, da; + + /* Don't cross c or zero */ + if ((c < 0 && a <= c) || (c >= 0 && a >= c)) { + da = round(a - c); + } else { + da = round(a); + } + t = a - da; + + *loss = 0; + + assert(da != 0); + + if (fabs(da) > MAX_ITERATIONS) { + /* Too expensive to compute this value, so give up */ + sf_error("hyp2f1", SF_ERROR_NO_RESULT, NULL); + *loss = 1.0; + return NAN; + } + + if (da < 0) { + /* Recurse down */ + f2 = 0; + f1 = hys2f1(t, b, c, x, &err); + *loss += err; + f0 = hys2f1(t - 1, b, c, x, &err); + *loss += err; + t -= 1; + for (n = 1; n < -da; ++n) { + f2 = f1; + f1 = f0; + f0 = -(2 * t - c - t * x + b * x) / (c - t) * f1 - + t * (x - 1) / (c - t) * f2; + t -= 1; + } + } else { + /* Recurse up */ + f2 = 0; + f1 = hys2f1(t, b, c, x, &err); + *loss += err; + f0 = hys2f1(t + 1, b, c, x, &err); + *loss += err; + t += 1; + for (n = 1; n < da; ++n) { + f2 = f1; + f1 = f0; + f0 = -((2 * t - c - t * x + b * x) * f1 + (c - t) * f2) / (t * (x - 1)); + t += 1; + } + } + + return f0; +} diff --git a/gtsam/3rdparty/cephes/cephes/hyperg.c b/gtsam/3rdparty/cephes/cephes/hyperg.c new file mode 100644 index 000000000..ac23e7133 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/hyperg.c @@ -0,0 +1,362 @@ +/* hyperg.c + * + * Confluent hypergeometric function + * + * + * + * SYNOPSIS: + * + * double a, b, x, y, hyperg(); + * + * y = hyperg( a, b, x ); + * + * + * + * DESCRIPTION: + * + * Computes the confluent hypergeometric function + * + * 1 2 + * a x a(a+1) x + * F ( a,b;x ) = 1 + ---- + --------- + ... + * 1 1 b 1! b(b+1) 2! + * + * Many higher transcendental functions are special cases of + * this power series. + * + * As is evident from the formula, b must not be a negative + * integer or zero unless a is an integer with 0 >= a > b. + * + * The routine attempts both a direct summation of the series + * and an asymptotic expansion. In each case error due to + * roundoff, cancellation, and nonconvergence is estimated. + * The result with smaller estimated error is returned. + * + * + * + * ACCURACY: + * + * Tested at random points (a, b, x), all three variables + * ranging from 0 to 30. + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 30000 1.8e-14 1.1e-15 + * + * Larger errors can be observed when b is near a negative + * integer or zero. Certain combinations of arguments yield + * serious cancellation error in the power series summation + * and also are not in the region of near convergence of the + * asymptotic series. An error message is printed if the + * self-estimated relative error is greater than 1.0e-12. + * + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1988, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" +#include + +extern double MACHEP; + + +/* the `type` parameter determines what converging factor to use */ +static double hyp2f0(double a, double b, double x, int type, double *err) +{ + double a0, alast, t, tlast, maxt; + double n, an, bn, u, sum, temp; + + an = a; + bn = b; + a0 = 1.0e0; + alast = 1.0e0; + sum = 0.0; + n = 1.0e0; + t = 1.0e0; + tlast = 1.0e9; + maxt = 0.0; + + do { + if (an == 0) + goto pdone; + if (bn == 0) + goto pdone; + + u = an * (bn * x / n); + + /* check for blowup */ + temp = fabs(u); + if ((temp > 1.0) && (maxt > (DBL_MAX / temp))) + goto error; + + a0 *= u; + t = fabs(a0); + + /* terminating condition for asymptotic series: + * the series is divergent (if a or b is not a negative integer), + * but its leading part can be used as an asymptotic expansion + */ + if (t > tlast) + goto ndone; + + tlast = t; + sum += alast; /* the sum is one term behind */ + alast = a0; + + if (n > 200) + goto ndone; + + an += 1.0e0; + bn += 1.0e0; + n += 1.0e0; + if (t > maxt) + maxt = t; + } + while (t > MACHEP); + + + pdone: /* series converged! */ + + /* estimate error due to roundoff and cancellation */ + *err = fabs(MACHEP * (n + maxt)); + + alast = a0; + goto done; + + ndone: /* series did not converge */ + + /* The following "Converging factors" are supposed to improve accuracy, + * but do not actually seem to accomplish very much. */ + + n -= 1.0; + x = 1.0 / x; + + switch (type) { /* "type" given as subroutine argument */ + case 1: + alast *= + (0.5 + (0.125 + 0.25 * b - 0.5 * a + 0.25 * x - 0.25 * n) / x); + break; + + case 2: + alast *= 2.0 / 3.0 - b + 2.0 * a + x - n; + break; + + default: + ; + } + + /* estimate error due to roundoff, cancellation, and nonconvergence */ + *err = MACHEP * (n + maxt) + fabs(a0); + + done: + sum += alast; + return (sum); + + /* series blew up: */ + error: + *err = INFINITY; + sf_error("hyperg", SF_ERROR_NO_RESULT, NULL); + return (sum); +} + + +/* asymptotic formula for hypergeometric function: + * + * ( -a + * -- ( |z| + * | (b) ( -------- 2f0( a, 1+a-b, -1/x ) + * ( -- + * ( | (b-a) + * + * + * x a-b ) + * e |x| ) + * + -------- 2f0( b-a, 1-a, 1/x ) ) + * -- ) + * | (a) ) + */ + +static double hy1f1a(double a, double b, double x, double *err) +{ + double h1, h2, t, u, temp, acanc, asum, err1, err2; + + if (x == 0) { + acanc = 1.0; + asum = INFINITY; + goto adone; + } + temp = log(fabs(x)); + t = x + temp * (a - b); + u = -temp * a; + + if (b > 0) { + temp = lgam(b); + t += temp; + u += temp; + } + + h1 = hyp2f0(a, a - b + 1, -1.0 / x, 1, &err1); + + temp = exp(u) / gamma(b - a); + h1 *= temp; + err1 *= temp; + + h2 = hyp2f0(b - a, 1.0 - a, 1.0 / x, 2, &err2); + + if (a < 0) + temp = exp(t) / gamma(a); + else + temp = exp(t - lgam(a)); + + h2 *= temp; + err2 *= temp; + + if (x < 0.0) + asum = h1; + else + asum = h2; + + acanc = fabs(err1) + fabs(err2); + + if (b < 0) { + temp = gamma(b); + asum *= temp; + acanc *= fabs(temp); + } + + + if (asum != 0.0) + acanc /= fabs(asum); + + if (acanc != acanc) + /* nan */ + acanc = 1.0; + + if (asum == INFINITY || asum == -INFINITY) + /* infinity */ + acanc = 0; + + acanc *= 30.0; /* fudge factor, since error of asymptotic formula + * often seems this much larger than advertised */ + + adone: + *err = acanc; + return (asum); +} + + +/* Power series summation for confluent hypergeometric function */ +static double hy1f1p(double a, double b, double x, double *err) +{ + double n, a0, sum, t, u, temp, maxn; + double an, bn, maxt; + double y, c, sumc; + + + /* set up for power series summation */ + an = a; + bn = b; + a0 = 1.0; + sum = 1.0; + c = 0.0; + n = 1.0; + t = 1.0; + maxt = 0.0; + *err = 1.0; + + maxn = 200.0 + 2 * fabs(a) + 2 * fabs(b); + + while (t > MACHEP) { + if (bn == 0) { /* check bn first since if both */ + sf_error("hyperg", SF_ERROR_SINGULAR, NULL); + return (INFINITY); /* an and bn are zero it is */ + } + if (an == 0) /* a singularity */ + return (sum); + if (n > maxn) { + /* too many terms; take the last one as error estimate */ + c = fabs(c) + fabs(t) * 50.0; + goto pdone; + } + u = x * (an / (bn * n)); + + /* check for blowup */ + temp = fabs(u); + if ((temp > 1.0) && (maxt > (DBL_MAX / temp))) { + *err = 1.0; /* blowup: estimate 100% error */ + return sum; + } + + a0 *= u; + + y = a0 - c; + sumc = sum + y; + c = (sumc - sum) - y; + sum = sumc; + + t = fabs(a0); + + an += 1.0; + bn += 1.0; + n += 1.0; + } + + pdone: + + /* estimate error due to roundoff and cancellation */ + if (sum != 0.0) { + *err = fabs(c / sum); + } + else { + *err = fabs(c); + } + + if (*err != *err) { + /* nan */ + *err = 1.0; + } + + return (sum); +} + + + +double hyperg(double a, double b, double x) +{ + double asum, psum, acanc, pcanc, temp; + + /* See if a Kummer transformation will help */ + temp = b - a; + if (fabs(temp) < 0.001 * fabs(a)) + return (exp(x) * hyperg(temp, b, -x)); + + + /* Try power & asymptotic series, starting from the one that is likely OK */ + if (fabs(x) < 10 + fabs(a) + fabs(b)) { + psum = hy1f1p(a, b, x, &pcanc); + if (pcanc < 1.0e-15) + goto done; + asum = hy1f1a(a, b, x, &acanc); + } + else { + psum = hy1f1a(a, b, x, &pcanc); + if (pcanc < 1.0e-15) + goto done; + asum = hy1f1p(a, b, x, &acanc); + } + + /* Pick the result with less estimated error */ + + if (acanc < pcanc) { + pcanc = acanc; + psum = asum; + } + + done: + if (pcanc > 1.0e-12) + sf_error("hyperg", SF_ERROR_LOSS, NULL); + + return (psum); +} diff --git a/gtsam/3rdparty/cephes/cephes/i0.c b/gtsam/3rdparty/cephes/cephes/i0.c new file mode 100644 index 000000000..4e85d556e --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/i0.c @@ -0,0 +1,180 @@ +/* i0.c + * + * Modified Bessel function of order zero + * + * + * + * SYNOPSIS: + * + * double x, y, i0(); + * + * y = i0( x ); + * + * + * + * DESCRIPTION: + * + * Returns modified Bessel function of order zero of the + * argument. + * + * The function is defined as i0(x) = j0( ix ). + * + * The range is partitioned into the two intervals [0,8] and + * (8, infinity). Chebyshev polynomial expansions are employed + * in each interval. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 30000 5.8e-16 1.4e-16 + * + */ + /* i0e.c + * + * Modified Bessel function of order zero, + * exponentially scaled + * + * + * + * SYNOPSIS: + * + * double x, y, i0e(); + * + * y = i0e( x ); + * + * + * + * DESCRIPTION: + * + * Returns exponentially scaled modified Bessel function + * of order zero of the argument. + * + * The function is defined as i0e(x) = exp(-|x|) j0( ix ). + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 30000 5.4e-16 1.2e-16 + * See i0(). + * + */ + +/* i0.c */ + + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +/* Chebyshev coefficients for exp(-x) I0(x) + * in the interval [0,8]. + * + * lim(x->0){ exp(-x) I0(x) } = 1. + */ +static double A[] = { + -4.41534164647933937950E-18, + 3.33079451882223809783E-17, + -2.43127984654795469359E-16, + 1.71539128555513303061E-15, + -1.16853328779934516808E-14, + 7.67618549860493561688E-14, + -4.85644678311192946090E-13, + 2.95505266312963983461E-12, + -1.72682629144155570723E-11, + 9.67580903537323691224E-11, + -5.18979560163526290666E-10, + 2.65982372468238665035E-9, + -1.30002500998624804212E-8, + 6.04699502254191894932E-8, + -2.67079385394061173391E-7, + 1.11738753912010371815E-6, + -4.41673835845875056359E-6, + 1.64484480707288970893E-5, + -5.75419501008210370398E-5, + 1.88502885095841655729E-4, + -5.76375574538582365885E-4, + 1.63947561694133579842E-3, + -4.32430999505057594430E-3, + 1.05464603945949983183E-2, + -2.37374148058994688156E-2, + 4.93052842396707084878E-2, + -9.49010970480476444210E-2, + 1.71620901522208775349E-1, + -3.04682672343198398683E-1, + 6.76795274409476084995E-1 +}; + +/* Chebyshev coefficients for exp(-x) sqrt(x) I0(x) + * in the inverted interval [8,infinity]. + * + * lim(x->inf){ exp(-x) sqrt(x) I0(x) } = 1/sqrt(2pi). + */ +static double B[] = { + -7.23318048787475395456E-18, + -4.83050448594418207126E-18, + 4.46562142029675999901E-17, + 3.46122286769746109310E-17, + -2.82762398051658348494E-16, + -3.42548561967721913462E-16, + 1.77256013305652638360E-15, + 3.81168066935262242075E-15, + -9.55484669882830764870E-15, + -4.15056934728722208663E-14, + 1.54008621752140982691E-14, + 3.85277838274214270114E-13, + 7.18012445138366623367E-13, + -1.79417853150680611778E-12, + -1.32158118404477131188E-11, + -3.14991652796324136454E-11, + 1.18891471078464383424E-11, + 4.94060238822496958910E-10, + 3.39623202570838634515E-9, + 2.26666899049817806459E-8, + 2.04891858946906374183E-7, + 2.89137052083475648297E-6, + 6.88975834691682398426E-5, + 3.36911647825569408990E-3, + 8.04490411014108831608E-1 +}; + +double i0(double x) +{ + double y; + + if (x < 0) + x = -x; + if (x <= 8.0) { + y = (x / 2.0) - 2.0; + return (exp(x) * chbevl(y, A, 30)); + } + + return (exp(x) * chbevl(32.0 / x - 2.0, B, 25) / sqrt(x)); + +} + + + + +double i0e(double x) +{ + double y; + + if (x < 0) + x = -x; + if (x <= 8.0) { + y = (x / 2.0) - 2.0; + return (chbevl(y, A, 30)); + } + + return (chbevl(32.0 / x - 2.0, B, 25) / sqrt(x)); + +} diff --git a/gtsam/3rdparty/cephes/cephes/i1.c b/gtsam/3rdparty/cephes/cephes/i1.c new file mode 100644 index 000000000..4553873f2 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/i1.c @@ -0,0 +1,184 @@ +/* i1.c + * + * Modified Bessel function of order one + * + * + * + * SYNOPSIS: + * + * double x, y, i1(); + * + * y = i1( x ); + * + * + * + * DESCRIPTION: + * + * Returns modified Bessel function of order one of the + * argument. + * + * The function is defined as i1(x) = -i j1( ix ). + * + * The range is partitioned into the two intervals [0,8] and + * (8, infinity). Chebyshev polynomial expansions are employed + * in each interval. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.9e-15 2.1e-16 + * + * + */ + /* i1e.c + * + * Modified Bessel function of order one, + * exponentially scaled + * + * + * + * SYNOPSIS: + * + * double x, y, i1e(); + * + * y = i1e( x ); + * + * + * + * DESCRIPTION: + * + * Returns exponentially scaled modified Bessel function + * of order one of the argument. + * + * The function is defined as i1(x) = -i exp(-|x|) j1( ix ). + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 2.0e-15 2.0e-16 + * See i1(). + * + */ + +/* i1.c 2 */ + + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1985, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +/* Chebyshev coefficients for exp(-x) I1(x) / x + * in the interval [0,8]. + * + * lim(x->0){ exp(-x) I1(x) / x } = 1/2. + */ + +static double A[] = { + 2.77791411276104639959E-18, + -2.11142121435816608115E-17, + 1.55363195773620046921E-16, + -1.10559694773538630805E-15, + 7.60068429473540693410E-15, + -5.04218550472791168711E-14, + 3.22379336594557470981E-13, + -1.98397439776494371520E-12, + 1.17361862988909016308E-11, + -6.66348972350202774223E-11, + 3.62559028155211703701E-10, + -1.88724975172282928790E-9, + 9.38153738649577178388E-9, + -4.44505912879632808065E-8, + 2.00329475355213526229E-7, + -8.56872026469545474066E-7, + 3.47025130813767847674E-6, + -1.32731636560394358279E-5, + 4.78156510755005422638E-5, + -1.61760815825896745588E-4, + 5.12285956168575772895E-4, + -1.51357245063125314899E-3, + 4.15642294431288815669E-3, + -1.05640848946261981558E-2, + 2.47264490306265168283E-2, + -5.29459812080949914269E-2, + 1.02643658689847095384E-1, + -1.76416518357834055153E-1, + 2.52587186443633654823E-1 +}; + +/* Chebyshev coefficients for exp(-x) sqrt(x) I1(x) + * in the inverted interval [8,infinity]. + * + * lim(x->inf){ exp(-x) sqrt(x) I1(x) } = 1/sqrt(2pi). + */ +static double B[] = { + 7.51729631084210481353E-18, + 4.41434832307170791151E-18, + -4.65030536848935832153E-17, + -3.20952592199342395980E-17, + 2.96262899764595013876E-16, + 3.30820231092092828324E-16, + -1.88035477551078244854E-15, + -3.81440307243700780478E-15, + 1.04202769841288027642E-14, + 4.27244001671195135429E-14, + -2.10154184277266431302E-14, + -4.08355111109219731823E-13, + -7.19855177624590851209E-13, + 2.03562854414708950722E-12, + 1.41258074366137813316E-11, + 3.25260358301548823856E-11, + -1.89749581235054123450E-11, + -5.58974346219658380687E-10, + -3.83538038596423702205E-9, + -2.63146884688951950684E-8, + -2.51223623787020892529E-7, + -3.88256480887769039346E-6, + -1.10588938762623716291E-4, + -9.76109749136146840777E-3, + 7.78576235018280120474E-1 +}; + +double i1(double x) +{ + double y, z; + + z = fabs(x); + if (z <= 8.0) { + y = (z / 2.0) - 2.0; + z = chbevl(y, A, 29) * z * exp(z); + } + else { + z = exp(z) * chbevl(32.0 / z - 2.0, B, 25) / sqrt(z); + } + if (x < 0.0) + z = -z; + return (z); +} + +/* i1e() */ + +double i1e(double x) +{ + double y, z; + + z = fabs(x); + if (z <= 8.0) { + y = (z / 2.0) - 2.0; + z = chbevl(y, A, 29) * z; + } + else { + z = chbevl(32.0 / z - 2.0, B, 25) / sqrt(z); + } + if (x < 0.0) + z = -z; + return (z); +} diff --git a/gtsam/3rdparty/cephes/cephes/igam.c b/gtsam/3rdparty/cephes/cephes/igam.c new file mode 100644 index 000000000..75f871ec5 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/igam.c @@ -0,0 +1,423 @@ +/* igam.c + * + * Incomplete Gamma integral + * + * + * + * SYNOPSIS: + * + * double a, x, y, igam(); + * + * y = igam( a, x ); + * + * DESCRIPTION: + * + * The function is defined by + * + * x + * - + * 1 | | -t a-1 + * igam(a,x) = ----- | e t dt. + * - | | + * | (a) - + * 0 + * + * + * In this implementation both arguments must be positive. + * The integral is evaluated by either a power series or + * continued fraction expansion, depending on the relative + * values of a and x. + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 200000 3.6e-14 2.9e-15 + * IEEE 0,100 300000 9.9e-14 1.5e-14 + */ + /* igamc() + * + * Complemented incomplete Gamma integral + * + * + * + * SYNOPSIS: + * + * double a, x, y, igamc(); + * + * y = igamc( a, x ); + * + * DESCRIPTION: + * + * The function is defined by + * + * + * igamc(a,x) = 1 - igam(a,x) + * + * inf. + * - + * 1 | | -t a-1 + * = ----- | e t dt. + * - | | + * | (a) - + * x + * + * + * In this implementation both arguments must be positive. + * The integral is evaluated by either a power series or + * continued fraction expansion, depending on the relative + * values of a and x. + * + * ACCURACY: + * + * Tested at random a, x. + * a x Relative error: + * arithmetic domain domain # trials peak rms + * IEEE 0.5,100 0,100 200000 1.9e-14 1.7e-15 + * IEEE 0.01,0.5 0,100 200000 1.4e-13 1.6e-15 + */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1985, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +/* Sources + * [1] "The Digital Library of Mathematical Functions", dlmf.nist.gov + * [2] Maddock et. al., "Incomplete Gamma Functions", + * https://www.boost.org/doc/libs/1_61_0/libs/math/doc/html/math_toolkit/sf_gamma/igamma.html + */ + +/* Scipy changes: + * - 05-01-2016: added asymptotic expansion for igam to improve the + * a ~ x regime. + * - 06-19-2016: additional series expansion added for igamc to + * improve accuracy at small arguments. + * - 06-24-2016: better choice of domain for the asymptotic series; + * improvements in accuracy for the asymptotic series when a and x + * are very close. + */ + +#include "mconf.h" +#include "lanczos.h" +#include "igam.h" + +#ifdef MAXITER +#undef MAXITER +#endif + +#define MAXITER 2000 +#define IGAM 1 +#define IGAMC 0 +#define SMALL 20 +#define LARGE 200 +#define SMALLRATIO 0.3 +#define LARGERATIO 4.5 + +extern double MACHEP, MAXLOG; +static double big = 4.503599627370496e15; +static double biginv = 2.22044604925031308085e-16; + +static double igamc_continued_fraction(double, double); +static double igam_series(double, double); +static double igamc_series(double, double); +static double asymptotic_series(double, double, int); + + +double igam(double a, double x) +{ + double absxma_a; + + if (x < 0 || a < 0) { + sf_error("gammainc", SF_ERROR_DOMAIN, NULL); + return NAN; + } else if (a == 0) { + if (x > 0) { + return 1; + } else { + return NAN; + } + } else if (x == 0) { + /* Zero integration limit */ + return 0; + } else if (isinf(a)) { + if (isinf(x)) { + return NAN; + } + return 0; + } else if (isinf(x)) { + return 1; + } + + /* Asymptotic regime where a ~ x; see [2]. */ + absxma_a = fabs(x - a) / a; + if ((a > SMALL) && (a < LARGE) && (absxma_a < SMALLRATIO)) { + return asymptotic_series(a, x, IGAM); + } else if ((a > LARGE) && (absxma_a < LARGERATIO / sqrt(a))) { + return asymptotic_series(a, x, IGAM); + } + + if ((x > 1.0) && (x > a)) { + return (1.0 - igamc(a, x)); + } + + return igam_series(a, x); +} + + +double igamc(double a, double x) +{ + double absxma_a; + + if (x < 0 || a < 0) { + sf_error("gammaincc", SF_ERROR_DOMAIN, NULL); + return NAN; + } else if (a == 0) { + if (x > 0) { + return 0; + } else { + return NAN; + } + } else if (x == 0) { + return 1; + } else if (isinf(a)) { + if (isinf(x)) { + return NAN; + } + return 1; + } else if (isinf(x)) { + return 0; + } + + /* Asymptotic regime where a ~ x; see [2]. */ + absxma_a = fabs(x - a) / a; + if ((a > SMALL) && (a < LARGE) && (absxma_a < SMALLRATIO)) { + return asymptotic_series(a, x, IGAMC); + } else if ((a > LARGE) && (absxma_a < LARGERATIO / sqrt(a))) { + return asymptotic_series(a, x, IGAMC); + } + + /* Everywhere else; see [2]. */ + if (x > 1.1) { + if (x < a) { + return 1.0 - igam_series(a, x); + } else { + return igamc_continued_fraction(a, x); + } + } else if (x <= 0.5) { + if (-0.4 / log(x) < a) { + return 1.0 - igam_series(a, x); + } else { + return igamc_series(a, x); + } + } else { + if (x * 1.1 < a) { + return 1.0 - igam_series(a, x); + } else { + return igamc_series(a, x); + } + } +} + + +/* Compute + * + * x^a * exp(-x) / gamma(a) + * + * corrected from (15) and (16) in [2] by replacing exp(x - a) with + * exp(a - x). + */ +double igam_fac(double a, double x) +{ + double ax, fac, res, num; + + if (fabs(a - x) > 0.4 * fabs(a)) { + ax = a * log(x) - x - lgam(a); + if (ax < -MAXLOG) { + sf_error("igam", SF_ERROR_UNDERFLOW, NULL); + return 0.0; + } + return exp(ax); + } + + fac = a + lanczos_g - 0.5; + res = sqrt(fac / exp(1)) / lanczos_sum_expg_scaled(a); + + if ((a < 200) && (x < 200)) { + res *= exp(a - x) * pow(x / fac, a); + } else { + num = x - a - lanczos_g + 0.5; + res *= exp(a * log1pmx(num / fac) + x * (0.5 - lanczos_g) / fac); + } + + return res; +} + + +/* Compute igamc using DLMF 8.9.2. */ +static double igamc_continued_fraction(double a, double x) +{ + int i; + double ans, ax, c, yc, r, t, y, z; + double pk, pkm1, pkm2, qk, qkm1, qkm2; + + ax = igam_fac(a, x); + if (ax == 0.0) { + return 0.0; + } + + /* continued fraction */ + y = 1.0 - a; + z = x + y + 1.0; + c = 0.0; + pkm2 = 1.0; + qkm2 = x; + pkm1 = x + 1.0; + qkm1 = z * x; + ans = pkm1 / qkm1; + + for (i = 0; i < MAXITER; i++) { + c += 1.0; + y += 1.0; + z += 2.0; + yc = y * c; + pk = pkm1 * z - pkm2 * yc; + qk = qkm1 * z - qkm2 * yc; + if (qk != 0) { + r = pk / qk; + t = fabs((ans - r) / r); + ans = r; + } + else + t = 1.0; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + if (fabs(pk) > big) { + pkm2 *= biginv; + pkm1 *= biginv; + qkm2 *= biginv; + qkm1 *= biginv; + } + if (t <= MACHEP) { + break; + } + } + + return (ans * ax); +} + + +/* Compute igam using DLMF 8.11.4. */ +static double igam_series(double a, double x) +{ + int i; + double ans, ax, c, r; + + ax = igam_fac(a, x); + if (ax == 0.0) { + return 0.0; + } + + /* power series */ + r = a; + c = 1.0; + ans = 1.0; + + for (i = 0; i < MAXITER; i++) { + r += 1.0; + c *= x / r; + ans += c; + if (c <= MACHEP * ans) { + break; + } + } + + return (ans * ax / a); +} + + +/* Compute igamc using DLMF 8.7.3. This is related to the series in + * igam_series but extra care is taken to avoid cancellation. + */ +static double igamc_series(double a, double x) +{ + int n; + double fac = 1; + double sum = 0; + double term, logx; + + for (n = 1; n < MAXITER; n++) { + fac *= -x / n; + term = fac / (a + n); + sum += term; + if (fabs(term) <= MACHEP * fabs(sum)) { + break; + } + } + + logx = log(x); + term = -expm1(a * logx - lgam1p(a)); + return term - exp(a * logx - lgam(a)) * sum; +} + + +/* Compute igam/igamc using DLMF 8.12.3/8.12.4. */ +static double asymptotic_series(double a, double x, int func) +{ + int k, n, sgn; + int maxpow = 0; + double lambda = x / a; + double sigma = (x - a) / a; + double eta, res, ck, ckterm, term, absterm; + double absoldterm = INFINITY; + double etapow[N] = {1}; + double sum = 0; + double afac = 1; + + if (func == IGAM) { + sgn = -1; + } else { + sgn = 1; + } + + if (lambda > 1) { + eta = sqrt(-2 * log1pmx(sigma)); + } else if (lambda < 1) { + eta = -sqrt(-2 * log1pmx(sigma)); + } else { + eta = 0; + } + res = 0.5 * erfc(sgn * eta * sqrt(a / 2)); + + for (k = 0; k < K; k++) { + ck = d[k][0]; + for (n = 1; n < N; n++) { + if (n > maxpow) { + etapow[n] = eta * etapow[n-1]; + maxpow += 1; + } + ckterm = d[k][n]*etapow[n]; + ck += ckterm; + if (fabs(ckterm) < MACHEP * fabs(ck)) { + break; + } + } + term = ck * afac; + absterm = fabs(term); + if (absterm > absoldterm) { + break; + } + sum += term; + if (absterm < MACHEP * fabs(sum)) { + break; + } + absoldterm = absterm; + afac /= a; + } + res += sgn * exp(-0.5 * a * eta * eta) * sum / sqrt(2 * M_PI * a); + + return res; +} diff --git a/gtsam/3rdparty/cephes/cephes/igam.h b/gtsam/3rdparty/cephes/cephes/igam.h new file mode 100644 index 000000000..0bc310633 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/igam.h @@ -0,0 +1,38 @@ +/* This file was automatically generated by _precomp/gammainc.py. + * Do not edit it manually! + */ + +#ifndef IGAM_H +#define IGAM_H + +#define K 25 +#define N 25 + +static const double d[K][N] = +{{-3.3333333333333333e-1, 8.3333333333333333e-2, -1.4814814814814815e-2, 1.1574074074074074e-3, 3.527336860670194e-4, -1.7875514403292181e-4, 3.9192631785224378e-5, -2.1854485106799922e-6, -1.85406221071516e-6, 8.296711340953086e-7, -1.7665952736826079e-7, 6.7078535434014986e-9, 1.0261809784240308e-8, -4.3820360184533532e-9, 9.1476995822367902e-10, -2.551419399494625e-11, -5.8307721325504251e-11, 2.4361948020667416e-11, -5.0276692801141756e-12, 1.1004392031956135e-13, 3.3717632624009854e-13, -1.3923887224181621e-13, 2.8534893807047443e-14, -5.1391118342425726e-16, -1.9752288294349443e-15}, +{-1.8518518518518519e-3, -3.4722222222222222e-3, 2.6455026455026455e-3, -9.9022633744855967e-4, 2.0576131687242798e-4, -4.0187757201646091e-7, -1.8098550334489978e-5, 7.6491609160811101e-6, -1.6120900894563446e-6, 4.6471278028074343e-9, 1.378633446915721e-7, -5.752545603517705e-8, 1.1951628599778147e-8, -1.7543241719747648e-11, -1.0091543710600413e-9, 4.1627929918425826e-10, -8.5639070264929806e-11, 6.0672151016047586e-14, 7.1624989648114854e-12, -2.9331866437714371e-12, 5.9966963656836887e-13, -2.1671786527323314e-16, -4.9783399723692616e-14, 2.0291628823713425e-14, -4.13125571381061e-15}, +{4.1335978835978836e-3, -2.6813271604938272e-3, 7.7160493827160494e-4, 2.0093878600823045e-6, -1.0736653226365161e-4, 5.2923448829120125e-5, -1.2760635188618728e-5, 3.4235787340961381e-8, 1.3721957309062933e-6, -6.298992138380055e-7, 1.4280614206064242e-7, -2.0477098421990866e-10, -1.4092529910867521e-8, 6.228974084922022e-9, -1.3670488396617113e-9, 9.4283561590146782e-13, 1.2872252400089318e-10, -5.5645956134363321e-11, 1.1975935546366981e-11, -4.1689782251838635e-15, -1.0940640427884594e-12, 4.6622399463901357e-13, -9.905105763906906e-14, 1.8931876768373515e-17, 8.8592218725911273e-15}, +{6.4943415637860082e-4, 2.2947209362139918e-4, -4.6918949439525571e-4, 2.6772063206283885e-4, -7.5618016718839764e-5, -2.3965051138672967e-7, 1.1082654115347302e-5, -5.6749528269915966e-6, 1.4230900732435884e-6, -2.7861080291528142e-11, -1.6958404091930277e-7, 8.0994649053880824e-8, -1.9111168485973654e-8, 2.3928620439808118e-12, 2.0620131815488798e-9, -9.4604966618551322e-10, 2.1541049775774908e-10, -1.388823336813903e-14, -2.1894761681963939e-11, 9.7909989511716851e-12, -2.1782191880180962e-12, 6.2088195734079014e-17, 2.126978363279737e-13, -9.3446887915174333e-14, 2.0453671226782849e-14}, +{-8.618882909167117e-4, 7.8403922172006663e-4, -2.9907248030319018e-4, -1.4638452578843418e-6, 6.6414982154651222e-5, -3.9683650471794347e-5, 1.1375726970678419e-5, 2.5074972262375328e-10, -1.6954149536558306e-6, 8.9075075322053097e-7, -2.2929348340008049e-7, 2.956794137544049e-11, 2.8865829742708784e-8, -1.4189739437803219e-8, 3.4463580499464897e-9, -2.3024517174528067e-13, -3.9409233028046405e-10, 1.8602338968504502e-10, -4.356323005056618e-11, 1.2786001016296231e-15, 4.6792750266579195e-12, -2.1492464706134829e-12, 4.9088156148096522e-13, -6.3385914848915603e-18, -5.0453320690800944e-14}, +{-3.3679855336635815e-4, -6.9728137583658578e-5, 2.7727532449593921e-4, -1.9932570516188848e-4, 6.7977804779372078e-5, 1.419062920643967e-7, -1.3594048189768693e-5, 8.0184702563342015e-6, -2.2914811765080952e-6, -3.252473551298454e-10, 3.4652846491085265e-7, -1.8447187191171343e-7, 4.8240967037894181e-8, -1.7989466721743515e-14, -6.3061945000135234e-9, 3.1624176287745679e-9, -7.8409242536974293e-10, 5.1926791652540407e-15, 9.3589442423067836e-11, -4.5134262161632782e-11, 1.0799129993116827e-11, -3.661886712685252e-17, -1.210902069055155e-12, 5.6807435849905643e-13, -1.3249659916340829e-13}, +{5.3130793646399222e-4, -5.9216643735369388e-4, 2.7087820967180448e-4, 7.9023532326603279e-7, -8.1539693675619688e-5, 5.6116827531062497e-5, -1.8329116582843376e-5, -3.0796134506033048e-9, 3.4651553688036091e-6, -2.0291327396058604e-6, 5.7887928631490037e-7, 2.338630673826657e-13, -8.8286007463304835e-8, 4.7435958880408128e-8, -1.2545415020710382e-8, 8.6496488580102925e-14, 1.6846058979264063e-9, -8.5754928235775947e-10, 2.1598224929232125e-10, -7.6132305204761539e-16, -2.6639822008536144e-11, 1.3065700536611057e-11, -3.1799163902367977e-12, 4.7109761213674315e-18, 3.6902800842763467e-13}, +{3.4436760689237767e-4, 5.1717909082605922e-5, -3.3493161081142236e-4, 2.812695154763237e-4, -1.0976582244684731e-4, -1.2741009095484485e-7, 2.7744451511563644e-5, -1.8263488805711333e-5, 5.7876949497350524e-6, 4.9387589339362704e-10, -1.0595367014026043e-6, 6.1667143761104075e-7, -1.7562973359060462e-7, -1.2974473287015439e-12, 2.695423606288966e-8, -1.4578352908731271e-8, 3.887645959386175e-9, -3.8810022510194121e-17, -5.3279941738772867e-10, 2.7437977643314845e-10, -6.9957960920705679e-11, 2.5899863874868481e-17, 8.8566890996696381e-12, -4.403168815871311e-12, 1.0865561947091654e-12}, +{-6.5262391859530942e-4, 8.3949872067208728e-4, -4.3829709854172101e-4, -6.969091458420552e-7, 1.6644846642067548e-4, -1.2783517679769219e-4, 4.6299532636913043e-5, 4.5579098679227077e-9, -1.0595271125805195e-5, 6.7833429048651666e-6, -2.1075476666258804e-6, -1.7213731432817145e-11, 3.7735877416110979e-7, -2.1867506700122867e-7, 6.2202288040189269e-8, 6.5977038267330006e-16, -9.5903864974256858e-9, 5.2132144922808078e-9, -1.3991589583935709e-9, 5.382058999060575e-16, 1.9484714275467745e-10, -1.0127287556389682e-10, 2.6077347197254926e-11, -5.0904186999932993e-18, -3.3721464474854592e-12}, +{-5.9676129019274625e-4, -7.2048954160200106e-5, 6.7823088376673284e-4, -6.4014752602627585e-4, 2.7750107634328704e-4, 1.8197008380465151e-7, -8.4795071170685032e-5, 6.105192082501531e-5, -2.1073920183404862e-5, -8.8585890141255994e-10, 4.5284535953805377e-6, -2.8427815022504408e-6, 8.7082341778646412e-7, 3.6886101871706965e-12, -1.5344695190702061e-7, 8.862466778790695e-8, -2.5184812301826817e-8, -1.0225912098215092e-14, 3.8969470758154777e-9, -2.1267304792235635e-9, 5.7370135528051385e-10, -1.887749850169741e-19, -8.0931538694657866e-11, 4.2382723283449199e-11, -1.1002224534207726e-11}, +{1.3324454494800656e-3, -1.9144384985654775e-3, 1.1089369134596637e-3, 9.932404122642299e-7, -5.0874501293093199e-4, 4.2735056665392884e-4, -1.6858853767910799e-4, -8.1301893922784998e-9, 4.5284402370562147e-5, -3.127053674781734e-5, 1.044986828530338e-5, 4.8435226265680926e-11, -2.1482565873456258e-6, 1.329369701097492e-6, -4.0295693092101029e-7, -1.7567877666323291e-13, 7.0145043163668257e-8, -4.040787734999483e-8, 1.1474026743371963e-8, 3.9642746853563325e-18, -1.7804938269892714e-9, 9.7480262548731646e-10, -2.6405338676507616e-10, 5.794875163403742e-18, 3.7647749553543836e-11}, +{1.579727660730835e-3, 1.6251626278391582e-4, -2.0633421035543276e-3, 2.1389686185689098e-3, -1.0108559391263003e-3, -3.9912705529919201e-7, 3.6235025084764691e-4, -2.8143901463712154e-4, 1.0449513336495887e-4, 2.1211418491830297e-9, -2.5779417251947842e-5, 1.7281818956040463e-5, -5.6413773872904282e-6, -1.1024320105776174e-11, 1.1223224418895175e-6, -6.8693396379526735e-7, 2.0653236975414887e-7, 4.6714772409838506e-14, -3.5609886164949055e-8, 2.0470855345905963e-8, -5.8091738633283358e-9, -1.332821287582869e-16, 9.0354604391335133e-10, -4.9598782517330834e-10, 1.3481607129399749e-10}, +{-4.0725121195140166e-3, 6.4033628338080698e-3, -4.0410161081676618e-3, -2.183732802866233e-6, 2.1740441801254639e-3, -1.9700440518418892e-3, 8.3595469747962458e-4, 1.9445447567109655e-8, -2.5779387120421696e-4, 1.9009987368139304e-4, -6.7696499937438965e-5, -1.4440629666426572e-10, 1.5712512518742269e-5, -1.0304008744776893e-5, 3.304517767401387e-6, 7.9829760242325709e-13, -6.4097794149313004e-7, 3.8894624761300056e-7, -1.1618347644948869e-7, -2.816808630596451e-15, 1.9878012911297093e-8, -1.1407719956357511e-8, 3.2355857064185555e-9, 4.1759468293455945e-20, -5.0423112718105824e-10}, +{-5.9475779383993003e-3, -5.4016476789260452e-4, 8.7910413550767898e-3, -9.8576315587856125e-3, 5.0134695031021538e-3, 1.2807521786221875e-6, -2.0626019342754683e-3, 1.7109128573523058e-3, -6.7695312714133799e-4, -6.9011545676562133e-9, 1.8855128143995902e-4, -1.3395215663491969e-4, 4.6263183033528039e-5, 4.0034230613321351e-11, -1.0255652921494033e-5, 6.612086372797651e-6, -2.0913022027253008e-6, -2.0951775649603837e-13, 3.9756029041993247e-7, -2.3956211978815887e-7, 7.1182883382145864e-8, 8.925574873053455e-16, -1.2101547235064676e-8, 6.9350618248334386e-9, -1.9661464453856102e-9}, +{1.7402027787522711e-2, -2.9527880945699121e-2, 2.0045875571402799e-2, 7.0289515966903407e-6, -1.2375421071343148e-2, 1.1976293444235254e-2, -5.4156038466518525e-3, -6.3290893396418616e-8, 1.8855118129005065e-3, -1.473473274825001e-3, 5.5515810097708387e-4, 5.2406834412550662e-10, -1.4357913535784836e-4, 9.9181293224943297e-5, -3.3460834749478311e-5, -3.5755837291098993e-12, 7.1560851960630076e-6, -4.5516802628155526e-6, 1.4236576649271475e-6, 1.8803149082089664e-14, -2.6623403898929211e-7, 1.5950642189595716e-7, -4.7187514673841102e-8, -6.5107872958755177e-17, 7.9795091026746235e-9}, +{3.0249124160905891e-2, 2.4817436002649977e-3, -4.9939134373457022e-2, 5.9915643009307869e-2, -3.2483207601623391e-2, -5.7212968652103441e-6, 1.5085251778569354e-2, -1.3261324005088445e-2, 5.5515262632426148e-3, 3.0263182257030016e-8, -1.7229548406756723e-3, 1.2893570099929637e-3, -4.6845138348319876e-4, -1.830259937893045e-10, 1.1449739014822654e-4, -7.7378565221244477e-5, 2.5625836246985201e-5, 1.0766165333192814e-12, -5.3246809282422621e-6, 3.349634863064464e-6, -1.0381253128684018e-6, -5.608909920621128e-15, 1.9150821930676591e-7, -1.1418365800203486e-7, 3.3654425209171788e-8}, +{-9.9051020880159045e-2, 1.7954011706123486e-1, -1.2989606383463778e-1, -3.1478872752284357e-5, 9.0510635276848131e-2, -9.2828824411184397e-2, 4.4412112839877808e-2, 2.7779236316835888e-7, -1.7229543805449697e-2, 1.4182925050891573e-2, -5.6214161633747336e-3, -2.39598509186381e-9, 1.6029634366079908e-3, -1.1606784674435773e-3, 4.1001337768153873e-4, 1.8365800754090661e-11, -9.5844256563655903e-5, 6.3643062337764708e-5, -2.076250624489065e-5, -1.1806020912804483e-13, 4.2131808239120649e-6, -2.6262241337012467e-6, 8.0770620494930662e-7, 6.0125912123632725e-16, -1.4729737374018841e-7}, +{-1.9994542198219728e-1, -1.5056113040026424e-2, 3.6470239469348489e-1, -4.6435192311733545e-1, 2.6640934719197893e-1, 3.4038266027147191e-5, -1.3784338709329624e-1, 1.276467178337056e-1, -5.6213828755200985e-2, -1.753150885483011e-7, 1.9235592956768113e-2, -1.5088821281095315e-2, 5.7401854451350123e-3, 1.0622382710310225e-9, -1.5335082692563998e-3, 1.0819320643228214e-3, -3.7372510193945659e-4, -6.6170909729031985e-12, 8.4263617380909628e-5, -5.5150706827483479e-5, 1.7769536448348069e-5, 3.8827923210205533e-14, -3.53513697488768e-6, 2.1865832130045269e-6, -6.6812849447625594e-7}, +{7.2438608504029431e-1, -1.3918010932653375, 1.0654143352413968, 1.876173868950258e-4, -8.2705501176152696e-1, 8.9352433347828414e-1, -4.4971003995291339e-1, -1.6107401567546652e-6, 1.9235590165271091e-1, -1.6597702160042609e-1, 6.8882222681814333e-2, 1.3910091724608687e-8, -2.146911561508663e-2, 1.6228980898865892e-2, -5.9796016172584256e-3, -1.1287469112826745e-10, 1.5167451119784857e-3, -1.0478634293553899e-3, 3.5539072889126421e-4, 8.1704322111801517e-13, -7.7773013442452395e-5, 5.0291413897007722e-5, -1.6035083867000518e-5, 1.2469354315487605e-14, 3.1369106244517615e-6}, +{1.6668949727276811, 1.165462765994632e-1, -3.3288393225018906, 4.4692325482864037, -2.6977693045875807, -2.600667859891061e-4, 1.5389017615694539, -1.4937962361134612, 6.8881964633233148e-1, 1.3077482004552385e-6, -2.5762963325596288e-1, 2.1097676102125449e-1, -8.3714408359219882e-2, -7.7920428881354753e-9, 2.4267923064833599e-2, -1.7813678334552311e-2, 6.3970330388900056e-3, 4.9430807090480523e-11, -1.5554602758465635e-3, 1.0561196919903214e-3, -3.5277184460472902e-4, 9.3002334645022459e-14, 7.5285855026557172e-5, -4.8186515569156351e-5, 1.5227271505597605e-5}, +{-6.6188298861372935, 1.3397985455142589e+1, -1.0789350606845146e+1, -1.4352254537875018e-3, 9.2333694596189809, -1.0456552819547769e+1, 5.5105526029033471, 1.2024439690716742e-5, -2.5762961164755816, 2.3207442745387179, -1.0045728797216284, -1.0207833290021914e-7, 3.3975092171169466e-1, -2.6720517450757468e-1, 1.0235252851562706e-1, 8.4329730484871625e-10, -2.7998284958442595e-2, 2.0066274144976813e-2, -7.0554368915086242e-3, 1.9402238183698188e-12, 1.6562888105449611e-3, -1.1082898580743683e-3, 3.654545161310169e-4, -5.1290032026971794e-11, -7.6340103696869031e-5}, +{-1.7112706061976095e+1, -1.1208044642899116, 3.7131966511885444e+1, -5.2298271025348962e+1, 3.3058589696624618e+1, 2.4791298976200222e-3, -2.061089403411526e+1, 2.088672775145582e+1, -1.0045703956517752e+1, -1.2238783449063012e-5, 4.0770134274221141, -3.473667358470195, 1.4329352617312006, 7.1359914411879712e-8, -4.4797257159115612e-1, 3.4112666080644461e-1, -1.2699786326594923e-1, -2.8953677269081528e-10, 3.3125776278259863e-2, -2.3274087021036101e-2, 8.0399993503648882e-3, -1.177805216235265e-9, -1.8321624891071668e-3, 1.2108282933588665e-3, -3.9479941246822517e-4}, +{7.389033153567425e+1, -1.5680141270402273e+2, 1.322177542759164e+2, 1.3692876877324546e-2, -1.2366496885920151e+2, 1.4620689391062729e+2, -8.0365587724865346e+1, -1.1259851148881298e-4, 4.0770132196179938e+1, -3.8210340013273034e+1, 1.719522294277362e+1, 9.3519707955168356e-7, -6.2716159907747034, 5.1168999071852637, -2.0319658112299095, -4.9507215582761543e-9, 5.9626397294332597e-1, -4.4220765337238094e-1, 1.6079998700166273e-1, -2.4733786203223402e-8, -4.0307574759979762e-2, 2.7849050747097869e-2, -9.4751858992054221e-3, 6.419922235909132e-6, 2.1250180774699461e-3}, +{2.1216837098382522e+2, 1.3107863022633868e+1, -4.9698285932871748e+2, 7.3121595266969204e+2, -4.8213821720890847e+2, -2.8817248692894889e-2, 3.2616720302947102e+2, -3.4389340280087117e+2, 1.7195193870816232e+2, 1.4038077378096158e-4, -7.52594195897599e+1, 6.651969984520934e+1, -2.8447519748152462e+1, -7.613702615875391e-7, 9.5402237105304373, -7.5175301113311376, 2.8943997568871961, -4.6612194999538201e-7, -8.0615149598794088e-1, 5.8483006570631029e-1, -2.0845408972964956e-1, 1.4765818959305817e-4, 5.1000433863753019e-2, -3.3066252141883665e-2, 1.5109265210467774e-2}, +{-9.8959643098322368e+2, 2.1925555360905233e+3, -1.9283586782723356e+3, -1.5925738122215253e-1, 1.9569985945919857e+3, -2.4072514765081556e+3, 1.3756149959336496e+3, 1.2920735237496668e-3, -7.525941715948055e+2, 7.3171668742208716e+2, -3.4137023466220065e+2, -9.9857390260608043e-6, 1.3356313181291573e+2, -1.1276295161252794e+2, 4.6310396098204458e+1, -7.9237387133614756e-6, -1.4510726927018646e+1, 1.1111771248100563e+1, -4.1690817945270892, 3.1008219800117808e-3, 1.1220095449981468, -7.6052379926149916e-1, 3.6262236505085254e-1, 2.216867741940747e-1, 4.8683443692930507e-1}}; + +#endif diff --git a/gtsam/3rdparty/cephes/cephes/igami.c b/gtsam/3rdparty/cephes/cephes/igami.c new file mode 100644 index 000000000..97fc93ff4 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/igami.c @@ -0,0 +1,339 @@ +/* + * (C) Copyright John Maddock 2006. + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. (See accompanying file + * LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt) + */ +#include "mconf.h" + +static double find_inverse_s(double, double); +static double didonato_SN(double, double, unsigned, double); +static double find_inverse_gamma(double, double, double); + + +static double find_inverse_s(double p, double q) +{ + /* + * Computation of the Incomplete Gamma Function Ratios and their Inverse + * ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR. + * ACM Transactions on Mathematical Software, Vol. 12, No. 4, + * December 1986, Pages 377-393. + * + * See equation 32. + */ + double s, t; + double a[4] = {0.213623493715853, 4.28342155967104, + 11.6616720288968, 3.31125922108741}; + double b[5] = {0.3611708101884203e-1, 1.27364489782223, + 6.40691597760039, 6.61053765625462, 1}; + + if (p < 0.5) { + t = sqrt(-2 * log(p)); + } + else { + t = sqrt(-2 * log(q)); + } + s = t - polevl(t, a, 3) / polevl(t, b, 4); + if(p < 0.5) + s = -s; + return s; +} + + +static double didonato_SN(double a, double x, unsigned N, double tolerance) +{ + /* + * Computation of the Incomplete Gamma Function Ratios and their Inverse + * ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR. + * ACM Transactions on Mathematical Software, Vol. 12, No. 4, + * December 1986, Pages 377-393. + * + * See equation 34. + */ + double sum = 1.0; + + if (N >= 1) { + unsigned i; + double partial = x / (a + 1); + + sum += partial; + for(i = 2; i <= N; ++i) { + partial *= x / (a + i); + sum += partial; + if(partial < tolerance) { + break; + } + } + } + return sum; +} + + +static double find_inverse_gamma(double a, double p, double q) +{ + /* + * In order to understand what's going on here, you will + * need to refer to: + * + * Computation of the Incomplete Gamma Function Ratios and their Inverse + * ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR. + * ACM Transactions on Mathematical Software, Vol. 12, No. 4, + * December 1986, Pages 377-393. + */ + double result; + + if (a == 1) { + if (q > 0.9) { + result = -log1p(-p); + } + else { + result = -log(q); + } + } + else if (a < 1) { + double g = Gamma(a); + double b = q * g; + + if ((b > 0.6) || ((b >= 0.45) && (a >= 0.3))) { + /* DiDonato & Morris Eq 21: + * + * There is a slight variation from DiDonato and Morris here: + * the first form given here is unstable when p is close to 1, + * making it impossible to compute the inverse of Q(a,x) for small + * q. Fortunately the second form works perfectly well in this case. + */ + double u; + if((b * q > 1e-8) && (q > 1e-5)) { + u = pow(p * g * a, 1 / a); + } + else { + u = exp((-q / a) - SCIPY_EULER); + } + result = u / (1 - (u / (a + 1))); + } + else if ((a < 0.3) && (b >= 0.35)) { + /* DiDonato & Morris Eq 22: */ + double t = exp(-SCIPY_EULER - b); + double u = t * exp(t); + result = t * exp(u); + } + else if ((b > 0.15) || (a >= 0.3)) { + /* DiDonato & Morris Eq 23: */ + double y = -log(b); + double u = y - (1 - a) * log(y); + result = y - (1 - a) * log(u) - log(1 + (1 - a) / (1 + u)); + } + else if (b > 0.1) { + /* DiDonato & Morris Eq 24: */ + double y = -log(b); + double u = y - (1 - a) * log(y); + result = y - (1 - a) * log(u) + - log((u * u + 2 * (3 - a) * u + (2 - a) * (3 - a)) + / (u * u + (5 - a) * u + 2)); + } + else { + /* DiDonato & Morris Eq 25: */ + double y = -log(b); + double c1 = (a - 1) * log(y); + double c1_2 = c1 * c1; + double c1_3 = c1_2 * c1; + double c1_4 = c1_2 * c1_2; + double a_2 = a * a; + double a_3 = a_2 * a; + + double c2 = (a - 1) * (1 + c1); + double c3 = (a - 1) * (-(c1_2 / 2) + + (a - 2) * c1 + + (3 * a - 5) / 2); + double c4 = (a - 1) * ((c1_3 / 3) - (3 * a - 5) * c1_2 / 2 + + (a_2 - 6 * a + 7) * c1 + + (11 * a_2 - 46 * a + 47) / 6); + double c5 = (a - 1) * (-(c1_4 / 4) + + (11 * a - 17) * c1_3 / 6 + + (-3 * a_2 + 13 * a -13) * c1_2 + + (2 * a_3 - 25 * a_2 + 72 * a - 61) * c1 / 2 + + (25 * a_3 - 195 * a_2 + 477 * a - 379) / 12); + + double y_2 = y * y; + double y_3 = y_2 * y; + double y_4 = y_2 * y_2; + result = y + c1 + (c2 / y) + (c3 / y_2) + (c4 / y_3) + (c5 / y_4); + } + } + else { + /* DiDonato and Morris Eq 31: */ + double s = find_inverse_s(p, q); + + double s_2 = s * s; + double s_3 = s_2 * s; + double s_4 = s_2 * s_2; + double s_5 = s_4 * s; + double ra = sqrt(a); + + double w = a + s * ra + (s_2 - 1) / 3; + w += (s_3 - 7 * s) / (36 * ra); + w -= (3 * s_4 + 7 * s_2 - 16) / (810 * a); + w += (9 * s_5 + 256 * s_3 - 433 * s) / (38880 * a * ra); + + if ((a >= 500) && (fabs(1 - w / a) < 1e-6)) { + result = w; + } + else if (p > 0.5) { + if (w < 3 * a) { + result = w; + } + else { + double D = fmax(2, a * (a - 1)); + double lg = lgam(a); + double lb = log(q) + lg; + if (lb < -D * 2.3) { + /* DiDonato and Morris Eq 25: */ + double y = -lb; + double c1 = (a - 1) * log(y); + double c1_2 = c1 * c1; + double c1_3 = c1_2 * c1; + double c1_4 = c1_2 * c1_2; + double a_2 = a * a; + double a_3 = a_2 * a; + + double c2 = (a - 1) * (1 + c1); + double c3 = (a - 1) * (-(c1_2 / 2) + + (a - 2) * c1 + + (3 * a - 5) / 2); + double c4 = (a - 1) * ((c1_3 / 3) + - (3 * a - 5) * c1_2 / 2 + + (a_2 - 6 * a + 7) * c1 + + (11 * a_2 - 46 * a + 47) / 6); + double c5 = (a - 1) * (-(c1_4 / 4) + + (11 * a - 17) * c1_3 / 6 + + (-3 * a_2 + 13 * a -13) * c1_2 + + (2 * a_3 - 25 * a_2 + 72 * a - 61) * c1 / 2 + + (25 * a_3 - 195 * a_2 + 477 * a - 379) / 12); + + double y_2 = y * y; + double y_3 = y_2 * y; + double y_4 = y_2 * y_2; + result = y + c1 + (c2 / y) + (c3 / y_2) + (c4 / y_3) + (c5 / y_4); + } + else { + /* DiDonato and Morris Eq 33: */ + double u = -lb + (a - 1) * log(w) - log(1 + (1 - a) / (1 + w)); + result = -lb + (a - 1) * log(u) - log(1 + (1 - a) / (1 + u)); + } + } + } + else { + double z = w; + double ap1 = a + 1; + double ap2 = a + 2; + if (w < 0.15 * ap1) { + /* DiDonato and Morris Eq 35: */ + double v = log(p) + lgam(ap1); + z = exp((v + w) / a); + s = log1p(z / ap1 * (1 + z / ap2)); + z = exp((v + z - s) / a); + s = log1p(z / ap1 * (1 + z / ap2)); + z = exp((v + z - s) / a); + s = log1p(z / ap1 * (1 + z / ap2 * (1 + z / (a + 3)))); + z = exp((v + z - s) / a); + } + + if ((z <= 0.01 * ap1) || (z > 0.7 * ap1)) { + result = z; + } + else { + /* DiDonato and Morris Eq 36: */ + double ls = log(didonato_SN(a, z, 100, 1e-4)); + double v = log(p) + lgam(ap1); + z = exp((v + z - ls) / a); + result = z * (1 - (a * log(z) - z - v + ls) / (a - z)); + } + } + } + return result; +} + + +double igami(double a, double p) +{ + int i; + double x, fac, f_fp, fpp_fp; + + if (isnan(a) || isnan(p)) { + return NAN; + } + else if ((a < 0) || (p < 0) || (p > 1)) { + sf_error("gammaincinv", SF_ERROR_DOMAIN, NULL); + } + else if (p == 0.0) { + return 0.0; + } + else if (p == 1.0) { + return INFINITY; + } + else if (p > 0.9) { + return igamci(a, 1 - p); + } + + x = find_inverse_gamma(a, p, 1 - p); + /* Halley's method */ + for (i = 0; i < 3; i++) { + fac = igam_fac(a, x); + if (fac == 0.0) { + return x; + } + f_fp = (igam(a, x) - p) * x / fac; + /* The ratio of the first and second derivatives simplifies */ + fpp_fp = -1.0 + (a - 1) / x; + if (isinf(fpp_fp)) { + /* Resort to Newton's method in the case of overflow */ + x = x - f_fp; + } + else { + x = x - f_fp / (1.0 - 0.5 * f_fp * fpp_fp); + } + } + + return x; +} + + +double igamci(double a, double q) +{ + int i; + double x, fac, f_fp, fpp_fp; + + if (isnan(a) || isnan(q)) { + return NAN; + } + else if ((a < 0.0) || (q < 0.0) || (q > 1.0)) { + sf_error("gammainccinv", SF_ERROR_DOMAIN, NULL); + } + else if (q == 0.0) { + return INFINITY; + } + else if (q == 1.0) { + return 0.0; + } + else if (q > 0.9) { + return igami(a, 1 - q); + } + + x = find_inverse_gamma(a, 1 - q, q); + for (i = 0; i < 3; i++) { + fac = igam_fac(a, x); + if (fac == 0.0) { + return x; + } + f_fp = (igamc(a, x) - q) * x / (-fac); + fpp_fp = -1.0 + (a - 1) / x; + if (isinf(fpp_fp)) { + x = x - f_fp; + } + else { + x = x - f_fp / (1.0 - 0.5 * f_fp * fpp_fp); + } + } + + return x; +} diff --git a/gtsam/3rdparty/cephes/cephes/incbet.c b/gtsam/3rdparty/cephes/cephes/incbet.c new file mode 100644 index 000000000..b03427f4f --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/incbet.c @@ -0,0 +1,369 @@ +/* incbet.c + * + * Incomplete beta integral + * + * + * SYNOPSIS: + * + * double a, b, x, y, incbet(); + * + * y = incbet( a, b, x ); + * + * + * DESCRIPTION: + * + * Returns incomplete beta integral of the arguments, evaluated + * from zero to x. The function is defined as + * + * x + * - - + * | (a+b) | | a-1 b-1 + * ----------- | t (1-t) dt. + * - - | | + * | (a) | (b) - + * 0 + * + * The domain of definition is 0 <= x <= 1. In this + * implementation a and b are restricted to positive values. + * The integral from x to 1 may be obtained by the symmetry + * relation + * + * 1 - incbet( a, b, x ) = incbet( b, a, 1-x ). + * + * The integral is evaluated by a continued fraction expansion + * or, when b*x is small, by a power series. + * + * ACCURACY: + * + * Tested at uniformly distributed random points (a,b,x) with a and b + * in "domain" and x between 0 and 1. + * Relative error + * arithmetic domain # trials peak rms + * IEEE 0,5 10000 6.9e-15 4.5e-16 + * IEEE 0,85 250000 2.2e-13 1.7e-14 + * IEEE 0,1000 30000 5.3e-12 6.3e-13 + * IEEE 0,10000 250000 9.3e-11 7.1e-12 + * IEEE 0,100000 10000 8.7e-10 4.8e-11 + * Outputs smaller than the IEEE gradual underflow threshold + * were excluded from these statistics. + * + * ERROR MESSAGES: + * message condition value returned + * incbet domain x<0, x>1 0.0 + * incbet underflow 0.0 + */ + + +/* + * Cephes Math Library, Release 2.3: March, 1995 + * Copyright 1984, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +#define MAXGAM 171.624376956302725 + +extern double MACHEP, MINLOG, MAXLOG; + +static double big = 4.503599627370496e15; +static double biginv = 2.22044604925031308085e-16; + + +/* Power series for incomplete beta integral. + * Use when b*x is small and x not too close to 1. */ + +static double pseries(double a, double b, double x) +{ + double s, t, u, v, n, t1, z, ai; + + ai = 1.0 / a; + u = (1.0 - b) * x; + v = u / (a + 1.0); + t1 = v; + t = u; + n = 2.0; + s = 0.0; + z = MACHEP * ai; + while (fabs(v) > z) { + u = (n - b) * x / n; + t *= u; + v = t / (a + n); + s += v; + n += 1.0; + } + s += t1; + s += ai; + + u = a * log(x); + if ((a + b) < MAXGAM && fabs(u) < MAXLOG) { + t = 1.0 / beta(a, b); + s = s * t * pow(x, a); + } + else { + t = -lbeta(a,b) + u + log(s); + if (t < MINLOG) + s = 0.0; + else + s = exp(t); + } + return (s); +} + + +/* Continued fraction expansion #1 for incomplete beta integral */ + +static double incbcf(double a, double b, double x) +{ + double xk, pk, pkm1, pkm2, qk, qkm1, qkm2; + double k1, k2, k3, k4, k5, k6, k7, k8; + double r, t, ans, thresh; + int n; + + k1 = a; + k2 = a + b; + k3 = a; + k4 = a + 1.0; + k5 = 1.0; + k6 = b - 1.0; + k7 = k4; + k8 = a + 2.0; + + pkm2 = 0.0; + qkm2 = 1.0; + pkm1 = 1.0; + qkm1 = 1.0; + ans = 1.0; + r = 1.0; + n = 0; + thresh = 3.0 * MACHEP; + do { + + xk = -(x * k1 * k2) / (k3 * k4); + pk = pkm1 + pkm2 * xk; + qk = qkm1 + qkm2 * xk; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + + xk = (x * k5 * k6) / (k7 * k8); + pk = pkm1 + pkm2 * xk; + qk = qkm1 + qkm2 * xk; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + + if (qk != 0) + r = pk / qk; + if (r != 0) { + t = fabs((ans - r) / r); + ans = r; + } + else + t = 1.0; + + if (t < thresh) + goto cdone; + + k1 += 1.0; + k2 += 1.0; + k3 += 2.0; + k4 += 2.0; + k5 += 1.0; + k6 -= 1.0; + k7 += 2.0; + k8 += 2.0; + + if ((fabs(qk) + fabs(pk)) > big) { + pkm2 *= biginv; + pkm1 *= biginv; + qkm2 *= biginv; + qkm1 *= biginv; + } + if ((fabs(qk) < biginv) || (fabs(pk) < biginv)) { + pkm2 *= big; + pkm1 *= big; + qkm2 *= big; + qkm1 *= big; + } + } + while (++n < 300); + + cdone: + return (ans); +} + + +/* Continued fraction expansion #2 for incomplete beta integral */ + +static double incbd(double a, double b, double x) +{ + double xk, pk, pkm1, pkm2, qk, qkm1, qkm2; + double k1, k2, k3, k4, k5, k6, k7, k8; + double r, t, ans, z, thresh; + int n; + + k1 = a; + k2 = b - 1.0; + k3 = a; + k4 = a + 1.0; + k5 = 1.0; + k6 = a + b; + k7 = a + 1.0;; + k8 = a + 2.0; + + pkm2 = 0.0; + qkm2 = 1.0; + pkm1 = 1.0; + qkm1 = 1.0; + z = x / (1.0 - x); + ans = 1.0; + r = 1.0; + n = 0; + thresh = 3.0 * MACHEP; + do { + + xk = -(z * k1 * k2) / (k3 * k4); + pk = pkm1 + pkm2 * xk; + qk = qkm1 + qkm2 * xk; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + + xk = (z * k5 * k6) / (k7 * k8); + pk = pkm1 + pkm2 * xk; + qk = qkm1 + qkm2 * xk; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + + if (qk != 0) + r = pk / qk; + if (r != 0) { + t = fabs((ans - r) / r); + ans = r; + } + else + t = 1.0; + + if (t < thresh) + goto cdone; + + k1 += 1.0; + k2 -= 1.0; + k3 += 2.0; + k4 += 2.0; + k5 += 1.0; + k6 += 1.0; + k7 += 2.0; + k8 += 2.0; + + if ((fabs(qk) + fabs(pk)) > big) { + pkm2 *= biginv; + pkm1 *= biginv; + qkm2 *= biginv; + qkm1 *= biginv; + } + if ((fabs(qk) < biginv) || (fabs(pk) < biginv)) { + pkm2 *= big; + pkm1 *= big; + qkm2 *= big; + qkm1 *= big; + } + } + while (++n < 300); + cdone: + return (ans); +} + + +double incbet(double aa, double bb, double xx) +{ + double a, b, t, x, xc, w, y; + int flag; + + if (aa <= 0.0 || bb <= 0.0) + goto domerr; + + if ((xx <= 0.0) || (xx >= 1.0)) { + if (xx == 0.0) + return (0.0); + if (xx == 1.0) + return (1.0); + domerr: + sf_error("incbet", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + flag = 0; + if ((bb * xx) <= 1.0 && xx <= 0.95) { + t = pseries(aa, bb, xx); + goto done; + } + + w = 1.0 - xx; + + /* Reverse a and b if x is greater than the mean. */ + if (xx > (aa / (aa + bb))) { + flag = 1; + a = bb; + b = aa; + xc = xx; + x = w; + } + else { + a = aa; + b = bb; + xc = w; + x = xx; + } + + if (flag == 1 && (b * x) <= 1.0 && x <= 0.95) { + t = pseries(a, b, x); + goto done; + } + + /* Choose expansion for better convergence. */ + y = x * (a + b - 2.0) - (a - 1.0); + if (y < 0.0) + w = incbcf(a, b, x); + else + w = incbd(a, b, x) / xc; + + /* Multiply w by the factor + * a b _ _ _ + * x (1-x) | (a+b) / ( a | (a) | (b) ) . */ + + y = a * log(x); + t = b * log(xc); + if ((a + b) < MAXGAM && fabs(y) < MAXLOG && fabs(t) < MAXLOG) { + t = pow(xc, b); + t *= pow(x, a); + t /= a; + t *= w; + t *= 1.0 / beta(a, b); + goto done; + } + /* Resort to logarithms. */ + y += t - lbeta(a,b); + y += log(w / a); + if (y < MINLOG) + t = 0.0; + else + t = exp(y); + + done: + + if (flag == 1) { + if (t <= MACHEP) + t = 1.0 - MACHEP; + else + t = 1.0 - t; + } + return (t); +} + + diff --git a/gtsam/3rdparty/cephes/cephes/incbi.c b/gtsam/3rdparty/cephes/cephes/incbi.c new file mode 100644 index 000000000..747c43f53 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/incbi.c @@ -0,0 +1,275 @@ +/* incbi() + * + * Inverse of incomplete beta integral + * + * + * + * SYNOPSIS: + * + * double a, b, x, y, incbi(); + * + * x = incbi( a, b, y ); + * + * + * + * DESCRIPTION: + * + * Given y, the function finds x such that + * + * incbet( a, b, x ) = y . + * + * The routine performs interval halving or Newton iterations to find the + * root of incbet(a,b,x) - y = 0. + * + * + * ACCURACY: + * + * Relative error: + * x a,b + * arithmetic domain domain # trials peak rms + * IEEE 0,1 .5,10000 50000 5.8e-12 1.3e-13 + * IEEE 0,1 .25,100 100000 1.8e-13 3.9e-15 + * IEEE 0,1 0,5 50000 1.1e-12 5.5e-15 + * VAX 0,1 .5,100 25000 3.5e-14 1.1e-15 + * With a and b constrained to half-integer or integer values: + * IEEE 0,1 .5,10000 50000 5.8e-12 1.1e-13 + * IEEE 0,1 .5,100 100000 1.7e-14 7.9e-16 + * With a = .5, b constrained to half-integer or integer values: + * IEEE 0,1 .5,10000 10000 8.3e-11 1.0e-11 + */ + + +/* + * Cephes Math Library Release 2.4: March,1996 + * Copyright 1984, 1996 by Stephen L. Moshier + */ + +#include "mconf.h" + +extern double MACHEP, MAXLOG, MINLOG; + +double incbi(double aa, double bb, double yy0) +{ + double a, b, y0, d, y, x, x0, x1, lgm, yp, di, dithresh, yl, yh, xt; + int i, rflg, dir, nflg; + + + i = 0; + if (yy0 <= 0) + return (0.0); + if (yy0 >= 1.0) + return (1.0); + x0 = 0.0; + yl = 0.0; + x1 = 1.0; + yh = 1.0; + nflg = 0; + + if (aa <= 1.0 || bb <= 1.0) { + dithresh = 1.0e-6; + rflg = 0; + a = aa; + b = bb; + y0 = yy0; + x = a / (a + b); + y = incbet(a, b, x); + goto ihalve; + } + else { + dithresh = 1.0e-4; + } + /* approximation to inverse function */ + + yp = -ndtri(yy0); + + if (yy0 > 0.5) { + rflg = 1; + a = bb; + b = aa; + y0 = 1.0 - yy0; + yp = -yp; + } + else { + rflg = 0; + a = aa; + b = bb; + y0 = yy0; + } + + lgm = (yp * yp - 3.0) / 6.0; + x = 2.0 / (1.0 / (2.0 * a - 1.0) + 1.0 / (2.0 * b - 1.0)); + d = yp * sqrt(x + lgm) / x + - (1.0 / (2.0 * b - 1.0) - 1.0 / (2.0 * a - 1.0)) + * (lgm + 5.0 / 6.0 - 2.0 / (3.0 * x)); + d = 2.0 * d; + if (d < MINLOG) { + x = 1.0; + goto under; + } + x = a / (a + b * exp(d)); + y = incbet(a, b, x); + yp = (y - y0) / y0; + if (fabs(yp) < 0.2) + goto newt; + + /* Resort to interval halving if not close enough. */ + ihalve: + + dir = 0; + di = 0.5; + for (i = 0; i < 100; i++) { + if (i != 0) { + x = x0 + di * (x1 - x0); + if (x == 1.0) + x = 1.0 - MACHEP; + if (x == 0.0) { + di = 0.5; + x = x0 + di * (x1 - x0); + if (x == 0.0) + goto under; + } + y = incbet(a, b, x); + yp = (x1 - x0) / (x1 + x0); + if (fabs(yp) < dithresh) + goto newt; + yp = (y - y0) / y0; + if (fabs(yp) < dithresh) + goto newt; + } + if (y < y0) { + x0 = x; + yl = y; + if (dir < 0) { + dir = 0; + di = 0.5; + } + else if (dir > 3) + di = 1.0 - (1.0 - di) * (1.0 - di); + else if (dir > 1) + di = 0.5 * di + 0.5; + else + di = (y0 - y) / (yh - yl); + dir += 1; + if (x0 > 0.75) { + if (rflg == 1) { + rflg = 0; + a = aa; + b = bb; + y0 = yy0; + } + else { + rflg = 1; + a = bb; + b = aa; + y0 = 1.0 - yy0; + } + x = 1.0 - x; + y = incbet(a, b, x); + x0 = 0.0; + yl = 0.0; + x1 = 1.0; + yh = 1.0; + goto ihalve; + } + } + else { + x1 = x; + if (rflg == 1 && x1 < MACHEP) { + x = 0.0; + goto done; + } + yh = y; + if (dir > 0) { + dir = 0; + di = 0.5; + } + else if (dir < -3) + di = di * di; + else if (dir < -1) + di = 0.5 * di; + else + di = (y - y0) / (yh - yl); + dir -= 1; + } + } + sf_error("incbi", SF_ERROR_LOSS, NULL); + if (x0 >= 1.0) { + x = 1.0 - MACHEP; + goto done; + } + if (x <= 0.0) { + under: + sf_error("incbi", SF_ERROR_UNDERFLOW, NULL); + x = 0.0; + goto done; + } + + newt: + + if (nflg) + goto done; + nflg = 1; + lgm = lgam(a + b) - lgam(a) - lgam(b); + + for (i = 0; i < 8; i++) { + /* Compute the function at this point. */ + if (i != 0) + y = incbet(a, b, x); + if (y < yl) { + x = x0; + y = yl; + } + else if (y > yh) { + x = x1; + y = yh; + } + else if (y < y0) { + x0 = x; + yl = y; + } + else { + x1 = x; + yh = y; + } + if (x == 1.0 || x == 0.0) + break; + /* Compute the derivative of the function at this point. */ + d = (a - 1.0) * log(x) + (b - 1.0) * log(1.0 - x) + lgm; + if (d < MINLOG) + goto done; + if (d > MAXLOG) + break; + d = exp(d); + /* Compute the step to the next approximation of x. */ + d = (y - y0) / d; + xt = x - d; + if (xt <= x0) { + y = (x - x0) / (x1 - x0); + xt = x0 + 0.5 * y * (x - x0); + if (xt <= 0.0) + break; + } + if (xt >= x1) { + y = (x1 - x) / (x1 - x0); + xt = x1 - 0.5 * y * (x1 - x); + if (xt >= 1.0) + break; + } + x = xt; + if (fabs(d / x) < 128.0 * MACHEP) + goto done; + } + /* Did not converge. */ + dithresh = 256.0 * MACHEP; + goto ihalve; + + done: + + if (rflg) { + if (x <= MACHEP) + x = 1.0 - MACHEP; + else + x = 1.0 - x; + } + return (x); +} diff --git a/gtsam/3rdparty/cephes/cephes/j0.c b/gtsam/3rdparty/cephes/cephes/j0.c new file mode 100644 index 000000000..094ef6cef --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/j0.c @@ -0,0 +1,246 @@ +/* j0.c + * + * Bessel function of order zero + * + * + * + * SYNOPSIS: + * + * double x, y, j0(); + * + * y = j0( x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of order zero of the argument. + * + * The domain is divided into the intervals [0, 5] and + * (5, infinity). In the first interval the following rational + * approximation is used: + * + * + * 2 2 + * (w - r ) (w - r ) P (w) / Q (w) + * 1 2 3 8 + * + * 2 + * where w = x and the two r's are zeros of the function. + * + * In the second interval, the Hankel asymptotic expansion + * is employed with two rational functions of degree 6/6 + * and 7/7. + * + * + * + * ACCURACY: + * + * Absolute error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 60000 4.2e-16 1.1e-16 + * + */ + /* y0.c + * + * Bessel function of the second kind, order zero + * + * + * + * SYNOPSIS: + * + * double x, y, y0(); + * + * y = y0( x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of the second kind, of order + * zero, of the argument. + * + * The domain is divided into the intervals [0, 5] and + * (5, infinity). In the first interval a rational approximation + * R(x) is employed to compute + * y0(x) = R(x) + 2 * log(x) * j0(x) / M_PI. + * Thus a call to j0() is required. + * + * In the second interval, the Hankel asymptotic expansion + * is employed with two rational functions of degree 6/6 + * and 7/7. + * + * + * + * ACCURACY: + * + * Absolute error, when y0(x) < 1; else relative error: + * + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.3e-15 1.6e-16 + * + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1989, 2000 by Stephen L. Moshier + */ + +/* Note: all coefficients satisfy the relative error criterion + * except YP, YQ which are designed for absolute error. */ + +#include "mconf.h" + +static double PP[7] = { + 7.96936729297347051624E-4, + 8.28352392107440799803E-2, + 1.23953371646414299388E0, + 5.44725003058768775090E0, + 8.74716500199817011941E0, + 5.30324038235394892183E0, + 9.99999999999999997821E-1, +}; + +static double PQ[7] = { + 9.24408810558863637013E-4, + 8.56288474354474431428E-2, + 1.25352743901058953537E0, + 5.47097740330417105182E0, + 8.76190883237069594232E0, + 5.30605288235394617618E0, + 1.00000000000000000218E0, +}; + +static double QP[8] = { + -1.13663838898469149931E-2, + -1.28252718670509318512E0, + -1.95539544257735972385E1, + -9.32060152123768231369E1, + -1.77681167980488050595E2, + -1.47077505154951170175E2, + -5.14105326766599330220E1, + -6.05014350600728481186E0, +}; + +static double QQ[7] = { + /* 1.00000000000000000000E0, */ + 6.43178256118178023184E1, + 8.56430025976980587198E2, + 3.88240183605401609683E3, + 7.24046774195652478189E3, + 5.93072701187316984827E3, + 2.06209331660327847417E3, + 2.42005740240291393179E2, +}; + +static double YP[8] = { + 1.55924367855235737965E4, + -1.46639295903971606143E7, + 5.43526477051876500413E9, + -9.82136065717911466409E11, + 8.75906394395366999549E13, + -3.46628303384729719441E15, + 4.42733268572569800351E16, + -1.84950800436986690637E16, +}; + +static double YQ[7] = { + /* 1.00000000000000000000E0, */ + 1.04128353664259848412E3, + 6.26107330137134956842E5, + 2.68919633393814121987E8, + 8.64002487103935000337E10, + 2.02979612750105546709E13, + 3.17157752842975028269E15, + 2.50596256172653059228E17, +}; + +/* 5.783185962946784521175995758455807035071 */ +static double DR1 = 5.78318596294678452118E0; + +/* 30.47126234366208639907816317502275584842 */ +static double DR2 = 3.04712623436620863991E1; + +static double RP[4] = { + -4.79443220978201773821E9, + 1.95617491946556577543E12, + -2.49248344360967716204E14, + 9.70862251047306323952E15, +}; + +static double RQ[8] = { + /* 1.00000000000000000000E0, */ + 4.99563147152651017219E2, + 1.73785401676374683123E5, + 4.84409658339962045305E7, + 1.11855537045356834862E10, + 2.11277520115489217587E12, + 3.10518229857422583814E14, + 3.18121955943204943306E16, + 1.71086294081043136091E18, +}; + +extern double SQ2OPI; + +double j0(double x) +{ + double w, z, p, q, xn; + + if (x < 0) + x = -x; + + if (x <= 5.0) { + z = x * x; + if (x < 1.0e-5) + return (1.0 - z / 4.0); + + p = (z - DR1) * (z - DR2); + p = p * polevl(z, RP, 3) / p1evl(z, RQ, 8); + return (p); + } + + w = 5.0 / x; + q = 25.0 / (x * x); + p = polevl(q, PP, 6) / polevl(q, PQ, 6); + q = polevl(q, QP, 7) / p1evl(q, QQ, 7); + xn = x - M_PI_4; + p = p * cos(xn) - w * q * sin(xn); + return (p * SQ2OPI / sqrt(x)); +} + +/* y0() 2 */ +/* Bessel function of second kind, order zero */ + +/* Rational approximation coefficients YP[], YQ[] are used here. + * The function computed is y0(x) - 2 * log(x) * j0(x) / M_PI, + * whose value at x = 0 is 2 * ( log(0.5) + EUL ) / M_PI + * = 0.073804295108687225. + */ + +double y0(double x) +{ + double w, z, p, q, xn; + + if (x <= 5.0) { + if (x == 0.0) { + sf_error("y0", SF_ERROR_SINGULAR, NULL); + return -INFINITY; + } + else if (x < 0.0) { + sf_error("y0", SF_ERROR_DOMAIN, NULL); + return NAN; + } + z = x * x; + w = polevl(z, YP, 7) / p1evl(z, YQ, 7); + w += M_2_PI * log(x) * j0(x); + return (w); + } + + w = 5.0 / x; + z = 25.0 / (x * x); + p = polevl(z, PP, 6) / polevl(z, PQ, 6); + q = polevl(z, QP, 7) / p1evl(z, QQ, 7); + xn = x - M_PI_4; + p = p * sin(xn) + w * q * cos(xn); + return (p * SQ2OPI / sqrt(x)); +} diff --git a/gtsam/3rdparty/cephes/cephes/j1.c b/gtsam/3rdparty/cephes/cephes/j1.c new file mode 100644 index 000000000..123194de8 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/j1.c @@ -0,0 +1,225 @@ +/* j1.c + * + * Bessel function of order one + * + * + * + * SYNOPSIS: + * + * double x, y, j1(); + * + * y = j1( x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of order one of the argument. + * + * The domain is divided into the intervals [0, 8] and + * (8, infinity). In the first interval a 24 term Chebyshev + * expansion is used. In the second, the asymptotic + * trigonometric representation is employed using two + * rational functions of degree 5/5. + * + * + * + * ACCURACY: + * + * Absolute error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 2.6e-16 1.1e-16 + * + * + */ + /* y1.c + * + * Bessel function of second kind of order one + * + * + * + * SYNOPSIS: + * + * double x, y, y1(); + * + * y = y1( x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of the second kind of order one + * of the argument. + * + * The domain is divided into the intervals [0, 8] and + * (8, infinity). In the first interval a 25 term Chebyshev + * expansion is used, and a call to j1() is required. + * In the second, the asymptotic trigonometric representation + * is employed using two rational functions of degree 5/5. + * + * + * + * ACCURACY: + * + * Absolute error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.0e-15 1.3e-16 + * + * (error criterion relative when |y1| > 1). + * + */ + + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1989, 2000 by Stephen L. Moshier + */ + +/* + * #define PIO4 .78539816339744830962 + * #define THPIO4 2.35619449019234492885 + * #define SQ2OPI .79788456080286535588 + */ + +#include "mconf.h" + +static double RP[4] = { + -8.99971225705559398224E8, + 4.52228297998194034323E11, + -7.27494245221818276015E13, + 3.68295732863852883286E15, +}; + +static double RQ[8] = { + /* 1.00000000000000000000E0, */ + 6.20836478118054335476E2, + 2.56987256757748830383E5, + 8.35146791431949253037E7, + 2.21511595479792499675E10, + 4.74914122079991414898E12, + 7.84369607876235854894E14, + 8.95222336184627338078E16, + 5.32278620332680085395E18, +}; + +static double PP[7] = { + 7.62125616208173112003E-4, + 7.31397056940917570436E-2, + 1.12719608129684925192E0, + 5.11207951146807644818E0, + 8.42404590141772420927E0, + 5.21451598682361504063E0, + 1.00000000000000000254E0, +}; + +static double PQ[7] = { + 5.71323128072548699714E-4, + 6.88455908754495404082E-2, + 1.10514232634061696926E0, + 5.07386386128601488557E0, + 8.39985554327604159757E0, + 5.20982848682361821619E0, + 9.99999999999999997461E-1, +}; + +static double QP[8] = { + 5.10862594750176621635E-2, + 4.98213872951233449420E0, + 7.58238284132545283818E1, + 3.66779609360150777800E2, + 7.10856304998926107277E2, + 5.97489612400613639965E2, + 2.11688757100572135698E2, + 2.52070205858023719784E1, +}; + +static double QQ[7] = { + /* 1.00000000000000000000E0, */ + 7.42373277035675149943E1, + 1.05644886038262816351E3, + 4.98641058337653607651E3, + 9.56231892404756170795E3, + 7.99704160447350683650E3, + 2.82619278517639096600E3, + 3.36093607810698293419E2, +}; + +static double YP[6] = { + 1.26320474790178026440E9, + -6.47355876379160291031E11, + 1.14509511541823727583E14, + -8.12770255501325109621E15, + 2.02439475713594898196E17, + -7.78877196265950026825E17, +}; + +static double YQ[8] = { + /* 1.00000000000000000000E0, */ + 5.94301592346128195359E2, + 2.35564092943068577943E5, + 7.34811944459721705660E7, + 1.87601316108706159478E10, + 3.88231277496238566008E12, + 6.20557727146953693363E14, + 6.87141087355300489866E16, + 3.97270608116560655612E18, +}; + + +static double Z1 = 1.46819706421238932572E1; +static double Z2 = 4.92184563216946036703E1; + +extern double THPIO4, SQ2OPI; + +double j1(double x) +{ + double w, z, p, q, xn; + + w = x; + if (x < 0) + return -j1(-x); + + if (w <= 5.0) { + z = x * x; + w = polevl(z, RP, 3) / p1evl(z, RQ, 8); + w = w * x * (z - Z1) * (z - Z2); + return (w); + } + + w = 5.0 / x; + z = w * w; + p = polevl(z, PP, 6) / polevl(z, PQ, 6); + q = polevl(z, QP, 7) / p1evl(z, QQ, 7); + xn = x - THPIO4; + p = p * cos(xn) - w * q * sin(xn); + return (p * SQ2OPI / sqrt(x)); +} + + +double y1(double x) +{ + double w, z, p, q, xn; + + if (x <= 5.0) { + if (x == 0.0) { + sf_error("y1", SF_ERROR_SINGULAR, NULL); + return -INFINITY; + } + else if (x <= 0.0) { + sf_error("y1", SF_ERROR_DOMAIN, NULL); + return NAN; + } + z = x * x; + w = x * (polevl(z, YP, 5) / p1evl(z, YQ, 8)); + w += M_2_PI * (j1(x) * log(x) - 1.0 / x); + return (w); + } + + w = 5.0 / x; + z = w * w; + p = polevl(z, PP, 6) / polevl(z, PQ, 6); + q = polevl(z, QP, 7) / p1evl(z, QQ, 7); + xn = x - THPIO4; + p = p * sin(xn) + w * q * cos(xn); + return (p * SQ2OPI / sqrt(x)); +} diff --git a/gtsam/3rdparty/cephes/cephes/jv.c b/gtsam/3rdparty/cephes/cephes/jv.c new file mode 100644 index 000000000..3434c18f3 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/jv.c @@ -0,0 +1,841 @@ +/* jv.c + * + * Bessel function of noninteger order + * + * + * + * SYNOPSIS: + * + * double v, x, y, jv(); + * + * y = jv( v, x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of order v of the argument, + * where v is real. Negative x is allowed if v is an integer. + * + * Several expansions are included: the ascending power + * series, the Hankel expansion, and two transitional + * expansions for large v. If v is not too large, it + * is reduced by recurrence to a region of best accuracy. + * The transitional expansions give 12D accuracy for v > 500. + * + * + * + * ACCURACY: + * Results for integer v are indicated by *, where x and v + * both vary from -125 to +125. Otherwise, + * x ranges from 0 to 125, v ranges as indicated by "domain." + * Error criterion is absolute, except relative when |jv()| > 1. + * + * arithmetic v domain x domain # trials peak rms + * IEEE 0,125 0,125 100000 4.6e-15 2.2e-16 + * IEEE -125,0 0,125 40000 5.4e-11 3.7e-13 + * IEEE 0,500 0,500 20000 4.4e-15 4.0e-16 + * Integer v: + * IEEE -125,125 -125,125 50000 3.5e-15* 1.9e-16* + * + */ + + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1989, 1992, 2000 by Stephen L. Moshier + */ + + +#include "mconf.h" +#define CEPHES_DEBUG 0 + +#if CEPHES_DEBUG +#include +#endif + +#define MAXGAM 171.624376956302725 + +extern double MACHEP, MINLOG, MAXLOG; + +#define BIG 1.44115188075855872E+17 + +static double jvs(double n, double x); +static double hankel(double n, double x); +static double recur(double *n, double x, double *newn, int cancel); +static double jnx(double n, double x); +static double jnt(double n, double x); + +double jv(double n, double x) +{ + double k, q, t, y, an; + int i, sign, nint; + + nint = 0; /* Flag for integer n */ + sign = 1; /* Flag for sign inversion */ + an = fabs(n); + y = floor(an); + if (y == an) { + nint = 1; + i = an - 16384.0 * floor(an / 16384.0); + if (n < 0.0) { + if (i & 1) + sign = -sign; + n = an; + } + if (x < 0.0) { + if (i & 1) + sign = -sign; + x = -x; + } + if (n == 0.0) + return (j0(x)); + if (n == 1.0) + return (sign * j1(x)); + } + + if ((x < 0.0) && (y != an)) { + sf_error("Jv", SF_ERROR_DOMAIN, NULL); + y = NAN; + goto done; + } + + if (x == 0 && n < 0 && !nint) { + sf_error("Jv", SF_ERROR_OVERFLOW, NULL); + return INFINITY / gamma(n + 1); + } + + y = fabs(x); + + if (y * y < fabs(n + 1) * MACHEP) { + return pow(0.5 * x, n) / gamma(n + 1); + } + + k = 3.6 * sqrt(y); + t = 3.6 * sqrt(an); + if ((y < t) && (an > 21.0)) + return (sign * jvs(n, x)); + if ((an < k) && (y > 21.0)) + return (sign * hankel(n, x)); + + if (an < 500.0) { + /* Note: if x is too large, the continued fraction will fail; but then the + * Hankel expansion can be used. */ + if (nint != 0) { + k = 0.0; + q = recur(&n, x, &k, 1); + if (k == 0.0) { + y = j0(x) / q; + goto done; + } + if (k == 1.0) { + y = j1(x) / q; + goto done; + } + } + + if (an > 2.0 * y) + goto rlarger; + + if ((n >= 0.0) && (n < 20.0) + && (y > 6.0) && (y < 20.0)) { + /* Recur backwards from a larger value of n */ + rlarger: + k = n; + + y = y + an + 1.0; + if (y < 30.0) + y = 30.0; + y = n + floor(y - n); + q = recur(&y, x, &k, 0); + y = jvs(y, x) * q; + goto done; + } + + if (k <= 30.0) { + k = 2.0; + } + else if (k < 90.0) { + k = (3 * k) / 4; + } + if (an > (k + 3.0)) { + if (n < 0.0) + k = -k; + q = n - floor(n); + k = floor(k) + q; + if (n > 0.0) + q = recur(&n, x, &k, 1); + else { + t = k; + k = n; + q = recur(&t, x, &k, 1); + k = t; + } + if (q == 0.0) { + y = 0.0; + goto done; + } + } + else { + k = n; + q = 1.0; + } + + /* boundary between convergence of + * power series and Hankel expansion + */ + y = fabs(k); + if (y < 26.0) + t = (0.0083 * y + 0.09) * y + 12.9; + else + t = 0.9 * y; + + if (x > t) + y = hankel(k, x); + else + y = jvs(k, x); +#if CEPHES_DEBUG + printf("y = %.16e, recur q = %.16e\n", y, q); +#endif + if (n > 0.0) + y /= q; + else + y *= q; + } + + else { + /* For large n, use the uniform expansion or the transitional expansion. + * But if x is of the order of n**2, these may blow up, whereas the + * Hankel expansion will then work. + */ + if (n < 0.0) { + sf_error("Jv", SF_ERROR_LOSS, NULL); + y = NAN; + goto done; + } + t = x / n; + t /= n; + if (t > 0.3) + y = hankel(n, x); + else + y = jnx(n, x); + } + + done:return (sign * y); +} + +/* Reduce the order by backward recurrence. + * AMS55 #9.1.27 and 9.1.73. + */ + +static double recur(double *n, double x, double *newn, int cancel) +{ + double pkm2, pkm1, pk, qkm2, qkm1; + + /* double pkp1; */ + double k, ans, qk, xk, yk, r, t, kf; + static double big = BIG; + int nflag, ctr; + int miniter, maxiter; + + /* Continued fraction for Jn(x)/Jn-1(x) + * AMS 9.1.73 + * + * x -x^2 -x^2 + * ------ --------- --------- ... + * 2 n + 2(n+1) + 2(n+2) + + * + * Compute it with the simplest possible algorithm. + * + * This continued fraction starts to converge when (|n| + m) > |x|. + * Hence, at least |x|-|n| iterations are necessary before convergence is + * achieved. There is a hard limit set below, m <= 30000, which is chosen + * so that no branch in `jv` requires more iterations to converge. + * The exact maximum number is (500/3.6)^2 - 500 ~ 19000 + */ + + maxiter = 22000; + miniter = fabs(x) - fabs(*n); + if (miniter < 1) + miniter = 1; + + if (*n < 0.0) + nflag = 1; + else + nflag = 0; + + fstart: + +#if CEPHES_DEBUG + printf("recur: n = %.6e, newn = %.6e, cfrac = ", *n, *newn); +#endif + + pkm2 = 0.0; + qkm2 = 1.0; + pkm1 = x; + qkm1 = *n + *n; + xk = -x * x; + yk = qkm1; + ans = 0.0; /* ans=0.0 ensures that t=1.0 in the first iteration */ + ctr = 0; + do { + yk += 2.0; + pk = pkm1 * yk + pkm2 * xk; + qk = qkm1 * yk + qkm2 * xk; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + + /* check convergence */ + if (qk != 0 && ctr > miniter) + r = pk / qk; + else + r = 0.0; + + if (r != 0) { + t = fabs((ans - r) / r); + ans = r; + } + else { + t = 1.0; + } + + if (++ctr > maxiter) { + sf_error("jv", SF_ERROR_UNDERFLOW, NULL); + goto done; + } + if (t < MACHEP) + goto done; + + /* renormalize coefficients */ + if (fabs(pk) > big) { + pkm2 /= big; + pkm1 /= big; + qkm2 /= big; + qkm1 /= big; + } + } + while (t > MACHEP); + + done: + if (ans == 0) + ans = 1.0; + +#if CEPHES_DEBUG + printf("%.6e\n", ans); +#endif + + /* Change n to n-1 if n < 0 and the continued fraction is small */ + if (nflag > 0) { + if (fabs(ans) < 0.125) { + nflag = -1; + *n = *n - 1.0; + goto fstart; + } + } + + + kf = *newn; + + /* backward recurrence + * 2k + * J (x) = --- J (x) - J (x) + * k-1 x k k+1 + */ + + pk = 1.0; + pkm1 = 1.0 / ans; + k = *n - 1.0; + r = 2 * k; + do { + pkm2 = (pkm1 * r - pk * x) / x; + /* pkp1 = pk; */ + pk = pkm1; + pkm1 = pkm2; + r -= 2.0; + /* + * t = fabs(pkp1) + fabs(pk); + * if( (k > (kf + 2.5)) && (fabs(pkm1) < 0.25*t) ) + * { + * k -= 1.0; + * t = x*x; + * pkm2 = ( (r*(r+2.0)-t)*pk - r*x*pkp1 )/t; + * pkp1 = pk; + * pk = pkm1; + * pkm1 = pkm2; + * r -= 2.0; + * } + */ + k -= 1.0; + } + while (k > (kf + 0.5)); + + /* Take the larger of the last two iterates + * on the theory that it may have less cancellation error. + */ + + if (cancel) { + if ((kf >= 0.0) && (fabs(pk) > fabs(pkm1))) { + k += 1.0; + pkm2 = pk; + } + } + *newn = k; +#if CEPHES_DEBUG + printf("newn %.6e rans %.6e\n", k, pkm2); +#endif + return (pkm2); +} + + + +/* Ascending power series for Jv(x). + * AMS55 #9.1.10. + */ + +static double jvs(double n, double x) +{ + double t, u, y, z, k; + int ex, sgngam; + + z = -x * x / 4.0; + u = 1.0; + y = u; + k = 1.0; + t = 1.0; + + while (t > MACHEP) { + u *= z / (k * (n + k)); + y += u; + k += 1.0; + if (y != 0) + t = fabs(u / y); + } +#if CEPHES_DEBUG + printf("power series=%.5e ", y); +#endif + t = frexp(0.5 * x, &ex); + ex = ex * n; + if ((ex > -1023) + && (ex < 1023) + && (n > 0.0) + && (n < (MAXGAM - 1.0))) { + t = pow(0.5 * x, n) / gamma(n + 1.0); +#if CEPHES_DEBUG + printf("pow(.5*x, %.4e)/gamma(n+1)=%.5e\n", n, t); +#endif + y *= t; + } + else { +#if CEPHES_DEBUG + z = n * log(0.5 * x); + k = lgam(n + 1.0); + t = z - k; + printf("log pow=%.5e, lgam(%.4e)=%.5e\n", z, n + 1.0, k); +#else + t = n * log(0.5 * x) - lgam_sgn(n + 1.0, &sgngam); +#endif + if (y < 0) { + sgngam = -sgngam; + y = -y; + } + t += log(y); +#if CEPHES_DEBUG + printf("log y=%.5e\n", log(y)); +#endif + if (t < -MAXLOG) { + return (0.0); + } + if (t > MAXLOG) { + sf_error("Jv", SF_ERROR_OVERFLOW, NULL); + return (INFINITY); + } + y = sgngam * exp(t); + } + return (y); +} + +/* Hankel's asymptotic expansion + * for large x. + * AMS55 #9.2.5. + */ + +static double hankel(double n, double x) +{ + double t, u, z, k, sign, conv; + double p, q, j, m, pp, qq; + int flag; + + m = 4.0 * n * n; + j = 1.0; + z = 8.0 * x; + k = 1.0; + p = 1.0; + u = (m - 1.0) / z; + q = u; + sign = 1.0; + conv = 1.0; + flag = 0; + t = 1.0; + pp = 1.0e38; + qq = 1.0e38; + + while (t > MACHEP) { + k += 2.0; + j += 1.0; + sign = -sign; + u *= (m - k * k) / (j * z); + p += sign * u; + k += 2.0; + j += 1.0; + u *= (m - k * k) / (j * z); + q += sign * u; + t = fabs(u / p); + if (t < conv) { + conv = t; + qq = q; + pp = p; + flag = 1; + } + /* stop if the terms start getting larger */ + if ((flag != 0) && (t > conv)) { +#if CEPHES_DEBUG + printf("Hankel: convergence to %.4E\n", conv); +#endif + goto hank1; + } + } + + hank1: + u = x - (0.5 * n + 0.25) * M_PI; + t = sqrt(2.0 / (M_PI * x)) * (pp * cos(u) - qq * sin(u)); +#if CEPHES_DEBUG + printf("hank: %.6e\n", t); +#endif + return (t); +} + + +/* Asymptotic expansion for large n. + * AMS55 #9.3.35. + */ + +static double lambda[] = { + 1.0, + 1.041666666666666666666667E-1, + 8.355034722222222222222222E-2, + 1.282265745563271604938272E-1, + 2.918490264641404642489712E-1, + 8.816272674437576524187671E-1, + 3.321408281862767544702647E+0, + 1.499576298686255465867237E+1, + 7.892301301158651813848139E+1, + 4.744515388682643231611949E+2, + 3.207490090890661934704328E+3 +}; + +static double mu[] = { + 1.0, + -1.458333333333333333333333E-1, + -9.874131944444444444444444E-2, + -1.433120539158950617283951E-1, + -3.172272026784135480967078E-1, + -9.424291479571202491373028E-1, + -3.511203040826354261542798E+0, + -1.572726362036804512982712E+1, + -8.228143909718594444224656E+1, + -4.923553705236705240352022E+2, + -3.316218568547972508762102E+3 +}; + +static double P1[] = { + -2.083333333333333333333333E-1, + 1.250000000000000000000000E-1 +}; + +static double P2[] = { + 3.342013888888888888888889E-1, + -4.010416666666666666666667E-1, + 7.031250000000000000000000E-2 +}; + +static double P3[] = { + -1.025812596450617283950617E+0, + 1.846462673611111111111111E+0, + -8.912109375000000000000000E-1, + 7.324218750000000000000000E-2 +}; + +static double P4[] = { + 4.669584423426247427983539E+0, + -1.120700261622299382716049E+1, + 8.789123535156250000000000E+0, + -2.364086914062500000000000E+0, + 1.121520996093750000000000E-1 +}; + +static double P5[] = { + -2.8212072558200244877E1, + 8.4636217674600734632E1, + -9.1818241543240017361E1, + 4.2534998745388454861E1, + -7.3687943594796316964E0, + 2.27108001708984375E-1 +}; + +static double P6[] = { + 2.1257013003921712286E2, + -7.6525246814118164230E2, + 1.0599904525279998779E3, + -6.9957962737613254123E2, + 2.1819051174421159048E2, + -2.6491430486951555525E1, + 5.7250142097473144531E-1 +}; + +static double P7[] = { + -1.9194576623184069963E3, + 8.0617221817373093845E3, + -1.3586550006434137439E4, + 1.1655393336864533248E4, + -5.3056469786134031084E3, + 1.2009029132163524628E3, + -1.0809091978839465550E2, + 1.7277275025844573975E0 +}; + + +static double jnx(double n, double x) +{ + double zeta, sqz, zz, zp, np; + double cbn, n23, t, z, sz; + double pp, qq, z32i, zzi; + double ak, bk, akl, bkl; + int sign, doa, dob, nflg, k, s, tk, tkp1, m; + static double u[8]; + static double ai, aip, bi, bip; + + /* Test for x very close to n. Use expansion for transition region if so. */ + cbn = cbrt(n); + z = (x - n) / cbn; + if (fabs(z) <= 0.7) + return (jnt(n, x)); + + z = x / n; + zz = 1.0 - z * z; + if (zz == 0.0) + return (0.0); + + if (zz > 0.0) { + sz = sqrt(zz); + t = 1.5 * (log((1.0 + sz) / z) - sz); /* zeta ** 3/2 */ + zeta = cbrt(t * t); + nflg = 1; + } + else { + sz = sqrt(-zz); + t = 1.5 * (sz - acos(1.0 / z)); + zeta = -cbrt(t * t); + nflg = -1; + } + z32i = fabs(1.0 / t); + sqz = cbrt(t); + + /* Airy function */ + n23 = cbrt(n * n); + t = n23 * zeta; + +#if CEPHES_DEBUG + printf("zeta %.5E, Airy(%.5E)\n", zeta, t); +#endif + airy(t, &ai, &aip, &bi, &bip); + + /* polynomials in expansion */ + u[0] = 1.0; + zzi = 1.0 / zz; + u[1] = polevl(zzi, P1, 1) / sz; + u[2] = polevl(zzi, P2, 2) / zz; + u[3] = polevl(zzi, P3, 3) / (sz * zz); + pp = zz * zz; + u[4] = polevl(zzi, P4, 4) / pp; + u[5] = polevl(zzi, P5, 5) / (pp * sz); + pp *= zz; + u[6] = polevl(zzi, P6, 6) / pp; + u[7] = polevl(zzi, P7, 7) / (pp * sz); + +#if CEPHES_DEBUG + for (k = 0; k <= 7; k++) + printf("u[%d] = %.5E\n", k, u[k]); +#endif + + pp = 0.0; + qq = 0.0; + np = 1.0; + /* flags to stop when terms get larger */ + doa = 1; + dob = 1; + akl = INFINITY; + bkl = INFINITY; + + for (k = 0; k <= 3; k++) { + tk = 2 * k; + tkp1 = tk + 1; + zp = 1.0; + ak = 0.0; + bk = 0.0; + for (s = 0; s <= tk; s++) { + if (doa) { + if ((s & 3) > 1) + sign = nflg; + else + sign = 1; + ak += sign * mu[s] * zp * u[tk - s]; + } + + if (dob) { + m = tkp1 - s; + if (((m + 1) & 3) > 1) + sign = nflg; + else + sign = 1; + bk += sign * lambda[s] * zp * u[m]; + } + zp *= z32i; + } + + if (doa) { + ak *= np; + t = fabs(ak); + if (t < akl) { + akl = t; + pp += ak; + } + else + doa = 0; + } + + if (dob) { + bk += lambda[tkp1] * zp * u[0]; + bk *= -np / sqz; + t = fabs(bk); + if (t < bkl) { + bkl = t; + qq += bk; + } + else + dob = 0; + } +#if CEPHES_DEBUG + printf("a[%d] %.5E, b[%d] %.5E\n", k, ak, k, bk); +#endif + if (np < MACHEP) + break; + np /= n * n; + } + + /* normalizing factor ( 4*zeta/(1 - z**2) )**1/4 */ + t = 4.0 * zeta / zz; + t = sqrt(sqrt(t)); + + t *= ai * pp / cbrt(n) + aip * qq / (n23 * n); + return (t); +} + +/* Asymptotic expansion for transition region, + * n large and x close to n. + * AMS55 #9.3.23. + */ + +static double PF2[] = { + -9.0000000000000000000e-2, + 8.5714285714285714286e-2 +}; + +static double PF3[] = { + 1.3671428571428571429e-1, + -5.4920634920634920635e-2, + -4.4444444444444444444e-3 +}; + +static double PF4[] = { + 1.3500000000000000000e-3, + -1.6036054421768707483e-1, + 4.2590187590187590188e-2, + 2.7330447330447330447e-3 +}; + +static double PG1[] = { + -2.4285714285714285714e-1, + 1.4285714285714285714e-2 +}; + +static double PG2[] = { + -9.0000000000000000000e-3, + 1.9396825396825396825e-1, + -1.1746031746031746032e-2 +}; + +static double PG3[] = { + 1.9607142857142857143e-2, + -1.5983694083694083694e-1, + 6.3838383838383838384e-3 +}; + + +static double jnt(double n, double x) +{ + double z, zz, z3; + double cbn, n23, cbtwo; + double ai, aip, bi, bip; /* Airy functions */ + double nk, fk, gk, pp, qq; + double F[5], G[4]; + int k; + + cbn = cbrt(n); + z = (x - n) / cbn; + cbtwo = cbrt(2.0); + + /* Airy function */ + zz = -cbtwo * z; + airy(zz, &ai, &aip, &bi, &bip); + + /* polynomials in expansion */ + zz = z * z; + z3 = zz * z; + F[0] = 1.0; + F[1] = -z / 5.0; + F[2] = polevl(z3, PF2, 1) * zz; + F[3] = polevl(z3, PF3, 2); + F[4] = polevl(z3, PF4, 3) * z; + G[0] = 0.3 * zz; + G[1] = polevl(z3, PG1, 1); + G[2] = polevl(z3, PG2, 2) * z; + G[3] = polevl(z3, PG3, 2) * zz; +#if CEPHES_DEBUG + for (k = 0; k <= 4; k++) + printf("F[%d] = %.5E\n", k, F[k]); + for (k = 0; k <= 3; k++) + printf("G[%d] = %.5E\n", k, G[k]); +#endif + pp = 0.0; + qq = 0.0; + nk = 1.0; + n23 = cbrt(n * n); + + for (k = 0; k <= 4; k++) { + fk = F[k] * nk; + pp += fk; + if (k != 4) { + gk = G[k] * nk; + qq += gk; + } +#if CEPHES_DEBUG + printf("fk[%d] %.5E, gk[%d] %.5E\n", k, fk, k, gk); +#endif + nk /= n23; + } + + fk = cbtwo * ai * pp / cbn + cbrt(4.0) * aip * qq / n; + return (fk); +} diff --git a/gtsam/3rdparty/cephes/cephes/k0.c b/gtsam/3rdparty/cephes/cephes/k0.c new file mode 100644 index 000000000..c5b31a1bf --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/k0.c @@ -0,0 +1,178 @@ +/* k0.c + * + * Modified Bessel function, third kind, order zero + * + * + * + * SYNOPSIS: + * + * double x, y, k0(); + * + * y = k0( x ); + * + * + * + * DESCRIPTION: + * + * Returns modified Bessel function of the third kind + * of order zero of the argument. + * + * The range is partitioned into the two intervals [0,8] and + * (8, infinity). Chebyshev polynomial expansions are employed + * in each interval. + * + * + * + * ACCURACY: + * + * Tested at 2000 random points between 0 and 8. Peak absolute + * error (relative when K0 > 1) was 1.46e-14; rms, 4.26e-15. + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.2e-15 1.6e-16 + * + * ERROR MESSAGES: + * + * message condition value returned + * K0 domain x <= 0 INFINITY + * + */ + /* k0e() + * + * Modified Bessel function, third kind, order zero, + * exponentially scaled + * + * + * + * SYNOPSIS: + * + * double x, y, k0e(); + * + * y = k0e( x ); + * + * + * + * DESCRIPTION: + * + * Returns exponentially scaled modified Bessel function + * of the third kind of order zero of the argument. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.4e-15 1.4e-16 + * See k0(). + * + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +/* Chebyshev coefficients for K0(x) + log(x/2) I0(x) + * in the interval [0,2]. The odd order coefficients are all + * zero; only the even order coefficients are listed. + * + * lim(x->0){ K0(x) + log(x/2) I0(x) } = -EUL. + */ + +static double A[] = { + 1.37446543561352307156E-16, + 4.25981614279661018399E-14, + 1.03496952576338420167E-11, + 1.90451637722020886025E-9, + 2.53479107902614945675E-7, + 2.28621210311945178607E-5, + 1.26461541144692592338E-3, + 3.59799365153615016266E-2, + 3.44289899924628486886E-1, + -5.35327393233902768720E-1 +}; + +/* Chebyshev coefficients for exp(x) sqrt(x) K0(x) + * in the inverted interval [2,infinity]. + * + * lim(x->inf){ exp(x) sqrt(x) K0(x) } = sqrt(pi/2). + */ +static double B[] = { + 5.30043377268626276149E-18, + -1.64758043015242134646E-17, + 5.21039150503902756861E-17, + -1.67823109680541210385E-16, + 5.51205597852431940784E-16, + -1.84859337734377901440E-15, + 6.34007647740507060557E-15, + -2.22751332699166985548E-14, + 8.03289077536357521100E-14, + -2.98009692317273043925E-13, + 1.14034058820847496303E-12, + -4.51459788337394416547E-12, + 1.85594911495471785253E-11, + -7.95748924447710747776E-11, + 3.57739728140030116597E-10, + -1.69753450938905987466E-9, + 8.57403401741422608519E-9, + -4.66048989768794782956E-8, + 2.76681363944501510342E-7, + -1.83175552271911948767E-6, + 1.39498137188764993662E-5, + -1.28495495816278026384E-4, + 1.56988388573005337491E-3, + -3.14481013119645005427E-2, + 2.44030308206595545468E0 +}; + +double k0(double x) +{ + double y, z; + + if (x == 0.0) { + sf_error("k0", SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + else if (x < 0.0) { + sf_error("k0", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (x <= 2.0) { + y = x * x - 2.0; + y = chbevl(y, A, 10) - log(0.5 * x) * i0(x); + return (y); + } + z = 8.0 / x - 2.0; + y = exp(-x) * chbevl(z, B, 25) / sqrt(x); + return (y); +} + + + + +double k0e(double x) +{ + double y; + + if (x == 0.0) { + sf_error("k0e", SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + else if (x < 0.0) { + sf_error("k0e", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (x <= 2.0) { + y = x * x - 2.0; + y = chbevl(y, A, 10) - log(0.5 * x) * i0(x); + return (y * exp(x)); + } + + y = chbevl(8.0 / x - 2.0, B, 25) / sqrt(x); + return (y); +} diff --git a/gtsam/3rdparty/cephes/cephes/k1.c b/gtsam/3rdparty/cephes/cephes/k1.c new file mode 100644 index 000000000..fc33e5c0e --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/k1.c @@ -0,0 +1,179 @@ +/* k1.c + * + * Modified Bessel function, third kind, order one + * + * + * + * SYNOPSIS: + * + * double x, y, k1(); + * + * y = k1( x ); + * + * + * + * DESCRIPTION: + * + * Computes the modified Bessel function of the third kind + * of order one of the argument. + * + * The range is partitioned into the two intervals [0,2] and + * (2, infinity). Chebyshev polynomial expansions are employed + * in each interval. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.2e-15 1.6e-16 + * + * ERROR MESSAGES: + * + * message condition value returned + * k1 domain x <= 0 INFINITY + * + */ + /* k1e.c + * + * Modified Bessel function, third kind, order one, + * exponentially scaled + * + * + * + * SYNOPSIS: + * + * double x, y, k1e(); + * + * y = k1e( x ); + * + * + * + * DESCRIPTION: + * + * Returns exponentially scaled modified Bessel function + * of the third kind of order one of the argument: + * + * k1e(x) = exp(x) * k1(x). + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 7.8e-16 1.2e-16 + * See k1(). + * + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +/* Chebyshev coefficients for x(K1(x) - log(x/2) I1(x)) + * in the interval [0,2]. + * + * lim(x->0){ x(K1(x) - log(x/2) I1(x)) } = 1. + */ + +static double A[] = { + -7.02386347938628759343E-18, + -2.42744985051936593393E-15, + -6.66690169419932900609E-13, + -1.41148839263352776110E-10, + -2.21338763073472585583E-8, + -2.43340614156596823496E-6, + -1.73028895751305206302E-4, + -6.97572385963986435018E-3, + -1.22611180822657148235E-1, + -3.53155960776544875667E-1, + 1.52530022733894777053E0 +}; + +/* Chebyshev coefficients for exp(x) sqrt(x) K1(x) + * in the interval [2,infinity]. + * + * lim(x->inf){ exp(x) sqrt(x) K1(x) } = sqrt(pi/2). + */ +static double B[] = { + -5.75674448366501715755E-18, + 1.79405087314755922667E-17, + -5.68946255844285935196E-17, + 1.83809354436663880070E-16, + -6.05704724837331885336E-16, + 2.03870316562433424052E-15, + -7.01983709041831346144E-15, + 2.47715442448130437068E-14, + -8.97670518232499435011E-14, + 3.34841966607842919884E-13, + -1.28917396095102890680E-12, + 5.13963967348173025100E-12, + -2.12996783842756842877E-11, + 9.21831518760500529508E-11, + -4.19035475934189648750E-10, + 2.01504975519703286596E-9, + -1.03457624656780970260E-8, + 5.74108412545004946722E-8, + -3.50196060308781257119E-7, + 2.40648494783721712015E-6, + -1.93619797416608296024E-5, + 1.95215518471351631108E-4, + -2.85781685962277938680E-3, + 1.03923736576817238437E-1, + 2.72062619048444266945E0 +}; + +extern double MINLOG; + +double k1(double x) +{ + double y, z; + + if (x == 0.0) { + sf_error("k1", SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + else if (x < 0.0) { + sf_error("k1", SF_ERROR_DOMAIN, NULL); + return NAN; + } + z = 0.5 * x; + + if (x <= 2.0) { + y = x * x - 2.0; + y = log(z) * i1(x) + chbevl(y, A, 11) / x; + return (y); + } + + return (exp(-x) * chbevl(8.0 / x - 2.0, B, 25) / sqrt(x)); +} + + + + +double k1e(double x) +{ + double y; + + if (x == 0.0) { + sf_error("k1e", SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + else if (x < 0.0) { + sf_error("k1e", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (x <= 2.0) { + y = x * x - 2.0; + y = log(0.5 * x) * i1(x) + chbevl(y, A, 11) / x; + return (y * exp(x)); + } + + return (chbevl(8.0 / x - 2.0, B, 25) / sqrt(x)); +} diff --git a/gtsam/3rdparty/cephes/cephes/kn.c b/gtsam/3rdparty/cephes/cephes/kn.c new file mode 100644 index 000000000..ff7584a15 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/kn.c @@ -0,0 +1,235 @@ +/* kn.c + * + * Modified Bessel function, third kind, integer order + * + * + * + * SYNOPSIS: + * + * double x, y, kn(); + * int n; + * + * y = kn( n, x ); + * + * + * + * DESCRIPTION: + * + * Returns modified Bessel function of the third kind + * of order n of the argument. + * + * The range is partitioned into the two intervals [0,9.55] and + * (9.55, infinity). An ascending power series is used in the + * low range, and an asymptotic expansion in the high range. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 90000 1.8e-8 3.0e-10 + * + * Error is high only near the crossover point x = 9.55 + * between the two expansions used. + */ + + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1988, 2000 by Stephen L. Moshier + */ + + +/* + * Algorithm for Kn. + * n-1 + * -n - (n-k-1)! 2 k + * K (x) = 0.5 (x/2) > -------- (-x /4) + * n - k! + * k=0 + * + * inf. 2 k + * n n - (x /4) + * + (-1) 0.5(x/2) > {p(k+1) + p(n+k+1) - 2log(x/2)} --------- + * - k! (n+k)! + * k=0 + * + * where p(m) is the psi function: p(1) = -EUL and + * + * m-1 + * - + * p(m) = -EUL + > 1/k + * - + * k=1 + * + * For large x, + * 2 2 2 + * u-1 (u-1 )(u-3 ) + * K (z) = sqrt(pi/2z) exp(-z) { 1 + ------- + ------------ + ...} + * v 1 2 + * 1! (8z) 2! (8z) + * asymptotically, where + * + * 2 + * u = 4 v . + * + */ + +#include "mconf.h" +#include + +#define EUL 5.772156649015328606065e-1 +#define MAXFAC 31 +extern double MACHEP, MAXLOG; + +double kn(int nn, double x) +{ + double k, kf, nk1f, nkf, zn, t, s, z0, z; + double ans, fn, pn, pk, zmn, tlg, tox; + int i, n; + + if (nn < 0) + n = -nn; + else + n = nn; + + if (n > MAXFAC) { + overf: + sf_error("kn", SF_ERROR_OVERFLOW, NULL); + return (INFINITY); + } + + if (x <= 0.0) { + if (x < 0.0) { + sf_error("kn", SF_ERROR_DOMAIN, NULL); + return NAN; + } + else { + sf_error("kn", SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + } + + + if (x > 9.55) + goto asymp; + + ans = 0.0; + z0 = 0.25 * x * x; + fn = 1.0; + pn = 0.0; + zmn = 1.0; + tox = 2.0 / x; + + if (n > 0) { + /* compute factorial of n and psi(n) */ + pn = -EUL; + k = 1.0; + for (i = 1; i < n; i++) { + pn += 1.0 / k; + k += 1.0; + fn *= k; + } + + zmn = tox; + + if (n == 1) { + ans = 1.0 / x; + } + else { + nk1f = fn / n; + kf = 1.0; + s = nk1f; + z = -z0; + zn = 1.0; + for (i = 1; i < n; i++) { + nk1f = nk1f / (n - i); + kf = kf * i; + zn *= z; + t = nk1f * zn / kf; + s += t; + if ((DBL_MAX - fabs(t)) < fabs(s)) + goto overf; + if ((tox > 1.0) && ((DBL_MAX / tox) < zmn)) + goto overf; + zmn *= tox; + } + s *= 0.5; + t = fabs(s); + if ((zmn > 1.0) && ((DBL_MAX / zmn) < t)) + goto overf; + if ((t > 1.0) && ((DBL_MAX / t) < zmn)) + goto overf; + ans = s * zmn; + } + } + + + tlg = 2.0 * log(0.5 * x); + pk = -EUL; + if (n == 0) { + pn = pk; + t = 1.0; + } + else { + pn = pn + 1.0 / n; + t = 1.0 / fn; + } + s = (pk + pn - tlg) * t; + k = 1.0; + do { + t *= z0 / (k * (k + n)); + pk += 1.0 / k; + pn += 1.0 / (k + n); + s += (pk + pn - tlg) * t; + k += 1.0; + } + while (fabs(t / s) > MACHEP); + + s = 0.5 * s / zmn; + if (n & 1) + s = -s; + ans += s; + + return (ans); + + + + /* Asymptotic expansion for Kn(x) */ + /* Converges to 1.4e-17 for x > 18.4 */ + + asymp: + + if (x > MAXLOG) { + sf_error("kn", SF_ERROR_UNDERFLOW, NULL); + return (0.0); + } + k = n; + pn = 4.0 * k * k; + pk = 1.0; + z0 = 8.0 * x; + fn = 1.0; + t = 1.0; + s = t; + nkf = INFINITY; + i = 0; + do { + z = pn - pk * pk; + t = t * z / (fn * z0); + nk1f = fabs(t); + if ((i >= n) && (nk1f > nkf)) { + goto adone; + } + nkf = nk1f; + s += t; + fn += 1.0; + pk += 2.0; + i += 1; + } + while (fabs(t / s) > MACHEP); + + adone: + ans = exp(-x) * sqrt(M_PI / (2.0 * x)) * s; + return (ans); +} diff --git a/gtsam/3rdparty/cephes/cephes/kolmogorov.c b/gtsam/3rdparty/cephes/cephes/kolmogorov.c new file mode 100644 index 000000000..2135e0ebb --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/kolmogorov.c @@ -0,0 +1,1147 @@ +/* File altered for inclusion in cephes module for Python: + * Main loop commented out.... */ +/* Travis Oliphant Nov. 1998 */ + +/* Re Kolmogorov statistics, here is Birnbaum and Tingey's (actually it was already present + * in Smirnov's paper) formula for the + * distribution of D+, the maximum of all positive deviations between a + * theoretical distribution function P(x) and an empirical one Sn(x) + * from n samples. + * + * + + * D = sup [P(x) - S (x)] + * n -inf < x < inf n + * + * + * [n(1-d)] + * + - v-1 n-v + * Pr{D > d} = > C d (d + v/n) (1 - d - v/n) + * n - n v + * v=0 + * + * (also equals the following sum, but note the terms may be large and alternating in sign) + * See Smirnov 1944, Dwass 1959 + * n + * - v-1 n-v + * = 1 - > C d (d + v/n) (1 - d - v/n) + * - n v + * v=[n(1-d)]+1 + * + * [n(1-d)] is the largest integer not exceeding n(1-d). + * nCv is the number of combinations of n things taken v at a time. + + * Sources: + * [1] Smirnov, N.V. "Approximate laws of distribution of random variables from empirical data" + * Usp. Mat. Nauk, 1944. http://mi.mathnet.ru/umn8798 + * [2] Birnbaum, Z. W. and Tingey, Fred H. + * "One-Sided Confidence Contours for Probability Distribution Functions", + * Ann. Math. Statist. 1951. https://doi.org/10.1214/aoms/1177729550 + * [3] Dwass, Meyer, "The Distribution of a Generalized $\mathrm{D}^+_n$ Statistic", + * Ann. Math. Statist., 1959. https://doi.org/10.1214/aoms/1177706085 + * [4] van Mulbregt, Paul, "Computing the Cumulative Distribution Function and Quantiles of the One-sided Kolmogorov-Smirnov Statistic" + * http://arxiv.org/abs/1802.06966 + * [5] van Mulbregt, Paul, "Computing the Cumulative Distribution Function and Quantiles of the limit of the Two-sided Kolmogorov-Smirnov Statistic" + * https://arxiv.org/abs/1803.00426 + * + */ + +#include "mconf.h" +#include +#include +#include + + +/* ************************************************************************ */ +/* Algorithm Configuration */ + +/* + * Kolmogorov Two-sided: + * Switchover between the two series to compute K(x) + * 0 <= x <= KOLMOG_CUTOVER and + * KOLMOG_CUTOVER < x < infty + */ +#define KOLMOG_CUTOVER 0.82 + + +/* + * Smirnov One-sided: + * n larger than SMIRNOV_MAX_COMPUTE_N will result in an approximation + */ +const int SMIRNOV_MAX_COMPUTE_N = 1000000; + +/* + * Use the upper sum formula, if the number of terms is at most SM_UPPER_MAX_TERMS, + * and n is at least SM_UPPERSUM_MIN_N + * Don't use the upper sum if lots of terms are involved as the series alternates + * sign and the terms get much bigger than 1. + */ +#define SM_UPPER_MAX_TERMS 3 +#define SM_UPPERSUM_MIN_N 10 + +/* ************************************************************************ */ +/* ************************************************************************ */ + +/* Assuming LOW and HIGH are constants. */ +#define CLIP(X, LOW, HIGH) ((X) < LOW ? LOW : MIN(X, HIGH)) +#ifndef MIN +#define MIN(a,b) (((a) < (b)) ? (a) : (b)) +#endif +#ifndef MAX +#define MAX(a,b) (((a) < (b)) ? (b) : (a)) +#endif + +/* from cephes constants */ +extern double MINLOG; + +/* exp() of anything below this returns 0 */ +static const int MIN_EXPABLE = (-708 - 38); + +#ifndef LOGSQRT2PI +#define LOGSQRT2PI 0.91893853320467274178032973640561764 +#endif + +/* Struct to hold the CDF, SF and PDF, which are computed simultaneously */ +typedef struct ThreeProbs { + double sf; + double cdf; + double pdf; +} ThreeProbs; + +#define RETURN_3PROBS(PSF, PCDF, PDF) \ + ret.cdf = (PCDF); \ + ret.sf = (PSF); \ + ret.pdf = (PDF); \ + return ret; + +static const double _xtol = DBL_EPSILON; +static const double _rtol = 2*DBL_EPSILON; + +static int +_within_tol(double x, double y, double atol, double rtol) +{ + double diff = fabs(x-y); + int result = (diff <= (atol + rtol * fabs(y))); + return result; +} + +#include "dd_real.h" + +/* Shorten some of the double-double names for readibility */ +#define valueD dd_to_double +#define add_dd dd_add_d_d +#define sub_dd dd_sub_d_d +#define mul_dd dd_mul_d_d +#define neg_D dd_neg +#define div_dd dd_div_d_d +#define add_DD dd_add +#define sub_DD dd_sub +#define mul_DD dd_mul +#define div_DD dd_div +#define add_Dd dd_add_dd_d +#define add_dD dd_add_d_dd +#define sub_Dd dd_sub_dd_d +#define sub_dD dd_sub_d_dd +#define mul_Dd dd_mul_dd_d +#define mul_dD dd_mul_d_dd +#define div_Dd dd_div_dd_d +#define div_dD dd_div_d_dd +#define frexpD dd_frexp +#define ldexpD dd_ldexp +#define logD dd_log +#define log1pD dd_log1p + + +/* ************************************************************************ */ +/* Kolmogorov : Two-sided **************************** */ +/* ************************************************************************ */ + +static ThreeProbs +_kolmogorov(double x) +{ + double P = 1.0; + double D = 0; + double sf, cdf, pdf; + ThreeProbs ret; + + if (isnan(x)) { + RETURN_3PROBS(NAN, NAN, NAN); + } + if (x <= 0) { + RETURN_3PROBS(1.0, 0.0, 0); + } + /* x <= 0.040611972203751713 */ + if (x <= (double)M_PI/sqrt(-MIN_EXPABLE * 8)) { + RETURN_3PROBS(1.0, 0.0, 0); + } + + P = 1.0; + if (x <= KOLMOG_CUTOVER) { + /* + * u = e^(-pi^2/(8x^2)) + * w = sqrt(2pi)/x + * P = w*u * (1 + u^8 + u^24 + u^48 + ...) + */ + double w = sqrt(2 * M_PI)/x; + double logu8 = -M_PI * M_PI/(x * x); /* log(u^8) */ + double u = exp(logu8/8); + if (u == 0) { + /* + * P = w*u, but u < 1e-308, and w > 1, + * so compute as logs, then exponentiate + */ + double logP = logu8/8 + log(w); + P = exp(logP); + } else { + /* Just unroll the loop, 3 iterations */ + double u8 = exp(logu8); + double u8cub = pow(u8, 3); + P = 1 + u8cub * P; + D = 5*5 + u8cub * D; + P = 1 + u8*u8 * P; + D = 3*3 + u8*u8 * D; + P = 1 + u8 * P; + D = 1*1 + u8 * D; + + D = M_PI * M_PI/4/(x*x) * D - P; + D *= w * u/x; + P = w * u * P; + } + cdf = P; + sf = 1-P; + pdf = D; + } + else { + /* + * v = e^(-2x^2) + * P = 2 (v - v^4 + v^9 - v^16 + ...) + * = 2v(1 - v^3*(1 - v^5*(1 - v^7*(1 - ...))) + */ + double logv = -2*x*x; + double v = exp(logv); + /* + * Want q^((2k-1)^2)(1-q^(4k-1)) / q(1-q^3) < epsilon to break out of loop. + * With KOLMOG_CUTOVER ~ 0.82, k <= 4. Just unroll the loop, 4 iterations + */ + double vsq = v*v; + double v3 = pow(v, 3); + double vpwr; + + vpwr = v3*v3*v; /* v**7 */ + P = 1 - vpwr * P; /* P <- 1 - (1-v**(2k-1)) * P */ + D = 3*3 - vpwr * D; + + vpwr = v3*vsq; + P = 1 - vpwr * P; + D = 2*2 - vpwr * D; + + vpwr = v3; + P = 1 - vpwr * P; + D = 1*1 - vpwr * D; + + P = 2 * v * P; + D = 8 * v * x * D; + sf = P; + cdf = 1 - sf; + pdf = D; + } + pdf = MAX(0, pdf); + cdf = CLIP(cdf, 0, 1); + sf = CLIP(sf, 0, 1); + RETURN_3PROBS(sf, cdf, pdf); +} + + +/* Find x such kolmogorov(x)=psf, kolmogc(x)=pcdf */ +static double +_kolmogi(double psf, double pcdf) +{ + double x, t; + double xmin = 0; + double xmax = INFINITY; + int iterations; + double a = xmin, b = xmax; + + if (!(psf >= 0.0 && pcdf >= 0.0 && pcdf <= 1.0 && psf <= 1.0)) { + sf_error("kolmogi", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (fabs(1.0 - pcdf - psf) > 4* DBL_EPSILON) { + sf_error("kolmogi", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (pcdf == 0.0) { + return 0.0; + } + if (psf == 0.0) { + return INFINITY; + } + + if (pcdf <= 0.5) { + /* p ~ (sqrt(2pi)/x) *exp(-pi^2/8x^2). Generate lower and upper bounds */ + double logpcdf = log(pcdf); + const double SQRT2 = M_SQRT2; + /* Now that 1 >= x >= sqrt(p) */ + /* Iterate twice: x <- pi/(sqrt(8) sqrt(log(sqrt(2pi)) - log(x) - log(pdf))) */ + a = M_PI / (2 * SQRT2 * sqrt(-(logpcdf + logpcdf/2 - LOGSQRT2PI))); + b = M_PI / (2 * SQRT2 * sqrt(-(logpcdf + 0 - LOGSQRT2PI))); + a = M_PI / (2 * SQRT2 * sqrt(-(logpcdf + log(a) - LOGSQRT2PI))); + b = M_PI / (2 * SQRT2 * sqrt(-(logpcdf + log(b) - LOGSQRT2PI))); + x = (a + b) / 2.0; + } + else { + /* + * Based on the approximation p ~ 2 exp(-2x^2) + * Found that needed to replace psf with a slightly smaller number in the second element + * as otherwise _kolmogorov(b) came back as a very small number but with + * the same sign as _kolmogorov(a) + * kolmogi(0.5) = 0.82757355518990772 + * so (1-q^(-(4-1)*2*x^2)) = (1-exp(-6*0.8275^2) ~ (1-exp(-4.1) + */ + const double jiggerb = 256 * DBL_EPSILON; + double pba = psf/(1.0 - exp(-4))/2, pbb = psf * (1 - jiggerb)/2; + double q0; + a = sqrt(-0.5 * log(pba)); + b = sqrt(-0.5 * log(pbb)); + /* + * Use inversion of + * p = q - q^4 + q^9 - q^16 + ...: + * q = p + p^4 + 4p^7 - p^9 + 22p^10 - 13p^12 + 140*p^13 ... + */ + { + double p = psf/2.0; + double p2 = p*p; + double p3 = p*p*p; + q0 = 1 + p3 * (1 + p3 * (4 + p2 *(-1 + p*(22 + p2* (-13 + 140 * p))))); + q0 *= p; + } + x = sqrt(-log(q0) / 2); + if (x < a || x > b) { + x = (a+b)/2; + } + } + assert(a <= b); + + iterations = 0; + do { + double x0 = x; + ThreeProbs probs = _kolmogorov(x0); + double df = ((pcdf < 0.5) ? (pcdf - probs.cdf) : (probs.sf - psf)); + double dfdx; + + if (fabs(df) == 0) { + break; + } + /* Update the bracketing interval */ + if (df > 0 && x > a) { + a = x; + } else if (df < 0 && x < b) { + b = x; + } + + dfdx = -probs.pdf; + if (fabs(dfdx) <= 0.0) { + x = (a+b)/2; + t = x0 - x; + } else { + t = df/dfdx; + x = x0 - t; + } + + /* + * Check out-of-bounds. + * Not expecting this to happen often --- kolmogorov is convex near x=infinity and + * concave near x=0, and we should be approaching from the correct side. + * If out-of-bounds, replace x with a midpoint of the bracket. + */ + if (x >= a && x <= b) { + if (_within_tol(x, x0, _xtol, _rtol)) { + break; + } + if ((x == a) || (x == b)) { + x = (a + b) / 2.0; + /* If the bracket is already so small ... */ + if (x == a || x == b) { + break; + } + } + } else { + x = (a + b) / 2.0; + if (_within_tol(x, x0, _xtol, _rtol)) { + break; + } + } + + if (++iterations > MAXITER) { + sf_error("kolmogi", SF_ERROR_SLOW, NULL); + break; + } + } while(1); + return (x); +} + + +double +kolmogorov(double x) +{ + if (isnan(x)) { + return NAN; + } + return _kolmogorov(x).sf; +} + +double +kolmogc(double x) +{ + if (isnan(x)) { + return NAN; + } + return _kolmogorov(x).cdf; +} + +double +kolmogp(double x) +{ + if (isnan(x)) { + return NAN; + } + if (x <= 0) { + return -0.0; + } + return -_kolmogorov(x).pdf; +} + +/* Functional inverse of Kolmogorov survival statistic for two-sided test. + * Finds x such that kolmogorov(x) = p. + */ +double +kolmogi(double p) +{ + if (isnan(p)) { + return NAN; + } + return _kolmogi(p, 1-p); +} + +/* Functional inverse of Kolmogorov cumulative statistic for two-sided test. + * Finds x such that kolmogc(x) = p = (or kolmogorov(x) = 1-p). + */ +double +kolmogci(double p) +{ + if (isnan(p)) { + return NAN; + } + return _kolmogi(1-p, p); +} + + + +/* ************************************************************************ */ +/* ********** Smirnov : One-sided ***************************************** */ +/* ************************************************************************ */ + +static double +nextPowerOf2(double x) +{ + double q = ldexp(x, 1-DBL_MANT_DIG); + double L = fabs(q+x); + if (L == 0) { + L = fabs(x); + } else { + int Lint = (int)(L); + if (Lint == L) { + L = Lint; + } + } + return L; +} + +static double +modNX(int n, double x, int *pNXFloor, double *pNX) +{ + /* + * Compute floor(n*x) and remainder *exactly*. + * If remainder is too close to 1 (E.g. (1, -DBL_EPSILON/2)) + * round up and adjust */ + double2 alphaD, nxD, nxfloorD; + int nxfloor; + double alpha; + + nxD = mul_dd(n, x); + nxfloorD = dd_floor(nxD); + alphaD = sub_DD(nxD, nxfloorD); + alpha = dd_hi(alphaD); + nxfloor = dd_to_int(nxfloorD); + assert(alpha >= 0); + assert(alpha <= 1); + if (alpha == 1) { + nxfloor += 1; + alpha = 0; + } + assert(alpha < 1.0); + *pNX = dd_to_double(nxD); + *pNXFloor = nxfloor; + return alpha; +} + +/* + * The binomial coefficient C overflows a 64 bit double, as the 11-bit + * exponent is too small. + * Store C as (Cman:double2, Cexpt:int). + * I.e a Mantissa/significand, and an exponent. + * Cman lies between 0.5 and 1, and the exponent has >=32-bit. + */ +static void +updateBinomial(double2 *Cman, int *Cexpt, int n, int j) +{ + int expt; + double2 rat = div_dd(n - j, j + 1.0); + double2 man2 = mul_DD(*Cman, rat); + man2 = frexpD(man2, &expt); + assert (!dd_is_zero(man2)); + *Cexpt += expt; + *Cman = man2; +} + + +static double2 +pow_D(double2 a, int m) +{ + /* + * Using dd_npwr() here would be quite time-consuming. + * Tradeoff accuracy-time by using pow(). + */ + double ans, r, adj; + if (m <= 0) { + if (m == 0) { + return DD_C_ONE; + } + return dd_inv(pow_D(a, -m)); + } + if (dd_is_zero(a)) { + return DD_C_ZERO; + } + ans = pow(a.x[0], m); + r = a.x[1]/a.x[0]; + adj = m*r; + if (fabs(adj) > 1e-8) { + if (fabs(adj) < 1e-4) { + /* Take 1st two terms of Taylor Series for (1+r)^m */ + adj += (m*r) * ((m-1)/2.0 * r); + } else { + /* Take exp of scaled log */ + adj = expm1(m*log1p(r)); + } + } + return dd_add_d_d(ans, ans*adj); +} + +static double +pow2(double a, double b, int m) +{ + return dd_to_double(pow_D(add_dd(a, b), m)); +} + +/* + * Not 1024 as too big. Want _MAX_EXPONENT < 1023-52 so as to keep both + * elements of the double2 normalized + */ +#define _MAX_EXPONENT 960 + +#define RETURN_M_E(MAND, EXPT) \ + *pExponent = EXPT;\ + return MAND; + + +static double2 +pow2Scaled_D(double2 a, int m, int *pExponent) +{ + /* Compute a^m = significand*2^expt and return as (significand, expt) */ + double2 ans, y; + int ansE, yE; + int maxExpt = _MAX_EXPONENT; + int q, r, y2mE, y2rE, y2mqE; + double2 y2r, y2m, y2mq; + + if (m <= 0) + { + int aE1, aE2; + if (m == 0) { + RETURN_M_E(DD_C_ONE, 0); + } + ans = pow2Scaled_D(a, -m, &aE1); + ans = frexpD(dd_inv(ans), &aE2); + ansE = -aE1 + aE2; + RETURN_M_E(ans, ansE); + } + y = frexpD(a, &yE); + if (m == 1) { + RETURN_M_E(y, yE); + } + /* + * y ^ maxExpt >= 2^{-960} + * => maxExpt = 960 / log2(y.x[0]) = 708 / log(y.x[0]) + * = 665/((1-y.x[0] + y.x[0]^2/2 - ...) + * <= 665/(1-y.x[0]) + * Quick check to see if we might need to break up the exponentiation + */ + if (m*(y.x[0]-1) / y.x[0] < -_MAX_EXPONENT * M_LN2) { + /* Now do it carefully, calling log() */ + double lg2y = log(y.x[0]) / M_LN2; + double lgAns = m * lg2y; + if (lgAns <= -_MAX_EXPONENT) { + maxExpt = (int)(nextPowerOf2(-_MAX_EXPONENT / lg2y + 1)/2); + } + } + if (m <= maxExpt) + { + double2 ans1 = pow_D(y, m); + ans = frexpD(ans1, &ansE); + ansE += m * yE; + RETURN_M_E(ans, ansE); + } + + q = m / maxExpt; + r = m % maxExpt; + /* y^m = (y^maxExpt)^q * y^r */ + y2r = pow2Scaled_D(y, r, &y2rE); + y2m = pow2Scaled_D(y, maxExpt, &y2mE); + y2mq = pow2Scaled_D(y2m, q, &y2mqE); + ans = frexpD(mul_DD(y2r, y2mq), &ansE); + y2mqE += y2mE * q; + ansE += y2mqE + y2rE; + ansE += m * yE; + RETURN_M_E(ans, ansE); +} + + +static double2 +pow4_D(double a, double b, double c, double d, int m) +{ + /* Compute ((a+b)/(c+d)) ^ m */ + double2 A, C, X; + if (m <= 0){ + if (m == 0) { + return DD_C_ONE; + } + return pow4_D(c, d, a, b, -m); + } + A = add_dd(a, b); + C = add_dd(c, d); + if (dd_is_zero(A)) { + return (dd_is_zero(C) ? DD_C_NAN : DD_C_ZERO); + } + if (dd_is_zero(C)) { + return (dd_is_negative(A) ? DD_C_NEGINF : DD_C_INF); + } + X = div_DD(A, C); + return pow_D(X, m); +} + +static double +pow4(double a, double b, double c, double d, int m) +{ + double2 ret = pow4_D(a, b, c, d, m); + return dd_to_double(ret); +} + + +static double2 +logpow4_D(double a, double b, double c, double d, int m) +{ + /* + * Compute log(((a+b)/(c+d)) ^ m) + * == m * log((a+b)/(c+d)) + * == m * log( 1 + (a+b-c-d)/(c+d)) + */ + double2 ans; + double2 A, C, X; + if (m == 0) { + return DD_C_ZERO; + } + A = add_dd(a, b); + C = add_dd(c, d); + if (dd_is_zero(A)) { + return (dd_is_zero(C) ? DD_C_ZERO : DD_C_NEGINF); + } + if (dd_is_zero(C)) { + return DD_C_INF; + } + X = div_DD(A, C); + assert(X.x[0] >= 0); + if (0.5 <= X.x[0] && X.x[0] <= 1.5) { + double2 A1 = sub_DD(A, C); + double2 X1 = div_DD(A1, C); + ans = log1pD(X1); + } else { + ans = logD(X); + } + ans = mul_dD(m, ans); + return ans; +} + +static double +logpow4(double a, double b, double c, double d, int m) +{ + double2 ans = logpow4_D(a, b, c, d, m); + return dd_to_double(ans); +} + +/* + * Compute a single term in the summation, A_v(n, x): + * A_v(n, x) = Binomial(n,v) * (1-x-v/n)^(n-v) * (x+v/n)^(v-1) + */ +static void +computeAv(int n, double x, int v, double2 Cman, int Cexpt, + double2 *pt1, double2 *pt2, double2 *pAv) +{ + int t1E, t2E, ansE; + double2 Av; + double2 t2x = sub_Dd(div_dd(n - v, n), x); /* 1 - x - v/n */ + double2 t2 = pow2Scaled_D(t2x, n-v, &t2E); + double2 t1x = add_Dd(div_dd(v, n), x); /* x + v/n */ + double2 t1 = pow2Scaled_D(t1x, v-1, &t1E); + double2 ans = mul_DD(t1, t2); + ans = mul_DD(ans, Cman); + ansE = Cexpt + t1E + t2E; + Av = ldexpD(ans, ansE); + *pAv = Av; + *pt1 = t1; + *pt2 = t2; +} + + +static ThreeProbs +_smirnov(int n, double x) +{ + double nx, alpha; + double2 AjSum = DD_C_ZERO; + double2 dAjSum = DD_C_ZERO; + double cdf, sf, pdf; + + int bUseUpperSum; + int nxfl, n1mxfl, n1mxceil; + ThreeProbs ret; + + if (!(n > 0 && x >= 0.0 && x <= 1.0)) { + RETURN_3PROBS(NAN, NAN, NAN); + } + if (n == 1) { + RETURN_3PROBS(1-x, x, 1.0); + } + if (x == 0.0) { + RETURN_3PROBS(1.0, 0.0, 1.0); + } + if (x == 1.0) { + RETURN_3PROBS(0.0, 1.0, 0.0); + } + + alpha = modNX(n, x, &nxfl, &nx); + n1mxfl = n - nxfl - (alpha == 0 ? 0 : 1); + n1mxceil = n - nxfl; + /* + * If alpha is 0, don't actually want to include the last term + * in either the lower or upper summations. + */ + if (alpha == 0) { + n1mxfl -= 1; + n1mxceil += 1; + } + + /* Special case: x <= 1/n */ + if (nxfl == 0 || (nxfl == 1 && alpha == 0)) { + double t = pow2(1, x, n-1); + pdf = (nx + 1) * t / (1+x); + cdf = x * t; + sf = 1 - cdf; + /* Adjust if x=1/n *exactly* */ + if (nxfl == 1) { + assert(alpha == 0); + pdf -= 0.5; + } + RETURN_3PROBS(sf, cdf, pdf); + } + /* Special case: x is so big, the sf underflows double64 */ + if (-2 * n * x*x < MINLOG) { + RETURN_3PROBS(0, 1, 0); + } + /* Special case: x >= 1 - 1/n */ + if (nxfl >= n-1) { + sf = pow2(1, -x, n); + cdf = 1 - sf; + pdf = n * sf/(1-x); + RETURN_3PROBS(sf, cdf, pdf); + } + /* Special case: n is so big, take too long to compute */ + if (n > SMIRNOV_MAX_COMPUTE_N) { + /* p ~ e^(-(6nx+1)^2 / 18n) */ + double logp = -pow(6.0*n*x+1, 2)/18.0/n; + /* Maximise precision for small p-value. */ + if (logp < -M_LN2) { + sf = exp(logp); + cdf = 1 - sf; + } else { + cdf = -expm1(logp); + sf = 1 - cdf; + } + pdf = (6.0*n*x+1) * 2 * sf/3; + RETURN_3PROBS(sf, cdf, pdf); + } + { + /* + * Use the upper sum if n is large enough, and x is small enough and + * the number of terms is going to be small enough. + * Otherwise it just drops accuracy, about 1.6bits * nUpperTerms + */ + int nUpperTerms = n - n1mxceil + 1; + bUseUpperSum = (nUpperTerms <= 1 && x < 0.5); + bUseUpperSum = (bUseUpperSum || + ((n >= SM_UPPERSUM_MIN_N) + && (nUpperTerms <= SM_UPPER_MAX_TERMS) + && (x <= 0.5 / sqrt(n)))); + } + + { + int start=0, step=1, nTerms=n1mxfl+1; + int j, firstJ = 0; + int vmid = n/2; + double2 Cman = DD_C_ONE; + int Cexpt = 0; + double2 Aj, dAj, t1, t2, dAjCoeff; + double2 oneOverX = div_dd(1, x); + + if (bUseUpperSum) { + start = n; + step = -1; + nTerms = n - n1mxceil + 1; + + t1 = pow4_D(1, x, 1, 0, n - 1); + t2 = DD_C_ONE; + Aj = t1; + + dAjCoeff = div_dD(n - 1, add_dd(1, x)); + dAjCoeff = add_DD(dAjCoeff, oneOverX); + } else { + t1 = oneOverX; + t2 = pow4_D(1, -x, 1, 0, n); + Aj = div_Dd(t2, x); + + dAjCoeff = div_DD(sub_dD(-1, mul_dd(n - 1, x)), sub_dd(1, x)); + dAjCoeff = div_Dd(dAjCoeff, x); + dAjCoeff = add_DD(dAjCoeff, oneOverX); + } + + dAj = mul_DD(Aj, dAjCoeff); + AjSum = add_DD(AjSum, Aj); + dAjSum = add_DD(dAjSum, dAj); + + updateBinomial(&Cman, &Cexpt, n, 0); + firstJ ++; + + for (j = firstJ; j < nTerms; j += 1) { + int v = start + j * step; + + computeAv(n, x, v, Cman, Cexpt, &t1, &t2, &Aj); + + if (dd_isfinite(Aj) && !dd_is_zero(Aj)) { + /* coeff = 1/x + (j-1)/(x+j/n) - (n-j)/(1-x-j/n) */ + dAjCoeff = sub_DD(div_dD((n * (v - 1)), add_dd(nxfl + v, alpha)), + div_dD(((n - v) * n), sub_dd(n - nxfl - v, alpha))); + dAjCoeff = add_DD(dAjCoeff, oneOverX); + dAj = mul_DD(Aj, dAjCoeff); + + assert(dd_isfinite(Aj)); + AjSum = add_DD(AjSum, Aj); + dAjSum = add_DD(dAjSum, dAj); + } + /* Safe to terminate early? */ + if (!dd_is_zero(Aj)) { + if ((4*(nTerms-j) * fabs(dd_to_double(Aj)) < DBL_EPSILON * dd_to_double(AjSum)) + && (j != nTerms - 1)) { + break; + } + } + else if (j > vmid) { + assert(dd_is_zero(Aj)); + break; + } + + updateBinomial(&Cman, &Cexpt, n, j); + } + assert(dd_isfinite(AjSum)); + assert(dd_isfinite(dAjSum)); + { + double2 derivD = mul_dD(x, dAjSum); + double2 probD = mul_dD(x, AjSum); + double deriv = dd_to_double(derivD); + double prob = dd_to_double(probD); + + assert (nx != 1 || alpha > 0); + if (step < 0) { + cdf = prob; + sf = 1-prob; + pdf = deriv; + } else { + cdf = 1-prob; + sf = prob; + pdf = -deriv; + } + } + } + + pdf = MAX(0, pdf); + cdf = CLIP(cdf, 0, 1); + sf = CLIP(sf, 0, 1); + RETURN_3PROBS(sf, cdf, pdf); +} + +/* + * Functional inverse of Smirnov distribution + * finds x such that smirnov(n, x) = psf; smirnovc(n, x) = pcdf). + */ +static double +_smirnovi(int n, double psf, double pcdf) +{ + /* + * Need to use a bracketing NR algorithm here and be very careful + * about the starting point. + */ + double x, logpcdf; + int iterations = 0; + int function_calls = 0; + double a=0, b=1; + double maxlogpcdf, psfrootn; + double dx, dxold; + + if (!(n > 0 && psf >= 0.0 && pcdf >= 0.0 && pcdf <= 1.0 && psf <= 1.0)) { + sf_error("smirnovi", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (fabs(1.0 - pcdf - psf) > 4* DBL_EPSILON) { + sf_error("smirnovi", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + /* STEP 1: Handle psf==0, or pcdf == 0 */ + if (pcdf == 0.0) { + return 0.0; + } + if (psf == 0.0) { + return 1.0; + } + /* STEP 2: Handle n=1 */ + if (n == 1) { + return pcdf; + } + + /* STEP 3 Handle psf *very* close to 0. Correspond to (n-1)/n < x < 1 */ + psfrootn = pow(psf, 1.0 / n); + /* xmin > 1 - 1.0 / n */ + if (n < 150 && n*psfrootn <= 1) { + /* Solve exactly. */ + x = 1 - psfrootn; + return x; + } + + logpcdf = (pcdf < 0.5 ? log(pcdf) : log1p(-psf)); + + /* + * STEP 4 Find bracket and initial estimate for use in N-R + * 4(a) Handle 0 < x <= 1/n: pcdf = x * (1+x)^*(n-1) + */ + maxlogpcdf = logpow4(1, 0.0, n, 0, 1) + logpow4(n, 1, n, 0, n - 1); + if (logpcdf <= maxlogpcdf) { + double xmin = pcdf / SCIPY_El; + double xmax = pcdf; + double P1 = pow4(n, 1, n, 0, n - 1) / n; + double R = pcdf/P1; + double z0 = R; + /* + * Do one iteration of N-R solving: z*e^(z-1) = R, with z0=pcdf/P1 + * z <- z - (z exp(z-1) - pcdf)/((z+1)exp(z-1)) + * If z_0 = R, z_1 = R(1-exp(1-R))/(R+1) + */ + if (R >= 1) { + /* + * R=1 is OK; + * R>1 can happen due to truncation error for x = (1-1/n)+-eps + */ + R = 1; + x = R/n; + return x; + } + z0 = (z0*z0 + R * exp(1-z0))/(1+z0); + x = z0/n; + a = xmin*(1 - 4 * DBL_EPSILON); + a = MAX(a, 0); + b = xmax * (1 + 4 * DBL_EPSILON); + b = MIN(b, 1.0/n); + x = CLIP(x, a, b); + } + else + { + /* 4(b) : 1/n < x < (n-1)/n */ + double xmin = 1 - psfrootn; + double logpsf = (psf < 0.5 ? log(psf) : log1p(-pcdf)); + double xmax = sqrt(-logpsf / (2.0L * n)); + double xmax6 = xmax - 1.0L / (6 * n); + a = xmin; + b = xmax; + /* Allow for a little rounding error */ + a *= 1 - 4 * DBL_EPSILON; + b *= 1 + 4 * DBL_EPSILON; + a = MAX(xmin, 1.0/n); + b = MIN(xmax, 1-1.0/n); + x = xmax6; + } + if (x < a || x > b) { + x = (a + b)/2; + } + assert (x < 1); + + /* + * Skip computing fa, fb as that takes cycles and the exact values + * are not needed. + */ + + /* STEP 5 Run N-R. + * smirnov should be well-enough behaved for NR starting at this location. + * Use smirnov(n, x)-psf, or pcdf - smirnovc(n, x), whichever has smaller p. + */ + dxold = b - a; + dx = dxold; + do { + double dfdx, x0 = x, deltax, df; + assert(x < 1); + assert(x > 0); + { + ThreeProbs probs = _smirnov(n, x0); + ++function_calls; + df = ((pcdf < 0.5) ? (pcdf - probs.cdf) : (probs.sf - psf)); + dfdx = -probs.pdf; + } + if (df == 0) { + return x; + } + /* Update the bracketing interval */ + if (df > 0 && x > a) { + a = x; + } else if (df < 0 && x < b) { + b = x; + } + + if (dfdx == 0) { + /* + * x was not within tolerance, but now we hit a 0 derivative. + * This implies that x >> 1/sqrt(n), and even then |smirnovp| >= |smirnov| + * so this condition is unexpected. Do a bisection step. + */ + x = (a+b)/2; + deltax = x0 - x; + } else { + deltax = df / dfdx; + x = x0 - deltax; + } + /* + * Check out-of-bounds. + * Not expecting this to happen ofen --- smirnov is convex near x=1 and + * concave near x=0, and we should be approaching from the correct side. + * If out-of-bounds, replace x with a midpoint of the bracket. + * Also check fast enough convergence. + */ + if ((a <= x) && (x <= b) && (fabs(2 * deltax) <= fabs(dxold) || fabs(dxold) < 256 * DBL_EPSILON)) { + dxold = dx; + dx = deltax; + } else { + dxold = dx; + dx = dx / 2; + x = (a + b) / 2; + deltax = x0 - x; + } + /* + * Note that if psf is close to 1, f(x) -> 1, f'(x) -> -1. + * => abs difference |x-x0| is approx |f(x)-p| >= DBL_EPSILON, + * => |x-x0|/x >= DBL_EPSILON/x. + * => cannot use a purely relative criteria as it will fail for x close to 0. + */ + if (_within_tol(x, x0, (psf < 0.5 ? 0 : _xtol), _rtol)) { + break; + } + if (++iterations > MAXITER) { + sf_error("smirnovi", SF_ERROR_SLOW, NULL); + return (x); + } + } while (1); + return x; +} + + +double +smirnov(int n, double d) +{ + ThreeProbs probs; + if (isnan(d)) { + return NAN; + } + probs = _smirnov(n, d); + return probs.sf; +} + +double +smirnovc(int n, double d) +{ + ThreeProbs probs; + if (isnan(d)) { + return NAN; + } + probs = _smirnov(n, d); + return probs.cdf; +} + + +/* + * Derivative of smirnov(n, d) + * One interior point of discontinuity at d=1/n. +*/ +double +smirnovp(int n, double d) +{ + ThreeProbs probs; + if (!(n > 0 && d >= 0.0 && d <= 1.0)) { + return (NAN); + } + if (n == 1) { + /* Slope is always -1 for n=1, even at d = 1.0 */ + return -1.0; + } + if (d == 1.0) { + return -0.0; + } + /* + * If d is 0, the derivative is discontinuous, but approaching + * from the right the limit is -1 + */ + if (d == 0.0) { + return -1.0; + } + probs = _smirnov(n, d); + return -probs.pdf; +} + + +double +smirnovi(int n, double p) +{ + if (isnan(p)) { + return NAN; + } + return _smirnovi(n, p, 1-p); +} + +double +smirnovci(int n, double p) +{ + if (isnan(p)) { + return NAN; + } + return _smirnovi(n, 1-p, p); +} diff --git a/gtsam/3rdparty/cephes/cephes/lanczos.c b/gtsam/3rdparty/cephes/cephes/lanczos.c new file mode 100644 index 000000000..f92a8d208 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/lanczos.c @@ -0,0 +1,56 @@ +/* (C) Copyright John Maddock 2006. + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. (See accompanying file + * LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt) + */ + +/* Scipy changes: + * - 06-22-2016: Removed all code not related to double precision and + * ported to c for use in Cephes + */ + +#include "mconf.h" +#include "lanczos.h" + + +static double lanczos_sum(double x) +{ + return ratevl(x, lanczos_num, + sizeof(lanczos_num) / sizeof(lanczos_num[0]) - 1, + lanczos_denom, + sizeof(lanczos_denom) / sizeof(lanczos_denom[0]) - 1); +} + + +double lanczos_sum_expg_scaled(double x) +{ + return ratevl(x, lanczos_sum_expg_scaled_num, + sizeof(lanczos_sum_expg_scaled_num) / sizeof(lanczos_sum_expg_scaled_num[0]) - 1, + lanczos_sum_expg_scaled_denom, + sizeof(lanczos_sum_expg_scaled_denom) / sizeof(lanczos_sum_expg_scaled_denom[0]) - 1); +} + + +static double lanczos_sum_near_1(double dx) +{ + double result = 0; + unsigned k; + + for (k = 1; k <= sizeof(lanczos_sum_near_1_d)/sizeof(lanczos_sum_near_1_d[0]); ++k) { + result += (-lanczos_sum_near_1_d[k-1]*dx)/(k*dx + k*k); + } + return result; +} + + +static double lanczos_sum_near_2(double dx) +{ + double result = 0; + double x = dx + 2; + unsigned k; + + for(k = 1; k <= sizeof(lanczos_sum_near_2_d)/sizeof(lanczos_sum_near_2_d[0]); ++k) { + result += (-lanczos_sum_near_2_d[k-1]*dx)/(x + k*x + k*k - 1); + } + return result; +} diff --git a/gtsam/3rdparty/cephes/cephes/lanczos.h b/gtsam/3rdparty/cephes/cephes/lanczos.h new file mode 100644 index 000000000..92ab8c1b2 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/lanczos.h @@ -0,0 +1,133 @@ +/* (C) Copyright John Maddock 2006. + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. (See accompanying file + * LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt) + */ + +/* Both lanczos.h and lanczos.c were formed from Boost's lanczos.hpp + * + * Scipy changes: + * - 06-22-2016: Removed all code not related to double precision and + * ported to c for use in Cephes. Note that the order of the + * coefficients is reversed to match the behavior of polevl. + */ + +/* + * Optimal values for G for each N are taken from + * https://web.viu.ca/pughg/phdThesis/phdThesis.pdf, + * as are the theoretical error bounds. + * + * Constants calculated using the method described by Godfrey + * https://my.fit.edu/~gabdo/gamma.txt and elaborated by Toth at + * https://www.rskey.org/gamma.htm using NTL::RR at 1000 bit precision. + */ + +/* + * Lanczos Coefficients for N=13 G=6.024680040776729583740234375 + * Max experimental error (with arbitrary precision arithmetic) 1.196214e-17 + * Generated with compiler: Microsoft Visual C++ version 8.0 on Win32 at Mar 23 2006 + * + * Use for double precision. + */ + +#ifndef LANCZOS_H +#define LANCZOS_H + + +static const double lanczos_num[13] = { + 2.506628274631000270164908177133837338626, + 210.8242777515793458725097339207133627117, + 8071.672002365816210638002902272250613822, + 186056.2653952234950402949897160456992822, + 2876370.628935372441225409051620849613599, + 31426415.58540019438061423162831820536287, + 248874557.8620541565114603864132294232163, + 1439720407.311721673663223072794912393972, + 6039542586.35202800506429164430729792107, + 17921034426.03720969991975575445893111267, + 35711959237.35566804944018545154716670596, + 42919803642.64909876895789904700198885093, + 23531376880.41075968857200767445163675473 +}; + +static const double lanczos_denom[13] = { + 1, + 66, + 1925, + 32670, + 357423, + 2637558, + 13339535, + 45995730, + 105258076, + 150917976, + 120543840, + 39916800, + 0 +}; + +static const double lanczos_sum_expg_scaled_num[13] = { + 0.006061842346248906525783753964555936883222, + 0.5098416655656676188125178644804694509993, + 19.51992788247617482847860966235652136208, + 449.9445569063168119446858607650988409623, + 6955.999602515376140356310115515198987526, + 75999.29304014542649875303443598909137092, + 601859.6171681098786670226533699352302507, + 3481712.15498064590882071018964774556468, + 14605578.08768506808414169982791359218571, + 43338889.32467613834773723740590533316085, + 86363131.28813859145546927288977868422342, + 103794043.1163445451906271053616070238554, + 56906521.91347156388090791033559122686859 +}; + +static const double lanczos_sum_expg_scaled_denom[13] = { + 1, + 66, + 1925, + 32670, + 357423, + 2637558, + 13339535, + 45995730, + 105258076, + 150917976, + 120543840, + 39916800, + 0 +}; + +static const double lanczos_sum_near_1_d[12] = { + 0.3394643171893132535170101292240837927725e-9, + -0.2499505151487868335680273909354071938387e-8, + 0.8690926181038057039526127422002498960172e-8, + -0.1933117898880828348692541394841204288047e-7, + 0.3075580174791348492737947340039992829546e-7, + -0.2752907702903126466004207345038327818713e-7, + -0.1515973019871092388943437623825208095123e-5, + 0.004785200610085071473880915854204301886437, + -0.1993758927614728757314233026257810172008, + 1.483082862367253753040442933770164111678, + -3.327150580651624233553677113928873034916, + 2.208709979316623790862569924861841433016 +}; + +static const double lanczos_sum_near_2_d[12] = { + 0.1009141566987569892221439918230042368112e-8, + -0.7430396708998719707642735577238449585822e-8, + 0.2583592566524439230844378948704262291927e-7, + -0.5746670642147041587497159649318454348117e-7, + 0.9142922068165324132060550591210267992072e-7, + -0.8183698410724358930823737982119474130069e-7, + -0.4506604409707170077136555010018549819192e-5, + 0.01422519127192419234315002746252160965831, + -0.5926941084905061794445733628891024027949, + 4.408830289125943377923077727900630927902, + -9.8907772644920670589288081640128194231, + 6.565936202082889535528455955485877361223 +}; + +static const double lanczos_g = 6.024680040776729583740234375; + +#endif diff --git a/gtsam/3rdparty/cephes/cephes/mconf.h b/gtsam/3rdparty/cephes/cephes/mconf.h new file mode 100644 index 000000000..5e971afad --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/mconf.h @@ -0,0 +1,131 @@ +/* mconf.h + * + * Common include file for math routines + * + * + * + * SYNOPSIS: + * + * #include "mconf.h" + * + * + * + * DESCRIPTION: + * + * The file includes a conditional assembly definition for the type of + * computer arithmetic (IEEE, Motorola IEEE, or UNKnown). + * + * For little-endian computers, such as IBM PC, that follow the + * IEEE Standard for Binary Floating Point Arithmetic (ANSI/IEEE + * Std 754-1985), the symbol IBMPC should be defined. These + * numbers have 53-bit significands. In this mode, constants + * are provided as arrays of hexadecimal 16 bit integers. + * + * Big-endian IEEE format is denoted MIEEE. On some RISC + * systems such as Sun SPARC, double precision constants + * must be stored on 8-byte address boundaries. Since integer + * arrays may be aligned differently, the MIEEE configuration + * may fail on such machines. + * + * To accommodate other types of computer arithmetic, all + * constants are also provided in a normal decimal radix + * which one can hope are correctly converted to a suitable + * format by the available C language compiler. To invoke + * this mode, define the symbol UNK. + * + * An important difference among these modes is a predefined + * set of machine arithmetic constants for each. The numbers + * MACHEP (the machine roundoff error), MAXNUM (largest number + * represented), and several other parameters are preset by + * the configuration symbol. Check the file const.c to + * ensure that these values are correct for your computer. + * + * Configurations NANS, INFINITIES, MINUSZERO, and DENORMAL + * may fail on many systems. Verify that they are supposed + * to work on your computer. + */ + +/* + * Cephes Math Library Release 2.3: June, 1995 + * Copyright 1984, 1987, 1989, 1995 by Stephen L. Moshier + */ + +#ifndef CEPHES_MCONF_H +#define CEPHES_MCONF_H + +#include +#include + +#include "cephes.h" +#include "polevl.h" +#include "sf_error.h" + +#define MAXITER 500 +#define EDOM 33 +#define ERANGE 34 + +/* Type of computer arithmetic */ + +/* UNKnown arithmetic, invokes coefficients given in + * normal decimal format. Beware of range boundary + * problems (MACHEP, MAXLOG, etc. in const.c) and + * roundoff problems in pow.c: + * (Sun SPARCstation) + */ + +/* SciPy note: by defining UNK, we prevent the compiler from + * casting integers to floating point numbers. If the Endianness + * is detected incorrectly, this causes problems on some platforms. + */ +#define UNK 1 + +/* Define to support tiny denormal numbers, else undefine. */ +#define DENORMAL 1 + +#define gamma Gamma + +/* + * Enable loop unrolling on GCC and use faster isnan et al. + */ +#if !defined(__clang__) && defined(__GNUC__) && defined(__GNUC_MINOR__) +#if __GNUC__ >= 5 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 4) +#pragma GCC optimize("unroll-loops") +#define cephes_isnan(x) __builtin_isnan(x) +#define cephes_isinf(x) __builtin_isinf(x) +#define cephes_isfinite(x) __builtin_isfinite(x) +#endif +#endif +#ifndef cephes_isnan +#define cephes_isnan(x) isnan(x) +#define cephes_isinf(x) isinf(x) +#define cephes_isfinite(x) isfinite(x) +#endif + +/* M_PI et al. are not defined in math.h in C99, even with _USE_MATH_DEFINES */ +#if !defined(M_PI) +#define M_PI 3.14159265358979323846 +#endif +#ifndef M_PI_2 +#define M_PI_2 1.57079632679489661923 /* pi/2 */ +#define M_1_PI 0.31830988618379067154 /* 1/pi */ +#define M_2_PI 0.63661977236758134308 /* 2/pi */ +#define M_E 2.71828182845904523536 +#define M_LOG2E 1.44269504088896340736 +#define M_LOG10E 0.434294481903251827651 +#define M_LN2 0.693147180559945309417 +#define M_LN10 2.30258509299404568402 +#define M_PI 3.14159265358979323846 +#define M_PI_2 1.57079632679489661923 +#define M_PI_4 0.785398163397448309616 +#define M_1_PI 0.318309886183790671538 +#define M_2_PI 0.636619772367581343076 +#define M_2_SQRTPI 1.12837916709551257390 +#define M_SQRT2 1.41421356237309504880 +#define M_SQRT1_2 0.707106781186547524401 +#endif + +/* Constants needed that are not available in the C standard library */ +#define SCIPY_EULER 0.577215664901532860606512090082402431 /* Euler constant */ +#define SCIPY_El 2.718281828459045235360287471352662498L /* e as long double */ + +#endif /* CEPHES_MCONF_H */ diff --git a/gtsam/3rdparty/cephes/cephes/nbdtr.c b/gtsam/3rdparty/cephes/cephes/nbdtr.c new file mode 100644 index 000000000..7697f257e --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/nbdtr.c @@ -0,0 +1,207 @@ +/* nbdtr.c + * + * Negative binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, nbdtr(); + * + * y = nbdtr( k, n, p ); + * + * DESCRIPTION: + * + * Returns the sum of the terms 0 through k of the negative + * binomial distribution: + * + * k + * -- ( n+j-1 ) n j + * > ( ) p (1-p) + * -- ( j ) + * j=0 + * + * In a sequence of Bernoulli trials, this is the probability + * that k or fewer failures precede the nth success. + * + * The terms are not computed individually; instead the incomplete + * beta integral is employed, according to the formula + * + * y = nbdtr( k, n, p ) = incbet( n, k+1, p ). + * + * The arguments must be positive, with p ranging from 0 to 1. + * + * ACCURACY: + * + * Tested at random points (a,b,p), with p between 0 and 1. + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,100 100000 1.7e-13 8.8e-15 + * See also incbet.c. + * + */ + /* nbdtrc.c + * + * Complemented negative binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, nbdtrc(); + * + * y = nbdtrc( k, n, p ); + * + * DESCRIPTION: + * + * Returns the sum of the terms k+1 to infinity of the negative + * binomial distribution: + * + * inf + * -- ( n+j-1 ) n j + * > ( ) p (1-p) + * -- ( j ) + * j=k+1 + * + * The terms are not computed individually; instead the incomplete + * beta integral is employed, according to the formula + * + * y = nbdtrc( k, n, p ) = incbet( k+1, n, 1-p ). + * + * The arguments must be positive, with p ranging from 0 to 1. + * + * ACCURACY: + * + * Tested at random points (a,b,p), with p between 0 and 1. + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,100 100000 1.7e-13 8.8e-15 + * See also incbet.c. + */ + +/* nbdtrc + * + * Complemented negative binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, nbdtrc(); + * + * y = nbdtrc( k, n, p ); + * + * DESCRIPTION: + * + * Returns the sum of the terms k+1 to infinity of the negative + * binomial distribution: + * + * inf + * -- ( n+j-1 ) n j + * > ( ) p (1-p) + * -- ( j ) + * j=k+1 + * + * The terms are not computed individually; instead the incomplete + * beta integral is employed, according to the formula + * + * y = nbdtrc( k, n, p ) = incbet( k+1, n, 1-p ). + * + * The arguments must be positive, with p ranging from 0 to 1. + * + * ACCURACY: + * + * See incbet.c. + */ + /* nbdtri + * + * Functional inverse of negative binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, nbdtri(); + * + * p = nbdtri( k, n, y ); + * + * DESCRIPTION: + * + * Finds the argument p such that nbdtr(k,n,p) is equal to y. + * + * ACCURACY: + * + * Tested at random points (a,b,y), with y between 0 and 1. + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,100 100000 1.5e-14 8.5e-16 + * See also incbi.c. + */ + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +double nbdtrc(int k, int n, double p) +{ + double dk, dn; + + if ((p < 0.0) || (p > 1.0)) + goto domerr; + if (k < 0) { + domerr: + sf_error("nbdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + dk = k + 1; + dn = n; + return (incbet(dk, dn, 1.0 - p)); +} + + + +double nbdtr(int k, int n, double p) +{ + double dk, dn; + + if ((p < 0.0) || (p > 1.0)) + goto domerr; + if (k < 0) { + domerr: + sf_error("nbdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + dk = k + 1; + dn = n; + return (incbet(dn, dk, p)); +} + + + +double nbdtri(int k, int n, double p) +{ + double dk, dn, w; + + if ((p < 0.0) || (p > 1.0)) + goto domerr; + if (k < 0) { + domerr: + sf_error("nbdtri", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + dk = k + 1; + dn = n; + w = incbi(dn, dk, p); + return (w); +} diff --git a/gtsam/3rdparty/cephes/cephes/ndtr.c b/gtsam/3rdparty/cephes/cephes/ndtr.c new file mode 100644 index 000000000..168e98b5a --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ndtr.c @@ -0,0 +1,305 @@ +/* ndtr.c + * + * Normal distribution function + * + * + * + * SYNOPSIS: + * + * double x, y, ndtr(); + * + * y = ndtr( x ); + * + * + * + * DESCRIPTION: + * + * Returns the area under the Gaussian probability density + * function, integrated from minus infinity to x: + * + * x + * - + * 1 | | 2 + * ndtr(x) = --------- | exp( - t /2 ) dt + * sqrt(2pi) | | + * - + * -inf. + * + * = ( 1 + erf(z) ) / 2 + * = erfc(z) / 2 + * + * where z = x/sqrt(2). Computation is via the functions + * erf and erfc. + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -13,0 30000 3.4e-14 6.7e-15 + * + * + * ERROR MESSAGES: + * + * message condition value returned + * erfc underflow x > 37.519379347 0.0 + * + */ +/* erf.c + * + * Error function + * + * + * + * SYNOPSIS: + * + * double x, y, erf(); + * + * y = erf( x ); + * + * + * + * DESCRIPTION: + * + * The integral is + * + * x + * - + * 2 | | 2 + * erf(x) = -------- | exp( - t ) dt. + * sqrt(pi) | | + * - + * 0 + * + * For 0 <= |x| < 1, erf(x) = x * P4(x**2)/Q5(x**2); otherwise + * erf(x) = 1 - erfc(x). + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,1 30000 3.7e-16 1.0e-16 + * + */ +/* erfc.c + * + * Complementary error function + * + * + * + * SYNOPSIS: + * + * double x, y, erfc(); + * + * y = erfc( x ); + * + * + * + * DESCRIPTION: + * + * + * 1 - erf(x) = + * + * inf. + * - + * 2 | | 2 + * erfc(x) = -------- | exp( - t ) dt + * sqrt(pi) | | + * - + * x + * + * + * For small x, erfc(x) = 1 - erf(x); otherwise rational + * approximations are computed. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,26.6417 30000 5.7e-14 1.5e-14 + */ + + +/* + * Cephes Math Library Release 2.2: June, 1992 + * Copyright 1984, 1987, 1988, 1992 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include /* DBL_EPSILON */ +#include "mconf.h" + +extern double MAXLOG; + +static double P[] = { + 2.46196981473530512524E-10, + 5.64189564831068821977E-1, + 7.46321056442269912687E0, + 4.86371970985681366614E1, + 1.96520832956077098242E2, + 5.26445194995477358631E2, + 9.34528527171957607540E2, + 1.02755188689515710272E3, + 5.57535335369399327526E2 +}; + +static double Q[] = { + /* 1.00000000000000000000E0, */ + 1.32281951154744992508E1, + 8.67072140885989742329E1, + 3.54937778887819891062E2, + 9.75708501743205489753E2, + 1.82390916687909736289E3, + 2.24633760818710981792E3, + 1.65666309194161350182E3, + 5.57535340817727675546E2 +}; + +static double R[] = { + 5.64189583547755073984E-1, + 1.27536670759978104416E0, + 5.01905042251180477414E0, + 6.16021097993053585195E0, + 7.40974269950448939160E0, + 2.97886665372100240670E0 +}; + +static double S[] = { + /* 1.00000000000000000000E0, */ + 2.26052863220117276590E0, + 9.39603524938001434673E0, + 1.20489539808096656605E1, + 1.70814450747565897222E1, + 9.60896809063285878198E0, + 3.36907645100081516050E0 +}; + +static double T[] = { + 9.60497373987051638749E0, + 9.00260197203842689217E1, + 2.23200534594684319226E3, + 7.00332514112805075473E3, + 5.55923013010394962768E4 +}; + +static double U[] = { + /* 1.00000000000000000000E0, */ + 3.35617141647503099647E1, + 5.21357949780152679795E2, + 4.59432382970980127987E3, + 2.26290000613890934246E4, + 4.92673942608635921086E4 +}; + +#define UTHRESH 37.519379347 + + +double ndtr(double a) +{ + double x, y, z; + + if (cephes_isnan(a)) { + sf_error("ndtr", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + x = a * M_SQRT1_2; + z = fabs(x); + + if (z < M_SQRT1_2) { + y = 0.5 + 0.5 * erf(x); + } + else { + y = 0.5 * erfc(z); + if (x > 0) { + y = 1.0 - y; + } + } + + return y; +} + + +double erfc(double a) +{ + double p, q, x, y, z; + + if (cephes_isnan(a)) { + sf_error("erfc", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (a < 0.0) { + x = -a; + } + else { + x = a; + } + + if (x < 1.0) { + return 1.0 - erf(a); + } + + z = -a * a; + + if (z < -MAXLOG) { + goto under; + } + + z = exp(z); + + if (x < 8.0) { + p = polevl(x, P, 8); + q = p1evl(x, Q, 8); + } + else { + p = polevl(x, R, 5); + q = p1evl(x, S, 6); + } + y = (z * p) / q; + + if (a < 0) { + y = 2.0 - y; + } + + if (y != 0.0) { + return y; + } + +under: + sf_error("erfc", SF_ERROR_UNDERFLOW, NULL); + if (a < 0) { + return 2.0; + } + else { + return 0.0; + } +} + + + +double erf(double x) +{ + double y, z; + + if (cephes_isnan(x)) { + sf_error("erf", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (x < 0.0) { + return -erf(-x); + } + + if (fabs(x) > 1.0) { + return (1.0 - erfc(x)); + } + z = x * x; + + y = x * polevl(z, T, 4) / p1evl(z, U, 5); + return y; +} diff --git a/gtsam/3rdparty/cephes/cephes/ndtri.c b/gtsam/3rdparty/cephes/cephes/ndtri.c new file mode 100644 index 000000000..e7fe5cce0 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ndtri.c @@ -0,0 +1,176 @@ +/* ndtri.c + * + * Inverse of Normal distribution function + * + * + * + * SYNOPSIS: + * + * double x, y, ndtri(); + * + * x = ndtri( y ); + * + * + * + * DESCRIPTION: + * + * Returns the argument, x, for which the area under the + * Gaussian probability density function (integrated from + * minus infinity to x) is equal to y. + * + * + * For small arguments 0 < y < exp(-2), the program computes + * z = sqrt( -2.0 * log(y) ); then the approximation is + * x = z - log(z)/z - (1/z) P(1/z) / Q(1/z). + * There are two rational functions P/Q, one for 0 < y < exp(-32) + * and the other for y up to exp(-2). For larger arguments, + * w = y - 0.5, and x/sqrt(2pi) = w + w**3 R(w**2)/S(w**2)). + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0.125, 1 20000 7.2e-16 1.3e-16 + * IEEE 3e-308, 0.135 50000 4.6e-16 9.8e-17 + * + * + * ERROR MESSAGES: + * + * message condition value returned + * ndtri domain x < 0 NAN + * ndtri domain x > 1 NAN + * + */ + + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +/* sqrt(2pi) */ +static double s2pi = 2.50662827463100050242E0; + +/* approximation for 0 <= |y - 0.5| <= 3/8 */ +static double P0[5] = { + -5.99633501014107895267E1, + 9.80010754185999661536E1, + -5.66762857469070293439E1, + 1.39312609387279679503E1, + -1.23916583867381258016E0, +}; + +static double Q0[8] = { + /* 1.00000000000000000000E0, */ + 1.95448858338141759834E0, + 4.67627912898881538453E0, + 8.63602421390890590575E1, + -2.25462687854119370527E2, + 2.00260212380060660359E2, + -8.20372256168333339912E1, + 1.59056225126211695515E1, + -1.18331621121330003142E0, +}; + +/* Approximation for interval z = sqrt(-2 log y ) between 2 and 8 + * i.e., y between exp(-2) = .135 and exp(-32) = 1.27e-14. + */ +static double P1[9] = { + 4.05544892305962419923E0, + 3.15251094599893866154E1, + 5.71628192246421288162E1, + 4.40805073893200834700E1, + 1.46849561928858024014E1, + 2.18663306850790267539E0, + -1.40256079171354495875E-1, + -3.50424626827848203418E-2, + -8.57456785154685413611E-4, +}; + +static double Q1[8] = { + /* 1.00000000000000000000E0, */ + 1.57799883256466749731E1, + 4.53907635128879210584E1, + 4.13172038254672030440E1, + 1.50425385692907503408E1, + 2.50464946208309415979E0, + -1.42182922854787788574E-1, + -3.80806407691578277194E-2, + -9.33259480895457427372E-4, +}; + +/* Approximation for interval z = sqrt(-2 log y ) between 8 and 64 + * i.e., y between exp(-32) = 1.27e-14 and exp(-2048) = 3.67e-890. + */ + +static double P2[9] = { + 3.23774891776946035970E0, + 6.91522889068984211695E0, + 3.93881025292474443415E0, + 1.33303460815807542389E0, + 2.01485389549179081538E-1, + 1.23716634817820021358E-2, + 3.01581553508235416007E-4, + 2.65806974686737550832E-6, + 6.23974539184983293730E-9, +}; + +static double Q2[8] = { + /* 1.00000000000000000000E0, */ + 6.02427039364742014255E0, + 3.67983563856160859403E0, + 1.37702099489081330271E0, + 2.16236993594496635890E-1, + 1.34204006088543189037E-2, + 3.28014464682127739104E-4, + 2.89247864745380683936E-6, + 6.79019408009981274425E-9, +}; + +double ndtri(double y0) +{ + double x, y, z, y2, x0, x1; + int code; + + if (y0 == 0.0) { + return -INFINITY; + } + if (y0 == 1.0) { + return INFINITY; + } + if (y0 < 0.0 || y0 > 1.0) { + sf_error("ndtri", SF_ERROR_DOMAIN, NULL); + return NAN; + } + code = 1; + y = y0; + if (y > (1.0 - 0.13533528323661269189)) { /* 0.135... = exp(-2) */ + y = 1.0 - y; + code = 0; + } + + if (y > 0.13533528323661269189) { + y = y - 0.5; + y2 = y * y; + x = y + y * (y2 * polevl(y2, P0, 4) / p1evl(y2, Q0, 8)); + x = x * s2pi; + return (x); + } + + x = sqrt(-2.0 * log(y)); + x0 = x - log(x) / x; + + z = 1.0 / x; + if (x < 8.0) /* y > exp(-32) = 1.2664165549e-14 */ + x1 = z * polevl(z, P1, 8) / p1evl(z, Q1, 8); + else + x1 = z * polevl(z, P2, 8) / p1evl(z, Q2, 8); + x = x0 - x1; + if (code != 0) + x = -x; + return (x); +} diff --git a/gtsam/3rdparty/cephes/cephes/owens_t.c b/gtsam/3rdparty/cephes/cephes/owens_t.c new file mode 100644 index 000000000..6eb063510 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/owens_t.c @@ -0,0 +1,364 @@ +/* Copyright Benjamin Sobotta 2012 + * + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. (See accompanying file + * LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt) + */ + +/* + * Reference: + * Mike Patefield, David Tandy + * FAST AND ACCURATE CALCULATION OF OWEN'S T-FUNCTION + * Journal of Statistical Software, 5 (5), 1-25 + */ +#include "mconf.h" + +static const int SELECT_METHOD[] = { + 0, 0, 1, 12, 12, 12, 12, 12, 12, 12, 12, 15, 15, 15, 8, + 0, 1, 1, 2, 2, 4, 4, 13, 13, 14, 14, 15, 15, 15, 8, + 1, 1, 2, 2, 2, 4, 4, 14, 14, 14, 14, 15, 15, 15, 9, + 1, 1, 2, 4, 4, 4, 4, 6, 6, 15, 15, 15, 15, 15, 9, + 1, 2 , 2, 4, 4, 5 , 5, 7, 7, 16 ,16, 16, 11, 11, 10, + 1, 2 , 4, 4 , 4, 5 , 5, 7, 7, 16, 16, 16, 11, 11, 11, + 1, 2 , 3, 3, 5, 5 , 7, 7, 16, 16, 16, 16, 16, 11, 11, + 1, 2 , 3 , 3 , 5, 5, 17, 17, 17, 17, 16, 16, 16, 11, 11 +}; + +static const double HRANGE[] = {0.02, 0.06, 0.09, 0.125, 0.26, 0.4, 0.6, 1.6, + 1.7, 2.33, 2.4, 3.36, 3.4, 4.8}; + +static const double ARANGE[] = {0.025, 0.09, 0.15, 0.36, 0.5, 0.9, 0.99999}; + +static const double ORD[] = {2, 3, 4, 5, 7, 10, 12, 18, 10, 20, 30, 0, 4, 7, + 8, 20, 0, 0}; + +static const int METHODS[] = {1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 3, 4, 4, 4, 4, + 5, 6}; + +static const double C[] = { + 0.99999999999999999999999729978162447266851932041876728736094298092917625009873, + -0.99999999999999999999467056379678391810626533251885323416799874878563998732905968, + 0.99999999999999999824849349313270659391127814689133077036298754586814091034842536, + -0.9999999999999997703859616213643405880166422891953033591551179153879839440241685, + 0.99999999999998394883415238173334565554173013941245103172035286759201504179038147, + -0.9999999999993063616095509371081203145247992197457263066869044528823599399470977, + 0.9999999999797336340409464429599229870590160411238245275855903767652432017766116267, + -0.999999999574958412069046680119051639753412378037565521359444170241346845522403274, + 0.9999999933226234193375324943920160947158239076786103108097456617750134812033362048, + -0.9999999188923242461073033481053037468263536806742737922476636768006622772762168467, + 0.9999992195143483674402853783549420883055129680082932629160081128947764415749728967, + -0.999993935137206712830997921913316971472227199741857386575097250553105958772041501, + 0.99996135597690552745362392866517133091672395614263398912807169603795088421057688716, + -0.99979556366513946026406788969630293820987757758641211293079784585126692672425362469, + 0.999092789629617100153486251423850590051366661947344315423226082520411961968929483, + -0.996593837411918202119308620432614600338157335862888580671450938858935084316004769854, + 0.98910017138386127038463510314625339359073956513420458166238478926511821146316469589567, + -0.970078558040693314521331982203762771512160168582494513347846407314584943870399016019, + 0.92911438683263187495758525500033707204091967947532160289872782771388170647150321633673, + -0.8542058695956156057286980736842905011429254735181323743367879525470479126968822863, + 0.73796526033030091233118357742803709382964420335559408722681794195743240930748630755, + -0.58523469882837394570128599003785154144164680587615878645171632791404210655891158, + 0.415997776145676306165661663581868460503874205343014196580122174949645271353372263, + -0.2588210875241943574388730510317252236407805082485246378222935376279663808416534365, + 0.1375535825163892648504646951500265585055789019410617565727090346559210218472356689, + -0.0607952766325955730493900985022020434830339794955745989150270485056436844239206648, + 0.0216337683299871528059836483840390514275488679530797294557060229266785853764115, + -0.00593405693455186729876995814181203900550014220428843483927218267309209471516256, + 0.0011743414818332946510474576182739210553333860106811865963485870668929503649964142, + -1.489155613350368934073453260689881330166342484405529981510694514036264969925132E-4, + 9.072354320794357587710929507988814669454281514268844884841547607134260303118208E-6 +}; + +static const double PTS[] = { + 0.35082039676451715489E-02, 0.31279042338030753740E-01, + 0.85266826283219451090E-01, 0.16245071730812277011E+00, + 0.25851196049125434828E+00, 0.36807553840697533536E+00, + 0.48501092905604697475E+00, 0.60277514152618576821E+00, + 0.71477884217753226516E+00, 0.81475510988760098605E+00, + 0.89711029755948965867E+00, 0.95723808085944261843E+00, + 0.99178832974629703586E+00 +}; + +static const double WTS[] = { + 0.18831438115323502887E-01, 0.18567086243977649478E-01, + 0.18042093461223385584E-01, 0.17263829606398753364E-01, + 0.16243219975989856730E-01, 0.14994592034116704829E-01, + 0.13535474469662088392E-01, 0.11886351605820165233E-01, + 0.10070377242777431897E-01, 0.81130545742299586629E-02, + 0.60419009528470238773E-02, 0.38862217010742057883E-02, + 0.16793031084546090448E-02 +}; + + +static int get_method(double h, double a) { + int ihint, iaint, i; + + ihint = 14; + iaint = 7; + + for (i = 0; i < 14; i++) { + if (h <= HRANGE[i]) { + ihint = i; + break; + } + } + + for (i = 0; i < 7; i++) { + if (a <= ARANGE[i]) { + iaint = i; + break; + } + } + return SELECT_METHOD[iaint * 15 + ihint]; +} + + +static double owens_t_norm1(double x) { + return erf(x / sqrt(2)) / 2; +} + + +static double owens_t_norm2(double x) { + return erfc(x / sqrt(2)) / 2; +} + + +static double owensT1(double h, double a, double m) { + int j = 1; + int jj = 1; + + double hs = -0.5 * h * h; + double dhs = exp(hs); + double as = a * a; + double aj = a / (2 * M_PI); + double dj = expm1(hs); + double gj = hs * dhs; + + double val = atan(a) / (2 * M_PI); + + while (1) { + val += dj*aj / jj; + + if (m <= j) { + break; + } + j++; + jj += 2; + aj *= as; + dj = gj - dj; + gj *= hs / j; + } + + return val; +} + + +static double owensT2(double h, double a, double ah, double m) { + int i = 1; + int maxi = 2 * m + 1; + double hs = h * h; + double as = -a * a; + double y = 1.0 / hs; + double val = 0.0; + double vi = a*exp(-0.5 * ah * ah) / sqrt(2 * M_PI); + double z = (ndtr(ah) - 0.5) / h; + + while (1) { + val += z; + if (maxi <= i) { + break; + } + z = y * (vi - i * z); + vi *= as; + i += 2; + } + val *= exp(-0.5 * hs) / sqrt(2 * M_PI); + + return val; +} + + +static double owensT3(double h, double a, double ah) { + double aa, hh, y, vi, zi, result; + int i; + + aa = a * a; + hh = h * h; + y = 1 / hh; + + vi = a * exp(-ah * ah/ 2) / sqrt(2 * M_PI); + zi = owens_t_norm1(ah) / h; + result = 0; + + for(i = 0; i<= 30; i++) { + result += zi * C[i]; + zi = y * ((2 * i + 1) * zi - vi); + vi *= aa; + } + + result *= exp(-hh / 2) / sqrt(2 * M_PI); + + return result; +} + + +static double owensT4(double h, double a, double m) { + double maxi, hh, naa, ai, yi, result; + int i; + + maxi = 2 * m + 1; + hh = h * h; + naa = -a * a; + + i = 1; + ai = a * exp(-hh * (1 - naa) / 2) / (2 * M_PI); + yi = 1; + result = 0; + + while (1) { + result += ai * yi; + + if (maxi <= i) { + break; + } + + i += 2; + yi = (1 - hh * yi) / i; + ai *= naa; + } + + return result; +} + + +static double owensT5(double h, double a) { + double result, r, aa, nhh; + int i; + + result = 0; + r = 0; + aa = a * a; + nhh = -0.5 * h * h; + + for (i = 1; i < 14; i++) { + r = 1 + aa * PTS[i - 1]; + result += WTS[i - 1] * exp(nhh * r) / r; + } + + result *= a; + + return result; +} + + +static double owensT6(double h, double a) { + double normh, y, r, result; + + normh = owens_t_norm2(h); + y = 1 - a; + r = atan2(y, (1 + a)); + result = normh * (1 - normh) / 2; + + if (r != 0) { + result -= r * exp(-y * h * h / (2 * r)) / (2 * M_PI); + } + + return result; +} + + +static double owens_t_dispatch(double h, double a, double ah) { + int index, meth_code; + double m, result; + + if (h == 0) { + return atan(a) / (2 * M_PI); + } + if (a == 0) { + return 0; + } + if (a == 1) { + return owens_t_norm2(-h) * owens_t_norm2(h) / 2; + } + + index = get_method(h, a); + m = ORD[index]; + meth_code = METHODS[index]; + + switch(meth_code) { + case 1: + result = owensT1(h, a, m); + break; + case 2: + result = owensT2(h, a, ah, m); + break; + case 3: + result = owensT3(h, a, ah); + break; + case 4: + result = owensT4(h, a, m); + break; + case 5: + result = owensT5(h, a); + break; + case 6: + result = owensT6(h, a); + break; + default: + result = NAN; + } + + return result; +} + + +double owens_t(double h, double a) { + double result, fabs_a, fabs_ah, normh, normah; + + if (cephes_isnan(h) || cephes_isnan(a)) { + return NAN; + } + + /* exploit that T(-h,a) == T(h,a) */ + h = fabs(h); + + /* + * Use equation (2) in the paper to remap the arguments such that + * h >= 0 and 0 <= a <= 1 for the call of the actual computation + * routine. + */ + fabs_a = fabs(a); + fabs_ah = fabs_a * h; + + if (fabs_a == INFINITY) { + /* See page 13 in the paper */ + result = 0.5 * owens_t_norm2(h); + } + else if (h == INFINITY) { + result = 0; + } + else if (fabs_a <= 1) { + result = owens_t_dispatch(h, fabs_a, fabs_ah); + } + else { + if (fabs_ah <= 0.67) { + normh = owens_t_norm1(h); + normah = owens_t_norm1(fabs_ah); + result = 0.25 - normh * normah - + owens_t_dispatch(fabs_ah, (1 / fabs_a), h); + } + else { + normh = owens_t_norm2(h); + normah = owens_t_norm2(fabs_ah); + result = (normh + normah) / 2 - normh * normah - + owens_t_dispatch(fabs_ah, (1 / fabs_a), h); + } + } + + if (a < 0) { + /* exploit that T(h,-a) == -T(h,a) */ + return -result; + } + + return result; +} diff --git a/gtsam/3rdparty/cephes/cephes/pdtr.c b/gtsam/3rdparty/cephes/cephes/pdtr.c new file mode 100644 index 000000000..0249074d9 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/pdtr.c @@ -0,0 +1,173 @@ +/* pdtr.c + * + * Poisson distribution + * + * + * + * SYNOPSIS: + * + * int k; + * double m, y, pdtr(); + * + * y = pdtr( k, m ); + * + * + * + * DESCRIPTION: + * + * Returns the sum of the first k terms of the Poisson + * distribution: + * + * k j + * -- -m m + * > e -- + * -- j! + * j=0 + * + * The terms are not summed directly; instead the incomplete + * Gamma integral is employed, according to the relation + * + * y = pdtr( k, m ) = igamc( k+1, m ). + * + * The arguments must both be nonnegative. + * + * + * + * ACCURACY: + * + * See igamc(). + * + */ +/* pdtrc() + * + * Complemented poisson distribution + * + * + * + * SYNOPSIS: + * + * int k; + * double m, y, pdtrc(); + * + * y = pdtrc( k, m ); + * + * + * + * DESCRIPTION: + * + * Returns the sum of the terms k+1 to infinity of the Poisson + * distribution: + * + * inf. j + * -- -m m + * > e -- + * -- j! + * j=k+1 + * + * The terms are not summed directly; instead the incomplete + * Gamma integral is employed, according to the formula + * + * y = pdtrc( k, m ) = igam( k+1, m ). + * + * The arguments must both be nonnegative. + * + * + * + * ACCURACY: + * + * See igam.c. + * + */ +/* pdtri() + * + * Inverse Poisson distribution + * + * + * + * SYNOPSIS: + * + * int k; + * double m, y, pdtr(); + * + * m = pdtri( k, y ); + * + * + * + * + * DESCRIPTION: + * + * Finds the Poisson variable x such that the integral + * from 0 to x of the Poisson density is equal to the + * given probability y. + * + * This is accomplished using the inverse Gamma integral + * function and the relation + * + * m = igamci( k+1, y ). + * + * + * + * + * ACCURACY: + * + * See igami.c. + * + * ERROR MESSAGES: + * + * message condition value returned + * pdtri domain y < 0 or y >= 1 0.0 + * k < 0 + * + */ + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +double pdtrc(double k, double m) +{ + double v; + + if (k < 0.0 || m < 0.0) { + sf_error("pdtrc", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (m == 0.0) { + return 0.0; + } + v = floor(k) + 1; + return (igam(v, m)); +} + + +double pdtr(double k, double m) +{ + double v; + + if (k < 0 || m < 0) { + sf_error("pdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (m == 0.0) { + return 1.0; + } + v = floor(k) + 1; + return (igamc(v, m)); +} + + +double pdtri(int k, double y) +{ + double v; + + if ((k < 0) || (y < 0.0) || (y >= 1.0)) { + sf_error("pdtri", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + v = k + 1; + v = igamci(v, y); + return (v); +} diff --git a/gtsam/3rdparty/cephes/cephes/poch.c b/gtsam/3rdparty/cephes/cephes/poch.c new file mode 100644 index 000000000..4c04fa14e --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/poch.c @@ -0,0 +1,81 @@ +/* + * Pochhammer symbol (a)_m = gamma(a + m) / gamma(a) + */ +#include "mconf.h" + +static double is_nonpos_int(double x) +{ + return x <= 0 && x == ceil(x) && fabs(x) < 1e13; +} + +double poch(double a, double m) +{ + double r; + + r = 1.0; + + /* + * 1. Reduce magnitude of `m` to |m| < 1 by using recurrence relations. + * + * This may end up in over/underflow, but then the function itself either + * diverges or goes to zero. In case the remainder goes to the opposite + * direction, we end up returning 0*INF = NAN, which is OK. + */ + + /* Recurse down */ + while (m >= 1.0) { + if (a + m == 1) { + break; + } + m -= 1.0; + r *= (a + m); + if (!isfinite(r) || r == 0) { + break; + } + } + + /* Recurse up */ + while (m <= -1.0) { + if (a + m == 0) { + break; + } + r /= (a + m); + m += 1.0; + if (!isfinite(r) || r == 0) { + break; + } + } + + /* + * 2. Evaluate function with reduced `m` + * + * Now either `m` is not big, or the `r` product has over/underflown. + * If so, the function itself does similarly. + */ + + if (m == 0) { + /* Easy case */ + return r; + } + else if (a > 1e4 && fabs(m) <= 1) { + /* Avoid loss of precision */ + return r * pow(a, m) * ( + 1 + + m*(m-1)/(2*a) + + m*(m-1)*(m-2)*(3*m-1)/(24*a*a) + + m*m*(m-1)*(m-1)*(m-2)*(m-3)/(48*a*a*a) + ); + } + + /* Check for infinity */ + if (is_nonpos_int(a + m) && !is_nonpos_int(a) && a + m != m) { + return INFINITY; + } + + /* Check for zero */ + if (!is_nonpos_int(a + m) && is_nonpos_int(a)) { + return 0; + } + + return r * exp(lgam(a + m) - lgam(a)) * gammasgn(a + m) * gammasgn(a); +} diff --git a/gtsam/3rdparty/cephes/cephes/polevl.h b/gtsam/3rdparty/cephes/cephes/polevl.h new file mode 100644 index 000000000..eb23ddf88 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/polevl.h @@ -0,0 +1,165 @@ +/* polevl.c + * p1evl.c + * + * Evaluate polynomial + * + * + * + * SYNOPSIS: + * + * int N; + * double x, y, coef[N+1], polevl[]; + * + * y = polevl( x, coef, N ); + * + * + * + * DESCRIPTION: + * + * Evaluates polynomial of degree N: + * + * 2 N + * y = C + C x + C x +...+ C x + * 0 1 2 N + * + * Coefficients are stored in reverse order: + * + * coef[0] = C , ..., coef[N] = C . + * N 0 + * + * The function p1evl() assumes that c_N = 1.0 so that coefficent + * is omitted from the array. Its calling arguments are + * otherwise the same as polevl(). + * + * + * SPEED: + * + * In the interest of speed, there are no checks for out + * of bounds arithmetic. This routine is used by most of + * the functions in the library. Depending on available + * equipment features, the user may wish to rewrite the + * program in microcode or assembly language. + * + */ + +/* + * Cephes Math Library Release 2.1: December, 1988 + * Copyright 1984, 1987, 1988 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +/* Sources: + * [1] Holin et. al., "Polynomial and Rational Function Evaluation", + * https://www.boost.org/doc/libs/1_61_0/libs/math/doc/html/math_toolkit/roots/rational.html + */ + +/* Scipy changes: + * - 06-23-2016: add code for evaluating rational functions + */ + +#ifndef CEPHES_POLEV +#define CEPHES_POLEV + +#include + +static inline double polevl(double x, const double coef[], int N) +{ + double ans; + int i; + const double *p; + + p = coef; + ans = *p++; + i = N; + + do + ans = ans * x + *p++; + while (--i); + + return (ans); +} + +/* p1evl() */ +/* N + * Evaluate polynomial when coefficient of x is 1.0. + * That is, C_{N} is assumed to be 1, and that coefficient + * is not included in the input array coef. + * coef must have length N and contain the polynomial coefficients + * stored as + * coef[0] = C_{N-1} + * coef[1] = C_{N-2} + * ... + * coef[N-2] = C_1 + * coef[N-1] = C_0 + * Otherwise same as polevl. + */ + +static inline double p1evl(double x, const double coef[], int N) +{ + double ans; + const double *p; + int i; + + p = coef; + ans = x + *p++; + i = N - 1; + + do + ans = ans * x + *p++; + while (--i); + + return (ans); +} + +/* Evaluate a rational function. See [1]. */ + +static inline double ratevl(double x, const double num[], int M, + const double denom[], int N) +{ + int i, dir; + double y, num_ans, denom_ans; + double absx = fabs(x); + const double *p; + + if (absx > 1) { + /* Evaluate as a polynomial in 1/x. */ + dir = -1; + p = num + M; + y = 1 / x; + } else { + dir = 1; + p = num; + y = x; + } + + /* Evaluate the numerator */ + num_ans = *p; + p += dir; + for (i = 1; i <= M; i++) { + num_ans = num_ans * y + *p; + p += dir; + } + + /* Evaluate the denominator */ + if (absx > 1) { + p = denom + N; + } else { + p = denom; + } + + denom_ans = *p; + p += dir; + for (i = 1; i <= N; i++) { + denom_ans = denom_ans * y + *p; + p += dir; + } + + if (absx > 1) { + i = N - M; + return pow(x, i) * num_ans / denom_ans; + } else { + return num_ans / denom_ans; + } +} + +#endif diff --git a/gtsam/3rdparty/cephes/cephes/psi.c b/gtsam/3rdparty/cephes/cephes/psi.c new file mode 100644 index 000000000..190c6d162 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/psi.c @@ -0,0 +1,205 @@ +/* psi.c + * + * Psi (digamma) function + * + * + * SYNOPSIS: + * + * double x, y, psi(); + * + * y = psi( x ); + * + * + * DESCRIPTION: + * + * d - + * psi(x) = -- ln | (x) + * dx + * + * is the logarithmic derivative of the gamma function. + * For integer x, + * n-1 + * - + * psi(n) = -EUL + > 1/k. + * - + * k=1 + * + * This formula is used for 0 < n <= 10. If x is negative, it + * is transformed to a positive argument by the reflection + * formula psi(1-x) = psi(x) + pi cot(pi x). + * For general positive x, the argument is made greater than 10 + * using the recurrence psi(x+1) = psi(x) + 1/x. + * Then the following asymptotic expansion is applied: + * + * inf. B + * - 2k + * psi(x) = log(x) - 1/2x - > ------- + * - 2k + * k=1 2k x + * + * where the B2k are Bernoulli numbers. + * + * ACCURACY: + * Relative error (except absolute when |psi| < 1): + * arithmetic domain # trials peak rms + * IEEE 0,30 30000 1.3e-15 1.4e-16 + * IEEE -30,0 40000 1.5e-15 2.2e-16 + * + * ERROR MESSAGES: + * message condition value returned + * psi singularity x integer <=0 INFINITY + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1992, 2000 by Stephen L. Moshier + */ + +/* + * Code for the rational approximation on [1, 2] is: + * + * (C) Copyright John Maddock 2006. + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. (See accompanying file + * LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt) + */ + +#include "mconf.h" + +static double A[] = { + 8.33333333333333333333E-2, + -2.10927960927960927961E-2, + 7.57575757575757575758E-3, + -4.16666666666666666667E-3, + 3.96825396825396825397E-3, + -8.33333333333333333333E-3, + 8.33333333333333333333E-2 +}; + + +static double digamma_imp_1_2(double x) +{ + /* + * Rational approximation on [1, 2] taken from Boost. + * + * Now for the approximation, we use the form: + * + * digamma(x) = (x - root) * (Y + R(x-1)) + * + * Where root is the location of the positive root of digamma, + * Y is a constant, and R is optimised for low absolute error + * compared to Y. + * + * Maximum Deviation Found: 1.466e-18 + * At double precision, max error found: 2.452e-17 + */ + double r, g; + + static const float Y = 0.99558162689208984f; + + static const double root1 = 1569415565.0 / 1073741824.0; + static const double root2 = (381566830.0 / 1073741824.0) / 1073741824.0; + static const double root3 = 0.9016312093258695918615325266959189453125e-19; + + static double P[] = { + -0.0020713321167745952, + -0.045251321448739056, + -0.28919126444774784, + -0.65031853770896507, + -0.32555031186804491, + 0.25479851061131551 + }; + static double Q[] = { + -0.55789841321675513e-6, + 0.0021284987017821144, + 0.054151797245674225, + 0.43593529692665969, + 1.4606242909763515, + 2.0767117023730469, + 1.0 + }; + g = x - root1; + g -= root2; + g -= root3; + r = polevl(x - 1.0, P, 5) / polevl(x - 1.0, Q, 6); + + return g * Y + g * r; +} + + +static double psi_asy(double x) +{ + double y, z; + + if (x < 1.0e17) { + z = 1.0 / (x * x); + y = z * polevl(z, A, 6); + } + else { + y = 0.0; + } + + return log(x) - (0.5 / x) - y; +} + + +double psi(double x) +{ + double y = 0.0; + double q, r; + int i, n; + + if (isnan(x)) { + return x; + } + else if (x == INFINITY) { + return x; + } + else if (x == -INFINITY) { + return NAN; + } + else if (x == 0) { + sf_error("psi", SF_ERROR_SINGULAR, NULL); + return copysign(INFINITY, -x); + } + else if (x < 0.0) { + /* argument reduction before evaluating tan(pi * x) */ + r = modf(x, &q); + if (r == 0.0) { + sf_error("psi", SF_ERROR_SINGULAR, NULL); + return NAN; + } + y = -M_PI / tan(M_PI * r); + x = 1.0 - x; + } + + /* check for positive integer up to 10 */ + if ((x <= 10.0) && (x == floor(x))) { + n = (int)x; + for (i = 1; i < n; i++) { + y += 1.0 / i; + } + y -= SCIPY_EULER; + return y; + } + + /* use the recurrence relation to move x into [1, 2] */ + if (x < 1.0) { + y -= 1.0 / x; + x += 1.0; + } + else if (x < 10.0) { + while (x > 2.0) { + x -= 1.0; + y += 1.0 / x; + } + } + if ((1.0 <= x) && (x <= 2.0)) { + y += digamma_imp_1_2(x); + return y; + } + + /* x is large, use the asymptotic series */ + y += psi_asy(x); + return y; +} diff --git a/gtsam/3rdparty/cephes/cephes/rgamma.c b/gtsam/3rdparty/cephes/cephes/rgamma.c new file mode 100644 index 000000000..6420ccaa9 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/rgamma.c @@ -0,0 +1,128 @@ +/* rgamma.c + * + * Reciprocal Gamma function + * + * + * + * SYNOPSIS: + * + * double x, y, rgamma(); + * + * y = rgamma( x ); + * + * + * + * DESCRIPTION: + * + * Returns one divided by the Gamma function of the argument. + * + * The function is approximated by a Chebyshev expansion in + * the interval [0,1]. Range reduction is by recurrence + * for arguments between -34.034 and +34.84425627277176174. + * 0 is returned for positive arguments outside this + * range. For arguments less than -34.034 the cosecant + * reflection formula is applied; lograrithms are employed + * to avoid unnecessary overflow. + * + * The reciprocal Gamma function has no singularities, + * but overflow and underflow may occur for large arguments. + * These conditions return either INFINITY or 0 with + * appropriate sign. + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -30,+30 30000 1.1e-15 2.0e-16 + * For arguments less than -34.034 the peak error is on the + * order of 5e-15 (DEC), excepting overflow or underflow. + */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1985, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +/* Chebyshev coefficients for reciprocal Gamma function + * in interval 0 to 1. Function is 1/(x Gamma(x)) - 1 + */ + +static double R[] = { + 3.13173458231230000000E-17, + -6.70718606477908000000E-16, + 2.20039078172259550000E-15, + 2.47691630348254132600E-13, + -6.60074100411295197440E-12, + 5.13850186324226978840E-11, + 1.08965386454418662084E-9, + -3.33964630686836942556E-8, + 2.68975996440595483619E-7, + 2.96001177518801696639E-6, + -8.04814124978471142852E-5, + 4.16609138709688864714E-4, + 5.06579864028608725080E-3, + -6.41925436109158228810E-2, + -4.98558728684003594785E-3, + 1.27546015610523951063E-1 +}; + +static char name[] = "rgamma"; + +extern double MAXLOG; + + +double rgamma(double x) +{ + double w, y, z; + int sign; + + if (x > 34.84425627277176174) { + return exp(-lgam(x)); + } + if (x < -34.034) { + w = -x; + z = sinpi(w); + if (z == 0.0) { + return 0.0; + } + if (z < 0.0) { + sign = 1; + z = -z; + } + else { + sign = -1; + } + + y = log(w * z) - log(M_PI) + lgam(w); + if (y < -MAXLOG) { + sf_error(name, SF_ERROR_UNDERFLOW, NULL); + return (sign * 0.0); + } + if (y > MAXLOG) { + sf_error(name, SF_ERROR_OVERFLOW, NULL); + return (sign * INFINITY); + } + return (sign * exp(y)); + } + z = 1.0; + w = x; + + while (w > 1.0) { /* Downward recurrence */ + w -= 1.0; + z *= w; + } + while (w < 0.0) { /* Upward recurrence */ + z /= w; + w += 1.0; + } + if (w == 0.0) /* Nonpositive integer */ + return (0.0); + if (w == 1.0) /* Other integer */ + return (1.0 / z); + + y = w * (1.0 + chbevl(4.0 * w - 2.0, R, 16)) / z; + return (y); +} diff --git a/gtsam/3rdparty/cephes/cephes/round.c b/gtsam/3rdparty/cephes/cephes/round.c new file mode 100644 index 000000000..0ed1f1415 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/round.c @@ -0,0 +1,63 @@ +/* round.c + * + * Round double to nearest or even integer valued double + * + * + * + * SYNOPSIS: + * + * double x, y, round(); + * + * y = round(x); + * + * + * + * DESCRIPTION: + * + * Returns the nearest integer to x as a double precision + * floating point result. If x ends in 0.5 exactly, the + * nearest even integer is chosen. + * + * + * + * ACCURACY: + * + * If x is greater than 1/(2*MACHEP), its closest machine + * representation is already an integer, so rounding does + * not change it. + */ + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +double round(double x) +{ + double y, r; + + /* Largest integer <= x */ + y = floor(x); + + /* Fractional part */ + r = x - y; + + /* Round up to nearest. */ + if (r > 0.5) + goto rndup; + + /* Round to even */ + if (r == 0.5) { + r = y - 2.0 * floor(0.5 * y); + if (r == 1.0) { + rndup: + y += 1.0; + } + } + + /* Else round down. */ + return (y); +} diff --git a/gtsam/3rdparty/cephes/cephes/scipy_iv.c b/gtsam/3rdparty/cephes/cephes/scipy_iv.c new file mode 100644 index 000000000..e7bb22011 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/scipy_iv.c @@ -0,0 +1,654 @@ +/* iv.c + * + * Modified Bessel function of noninteger order + * + * + * + * SYNOPSIS: + * + * double v, x, y, iv(); + * + * y = iv( v, x ); + * + * + * + * DESCRIPTION: + * + * Returns modified Bessel function of order v of the + * argument. If x is negative, v must be integer valued. + * + */ +/* iv.c */ +/* Modified Bessel function of noninteger order */ +/* If x < 0, then v must be an integer. */ + + +/* + * Parts of the code are copyright: + * + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1988, 2000 by Stephen L. Moshier + * + * And other parts: + * + * Copyright (c) 2006 Xiaogang Zhang + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. + * + * Boost Software License - Version 1.0 - August 17th, 2003 + * + * Permission is hereby granted, free of charge, to any person or + * organization obtaining a copy of the software and accompanying + * documentation covered by this license (the "Software") to use, reproduce, + * display, distribute, execute, and transmit the Software, and to prepare + * derivative works of the Software, and to permit third-parties to whom the + * Software is furnished to do so, all subject to the following: + * + * The copyright notices in the Software and this entire statement, + * including the above license grant, this restriction and the following + * disclaimer, must be included in all copies of the Software, in whole or + * in part, and all derivative works of the Software, unless such copies or + * derivative works are solely in the form of machine-executable object code + * generated by a source language processor. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE AND + * NON-INFRINGEMENT. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR ANYONE + * DISTRIBUTING THE SOFTWARE BE LIABLE FOR ANY DAMAGES OR OTHER LIABILITY, + * WHETHER IN CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * And the rest are: + * + * Copyright (C) 2009 Pauli Virtanen + * Distributed under the same license as Scipy. + * + */ + +#include "mconf.h" +#include +#include + +extern double MACHEP; + +static double iv_asymptotic(double v, double x); +static void ikv_asymptotic_uniform(double v, double x, double *Iv, double *Kv); +static void ikv_temme(double v, double x, double *Iv, double *Kv); + +double iv(double v, double x) +{ + int sign; + double t, ax, res; + + if (isnan(v) || isnan(x)) { + return NAN; + } + + /* If v is a negative integer, invoke symmetry */ + t = floor(v); + if (v < 0.0) { + if (t == v) { + v = -v; /* symmetry */ + t = -t; + } + } + /* If x is negative, require v to be an integer */ + sign = 1; + if (x < 0.0) { + if (t != v) { + sf_error("iv", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (v != 2.0 * floor(v / 2.0)) { + sign = -1; + } + } + + /* Avoid logarithm singularity */ + if (x == 0.0) { + if (v == 0.0) { + return 1.0; + } + if (v < 0.0) { + sf_error("iv", SF_ERROR_OVERFLOW, NULL); + return INFINITY; + } + else + return 0.0; + } + + ax = fabs(x); + if (fabs(v) > 50) { + /* + * Uniform asymptotic expansion for large orders. + * + * This appears to overflow slightly later than the Boost + * implementation of Temme's method. + */ + ikv_asymptotic_uniform(v, ax, &res, NULL); + } + else { + /* Otherwise: Temme's method */ + ikv_temme(v, ax, &res, NULL); + } + res *= sign; + return res; +} + + +/* + * Compute Iv from (AMS5 9.7.1), asymptotic expansion for large |z| + * Iv ~ exp(x)/sqrt(2 pi x) ( 1 + (4*v*v-1)/8x + (4*v*v-1)(4*v*v-9)/8x/2! + ...) + */ +static double iv_asymptotic(double v, double x) +{ + double mu; + double sum, term, prefactor, factor; + int k; + + prefactor = exp(x) / sqrt(2 * M_PI * x); + + if (prefactor == INFINITY) { + return prefactor; + } + + mu = 4 * v * v; + sum = 1.0; + term = 1.0; + k = 1; + + do { + factor = (mu - (2 * k - 1) * (2 * k - 1)) / (8 * x) / k; + if (k > 100) { + /* didn't converge */ + sf_error("iv(iv_asymptotic)", SF_ERROR_NO_RESULT, NULL); + break; + } + term *= -factor; + sum += term; + ++k; + } while (fabs(term) > MACHEP * fabs(sum)); + return sum * prefactor; +} + + +/* + * Uniform asymptotic expansion factors, (AMS5 9.3.9; AMS5 9.3.10) + * + * Computed with: + * -------------------- + import numpy as np + t = np.poly1d([1,0]) + def up1(p): + return .5*t*t*(1-t*t)*p.deriv() + 1/8. * ((1-5*t*t)*p).integ() + us = [np.poly1d([1])] + for k in range(10): + us.append(up1(us[-1])) + n = us[-1].order + for p in us: + print "{" + ", ".join(["0"]*(n-p.order) + map(repr, p)) + "}," + print "N_UFACTORS", len(us) + print "N_UFACTOR_TERMS", us[-1].order + 1 + * -------------------- + */ +#define N_UFACTORS 11 +#define N_UFACTOR_TERMS 31 +static const double asymptotic_ufactors[N_UFACTORS][N_UFACTOR_TERMS] = { + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, -0.20833333333333334, 0.0, 0.125, 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.3342013888888889, 0.0, -0.40104166666666669, 0.0, 0.0703125, 0.0, + 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + -1.0258125964506173, 0.0, 1.8464626736111112, 0.0, + -0.89121093750000002, 0.0, 0.0732421875, 0.0, 0.0, 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 4.6695844234262474, 0.0, -11.207002616222995, 0.0, 8.78912353515625, + 0.0, -2.3640869140624998, 0.0, 0.112152099609375, 0.0, 0.0, 0.0, 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -28.212072558200244, 0.0, + 84.636217674600744, 0.0, -91.818241543240035, 0.0, 42.534998745388457, + 0.0, -7.3687943594796312, 0.0, 0.22710800170898438, 0.0, 0.0, 0.0, + 0.0, 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 212.5701300392171, 0.0, + -765.25246814118157, 0.0, 1059.9904525279999, 0.0, + -699.57962737613275, 0.0, 218.19051174421159, 0.0, + -26.491430486951554, 0.0, 0.57250142097473145, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, -1919.4576623184068, 0.0, + 8061.7221817373083, 0.0, -13586.550006434136, 0.0, 11655.393336864536, + 0.0, -5305.6469786134048, 0.0, 1200.9029132163525, 0.0, + -108.09091978839464, 0.0, 1.7277275025844574, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0}, + {0, 0, 0, 0, 0, 0, 20204.291330966149, 0.0, -96980.598388637503, 0.0, + 192547.0012325315, 0.0, -203400.17728041555, 0.0, 122200.46498301747, + 0.0, -41192.654968897557, 0.0, 7109.5143024893641, 0.0, + -493.915304773088, 0.0, 6.074042001273483, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0}, + {0, 0, 0, -242919.18790055133, 0.0, 1311763.6146629769, 0.0, + -2998015.9185381061, 0.0, 3763271.2976564039, 0.0, + -2813563.2265865342, 0.0, 1268365.2733216248, 0.0, + -331645.17248456361, 0.0, 45218.768981362737, 0.0, + -2499.8304818112092, 0.0, 24.380529699556064, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0}, + {3284469.8530720375, 0.0, -19706819.11843222, 0.0, 50952602.492664628, + 0.0, -74105148.211532637, 0.0, 66344512.274729028, 0.0, + -37567176.660763353, 0.0, 13288767.166421819, 0.0, + -2785618.1280864552, 0.0, 308186.40461266245, 0.0, + -13886.089753717039, 0.0, 110.01714026924674, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0} +}; + + +/* + * Compute Iv, Kv from (AMS5 9.7.7 + 9.7.8), asymptotic expansion for large v + */ +static void ikv_asymptotic_uniform(double v, double x, + double *i_value, double *k_value) +{ + double i_prefactor, k_prefactor; + double t, t2, eta, z; + double i_sum, k_sum, term, divisor; + int k, n; + int sign = 1; + + if (v < 0) { + /* Negative v; compute I_{-v} and K_{-v} and use (AMS 9.6.2) */ + sign = -1; + v = -v; + } + + z = x / v; + t = 1 / sqrt(1 + z * z); + t2 = t * t; + eta = sqrt(1 + z * z) + log(z / (1 + 1 / t)); + + i_prefactor = sqrt(t / (2 * M_PI * v)) * exp(v * eta); + i_sum = 1.0; + + k_prefactor = sqrt(M_PI * t / (2 * v)) * exp(-v * eta); + k_sum = 1.0; + + divisor = v; + for (n = 1; n < N_UFACTORS; ++n) { + /* + * Evaluate u_k(t) with Horner's scheme; + * (using the knowledge about which coefficients are zero) + */ + term = 0; + for (k = N_UFACTOR_TERMS - 1 - 3 * n; + k < N_UFACTOR_TERMS - n; k += 2) { + term *= t2; + term += asymptotic_ufactors[n][k]; + } + for (k = 1; k < n; k += 2) { + term *= t2; + } + if (n % 2 == 1) { + term *= t; + } + + /* Sum terms */ + term /= divisor; + i_sum += term; + k_sum += (n % 2 == 0) ? term : -term; + + /* Check convergence */ + if (fabs(term) < MACHEP) { + break; + } + + divisor *= v; + } + + if (fabs(term) > 1e-3 * fabs(i_sum)) { + /* Didn't converge */ + sf_error("ikv_asymptotic_uniform", SF_ERROR_NO_RESULT, NULL); + } + if (fabs(term) > MACHEP * fabs(i_sum)) { + /* Some precision lost */ + sf_error("ikv_asymptotic_uniform", SF_ERROR_LOSS, NULL); + } + + if (k_value != NULL) { + /* symmetric in v */ + *k_value = k_prefactor * k_sum; + } + + if (i_value != NULL) { + if (sign == 1) { + *i_value = i_prefactor * i_sum; + } + else { + /* (AMS 9.6.2) */ + *i_value = (i_prefactor * i_sum + + (2 / M_PI) * sin(M_PI * v) * k_prefactor * k_sum); + } + } +} + + +/* + * The following code originates from the Boost C++ library, + * from file `boost/math/special_functions/detail/bessel_ik.hpp`, + * converted from C++ to C. + */ + +#ifdef DEBUG +#define BOOST_ASSERT(a) assert(a) +#else +#define BOOST_ASSERT(a) +#endif + +/* + * Modified Bessel functions of the first and second kind of fractional order + * + * Calculate K(v, x) and K(v+1, x) by method analogous to + * Temme, Journal of Computational Physics, vol 21, 343 (1976) + */ +static int temme_ik_series(double v, double x, double *K, double *K1) +{ + double f, h, p, q, coef, sum, sum1, tolerance; + double a, b, c, d, sigma, gamma1, gamma2; + unsigned long k; + double gp; + double gm; + + + /* + * |x| <= 2, Temme series converge rapidly + * |x| > 2, the larger the |x|, the slower the convergence + */ + BOOST_ASSERT(fabs(x) <= 2); + BOOST_ASSERT(fabs(v) <= 0.5f); + + gp = gamma(v + 1) - 1; + gm = gamma(-v + 1) - 1; + + a = log(x / 2); + b = exp(v * a); + sigma = -a * v; + c = fabs(v) < MACHEP ? 1 : sin(M_PI * v) / (v * M_PI); + d = fabs(sigma) < MACHEP ? 1 : sinh(sigma) / sigma; + gamma1 = fabs(v) < MACHEP ? -SCIPY_EULER : (0.5f / v) * (gp - gm) * c; + gamma2 = (2 + gp + gm) * c / 2; + + /* initial values */ + p = (gp + 1) / (2 * b); + q = (1 + gm) * b / 2; + f = (cosh(sigma) * gamma1 + d * (-a) * gamma2) / c; + h = p; + coef = 1; + sum = coef * f; + sum1 = coef * h; + + /* series summation */ + tolerance = MACHEP; + for (k = 1; k < MAXITER; k++) { + f = (k * f + p + q) / (k * k - v * v); + p /= k - v; + q /= k + v; + h = p - k * f; + coef *= x * x / (4 * k); + sum += coef * f; + sum1 += coef * h; + if (fabs(coef * f) < fabs(sum) * tolerance) { + break; + } + } + if (k == MAXITER) { + sf_error("ikv_temme(temme_ik_series)", SF_ERROR_NO_RESULT, NULL); + } + + *K = sum; + *K1 = 2 * sum1 / x; + + return 0; +} + +/* Evaluate continued fraction fv = I_(v+1) / I_v, derived from + * Abramowitz and Stegun, Handbook of Mathematical Functions, 1972, 9.1.73 */ +static int CF1_ik(double v, double x, double *fv) +{ + double C, D, f, a, b, delta, tiny, tolerance; + unsigned long k; + + + /* + * |x| <= |v|, CF1_ik converges rapidly + * |x| > |v|, CF1_ik needs O(|x|) iterations to converge + */ + + /* + * modified Lentz's method, see + * Lentz, Applied Optics, vol 15, 668 (1976) + */ + tolerance = 2 * MACHEP; + tiny = 1 / sqrt(DBL_MAX); + C = f = tiny; /* b0 = 0, replace with tiny */ + D = 0; + for (k = 1; k < MAXITER; k++) { + a = 1; + b = 2 * (v + k) / x; + C = b + a / C; + D = b + a * D; + if (C == 0) { + C = tiny; + } + if (D == 0) { + D = tiny; + } + D = 1 / D; + delta = C * D; + f *= delta; + if (fabs(delta - 1) <= tolerance) { + break; + } + } + if (k == MAXITER) { + sf_error("ikv_temme(CF1_ik)", SF_ERROR_NO_RESULT, NULL); + } + + *fv = f; + + return 0; +} + +/* + * Calculate K(v, x) and K(v+1, x) by evaluating continued fraction + * z1 / z0 = U(v+1.5, 2v+1, 2x) / U(v+0.5, 2v+1, 2x), see + * Thompson and Barnett, Computer Physics Communications, vol 47, 245 (1987) + */ +static int CF2_ik(double v, double x, double *Kv, double *Kv1) +{ + + double S, C, Q, D, f, a, b, q, delta, tolerance, current, prev; + unsigned long k; + + /* + * |x| >= |v|, CF2_ik converges rapidly + * |x| -> 0, CF2_ik fails to converge + */ + + BOOST_ASSERT(fabs(x) > 1); + + /* + * Steed's algorithm, see Thompson and Barnett, + * Journal of Computational Physics, vol 64, 490 (1986) + */ + tolerance = MACHEP; + a = v * v - 0.25f; + b = 2 * (x + 1); /* b1 */ + D = 1 / b; /* D1 = 1 / b1 */ + f = delta = D; /* f1 = delta1 = D1, coincidence */ + prev = 0; /* q0 */ + current = 1; /* q1 */ + Q = C = -a; /* Q1 = C1 because q1 = 1 */ + S = 1 + Q * delta; /* S1 */ + for (k = 2; k < MAXITER; k++) { /* starting from 2 */ + /* continued fraction f = z1 / z0 */ + a -= 2 * (k - 1); + b += 2; + D = 1 / (b + a * D); + delta *= b * D - 1; + f += delta; + + /* series summation S = 1 + \sum_{n=1}^{\infty} C_n * z_n / z_0 */ + q = (prev - (b - 2) * current) / a; + prev = current; + current = q; /* forward recurrence for q */ + C *= -a / k; + Q += C * q; + S += Q * delta; + + /* S converges slower than f */ + if (fabs(Q * delta) < fabs(S) * tolerance) { + break; + } + } + if (k == MAXITER) { + sf_error("ikv_temme(CF2_ik)", SF_ERROR_NO_RESULT, NULL); + } + + *Kv = sqrt(M_PI / (2 * x)) * exp(-x) / S; + *Kv1 = *Kv * (0.5f + v + x + (v * v - 0.25f) * f) / x; + + return 0; +} + +/* Flags for what to compute */ +enum { + need_i = 0x1, + need_k = 0x2 +}; + +/* + * Compute I(v, x) and K(v, x) simultaneously by Temme's method, see + * Temme, Journal of Computational Physics, vol 19, 324 (1975) + */ +static void ikv_temme(double v, double x, double *Iv_p, double *Kv_p) +{ + /* Kv1 = K_(v+1), fv = I_(v+1) / I_v */ + /* Ku1 = K_(u+1), fu = I_(u+1) / I_u */ + double u, Iv, Kv, Kv1, Ku, Ku1, fv; + double W, current, prev, next; + int reflect = 0; + unsigned n, k; + int kind; + + kind = 0; + if (Iv_p != NULL) { + kind |= need_i; + } + if (Kv_p != NULL) { + kind |= need_k; + } + + if (v < 0) { + reflect = 1; + v = -v; /* v is non-negative from here */ + kind |= need_k; + } + n = round(v); + u = v - n; /* -1/2 <= u < 1/2 */ + + if (x < 0) { + if (Iv_p != NULL) + *Iv_p = NAN; + if (Kv_p != NULL) + *Kv_p = NAN; + sf_error("ikv_temme", SF_ERROR_DOMAIN, NULL); + return; + } + if (x == 0) { + Iv = (v == 0) ? 1 : 0; + if (kind & need_k) { + sf_error("ikv_temme", SF_ERROR_OVERFLOW, NULL); + Kv = INFINITY; + } + else { + Kv = NAN; /* any value will do */ + } + + if (reflect && (kind & need_i)) { + double z = (u + n % 2); + + Iv = sin((double)M_PI * z) == 0 ? Iv : INFINITY; + if (Iv == INFINITY || Iv == -INFINITY) { + sf_error("ikv_temme", SF_ERROR_OVERFLOW, NULL); + } + } + + if (Iv_p != NULL) { + *Iv_p = Iv; + } + if (Kv_p != NULL) { + *Kv_p = Kv; + } + return; + } + /* x is positive until reflection */ + W = 1 / x; /* Wronskian */ + if (x <= 2) { /* x in (0, 2] */ + temme_ik_series(u, x, &Ku, &Ku1); /* Temme series */ + } + else { /* x in (2, \infty) */ + CF2_ik(u, x, &Ku, &Ku1); /* continued fraction CF2_ik */ + } + prev = Ku; + current = Ku1; + for (k = 1; k <= n; k++) { /* forward recurrence for K */ + next = 2 * (u + k) * current / x + prev; + prev = current; + current = next; + } + Kv = prev; + Kv1 = current; + if (kind & need_i) { + double lim = (4 * v * v + 10) / (8 * x); + + lim *= lim; + lim *= lim; + lim /= 24; + if ((lim < MACHEP * 10) && (x > 100)) { + /* + * x is huge compared to v, CF1 may be very slow + * to converge so use asymptotic expansion for large + * x case instead. Note that the asymptotic expansion + * isn't very accurate - so it's deliberately very hard + * to get here - probably we're going to overflow: + */ + Iv = iv_asymptotic(v, x); + } + else { + CF1_ik(v, x, &fv); /* continued fraction CF1_ik */ + Iv = W / (Kv * fv + Kv1); /* Wronskian relation */ + } + } + else { + Iv = NAN; /* any value will do */ + } + + if (reflect) { + double z = (u + n % 2); + + if (Iv_p != NULL) { + *Iv_p = Iv + (2 / M_PI) * sin(M_PI * z) * Kv; /* reflection formula */ + } + if (Kv_p != NULL) { + *Kv_p = Kv; + } + } + else { + if (Iv_p != NULL) { + *Iv_p = Iv; + } + if (Kv_p != NULL) { + *Kv_p = Kv; + } + } + return; +} diff --git a/gtsam/3rdparty/cephes/cephes/sf_error.c b/gtsam/3rdparty/cephes/cephes/sf_error.c new file mode 100644 index 000000000..95a47c797 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/sf_error.c @@ -0,0 +1,45 @@ +#include "sf_error.h" + +#include +#include + +const char *sf_error_messages[] = {"no error", + "singularity", + "underflow", + "overflow", + "too slow convergence", + "loss of precision", + "no result obtained", + "domain error", + "invalid input argument", + "other error", + NULL}; + +/* If this isn't volatile clang tries to optimize it away */ +static volatile sf_action_t sf_error_actions[] = { + SF_ERROR_IGNORE, /* SF_ERROR_OK */ + SF_ERROR_IGNORE, /* SF_ERROR_SINGULAR */ + SF_ERROR_IGNORE, /* SF_ERROR_UNDERFLOW */ + SF_ERROR_IGNORE, /* SF_ERROR_OVERFLOW */ + SF_ERROR_IGNORE, /* SF_ERROR_SLOW */ + SF_ERROR_IGNORE, /* SF_ERROR_LOSS */ + SF_ERROR_IGNORE, /* SF_ERROR_NO_RESULT */ + SF_ERROR_IGNORE, /* SF_ERROR_DOMAIN */ + SF_ERROR_IGNORE, /* SF_ERROR_ARG */ + SF_ERROR_IGNORE, /* SF_ERROR_OTHER */ + SF_ERROR_IGNORE /* SF_ERROR__LAST */ +}; + +void sf_error_set_action(sf_error_t code, sf_action_t action) { + sf_error_actions[(int)code] = action; +} + +sf_action_t sf_error_get_action(sf_error_t code) { + return sf_error_actions[(int)code]; +} + +void sf_error(const char *func_name, sf_error_t code, const char *fmt, ...) { + va_list ap; + va_start(ap, fmt); + va_end(ap); +} diff --git a/gtsam/3rdparty/cephes/cephes/sf_error.h b/gtsam/3rdparty/cephes/cephes/sf_error.h new file mode 100644 index 000000000..43986df81 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/sf_error.h @@ -0,0 +1,38 @@ +#ifndef SF_ERROR_H_ +#define SF_ERROR_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum { + SF_ERROR_OK = 0, /* no error */ + SF_ERROR_SINGULAR, /* singularity encountered */ + SF_ERROR_UNDERFLOW, /* floating point underflow */ + SF_ERROR_OVERFLOW, /* floating point overflow */ + SF_ERROR_SLOW, /* too many iterations required */ + SF_ERROR_LOSS, /* loss of precision */ + SF_ERROR_NO_RESULT, /* no result obtained */ + SF_ERROR_DOMAIN, /* out of domain */ + SF_ERROR_ARG, /* invalid input parameter */ + SF_ERROR_OTHER, /* unclassified error */ + SF_ERROR__LAST +} sf_error_t; + +typedef enum { + SF_ERROR_IGNORE = 0, /* Ignore errors */ + SF_ERROR_WARN, /* Warn on errors */ + SF_ERROR_RAISE /* Raise on errors */ +} sf_action_t; + +extern const char *sf_error_messages[]; +void sf_error(const char *func_name, sf_error_t code, const char *fmt, ...); +void sf_error_check_fpe(const char *func_name); +void sf_error_set_action(sf_error_t code, sf_action_t action); +sf_action_t sf_error_get_action(sf_error_t code); + +#ifdef __cplusplus +} +#endif + +#endif /* SF_ERROR_H_ */ diff --git a/gtsam/3rdparty/cephes/cephes/shichi.c b/gtsam/3rdparty/cephes/cephes/shichi.c new file mode 100644 index 000000000..75104e724 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/shichi.c @@ -0,0 +1,305 @@ +/* shichi.c + * + * Hyperbolic sine and cosine integrals + * + * + * + * SYNOPSIS: + * + * double x, Chi, Shi, shichi(); + * + * shichi( x, &Chi, &Shi ); + * + * + * DESCRIPTION: + * + * Approximates the integrals + * + * x + * - + * | | cosh t - 1 + * Chi(x) = eul + ln x + | ----------- dt, + * | | t + * - + * 0 + * + * x + * - + * | | sinh t + * Shi(x) = | ------ dt + * | | t + * - + * 0 + * + * where eul = 0.57721566490153286061 is Euler's constant. + * The integrals are evaluated by power series for x < 8 + * and by Chebyshev expansions for x between 8 and 88. + * For large x, both functions approach exp(x)/2x. + * Arguments greater than 88 in magnitude return INFINITY. + * + * + * ACCURACY: + * + * Test interval 0 to 88. + * Relative error: + * arithmetic function # trials peak rms + * IEEE Shi 30000 6.9e-16 1.6e-16 + * Absolute error, except relative when |Chi| > 1: + * IEEE Chi 30000 8.4e-16 1.4e-16 + */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + + +#include "mconf.h" + +/* x exp(-x) shi(x), inverted interval 8 to 18 */ +static double S1[] = { + 1.83889230173399459482E-17, + -9.55485532279655569575E-17, + 2.04326105980879882648E-16, + 1.09896949074905343022E-15, + -1.31313534344092599234E-14, + 5.93976226264314278932E-14, + -3.47197010497749154755E-14, + -1.40059764613117131000E-12, + 9.49044626224223543299E-12, + -1.61596181145435454033E-11, + -1.77899784436430310321E-10, + 1.35455469767246947469E-9, + -1.03257121792819495123E-9, + -3.56699611114982536845E-8, + 1.44818877384267342057E-7, + 7.82018215184051295296E-7, + -5.39919118403805073710E-6, + -3.12458202168959833422E-5, + 8.90136741950727517826E-5, + 2.02558474743846862168E-3, + 2.96064440855633256972E-2, + 1.11847751047257036625E0 +}; + +/* x exp(-x) shi(x), inverted interval 18 to 88 */ +static double S2[] = { + -1.05311574154850938805E-17, + 2.62446095596355225821E-17, + 8.82090135625368160657E-17, + -3.38459811878103047136E-16, + -8.30608026366935789136E-16, + 3.93397875437050071776E-15, + 1.01765565969729044505E-14, + -4.21128170307640802703E-14, + -1.60818204519802480035E-13, + 3.34714954175994481761E-13, + 2.72600352129153073807E-12, + 1.66894954752839083608E-12, + -3.49278141024730899554E-11, + -1.58580661666482709598E-10, + -1.79289437183355633342E-10, + 1.76281629144264523277E-9, + 1.69050228879421288846E-8, + 1.25391771228487041649E-7, + 1.16229947068677338732E-6, + 1.61038260117376323993E-5, + 3.49810375601053973070E-4, + 1.28478065259647610779E-2, + 1.03665722588798326712E0 +}; + +/* x exp(-x) chin(x), inverted interval 8 to 18 */ +static double C1[] = { + -8.12435385225864036372E-18, + 2.17586413290339214377E-17, + 5.22624394924072204667E-17, + -9.48812110591690559363E-16, + 5.35546311647465209166E-15, + -1.21009970113732918701E-14, + -6.00865178553447437951E-14, + 7.16339649156028587775E-13, + -2.93496072607599856104E-12, + -1.40359438136491256904E-12, + 8.76302288609054966081E-11, + -4.40092476213282340617E-10, + -1.87992075640569295479E-10, + 1.31458150989474594064E-8, + -4.75513930924765465590E-8, + -2.21775018801848880741E-7, + 1.94635531373272490962E-6, + 4.33505889257316408893E-6, + -6.13387001076494349496E-5, + -3.13085477492997465138E-4, + 4.97164789823116062801E-4, + 2.64347496031374526641E-2, + 1.11446150876699213025E0 +}; + +/* x exp(-x) chin(x), inverted interval 18 to 88 */ +static double C2[] = { + 8.06913408255155572081E-18, + -2.08074168180148170312E-17, + -5.98111329658272336816E-17, + 2.68533951085945765591E-16, + 4.52313941698904694774E-16, + -3.10734917335299464535E-15, + -4.42823207332531972288E-15, + 3.49639695410806959872E-14, + 6.63406731718911586609E-14, + -3.71902448093119218395E-13, + -1.27135418132338309016E-12, + 2.74851141935315395333E-12, + 2.33781843985453438400E-11, + 2.71436006377612442764E-11, + -2.56600180000355990529E-10, + -1.61021375163803438552E-9, + -4.72543064876271773512E-9, + -3.00095178028681682282E-9, + 7.79387474390914922337E-8, + 1.06942765566401507066E-6, + 1.59503164802313196374E-5, + 3.49592575153777996871E-4, + 1.28475387530065247392E-2, + 1.03665693917934275131E0 +}; + +static double hyp3f0(double a1, double a2, double a3, double z); + +/* Sine and cosine integrals */ + +extern double MACHEP; + +int shichi(double x, double *si, double *ci) +{ + double k, z, c, s, a, b; + short sign; + + if (x < 0.0) { + sign = -1; + x = -x; + } + else + sign = 0; + + + if (x == 0.0) { + *si = 0.0; + *ci = -INFINITY; + return (0); + } + + if (x >= 8.0) + goto chb; + + if (x >= 88.0) + goto asymp; + + z = x * x; + + /* Direct power series expansion */ + a = 1.0; + s = 1.0; + c = 0.0; + k = 2.0; + + do { + a *= z / k; + c += a / k; + k += 1.0; + a /= k; + s += a / k; + k += 1.0; + } + while (fabs(a / s) > MACHEP); + + s *= x; + goto done; + + +chb: + /* Chebyshev series expansions */ + if (x < 18.0) { + a = (576.0 / x - 52.0) / 10.0; + k = exp(x) / x; + s = k * chbevl(a, S1, 22); + c = k * chbevl(a, C1, 23); + goto done; + } + + if (x <= 88.0) { + a = (6336.0 / x - 212.0) / 70.0; + k = exp(x) / x; + s = k * chbevl(a, S2, 23); + c = k * chbevl(a, C2, 24); + goto done; + } + +asymp: + if (x > 1000) { + *si = INFINITY; + *ci = INFINITY; + } + else { + /* Asymptotic expansions + * http://functions.wolfram.com/GammaBetaErf/CoshIntegral/06/02/ + * http://functions.wolfram.com/GammaBetaErf/SinhIntegral/06/02/0001/ + */ + a = hyp3f0(0.5, 1, 1, 4.0/(x*x)); + b = hyp3f0(1, 1, 1.5, 4.0/(x*x)); + *si = cosh(x)/x * a + sinh(x)/(x*x) * b; + *ci = sinh(x)/x * a + cosh(x)/(x*x) * b; + } + if (sign) { + *si = -*si; + } + return 0; + +done: + if (sign) + s = -s; + + *si = s; + + *ci = SCIPY_EULER + log(x) + c; + return (0); +} + + +/* + * Evaluate 3F0(a1, a2, a3; z) + * + * The series is only asymptotic, so this requires z large enough. + */ +static double hyp3f0(double a1, double a2, double a3, double z) +{ + int n, maxiter; + double err, sum, term, m; + + m = pow(z, -1.0/3); + if (m < 50) { + maxiter = m; + } + else { + maxiter = 50; + } + + term = 1.0; + sum = term; + for (n = 0; n < maxiter; ++n) { + term *= (a1 + n) * (a2 + n) * (a3 + n) * z / (n + 1); + sum += term; + if (fabs(term) < 1e-13 * fabs(sum) || term == 0) { + break; + } + } + + err = fabs(term); + + if (err > 1e-13 * fabs(sum)) { + return NAN; + } + + return sum; +} diff --git a/gtsam/3rdparty/cephes/cephes/sici.c b/gtsam/3rdparty/cephes/cephes/sici.c new file mode 100644 index 000000000..7bb79bc25 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/sici.c @@ -0,0 +1,276 @@ +/* sici.c + * + * Sine and cosine integrals + * + * + * + * SYNOPSIS: + * + * double x, Ci, Si, sici(); + * + * sici( x, &Si, &Ci ); + * + * + * DESCRIPTION: + * + * Evaluates the integrals + * + * x + * - + * | cos t - 1 + * Ci(x) = eul + ln x + | --------- dt, + * | t + * - + * 0 + * x + * - + * | sin t + * Si(x) = | ----- dt + * | t + * - + * 0 + * + * where eul = 0.57721566490153286061 is Euler's constant. + * The integrals are approximated by rational functions. + * For x > 8 auxiliary functions f(x) and g(x) are employed + * such that + * + * Ci(x) = f(x) sin(x) - g(x) cos(x) + * Si(x) = pi/2 - f(x) cos(x) - g(x) sin(x) + * + * + * ACCURACY: + * Test interval = [0,50]. + * Absolute error, except relative when > 1: + * arithmetic function # trials peak rms + * IEEE Si 30000 4.4e-16 7.3e-17 + * IEEE Ci 30000 6.9e-16 5.1e-17 + */ + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +static double SN[] = { + -8.39167827910303881427E-11, + 4.62591714427012837309E-8, + -9.75759303843632795789E-6, + 9.76945438170435310816E-4, + -4.13470316229406538752E-2, + 1.00000000000000000302E0, +}; + +static double SD[] = { + 2.03269266195951942049E-12, + 1.27997891179943299903E-9, + 4.41827842801218905784E-7, + 9.96412122043875552487E-5, + 1.42085239326149893930E-2, + 9.99999999999999996984E-1, +}; + +static double CN[] = { + 2.02524002389102268789E-11, + -1.35249504915790756375E-8, + 3.59325051419993077021E-6, + -4.74007206873407909465E-4, + 2.89159652607555242092E-2, + -1.00000000000000000080E0, +}; + +static double CD[] = { + 4.07746040061880559506E-12, + 3.06780997581887812692E-9, + 1.23210355685883423679E-6, + 3.17442024775032769882E-4, + 5.10028056236446052392E-2, + 4.00000000000000000080E0, +}; + +static double FN4[] = { + 4.23612862892216586994E0, + 5.45937717161812843388E0, + 1.62083287701538329132E0, + 1.67006611831323023771E-1, + 6.81020132472518137426E-3, + 1.08936580650328664411E-4, + 5.48900223421373614008E-7, +}; + +static double FD4[] = { + /* 1.00000000000000000000E0, */ + 8.16496634205391016773E0, + 7.30828822505564552187E0, + 1.86792257950184183883E0, + 1.78792052963149907262E-1, + 7.01710668322789753610E-3, + 1.10034357153915731354E-4, + 5.48900252756255700982E-7, +}; + +static double FN8[] = { + 4.55880873470465315206E-1, + 7.13715274100146711374E-1, + 1.60300158222319456320E-1, + 1.16064229408124407915E-2, + 3.49556442447859055605E-4, + 4.86215430826454749482E-6, + 3.20092790091004902806E-8, + 9.41779576128512936592E-11, + 9.70507110881952024631E-14, +}; + +static double FD8[] = { + /* 1.00000000000000000000E0, */ + 9.17463611873684053703E-1, + 1.78685545332074536321E-1, + 1.22253594771971293032E-2, + 3.58696481881851580297E-4, + 4.92435064317881464393E-6, + 3.21956939101046018377E-8, + 9.43720590350276732376E-11, + 9.70507110881952025725E-14, +}; + +static double GN4[] = { + 8.71001698973114191777E-2, + 6.11379109952219284151E-1, + 3.97180296392337498885E-1, + 7.48527737628469092119E-2, + 5.38868681462177273157E-3, + 1.61999794598934024525E-4, + 1.97963874140963632189E-6, + 7.82579040744090311069E-9, +}; + +static double GD4[] = { + /* 1.00000000000000000000E0, */ + 1.64402202413355338886E0, + 6.66296701268987968381E-1, + 9.88771761277688796203E-2, + 6.22396345441768420760E-3, + 1.73221081474177119497E-4, + 2.02659182086343991969E-6, + 7.82579218933534490868E-9, +}; + +static double GN8[] = { + 6.97359953443276214934E-1, + 3.30410979305632063225E-1, + 3.84878767649974295920E-2, + 1.71718239052347903558E-3, + 3.48941165502279436777E-5, + 3.47131167084116673800E-7, + 1.70404452782044526189E-9, + 3.85945925430276600453E-12, + 3.14040098946363334640E-15, +}; + +static double GD8[] = { + /* 1.00000000000000000000E0, */ + 1.68548898811011640017E0, + 4.87852258695304967486E-1, + 4.67913194259625806320E-2, + 1.90284426674399523638E-3, + 3.68475504442561108162E-5, + 3.57043223443740838771E-7, + 1.72693748966316146736E-9, + 3.87830166023954706752E-12, + 3.14040098946363335242E-15, +}; + +extern double MACHEP; + + +int sici(double x, double *si, double *ci) +{ + double z, c, s, f, g; + short sign; + + if (x < 0.0) { + sign = -1; + x = -x; + } + else + sign = 0; + + + if (x == 0.0) { + *si = 0.0; + *ci = -INFINITY; + return (0); + } + + + if (x > 1.0e9) { + if (cephes_isinf(x)) { + if (sign == -1) { + *si = -M_PI_2; + *ci = NAN; + } + else { + *si = M_PI_2; + *ci = 0; + } + return 0; + } + *si = M_PI_2 - cos(x) / x; + *ci = sin(x) / x; + } + + + + if (x > 4.0) + goto asympt; + + z = x * x; + s = x * polevl(z, SN, 5) / polevl(z, SD, 5); + c = z * polevl(z, CN, 5) / polevl(z, CD, 5); + + if (sign) + s = -s; + *si = s; + *ci = SCIPY_EULER + log(x) + c; /* real part if x < 0 */ + return (0); + + + + /* The auxiliary functions are: + * + * + * *si = *si - M_PI_2; + * c = cos(x); + * s = sin(x); + * + * t = *ci * s - *si * c; + * a = *ci * c + *si * s; + * + * *si = t; + * *ci = -a; + */ + + + asympt: + + s = sin(x); + c = cos(x); + z = 1.0 / (x * x); + if (x < 8.0) { + f = polevl(z, FN4, 6) / (x * p1evl(z, FD4, 7)); + g = z * polevl(z, GN4, 7) / p1evl(z, GD4, 7); + } + else { + f = polevl(z, FN8, 8) / (x * p1evl(z, FD8, 8)); + g = z * polevl(z, GN8, 8) / p1evl(z, GD8, 9); + } + *si = M_PI_2 - f * c - g * s; + if (sign) + *si = -(*si); + *ci = f * s - g * c; + + return (0); +} diff --git a/gtsam/3rdparty/cephes/cephes/sindg.c b/gtsam/3rdparty/cephes/cephes/sindg.c new file mode 100644 index 000000000..d9c37ebdb --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/sindg.c @@ -0,0 +1,219 @@ +/* sindg.c + * + * Circular sine of angle in degrees + * + * + * + * SYNOPSIS: + * + * double x, y, sindg(); + * + * y = sindg( x ); + * + * + * + * DESCRIPTION: + * + * Range reduction is into intervals of 45 degrees. + * + * Two polynomial approximating functions are employed. + * Between 0 and pi/4 the sine is approximated by + * x + x**3 P(x**2). + * Between pi/4 and pi/2 the cosine is represented as + * 1 - x**2 P(x**2). + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE +-1000 30000 2.3e-16 5.6e-17 + * + * ERROR MESSAGES: + * + * message condition value returned + * sindg total loss x > 1.0e14 (IEEE) 0.0 + * + */ + /* cosdg.c + * + * Circular cosine of angle in degrees + * + * + * + * SYNOPSIS: + * + * double x, y, cosdg(); + * + * y = cosdg( x ); + * + * + * + * DESCRIPTION: + * + * Range reduction is into intervals of 45 degrees. + * + * Two polynomial approximating functions are employed. + * Between 0 and pi/4 the cosine is approximated by + * 1 - x**2 P(x**2). + * Between pi/4 and pi/2 the sine is represented as + * x + x**3 P(x**2). + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE +-1000 30000 2.1e-16 5.7e-17 + * See also sin(). + * + */ + +/* Cephes Math Library Release 2.0: April, 1987 + * Copyright 1985, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 */ + +#include "mconf.h" + +static double sincof[] = { + 1.58962301572218447952E-10, + -2.50507477628503540135E-8, + 2.75573136213856773549E-6, + -1.98412698295895384658E-4, + 8.33333333332211858862E-3, + -1.66666666666666307295E-1 +}; + +static double coscof[] = { + 1.13678171382044553091E-11, + -2.08758833757683644217E-9, + 2.75573155429816611547E-7, + -2.48015872936186303776E-5, + 1.38888888888806666760E-3, + -4.16666666666666348141E-2, + 4.99999999999999999798E-1 +}; + +static double PI180 = 1.74532925199432957692E-2; /* pi/180 */ +static double lossth = 1.0e14; + +double sindg(double x) +{ + double y, z, zz; + int j, sign; + + /* make argument positive but save the sign */ + sign = 1; + if (x < 0) { + x = -x; + sign = -1; + } + + if (x > lossth) { + sf_error("sindg", SF_ERROR_NO_RESULT, NULL); + return (0.0); + } + + y = floor(x / 45.0); /* integer part of x/M_PI_4 */ + + /* strip high bits of integer part to prevent integer overflow */ + z = ldexp(y, -4); + z = floor(z); /* integer part of y/8 */ + z = y - ldexp(z, 4); /* y - 16 * (y/16) */ + + j = z; /* convert to integer for tests on the phase angle */ + /* map zeros to origin */ + if (j & 1) { + j += 1; + y += 1.0; + } + j = j & 07; /* octant modulo 360 degrees */ + /* reflect in x axis */ + if (j > 3) { + sign = -sign; + j -= 4; + } + + z = x - y * 45.0; /* x mod 45 degrees */ + z *= PI180; /* multiply by pi/180 to convert to radians */ + zz = z * z; + + if ((j == 1) || (j == 2)) { + y = 1.0 - zz * polevl(zz, coscof, 6); + } + else { + y = z + z * (zz * polevl(zz, sincof, 5)); + } + + if (sign < 0) + y = -y; + + return (y); +} + + +double cosdg(double x) +{ + double y, z, zz; + int j, sign; + + /* make argument positive */ + sign = 1; + if (x < 0) + x = -x; + + if (x > lossth) { + sf_error("cosdg", SF_ERROR_NO_RESULT, NULL); + return (0.0); + } + + y = floor(x / 45.0); + z = ldexp(y, -4); + z = floor(z); /* integer part of y/8 */ + z = y - ldexp(z, 4); /* y - 16 * (y/16) */ + + /* integer and fractional part modulo one octant */ + j = z; + if (j & 1) { /* map zeros to origin */ + j += 1; + y += 1.0; + } + j = j & 07; + if (j > 3) { + j -= 4; + sign = -sign; + } + + if (j > 1) + sign = -sign; + + z = x - y * 45.0; /* x mod 45 degrees */ + z *= PI180; /* multiply by pi/180 to convert to radians */ + + zz = z * z; + + if ((j == 1) || (j == 2)) { + y = z + z * (zz * polevl(zz, sincof, 5)); + } + else { + y = 1.0 - zz * polevl(zz, coscof, 6); + } + + if (sign < 0) + y = -y; + + return (y); +} + + +/* Degrees, minutes, seconds to radians: */ + +/* 1 arc second, in radians = 4.848136811095359935899141023579479759563533023727e-6 */ +static double P64800 = + 4.848136811095359935899141023579479759563533023727e-6; + +double radian(double d, double m, double s) +{ + return (((d * 60.0 + m) * 60.0 + s) * P64800); +} diff --git a/gtsam/3rdparty/cephes/cephes/sinpi.c b/gtsam/3rdparty/cephes/cephes/sinpi.c new file mode 100644 index 000000000..f0e52f990 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/sinpi.c @@ -0,0 +1,54 @@ +/* + * Implement sin(pi * x) and cos(pi * x) for real x. Since the periods + * of these functions are integral (and thus representable in double + * precision), it's possible to compute them with greater accuracy + * than sin(x) and cos(x). + */ +#include "mconf.h" + + +/* Compute sin(pi * x). */ +double sinpi(double x) +{ + double s = 1.0; + double r; + + if (x < 0.0) { + x = -x; + s = -1.0; + } + + r = fmod(x, 2.0); + if (r < 0.5) { + return s*sin(M_PI*r); + } + else if (r > 1.5) { + return s*sin(M_PI*(r - 2.0)); + } + else { + return -s*sin(M_PI*(r - 1.0)); + } +} + + +/* Compute cos(pi * x) */ +double cospi(double x) +{ + double r; + + if (x < 0.0) { + x = -x; + } + + r = fmod(x, 2.0); + if (r == 0.5) { + // We don't want to return -0.0 + return 0.0; + } + if (r < 1.0) { + return -sin(M_PI*(r - 0.5)); + } + else { + return sin(M_PI*(r - 1.5)); + } +} diff --git a/gtsam/3rdparty/cephes/cephes/spence.c b/gtsam/3rdparty/cephes/cephes/spence.c new file mode 100644 index 000000000..48e1c4087 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/spence.c @@ -0,0 +1,125 @@ +/* spence.c + * + * Dilogarithm + * + * + * + * SYNOPSIS: + * + * double x, y, spence(); + * + * y = spence( x ); + * + * + * + * DESCRIPTION: + * + * Computes the integral + * + * x + * - + * | | log t + * spence(x) = - | ----- dt + * | | t - 1 + * - + * 1 + * + * for x >= 0. A rational approximation gives the integral in + * the interval (0.5, 1.5). Transformation formulas for 1/x + * and 1-x are employed outside the basic expansion range. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,4 30000 3.9e-15 5.4e-16 + * + * + */ + +/* spence.c */ + + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1985, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +static double A[8] = { + 4.65128586073990045278E-5, + 7.31589045238094711071E-3, + 1.33847639578309018650E-1, + 8.79691311754530315341E-1, + 2.71149851196553469920E0, + 4.25697156008121755724E0, + 3.29771340985225106936E0, + 1.00000000000000000126E0, +}; + +static double B[8] = { + 6.90990488912553276999E-4, + 2.54043763932544379113E-2, + 2.82974860602568089943E-1, + 1.41172597751831069617E0, + 3.63800533345137075418E0, + 5.03278880143316990390E0, + 3.54771340985225096217E0, + 9.99999999999999998740E-1, +}; + +extern double MACHEP; + +double spence(double x) +{ + double w, y, z; + int flag; + + if (x < 0.0) { + sf_error("spence", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + if (x == 1.0) + return (0.0); + + if (x == 0.0) + return (M_PI * M_PI / 6.0); + + flag = 0; + + if (x > 2.0) { + x = 1.0 / x; + flag |= 2; + } + + if (x > 1.5) { + w = (1.0 / x) - 1.0; + flag |= 2; + } + + else if (x < 0.5) { + w = -x; + flag |= 1; + } + + else + w = x - 1.0; + + + y = -w * polevl(w, A, 7) / polevl(w, B, 7); + + if (flag & 1) + y = (M_PI * M_PI) / 6.0 - log(x) * log(1.0 - x) - y; + + if (flag & 2) { + z = log(x); + y = -0.5 * z * z - y; + } + + return (y); +} diff --git a/gtsam/3rdparty/cephes/cephes/stdtr.c b/gtsam/3rdparty/cephes/cephes/stdtr.c new file mode 100644 index 000000000..5a37536be --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/stdtr.c @@ -0,0 +1,203 @@ +/* stdtr.c + * + * Student's t distribution + * + * + * + * SYNOPSIS: + * + * double t, stdtr(); + * short k; + * + * y = stdtr( k, t ); + * + * + * DESCRIPTION: + * + * Computes the integral from minus infinity to t of the Student + * t distribution with integer k > 0 degrees of freedom: + * + * t + * - + * | | + * - | 2 -(k+1)/2 + * | ( (k+1)/2 ) | ( x ) + * ---------------------- | ( 1 + --- ) dx + * - | ( k ) + * sqrt( k pi ) | ( k/2 ) | + * | | + * - + * -inf. + * + * Relation to incomplete beta integral: + * + * 1 - stdtr(k,t) = 0.5 * incbet( k/2, 1/2, z ) + * where + * z = k/(k + t**2). + * + * For t < -2, this is the method of computation. For higher t, + * a direct method is derived from integration by parts. + * Since the function is symmetric about t=0, the area under the + * right tail of the density is found by calling the function + * with -t instead of t. + * + * ACCURACY: + * + * Tested at random 1 <= k <= 25. The "domain" refers to t. + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -100,-2 50000 5.9e-15 1.4e-15 + * IEEE -2,100 500000 2.7e-15 4.9e-17 + */ + +/* stdtri.c + * + * Functional inverse of Student's t distribution + * + * + * + * SYNOPSIS: + * + * double p, t, stdtri(); + * int k; + * + * t = stdtri( k, p ); + * + * + * DESCRIPTION: + * + * Given probability p, finds the argument t such that stdtr(k,t) + * is equal to p. + * + * ACCURACY: + * + * Tested at random 1 <= k <= 100. The "domain" refers to p: + * Relative error: + * arithmetic domain # trials peak rms + * IEEE .001,.999 25000 5.7e-15 8.0e-16 + * IEEE 10^-6,.001 25000 2.0e-12 2.9e-14 + */ + + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" +#include + +extern double MACHEP; + +double stdtr(int k, double t) +{ + double x, rk, z, f, tz, p, xsqk; + int j; + + if (k <= 0) { + sf_error("stdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + if (t == 0) + return (0.5); + + if (t < -2.0) { + rk = k; + z = rk / (rk + t * t); + p = 0.5 * incbet(0.5 * rk, 0.5, z); + return (p); + } + + /* compute integral from -t to + t */ + + if (t < 0) + x = -t; + else + x = t; + + rk = k; /* degrees of freedom */ + z = 1.0 + (x * x) / rk; + + /* test if k is odd or even */ + if ((k & 1) != 0) { + + /* computation for odd k */ + + xsqk = x / sqrt(rk); + p = atan(xsqk); + if (k > 1) { + f = 1.0; + tz = 1.0; + j = 3; + while ((j <= (k - 2)) && ((tz / f) > MACHEP)) { + tz *= (j - 1) / (z * j); + f += tz; + j += 2; + } + p += f * xsqk / z; + } + p *= 2.0 / M_PI; + } + + + else { + + /* computation for even k */ + + f = 1.0; + tz = 1.0; + j = 2; + + while ((j <= (k - 2)) && ((tz / f) > MACHEP)) { + tz *= (j - 1) / (z * j); + f += tz; + j += 2; + } + p = f * x / sqrt(z * rk); + } + + /* common exit */ + + + if (t < 0) + p = -p; /* note destruction of relative accuracy */ + + p = 0.5 + 0.5 * p; + return (p); +} + +double stdtri(int k, double p) +{ + double t, rk, z; + int rflg; + + if (k <= 0 || p <= 0.0 || p >= 1.0) { + sf_error("stdtri", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + rk = k; + + if (p > 0.25 && p < 0.75) { + if (p == 0.5) + return (0.0); + z = 1.0 - 2.0 * p; + z = incbi(0.5, 0.5 * rk, fabs(z)); + t = sqrt(rk * z / (1.0 - z)); + if (p < 0.5) + t = -t; + return (t); + } + rflg = -1; + if (p >= 0.5) { + p = 1.0 - p; + rflg = 1; + } + z = incbi(0.5 * rk, 0.5, 2.0 * p); + + if (DBL_MAX * z < rk) + return (rflg * INFINITY); + t = sqrt(rk / z - rk); + return (rflg * t); +} diff --git a/gtsam/3rdparty/cephes/cephes/struve.c b/gtsam/3rdparty/cephes/cephes/struve.c new file mode 100644 index 000000000..26c86fa2d --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/struve.c @@ -0,0 +1,408 @@ +/* + * Compute the Struve function. + * + * Notes + * ----- + * + * We use three expansions for the Struve function discussed in [1]: + * + * - power series + * - expansion in Bessel functions + * - asymptotic large-z expansion + * + * Rounding errors are estimated based on the largest terms in the sums. + * + * ``struve_convergence.py`` plots the convergence regions of the different + * expansions. + * + * (i) + * + * Looking at the error in the asymptotic expansion, one finds that + * it's not worth trying if z ~> 0.7 * v + 12 for v > 0. + * + * (ii) + * + * The Bessel function expansion tends to fail for |z| >~ |v| and is not tried + * there. + * + * For Struve H it covers the quadrant v > z where the power series may fail to + * produce reasonable results. + * + * (iii) + * + * The three expansions together cover for Struve H the region z > 0, v real. + * + * They also cover Struve L, except that some loss of precision may occur around + * the transition region z ~ 0.7 |v|, v < 0, |v| >> 1 where the function changes + * rapidly. + * + * (iv) + * + * The power series is evaluated in double-double precision. This fixes accuracy + * issues in Struve H for |v| << |z| before the asymptotic expansion kicks in. + * Moreover, it improves the Struve L behavior for negative v. + * + * + * References + * ---------- + * [1] NIST Digital Library of Mathematical Functions + * https://dlmf.nist.gov/11 + */ + +/* + * Copyright (C) 2013 Pauli Virtanen + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * a. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * b. 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. + * c. Neither the name of Enthought nor the names of the SciPy Developers + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS + * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, + * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) 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. + */ + +#include "mconf.h" +#include "dd_real.h" + +// #include "amos_wrappers.h" + +#define STRUVE_MAXITER 10000 +#define SUM_EPS 1e-16 /* be sure we are in the tail of the sum */ +#define SUM_TINY 1e-100 +#define GOOD_EPS 1e-12 +#define ACCEPTABLE_EPS 1e-7 +#define ACCEPTABLE_ATOL 1e-300 + +#define MIN(a, b) ((a) < (b) ? (a) : (b)) + +double struve_power_series(double v, double x, int is_h, double *err); +double struve_asymp_large_z(double v, double z, int is_h, double *err); +double struve_bessel_series(double v, double z, int is_h, double *err); + +static double bessel_y(double v, double x); +static double bessel_j(double v, double x); +static double struve_hl(double v, double x, int is_h); + +double struve_h(double v, double z) +{ + return struve_hl(v, z, 1); +} + +double struve_l(double v, double z) +{ + return struve_hl(v, z, 0); +} + +static double struve_hl(double v, double z, int is_h) +{ + double value[4], err[4], tmp; + int n; + + if (z < 0) { + n = v; + if (v == n) { + tmp = (n % 2 == 0) ? -1 : 1; + return tmp * struve_hl(v, -z, is_h); + } + else { + return NAN; + } + } + else if (z == 0) { + if (v < -1) { + return gammasgn(v + 1.5) * INFINITY; + } + else if (v == -1) { + return 2 / sqrt(M_PI) / Gamma(0.5); + } + else { + return 0; + } + } + + n = -v - 0.5; + if (n == -v - 0.5 && n > 0) { + if (is_h) { + return (n % 2 == 0 ? 1 : -1) * bessel_j(n + 0.5, z); + } + else { + return iv(n + 0.5, z); + } + } + + /* Try the asymptotic expansion */ + if (z >= 0.7*v + 12) { + value[0] = struve_asymp_large_z(v, z, is_h, &err[0]); + if (err[0] < GOOD_EPS * fabs(value[0])) { + return value[0]; + } + } + else { + err[0] = INFINITY; + } + + /* Try power series */ + value[1] = struve_power_series(v, z, is_h, &err[1]); + if (err[1] < GOOD_EPS * fabs(value[1])) { + return value[1]; + } + + /* Try bessel series */ + if (fabs(z) < fabs(v) + 20) { + value[2] = struve_bessel_series(v, z, is_h, &err[2]); + if (err[2] < GOOD_EPS * fabs(value[2])) { + return value[2]; + } + } + else { + err[2] = INFINITY; + } + + /* Return the best of the three, if it is acceptable */ + n = 0; + if (err[1] < err[n]) n = 1; + if (err[2] < err[n]) n = 2; + if (err[n] < ACCEPTABLE_EPS * fabs(value[n]) || err[n] < ACCEPTABLE_ATOL) { + return value[n]; + } + + /* Maybe it really is an overflow? */ + tmp = -lgam(v + 1.5) + (v + 1)*log(z/2); + if (!is_h) { + tmp = fabs(tmp); + } + if (tmp > 700) { + sf_error("struve", SF_ERROR_OVERFLOW, NULL); + return INFINITY * gammasgn(v + 1.5); + } + + /* Failure */ + sf_error("struve", SF_ERROR_NO_RESULT, NULL); + return NAN; +} + + +/* + * Power series for Struve H and L + * https://dlmf.nist.gov/11.2.1 + * + * Starts to converge roughly at |n| > |z| + */ +double struve_power_series(double v, double z, int is_h, double *err) +{ + int n, sgn; + double term, sum, maxterm, scaleexp, tmp; + double2 cterm, csum, cdiv, z2, c2v, ctmp; + + if (is_h) { + sgn = -1; + } + else { + sgn = 1; + } + + tmp = -lgam(v + 1.5) + (v + 1)*log(z/2); + if (tmp < -600 || tmp > 600) { + /* Scale exponent to postpone underflow/overflow */ + scaleexp = tmp/2; + tmp -= scaleexp; + } + else { + scaleexp = 0; + } + + term = 2 / sqrt(M_PI) * exp(tmp) * gammasgn(v + 1.5); + sum = term; + maxterm = 0; + + cterm = dd_create_d(term); + csum = dd_create_d(sum); + z2 = dd_create_d(sgn*z*z); + c2v = dd_create_d(2*v); + + for (n = 0; n < STRUVE_MAXITER; ++n) { + /* cdiv = (3 + 2*n) * (3 + 2*n + 2*v)) */ + cdiv = dd_create_d(3 + 2*n); + ctmp = dd_create_d(3 + 2*n); + ctmp = dd_add(ctmp, c2v); + cdiv = dd_mul(cdiv, ctmp); + + /* cterm *= z2 / cdiv */ + cterm = dd_mul(cterm, z2); + cterm = dd_div(cterm, cdiv); + + csum = dd_add(csum, cterm); + + term = dd_to_double(cterm); + sum = dd_to_double(csum); + + if (fabs(term) > maxterm) { + maxterm = fabs(term); + } + if (fabs(term) < SUM_TINY * fabs(sum) || term == 0 || !isfinite(sum)) { + break; + } + } + + *err = fabs(term) + fabs(maxterm) * 1e-22; + + if (scaleexp != 0) { + sum *= exp(scaleexp); + *err *= exp(scaleexp); + } + + if (sum == 0 && term == 0 && v < 0 && !is_h) { + /* Spurious underflow */ + *err = INFINITY; + return NAN; + } + + return sum; +} + + +/* + * Bessel series + * https://dlmf.nist.gov/11.4.19 + */ +double struve_bessel_series(double v, double z, int is_h, double *err) +{ + int n; + double term, cterm, sum, maxterm; + + if (is_h && v < 0) { + /* Works less reliably in this region */ + *err = INFINITY; + return NAN; + } + + sum = 0; + maxterm = 0; + + cterm = sqrt(z / (2*M_PI)); + + for (n = 0; n < STRUVE_MAXITER; ++n) { + if (is_h) { + term = cterm * bessel_j(n + v + 0.5, z) / (n + 0.5); + cterm *= z/2 / (n + 1); + } + else { + term = cterm * iv(n + v + 0.5, z) / (n + 0.5); + cterm *= -z/2 / (n + 1); + } + sum += term; + if (fabs(term) > maxterm) { + maxterm = fabs(term); + } + if (fabs(term) < SUM_EPS * fabs(sum) || term == 0 || !isfinite(sum)) { + break; + } + } + + *err = fabs(term) + fabs(maxterm) * 1e-16; + + /* Account for potential underflow of the Bessel functions */ + *err += 1e-300 * fabs(cterm); + + return sum; +} + + +/* + * Large-z expansion for Struve H and L + * https://dlmf.nist.gov/11.6.1 + */ +double struve_asymp_large_z(double v, double z, int is_h, double *err) +{ + int n, sgn, maxiter; + double term, sum, maxterm; + double m; + + if (is_h) { + sgn = -1; + } + else { + sgn = 1; + } + + /* Asymptotic expansion divergenge point */ + m = z/2; + if (m <= 0) { + maxiter = 0; + } + else if (m > STRUVE_MAXITER) { + maxiter = STRUVE_MAXITER; + } + else { + maxiter = (int)m; + } + if (maxiter == 0) { + *err = INFINITY; + return NAN; + } + + if (z < v) { + /* Exclude regions where our error estimation fails */ + *err = INFINITY; + return NAN; + } + + /* Evaluate sum */ + term = -sgn / sqrt(M_PI) * exp(-lgam(v + 0.5) + (v - 1) * log(z/2)) * gammasgn(v + 0.5); + sum = term; + maxterm = 0; + + for (n = 0; n < maxiter; ++n) { + term *= sgn * (1 + 2*n) * (1 + 2*n - 2*v) / (z*z); + sum += term; + if (fabs(term) > maxterm) { + maxterm = fabs(term); + } + if (fabs(term) < SUM_EPS * fabs(sum) || term == 0 || !isfinite(sum)) { + break; + } + } + + if (is_h) { + sum += bessel_y(v, z); + } + else { + sum += iv(v, z); + } + + /* + * This error estimate is strictly speaking valid only for + * n > v - 0.5, but numerical results indicate that it works + * reasonably. + */ + *err = fabs(term) + fabs(maxterm) * 1e-16; + + return sum; +} + + +static double bessel_y(double v, double x) +{ + return cbesy_wrap_real(v, x); +} + +static double bessel_j(double v, double x) +{ + return cbesj_wrap_real(v, x); +} diff --git a/gtsam/3rdparty/cephes/cephes/tandg.c b/gtsam/3rdparty/cephes/cephes/tandg.c new file mode 100644 index 000000000..1ea86329b --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/tandg.c @@ -0,0 +1,141 @@ +/* tandg.c + * + * Circular tangent of argument in degrees + * + * + * + * SYNOPSIS: + * + * double x, y, tandg(); + * + * y = tandg( x ); + * + * + * + * DESCRIPTION: + * + * Returns the circular tangent of the argument x in degrees. + * + * Range reduction is modulo pi/4. A rational function + * x + x**3 P(x**2)/Q(x**2) + * is employed in the basic interval [0, pi/4]. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,10 30000 3.2e-16 8.4e-17 + * + * ERROR MESSAGES: + * + * message condition value returned + * tandg total loss x > 1.0e14 (IEEE) 0.0 + * tandg singularity x = 180 k + 90 INFINITY + */ + /* cotdg.c + * + * Circular cotangent of argument in degrees + * + * + * + * SYNOPSIS: + * + * double x, y, cotdg(); + * + * y = cotdg( x ); + * + * + * + * DESCRIPTION: + * + * Returns the circular cotangent of the argument x in degrees. + * + * Range reduction is modulo pi/4. A rational function + * x + x**3 P(x**2)/Q(x**2) + * is employed in the basic interval [0, pi/4]. + * + * + * ERROR MESSAGES: + * + * message condition value returned + * cotdg total loss x > 1.0e14 (IEEE) 0.0 + * cotdg singularity x = 180 k INFINITY + */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +static double PI180 = 1.74532925199432957692E-2; +static double lossth = 1.0e14; + +static double tancot(double, int); + +double tandg(double x) +{ + return (tancot(x, 0)); +} + + +double cotdg(double x) +{ + return (tancot(x, 1)); +} + + +static double tancot(double xx, int cotflg) +{ + double x; + int sign; + + /* make argument positive but save the sign */ + if (xx < 0) { + x = -xx; + sign = -1; + } + else { + x = xx; + sign = 1; + } + + if (x > lossth) { + sf_error("tandg", SF_ERROR_NO_RESULT, NULL); + return 0.0; + } + + /* modulo 180 */ + x = x - 180.0 * floor(x / 180.0); + if (cotflg) { + if (x <= 90.0) { + x = 90.0 - x; + } + else { + x = x - 90.0; + sign *= -1; + } + } + else { + if (x > 90.0) { + x = 180.0 - x; + sign *= -1; + } + } + if (x == 0.0) { + return 0.0; + } + else if (x == 45.0) { + return sign * 1.0; + } + else if (x == 90.0) { + sf_error((cotflg ? "cotdg" : "tandg"), SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + /* x is now transformed into [0, 90) */ + return sign * tan(x * PI180); +} diff --git a/gtsam/3rdparty/cephes/cephes/tukey.c b/gtsam/3rdparty/cephes/cephes/tukey.c new file mode 100644 index 000000000..751314a87 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/tukey.c @@ -0,0 +1,68 @@ + +/* Compute the CDF of the Tukey-Lambda distribution + * using a bracketing search with special checks + * + * The PPF of the Tukey-lambda distribution is + * G(p) = (p**lam + (1-p)**lam) / lam + * + * Author: Travis Oliphant + */ + +#include + +#define SMALLVAL 1e-4 +#define EPS 1.0e-14 +#define MAXCOUNT 60 + +double tukeylambdacdf(double x, double lmbda) +{ + double pmin, pmid, pmax, plow, phigh, xeval; + int count; + + if (isnan(x) || isnan(lmbda)) { + return NAN; + } + + xeval = 1.0 / lmbda; + if (lmbda > 0.0) { + if (x <= (-xeval)) { + return 0.0; + } + if (x >= xeval) { + return 1.0; + } + } + + if ((-SMALLVAL < lmbda) && (lmbda < SMALLVAL)) { + if (x >= 0) { + return 1.0 / (1.0 + exp(-x)); + } + else { + return exp(x) / (1.0 + exp(x)); + } + } + + pmin = 0.0; + pmid = 0.5; + pmax = 1.0; + plow = pmin; + phigh = pmax; + count = 0; + + while ((count < MAXCOUNT) && (fabs(pmid - plow) > EPS)) { + xeval = (pow(pmid, lmbda) - pow(1.0 - pmid, lmbda)) / lmbda; + if (xeval == x) { + return pmid; + } + if (xeval > x) { + phigh = pmid; + pmid = (pmid + plow) / 2.0; + } + else { + plow = pmid; + pmid = (pmid + phigh) / 2.0; + } + count++; + } + return pmid; +} diff --git a/gtsam/3rdparty/cephes/cephes/unity.c b/gtsam/3rdparty/cephes/cephes/unity.c new file mode 100644 index 000000000..76bc7f08d --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/unity.c @@ -0,0 +1,190 @@ +/* unity.c + * + * Relative error approximations for function arguments near + * unity. + * + * log1p(x) = log(1+x) + * expm1(x) = exp(x) - 1 + * cosm1(x) = cos(x) - 1 + * lgam1p(x) = lgam(1+x) + * + */ + +/* Scipy changes: + * - 06-10-2016: added lgam1p + */ + +#include "mconf.h" + +extern double MACHEP; + + + +/* log1p(x) = log(1 + x) */ + +/* Coefficients for log(1+x) = x - x**2/2 + x**3 P(x)/Q(x) + * 1/sqrt(2) <= x < sqrt(2) + * Theoretical peak relative error = 2.32e-20 + */ +static const double LP[] = { + 4.5270000862445199635215E-5, + 4.9854102823193375972212E-1, + 6.5787325942061044846969E0, + 2.9911919328553073277375E1, + 6.0949667980987787057556E1, + 5.7112963590585538103336E1, + 2.0039553499201281259648E1, +}; + +static const double LQ[] = { + /* 1.0000000000000000000000E0, */ + 1.5062909083469192043167E1, + 8.3047565967967209469434E1, + 2.2176239823732856465394E2, + 3.0909872225312059774938E2, + 2.1642788614495947685003E2, + 6.0118660497603843919306E1, +}; + +double log1p(double x) +{ + double z; + + z = 1.0 + x; + if ((z < M_SQRT1_2) || (z > M_SQRT2)) + return (log(z)); + z = x * x; + z = -0.5 * z + x * (z * polevl(x, LP, 6) / p1evl(x, LQ, 6)); + return (x + z); +} + + +/* log(1 + x) - x */ +double log1pmx(double x) +{ + if (fabs(x) < 0.5) { + int n; + double xfac = x; + double term; + double res = 0; + + for(n = 2; n < MAXITER; n++) { + xfac *= -x; + term = xfac / n; + res += term; + if (fabs(term) < MACHEP * fabs(res)) { + break; + } + } + return res; + } + else { + return log1p(x) - x; + } +} + + +/* expm1(x) = exp(x) - 1 */ + +/* e^x = 1 + 2x P(x^2)/( Q(x^2) - P(x^2) ) + * -0.5 <= x <= 0.5 + */ + +static double EP[3] = { + 1.2617719307481059087798E-4, + 3.0299440770744196129956E-2, + 9.9999999999999999991025E-1, +}; + +static double EQ[4] = { + 3.0019850513866445504159E-6, + 2.5244834034968410419224E-3, + 2.2726554820815502876593E-1, + 2.0000000000000000000897E0, +}; + +double expm1(double x) +{ + double r, xx; + + if (!cephes_isfinite(x)) { + if (cephes_isnan(x)) { + return x; + } + else if (x > 0) { + return x; + } + else { + return -1.0; + } + + } + if ((x < -0.5) || (x > 0.5)) + return (exp(x) - 1.0); + xx = x * x; + r = x * polevl(xx, EP, 2); + r = r / (polevl(xx, EQ, 3) - r); + return (r + r); +} + + + +/* cosm1(x) = cos(x) - 1 */ + +static double coscof[7] = { + 4.7377507964246204691685E-14, + -1.1470284843425359765671E-11, + 2.0876754287081521758361E-9, + -2.7557319214999787979814E-7, + 2.4801587301570552304991E-5, + -1.3888888888888872993737E-3, + 4.1666666666666666609054E-2, +}; + +double cosm1(double x) +{ + double xx; + + if ((x < -M_PI_4) || (x > M_PI_4)) + return (cos(x) - 1.0); + xx = x * x; + xx = -0.5 * xx + xx * xx * polevl(xx, coscof, 6); + return xx; +} + + +/* Compute lgam(x + 1) around x = 0 using its Taylor series. */ +static double lgam1p_taylor(double x) +{ + int n; + double xfac, coeff, res; + + if (x == 0) { + return 0; + } + res = -SCIPY_EULER * x; + xfac = -x; + for (n = 2; n < 42; n++) { + xfac *= -x; + coeff = zeta(n, 1) * xfac / n; + res += coeff; + if (fabs(coeff) < MACHEP * fabs(res)) { + break; + } + } + + return res; +} + + +/* Compute lgam(x + 1). */ +double lgam1p(double x) +{ + if (fabs(x) <= 0.5) { + return lgam1p_taylor(x); + } else if (fabs(x - 1) < 0.5) { + return log(x) + lgam1p_taylor(x - 1); + } else { + return lgam(x + 1); + } +} diff --git a/gtsam/3rdparty/cephes/cephes/yn.c b/gtsam/3rdparty/cephes/cephes/yn.c new file mode 100644 index 000000000..c02ff0acd --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/yn.c @@ -0,0 +1,105 @@ +/* yn.c + * + * Bessel function of second kind of integer order + * + * + * + * SYNOPSIS: + * + * double x, y, yn(); + * int n; + * + * y = yn( n, x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of order n, where n is a + * (possibly negative) integer. + * + * The function is evaluated by forward recurrence on + * n, starting with values computed by the routines + * y0() and y1(). + * + * If n = 0 or 1 the routine for y0 or y1 is called + * directly. + * + * + * + * ACCURACY: + * + * + * Absolute error, except relative + * when y > 1: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 3.4e-15 4.3e-16 + * + * + * ERROR MESSAGES: + * + * message condition value returned + * yn singularity x = 0 INFINITY + * yn overflow INFINITY + * + * Spot checked against tables for x, n between 0 and 100. + * + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" +extern double MAXLOG; + +double yn(int n, double x) +{ + double an, anm1, anm2, r; + int k, sign; + + if (n < 0) { + n = -n; + if ((n & 1) == 0) /* -1**n */ + sign = 1; + else + sign = -1; + } + else + sign = 1; + + + if (n == 0) + return (sign * y0(x)); + if (n == 1) + return (sign * y1(x)); + + /* test for overflow */ + if (x == 0.0) { + sf_error("yn", SF_ERROR_SINGULAR, NULL); + return -INFINITY * sign; + } + else if (x < 0.0) { + sf_error("yn", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + /* forward recurrence on n */ + + anm2 = y0(x); + anm1 = y1(x); + k = 1; + r = 2 * k; + do { + an = r * anm1 / x - anm2; + anm2 = anm1; + anm1 = an; + r += 2.0; + ++k; + } + while (k < n); + + + return (sign * an); +} diff --git a/gtsam/3rdparty/cephes/cephes/yv.c b/gtsam/3rdparty/cephes/cephes/yv.c new file mode 100644 index 000000000..e61a15521 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/yv.c @@ -0,0 +1,46 @@ +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +extern double MACHEP; + + +/* + * Bessel function of noninteger order + */ +double yv(double v, double x) +{ + double y, t; + int n; + + n = v; + if (n == v) { + y = yn(n, x); + return (y); + } + else if (v == floor(v)) { + /* Zero in denominator. */ + sf_error("yv", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + t = M_PI * v; + y = (cos(t) * jv(v, x) - jv(-v, x)) / sin(t); + + if (cephes_isinf(y)) { + if (v > 0) { + sf_error("yv", SF_ERROR_OVERFLOW, NULL); + return -INFINITY; + } + else if (v < -1e10) { + /* Whether it's +inf or -inf is numerically ill-defined. */ + sf_error("yv", SF_ERROR_DOMAIN, NULL); + return NAN; + } + } + + return (y); +} diff --git a/gtsam/3rdparty/cephes/cephes/zeta.c b/gtsam/3rdparty/cephes/cephes/zeta.c new file mode 100644 index 000000000..554933a24 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/zeta.c @@ -0,0 +1,160 @@ +/* zeta.c + * + * Riemann zeta function of two arguments + * + * + * + * SYNOPSIS: + * + * double x, q, y, zeta(); + * + * y = zeta( x, q ); + * + * + * + * DESCRIPTION: + * + * + * + * inf. + * - -x + * zeta(x,q) = > (k+q) + * - + * k=0 + * + * where x > 1 and q is not a negative integer or zero. + * The Euler-Maclaurin summation formula is used to obtain + * the expansion + * + * n + * - -x + * zeta(x,q) = > (k+q) + * - + * k=1 + * + * 1-x inf. B x(x+1)...(x+2j) + * (n+q) 1 - 2j + * + --------- - ------- + > -------------------- + * x-1 x - x+2j+1 + * 2(n+q) j=1 (2j)! (n+q) + * + * where the B2j are Bernoulli numbers. Note that (see zetac.c) + * zeta(x,1) = zetac(x) + 1. + * + * + * + * ACCURACY: + * + * + * + * REFERENCE: + * + * Gradshteyn, I. S., and I. M. Ryzhik, Tables of Integrals, + * Series, and Products, p. 1073; Academic Press, 1980. + * + */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" +extern double MACHEP; + +/* Expansion coefficients + * for Euler-Maclaurin summation formula + * (2k)! / B2k + * where B2k are Bernoulli numbers + */ +static double A[] = { + 12.0, + -720.0, + 30240.0, + -1209600.0, + 47900160.0, + -1.8924375803183791606e9, /*1.307674368e12/691 */ + 7.47242496e10, + -2.950130727918164224e12, /*1.067062284288e16/3617 */ + 1.1646782814350067249e14, /*5.109094217170944e18/43867 */ + -4.5979787224074726105e15, /*8.028576626982912e20/174611 */ + 1.8152105401943546773e17, /*1.5511210043330985984e23/854513 */ + -7.1661652561756670113e18 /*1.6938241367317436694528e27/236364091 */ +}; + +/* 30 Nov 86 -- error in third coefficient fixed */ + + +double zeta(double x, double q) +{ + int i; + double a, b, k, s, t, w; + + if (x == 1.0) + goto retinf; + + if (x < 1.0) { + domerr: + sf_error("zeta", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + if (q <= 0.0) { + if (q == floor(q)) { + sf_error("zeta", SF_ERROR_SINGULAR, NULL); + retinf: + return (INFINITY); + } + if (x != floor(x)) + goto domerr; /* because q^-x not defined */ + } + + /* Asymptotic expansion + * https://dlmf.nist.gov/25.11#E43 + */ + if (q > 1e8) { + return (1/(x - 1) + 1/(2*q)) * pow(q, 1 - x); + } + + /* Euler-Maclaurin summation formula */ + + /* Permit negative q but continue sum until n+q > +9 . + * This case should be handled by a reflection formula. + * If q<0 and x is an integer, there is a relation to + * the polyGamma function. + */ + s = pow(q, -x); + a = q; + i = 0; + b = 0.0; + while ((i < 9) || (a <= 9.0)) { + i += 1; + a += 1.0; + b = pow(a, -x); + s += b; + if (fabs(b / s) < MACHEP) + goto done; + } + + w = a; + s += b * w / (x - 1.0); + s -= 0.5 * b; + a = 1.0; + k = 0.0; + for (i = 0; i < 12; i++) { + a *= x + k; + b /= w; + t = a * b / A[i]; + s = s + t; + t = fabs(t / s); + if (t < MACHEP) + goto done; + k += 1.0; + a *= x + k; + b /= w; + k += 1.0; + } +done: + return (s); +} diff --git a/gtsam/3rdparty/cephes/cephes/zetac.c b/gtsam/3rdparty/cephes/cephes/zetac.c new file mode 100644 index 000000000..841433183 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/zetac.c @@ -0,0 +1,345 @@ +/* zetac.c + * + * Riemann zeta function + * + * + * + * SYNOPSIS: + * + * double x, y, zetac(); + * + * y = zetac( x ); + * + * + * + * DESCRIPTION: + * + * + * + * inf. + * - -x + * zetac(x) = > k , x > 1, + * - + * k=2 + * + * is related to the Riemann zeta function by + * + * Riemann zeta(x) = zetac(x) + 1. + * + * Extension of the function definition for x < 1 is implemented. + * Zero is returned for x > log2(INFINITY). + * + * ACCURACY: + * + * Tabulated values have full machine accuracy. + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 1,50 10000 9.8e-16 1.3e-16 + * + * + */ + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" +#include "lanczos.h" + +/* Riemann zeta(x) - 1 + * for integer arguments between 0 and 30. + */ +static const double azetac[] = { + -1.50000000000000000000E0, + 0.0, /* Not used; zetac(1.0) is infinity. */ + 6.44934066848226436472E-1, + 2.02056903159594285400E-1, + 8.23232337111381915160E-2, + 3.69277551433699263314E-2, + 1.73430619844491397145E-2, + 8.34927738192282683980E-3, + 4.07735619794433937869E-3, + 2.00839282608221441785E-3, + 9.94575127818085337146E-4, + 4.94188604119464558702E-4, + 2.46086553308048298638E-4, + 1.22713347578489146752E-4, + 6.12481350587048292585E-5, + 3.05882363070204935517E-5, + 1.52822594086518717326E-5, + 7.63719763789976227360E-6, + 3.81729326499983985646E-6, + 1.90821271655393892566E-6, + 9.53962033872796113152E-7, + 4.76932986787806463117E-7, + 2.38450502727732990004E-7, + 1.19219925965311073068E-7, + 5.96081890512594796124E-8, + 2.98035035146522801861E-8, + 1.49015548283650412347E-8, + 7.45071178983542949198E-9, + 3.72533402478845705482E-9, + 1.86265972351304900640E-9, + 9.31327432419668182872E-10 +}; + +/* 2**x (1 - 1/x) (zeta(x) - 1) = P(1/x)/Q(1/x), 1 <= x <= 10 */ +static double P[9] = { + 5.85746514569725319540E11, + 2.57534127756102572888E11, + 4.87781159567948256438E10, + 5.15399538023885770696E9, + 3.41646073514754094281E8, + 1.60837006880656492731E7, + 5.92785467342109522998E5, + 1.51129169964938823117E4, + 2.01822444485997955865E2, +}; + +static double Q[8] = { + /* 1.00000000000000000000E0, */ + 3.90497676373371157516E11, + 5.22858235368272161797E10, + 5.64451517271280543351E9, + 3.39006746015350418834E8, + 1.79410371500126453702E7, + 5.66666825131384797029E5, + 1.60382976810944131506E4, + 1.96436237223387314144E2, +}; + +/* log(zeta(x) - 1 - 2**-x), 10 <= x <= 50 */ +static double A[11] = { + 8.70728567484590192539E6, + 1.76506865670346462757E8, + 2.60889506707483264896E10, + 5.29806374009894791647E11, + 2.26888156119238241487E13, + 3.31884402932705083599E14, + 5.13778997975868230192E15, + -1.98123688133907171455E15, + -9.92763810039983572356E16, + 7.82905376180870586444E16, + 9.26786275768927717187E16, +}; + +static double B[10] = { + /* 1.00000000000000000000E0, */ + -7.92625410563741062861E6, + -1.60529969932920229676E8, + -2.37669260975543221788E10, + -4.80319584350455169857E11, + -2.07820961754173320170E13, + -2.96075404507272223680E14, + -4.86299103694609136686E15, + 5.34589509675789930199E15, + 5.71464111092297631292E16, + -1.79915597658676556828E16, +}; + +/* (1-x) (zeta(x) - 1), 0 <= x <= 1 */ +static double R[6] = { + -3.28717474506562731748E-1, + 1.55162528742623950834E1, + -2.48762831680821954401E2, + 1.01050368053237678329E3, + 1.26726061410235149405E4, + -1.11578094770515181334E5, +}; + +static double S[5] = { + /* 1.00000000000000000000E0, */ + 1.95107674914060531512E1, + 3.17710311750646984099E2, + 3.03835500874445748734E3, + 2.03665876435770579345E4, + 7.43853965136767874343E4, +}; + +static double TAYLOR0[10] = { + -1.0000000009110164892, + -1.0000000057646759799, + -9.9999983138417361078e-1, + -1.0000013011460139596, + -1.000001940896320456, + -9.9987929950057116496e-1, + -1.000785194477042408, + -1.0031782279542924256, + -9.1893853320467274178e-1, + -1.5, +}; + +#define MAXL2 127 +#define SQRT_2_PI 0.79788456080286535587989 + +extern double MACHEP; + +static double zeta_reflection(double); +static double zetac_smallneg(double); +static double zetac_positive(double); + + +/* + * Riemann zeta function, minus one + */ +double zetac(double x) +{ + if (isnan(x)) { + return x; + } + else if (x == -INFINITY) { + return NAN; + } + else if (x < 0.0 && x > -0.01) { + return zetac_smallneg(x); + } + else if (x < 0.0) { + return zeta_reflection(-x) - 1; + } + else { + return zetac_positive(x); + } +} + + +/* + * Riemann zeta function + */ +double riemann_zeta(double x) +{ + if (isnan(x)) { + return x; + } + else if (x == -INFINITY) { + return NAN; + } + else if (x < 0.0 && x > -0.01) { + return 1 + zetac_smallneg(x); + } + else if (x < 0.0) { + return zeta_reflection(-x); + } + else { + return 1 + zetac_positive(x); + } +} + + +/* + * Compute zetac for positive arguments + */ +static inline double zetac_positive(double x) +{ + int i; + double a, b, s, w; + + if (x == 1.0) { + return INFINITY; + } + + if (x >= MAXL2) { + /* because first term is 2**-x */ + return 0.0; + } + + /* Tabulated values for integer argument */ + w = floor(x); + if (w == x) { + i = x; + if (i < 31) { +#ifdef UNK + return (azetac[i]); +#else + return (*(double *) &azetac[4 * i]); +#endif + } + } + + if (x < 1.0) { + w = 1.0 - x; + a = polevl(x, R, 5) / (w * p1evl(x, S, 5)); + return a; + } + + if (x <= 10.0) { + b = pow(2.0, x) * (x - 1.0); + w = 1.0 / x; + s = (x * polevl(w, P, 8)) / (b * p1evl(w, Q, 8)); + return s; + } + + if (x <= 50.0) { + b = pow(2.0, -x); + w = polevl(x, A, 10) / p1evl(x, B, 10); + w = exp(w) + b; + return w; + } + + /* Basic sum of inverse powers */ + s = 0.0; + a = 1.0; + do { + a += 2.0; + b = pow(a, -x); + s += b; + } + while (b / s > MACHEP); + + b = pow(2.0, -x); + s = (s + b) / (1.0 - b); + return s; +} + + +/* + * Compute zetac for small negative x. We can't use the reflection + * formula because to double precision 1 - x = 1 and zetac(1) = inf. + */ +static inline double zetac_smallneg(double x) +{ + return polevl(x, TAYLOR0, 9); +} + + +/* + * Compute zetac using the reflection formula (see DLMF 25.4.2) plus + * the Lanczos approximation for Gamma to avoid overflow. + */ +static inline double zeta_reflection(double x) +{ + double base, large_term, small_term, hx, x_shift; + + hx = x / 2; + if (hx == floor(hx)) { + /* Hit a zero of the sine factor */ + return 0; + } + + /* Reduce the argument to sine */ + x_shift = fmod(x, 4); + small_term = -SQRT_2_PI * sin(0.5 * M_PI * x_shift); + small_term *= lanczos_sum_expg_scaled(x + 1) * zeta(x + 1, 1); + + /* Group large terms together to prevent overflow */ + base = (x + lanczos_g + 0.5) / (2 * M_PI * M_E); + large_term = pow(base, x + 0.5); + if (isfinite(large_term)) { + return large_term * small_term; + } + /* + * We overflowed, but we might be able to stave off overflow by + * factoring in the small term earlier. To do this we compute + * + * (sqrt(large_term) * small_term) * sqrt(large_term) + * + * Since we only call this method for negative x bounded away from + * zero, the small term can only be as small sine on that region; + * i.e. about machine epsilon. This means that if the above still + * overflows, then there was truly no avoiding it. + */ + large_term = pow(base, 0.5 * x + 0.25); + return (large_term * small_term) * large_term; +} diff --git a/gtsam/3rdparty/cephes/dllexport.h b/gtsam/3rdparty/cephes/dllexport.h new file mode 100644 index 000000000..525587164 --- /dev/null +++ b/gtsam/3rdparty/cephes/dllexport.h @@ -0,0 +1,25 @@ +// Macros for exporting DLL symbols on Windows +// Usage example: +// In header file: +// class CEPHES_EXPORT MyClass { ... }; +// +// Results in the following declarations: +// When included while compiling the library itself: +// class __declspec(dllexport) MyClass { ... }; +// When included while compiling other code against the library: +// class __declspec(dllimport) MyClass { ... }; + +#pragma once + +#ifdef _WIN32 +# define CEPHES_EXPORT __declspec(dllimport) +# define CEPHES_EXTERN_EXPORT __declspec(dllimport) +#else +#ifdef __APPLE__ +# define CEPHES_EXPORT __attribute__((visibility("default"))) +# define CEPHES_EXTERN_EXPORT extern +#else +# define CEPHES_EXPORT +# define CEPHES_EXTERN_EXPORT extern +#endif +#endif diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index cb87a6bcd..1fc8e4570 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -59,7 +59,6 @@ endif() # if GTSAM_USE_BOOST_FEATURES is not set, then we need to exclude the following: if(NOT GTSAM_USE_BOOST_FEATURES) list (APPEND excluded_sources - "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/GncOptimizer.h" "${CMAKE_CURRENT_SOURCE_DIR}/inference/graph.h" "${CMAKE_CURRENT_SOURCE_DIR}/inference/graph-inl.h" ) @@ -111,6 +110,9 @@ if(GTSAM_SUPPORT_NESTED_DISSECTION) list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam-if) endif() +# Link to cephes library +list(APPEND GTSAM_ADDITIONAL_LIBRARIES cephes-gtsam-if) + # Versions set(gtsam_version ${GTSAM_VERSION_STRING}) set(gtsam_soversion ${GTSAM_VERSION_MAJOR}) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 5c250eab4..7e0f2e844 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -8,10 +8,10 @@ * Functionality to serialize std::optional to boost::archive * Inspired from this PR: https://github.com/boostorg/serialization/pull/163 * ---------------------------------------------------------------------------- */ +#pragma once // Defined only if boost serialization is enabled #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION -#pragma once #include #include @@ -55,7 +55,14 @@ namespace std { template<> struct is_trivially_move_constructible& t, const unsigned int version) { } // namespace serialization } // namespace boost -#endif +#endif // BOOST_VERSION < 108400 +#endif // GTSAM_ENABLE_BOOST_SERIALIZATION diff --git a/gtsam/base/utilities.h b/gtsam/base/utilities.h index 03e9636da..a67c5d1b6 100644 --- a/gtsam/base/utilities.h +++ b/gtsam/base/utilities.h @@ -4,6 +4,8 @@ #include #include +#include + namespace gtsam { /** * For Python __str__(). @@ -11,7 +13,7 @@ namespace gtsam { * of an object when it prints to cout. * https://stackoverflow.com/questions/5419356/redirect-stdout-stderr-to-a-string */ -struct RedirectCout { +struct GTSAM_EXPORT RedirectCout { /// constructor -- redirect stdout buffer to a stringstream buffer RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {} diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 7202e6853..c56818448 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -62,6 +62,22 @@ namespace gtsam { return error(values.discrete()); } + /* ************************************************************************ */ + AlgebraicDecisionTree DecisionTreeFactor::errorTree() const { + // Get all possible assignments + DiscreteKeys dkeys = discreteKeys(); + // Reverse to make cartesian product output a more natural ordering. + DiscreteKeys rdkeys(dkeys.rbegin(), dkeys.rend()); + const auto assignments = DiscreteValues::CartesianProduct(rdkeys); + + // Construct vector with error values + std::vector errors; + for (const auto& assignment : assignments) { + errors.push_back(error(assignment)); + } + return AlgebraicDecisionTree(dkeys, errors); + } + /* ************************************************************************ */ double DecisionTreeFactor::safe_div(const double& a, const double& b) { // The use for safe_div is when we divide the product factor by the sum diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index a9a7e5ce0..784b11e51 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -292,6 +292,9 @@ namespace gtsam { */ double error(const HybridValues& values) const override; + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree errorTree() const override; + /// @} private: diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 24b2b55e4..771efbe5b 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -18,9 +18,10 @@ #pragma once +#include +#include #include #include -#include #include namespace gtsam { @@ -35,7 +36,7 @@ class HybridValues; * * @ingroup discrete */ -class GTSAM_EXPORT DiscreteFactor: public Factor { +class GTSAM_EXPORT DiscreteFactor : public Factor { public: // typedefs needed to play nice with gtsam typedef DiscreteFactor This; ///< This class @@ -103,7 +104,11 @@ class GTSAM_EXPORT DiscreteFactor: public Factor { */ double error(const HybridValues& c) const override; - /// Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor + /// Compute error for each assignment and return as a tree + virtual AlgebraicDecisionTree errorTree() const = 0; + + /// Multiply in a DecisionTreeFactor and return the result as + /// DecisionTreeFactor virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0; virtual DecisionTreeFactor toDecisionTreeFactor() const = 0; @@ -111,7 +116,7 @@ class GTSAM_EXPORT DiscreteFactor: public Factor { /// @} /// @name Wrapper support /// @{ - + /// Translation table from values to strings. using Names = DiscreteValues::Names; @@ -175,4 +180,4 @@ template<> struct traits : public Testable {}; std::vector expNormalize(const std::vector &logProbs); -}// namespace gtsam +} // namespace gtsam diff --git a/gtsam/discrete/DiscreteValues.h b/gtsam/discrete/DiscreteValues.h index 9ec08302b..9fdff014c 100644 --- a/gtsam/discrete/DiscreteValues.h +++ b/gtsam/discrete/DiscreteValues.h @@ -126,12 +126,12 @@ inline std::vector cartesianProduct(const DiscreteKeys& keys) { } /// Free version of markdown. -std::string markdown(const DiscreteValues& values, +std::string GTSAM_EXPORT markdown(const DiscreteValues& values, const KeyFormatter& keyFormatter = DefaultKeyFormatter, const DiscreteValues::Names& names = {}); /// Free version of html. -std::string html(const DiscreteValues& values, +std::string GTSAM_EXPORT html(const DiscreteValues& values, const KeyFormatter& keyFormatter = DefaultKeyFormatter, const DiscreteValues::Names& names = {}); diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index f4e023a4d..b360617f5 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -168,6 +168,11 @@ double TableFactor::error(const HybridValues& values) const { return error(values.discrete()); } +/* ************************************************************************ */ +AlgebraicDecisionTree TableFactor::errorTree() const { + return toDecisionTreeFactor().errorTree(); +} + /* ************************************************************************ */ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const { return toDecisionTreeFactor() * f; diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 828e794e6..228b36337 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -358,6 +358,9 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { */ double error(const HybridValues& values) const override; + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree errorTree() const override; + /// @} }; diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp index 9d73475a3..d764da7bf 100644 --- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp +++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp @@ -67,6 +67,24 @@ TEST( DecisionTreeFactor, constructors) EXPECT_DOUBLES_EQUAL(0.8, f4(x121), 1e-9); } +/* ************************************************************************* */ +TEST(DecisionTreeFactor, Error) { + // Declare a bunch of keys + DiscreteKey X(0,2), Y(1,3), Z(2,2); + + // Create factors + DecisionTreeFactor f(X & Y & Z, "2 5 3 6 4 7 25 55 35 65 45 75"); + + auto errors = f.errorTree(); + // regression + AlgebraicDecisionTree expected( + {X, Y, Z}, + vector{-0.69314718, -1.6094379, -1.0986123, -1.7917595, + -1.3862944, -1.9459101, -3.2188758, -4.0073332, -3.5553481, + -4.1743873, -3.8066625, -4.3174881}); + EXPECT(assert_equal(expected, errors, 1e-6)); +} + /* ************************************************************************* */ TEST(DecisionTreeFactor, multiplication) { DiscreteKey v0(0, 2), v1(1, 2), v2(2, 2); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 2b9c5a45a..7e05ee4da 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -396,7 +396,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { Matrix3 AdjointMap() const { return matrix(); } // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE - struct ChartAtOrigin { + struct GTSAM_EXPORT ChartAtOrigin { static Rot3 Retract(const Vector3& v, ChartJacobian H = {}); static Vector3 Local(const Rot3& r, ChartJacobian H = {}); }; diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 834d5a147..6d77e8eda 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -39,6 +39,11 @@ class KeyList { void remove(size_t key); void serialize() const; + + // Special dunder methods for Python wrapping + __len__(); + __contains__(size_t key); + __iter__(); }; // Actually a FastSet @@ -64,6 +69,11 @@ class KeySet { bool count(size_t key) const; // returns true if value exists void serialize() const; + + // Special dunder methods for Python wrapping + __len__(); + __contains__(size_t key); + __iter__(); }; // Actually a vector, needed for Matlab @@ -85,6 +95,11 @@ class KeyVector { void push_back(size_t key) const; void serialize() const; + + // Special dunder methods for Python wrapping + __len__(); + __contains__(size_t key); + __iter__(); }; // Actually a FastMap diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 753e35bf0..c105a329e 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -313,14 +313,14 @@ AlgebraicDecisionTree GaussianMixture::logProbability( } /* *******************************************************************************/ -AlgebraicDecisionTree GaussianMixture::error( +AlgebraicDecisionTree GaussianMixture::errorTree( const VectorValues &continuousValues) const { auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) { return conditional->error(continuousValues) + // logConstant_ - conditional->logNormalizationConstant(); }; - DecisionTree errorTree(conditionals_, errorFunc); - return errorTree; + DecisionTree error_tree(conditionals_, errorFunc); + return error_tree; } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 0b68fcfd0..521a4ca7a 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -214,7 +214,7 @@ class GTSAM_EXPORT GaussianMixture * @return AlgebraicDecisionTree A decision tree on the discrete keys * only, with the leaf values as the error for each assignment. */ - AlgebraicDecisionTree error(const VectorValues &continuousValues) const; + AlgebraicDecisionTree errorTree(const VectorValues &continuousValues) const; /** * @brief Compute the logProbability of this Gaussian Mixture. diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 0c7ff0e87..a3db16d04 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -102,14 +102,14 @@ GaussianFactorGraphTree GaussianMixtureFactor::asGaussianFactorGraphTree() } /* *******************************************************************************/ -AlgebraicDecisionTree GaussianMixtureFactor::error( +AlgebraicDecisionTree GaussianMixtureFactor::errorTree( const VectorValues &continuousValues) const { // functor to convert from sharedFactor to double error value. auto errorFunc = [&continuousValues](const sharedFactor &gf) { return gf->error(continuousValues); }; - DecisionTree errorTree(factors_, errorFunc); - return errorTree; + DecisionTree error_tree(factors_, errorFunc); + return error_tree; } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 1325cfe93..63ca9e923 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -135,7 +135,7 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { * @return AlgebraicDecisionTree A decision tree with the same keys * as the factors involved, and leaf values as the error. */ - AlgebraicDecisionTree error(const VectorValues &continuousValues) const; + AlgebraicDecisionTree errorTree(const VectorValues &continuousValues) const; /** * @brief Compute the log-likelihood, including the log-normalizing constant. diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 3bafe5a9c..b02967555 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -281,6 +281,36 @@ HybridValues HybridBayesNet::sample() const { return sample(&kRandomNumberGenerator); } +/* ************************************************************************* */ +AlgebraicDecisionTree HybridBayesNet::errorTree( + const VectorValues &continuousValues) const { + AlgebraicDecisionTree result(0.0); + + // Iterate over each conditional. + for (auto &&conditional : *this) { + if (auto gm = conditional->asMixture()) { + // If conditional is hybrid, compute error for all assignments. + result = result + gm->errorTree(continuousValues); + + } else if (auto gc = conditional->asGaussian()) { + // If continuous, get the error and add it to the result + double error = gc->error(continuousValues); + // Add the computed error to every leaf of the result tree. + result = result.apply( + [error](double leaf_value) { return leaf_value + error; }); + + } else if (auto dc = conditional->asDiscrete()) { + // If discrete, add the discrete error in the right branch + result = result.apply( + [dc](const Assignment &assignment, double leaf_value) { + return leaf_value + dc->error(DiscreteValues(assignment)); + }); + } + } + + return result; +} + /* ************************************************************************* */ AlgebraicDecisionTree HybridBayesNet::logProbability( const VectorValues &continuousValues) const { diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index e71cfe9b4..032cd55b9 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -187,6 +187,23 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * @param continuousValues Continuous values at which to compute the error. * @return AlgebraicDecisionTree */ + AlgebraicDecisionTree errorTree( + const VectorValues &continuousValues) const; + + /** + * @brief Error method using HybridValues which returns specific error for + * assignment. + */ + using Base::error; + + /** + * @brief Compute log probability for each discrete assignment, + * and return as a tree. + * + * @param continuousValues Continuous values at which + * to compute the log probability. + * @return AlgebraicDecisionTree + */ AlgebraicDecisionTree logProbability( const VectorValues &continuousValues) const; diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 2029b48e0..b764dc9e0 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -74,6 +74,85 @@ const Ordering HybridOrdering(const HybridGaussianFactorGraph &graph) { index, KeyVector(discrete_keys.begin(), discrete_keys.end()), true); } +/* ************************************************************************ */ +void HybridGaussianFactorGraph::printErrors( + const HybridValues &values, const std::string &str, + const KeyFormatter &keyFormatter, + const std::function + &printCondition) const { + std::cout << str << "size: " << size() << std::endl << std::endl; + + std::stringstream ss; + + for (size_t i = 0; i < factors_.size(); i++) { + auto &&factor = factors_[i]; + std::cout << "Factor " << i << ": "; + + // Clear the stringstream + ss.str(std::string()); + + if (auto gmf = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + gmf->errorTree(values.continuous()).print("", keyFormatter); + std::cout << std::endl; + } + } else if (auto hc = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + + if (hc->isContinuous()) { + std::cout << "error = " << hc->asGaussian()->error(values) << "\n"; + } else if (hc->isDiscrete()) { + std::cout << "error = "; + hc->asDiscrete()->errorTree().print("", keyFormatter); + std::cout << "\n"; + } else { + // Is hybrid + std::cout << "error = "; + hc->asMixture()->errorTree(values.continuous()).print(); + std::cout << "\n"; + } + } + } else if (auto gf = std::dynamic_pointer_cast(factor)) { + const double errorValue = (factor != nullptr ? gf->error(values) : .0); + if (!printCondition(factor.get(), errorValue, i)) + continue; // User-provided filter did not pass + + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = " << errorValue << "\n"; + } + } else if (auto df = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + df->errorTree().print("", keyFormatter); + } + + } else { + continue; + } + + std::cout << "\n"; + } + std::cout.flush(); +} + /* ************************************************************************ */ static GaussianFactorGraphTree addGaussian( const GaussianFactorGraphTree &gfgTree, @@ -96,7 +175,6 @@ static GaussianFactorGraphTree addGaussian( // TODO(dellaert): it's probably more efficient to first collect the discrete // keys, and then loop over all assignments to populate a vector. GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { - GaussianFactorGraphTree result; for (auto &f : factors_) { @@ -198,6 +276,51 @@ GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) { } /* ************************************************************************ */ +using Result = std::pair, + GaussianMixtureFactor::sharedFactor>; + +// Integrate the probability mass in the last continuous conditional using +// the unnormalized probability q(μ;m) = exp(-error(μ;m)) at the mean. +// discrete_probability = exp(-error(μ;m)) * sqrt(det(2π Σ_m)) +static std::shared_ptr createDiscreteFactor( + const DecisionTree &eliminationResults, + const DiscreteKeys &discreteSeparator) { + auto probability = [&](const Result &pair) -> double { + const auto &[conditional, factor] = pair; + static const VectorValues kEmpty; + // If the factor is not null, it has no keys, just contains the residual. + if (!factor) return 1.0; // TODO(dellaert): not loving this. + return exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); + }; + + DecisionTree probabilities(eliminationResults, probability); + + return std::make_shared(discreteSeparator, probabilities); +} + +// Create GaussianMixtureFactor on the separator, taking care to correct +// for conditional constants. +static std::shared_ptr createGaussianMixtureFactor( + const DecisionTree &eliminationResults, + const KeyVector &continuousSeparator, + const DiscreteKeys &discreteSeparator) { + // Correct for the normalization constant used up by the conditional + auto correct = [&](const Result &pair) -> GaussianFactor::shared_ptr { + const auto &[conditional, factor] = pair; + if (factor) { + auto hf = std::dynamic_pointer_cast(factor); + if (!hf) throw std::runtime_error("Expected HessianFactor!"); + hf->constantTerm() += 2.0 * conditional->logNormalizationConstant(); + } + return factor; + }; + DecisionTree newFactors(eliminationResults, + correct); + + return std::make_shared(continuousSeparator, + discreteSeparator, newFactors); +} + static std::pair> hybridElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys, @@ -217,9 +340,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // FG has a nullptr as we're looping over the factors. factorGraphTree = removeEmpty(factorGraphTree); - using Result = std::pair, - GaussianMixtureFactor::sharedFactor>; - // This is the elimination method on the leaf nodes auto eliminate = [&](const GaussianFactorGraph &graph) -> Result { if (graph.empty()) { @@ -234,53 +354,22 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // Perform elimination! DecisionTree eliminationResults(factorGraphTree, eliminate); - // Separate out decision tree into conditionals and remaining factors. - const auto [conditionals, newFactors] = unzip(eliminationResults); + // If there are no more continuous parents we create a DiscreteFactor with the + // error for each discrete choice. Otherwise, create a GaussianMixtureFactor + // on the separator, taking care to correct for conditional constants. + auto newFactor = + continuousSeparator.empty() + ? createDiscreteFactor(eliminationResults, discreteSeparator) + : createGaussianMixtureFactor(eliminationResults, continuousSeparator, + discreteSeparator); // Create the GaussianMixture from the conditionals + GaussianMixture::Conditionals conditionals( + eliminationResults, [](const Result &pair) { return pair.first; }); auto gaussianMixture = std::make_shared( frontalKeys, continuousSeparator, discreteSeparator, conditionals); - if (continuousSeparator.empty()) { - // If there are no more continuous parents, then we create a - // DiscreteFactor here, with the error for each discrete choice. - - // Integrate the probability mass in the last continuous conditional using - // the unnormalized probability q(μ;m) = exp(-error(μ;m)) at the mean. - // discrete_probability = exp(-error(μ;m)) * sqrt(det(2π Σ_m)) - auto probability = [&](const Result &pair) -> double { - static const VectorValues kEmpty; - // If the factor is not null, it has no keys, just contains the residual. - const auto &factor = pair.second; - if (!factor) return 1.0; // TODO(dellaert): not loving this. - return exp(-factor->error(kEmpty)) / pair.first->normalizationConstant(); - }; - - DecisionTree probabilities(eliminationResults, probability); - - return { - std::make_shared(gaussianMixture), - std::make_shared(discreteSeparator, probabilities)}; - } else { - // Otherwise, we create a resulting GaussianMixtureFactor on the separator, - // taking care to correct for conditional constant. - - // Correct for the normalization constant used up by the conditional - auto correct = [&](const Result &pair) { - const auto &factor = pair.second; - if (!factor) return; - auto hf = std::dynamic_pointer_cast(factor); - if (!hf) throw std::runtime_error("Expected HessianFactor!"); - hf->constantTerm() += 2.0 * pair.first->logNormalizationConstant(); - }; - eliminationResults.visit(correct); - - const auto mixtureFactor = std::make_shared( - continuousSeparator, discreteSeparator, newFactors); - - return {std::make_shared(gaussianMixture), - mixtureFactor}; - } + return {std::make_shared(gaussianMixture), newFactor}; } /* ************************************************************************ @@ -410,7 +499,7 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, } /* ************************************************************************ */ -AlgebraicDecisionTree HybridGaussianFactorGraph::error( +AlgebraicDecisionTree HybridGaussianFactorGraph::errorTree( const VectorValues &continuousValues) const { AlgebraicDecisionTree error_tree(0.0); @@ -421,7 +510,7 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::error( if (auto gaussianMixture = dynamic_pointer_cast(f)) { // Compute factor error and add it. - error_tree = error_tree + gaussianMixture->error(continuousValues); + error_tree = error_tree + gaussianMixture->errorTree(continuousValues); } else if (auto gaussian = dynamic_pointer_cast(f)) { // If continuous only, get the (double) error // and add it to the error_tree @@ -450,7 +539,7 @@ double HybridGaussianFactorGraph::probPrime(const HybridValues &values) const { /* ************************************************************************ */ AlgebraicDecisionTree HybridGaussianFactorGraph::probPrime( const VectorValues &continuousValues) const { - AlgebraicDecisionTree error_tree = this->error(continuousValues); + AlgebraicDecisionTree error_tree = this->errorTree(continuousValues); AlgebraicDecisionTree prob_tree = error_tree.apply([](double error) { // NOTE: The 0.5 term is handled by each factor return exp(-error); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index b3f159150..1708ff64b 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -140,9 +140,19 @@ class GTSAM_EXPORT HybridGaussianFactorGraph /// @{ // TODO(dellaert): customize print and equals. - // void print(const std::string& s = "HybridGaussianFactorGraph", - // const KeyFormatter& keyFormatter = DefaultKeyFormatter) const - // override; + // void print( + // const std::string& s = "HybridGaussianFactorGraph", + // const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; + + void printErrors( + const HybridValues& values, + const std::string& str = "HybridGaussianFactorGraph: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const std::function& + printCondition = + [](const Factor*, double, size_t) { return true; }) const; + // bool equals(const This& fg, double tol = 1e-9) const override; /// @} @@ -161,7 +171,8 @@ class GTSAM_EXPORT HybridGaussianFactorGraph * @param continuousValues Continuous values at which to compute the error. * @return AlgebraicDecisionTree */ - AlgebraicDecisionTree error(const VectorValues& continuousValues) const; + AlgebraicDecisionTree errorTree( + const VectorValues& continuousValues) const; /** * @brief Compute unnormalized probability \f$ P(X | M, Z) \f$ diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index e51adb9cd..cdd448412 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -42,6 +42,98 @@ void HybridNonlinearFactorGraph::print(const std::string& s, } } +/* ************************************************************************* */ +void HybridNonlinearFactorGraph::printErrors( + const HybridValues& values, const std::string& str, + const KeyFormatter& keyFormatter, + const std::function& printCondition) const { + std::cout << str << "size: " << size() << std::endl << std::endl; + + std::stringstream ss; + + for (size_t i = 0; i < factors_.size(); i++) { + auto&& factor = factors_[i]; + std::cout << "Factor " << i << ": "; + + // Clear the stringstream + ss.str(std::string()); + + if (auto mf = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + mf->errorTree(values.nonlinear()).print("", keyFormatter); + std::cout << std::endl; + } + } else if (auto gmf = + std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + gmf->errorTree(values.continuous()).print("", keyFormatter); + std::cout << std::endl; + } + } else if (auto gm = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + gm->errorTree(values.continuous()).print("", keyFormatter); + std::cout << std::endl; + } + } else if (auto nf = std::dynamic_pointer_cast(factor)) { + const double errorValue = (factor != nullptr ? nf->error(values) : .0); + if (!printCondition(factor.get(), errorValue, i)) + continue; // User-provided filter did not pass + + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = " << errorValue << "\n"; + } + } else if (auto gf = std::dynamic_pointer_cast(factor)) { + const double errorValue = (factor != nullptr ? gf->error(values) : .0); + if (!printCondition(factor.get(), errorValue, i)) + continue; // User-provided filter did not pass + + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = " << errorValue << "\n"; + } + } else if (auto df = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + df->errorTree().print("", keyFormatter); + std::cout << std::endl; + } + + } else { + continue; + } + + std::cout << "\n"; + } + std::cout.flush(); +} + /* ************************************************************************* */ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize( const Values& continuousValues) const { diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.h b/gtsam/hybrid/HybridNonlinearFactorGraph.h index 89b4fb391..54dc9e93f 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.h +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.h @@ -34,7 +34,7 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { protected: public: using Base = HybridFactorGraph; - using This = HybridNonlinearFactorGraph; ///< this class + using This = HybridNonlinearFactorGraph; ///< this class using shared_ptr = std::shared_ptr; ///< shared_ptr to This using Values = gtsam::Values; ///< backwards compatibility @@ -63,6 +63,16 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { const std::string& s = "HybridNonlinearFactorGraph", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; + /** print errors along with factors*/ + void printErrors( + const HybridValues& values, + const std::string& str = "HybridNonlinearFactorGraph: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const std::function& + printCondition = + [](const Factor*, double, size_t) { return true; }) const; + /// @} /// @name Standard Interface /// @{ diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index df8e0193a..09a641b48 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -131,13 +131,13 @@ class MixtureFactor : public HybridFactor { * @return AlgebraicDecisionTree A decision tree with the same keys * as the factor, and leaf values as the error. */ - AlgebraicDecisionTree error(const Values& continuousValues) const { + AlgebraicDecisionTree errorTree(const Values& continuousValues) const { // functor to convert from sharedFactor to double error value. auto errorFunc = [continuousValues](const sharedFactor& factor) { return factor->error(continuousValues); }; - DecisionTree errorTree(factors_, errorFunc); - return errorTree; + DecisionTree result(factors_, errorFunc); + return result; } /** diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index f15c06165..4da61912e 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -97,7 +97,7 @@ TEST(GaussianMixture, LogProbability) { /// Check error. TEST(GaussianMixture, Error) { using namespace equal_constants; - auto actual = mixture.error(vv); + auto actual = mixture.errorTree(vv); // Check result. std::vector discrete_keys = {mode}; @@ -134,7 +134,7 @@ TEST(GaussianMixture, Likelihood) { std::vector leaves = {conditionals[0]->likelihood(vv)->error(vv), conditionals[1]->likelihood(vv)->error(vv)}; AlgebraicDecisionTree expected(discrete_keys, leaves); - EXPECT(assert_equal(expected, likelihood->error(vv), 1e-6)); + EXPECT(assert_equal(expected, likelihood->errorTree(vv), 1e-6)); // Check that the ratio of probPrime to evaluate is the same for all modes. std::vector ratio(2); diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 75ba5a059..9cc7e6bfd 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -178,7 +178,7 @@ TEST(GaussianMixtureFactor, Error) { continuousValues.insert(X(2), Vector2(1, 1)); // error should return a tree of errors, with nodes for each discrete value. - AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousValues); + AlgebraicDecisionTree error_tree = mixtureFactor.errorTree(continuousValues); std::vector discrete_keys = {m1}; // Error values for regression test diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 5248fce01..00dc36cd0 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -153,6 +153,45 @@ TEST(HybridBayesNet, Choose) { *gbn.at(3))); } +/* ****************************************************************************/ +// Test error for a hybrid Bayes net P(X0|X1) P(X1|Asia) P(Asia). +TEST(HybridBayesNet, Error) { + const auto continuousConditional = GaussianConditional::sharedMeanAndStddev( + X(0), 2 * I_1x1, X(1), Vector1(-4.0), 5.0); + + const SharedDiagonal model0 = noiseModel::Diagonal::Sigmas(Vector1(2.0)), + model1 = noiseModel::Diagonal::Sigmas(Vector1(3.0)); + + const auto conditional0 = std::make_shared( + X(1), Vector1::Constant(5), I_1x1, model0), + conditional1 = std::make_shared( + X(1), Vector1::Constant(2), I_1x1, model1); + + auto gm = + new GaussianMixture({X(1)}, {}, {Asia}, {conditional0, conditional1}); + // Create hybrid Bayes net. + HybridBayesNet bayesNet; + bayesNet.push_back(continuousConditional); + bayesNet.emplace_back(gm); + bayesNet.emplace_back(new DiscreteConditional(Asia, "99/1")); + + // Create values at which to evaluate. + HybridValues values; + values.insert(asiaKey, 0); + values.insert(X(0), Vector1(-6)); + values.insert(X(1), Vector1(1)); + + AlgebraicDecisionTree actual_errors = + bayesNet.errorTree(values.continuous()); + + // Regression. + // Manually added all the error values from the 3 conditional types. + AlgebraicDecisionTree expected_errors( + {Asia}, std::vector{2.33005033585, 5.38619084965}); + + EXPECT(assert_equal(expected_errors, actual_errors)); +} + /* ****************************************************************************/ // Test Bayes net optimize TEST(HybridBayesNet, OptimizeAssignment) { diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index b240e1626..5be2f2742 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -580,7 +580,7 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrimeTree) { HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential(); HybridValues delta = hybridBayesNet->optimize(); - auto error_tree = graph.error(delta.continuous()); + auto error_tree = graph.errorTree(delta.continuous()); std::vector discrete_keys = {{M(0), 2}, {M(1), 2}}; std::vector leaves = {0.9998558, 0.4902432, 0.5193694, 0.0097568}; @@ -658,7 +658,7 @@ bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements, } /* ****************************************************************************/ -// Check that the factor graph unnormalized probability is proportional to the +// Check that the bayes net unnormalized probability is proportional to the // Bayes net probability for the given measurements. bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements, const HybridBayesNet &posterior, size_t num_samples = 100) { diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index a493de5c5..93081d309 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -327,8 +327,8 @@ GaussianFactorGraph::shared_ptr batchGFG(double between, NonlinearFactorGraph graph; graph.addPrior(X(0), 0, Isotropic::Sigma(1, 0.1)); - auto between_x0_x1 = std::make_shared( - X(0), X(1), between, Isotropic::Sigma(1, 1.0)); + auto between_x0_x1 = std::make_shared(X(0), X(1), between, + Isotropic::Sigma(1, 1.0)); graph.push_back(between_x0_x1); @@ -397,6 +397,25 @@ TEST(HybridFactorGraph, Partial_Elimination) { EXPECT(remainingFactorGraph->at(2)->keys() == KeyVector({M(0), M(1)})); } +TEST(HybridFactorGraph, PrintErrors) { + Switching self(3); + + // Get nonlinear factor graph and add linear factors to be holistic + HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph; + fg.add(self.linearizedFactorGraph); + + // Optimize to get HybridValues + HybridBayesNet::shared_ptr bn = + self.linearizedFactorGraph.eliminateSequential(); + HybridValues hv = bn->optimize(); + + // Print and verify + // fg.print(); + // std::cout << "\n\n\n" << std::endl; + // fg.printErrors( + // HybridValues(hv.continuous(), DiscreteValues(), self.linearizationPoint)); +} + /**************************************************************************** * Test full elimination */ @@ -564,7 +583,7 @@ factor 6: P( m1 | m0 ): )"; #else -string expected_hybridFactorGraph = R"( + string expected_hybridFactorGraph = R"( size: 7 factor 0: A[x0] = [ @@ -759,9 +778,9 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { KeyVector contKeys = {X(0), X(1)}; auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); auto still = std::make_shared(X(0), X(1), Pose2(0, 0, 0), - noise_model), + noise_model), moving = std::make_shared(X(0), X(1), odometry, - noise_model); + noise_model); std::vector motion_models = {still, moving}; fg.emplace_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models); @@ -788,7 +807,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { initialEstimate.insert(L(1), Point2(4.1, 1.8)); // We want to eliminate variables not connected to DCFactors first. - const Ordering ordering {L(0), L(1), X(0), X(1)}; + const Ordering ordering{L(0), L(1), X(0), X(1)}; HybridGaussianFactorGraph linearized = *fg.linearize(initialEstimate); diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 67a7fd8ae..0b2564403 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -97,7 +97,8 @@ TEST(MixtureFactor, Error) { continuousValues.insert(X(1), 0); continuousValues.insert(X(2), 1); - AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousValues); + AlgebraicDecisionTree error_tree = + mixtureFactor.errorTree(continuousValues); DiscreteKey m1(1, 2); std::vector discrete_keys = {m1}; diff --git a/gtsam/navigation/BarometricFactor.h b/gtsam/navigation/BarometricFactor.h index 38677ed58..70cae8d36 100644 --- a/gtsam/navigation/BarometricFactor.h +++ b/gtsam/navigation/BarometricFactor.h @@ -91,7 +91,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN { -0.00649; } - inline double baroOut(const double& meters) { + inline double baroOut(const double& meters) const { double temp = 15.04 - 0.00649 * meters; return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256); } diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 4410d629c..3e2817752 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -106,7 +106,8 @@ bool NavState::equals(const NavState& other, double tol) const { //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { - auto [nRb, n_t, n_v] = (*this); + Rot3 nRb = R_; + Point3 n_t = t_, n_v = v_; Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0); const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0); diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 8e6090e06..92864c18a 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -294,7 +294,7 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GPSFactor& expected, double tol); + bool equals(const gtsam::NonlinearFactor& expected, double tol); // Standard Interface gtsam::Point3 measurementIn() const; @@ -307,12 +307,29 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor { // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GPSFactor2& expected, double tol); + bool equals(const gtsam::NonlinearFactor& expected, double tol); // Standard Interface gtsam::Point3 measurementIn() const; }; +#include +virtual class BarometricFactor : gtsam::NonlinearFactor { + BarometricFactor(); + BarometricFactor(size_t key, size_t baroKey, const double& baroIn, + const gtsam::noiseModel::Base* model); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol); + + // Standard Interface + const double& measurementIn() const; + double heightOut(double n) const; + double baroOut(const double& meters) const; +}; + #include virtual class Scenario { gtsam::Pose3 pose(double t) const; diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index ac2942032..c4015db37 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -42,7 +42,7 @@ using CustomErrorFunction = std::function #include -#include +#include namespace gtsam { /* @@ -36,8 +36,7 @@ namespace gtsam { * Equivalent to chi2inv in Matlab. */ static double Chi2inv(const double alpha, const size_t dofs) { - boost::math::chi_squared_distribution chi2(dofs); - return boost::math::quantile(chi2, alpha); + return internal::chi_squared_quantile(dofs, alpha); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index d05e72ee2..b1237b790 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -76,9 +76,9 @@ class GncParams { /// Use IndexVector for inliers and outliers since it is fast using IndexVector = FastVector; ///< Slots in the factor graph corresponding to measurements that we know are inliers - IndexVector knownInliers = IndexVector(); + IndexVector knownInliers; ///< Slots in the factor graph corresponding to measurements that we know are outliers - IndexVector knownOutliers = IndexVector(); + IndexVector knownOutliers; /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). void setLossType(const GncLossType type) { diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index a93f9570e..1fe909a11 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -197,6 +197,61 @@ namespace gtsam { } }; +// Added this section for compile gtsam python on windows. +// msvc don't deduct the template arguments correctly, due possible bug in msvc. +#ifdef _WIN32 + // Handle dynamic matrices + template + struct handle_matrix, true> { + inline Eigen::Matrix operator()(Key j, const Value* const pointer) { + auto ptr = dynamic_cast>*>(pointer); + if (ptr) { + // value returns a const Matrix&, and the return makes a copy !!!!! + return ptr->value(); + } else { + // If a fixed matrix was stored, we end up here as well. + throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix)); + } + } + }; + + // Handle fixed matrices + template + struct handle_matrix, false> { + inline Eigen::Matrix operator()(Key j, const Value* const pointer) { + auto ptr = dynamic_cast>*>(pointer); + if (ptr) { + // value returns a const MatrixMN&, and the return makes a copy !!!!! + return ptr->value(); + } else { + Matrix A; + // Check if a dynamic matrix was stored + auto ptr = dynamic_cast*>(pointer); + if (ptr) { + A = ptr->value(); + } else { + // Or a dynamic vector + A = handle_matrix()(j, pointer); // will throw if not.... + } + // Yes: check size, and throw if not a match + if (A.rows() != M || A.cols() != N) + throw NoMatchFoundForFixed(M, N, A.rows(), A.cols()); + else + return A; // copy but not malloc + } + } + }; + + // Handle matrices + template + struct handle> { + Eigen::Matrix operator()(Key j, const Value* const pointer) { + return handle_matrix, + (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer); + } + }; +#endif // #ifdef _WIN32 + } // internal /* ************************************************************************* */ diff --git a/gtsam/nonlinear/internal/ChiSquaredInverse.h b/gtsam/nonlinear/internal/ChiSquaredInverse.h new file mode 100644 index 000000000..dbf83f92b --- /dev/null +++ b/gtsam/nonlinear/internal/ChiSquaredInverse.h @@ -0,0 +1,44 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ChiSquaredInverse.h + * @brief Implementation of the Chi Squared inverse function. + * + * Uses the cephes 3rd party library to help with + * incomplete gamma inverse functions. + * + * @author Varun Agrawal + */ + +#pragma once + +#include + +namespace gtsam { +namespace internal { + +/** + * @brief Compute the quantile function of the Chi-Squared distribution. + * + * The quantile function of the Chi-squared distribution is the quantile of + * the specific (inverse) incomplete Gamma distribution. + * + * @param dofs Degrees of freedom + * @param alpha Quantile value + * @return double + */ +double chi_squared_quantile(const double dofs, const double alpha) { + return 2 * igami(dofs / 2, alpha); +} + +} // namespace internal +} // namespace gtsam diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 27cfdfa2b..35102401e 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -69,7 +69,7 @@ using MatchIndicesMap = std::map; * correspondence indices, from each image. * @param Length-N list of keypoints, for N images/cameras. */ -std::vector tracksFromPairwiseMatches( +std::vector GTSAM_EXPORT tracksFromPairwiseMatches( const MatchIndicesMap& matches, const KeypointsVector& keypoints, bool verbose = false); diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index 9496fc1a6..d7a63eae0 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -53,6 +53,11 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { /// Multiply into a decisiontree DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree errorTree() const override { + throw std::runtime_error("AllDiff::error not implemented"); + } + /* * Ensure Arc-consistency by checking every possible value of domain j. * @param j domain to be checked diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index b207acb9d..18b335092 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -91,6 +91,11 @@ class BinaryAllDiff : public Constraint { const Domains&) const override { throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); } + + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree errorTree() const override { + throw std::runtime_error("BinaryAllDiff::error not implemented"); + } }; } // namespace gtsam diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 0f5b5fdf9..7f7b717c2 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -69,6 +69,11 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { } } + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree errorTree() const override { + throw std::runtime_error("Domain::error not implemented"); + } + // Return concise string representation, mostly to debug arc consistency. // Converts from base 0 to base1. std::string base1Str() const; diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index 1c726d4d0..3f7f22d6a 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -49,6 +49,11 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { } } + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree errorTree() const override { + throw std::runtime_error("SingleValue::error not implemented"); + } + /// Calculate value double operator()(const DiscreteValues& values) const override; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index f874c2f21..ba55ac2af 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -94,6 +94,14 @@ set(interface_headers set(GTSAM_PYTHON_TARGET gtsam_py) set(GTSAM_PYTHON_UNSTABLE_TARGET gtsam_unstable_py) +set(GTSAM_OUTPUT_NAME "gtsam") +set(GTSAM_UNSTABLE_OUTPUT_NAME "gtsam_unstable") + +if(MSVC) + set(GTSAM_OUTPUT_NAME "gtsam_py") + set(GTSAM_UNSTABLE_OUTPUT_NAME "gtsam_unstable_py") +endif() + pybind_wrap(${GTSAM_PYTHON_TARGET} # target "${interface_headers}" # interface_headers "gtsam.cpp" # generated_cpp @@ -109,12 +117,30 @@ pybind_wrap(${GTSAM_PYTHON_TARGET} # target set_target_properties(${GTSAM_PYTHON_TARGET} PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" INSTALL_RPATH_USE_LINK_PATH TRUE - OUTPUT_NAME "gtsam" + OUTPUT_NAME "${GTSAM_OUTPUT_NAME}" LIBRARY_OUTPUT_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam" DEBUG_POSTFIX "" # Otherwise you will have a wrong name RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name ) +if(WIN32) + set_target_properties(${GTSAM_PYTHON_TARGET} PROPERTIES + SUFFIX ".pyd" + ) + ADD_CUSTOM_COMMAND(TARGET ${GTSAM_PYTHON_TARGET} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_if_different + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/${GTSAM_OUTPUT_NAME}.pyd" + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/gtsam.pyd" + ) + ADD_CUSTOM_COMMAND(TARGET ${GTSAM_PYTHON_TARGET} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_if_different + "$;$" + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/" + COMMAND_EXPAND_LISTS + VERBATIM + ) +endif() + # Set the path for the GTSAM python module set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam) @@ -188,7 +214,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) set_target_properties(${GTSAM_PYTHON_UNSTABLE_TARGET} PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" INSTALL_RPATH_USE_LINK_PATH TRUE - OUTPUT_NAME "gtsam_unstable" + OUTPUT_NAME "${GTSAM_UNSTABLE_OUTPUT_NAME}" LIBRARY_OUTPUT_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable" DEBUG_POSTFIX "" # Otherwise you will have a wrong name RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name @@ -208,13 +234,39 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) # Add gtsam_unstable to the install target list(APPEND GTSAM_PYTHON_DEPENDENCIES ${GTSAM_PYTHON_UNSTABLE_TARGET}) - + if(WIN32) + set_target_properties(${GTSAM_PYTHON_UNSTABLE_TARGET} PROPERTIES + SUFFIX ".pyd" + ) + ADD_CUSTOM_COMMAND(TARGET ${GTSAM_PYTHON_UNSTABLE_TARGET} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_if_different + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/${GTSAM_UNSTABLE_OUTPUT_NAME}.pyd" + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/gtsam_unstable.pyd" + ) + ADD_CUSTOM_COMMAND(TARGET ${GTSAM_PYTHON_UNSTABLE_TARGET} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_if_different + "$;$" + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/" + COMMAND_EXPAND_LISTS + VERBATIM + ) + endif() + # Custom make command to run all GTSAM_UNSTABLE Python tests + add_custom_target( + python-test-unstable + COMMAND + ${CMAKE_COMMAND} -E env # add package to python path so no need to install + "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}" + ${PYTHON_EXECUTABLE} -m unittest discover -v -s . + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} + WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/tests" + ) endif() # Add custom target so we can install with `make python-install` set(GTSAM_PYTHON_INSTALL_TARGET python-install) add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND ${PYTHON_EXECUTABLE} -m pip install . + COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) diff --git a/python/gtsam/tests/test_Utilities.py b/python/gtsam/tests/test_Utilities.py index 851684f12..3dd472c75 100644 --- a/python/gtsam/tests/test_Utilities.py +++ b/python/gtsam/tests/test_Utilities.py @@ -12,13 +12,14 @@ Author: Varun Agrawal import unittest import numpy as np +from gtsam.utils.test_case import GtsamTestCase import gtsam -from gtsam.utils.test_case import GtsamTestCase class TestUtilites(GtsamTestCase): """Test various GTSAM utilities.""" + def test_createKeyList(self): """Test createKeyList.""" I = [0, 1, 2] @@ -28,6 +29,17 @@ class TestUtilites(GtsamTestCase): kl = gtsam.utilities.createKeyList("s", I) self.assertEqual(kl.size(), 3) + def test_KeyList_iteration(self): + """Tests for KeyList iteration""" + I = [0, 1, 2] + kl = gtsam.utilities.createKeyList(I) + + self.assertEqual(len(kl), len(I)) + + for i, key in enumerate(kl): + self.assertTrue(key in kl) + self.assertEqual(I[i], key) + def test_createKeyVector(self): """Test createKeyVector.""" I = [0, 1, 2] @@ -37,6 +49,17 @@ class TestUtilites(GtsamTestCase): kl = gtsam.utilities.createKeyVector("s", I) self.assertEqual(len(kl), 3) + def test_KeyVector_iteration(self): + """Tests for KeyVector iteration""" + I = [0, 1, 2] + kv = gtsam.utilities.createKeyVector(I) + + self.assertEqual(len(kv), len(I)) + + for i, key in enumerate(kv): + self.assertTrue(key in kv) + self.assertEqual(I[i], key) + def test_createKeySet(self): """Test createKeySet.""" I = [0, 1, 2] @@ -46,6 +69,17 @@ class TestUtilites(GtsamTestCase): kl = gtsam.utilities.createKeySet("s", I) self.assertEqual(kl.size(), 3) + def test_KeySet_iteration(self): + """Tests for KeySet iteration""" + I = [0, 1, 2] + ks = gtsam.utilities.createKeySet(I) + + self.assertEqual(len(ks), len(I)) + + for i, key in enumerate(ks): + self.assertTrue(key in ks) + self.assertEqual(I[i], key) + def test_extractPoint2(self): """Test extractPoint2.""" initial = gtsam.Values() diff --git a/python/setup.py.in b/python/setup.py.in index e15e39075..824a6656e 100644 --- a/python/setup.py.in +++ b/python/setup.py.in @@ -11,7 +11,8 @@ print("PACKAGES: ", packages) package_data = { '': [ "./*.so", - "./*.dll" + "./*.dll", + "./*.pyd", ] } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 44b7505fc..082b261ad 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -8,7 +8,6 @@ if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") # might not be best test - Richar endif() if (NOT GTSAM_USE_BOOST_FEATURES) - list(APPEND excluded_tests "testGncOptimizer.cpp") list(APPEND excluded_tests "testGraph.cpp") endif() diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 5424a5744..4e0ebf516 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -750,7 +750,8 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { // add a few outliers SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( Vector3(0.1, 0.1, 0.01)); - graph->push_back(BetweenFactor(90, 50, Pose2(), betweenNoise)); // some arbitrary and incorrect between factor + // some arbitrary and incorrect between factor + graph->push_back(BetweenFactor(90, 50, Pose2(), betweenNoise)); /// get expected values by optimizing outlier-free graph Values expectedWithOutliers = LevenbergMarquardtOptimizer(*graph, *initial) @@ -759,9 +760,9 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { // CHECK(assert_equal(expected, expectedWithOutliers, 1e-3)); // GNC - // Note: in difficult instances, we set the odometry measurements to be - // inliers, but this problem is simple enought to succeed even without that - // assumption GncParams::IndexVector knownInliers; + // NOTE: in difficult instances, we set the odometry measurements to be + // inliers, but this problem is simple enough to succeed even without that + // assumption. GncParams gncParams; auto gnc = GncOptimizer>(*graph, *initial, gncParams); diff --git a/wrap/gtwrap/interface_parser/classes.py b/wrap/gtwrap/interface_parser/classes.py index b63a0b5eb..8967bea93 100644 --- a/wrap/gtwrap/interface_parser/classes.py +++ b/wrap/gtwrap/interface_parser/classes.py @@ -12,13 +12,14 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae from typing import Any, Iterable, List, Union -from pyparsing import Literal, Optional, ZeroOrMore # type: ignore +from pyparsing import ZeroOrMore # type: ignore +from pyparsing import Literal, Optional, Word, alphas from .enum import Enum from .function import ArgumentList, ReturnType from .template import Template -from .tokens import (CLASS, COLON, CONST, IDENT, LBRACE, LPAREN, OPERATOR, - RBRACE, RPAREN, SEMI_COLON, STATIC, VIRTUAL) +from .tokens import (CLASS, COLON, CONST, DUNDER, IDENT, LBRACE, LPAREN, + OPERATOR, RBRACE, RPAREN, SEMI_COLON, STATIC, VIRTUAL) from .type import TemplatedType, Typename from .utils import collect_namespaces from .variable import Variable @@ -212,6 +213,26 @@ class Operator: ) +class DunderMethod: + """Special Python double-underscore (dunder) methods, e.g. __iter__, __contains__""" + rule = ( + DUNDER # + + (Word(alphas))("name") # + + DUNDER # + + LPAREN # + + ArgumentList.rule("args_list") # + + RPAREN # + + SEMI_COLON # BR + ).setParseAction(lambda t: DunderMethod(t.name, t.args_list)) + + def __init__(self, name: str, args: ArgumentList): + self.name = name + self.args = args + + def __repr__(self) -> str: + return f"DunderMethod: __{self.name}__({self.args})" + + class Class: """ Rule to parse a class defined in the interface file. @@ -223,11 +244,13 @@ class Class: }; ``` """ + class Members: """ Rule for all the members within a class. """ - rule = ZeroOrMore(Constructor.rule # + rule = ZeroOrMore(DunderMethod.rule # + ^ Constructor.rule # ^ Method.rule # ^ StaticMethod.rule # ^ Variable.rule # @@ -235,11 +258,12 @@ class Class: ^ Enum.rule # ).setParseAction(lambda t: Class.Members(t.asList())) - def __init__(self, - members: List[Union[Constructor, Method, StaticMethod, - Variable, Operator]]): + def __init__(self, members: List[Union[Constructor, Method, + StaticMethod, Variable, + Operator, Enum, DunderMethod]]): self.ctors = [] self.methods = [] + self.dunder_methods = [] self.static_methods = [] self.properties = [] self.operators = [] @@ -251,6 +275,8 @@ class Class: self.methods.append(m) elif isinstance(m, StaticMethod): self.static_methods.append(m) + elif isinstance(m, DunderMethod): + self.dunder_methods.append(m) elif isinstance(m, Variable): self.properties.append(m) elif isinstance(m, Operator): @@ -271,8 +297,8 @@ class Class: + SEMI_COLON # BR ).setParseAction(lambda t: Class( t.template, t.is_virtual, t.name, t.parent_class, t.members.ctors, t. - members.methods, t.members.static_methods, t.members.properties, t. - members.operators, t.members.enums)) + members.methods, t.members.static_methods, t.members.dunder_methods, t. + members.properties, t.members.operators, t.members.enums)) def __init__( self, @@ -283,6 +309,7 @@ class Class: ctors: List[Constructor], methods: List[Method], static_methods: List[StaticMethod], + dunder_methods: List[DunderMethod], properties: List[Variable], operators: List[Operator], enums: List[Enum], @@ -308,6 +335,7 @@ class Class: self.ctors = ctors self.methods = methods self.static_methods = static_methods + self.dunder_methods = dunder_methods self.properties = properties self.operators = operators self.enums = enums @@ -326,6 +354,8 @@ class Class: method.parent = self for static_method in self.static_methods: static_method.parent = self + for dunder_method in self.dunder_methods: + dunder_method.parent = self for _property in self.properties: _property.parent = self diff --git a/wrap/gtwrap/interface_parser/function.py b/wrap/gtwrap/interface_parser/function.py index b40884488..5385c744f 100644 --- a/wrap/gtwrap/interface_parser/function.py +++ b/wrap/gtwrap/interface_parser/function.py @@ -82,7 +82,7 @@ class ArgumentList: return ArgumentList([]) def __repr__(self) -> str: - return repr(tuple(self.args_list)) + return ",".join([repr(x) for x in self.args_list]) def __len__(self) -> int: return len(self.args_list) diff --git a/wrap/gtwrap/interface_parser/tokens.py b/wrap/gtwrap/interface_parser/tokens.py index 02e6d82f8..11c99d19c 100644 --- a/wrap/gtwrap/interface_parser/tokens.py +++ b/wrap/gtwrap/interface_parser/tokens.py @@ -22,6 +22,7 @@ RAW_POINTER, SHARED_POINTER, REF = map(Literal, "@*&") LPAREN, RPAREN, LBRACE, RBRACE, COLON, SEMI_COLON = map(Suppress, "(){}:;") LOPBRACK, ROPBRACK, COMMA, EQUAL = map(Suppress, "<>,=") +DUNDER = Suppress(Literal("__")) # Default argument passed to functions/methods. # Allow anything up to ',' or ';' except when they diff --git a/wrap/gtwrap/pybind_wrapper.py b/wrap/gtwrap/pybind_wrapper.py index 78730a909..479c2d67d 100755 --- a/wrap/gtwrap/pybind_wrapper.py +++ b/wrap/gtwrap/pybind_wrapper.py @@ -45,6 +45,8 @@ class PybindWrapper: 'continue', 'global', 'pass' ] + self.dunder_methods = ('len', 'contains', 'iter') + # amount of indentation to add before each function/method declaration. self.method_indent = '\n' + (' ' * 8) @@ -153,6 +155,51 @@ class PybindWrapper: suffix=suffix) return ret + def _wrap_dunder(self, + method, + cpp_class, + prefix, + suffix, + method_suffix=""): + """ + Wrap a Python double-underscore (dunder) method. + + E.g. __len__() gets wrapped as `.def("__len__", [](gtsam::KeySet* self) {return self->size();})` + + Supported methods are: + - __contains__(T x) + - __len__() + - __iter__() + """ + py_method = method.name + method_suffix + args_names = method.args.names() + py_args_names = self._py_args_names(method.args) + args_signature_with_names = self._method_args_signature(method.args) + + if method.name == 'len': + function_call = "return std::distance(self->begin(), self->end());" + elif method.name == 'contains': + function_call = f"return std::find(self->begin(), self->end(), {method.args.args_list[0].name}) != self->end();" + elif method.name == 'iter': + function_call = "return py::make_iterator(self->begin(), self->end());" + + ret = ('{prefix}.def("__{py_method}__",' + '[]({self}{opt_comma}{args_signature_with_names}){{' + '{function_call}' + '}}' + '{py_args_names}){suffix}'.format( + prefix=prefix, + py_method=py_method, + self=f"{cpp_class}* self", + opt_comma=', ' if args_names else '', + args_signature_with_names=args_signature_with_names, + function_call=function_call, + py_args_names=py_args_names, + suffix=suffix, + )) + + return ret + def _wrap_method(self, method, cpp_class, @@ -235,6 +282,20 @@ class PybindWrapper: return ret + def wrap_dunder_methods(self, + methods, + cpp_class, + prefix='\n' + ' ' * 8, + suffix=''): + res = "" + for method in methods: + res += self._wrap_dunder(method=method, + cpp_class=cpp_class, + prefix=prefix, + suffix=suffix) + + return res + def wrap_methods(self, methods, cpp_class, @@ -398,6 +459,7 @@ class PybindWrapper: '{wrapped_ctors}' '{wrapped_methods}' '{wrapped_static_methods}' + '{wrapped_dunder_methods}' '{wrapped_properties}' '{wrapped_operators};\n'.format( class_declaration=class_declaration, @@ -406,6 +468,8 @@ class PybindWrapper: instantiated_class.methods, cpp_class), wrapped_static_methods=self.wrap_methods( instantiated_class.static_methods, cpp_class), + wrapped_dunder_methods=self.wrap_dunder_methods( + instantiated_class.dunder_methods, cpp_class), wrapped_properties=self.wrap_properties( instantiated_class.properties, cpp_class), wrapped_operators=self.wrap_operators( diff --git a/wrap/gtwrap/template_instantiator/classes.py b/wrap/gtwrap/template_instantiator/classes.py index ce51d5b96..702654678 100644 --- a/wrap/gtwrap/template_instantiator/classes.py +++ b/wrap/gtwrap/template_instantiator/classes.py @@ -57,6 +57,8 @@ class InstantiatedClass(parser.Class): # Instantiate all instance methods self.methods = self.instantiate_methods(typenames) + + self.dunder_methods = original.dunder_methods super().__init__( self.template, @@ -66,6 +68,7 @@ class InstantiatedClass(parser.Class): self.ctors, self.methods, self.static_methods, + self.dunder_methods, self.properties, self.operators, self.enums, diff --git a/wrap/requirements.txt b/wrap/requirements.txt index 0aac9302e..f43fdda61 100644 --- a/wrap/requirements.txt +++ b/wrap/requirements.txt @@ -1,2 +1,2 @@ -pyparsing==2.4.7 +pyparsing==3.1.1 pytest>=6.2.4 diff --git a/wrap/tests/expected/matlab/FastSet.m b/wrap/tests/expected/matlab/FastSet.m new file mode 100644 index 000000000..4d2a1813e --- /dev/null +++ b/wrap/tests/expected/matlab/FastSet.m @@ -0,0 +1,36 @@ +%class FastSet, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +%-------Constructors------- +%FastSet() +% +classdef FastSet < handle + properties + ptr_FastSet = 0 + end + methods + function obj = FastSet(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + class_wrapper(73, my_ptr); + elseif nargin == 0 + my_ptr = class_wrapper(74); + else + error('Arguments do not match any overload of FastSet constructor'); + end + obj.ptr_FastSet = my_ptr; + end + + function delete(obj) + class_wrapper(75, obj.ptr_FastSet); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected/matlab/MyFactorPosePoint2.m b/wrap/tests/expected/matlab/MyFactorPosePoint2.m index 4a30bd489..ac5b134f9 100644 --- a/wrap/tests/expected/matlab/MyFactorPosePoint2.m +++ b/wrap/tests/expected/matlab/MyFactorPosePoint2.m @@ -15,9 +15,9 @@ classdef MyFactorPosePoint2 < handle function obj = MyFactorPosePoint2(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(73, my_ptr); + class_wrapper(76, my_ptr); elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base') - my_ptr = class_wrapper(74, varargin{1}, varargin{2}, varargin{3}, varargin{4}); + my_ptr = class_wrapper(77, varargin{1}, varargin{2}, varargin{3}, varargin{4}); else error('Arguments do not match any overload of MyFactorPosePoint2 constructor'); end @@ -25,7 +25,7 @@ classdef MyFactorPosePoint2 < handle end function delete(obj) - class_wrapper(75, obj.ptr_MyFactorPosePoint2); + class_wrapper(78, obj.ptr_MyFactorPosePoint2); end function display(obj), obj.print(''); end @@ -36,19 +36,19 @@ classdef MyFactorPosePoint2 < handle % PRINT usage: print(string s, KeyFormatter keyFormatter) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.KeyFormatter') - class_wrapper(76, this, varargin{:}); + class_wrapper(79, this, varargin{:}); return end % PRINT usage: print(string s) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'char') - class_wrapper(77, this, varargin{:}); + class_wrapper(80, this, varargin{:}); return end % PRINT usage: print() : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - class_wrapper(78, this, varargin{:}); + class_wrapper(81, this, varargin{:}); return end error('Arguments do not match any overload of function MyFactorPosePoint2.print'); diff --git a/wrap/tests/expected/matlab/class_wrapper.cpp b/wrap/tests/expected/matlab/class_wrapper.cpp index c4be52018..e33f14238 100644 --- a/wrap/tests/expected/matlab/class_wrapper.cpp +++ b/wrap/tests/expected/matlab/class_wrapper.cpp @@ -31,6 +31,8 @@ typedef std::set*> Collector_ForwardKinematic static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_TemplatedConstructor; static Collector_TemplatedConstructor collector_TemplatedConstructor; +typedef std::set*> Collector_FastSet; +static Collector_FastSet collector_FastSet; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; @@ -101,6 +103,12 @@ void _deleteAllObjects() collector_TemplatedConstructor.erase(iter++); anyDeleted = true; } } + { for(Collector_FastSet::iterator iter = collector_FastSet.begin(); + iter != collector_FastSet.end(); ) { + delete *iter; + collector_FastSet.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -844,7 +852,40 @@ void TemplatedConstructor_deconstructor_72(int nargout, mxArray *out[], int narg delete self; } -void MyFactorPosePoint2_collectorInsertAndMakeBase_73(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void FastSet_collectorInsertAndMakeBase_73(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef std::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_FastSet.insert(self); +} + +void FastSet_constructor_74(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef std::shared_ptr Shared; + + Shared *self = new Shared(new FastSet()); + collector_FastSet.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void FastSet_deconstructor_75(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef std::shared_ptr Shared; + checkArguments("delete_FastSet",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_FastSet::iterator item; + item = collector_FastSet.find(self); + if(item != collector_FastSet.end()) { + collector_FastSet.erase(item); + } + delete self; +} + +void MyFactorPosePoint2_collectorInsertAndMakeBase_76(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef std::shared_ptr> Shared; @@ -853,7 +894,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_73(int nargout, mxArray *out[ collector_MyFactorPosePoint2.insert(self); } -void MyFactorPosePoint2_constructor_74(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_constructor_77(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef std::shared_ptr> Shared; @@ -868,7 +909,7 @@ void MyFactorPosePoint2_constructor_74(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void MyFactorPosePoint2_deconstructor_75(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_deconstructor_78(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef std::shared_ptr> Shared; checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1); @@ -881,7 +922,7 @@ void MyFactorPosePoint2_deconstructor_75(int nargout, mxArray *out[], int nargin delete self; } -void MyFactorPosePoint2_print_76(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_print_79(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,2); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -890,7 +931,7 @@ void MyFactorPosePoint2_print_76(int nargout, mxArray *out[], int nargin, const obj->print(s,keyFormatter); } -void MyFactorPosePoint2_print_77(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_print_80(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,1); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -898,7 +939,7 @@ void MyFactorPosePoint2_print_77(int nargout, mxArray *out[], int nargin, const obj->print(s,gtsam::DefaultKeyFormatter); } -void MyFactorPosePoint2_print_78(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_print_81(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,0); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -1137,22 +1178,31 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) TemplatedConstructor_deconstructor_72(nargout, out, nargin-1, in+1); break; case 73: - MyFactorPosePoint2_collectorInsertAndMakeBase_73(nargout, out, nargin-1, in+1); + FastSet_collectorInsertAndMakeBase_73(nargout, out, nargin-1, in+1); break; case 74: - MyFactorPosePoint2_constructor_74(nargout, out, nargin-1, in+1); + FastSet_constructor_74(nargout, out, nargin-1, in+1); break; case 75: - MyFactorPosePoint2_deconstructor_75(nargout, out, nargin-1, in+1); + FastSet_deconstructor_75(nargout, out, nargin-1, in+1); break; case 76: - MyFactorPosePoint2_print_76(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_collectorInsertAndMakeBase_76(nargout, out, nargin-1, in+1); break; case 77: - MyFactorPosePoint2_print_77(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_constructor_77(nargout, out, nargin-1, in+1); break; case 78: - MyFactorPosePoint2_print_78(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_deconstructor_78(nargout, out, nargin-1, in+1); + break; + case 79: + MyFactorPosePoint2_print_79(nargout, out, nargin-1, in+1); + break; + case 80: + MyFactorPosePoint2_print_80(nargout, out, nargin-1, in+1); + break; + case 81: + MyFactorPosePoint2_print_81(nargout, out, nargin-1, in+1); break; } } catch(const std::exception& e) { diff --git a/wrap/tests/expected/python/class_pybind.cpp b/wrap/tests/expected/python/class_pybind.cpp index 86d69c2e0..2292f46be 100644 --- a/wrap/tests/expected/python/class_pybind.cpp +++ b/wrap/tests/expected/python/class_pybind.cpp @@ -91,6 +91,12 @@ PYBIND11_MODULE(class_py, m_) { .def(py::init(), py::arg("arg")) .def(py::init(), py::arg("arg")); + py::class_>(m_, "FastSet") + .def(py::init<>()) + .def("__len__",[](FastSet* self){return std::distance(self->begin(), self->end());}) + .def("__contains__",[](FastSet* self, size_t key){return std::find(self->begin(), self->end(), key) != self->end();}, py::arg("key")) + .def("__iter__",[](FastSet* self){return py::make_iterator(self->begin(), self->end());}); + py::class_, std::shared_ptr>>(m_, "MyFactorPosePoint2") .def(py::init>(), py::arg("key1"), py::arg("key2"), py::arg("measured"), py::arg("noiseModel")) .def("print",[](MyFactor* self, const string& s, const gtsam::KeyFormatter& keyFormatter){ py::scoped_ostream_redirect output; self->print(s, keyFormatter);}, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter) diff --git a/wrap/tests/fixtures/class.i b/wrap/tests/fixtures/class.i index 766f55329..775bbc737 100644 --- a/wrap/tests/fixtures/class.i +++ b/wrap/tests/fixtures/class.i @@ -145,3 +145,12 @@ class TemplatedConstructor { class SuperCoolFactor; typedef SuperCoolFactor SuperCoolFactorPose3; + +/// @brief class with dunder methods for container behavior +class FastSet { + FastSet(); + + __len__(); + __contains__(size_t key); + __iter__(); +}; \ No newline at end of file diff --git a/wrap/tests/test_interface_parser.py b/wrap/tests/test_interface_parser.py index 45415995f..2a923b3c5 100644 --- a/wrap/tests/test_interface_parser.py +++ b/wrap/tests/test_interface_parser.py @@ -18,11 +18,12 @@ import unittest sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -from gtwrap.interface_parser import (ArgumentList, Class, Constructor, Enum, - Enumerator, ForwardDeclaration, - GlobalFunction, Include, Method, Module, - Namespace, Operator, ReturnType, - StaticMethod, TemplatedType, Type, +from gtwrap.interface_parser import (ArgumentList, Class, Constructor, + DunderMethod, Enum, Enumerator, + ForwardDeclaration, GlobalFunction, + Include, Method, Module, Namespace, + Operator, ReturnType, StaticMethod, + TemplatedType, Type, TypedefTemplateInstantiation, Typename, Variable) from gtwrap.template_instantiator.classes import InstantiatedClass @@ -344,6 +345,17 @@ class TestInterfaceParser(unittest.TestCase): self.assertEqual(1, len(ret.args)) self.assertEqual("const T & name", ret.args.args_list[0].to_cpp()) + def test_dunder_method(self): + """Test for special python dunder methods.""" + iter_string = "__iter__();" + ret = DunderMethod.rule.parse_string(iter_string)[0] + self.assertEqual("iter", ret.name) + + contains_string = "__contains__(size_t key);" + ret = DunderMethod.rule.parse_string(contains_string)[0] + self.assertEqual("contains", ret.name) + self.assertTrue(len(ret.args) == 1) + def test_operator_overload(self): """Test for operator overloading.""" # Unary operator