src/CircularEngine.cpp
Go to the documentation of this file.
1 
11 
12 namespace GeographicLib {
13 
14  using namespace std;
15 
17  real& gradx, real& grady, real& gradz) const
18  {
19  gradp = _gradp && gradp;
20  const vector<real>& root( SphericalEngine::sqrttable() );
21 
22  // Initialize outer sum
23  real vc = 0, vc2 = 0, vs = 0, vs2 = 0; // v [N + 1], v [N + 2]
24  // vr, vt, vl and similar w variable accumulate the sums for the
25  // derivatives wrt r, theta, and lambda, respectively.
26  real vrc = 0, vrc2 = 0, vrs = 0, vrs2 = 0; // vr[N + 1], vr[N + 2]
27  real vtc = 0, vtc2 = 0, vts = 0, vts2 = 0; // vt[N + 1], vt[N + 2]
28  real vlc = 0, vlc2 = 0, vls = 0, vls2 = 0; // vl[N + 1], vl[N + 2]
29  for (int m = _M; m >= 0; --m) { // m = M .. 0
30  // Now Sc[m] = wc, Ss[m] = ws
31  // Sc'[m] = wtc, Ss'[m] = wtc
32  if (m) {
33  real v, A, B; // alpha[m], beta[m + 1]
34  switch (_norm) {
35  case FULL:
36  v = root[2] * root[2 * m + 3] / root[m + 1];
37  A = cl * v * _uq;
38  B = - v * root[2 * m + 5] / (root[8] * root[m + 2]) * _uq2;
39  break;
40  case SCHMIDT:
41  v = root[2] * root[2 * m + 1] / root[m + 1];
42  A = cl * v * _uq;
43  B = - v * root[2 * m + 3] / (root[8] * root[m + 2]) * _uq2;
44  break;
45  default:
46  A = B = 0;
47  }
48  v = A * vc + B * vc2 + _wc[m] ; vc2 = vc ; vc = v;
49  v = A * vs + B * vs2 + _ws[m] ; vs2 = vs ; vs = v;
50  if (gradp) {
51  v = A * vrc + B * vrc2 + _wrc[m]; vrc2 = vrc; vrc = v;
52  v = A * vrs + B * vrs2 + _wrs[m]; vrs2 = vrs; vrs = v;
53  v = A * vtc + B * vtc2 + _wtc[m]; vtc2 = vtc; vtc = v;
54  v = A * vts + B * vts2 + _wts[m]; vts2 = vts; vts = v;
55  v = A * vlc + B * vlc2 + m*_ws[m]; vlc2 = vlc; vlc = v;
56  v = A * vls + B * vls2 - m*_wc[m]; vls2 = vls; vls = v;
57  }
58  } else {
59  real A, B, qs;
60  switch (_norm) {
61  case FULL:
62  A = root[3] * _uq; // F[1]/(q*cl) or F[1]/(q*sl)
63  B = - root[15]/2 * _uq2; // beta[1]/q
64  break;
65  case SCHMIDT:
66  A = _uq;
67  B = - root[3]/2 * _uq2;
68  break;
69  default:
70  A = B = 0;
71  }
72  qs = _q / SphericalEngine::scale();
73  vc = qs * (_wc[m] + A * (cl * vc + sl * vs ) + B * vc2);
74  if (gradp) {
75  qs /= _r;
76  // The components of the gradient in circular coordinates are
77  // r: dV/dr
78  // theta: 1/r * dV/dtheta
79  // lambda: 1/(r*u) * dV/dlambda
80  vrc = - qs * (_wrc[m] + A * (cl * vrc + sl * vrs) + B * vrc2);
81  vtc = qs * (_wtc[m] + A * (cl * vtc + sl * vts) + B * vtc2);
82  vlc = qs / _u * ( A * (cl * vlc + sl * vls) + B * vlc2);
83  }
84  }
85  }
86 
87  if (gradp) {
88  // Rotate into cartesian (geocentric) coordinates
89  gradx = cl * (_u * vrc + _t * vtc) - sl * vlc;
90  grady = sl * (_u * vrc + _t * vtc) + cl * vlc;
91  gradz = _t * vrc - _u * vtc ;
92  }
93  return vc;
94  }
95 
96 } // namespace GeographicLib
GeographicLib::SphericalEngine::sqrttable
static std::vector< real > & sqrttable()
Definition: SphericalEngine.cpp:147
B
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
GeographicLib
Namespace for GeographicLib.
Definition: JacobiConformal.hpp:15
CircularEngine.hpp
Header for GeographicLib::CircularEngine class.
GeographicLib::SphericalEngine::scale
static real scale()
Definition: SphericalEngine.hpp:49
test_cases::vs
std::vector< Vector3 > vs
Definition: testPose3.cpp:880
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
GeographicLib::Math::real
double real
Definition: Math.hpp:129
GeographicLib::CircularEngine::Value
Math::real Value(bool gradp, real sl, real cl, real &gradx, real &grady, real &gradz) const
Definition: src/CircularEngine.cpp:16
cl
Definition: cxx11_tensor_builtins_sycl.cpp:30
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
std
Definition: BFloat16.h:88
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
real
Definition: main.h:100


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:15