//额外辅助方法部分,暂定到达目标就算停止
public bool IsAtDestination()
{
//原作中crowd用2D做检查，simple用3D做检查。
//这目前来看似乎没什么道理，所以索性整合在一起做一个方法，还可以返回给上层UnityAgent用
bool Check3D = Vector3Util.SloppyEquals(desiredPosition.point, plannerGoal.point, 0.005f);
Vector3 pos = desiredPosition.point;
Vector3 goal = plannerGoal.point;
bool Check2D = Vector2Util.SloppyEquals(new Vector2(pos.x, pos.z), new Vector2(goal.x, goal.z), 0.005f);
return(Check3D | Check2D);
}
/// <summary>
/// True if the two points are in range of each other. (Based on the current goal type.)
/// </summary>
/// <param name="u">Point u.</param>
/// <param name="v">Point v.</param>
/// <returns>True if the two points are in range of each other. </returns>
public bool IsInRange(Vector3 u, Vector3 v)
{
switch (data.goalType)
{
case RangeType.At:
return(Vector3Util.IsInRange(u, v, data.radiusAt, data.heightTolerence));
case RangeType.Near:
return(Vector3Util.IsInRange(u, v, data.radiusNear, data.heightTolerence));
default:
// Assume most restrictive.
return(Vector3Util.SloppyEquals(u, v, ExactTolerance));
}
}
/// <summary>
/// Updates the planner.
/// </summary>
/// <returns>True on success.</returns>
public bool Update()
{
// Note: Will never return false.
Vector3 pos = agent.data.position.point;
Vector3 goal = agent.data.goal.point;
// Uses a more accurate than normal location check
// in order to prevent snapping at the end of movement.
if (Vector3Util.SloppyEquals(pos, goal, NavAgent.ExactTolerance))
{
mSpeed = 0;
agent.data.desiredPosition = agent.data.goal;
agent.data.desiredVelocity = Vector3.zero;
agent.data.desiredSpeedSq = 0;
return(true);
}
float maxDelta = agent.crowdConfig.maxAcceleration * Time.deltaTime;
float desiredSpeed = agent.crowdConfig.maxSpeed;
// This test assumes the agent can halt in less than the given number of radii.
if (Vector3Util.GetDistance2D(pos, goal) < agent.crowdConfig.radius * 3)
{
desiredSpeed = Mathf.Max(mSpeed - maxDelta, desiredSpeed * 0.2f);
}
else
{
desiredSpeed = Mathf.Min(mSpeed + maxDelta, desiredSpeed);
}
Vector3 result = Vector3.MoveTowards(pos, goal, desiredSpeed * Time.deltaTime);
agent.data.desiredPosition.point = result;
agent.data.desiredPosition.polyRef = 0;
agent.data.desiredVelocity = (result - pos).normalized * desiredSpeed;
agent.data.desiredSpeedSq = desiredSpeed * desiredSpeed;
mSpeed = desiredSpeed;
return(true);
}
/// <summary>
/// Updates the planner.
/// </summary>
/// <returns>True on success.</returns>
public bool Update()
{
// Detect position and goal changes, and constrain to navmesh.
bool newPos = (agent.data.flags & NavFlag.HasNewPosition) != 0;
bool newGoal = (agent.data.flags & NavFlag.HasNewGoal) != 0;
NavmeshPoint pt;
if (newPos)
{
pt = agent.GetPointSearch(agent.data.position);
if (pt.polyRef == 0)
{
// Ignore new position.
Debug.LogWarning(string.Format(
"{0}: Could not constrain position to navigation mesh. Ignoring: {1}"
, agent.transform.name, pt.ToString()));
newPos = false;
}
else
{
agent.data.desiredPosition = pt;
}
}
if (newGoal)
{
pt = agent.GetPointSearch(agent.data.goal);
if (pt.polyRef == 0)
{
// Ignore new goal.
Debug.LogWarning(string.Format(
"{0}: Could not constrain goal to navigation mesh. Ignoring: {1}"
, agent.transform.name, pt.ToString()));
newGoal = false;
}
else
{
agent.data.plannerGoal = pt;
}
}
agent.data.flags &= ~(NavFlag.HasNewPosition | NavFlag.HasNewGoal);
// Handle planning.
if (newGoal || newPos)
{
// Replanning needed.
if (!HandlePlanning(newPos, newGoal))
{
// Critical failure.
return(false);
}
}
// Is any movement needed?
if (Vector3Util.SloppyEquals(agent.data.desiredPosition.point
, agent.data.plannerGoal.point
, NavAgent.ExactTolerance))
{
// At goal. Don't need to do anything.
mSpeed = 0;
agent.data.desiredPosition = agent.data.plannerGoal;
agent.data.desiredVelocity = Vector3.zero;
agent.data.desiredSpeedSq = 0;
return(true);
}
// Handle speed adjustments.
float maxDelta = agent.crowdConfig.maxAcceleration * Time.deltaTime;
float desiredSpeed = agent.crowdConfig.maxSpeed;
// This shortcut test assumes the agent can halt in less than the
// specified number of radii.
if (Vector3Util.GetDistance2D(agent.data.desiredPosition.point
, agent.data.plannerGoal.point) < agent.crowdConfig.radius * 3)
{
// Slow down to as low of 20% of maximum speed.
desiredSpeed = Mathf.Max(mSpeed - maxDelta, desiredSpeed * 0.2f);
}
else
{
// Speed up to as high as maximum speed.
desiredSpeed = Mathf.Min(mSpeed + maxDelta, desiredSpeed);
}
// Perform the move.
if (--mOptimizationTimer < 1)
{
agent.corridor.OptimizePathTopology(true);
mOptimizationTimer = OptimizationFrequency;
}
Vector3 movePos = Vector3.MoveTowards(agent.data.desiredPosition.point
, agent.corridor.Corners.verts[0]
, desiredSpeed * Time.deltaTime);
agent.corridor.MovePosition(movePos);
movePos = agent.corridor.Position.point;
agent.data.desiredVelocity =
(movePos - agent.data.desiredPosition.point).normalized * desiredSpeed;
agent.data.desiredSpeedSq = desiredSpeed * desiredSpeed;
agent.data.desiredPosition = agent.corridor.Position;
mSpeed = desiredSpeed;
return(true);
}