diff --git a/CMakeLists.txt b/CMakeLists.txt
index 16848a7219..66b1804aff 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 0000000000..9addddd60f
--- /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 0000000000..e840e9e493
--- /dev/null
+++ b/gtsam/3rdparty/cephes/CMakeLists.txt
@@ -0,0 +1,123 @@
+cmake_minimum_required(VERSION 3.12)
+enable_testing()
+project(
+ cephes
+ DESCRIPTION "Cephes Mathematical Function Library"
+ VERSION 1.0.0
+ LANGUAGES C)
+
+set(CEPHES_HEADER_FILES
+ cephes.h
+ cephes/cephes_names.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/cephes.h b/gtsam/3rdparty/cephes/cephes.h
new file mode 100644
index 0000000000..629733eef0
--- /dev/null
+++ b/gtsam/3rdparty/cephes/cephes.h
@@ -0,0 +1,163 @@
+#ifndef CEPHES_H
+#define CEPHES_H
+
+#include "cephes/cephes_names.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern int airy(double x, double *ai, double *aip, double *bi, double *bip);
+
+extern double bdtrc(double k, int n, double p);
+extern double bdtr(double k, int n, double p);
+extern double bdtri(double k, int n, double y);
+
+extern double besselpoly(double a, double lambda, double nu);
+
+extern double beta(double a, double b);
+extern double lbeta(double a, double b);
+
+extern double btdtr(double a, double b, double x);
+
+extern double cbrt(double x);
+extern double chbevl(double x, double array[], int n);
+extern double chdtrc(double df, double x);
+extern double chdtr(double df, double x);
+extern double chdtri(double df, double y);
+extern double dawsn(double xx);
+
+extern double ellie(double phi, double m);
+extern double ellik(double phi, double m);
+extern double ellpe(double x);
+
+extern int ellpj(double u, double m, double *sn, double *cn, double *dn, double *ph);
+extern double ellpk(double x);
+extern double exp10(double x);
+extern double exp2(double x);
+
+extern double expn(int n, double x);
+
+extern double fdtrc(double a, double b, double x);
+extern double fdtr(double a, double b, double x);
+extern double fdtri(double a, double b, double y);
+
+extern int fresnl(double xxa, double *ssa, double *cca);
+extern double Gamma(double x);
+extern double lgam(double x);
+extern double lgam_sgn(double x, int *sign);
+extern double gammasgn(double x);
+
+extern double gdtr(double a, double b, double x);
+extern double gdtrc(double a, double b, double x);
+extern double gdtri(double a, double b, double y);
+
+extern double hyp2f1(double a, double b, double c, double x);
+extern double hyperg(double a, double b, double x);
+extern double threef0(double a, double b, double c, double x, double *err);
+
+extern double i0(double x);
+extern double i0e(double x);
+extern double i1(double x);
+extern double i1e(double x);
+extern double igamc(double a, double x);
+extern double igam(double a, double x);
+extern double igam_fac(double a, double x);
+extern double igamci(double a, double q);
+extern double igami(double a, double p);
+
+extern double incbet(double aa, double bb, double xx);
+extern double incbi(double aa, double bb, double yy0);
+
+extern double iv(double v, double x);
+extern double j0(double x);
+extern double y0(double x);
+extern double j1(double x);
+extern double y1(double x);
+
+extern double jn(int n, double x);
+extern double jv(double n, double x);
+extern double k0(double x);
+extern double k0e(double x);
+extern double k1(double x);
+extern double k1e(double x);
+extern double kn(int nn, double x);
+
+extern double nbdtrc(int k, int n, double p);
+extern double nbdtr(int k, int n, double p);
+extern double nbdtri(int k, int n, double p);
+
+extern double ndtr(double a);
+extern double log_ndtr(double a);
+extern double erfc(double a);
+extern double erf(double x);
+extern double erfinv(double y);
+extern double erfcinv(double y);
+extern double ndtri(double y0);
+
+extern double pdtrc(double k, double m);
+extern double pdtr(double k, double m);
+extern double pdtri(int k, double y);
+
+extern double poch(double x, double m);
+
+extern double psi(double x);
+
+extern double rgamma(double x);
+extern double round(double x);
+
+extern int shichi(double x, double *si, double *ci);
+extern int sici(double x, double *si, double *ci);
+
+extern double radian(double d, double m, double s);
+extern double sindg(double x);
+extern double sinpi(double x);
+extern double cosdg(double x);
+extern double cospi(double x);
+
+extern double spence(double x);
+
+extern double stdtr(int k, double t);
+extern double stdtri(int k, double p);
+
+extern double struve_h(double v, double x);
+extern double struve_l(double v, double x);
+extern double struve_power_series(double v, double x, int is_h, double *err);
+extern double struve_asymp_large_z(double v, double z, int is_h, double *err);
+extern double struve_bessel_series(double v, double z, int is_h, double *err);
+
+extern double yv(double v, double x);
+
+extern double tandg(double x);
+extern double cotdg(double x);
+
+extern double log1p(double x);
+extern double log1pmx(double x);
+extern double expm1(double x);
+extern double cosm1(double x);
+extern double lgam1p(double x);
+
+extern double yn(int n, double x);
+extern double zeta(double x, double q);
+extern double zetac(double x);
+
+extern double smirnov(int n, double d);
+extern double smirnovi(int n, double p);
+extern double smirnovp(int n, double d);
+extern double smirnovc(int n, double d);
+extern double smirnovci(int n, double p);
+extern double kolmogorov(double x);
+extern double kolmogi(double p);
+extern double kolmogp(double x);
+extern double kolmogc(double x);
+extern double kolmogci(double p);
+
+extern double lanczos_sum_expg_scaled(double x);
+
+extern 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 0000000000..95e16a55f8
--- /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 0000000000..29fcdf1aff
--- /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 0000000000..a58fe20376
--- /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 0000000000..c0389deea0
--- /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 0000000000..fa115c7b70
--- /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 0000000000..a83c078341
--- /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/cephes_names.h b/gtsam/3rdparty/cephes/cephes/cephes_names.h
new file mode 100644
index 0000000000..94be8c880a
--- /dev/null
+++ b/gtsam/3rdparty/cephes/cephes/cephes_names.h
@@ -0,0 +1,114 @@
+#ifndef CEPHES_NAMES_H
+#define CEPHES_NAMES_H
+
+#define airy cephes_airy
+#define bdtrc cephes_bdtrc
+#define bdtr cephes_bdtr
+#define bdtri cephes_bdtri
+#define besselpoly cephes_besselpoly
+#define beta cephes_beta
+#define lbeta cephes_lbeta
+#define btdtr cephes_btdtr
+#define cbrt cephes_cbrt
+#define chdtrc cephes_chdtrc
+#define chbevl cephes_chbevl
+#define chdtr cephes_chdtr
+#define chdtri cephes_chdtri
+#define dawsn cephes_dawsn
+#define ellie cephes_ellie
+#define ellik cephes_ellik
+#define ellpe cephes_ellpe
+#define ellpj cephes_ellpj
+#define ellpk cephes_ellpk
+#define exp10 cephes_exp10
+#define exp2 cephes_exp2
+#define expn cephes_expn
+#define fdtrc cephes_fdtrc
+#define fdtr cephes_fdtr
+#define fdtri cephes_fdtri
+#define fresnl cephes_fresnl
+#define Gamma cephes_Gamma
+#define lgam cephes_lgam
+#define lgam_sgn cephes_lgam_sgn
+#define gammasgn cephes_gammasgn
+#define gdtr cephes_gdtr
+#define gdtrc cephes_gdtrc
+#define gdtri cephes_gdtri
+#define hyp2f1 cephes_hyp2f1
+#define hyperg cephes_hyperg
+#define i0 cephes_i0
+#define i0e cephes_i0e
+#define i1 cephes_i1
+#define i1e cephes_i1e
+#define igamc cephes_igamc
+#define igam cephes_igam
+#define igami cephes_igami
+#define incbet cephes_incbet
+#define incbi cephes_incbi
+#define iv cephes_iv
+#define j0 cephes_j0
+#define y0 cephes_y0
+#define j1 cephes_j1
+#define y1 cephes_y1
+#define jn cephes_jn
+#define jv cephes_jv
+#define k0 cephes_k0
+#define k0e cephes_k0e
+#define k1 cephes_k1
+#define k1e cephes_k1e
+#define kn cephes_kn
+#define nbdtrc cephes_nbdtrc
+#define nbdtr cephes_nbdtr
+#define nbdtri cephes_nbdtri
+#define ndtr cephes_ndtr
+#define erfc cephes_erfc
+#define erf cephes_erf
+#define erfinv cephes_erfinv
+#define erfcinv cephes_erfcinv
+#define ndtri cephes_ndtri
+#define pdtrc cephes_pdtrc
+#define pdtr cephes_pdtr
+#define pdtri cephes_pdtri
+#define poch cephes_poch
+#define psi cephes_psi
+#define rgamma cephes_rgamma
+#define riemann_zeta cephes_riemann_zeta
+// #define round cephes_round // Commented out since it clashes with std::round
+#define shichi cephes_shichi
+#define sici cephes_sici
+#define radian cephes_radian
+#define sindg cephes_sindg
+#define sinpi cephes_sinpi
+#define cosdg cephes_cosdg
+#define cospi cephes_cospi
+#define sincos cephes_sincos
+#define spence cephes_spence
+#define stdtr cephes_stdtr
+#define stdtri cephes_stdtri
+#define struve_h cephes_struve_h
+#define struve_l cephes_struve_l
+#define struve_power_series cephes_struve_power_series
+#define struve_asymp_large_z cephes_struve_asymp_large_z
+#define struve_bessel_series cephes_struve_bessel_series
+#define yv cephes_yv
+#define tandg cephes_tandg
+#define cotdg cephes_cotdg
+#define log1p cephes_log1p
+#define expm1 cephes_expm1
+#define cosm1 cephes_cosm1
+#define yn cephes_yn
+#define zeta cephes_zeta
+#define zetac cephes_zetac
+#define smirnov cephes_smirnov
+#define smirnovc cephes_smirnovc
+#define smirnovi cephes_smirnovi
+#define smirnovci cephes_smirnovci
+#define smirnovp cephes_smirnovp
+#define kolmogorov cephes_kolmogorov
+#define kolmogi cephes_kolmogi
+#define kolmogp cephes_kolmogp
+#define kolmogc cephes_kolmogc
+#define kolmogci cephes_kolmogci
+#define owens_t cephes_owens_t
+
+#endif
diff --git a/gtsam/3rdparty/cephes/cephes/chbevl.c b/gtsam/3rdparty/cephes/cephes/chbevl.c
new file mode 100644
index 0000000000..a0e9c5c52a
--- /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 0000000000..d576e7a8db
--- /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 0000000000..8631554cca
--- /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 0000000000..7049f191ed
--- /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 0000000000..fec97c4780
--- /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 0000000000..c37f57a7b9
--- /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 0000000000..4e09da1432
--- /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 0000000000..d2b9ac1d65
--- /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 0000000000..8a2823f3a0
--- /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 0000000000..ee73e062a2
--- /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 0000000000..1ef8e0c128
--- /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 0000000000..6891a8244c
--- /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 0000000000..3842a7403a
--- /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 0000000000..f7f49284c1
--- /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 0000000000..0a71d3c52f
--- /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 0000000000..14911f59c0
--- /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 0000000000..2a6ee14c09
--- /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 0000000000..8ced026877
--- /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 0000000000..9c119ed8f7
--- /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 0000000000..50620fa2e1
--- /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 0000000000..2a61defedb
--- /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 0000000000..9d74318ff2
--- /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 0000000000..597c8d4d93
--- /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 0000000000..7f0a84d02a
--- /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 0000000000..ac23e71339
--- /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 0000000000..4e85d556ef
--- /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 0000000000..4553873f2c
--- /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 0000000000..75f871ec51
--- /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 0000000000..0bc310633c
--- /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 0000000000..97fc93ff4d
--- /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 0000000000..b03427f4f7
--- /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 0000000000..747c43f538
--- /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 0000000000..094ef6cef1
--- /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 0000000000..123194de84
--- /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 0000000000..3434c18f31
--- /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 0000000000..c5b31a1bf1
--- /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 0000000000..fc33e5c0ee
--- /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 0000000000..ff7584a154
--- /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 0000000000..2135e0ebbd
--- /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 0000000000..f92a8d2088
--- /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 0000000000..92ab8c1b26
--- /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 0000000000..c59d17a470
--- /dev/null
+++ b/gtsam/3rdparty/cephes/cephes/mconf.h
@@ -0,0 +1,132 @@
+/* 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_names.h"
+#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 0000000000..7697f257ee
--- /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 0000000000..168e98b5ab
--- /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 0000000000..e7fe5cce04
--- /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 0000000000..6eb063510e
--- /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 0000000000..0249074d98
--- /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 0000000000..4c04fa14eb
--- /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 0000000000..eb23ddf88a
--- /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 0000000000..190c6d1628
--- /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 0000000000..6420ccaa94
--- /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 0000000000..0ed1f1415b
--- /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 0000000000..e7bb220119
--- /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 0000000000..95a47c797b
--- /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 0000000000..43986df812
--- /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 0000000000..75104e7247
--- /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 0000000000..7bb79bc25f
--- /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 0000000000..d9c37ebdbf
--- /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 0000000000..f0e52f9904
--- /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 0000000000..48e1c40878
--- /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 0000000000..5a37536bed
--- /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 0000000000..26c86fa2d7
--- /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 0000000000..1ea86329be
--- /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 0000000000..751314a875
--- /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 0000000000..76bc7f08df
--- /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 0000000000..c02ff0acd8
--- /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 0000000000..e61a155214
--- /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 0000000000..554933a24c
--- /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 0000000000..8414331832
--- /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/CMakeLists.txt b/gtsam/CMakeLists.txt
index cb87a6bcdb..1fc8e45707 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/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h
index b646d009ee..0fe576159a 100644
--- a/gtsam/nonlinear/GncOptimizer.h
+++ b/gtsam/nonlinear/GncOptimizer.h
@@ -28,7 +28,7 @@
#include
#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 d05e72ee2d..b1237b7901 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/internal/ChiSquaredInverse.h b/gtsam/nonlinear/internal/ChiSquaredInverse.h
new file mode 100644
index 0000000000..dbf83f92b4
--- /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/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 44b7505fc4..082b261ada 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 5424a5744f..4e0ebf516c 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);