mock/coordTrans/blh2xyz.cpp

//
// Academic License - for use in teaching, academic research, and meeting
// course requirements at degree granting institutions only.  Not for
// government, commercial, or other organizational use.
// File: blh2xyz.cpp
//
// MATLAB Coder version            : 5.3
// C/C++ source code generated on  : 04-Apr-2023 15:36:25
//

// Include Files
#include "blh2xyz.h"
#include <cmath>

// Function Definitions
//
// Example:
// (BLh)WGS84-(XYZ)WGS84
//  LAT = 40.9987167395335;
//  LON = 39.7652393428761;
//  h = 51.403;
//
// Arguments    : double LAT
//                double LON
//                double h
//                double *X
//                double *Y
//                double *Z
// Return Type  : void
//
void blh2xyz(double LAT, double LON, double h, double *X, double *Y, double *Z)
{
  double N;
  double a_tmp;
  double lat;
  double lon;
  //  WGS84
  lat = LAT * 3.1415926535897931 / 180.0;
  lon = LON * 3.1415926535897931 / 180.0;
  a_tmp = std::sin(lat);
  N = 6.378137E+6 / std::sqrt(1.0 - 0.0066943799902085318 * (a_tmp * a_tmp));
  lat = (N + h) * std::cos(lat);
  *X = lat * std::cos(lon);
  *Y = lat * std::sin(lon);
  *Z = (N * 0.9933056200097915 + h) * a_tmp;
}

//
// File trailer for blh2xyz.cpp
//
// [EOF]
//