SphericalEngine.cpp
Go to the documentation of this file.
1 
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>
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>
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 
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));
398  Utility::readarray<double, real, false>(stream, C);
400  Utility::readarray<double, real, false>(stream, S);
401  return;
402  }
403 
406  SphericalEngine::Value<true, SphericalEngine::FULL, 1>
407  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
409  SphericalEngine::Value<false, SphericalEngine::FULL, 1>
410  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
412  SphericalEngine::Value<true, SphericalEngine::SCHMIDT, 1>
413  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
415  SphericalEngine::Value<false, SphericalEngine::SCHMIDT, 1>
416  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
417 
419  SphericalEngine::Value<true, SphericalEngine::FULL, 2>
420  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
422  SphericalEngine::Value<false, SphericalEngine::FULL, 2>
423  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
425  SphericalEngine::Value<true, SphericalEngine::SCHMIDT, 2>
426  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
428  SphericalEngine::Value<false, SphericalEngine::SCHMIDT, 2>
429  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
430 
432  SphericalEngine::Value<true, SphericalEngine::FULL, 3>
433  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
435  SphericalEngine::Value<false, SphericalEngine::FULL, 3>
436  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
438  SphericalEngine::Value<true, SphericalEngine::SCHMIDT, 3>
439  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
441  SphericalEngine::Value<false, SphericalEngine::SCHMIDT, 3>
442  (const coeff[], const real[], real, real, real, real, real&, real&, real&);
443 
445  SphericalEngine::Circle<true, SphericalEngine::FULL, 1>
446  (const coeff[], const real[], real, real, real);
448  SphericalEngine::Circle<false, SphericalEngine::FULL, 1>
449  (const coeff[], const real[], real, real, real);
451  SphericalEngine::Circle<true, SphericalEngine::SCHMIDT, 1>
452  (const coeff[], const real[], real, real, real);
454  SphericalEngine::Circle<false, SphericalEngine::SCHMIDT, 1>
455  (const coeff[], const real[], real, real, real);
456 
458  SphericalEngine::Circle<true, SphericalEngine::FULL, 2>
459  (const coeff[], const real[], real, real, real);
461  SphericalEngine::Circle<false, SphericalEngine::FULL, 2>
462  (const coeff[], const real[], real, real, real);
464  SphericalEngine::Circle<true, SphericalEngine::SCHMIDT, 2>
465  (const coeff[], const real[], real, real, real);
467  SphericalEngine::Circle<false, SphericalEngine::SCHMIDT, 2>
468  (const coeff[], const real[], real, real, real);
469 
471  SphericalEngine::Circle<true, SphericalEngine::FULL, 3>
472  (const coeff[], const real[], real, real, real);
474  SphericalEngine::Circle<false, SphericalEngine::FULL, 3>
475  (const coeff[], const real[], real, real, real);
477  SphericalEngine::Circle<true, SphericalEngine::SCHMIDT, 3>
478  (const coeff[], const real[], real, real, real);
480  SphericalEngine::Circle<false, SphericalEngine::SCHMIDT, 3>
481  (const coeff[], const real[], real, real, real);
483 
484 } // namespace GeographicLib
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
gtsam.examples.DogLegOptimizerExample.int
int
Definition: DogLegOptimizerExample.py:111
GeographicLib::SphericalEngine::sqrttable
static std::vector< real > & sqrttable()
Definition: SphericalEngine.cpp:147
SphericalEngine.hpp
Header for GeographicLib::SphericalEngine class.
B
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
GeographicLib::Utility::str
static std::string str(T x, int p=-1)
Definition: Utility.hpp:276
GeographicLib
Namespace for GeographicLib.
Definition: JacobiConformal.hpp:15
gtsam.examples.SFMExample_bal.stream
stream
Definition: SFMExample_bal.py:24
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
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
CircularEngine.hpp
Header for GeographicLib::CircularEngine class.
real
float real
Definition: datatypes.h:10
GEOGRAPHICLIB_EXPORT
#define GEOGRAPHICLIB_EXPORT
Definition: Constants.hpp:91
Eigen::PlainObjectBase::resize
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index rows, Index cols)
Definition: PlainObjectBase.h:271
GeographicLib::SphericalEngine::Circle
static CircularEngine Circle(const coeff c[], const real f[], real p, real z, real a)
Definition: SphericalEngine.cpp:298
GeographicLib::SphericalEngine::coeff::Ssize
static int Ssize(int N, int M)
Definition: SphericalEngine.hpp:247
GeographicLib::GeographicErr
Exception handling for GeographicLib.
Definition: Constants.hpp:389
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
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
n
int n
Definition: BiCGSTAB_simple.cpp:1
GeographicLib::Math::real
double real
Definition: Math.hpp:129
GeographicLib::SphericalEngine::RootTable
static void RootTable(int N)
Definition: SphericalEngine.cpp:376
GeographicLib::SphericalEngine::coeff
Package up coefficients for SphericalEngine.
Definition: SphericalEngine.hpp:99
GeographicLib::CircularEngine::SetCoeff
void SetCoeff(int m, real wc, real ws)
Definition: CircularEngine.hpp:91
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:54
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
GeographicLib::CircularEngine
Spherical harmonic sums for a circle.
Definition: CircularEngine.hpp:52
l
static const Line3 l(Rot3(), 1, 1)
Utility.hpp
Header for GeographicLib::Utility class.
cl
Definition: cxx11_tensor_builtins_sycl.cpp:30
L
MatrixXd L
Definition: LLT_example.cpp:6
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
y
Scalar * y
Definition: level1_cplx_impl.h:124
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
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
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
N
#define N
Definition: igam.h:9
GeographicLib::SphericalEngine::coeff::Csize
static int Csize(int N, int M)
Definition: SphericalEngine.hpp:236
GeographicLib::Math::sq
static T sq(T x)
Definition: Math.hpp:232
align_3::t
Point2 t(10, 10)
max
#define max(a, b)
Definition: datatypes.h:20
GeographicLib::Math::hypot
static T hypot(T x, T y)
Definition: Math.hpp:243
real
Definition: main.h:100
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
R
Rot2 R(Rot2::fromAngle(0.1))
S
DiscreteKey S(1, 2)
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:36:23