| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /* GATE PROJECT LICENSE: | ||
| 2 | +----------------------------------------------------------------------------+ | ||
| 3 | | Copyright(c) 2018-2025, Stefan Meislinger <sm@opengate.at> | | ||
| 4 | | All rights reserved. | | ||
| 5 | | | | ||
| 6 | | Redistribution and use in source and binary forms, with or without | | ||
| 7 | | modification, are permitted provided that the following conditions are met:| | ||
| 8 | | | | ||
| 9 | | 1. Redistributions of source code must retain the above copyright notice, | | ||
| 10 | | this list of conditions and the following disclaimer. | | ||
| 11 | | 2. Redistributions in binary form must reproduce the above copyright | | ||
| 12 | | notice, this list of conditions and the following disclaimer in the | | ||
| 13 | | documentation and/or other materials provided with the distribution. | | ||
| 14 | | | | ||
| 15 | | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"| | ||
| 16 | | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE | | ||
| 17 | | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE | | ||
| 18 | | ARE DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE | | ||
| 19 | | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR | | ||
| 20 | | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF | | ||
| 21 | | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS | | ||
| 22 | | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN | | ||
| 23 | | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) | | ||
| 24 | | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF | | ||
| 25 | | THE POSSIBILITY OF SUCH DAMAGE. | | ||
| 26 | +----------------------------------------------------------------------------+ | ||
| 27 | */ | ||
| 28 | |||
| 29 | #include "gate/geopositions.h" | ||
| 30 | |||
| 31 | #include "gate/mathematics.h" | ||
| 32 | |||
| 33 | 5 | static gate_real64_t gate_geopos_meters_per_latitude_rad(gate_real64_t latitude) | |
| 34 | { | ||
| 35 | static gate_real64_t const lat_term_1 = 111132.92f; | ||
| 36 | static gate_real64_t const lat_term_2 = -559.82f; | ||
| 37 | static gate_real64_t const lat_term_3 = 1.175f; | ||
| 38 | static gate_real64_t const lat_term_4 = -0.0023f; | ||
| 39 | |||
| 40 | return lat_term_1 | ||
| 41 | 5 | + lat_term_2 * gate_math_cos(2.0 * latitude) | |
| 42 | 5 | + lat_term_3 * gate_math_cos(4.0 * latitude) | |
| 43 | 5 | + lat_term_4 * gate_math_cos(6.0 * latitude); | |
| 44 | } | ||
| 45 | |||
| 46 | 5 | gate_real64_t gate_geopos_meters_per_latitude(gate_real64_t lat) | |
| 47 | { | ||
| 48 | 5 | return gate_geopos_meters_per_latitude_rad(gate_math_deg2rad(lat)); | |
| 49 | } | ||
| 50 | |||
| 51 | 5 | static gate_real64_t gate_geopos_meters_per_longitude_rad(gate_real64_t latitude) | |
| 52 | { | ||
| 53 | static gate_real64_t const lon_term_1 = 111412.84f; | ||
| 54 | static gate_real64_t const lon_term_2 = -93.5f; | ||
| 55 | static gate_real64_t const lon_term_3 = 0.118f; | ||
| 56 | |||
| 57 | 5 | return lon_term_1 * gate_math_cos(latitude) | |
| 58 | 5 | + lon_term_2 * gate_math_cos(3.0 * latitude) | |
| 59 | 5 | + lon_term_3 * gate_math_cos(5.0 * latitude); | |
| 60 | } | ||
| 61 | |||
| 62 | 5 | gate_real64_t gate_geopos_meters_per_longitude(gate_real64_t lat) | |
| 63 | { | ||
| 64 | 5 | return gate_geopos_meters_per_longitude_rad(gate_math_deg2rad(lat)); | |
| 65 | } | ||
| 66 | |||
| 67 | 3 | gate_real64_t gate_geopos_distance(gate_geopos_t const* pos1, gate_geopos_t const* pos2) | |
| 68 | { | ||
| 69 | 3 | gate_real64_t center_lat = (pos1->latitude + pos2->latitude) / 2.0; | |
| 70 | 3 | gate_real64_t center_lon = (pos1->longitude + pos2->longitude) / 2.0; | |
| 71 | 3 | gate_real64_t meters_per_lat = gate_geopos_meters_per_latitude(center_lat); | |
| 72 | 3 | gate_real64_t meters_per_lon = gate_geopos_meters_per_longitude(center_lon); | |
| 73 | |||
| 74 | 3 | gate_real64_t diff_lat = gate_math_abs_r64(pos2->latitude - pos1->latitude); | |
| 75 | 3 | gate_real64_t diff_lon = gate_math_abs_r64(pos2->longitude - pos1->longitude); | |
| 76 | |||
| 77 | 3 | gate_real64_t y = diff_lat * meters_per_lat; | |
| 78 | 3 | gate_real64_t x = diff_lon * meters_per_lon; | |
| 79 | |||
| 80 | 3 | return gate_math_sqrt(x * x + y * y); | |
| 81 | } | ||
| 82 |