diff --git a/.github/workflows/publish-to-pypi.yml b/.github/workflows/publish-to-pypi.yml index 9352b7d..6f8560c 100644 --- a/.github/workflows/publish-to-pypi.yml +++ b/.github/workflows/publish-to-pypi.yml @@ -24,7 +24,6 @@ jobs: python3 -m pip install numpy python3 -m pip install setuptools python3 -m pip install setuptools-scm - python3 -m pip install numpy python3 setup.py sdist - name: Store the distribution packages uses: actions/upload-artifact@v3 diff --git a/.github/workflows/workflow.yml b/.github/workflows/workflow.yml index b7b0abe..79f9832 100644 --- a/.github/workflows/workflow.yml +++ b/.github/workflows/workflow.yml @@ -33,7 +33,7 @@ jobs: - name: Install dependencies run: | python -m pip install --upgrade pip - python -m pip install .[dev] + python -m pip install .[tests] - name: Generate coverage report run: python -m pytest --cov=transforms84 --cov-report=xml:pytest_cov.xml - name: Upload coverage to Codecov diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 5d20c1f..dbf685e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -12,13 +12,13 @@ repos: - id: debug-statements language_version: python3 -# - repo: https://github.com/cpp-linter/cpp-linter-hooks -# rev: v0.5.1 -# hooks: -# - id: clang-format -# args: [--style=Google] # Other coding style: LLVM, GNU, Chromium, Microsoft, Mozilla, WebKit. -# - id: clang-tidy -# args: [--checks='boost-*,bugprone-*,performance-*,readability-*,portability-*,modernize-*,clang-analyzer-*,cppcoreguidelines-*'] +- repo: https://github.com/cpp-linter/cpp-linter-hooks + rev: v0.5.1 + hooks: + - id: clang-format + args: [--style=WebKit] # Other coding style: LLVM, GNU, Chromium, Microsoft, Mozilla, WebKit. + # - id: clang-tidy + # args: [--checks='boost-*,bugprone-*,performance-*,readability-*,portability-*,modernize-*,clang-analyzer-*,cppcoreguidelines-*'] - repo: https://github.com/asottile/pyupgrade rev: v3.4.0 diff --git a/README.md b/README.md index 331b54a..33dcf41 100644 --- a/README.md +++ b/README.md @@ -6,12 +6,14 @@ Small geographic coordinate systems Python library with a few additional helper functions. +This package focuses on: +1. Performance +2. Input and output coordinates of ideal mathematical shapes. Ideally, all coordinates should be of shapes (3,1) or (nPoints,3,1), but shapes (3,) and (nPoints,3) are supported too. +3. Functions that adapt to differing input matrices shapes: one-to-one, many-to-many and one-to-many points. See [below](#many-to-many--one-to-many) for an example. + ## Installation `pip install transforms84` -## Examples -See the Jupyter notebooks in [examples](examples) to see how to use the transform84. Run `pip install transforms84[examples]` to run the examples locally. - ## Operations ### Transformations The following transformations have been implemented: @@ -32,5 +34,49 @@ The following functions have been implemented: - [rad, rad, X] → [deg, deg, X] - [deg, deg, X] → [rad, rad, X] +## Examples +See the Jupyter notebooks in [examples](examples) to see how to use the transform84. Run `pip install transforms84[examples]` to run the examples locally. + +### Many-to-many & one-to-many +The `transforms.ECEF2ENU` transformation accepts same and differing matrix shape sizes. Below showcases the many-to-many method where three target points, `rrm_target`, in the geodetic coordinate system ([WGS84](https://en.wikipedia.org/wiki/World_Geodetic_System)) are transformed to the local ENU coordinate system about the point `rrm_local`, where both matrices are of shape (3, 3, 1): +``` +>> import numpy as np +>> from transforms84.systems import WGS84 +>> from transforms84.helpers import DDM2RRM +>> from transforms84.transforms import ECEF2ENU, geodetic2ECEF + +>> rrm_local = DDM2RRM(np.array([[[30], [31], [0]], [[30], [31], [0]], [[30], [31], [0]]], dtype=np.float64)) # convert each point from [deg, deg, X] to [rad, rad, X] +>> rrm_target = DDM2RRM(np.array([[[31], [32], [0]], [[31], [32], [0]], [[31], [32], [0]]], dtype=np.float64)) +>> ECEF2ENU(rrm_local, geodetic2ECEF(rrm_target, WGS84.a, WGS84.b), WGS84.a, WGS84.b) # geodetic2ECEF -> ECEF2ENU +array([[[ 4.06379074e+01], + [-6.60007585e-01], + [ 1.46643956e+05]], + + [[ 4.06379074e+01], + [-6.60007585e-01], + [ 1.46643956e+05]], + + [[ 4.06379074e+01], + [-6.60007585e-01], + [ 1.46643956e+05]]]) +``` + +We can achieve the same result using the one-to-many method with a single local point of shape (3, 1): +``` +>> rrm_local = DDM2RRM(np.array([[30], [31], [0]], dtype=np.float64)) +>> ECEF2ENU(rrm_local, geodetic2ECEF(rrm_target, WGS84.a, WGS84.b), WGS84.a, WGS84.b) +array([[[ 4.06379074e+01], + [-6.60007585e-01], + [ 1.46643956e+05]], + + [[ 4.06379074e+01], + [-6.60007585e-01], + [ 1.46643956e+05]], + + [[ 4.06379074e+01], + [-6.60007585e-01], + [ 1.46643956e+05]]]) +``` + ## Contributing -PRs are always welcome and appreciated! +PRs are always welcome and appreciated! Please install the pre-commit hooks before making commits. diff --git a/distances.c b/distances.c deleted file mode 100644 index e10ca3e..0000000 --- a/distances.c +++ /dev/null @@ -1,130 +0,0 @@ -#include -#include - -/* -Calculate the Haversine distance between two points in double precision. -https://en.wikipedia.org/wiki/Haversine_formula#Formulation - -@param float *rrmStart array of size nx3 of start point azimuth, elevation, range [rad, rad, m] -@param float *rrmEnd array of size nx3 of start point azimuth, elevation, range [rad, rad, m] -@param size_t nPoints Number of target points -@param double mRadiusSphere Radius of sphere in metres -@param float *mRadiusSphere array of size nx3 of distance between start and end points -*/ -void HaversineDouble(const double *rrmStart, const double *rrmEnd, int nPoints, double mRadiusSphere, double *mDistance) -{ - int i; - for (int iPoint = 0; iPoint < nPoints; ++iPoint) - { - i = iPoint * 3; - mDistance[iPoint] = 2 * mRadiusSphere * asin(sqrt((1 - cos(rrmEnd[i + 0] - rrmStart[i + 0]) + cos(rrmStart[i + 0]) * cos(rrmEnd[i + 0]) * (1 - cos(rrmEnd[i + 1] - rrmStart[i + 1]))) / 2)); - } -} - -/* -Calculate the Haversine distance between two points in float precision. -https://en.wikipedia.org/wiki/Haversine_formula#Formulation - -@param float *rrmStart array of size nx3 of start point azimuth, elevation, range [rad, rad, m] -@param float *rrmEnd array of size nx3 of start point azimuth, elevation, range [rad, rad, m] -@param size_t nPoints Number of target points -@param double mRadiusSphere Radius of sphere in metres -@param float *mRadiusSphere array of size nx3 of distance between start and end points -*/ -void HaversineFloat(const float *rrmStart, const float *rrmEnd, int nPoints, float mRadiusSphere, float *mDistance) -{ - int i; - for (int iPoint = 0; iPoint < nPoints; ++iPoint) - { - i = iPoint * 3; - mDistance[iPoint] = 2 * mRadiusSphere * asin(sqrt((1 - cos(rrmEnd[i + 0] - rrmStart[i + 0]) + cos(rrmStart[i + 0]) * cos(rrmEnd[i + 0]) * (1 - cos(rrmEnd[i + 1] - rrmStart[i + 1]))) / 2)); - } -} - -static PyObject *HaversineWrapper(PyObject *self, PyObject *args) -{ - PyArrayObject *rrmStart, *rrmEnd; - double mRadiusSphere; - - // Parse the input tuple - if (!PyArg_ParseTuple(args, "O!O!d", &PyArray_Type, &rrmStart, &PyArray_Type, &rrmEnd, &mRadiusSphere)) - return NULL; - - - // checks - if (!(PyArray_ISCONTIGUOUS(rrmStart)) || !(PyArray_ISCONTIGUOUS(rrmEnd))) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); - return NULL; - } - if (PyArray_NDIM(rrmStart) != PyArray_NDIM(rrmEnd)) - { - PyErr_SetString(PyExc_ValueError, "Input arrays have non-matching dimensions."); - return NULL; - } - if (PyArray_SIZE(rrmStart) != PyArray_SIZE(rrmEnd)) - { - PyErr_SetString(PyExc_ValueError, "Input arrays are of unequal size."); - return NULL; - } - if ((PyArray_SIZE(rrmStart) % 3) != 0 || (PyArray_SIZE(rrmEnd) % 3) != 0) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a multiple of 3."); - return NULL; - } - if (PyArray_TYPE(rrmStart) != PyArray_TYPE(rrmEnd)) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must have the same type."); - return NULL; - } - - npy_intp nPoints = PyArray_SIZE(rrmStart) / 3; - PyObject *result_array = PyArray_SimpleNew(1, &nPoints, PyArray_TYPE(rrmEnd)); - if (result_array == NULL) - return NULL; - if (PyArray_TYPE(rrmEnd) == NPY_DOUBLE) - { - double *data1 = (double *)PyArray_DATA(rrmStart); - double *data2 = (double *)PyArray_DATA(rrmEnd); - double *result_data = (double *)PyArray_DATA((PyArrayObject *)result_array); - HaversineDouble(data1, data2, nPoints, mRadiusSphere, result_data); - } - else if (PyArray_TYPE(rrmEnd) == NPY_FLOAT) - { - float *data1 = (float *)PyArray_DATA(rrmStart); - float *data2 = (float *)PyArray_DATA(rrmEnd); - float *result_data = (float *)PyArray_DATA((PyArrayObject *)result_array); - HaversineFloat(data1, data2, nPoints, mRadiusSphere, result_data); - } - else - { - PyErr_SetString(PyExc_ValueError, "Only 32 and 64 bit float types accepted."); - return NULL; - } - return result_array; -} - -// Method definition object for this extension, these argumens mean: -// ml_name: The name of the method -// ml_meth: Function pointer to the method implementation -// ml_flags: Flags indicating special features of this method, such as -// accepting arguments, accepting keyword arguments, being a class method, or being a static method of a class. -// ml_doc: The docstring for the method -static PyMethodDef MyMethods[] = { - {"Haversine", HaversineWrapper, METH_VARARGS, "Haversine function"}, - {NULL, NULL, 0, NULL}}; - -// Module definition -static struct PyModuleDef distances = { - PyModuleDef_HEAD_INIT, - "distances", - "Module that contains functions to calculate distances between points", - -1, - MyMethods}; - -// Module initialization function -PyMODINIT_FUNC PyInit_distances(void) -{ - import_array(); // Initialize the NumPy C API - return PyModule_Create(&distances); -} diff --git a/examples/example1.ipynb b/examples/example1.ipynb index 71e6b0a..d881f0d 100644 --- a/examples/example1.ipynb +++ b/examples/example1.ipynb @@ -9,13 +9,13 @@ "import numpy as np\n", "\n", "from transforms84.distances import Haversine\n", - "from transforms84.geodetic_systems import WGS84\n", "from transforms84.helpers import (\n", " DDM2RRM,\n", " RRM2DDM,\n", " deg_angular_difference,\n", " deg_angular_differences,\n", ")\n", + "from transforms84.systems import WGS84\n", "from transforms84.transforms import ECEF2ENU, ENU2AER, geodetic2ECEF" ] }, @@ -84,6 +84,13 @@ "ddm_local2target" ] }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "If we duplicate the local and target points three times each we will get an output of three local azimuth-elevation-range coordinates." + ] + }, { "cell_type": "code", "execution_count": 5, @@ -92,17 +99,17 @@ { "data": { "text/plain": [ - "array([[[0.52359878],\n", - " [0.54105207],\n", - " [0. ]],\n", + "array([[[ 4.06379074e+01],\n", + " [-6.60007585e-01],\n", + " [ 1.46643956e+05]],\n", "\n", - " [[0.52359878],\n", - " [0.54105207],\n", - " [0. ]],\n", + " [[ 4.06379074e+01],\n", + " [-6.60007585e-01],\n", + " [ 1.46643956e+05]],\n", "\n", - " [[0.52359878],\n", - " [0.54105207],\n", - " [0. ]]])" + " [[ 4.06379074e+01],\n", + " [-6.60007585e-01],\n", + " [ 1.46643956e+05]]])" ] }, "execution_count": 5, @@ -111,57 +118,34 @@ } ], "source": [ - "np.tile(rrm_local, 3).T.reshape((-1, 3, 1))" + "num_repeats = 3\n", + "rrm_targets = np.ascontiguousarray(\n", + " np.tile(rrm_target, num_repeats).T.reshape((-1, 3, 1))\n", + ")\n", + "rrm_locals = np.ascontiguousarray(\n", + " np.tile(rrm_local, rrm_targets.shape[0]).T.reshape((-1, 3, 1))\n", + ")\n", + "ddm_local2targets = RRM2DDM(\n", + " ENU2AER(\n", + " ECEF2ENU(\n", + " rrm_locals, geodetic2ECEF(rrm_targets, WGS84.a, WGS84.b), WGS84.a, WGS84.b\n", + " )\n", + " )\n", + ")\n", + "assert np.all(np.unique(ddm_local2targets, axis=0)[0] == ddm_local2target)\n", + "ddm_local2targets" ] }, { - "cell_type": "code", - "execution_count": 6, + "cell_type": "markdown", "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "(array([[[31.],\n", - " [32.],\n", - " [ 0.]],\n", - " \n", - " [[31.],\n", - " [32.],\n", - " [ 0.]],\n", - " \n", - " [[31.],\n", - " [32.],\n", - " [ 0.]]]),\n", - " array([[[0.54105207],\n", - " [0.55850536],\n", - " [0. ]],\n", - " \n", - " [[0.54105207],\n", - " [0.55850536],\n", - " [0. ]],\n", - " \n", - " [[0.54105207],\n", - " [0.55850536],\n", - " [0. ]]]))" - ] - }, - "execution_count": 6, - "metadata": {}, - "output_type": "execute_result" - } - ], "source": [ - "rrm_targets = np.ascontiguousarray(np.tile(rrm_target, 3).T.reshape((-1, 3, 1)))\n", - "rrm_locals = np.ascontiguousarray(\n", - " np.tile(rrm_local, rrm_targets.shape[0]).T.reshape((-1, 3, 1))\n", - ")\n", - "RRM2DDM(rrm_targets), rrm_targets" + "Alternatively, we can keep the origin point fixed to one point and just duplicate the target points three times." ] }, { "cell_type": "code", - "execution_count": 7, + "execution_count": 6, "metadata": {}, "outputs": [ { @@ -180,21 +164,21 @@ " [ 1.46643956e+05]]])" ] }, - "execution_count": 7, + "execution_count": 6, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "ddm_local2targets = RRM2DDM(\n", + "ddm_local2targets1 = RRM2DDM(\n", " ENU2AER(\n", " ECEF2ENU(\n", - " rrm_locals, geodetic2ECEF(rrm_targets, WGS84.a, WGS84.b), WGS84.a, WGS84.b\n", + " rrm_local, geodetic2ECEF(rrm_targets, WGS84.a, WGS84.b), WGS84.a, WGS84.b\n", " )\n", " )\n", ")\n", - "assert np.all(np.unique(ddm_local2targets, axis=0)[0] == ddm_local2target)\n", - "ddm_local2targets" + "assert np.all(np.unique(ddm_local2targets1, axis=0)[0] == ddm_local2target)\n", + "ddm_local2targets1" ] }, { @@ -207,7 +191,7 @@ }, { "cell_type": "code", - "execution_count": 8, + "execution_count": 7, "metadata": {}, "outputs": [ { @@ -216,7 +200,7 @@ "(array([146775.8833037]), array([131.92690735]))" ] }, - "execution_count": 8, + "execution_count": 7, "metadata": {}, "output_type": "execute_result" } @@ -226,6 +210,28 @@ "m_distance, m_distance - ddm_local2target[2, 0]" ] }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "array([146775.77, 146775.77, 146775.77], dtype=float32)" + ] + }, + "execution_count": 8, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "Haversine(\n", + " rrm_locals.astype(np.float32), rrm_targets.astype(np.float32), WGS84.mean_radius\n", + ")" + ] + }, { "cell_type": "code", "execution_count": 9, @@ -251,6 +257,27 @@ "cell_type": "code", "execution_count": 10, "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "array([146775.8833037, 146775.8833037, 146775.8833037])" + ] + }, + "execution_count": 10, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "m_distances2 = Haversine(rrm_local, rrm_targets, WGS84.mean_radius)\n", + "m_distances2" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": {}, "outputs": [ { "data": { @@ -260,7 +287,7 @@ " 146775.8833037, 146775.8833037])" ] }, - "execution_count": 10, + "execution_count": 11, "metadata": {}, "output_type": "execute_result" } @@ -276,7 +303,7 @@ }, { "cell_type": "code", - "execution_count": 11, + "execution_count": 12, "metadata": {}, "outputs": [ { @@ -285,7 +312,7 @@ "np.True_" ] }, - "execution_count": 11, + "execution_count": 12, "metadata": {}, "output_type": "execute_result" } @@ -303,7 +330,7 @@ }, { "cell_type": "code", - "execution_count": 12, + "execution_count": 13, "metadata": {}, "outputs": [ { @@ -312,7 +339,7 @@ "(20.0, 20.0, 340.0)" ] }, - "execution_count": 12, + "execution_count": 13, "metadata": {}, "output_type": "execute_result" } @@ -327,7 +354,7 @@ }, { "cell_type": "code", - "execution_count": 13, + "execution_count": 14, "metadata": {}, "outputs": [ { @@ -338,7 +365,7 @@ " array([340., 340.], dtype=float32))" ] }, - "execution_count": 13, + "execution_count": 14, "metadata": {}, "output_type": "execute_result" } diff --git a/examples/example2.ipynb b/examples/example2.ipynb index b30dc4a..a842d1b 100644 --- a/examples/example2.ipynb +++ b/examples/example2.ipynb @@ -11,8 +11,8 @@ "from czml3 import Document, Packet, Preamble\n", "from czml3.properties import Color, Material, Polygon, PositionList, SolidColorMaterial\n", "\n", - "from transforms84.geodetic_systems import WGS84\n", "from transforms84.helpers import DDM2RRM, RRM2DDM\n", + "from transforms84.systems import WGS84\n", "from transforms84.transforms import (\n", " AER2ENU,\n", " ENU2ECEF,\n", diff --git a/helpers.c b/helpers.c deleted file mode 100644 index c018338..0000000 --- a/helpers.c +++ /dev/null @@ -1,433 +0,0 @@ -#include -#include -#include -#define PI 3.14159265358979323846 - -/* -Calculate the angular difference between two numbers of double precision. - -@param double *AngleStart array of size n angles. This is the start angle if smallestAngle is false. -@param double *AngleEnd array of size n angles. This is the end angle if smallestAngle is false. -@param int nAngles Number of angles in array -@param int smallestAngle Whether to calculate the angular difference between the start and end angles or the smallest angle. -@param double Difference Angular difference -*/ -double AngularDifferenceDouble(const double AngleStart, const double AngleEnd, const double MaxValue, int smallestAngle) -{ - double Difference = fmod(fabs(AngleStart - AngleEnd), MaxValue); - if (smallestAngle) - Difference = fmin(Difference, MaxValue - Difference); - else if (AngleStart > AngleEnd) - Difference = MaxValue - Difference; - return Difference; -} - -/* -Calculate the angular difference between two numbers of float precision. - -@param float AngleStart angle. This is the start angle if smallestAngle is false. -@param float AngleEnd angle. This is the end angle if smallestAngle is false. -@param float MaxValue angle. -@param int nAngles Number of angles in array -@param int smallestAngle Whether to calculate the angular difference between the start and end angles or the smallest angle. -@param float Difference Angular difference -*/ -float AngularDifferenceFloat(const float AngleStart, const float AngleEnd, const float MaxValue, int smallestAngle) -{ - float Difference = fmod(fabsf(AngleStart - AngleEnd), MaxValue); - if (smallestAngle) - Difference = fmin(Difference, MaxValue - Difference); - else if (AngleStart > AngleEnd) - Difference = MaxValue - Difference; - return Difference; -} - -/* -Calculate the angular difference between two numbers of float precision. - -@param float *AngleStart array of size n angles. This is the start angle if smallestAngle is false. -@param float *AngleEnd array of size n angles. This is the end angle if smallestAngle is false. -@param int nAngles Number of angles in array -@param int smallestAngle Whether to calculate the angular difference between the start and end angles or the smallest angle. -@param float Difference Angular difference */ -void AngularDifferencesFloat(const float *AngleStart, const float *AngleEnd, const float MaxValue, int nAngles, int smallestAngle, float *Difference) -{ - int i; - for (i = 0; i < nAngles; ++i) - { - Difference[i] = fmod(fabsf(AngleStart[i] - AngleEnd[i]), MaxValue); - if (smallestAngle) - Difference[i] = fmin(Difference[i], MaxValue - Difference[i]); - else if (AngleStart[i] > AngleEnd[i]) - Difference[i] = MaxValue - Difference[i]; - } -} - -/* -Calculate the angular difference between two numbers of float precision. - -@param double *AngleStart array of size n angles. This is the start angle if smallestAngle is false. -@param double *AngleEnd array of size n angles. This is the end angle if smallestAngle is false. -@param int nAngles Number of angles in array -@param int smallestAngle Whether to calculate the angular difference between the start and end angles or the smallest angle. -@param double Difference Angular difference */ -void AngularDifferencesDouble(const double *AngleStart, const double *AngleEnd, const double MaxValue, int nAngles, int smallestAngle, double *Difference) -{ - int i; - for (i = 0; i < nAngles; ++i) - { - Difference[i] = fmod(fabs(AngleStart[i] - AngleEnd[i]), MaxValue); - if (smallestAngle) - Difference[i] = fmin(Difference[i], MaxValue - Difference[i]); - else if (AngleStart[i] > AngleEnd[i]) - Difference[i] = MaxValue - Difference[i]; - } -} - -/* -Convert a point from X, X, m to Y, Y, m in double precision - -@param double *ddmPoint array of size nx3 -@param int nPoints Number of angles in array -@param double *rrmPoint array of size nx3 -*/ -void XXM2YYMDouble(const double *rrmPoint, int nPoints, const double transform, double *ddmPoint) -{ - int iPoint, i; - for (iPoint = 0; iPoint < nPoints; ++iPoint) - { - i = iPoint * 3; - ddmPoint[i + 0] = rrmPoint[i + 0] * transform; - ddmPoint[i + 1] = rrmPoint[i + 1] * transform; - ddmPoint[i + 2] = rrmPoint[i + 2]; - } -} - -/* -Convert a point from X, X, m to Y, Y, m in float precision - -@param float *ddmPoint array of size nx3 -@param int nPoints Number of angles in array -@param float *rrmPoint array of size nx3 -*/ -void XXM2YYMFloat(const float *rrmPoint, int nPoints, const float transform, float *ddmPoint) -{ - int iPoint, i; - for (iPoint = 0; iPoint < nPoints; ++iPoint) - { - i = iPoint * 3; - ddmPoint[i + 0] = rrmPoint[i + 0] * transform; - ddmPoint[i + 1] = rrmPoint[i + 1] * transform; - ddmPoint[i + 2] = rrmPoint[i + 2]; - } -} - -static PyObject *DegAngularDifferenceWrapper(PyObject *self, PyObject *args) -{ - double degAngleStart, degAngleEnd; - int smallestAngle; - - // Parse the input tuple - if (!PyArg_ParseTuple(args, "ddi", °AngleStart, °AngleEnd, &smallestAngle)) - return NULL; - - if (!((smallestAngle == 0) || (smallestAngle == 1))) - { - PyErr_SetString(PyExc_ValueError, "smallestAngle must be True or False"); - return NULL; - } - - if (sizeof(degAngleEnd) == sizeof(double)) - { - double maxValue = 360.0; - double result_data = AngularDifferenceDouble(degAngleStart, degAngleEnd, maxValue, smallestAngle); - return Py_BuildValue("d", result_data); // Convert double to PyObject* - } - else if (sizeof(degAngleEnd) == sizeof(float)) - { - float maxValue = 360.0; - float result_data = AngularDifferenceFloat(degAngleStart, degAngleEnd, maxValue, smallestAngle); - return Py_BuildValue("f", result_data); // Convert float to PyObject* - } - else - { - PyErr_SetString(PyExc_ValueError, "Only 32 and 64 bit float types accepted."); - return NULL; - } -} - -static PyObject *RadAngularDifferenceWrapper(PyObject *self, PyObject *args) -{ - double degAngleStart, degAngleEnd; - int smallestAngle; - - // Parse the input tuple - if (!PyArg_ParseTuple(args, "ddi", °AngleStart, °AngleEnd, &smallestAngle)) - return NULL; - - if (!((smallestAngle == 0) || (smallestAngle == 1))) - { - PyErr_SetString(PyExc_ValueError, "smallestAngle must be True or False"); - return NULL; - } - - if (sizeof(degAngleEnd) == sizeof(double)) - { - double maxValue = 2.0 * PI; - double result_data = AngularDifferenceDouble(degAngleStart, degAngleEnd, maxValue, smallestAngle); - return Py_BuildValue("d", result_data); // Convert double to PyObject* - } - else if (sizeof(degAngleEnd) == sizeof(float)) - { - float maxValue = 2.0 * PI; - float result_data = AngularDifferenceFloat(degAngleStart, degAngleEnd, maxValue, smallestAngle); - return Py_BuildValue("f", result_data); // Convert float to PyObject* - } - else - { - PyErr_SetString(PyExc_ValueError, "Only 32 and 64 bit float types accepted."); - return NULL; - } -} - -static PyObject *DegAngularDifferencesWrapper(PyObject *self, PyObject *args) -{ - PyArrayObject *degAngleStart, *degAngleEnd; - int smallestAngle; - - // Parse the input tuple - if (!PyArg_ParseTuple(args, "O!O!i", &PyArray_Type, °AngleStart, &PyArray_Type, °AngleEnd, &smallestAngle)) - return NULL; - - // checks - if (!((smallestAngle == 0) || (smallestAngle == 1))) - { - PyErr_SetString(PyExc_ValueError, "smallestAngle must be True or False"); - return NULL; - } - if (!(PyArray_ISCONTIGUOUS(degAngleStart)) || !(PyArray_ISCONTIGUOUS(degAngleEnd))) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); - return NULL; - } - if (PyArray_NDIM(degAngleStart) != PyArray_NDIM(degAngleEnd)) - { - PyErr_SetString(PyExc_ValueError, "Input arrays have non-matching dimensions."); - return NULL; - } - if (PyArray_SIZE(degAngleStart) != PyArray_SIZE(degAngleEnd)) - { - PyErr_SetString(PyExc_ValueError, "Input arrays are of unequal size."); - return NULL; - } - if (PyArray_TYPE(degAngleStart) != PyArray_TYPE(degAngleEnd)) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must have the same type."); - return NULL; - } - - PyObject *result_array = PyArray_SimpleNew(PyArray_NDIM(degAngleEnd), PyArray_SHAPE(degAngleEnd), PyArray_TYPE(degAngleEnd)); - if (result_array == NULL) - { - PyErr_SetString(PyExc_ValueError, "Could not create output array."); - return NULL; - } - npy_intp nPoints = PyArray_SIZE(degAngleStart); - if (PyArray_TYPE(degAngleEnd) == NPY_DOUBLE) - { - double *data1 = (double *)PyArray_DATA(degAngleStart); - double *data2 = (double *)PyArray_DATA(degAngleEnd); - double *result_data = (double *)PyArray_DATA((PyArrayObject *)result_array); - AngularDifferencesDouble(data1, data2, 360.0, nPoints, smallestAngle, result_data); - } - else if (PyArray_TYPE(degAngleEnd) == NPY_FLOAT) - { - float *data1 = (float *)PyArray_DATA(degAngleStart); - float *data2 = (float *)PyArray_DATA(degAngleEnd); - float *result_data = (float *)PyArray_DATA((PyArrayObject *)result_array); - AngularDifferencesFloat(data1, data2, 360.0, nPoints, smallestAngle, result_data); - } - else - { - PyErr_SetString(PyExc_ValueError, "Only 32 and 64 bit float types accepted."); - return NULL; - } - return result_array; -} - -static PyObject *RadAngularDifferencesWrapper(PyObject *self, PyObject *args) -{ - PyArrayObject *radAngleStart, *radAngleEnd; - int smallestAngle; - - // Parse the input tuple - if (!PyArg_ParseTuple(args, "O!O!i", &PyArray_Type, &radAngleStart, &PyArray_Type, &radAngleEnd, &smallestAngle)) - return NULL; - - // checks - if (!((smallestAngle == 0) || (smallestAngle == 1))) - { - PyErr_SetString(PyExc_ValueError, "smallestAngle must be True or False"); - return NULL; - } - if (!(PyArray_ISCONTIGUOUS(radAngleStart)) || !(PyArray_ISCONTIGUOUS(radAngleEnd))) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); - return NULL; - } - if (PyArray_NDIM(radAngleStart) != PyArray_NDIM(radAngleEnd)) - { - PyErr_SetString(PyExc_ValueError, "Input arrays have non-matching dimensions."); - return NULL; - } - if (PyArray_SIZE(radAngleStart) != PyArray_SIZE(radAngleEnd)) - { - PyErr_SetString(PyExc_ValueError, "Input arrays are of unequal size."); - return NULL; - } - if (PyArray_TYPE(radAngleStart) != PyArray_TYPE(radAngleEnd)) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must have the same type."); - return NULL; - } - - PyObject *result_array = PyArray_SimpleNew(PyArray_NDIM(radAngleEnd), PyArray_SHAPE(radAngleStart), PyArray_TYPE(radAngleEnd)); - if (result_array == NULL) - { - PyErr_SetString(PyExc_ValueError, "Could not create output array."); - return NULL; - } - npy_intp nPoints = PyArray_SIZE(radAngleStart); - if (PyArray_TYPE(radAngleEnd) == NPY_DOUBLE) - { - double *data1 = (double *)PyArray_DATA(radAngleStart); - double *data2 = (double *)PyArray_DATA(radAngleEnd); - double *result_data = (double *)PyArray_DATA((PyArrayObject *)result_array); - AngularDifferencesDouble(data1, data2, 2.0 * PI, nPoints, smallestAngle, result_data); - } - else if (PyArray_TYPE(radAngleEnd) == NPY_FLOAT) - { - float *data1 = (float *)PyArray_DATA(radAngleStart); - float *data2 = (float *)PyArray_DATA(radAngleEnd); - float *result_data = (float *)PyArray_DATA((PyArrayObject *)result_array); - AngularDifferencesFloat(data1, data2, 2.0 * PI, nPoints, smallestAngle, result_data); - } - else - { - PyErr_SetString(PyExc_ValueError, "Only 32 and 64 bit float types accepted."); - return NULL; - } - return result_array; -} - -static PyObject *RRM2DDMWrapper(PyObject *self, PyObject *args) -{ - PyArrayObject *rrmPoint; - - // Parse the input tuple - if (!PyArg_ParseTuple(args, "O!", &PyArray_Type, &rrmPoint)) - return NULL; - - // Checks - if (!(PyArray_ISCONTIGUOUS(rrmPoint))) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); - return NULL; - } - - PyObject *result_array = PyArray_SimpleNew(PyArray_NDIM(rrmPoint), PyArray_SHAPE(rrmPoint), PyArray_TYPE(rrmPoint)); - if (result_array == NULL) - { - PyErr_SetString(PyExc_ValueError, "Could not create output array."); - return NULL; - } - if (PyArray_TYPE(rrmPoint) == NPY_DOUBLE) - { - double *data1 = (double *)PyArray_DATA(rrmPoint); - double *result_data = (double *)PyArray_DATA((PyArrayObject *)result_array); - XXM2YYMDouble(data1, PyArray_SIZE(rrmPoint) / 3, 180.0 / PI, result_data); - } - else if (PyArray_TYPE(rrmPoint) == NPY_FLOAT) - { - float *data1 = (float *)PyArray_DATA(rrmPoint); - float *result_data = (float *)PyArray_DATA((PyArrayObject *)result_array); - XXM2YYMFloat(data1, PyArray_SIZE(rrmPoint) / 3, 180.0 / PI, result_data); - } - else - { - PyErr_SetString(PyExc_ValueError, "Only 32 and 64 bit float types accepted."); - return NULL; - } - return result_array; -} - -static PyObject *DDM2RRMWrapper(PyObject *self, PyObject *args) -{ - PyArrayObject *ddmPoint; - - // Parse the input tuple - if (!PyArg_ParseTuple(args, "O!", &PyArray_Type, &ddmPoint)) - return NULL; - - // Checks - if (!(PyArray_ISCONTIGUOUS(ddmPoint))) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); - return NULL; - } - - PyObject *result_array = PyArray_SimpleNew(PyArray_NDIM(ddmPoint), PyArray_SHAPE(ddmPoint), PyArray_TYPE(ddmPoint)); - if (result_array == NULL) - { - PyErr_SetString(PyExc_ValueError, "Could not create output array."); - return NULL; - } - if (PyArray_TYPE(ddmPoint) == NPY_DOUBLE) - { - double *data1 = (double *)PyArray_DATA(ddmPoint); - double *result_data = (double *)PyArray_DATA((PyArrayObject *)result_array); - XXM2YYMDouble(data1, PyArray_SIZE(ddmPoint) / 3, PI / 180.0, result_data); - } - else if (PyArray_TYPE(ddmPoint) == NPY_FLOAT) - { - float *data1 = (float *)PyArray_DATA(ddmPoint); - float *result_data = (float *)PyArray_DATA((PyArrayObject *)result_array); - XXM2YYMFloat(data1, PyArray_SIZE(ddmPoint) / 3, PI / 180.0, result_data); - } - else - { - PyErr_SetString(PyExc_ValueError, "Only 32 and 64 bit float types accepted."); - return NULL; - } - return result_array; -} - -// Method definition object for this extension, these argumens mean: -// ml_name: The name of the method -// ml_meth: Function pointer to the method implementation -// ml_flags: Flags indicating special features of this method, such as -// accepting arguments, accepting keyword arguments, being a class method, or being a static method of a class. -// ml_doc: The docstring for the method -static PyMethodDef MyMethods[] = { - {"deg_angular_differences", DegAngularDifferencesWrapper, METH_VARARGS, "Angular difference between two angles in degrees"}, - {"rad_angular_differences", RadAngularDifferencesWrapper, METH_VARARGS, "Angular difference between two angles in radians"}, - {"deg_angular_difference", DegAngularDifferenceWrapper, METH_VARARGS, "Angular difference between two angles in radians"}, - {"rad_angular_difference", RadAngularDifferenceWrapper, METH_VARARGS, "Angular difference between two angles in radians"}, - {"RRM2DDM", RRM2DDMWrapper, METH_VARARGS, "Converts arrays of [rad, rad, m] to [deg, deg, m]"}, - {"DDM2RRM", DDM2RRMWrapper, METH_VARARGS, "Converts arrays of [rad, rad, m] to [deg, deg, m]"}, - {NULL, NULL, 0, NULL}}; - -// Module definition -static struct PyModuleDef helpers = { - PyModuleDef_HEAD_INIT, - "helpers", - "Module containing helper / miscellaneous functions", - -1, - MyMethods}; - -// Module initialization function -PyMODINIT_FUNC PyInit_helpers(void) -{ - import_array(); // Initialize the NumPy C API - return PyModule_Create(&helpers); -} diff --git a/include/distances.c b/include/distances.c new file mode 100644 index 0000000..e8cc665 --- /dev/null +++ b/include/distances.c @@ -0,0 +1,148 @@ +#include +#include +#define NCOORDSINPOINT 3 + +/* +Calculate the Haversine distance between two points in double precision. +https://en.wikipedia.org/wiki/Haversine_formula#Formulation + +@param float *rrmStart array of size nx3 of start point azimuth, elevation, +range [rad, rad, m] +@param float *rrmEnd array of size nx3 of start point azimuth, elevation, range +[rad, rad, m] +@param size_t nPoints Number of target points +@param double mRadiusSphere Radius of sphere in metres +@param float *mRadiusSphere array of size nx3 of distance between start and end +points +*/ +void HaversineDouble(const double* rrmStart, + const double* rrmEnd, + int nPoints, + int isArraysSizeEqual, + double mRadiusSphere, + double* mDistance) +{ + int iPointEnd, iPointStart; + for (int iPoint = 0; iPoint < nPoints; ++iPoint) { + iPointEnd = iPoint * NCOORDSINPOINT; + iPointStart = iPointEnd * isArraysSizeEqual; + mDistance[iPoint] = 2.0 * mRadiusSphere * asin(sqrt((1.0 - cos(rrmEnd[iPointEnd] - rrmStart[iPointStart]) + cos(rrmStart[iPointStart]) * cos(rrmEnd[iPointEnd]) * (1.0 - cos(rrmEnd[iPointEnd + 1] - rrmStart[iPointStart + 1]))) / 2.0)); + } +} + +/* +Calculate the Haversine distance between two points in float precision. +https://en.wikipedia.org/wiki/Haversine_formula#Formulation + +@param float *rrmStart array of size nx3 of start point azimuth, elevation, +range [rad, rad, m] +@param float *rrmEnd array of size nx3 of start point azimuth, elevation, range +[rad, rad, m] +@param size_t nPoints Number of target points +@param double mRadiusSphere Radius of sphere in metres +@param float *mRadiusSphere array of size nx3 of distance between start and end +points +*/ +void HaversineFloat(const float* rrmStart, + const float* rrmEnd, + int nPoints, + int isArraysSizeEqual, + float mRadiusSphere, + float* mDistance) +{ + int iPointEnd, iPointStart; + for (int iPoint = 0; iPoint < nPoints; ++iPoint) { + iPointEnd = iPoint * NCOORDSINPOINT; + iPointStart = iPointEnd * isArraysSizeEqual; + mDistance[iPoint] = 2.0 * mRadiusSphere * asin(sqrt((1.0 - cos(rrmEnd[iPointEnd] - rrmStart[iPointStart]) + cos(rrmStart[iPointStart]) * cos(rrmEnd[iPointEnd]) * (1.0 - cos(rrmEnd[iPointEnd + 1] - rrmStart[iPointStart + 1]))) / 2.0)); + } +} + +static PyObject* +HaversineWrapper(PyObject* self, PyObject* args) +{ + PyArrayObject *rrmStart, *rrmEnd; + double mRadiusSphere; + + // checks + if (!PyArg_ParseTuple(args, + "O!O!d", + &PyArray_Type, + &rrmStart, + &PyArray_Type, + &rrmEnd, + &mRadiusSphere)) + return NULL; + if (!(PyArray_ISCONTIGUOUS(rrmStart)) || !(PyArray_ISCONTIGUOUS(rrmEnd))) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); + return NULL; + } + if (!((PyArray_NDIM(rrmStart) == PyArray_NDIM(rrmEnd)) && (PyArray_SIZE(rrmStart) == PyArray_SIZE(rrmEnd)) || ((PyArray_Size(rrmStart) == NCOORDSINPOINT) && (PyArray_SIZE(rrmStart) < PyArray_SIZE(rrmEnd))))) { + PyErr_SetString(PyExc_ValueError, + "Input arrays must have matching size and dimensions or " + "the start point must be of size three."); + return NULL; + } + if ((PyArray_SIZE(rrmStart) % NCOORDSINPOINT) != 0 || (PyArray_SIZE(rrmEnd) % NCOORDSINPOINT) != 0) { + PyErr_SetString(PyExc_ValueError, + "Input arrays must be a multiple of three."); + return NULL; + } + if (PyArray_TYPE(rrmStart) != PyArray_TYPE(rrmEnd)) { + PyErr_SetString(PyExc_ValueError, "Input arrays must have the same type."); + return NULL; + } + + npy_intp nPoints = PyArray_SIZE(rrmEnd) / NCOORDSINPOINT; + PyObject* result_array = PyArray_SimpleNew(1, &nPoints, PyArray_TYPE(rrmEnd)); + int isArraysSizeEqual = (PyArray_Size(rrmStart) == PyArray_Size(rrmEnd)); + if (result_array == NULL) + return NULL; + if (PyArray_TYPE(rrmEnd) == NPY_DOUBLE) { + double* data1 = (double*)PyArray_DATA(rrmStart); + double* data2 = (double*)PyArray_DATA(rrmEnd); + double* result_data = (double*)PyArray_DATA((PyArrayObject*)result_array); + HaversineDouble( + data1, data2, nPoints, isArraysSizeEqual, mRadiusSphere, result_data); + } else if (PyArray_TYPE(rrmEnd) == NPY_FLOAT) { + float* data1 = (float*)PyArray_DATA(rrmStart); + float* data2 = (float*)PyArray_DATA(rrmEnd); + float* result_data = (float*)PyArray_DATA((PyArrayObject*)result_array); + HaversineFloat( + data1, data2, nPoints, isArraysSizeEqual, mRadiusSphere, result_data); + } else { + PyErr_SetString(PyExc_ValueError, + "Only 32 and 64 bit float types accepted."); + return NULL; + } + return result_array; +} + +// Method definition object for this extension, these argumens mean: +// ml_name: The name of the method +// ml_meth: Function pointer to the method implementation +// ml_flags: Flags indicating special features of this method, such as +// accepting arguments, accepting keyword arguments, being a class +// method, or being a static method of a class. +// ml_doc: The docstring for the method +static PyMethodDef MyMethods[] = { + { "Haversine", HaversineWrapper, METH_VARARGS, "Haversine function" }, + { NULL, NULL, 0, NULL } +}; + +// Module definition +static struct PyModuleDef distances = { + PyModuleDef_HEAD_INIT, + "distances", + "Module that contains functions to calculate distances between points", + -1, + MyMethods +}; + +// Module initialization function +PyMODINIT_FUNC +PyInit_distances(void) +{ + import_array(); // Initialize the NumPy C API + return PyModule_Create(&distances); +} diff --git a/include/helpers.c b/include/helpers.c new file mode 100644 index 0000000..a009368 --- /dev/null +++ b/include/helpers.c @@ -0,0 +1,486 @@ +#include +#include +#include + +#define NCOORDSINPOINT 3 +#define DEGCIRCLE 360.0 +#define PI 3.14159265358979323846 + +/* +Calculate the angular difference between two numbers of double precision. + +@param double *AngleStart array of size n angles. This is the start angle if +smallestAngle is false. +@param double *AngleEnd array of size n angles. This is the end angle if +smallestAngle is false. +@param int nAngles Number of angles in array +@param int smallestAngle Whether to calculate the angular difference between +the start and end angles or the smallest angle. +@param double Difference Angular difference +*/ +double +AngularDifferenceDouble(const double AngleStart, + const double AngleEnd, + const double MaxValue, + int smallestAngle) +{ + double Difference = fmod(fabs(AngleStart - AngleEnd), MaxValue); + if (smallestAngle) + Difference = fmin(Difference, MaxValue - Difference); + else if (AngleStart > AngleEnd) + Difference = MaxValue - Difference; + return Difference; +} + +/* +Calculate the angular difference between two numbers of float precision. + +@param float AngleStart angle. This is the start angle if smallestAngle is +false. +@param float AngleEnd angle. This is the end angle if smallestAngle is false. +@param float MaxValue angle. +@param int nAngles Number of angles in array +@param int smallestAngle Whether to calculate the angular difference between +the start and end angles or the smallest angle. +@param float Difference Angular difference +*/ +float AngularDifferenceFloat(const float AngleStart, + const float AngleEnd, + const float MaxValue, + int smallestAngle) +{ + float Difference = fmod(fabsf(AngleStart - AngleEnd), MaxValue); + if (smallestAngle) + Difference = fmin(Difference, MaxValue - Difference); + else if (AngleStart > AngleEnd) + Difference = MaxValue - Difference; + return Difference; +} + +/* +Calculate the angular difference between two numbers of float precision. + +@param float *AngleStart array of size n angles. This is the start angle if +smallestAngle is false. +@param float *AngleEnd array of size n angles. This is the end angle if +smallestAngle is false. +@param int nAngles Number of angles in array +@param int smallestAngle Whether to calculate the angular difference between +the start and end angles or the smallest angle. +@param float Difference Angular difference */ +void AngularDifferencesFloat(const float* AngleStart, + const float* AngleEnd, + const float MaxValue, + int nAngles, + int smallestAngle, + float* Difference) +{ + int i; + for (i = 0; i < nAngles; ++i) { + Difference[i] = fmod(fabsf(AngleStart[i] - AngleEnd[i]), MaxValue); + if (smallestAngle) + Difference[i] = fmin(Difference[i], MaxValue - Difference[i]); + else if (AngleStart[i] > AngleEnd[i]) + Difference[i] = MaxValue - Difference[i]; + } +} + +/* +Calculate the angular difference between two numbers of float precision. + +@param double *AngleStart array of size n angles. This is the start angle if +smallestAngle is false. +@param double *AngleEnd array of size n angles. This is the end angle if +smallestAngle is false. +@param int nAngles Number of angles in array +@param int smallestAngle Whether to calculate the angular difference between +the start and end angles or the smallest angle. +@param double Difference Angular difference */ +void AngularDifferencesDouble(const double* AngleStart, + const double* AngleEnd, + const double MaxValue, + int nAngles, + int smallestAngle, + double* Difference) +{ + int i; + for (i = 0; i < nAngles; ++i) { + Difference[i] = fmod(fabs(AngleStart[i] - AngleEnd[i]), MaxValue); + if (smallestAngle) + Difference[i] = fmin(Difference[i], MaxValue - Difference[i]); + else if (AngleStart[i] > AngleEnd[i]) + Difference[i] = MaxValue - Difference[i]; + } +} + +/* +Convert a point from X, X, m to Y, Y, m in double precision + +@param double *ddmPoint array of size nx3 +@param int nPoints Number of angles in array +@param double *rrmPoint array of size nx3 +*/ +void XXM2YYMDouble(const double* rrmPoint, + int nPoints, + const double transform, + double* ddmPoint) +{ + int iPoint, i; + for (iPoint = 0; iPoint < nPoints; ++iPoint) { + i = iPoint * NCOORDSINPOINT; + ddmPoint[i + 0] = rrmPoint[i + 0] * transform; + ddmPoint[i + 1] = rrmPoint[i + 1] * transform; + ddmPoint[i + 2] = rrmPoint[i + 2]; + } +} + +/* +Convert a point from X, X, m to Y, Y, m in float precision + +@param float *ddmPoint array of size nx3 +@param int nPoints Number of angles in array +@param float *rrmPoint array of size nx3 +*/ +void XXM2YYMFloat(const float* rrmPoint, + int nPoints, + const float transform, + float* ddmPoint) +{ + int iPoint, i; + for (iPoint = 0; iPoint < nPoints; ++iPoint) { + i = iPoint * NCOORDSINPOINT; + ddmPoint[i + 0] = rrmPoint[i + 0] * transform; + ddmPoint[i + 1] = rrmPoint[i + 1] * transform; + ddmPoint[i + 2] = rrmPoint[i + 2]; + } +} + +static PyObject* +DegAngularDifferenceWrapper(PyObject* self, PyObject* args) +{ + double degAngleStart, degAngleEnd; + int smallestAngle; + + // Parse the input tuple + if (!PyArg_ParseTuple( + args, "ddi", °AngleStart, °AngleEnd, &smallestAngle)) + return NULL; + + if (!((smallestAngle == 0) || (smallestAngle == 1))) { + PyErr_SetString(PyExc_ValueError, "smallestAngle must be True or False"); + return NULL; + } + + if (sizeof(degAngleEnd) == sizeof(double)) { + double maxValue = DEGCIRCLE; + double result_data = AngularDifferenceDouble( + degAngleStart, degAngleEnd, maxValue, smallestAngle); + return Py_BuildValue("d", result_data); // Convert double to PyObject* + } else if (sizeof(degAngleEnd) == sizeof(float)) { + float maxValue = DEGCIRCLE; + float result_data = AngularDifferenceFloat( + degAngleStart, degAngleEnd, maxValue, smallestAngle); + return Py_BuildValue("f", result_data); // Convert float to PyObject* + } else { + PyErr_SetString(PyExc_ValueError, + "Only 32 and 64 bit float types accepted."); + return NULL; + } +} + +static PyObject* +RadAngularDifferenceWrapper(PyObject* self, PyObject* args) +{ + double degAngleStart, degAngleEnd; + int smallestAngle; + + // Parse the input tuple + if (!PyArg_ParseTuple( + args, "ddi", °AngleStart, °AngleEnd, &smallestAngle)) + return NULL; + + if (!((smallestAngle == 0) || (smallestAngle == 1))) { + PyErr_SetString(PyExc_ValueError, "smallestAngle must be True or False"); + return NULL; + } + + if (sizeof(degAngleEnd) == sizeof(double)) { + double maxValue = 2.0 * PI; + double result_data = AngularDifferenceDouble( + degAngleStart, degAngleEnd, maxValue, smallestAngle); + return Py_BuildValue("d", result_data); // Convert double to PyObject* + } else if (sizeof(degAngleEnd) == sizeof(float)) { + float maxValue = 2.0 * PI; + float result_data = AngularDifferenceFloat( + degAngleStart, degAngleEnd, maxValue, smallestAngle); + return Py_BuildValue("f", result_data); // Convert float to PyObject* + } else { + PyErr_SetString(PyExc_ValueError, + "Only 32 and 64 bit float types accepted."); + return NULL; + } +} + +static PyObject* +DegAngularDifferencesWrapper(PyObject* self, PyObject* args) +{ + PyArrayObject *degAngleStart, *degAngleEnd; + int smallestAngle; + + // Parse the input tuple + if (!PyArg_ParseTuple(args, + "O!O!i", + &PyArray_Type, + °AngleStart, + &PyArray_Type, + °AngleEnd, + &smallestAngle)) + return NULL; + + // checks + if (!((smallestAngle == 0) || (smallestAngle == 1))) { + PyErr_SetString(PyExc_ValueError, "smallestAngle must be True or False"); + return NULL; + } + if (!(PyArray_ISCONTIGUOUS(degAngleStart)) || !(PyArray_ISCONTIGUOUS(degAngleEnd))) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); + return NULL; + } + if (PyArray_NDIM(degAngleStart) != PyArray_NDIM(degAngleEnd)) { + PyErr_SetString(PyExc_ValueError, + "Input arrays have non-matching dimensions."); + return NULL; + } + if (PyArray_SIZE(degAngleStart) != PyArray_SIZE(degAngleEnd)) { + PyErr_SetString(PyExc_ValueError, "Input arrays are of unequal size."); + return NULL; + } + if (PyArray_TYPE(degAngleStart) != PyArray_TYPE(degAngleEnd)) { + PyErr_SetString(PyExc_ValueError, "Input arrays must have the same type."); + return NULL; + } + + PyObject* result_array = PyArray_SimpleNew(PyArray_NDIM(degAngleEnd), + PyArray_SHAPE(degAngleEnd), + PyArray_TYPE(degAngleEnd)); + if (result_array == NULL) { + PyErr_SetString(PyExc_ValueError, "Could not create output array."); + return NULL; + } + npy_intp nPoints = PyArray_SIZE(degAngleStart); + if (PyArray_TYPE(degAngleEnd) == NPY_DOUBLE) { + double* data1 = (double*)PyArray_DATA(degAngleStart); + double* data2 = (double*)PyArray_DATA(degAngleEnd); + double* result_data = (double*)PyArray_DATA((PyArrayObject*)result_array); + AngularDifferencesDouble( + data1, data2, DEGCIRCLE, nPoints, smallestAngle, result_data); + } else if (PyArray_TYPE(degAngleEnd) == NPY_FLOAT) { + float* data1 = (float*)PyArray_DATA(degAngleStart); + float* data2 = (float*)PyArray_DATA(degAngleEnd); + float* result_data = (float*)PyArray_DATA((PyArrayObject*)result_array); + AngularDifferencesFloat( + data1, data2, DEGCIRCLE, nPoints, smallestAngle, result_data); + } else { + PyErr_SetString(PyExc_ValueError, + "Only 32 and 64 bit float types accepted."); + return NULL; + } + return result_array; +} + +static PyObject* +RadAngularDifferencesWrapper(PyObject* self, PyObject* args) +{ + PyArrayObject *radAngleStart, *radAngleEnd; + int smallestAngle; + + // Parse the input tuple + if (!PyArg_ParseTuple(args, + "O!O!i", + &PyArray_Type, + &radAngleStart, + &PyArray_Type, + &radAngleEnd, + &smallestAngle)) + return NULL; + + // checks + if (!((smallestAngle == 0) || (smallestAngle == 1))) { + PyErr_SetString(PyExc_ValueError, "smallestAngle must be True or False"); + return NULL; + } + if (!(PyArray_ISCONTIGUOUS(radAngleStart)) || !(PyArray_ISCONTIGUOUS(radAngleEnd))) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); + return NULL; + } + if (PyArray_NDIM(radAngleStart) != PyArray_NDIM(radAngleEnd)) { + PyErr_SetString(PyExc_ValueError, + "Input arrays have non-matching dimensions."); + return NULL; + } + if (PyArray_SIZE(radAngleStart) != PyArray_SIZE(radAngleEnd)) { + PyErr_SetString(PyExc_ValueError, "Input arrays are of unequal size."); + return NULL; + } + if (PyArray_TYPE(radAngleStart) != PyArray_TYPE(radAngleEnd)) { + PyErr_SetString(PyExc_ValueError, "Input arrays must have the same type."); + return NULL; + } + + PyObject* result_array = PyArray_SimpleNew(PyArray_NDIM(radAngleEnd), + PyArray_SHAPE(radAngleStart), + PyArray_TYPE(radAngleEnd)); + if (result_array == NULL) { + PyErr_SetString(PyExc_ValueError, "Could not create output array."); + return NULL; + } + npy_intp nPoints = PyArray_SIZE(radAngleStart); + if (PyArray_TYPE(radAngleEnd) == NPY_DOUBLE) { + double* data1 = (double*)PyArray_DATA(radAngleStart); + double* data2 = (double*)PyArray_DATA(radAngleEnd); + double* result_data = (double*)PyArray_DATA((PyArrayObject*)result_array); + AngularDifferencesDouble( + data1, data2, 2.0 * PI, nPoints, smallestAngle, result_data); + } else if (PyArray_TYPE(radAngleEnd) == NPY_FLOAT) { + float* data1 = (float*)PyArray_DATA(radAngleStart); + float* data2 = (float*)PyArray_DATA(radAngleEnd); + float* result_data = (float*)PyArray_DATA((PyArrayObject*)result_array); + AngularDifferencesFloat( + data1, data2, 2.0 * PI, nPoints, smallestAngle, result_data); + } else { + PyErr_SetString(PyExc_ValueError, + "Only 32 and 64 bit float types accepted."); + return NULL; + } + return result_array; +} + +static PyObject* +RRM2DDMWrapper(PyObject* self, PyObject* args) +{ + PyArrayObject* rrmPoint; + + // Parse the input tuple + if (!PyArg_ParseTuple(args, "O!", &PyArray_Type, &rrmPoint)) + return NULL; + + // Checks + if (!(PyArray_ISCONTIGUOUS(rrmPoint))) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); + return NULL; + } + + PyObject* result_array = PyArray_SimpleNew( + PyArray_NDIM(rrmPoint), PyArray_SHAPE(rrmPoint), PyArray_TYPE(rrmPoint)); + if (result_array == NULL) { + PyErr_SetString(PyExc_ValueError, "Could not create output array."); + return NULL; + } + if (PyArray_TYPE(rrmPoint) == NPY_DOUBLE) { + double* data1 = (double*)PyArray_DATA(rrmPoint); + double* result_data = (double*)PyArray_DATA((PyArrayObject*)result_array); + XXM2YYMDouble( + data1, PyArray_SIZE(rrmPoint) / NCOORDSINPOINT, 180.0 / PI, result_data); + } else if (PyArray_TYPE(rrmPoint) == NPY_FLOAT) { + float* data1 = (float*)PyArray_DATA(rrmPoint); + float* result_data = (float*)PyArray_DATA((PyArrayObject*)result_array); + XXM2YYMFloat( + data1, PyArray_SIZE(rrmPoint) / NCOORDSINPOINT, 180.0 / PI, result_data); + } else { + PyErr_SetString(PyExc_ValueError, + "Only 32 and 64 bit float types accepted."); + return NULL; + } + return result_array; +} + +static PyObject* +DDM2RRMWrapper(PyObject* self, PyObject* args) +{ + PyArrayObject* ddmPoint; + + // Parse the input tuple + if (!PyArg_ParseTuple(args, "O!", &PyArray_Type, &ddmPoint)) + return NULL; + + // Checks + if (!(PyArray_ISCONTIGUOUS(ddmPoint))) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); + return NULL; + } + + PyObject* result_array = PyArray_SimpleNew( + PyArray_NDIM(ddmPoint), PyArray_SHAPE(ddmPoint), PyArray_TYPE(ddmPoint)); + if (result_array == NULL) { + PyErr_SetString(PyExc_ValueError, "Could not create output array."); + return NULL; + } + if (PyArray_TYPE(ddmPoint) == NPY_DOUBLE) { + double* data1 = (double*)PyArray_DATA(ddmPoint); + double* result_data = (double*)PyArray_DATA((PyArrayObject*)result_array); + XXM2YYMDouble( + data1, PyArray_SIZE(ddmPoint) / NCOORDSINPOINT, PI / 180.0, result_data); + } else if (PyArray_TYPE(ddmPoint) == NPY_FLOAT) { + float* data1 = (float*)PyArray_DATA(ddmPoint); + float* result_data = (float*)PyArray_DATA((PyArrayObject*)result_array); + XXM2YYMFloat( + data1, PyArray_SIZE(ddmPoint) / NCOORDSINPOINT, PI / 180.0, result_data); + } else { + PyErr_SetString(PyExc_ValueError, + "Only 32 and 64 bit float types accepted."); + return NULL; + } + return result_array; +} + +// Method definition object for this extension, these argumens mean: +// ml_name: The name of the method +// ml_meth: Function pointer to the method implementation +// ml_flags: Flags indicating special features of this method, such as +// accepting arguments, accepting keyword arguments, being a class +// method, or being a static method of a class. +// ml_doc: The docstring for the method +static PyMethodDef MyMethods[] = { + { "deg_angular_differences", + DegAngularDifferencesWrapper, + METH_VARARGS, + "Angular difference between two angles in degrees" }, + { "rad_angular_differences", + RadAngularDifferencesWrapper, + METH_VARARGS, + "Angular difference between two angles in radians" }, + { "deg_angular_difference", + DegAngularDifferenceWrapper, + METH_VARARGS, + "Angular difference between two angles in radians" }, + { "rad_angular_difference", + RadAngularDifferenceWrapper, + METH_VARARGS, + "Angular difference between two angles in radians" }, + { "RRM2DDM", + RRM2DDMWrapper, + METH_VARARGS, + "Converts arrays of [rad, rad, m] to [deg, deg, m]" }, + { "DDM2RRM", + DDM2RRMWrapper, + METH_VARARGS, + "Converts arrays of [rad, rad, m] to [deg, deg, m]" }, + { NULL, NULL, 0, NULL } +}; + +// Module definition +static struct PyModuleDef helpers = { + PyModuleDef_HEAD_INIT, + "helpers", + "Module containing helper / miscellaneous functions", + -1, + MyMethods +}; + +// Module initialization function +PyMODINIT_FUNC +PyInit_helpers(void) +{ + import_array(); // Initialize the NumPy C API + return PyModule_Create(&helpers); +} diff --git a/include/transforms.c b/include/transforms.c new file mode 100644 index 0000000..f9cca85 --- /dev/null +++ b/include/transforms.c @@ -0,0 +1,689 @@ +#include +#include +#define NCOORDSINPOINT 3 + +/* +Geodetic to ECEF transformation of float precision. +https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates + +@param double *rrmLLA array of size nx3 latitude (phi), longitude (gamma), +height (h) [rad, rad, m] +@param size_t nPoints Number of LLA points +@param double a semi-major axis +@param double b semi-minor axis +@param double *mmmXYZ array of size nx3 X, Y, Z [rad, rad, m] +*/ +void geodetic2ECEFFloat(const float* rrmLLA, + size_t nPoints, + double a, + double b, + float* mmmXYZ) +{ + float e2 = 1 - (b * b) / (a * a); + size_t iPoint, i; + float N; + for (iPoint = 0; iPoint < nPoints; ++iPoint) { + i = iPoint * NCOORDSINPOINT; + N = a / sqrt(1 - e2 * (sin(rrmLLA[i + 0]) * sin(rrmLLA[i + 0]))); + mmmXYZ[i + 0] = (N + rrmLLA[i + 2]) * cos(rrmLLA[i + 0]) * cos(rrmLLA[i + 1]); + mmmXYZ[i + 1] = (N + rrmLLA[i + 2]) * cos(rrmLLA[i + 0]) * sin(rrmLLA[i + 1]); + mmmXYZ[i + 2] = ((1 - e2) * N + rrmLLA[i + 2]) * sin(rrmLLA[i + 0]); + } +} + +/* +Geodetic to ECEF transformation of double precision. +https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates + +@param double *rrmLLA array of size nx3 latitude (phi), longitude (gamma), +height (h) [rad, rad, m] +@param size_t nPoints Number of LLA points +@param double a semi-major axis +@param double b semi-minor axis +@param double *mmmXYZ array of size nx3 X, Y, Z [m, m, m] +*/ +void geodetic2ECEFDouble(const double* rrmLLA, + size_t nPoints, + double a, + double b, + double* mmmXYZ) +{ + double e2 = 1 - (b * b) / (a * a); + size_t iPoint, i; + double N; + for (iPoint = 0; iPoint < nPoints; ++iPoint) { + i = iPoint * NCOORDSINPOINT; + N = a / sqrt(1 - e2 * sin(rrmLLA[i + 0]) * sin(rrmLLA[i + 0])); + mmmXYZ[i + 0] = (N + rrmLLA[i + 2]) * cos(rrmLLA[i + 0]) * cos(rrmLLA[i + 1]); + mmmXYZ[i + 1] = (N + rrmLLA[i + 2]) * cos(rrmLLA[i + 0]) * sin(rrmLLA[i + 1]); + mmmXYZ[i + 2] = ((1 - e2) * N + rrmLLA[i + 2]) * sin(rrmLLA[i + 0]); + } +} + +/* +ECEF to geodetic transformation of float precision. +https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#The_application_of_Ferrari's_solution + +@param double *mmmXYZ array of size nx3 X, Y, Z [m, m, m] +@param size_t nPoints Number of ECEF points +@param double a semi-major axis +@param double b semi-minor axis +@param double *rrmLLA array of size nx3 latitude (phi), longitude (gamma), +height (h) [rad, rad, m] +*/ +void ECEF2geodeticFloat(const float* mmmXYZ, + size_t nPoints, + double a, + double b, + float* rrmLLA) +{ + float e2 = ((a * a) - (b * b)) / (a * a); + float ed2 = ((a * a) - (b * b)) / (b * b); + size_t iPoint, i; + for (iPoint = 0; iPoint < nPoints; ++iPoint) { + i = iPoint * NCOORDSINPOINT; + float p = sqrt(mmmXYZ[i + 0] * mmmXYZ[i + 0] + mmmXYZ[i + 1] * mmmXYZ[i + 1]); + float F = 54 * b * b * mmmXYZ[i + 2] * mmmXYZ[i + 2]; + float G = p * p + (1 - e2) * mmmXYZ[i + 2] * mmmXYZ[i + 2] - e2 * (a * a - b * b); + float c = e2 * e2 * F * p * p / (G * G * G); + float s = cbrt(1 + c + sqrt(c * c + 2 * c)); + float k = s + 1 + 1 / s; + float P = F / (3 * k * k * G * G); + float Q = sqrt(1 + 2 * e2 * e2 * P); + float r0 = -P * e2 * p / (1 + Q) + sqrt(0.5 * a * a * (1 + 1 / Q) - P * (1 - e2) * mmmXYZ[i + 2] * mmmXYZ[i + 2] / (Q * (1 + Q)) - 0.5 * P * p * p); + float U = sqrt((p - e2 * r0) * (p - e2 * r0) + mmmXYZ[i + 2] * mmmXYZ[i + 2]); + float V = sqrt((p - e2 * r0) * (p - e2 * r0) + (1 - e2) * mmmXYZ[i + 2] * mmmXYZ[i + 2]); + float z0 = b * b * mmmXYZ[i + 2] / (a * V); + rrmLLA[i + 0] = atan((mmmXYZ[i + 2] + ed2 * z0) / p); + rrmLLA[i + 1] = atan2(mmmXYZ[i + 1], mmmXYZ[i + 0]); + rrmLLA[i + 2] = U * (1 - b * b / (a * V)); + } +} + +/* +ECEF to geodetic transformation of double precision. +https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#The_application_of_Ferrari's_solution + +@param double *mmmXYZ array of size nx3 X, Y, Z [m, m, m] +@param size_t nPoints Number of ECEF points +@param double a semi-major axis +@param double b semi-minor axis +@param double *rrmLLA array of size nx3 latitude (phi), longitude (gamma), +height (h) [rad, rad, m] +*/ +void ECEF2geodeticDouble(const double* mmmXYZ, + size_t nPoints, + double a, + double b, + double* rrmLLA) +{ + double e2 = ((a * a) - (b * b)) / (a * a); + double ed2 = ((a * a) - (b * b)) / (b * b); + size_t iPoint, i; + for (iPoint = 0; iPoint < nPoints; ++iPoint) { + i = iPoint * NCOORDSINPOINT; + double p = sqrt(mmmXYZ[i + 0] * mmmXYZ[i + 0] + mmmXYZ[i + 1] * mmmXYZ[i + 1]); + double F = 54 * b * b * mmmXYZ[i + 2] * mmmXYZ[i + 2]; + double G = p * p + (1 - e2) * mmmXYZ[i + 2] * mmmXYZ[i + 2] - e2 * (a * a - b * b); + double c = e2 * e2 * F * p * p / (G * G * G); + double s = cbrt(1 + c + sqrt(c * c + 2 * c)); + double k = s + 1 + 1 / s; + double P = F / (3 * k * k * G * G); + double Q = sqrt(1 + 2 * e2 * e2 * P); + double r0 = -P * e2 * p / (1 + Q) + sqrt(0.5 * a * a * (1 + 1 / Q) - P * (1 - e2) * mmmXYZ[i + 2] * mmmXYZ[i + 2] / (Q * (1 + Q)) - 0.5 * P * p * p); + double U = sqrt((p - e2 * r0) * (p - e2 * r0) + mmmXYZ[i + 2] * mmmXYZ[i + 2]); + double V = sqrt((p - e2 * r0) * (p - e2 * r0) + (1 - e2) * mmmXYZ[i + 2] * mmmXYZ[i + 2]); + double z0 = b * b * mmmXYZ[i + 2] / (a * V); + rrmLLA[i + 0] = atan((mmmXYZ[i + 2] + ed2 * z0) / p); + rrmLLA[i + 1] = atan2(mmmXYZ[i + 1], mmmXYZ[i + 0]); + rrmLLA[i + 2] = U * (1 - b * b / (a * V)); + } +} + +/* +ECEF to ENU transformation of float precision. +https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ECEF_to_ENU + +@param double *rrmLLALocalOrigin array of size nx3 of local reference point X, +Y, Z [m, m, m] +@param double *mmmXYZTarget array of size nx3 of target point X, Y, Z [m, m, m] +@param size_t nPoints Number of target points +@param double a semi-major axis +@param double b semi-minor axis +@param double *mmmLocal array of size nx3 X, Y, Z [m, m, m] +*/ +void ECEF2ENUFloat(const float* rrmLLALocalOrigin, + const float* mmmXYZTarget, + size_t nTargets, + int isOriginSizeOfTargets, + double a, + double b, + float* mmmLocal) +{ + int nOriginPoints = (nTargets - 1) * isOriginSizeOfTargets + 1; + float* mmmXYZLocalOrigin = (float*)malloc(nOriginPoints * NCOORDSINPOINT * sizeof(float)); + geodetic2ECEFFloat(rrmLLALocalOrigin, nOriginPoints, a, b, mmmXYZLocalOrigin); + float DeltaX, DeltaY, DeltaZ; + size_t iPoint, iTarget, iOrigin; + for (iPoint = 0; iPoint < nTargets; ++iPoint) { + iTarget = iPoint * NCOORDSINPOINT; + iOrigin = iTarget * isOriginSizeOfTargets; + DeltaX = mmmXYZTarget[iTarget + 0] - mmmXYZLocalOrigin[iOrigin + 0]; + DeltaY = mmmXYZTarget[iTarget + 1] - mmmXYZLocalOrigin[iOrigin + 1]; + DeltaZ = mmmXYZTarget[iTarget + 2] - mmmXYZLocalOrigin[iOrigin + 2]; + mmmLocal[iTarget + 0] = -sin(rrmLLALocalOrigin[iOrigin + 1]) * DeltaX + cos(rrmLLALocalOrigin[iOrigin + 1]) * DeltaY; + mmmLocal[iTarget + 1] = -sin(rrmLLALocalOrigin[iOrigin + 0]) * cos(rrmLLALocalOrigin[iOrigin + 1]) * DeltaX + -sin(rrmLLALocalOrigin[iOrigin + 0]) * sin(rrmLLALocalOrigin[iOrigin + 1]) * DeltaY + cos(rrmLLALocalOrigin[iOrigin + 0]) * DeltaZ; + mmmLocal[iTarget + 2] = cos(rrmLLALocalOrigin[iOrigin + 0]) * cos(rrmLLALocalOrigin[iOrigin + 1]) * DeltaX + cos(rrmLLALocalOrigin[iOrigin + 0]) * sin(rrmLLALocalOrigin[iOrigin + 1]) * DeltaY + sin(rrmLLALocalOrigin[iOrigin + 0]) * DeltaZ; + } + free(mmmXYZLocalOrigin); +} + +/* +ECEF to ENU transformation of double precision. +https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ECEF_to_ENU + +@param double *rrmLLALocalOrigin array of size nx3 of local reference point X, +Y, Z [m, m, m] +@param double *mmmXYZTarget array of size nx3 of target point X, Y, Z [m, m, m] +@param size_t nPoints Number of target points +@param double a semi-major axis +@param double b semi-minor axis +@param double *mmmLocal array of size nx3 X, Y, Z [m, m, m] +*/ +void ECEF2ENUDouble(const double* rrmLLALocalOrigin, + const double* mmmXYZTarget, + size_t nTargets, + int isOriginSizeOfTargets, + double a, + double b, + double* mmmLocal) +{ + int nOriginPoints = (nTargets - 1) * isOriginSizeOfTargets + 1; + double* mmmXYZLocalOrigin = (double*)malloc(nOriginPoints * NCOORDSINPOINT * sizeof(double)); + geodetic2ECEFDouble( + rrmLLALocalOrigin, nOriginPoints, a, b, mmmXYZLocalOrigin); + double DeltaX, DeltaY, DeltaZ; + size_t iPoint, iTarget, iOrigin; + for (iPoint = 0; iPoint < nTargets; ++iPoint) { + iTarget = iPoint * NCOORDSINPOINT; + iOrigin = iTarget * isOriginSizeOfTargets; + DeltaX = mmmXYZTarget[iTarget + 0] - mmmXYZLocalOrigin[iOrigin + 0]; + DeltaY = mmmXYZTarget[iTarget + 1] - mmmXYZLocalOrigin[iOrigin + 1]; + DeltaZ = mmmXYZTarget[iTarget + 2] - mmmXYZLocalOrigin[iOrigin + 2]; + mmmLocal[iTarget + 0] = -sin(rrmLLALocalOrigin[iOrigin + 1]) * DeltaX + cos(rrmLLALocalOrigin[iOrigin + 1]) * DeltaY; + mmmLocal[iTarget + 1] = -sin(rrmLLALocalOrigin[iOrigin + 0]) * cos(rrmLLALocalOrigin[iOrigin + 1]) * DeltaX + -sin(rrmLLALocalOrigin[iOrigin + 0]) * sin(rrmLLALocalOrigin[iOrigin + 1]) * DeltaY + cos(rrmLLALocalOrigin[iOrigin + 0]) * DeltaZ; + mmmLocal[iTarget + 2] = cos(rrmLLALocalOrigin[iOrigin + 0]) * cos(rrmLLALocalOrigin[iOrigin + 1]) * DeltaX + cos(rrmLLALocalOrigin[iOrigin + 0]) * sin(rrmLLALocalOrigin[iOrigin + 1]) * DeltaY + sin(rrmLLALocalOrigin[iOrigin + 0]) * DeltaZ; + } + free(mmmXYZLocalOrigin); +} + +/* +ECEF to ENU transformation of float precision. +https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ENU_to_ECEF +https://www.lddgo.net/en/coordinate/ecef-enu + +@param double *rrmLLALocalOrigin array of size nx3 of local reference point +latitude, longitude, height [rad, rad, m] +@param float *mmmXYZTarget array of size nx3 of target point X, Y, Z [m, m, m] +@param size_t nPoints Number of target points +@param double a semi-major axis +@param double b semi-minor axis +@param float *mmmLocal array of size nx3 X, Y, Z [m, m, m] +*/ +void ENU2ECEFFloat(const float* rrmLLALocalOrigin, + const float* mmmTargetLocal, + size_t nTargets, + int isOriginSizeOfTargets, + double a, + double b, + float* mmmXYZTarget) +{ + int nOriginPoints = (nTargets - 1) * isOriginSizeOfTargets + 1; + float* mmmXYZLocalOrigin = (float*)malloc(nOriginPoints * NCOORDSINPOINT * sizeof(float)); + geodetic2ECEFFloat(rrmLLALocalOrigin, nOriginPoints, a, b, mmmXYZLocalOrigin); + size_t iPoint, iTarget, iOrigin; + for (iPoint = 0; iPoint < nTargets; ++iPoint) { + iTarget = iPoint * NCOORDSINPOINT; + iOrigin = iTarget * isOriginSizeOfTargets; + mmmXYZTarget[iTarget + 0] = -sin(rrmLLALocalOrigin[iOrigin + 1]) * mmmTargetLocal[iTarget + 0] + -sin(rrmLLALocalOrigin[iOrigin + 0]) * cos(rrmLLALocalOrigin[iOrigin + 1]) * mmmTargetLocal[iTarget + 1] + cos(rrmLLALocalOrigin[iOrigin + 0]) * cos(rrmLLALocalOrigin[iOrigin + 1]) * mmmTargetLocal[iTarget + 2] + mmmXYZLocalOrigin[iOrigin + 0]; + mmmXYZTarget[iTarget + 1] = cos(rrmLLALocalOrigin[iOrigin + 1]) * mmmTargetLocal[iTarget + 0] + -sin(rrmLLALocalOrigin[iOrigin + 0]) * sin(rrmLLALocalOrigin[iOrigin + 1]) * mmmTargetLocal[iTarget + 1] + cos(rrmLLALocalOrigin[iOrigin + 0]) * sin(rrmLLALocalOrigin[iOrigin + 1]) * mmmTargetLocal[2] + mmmXYZLocalOrigin[iOrigin + 1]; + mmmXYZTarget[iTarget + 2] = cos(rrmLLALocalOrigin[iOrigin + 0]) * mmmTargetLocal[iTarget + 1] + sin(rrmLLALocalOrigin[iOrigin + 0]) * mmmTargetLocal[2] + mmmXYZLocalOrigin[iOrigin + 2]; + } + free(mmmXYZLocalOrigin); +} + +/* +ECEF to ENU transformation of double precision. +https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ENU_to_ECEF +https://www.lddgo.net/en/coordinate/ecef-enu + +@param double *rrmLLALocalOrigin array of size nx3 of local reference point +latitude, longitude, height [rad, rad, m] +@param double *mmmLocal array of size nx3 X, Y, Z [m, m, m] +@param size_t nPoints Number of target points +@param double a semi-major axis +@param double b semi-minor axis +@param double *mmmXYZTarget array of size nx3 of target point X, Y, Z [m, m, m] +*/ +void ENU2ECEFDouble(const double* rrmLLALocalOrigin, + const double* mmmTargetLocal, + size_t nTargets, + int isOriginSizeOfTargets, + double a, + double b, + double* mmmXYZTarget) +{ + int nOriginPoints = (nTargets - 1) * isOriginSizeOfTargets + 1; + double* mmmXYZLocalOrigin = (double*)malloc(nOriginPoints * NCOORDSINPOINT * sizeof(double)); + geodetic2ECEFDouble( + rrmLLALocalOrigin, nOriginPoints, a, b, mmmXYZLocalOrigin); + size_t iPoint, iTarget, iOrigin; + for (iPoint = 0; iPoint < nTargets; ++iPoint) { + iTarget = iPoint * NCOORDSINPOINT; + iOrigin = iTarget * isOriginSizeOfTargets; + mmmXYZTarget[iTarget + 0] = -sin(rrmLLALocalOrigin[iOrigin + 1]) * mmmTargetLocal[iTarget + 0] + -sin(rrmLLALocalOrigin[iOrigin + 0]) * cos(rrmLLALocalOrigin[iOrigin + 1]) * mmmTargetLocal[iTarget + 1] + cos(rrmLLALocalOrigin[iOrigin + 0]) * cos(rrmLLALocalOrigin[iOrigin + 1]) * mmmTargetLocal[iTarget + 2] + mmmXYZLocalOrigin[iOrigin + 0]; + mmmXYZTarget[iTarget + 1] = cos(rrmLLALocalOrigin[iOrigin + 1]) * mmmTargetLocal[iTarget + 0] + -sin(rrmLLALocalOrigin[iOrigin + 0]) * sin(rrmLLALocalOrigin[iOrigin + 1]) * mmmTargetLocal[iTarget + 1] + cos(rrmLLALocalOrigin[iOrigin + 0]) * sin(rrmLLALocalOrigin[iOrigin + 1]) * mmmTargetLocal[2] + mmmXYZLocalOrigin[iOrigin + 1]; + mmmXYZTarget[iTarget + 2] = cos(rrmLLALocalOrigin[iOrigin + 0]) * mmmTargetLocal[iTarget + 1] + sin(rrmLLALocalOrigin[iOrigin + 0]) * mmmTargetLocal[iTarget + 2] + mmmXYZLocalOrigin[iOrigin + 2]; + } + free(mmmXYZLocalOrigin); +} + +/* +ENU to AER transformation of float precision. +https://x-lumin.com/wp-content/uploads/2020/09/Coordinate_Transforms.pdf <- +includes additional errors and factors that could be implemented +https://www.lddgo.net/en/coordinate/ecef-enu + +@param float *mmmLocal array of size nx3 X, Y, Z [m, m, m] +@param size_t nPoints Number of target points +@param float *rrmAER array of size nx3 of target point azimuth, elevation, +range [rad, rad, m] +*/ +void ENU2AERFloat(const float* mmmENU, size_t nPoints, float* rrmAER) +{ + size_t iPoint, i; + for (iPoint = 0; iPoint < nPoints; ++iPoint) { + i = iPoint * NCOORDSINPOINT; + rrmAER[i + 0] = atan2(mmmENU[i + 0], mmmENU[i + 1]); + rrmAER[i + 2] = sqrt(mmmENU[i + 0] * mmmENU[i + 0] + mmmENU[i + 1] * mmmENU[i + 1] + mmmENU[i + 2] * mmmENU[i + 2]); + rrmAER[i + 1] = asin(mmmENU[i + 2] / rrmAER[i + 2]); + } +} + +/* +ENU to AER transformation of double precision. +https://x-lumin.com/wp-content/uploads/2020/09/Coordinate_Transforms.pdf +https://www.lddgo.net/en/coordinate/ecef-enu + +@param double *mmmLocal array of size nx3 X, Y, Z [m, m, m] +@param size_t nPoints Number of target points +@param double *rrmAER array of size nx3 of target point azimuth, elevation, +range [rad, rad, m] +*/ +void ENU2AERDouble(const double* mmmENU, size_t nPoints, double* rrmAER) +{ + size_t iPoint, i; + for (iPoint = 0; iPoint < nPoints; ++iPoint) { + i = iPoint * NCOORDSINPOINT; + rrmAER[i + 0] = atan2(mmmENU[i + 0], mmmENU[i + 1]); + rrmAER[i + 2] = sqrt(mmmENU[i + 0] * mmmENU[i + 0] + mmmENU[i + 1] * mmmENU[i + 1] + mmmENU[i + 2] * mmmENU[i + 2]); + rrmAER[i + 1] = asin(mmmENU[i + 2] / rrmAER[i + 2]); + } +} + +/* +AER to ENU transformation of float precision. +https://x-lumin.com/wp-content/uploads/2020/09/Coordinate_Transforms.pdf + +@param float *rrmAER array of size nx3 of target point azimuth, elevation, +range [rad, rad, m] +@param size_t nPoints Number of target points +@param float *mmmLocal array of size nx3 X, Y, Z [m, m, m] +*/ +void AER2ENUFloat(const float* rrmAER, size_t nPoints, float* mmmENU) +{ + size_t iPoint, i; + for (iPoint = 0; iPoint < nPoints; ++iPoint) { + i = iPoint * NCOORDSINPOINT; + mmmENU[i + 0] = cos(rrmAER[i + 1]) * sin(rrmAER[i + 0]) * rrmAER[i + 2]; + mmmENU[i + 1] = cos(rrmAER[i + 1]) * cos(rrmAER[i + 0]) * rrmAER[i + 2]; + mmmENU[i + 2] = sin(rrmAER[i + 1]) * rrmAER[i + 2]; + } +} + +/* +AER to ENU transformation of double precision. +https://x-lumin.com/wp-content/uploads/2020/09/Coordinate_Transforms.pdf + +@param double *rrmAER array of size nx3 of target point azimuth, elevation, +range [rad, rad, m] +@param size_t nPoints Number of target points +@param double *mmmLocal array of size nx3 X, Y, Z [m, m, m] +*/ +void AER2ENUDouble(const double* rrmAER, size_t nPoints, double* mmmENU) +{ + size_t iPoint, i; + for (iPoint = 0; iPoint < nPoints; ++iPoint) { + i = iPoint * NCOORDSINPOINT; + mmmENU[i + 0] = cos(rrmAER[i + 1]) * sin(rrmAER[i + 0]) * rrmAER[i + 2]; + mmmENU[i + 1] = cos(rrmAER[i + 1]) * cos(rrmAER[i + 0]) * rrmAER[i + 2]; + mmmENU[i + 2] = sin(rrmAER[i + 1]) * rrmAER[i + 2]; + } +} + +static PyObject* +geodetic2ECEFWrapper(PyObject* self, PyObject* args) +{ + PyArrayObject* rrmLLA; + double a, b; + + // checks + if (!PyArg_ParseTuple(args, "O!dd", &PyArray_Type, &rrmLLA, &a, &b)) + return NULL; + if (!(PyArray_ISCONTIGUOUS(rrmLLA))) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); + return NULL; + } + if ((PyArray_SIZE(rrmLLA) % NCOORDSINPOINT) != 0) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a multiple of 3."); + return NULL; + } + + PyObject* result_array = PyArray_SimpleNew( + PyArray_NDIM(rrmLLA), PyArray_SHAPE(rrmLLA), PyArray_TYPE(rrmLLA)); + if (result_array == NULL) + return NULL; + size_t nPoints = PyArray_SIZE(rrmLLA) / NCOORDSINPOINT; + if (PyArray_TYPE(rrmLLA) == NPY_DOUBLE) { + double* data1 = (double*)PyArray_DATA(rrmLLA); + double* result_data = (double*)PyArray_DATA((PyArrayObject*)result_array); + geodetic2ECEFDouble(data1, nPoints, a, b, result_data); + } else if (PyArray_TYPE(rrmLLA) == NPY_FLOAT) { + float* data1 = (float*)PyArray_DATA(rrmLLA); + float* result_data = (float*)PyArray_DATA((PyArrayObject*)result_array); + geodetic2ECEFFloat(data1, nPoints, a, b, result_data); + } else { + PyErr_SetString(PyExc_ValueError, + "Only 32 and 64 bit float types accepted."); + return NULL; + } + return result_array; +} + +static PyObject* +ECEF2geodeticWrapper(PyObject* self, PyObject* args) +{ + PyArrayObject* mmmXYZ; + double a, b; + + // checks + if (!PyArg_ParseTuple(args, "O!dd", &PyArray_Type, &mmmXYZ, &a, &b)) + return NULL; + if (!(PyArray_ISCONTIGUOUS(mmmXYZ))) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); + return NULL; + } + if ((PyArray_SIZE(mmmXYZ) % NCOORDSINPOINT) != 0) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a multiple of 3."); + return NULL; + } + + PyObject* result_array = PyArray_SimpleNew( + PyArray_NDIM(mmmXYZ), PyArray_SHAPE(mmmXYZ), PyArray_TYPE(mmmXYZ)); + if (result_array == NULL) + return NULL; + size_t nPoints = PyArray_SIZE(mmmXYZ) / NCOORDSINPOINT; + if (PyArray_TYPE(mmmXYZ) == NPY_DOUBLE) { + double* data1 = (double*)PyArray_DATA(mmmXYZ); + double* result_data = (double*)PyArray_DATA((PyArrayObject*)result_array); + ECEF2geodeticDouble(data1, nPoints, a, b, result_data); + } else if (PyArray_TYPE(mmmXYZ) == NPY_FLOAT) { + float* data1 = (float*)PyArray_DATA(mmmXYZ); + float* result_data = (float*)PyArray_DATA((PyArrayObject*)result_array); + ECEF2geodeticFloat(data1, nPoints, a, b, result_data); + } else { + PyErr_SetString(PyExc_ValueError, + "Only 32 and 64 bit float types accepted."); + return NULL; + } + return result_array; +} + +static PyObject* +ECEF2ENUWrapper(PyObject* self, PyObject* args) +{ + PyArrayObject *rrmLLALocalOrigin, *mmmXYZTarget; + double a, b; + + // checks + if (!PyArg_ParseTuple(args, + "O!O!dd", + &PyArray_Type, + &rrmLLALocalOrigin, + &PyArray_Type, + &mmmXYZTarget, + &a, + &b)) + return NULL; + if (!(PyArray_ISCONTIGUOUS(rrmLLALocalOrigin)) || !(PyArray_ISCONTIGUOUS(mmmXYZTarget))) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); + return NULL; + } + if (!((PyArray_NDIM(rrmLLALocalOrigin) == PyArray_NDIM(mmmXYZTarget)) && (PyArray_SIZE(rrmLLALocalOrigin) == PyArray_SIZE(mmmXYZTarget)) || ((PyArray_Size(rrmLLALocalOrigin) == NCOORDSINPOINT) && (PyArray_SIZE(rrmLLALocalOrigin) < PyArray_SIZE(mmmXYZTarget))))) { + PyErr_SetString(PyExc_ValueError, + "Input arrays must have matching size and dimensions or " + "the origin must be of size three."); + return NULL; + } + if ((PyArray_SIZE(rrmLLALocalOrigin) % NCOORDSINPOINT) != 0 || (PyArray_SIZE(mmmXYZTarget) % NCOORDSINPOINT) != 0) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a multiple of 3."); + return NULL; + } + if (PyArray_TYPE(rrmLLALocalOrigin) != PyArray_TYPE(mmmXYZTarget)) { + PyErr_SetString(PyExc_ValueError, "Input arrays must have the same type."); + return NULL; + } + + PyObject* result_array = PyArray_SimpleNew(PyArray_NDIM(mmmXYZTarget), + PyArray_SHAPE(mmmXYZTarget), + PyArray_TYPE(mmmXYZTarget)); + if (result_array == NULL) + return NULL; + size_t nPoints = PyArray_SIZE(mmmXYZTarget) / NCOORDSINPOINT; + int isOriginSizeOfTargets = (PyArray_Size(rrmLLALocalOrigin) == PyArray_Size(mmmXYZTarget)); + if (PyArray_TYPE(rrmLLALocalOrigin) == NPY_DOUBLE) { + double* data1 = (double*)PyArray_DATA(rrmLLALocalOrigin); + double* data2 = (double*)PyArray_DATA(mmmXYZTarget); + double* result_data = (double*)PyArray_DATA((PyArrayObject*)result_array); + ECEF2ENUDouble( + data1, data2, nPoints, isOriginSizeOfTargets, a, b, result_data); + } else if (PyArray_TYPE(rrmLLALocalOrigin) == NPY_FLOAT) { + float* data1 = (float*)PyArray_DATA(rrmLLALocalOrigin); + float* data2 = (float*)PyArray_DATA(mmmXYZTarget); + float* result_data = (float*)PyArray_DATA((PyArrayObject*)result_array); + ECEF2ENUFloat( + data1, data2, nPoints, isOriginSizeOfTargets, a, b, result_data); + } else { + PyErr_SetString(PyExc_ValueError, + "Only 32 and 64 bit float types accepted."); + return NULL; + } + return result_array; +} + +static PyObject* +ENU2ECEFWrapper(PyObject* self, PyObject* args) +{ + PyArrayObject *rrmLLALocalOrigin, *mmmLocal; + double a, b; + + // checks + if (!PyArg_ParseTuple(args, + "O!O!dd", + &PyArray_Type, + &rrmLLALocalOrigin, + &PyArray_Type, + &mmmLocal, + &a, + &b)) + return NULL; + if (!(PyArray_ISCONTIGUOUS(rrmLLALocalOrigin)) || !(PyArray_ISCONTIGUOUS(mmmLocal))) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); + return NULL; + } + if (!((PyArray_NDIM(rrmLLALocalOrigin) == PyArray_NDIM(mmmLocal)) && (PyArray_SIZE(rrmLLALocalOrigin) == PyArray_SIZE(mmmLocal)) || ((PyArray_Size(rrmLLALocalOrigin) == NCOORDSINPOINT) && (PyArray_SIZE(rrmLLALocalOrigin) < PyArray_SIZE(mmmLocal))))) { + PyErr_SetString(PyExc_ValueError, + "Input arrays must have matching size and dimensions or " + "the origin must be of size three."); + return NULL; + } + if ((PyArray_SIZE(rrmLLALocalOrigin) % NCOORDSINPOINT) != 0 || (PyArray_SIZE(mmmLocal) % NCOORDSINPOINT) != 0) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a multiple of 3."); + return NULL; + } + if (PyArray_TYPE(rrmLLALocalOrigin) != PyArray_TYPE(mmmLocal)) { + PyErr_SetString(PyExc_ValueError, "Input arrays must have the same type."); + return NULL; + } + + PyObject* result_array = PyArray_SimpleNew( + PyArray_NDIM(mmmLocal), PyArray_SHAPE(mmmLocal), PyArray_TYPE(mmmLocal)); + if (result_array == NULL) + return NULL; + size_t nPoints = PyArray_SIZE(mmmLocal) / NCOORDSINPOINT; + int isOriginSizeOfTargets = (PyArray_Size(rrmLLALocalOrigin) == PyArray_Size(mmmLocal)); + if (PyArray_TYPE(rrmLLALocalOrigin) == NPY_DOUBLE) { + double* data1 = (double*)PyArray_DATA(rrmLLALocalOrigin); + double* data2 = (double*)PyArray_DATA(mmmLocal); + double* result_data = (double*)PyArray_DATA((PyArrayObject*)result_array); + ENU2ECEFDouble( + data1, data2, nPoints, isOriginSizeOfTargets, a, b, result_data); + } else if (PyArray_TYPE(rrmLLALocalOrigin) == NPY_FLOAT) { + float* data1 = (float*)PyArray_DATA(rrmLLALocalOrigin); + float* data2 = (float*)PyArray_DATA(mmmLocal); + float* result_data = (float*)PyArray_DATA((PyArrayObject*)result_array); + ENU2ECEFFloat( + data1, data2, nPoints, isOriginSizeOfTargets, a, b, result_data); + } else { + PyErr_SetString(PyExc_ValueError, + "Only 32 and 64 bit float types accepted."); + return NULL; + } + return result_array; +} + +static PyObject* +ENU2AERWrapper(PyObject* self, PyObject* args) +{ + PyArrayObject* mmmENU; + + // checks + if (!PyArg_ParseTuple(args, "O!", &PyArray_Type, &mmmENU)) + return NULL; + if (!(PyArray_ISCONTIGUOUS(mmmENU))) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); + return NULL; + } + if ((PyArray_SIZE(mmmENU) % NCOORDSINPOINT) != 0) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a multiple of 3."); + return NULL; + } + + PyObject* result_array = PyArray_SimpleNew( + PyArray_NDIM(mmmENU), PyArray_SHAPE(mmmENU), PyArray_TYPE(mmmENU)); + if (result_array == NULL) + return NULL; + size_t nPoints = PyArray_SIZE(mmmENU) / NCOORDSINPOINT; + if (PyArray_TYPE(mmmENU) == NPY_DOUBLE) { + double* data1 = (double*)PyArray_DATA(mmmENU); + double* result_data = (double*)PyArray_DATA((PyArrayObject*)result_array); + ENU2AERDouble(data1, nPoints, result_data); + } else if (PyArray_TYPE(mmmENU) == NPY_FLOAT) { + float* data1 = (float*)PyArray_DATA(mmmENU); + float* result_data = (float*)PyArray_DATA((PyArrayObject*)result_array); + ENU2AERFloat(data1, nPoints, result_data); + } else { + PyErr_SetString(PyExc_ValueError, + "Only 32 and 64 bit float types accepted."); + return NULL; + } + return result_array; +} + +static PyObject* +AER2ENUWrapper(PyObject* self, PyObject* args) +{ + PyArrayObject* rrmAER; + + // checks + if (!PyArg_ParseTuple(args, "O!", &PyArray_Type, &rrmAER)) + return NULL; + if (!(PyArray_ISCONTIGUOUS(rrmAER))) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); + return NULL; + } + if ((PyArray_SIZE(rrmAER) % NCOORDSINPOINT) != 0) { + PyErr_SetString(PyExc_ValueError, "Input arrays must be a multiple of 3."); + return NULL; + } + + PyObject* result_array = PyArray_SimpleNew( + PyArray_NDIM(rrmAER), PyArray_SHAPE(rrmAER), PyArray_TYPE(rrmAER)); + if (result_array == NULL) + return NULL; + size_t nPoints = PyArray_SIZE(rrmAER) / NCOORDSINPOINT; + if (PyArray_TYPE(rrmAER) == NPY_DOUBLE) { + double* data1 = (double*)PyArray_DATA(rrmAER); + double* result_data = (double*)PyArray_DATA((PyArrayObject*)result_array); + AER2ENUDouble(data1, nPoints, result_data); + } else if (PyArray_TYPE(rrmAER) == NPY_FLOAT) { + float* data1 = (float*)PyArray_DATA(rrmAER); + float* result_data = (float*)PyArray_DATA((PyArrayObject*)result_array); + AER2ENUFloat(data1, nPoints, result_data); + } else { + PyErr_SetString(PyExc_ValueError, + "Only 32 and 64 bit float types accepted."); + return NULL; + } + return result_array; +} + +// Method definition object for this extension, these argumens mean: +// ml_name: The name of the method +// ml_meth: Function pointer to the method implementation +// ml_flags: Flags indicating special features of this method, such as +// accepting arguments, accepting keyword arguments, being a class +// method, or being a static method of a class. +// ml_doc: The docstring for the method +static PyMethodDef MyMethods[] = { + { "geodetic2ECEF", + geodetic2ECEFWrapper, + METH_VARARGS, + "Convert geodetic coordinate system to ECEF." }, + { "ECEF2geodetic", + ECEF2geodeticWrapper, + METH_VARARGS, + "Convert ECEF to geodetic coordinate system." }, + { "ECEF2ENU", ECEF2ENUWrapper, METH_VARARGS, "Convert ECEF to ENU." }, + { "ENU2ECEF", ENU2ECEFWrapper, METH_VARARGS, "Convert ENU to ECEF." }, + { "ENU2AER", ENU2AERWrapper, METH_VARARGS, "Convert ENU to AER." }, + { "AER2ENU", AER2ENUWrapper, METH_VARARGS, "Convert AER to ENU." }, + { NULL, NULL, 0, NULL } +}; + +// Module definition +static struct PyModuleDef transforms = { + PyModuleDef_HEAD_INIT, + "transforms", + "Module that contains transform functions.", + -1, + MyMethods +}; + +// Module initialization function +PyMODINIT_FUNC +PyInit_transforms(void) +{ + import_array(); // Initialize the NumPy C API + return PyModule_Create(&transforms); +} diff --git a/setup.py b/setup.py index 10b801f..8a74187 100644 --- a/setup.py +++ b/setup.py @@ -5,17 +5,17 @@ ext_modules=[ Extension( "transforms84.transforms", - sources=["transforms.c"], + sources=["include/transforms.c"], include_dirs=[np.get_include()], ), Extension( "transforms84.distances", - sources=["distances.c"], + sources=["include/distances.c"], include_dirs=[np.get_include()], ), Extension( "transforms84.helpers", - sources=["helpers.c"], + sources=["include/helpers.c"], include_dirs=[np.get_include()], ), ], diff --git a/src/transforms84/__init__.py b/src/transforms84/__init__.py index f27cd76..7ea9c88 100644 --- a/src/transforms84/__init__.py +++ b/src/transforms84/__init__.py @@ -1,4 +1,4 @@ import numpy as np DTYPES_SUPPORTED = np.float32 | np.float64 -__version__ = "0.1.1" +__version__ = "0.2.0" diff --git a/tests/test_ECEF2ENU.py b/tests/test_ECEF2ENU.py index 7626a4f..c482a12 100644 --- a/tests/test_ECEF2ENU.py +++ b/tests/test_ECEF2ENU.py @@ -1,8 +1,9 @@ import numpy as np import pytest +from transforms84.helpers import DDM2RRM from transforms84.systems import WGS84 -from transforms84.transforms import ECEF2ENU +from transforms84.transforms import ECEF2ENU, geodetic2ECEF # https://www.lddgo.net/en/coordinate/ecef-enu @@ -35,16 +36,13 @@ def test_ECEF2ENU_raise_wrong_size(): ref_point = np.array([[5010306], [2336344], [3170376.2], [1]], dtype=np.float64) with pytest.raises(ValueError): ECEF2ENU(ref_point, ENU, WGS84.a, WGS84.b) - - -def test_ENU2ECEF_raise_wrong_size(): XYZ = np.array([[3906.67536618], [2732.16708], [1519.47079847]], dtype=np.float32) ref_point = np.array([[5010306], [2336344], [3170376.2], [1]], dtype=np.float64) with pytest.raises(ValueError): ECEF2ENU(ref_point, XYZ, WGS84.a, WGS84.b) -def test_ENU2ECEF_float32_point(tolerance_float_atol): +def test_ECEF2ENU_float32_point(tolerance_float_atol): XYZ = np.array([[3906.67536618], [2732.16708], [1519.47079847]], dtype=np.float32) ref_point = np.array([[0.1], [0.2], [5000]], dtype=np.float32) out = ECEF2ENU(ref_point, XYZ, WGS84.a, WGS84.b) @@ -53,7 +51,7 @@ def test_ENU2ECEF_float32_point(tolerance_float_atol): assert np.isclose(out[2, 0], -6378422.76482545, atol=tolerance_float_atol) -def test_ENU2ECEF_float64_point(): +def test_ECEF2ENU_float64_point(): XYZ = np.array([[3906.67536618], [2732.16708], [1519.47079847]], dtype=np.float64) ref_point = np.array([[0.1], [0.2], [5000]], dtype=np.float64) out = ECEF2ENU(ref_point, XYZ, WGS84.a, WGS84.b) @@ -62,7 +60,7 @@ def test_ENU2ECEF_float64_point(): assert np.isclose(out[2, 0], -6378422.76482545) -def test_ENU2ECEF_float32_points(tolerance_float_atol): +def test_ECEF2ENU_float32_points(tolerance_float_atol): XYZ = np.array( [ [[3906.67536618], [2732.16708], [1519.47079847]], @@ -81,7 +79,7 @@ def test_ENU2ECEF_float32_points(tolerance_float_atol): ) -def test_ENU2ECEF_float64_points(): +def test_ECEF2ENU_float64_points(): XYZ = np.array( [ [[3906.67536618], [2732.16708], [1519.47079847]], @@ -96,3 +94,43 @@ def test_ENU2ECEF_float64_points(): assert np.all(np.isclose(out[:, 0, 0], 1901.5690521235)) assert np.all(np.isclose(out[:, 1, 0], 5316.9485968901)) assert np.all(np.isclose(out[:, 2, 0], -6378422.76482545)) + + +def test_ECEF2ENU_one2many_double(): + rrm_target = DDM2RRM(np.array([[31], [32], [0]], dtype=np.float64)) + num_repeats = 3 + rrm_targets = np.ascontiguousarray( + np.tile(rrm_target, num_repeats).T.reshape((-1, 3, 1)) + ) + rrm_local = DDM2RRM(np.array([[30], [31], [0]], dtype=np.float64)) + rrm_locals = np.ascontiguousarray( + np.tile(rrm_local, rrm_targets.shape[0]).T.reshape((-1, 3, 1)) + ) + assert np.all( + ECEF2ENU( + rrm_locals, geodetic2ECEF(rrm_targets, WGS84.a, WGS84.b), WGS84.a, WGS84.b + ) + == ECEF2ENU( + rrm_local, geodetic2ECEF(rrm_targets, WGS84.a, WGS84.b), WGS84.a, WGS84.b + ) + ) + + +def test_ECEF2ENU_one2many_float(): + rrm_target = DDM2RRM(np.array([[31], [32], [0]], dtype=np.float32)) + num_repeats = 3 + rrm_targets = np.ascontiguousarray( + np.tile(rrm_target, num_repeats).T.reshape((-1, 3, 1)) + ) + rrm_local = DDM2RRM(np.array([[30], [31], [0]], dtype=np.float32)) + rrm_locals = np.ascontiguousarray( + np.tile(rrm_local, rrm_targets.shape[0]).T.reshape((-1, 3, 1)) + ) + assert np.all( + ECEF2ENU( + rrm_locals, geodetic2ECEF(rrm_targets, WGS84.a, WGS84.b), WGS84.a, WGS84.b + ) + == ECEF2ENU( + rrm_local, geodetic2ECEF(rrm_targets, WGS84.a, WGS84.b), WGS84.a, WGS84.b + ) + ) diff --git a/tests/test_ENU2ECEF.py b/tests/test_ENU2ECEF.py index 8b6cb05..157d642 100644 --- a/tests/test_ENU2ECEF.py +++ b/tests/test_ENU2ECEF.py @@ -1,8 +1,9 @@ import numpy as np import pytest +from transforms84.helpers import DDM2RRM from transforms84.systems import WGS84 -from transforms84.transforms import ENU2ECEF +from transforms84.transforms import ENU2ECEF, geodetic2ECEF # https://www.lddgo.net/en/coordinate/ecef-enu @@ -84,3 +85,43 @@ def test_ENU2ECEF_float64_points(): assert np.all(np.isclose(out[:, 0, 0], 3906.67536618)) assert np.all(np.isclose(out[:, 1, 0], 2732.16708)) assert np.all(np.isclose(out[:, 2, 0], 1519.47079847)) + + +def test_ENU2ECEF_one2many_double(): + rrm_target = DDM2RRM(np.array([[31], [32], [0]], dtype=np.float64)) + num_repeats = 3 + rrm_targets = np.ascontiguousarray( + np.tile(rrm_target, num_repeats).T.reshape((-1, 3, 1)) + ) + rrm_local = DDM2RRM(np.array([[30], [31], [0]], dtype=np.float64)) + rrm_locals = np.ascontiguousarray( + np.tile(rrm_local, rrm_targets.shape[0]).T.reshape((-1, 3, 1)) + ) + assert np.all( + ENU2ECEF( + rrm_locals, geodetic2ECEF(rrm_targets, WGS84.a, WGS84.b), WGS84.a, WGS84.b + ) + == ENU2ECEF( + rrm_local, geodetic2ECEF(rrm_targets, WGS84.a, WGS84.b), WGS84.a, WGS84.b + ) + ) + + +def test_ENU2ECEF_one2many_float(): + rrm_target = DDM2RRM(np.array([[31], [32], [0]], dtype=np.float32)) + num_repeats = 3 + rrm_targets = np.ascontiguousarray( + np.tile(rrm_target, num_repeats).T.reshape((-1, 3, 1)) + ) + rrm_local = DDM2RRM(np.array([[30], [31], [0]], dtype=np.float32)) + rrm_locals = np.ascontiguousarray( + np.tile(rrm_local, rrm_targets.shape[0]).T.reshape((-1, 3, 1)) + ) + assert np.all( + ENU2ECEF( + rrm_locals, geodetic2ECEF(rrm_targets, WGS84.a, WGS84.b), WGS84.a, WGS84.b + ) + == ENU2ECEF( + rrm_local, geodetic2ECEF(rrm_targets, WGS84.a, WGS84.b), WGS84.a, WGS84.b + ) + ) diff --git a/tests/test_distances.py b/tests/test_distances.py index 6799986..3cfe346 100644 --- a/tests/test_distances.py +++ b/tests/test_distances.py @@ -2,6 +2,7 @@ import pytest from transforms84.distances import Haversine +from transforms84.helpers import DDM2RRM from transforms84.systems import WGS84 # https://calculator.academy/haversine-distance-calculator/ @@ -97,3 +98,35 @@ def test_Haversine_with_height_double(): Haversine(rrm_start, rrm_end, WGS84.mean_radius), Haversine(rrm_start_with_height, rrm_end, WGS84.mean_radius), ) + + +def test_Haversine_one2many_double(): + rrm_target = DDM2RRM(np.array([[31], [32], [0]], dtype=np.float64)) + num_repeats = 3 + rrm_targets = np.ascontiguousarray( + np.tile(rrm_target, num_repeats).T.reshape((-1, 3, 1)) + ) + rrm_local = DDM2RRM(np.array([[30], [31], [0]], dtype=np.float64)) + rrm_locals = np.ascontiguousarray( + np.tile(rrm_local, rrm_targets.shape[0]).T.reshape((-1, 3, 1)) + ) + assert np.all( + Haversine(rrm_local, rrm_targets, WGS84.mean_radius) + == Haversine(rrm_locals, rrm_targets, WGS84.mean_radius) + ) + + +def test_Haversine_one2many_float(): + rrm_target = DDM2RRM(np.array([[31], [32], [0]], dtype=np.float32)) + num_repeats = 3 + rrm_targets = np.ascontiguousarray( + np.tile(rrm_target, num_repeats).T.reshape((-1, 3, 1)) + ) + rrm_local = DDM2RRM(np.array([[30], [31], [0]], dtype=np.float32)) + rrm_locals = np.ascontiguousarray( + np.tile(rrm_local, rrm_targets.shape[0]).T.reshape((-1, 3, 1)) + ) + assert np.all( + Haversine(rrm_local, rrm_targets, WGS84.mean_radius) + == Haversine(rrm_locals, rrm_targets, WGS84.mean_radius) + ) diff --git a/transforms.c b/transforms.c deleted file mode 100644 index 4a8faaf..0000000 --- a/transforms.c +++ /dev/null @@ -1,649 +0,0 @@ -#include -#include - -/* -Geodetic to ECEF transformation of float precision. -https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates - -@param double *rrmLLA array of size nx3 latitude (phi), longitude (gamma), height (h) [rad, rad, m] -@param size_t nPoints Number of LLA points -@param double a semi-major axis -@param double b semi-minor axis -@param double *mmmXYZ array of size nx3 X, Y, Z [rad, rad, m] -*/ -void geodetic2ECEFFloat(const float *rrmLLA, size_t nPoints, double a, double b, float *mmmXYZ) -{ - float e2 = 1 - (b * b) / (a * a); - size_t iPoint, i; - float N; - for (iPoint = 0; iPoint < nPoints; ++iPoint) - { - i = iPoint * 3; - N = a / sqrt(1 - e2 * (sin(rrmLLA[i + 0]) * sin(rrmLLA[i + 0]))); - mmmXYZ[i + 0] = (N + rrmLLA[i + 2]) * cos(rrmLLA[i + 0]) * cos(rrmLLA[i + 1]); - mmmXYZ[i + 1] = (N + rrmLLA[i + 2]) * cos(rrmLLA[i + 0]) * sin(rrmLLA[i + 1]); - mmmXYZ[i + 2] = ((1 - e2) * N + rrmLLA[i + 2]) * sin(rrmLLA[i + 0]); - } -} - -/* -Geodetic to ECEF transformation of double precision. -https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates - -@param double *rrmLLA array of size nx3 latitude (phi), longitude (gamma), height (h) [rad, rad, m] -@param size_t nPoints Number of LLA points -@param double a semi-major axis -@param double b semi-minor axis -@param double *mmmXYZ array of size nx3 X, Y, Z [m, m, m] -*/ -void geodetic2ECEFDouble(const double *rrmLLA, size_t nPoints, double a, double b, double *mmmXYZ) -{ - double e2 = 1 - (b * b) / (a * a); - size_t iPoint, i; - double N; - for (iPoint = 0; iPoint < nPoints; ++iPoint) - { - i = iPoint * 3; - N = a / sqrt(1 - e2 * sin(rrmLLA[i + 0]) * sin(rrmLLA[i + 0])); - mmmXYZ[i + 0] = (N + rrmLLA[i + 2]) * cos(rrmLLA[i + 0]) * cos(rrmLLA[i + 1]); - mmmXYZ[i + 1] = (N + rrmLLA[i + 2]) * cos(rrmLLA[i + 0]) * sin(rrmLLA[i + 1]); - mmmXYZ[i + 2] = ((1 - e2) * N + rrmLLA[i + 2]) * sin(rrmLLA[i + 0]); - } -} - -/* -ECEF to geodetic transformation of float precision. -https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#The_application_of_Ferrari's_solution - -@param double *mmmXYZ array of size nx3 X, Y, Z [m, m, m] -@param size_t nPoints Number of ECEF points -@param double a semi-major axis -@param double b semi-minor axis -@param double *rrmLLA array of size nx3 latitude (phi), longitude (gamma), height (h) [rad, rad, m] -*/ -void ECEF2geodeticFloat(const float *mmmXYZ, size_t nPoints, double a, double b, float *rrmLLA) -{ - float e2 = ((a * a) - (b * b)) / (a * a); - float ed2 = ((a * a) - (b * b)) / (b * b); - size_t iPoint, i; - for (iPoint = 0; iPoint < nPoints; ++iPoint) - { - i = iPoint * 3; - float p = sqrt(mmmXYZ[i + 0] * mmmXYZ[i + 0] + mmmXYZ[i + 1] * mmmXYZ[i + 1]); - float F = 54 * b * b * mmmXYZ[i + 2] * mmmXYZ[i + 2]; - float G = p * p + (1 - e2) * mmmXYZ[i + 2] * mmmXYZ[i + 2] - e2 * (a * a - b * b); - float c = e2 * e2 * F * p * p / (G * G * G); - float s = cbrt(1 + c + sqrt(c * c + 2 * c)); - float k = s + 1 + 1 / s; - float P = F / (3 * k * k * G * G); - float Q = sqrt(1 + 2 * e2 * e2 * P); - float r0 = -P * e2 * p / (1 + Q) + sqrt(0.5 * a * a * (1 + 1 / Q) - P * (1 - e2) * mmmXYZ[i + 2] * mmmXYZ[i + 2] / (Q * (1 + Q)) - 0.5 * P * p * p); - float U = sqrt((p - e2 * r0) * (p - e2 * r0) + mmmXYZ[i + 2] * mmmXYZ[i + 2]); - float V = sqrt((p - e2 * r0) * (p - e2 * r0) + (1 - e2) * mmmXYZ[i + 2] * mmmXYZ[i + 2]); - float z0 = b * b * mmmXYZ[i + 2] / (a * V); - rrmLLA[i + 0] = atan((mmmXYZ[i + 2] + ed2 * z0) / p); - rrmLLA[i + 1] = atan2(mmmXYZ[i + 1], mmmXYZ[i + 0]); - rrmLLA[i + 2] = U * (1 - b * b / (a * V)); - } -} - -/* -ECEF to geodetic transformation of double precision. -https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#The_application_of_Ferrari's_solution - -@param double *mmmXYZ array of size nx3 X, Y, Z [m, m, m] -@param size_t nPoints Number of ECEF points -@param double a semi-major axis -@param double b semi-minor axis -@param double *rrmLLA array of size nx3 latitude (phi), longitude (gamma), height (h) [rad, rad, m] -*/ -void ECEF2geodeticDouble(const double *mmmXYZ, size_t nPoints, double a, double b, double *rrmLLA) -{ - - double e2 = ((a * a) - (b * b)) / (a * a); - double ed2 = ((a * a) - (b * b)) / (b * b); - size_t iPoint, i; - for (iPoint = 0; iPoint < nPoints; ++iPoint) - { - i = iPoint * 3; - double p = sqrt(mmmXYZ[i + 0] * mmmXYZ[i + 0] + mmmXYZ[i + 1] * mmmXYZ[i + 1]); - double F = 54 * b * b * mmmXYZ[i + 2] * mmmXYZ[i + 2]; - double G = p * p + (1 - e2) * mmmXYZ[i + 2] * mmmXYZ[i + 2] - e2 * (a * a - b * b); - double c = e2 * e2 * F * p * p / (G * G * G); - double s = cbrt(1 + c + sqrt(c * c + 2 * c)); - double k = s + 1 + 1 / s; - double P = F / (3 * k * k * G * G); - double Q = sqrt(1 + 2 * e2 * e2 * P); - double r0 = -P * e2 * p / (1 + Q) + sqrt(0.5 * a * a * (1 + 1 / Q) - P * (1 - e2) * mmmXYZ[i + 2] * mmmXYZ[i + 2] / (Q * (1 + Q)) - 0.5 * P * p * p); - double U = sqrt((p - e2 * r0) * (p - e2 * r0) + mmmXYZ[i + 2] * mmmXYZ[i + 2]); - double V = sqrt((p - e2 * r0) * (p - e2 * r0) + (1 - e2) * mmmXYZ[i + 2] * mmmXYZ[i + 2]); - double z0 = b * b * mmmXYZ[i + 2] / (a * V); - rrmLLA[i + 0] = atan((mmmXYZ[i + 2] + ed2 * z0) / p); - rrmLLA[i + 1] = atan2(mmmXYZ[i + 1], mmmXYZ[i + 0]); - rrmLLA[i + 2] = U * (1 - b * b / (a * V)); - } -} - -/* -ECEF to ENU transformation of float precision. -https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ECEF_to_ENU - -@param double *rrmLLALocalOrigin array of size nx3 of local reference point X, Y, Z [m, m, m] -@param double *mmmXYZTarget array of size nx3 of target point X, Y, Z [m, m, m] -@param size_t nPoints Number of target points -@param double a semi-major axis -@param double b semi-minor axis -@param double *mmmLocal array of size nx3 X, Y, Z [m, m, m] -*/ -void ECEF2ENUFloat(const float *rrmLLALocalOrigin, const float *mmmXYZTarget, size_t nPoints, double a, double b, float *mmmLocal) -{ - float mmmXYZLocalOrigin[3] = {0, 0, 0}; - float DeltaX, DeltaY, DeltaZ; - geodetic2ECEFFloat(rrmLLALocalOrigin, 1, a, b, mmmXYZLocalOrigin); - size_t iPoint, i; - for (iPoint = 0; iPoint < nPoints; ++iPoint) - { - i = iPoint * 3; - DeltaX = mmmXYZTarget[i + 0] - mmmXYZLocalOrigin[0]; - DeltaY = mmmXYZTarget[i + 1] - mmmXYZLocalOrigin[1]; - DeltaZ = mmmXYZTarget[i + 2] - mmmXYZLocalOrigin[2]; - mmmLocal[i + 0] = -sin(rrmLLALocalOrigin[i + 1]) * DeltaX + cos(rrmLLALocalOrigin[i + 1]) * DeltaY; - mmmLocal[i + 1] = -sin(rrmLLALocalOrigin[i + 0]) * cos(rrmLLALocalOrigin[i + 1]) * DeltaX + -sin(rrmLLALocalOrigin[i + 0]) * sin(rrmLLALocalOrigin[i + 1]) * DeltaY + cos(rrmLLALocalOrigin[i + 0]) * DeltaZ; - mmmLocal[i + 2] = cos(rrmLLALocalOrigin[i + 0]) * cos(rrmLLALocalOrigin[i + 1]) * DeltaX + cos(rrmLLALocalOrigin[i + 0]) * sin(rrmLLALocalOrigin[i + 1]) * DeltaY + sin(rrmLLALocalOrigin[i + 0]) * DeltaZ; - } -} - -/* -ECEF to ENU transformation of double precision. -https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ECEF_to_ENU - -@param double *rrmLLALocalOrigin array of size nx3 of local reference point X, Y, Z [m, m, m] -@param double *mmmXYZTarget array of size nx3 of target point X, Y, Z [m, m, m] -@param size_t nPoints Number of target points -@param double a semi-major axis -@param double b semi-minor axis -@param double *mmmLocal array of size nx3 X, Y, Z [m, m, m] -*/ -void ECEF2ENUDouble(const double *rrmLLALocalOrigin, const double *mmmXYZTarget, size_t nPoints, double a, double b, double *mmmLocal) -{ - double mmmXYZLocalOrigin[3] = {0.0, 0.0, 0.0}; - double DeltaX, DeltaY, DeltaZ; - geodetic2ECEFDouble(rrmLLALocalOrigin, 1, a, b, mmmXYZLocalOrigin); - size_t iPoint, i; - for (iPoint = 0; iPoint < nPoints; ++iPoint) - { - i = iPoint * 3; - DeltaX = mmmXYZTarget[i + 0] - mmmXYZLocalOrigin[0]; - DeltaY = mmmXYZTarget[i + 1] - mmmXYZLocalOrigin[1]; - DeltaZ = mmmXYZTarget[i + 2] - mmmXYZLocalOrigin[2]; - mmmLocal[i + 0] = -sin(rrmLLALocalOrigin[i + 1]) * DeltaX + cos(rrmLLALocalOrigin[i + 1]) * DeltaY; - mmmLocal[i + 1] = -sin(rrmLLALocalOrigin[i + 0]) * cos(rrmLLALocalOrigin[i + 1]) * DeltaX + -sin(rrmLLALocalOrigin[i + 0]) * sin(rrmLLALocalOrigin[i + 1]) * DeltaY + cos(rrmLLALocalOrigin[i + 0]) * DeltaZ; - mmmLocal[i + 2] = cos(rrmLLALocalOrigin[i + 0]) * cos(rrmLLALocalOrigin[i + 1]) * DeltaX + cos(rrmLLALocalOrigin[i + 0]) * sin(rrmLLALocalOrigin[i + 1]) * DeltaY + sin(rrmLLALocalOrigin[i + 0]) * DeltaZ; - } -} - -/* -ECEF to ENU transformation of float precision. -https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ENU_to_ECEF -https://www.lddgo.net/en/coordinate/ecef-enu - -@param double *rrmLLALocalOrigin array of size nx3 of local reference point latitude, longitude, height [rad, rad, m] -@param float *mmmXYZTarget array of size nx3 of target point X, Y, Z [m, m, m] -@param size_t nPoints Number of target points -@param double a semi-major axis -@param double b semi-minor axis -@param float *mmmLocal array of size nx3 X, Y, Z [m, m, m] -*/ -void ENU2ECEFFloat(const float *rrmLLALocalOrigin, const float *mmmLocal, size_t nPoints, double a, double b, float *mmmXYZTarget) -{ - float mmmXYZLocalOrigin[3] = {0, 0, 0}; - geodetic2ECEFFloat(rrmLLALocalOrigin, 1, a, b, mmmXYZLocalOrigin); - size_t iPoint, i; - for (iPoint = 0; iPoint < nPoints; ++iPoint) - { - i = iPoint * 3; - mmmXYZTarget[i + 0] = -sin(rrmLLALocalOrigin[i + 1]) * mmmLocal[i + 0] + -sin(rrmLLALocalOrigin[i + 0]) * cos(rrmLLALocalOrigin[i + 1]) * mmmLocal[i + 1] + cos(rrmLLALocalOrigin[i + 0]) * cos(rrmLLALocalOrigin[i + 1]) * mmmLocal[i + 2] + mmmXYZLocalOrigin[0]; - mmmXYZTarget[i + 1] = cos(rrmLLALocalOrigin[i + 1]) * mmmLocal[i + 0] + -sin(rrmLLALocalOrigin[i + 0]) * sin(rrmLLALocalOrigin[i + 1]) * mmmLocal[i + 1] + cos(rrmLLALocalOrigin[i + 0]) * sin(rrmLLALocalOrigin[i + 1]) * mmmLocal[2] + mmmXYZLocalOrigin[1]; - mmmXYZTarget[i + 2] = cos(rrmLLALocalOrigin[i + 0]) * mmmLocal[i + 1] + sin(rrmLLALocalOrigin[i + 0]) * mmmLocal[2] + mmmXYZLocalOrigin[2]; - } -} - -/* -ECEF to ENU transformation of double precision. -https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ENU_to_ECEF -https://www.lddgo.net/en/coordinate/ecef-enu - -@param double *rrmLLALocalOrigin array of size nx3 of local reference point latitude, longitude, height [rad, rad, m] -@param double *mmmLocal array of size nx3 X, Y, Z [m, m, m] -@param size_t nPoints Number of target points -@param double a semi-major axis -@param double b semi-minor axis -@param double *mmmXYZTarget array of size nx3 of target point X, Y, Z [m, m, m] -*/ -void ENU2ECEFDouble(const double *rrmLLALocalOrigin, const double *mmmLocal, size_t nPoints, double a, double b, double *mmmXYZTarget) -{ - double mmmXYZLocalOrigin[3] = {0, 0, 0}; - geodetic2ECEFDouble(rrmLLALocalOrigin, 1, a, b, mmmXYZLocalOrigin); - size_t iPoint, i; - for (iPoint = 0; iPoint < nPoints; ++iPoint) - { - i = iPoint * 3; - mmmXYZTarget[i + 0] = -sin(rrmLLALocalOrigin[i + 1]) * mmmLocal[i + 0] + -sin(rrmLLALocalOrigin[i + 0]) * cos(rrmLLALocalOrigin[i + 1]) * mmmLocal[i + 1] + cos(rrmLLALocalOrigin[i + 0]) * cos(rrmLLALocalOrigin[i + 1]) * mmmLocal[i + 2] + mmmXYZLocalOrigin[0]; - mmmXYZTarget[i + 1] = cos(rrmLLALocalOrigin[i + 1]) * mmmLocal[i + 0] + -sin(rrmLLALocalOrigin[i + 0]) * sin(rrmLLALocalOrigin[i + 1]) * mmmLocal[i + 1] + cos(rrmLLALocalOrigin[i + 0]) * sin(rrmLLALocalOrigin[i + 1]) * mmmLocal[2] + mmmXYZLocalOrigin[1]; - mmmXYZTarget[i + 2] = cos(rrmLLALocalOrigin[i + 0]) * mmmLocal[i + 1] + sin(rrmLLALocalOrigin[i + 0]) * mmmLocal[i + 2] + mmmXYZLocalOrigin[2]; - } -} - -/* -ENU to AER transformation of float precision. -https://x-lumin.com/wp-content/uploads/2020/09/Coordinate_Transforms.pdf <- includes additional errors and factors that could be implemented -https://www.lddgo.net/en/coordinate/ecef-enu - -@param float *mmmLocal array of size nx3 X, Y, Z [m, m, m] -@param size_t nPoints Number of target points -@param float *rrmAER array of size nx3 of target point azimuth, elevation, range [rad, rad, m] -*/ -void ENU2AERFloat(const float *mmmENU, size_t nPoints, float *rrmAER) -{ - size_t iPoint, i; - for (iPoint = 0; iPoint < nPoints; ++iPoint) - { - i = iPoint * 3; - rrmAER[i + 0] = atan2(mmmENU[i + 0], mmmENU[i + 1]); - rrmAER[i + 2] = sqrt(mmmENU[i + 0] * mmmENU[i + 0] + mmmENU[i + 1] * mmmENU[i + 1] + mmmENU[i + 2] * mmmENU[i + 2]); - rrmAER[i + 1] = asin(mmmENU[i + 2] / rrmAER[i + 2]); - } -} - -/* -ENU to AER transformation of double precision. -https://x-lumin.com/wp-content/uploads/2020/09/Coordinate_Transforms.pdf -https://www.lddgo.net/en/coordinate/ecef-enu - -@param double *mmmLocal array of size nx3 X, Y, Z [m, m, m] -@param size_t nPoints Number of target points -@param double *rrmAER array of size nx3 of target point azimuth, elevation, range [rad, rad, m] -*/ -void ENU2AERDouble(const double *mmmENU, size_t nPoints, double *rrmAER) -{ - size_t iPoint, i; - for (iPoint = 0; iPoint < nPoints; ++iPoint) - { - i = iPoint * 3; - rrmAER[i + 0] = atan2(mmmENU[i + 0], mmmENU[i + 1]); - rrmAER[i + 2] = sqrt(mmmENU[i + 0] * mmmENU[i + 0] + mmmENU[i + 1] * mmmENU[i + 1] + mmmENU[i + 2] * mmmENU[i + 2]); - rrmAER[i + 1] = asin(mmmENU[i + 2] / rrmAER[i + 2]); - } -} - -/* -AER to ENU transformation of float precision. -https://x-lumin.com/wp-content/uploads/2020/09/Coordinate_Transforms.pdf - -@param float *rrmAER array of size nx3 of target point azimuth, elevation, range [rad, rad, m] -@param size_t nPoints Number of target points -@param float *mmmLocal array of size nx3 X, Y, Z [m, m, m] -*/ -void AER2ENUFloat(const float *rrmAER, size_t nPoints, float *mmmENU) -{ - size_t iPoint, i; - for (iPoint = 0; iPoint < nPoints; ++iPoint) - { - i = iPoint * 3; - mmmENU[i + 0] = cos(rrmAER[i + 1]) * sin(rrmAER[i + 0]) * rrmAER[i + 2]; - mmmENU[i + 1] = cos(rrmAER[i + 1]) * cos(rrmAER[i + 0]) * rrmAER[i + 2]; - mmmENU[i + 2] = sin(rrmAER[i + 1]) * rrmAER[i + 2]; - } -} - -/* -AER to ENU transformation of double precision. -https://x-lumin.com/wp-content/uploads/2020/09/Coordinate_Transforms.pdf - -@param double *rrmAER array of size nx3 of target point azimuth, elevation, range [rad, rad, m] -@param size_t nPoints Number of target points -@param double *mmmLocal array of size nx3 X, Y, Z [m, m, m] -*/ -void AER2ENUDouble(const double *rrmAER, size_t nPoints, double *mmmENU) -{ - size_t iPoint, i; - for (iPoint = 0; iPoint < nPoints; ++iPoint) - { - i = iPoint * 3; - mmmENU[i + 0] = cos(rrmAER[i + 1]) * sin(rrmAER[i + 0]) * rrmAER[i + 2]; - mmmENU[i + 1] = cos(rrmAER[i + 1]) * cos(rrmAER[i + 0]) * rrmAER[i + 2]; - mmmENU[i + 2] = sin(rrmAER[i + 1]) * rrmAER[i + 2]; - } -} - -static PyObject *geodetic2ECEFWrapper(PyObject *self, PyObject *args) -{ - PyArrayObject *rrmLLA; - double a, b; - - // Parse the input tuple - if (!PyArg_ParseTuple(args, "O!dd", &PyArray_Type, &rrmLLA, &a, &b)) - return NULL; - - // checks - if (!(PyArray_ISCONTIGUOUS(rrmLLA))) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); - return NULL; - } - if ((PyArray_SIZE(rrmLLA) % 3) != 0) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a multiple of 3."); - return NULL; - } - - PyObject *result_array = PyArray_SimpleNew(PyArray_NDIM(rrmLLA), PyArray_SHAPE(rrmLLA), PyArray_TYPE(rrmLLA)); - if (result_array == NULL) - return NULL; - npy_intp nPoints = PyArray_SIZE(rrmLLA) / 3; - if (PyArray_TYPE(rrmLLA) == NPY_DOUBLE) - { - double *data1 = (double *)PyArray_DATA(rrmLLA); - double *result_data = (double *)PyArray_DATA((PyArrayObject *)result_array); - geodetic2ECEFDouble(data1, nPoints, a, b, result_data); - } - else if (PyArray_TYPE(rrmLLA) == NPY_FLOAT) - { - float *data1 = (float *)PyArray_DATA(rrmLLA); - float *result_data = (float *)PyArray_DATA((PyArrayObject *)result_array); - geodetic2ECEFFloat(data1, nPoints, a, b, result_data); - } - else - { - PyErr_SetString(PyExc_ValueError, "Only 32 and 64 bit float types accepted."); - return NULL; - } - return result_array; -} - -static PyObject *ECEF2geodeticWrapper(PyObject *self, PyObject *args) -{ - PyArrayObject *mmmXYZ; - double a, b; - - // Parse the input tuple - if (!PyArg_ParseTuple(args, "O!dd", &PyArray_Type, &mmmXYZ, &a, &b)) - return NULL; - - // checks - if (!(PyArray_ISCONTIGUOUS(mmmXYZ))) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); - return NULL; - } - if ((PyArray_SIZE(mmmXYZ) % 3) != 0) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a multiple of 3."); - return NULL; - } - - PyObject *result_array = PyArray_SimpleNew(PyArray_NDIM(mmmXYZ), PyArray_SHAPE(mmmXYZ), PyArray_TYPE(mmmXYZ)); - if (result_array == NULL) - return NULL; - npy_intp nPoints = PyArray_SIZE(mmmXYZ) / 3; - if (PyArray_TYPE(mmmXYZ) == NPY_DOUBLE) - { - double *data1 = (double *)PyArray_DATA(mmmXYZ); - double *result_data = (double *)PyArray_DATA((PyArrayObject *)result_array); - ECEF2geodeticDouble(data1, nPoints, a, b, result_data); - } - else if (PyArray_TYPE(mmmXYZ) == NPY_FLOAT) - { - float *data1 = (float *)PyArray_DATA(mmmXYZ); - float *result_data = (float *)PyArray_DATA((PyArrayObject *)result_array); - ECEF2geodeticFloat(data1, nPoints, a, b, result_data); - } - else - { - PyErr_SetString(PyExc_ValueError, "Only 32 and 64 bit float types accepted."); - return NULL; - } - return result_array; -} - -static PyObject *ECEF2ENUWrapper(PyObject *self, PyObject *args) -{ - PyArrayObject *rrmLLALocalOrigin, *mmmXYZTarget; - double a, b; - - // Parse the input tuple - if (!PyArg_ParseTuple(args, "O!O!dd", &PyArray_Type, &rrmLLALocalOrigin, &PyArray_Type, &mmmXYZTarget, &a, &b)) - return NULL; - - // checks - if (!(PyArray_ISCONTIGUOUS(rrmLLALocalOrigin)) || !(PyArray_ISCONTIGUOUS(mmmXYZTarget))) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); - return NULL; - } - if (PyArray_NDIM(rrmLLALocalOrigin) != PyArray_NDIM(mmmXYZTarget)) - { - PyErr_SetString(PyExc_ValueError, "Input arrays have non-matching dimensions."); - return NULL; - } - if (PyArray_SIZE(rrmLLALocalOrigin) != PyArray_SIZE(mmmXYZTarget)) - { - PyErr_SetString(PyExc_ValueError, "Input arrays are of unequal size."); - return NULL; - } - if ((PyArray_SIZE(rrmLLALocalOrigin) % 3) != 0 || (PyArray_SIZE(mmmXYZTarget) % 3) != 0) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a multiple of 3."); - return NULL; - } - if (PyArray_TYPE(rrmLLALocalOrigin) != PyArray_TYPE(mmmXYZTarget)) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must have the same type."); - return NULL; - } - - PyObject *result_array = PyArray_SimpleNew(PyArray_NDIM(rrmLLALocalOrigin), PyArray_SHAPE(rrmLLALocalOrigin), PyArray_TYPE(rrmLLALocalOrigin)); - if (result_array == NULL) - return NULL; - npy_intp nPoints = PyArray_SIZE(rrmLLALocalOrigin) / 3; - if (PyArray_TYPE(rrmLLALocalOrigin) == NPY_DOUBLE) - { - double *data1 = (double *)PyArray_DATA(rrmLLALocalOrigin); - double *data2 = (double *)PyArray_DATA(mmmXYZTarget); - double *result_data = (double *)PyArray_DATA((PyArrayObject *)result_array); - ECEF2ENUDouble(data1, data2, nPoints, a, b, result_data); - } - else if (PyArray_TYPE(rrmLLALocalOrigin) == NPY_FLOAT) - { - float *data1 = (float *)PyArray_DATA(rrmLLALocalOrigin); - float *data2 = (float *)PyArray_DATA(mmmXYZTarget); - float *result_data = (float *)PyArray_DATA((PyArrayObject *)result_array); - ECEF2ENUFloat(data1, data2, nPoints, a, b, result_data); - } - else - { - PyErr_SetString(PyExc_ValueError, "Only 32 and 64 bit float types accepted."); - return NULL; - } - return result_array; -} - -static PyObject *ENU2ECEFWrapper(PyObject *self, PyObject *args) -{ - PyArrayObject *rrmLLALocalOrigin, *mmmLocal; - double a, b; - - // Parse the input tuple - if (!PyArg_ParseTuple(args, "O!O!dd", &PyArray_Type, &rrmLLALocalOrigin, &PyArray_Type, &mmmLocal, &a, &b)) - return NULL; - - // checks - if (!(PyArray_ISCONTIGUOUS(rrmLLALocalOrigin)) || !(PyArray_ISCONTIGUOUS(mmmLocal))) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); - return NULL; - } - if (PyArray_NDIM(rrmLLALocalOrigin) != PyArray_NDIM(mmmLocal)) - { - PyErr_SetString(PyExc_ValueError, "Input arrays have non-matching dimensions."); - return NULL; - } - if (PyArray_SIZE(rrmLLALocalOrigin) != PyArray_SIZE(mmmLocal)) - { - PyErr_SetString(PyExc_ValueError, "Input arrays are of unequal size."); - return NULL; - } - if ((PyArray_SIZE(rrmLLALocalOrigin) % 3) != 0 || (PyArray_SIZE(mmmLocal) % 3) != 0) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a multiple of 3."); - return NULL; - } - if (PyArray_TYPE(rrmLLALocalOrigin) != PyArray_TYPE(mmmLocal)) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must have the same type."); - return NULL; - } - - PyObject *result_array = PyArray_SimpleNew(PyArray_NDIM(rrmLLALocalOrigin), PyArray_SHAPE(rrmLLALocalOrigin), PyArray_TYPE(rrmLLALocalOrigin)); - if (result_array == NULL) - return NULL; - npy_intp nPoints = PyArray_SIZE(rrmLLALocalOrigin) / 3; - if (PyArray_TYPE(rrmLLALocalOrigin) == NPY_DOUBLE) - { - double *data1 = (double *)PyArray_DATA(rrmLLALocalOrigin); - double *data2 = (double *)PyArray_DATA(mmmLocal); - double *result_data = (double *)PyArray_DATA((PyArrayObject *)result_array); - ENU2ECEFDouble(data1, data2, nPoints, a, b, result_data); - } - else if (PyArray_TYPE(rrmLLALocalOrigin) == NPY_FLOAT) - { - float *data1 = (float *)PyArray_DATA(rrmLLALocalOrigin); - float *data2 = (float *)PyArray_DATA(mmmLocal); - float *result_data = (float *)PyArray_DATA((PyArrayObject *)result_array); - ENU2ECEFFloat(data1, data2, nPoints, a, b, result_data); - } - else - { - PyErr_SetString(PyExc_ValueError, "Only 32 and 64 bit float types accepted."); - return NULL; - } - return result_array; -} - -static PyObject *ENU2AERWrapper(PyObject *self, PyObject *args) -{ - PyArrayObject *mmmENU; - - // Parse the input tuple - if (!PyArg_ParseTuple(args, "O!", &PyArray_Type, &mmmENU)) - return NULL; - - // checks - if (!(PyArray_ISCONTIGUOUS(mmmENU))) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); - return NULL; - } - if ((PyArray_SIZE(mmmENU) % 3) != 0) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a multiple of 3."); - return NULL; - } - - PyObject *result_array = PyArray_SimpleNew(PyArray_NDIM(mmmENU), PyArray_SHAPE(mmmENU), PyArray_TYPE(mmmENU)); - if (result_array == NULL) - return NULL; - npy_intp nPoints = PyArray_SIZE(mmmENU) / 3; - if (PyArray_TYPE(mmmENU) == NPY_DOUBLE) - { - double *data1 = (double *)PyArray_DATA(mmmENU); - double *result_data = (double *)PyArray_DATA((PyArrayObject *)result_array); - ENU2AERDouble(data1, nPoints, result_data); - } - else if (PyArray_TYPE(mmmENU) == NPY_FLOAT) - { - float *data1 = (float *)PyArray_DATA(mmmENU); - float *result_data = (float *)PyArray_DATA((PyArrayObject *)result_array); - ENU2AERFloat(data1, nPoints, result_data); - } - else - { - PyErr_SetString(PyExc_ValueError, "Only 32 and 64 bit float types accepted."); - return NULL; - } - return result_array; -} - -static PyObject *AER2ENUWrapper(PyObject *self, PyObject *args) -{ - PyArrayObject *rrmAER; - - // Parse the input tuple - if (!PyArg_ParseTuple(args, "O!", &PyArray_Type, &rrmAER)) - return NULL; - - // checks - if (!(PyArray_ISCONTIGUOUS(rrmAER))) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a C contiguous."); - return NULL; - } - if ((PyArray_SIZE(rrmAER) % 3) != 0) - { - PyErr_SetString(PyExc_ValueError, "Input arrays must be a multiple of 3."); - return NULL; - } - - PyObject *result_array = PyArray_SimpleNew(PyArray_NDIM(rrmAER), PyArray_SHAPE(rrmAER), PyArray_TYPE(rrmAER)); - if (result_array == NULL) - return NULL; - npy_intp nPoints = PyArray_SIZE(rrmAER) / 3; - if (PyArray_TYPE(rrmAER) == NPY_DOUBLE) - { - double *data1 = (double *)PyArray_DATA(rrmAER); - double *result_data = (double *)PyArray_DATA((PyArrayObject *)result_array); - AER2ENUDouble(data1, nPoints, result_data); - } - else if (PyArray_TYPE(rrmAER) == NPY_FLOAT) - { - float *data1 = (float *)PyArray_DATA(rrmAER); - float *result_data = (float *)PyArray_DATA((PyArrayObject *)result_array); - AER2ENUFloat(data1, nPoints, result_data); - } - else - { - PyErr_SetString(PyExc_ValueError, "Only 32 and 64 bit float types accepted."); - return NULL; - } - return result_array; -} - -// Method definition object for this extension, these argumens mean: -// ml_name: The name of the method -// ml_meth: Function pointer to the method implementation -// ml_flags: Flags indicating special features of this method, such as -// accepting arguments, accepting keyword arguments, being a class method, or being a static method of a class. -// ml_doc: The docstring for the method -static PyMethodDef MyMethods[] = { - {"geodetic2ECEF", geodetic2ECEFWrapper, METH_VARARGS, "Convert geodetic coordinate system to ECEF."}, - {"ECEF2geodetic", ECEF2geodeticWrapper, METH_VARARGS, "Convert ECEF to geodetic coordinate system."}, - {"ECEF2ENU", ECEF2ENUWrapper, METH_VARARGS, "Convert ECEF to ENU."}, - {"ENU2ECEF", ENU2ECEFWrapper, METH_VARARGS, "Convert ENU to ECEF."}, - {"ENU2AER", ENU2AERWrapper, METH_VARARGS, "Convert ENU to AER."}, - {"AER2ENU", AER2ENUWrapper, METH_VARARGS, "Convert AER to ENU."}, - {NULL, NULL, 0, NULL}}; - -// Module definition -static struct PyModuleDef transforms = { - PyModuleDef_HEAD_INIT, - "transforms", - "Module that contains transform functions.", - -1, - MyMethods}; - -// Module initialization function -PyMODINIT_FUNC PyInit_transforms(void) -{ - import_array(); // Initialize the NumPy C API - return PyModule_Create(&transforms); -}