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);
}
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);
}
// 计算目标路径点 -----
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);
}