Find nearest point in polyline/path

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.

Freehanded

Can you describe what the problem is? – Kistner

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: https://mcmap.net/q/80375/-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];
}

Freehanded

thanks for your code snippet , it helped alot. Vote up for this. Also can you help on how can i track all the points on turning points of polyline ? If i get GPS updates after user has turned on road then marker look like jump from x to y. instead i want to make it look traversing curve on polyline . – Erebus

@Erebus What exactly means "make it look traversing curve on polyline"? – Freehanded

i am working on navigation app, i get GPS update at point x and then directly at y and between that there is right turn as per polyline but my GMSMarker moves directly from x to y diagonally with animation, i need it move along polyline like a smooth turn. Now clear my point ? – Erebus

For details please look at: #41061422 – Erebus

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

Lobworm