GeographicLib  1.21
LocalCartesian.cpp
Go to the documentation of this file.
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