ellie.c
Go to the documentation of this file.
1 /* ellie.c
2  *
3  * Incomplete elliptic integral of the second kind
4  *
5  *
6  *
7  * SYNOPSIS:
8  *
9  * double phi, m, y, ellie();
10  *
11  * y = ellie( phi, m );
12  *
13  *
14  *
15  * DESCRIPTION:
16  *
17  * Approximates the integral
18  *
19  *
20  * phi
21  * -
22  * | |
23  * | 2
24  * E(phi_\m) = | sqrt( 1 - m sin t ) dt
25  * |
26  * | |
27  * -
28  * 0
29  *
30  * of amplitude phi and modulus m, using the arithmetic -
31  * geometric mean algorithm.
32  *
33  *
34  *
35  * ACCURACY:
36  *
37  * Tested at random arguments with phi in [-10, 10] and m in
38  * [0, 1].
39  * Relative error:
40  * arithmetic domain # trials peak rms
41  * IEEE -10,10 150000 3.3e-15 1.4e-16
42  */
43 
44 
45 /*
46  * Cephes Math Library Release 2.0: April, 1987
47  * Copyright 1984, 1987, 1993 by Stephen L. Moshier
48  * Direct inquiries to 30 Frost Street, Cambridge, MA 02140
49  */
50 /* Copyright 2014, Eric W. Moore */
51 
52 /* Incomplete elliptic integral of second kind */
53 
54 #include "mconf.h"
55 
56 extern double MACHEP;
57 
58 static double ellie_neg_m(double phi, double m);
59 
60 double ellie(double phi, double m)
61 {
62  double a, b, c, e, temp;
63  double lphi, t, E, denom, npio2;
64  int d, mod, sign;
65 
66  if (cephes_isnan(phi) || cephes_isnan(m))
67  return NAN;
68  if (m > 1.0)
69  return NAN;
70  if (cephes_isinf(phi))
71  return phi;
72  if (cephes_isinf(m))
73  return -m;
74  if (m == 0.0)
75  return (phi);
76  lphi = phi;
77  npio2 = floor(lphi / M_PI_2);
78  if (fmod(fabs(npio2), 2.0) == 1.0)
79  npio2 += 1;
80  lphi = lphi - npio2 * M_PI_2;
81  if (lphi < 0.0) {
82  lphi = -lphi;
83  sign = -1;
84  }
85  else {
86  sign = 1;
87  }
88  a = 1.0 - m;
89  E = ellpe(m);
90  if (a == 0.0) {
91  temp = sin(lphi);
92  goto done;
93  }
94  if (a > 1.0) {
95  temp = ellie_neg_m(lphi, m);
96  goto done;
97  }
98 
99  if (lphi < 0.135) {
100  double m11= (((((-7.0/2816.0)*m + (5.0/1056.0))*m - (7.0/2640.0))*m
101  + (17.0/41580.0))*m - (1.0/155925.0))*m;
102  double m9 = ((((-5.0/1152.0)*m + (1.0/144.0))*m - (1.0/360.0))*m
103  + (1.0/5670.0))*m;
104  double m7 = ((-m/112.0 + (1.0/84.0))*m - (1.0/315.0))*m;
105  double m5 = (-m/40.0 + (1.0/30))*m;
106  double m3 = -m/6.0;
107  double p2 = lphi * lphi;
108 
109  temp = ((((m11*p2 + m9)*p2 + m7)*p2 + m5)*p2 + m3)*p2*lphi + lphi;
110  goto done;
111  }
112  t = tan(lphi);
113  b = sqrt(a);
114  /* Thanks to Brian Fitzgerald <fitzgb@mml0.meche.rpi.edu>
115  * for pointing out an instability near odd multiples of pi/2. */
116  if (fabs(t) > 10.0) {
117  /* Transform the amplitude */
118  e = 1.0 / (b * t);
119  /* ... but avoid multiple recursions. */
120  if (fabs(e) < 10.0) {
121  e = atan(e);
122  temp = E + m * sin(lphi) * sin(e) - ellie(e, m);
123  goto done;
124  }
125  }
126  c = sqrt(m);
127  a = 1.0;
128  d = 1;
129  e = 0.0;
130  mod = 0;
131 
132  while (fabs(c / a) > MACHEP) {
133  temp = b / a;
134  lphi = lphi + atan(t * temp) + mod * M_PI;
135  denom = 1 - temp * t * t;
136  if (fabs(denom) > 10*MACHEP) {
137  t = t * (1.0 + temp) / denom;
138  mod = (lphi + M_PI_2) / M_PI;
139  }
140  else {
141  t = tan(lphi);
142  mod = (int)floor((lphi - atan(t))/M_PI);
143  }
144  c = (a - b) / 2.0;
145  temp = sqrt(a * b);
146  a = (a + b) / 2.0;
147  b = temp;
148  d += d;
149  e += c * sin(lphi);
150  }
151 
152  temp = E / ellpk(1.0 - m);
153  temp *= (atan(t) + mod * M_PI) / (d * a);
154  temp += e;
155 
156  done:
157 
158  if (sign < 0)
159  temp = -temp;
160  temp += npio2 * E;
161  return (temp);
162 }
163 
164 /* N.B. This will evaluate its arguments multiple times. */
165 #define MAX3(a, b, c) (a > b ? (a > c ? a : c) : (b > c ? b : c))
166 
167 /* To calculate legendre's incomplete elliptical integral of the second kind for
168  * negative m, we use a power series in phi for small m*phi*phi, an asymptotic
169  * series in m for large m*phi*phi* and the relation to Carlson's symmetric
170  * integrals, R_F(x,y,z) and R_D(x,y,z).
171  *
172  * E(phi, m) = sin(phi) * R_F(cos(phi)^2, 1 - m * sin(phi)^2, 1.0)
173  * - m * sin(phi)^3 * R_D(cos(phi)^2, 1 - m * sin(phi)^2, 1.0) / 3
174  *
175  * = R_F(c-1, c-m, c) - m * R_D(c-1, c-m, c) / 3
176  *
177  * where c = csc(phi)^2. We use the second form of this for (approximately)
178  * phi > 1/(sqrt(DBL_MAX) ~ 1e-154, where csc(phi)^2 overflows. Elsewhere we
179  * use the first form, accounting for the smallness of phi.
180  *
181  * The algorithm used is described in Carlson, B. C. Numerical computation of
182  * real or complex elliptic integrals. (1994) https://arxiv.org/abs/math/9409227
183  * Most variable names reflect Carlson's usage.
184  *
185  * In this routine, we assume m < 0 and 0 > phi > pi/2.
186  */
187 double ellie_neg_m(double phi, double m)
188 {
189  double x, y, z, x1, y1, z1, ret, Q;
190  double A0f, Af, Xf, Yf, Zf, E2f, E3f, scalef;
191  double A0d, Ad, seriesn, seriesd, Xd, Yd, Zd, E2d, E3d, E4d, E5d, scaled;
192  int n = 0;
193  double mpp = (m*phi)*phi;
194 
195  if (-mpp < 1e-6 && phi < -m) {
196  return phi + (mpp*phi*phi/30.0 - mpp*mpp/40.0 - mpp/6.0)*phi;
197  }
198 
199  if (-mpp > 1e6) {
200  double sm = sqrt(-m);
201  double sp = sin(phi);
202  double cp = cos(phi);
203 
204  double a = -cosm1(phi);
205  double b1 = log(4*sp*sm/(1+cp));
206  double b = -(0.5 + b1) / 2.0 / m;
207  double c = (0.75 + cp/sp/sp - b1) / 16.0 / m / m;
208  return (a + b + c) * sm;
209  }
210 
211  if (phi > 1e-153 && m > -1e200) {
212  double s = sin(phi);
213  double csc2 = 1.0 / s / s;
214  scalef = 1.0;
215  scaled = m / 3.0;
216  x = 1.0 / tan(phi) / tan(phi);
217  y = csc2 - m;
218  z = csc2;
219  }
220  else {
221  scalef = phi;
222  scaled = mpp * phi / 3.0;
223  x = 1.0;
224  y = 1 - mpp;
225  z = 1.0;
226  }
227 
228  if (x == y && x == z) {
229  return (scalef + scaled/x)/sqrt(x);
230  }
231 
232  A0f = (x + y + z) / 3.0;
233  Af = A0f;
234  A0d = (x + y + 3.0*z) / 5.0;
235  Ad = A0d;
236  x1 = x; y1 = y; z1 = z; seriesd = 0.0; seriesn = 1.0;
237  /* Carlson gives 1/pow(3*r, 1.0/6.0) for this constant. if r == eps,
238  * it is ~338.38. */
239  Q = 400.0 * MAX3(fabs(A0f-x), fabs(A0f-y), fabs(A0f-z));
240 
241  while (Q > fabs(Af) && Q > fabs(Ad) && n <= 100) {
242  double sx = sqrt(x1);
243  double sy = sqrt(y1);
244  double sz = sqrt(z1);
245  double lam = sx*sy + sx*sz + sy*sz;
246  seriesd += seriesn / (sz * (z1 + lam));
247  x1 = (x1 + lam) / 4.0;
248  y1 = (y1 + lam) / 4.0;
249  z1 = (z1 + lam) / 4.0;
250  Af = (x1 + y1 + z1) / 3.0;
251  Ad = (Ad + lam) / 4.0;
252  n += 1;
253  Q /= 4.0;
254  seriesn /= 4.0;
255  }
256 
257  Xf = (A0f - x) / Af / (1 << 2*n);
258  Yf = (A0f - y) / Af / (1 << 2*n);
259  Zf = -(Xf + Yf);
260 
261  E2f = Xf*Yf - Zf*Zf;
262  E3f = Xf*Yf*Zf;
263 
264  ret = scalef * (1.0 - E2f/10.0 + E3f/14.0 + E2f*E2f/24.0
265  - 3.0*E2f*E3f/44.0) / sqrt(Af);
266 
267  Xd = (A0d - x) / Ad / (1 << 2*n);
268  Yd = (A0d - y) / Ad / (1 << 2*n);
269  Zd = -(Xd + Yd)/3.0;
270 
271  E2d = Xd*Yd - 6.0*Zd*Zd;
272  E3d = (3*Xd*Yd - 8.0*Zd*Zd)*Zd;
273  E4d = 3.0*(Xd*Yd - Zd*Zd)*Zd*Zd;
274  E5d = Xd*Yd*Zd*Zd*Zd;
275 
276  ret -= scaled * (1.0 - 3.0*E2d/14.0 + E3d/6.0 + 9.0*E2d*E2d/88.0
277  - 3.0*E4d/22.0 - 9.0*E2d*E3d/52.0 + 3.0*E5d/26.0)
278  /(1 << 2*n) / Ad / sqrt(Ad);
279  ret -= 3.0 * scaled * seriesd;
280  return ret;
281 }
282 
MAX3
#define MAX3(a, b, c)
Definition: ellie.c:165
gtsam.examples.DogLegOptimizerExample.int
int
Definition: DogLegOptimizerExample.py:111
simple_graph::b1
Vector2 b1(2, -1)
ellie
double ellie(double phi, double m)
Definition: ellie.c:60
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
atan
const EIGEN_DEVICE_FUNC AtanReturnType atan() const
Definition: ArrayCwiseUnaryOps.h:283
cephes_isnan
#define cephes_isnan(x)
Definition: mconf.h:99
d
static const double d[K][N]
Definition: igam.h:11
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
b
Scalar * b
Definition: benchVecAdd.cpp:17
z1
Point2 z1
Definition: testTriangulationFactor.cpp:45
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
ret
DenseIndex ret
Definition: level1_cplx_impl.h:44
sign
const EIGEN_DEVICE_FUNC SignReturnType sign() const
Definition: ArrayCwiseUnaryOps.h:219
log
const EIGEN_DEVICE_FUNC LogReturnType log() const
Definition: ArrayCwiseUnaryOps.h:128
cephes_isinf
#define cephes_isinf(x)
Definition: mconf.h:100
Q
Quaternion Q
Definition: testQuaternion.cpp:27
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
boost::multiprecision::fabs
Real fabs(const Real &a)
Definition: boostmultiprec.cpp:119
n
int n
Definition: BiCGSTAB_simple.cpp:1
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
y1
double y1(double x)
Definition: j1.c:199
Eigen::bfloat16_impl::fmod
EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 fmod(const bfloat16 &a, const bfloat16 &b)
Definition: BFloat16.h:567
ellpk
double ellpk(double x)
Definition: ellpk.c:97
x1
Pose3 x1
Definition: testPose3.cpp:663
ellie_neg_m
static double ellie_neg_m(double phi, double m)
Definition: ellie.c:187
cosm1
double cosm1(double x)
Definition: unity.c:144
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
tan
const EIGEN_DEVICE_FUNC TanReturnType tan() const
Definition: ArrayCwiseUnaryOps.h:269
y
Scalar * y
Definition: level1_cplx_impl.h:124
M_PI_2
#define M_PI_2
Definition: mconf.h:118
E
DiscreteKey E(5, 2)
m3
static const DiscreteKey m3(M(3), 2)
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
mconf.h
ellpe
double ellpe(double x)
Definition: ellpe.c:95
MACHEP
double MACHEP
Definition: const.c:54
M_PI
#define M_PI
Definition: mconf.h:117
align_3::t
Point2 t(10, 10)
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
floor
const EIGEN_DEVICE_FUNC FloorReturnType floor() const
Definition: ArrayCwiseUnaryOps.h:481


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:29