/**
* lpt1에서 lpt2방향으로 distance만큼 이동한 후의 위치를 구한다
*/
public static RPoint GetRPointOfDistance(RPoint lpt1, RPoint lpt2, double distance)
{
var angle = lpt1.GetAngle(lpt2);
var dpt = new RPoint(lpt1.X + distance, lpt1.Y);
return(Rotate(lpt1, dpt, angle));
}
/**
* waypoints에서 pt에 가장 가까운 2개의 점을 리턴한다.
* 보통은 pt의 앞뒤로 하나씩 나오니 (n, n+1) 이 리턴된다.
* 그런데 위치가 애매한 경우 (n, n) 이 나올 수도 있다.
*/
public static (int prev, int next) FindClosestWaypointIndex(RPoint pt, List <RPoint> waypoints)
{
(int prev, int next)closestIndex = (-1, -1); // 그냥 제일 가까운 index
(int prev, int next)centeredIndex = (-1, -1); // (prev, index) 중심에 제일 가까운 index
var oldDist = Double.MaxValue; // pt에서 waypoint prev까지의 거리
var oldCenterDist = Double.MaxValue; // pt가 waypoint line 중심에서 얼마나 벗어나 있는가
for (var i = 0; i < waypoints.Count - 1; i++)
{
RPoint prev = waypoints[i];
RPoint next = waypoints[i + 1];
double angle = prev.GetAngle(next);
RPoint rotatedNext = DMath.Rotate(prev, next, -angle);
RPoint rotatedPt = DMath.Rotate(prev, pt, -angle);
if (rotatedPt.X <= rotatedNext.X)
{
var cross = pt.GetClosestPointFromLine(prev, next);
var middle = GetMiddle(prev, next);
var newDist = prev.DistanceTo(pt);
if (closestIndex.next < 0 || newDist < oldDist)
{
oldDist = newDist;
if (prev.X <= rotatedPt.X)
{
closestIndex = (i, i + 1);
}
else
{
closestIndex = (i, i);
}
}
var newCenterDist = cross.DistanceTo(middle);
if (prev.X <= rotatedPt.X && newCenterDist < oldCenterDist && newDist < 0.7)
{
centeredIndex = (i, i + 1);
oldCenterDist = newCenterDist;
}
}
}
if (closestIndex.prev == -1)
{
throw new Exception($"cannot find closest waypoint of {pt}");
}
if (centeredIndex.prev >= 0)
{
return(centeredIndex);
}
else
{
return(closestIndex);
}
}