source/ecef-wgs84.cc

   1 /*
   2   Earth Centered Earth Fixed (ECEF) WGS84 Navigation Utility Copyright ©  2013-2017  Infinite Delta Corp
   3
   4   This program is free software: you can redistribute it and/or modify
   5   it under the terms of the GNU Lesser General Public License as published by
   6   the Free Software Foundation, either version 2 of the License, or
   7   (at your option) any later version.
   8
   9   This program is distributed in the hope that it will be useful,
  10   but WITHOUT ANY WARRANTY; without even the implied warranty of
  11   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12   GNU General Public License for more details.
  13
  14   You should have received a copy of the GNU Lesser General Public License
  15   along with this program.  If not, see <http://www.gnu.org/licenses/>.
  16
  17 */
  18
  19 #include <ecef-wgs84.h>
  20
  21 namespace ecef
  22 {
  23
  24 inline double myFunc(const vec v)
  25 {
  26   double g2 = sqr(wgs84Const::a);
  27   double h2 = sqr(wgs84Const::a * (1-1/wgs84Const::fr));
  28   return v.x/g2 + v.y/g2 + v.z/h2;
  29 }
  30
  31 int wgs84Line::trimToEllipsoid()
  32 {
  33   vec v(p[0] - p[1]), e(p[1]);
  34   double d = sqr(2*myFunc(v*e)) - 4*myFunc(v*v)*(myFunc(e*e)-1);
  35   if (d < 0) return 0;
  36   d = sqrt(d);
  37   double t = (-2*myFunc(v*e) + d) / (2*myFunc(v*v));
  38   p[0] = e + v*t;
  39   t = (-2*myFunc(v*e) - d) / (2*myFunc(v*v));
  40   p[1] = e + v*t;
  41   return (p[0] == p[1]) ? 1 : 2;
  42 }
  43 }