00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include <GeographicLib/LocalCartesian.hpp>
00011
00012 namespace GeographicLib {
00013
00014 using namespace std;
00015
00016 void LocalCartesian::Reset(real lat0, real lon0, real h0) {
00017 _lat0 = lat0;
00018 _lon0 = Math::AngNormalize(lon0);
00019 _h0 = h0;
00020 _earth.Forward(_lat0, _lon0, _h0, _x0, _y0, _z0);
00021 real
00022 phi = lat0 * Math::degree(),
00023 sphi = sin(phi),
00024 cphi = abs(_lat0) == 90 ? 0 : cos(phi),
00025 lam = lon0 * Math::degree(),
00026 slam = _lon0 == -180 ? 0 : sin(lam),
00027 clam = abs(_lon0) == 90 ? 0 : cos(lam);
00028 _earth.Rotation(sphi, cphi, slam, clam, _r);
00029 }
00030
00031 void LocalCartesian::MatrixMultiply(real M[dim2_]) const {
00032 real t[dim2_];
00033 copy(M, M + dim2_, t);
00034 for (size_t i = 0; i < dim2_; ++i) {
00035 size_t row = i / dim_, col = i % dim_;
00036 M[i] = _r[row] * t[col] + _r[row+3] * t[col+3] + _r[row+6] * t[col+6];
00037 }
00038 }
00039
00040 void LocalCartesian::IntForward(real lat, real lon, real h,
00041 real& x, real& y, real& z,
00042 real M[dim2_]) const {
00043 real xc, yc, zc;
00044 _earth.IntForward(lat, lon, h, xc, yc, zc, M);
00045 xc -= _x0; yc -= _y0; zc -= _z0;
00046 x = _r[0] * xc + _r[3] * yc + _r[6] * zc;
00047 y = _r[1] * xc + _r[4] * yc + _r[7] * zc;
00048 z = _r[2] * xc + _r[5] * yc + _r[8] * zc;
00049 if (M)
00050 MatrixMultiply(M);
00051 }
00052
00053 void LocalCartesian::IntReverse(real x, real y, real z,
00054 real& lat, real& lon, real& h,
00055 real M[dim2_]) const {
00056 real
00057 xc = _x0 + _r[0] * x + _r[1] * y + _r[2] * z,
00058 yc = _y0 + _r[3] * x + _r[4] * y + _r[5] * z,
00059 zc = _z0 + _r[6] * x + _r[7] * y + _r[8] * z;
00060 _earth.IntReverse(xc, yc, zc, lat, lon, h, M);
00061 if (M)
00062 MatrixMultiply(M);
00063 }
00064
00065 }