GeographicLib
1.21
|
00001 /** 00002 * \file LocalCartesian.cpp 00003 * \brief Implementation for GeographicLib::LocalCartesian class 00004 * 00005 * Copyright (c) Charles Karney (2008-2011) <charles@karney.com> and licensed 00006 * under the MIT/X11 License. For more information, see 00007 * http://geographiclib.sourceforge.net/ 00008 **********************************************************************/ 00009 00010 #include <GeographicLib/LocalCartesian.hpp> 00011 00012 #define GEOGRAPHICLIB_LOCALCARTESIAN_CPP \ 00013 "$Id: 4d15764c089e07855bf6db300271e18f4fa89624 $" 00014 00015 RCSID_DECL(GEOGRAPHICLIB_LOCALCARTESIAN_CPP) 00016 RCSID_DECL(GEOGRAPHICLIB_LOCALCARTESIAN_HPP) 00017 00018 namespace GeographicLib { 00019 00020 using namespace std; 00021 00022 void LocalCartesian::Reset(real lat0, real lon0, real h0) throw() { 00023 _lat0 = lat0; 00024 _lon0 = lon0 >= 180 ? lon0 - 360 : (lon0 < -180 ? lon0 + 360 : lon0); 00025 _h0 = h0; 00026 _earth.Forward(_lat0, _lon0, _h0, _x0, _y0, _z0); 00027 real 00028 phi = lat0 * Math::degree<real>(), 00029 sphi = sin(phi), 00030 cphi = abs(_lat0) == 90 ? 0 : cos(phi), 00031 lam = lon0 * Math::degree<real>(), 00032 slam = _lon0 == -180 ? 0 : sin(lam), 00033 clam = abs(_lon0) == 90 ? 0 : cos(lam); 00034 _earth.Rotation(sphi, cphi, slam, clam, _r); 00035 } 00036 00037 void LocalCartesian::MatrixMultiply(real M[dim2_]) const throw() { 00038 real t[dim2_]; 00039 copy(M, M + dim2_, t); 00040 for (size_t i = 0; i < dim2_; ++i) { 00041 size_t row = i / dim_, col = i % dim_; 00042 M[i] = _r[row] * t[col] + _r[row+3] * t[col+3] + _r[row+6] * t[col+6]; 00043 } 00044 } 00045 00046 void LocalCartesian::IntForward(real lat, real lon, real h, 00047 real& x, real& y, real& z, 00048 real M[dim2_]) const throw() { 00049 real xc, yc, zc; 00050 _earth.IntForward(lat, lon, h, xc, yc, zc, M); 00051 xc -= _x0; yc -= _y0; zc -= _z0; 00052 x = _r[0] * xc + _r[3] * yc + _r[6] * zc; 00053 y = _r[1] * xc + _r[4] * yc + _r[7] * zc; 00054 z = _r[2] * xc + _r[5] * yc + _r[8] * zc; 00055 if (M) 00056 MatrixMultiply(M); 00057 } 00058 00059 void LocalCartesian::IntReverse(real x, real y, real z, 00060 real& lat, real& lon, real& h, 00061 real M[dim2_]) const throw() { 00062 real 00063 xc = _x0 + _r[0] * x + _r[1] * y + _r[2] * z, 00064 yc = _y0 + _r[3] * x + _r[4] * y + _r[5] * z, 00065 zc = _z0 + _r[6] * x + _r[7] * y + _r[8] * z; 00066 _earth.IntReverse(xc, yc, zc, lat, lon, h, M); 00067 if (M) 00068 MatrixMultiply(M); 00069 } 00070 00071 } // namespace GeographicLib