示例#1
0
        private int GetNextPoint(PATH_POINT[] path, int pathCount, int curIndex, int forward, out PATH_POINT pNext)
        {
            pNext = new PATH_POINT();
            if (pathCount <= 1)
            {
                return(-1);
            }

            //C++ TO C# CONVERTER TODO TASK: The memory management function 'memset' has no equivalent in C#:
            //memset(pNext, 0, sizeof(PATH_POINT));
            int nextIndex = curIndex;

            if (forward == TRACE_FORWARD)
            {
                if (curIndex == pathCount - 1)
                {
                    pNext.x = path[curIndex].x + path[curIndex].x - path[curIndex - 1].x;
                    pNext.y = path[curIndex].y + path[curIndex].y - path[curIndex - 1].y;
                    pNext.v = path[curIndex].v;
                }
                else
                {
                    nextIndex = curIndex + 1;
                    pNext.x   = path[nextIndex].x;
                    pNext.y   = path[nextIndex].y;
                    pNext.v   = path[nextIndex].v;
                }
            }
            else
            {
                if (curIndex == 0)
                {
                    pNext.x = path[curIndex].x + path[curIndex].x - path[curIndex + 1].x;
                    pNext.y = path[curIndex].y + path[curIndex].y - path[curIndex + 1].y;
                    pNext.v = path[curIndex].v;
                }
                else
                {
                    nextIndex = curIndex - 1;
                    pNext.x   = path[nextIndex].x;
                    pNext.y   = path[nextIndex].y;
                    pNext.v   = path[nextIndex].v;
                }
            }

            return(nextIndex);
        }
示例#2
0
        private int GetPrevPoint(PATH_POINT[] path, int pathCount, int curIndex, int forward, out PATH_POINT pPrev)
        {
            pPrev = new PATH_POINT();
            if (pathCount <= 1)
            {
                return(-1);
            }
            int prevIndex = curIndex;

            if (forward == TRACE_FORWARD)
            {
                if (curIndex == 0)
                {
                    pPrev.x = path[curIndex].x + path[curIndex].x - path[curIndex + 1].x;
                    pPrev.y = path[curIndex].y + path[curIndex].y - path[curIndex + 1].y;
                    pPrev.v = path[curIndex].v;
                }
                else
                {
                    prevIndex = curIndex - 1;
                    pPrev.x   = path[prevIndex].x;
                    pPrev.y   = path[prevIndex].y;
                    pPrev.v   = path[prevIndex].v;
                }
            }
            else
            {
                if (curIndex == pathCount - 1)
                {
                    pPrev.x = path[curIndex].x + path[curIndex].x - path[curIndex - 1].x;
                    pPrev.y = path[curIndex].y + path[curIndex].y - path[curIndex - 1].y;
                    pPrev.v = path[curIndex].v;
                }
                else
                {
                    prevIndex = curIndex + 1;
                    pPrev.x   = path[prevIndex].x;
                    pPrev.y   = path[prevIndex].y;
                    pPrev.v   = path[prevIndex].v;
                }
            }

            return(prevIndex);
        }
示例#3
0
        // 计算目标路径点 -----
        private int CalcTargetPoint(POINT cur_pos)
        {
            PATH_POINT target_point = new PATH_POINT();
            int        target_index = -1;
            double     minDistance  = -1;

            do
            {
                int          path_point_cnt = m_param.PathPointsCount;
                PATH_POINT[] path           = m_param.PathPoints;
                if (path_point_cnt == 1)
                {
                    target_index = 0;
                    break;
                }

                double     dist2Point = 0;
                double     dc2t       = 0;
                double     dc2p       = 0;
                double     dp2t       = 0;
                PATH_POINT prevPoint  = new PATH_POINT();
                for (int i = 0; i < path_point_cnt; i++)
                {
                    PATH_POINT prev = new PATH_POINT();
                    GetPrevPoint(path, path_point_cnt, i, m_param.TrackDirection, out prev);

                    double d1 = (cur_pos.X - path[i].x) * (cur_pos.X - path[i].x) + (cur_pos.Y - path[i].y) * (cur_pos.Y - path[i].y);
                    double d2 = (prev.x - path[i].x) * (prev.x - path[i].x) + (prev.y - path[i].y) * (prev.y - path[i].y);
                    double d3 = (cur_pos.X - prev.x) * (cur_pos.X - prev.x) + (cur_pos.Y - prev.y) * (cur_pos.Y - prev.y);
                    double _a = Math.Sqrt(d1);
                    double _b = Math.Sqrt(d2);
                    double _c = Math.Sqrt(d3);

                    double mdist = 0;
                    if ((d2 + d1) >= d3 && (d2 + d3) >= d1)
                    { // 表示当前点与路径的垂足,在路径线段范围内;此情况下,当前点与路径段的最近距离,由垂线段长度决定
                        // 根据海伦公式求三角形高,从而得到距离
                        double _p = (_a + _b + _c) / 2.0;
                        double _s = Math.Sqrt(_p * (_p - _a) * (_p - _b) * (_p - _c));
                        mdist = 2 * _s / _b;
                    }
                    else // 否则,路径段端点与当前点距离中较小的作为路径与当前点的最小距离
                    {
                        mdist = Math.Min(_a, _c);
                    }

                    if (minDistance < 0 || ((int)minDistance) > ((int)mdist))
                    {
                        minDistance = mdist;
                        dist2Point  = _a;
                        //C++ TO C# CONVERTER WARNING: The following line was determined to be a copy assignment (rather than a reference assignment) - this should be verified and a 'CopyFrom' method should be created if it does not yet exist:
                        //ORIGINAL LINE: prevPoint = prev;
                        prevPoint    = prev;
                        target_index = i;
                        dc2t         = d1;
                        dc2p         = d3;
                        dp2t         = d2;
                    }
                }

                //C++ TO C# CONVERTER TODO TASK: The memory management function 'memcpy' has no equivalent in C#:

                // memcpy(target_point, path + target_index, sizeof(PATH_POINT));
                //double tooClosingDistance = target_point.v *  CLOSING_VELOCITY_K;
                //if(tooClosingDistance < TOO_CLOSING_DISTANCE_MIN) tooClosingDistance = TOO_CLOSING_DISTANCE_MIN;
                double tooClosingDistance = TOO_CLOSING_DISTANCE_MIN;
                if (dist2Point > tooClosingDistance) // 如果离目标点不是太近,或者一段路径还没走到一半,则直接使用目标点
                //|| dc2p <= dc2t )
                {
                    break;
                }

                PATH_POINT next = new PATH_POINT();
                GetNextPoint(path, path_point_cnt, target_index, m_param.TrackDirection, out next);
                //C++ TO C# CONVERTER TODO TASK: The memory management function 'memcpy' has no equivalent in C#:
                target_point = next;
            } while (true);

            m_result.target.x      = target_point.x;
            m_result.target.y      = target_point.y;
            m_result.target.v      = target_point.v;
            m_result.targetIndex   = target_index;
            m_result.distance2Path = (int)minDistance;

            return(0);
        }