public void CalculateAgentNeighbours(IAgentBehaviour agent)
{
foreach (KeyValuePair <int, Quadtree <QTData> > qdTree in _dicQdTree)
{
agent.CalculateNeighbours(qdTree.Key, qdTree.Value);
}
}
// private TSVector _selfCollisionPos;
// private TSVector _lastAvoidVector;
// private TSVector _lastAvoidPos;
/// <summary>
/// Gets the desired steering delta velocity
/// </summary>
/// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param>
/// <param name="output">The steering output to be populated.</param>
public static TSVector GetDesiredSteering(IAgentBehaviour agent)//FP deltaTime,
{
// _selfCollisionPos = TSVector.zero;
// _lastAvoidVector = TSVector.zero;
// _lastAvoidPos = TSVector.zero;
var otherUnits = agent.neighbours;
int othersCount = otherUnits.Count;
if (othersCount == 0)
{
// if the scanner has found no units to avoid, exit
return(TSVector.zero);
}
TSVector avoidVector = Avoid(otherUnits, agent, othersCount, agent.velocity);
if (avoidVector.sqrMagnitude < (minimumAvoidVectorMagnitude * minimumAvoidVectorMagnitude))
{
// if the computed avoid vector's magnitude is less than the minimumAvoidVectorMagnitude, then discard it
return(TSVector.zero);
}
// apply the avoidance force as a full deceleration capped force (not over time)
// _lastAvoidVector = steeringVector;
return(avoidVector);
}
//
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 void AddAgent(IAgentBehaviour agent)
{
if (_agents.Count == 0)
{
leader = agent;
}
_agents.Add(agent);
agent.group = this;
}
public virtual void Reset()
{
_moveFailedCount = 0;
_startPos = TSVector.zero;
_targetPos = TSVector.MinValue;
_behaviour = null;
_startPosId = -1;
_preBoidsForce = TSVector.zero;
}
public void Init(int eType, IAgentBehaviour agent)
{
_activeBoids = (int)eType - (int)EBoidsActiveType.collisionAvoidance - (int)EBoidsActiveType.avoidUnit;
this._behaviour = agent;
this.Init(agent);
if (_isPosWalkable == null)
{
_isPosWalkable = IsWalkable;
}
}
//navigate agent toward desiredDirection
internal static TSVector SteerTowards(IAgentBehaviour agent, TSVector desiredDirection, TSVector basicVelocity)
{
//desired speed
TSVector desiredVelocity = desiredDirection * (agent.maxSpeed);
//The velocity change
TSVector velocityChange = desiredVelocity - basicVelocity;//agent.velocity
//force
return(velocityChange * agent.baseData.maxForce * agent.invMaxSpeed);
}
internal void RemoveAgent(IAgentBehaviour agent, TSVector dest, GridMap map)
{
FlowField ff;
int idx = map.GetGridNodeId(dest);
if (_dicFlowFields.TryGetValue(idx, out ff))
{
ff.RemoveAgent(agent);
}
}
/// <summary>
/// Avoids the specified units.
/// </summary>
/// <param name="units">The units list.</param>
/// <param name="unitsLength">Length of the units list.</param>
/// <param name="currentVelocity">This unit's current velocity.</param>
/// <returns>An avoid vector, if there are any to avoid, otherwise TSVector.zero.</returns>
private static TSVector Avoid(List <IAgentBehaviour> units, IAgentBehaviour agent, int unitsLength, TSVector currentVelocity)
{
TSVector normalVelocity = CustomMath.Normalize(currentVelocity);
TSVector selfPos = agent.position + normalVelocity;
TSVector combinedAvoidVector = TSVector.zero;
// iterate through scanned units list
for (int i = 0; i < unitsLength; i++)
{
var other = units[i];
if (!other.enabled)
{
continue;
}
//if (_unitData.transientGroup != null && object.ReferenceEquals(other.transientGroup, _unitData.transientGroup))
//{
// // ignore units in same transient unit group
// // continue;
//}
//if (other.determination < _unitData.determination)
//{
// // ignore units with lower determination
// continue;
//}
TSVector otherPos = other.position;
TSVector direction = otherPos - selfPos;
FP distance = direction.magnitude;
FP omniAwareRadius = agent.colliderRadius;
if (distance > omniAwareRadius && TSVector.Dot(normalVelocity, direction / distance) > _fovReverseAngleCos)
{
// the other unit is behind me and outside my 'omni aware radius', ignore it
continue;
}
FP combinedRadius = other.colliderRadius + other.colliderRadius + radiusMargin * GridMap.GetNodeSize();
TSVector otherVelocity = other.velocity;
TSVector avoidVector = GetAvoidVector(selfPos, currentVelocity, normalVelocity, agent, otherPos, otherVelocity, other, combinedRadius);
if (accumulateAvoidVectors)
{
// if accumulating, then keep summing avoid vectors up
combinedAvoidVector += avoidVector;
}
else
{
// if not accumulating, then break after the first avoid vector is found
combinedAvoidVector = avoidVector;
break;
}
}
return(combinedAvoidVector);
}
internal void AddAgent(IAgentBehaviour agent, TSVector dest, GridMap map)
{
FlowField ff;
int idx = map.GetGridNodeId(dest);
if (!_dicFlowFields.TryGetValue(idx, out ff))
{
ff = new FlowField(dest, map);
_dicFlowFields[idx] = ff;
}
ff.AddAgent(agent);
}
/// <summary>
///
/// </summary>
/// <param name="forceToApply">basic force</param>
/// <param name="basicVelocity">basic Velocity</param>
/// <returns></returns>
protected TSVector ApplySeperateForce(TSVector toAcc, List <IAgentBehaviour> agents, bool bSkipStatic)//,out bool isTminStaticAgent
{
int count = agents.Count;
TSVector boidsVelocity = TSVector.zero;
TSVector forceToApply = TSVector.zero;
{
TSVector totalForce = TSVector.zero;
int neighboursCountSep = 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
Boids.BoidsBehaviourSeparation(_behaviour, a, sepSqr, ref totalForce, ref neighboursCountSep, bSkipStatic);
}
if (count > 0)
{
TSVector sep = totalForce * (_behaviour.baseData.maxForce) / count;
FP lenSqr = sep.sqrMagnitude;
if (lenSqr > _behaviour.baseData.maxForceSqr)
{
FP fval = _behaviour.baseData.maxForce / TSMath.Sqrt(lenSqr);
sep = sep * fval;
}
forceToApply = sep;
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
}
}
}
if (forceToApply != TSVector.zero)
{
FP max = TSMath.Max(TSMath.Abs(forceToApply.x), TSMath.Abs(forceToApply.z));
if (max > _behaviour.baseData.maxForce * FP.EN1 * 7)
{
forceToApply = forceToApply / max;
forceToApply = _behaviour.baseData.maxForce * forceToApply.normalized * FP.EN1 * 6;
}
return((forceToApply + toAcc) * _behaviour.baseData.invMass);//
}
return(forceToApply + toAcc);
}
//Cohesion navigation
internal static void BoidsBehaviourCohesion(IAgentBehaviour agent, IAgentBehaviour a, FP nrSqr, ref TSVector centerOfMass, ref int neighboursCount)
{
if (a != agent && a.agent != null && a.enabled)
{
FP distanceSqr = (agent.position - a.position).sqrMagnitude;
if (distanceSqr < nrSqr)
{
//sum up the position of our neighbours
centerOfMass = centerOfMass + a.position;
neighboursCount++;
}
}
}
public void RemoveAgent(IAgentBehaviour agent)
{
AgentGroup aGroup = agent.group;
if (aGroup != null)
{
aGroup.RemoveAgent(agent);
if (aGroup._agents.Count == 0)//store group
{
s_groupPool.Store(aGroup);
_group.Remove(aGroup.groupId);
}
}
}
public void Loop(int frameTime)//ms
{
int iCount = _allAgents.Count;
// int fDtSecond = PathFindingManager.instance.DeltaTime;// * 0.001f;
_updateIdx = (_updateIdx + 1) % PathFindingManager.C_FrameCountPerUpdate;
for (int i = 0; i < iCount; i = i + 1)
{
IAgentBehaviour agent = _allAgents[i];
if (agent.agent != null)
{
agent.agent.Loop(frameTime, null, _updateIdx != i % PathFindingManager.C_FrameCountPerUpdate);
}
}
}
internal static TSVector BoidsBehaviourSeek(IAgentBehaviour agent, TSVector velocity, TSVector dest)
{
if (agent.position == dest)
{
return(TSVector.zero);
}
TSVector desired = dest - agent.position;
//Desired velocity
desired = CustomMath.Normalize(desired) * (agent.maxSpeed);
//velocity change
TSVector velocityChange = desired - velocity;
// force
return(velocityChange * agent.baseData.maxForce * agent.invMaxSpeed);
}
public void UpdateGroup(IAgentBehaviour agent, int groupId)
{
if (agent.group != null)
{
RemoveAgent(agent);
}
AgentGroup group;
if (!_group.TryGetValue(groupId, out group))
{
group = s_groupPool.New();
group.groupId = groupId;
_group[groupId] = group;
}
group.AddAgent(agent);
}
//Alignment navigation
internal static void BoidsBehaviourAlignment(IAgentBehaviour agent, IAgentBehaviour a, FP nrSqr, ref TSVector averageHeading, ref int neighboursCount)
{
if (a.agent == null || !a.enabled)
{
return;
}
FP distanceSqr = (agent.position - a.position).sqrMagnitude;
//within the max distance and are moving
if (distanceSqr < nrSqr && a.velocity != TSVector.zero)
{
//Sum up headings
averageHeading = averageHeading + a.velocity.normalized;//warning:.normalized;,to be checked
neighboursCount++;
}
}
public void RemoveAgent(IAgentBehaviour agent)
{
if (agent == leader)
{
if (_agents.Count > 0)
{
leader = _agents[0];
}
else
{
leader = null;
}
}
_agents.Remove(agent);
agent.group = null;
}
//deprecated
public void UpdateAgents(int frameTime)
{
_updateIdx = (_updateIdx + 1) % C_FrameCountPerUpdate;
PathManager pm = _pathManagers[_pathManagers.Length - 1];
foreach (KeyValuePair <int, List <IAgentBehaviour> > kv in _agents)
{
List <IAgentBehaviour> agents = kv.Value;
int agentCount = agents.Count;
for (int i = agentCount - 1; i >= 0; i--)
{
IAgentBehaviour agent = agents[i];
if (agent != null)
{
agent.Loop(frameTime, _updateIdx != i % C_FrameCountPerUpdate);
}
}
}
}
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--;
}
internal FlowField UpdateAgent(IAgentBehaviour agent, TSVector preDest, TSVector dest, GridMap map)
{
if (preDest != dest)
{
FlowField ff;
bool hasPre = preDest != TSVector.MinValue;
int idx = map.GetGridNodeId(preDest);
if (hasPre && _dicFlowFields.TryGetValue(idx, out ff))
{
ff.RemoveAgent(agent);
}
idx = map.GetGridNodeId(dest);
if (!_dicFlowFields.TryGetValue(idx, out ff))
{
ff = new FlowField(dest, map);
_dicFlowFields[idx] = ff;
}
ff.AddAgent(agent);
return(ff);
}
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);
}
internal void AddAgent(IAgentBehaviour agent)
{
_allAgents.Add(agent);
// PathFindingManager.instance.AddAgent(agent);
}
public void Init(int eType, IAgentBehaviour agent)
{
_activeBoids = (int)eType - (int)EBoidsActiveType.collisionAvoidance - (int)EBoidsActiveType.avoidUnit;
this._behaviour = agent;
this.Init(agent);
}
internal void RemoveAgent(IAgentBehaviour agent)
{
_allAgents.Remove(agent);
// PathFindingManager.instance.RemoveAgent(agent);
}
internal FlowFieldAgent(IAgentBehaviour go) : base(go)
{
}
//void GenerateObstacleVOs(VOBuffer vos)
//{
// var range = maxSpeed * obstacleTimeHorizon;
// // Iterate through all obstacles that we might need to avoid
// for (int i = 0; i < simulator.obstacles.Count; i++)
// {
// var obstacle = simulator.obstacles[i];
// var vertex = obstacle;
// // Iterate through all edges (defined by vertex and vertex.dir) in the obstacle
// do
// {
// // Ignore the edge if the agent should not collide with it
// if (vertex.ignore || (vertex.layer & collidesWith) == 0)
// {
// vertex = vertex.next;
// continue;
// }
// // Start and end points of the current segment
// FP elevation1, elevation2;
// var p1 = To2D(vertex.position, out elevation1);
// var p2 = To2D(vertex.next.position, out elevation2);
// TSVector2 dir = (p2 - p1).normalized;
// // Signed distance from the line (not segment, lines are infinite)
// // TODO: Can be optimized
// FP dist = VO.SignedDistanceFromLine(p1, dir, position);
// if (dist >= -FP.One/100 && dist < range)
// {
// FP factorAlongSegment = TSVector2.Dot(position - p1, p2 - p1) / (p2 - p1).sqrMagnitude;
// // Calculate the elevation (y) coordinate of the point on the segment closest to the agent
// var segmentY = TSMath.Lerp(elevation1, elevation2, factorAlongSegment);
// // Calculate distance from the segment (not line)
// var sqrDistToSegment = (TSVector2.Lerp(p1, p2, factorAlongSegment) - position).sqrMagnitude;
// // Ignore the segment if it is too far away
// // or the agent is too high up (or too far down) on the elevation axis (usually y axis) to avoid it.
// // If the XY plane is used then all elevation checks are disabled
// if (sqrDistToSegment < range * range && (simulator.movementPlane == MovementPlane.XY || (elevationCoordinate <= segmentY + vertex.height && elevationCoordinate + height >= segmentY)))
// {
// vos.Add(VO.SegmentObstacle(p2 - position, p1 - position, TSVector2.zero, radius * FP.One/100, 1 / ObstacleTimeHorizon, 1 / simulator.DeltaTime));
// }
// }
// vertex = vertex.next;
// } while (vertex != obstacle && vertex != null && vertex.next != null);
// }
//}
#if USING_RVO
void GenerateNeighbourAgentVOs(VOBuffer vos, FP deltaTime)
{
FP inverseAgentTimeHorizon = 1 / agentTimeHorizon;
// The RVO algorithm assumes we will continue to
// move in roughly the same direction
TSVector2 optimalVelocity = currentVelocity;
int count = _behaviour.neighbours.Count;
for (int o = 0; o < count; o++)
{
IAgentBehaviour other = _behaviour.neighbours[o];
// Don't avoid ourselves
if (other == this)
{
continue;
}
// Interval along the y axis in which the agents overlap
FP maxY = TSMath.Min(elevationCoordinate + height, other.OrcaAgent.elevationCoordinate + other.OrcaAgent.height);
FP minY = TSMath.Max(elevationCoordinate, other.OrcaAgent.elevationCoordinate);
// The agents cannot collide since they are on different y-levels
if (maxY - minY < 0)
{
continue;
}
FP totalRadius = radius + other.colliderRadius;
// Describes a circle on the border of the VO
TSVector2 voBoundingOrigin = CustomMath.TSVecToVec2(other.position - _behaviour.position);
FP avoidanceStrength;
if (other.OrcaAgent.locked || other.OrcaAgent.manuallyControlled)
{
avoidanceStrength = 1;
}
else if (other.OrcaAgent.Priority > CustomMath.EPSILON || Priority > CustomMath.EPSILON)
{
avoidanceStrength = other.OrcaAgent.Priority / (Priority + other.OrcaAgent.Priority);
}
else
{
// Both this agent's priority and the other agent's priority is zero or negative
// Assume they have the same priority
avoidanceStrength = CustomMath.FPHalf;
}
// We assume that the other agent will continue to move with roughly the same velocity if the priorities for the agents are similar.
// If the other agent has a higher priority than this agent (avoidanceStrength > 0.5) then we will assume it will move more along its
// desired velocity. This will have the effect of other agents trying to clear a path for where a high priority agent wants to go.
// If this is not done then even high priority agents can get stuck when it is really crowded and they have had to slow down.
TSVector2 otherOptimalVelocity = TSVector2.Lerp(other.OrcaAgent.currentVelocity, other.OrcaAgent.desiredVelocity, 2 * avoidanceStrength - 1);
var voCenter = TSVector2.Lerp(optimalVelocity, otherOptimalVelocity, avoidanceStrength);
vos.Add(new VO(voBoundingOrigin, voCenter, totalRadius, inverseAgentTimeHorizon, 1 / deltaTime));
if (DebugDraw)
{
DrawVO(position + voBoundingOrigin * inverseAgentTimeHorizon + voCenter, totalRadius * inverseAgentTimeHorizon, position + voCenter);
}
}
}
internal IgnoreObstacleAgent(IAgentBehaviour go) : base(go)
{
}
//multi threads
#if SEARCH_ENEMY
public void FindTarget()
{
if (_nextCheckActionTime > _baseData.pfm.CurTime)
{
return;
}
int eCount = _listEnemyAgents.Count;
//enemy in sight,either move to spec or AStar path finding
if (eCount > 0 && _agentType == EAgentType.astar && !_pathBlocked && _agent != null)
{
AStarAgent aStarAgent = _agent as AStarAgent;
_needRepath = aStarAgent.NeedRePath();
bool isFindingPath = aStarAgent.IsFindingPath();
for (int i = 0; i < eCount; i++)
{
IAgentBehaviour enemyAgent = _listEnemyAgents[i]; //closest enemy
int idx = _map.GetGridNodeId(enemyAgent.position);
FP curTargetDstSqr = (enemyAgent.position - position).sqrMagnitude;
TSVector targetPos = enemyAgent.position;
_targetPos = enemyAgent.position;
if (_dicUnReachableAgents.Contains(idx) && _needRepath != ERepathType.hasOpen) //
{
if (eCount - 1 == i) //can't reach enemy agent,then walk to default target pos
{
targetPos = _defaultTargetPos;
enemyAgent = null;
_targetPos = _defaultTargetPos;
idx = _map.GetGridNodeId(_defaultTargetPos);
if (_dicUnReachableAgents.Contains(idx))
{
_targetDistSqr = (_defaultTargetPos - _baseData.position).sqrMagnitude;
//_nextCheckActionTime = _baseData.pfm.CurTime + 1000;
// _dicUnReachableAgents.Clear();//
return;
}
curTargetDstSqr = (_defaultTargetPos - _baseData.position).sqrMagnitude;
}
else
{
continue;
}
}
bool bSameTarget = this.agent.TargetPos == targetPos;
bool bNeedChangeTarget = !bSameTarget && curTargetDstSqr < _targetDistSqr - CustomMath.FPHalf;
bool bToDefault = _targetPos == _defaultTargetPos;
bool bNeedReFinding = _needRepath != ERepathType.none || !isFindingPath;
bool canFind = _pathManager.CanFindPathNow();
if (!canFind)
{
_frameOffset = 30;
}
bool bNeedFindPath = ((bNeedChangeTarget && !bToDefault) || bNeedReFinding) &&
canFind;
if (bNeedFindPath) //|| aStarAgent._asm==null
{
_aStarPathType = EAStarPathType.quickPath;
_targetDistSqr = curTargetDstSqr;
#if UNITY_EDITOR
if (PathFindingManager.DEBUG)
{
if (_needRepath != ERepathType.none)
{
UnityEngine.Debug.Log("needrepath findpath");
}
if (!isFindingPath)
{
UnityEngine.Debug.Log("not FindingPath findpath");
}
if (bNeedChangeTarget && !bToDefault)
{
UnityEngine.Debug.Log("not bSameTarget findpath");
}
}
#endif
break;
}
else if (bSameTarget) // && aStarAgent._asm != null same target and still finding the path,should't change target
{
break;
}
}
_frameOffset = _frameOffset > 0 ? _frameOffset : 300;
return;
}
if (eCount == 0)
{
if (_agentType == EAgentType.astar && _agent != null)
{
AStarAgent aStarAgent = _agent as AStarAgent;
if (aStarAgent.IsNearTargetPos())
{
_needRepath = aStarAgent.NeedRePath();
bool isFindingPath = aStarAgent.IsFindingPath();
bool bNeedReFinding = _needRepath != ERepathType.none || !isFindingPath;
if (bNeedReFinding) //|
{
//_needRepath = true;
_aStarPathType = EAStarPathType.quickPath;
this.agent.StartPos = _baseData.position;
this.agent.TargetPos = _defaultTargetPos;
//TSVector pathEndPos = _pathManager.FindQuickPath(this.agent, this.agent.StartPos, this.agent.TargetPos, _map, false);//
_targetPos = _defaultTargetPos;
}
}
}
}
}
/// <summary>
/// Gets an avoidance vector.
/// </summary>
/// <param name="selfPos">This unit's position.</param>
/// <param name="currentVelocity">This unit's current velocity.</param>
/// <param name="normalVelocity">This unit's normalized current velocity.</param>
/// <param name="unitData">This unit's UnitFacade.</param>
/// <param name="otherPos">The other unit's position.</param>
/// <param name="otherVelocity">The other unit's velocity.</param>
/// <param name="otherData">The other unit's UnitFacade.</param>
/// <param name="combinedRadius">The combined radius.</param>
/// <returns>An avoidance vector from the other unit's collision position to this unit's collision position - if a collision actually is detected.</returns>
private static TSVector GetAvoidVector(TSVector selfPos, TSVector currentVelocity, TSVector normalVelocity, IAgentBehaviour unitData, TSVector otherPos, TSVector otherVelocity, IAgentBehaviour otherData, FP combinedRadius)
{
TSVector selfCollisionPos = TSVector.zero;
TSVector avoidDirection = GetAvoidDirectionVector(selfPos, currentVelocity, otherPos, otherVelocity, combinedRadius, out selfCollisionPos);
FP avoidMagnitude = avoidDirection.magnitude;
if (avoidMagnitude == 0)
{
// if there is absolutely no magnitude to the found avoid direction, then ignore it
return(TSVector.zero);
}
FP vectorLength = combinedRadius * CustomMath.FPHalf;
if (vectorLength <= 0)
{
// if the units' combined radius is 0, then we cannot avoid
return(TSVector.zero);
}
// normalize the avoid vector and then set it's magnitude to the desired vector length (half of the combined radius)
TSVector avoidNormalized = (avoidDirection / avoidMagnitude);
TSVector avoidVector = avoidNormalized * vectorLength;
FP dotAngle = TSVector.Dot(avoidNormalized, normalVelocity);
if (dotAngle <= _cosAvoidAngle)
{
// the collision is considered "head-on", thus we compute a perpendicular avoid vector instead
avoidVector = new TSVector(avoidVector.z, avoidVector.y, -avoidVector.x);
}
else if (preventPassingInFront
//&& (otherData.determination > unitData.determination)
&& (TSVector.Dot(otherVelocity, avoidVector) > 0 && TSVector.Dot(currentVelocity, otherVelocity) >= 0))
{
// if supposed to be preventing front-passing, then check whether we should prevent it in this case and if so compute a different avoid vector
avoidVector = selfCollisionPos - selfPos;
}
// scale the avoid vector depending on the distance to collision, shorter distances need larger magnitudes and vice versa
FP collisionDistance = TSMath.Max(1, (selfPos - selfCollisionPos).magnitude);
avoidVector *= currentVelocity.magnitude / collisionDistance;
return(avoidVector);
}
