src/LocalCartesian.cpp
Go to the documentation of this file.
1 
11 
12 namespace GeographicLib {
13 
14  using namespace std;
15 
17  _lat0 = Math::LatFix(lat0);
18  _lon0 = Math::AngNormalize(lon0);
19  _h0 = h0;
20  _earth.Forward(_lat0, _lon0, _h0, _x0, _y0, _z0);
21  real sphi, cphi, slam, clam;
22  Math::sincosd(_lat0, sphi, cphi);
23  Math::sincosd(_lon0, slam, clam);
24  Geocentric::Rotation(sphi, cphi, slam, clam, _r);
25  }
26 
27  void LocalCartesian::MatrixMultiply(real M[dim2_]) const {
28  // M = r' . M
29  real t[dim2_];
30  copy(M, M + dim2_, t);
31  for (size_t i = 0; i < dim2_; ++i) {
32  size_t row = i / dim_, col = i % dim_;
33  M[i] = _r[row] * t[col] + _r[row+3] * t[col+3] + _r[row+6] * t[col+6];
34  }
35  }
36 
38  real& x, real& y, real& z,
39  real M[dim2_]) const {
40  real xc, yc, zc;
41  _earth.IntForward(lat, lon, h, xc, yc, zc, M);
42  xc -= _x0; yc -= _y0; zc -= _z0;
43  x = _r[0] * xc + _r[3] * yc + _r[6] * zc;
44  y = _r[1] * xc + _r[4] * yc + _r[7] * zc;
45  z = _r[2] * xc + _r[5] * yc + _r[8] * zc;
46  if (M)
47  MatrixMultiply(M);
48  }
49 
51  real& lat, real& lon, real& h,
52  real M[dim2_]) const {
53  real
54  xc = _x0 + _r[0] * x + _r[1] * y + _r[2] * z,
55  yc = _y0 + _r[3] * x + _r[4] * y + _r[5] * z,
56  zc = _z0 + _r[6] * x + _r[7] * y + _r[8] * z;
57  _earth.IntReverse(xc, yc, zc, lat, lon, h, M);
58  if (M)
59  MatrixMultiply(M);
60  }
61 
62 } // namespace GeographicLib
static T AngNormalize(T x)
Definition: Math.hpp:440
void Reset(real lat0, real lon0, real h0=0)
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
Scalar * y
void IntForward(real lat, real lon, real h, real &x, real &y, real &z, real M[dim2_]) const
Header for GeographicLib::LocalCartesian class.
static const double lat
void MatrixMultiply(real M[dim2_]) const
static T LatFix(T x)
Definition: Math.hpp:467
const double h0
Definition: BFloat16.h:88
static void sincosd(T x, T &sinx, T &cosx)
Definition: Math.hpp:558
const double lat0
m row(1)
Definition: main.h:100
Namespace for GeographicLib.
const double lon0
const double h
static void Rotation(real sphi, real cphi, real slam, real clam, real M[dim2_])
m col(1)
static const double lon
int EIGEN_BLAS_FUNC() copy(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Definition: level1_impl.h:29
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
void IntReverse(real x, real y, real z, real &lat, real &lon, real &h, real M[dim2_]) const
Point2 t(10, 10)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:33