public static float CosABC(POS2D p1, POS2D p2, POS2D p3)
{
float x1 = (float)(p1.u - p2.u);
float y1 = (float)(p1.v - p2.v);
float x2 = (float)(p3.u - p2.u);
float y2 = (float)(p3.v - p2.v);
return (float)(x1 * x2 + y1 * y2) / (float)(System.Math.Sqrt(x1 * x1 + y1 * y1) * System.Math.Sqrt(x2 * x2 + y2 * y2));
}
/*---------------------------------------------------------------
* 某点是否可达
---------------------------------------------------------------*/
bool IsPosReachable(POS2D pos)
{
return IsPosReachable(pos.u, pos.v);
}
/*---------------------------------------------------------------
* 坐标->坐标，点式
---------------------------------------------------------------*/
bool CanWalkNeighbor(POS2D pos1, POS2D pos2)
{
if (!IsPosReachable(pos1) || !IsPosReachable(pos2))
{
return false;
}
int du = ABS(pos2.u - pos1.u);
int dv = ABS(pos2.v - pos1.v);
if (du > 1 || dv > 1)
{
return false;
}
bool retflag = false;
switch (du + dv)
{
case 0:
retflag = true;
break;
case 1:
retflag = true;
break;
case 2:
retflag = IsPosReachable(pos1.u, pos2.v) && (IsPosReachable(pos2.u, pos1.v));
break;
default:
retflag = false;
break;
}
return retflag;
}
/*---------------------------------------------------------------
* 坐标->坐标，坐标式
---------------------------------------------------------------*/
bool CanWalkNeighbor(int x1, int z1, int x2, int z2)
{
POS2D pos1 = new POS2D(x1, z1);
POS2D pos2 = new POS2D(x2, z2);
return CanWalkNeighbor(pos1, pos2);
}
/*---------------------------------------------------------------
* 是否直达
---------------------------------------------------------------*/
bool CanGoStraightForward(POS2D posFrom, POS2D posTo)
{
POS2D posStop = new POS2D();
return CanGoStraightForward(posFrom, posTo, posStop);
}
/*---------------------------------------------------------------
* 是否直达
---------------------------------------------------------------*/
bool CanGoStraightForward(Vector3 posFrom, Vector3 posTo, POS2D posStop)
{
float x, z, dx, dz, s1, s2, k, tx, tz;
x = posFrom.x;
z = posFrom.z;
tx = posTo.x - posFrom.x;
tz = posTo.z - posFrom.z;
s1 = SIGN((int)tx);
s2 = SIGN((int)tz);
dx = ABS((int)tx);
dz = ABS((int)tz);
POS2D _posFrom = GetMapPos(posFrom); //GetMapPos(Vector3);服务器通过地图大小拓扑出二维坐标，此处直接转换，会有误差
POS2D _posTo = GetMapPos(posTo); //GetMapPos(Vector3);服务器通过地图大小拓扑出二维坐标，此处直接转换，会有误差
// optimize for one pixel move
if (ABS((int)_posFrom.u - _posTo.u) <= 1 && ABS((int)_posFrom.v - _posTo.v) <= 1)
{
// 双向通行的格子才允许进入
return CanWalkNeighbor(_posFrom, _posTo) && CanWalkNeighbor(_posTo, _posFrom);
}
// Interchange ?
bool bInterchange = false;
k = dz > dx ? tx / tz : tz / tx; //斜率
if (dz > dx)
{
float tmp = dx;
dx = dz;
dz = tmp;
bInterchange = true;
}
// Init the error term
float e = dz - dx;
// Start loop, now dx >= dy
Vector3 pos = posFrom;
Vector3 last = pos;
for (int i = 0; i <= (int)(dx + 1); i++)
{
// save the last reachable pos
posStop = GetMapPos(pos); //GetMapPos(Vector3);服务器通过地图大小拓扑出二维坐标，此处直接转换，会有误差
pos.x = x;
pos.z = z;
POS2D _pos = GetMapPos(pos); //GetMapPos(Vector3);服务器通过地图大小拓扑出二维坐标，此处直接转换，会有误差
// 双向通行的格子才允许进入
if (!CanWalkNeighbor(posStop, _pos) || !CanWalkNeighbor(_pos, posStop))
{
return false;
}
while (e > 0)
{
e -= dx;
}
if (bInterchange)
{
z += s2;
x += k * s2;
}
else
{
x += s1;
z += k * s1;
}
e += dz;
}
return true;
}
/*---------------------------------------------------------------
* 是否直达
---------------------------------------------------------------*/
bool CanGoStraightForward(POS2D posFrom, POS2D posTo, POS2D posStop)
{
int x, y, dx, dy, s1, s2;
x = posFrom.u;
y = posFrom.v;
dx = posTo.u - posFrom.u;
dy = posTo.v - posFrom.v;
s1 = SIGN(dx);
s2 = SIGN(dy);
dx = ABS(dx);
dy = ABS(dy);
// optimize for one pixel move
if (dx <= 1 && dy <= 1)
{
return CanWalkNeighbor(posFrom, posTo) && CanWalkNeighbor(posTo, posFrom);
}
// Interchange
bool bInterchange = false;
if (dy > dx)
{
int tmp = dx;
dx = dy;
dy = tmp;
bInterchange = true;
}
// Init the error term
int e = 2 * dy - dx;
// Start loop, now dx >= dy
POS2D pos = posFrom;
POS2D last = pos;
for (int i = 0; i <= dx; i++)
{
// save the last reachable pos
posStop = pos;
pos.u = x;
pos.v = y;
if (!CanWalkNeighbor(posStop, pos) || !CanWalkNeighbor(pos, posStop))
{
return false;
}
while (e > 0)
{
if (bInterchange)
x += s1;
else
y += s2;
e -= 2 * dx;
}
if (bInterchange)
y += s2;
else
x += s1;
e += 2 * dy;
}
return true;
}
/*---------------------------------------------------------------
* 判断各个坐标在指定最近数据的路点是否可达：如果为true则表示该点是可达的，否则不可达
---------------------------------------------------------------*/
public bool[][] IsOk()
{
bool[][] result = new bool[iMapLength][];
wayPoint_near = new int[iMapLength][];
for (int i = -halfMapLength; i < halfMapLength; ++i)
{
//坐标转换，用于实现地图坐标到数组坐标的映射
int iX = i + halfMapLength;
result[iX] = new bool[iMapLength];
wayPoint_near[iX] = new int[iMapLength];
for (int j = -halfMapLength; j < halfMapLength; ++j)
{
//坐标转换
int iY = j + halfMapLength;
//初始化不可达
result[iX][iY] = false;
wayPoint_near[iX][iY] = -1;
// 当前点
POS2D currentPos2D = new POS2D(iX, iY);
//如果该点本身不可达，则直接略过
if (!canReachable[iX][iY]) continue;
//遍历各个路点,找出最短&次短距离...的坐标
SortedDictionary<int, ArrayList> distance_index = new SortedDictionary<int, ArrayList>();
for (int k = 0; k < wayPoints.Length; ++k)
{
POS2D wayPoint2D = new POS2D((int)(wayPoints[k].x + halfMapLength), (int)(wayPoints[k].z + halfMapLength));
//求当前点与路点距离
int fDistance = (int)((currentPos2D.u - wayPoint2D.u) * (currentPos2D.u - wayPoint2D.u) +
(currentPos2D.v - wayPoint2D.v) * (currentPos2D.v - wayPoint2D.v));
//按顺序插入，自动排序
if (distance_index.ContainsKey(fDistance))
{
ArrayList iLocalArray = distance_index[fDistance];
iLocalArray.Add(k);
}
else
{
ArrayList iarray = new ArrayList();
iarray.Add(k);
distance_index.Add(fDistance, iarray);
}
}
//在可达路点中选取iWaypointNum个路点判断是否可达
int iCount = 0, iWaypointNum = 15;
foreach (int distance in distance_index.Keys)
{
//双层for跳出
if (result[iX][iY])
break;
if (iCount < iWaypointNum)
{
//取其中的路点坐标
foreach (int iValue in distance_index[distance])
{
Vector3 minVector = wayPoints[iValue];
POS2D minVector2D = GetMapPosEx(minVector); //new POS2D((int)(minVector.x + halfMapLength), (int)(minVector.z + halfMapLength));
//判断当前点距离对应路点是否可达
if (CanGoStraightForward(currentPos2D, minVector2D))
{
wayPoint_near[iX][iY] = iValue;
result[iX][iY] = true;
break;
}
iCount++;
}//foreach (int iValue in iarray)
}
}//foreach (int distance in distance_index.Keys)
}
}
return result;
}