geodesicinverse.cpp
Go to the documentation of this file.
1 
10 // Compile in Matlab with
11 // [Unix]
12 // mex -I/usr/local/include -L/usr/local/lib -Wl,-rpath=/usr/local/lib
13 // -lGeographic geodesicinverse.cpp
14 // [Windows]
15 // mex -I../include -L../windows/Release
16 // -lGeographic geodesicinverse.cpp
17 
18 #include <algorithm>
21 #include <mex.h>
22 
23 using namespace std;
24 using namespace GeographicLib;
25 
26 template<class G> void
27 compute(double a, double f, mwSize m, const double* latlong,
28  double* geodesic, double* aux) {
29  const double* lat1 = latlong;
30  const double* lon1 = latlong + m;
31  const double* lat2 = latlong + 2*m;
32  const double* lon2 = latlong + 3*m;
33  double* azi1 = geodesic;
34  double* azi2 = geodesic + m;
35  double* s12 = geodesic + 2*m;
36  double* a12 = NULL;
37  double* m12 = NULL;
38  double* M12 = NULL;
39  double* M21 = NULL;
40  double* S12 = NULL;
41  if (aux) {
42  a12 = aux;
43  m12 = aux + m;
44  M12 = aux + 2*m;
45  M21 = aux + 3*m;
46  S12 = aux + 4*m;
47  }
48 
49  const G g(a, f);
50  for (mwIndex i = 0; i < m; ++i) {
51  if (abs(lat1[i]) <= 90 && lon1[i] >= -540 && lon1[i] < 540 &&
52  abs(lat2[i]) <= 90 && lon2[i] >= -540 && lon2[i] < 540) {
53  if (aux)
54  a12[i] = g.Inverse(lat1[i], lon1[i], lat2[i], lon2[i],
55  s12[i], azi1[i], azi2[i],
56  m12[i], M12[i], M21[i], S12[i]);
57  else
58  g.Inverse(lat1[i], lon1[i], lat2[i], lon2[i],
59  s12[i], azi1[i], azi2[i]);
60  }
61  }
62 }
63 
64 void mexFunction( int nlhs, mxArray* plhs[],
65  int nrhs, const mxArray* prhs[] ) {
66 
67  if (nrhs < 1)
68  mexErrMsgTxt("One input argument required.");
69  else if (nrhs > 3)
70  mexErrMsgTxt("More than three input arguments specified.");
71  else if (nrhs == 2)
72  mexErrMsgTxt("Must specify flattening with the equatorial radius.");
73  else if (nlhs > 2)
74  mexErrMsgTxt("More than two output arguments specified.");
75 
76  if (!( mxIsDouble(prhs[0]) && !mxIsComplex(prhs[0]) ))
77  mexErrMsgTxt("latlong coordinates are not of type double.");
78 
79  if (mxGetN(prhs[0]) != 4)
80  mexErrMsgTxt("latlong coordinates must be M x 4 matrix.");
81 
82  double a = Constants::WGS84_a<double>(), f = Constants::WGS84_f<double>();
83  if (nrhs == 3) {
84  if (!( mxIsDouble(prhs[1]) && !mxIsComplex(prhs[1]) &&
85  mxGetNumberOfElements(prhs[1]) == 1 ))
86  mexErrMsgTxt("Equatorial radius is not a real scalar.");
87  a = mxGetScalar(prhs[1]);
88  if (!( mxIsDouble(prhs[2]) && !mxIsComplex(prhs[2]) &&
89  mxGetNumberOfElements(prhs[2]) == 1 ))
90  mexErrMsgTxt("Flattening is not a real scalar.");
91  f = mxGetScalar(prhs[2]);
92  }
93 
94  mwSize m = mxGetM(prhs[0]);
95 
96  const double* latlong = mxGetPr(prhs[0]);
97 
98  double* geodesic = mxGetPr(plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL));
99  std::fill(geodesic, geodesic + 3*m, Math::NaN<double>());
100 
101  double* aux =
102  nlhs == 2 ? mxGetPr(plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL)) :
103  NULL;
104  if (aux)
105  std::fill(aux, aux + 5*m, Math::NaN<double>());
106 
107  try {
108  if (std::abs(f) <= 0.02)
109  compute<Geodesic>(a, f, m, latlong, geodesic, aux);
110  else
111  compute<GeodesicExact>(a, f, m, latlong, geodesic, aux);
112  }
113  catch (const std::exception& e) {
114  mexErrMsgTxt(e.what());
115  }
116 }
Matrix3f m
JacobiRotation< float > G
Definition: BFloat16.h:88
void g(const string &key, int i)
Definition: testBTree.cpp:41
Header for GeographicLib::Geodesic class.
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Namespace for GeographicLib.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
#define NULL
Definition: ccolamd.c:609
Header for GeographicLib::GeodesicExact class.
void compute(double a, double f, mwSize m, const double *latlong, double *geodesic, double *aux)
#define abs(x)
Definition: datatypes.h:17


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:18