/// <summary>
/// 两个矢量位置间的角度
/// </summary>
/// <param name="pos1">位置坐标1</param>
/// <param name="pos2">位置坐标2</param>
/// <returns>返回角度</returns>
public static float PolarAngle2(PathVector3 pos1, PathVector3 pos2)
{
float vx = pos2.x - pos1.x;
float vy = pos2.z - pos1.z;
float mag = (float)System.Math.Sqrt(vx * vx + vy * vy);
if (mag == 0)
{
return(0);
}
float angle = RadToDeg * (float)System.Math.Asin(vx / mag);
if (vy < 0)
{
angle -= 180;
angle *= -1;
}
else if (vx < 0 && vy > 0)
{
angle += 360;
}
return(angle);
}
public void SetPosition( PathVector3 v )
{
m_tx = v.tx(m_map);
m_tz = v.tz(m_map);
position.x = v.x;
position.y = v.y;
position.z = v.z;
}
public bool CheckValid(PathVector3 v)
{
if (v.x < startx || v.z < startz || v.x >= boundx || v.z >= boundz)
{
return(false);
}
else
{
return(true);
}
}
/// <summary>
/// 是否处于同一个tile中
/// </summary>
public static bool IsSameTile(Vector3 pos1, Vector3 pos2, PathMap map)
{
PathVector3 v1 = new PathVector3(pos1.x, pos1.y, pos1.z);
PathVector3 v2 = new PathVector3(pos2.x, pos2.y, pos2.z);
if (v1.tx(map) == v2.tx(map) && v2.tz(map) == v2.tz(map))
{
return(true);
}
else
{
return(false);
}
}
/// <summary>
/// 计算当前节点的花费
/// </summary>
/// <param name="origin">一个长度为3的数组保存起始点的X Y Z三个坐标位置</param>
/// <param name="target">一个长度为3的数组保存目标点的X Y Z三个坐标位置</param>
/// <returns>返回花费,值越大表示这个节点的代价越高</returns>
public float SetCost(PathVector3 origin, PathVector3 destination, float extraCost )
{
int ox = origin.tx(map);
int oz = origin.tz(map);
int dx = destination.tx(map);
int dz = destination.tz(map);
//fromStart = (float)(System.Math.Abs(tx - orx) + System.Math.Abs(tz - orz)) + extraCost;
fromStart = parentCount + extraCost;
heuristic = (float)(System.Math.Abs(tx - dx) + System.Math.Abs(tz - dz));
cost = fromStart + heuristic;
return cost;
}
// Update is called once per frame
void Update()
{
if (m_currentAction != null)
{
m_currentAction();
}
// get debug info
nodeCount = m_finder.NodesCount;
PathVector3 next = m_finder.GetCurrentNodePosition();
if (next != null)
{
nodePosition = GridUtility.FloatToVector(next);
}
}
/// <summary>
/// 距离（不包括y轴)
/// </summary>
public static float TileDistance(PathVector3 vec1, PathVector3 vec2, PathMap map)
{
/*
* vec1.x = (float)System.Math.Floor(vec1.x);
* vec1.y = 0;
* vec1.z = (float)System.Math.Floor(vec1.z);
*
* vec2.x = (float)System.Math.Floor(vec2.x);
* vec2.y = 0;
* vec2.z = (float)System.Math.Floor(vec2.z);
*/
//return (float)System.Math.Sqrt((vec1.x - vec2.x) * (vec1.x - vec2.x) + (vec1.z - vec2.z) * (vec1.z - vec2.z));
int tx1 = vec1.tx(map);
int tz1 = vec1.tz(map);
int tx2 = vec2.tx(map);
int tz2 = vec2.tz(map);
return(System.Math.Max(System.Math.Abs(tx1 - tx2), System.Math.Abs(tz1 - tz2)));
}
/// <summary>
/// 移动到目标
/// </summary>
/// <param name="finder"></param>
/// <param name="entityTransform"></param>
/// <param name="entityMoveSpeed"></param>
/// <param name="entityRotSpeed"></param>
/// <param name="tolerance"></param>
/// <returns></returns>
public static bool MoveToTarget(PathFinder finder, ref Transform entityTransform, float entityMoveSpeed, float entityRotSpeed)
{
if (finder.NodesCount <= 0)
{
return(false);
}
// current pos
Vector3 mypos = entityTransform.position;
float oldy = mypos.y;
mypos.y = 0;
PathVector3 f = GridUtility.VectorToPath(mypos);
// current angle y
float angley = entityTransform.eulerAngles.y;
// move to
bool b = finder.MoveToTargetNode(ref f, ref angley, entityMoveSpeed, entityRotSpeed);
if (b)
{
// get new position
entityTransform.position = FloatToVector(f);
Vector3 pos = entityTransform.position;
pos.y = oldy;
entityTransform.position = pos;
// get new angle y
entityTransform.eulerAngles = new Vector3(0, angley, 0);
}
if (b)
{
return(true);
}
else
{
return(false);
}
}
public void SetBlock(Vector3 pos, int data)
{
if (m_map == null || m_map.m_Data == null || m_map.m_Data.Length == 0)
{
Build();
Debug.Log("map is null");
return;
}
PathVector3 pv3 = GridUtility.VectorToPath(pos);
int tx = pv3.tx(m_map);
int tz = pv3.tz(m_map);
bool ok = m_map.CheckValid(pv3);
if (!ok)
{
Debug.Log("position error:" + pos);
return;
}
blocks[tx + tz * m_mapSizeX] = data;
}
public float distanceTo(PathVector3 pv, PathMap map)
{
return(PathHelper.TileDistance(this, pv, map));
}
// 复制
public void Set(PathVector3 pv)
{
x = pv.x;
y = pv.y;
z = pv.z;
}
public void SetBlock(PathVector3 v, int entrySize, int dynamic, float movespeed)
{
SetBlock(v.x, v.z, entrySize, dynamic, movespeed);
}
public void RemoveBlock(PathVector3 v, int entitySize, int dy, float mv)
{
RemoveBlock(v.x, v.z, entitySize, dy, mv);
}
/// <summary>
/// Build Map
/// </summary>
//[ContextMenu("Build")]
public void Build()
{
if (blocks == null)
{
blocks = new int[m_mapSizeX * m_mapSizeZ];
old_width = m_mapSizeX;
old_height = m_mapSizeZ;
}
else
{
if (blocks.Length != m_mapSizeX * m_mapSizeZ)
{
int[] newblock = new int[m_mapSizeX * m_mapSizeZ];
for (int i = 0; i < m_mapSizeX * m_mapSizeZ; i++)
{
if (blocks.Length <= i)
{
continue;
}
int tx = i - ((i / m_mapSizeX) * m_mapSizeX);
int tz = i / m_mapSizeX;
if (tx >= old_width || tz >= old_height)
{
continue;
}
int index = tx + tz * old_width;
if (index >= blocks.Length)
{
continue;
}
newblock[i] = blocks[index];
}
blocks = newblock;
old_width = m_mapSizeX;
old_height = m_mapSizeZ;
}
}
Vector3 startpos = this.transform.position;
// create map
m_map = new PathMap(m_mapSizeX, m_mapSizeZ, startpos.x, startpos.z, m_quadSize);
// build a empty grid map
for (int i = 0; i < m_mapSizeX; i++)
{
for (int k = 0; k < m_mapSizeZ; k++)
{
//MapData data = new MapData();
//data.fieldtype = MapData.FieldTypeID.FREE;
//m_map.SetData(i, k, data);
if (blocks[i + k * m_mapSizeX] > 0)
{
PathVector3 pv3 = new PathVector3();
pv3.Set(i, k, m_map);
m_map.SetBlock(pv3, 0, 0, 0);
}
}
}
/*
* // Find obstacles;
* GameObject[] nodes = (GameObject[])GameObject.FindGameObjectsWithTag(m_NodeTag);
* foreach (GameObject nodeobj in nodes)
* {
*
* GridNode node = nodeobj.GetComponent<GridNode>();
*
* Vector3 pos = nodeobj.transform.position;
* if (node.m_IsBlocked == true)
* {
* m_map.SetBlock(pos.x, pos.z, node.m_NodeSize, 0, 0);
* }
*
* MapData data = this.GetData((int)pos.x, (int)pos.z);
* if (data != null)
* {
*
* }
*
* }
*/
}
/// <summary>
/// 距离（不包括y轴),float version, only for compare
/// </summary>
public static float Distance3Fast(PathVector3 vec1, PathVector3 vec2)
{
return((vec1.x - vec2.x) * (vec1.x - vec2.x) + (vec1.z - vec2.z) * (vec1.z - vec2.z));
}
/// <summary>
/// 距离（不包括y轴)
/// </summary>
public static float Distance3(PathVector3 vec1, PathVector3 vec2)
{
return((float)System.Math.Sqrt((vec1.x - vec2.x) * (vec1.x - vec2.x) + (vec1.z - vec2.z) * (vec1.z - vec2.z)));
}
/// <summary>
/// 把长度为3的浮点数组转为向量
/// </summary>
/// <param name="vec">长度为3的浮点数组</param>
/// <returns>返回向量</returns>
public static Vector3 FloatToVector(PathVector3 v)
{
return(new Vector3(v.x, v.y, v.z));
}
public static PathVector3 VectorToPath(Vector3 vec)
{
PathVector3 pv = new PathVector3(vec.x, vec.y, vec.z);
return(pv);
}
