示例#1
0
        /**
         * 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));
        }
示例#2
0
        /**
         * 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);
            }
        }