//使用现有路径从起点到目标点建立标准的直接路径
public NavStatus BuildStraightPath(NavmeshPoint start, NavmeshPoint goal, NavmeshQuery query)
{
int iStart = FindPolyRef(0, start.polyRef);
int iGoal = -1;
if (iStart != -1)
{
iGoal = FindPolyRefReverse(iStart, goal.polyRef);
}
if (iGoal == -1)
{
return(NavStatus.Failure | NavStatus.InvalidParam);
}
//NavmeshQuery是直通C++的底层寻路
Array.Clear(straightPoints, 0, straightPoints.Length);
NavStatus status = query.GetStraightPath(start.point, goal.point
, path, iStart, iGoal - iStart + 1
, straightPoints, straightFlags, straightPath, out straightCount);
if (straightCount == 0)
{
return(NavStatus.Failure);
}
return(status);
}
public static bool GetNavmeshPoint(Vector3 source, Vector3 extends, out NavmeshPoint point, NavmeshQueryFilter filter = null)
{
point = new NavmeshPoint();
if (m_Map == null)
{
return(false);
}
var query = m_Map.Query;
if (query == null)
{
return(false);
}
if (filter == null)
{
filter = m_Map.DefaultQueryFilter;
}
var status = query.GetNearestPoint(source, extends, filter, out point);
if (NavUtil.Failed(status))
{
return(false);
}
return(true);
}
public static extern NavStatus dtqFindPath(IntPtr query
, NavmeshPoint startPosition
, NavmeshPoint endPosition
, IntPtr filter
, [In, Out] uint[] resultPath
, ref int pathCount
, int maxPath);
public static extern NavStatus dtqFindDistanceToWall(IntPtr query
, NavmeshPoint position
, float searchRadius
, IntPtr filter
, ref float distance
, ref Vector3 closestPoint
, ref Vector3 normal);
//从起点到终点的寻路
//如果没找到路就返回-1
public int PlanPath(NavmeshPoint start, NavmeshPoint end)
{
path.straightCount = 0;
if (start.polyRef == 0 || end.polyRef == 0 || path == null)
{
return(-1);
}
if (start.polyRef == end.polyRef)
{
path.pathCount = 1;
path.path[0] = start.polyRef;
navStatus = NavStatus.Sucess;
return(1);
}
if (path.FixPath(start.polyRef, end.polyRef))
{
navStatus = NavStatus.Sucess;
return(path.pathCount);
}
//这是底层寻路的方法了
navStatus = navGroup.query.FindPath(start, end, navGroup.filter, path.path, out path.pathCount);
return(path.pathCount);
}
/// <summary>
/// Handles planning when position feedback is required. (Replaces normal planning.)
/// </summary>
/// <returns>False on a critical failure.</returns>
private bool HandlePositionFeedback()
{
// Important: Should only call this method once the desired position and planner goal
// have been successfully constrained to the navigation mesh.
// Make sure the path is still valid. Replan if needed.
if (agent.PlanPath(agent.data.desiredPosition, agent.data.plannerGoal) <= 0)
{
// A critical failure.
Debug.LogError(string.Format(
"{0}: Path planning failed on position feedback. Position: {1}, Goal: {2}"
, agent.transform.name
, agent.data.desiredPosition.ToString()
, agent.data.plannerGoal.ToString()));
return(false);
}
agent.RemoveFromCrowd();
agent.AddToCrowd(agent.data.desiredPosition.point);
// Next assignment will usually force crowd target replanning.
mTarget = agent.data.desiredPosition;
// Since the path is known to be good, setting the local
// target should never fail.
return(SetLocalTarget());
}
/// <summary>
/// Builds a straight path and gets the point farthest along the path that does not exceed
/// the specified maximum number of polygons.
/// </summary>
/// <remarks>
/// <para>Limits:</para>
/// <ul>
/// <li>The path must exist and both the start and goal points must be within the path.</li>
/// <li>
/// The goal's polygon reference must be in or after the start points polygon reference.
/// </li>
/// </ul>
/// <para>
/// <b>Special Case:</b>The result will exceed <paramref name="maxLength"/> if the
/// first straight path point is greater than <paramref name="maxLength"/> from the start
/// point.
/// </para>
/// </remarks>
/// <param name="start">The start point within the current path.</param>
/// <param name="goal">The end point located in or after the start point polygon.</param>
/// <param name="maxLength">
/// The maximum allowed number of polygons between the start and target. [Limit: >= 1]
/// </param>
/// <param name="target">The resulting target point.</param>
/// <returns>The straight path index of the target point, or -1 on error.</returns>
public int GetLocalTarget(NavmeshPoint start, NavmeshPoint goal, int maxLength
, NavmeshQuery query
, out NavmeshPoint target)
{
if (NavUtil.Failed(BuildStraightPath(start, goal, query)))
{
target = new NavmeshPoint();
return(-1);
}
int targetIndex = straightCount; // Will be decremented.
int iStart = FindPolyRef(0, start.polyRef);
// Start at the end of the straight path and search back toward
// the start until the number of polygons is less than
// maxLength.
uint targetRef = 0;
do
{
targetIndex--;
targetRef = (straightPath[targetIndex] == 0 ?
goal.polyRef : straightPath[targetIndex]);
}while (FindPolyRefReverse(iStart, targetRef) - iStart + 1
> maxLength &&
targetIndex > 0);
target = new NavmeshPoint(targetRef, straightPoints[targetIndex]);
return(targetIndex);
}
//给出目标进行移动的方法
private void MoveTo(NavmeshPoint goal, bool ignoreGoalRotation)
{
if (mNavAgent == null)
{
return;
}
goal = mNavAgent.GetPointSearch(goal);
if (goal.polyRef == 0)
{
return;
}
mNavAgent.flags &= ~NavFlag.GoalIsDynamic;
mNavAgent.targetMode = mNavAgent.ChangeMode();
mNavAgent.goal = goal;
if (ignoreGoalRotation)
{
mNavAgent.flags = (mNavAgent.flags | NavFlag.HasNewGoal) & ~NavFlag.GoalRotationEnabled;
}
else
{
mNavAgent.flags |= (NavFlag.HasNewGoal | NavFlag.GoalRotationEnabled);
}
}
public static extern bool dtpcMoveOverOffmeshConnection(IntPtr corridor
, uint offMeshConRef
, [In, Out] uint[] refs // size 2
, ref Vector3 startPos
, ref Vector3 endPos
, ref NavmeshPoint resultPos
, IntPtr navquery);
/// <summary>
/// Move to the goal using the standard navigation mode.
/// </summary>
/// <param name="goal">The goal.</param>
/// <param name="rangeType">The range check type.</param>
/// <param name="ignoreGoalRotation">
/// True if the final agent rotation does not matter.
/// </param>
/// <returns>True if the move request succeeded.</returns>
public bool MoveTo(NavmeshPoint goal, RangeType rangeType, bool ignoreGoalRotation)
{
goal = agent.GetPointSearch(goal);
if (goal.polyRef == 0)
{
return(false);
}
agent.data.flags &= ~NavFlag.GoalIsDynamic;
agent.data.targetMode = agent.ChooseMoveToMode();
agent.data.goal = goal;
agent.data.goalType = rangeType;
if (ignoreGoalRotation)
{
agent.data.flags =
(agent.data.flags | NavFlag.HasNewGoal) & ~NavFlag.GoalRotationEnabled;
}
else
{
agent.data.flags |= (NavFlag.HasNewGoal | NavFlag.GoalRotationEnabled);
}
// Forces evaluation during next upate.
mAtGoal = false;
PostEvent(NavEventType.NewGoal);
return(true);
}
private bool HandlePlanning(bool retargetPos, bool retargetGoal)
{
/*
* Design notes:
*
* Tries to do a corridor move on the position and/or target.
* A replan is only triggered if the move(s) fail to reach the desired polygon(s).
*
* It is important to note that the replan may fail to reach the goal. But that is a
* problem for the AI, not the planner.
*/
bool needsFullReplan = false;
// Convenience vars. Only used for input. Not updated.
NavmeshPoint goal = agent.data.plannerGoal;
NavmeshPoint pos = agent.data.desiredPosition;
PathCorridor corridor = agent.corridor;
if (retargetGoal && retargetPos)
{
corridor.Move(pos.point, goal.point);
retargetGoal = (goal.polyRef != corridor.Target.polyRef ||
pos.polyRef != corridor.Position.polyRef);
}
else if (retargetPos)
{
corridor.MovePosition(pos.point);
needsFullReplan = (pos.polyRef != corridor.Position.polyRef);
}
else if (retargetGoal)
{
corridor.MoveTarget(goal.point);
needsFullReplan = (goal.polyRef != corridor.Target.polyRef);
}
if (needsFullReplan)
{
// Debug.Log("Full Replan");
if (agent.PlanCorridor(pos, goal) <= 0)
{
Debug.LogError(agent.transform.name + ": Could not replan corridor.");
return(false);
}
}
else if (retargetPos || retargetGoal)
{
// Debug.Log("Optimize Only");
// Any chagnes are likely to be larger than normal. So force an optimization.
agent.corridor.OptimizePathTopology(true);
mOptimizationTimer = OptimizationFrequency;
}
agent.data.desiredPosition = agent.corridor.Position;
agent.data.plannerGoal = agent.corridor.Target;
return(true);
}
public static extern NavStatus dtqMoveAlongSurface(IntPtr query
, NavmeshPoint startPosition
, [In] ref Vector3 endPosition
, IntPtr filter
, ref Vector3 resultPosition
, [In, Out] uint[] visitedPolyRefs
, ref int visitedCount
, int maxVisited);
private bool Follow(NavmeshPoint pt)
{
if (!IsInited || !NavMeshMap.IsVaild)
{
return(false);
}
return(m_Agent.RequestMoveTarget(pt));
}
public static extern int dtpcMoveTargetPosition(IntPtr corridor
, [In] ref Vector3 npos
, ref NavmeshPoint pos
, [In, Out] Vector3[] cornerVerts
, [In, Out] WaypointFlag[] cornerFlags
, [In, Out] uint[] cornerPolys
, int maxCorners
, IntPtr navquery
, IntPtr filter);
public void Update()
{
if (mPolyRefs.HandleResize())
{
mStraightPath = new Vector3[mPolyRefs.MaxElementCount];
mVisitedCount = 0;
mStraightCount = 0;
}
if (Input.GetKeyDown(StdButtons.SelectA) ||
Input.GetKey(StdButtons.SelectB))
{
Vector3 trasha;
string trashb;
NavmeshPoint point;
QEUtil.SearchResult searchResult =
QEUtil.HandleStandardPolySearch(mHelper, out trasha, out point, out trashb);
if ((searchResult & QEUtil.SearchResult.HitNavmesh) == 0)
{
return;
}
if (mPosition.polyRef == 0 || Input.GetKey(StdButtons.SelectB))
{
mPosition = point;
mGoal.polyRef = 0;
}
else
{
mGoal = point;
}
mVisitedCount = 0;
mStraightCount = 0;
}
if (mVisitedCount == 0 &&
mPosition.polyRef != 0 &&
mGoal.polyRef != 0)
{
NavStatus status = mHelper.query.MoveAlongSurface(mPosition, mGoal.point
, mHelper.filter
, out mTarget.point, mPolyRefs.buffer, out mVisitedCount);
mMessages = "MoveAlongSurface: " + status.ToString();
if (mVisitedCount > 0)
{
mHelper.query.GetStraightPath(mPosition.point, mTarget.point
, mPolyRefs.buffer, 0, mVisitedCount
, mStraightPath
, null, null, out mStraightCount);
}
}
}
public static extern NavStatus dtqRaycast(IntPtr query
, NavmeshPoint startPosition
, [In] ref Vector3 endPosition
, IntPtr filter
, ref float hitParameter
, ref Vector3 hitNormal
, [In, Out] uint[] path
, ref int pathCount
, int maxPath);
//这个方法是真的移动计划的移动方法
//根据路线进行移动(注意，这个Agent目标移动数据的修改)
private bool HandlePlanning(bool retargetPos, bool retargetGoal)
{
bool needsFullReplan = false;
//获取记录下来的这些信息
NavmeshPoint goal = theAgent.plannerGoal;
NavmeshPoint pos = theAgent.desiredPosition;
//获取路线引用
PathCorridor corridor = theAgent.corridor;
if (retargetGoal && retargetPos)
{
//此处调用CritterAI的PathCorridor,进而调用PathCorridorEx走recast
//从当前的位置转移到希望运动到的位置，另外将目标移动到希望移动到的目标
corridor.Move(pos.point, goal.point);
retargetGoal = (goal.polyRef != corridor.Target.polyRef || pos.polyRef != corridor.Position.polyRef);
}
else if (retargetPos)
{
//此处调用CritterAI的PathCorridor,进而调用PathCorridorEx走recast
//从当前的位置转移到希望运动到的位置
corridor.MovePosition(pos.point);
needsFullReplan = (pos.polyRef != corridor.Position.polyRef);
}
else if (retargetGoal)
{
//此处调用CritterAI的PathCorridor,进而调用PathCorridorEx走recast
//将目标移动到希望移动到的目标
corridor.MoveTarget(goal.point);
needsFullReplan = (goal.polyRef != corridor.Target.polyRef);
}
if (needsFullReplan)
{
//完全重新计算路径
if (theAgent.PlanCorridor(pos, goal) <= 0)
{
//Debug.LogError(theAgent.transform.name + ": Could not replan corridor.");
SetPathInformation(pos, goal);
return(false);
}
}
else if (retargetPos || retargetGoal)
{
//CritterAI作者加的强制优化
theAgent.corridor.OptimizePathTopology(true);
mOptimizationTimer = OptimizationFrequency;
}
//最后重新记录位置就可以了
theAgent.desiredPosition = theAgent.corridor.Position;
theAgent.plannerGoal = theAgent.corridor.Target;
return(true & SetPathInformation(theAgent.desiredPosition, theAgent.plannerGoal));
}
//是否有路径
public bool HasPathToAim(Vector3 aim)
{
NavmeshPoint pt = GetNavPoint(aim);
if (pt.polyRef == 0)
{
return(false);
}
return(true);
}
public static extern int dtpcSetCorridor(IntPtr corridor
, [In] ref Vector3 target
, [In] uint[] path
, int pathCount
, ref NavmeshPoint resultTarget
, [In, Out] Vector3[] cornerVerts
, [In, Out] WaypointFlag[] cornerFlags
, [In, Out] uint[] cornerPolys
, int maxCorners
, IntPtr navquery
, IntPtr filter);
//外部的设定目标的方法
public void SetDestination(Vector3 aimPoint)
{
NavmeshPoint pt = GetNavPoint(aimPoint);
if (pt.polyRef == 0)
{
//Debug.Log("没有路线："+aimPoint);
return;
}
nowDestination = aimPoint;
MoveTo(pt, true);
}
//调用底层的NavmeshQuery（导航网格查询）进行查询，这样就将设定的点与导航网格的点联系起来
//如果该点没有多边形引用，则对其执行加宽搜索
public NavmeshPoint GetPointSearch(NavmeshPoint point)
{
NavmeshPoint result = point;
if (result.polyRef == 0)
{
navStatus = navGroup.query.GetNearestPoint(point.point, navGroup.extents, navGroup.filter, out result);
if (result.polyRef == 0)
{
navStatus = navGroup.query.GetNearestPoint(point.point, wideExtents, navGroup.filter, out result);
}
}
return(result);
}
/// <summary>
/// Starts up the planner.
/// </summary>
/// <returns>True on success.</returns>
public bool Enter()
{
if (agent.navGroup.query == null)
{
return(false);
}
NavmeshPoint pos = agent.GetPointSearch(agent.data.position);
if (pos.polyRef == 0)
{
Debug.LogError(string.Format(
"{0}: Could not constrain position to navigation mesh. {1}"
, agent.transform.name, pos.ToString()));
return(false);
}
NavmeshPoint goal = agent.GetPointSearch(agent.data.goal);
if (goal.polyRef == 0)
{
Debug.LogError(string.Format("{0}: Could not constrain goal to navigation mesh. {1}"
, agent.transform.name, goal.ToString()));
return(false);
}
agent.RemoveFromCrowd();
agent.SetCorridorAssets(true);
agent.SetPathAssets(false);
if (agent.PlanCorridor(pos, goal) <= 0)
{
Debug.LogError(string.Format("{0}: Could not plan corridor on enter."
, agent.transform.name));
agent.data.flags &= ~NavFlag.CorridorInUse;
return(false);
}
agent.data.desiredPosition = agent.corridor.Position;
agent.data.plannerGoal = agent.corridor.Target;
agent.data.flags &= ~(NavFlag.HasNewPosition | NavFlag.HasNewGoal);
mSpeed = Mathf.Sqrt(agent.data.desiredSpeedSq);
mOptimizationTimer = OptimizationFrequency;
return(true);
}
/// <summary>
/// Move straight to the goal, ignoring the navigation mesh.
/// </summary>
/// <param name="goal">The goal.</param>
/// <param name="goalRotation">The rotation at the end of the movement.</param>
/// <param name="rangeType">The range check type.</param>
/// <returns>True if the move request succeeded.</returns>
public void MoveStraightTo(NavmeshPoint goal, Quaternion goalRotation, RangeType rangeType)
{
agent.data.flags &= ~NavFlag.GoalIsDynamic;
agent.data.targetMode = NavMode.MoveToLocal;
agent.data.goal = goal;
agent.data.goalRotation = goalRotation;
agent.data.goalType = rangeType;
agent.data.flags |= (NavFlag.HasNewGoal | NavFlag.GoalRotationEnabled);
// Forces evaluation during next upate.
mAtGoal = false;
PostEvent(NavEventType.NewGoal);
}
//从池子里面获取通道，如果没有就需要建立一个（目前都是在NavAgent里面处理的）
public PathCorridor GetCorridor(NavmeshPoint position, NavmeshQuery query, NavmeshQueryFilter filter)
{
if (mCorridors.Count > 0)
{
PathCorridor corr = mCorridors.Pop();
if (PathCorridor.LoadLocals(corr, position, query, filter))
{
return(corr);
}
return(null);
}
return(new PathCorridor(mMaxPathSize, mMaxStraightPathSize, query, filter));
}
//实际上这是一个不断开闭的过程
//循环过程中也会不断Enter
public override bool Enter()
{
if (theAgent.navGroup.query == null)
{
return(false);
}
//在导航网格上找到自己的位置
NavmeshPoint pos = theAgent.GetPointSearch(theAgent.position);
if (pos.polyRef == 0)
{
//Debug.LogError(string.Format("{0}: Could not constrain position to navigation mesh. {1}", theAgent.transform.name, pos.ToString()));
return(false);
}
//在导航网格上找到目标的位置
NavmeshPoint goal = theAgent.GetPointSearch(theAgent.goal);
if (goal.polyRef == 0)
{
//Debug.LogError(string.Format("{0}: Could not constrain goal to navigation mesh. {1}", theAgent.transform.name, goal.ToString()));
return(false);
}
theAgent.RemoveFromCrowd();
theAgent.SetCorridorAssets(true);
theAgent.SetPathAssets(true);
//agent自己寻路，发现没有路就报错
if (theAgent.PlanCorridor(pos, goal) <= 0)
{
//Debug.LogError(string.Format("{0}: Could not plan corridor on enter.", theAgent.transform.name));
theAgent.flags &= ~NavFlag.CorridorInUse;
return(false);
}
//存储寻路之后的结果
theAgent.desiredPosition = theAgent.corridor.Position;
theAgent.plannerGoal = theAgent.corridor.Target;
theAgent.flags &= ~(NavFlag.HasNewPosition | NavFlag.HasNewGoal);
//设定速度和重置计数器
mSpeed = Mathf.Sqrt(theAgent.desiredSpeedSq);
mOptimizationTimer = OptimizationFrequency;
return(true);
}
/// <summary>
/// Starts up the planner.
/// </summary>
/// <returns>True if successful.</returns>
public bool Enter()
{
if (agent == null || agent.navGroup.query == null || agent.navGroup.crowd == null)
{
// Agent in invalid state.
return(false);
}
NavmeshPoint pos = agent.GetPointSearch(agent.data.position);
if (pos.polyRef == 0)
{
Debug.LogError(string.Format(
"{0}: Could not constrain position to navigation mesh. {1}"
, agent.transform.name, pos.ToString()));
return(false);
}
NavmeshPoint goal = agent.GetPointSearch(agent.data.goal);
if (goal.polyRef == 0)
{
Debug.LogError(string.Format("{0}: Could not constrain goal to navigation mesh. {1}"
, agent.transform.name, goal.ToString()));
return(false);
}
if (agent.AddToCrowd(pos.point) == null)
{
Debug.LogError(string.Format("{0}: Could not add agent to the crowd."
, agent.transform.name));
return(false);
}
agent.data.desiredPosition = pos;
agent.data.plannerGoal = goal;
agent.data.flags &= ~(NavFlag.HasNewPosition | NavFlag.HasNewGoal);
agent.SetCorridorAssets(false);
agent.SetPathAssets(true);
return(HandlePositionFeedback());
}
public void SimUpdate(AgentContext context)
{
if (state != TaskState.Active)
{
return;
}
NavmeshPoint pt = context.navCon.GetNavPoint(target.position);
if (pt.polyRef == 0)
{
// Not a valid target.
return;
}
context.navCon.MoveTo(pt, RangeType.At, true);
}
public List <Waypoint> GenerateWaypointsBetweenTwoPoints(Navmesh navmesh, Vector3 startPos, Vector3 endPos)
{
List <Waypoint> waypoints = new List <Waypoint>();
NavmeshQuery query;
var status = NavmeshQuery.Create(navmesh, 1024, out query);
if (!NavUtil.Failed(status))
{
org.critterai.Vector3 navStartPointVect;
org.critterai.Vector3 navEndPointVect;
var navStartPointStatus = query.GetNearestPoint(1, new org.critterai.Vector3(startPos.x, startPos.y, startPos.z), out navStartPointVect);
var navEndPointStatus = query.GetNearestPoint(1, new org.critterai.Vector3(startPos.x, startPos.y, startPos.z), out navEndPointVect);
if (navStartPointStatus == NavStatus.Sucess && navEndPointStatus == NavStatus.Sucess)
{
NavmeshPoint navStartPoint = new NavmeshPoint(1, new org.critterai.Vector3(startPos.x, startPos.y, startPos.z));
NavmeshPoint navEndPoint = new NavmeshPoint(1, new org.critterai.Vector3(endPos.x, endPos.y, endPos.z));
uint[] path = new uint[1024];
int pathCount;
status = query.FindPath(navStartPoint, navEndPoint, new NavmeshQueryFilter(), path, out pathCount);
if (!NavUtil.Failed(status))
{
const int MaxStraightPath = 4;
int wpCount;
org.critterai.Vector3[] wpPoints = new org.critterai.Vector3[MaxStraightPath];
uint[] wpPath = new uint[MaxStraightPath];
WaypointFlag[] wpFlags = new WaypointFlag[MaxStraightPath];
status = query.GetStraightPath(navStartPoint.point, navEndPoint.point
, path, 0, pathCount, wpPoints, wpFlags, wpPath
, out wpCount);
if (!NavUtil.Failed(status) && wpCount > 0)
{
foreach (var wp in wpPoints)
{
Mogre.Vector3 wayPointPos = new Vector3(wp.x, wp.y, wp.z);
waypoints.Add(new Waypoint(wayPointPos, new Vector3()));
}
}
}
}
}
return(waypoints);
}
/// <summary>
/// Adjust the position of an existing follow goal.
/// </summary>
/// <remarks>
/// <para>
/// Expected to be called frequently. Problems can occur if the goal delta is too large.
/// </para>
/// <para>
/// Must be in follow mode. (See: <see cref="Follow"/>)
/// </para>
/// <para>
/// Will not trigger a 'new goal' event.
/// </para>
/// </remarks>
/// <param name="goal">The adjusted goal.</param>
/// <returns>True if successful.</returns>
public bool AdjustFollow(NavmeshPoint goal)
{
goal = agent.GetPointSearch(goal);
if (goal.polyRef == 0 || (agent.data.flags & NavFlag.GoalIsDynamic) == 0)
{
// Invalid request.
return(false);
}
agent.data.goal = goal;
// Always force evaluation during next upate.
mAtGoal = false;
return(true);
}
/// <summary>
/// Starts up the planner.
/// </summary>
/// <returns>True if successful.</returns>
public bool Enter()
{
if (agent == null || agent.navGroup.query == null || agent.navGroup.crowd == null)
{
return(false);
}
NavmeshPoint pos = agent.GetPointSearch(agent.data.position);
if (pos.polyRef == 0)
{
Debug.LogError(string.Format(
"{0}: Could not constrain position to navigation mesh. {1}"
, agent.transform.name, pos.ToString()));
return(false);
}
agent.RemoveFromCrowd();
CrowdAgentParams config = agent.crowdConfig;
config.maxSpeed = 0;
agent.crowdAgent =
agent.navGroup.crowd.AddAgent(agent.data.plannerGoal.point, config);
if (agent.crowdAgent == null)
{
Debug.LogError(string.Format("{0}: Could not add agent to the crowd."
, agent.transform.name));
return(false);
}
agent.data.flags &= ~NavFlag.CrowdConfigUpdated;
agent.data.desiredPosition = agent.data.position;
agent.data.desiredSpeedSq = 0;
agent.data.desiredVelocity = Vector3.zero;
agent.SetCorridorAssets(false);
agent.SetPathAssets(false);
return(true);
}
public static extern bool dtcAdjustMoveTarget(IntPtr crowd
, int agentIndex
, NavmeshPoint position);
public static extern NavStatus dtqFindPathExt(IntPtr query
, ref NavmeshPoint startPosition
, ref NavmeshPoint endPosition
, [In] ref Vector3 extents
, IntPtr filter
, [In, Out] uint[] resultPath
, ref int pathCount
, int maxPath);
public static extern NavStatus dtqMoveAlongSurface(IntPtr query
, NavmeshPoint startPosition
, [In] ref Vector3 endPosition
, IntPtr filter
, ref Vector3 resultPosition
, [In, Out] uint[] visitedPolyRefs
, ref int visitedCount
, int maxVisited);
public static extern NavStatus dtqFindRandomPointCircle(IntPtr query
, NavmeshPoint start
, float radius
, IntPtr filter
, ref NavmeshPoint randomPt);
public static extern bool dtpcMoveOverOffmeshConnection(IntPtr corridor
, uint offMeshConRef
, [In, Out] uint[] refs // size 2
, ref Vector3 startPos
, ref Vector3 endPos
, ref NavmeshPoint resultPos
, IntPtr navquery);
/// <summary>
/// Finds a path from the start point to the end point using the <see cref="path"/> object.
/// </summary>
/// <remarks>
/// <para
/// ><b>Warning</b>: The path object must be available before calling this method.
/// </para>
/// <para>
/// This method will perform full planning only if <paramref name="start"/> or
/// <paramref name="end"/> is outside the existing path.
/// </para>
/// <para>
/// The input points are all expected to be valid. (E.g. polyRef != 0)
/// </para>
/// </remarks>
/// <param name="start">
/// A valid navigation mesh point representing the start of the path.
/// </param>
/// <param name="end">
/// A valid navigation mesh point representing the end of the path.
/// </param>
/// <returns>The length of the path, or -1 on failure.</returns>
public int PlanPath(NavmeshPoint start, NavmeshPoint end)
{
path.straightCount = 0;
if (start.polyRef == 0 || end.polyRef == 0 || path == null)
return -1;
if (start.polyRef == end.polyRef)
{
// Debug.Log("Replan Path: Points in same poly.");
path.pathCount = 1;
path.path[0] = start.polyRef;
data.navStatus = NavStatus.Sucess;
return 1;
}
if (path.FixPath(start.polyRef, end.polyRef))
{
// Debug.Log("Replan Path: Fixed existing path");
data.navStatus = NavStatus.Sucess;
return path.pathCount;
}
// Debug.Log("Replace Path: Full query.");
data.navStatus = navGroup.query.FindPath(start
, end
, navGroup.filter
, path.path
, out path.pathCount);
return path.pathCount;
}
/// <summary>
/// Builds a standard straight path from the start to the goal point using the exiting path.
/// </summary>
/// <remarks>
/// <para>Limits:</para>
/// <ul>
/// <li>The path must exist and both the start and goal points must be within the path.</li>
/// <li>
/// The goal's polygon reference must be in or after the start points polygon reference.
/// </li>
/// </ul>
/// </remarks>
/// <param name="start">The start point.</param>
/// <param name="goal">The goal point.</param>
/// <returns>The status of the operation.</returns>
public NavStatus BuildStraightPath(NavmeshPoint start, NavmeshPoint goal, NavmeshQuery query)
{
int iStart = FindPolyRef(0, start.polyRef);
int iGoal = -1;
if (iStart != -1)
iGoal = FindPolyRefReverse(iStart, goal.polyRef);
if (iGoal == -1)
return (NavStatus.Failure | NavStatus.InvalidParam);
NavStatus status = query.GetStraightPath(start.point, goal.point
, path, iStart, iGoal - iStart + 1
, straightPoints, straightFlags, straightPath, out straightCount);
if (straightCount == 0)
return NavStatus.Failure;
return status;
}
public static extern NavStatus dtqFindDistanceToWall(IntPtr query
, NavmeshPoint position
, float searchRadius
, IntPtr filter
, ref float distance
, ref Vector3 closestPoint
, ref Vector3 normal);
/// <summary>
/// Uses the long distance path to determine the best target and, if necessary, feeds it
/// to the crowd manager.
/// </summary>
/// <returns>False if a target could not be determined. (Agent wandered outside
/// current path.)</returns>
private bool SetLocalTarget()
{
mGoalInRange = false;
NavmeshPoint target;
int targetIndex = agent.path.GetLocalTarget(
agent.data.desiredPosition, agent.data.plannerGoal
, 32 // TODO: Make configurable.
, agent.navGroup.query
, out target);
if (targetIndex == -1)
{
// Position or goal not in current path. (Not necessarily an error.) May have been
// caused by the crowd manager, which has its own path planning.
return false;
}
else if (target.point != mTarget.point)
{
// New target. Send to crowd manager.
if (mAtGoalState != StateNormal)
{
// Assume that the agent is no longer at the goal.
agent.data.flags |= NavFlag.CrowdConfigUpdated;
if (mAtGoalState != StateNormal)
TransitionToNormalGoalState();
}
agent.crowdAgent.RequestMoveTarget(target);
}
mTarget = target;
mGoalInRange = (mTarget.point == agent.data.plannerGoal.point);
// Debug.LogWarning(mTarget.ToString() + ", " + mGoalInRange);
return true;
}
/// <summary>
/// Handles planning when position feedback is required. (Replaces normal planning.)
/// </summary>
/// <returns>False on a critical failure.</returns>
private bool HandlePositionFeedback()
{
// Important: Should only call this method once the desired position and planner goal
// have been successfully constrained to the navigation mesh.
// Make sure the path is still valid. Replan if needed.
if (agent.PlanPath(agent.data.desiredPosition, agent.data.plannerGoal) <= 0)
{
// A critical failure.
Debug.LogError(string.Format(
"{0}: Path planning failed on position feedback. Position: {1}, Goal: {2}"
, agent.transform.name
, agent.data.desiredPosition.ToString()
, agent.data.plannerGoal.ToString()));
return false;
}
agent.RemoveFromCrowd();
agent.AddToCrowd(agent.data.desiredPosition.point);
// Next assignment will usually force crowd target replanning.
mTarget = agent.data.desiredPosition;
// Since the path is known to be good, setting the local
// target should never fail.
return SetLocalTarget();
}
public static extern NavStatus dtqFindNearestPoly(IntPtr query
, [In] ref Vector3 position
, [In] ref Vector3 extents
, IntPtr filter
, ref NavmeshPoint nearest);
/// <summary>
/// Builds a straight path and gets the point farthest along the path that does not exceed
/// the specified maximum number of polygons.
/// </summary>
/// <remarks>
/// <para>Limits:</para>
/// <ul>
/// <li>The path must exist and both the start and goal points must be within the path.</li>
/// <li>
/// The goal's polygon reference must be in or after the start points polygon reference.
/// </li>
/// </ul>
/// <para>
/// <b>Special Case:</b>The result will exceed <paramref name="maxLength"/> if the
/// first straight path point is greater than <paramref name="maxLength"/> from the start
/// point.
/// </para>
/// </remarks>
/// <param name="start">The start point within the current path.</param>
/// <param name="goal">The end point located in or after the start point polygon.</param>
/// <param name="maxLength">
/// The maximum allowed number of polygons between the start and target. [Limit: >= 1]
/// </param>
/// <param name="target">The resulting target point.</param>
/// <returns>The straight path index of the target point, or -1 on error.</returns>
public int GetLocalTarget(NavmeshPoint start, NavmeshPoint goal, int maxLength
, NavmeshQuery query
, out NavmeshPoint target)
{
if (NavUtil.Failed(BuildStraightPath(start, goal, query)))
{
target = new NavmeshPoint();
return -1;
}
int targetIndex = straightCount; // Will be decremented.
int iStart = FindPolyRef(0, start.polyRef);
// Start at the end of the straight path and search back toward
// the start until the number of polygons is less than
// maxLength.
uint targetRef = 0;
do
{
targetIndex--;
targetRef = (straightPath[targetIndex] == 0 ?
goal.polyRef : straightPath[targetIndex]);
}
while (FindPolyRefReverse(iStart, targetRef) - iStart + 1
> maxLength
&& targetIndex > 0);
target = new NavmeshPoint(targetRef, straightPoints[targetIndex]);
return targetIndex;
}
/// <summary>
/// Performs an widening search for the point if it does not have a polygon reference.
/// </summary>
/// <remarks>
/// <para>
/// A search will only be attempted if the point's polygon reference is zero.
/// </para>
/// <para>
/// The result's polygon reference will remain zero if the search failed.
/// </para>
/// </remarks>
/// <param name="point">A point that needs to be constrained to the navigation mesh.</param>
/// <returns>The point, constrained to the navigation mesh.</returns>
public NavmeshPoint GetPointSearch(NavmeshPoint point)
{
NavmeshPoint result = point;
if (result.polyRef == 0)
{
data.navStatus = navGroup.query.GetNearestPoint(
point.point, navGroup.extents, navGroup.filter, out result);
if (result.polyRef == 0)
{
data.navStatus = navGroup.query.GetNearestPoint(
point.point , wideExtents, navGroup.filter , out result);
}
}
return result;
}
public static extern void dtpcReset(IntPtr corridor
, NavmeshPoint position);
/// <summary>
/// Finds a path from the start point to the end point and loads it into the corridor.
/// </summary>
/// <remarks>
/// <para>
/// <b>Warning</b>: The corridor must be available before calling this method.
/// </para>
/// <para>
/// The input points are all expected to be valid. (E.g. polyRef != 0)
/// </para>
/// </remarks>
/// <param name="start">
/// A valid navigation mesh point representing the start of the path.
/// </param>
/// <param name="end">
/// A valid navigation mesh point representing the end of the path.
/// </param>
/// <returns>The length of the path, or -1 on failure.</returns>
public int PlanCorridor(NavmeshPoint start, NavmeshPoint end)
{
// Note: Don't try to re-use the current corridor. Unlike
// paths, the corridor is more likely to become malformed
// over time.
int pathCount;
if (start.polyRef == 0 || end.polyRef == 0 || corridor == null)
return -1;
if (start.polyRef == end.polyRef)
{
corridor.Reset(start);
corridor.MoveTarget(end.point);
data.navStatus = NavStatus.Sucess;
return 1;
}
else
{
data.navStatus = navGroup.query.FindPath(start
, end
, navGroup.filter
, agentGroup.pathBuffer
, out pathCount);
}
corridor.Reset(start);
if (pathCount > 0)
{
corridor.SetCorridor(end.point
, agentGroup.pathBuffer
, pathCount);
}
return pathCount;
}
public static extern int dtpcMove(IntPtr corridor
, [In] ref Vector3 npos
, [In] ref Vector3 ntarget
, ref NavmeshPoint pos
, ref NavmeshPoint target
, [In, Out] Vector3[] cornerVerts
, [In, Out] WaypointFlag[] cornerFlags
, [In, Out] uint[] cornerPolys
, int maxCorners
, IntPtr navquery
, IntPtr filter);
/// <summary>
/// Gets a standard corridor from the pool. (Or creates one if none is available.)
/// </summary>
/// <remarks>
/// <para>
/// The corridor maximum path will equal <see cref="MaxPathSize"/>, and maximum corners
/// will equal <see cref="MaxStraightPathSize"/> .
/// </para>
/// </remarks>
/// <param name="query">The query to assign to the corridor.</param>
/// <param name="filter">The filter to assign to the corridor.</param>
/// <returns>A standard corridor, or null on error.</returns>
public PathCorridor GetCorridor(NavmeshPoint position, NavmeshQuery query, NavmeshQueryFilter filter)
{
if (mCorridors.Count > 0)
{
PathCorridor corr = mCorridors.Pop();
if (PathCorridor.LoadLocals(corr, position, query, filter))
return corr;
return null;
}
return new PathCorridor(mMaxPathSize, mMaxStraightPathSize, query, filter);
}
public static extern NavStatus dtqFindRandomPoint(IntPtr query
, IntPtr filter
, ref NavmeshPoint randomPt);
public static extern int dtpcSetCorridor(IntPtr corridor
, [In] ref Vector3 target
, [In] uint[] path
, int pathCount
, ref NavmeshPoint resultTarget
, [In, Out] Vector3[] cornerVerts
, [In, Out] WaypointFlag[] cornerFlags
, [In, Out] uint[] cornerPolys
, int maxCorners
, IntPtr navquery
, IntPtr filter);
public static extern NavStatus dtqGetPolyHeight(IntPtr query
, NavmeshPoint position
, ref float height);
public static extern NavStatus dtqInitSlicedFindPath(IntPtr query
, NavmeshPoint startPosition
, NavmeshPoint endPosition
, IntPtr filter);
public static extern NavStatus dtqRaycast(IntPtr query
, NavmeshPoint startPosition
, [In] ref Vector3 endPosition
, IntPtr filter
, ref float hitParameter
, ref Vector3 hitNormal
, [In, Out] uint[] path
, ref int pathCount
, int maxPath);
