/// <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);
}