//额外辅助方法部分,暂定到达目标就算停止
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>
/// Returns the normalized vector that is perpendicular to line AB. (Costly method!)
/// </summary>
/// <remarks>
/// <para>
/// The direction of the vector will be to the right when viewed from point A to point B
/// along the line.
/// </para>
/// <para>
/// Special Case: A zero length vector will be returned if the points are collocated.
/// </para>
/// </remarks>
/// <param name="a">Point A on line AB.</param>
/// <param name="b">Point B on line AB.</param>
/// <returns>
/// The normalized vector that is perpendicular to line AB, or a zero length vector if the
/// points are collocated.
/// </returns>
public static Vector2 GetNormalAB(Vector2 a, Vector2 b)
{
if (Vector2Util.SloppyEquals(a, b, MathUtil.Tolerance))
{
// Points do not form a line.
return(new Vector2());
}
Vector2 result = Vector2Util.GetDirectionAB(a, b);
float origX = result.x;
result.x = result.y;
result.y = -origX;
return(result);
}
/// <summary>
/// Update the planner.
/// </summary>
/// <returns>True if successful.</returns>
public bool Update()
{
if (mAtGoalState == StateNormal)
{
agent.SyncCrowdToDesired();
}
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 new position to the 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 new goal to the navigation mesh. Ignoring: {1}"
, agent.transform.name, pt.ToString()));
newGoal = false;
}
else
{
agent.data.plannerGoal = pt;
}
}
if (mAtGoalState != StateNormal && newGoal || newPos)
{
TransitionToNormalGoalState();
}
agent.data.flags &= ~(NavFlag.HasNewPosition | NavFlag.HasNewGoal);
if (newPos)
{
// Note: This call inherently handles everything, not just the
// position change.
return(HandlePositionFeedback());
}
if (!HandleNormalPlanning(newGoal))
{
// A critical failure has occurred.
return(false);
}
if ((agent.data.flags & NavFlag.CrowdConfigUpdated) != 0)
{
CrowdAgentParams config = agent.crowdConfig;
if (mAtGoalState != StateNormal)
{
config.maxSpeed = 0;
}
agent.crowdAgent.SetConfig(config);
agent.data.flags &= ~NavFlag.CrowdConfigUpdated;
}
if (mAtGoalState == StateInRange)
{
// Need to manually move the agent to the goal.
Vector3 pos = agent.data.desiredPosition.point;
Vector3 goal = agent.data.plannerGoal.point;
if (Vector2Util.SloppyEquals(
new Vector2(pos.x, pos.z), new Vector2(goal.x, goal.z), 0.005f))
{
// Snap to the goal and, essentially, go into an idle
// state.
mAtGoalState = StateIdle;
agent.data.desiredPosition = agent.data.plannerGoal;
agent.data.desiredSpeedSq = 0;
agent.data.desiredVelocity = Vector3.zero;
// Debug.Log("Reached goal. Going idle.");
}
else
{
// Need to manually move the agent to the goal.
// Assumptions: The distance is short, and the crowd has
// already slowed the agent's speed to a minimal value.
float desiredSpeed = Mathf.Sqrt(agent.data.desiredSpeedSq);
agent.data.desiredPosition.point =
Vector3.MoveTowards(pos, goal, desiredSpeed * Time.deltaTime);
// While the point will be on the mesh surface, we
// can't assume we are already in the goal's polygon.
agent.data.desiredPosition.polyRef = 0;
agent.data.desiredVelocity =
(agent.data.desiredPosition.point - pos).normalized * desiredSpeed;
}
}
return(true);
}