Find nearest point in polyline/path - ios

Question

I need to find nearest point from given CLLocationCoordinate2D on array of GMSPolyline. I can convert this to GMSPath if that's better. Is there any ready method (or any repository) for such calculations? I have some problems with implementation. I wonder how to create an algorithm:
1. for all polylines
1.1. find smallest distance between polyline and touch point, save CLLocationCoordinate2D
2. for all distances from point 1.1.
2.1. find the shortest one, it's CLLocationCoordinate2D is our point
Now the question is how to achieve point 1.1..?
Basing on SOF shortest distance question, I wrote such code:
- (void)findNearestLineSegmentToCoordinate:(CLLocationCoordinate2D)coordinate {
GMSPolyline *bestPolyline;
double bestDistance = DBL_MAX;
CGPoint originPoint = CGPointMake(coordinate.longitude, coordinate.latitude);
for (GMSPolyline *polyline in self.polylines) {
polyline.strokeColor = [UIColor redColor]; // TMP
if (polyline.path.count < 2) { // we need at least 2 points: start and end
return;
}
for (NSInteger index = 0; index < polyline.path.count - 1; index++) {
CLLocationCoordinate2D startCoordinate = [polyline.path coordinateAtIndex:index];
CGPoint startPoint = CGPointMake(startCoordinate.longitude, startCoordinate.latitude);
CLLocationCoordinate2D endCoordinate = [polyline.path coordinateAtIndex:(index + 1)];
CGPoint endPoint = CGPointMake(endCoordinate.longitude, endCoordinate.latitude);
double distance = [self distanceToPoint:originPoint fromLineSegmentBetween:startPoint and:endPoint];
if (distance < bestDistance) {
bestDistance = distance;
bestPolyline = polyline;
}
}
}
bestPolyline.map = nil;
bestPolyline.strokeColor = [UIColor greenColor]; // TMP
bestPolyline.map = self.aView.mapView;
}
Still, the problem is with exact point. Any algorithm? I'll post answer here when found.

Answer 1

Ok, I've managed to write it. Method nearestPointToPoint:onLineSegmentPointA:pointB:distance: allows you both to find closest coordinate and distance between selected point and segment line (so line with start and end).
- (CLLocationCoordinate2D)nearestPolylineLocationToCoordinate:(CLLocationCoordinate2D)coordinate {
GMSPolyline *bestPolyline;
double bestDistance = DBL_MAX;
CGPoint bestPoint;
CGPoint originPoint = CGPointMake(coordinate.longitude, coordinate.latitude);
for (GMSPolyline *polyline in self.polylines) {
if (polyline.path.count < 2) { // we need at least 2 points: start and end
return kCLLocationCoordinate2DInvalid;
}
for (NSInteger index = 0; index < polyline.path.count - 1; index++) {
CLLocationCoordinate2D startCoordinate = [polyline.path coordinateAtIndex:index];
CGPoint startPoint = CGPointMake(startCoordinate.longitude, startCoordinate.latitude);
CLLocationCoordinate2D endCoordinate = [polyline.path coordinateAtIndex:(index + 1)];
CGPoint endPoint = CGPointMake(endCoordinate.longitude, endCoordinate.latitude);
double distance;
CGPoint point = [self nearestPointToPoint:originPoint onLineSegmentPointA:startPoint pointB:endPoint distance:&distance];
if (distance < bestDistance) {
bestDistance = distance;
bestPolyline = polyline;
bestPoint = point;
}
}
}
return CLLocationCoordinate2DMake(bestPoint.y, bestPoint.x);
}
Method nearestPolylineLocationToCoordinate: will browse through all polylines (you just need to supply polylines array == self.polylines) and find the best one.
// taken and modified from: http://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment
- (CGPoint)nearestPointToPoint:(CGPoint)origin onLineSegmentPointA:(CGPoint)pointA pointB:(CGPoint)pointB distance:(double *)distance {
CGPoint dAP = CGPointMake(origin.x - pointA.x, origin.y - pointA.y);
CGPoint dAB = CGPointMake(pointB.x - pointA.x, pointB.y - pointA.y);
CGFloat dot = dAP.x * dAB.x + dAP.y * dAB.y;
CGFloat squareLength = dAB.x * dAB.x + dAB.y * dAB.y;
CGFloat param = dot / squareLength;
CGPoint nearestPoint;
if (param < 0 || (pointA.x == pointB.x && pointA.y == pointB.y)) {
nearestPoint.x = pointA.x;
nearestPoint.y = pointA.y;
} else if (param > 1) {
nearestPoint.x = pointB.x;
nearestPoint.y = pointB.y;
} else {
nearestPoint.x = pointA.x + param * dAB.x;
nearestPoint.y = pointA.y + param * dAB.y;
}
CGFloat dx = origin.x - nearestPoint.x;
CGFloat dy = origin.y - nearestPoint.y;
*distance = sqrtf(dx * dx + dy * dy);
return nearestPoint;
}
You can use it eg in:
- (void)mapView:(GMSMapView *)mapView didEndDraggingMarker:(GMSMarker *)marker {
marker.position = [self nearestPolylineLocationToCoordinate:marker.position];
}

Answer 2

#Vive's nearestPointToPoint() to Swift 5
func nearestPointToPoint(_ origin: CGPoint, _ pointA: CGPoint, _ pointB: CGPoint) -> (CGPoint, Double) {
let dAP = CGPoint(x: origin.x - pointA.x, y: origin.y - pointA.y)
let dAB = CGPoint(x: pointB.x - pointA.x, y: pointB.y - pointA.y)
let dot = dAP.x * dAB.x + dAP.y * dAB.y
let squareLength = dAB.x * dAB.x + dAB.y * dAB.y
let param = dot / squareLength
// abnormal value at near latitude 180
// var nearestPoint = CGPoint()
// if param < 0 || (pointA.x == pointB.x && pointA.y == pointB.y) {
// nearestPoint.x = pointA.x
// nearestPoint.y = pointA.y
// } else if param > 1 {
// nearestPoint.x = pointB.x
// nearestPoint.y = pointB.y
// } else {
// nearestPoint.x = pointA.x + param * dAB.x
// nearestPoint.y = pointA.y + param * dAB.y
// }
let nearestPoint = CGPoint(x: pointA.x + param * dAB.x,
y: pointA.y + param * dAB.y)
let dx = origin.x - nearestPoint.x
let dy = origin.y - nearestPoint.y
let distance = sqrtf(Float(dx * dx + dy * dy))
return (nearestPoint, Double(distance))
}
Result is
me: (53, -1), pointA: (52, -2), pointB(52, 0) => (52, -1)
me: (53, 0), pointA: (52, -1), pointB(52, 1) => (52, 0)
me: (53, 1), pointA: (52, 0), pointB(52, 2) => (52, 1)
me: (-1, -77), pointA: (0, -78), pointB(-2, -78) => (-1, -78)
me: (0, -77), pointA: (-1, -78), pointB(1, -78) => (0, -78)
me: (1, -77), pointA: (2, -78), pointB(0, -78) => (1, -78)
me: (1, 179), pointA: (0, 178), pointB(0, 180) => (0, 179)
me: (1, 180), pointA: (0, 179), pointB(0, -179) => (0, 180)
me: (1, -179), pointA: (0, 180), pointB(0, -178) => (0, -179)
But some error at latitude 180. I can't fix it.
me: (0, 180), pointA: (0, 179), pointB(1, 180) => (0.5, 179.5)
me: (0, -179), pointA: (0, 179), pointB(1, -179) => (0.99, -178.99) // abnormal