|
| 1 | + |
| 2 | +#include "Utils.h" |
| 3 | + |
| 4 | +int EARTH_RADIUS = 6371009; |
| 5 | +double TO_RAD = 0.017453292519943295; |
| 6 | + |
| 7 | +@interface CartoAdditionsUtils () |
| 8 | + |
| 9 | +@end |
| 10 | + |
| 11 | +@implementation CartoAdditionsUtils |
| 12 | + |
| 13 | ++ (long)isLocationOn:(NTMapPos *)point |
| 14 | + poly:(NTMapPosVector *)poly { |
| 15 | + return [CartoAdditionsUtils isLocationOn:point poly:poly closed:false]; |
| 16 | +} |
| 17 | + |
| 18 | ++ (long)isLocationOn:(NTMapPos *)point |
| 19 | + poly:(NTMapPosVector *)poly |
| 20 | + closed:(bool)closed { |
| 21 | + return [CartoAdditionsUtils isLocationOn:point poly:poly closed:false geodesic:true]; |
| 22 | +} |
| 23 | + |
| 24 | ++ (long)isLocationOn:(NTMapPos *)point |
| 25 | + poly:(NTMapPosVector *)poly |
| 26 | + closed:(bool)closed |
| 27 | + geodesic:(bool)geodesic { |
| 28 | + return [CartoAdditionsUtils isLocationOn:point poly:poly closed:false geodesic:true toleranceEarth:0.1]; |
| 29 | +} |
| 30 | + |
| 31 | + |
| 32 | + |
| 33 | +double arcHav(double x) { |
| 34 | + return 2 * asin(sqrt(x)); |
| 35 | +} |
| 36 | + |
| 37 | +double hav(double x) { |
| 38 | + double sinHalf = sin(x * 0.5f); |
| 39 | + return sinHalf * sinHalf; |
| 40 | +} |
| 41 | + |
| 42 | +double havDistance(double lat1, double lat2, double dLng) { |
| 43 | + return hav(lat1 - lat2) + hav(dLng) * cos(lat1) * cos(lat2); |
| 44 | +} |
| 45 | + |
| 46 | +double wrap(double n, double min, double max) { |
| 47 | + return n >= min && n < max ? n : (fmod((n - min), (max - min))) + min; |
| 48 | +} |
| 49 | + |
| 50 | +double clamp(double x, double low, double high) { |
| 51 | + return x < low ? low : x > high ? high : x; |
| 52 | +} |
| 53 | + |
| 54 | +double mercator(double lat) { |
| 55 | + return log(tan(lat * 0.5 + M_PI / 4.0f)); |
| 56 | +} |
| 57 | + |
| 58 | +double inverseMercator(double y) { |
| 59 | + return 2 * atan(exp(y)) - M_PI / 2.0f; |
| 60 | +} |
| 61 | + |
| 62 | +double sinSumFromHav(double x, double y) { |
| 63 | + double a = sqrt(x * (1 - x)); |
| 64 | + double b = sqrt(y * (1 - y)); |
| 65 | + return 2 * (a + b - 2 * (a * y + b * x)); |
| 66 | +} |
| 67 | + |
| 68 | +double sinFromHav(double h) { |
| 69 | + return 2 * sqrt(h * (1 - h)); |
| 70 | +} |
| 71 | + |
| 72 | +double havFromSin(double x) { |
| 73 | + double x2 = x * x; |
| 74 | + return (x2 / (1 + sqrt(1 - x2))) * 0.5; |
| 75 | +} |
| 76 | + |
| 77 | +double sinDeltaBearing(double lat1, double lng1, double lat2, double lng2, double lat3, double lng3) { |
| 78 | + double sinLat1 = sin(lat1); |
| 79 | + double cosLat2 = cos(lat2); |
| 80 | + double cosLat3 = cos(lat3); |
| 81 | + double lat31 = lat3 - lat1; |
| 82 | + double lng31 = lng3 - lng1; |
| 83 | + double lat21 = lat2 - lat1; |
| 84 | + double lng21 = lng2 - lng1; |
| 85 | + double a = sin(lng31) * cosLat3; |
| 86 | + double c = sin(lng21) * cosLat2; |
| 87 | + double b = sin(lat31) + 2 * sinLat1 * cosLat3 * hav(lng31); |
| 88 | + double d = sin(lat21) + 2 * sinLat1 * cosLat2 * hav(lng21); |
| 89 | + double denom = (a * a + b * b) * (c * c + d * d); |
| 90 | + return denom <= 0 ? 1 : (a * d - b * c) / sqrt(denom); |
| 91 | +} |
| 92 | + |
| 93 | +double toRadians(double value) { |
| 94 | + return value * TO_RAD; |
| 95 | +} |
| 96 | + |
| 97 | + |
| 98 | +double computeAngleBetween(NTMapPos *from, NTMapPos *to) { |
| 99 | + return distanceRadians(toRadians([from getY]), toRadians([from getX]), toRadians([to getY]), toRadians([to getX])); |
| 100 | +} |
| 101 | + |
| 102 | +double computeDistanceBetween(NTMapPos *from, NTMapPos *to) { |
| 103 | + return computeAngleBetween(from, to) * EARTH_RADIUS; |
| 104 | +} |
| 105 | + |
| 106 | +double distanceRadians(double lat1, double lng1, double lat2, double lng2) { |
| 107 | + return arcHav(havDistance(lat1, lat2, lng1 - lng2)); |
| 108 | +} |
| 109 | + |
| 110 | + |
| 111 | ++ (double)distanceToEndWithInt:(int)index |
| 112 | + poly:(NTMapPosVector *)poly { |
| 113 | + int result = 0; |
| 114 | + long size = [poly size]; |
| 115 | + NTMapPos *last = nil; |
| 116 | + NTMapPos *element; |
| 117 | + for (int i = index; i < size; i++) { |
| 118 | + element = [poly get:i]; |
| 119 | + if (last != nil) { |
| 120 | + result += computeDistanceBetween(last, element); |
| 121 | + } |
| 122 | + last = element; |
| 123 | + } |
| 124 | + return result; |
| 125 | +} |
| 126 | + |
| 127 | +bool isOnSegmentGC(double lat1, double lng1, double lat2, double lng2, double lat3, double lng3, double havTolerance) { |
| 128 | + double havDist13 = havDistance(lat1, lat3, lng1 - lng3); |
| 129 | + if (havDist13 <= havTolerance) { |
| 130 | + return true; |
| 131 | + } |
| 132 | + double havDist23 = havDistance(lat2, lat3, lng2 - lng3); |
| 133 | + if (havDist23 <= havTolerance) { |
| 134 | + return true; |
| 135 | + } |
| 136 | + double sinBearing = sinDeltaBearing(lat1, lng1, lat2, lng2, lat3, lng3); |
| 137 | + double sinDist13 = sinFromHav(havDist13); |
| 138 | + double havCrossTrack = havFromSin(sinDist13 * sinBearing); |
| 139 | + if (havCrossTrack > havTolerance) { |
| 140 | + return false; |
| 141 | + } |
| 142 | + double havDist12 = havDistance(lat1, lat2, lng1 - lng2); |
| 143 | + double term = havDist12 + havCrossTrack * (1 - 2 * havDist12); |
| 144 | + if (havDist13 > term || havDist23 > term) { |
| 145 | + return false; |
| 146 | + } |
| 147 | + if (havDist12 < 0.74) { |
| 148 | + return true; |
| 149 | + } |
| 150 | + double cosCrossTrack = 1 - 2 * havCrossTrack; |
| 151 | + double havAlongTrack13 = (havDist13 - havCrossTrack) / cosCrossTrack; |
| 152 | + double havAlongTrack23 = (havDist23 - havCrossTrack) / cosCrossTrack; |
| 153 | + double sinSumAlongTrack = sinSumFromHav(havAlongTrack13, havAlongTrack23); |
| 154 | + return sinSumAlongTrack > 0; |
| 155 | +} |
| 156 | + |
| 157 | ++ (long)isLocationOn:(NTMapPos *)point |
| 158 | + poly:(NTMapPosVector *)poly |
| 159 | + closed:(bool)closed |
| 160 | + geodesic:(bool)geodesic |
| 161 | + toleranceEarth:(double)toleranceEarth { |
| 162 | + long size = [poly size]; |
| 163 | + if (size == 0) { |
| 164 | + return -1; |
| 165 | + } |
| 166 | + double tolerance = toleranceEarth / EARTH_RADIUS; |
| 167 | + double havTolerance = hav(tolerance); |
| 168 | + double lat3 = toRadians([point getY]); |
| 169 | + double lng3 = toRadians([point getX]); |
| 170 | + NTMapPos *prev = ([poly get:closed ? (int) (size - 1) : 0]); |
| 171 | + double lat1 = toRadians([prev getY]); |
| 172 | + double lng1 = toRadians([prev getX]); |
| 173 | + if (geodesic) { |
| 174 | + for (int index = 0; index < size; index++) { |
| 175 | + NTMapPos *point2 = ([poly get:index]); |
| 176 | + double lat2 = toRadians([point2 getY]); |
| 177 | + double lng2 = toRadians([point2 getX]); |
| 178 | + if (isOnSegmentGC(lat1, lng1, lat2, lng2, lat3, lng3, havTolerance)) { |
| 179 | + return index; |
| 180 | + } |
| 181 | + lat1 = lat2; |
| 182 | + lng1 = lng2; |
| 183 | + } |
| 184 | + } else { |
| 185 | + double minAcceptable = lat3 - tolerance; |
| 186 | + double maxAcceptable = lat3 + tolerance; |
| 187 | + double y1 = mercator(lat1); |
| 188 | + double y3 = mercator(lat3); |
| 189 | + NSMutableArray *xTry = [NSMutableArray array]; |
| 190 | + for (int index = 0; index < size; index++) { |
| 191 | + NTMapPos *point2 = ([poly get:index]); |
| 192 | + double lat2 = toRadians([point2 getY]); |
| 193 | + double y2 = mercator(lat2); |
| 194 | + double lng2 = toRadians([point2 getX]); |
| 195 | + if (MAX(lat1, lat2) >= minAcceptable && MIN(lat1, lat2) <= maxAcceptable) { |
| 196 | + double x2 = wrap(lng2 - lng1, -M_PI, M_PI); |
| 197 | + double x3Base = wrap(lng3 - lng1, -M_PI, M_PI); |
| 198 | + [xTry replaceObjectAtIndex:0 withObject:@(x3Base)]; |
| 199 | + [xTry replaceObjectAtIndex:1 withObject:@(x3Base + 2 * M_PI)]; |
| 200 | + [xTry replaceObjectAtIndex:2 withObject:@(x3Base - 2 * M_PI)]; |
| 201 | + for (int index2 = 0; index2 < [xTry count]; index2++) { |
| 202 | + double x3 =[[ xTry objectAtIndex:index2] boolValue]; |
| 203 | + double dy = y2 - y1; |
| 204 | + double len2 = x2 * x2 + dy * dy; |
| 205 | + double t = len2 <= 0 ? 0 : clamp((x3 * x2 + (y3 - y1) * dy) / len2, 0, 1); |
| 206 | + double xClosest = t * x2; |
| 207 | + double yClosest = y1 + t * dy; |
| 208 | + double latClosest = inverseMercator(yClosest); |
| 209 | + double havDist = havDistance(lat3, latClosest, x3 - xClosest); |
| 210 | + if (havDist < havTolerance) { |
| 211 | + return index; |
| 212 | + } |
| 213 | + } |
| 214 | + } |
| 215 | + lat1 = lat2; |
| 216 | + lng1 = lng2; |
| 217 | + y1 = y2; |
| 218 | + } |
| 219 | + } |
| 220 | + return -1; |
| 221 | +} |
| 222 | +@end |
0 commit comments