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));
}