public void GetWay_PASS(APoint P_start, APoint P_end) //P_start、P_end两点参数
{
Get_MAP(); //获取地图信息
Open_List.Add(P_start); //首个添加P_start
while (!(IsInOpenList(P_end.x, P_end.y) || Open_List.Count == 0))
{
APoint P_now = GetMinFFromOpenList();
Open_List.Remove(P_now);
Close_List.Add(P_now);
Check8Point_PASS(P_now, Map_Inf, ref P_end);
}
APoint p = GetAPointFromOpenList(P_end.x, P_end.y);
if (p == null)
{
MessageBox.Show("无可行路径！");
}
else
{
while (p.father != null)
{
if (p.x == p.father.x || p.y == p.father.y)
{
Cost10_num++;
}
else
{
Cost14_num++;
}
p = p.father;
Map_Inf[p.x, p.y] = 4;//标记上色
}
}
Refresh_MAP();
}
public MirResult MoveAndFireToPoint(MapInfo mapInfo, APoint point, CancellationTokenSource cancellationTokenSource, int distance = 2)
{
return(MoveToPoint(mapInfo, point, cancellationTokenSource, distance, () => {
bool move = true;
var actionResult = MirAction.FindMaster();
//#TODO 判断 actionResult ,并输出日志
var masters = actionResult.Data;
while (masters.Any())
{
move = true;
var master = masters.NearbyMaster(MirContext.Position);
MirAction.AttackMaster(master, cancellationTokenSource);
}
var actionResult2 = MirAction.FindItems();
//#TODO 判断 actionResult ,并输出日志
var items = actionResult2.Data;
while (items.Any())
{
//#TODO ,需要判断物品所在位置被人怪站住的情况
var item = items.NearbyItem(MirContext.Position);
MirAction.PickupItem(item);
}
return move;
}));
}
// 根据tiledmap坐标点获得地图坐标
public static Vector2 GetVec2ByApoint(APoint p)
{
float x = p.x * JsonTest.getMe().m_mapData.getgeziw() - 204;
float y = (p.y) * JsonTest.getMe().m_mapData.getgezih() - 118;
return(new Vector2(x, y));
}
/// <summary>
/// 计算距离
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <returns></returns>
public int CalculationDistance(APoint p1, APoint p2)
{
int dx = Math.Abs(p1.X - p2.Y);
int dy = Math.Abs(p1.Y - p2.Y);
return(dx > dy ? dx : dy);
}
public PointData(APoint p, float g, float h, PointData parent)
{
this.point = p;
this.g = g;
this.h = h;
this.parent = parent;
}
// Corner cells not used
private void SearchPath()
{
while (_currentX != _lastX || _currentY != _lastY)
{
MarkCell(_currentX, _currentY, 0, -1);
//MarkCell(_currentX, _currentY, 1, -1);
MarkCell(_currentX, _currentY, 1, 0);
//MarkCell(_currentX, _currentY, 1, 1);
MarkCell(_currentX, _currentY, 0, 1);
//MarkCell(_currentX, _currentY, -1, 1);
MarkCell(_currentX, _currentY, -1, 0);
//MarkCell(_currentX, _currentY, -1, -1);
int x = 0, y = 0;
if (!FindNextCell(ref x, ref y))
{
break;
}
if (x == -1 || y == -1)
{
break;
}
APoint tmp = _openList.Find(point => point.X == x && point.Y == y);
var toCloseList = new APoint(tmp);
_openList.Remove(tmp);
_closeList.Add(toCloseList);
_currentX = x;
_currentY = y;
}
TraceBack();
}
/**
* h函数
*/
static float h(APoint pnt)
{
// return hBFS(pnt);
return(Astar.hEuclidianDistance(pnt));
// return hPowEuclidianDistance(pnt);
// return hManhattanDistance(pnt);
}
private void MarkCell(int parentX, int parentY, int kx, int ky)
{
int g = GCost(parentX, parentY, kx, ky);
if (g != -1)
{
int h = HCost(parentX + kx, parentY + ky);
if (_openList.Exists(point => parentX == point.X && parentY == point.Y))
{
APoint tmp = _openList.Find(point => parentX == point.X && parentY == point.Y);
if (tmp.F > g + h)
{
tmp.F = g + h;
tmp.ParentX = parentX;
tmp.ParentY = parentY;
}
}
else
if (!_closeList.Exists(point => parentX + kx == point.X && parentY + ky == point.Y))
{
APoint tmp = new APoint()
{
F = g + h,
H = h,
ParentX = parentX, ParentY = parentY,
X = parentX + kx, Y = parentY + ky
};
_openList.Add(tmp);
}
}
}
/// <summary>
/// 计算移动方式
/// <para>必须保证 p1 p2相临再跑一步的距离内</para>
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
/// <returns></returns>
public RunType CalculationRunType(PositionInfo position, APoint p2)
{
var xd = position.Point.X - p2.X;
var yd = position.Point.Y - p2.Y;
if (Math.Abs(xd) <= 1 && Math.Abs(yd) <= 1)
{
return(RunType.Normal);
}
else
{
//APoint p3 = new APoint(p2.X + x, p2.Y + y);
var maze = MirContext.Maze(position.MapInfo);
int x = p2.X + xd;
int y = p2.Y + yd;
if (x > MirContext.ReadMap.Width || y > MirContext.ReadMap.Height)//跑步终点坐标超出地图
{
return(RunType.Normal);
}
else
{
//跑步终点坐标有障碍物则走，否则跑
return(maze[x, y] == 1 ? RunType.Normal : RunType.FastRun);
}
}
}
public double Perimeter()
{
Double ab = APoint.Distance(BPoint);
Double ac = APoint.Distance(CPoint);
Double bc = APoint.Distance(CPoint);
return(ab + ac + bc);
}
private string getKey(APoint pnt)
{
// return String.format("%d-%d", pnt.x, pnt.y);
if (pnt == null)
{
return(null);
}
return(pnt.x + "-" + pnt.y);
}
public PointData find(APoint pnt)
{
string key = this.getKey(pnt);
if (index.ContainsKey(key))
{
return(this.index[key]);
}
return(null);
}
public override double GetHeuristicCost(APoint goal)
{
//return GetManhattanDistance(this, (ASquare)goal);
return(GetEuclidDistance(this, (ASquare)goal));
//if (AdditionalAngleCoeffs.ContainsKey(goal as ASquare))
//{
// res += AdditionalAngleCoeffs[goal as ASquare];
//}
//return res;
}
public APoint(APoint point)
{
X = point.X;
Y = point.Y;
F = point.F;
H = point.H;
ParentX = point.ParentX;
ParentY = point.ParentY;
}
private APoint GetMinFFromOpenList()
{
APoint Pmin = null;
foreach (APoint p in Open_List)
{
if (Pmin == null || Pmin.G_value + Pmin.H_value > p.G_value + p.H_value)
{
Pmin = p;
}
}
return(Pmin);
}
public static void Main(String[] args)
{
Console.WriteLine("Number of points created: " + APoint.GetSize());
APoint p = new APoint(12, 123), q = new APoint(200, 10), r = new APoint(99, 12);
APoint s = p;
q = null;
Console.WriteLine("Number of points created: " + APoint.GetSize());
Console.WriteLine("r is point number " + r.GetIndex());
for (int i = 0; i < APoint.GetSize(); i++)
{
Console.WriteLine("APoint number " + i + " is " + APoint.GetPoint(i));
}
}
private int Get_CostG(APoint p)
{
if (p.father == null)//根节点G值为0
{
return(0);
}
if (p.x == p.father.x || p.y == p.father.y)//横向、纵向移动
{
return(p.father.G_value + 10);
}
else//斜方向移动
{
return(p.father.G_value + 14);
}
}
private void Check8Point(APoint P_now, byte[,] Map, ref APoint P_end)
{
for (int i = 0; i <= 7; i++)
{
int X_temp = P_now.x + Offset_x[i];
int Y_temp = P_now.y + Offset_y[i];
if (X_temp >= 0 && X_temp <= Awidth && Y_temp >= 0 && Y_temp <= Aheight)
{
if (Map[X_temp, P_now.y] == 1 || Map[P_now.x, Y_temp] == 1) //若po到pt的反对角上有障碍
{
continue; //忽略当前方格
}
if (Map[X_temp, Y_temp] == 0 && !IsInCloseList(X_temp, Y_temp)) //0为可行区间
{
if (IsInOpenList(X_temp, Y_temp)) //如果P_next在OpenList中且不在UnableList中
{
APoint P_next = GetAPointFromOpenList(X_temp, Y_temp);
int G_new = 0;
if (P_now.x == P_next.x || P_now.y == P_next.y)
{
G_new = P_now.G_value + 10;
}
else
{
G_new = P_now.G_value + 14;
}
if (G_new < P_next.G_value)//更新P_next的G值
{
Open_List.Remove(P_next);
P_next.father = P_now;
P_next.G_value = G_new;
Open_List.Add(P_next);
}
}
else
{//不在开启列表中
APoint P_next = new APoint();
P_next.x = X_temp;
P_next.y = Y_temp;
P_next.father = P_now;
P_next.G_value = Get_CostG(P_next);
P_next.H_value = Get_CostH(P_next, P_end);
Open_List.Add(P_next);
}
}
}
}
}
// 2011.10.31
private void ATbCalibCheckerboard_FormClosed(object sender, FormClosedEventArgs e)
{
tmrLive.Enabled = false;
tmrTime.Enabled = false;
// 2015.03.17
cogDisplay.Dispose();
cogDisplayStatusBar.Dispose();
// 2017.01.09
m_aCalibCheckerboard = null;
#if (!_USE_BASLER_PYLON && !_USE_IMAGING_CONTROL)
m_aAcqFifo = null;
#endif
m_aPoint = null;
m_aDisplay = null;
}
private void ATbEdtFixtureNPointToNPoint_FormClosed(object sender, FormClosedEventArgs e)
{
tmrLive.Enabled = false;
tmrTime.Enabled = false;
// 2017.01.09
cogFixtureNPointToNPointEditV2.Subject = null;
// 2015.03.17
cogFixtureNPointToNPointEditV2.Dispose();
// 2017.01.09
m_aFixtureNPointToNPoint = null;
#if (!_USE_BASLER_PYLON && !_USE_IMAGING_CONTROL)
m_aAcqFifo = null;
#endif
m_aPoint = null;
}
public ATbCalibNPointToNPoint(int nType, int nPoint, int nToolIndex)
{
InitializeComponent();
cogDisplayStatusBar.Display = cogDisplay;
m_aPoint = AVisionProBuild.GetPoint(nType, nPoint);
m_nType = nType;
m_nPoint = nPoint;
m_nToolIndex = nToolIndex;
// 2014.10.30
#if (!_USE_BASLER_PYLON && !_USE_IMAGING_CONTROL && !_USE_1Camera)
if (m_aPoint.GetToolCount("AcqFifo") > 0)
#endif
{
// 2014.10.30
#if (!_USE_BASLER_PYLON && !_USE_IMAGING_CONTROL && !_USE_1Camera)
m_aAcqFifo = m_aPoint.GetTool("AcqFifo", 0) as AAcqFifo;
#elif _USE_1Camera
m_aAcqFifo = AVisionProBuild.GetAcq();
#endif
}
// 2016.07.29
if (m_aPoint.GetToolCount("CalibNPointToNPoint") > m_nToolIndex)
{
m_aCalibNPointToNPoint = m_aPoint.GetTool("CalibNPointToNPoint", m_nToolIndex) as ACalibNPointToNPoint;
}
else
{
m_aCalibNPointToNPoint = new ACalibNPointToNPoint();
m_aCalibNPointToNPoint.Name = AVisionProBuild.MakeName("CalibNPointToNPoint", DateTime.Now);
m_aPoint.Add("CalibNPointToNPoint", m_aCalibNPointToNPoint);
}
m_aDisplay = new ADisplay(cogDisplay, "");
lblTitle.Text = AVisionProBuild.GetTypeName(m_nType) + " [" + m_aPoint.Name + "]";
//m_aDisplay.Display.Image = m_aCalibNPointToNPoint.InputImage;
//m_aDisplay.Display.Fit(true);
InitLanguage();
// 2011.10.07 위치이동
InitializeCalibration();
}
public ATbEdtFixtureNPointToNPoint(int nType, int nPoint, int nToolIndex)
{
InitializeComponent();
m_aPoint = AVisionProBuild.GetPoint(nType, nPoint);
m_nType = nType;
m_nPoint = nPoint;
m_nToolIndex = nToolIndex;
// 2014.10.30
#if (!_USE_BASLER_PYLON && !_USE_IMAGING_CONTROL && !_USE_1Camera)
if (m_aPoint.GetToolCount("AcqFifo") > 0)
#endif
{
// 2014.10.30
#if (!_USE_BASLER_PYLON && !_USE_IMAGING_CONTROL && !_USE_1Camera)
m_aAcqFifo = m_aPoint.GetTool("AcqFifo", 0) as AAcqFifo;
#elif _USE_1Camera
m_aAcqFifo = AVisionProBuild.GetAcq();
#endif
}
// 2016.07.29
if (m_aPoint.GetToolCount("FixtureNPointToNPoint") > m_nToolIndex)
{
m_aFixtureNPointToNPoint = m_aPoint.GetTool("FixtureNPointToNPoint", m_nToolIndex) as AFixtureNPointToNPoint;
}
else
{
m_aFixtureNPointToNPoint = new AFixtureNPointToNPoint();
m_aFixtureNPointToNPoint.Name = AVisionProBuild.MakeName("FixtureNPointToNPoint", DateTime.Now);
m_aPoint.Add("FixtureNPointToNPoint", m_aFixtureNPointToNPoint);
}
cogFixtureNPointToNPointEditV2.Subject = m_aFixtureNPointToNPoint.GetTool();
InitLanguage();
InitializeFixtureNPointToNPoint();
// 2016.04.06
m_cogImage = m_aFixtureNPointToNPoint.InputImage;
}
public APoint GetNextPoint(APoint point, MirDirection mirDirection, RunType runType)
{
int step = runType == RunType.Normal ? 1 : 2;
APoint temp = null;
switch (mirDirection)
{
case MirDirection.Up:
temp = new APoint(point.X, point.Y + step);
break;
case MirDirection.UpRight:
temp = new APoint(point.X + step, point.Y + step);
break;
case MirDirection.Right:
temp = new APoint(point.X + step, point.Y);
break;
case MirDirection.DownRight:
temp = new APoint(point.X + step, point.Y - step);
break;
case MirDirection.Down:
temp = new APoint(point.X, point.Y - step);
break;
case MirDirection.DownLeft:
temp = new APoint(point.X - step, point.Y - step);
break;
case MirDirection.Left:
temp = new APoint(point.X - step, point.Y);
break;
case MirDirection.UpLeft:
temp = new APoint(point.X - step, point.Y + step);
break;
}
return(temp);
}
/// <summary>
/// 计算位置方向
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <returns></returns>
public MirDirection CalculationDirection(APoint p1, APoint p2)
{
if (p2 == null || p1 == null)
{
return(MirDirection.None);
}
if (p1.X == p2.X && p1.Y < p2.Y)
{
return(MirDirection.Up);
}
if (p1.X == p2.X && p1.Y > p2.Y)
{
return(MirDirection.Down);
}
else if (p1.X < p2.X && p1.Y < p2.Y)
{
return(MirDirection.UpRight);
}
else if (p1.X < p2.X && p1.Y > p2.Y)
{
return(MirDirection.DownRight);
}
else if (p1.X > p2.X && p1.Y == p2.Y)
{
return(MirDirection.Left);
}
else if (p1.X > p2.X && p1.Y > p2.Y)
{
return(MirDirection.DownLeft);
}
else if (p1.X > p2.X && p1.Y < p2.Y)
{
return(MirDirection.UpLeft);
}
else
{
return(MirDirection.None);
}
}
public static APoint mEndPos; //结束点
//寻路
public static List <APoint> SearchRoad(APoint startPos, APoint endPos, TILEDMAP_TYPE[][] roadMap)
{
Astar.mStartPos = null;
Astar.mEndPos = null;
Astar.mRoadPointArr = null;
Astar.mStartPos = new APoint(startPos);
Astar.mEndPos = new APoint(endPos);
Astar.mRoadPointArr = roadMap;
List <APoint> _ret = Astar.Search();
List <APoint> _ret_new = new List <APoint>();
_ret_new.Add(endPos);
foreach (var data in _ret)
{
_ret_new.Add(data);
}
return(_ret_new);
}
/**
* 欧式距离平方,大于等于实际值
*/
static float hPowEuclidianDistance(APoint pnt)
{
return((float)(Math.Pow(pnt.x - Astar.mEndPos.x, 2) + Math.Pow(pnt.y - Astar.mEndPos.y, 2)));
}
/**
* 曼哈顿距离,小于等于实际值
*/
static float hManhattanDistance(APoint pnt)
{
return(Math.Abs(pnt.x - Astar.mEndPos.x) + Math.Abs(pnt.y - Astar.mEndPos.y));
}
/**
* 搜索算法
*/
public static List <APoint> Search()
{
MinHeap heap = new MinHeap(); // 用最小堆来记录扩展的点
XyData[] directs = new XyData[4]; // 可以扩展的四个方向
//[{ x: 1, y: 0}, { x: 0, y: 1}, { x: -1, y: 0}, { x: 0, y: -1}];
directs[0] = new XyData(1, 0);
directs[1] = new XyData(0, 1);
directs[2] = new XyData(-1, 0);
directs[3] = new XyData(0, -1);
heap.add(new PointData(Astar.mStartPos, 0, 0, null)); // 把起始点放入堆
PointData lastData = null; // 找到的最后一个点的数据,用来反推路径
for (bool finish = false; !finish && !heap.isEmpty();)
{
PointData data = heap.getAndRemoveMin(); // 取出f值最小的点
APoint point = data.point;
if (MapMgr.width <= point.x)
{
continue;
}
if (Astar.mRoadPointArr[point.x] == null)
{
continue;
}
if (Astar.mRoadPointArr[point.x][point.y] == TILEDMAP_TYPE.SPACE) // 将取出的点标识为已访问点
{
Astar.mRoadPointArr[point.x][point.y] = TILEDMAP_TYPE.VISITED;
}
for (int i = 0; i < directs.Length; ++i) // 遍历四个方向的点
{
APoint newPnt = new APoint(point.x + directs[i].x, point.y + directs[i].y);
if (newPnt.x >= 0 && newPnt.x < MapMgr.width && newPnt.y >= 0 &&
newPnt.y < MapMgr.height)
{
TILEDMAP_TYPE e = Astar.mRoadPointArr[newPnt.x][newPnt.y];
if (Astar.mEndPos.equals(newPnt)) // 如果是终点,则跳出循环,不用再找
{
lastData = data;
finish = true;
break;
}
if (e != TILEDMAP_TYPE.SPACE) // 如果不是空地,就不需要再扩展
{
continue;
}
PointData inQueueData = heap.find(newPnt);
if (inQueueData != null) // 如果在堆里,则更新g值
{
if (inQueueData.g > data.g + 1)
{
inQueueData.g = data.g + 1;
inQueueData.parent = data;
}
}
else // 如果不在堆里,则放入堆中
{
float h = Astar.h(newPnt);
PointData newData = new PointData(newPnt, data.g + 1, h, data);
heap.add(newData);
}
}
}
}
List <APoint> arr = new List <APoint>();
// 反向找出路径
for (PointData pathData = lastData; pathData != null;)
{
APoint pnt = pathData.point;
if (Astar.mRoadPointArr[pnt.x][pnt.y] == TILEDMAP_TYPE.VISITED)
{
Astar.mRoadPointArr[pnt.x][pnt.y] = TILEDMAP_TYPE.ON_PATH;
arr.Add(pnt);
}
pathData = pathData.parent;
}
return(arr);
}
/// <summary>
/// 设置阻挡 zudang： true 是阻挡 false 可以行走
/// </summary>
/// <param name="pos"></param>
/// <param name="isStop"></param>
public void SetStopPoint(Vector2 pos, bool isStop)
{
APoint _APoint = GetAPointByPosition(pos);
JsonTest.getMe().SetStopPoint(_APoint.x, _APoint.x, isStop);
}
public override double GetCost(APoint goal)
{
var dist = GetEuclidDistance(this, goal as ASquare);
return((Weight + (goal as ASquare).Weight) * dist);
}
static void Main(string[] args)
{
var point = new APoint();
var radialPoint = new RadialPoint();
}
