/// <summary>
/// 添加附近的障碍点
/// </summary>
/// <param name="p"></param>
public void AddNearBlock(Pos p)
{
var dist = (p - Pos).Length;
var d = dist - RobotObj.Size;
if (d < 0)
{
NearToBlockCount++;
return;
}
if (d == 0 || d <= RobotObj.SafeDist)
{
CloseToBlockDist.Add(d);
return;
}
}
/// <summary>
/// 删除附近的障碍点
/// </summary>
/// <param name="p"></param>
public void RemoveNearBlock(Pos p)
{
var dist = (p - Pos).Length;
var d = dist - RobotObj.Size;
if (d < 0 && NearToBlockCount > 0)
{
NearToBlockCount--;
return;
}
if (d == 0 || d <= RobotObj.SafeDist)
{
CloseToBlockDist.Remove(d);
return;
}
}
