public PointF FindPointInPath(BleReceiver source, double distance) { PointF end, lastWayPoint; double diff; var start = source.Position; if (Equals(source, From)) { end = To.Position; if (distance >= Distance) { return(end); } if (WayPoints.IsNullOrEmpty()) { return(GeometryUtil.CalculatePointInBetween(start, end, distance)); } var firstWayPoint = WayPoints.First(); var currentDistance = GeometryUtil.GetDistance(start, firstWayPoint); if (distance < currentDistance) { return(GeometryUtil.CalculatePointInBetween(start, firstWayPoint, distance)); } var acccumelativeDistance = currentDistance; var wayPointCount = WayPoints.Count; for (int i = 0; i < wayPointCount - 1; i++) { currentDistance = GeometryUtil.GetDistance(WayPoints[i], WayPoints[i + 1]); //if (acccumelativeDistance + currentDistance > distance) //{ //} acccumelativeDistance += currentDistance; if (acccumelativeDistance < distance) { continue; } diff = currentDistance - (acccumelativeDistance - distance); return(GeometryUtil.CalculatePointInBetween(WayPoints[i], WayPoints[i + 1], diff)); } lastWayPoint = WayPoints.Last(); //lastWayPoint = WayPoints.Last(); } else if (Equals(source, To)) { end = From.Position; if (distance >= Distance) { return(end); } if (WayPoints.IsNullOrEmpty()) { return(GeometryUtil.CalculatePointInBetween(start, end, distance)); } lastWayPoint = WayPoints.First(); var currentDistance = GeometryUtil.GetDistance(start, lastWayPoint); if (distance < currentDistance) { return(GeometryUtil.CalculatePointInBetween(start, lastWayPoint, distance)); } var acccumelativeDistance = currentDistance; var wayPointCount = WayPoints.Count; for (int i = wayPointCount - 1; i > 0; i--) { currentDistance = GeometryUtil.GetDistance(WayPoints[i], WayPoints[i - 1]); acccumelativeDistance += currentDistance; if (acccumelativeDistance < distance) { continue; } diff = currentDistance - (acccumelativeDistance - distance); return(GeometryUtil.CalculatePointInBetween(WayPoints[i], WayPoints[i - 1], diff)); } } else { //TODO log warning return(start); } diff = GeometryUtil.GetDistance(lastWayPoint, end) - (Distance - distance); return(GeometryUtil.CalculatePointInBetween(lastWayPoint, end, diff)); }