SphericalEngine.cpp
Go to the documentation of this file.
1 
133 #include <GeographicLib/SphericalEngine.hpp>
134 #include <GeographicLib/CircularEngine.hpp>
135 #include <GeographicLib/Utility.hpp>
136 
137 #if defined(_MSC_VER)
138 // Squelch warnings about constant conditional expressions and potentially
139 // uninitialized local variables
140 # pragma warning (disable: 4127 4701)
141 #endif
142 
143 namespace GeographicLib {
144 
145  using namespace std;
146 
147  vector<Math::real>& SphericalEngine::sqrttable() {
148  static vector<real> sqrttable(0);
149  return sqrttable;
150  }
151 
152  template<bool gradp, SphericalEngine::normalization norm, int L>
153  Math::real SphericalEngine::Value(const coeff c[], const real f[],
154  real x, real y, real z, real a,
155  real& gradx, real& grady, real& gradz)
156  {
157  GEOGRAPHICLIB_STATIC_ASSERT(L > 0, "L must be positive");
158  GEOGRAPHICLIB_STATIC_ASSERT(norm == FULL || norm == SCHMIDT,
159  "Unknown normalization");
160  int N = c[0].nmx(), M = c[0].mmx();
161 
162  real
163  p = Math::hypot(x, y),
164  cl = p != 0 ? x / p : 1, // cos(lambda); at pole, pick lambda = 0
165  sl = p != 0 ? y / p : 0, // sin(lambda)
166  r = Math::hypot(z, p),
167  t = r != 0 ? z / r : 0, // cos(theta); at origin, pick theta = pi/2
168  u = r != 0 ? max(p / r, eps()) : 1, // sin(theta); but avoid the pole
169  q = a / r;
170  real
171  q2 = Math::sq(q),
172  uq = u * q,
173  uq2 = Math::sq(uq),
174  tu = t / u;
175  // Initialize outer sum
176  real vc = 0, vc2 = 0, vs = 0, vs2 = 0; // v [N + 1], v [N + 2]
177  // vr, vt, vl and similar w variable accumulate the sums for the
178  // derivatives wrt r, theta, and lambda, respectively.
179  real vrc = 0, vrc2 = 0, vrs = 0, vrs2 = 0; // vr[N + 1], vr[N + 2]
180  real vtc = 0, vtc2 = 0, vts = 0, vts2 = 0; // vt[N + 1], vt[N + 2]
181  real vlc = 0, vlc2 = 0, vls = 0, vls2 = 0; // vl[N + 1], vl[N + 2]
182  int k[L];
183  const vector<real>& root( sqrttable() );
184  for (int m = M; m >= 0; --m) { // m = M .. 0
185  // Initialize inner sum
186  real
187  wc = 0, wc2 = 0, ws = 0, ws2 = 0, // w [N - m + 1], w [N - m + 2]
188  wrc = 0, wrc2 = 0, wrs = 0, wrs2 = 0, // wr[N - m + 1], wr[N - m + 2]
189  wtc = 0, wtc2 = 0, wts = 0, wts2 = 0; // wt[N - m + 1], wt[N - m + 2]
190  for (int l = 0; l < L; ++l)
191  k[l] = c[l].index(N, m) + 1;
192  for (int n = N; n >= m; --n) { // n = N .. m; l = N - m .. 0
193  real w, A, Ax, B, R; // alpha[l], beta[l + 1]
194  switch (norm) {
195  case FULL:
196  w = root[2 * n + 1] / (root[n - m + 1] * root[n + m + 1]);
197  Ax = q * w * root[2 * n + 3];
198  A = t * Ax;
199  B = - q2 * root[2 * n + 5] /
200  (w * root[n - m + 2] * root[n + m + 2]);
201  break;
202  case SCHMIDT:
203  w = root[n - m + 1] * root[n + m + 1];
204  Ax = q * (2 * n + 1) / w;
205  A = t * Ax;
206  B = - q2 * w / (root[n - m + 2] * root[n + m + 2]);
207  break;
208  default: break; // To suppress warning message from Visual Studio
209  }
210  R = c[0].Cv(--k[0]);
211  for (int l = 1; l < L; ++l)
212  R += c[l].Cv(--k[l], n, m, f[l]);
213  R *= scale();
214  w = A * wc + B * wc2 + R; wc2 = wc; wc = w;
215  if (gradp) {
216  w = A * wrc + B * wrc2 + (n + 1) * R; wrc2 = wrc; wrc = w;
217  w = A * wtc + B * wtc2 - u*Ax * wc2; wtc2 = wtc; wtc = w;
218  }
219  if (m) {
220  R = c[0].Sv(k[0]);
221  for (int l = 1; l < L; ++l)
222  R += c[l].Sv(k[l], n, m, f[l]);
223  R *= scale();
224  w = A * ws + B * ws2 + R; ws2 = ws; ws = w;
225  if (gradp) {
226  w = A * wrs + B * wrs2 + (n + 1) * R; wrs2 = wrs; wrs = w;
227  w = A * wts + B * wts2 - u*Ax * ws2; wts2 = wts; wts = w;
228  }
229  }
230  }
231  // Now Sc[m] = wc, Ss[m] = ws
232  // Sc'[m] = wtc, Ss'[m] = wtc
233  if (m) {
234  real v, A, B; // alpha[m], beta[m + 1]
235  switch (norm) {
236  case FULL:
237  v = root[2] * root[2 * m + 3] / root[m + 1];
238  A = cl * v * uq;
239  B = - v * root[2 * m + 5] / (root[8] * root[m + 2]) * uq2;
240  break;
241  case SCHMIDT:
242  v = root[2] * root[2 * m + 1] / root[m + 1];
243  A = cl * v * uq;
244  B = - v * root[2 * m + 3] / (root[8] * root[m + 2]) * uq2;
245  break;
246  default: break; // To suppress warning message from Visual Studio
247  }
248  v = A * vc + B * vc2 + wc ; vc2 = vc ; vc = v;
249  v = A * vs + B * vs2 + ws ; vs2 = vs ; vs = v;
250  if (gradp) {
251  // Include the terms Sc[m] * P'[m,m](t) and Ss[m] * P'[m,m](t)
252  wtc += m * tu * wc; wts += m * tu * ws;
253  v = A * vrc + B * vrc2 + wrc; vrc2 = vrc; vrc = v;
254  v = A * vrs + B * vrs2 + wrs; vrs2 = vrs; vrs = v;
255  v = A * vtc + B * vtc2 + wtc; vtc2 = vtc; vtc = v;
256  v = A * vts + B * vts2 + wts; vts2 = vts; vts = v;
257  v = A * vlc + B * vlc2 + m*ws; vlc2 = vlc; vlc = v;
258  v = A * vls + B * vls2 - m*wc; vls2 = vls; vls = v;
259  }
260  } else {
261  real A, B, qs;
262  switch (norm) {
263  case FULL:
264  A = root[3] * uq; // F[1]/(q*cl) or F[1]/(q*sl)
265  B = - root[15]/2 * uq2; // beta[1]/q
266  break;
267  case SCHMIDT:
268  A = uq;
269  B = - root[3]/2 * uq2;
270  break;
271  default: break; // To suppress warning message from Visual Studio
272  }
273  qs = q / scale();
274  vc = qs * (wc + A * (cl * vc + sl * vs ) + B * vc2);
275  if (gradp) {
276  qs /= r;
277  // The components of the gradient in spherical coordinates are
278  // r: dV/dr
279  // theta: 1/r * dV/dtheta
280  // lambda: 1/(r*u) * dV/dlambda
281  vrc = - qs * (wrc + A * (cl * vrc + sl * vrs) + B * vrc2);
282  vtc = qs * (wtc + A * (cl * vtc + sl * vts) + B * vtc2);
283  vlc = qs / u * ( A * (cl * vlc + sl * vls) + B * vlc2);
284  }
285  }
286  }
287 
288  if (gradp) {
289  // Rotate into cartesian (geocentric) coordinates
290  gradx = cl * (u * vrc + t * vtc) - sl * vlc;
291  grady = sl * (u * vrc + t * vtc) + cl * vlc;
292  gradz = t * vrc - u * vtc ;
293  }
294  return vc;
295  }
296 
297  template<bool gradp, SphericalEngine::normalization norm, int L>
298  CircularEngine SphericalEngine::Circle(const coeff c[], const real f[],
299  real p, real z, real a) {
300 
301  GEOGRAPHICLIB_STATIC_ASSERT(L > 0, "L must be positive");
302  GEOGRAPHICLIB_STATIC_ASSERT(norm == FULL || norm == SCHMIDT,
303  "Unknown normalization");
304  int N = c[0].nmx(), M = c[0].mmx();
305 
306  real
307  r = Math::hypot(z, p),
308  t = r != 0 ? z / r : 0, // cos(theta); at origin, pick theta = pi/2
309  u = r != 0 ? max(p / r, eps()) : 1, // sin(theta); but avoid the pole
310  q = a / r;
311  real
312  q2 = Math::sq(q),
313  tu = t / u;
314  CircularEngine circ(M, gradp, norm, a, r, u, t);
315  int k[L];
316  const vector<real>& root( sqrttable() );
317  for (int m = M; m >= 0; --m) { // m = M .. 0
318  // Initialize inner sum
319  real
320  wc = 0, wc2 = 0, ws = 0, ws2 = 0, // w [N - m + 1], w [N - m + 2]
321  wrc = 0, wrc2 = 0, wrs = 0, wrs2 = 0, // wr[N - m + 1], wr[N - m + 2]
322  wtc = 0, wtc2 = 0, wts = 0, wts2 = 0; // wt[N - m + 1], wt[N - m + 2]
323  for (int l = 0; l < L; ++l)
324  k[l] = c[l].index(N, m) + 1;
325  for (int n = N; n >= m; --n) { // n = N .. m; l = N - m .. 0
326  real w, A, Ax, B, R; // alpha[l], beta[l + 1]
327  switch (norm) {
328  case FULL:
329  w = root[2 * n + 1] / (root[n - m + 1] * root[n + m + 1]);
330  Ax = q * w * root[2 * n + 3];
331  A = t * Ax;
332  B = - q2 * root[2 * n + 5] /
333  (w * root[n - m + 2] * root[n + m + 2]);
334  break;
335  case SCHMIDT:
336  w = root[n - m + 1] * root[n + m + 1];
337  Ax = q * (2 * n + 1) / w;
338  A = t * Ax;
339  B = - q2 * w / (root[n - m + 2] * root[n + m + 2]);
340  break;
341  default: break; // To suppress warning message from Visual Studio
342  }
343  R = c[0].Cv(--k[0]);
344  for (int l = 1; l < L; ++l)
345  R += c[l].Cv(--k[l], n, m, f[l]);
346  R *= scale();
347  w = A * wc + B * wc2 + R; wc2 = wc; wc = w;
348  if (gradp) {
349  w = A * wrc + B * wrc2 + (n + 1) * R; wrc2 = wrc; wrc = w;
350  w = A * wtc + B * wtc2 - u*Ax * wc2; wtc2 = wtc; wtc = w;
351  }
352  if (m) {
353  R = c[0].Sv(k[0]);
354  for (int l = 1; l < L; ++l)
355  R += c[l].Sv(k[l], n, m, f[l]);
356  R *= scale();
357  w = A * ws + B * ws2 + R; ws2 = ws; ws = w;
358  if (gradp) {
359  w = A * wrs + B * wrs2 + (n + 1) * R; wrs2 = wrs; wrs = w;
360  w = A * wts + B * wts2 - u*Ax * ws2; wts2 = wts; wts = w;
361  }
362  }
363  }
364  if (!gradp)
365  circ.SetCoeff(m, wc, ws);
366  else {
367  // Include the terms Sc[m] * P'[m,m](t) and Ss[m] * P'[m,m](t)
368  wtc += m * tu * wc; wts += m * tu * ws;
369  circ.SetCoeff(m, wc, ws, wrc, wrs, wtc, wts);
370  }
371  }
372 
373  return circ;
374  }
375 
376  void SphericalEngine::RootTable(int N) {
377  // Need square roots up to max(2 * N + 5, 15).
378  vector<real>& root( sqrttable() );
379  int L = max(2 * N + 5, 15) + 1, oldL = int(root.size());
380  if (oldL >= L)
381  return;
382  root.resize(L);
383  for (int l = oldL; l < L; ++l)
384  root[l] = sqrt(real(l));
385  }
386 
387  void SphericalEngine::coeff::readcoeffs(std::istream& stream, int& N, int& M,
388  std::vector<real>& C,
389  std::vector<real>& S) {
390  int nm[2];
391  Utility::readarray<int, int, false>(stream, nm, 2);
392  N = nm[0]; M = nm[1];
393  if (!(N >= M && M >= -1 && N * M >= 0))
394  // The last condition is that M = -1 implies N = -1 and vice versa.
395  throw GeographicErr("Bad degree and order " +
396  Utility::str(N) + " " + Utility::str(M));
397  C.resize(SphericalEngine::coeff::Csize(N, M));
398  Utility::readarray<double, real, false>(stream, C);
399  S.resize(SphericalEngine::coeff::Ssize(N, M));
400  Utility::readarray<double, real, false>(stream, S);
401  return;
402  }
403 
405  template Math::real GEOGRAPHICLIB_EXPORT
406  SphericalEngine::Value<true, SphericalEngine::FULL, 1>
407  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
408  template Math::real GEOGRAPHICLIB_EXPORT
409  SphericalEngine::Value<false, SphericalEngine::FULL, 1>
410  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
411  template Math::real GEOGRAPHICLIB_EXPORT
412  SphericalEngine::Value<true, SphericalEngine::SCHMIDT, 1>
413  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
414  template Math::real GEOGRAPHICLIB_EXPORT
415  SphericalEngine::Value<false, SphericalEngine::SCHMIDT, 1>
416  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
417 
418  template Math::real GEOGRAPHICLIB_EXPORT
419  SphericalEngine::Value<true, SphericalEngine::FULL, 2>
420  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
421  template Math::real GEOGRAPHICLIB_EXPORT
422  SphericalEngine::Value<false, SphericalEngine::FULL, 2>
423  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
424  template Math::real GEOGRAPHICLIB_EXPORT
425  SphericalEngine::Value<true, SphericalEngine::SCHMIDT, 2>
426  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
427  template Math::real GEOGRAPHICLIB_EXPORT
428  SphericalEngine::Value<false, SphericalEngine::SCHMIDT, 2>
429  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
430 
431  template Math::real GEOGRAPHICLIB_EXPORT
432  SphericalEngine::Value<true, SphericalEngine::FULL, 3>
433  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
434  template Math::real GEOGRAPHICLIB_EXPORT
435  SphericalEngine::Value<false, SphericalEngine::FULL, 3>
436  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
437  template Math::real GEOGRAPHICLIB_EXPORT
438  SphericalEngine::Value<true, SphericalEngine::SCHMIDT, 3>
439  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
440  template Math::real GEOGRAPHICLIB_EXPORT
441  SphericalEngine::Value<false, SphericalEngine::SCHMIDT, 3>
442  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
443 
444  template CircularEngine GEOGRAPHICLIB_EXPORT
445  SphericalEngine::Circle<true, SphericalEngine::FULL, 1>
446  (const coeff[], const real[], real, real, real);
447  template CircularEngine GEOGRAPHICLIB_EXPORT
448  SphericalEngine::Circle<false, SphericalEngine::FULL, 1>
449  (const coeff[], const real[], real, real, real);
450  template CircularEngine GEOGRAPHICLIB_EXPORT
451  SphericalEngine::Circle<true, SphericalEngine::SCHMIDT, 1>
452  (const coeff[], const real[], real, real, real);
453  template CircularEngine GEOGRAPHICLIB_EXPORT
454  SphericalEngine::Circle<false, SphericalEngine::SCHMIDT, 1>
455  (const coeff[], const real[], real, real, real);
456 
457  template CircularEngine GEOGRAPHICLIB_EXPORT
458  SphericalEngine::Circle<true, SphericalEngine::FULL, 2>
459  (const coeff[], const real[], real, real, real);
460  template CircularEngine GEOGRAPHICLIB_EXPORT
461  SphericalEngine::Circle<false, SphericalEngine::FULL, 2>
462  (const coeff[], const real[], real, real, real);
463  template CircularEngine GEOGRAPHICLIB_EXPORT
464  SphericalEngine::Circle<true, SphericalEngine::SCHMIDT, 2>
465  (const coeff[], const real[], real, real, real);
466  template CircularEngine GEOGRAPHICLIB_EXPORT
467  SphericalEngine::Circle<false, SphericalEngine::SCHMIDT, 2>
468  (const coeff[], const real[], real, real, real);
469 
470  template CircularEngine GEOGRAPHICLIB_EXPORT
471  SphericalEngine::Circle<true, SphericalEngine::FULL, 3>
472  (const coeff[], const real[], real, real, real);
473  template CircularEngine GEOGRAPHICLIB_EXPORT
474  SphericalEngine::Circle<false, SphericalEngine::FULL, 3>
475  (const coeff[], const real[], real, real, real);
476  template CircularEngine GEOGRAPHICLIB_EXPORT
477  SphericalEngine::Circle<true, SphericalEngine::SCHMIDT, 3>
478  (const coeff[], const real[], real, real, real);
479  template CircularEngine GEOGRAPHICLIB_EXPORT
480  SphericalEngine::Circle<false, SphericalEngine::SCHMIDT, 3>
481  (const coeff[], const real[], real, real, real);
483 
484 } // namespace GeographicLib
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
align_3::q2
Point2 q2
Definition: testPose2.cpp:753
max
#define max(a, b)
Definition: datatypes.h:20
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:38
GEOGRAPHICLIB_EXPORT
#define GEOGRAPHICLIB_EXPORT
Definition: Constants.hpp:91
real
float real
Definition: datatypes.h:10
y
Scalar * y
Definition: level1_cplx_impl.h:102
int
return int(ret)+1
Utility.hpp
Header for GeographicLib::Utility class.
GeographicLib::SphericalEngine::sqrttable
static std::vector< real > & sqrttable()
Definition: SphericalEngine.cpp:147
v
ArrayXcf v
Definition: Cwise_arg.cpp:1
n
int n
Definition: BiCGSTAB_simple.cpp:1
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
R
Rot2 R(Rot2::fromAngle(0.1))
sqrt
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
Definition: ArrayCwiseUnaryOps.h:152
L
MatrixXd L
Definition: LLT_example.cpp:6
std
Definition: Half.h:150
N
#define N
Definition: gksort.c:12
mpfr::root
const mpreal root(const mpreal &x, unsigned long int k, mp_rnd_t r=mpreal::get_default_rnd())
Definition: mpreal.h:2194
GeographicLib::SphericalEngine::Circle
static CircularEngine Circle(const coeff c[], const real f[], real p, real z, real a)
Definition: SphericalEngine.cpp:298
B
Matrix< SCALARB, Dynamic, Dynamic > B
Definition: bench_gemm.cpp:36
a
Array33i a
Definition: Cwise_product.cpp:1
GeographicLib::SphericalEngine::coeff::readcoeffs
static void readcoeffs(std::istream &stream, int &N, int &M, std::vector< real > &C, std::vector< real > &S)
Definition: SphericalEngine.cpp:387
l
static const Line3 l(Rot3(), 1, 1)
GeographicLib::Math::hypot
static T hypot(T x, T y)
Definition: Math.hpp:243
GeographicLib::SphericalEngine::coeff
Package up coefficients for SphericalEngine.
Definition: SphericalEngine.hpp:99
GeographicLib::Math::sq
static T sq(T x)
Definition: Math.hpp:232
gtsam::symbol_shorthand::S
Key S(std::uint64_t j)
Definition: inference/Symbol.h:161
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:211
GeographicLib
Namespace for GeographicLib.
Definition: JacobiConformal.hpp:15
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1520
GeographicLib::Utility::str
static std::string str(T x, int p=-1)
Definition: Utility.hpp:276
gtsam.examples.SFMExample_bal.stream
stream
Definition: SFMExample_bal.py:33
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
GeographicLib::CircularEngine::SetCoeff
void SetCoeff(int m, real wc, real ws)
Definition: CircularEngine.hpp:91
CircularEngine.hpp
Header for GeographicLib::CircularEngine class.
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:37
GeographicLib::CircularEngine
Spherical harmonic sums for a circle.
Definition: CircularEngine.hpp:52
GeographicLib::GeographicErr
Exception handling for GeographicLib.
Definition: Constants.hpp:389
p
float * p
Definition: Tutorial_Map_using.cpp:9
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
scale
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 y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics scale
Definition: gnuplot_common_settings.hh:12
GeographicLib::SphericalEngine::Value
static Math::real Value(const coeff c[], const real f[], real x, real y, real z, real a, real &gradx, real &grady, real &gradz)
Definition: SphericalEngine.cpp:153
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
SphericalEngine.hpp
Header for GeographicLib::SphericalEngine class.
align_3::t
Point2 t(10, 10)

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:44:49