public override AStarNode GetAStarNode(int id, int pathId, IAStarMap map, bool isIgnored = false)
{
GridMap gMap = map as GridMap;
IInt2 pos = gMap.GetGridPos(id);
return(GetAStarNode(pos.x, pos.y, pathId, gMap, isIgnored));
}
internal static FP CalculateHValue(IInt2 gridPos1, IInt2 gridPos2, Heuristic heuristic)
{
FP h = FP.Zero;
switch (heuristic)
{
case Heuristic.Euclidean:
h = TSMath.Sqrt((gridPos1 - gridPos2).sqrMagnitudeLong) * HeuristicFactor;
break;
case Heuristic.Manhattan:
h = (Math.Abs(gridPos2.x - gridPos1.x) + Math.Abs(gridPos2.y - gridPos1.y)) * HeuristicFactor;
// return h;
break;
case Heuristic.DiagonalManhattan:
IInt2 p = gridPos2 - gridPos1;
p.x = Math.Abs(p.x);
p.y = Math.Abs(p.y);
int diag = Math.Min(p.x, p.y);
int diag2 = Math.Max(p.x, p.y);
h = ((CustomMath.DiagonalCost * diag + (diag2 - diag))) * HeuristicFactor;
break;
}
return(h * GridMap.GetNodeSize());
}
public bool GetGridCoord(TSVector position, ref IInt2 coord)
{
TSVector vec = GetGridFloatCoord(position);
int ix = TSMath.Floor(vec.x).AsInt();
int iz = TSMath.Floor(vec.z).AsInt();
int x = CustomMath.Clamp(ix, 0, _mapSizeX - 1);
int z = CustomMath.Clamp(iz, 0, _mapSizeZ - 1);
//TSVector2 coord;
coord.x = x;
coord.y = z;
return(x == ix && z == iz);
}
public bool IsWalkableSkipDynamic(TSVector pos)
{
int idx = flowField._gridMap.GetGridNodeId(pos);
if (flowField._gridMap.IsDynamicUnwalkableNode(idx))
{
TSVector centerPos = flowField._gridMap.GetWorldPosition(idx);
FP dot = TSVector.Dot(pos - _behaviour.position, centerPos - _behaviour.position);
return(dot <= 0);// CustomMath.FPHalf;//>=60 degree
}
IInt2 iPos = flowField._gridMap.GetNearestGridCoordWithoutClamp(pos);
return(flowField._gridMap.IsWalkable(iPos.x, iPos.y, true));
}
internal static TSVector BoidsBehaviourAvoidObstacle(TSVector agentPos, GridMap map, TSVector towardDir)
{
IInt2 floor = IInt2.zero;// GetNearestGridCoordWithoutClamp(agent.position);
map.GetGridCoord(agentPos, ref floor);
TSVector aPos = agentPos;// map.GetWorldPosition(map.GetIdx(floor.x,floor.y));
TSVector desiredDirection = TSVector.zero;
int iCount = 0;
towardDir = CustomMath.Normalize(towardDir);
for (int i = floor.x - 1; i <= floor.x + 1; i++)
{
for (int j = floor.y - 1; j <= floor.y + 1; j++)
{
if (!map.IsWalkable(i, j, true))
{
// int idx = map.GetIdx(i,j);
TSVector pos = map.GetWorldPositionWithOutClamp(i, j);
TSVector dir = aPos;// agentPos;
dir.Set(aPos.x - pos.x, 0, aPos.z - pos.z);
//FP dstSqr = dir.sqrMagnitude;
FP dot = 1;// TSVector.Dot(dir,towardDir);
//if(dot<=FP.Zero)
//{
// dot = FP.One/(9*dstSqr);
//}
//else
//{
// dot = dot / dstSqr;
//}
desiredDirection = desiredDirection + dir * dot;
iCount++;
}
}
}
//if (iCount > 0)
//{
// desiredDirection = desiredDirection / iCount;
//}
return(desiredDirection);//.normalized*C_OBSTACLE_REPELL_FORCE;
}
public override bool Equals(System.Object o) //hashcode is valid
{
if (PathFinding.PathFindingManager.DEBUG)
{
if (!(o is IInt2))
{
return(false);
}
IInt2 rhs = (IInt2)o;
bool bEqual = x == rhs.x && y == rhs.y;
#if UNITY_EDITOR && !MULTI_THREAD
if (!bEqual)
{
UnityEngine.Debug.LogError("IInt2 unequal ");
}
#endif
return(bEqual);
}
return(true);
}
public override AStarNode GetAStarNeighbour(AStarNode node, int index, int pathId, IAStarMap map, IInt2 curPos)
{
GridNode gNode = (GridNode)node;
if (gNode != null && index >= 0 && index < GridMap.neighbourOffset.Length)
{
int x = gNode.gridPos.x + GridMap.neighbourOffset[index].x;
int z = gNode.gridPos.y + GridMap.neighbourOffset[index].y;
bool bSkip = false;
#if !DYNAMIC_FORCE
IInt2 pos = IInt2.zero;
pos.x = x;
pos.y = z;
IInt2 diff = curPos - pos;
int posIdx = map.GetIdx(pos.x, pos.y);
int curposIdx = map.GetIdx(curPos.x, curPos.y);
EDynamicBlockStatus status = map.DynamicBlockedStatus(posIdx, -1, -1, curposIdx);
bSkip = (status == EDynamicBlockStatus.blocked && diff.sqrMagnitudeInt > GridMap.c_dynamicRangeSqr) ||
status == EDynamicBlockStatus.ignore;
#endif
return(GetAStarNode(x, z, pathId, map, bSkip));
}
return(null);
}
public bool Equals(IInt2 rhs)
{
return(x == rhs.x && y == rhs.y);
}
/// <summary>
/// RunAStar
/// </summary>
public void RunAStar()
{
_revsCurrentRun = 0;
PathFinding.AStarAgent agent = _agent as PathFinding.AStarAgent;
_curPosIdx = -1;
IInt2 curPos = IInt2.zero;
if (agent != null)
{
curPos.x = -1;
curPos.y = -1;
_curPosIdx = agent._map.GetGridNodeId(agent._behaviour.position);
curPos = agent._map.GetGridPos(_curPosIdx);
}
while (_goal != null)
{
//Get the node with the min f from Storage
_goal.CurrentNode = _storage.RemoveCheapestOpenNode(_map);
#if UNITY_EDITOR && !MULTI_THREAD
if (PathFinding.PathFindingManager.DEBUG)
{
if (_goal.CurrentNode != null)
{
UnityEngine.Vector3 pos = CustomMath.TsVecToVector3(((PathFinding.GridMap)_map).GetWorldPosition(_goal.CurrentNode.NodeID));
UnityEngine.Debug.DrawLine(pos - UnityEngine.Vector3.right * 0.2f, pos + UnityEngine.Vector3.right * 0.2f, UnityEngine.Color.red, 1);
}
}
#endif
if (_totalRevs > MAX_REVS)//need find another target
{
_goal.CurrentNode = null;
}
//Add the node to the closed list
if (_goal.CurrentNode != null && _map.CheckNodeValid(_goal.CurrentNode))
{
_storage.AddToClosedSet(_goal.CurrentNode, _map);
}
else
{
IsWorking = false;
break;
}
//_currentNode = newNode;
if (_goal.IsAStarFinished(_goal.CurrentNode))
{
IsWorking = false;
break;
}
int numberOfNeighbours = _map.GetNeighboursCount(_goal.CurrentNode);
//scan its neighbours
for (short i = 0; i < numberOfNeighbours; i++)
{
AStarNode neighbour = _storage.GetAStarNeighbour(_goal.CurrentNode, i, _searchId, _map, curPos);
if (neighbour != null)
{
LinkChild(_goal.CurrentNode, neighbour, i);
}
}
#if ASTARDEBUG
_totalRevs++;
#endif
if (_maxRevolutions >= 0 && _revsCurrentRun++ >= _maxRevolutions)
{
break;
}
if (ShouldPause(_revsCurrentRun))
{
break;
}
}
}
/// <summary>
/// three step:first step,find fast path,maybe not a valid path navigating to target position
/// </summary>
/// <param name="agent"></param>
/// <param name="start"></param>
/// <param name="target"></param>
/// <param name="mapId"></param>
/// <param name="isTried">is tried to find quick path and finished </param>
/// <returns></returns>
public EPathReturnStatus FindQuickPath(AgentBase agent, int revs, TSVector start, TSVector target, GridMap map, bool isTried)//, List<TSVector> vecPath
{
//GridMap map = GridMapManager.instance.GetMap(mapId);
if (map.GetGridNodeId(start) == map.GetGridNodeId(target))
{
return(EPathReturnStatus.ok);
}
AStarAgent aAgent = ((AStarAgent)agent);
AStarMachine.AStarMachine asm = isTried?null: aAgent._asm;
bool bInqueue = asm != null && asm.IsWorking;
if (asm == null)
{
asm = _pathPool.New();
GridAStarGoal goal = _goalPool.New();
goal.Ini(agent);
GridAStarStorage storage = _storagePool.New();
storage.Ini();
asm.Ini(agent, goal, storage, map);
}
int curIdx = map.GetGridNodeId(aAgent._behaviour.position);
StartFindingPath(asm, start, target, true, curIdx);
if (asm.IsWorking)
{
int startIdx = map.GetGridNodeId(start);
int tIdx = map.GetGridNodeId(target);
IInt2 pos = map.GetGridPos(tIdx);
bool isReachable = map.IsExistingNeighbourWalkable(pos.x, pos.y, startIdx, tIdx, curIdx);
if (!isTried && isReachable)
{
ProcessFinishedPath(asm, EPathType.quickPath, false);//quick path is valid
}
if (!isReachable)
{
aAgent._behaviour.AddUnReachablePos(map, tIdx);
}
if (!isTried && isReachable)//target possible reachable
{
int sIdx = map.GetGridNodeId(aAgent._vecRunningPath[0]);
asm.StartSearch(sIdx, tIdx, false, start, _pathId++, startIdx);//
//if (!bInqueue)
//{
// _pathQueue.Enqueue(asm);//find full path
//}
aAgent._asm = asm;
}
else//can't reach target
{
asm.IsWorking = false;
// asm.Reset();
ClearFinishedPath(asm);
if (!isTried)
{
return(EPathReturnStatus.fail);
#if UNITY_EDITOR && !MULTI_THREAD
if (PathFindingManager.DEBUG)
{
UnityEngine.Debug.LogWarning("can't reach target:" + pos.x + "," + pos.y);
}
#endif
}
else
{
return(EPathReturnStatus.fail);
}
}
// asm._start=
}
else
{
if (isTried)//remove working asm
{
if (aAgent._asm != null)
{
aAgent._asm.IsWorking = false;
ClearFinishedPath(aAgent._asm);
}
aAgent._asm = asm;
}
ProcessFinishedPath(asm, EPathType.quickPath);//quick path is valid
}
if (aAgent._vecRunningPath.Count > 0)
{
return(EPathReturnStatus.ok);
}
return(EPathReturnStatus.fail);//warning
}
public bool IsWalkable(TSVector pos)
{
IInt2 iPos = _behaviour.map.GetNearestGridCoordWithoutClamp(pos);
return(_behaviour.map.IsInGridArea(iPos.x, iPos.y));
}
public virtual AStarNode GetAStarNeighbour(AStarNode pAStarNode, int iNeighbor, int searchId, IAStarMap map, IInt2 curPos)
{
return(null);
}
internal TSVector steeringBehaviourFlowField(IAgentBehaviour agent)
{
//bilinear interpolation
TSVector vec = _gridMap.GetGridFloatCoord(agent.position);
IInt2 floor = IInt2.zero;// GetNearestGridCoordWithoutClamp(agent.position);
floor.x = TSMath.Floor(vec.x).AsInt();
floor.y = TSMath.Floor(vec.z).AsInt();
bool bIn1 = IsValid(floor.x, floor.y);
TSVector f00 = (bIn1) ? GetFlowField(_gridMap.ClampX(floor.x), _gridMap.ClampZ(floor.y)) : TSVector.zero;//(posOrigin - pos1) ;// TSVector.zero
int offsetX = f00.x > 0 ? 1 : -1;
int offsetY = f00.z > 0 ? 1 : -1;
bool bIn2 = IsValid(floor.x, floor.y + offsetY);
bool bIn3 = IsValid(floor.x + offsetX, floor.y);
bool bIn4 = IsValid(floor.x + offsetX, floor.y + offsetY);
// bool bAllIn = bIn1 && bIn2 && bIn3 && bIn4;//bAllIn ||!
//
//TSVector posOrigin = (TSVector)vec;
//TSVector pos1 = TSVector.zero;
//pos1.Set((float)floor.x, 0, floor.y);
//TSVector pos2 = TSVector.zero;
//pos2.Set((float)floor.x, 0, floor.y + 1);
//TSVector pos3 = TSVector.zero;
//pos3.Set((float)floor.x + 1, 0, floor.y);
//TSVector pos4 = TSVector.zero;
//pos3.Set((float)floor.x + 1, 0, floor.y + 1);
//TSVector f00 = (bAllIn) ? _flowField[ClampX(floor.x), ClampZ(floor.y)] : (bIn1?TSVector.zero: ((bIn2&& bIn3&& bIn4) ? TSVector._northEst:(bIn2?TSVector._forward:(bIn3?TSVector._right:TSVector.zero))));
//TSVector f01 = (bAllIn) ? _flowField[ClampX(floor.x), ClampZ(floor.y + 1)] : (bIn2 ? TSVector.zero : ((bIn1 && bIn3 && bIn4) ? TSVector._southEst : (bIn1 ? -TSVector._forward : (bIn4 ? TSVector._right : TSVector.zero))));
//TSVector f10 = (bAllIn) ? _flowField[ClampX(floor.x + 1), ClampZ(floor.y)] : (bIn3 ? TSVector.zero : ((bIn2 && bIn1 && bIn4) ? TSVector._northWest : (bIn4 ? TSVector._forward : (bIn1 ? -TSVector._right : TSVector.zero))));
//TSVector f11 = (bAllIn) ? _flowField[ClampX(floor.x + 1), ClampZ(floor.y + 1)] : (bIn4 ? TSVector.zero : ((bIn2 && bIn3 && bIn1) ? TSVector._southWest : (bIn3 ? -TSVector._forward : (bIn2 ? -TSVector._right : TSVector.zero))));
TSVector f01 = (bIn2) ? GetFlowField(_gridMap.ClampX(floor.x), _gridMap.ClampZ(floor.y + offsetY)) : TSVector.zero; //(posOrigin - pos2);
TSVector f10 = (bIn3) ? GetFlowField(_gridMap.ClampX(floor.x + offsetX), _gridMap.ClampZ(floor.y)) : TSVector.zero; //(posOrigin - pos3);
TSVector f11 = (bIn4) ? GetFlowField(_gridMap.ClampX(floor.x + offsetX), _gridMap.ClampZ(floor.y + offsetY)) : TSVector.zero; // (posOrigin - pos4);
//Do the x interpolations
FP fx = agent.position.x;
FP fy = agent.position.z;
FP w = FP.One * _gridWidth * GridMap.GetNodeSize();
FP h = FP.One * _gridHeight * GridMap.GetNodeSize();
FP dstX0 = TSMath.Abs(vec.x - (floor.x + CustomMath.FPHalf));
FP dstX1 = TSMath.Abs(vec.x - (floor.x + offsetX + CustomMath.FPHalf));
FP dstY0 = TSMath.Abs(vec.z - (floor.y + CustomMath.FPHalf));
FP dstY1 = TSMath.Abs(vec.z - (floor.y + offsetY + CustomMath.FPHalf));
FP xW = (dstX0) / (dstX0 + dstX1);
FP xWeight = (fx < w && fx >= FP.Zero) ? (xW) : FP.Zero;//vec.x - floor.x
TSVector top = f00 * (1 - xWeight) + f10 * (xWeight);
TSVector bottom = f01 * (1 - xWeight) + f11 * xWeight;
FP yW = (dstY0) / (dstY0 + dstY1);
//Do the y interpolation
FP yWeight = (fy < h && fy >= FP.Zero) ? (yW) : FP.Zero;//vec.z - floor.y
//
TSVector desiredDirection = (top * (1 - yWeight) + bottom * yWeight);
//}
// desiredDirection = desiredDirection + Boids.BoidsBehaviourTerrainSeparation(vec,IsValid);
//desiredDirection = desiredDirection.normalized;
// return Boids.SteerTowards(agent, desiredDirection);
return(desiredDirection);
}
public static IVec3 ToIVec3XZ(IInt2 o)
{
return(new IVec3(o.x, 0, o.y));
}
public static IInt2 Max(IInt2 a, IInt2 b)
{
return(new IInt2(System.Math.Max(a.x, b.x), System.Math.Max(a.y, b.y)));
}
public static IInt2 Rotate(IInt2 v, int r)
{
r = r % 4;
return(new IInt2(v.x * Rotations[r * 4 + 0] + v.y * Rotations[r * 4 + 1], v.x * Rotations[r * 4 + 2] + v.y * Rotations[r * 4 + 3]));
}
/** Dot product of the two coordinates */
public static long DotLong(IInt2 a, IInt2 b)
{
return((long)a.x * (long)b.x + (long)a.y * (long)b.y);
}
public bool IsTileUnWalkable(IInt2 iPos)
{
int gidx = iPos.y * _mapSizeX + iPos.x;
return(_hsColliderNodes.ContainsKey(gidx));
}
public void CalculateColliders(Transform tr)
{
PathFinding.GridMap.SetNodeSize(1, 1);
//_nodeSize = PathFinding.GridMap.nodeSize;
float size = PathFinding.GridMap.GetNodeSize().AsFloat();
int childCount = tr.childCount;
HashSet <int> dicNodes = new HashSet <int>();
_listColliderNodes.Clear();
for (int i = 0; i < childCount; i++)
{
if (!tr.GetChild(i).gameObject.activeSelf)
{
continue;
}
SphereCollider sc = tr.GetChild(i).GetComponent <SphereCollider>();
BoxCollider bc = tr.GetChild(i).GetComponent <BoxCollider>();
Vector3 minPos = Vector3.zero;
Vector3 maxPos = Vector3.zero;
IInt2 iPos = IInt2.zero;
TSVector vec = TSVector.zero;
int gidx = 0;
if (sc)
{
Vector3 center = sc.center;//sc.transform.position+
minPos = center - (sc.radius) * sc.transform.localScale;
maxPos = center + sc.radius * sc.transform.localScale;
for (float x = minPos.x; x < maxPos.x + size - CustomMath.EPSILON; x = x + size)
{
if (x > maxPos.x)
{
x = maxPos.x;
}
for (float z = minPos.z; z < maxPos.z + size - CustomMath.EPSILON; z = z + size)
{
if (z > maxPos.z)
{
z = maxPos.z;
}
iPos = IInt2.zero;
vec = new TSVector(x, 0, z);
GetGridCoord(vec, ref iPos);
if ((CustomMath.TsVecToVector3(vec) - center).magnitude <= sc.radius)
{
gidx = iPos.y * _mapSizeX + iPos.x;
if (!dicNodes.Contains(gidx))
{
_listColliderNodes.Add(gidx);
dicNodes.Add(gidx);
}
}
}
}
vec = new TSVector(maxPos.x, 0, maxPos.z);
GetGridCoord(vec, ref iPos);
if ((CustomMath.TsVecToVector3(vec) - center).magnitude <= sc.radius)
{
gidx = iPos.y * _mapSizeX + iPos.x;
if (!dicNodes.Contains(gidx))
{
_listColliderNodes.Add(gidx);
dicNodes.Add(gidx);
}
}
}
else if (bc != null)
{
iPos = IInt2.zero;
vec = TSVector.zero;
minPos = bc.bounds.min; // * bc.transform.localScale.x;//bc.transform.position +
maxPos = bc.bounds.max; // * bc.transform.localScale.z;// bc.transform.position +
for (float x = minPos.x; x < maxPos.x + size - CustomMath.EPSILON; x = x + size)
{
if (x > maxPos.x)
{
x = maxPos.x;
}
for (float z = minPos.z; z < maxPos.z + size - CustomMath.EPSILON; z = z + size)
{
if (z > maxPos.z)
{
z = maxPos.z;
}
iPos = IInt2.zero;
vec = new TSVector(x, 0, z);
GetGridCoord(vec, ref iPos);
gidx = iPos.y * _mapSizeX + iPos.x;
if (!dicNodes.Contains(gidx))
{
dicNodes.Add(gidx);
_listColliderNodes.Add(gidx);
}
}
}
}
}
}
