/// <summary>
/// Provides a standard way detecting where the current
/// <see cref="hitPosition"/> is on the navigation mesh.
/// </summary>
public static SearchResult HandleStandardPolySearch(NavGroup helper
, out Vector3 geomPoint
, out NavmeshPoint navPoint
, out string message)
{
if (!hasHit)
{
message = "Outside source geometry.";
navPoint = new NavmeshPoint();
geomPoint = Vector3.zero;
return(SearchResult.Failed);
}
geomPoint = hitPosition;
NavStatus status =
helper.query.GetNearestPoint(geomPoint, helper.extents, helper.filter, out navPoint);
message = "GetNearestPoint: " + status.ToString();
if (NavUtil.Failed(status))
{
return(SearchResult.HitGeometry);
}
if (navPoint.polyRef == 0)
{
message = "Too far from navmesh: GetNearestPoint: " + status.ToString();
return(SearchResult.HitGeometry);
}
return(SearchResult.HitNavmesh);
}
/// <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);
}
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 float GetPathDistance(Vector3 start, Vector3 end)
* {
* List<Vector3> path = GeneratePath(start, end).;
* float distance = 0;
*
* for (int i = 0; i < path.Count - 1; i++)
* {
* distance += path[i].DistanceFrom(path[i + 1]);
* }
*
* return distance;
* }
*/
private oVector3[] StraightenPath(oVector3 start, oVector3 end, uint[] path, int pathCount)
{
oVector3[] straightPath = new oVector3[200];
int count = 0;
if (NavUtil.Failed(_query.GetStraightPath(start, end, path, 0, pathCount, straightPath, null, null, out count)))
{
throw new Exception("Failed to straighten path.");
}
return(straightPath.Take(count).ToArray());
}
public Pathfinder(Navmesh navMesh)
{
_navMesh = navMesh;
_filter = new NavmeshQueryFilter();
if (NavUtil.Failed(NavmeshQuery.Create(_navMesh, 1000, out _query)))
{
throw new Exception("NavQuery failed");
}
_pathCorridor = new PathCorridor(1000, 1000, _query, _filter);
}
public PathBuffer _FindPath(Vector3 start, Vector3 end, Vector3 extends, NavmeshQueryFilter filter = null)
{
var query = this.Query;
if (query == null)
{
return(null);
}
if (filter == null)
{
filter = this.DefaultQueryFilter;
}
NavmeshPoint startPoint;
var status = query.GetNearestPoint(start, extends, filter, out startPoint);
if (NavUtil.Failed(status))
{
return(null);
}
NavmeshPoint endPoint;
status = query.GetNearestPoint(end, extends, filter, out endPoint);
if (NavUtil.Failed(status))
{
return(null);
}
PathBuffer pathBuf = GetPathBuffer();
if (pathBuf == null || pathBuf.Buffer == null)
{
return(null);
}
try
{
int dataCount;
status = query.FindPath(ref startPoint, ref endPoint, extends, filter, pathBuf.Buffer, out dataCount);
if (NavUtil.Failed(status))
{
return(null);
}
pathBuf.SetDataCount(dataCount);
return(pathBuf);
} catch
{
StorePathBuffer(pathBuf);
}
return(null);
}
/// <summary>
/// Creates a new <see cref="Navmesh"/> object from the mesh data
/// </summary>
/// <returns>A new <see cref="Navmesh"/> object. Or null if the mesh is not available.</returns>
public Navmesh GetNavmesh()
{
if (!HasNavmesh)
{
return(null);
}
Navmesh result;
if (NavUtil.Failed(Navmesh.Create(mDataPack, out result)))
{
return(null);
}
return(result);
}
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);
}
public List <Vector3> GeneratePath(Vector3 start, Vector3 end)
{
List <Vector3> finalPath = new List <Vector3>();
if (NavUtil.Failed(GetNavMeshPoint(start, new oVector3(0.5f, 2, 0.5f), out NavmeshPoint origin)) || origin.point == new oVector3())
{
throw new PointNotOnNavMeshException(start);
}
if (NavUtil.Failed(GetNavMeshPoint(end, new oVector3(0.5f, 2, 0.5f), out NavmeshPoint destination)) || destination.point == new oVector3())
{
throw new PointNotOnNavMeshException(end);
}
uint[] path = new uint[500];
int pathCount;
if (origin.polyRef == destination.polyRef)
{
path[0] = origin.polyRef;
pathCount = 1;
}
else
{
NavStatus status = _query.FindPath(origin, destination, _filter, path, out pathCount);
if (NavUtil.Failed(status) || pathCount == 0)
{
Chat.WriteLine("FindPath failed?");
throw new Exception("FindPath failed: " + status);
}
else if (destination.polyRef != path[pathCount - 1])
{
//Chat.WriteLine("Unable to generate full path? " + status);
//throw new Exception("Unable to generate full path: " + status);
}
}
oVector3[] straightPath = StraightenPath(start.ToCAIVector3(), end.ToCAIVector3(), path, pathCount);
finalPath.AddRange(straightPath.Select(node => new Vector3(node.x, node.y, node.z)));
return(finalPath);
}
public PathCorridor GeneratePathCorridor(Vector3 start, Vector3 end)
{
if (NavUtil.Failed(GetNavMeshPoint(start, new oVector3(0.3f, 2, 0.3f), out NavmeshPoint origin)) || origin.point == new oVector3())
{
return(null);
}
if (NavUtil.Failed(GetNavMeshPoint(end, new oVector3(0.3f, 2, 0.3f), out NavmeshPoint destination)) || destination.point == new oVector3())
{
return(null);
}
uint[] path = new uint[250];
int pathCount;
if (origin.polyRef == destination.polyRef)
{
pathCount = 1;
path[0] = origin.polyRef;
}
else
{
NavStatus status = _query.FindPath(origin, destination, _filter, path, out pathCount);
if (NavUtil.Failed(status) || pathCount == 0)
{
// Handle pathfinding failure.
throw new Exception("path failed: " + status);
}
else if (destination.polyRef != path[pathCount - 1])
{
//Chat.WriteLine("Unable to generate full path: " + status);
//throw new Exception("Unable to generate full path: " + status);
//return null;
}
}
_pathCorridor.SetCorridor(end.ToCAIVector3(), path, pathCount);
_pathCorridor.Move(start.ToCAIVector3(), end.ToCAIVector3());
return(_pathCorridor);
}
private SearchResult NearestPoint(
NavGroup helper
, ref Vector3 geomPoint
, out NavmeshPoint navPoint
)
{
NavStatus status = helper.query.GetNearestPoint(hitPosition, helper.extents, helper.filter, out navPoint);
if (NavUtil.Failed(status))
{
return(SearchResult.HitGeometry);
}
if (navPoint.polyRef == 0)
{
return(SearchResult.HitGeometry);
}
return(SearchResult.HitNavmesh);
}
//创作一个新的直接路径，并且沿路获取点
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;
int iStart = FindPolyRef(0, start.polyRef);
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);
}
protected bool _GetGroudPt(Vector3 source, Vector3 extends, out Vector3 result, NavmeshQueryFilter filter = null)
{
result = source;
var query = this.Query;
if (query == null)
{
return(false);
}
NavmeshPoint navPoint;
if (filter == null)
{
filter = this.DefaultQueryFilter;
}
var status = query.GetNearestPoint(source, extends, filter, out navPoint);
if (NavUtil.Failed(status))
{
return(false);
}
result = navPoint.point;
return(true);
}
private void AddWanderAgent(Vector3 position)
{
if (mNav.crowd.AgentCount == mNav.crowd.MaxAgents)
{
return;
}
// Snap position to the navigation mesh.
NavmeshPoint navpos;
NavStatus status =
mNav.query.GetNearestPoint(position, mNav.extents, mNav.filter, out navpos);
if (NavUtil.Failed(status) || navpos.polyRef == 0)
{
return;
}
position = navpos.point;
// Create the base agent.
int selector = mNav.crowd.AgentCount % 3;
GameObject ao;
byte ag;
switch (selector)
{
case 0:
ao = (GameObject)Instantiate(agentA);
ag = agentGroupA;
break;
case 1:
ao = (GameObject)Instantiate(agentB);
ag = agentGroupB;
break;
default:
ao = (GameObject)Instantiate(agentC);
ag = agentGroupC;
break;
}
ao.name = agentNames[mNav.crowd.AgentCount % agentNames.Length];
// Position the agent.
Transform trans = ao.transform;
trans.position = position;
position.y = 0;
Vector3 campos = mCam.gameObject.transform.position;
campos.y = 0;
trans.rotation = Quaternion.LookRotation(campos - position);
// Add/Initialize the agent component.
CrowdDemoAgent agent = ao.GetComponent <CrowdDemoAgent>();
if (agent == null)
{
agent = ao.AddComponent <CrowdDemoAgent>();
}
agent.agentGroup = ag;
}
public void Update()
{
mHasPosition = false;
mHasHeight = false;
if (Input.GetKeyDown(StdButtons.SelectA))
{
Vector3 trash;
NavmeshPoint target;
QEUtil.SearchResult result =
QEUtil.HandleStandardPolySearch(mHelper, out trash, out target, out mMessage);
mHasPosition = (result & QEUtil.SearchResult.HitGeometry) != 0;
if ((result & QEUtil.SearchResult.HitNavmesh) == 0)
{
// Could not find polygon.
mPolyRef[0] = 0;
}
else if (target.polyRef == mPolyRef[0])
{
// Toggle off the selection.
mPolyRef[0] = 0;
}
else
{
// New good selection.
mPolyRef[0] = target.polyRef;
}
}
if (mPolyRef[0] == 0)
{
mMessage = "No polygon selected.";
return;
}
RaycastHit hit;
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
if (!Physics.Raycast(ray, out hit, 100))
{
mMessage = "Outside source geometry.";
return;
}
mPosition.point = hit.point;
mHasPosition = true;
NavStatus status = mHelper.query.GetPolyHeight(
new NavmeshPoint(mPolyRef[0], mPosition.point)
, out mHeight);
mMessage = "GetPolyHeight: " + status.ToString();
if (NavUtil.Failed(status))
{
return;
}
mHasHeight = true;
}
public NavManager CreateManager()
{
CheckCrowdAvoidanceSet();
if (!(mNavmeshData && NavmeshData.HasNavmesh))
{
Debug.LogError("Aborted initialization. Navigation mesh not available.");
return(null);
}
//Debug.Log("NavmeshData-------"+ NavmeshData);
Navmesh navmesh = NavmeshData.GetNavmesh();
if (navmesh == null)
{
NavStatus theStatus = Navmesh.Create(navMeshData, out navmesh);
Debug.Log("Navmesh.Create ---->" + theStatus + "---->" + (int)(theStatus & NavStatus.Sucess));
if (NavUtil.Failed(theStatus))
{
Debug.LogError("NavUtil.Failed(Navmesh.Create(navMeshData, out navmesh) Fail!");
}
Debug.Log("--------------------\n" + navMeshData + "---" + navMeshData.Length + "\n-----------------\nNavmesh-------" + navmesh);
}
if (navmesh == null)
{
Debug.LogError(" navmesh is null");
return(null);
}
NavmeshQuery query;
NavStatus status = NavmeshQuery.Create(navmesh, mMaxQueryNodes, out query);
if ((status & NavStatus.Sucess) == 0)
{
Debug.LogError(" Aborted initialization. Failed query creation: " + status.ToString());
return(null);
}
CrowdManager crowd = CrowdManager.Create(mMaxCrowdAgents, mMaxAgentRadius, navmesh);
if (crowd == null)
{
Debug.LogError("Aborted initialization. Failed crowd creation.");
return(null);
}
for (int i = 0; i < CrowdManager.MaxAvoidanceParams; i++)
{
crowd.SetAvoidanceConfig(i, CrowdAvoidanceConfig[i]);
}
NavGroup mGroup = new NavGroup(navmesh, query, crowd, crowd.QueryFilter, mExtents, false);
int count = AgentGroupSettingManager.GetGroupCount();
Dictionary <byte, NavAgentGroups> mAgentGroups = new Dictionary <byte, NavAgentGroups>(count);
for (int i = 0; i < count; i++)
{
byte groupId;
NavAgentGroups group = AgentGroupSettingManager.CreateAgentGroup(i, mMaxPath, mMaxStraightPath, out groupId);
group.angleAt = mAngleAt;
group.heightTolerance = mHeightTolerance;
group.turnThreshold = mTurnThreshold;
mAgentGroups.Add(groupId, group);
}
return(NavManager.Create(mMaxAgents, mGroup, mAgentGroups));
}