private void OnEnable()
{
if (unit == null && Static)
{
AgentBaseData data = new AgentBaseData();
data.id = -1;
data.mapId = 0;
data.playerId = 0;
data.eAgentType = EAgentType.none;
data.defaultTargetPos = CustomMath.Vector3ToTsvec(this.transform.position);
data.loopCallback = this.Loop;
data.boidsType = (byte)EBoidsActiveType.all;
data.maxSpeed = FP.One * 3;
data.position = CustomMath.Vector3ToTsvec(this.transform.position);
data.collidesWithLayer = 1;
data.viewRadius = FP.One * 6;
data.neighbourRadius = FP.One * data.maxSpeed;
data.neighbourRadiusSqr = data.neighbourRadius * data.neighbourRadius;
data.colliderRadius = 0.5f;//test
data.pfm = TestPathFinding._pfm;
unit = new PathFindingAgentBehaviour();
unit.enabled = true;
unit.Init(data);
}
if (unit != null)
{
unit.OnEnable();
}
}
//
public void AddAgent(IAgentBehaviour agent)
{
PathFindingAgentBehaviour af = agent as PathFindingAgentBehaviour;
#if !USING_DYNAMIC_TREE
List <IAgentBehaviour> list;
if (!_agents.TryGetValue(agent.campId, out list))//
{
list = new List <IAgentBehaviour>();
_agents[agent.campId] = list;
}
Quadtree <QTData> tree;
if (!_dicQdTree.TryGetValue(agent.campId, out tree))
{
tree = new Quadtree <QTData>();
_dicQdTree[agent.campId] = tree;
_camps.Add(agent.campId);
_agentUpdateNeighbourIdx[agent.campId] = 0;
}
list.Add(agent);
#else
AABB ab = af.aabb;
af.proxyId = _dynamicTree.CreateProxy(ref ab, af);
#endif
_allAgentCount++;
int pathMcount = _pathManagers.Length;
int idx = _allAgentCount % (pathMcount);
af.pathManager = _pathManagers[idx];
af.pathManager.AddAgent(af);
// _allAgents.Add(af);
}
//public AStarMachine.AStarMachine FindPath()
//{
// AStarMachine.AStarMachine p = _pathPool.New();
// return p;
//}
public void Loop(FP ft)
{
// return;
while (_pathQueue.Count > 0 && _revsCompleted < MaxRevsPerGameTick)
{
// return;//test
AStarMachine.AStarMachine thePath = _pathQueue.Peek();
if (!thePath.IsWorking)
{
_pathQueue.Dequeue();
thePath._inQueue = false;
ClearFinishedPath(thePath);
continue;
}
thePath.RunAStar();
_revsCompleted += thePath.RevsCurrentRun;
// UnityEngine.Debug.Log("_revsCompleted:"+ _revsCompleted);
// If not working,done
if (!thePath.IsWorking)
{
#if UNITY_EDITOR && PATHMANAGER_DEBUG && !MULTI_THREAD
if (PathFindingManager.DEBUG)
{
if (thePath.Agent != null && ((AgentBase)thePath.Agent)._behaviour != null)
{
PathFindingAgentBehaviour bg = ((PathFindingAgentBehaviour)(((AgentBase)thePath.Agent)._behaviour));
string name = bg.name;
if (thePath._totalRevs > 1500)
{
UnityEngine.Debug.LogError("full path fail:" + name + ",target:" + CustomMath.TsVecToVector3((bg.targetPos)).ToString());
}
// UnityEngine.Debug.Log("full path succeed:" + name + " revs:" + thePath._totalRevs);
}
}
#endif
// Post processing
thePath._inQueue = false;
_pathQueue.Dequeue();
this.ProcessFinishedPath(thePath, EPathType.fullPath, true);
}
else
{
if (thePath.MaxRevolutions < MaxRevsPerGameTick)
{
thePath.MaxRevolutions += 1;
}
// Take it off the front of the queue
// and put it on the rear of the queue.
_pathQueue.Enqueue(thePath);
thePath._inQueue = true;
// Remove
// thePath._inQueue = false;
_pathQueue.Dequeue();
}
}
}
private void OnDestroy()
{
PathFindingAgentBehaviour.ClearAgentPool();
if (s_AstarAgent != null)
{
s_AstarAgent.Clear();
}
_pfm.OnDestroy();
}
public void MoveProxy(PathFindingAgentBehaviour agent, TSVector diff)
{
AABB ab = new AABB();
ab.lowerBound = new TSVector2(agent.position.x - agent.colliderRadius, agent.position.z - agent.colliderRadius);
ab.upperBound = new TSVector2(agent.position.x + agent.colliderRadius, agent.position.z + agent.colliderRadius);
TSVector2 displace = CustomMath.TSVecToVec2(diff);
if (agent.proxyId < 0)
{
return;
}
_dynamicTree.MoveProxy(agent.proxyId, ref ab, ref displace);
}
public void CalculateAgentNeighbours(PathFindingAgentBehaviour agent, DynamicTreeQueryCallback callback, FP radius)
{
if (agent.proxyId < 0)
{
return;
}
AABB ab = new AABB();
ab.lowerBound = new TSVector2(agent.position.x - radius, agent.position.z - radius);
ab.upperBound = new TSVector2(agent.position.x + radius, agent.position.z + radius);
agent.pathManager._queryStack.Clear();
if (_dynamicTree != null)
{
_dynamicTree.Query(callback, ref ab, agent.pathManager._queryStack, false);
}
}
public void RemoveAgent(IAgentBehaviour agent)
{
PathFindingAgentBehaviour af = agent as PathFindingAgentBehaviour;
#if !USING_DYNAMIC_TREE
List <IAgentBehaviour> list;
if (_agents.TryGetValue(agent.campId, out list))
{
list.Remove(agent);
}
#else
if (agent.proxyId >= 0 && _dynamicTree != null)
{
_dynamicTree.DestroyProxy(af.proxyId);
}
af.proxyId = -1;
#endif
af.pathManager.RemoveAgent(af);
af.pathManager = null;
// _allAgents.Remove(af);
_allAgentCount--;
}
public void RemoveAgent(PathFindingAgentBehaviour agent)
{
_agentIdx.Remove(agent);
}
public void AddAgent(PathFindingAgentBehaviour agent)
{
_agentIdx.Add(agent);
}
bool DynamicTreeQueryCallback(int id)
{
PathFindingAgentBehaviour agent = unit._baseData.pfm.GetAgentByProxyId(id);
bool bOver = true;
if (agent != null)
{
if (id == unit.proxyId)
{
return(true);
}
if (_listEnemyAgents.Count < PathFindingAgentBehaviour.maxEnemyNeighbours)
{
bOver = false;
}
if (agent.playerId != unit.playerId)
{
// _listEnemyAgents.Add(agent);
FP dstSqr = (agent.position - unit._baseData.position).sqrMagnitude;
FP rDst = TSMath.Max(unit._baseData.colliderRadius + unit._baseData.viewRadius, attackRange + 1);
if (dstSqr > rDst * rDst)
{
return(true);
}
// _listEnemyAgentsDstSqr.Add(dstSqr);
List <FP> neighboursDestSqr = _listEnemyAgentsDstSqr;
List <IAgentBehaviour> nbs = _listEnemyAgents;
//sort
if (_listEnemyAgents.Count < PathFindingAgentBehaviour.maxEnemyNeighbours)
{
//neighbours.Add(agent);
//neighboursDestSqr.Add(distSqr);
nbs.Add(null);
neighboursDestSqr.Add(FP.MaxValue);
}
//// Insert the agent into the ordered list of neighbours
int i = nbs.Count - 1;
if (dstSqr < neighboursDestSqr[i])
{
while (i != 0 && dstSqr < neighboursDestSqr[i - 1])
{
nbs[i] = nbs[i - 1];
neighboursDestSqr[i] = neighboursDestSqr[i - 1];
i--;
}
nbs[i] = agent;
#if UNITY_EDITOR && !MULTI_THREAD
if (PathFindingManager.DEBUG)
{
if (i > 0)
{
if (nbs[i] == nbs[i - 1])
{
UnityEngine.Debug.LogError("DynamicTree error!");
}
}
}
#endif
neighboursDestSqr[i] = dstSqr;
}
//
}
}
if (bOver)//overflow
{
return(false);
}
return(true);
}
IEnumerator SpawnUnit()
{
if (randomSeed == 0)
{
randomSeed = (int)System.DateTime.Now.Ticks;
}
UnityEngine.Random.InitState(randomSeed);
TSRandom random = TSRandom.instance;
while (true && countLimit > 0)
{
for (int i = 0; i < 10 && countLimit > 0; i++)
{
TestAgent tagent = GameObject.Instantiate <TestAgent>(agent);
PathFindingAgentBehaviour unit = s_AstarAgent.New();
if (unit != null)
{
tagent._testPathFinding = this;
tagent.unit = unit;
int camp = countLimit % campCount;
//start pos
Vector3 vPos = startObj[camp].transform.position
+ new Vector3(UnityEngine.Random.Range(0, 2.0f), 0, UnityEngine.Random.Range(0, 2.0f));
TSVector pos = TSVector.zero;
pos.Set(CustomMath.FloatToFP(vPos.x), CustomMath.FloatToFP(vPos.y), CustomMath.FloatToFP(vPos.z));
TSVector sPos = pos;
tagent.gameObject.transform.position = CustomMath.TsVecToVector3(sPos);
// Debug.Log(pos);
//target pos
Vector3 tpos = destObj[camp].transform.position; // new Vector3(48.0f, 0.0f, 46.8f);
pos.Set(CustomMath.FloatToFP(tpos.x), CustomMath.FloatToFP(tpos.y), CustomMath.FloatToFP(tpos.z));
//get center
int idx = _map.GetGridNodeId(pos);
pos = _map.GetWorldPosition(idx);
TSVector targetPos = pos;
FP attackRange = _atkRanges[countLimit % _atkRanges.Length];
FP range = TSMath.Max(attackRange + GridMap.GetNodeSize() * CustomMath.FPHalf, FP.One * 3 * GridMap.GetNodeSize());
AgentBaseData data = new AgentBaseData();
data.id = countLimit;
data.mapId = 0;
data.playerId = camp;
data.eAgentType = EAgentType.none; //data.id%5== 0? EAgentType.ingoreObstacle:
#if !USING_FLOW_FIELD
data.defaultTargetPos = TSVector.zero; //astar
#else
data.defaultTargetPos = targetPos;
#endif
data.loopCallback = tagent.Loop;
data.boidsType = (byte)EBoidsActiveType.all;
data.maxSpeed = FP.One * maxSpeed;
data.position = sPos;
data.collidesWithLayer = 1;
data.viewRadius = FP.One * 6 * GridMap.GetNodeSize();
data.neighbourRadius = range;
data.random = random;
data.colliderRadius = 0.5f; //test
data.pfm = _pfm;
data.groupId = (byte)(data.eAgentType == EAgentType.ingoreObstacle ? 1 : 0);
data.targetFailCallback = tagent.FailFindPathCallback;
unit.enabled = false;
unit.Init(data);
EAgentType agentType = EAgentType.astar;
#if USING_FLOW_FIELD
agentType = EAgentType.flowFiled;
#endif
unit.ChangeAgentType(data.eAgentType == EAgentType.ingoreObstacle? data.eAgentType
: agentType);
unit.agent.TargetPos = targetPos;
// unit.OnEnable();//?????????
tagent.attackRange = attackRange; // FP.One * UnityEngine.Random.Range(0.8f, 5);
tagent._attackRange = attackRange.AsFloat();
// unit.AgentType = EAgentType.flowFiled;
#if !USING_FLOW_FIELD
unit.ChangeAgentType(EAgentType.astar); //astar
#endif
if (groupCount > 0)
{
AgentGroupManager.instance.UpdateGroup(unit, countLimit % groupCount);
}
tagent.transform.GetChild(0).GetComponent <SpriteRenderer>().color = _campColor[camp];
// unit.agent.StartPos = unit.position;
// unit._agent.TargetPos = (TSVector)destination;
tagent.gameObject.name = "unit" + countLimit;
tagent.transform.SetParent(Units);
// unit.agent.TargetPos = targetPos;
// unit.agent._activeBoids = (byte)EBoidsActiveType.all;
_listAgents.Add(tagent);
// PathFindingManager.Instance.AddAgent(this);
//_pm.FindFastPath(unit._agent, unit._agent.StartPos, unit._agent.TargetPos);//, unit._agent._vecPath
// break;
if (unit.group != null && (unit.group.leader as PathFindingAgentBehaviour) == unit && unit.AgentType == EAgentType.astar)
{
_pm.FindQuickPath(unit.agent, 10, unit.agent.StartPos, unit.agent.TargetPos, unit.map, false);
}
//if(unit.group!=null)
//{
// unit.agent._activeBoids = (byte)EBoidsActiveType.alignment& (byte)EBoidsActiveType.cohesion & (byte)EBoidsActiveType.terrainSeperation
//}
countLimit--;
if (countLimit % 5 == 0)
{
yield return(_wait);
}
}
}
}
yield return(null);
}
/// <summary>
///
/// </summary>
/// <param name="forceToApply">basic force</param>
/// <param name="basicVelocity">basic Velocity</param>
/// <returns></returns>
protected TSVector ApplyForce(FP deltaTime, TSVector desiredDirection, TSVector basicVelocity, TSVector pos, List <IAgentBehaviour> agents, bool bUsePreForce, bool bSkipStatic)//
{
// isTminStaticAgent = false;
int count = agents.Count;
TSVector boidsVelocity = TSVector.zero;
//TSVector forceToApply = TSVector.zero;
TSVector forceToApply = TSVector.zero;
TSVector avoidObstacle = TSVector.zero;
FP mAvoidObstacle = FP.Zero;
if (IsBoisTypeActive(EBoidsActiveType.terrainSeperation) && false)
{
avoidObstacle = Boids.BoidsBehaviourAvoidObstacle(pos, _behaviour.map, basicVelocity);
//desiredDirection = (desiredDirection + dir * S_terrainSepFactor);
//if(desiredDirection!=TSVector.zero)
//{
// desiredDirection = desiredDirection.normalized;
//}
avoidObstacle = CustomMath.Normalize(avoidObstacle, out mAvoidObstacle);
}
if (desiredDirection != TSVector.zero)
{
desiredDirection = CustomMath.Normalize(desiredDirection);
forceToApply += Boids.SteerTowards(_behaviour, desiredDirection, basicVelocity);
}
if (mAvoidObstacle > FP.Zero)
{
forceToApply += Boids.SteerTowards(_behaviour, avoidObstacle, basicVelocity);
}
if (PathFindingManager.c_useAvoidUnit)
{
if (IsBoisTypeActive(EBoidsActiveType.avoidUnit))
{
TSVector avoidVector = SteerForUnitAvoidance.GetDesiredSteering(_behaviour);
TSVector force = TSVector.ClampMagnitude(avoidVector / deltaTime * _behaviour.baseData.invMass, _behaviour.baseData.maxForce);
forceToApply += force;
}
//else if (IsBoisTypeActive(EBoidsActiveType.collisionAvoidance))
//{
// // bool isTminStaticAgent = false;
// forceToApply += CustomMath.TSVec2ToVec(ForceBasedAgent.computeForces(_behaviour, basicVelocity,out isTminStaticAgent));
//}
}
if (_activeBoids > 0 && !bUsePreForce &&
(IsBoisTypeActive(EBoidsActiveType.seperation) || IsBoisTypeActive(EBoidsActiveType.cohesion) ||
IsBoisTypeActive(EBoidsActiveType.alignment)))
{
// int count =(i%3== _updateStart)? this.neighbours.Count:0;//_group._thiss
TSVector totalForce = TSVector.zero;
TSVector averageHeading = TSVector.zero;
TSVector centerOfMass = TSVector.zero;
int neighboursCountSep = 0;
int neighboursCountAlig = 0;
int neighboursCountCoh = 0;
FP radius = _behaviour.colliderRadius * 4;
FP sepSqr = radius * radius;//
FP nrSqr = _behaviour.baseData.neighbourRadiusSqr * FP.EN2 * 25;
for (int j = 0; j < count; j++)
{
IAgentBehaviour a = agents[j];// _behaviour.neighbours
if (IsBoisTypeActive(EBoidsActiveType.seperation))
{
Boids.BoidsBehaviourSeparation(_behaviour, a, sepSqr, ref totalForce, ref neighboursCountSep, bSkipStatic);
}
if (IsBoisTypeActive(EBoidsActiveType.alignment))
{
Boids.BoidsBehaviourAlignment(_behaviour, a, nrSqr, ref averageHeading, ref neighboursCountAlig);
}
if (IsBoisTypeActive(EBoidsActiveType.cohesion))
{
Boids.BoidsBehaviourCohesion(_behaviour, a, nrSqr, ref centerOfMass, ref neighboursCountCoh);
}
if (a.enabled && a.agent == null) //the ally neighbour has a target,
//cause this agent change type to atar type targeted to the same target
{
PathFindingAgentBehaviour agent = _behaviour as PathFindingAgentBehaviour;
if (agent.AgentType == EAgentType.flowFiled)
{
TSVector target = (a as PathFindingAgentBehaviour).targetPos;
bool isDefaultTaget = target == a.baseData.defaultTargetPos;
if (target != TSVector.MaxValue && target != TSVector.MinValue && !isDefaultTaget)
{
agent.stopMoving = false;
agent._hasNeighbourTargetedDefaultPos = true;
target.y = 0;
agent.SetNewTargetPos(target, true);
}
if (!isDefaultTaget)
{
agent.preVelocity = TSVector.zero;
forceToApply = TSVector.zero;
// agent.velocity = TSVector.zero;
// agent.ChangeAgentType(EAgentType.astar);
return(forceToApply);
}
}
}
}
if (count > 0)
{
TSVector sep = TSVector.zero;
if (totalForce != TSVector.zero)
{
//totalForce.Multiply(agent.maxForce / neighboursCount)
sep = totalForce * (_behaviour.baseData.maxForce) / neighboursCountSep;
//sep=Boids.SteerTowards(_behaviour, sep, basicVelocity);
//
FP lenSqr = sep.sqrMagnitude;
if (lenSqr > _behaviour.baseData.maxForceSqr)
{
FP fval = _behaviour.baseData.maxForce / TSMath.Sqrt(lenSqr);
sep = sep * fval;
}
}
TSVector avh = TSVector.zero;
if (averageHeading != TSVector.zero) //average heading
{
averageHeading = averageHeading / (neighboursCountAlig);
avh = Boids.SteerTowards(_behaviour, CustomMath.Normalize(averageHeading), basicVelocity);
}
//average position
TSVector coh = TSVector.zero;
if (centerOfMass != TSVector.zero)// seek that position
{
centerOfMass = centerOfMass + _behaviour.position;
neighboursCountCoh++;
centerOfMass = centerOfMass / neighboursCountCoh;
coh = Boids.BoidsBehaviourSeek(_behaviour, basicVelocity, centerOfMass);
}
_preBoidsForce = sep * S_seperationFactor + avh * S_alignmentFactor + coh * S_cohesionFactor;
forceToApply += _preBoidsForce;
if (PathFindingManager.DEBUG)
{
#if UNITY_5_5_OR_NEWER && !MULTI_THREAD
if (FP.Abs(forceToApply.x) > GridMap.SCALE * 1000 || FP.Abs(forceToApply.z) > GridMap.SCALE * 1000)
{
UnityEngine.Debug.LogError("forceToApply error!");
}
#endif
}
}
}
else if (bUsePreForce)
{
forceToApply += _preBoidsForce;
}
if (forceToApply != TSVector.zero)
{
if (TSMath.Abs(forceToApply.x) > _behaviour.baseData.maxForce ||
TSMath.Abs(forceToApply.z) > _behaviour.baseData.maxForce)
//FP lengthSquared = forceToApply.sqrMagnitude;
//if (lengthSquared > _behaviour.baseData.maxForceSqr)//&& _activeBoids!=(byte)EBoidsActiveType.seperation)
{
forceToApply = forceToApply * FP.EN3;
forceToApply = _behaviour.baseData.maxForce * forceToApply.normalized;
}
return(forceToApply * _behaviour.baseData.invMass);//
}
return(forceToApply);
}
