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