public bool GeneratePolyPath(Vector3 startPos, Vector3 endPos)
{
polyResult.Initialize();
NavmeshPoint startPoint;
NavmeshPoint endPoint;
NavStatus status0 = query.GetNearestPoint(startPos, Vector3.one, mCrowdManager.QueryFilter, out startPoint);
NavStatus status1 = query.GetNearestPoint(endPos, Vector3.one, mCrowdManager.QueryFilter, out endPoint);
NavStatus status = query.FindPath(startPoint, endPoint
, mCrowdManager.QueryFilter, polyResult
, out polyResultCount);
//Debug.Log("GenerateNavPathResult:" + startPoint.point.ToString() + "~" + endPoint.point.ToString() + ":" + polyResultCount.ToString());
//string result = "";
//for (int i = 0; i < polyResultCount; i++)
//{
// result += polyResult[i].ToString() + ";";
//}
//Debug.Log(result);
if ((status & NavStatus.Sucess) == 0)
{
return(false);
}
return(true);
}
public static Pathfinder Create(string filePath)
{
BinaryFormatter formatter = new BinaryFormatter();
FileStream stream = null;
if (!File.Exists(filePath))
{
return(null);
}
try
{
stream = new FileStream(filePath, FileMode.Open, FileAccess.Read, FileShare.Read);
NavStatus status = Navmesh.Create((byte[])formatter.Deserialize(stream), out Navmesh navMesh);
if (status == NavStatus.Sucess)
{
return(new Pathfinder(navMesh));
}
}
finally
{
if (stream != null)
{
stream.Close();
}
}
return(null);
}
public Vector3 GetNearestPoint(Vector3 position)
{
NavmeshPoint point;
NavStatus status0 = query.GetNearestPoint(position, Vector3.one, mCrowdManager.QueryFilter, out point);
return(point.point);
}
private static NavStatus UnsafeCreate(byte[] serializedMesh
, bool safeStorage
, out Navmesh resultMesh)
{
if (serializedMesh == null || serializedMesh.Length == 0)
{
resultMesh = null;
return(NavStatus.Failure | NavStatus.InvalidParam);
}
IntPtr root = IntPtr.Zero;
NavStatus status = NavmeshEx.dtnmBuildDTNavMeshFromRaw(serializedMesh
, serializedMesh.Length
, safeStorage
, ref root);
if (NavUtil.Succeeded(status))
{
resultMesh = new Navmesh(root);
}
else
{
resultMesh = null;
}
return(status);
}
/// <summary>
/// Load a navigation mesh from data created from the <see cref="Navmesh.GetSerializedMesh"/>
/// method.
/// </summary>
/// <param name="serializedMesh">The serialized mesh.</param>
/// <param name="buildConfig">The build information. (Optional)</param>
/// <returns>The <see cref="NavStatus"/> flags for the operation.</returns>
public NavStatus Load(byte[] serializedMesh, NavmeshBuildInfo buildConfig)
{
if (serializedMesh == null)
{
return(NavStatus.Failure | NavStatus.InvalidParam);
}
// This roundabout method is used for validation.
Navmesh navmesh;
NavStatus status = Navmesh.Create(serializedMesh, out navmesh);
if ((status & NavStatus.Sucess) == 0)
{
return(status);
}
mDataPack = navmesh.GetSerializedMesh();
if (mDataPack == null)
{
return(NavStatus.Failure);
}
mBuildInfo = buildConfig;
mVersion++;
return(NavStatus.Sucess);
}
//从起点到终点的寻路
//如果没找到路就返回-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>
/// Gets a polygon and its tile.
/// </summary>
/// <param name="polyRef">The reference of the polygon.</param>
/// <param name="tile">The tile the polygon belongs to.</param>
/// <param name="poly">The polygon.</param>
/// <returns>The <see cref="NavStatus"/> flags for the operation.</returns>
public NavStatus GetTileAndPoly(uint polyRef
, out NavmeshTile tile
, out NavmeshPoly poly)
{
IntPtr pTile = IntPtr.Zero;
IntPtr pPoly = IntPtr.Zero;
NavStatus status = NavmeshEx.dtnmGetTileAndPolyByRef(root
, polyRef
, ref pTile
, ref pPoly);
if (NavUtil.Succeeded(status))
{
tile = new NavmeshTile(this, pTile);
poly = (NavmeshPoly)Marshal.PtrToStructure(pPoly
, typeof(NavmeshPoly));
}
else
{
tile = null;
poly = new NavmeshPoly();
}
return(status);
}
/// <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);
}
//使用现有路径从起点到目标点建立标准的直接路径
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);
}
/// <summary>
/// Loads a single-tile navigation mesh from the provided data.
/// </summary>
/// <param name="buildData">The tile build data.</param>
/// <param name="buildConfig">The build information. (Optional)</param>
/// <returns>The <see cref="NavStatus"/> flags for the operation.</returns>
public NavStatus Load(NavmeshTileBuildData buildData, NavmeshBuildInfo buildConfig)
{
if (buildData == null || buildData.IsDisposed)
{
return(NavStatus.Failure | NavStatus.InvalidParam);
}
Navmesh navmesh;
NavStatus status = Navmesh.Create(buildData, out navmesh);
if ((status & NavStatus.Sucess) == 0)
{
return(status);
}
mDataPack = navmesh.GetSerializedMesh();
if (mDataPack == null)
{
return(NavStatus.Failure);
}
mBuildInfo = buildConfig;
mVersion++;
return(NavStatus.Sucess);
}
private void CastRay()
{
NavStatus status = mHelper.query.Raycast(mPosition, mGoal.point
, mHelper.filter
, out mHitParam, out mHitNorm, mPolyRefs.buffer, out mResultCount);
mMessages = "Raycast: " + status.ToString();
}
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);
}
}
}
private void GeneratePath()
{
NavStatus status = helper.query.FindPath(position, goal
, helper.filter, path.buffer
, out pathCount);
pathID++;
mLocalMessage = "FindPath: " + status.ToString();
}
public void Create(Navmesh navmesh)
{
this._navmesh = navmesh; //一定要保存起来,Navmesh的析构函数会把非托管内存的指针清掉!!
NavStatus status = NavmeshQuery.Create(navmesh, 2048, out this._query);
if (status != NavStatus.Sucess)
{
LLogger.Error(status);
}
}
/// <summary>
/// 选择按钮点击
/// </summary>
/// <param name="btn">按钮</param>
public void SelectButtonClicked(Transform btn)
{
navStatus = NavStatus.navigation;
lineRenderer.gameObject.SetActive(true);
BakePath();
CancelInvoke("DisplayPath");
target = btn.GetComponent <SelectButton>().target;
InvokeRepeating("DisplayPath", 0, refresh);
Close();
}
public Vec3 SampleNavPosition(Vec3 searchPoint)
{
NavStatus status = this._pathManager.GetNearestPoint(searchPoint, out Vec3 resultPoint);
if (status != NavStatus.Sucess)
{
LLogger.Warning(status);
}
return(resultPoint);
}
private void FindPath()
{
NavStatus status = mGroup.query.FindPath(
mPathStart, mPathEnd, mGroup.filter, mPath.buffer, out mPathCount);
mCorridor.Reset(mPathStart);
if (NavUtil.Succeeded(status) && mPathCount > 0)
{
mCorridor.SetCorridor(mPathEnd.point, mPath.buffer, mPathCount);
}
}
//调用底层的NavmeshQuery（导航网格查询）进行查询，这样就将设定的点与导航网格的点联系起来
//如果该点没有多边形引用，则对其执行加宽搜索
public NavmeshPoint GetPointSearch(Vector3 point)
{
NavmeshPoint result;
navStatus = navGroup.query.GetNearestPoint(point, navGroup.extents, navGroup.filter, out result);
if (result.polyRef == 0)
{
navStatus = navGroup.query.GetNearestPoint(point, wideExtents, navGroup.filter, out result);
}
return(result);
}
private uint[] polyResultBuffer = null; //缓存寻路的三角形
public bool loadNavMesh(string fileName)
{
NavStatus status = loadMeshData(fileName);
if ((status & NavStatus.Sucess) == 0)
{
Debug.LogError(fileName + "Load failed" + status.ToString());
mCrowdManager = null;
return(false);
}
return(true);
}
public bool NavMeshRaycast(Vec3 src, Vec3 dest, out Vec3 hitPosition, out Vec3 hitNormal)
{
NavStatus status = this._pathManager.Raycast(src, dest, out float hitParameter, out hitNormal);
if (status != NavStatus.Sucess)
{
LLogger.Warning(status);
}
hitPosition = src + (dest - src) * hitParameter;
//True if the ray is terminated before reaching target position. Otherwise returns false.
return(hitParameter < 1);
}
void Start()
{
game = FindObjectOfType <GameController>();
lineRenderer.gameObject.SetActive(false);
LoadObjects();
LoadTarget();
LoadRoad();
Close();
navStatus = NavStatus.wating;
btnNav.interactable = false;
LoadMap();
}
public NavStatus GetNearestPoint(Vec3 searchPoint, out Vec3 vec3)
{
NavStatus status = this._query.GetNearestPoint(searchPoint.ToVector3(), DEFAULT_SEARCH_EXTENTS, QUERY_FILTER,
out NavmeshPoint startNavmeshPoint);
if (status != NavStatus.Sucess)
{
vec3 = Vec3.zero;
return(status);
}
vec3 = startNavmeshPoint.point.ToVec3();
return(NavStatus.Sucess);
}
public void PayloadConversion_toBits(
string payload, int userId, decimal sog, NavStatus navStatus, decimal lng, decimal lat, decimal cog)
{
var packet = _sut.ParsePayload(payload) as MessageType1;
Assert.Equal(168, packet.BitVector.Length);
Assert.Equal(userId, packet.UserId);
Assert.Equal(sog, packet.SOG);
Assert.Equal(navStatus, packet.NavigationalStatus);
Assert.Equal(lng, Math.Round(packet.Longitude, 5));
Assert.Equal(lat, Math.Round(packet.Latitude, 5));
Assert.Equal(cog, packet.COG);
}
private void HandleBake()
{
const string Category = "Bake To Target";
NavmeshBuild build = Context.Build; // Caller checks for null.
TileBuildData tdata = build.BuildData;
// Double check.
if (tdata.BakeableCount() == 0)
{
Debug.LogWarning(Category + ": No tiles were produced. (All tiles empty?)", build);
return;
}
if (Context.TaskCount > 0)
{
Debug.LogWarning(Category + ": There are in-progress background builds."
+ " The tiles associated with these builds will not be baked."
+ " In-progress builds: " + Context.TaskCount
, build);
}
NavmeshParams nconfig;
NavmeshTileData[] tiles;
bool success = tdata.GetMeshBuildData(build.TileSetDefinition.BoundsMin
, build.TileSetDefinition.TileWorldSize
, out nconfig, out tiles);
if (!success)
{
Logger.PostError("Bake to target: Error creating navigation mesh from build data."
, Context.Build);
return;
}
NavStatus status =
build.BuildTarget.Load(nconfig, tiles, NMBEditorUtil.GetConfig(build));
if ((status & NavStatus.Failure) == 0)
{
build.BuildData.SetAsBaked();
EditorUtility.SetDirty((Object)build.BuildTarget);
}
else
{
Logger.PostError("Bake to target: Target reported failure."
, (Object)Context.Build.BuildTarget);
}
}
private void GenerateStraightPath()
{
int iTarget = Mathf.Max(0, mTargetPathIndex);
NavStatus status = helper.query.GetStraightPath(mTarget.point, goal.point
, path.buffer, iTarget, pathCount - iTarget
, mStraightPath.buffer, null, null, out mStraightCount);
if (mLocalMessage.Length > 0)
{
mLocalMessage += ", ";
}
mLocalMessage = "GetStraightPath: " + status.ToString();
}
public void Update()
{
// Must set message before returning.
mHasPosition = false;
mResultCount = 0;
mYOffset += QEUtil.GetYFactor();
Vector3 trash;
QEUtil.SearchResult result =
QEUtil.HandleStandardPolySearch(mHelper, out trash, out mPosition, out mMessage);
mHasPosition = (result & QEUtil.SearchResult.HitNavmesh) != 0;
if (!mHasPosition)
{
return;
}
Vector3 pos = mPosition.point;
for (int i = 0; i < mSearchPoly.Length; i++)
{
mSearchPoly[i] = pos + mBasePoly[i];
mSearchPoly[i].y += mYOffset;
}
if (mPolyRefs.HandleResize())
{
int size = mPolyRefs.MaxElementCount;
mParentRefs = new uint[size];
mCosts = new float[size];
mCentroids = new Vector3[size];
}
NavStatus status = mHelper.query.FindPolys(mPosition.polyRef, mSearchPoly
, mHelper.filter
, mPolyRefs.buffer, mParentRefs, mCosts, out mResultCount);
mMessage = "FindPolys: " + status.ToString() + ".";
if (mResultCount > 0)
{
NavDebug.GetCentroids(mHelper.mesh, mPolyRefs.buffer, mResultCount, mCentroids);
}
}
/// <summary>
/// Loads a navigation mesh.
/// </summary>
/// <param name="config">The mesh configuration.</param>
/// <param name="tiles">The tiles to add to the mesh.</param>
/// <param name="buildConfig">The build information. (Optional)</param>
/// <returns>The <see cref="NavStatus"/> flags for the operation.</returns>
public NavStatus Load(NavmeshParams config
, NavmeshTileData[] tiles
, NavmeshBuildInfo buildConfig)
{
if (config == null || tiles == null || tiles.Length > config.maxTiles)
{
return(NavStatus.Failure | NavStatus.InvalidParam);
}
Navmesh navmesh;
NavStatus status = Navmesh.Create(config, out navmesh);
if ((status & NavStatus.Sucess) == 0)
{
return(status);
}
foreach (NavmeshTileData tile in tiles)
{
if (tile == null)
{
continue;
}
uint trash;
status = navmesh.AddTile(tile, Navmesh.NullTile, out trash);
if ((status & NavStatus.Sucess) == 0)
{
return(status | NavStatus.InvalidParam);
}
}
mDataPack = navmesh.GetSerializedMesh();
if (mDataPack == null)
{
return(NavStatus.Failure);
}
mBuildInfo = buildConfig;
mVersion++;
return(NavStatus.Sucess);
}
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);
}
private static bool LoadMesh(CAIBakedNavmesh targ, string filePath)
{
string msg = null;
if (filePath.Length == 0)
{
return(false);
}
FileStream fs = null;
BinaryFormatter formatter = new BinaryFormatter();
try
{
fs = new FileStream(filePath, FileMode.Open);
System.Object obj = formatter.Deserialize(fs);
NavStatus status = targ.Load((byte[])obj, null);
if ((status & NavStatus.Sucess) == 0)
{
msg = status.ToString();
}
}
catch (System.Exception ex)
{
msg = ex.Message;
}
finally
{
if (fs != null)
{
fs.Close();
}
}
if (msg != null)
{
Debug.LogError(targ.name + ": BakedNavmesh: Load bytes failed: "
+ msg);
return(false);
}
return(true);
}
/// <summary>
/// Creates a single-tile navigation mesh.
/// </summary>
/// <param name="buildData">The tile build data.</param>
/// <param name="resultMesh">The result mesh.</param>
/// <returns>The <see cref="NavStatus"/> flags for the operation.
/// </returns>
public static NavStatus Create(NavmeshTileBuildData buildData
, out Navmesh resultMesh)
{
IntPtr navMesh = IntPtr.Zero;
NavStatus status = NavmeshEx.dtnmBuildSingleTileMesh(buildData
, ref navMesh);
if (NavUtil.Succeeded(status))
{
resultMesh = new Navmesh(navMesh);
}
else
{
resultMesh = null;
}
return(status);
}
public static void CheckReturn (NavStatus status)
{
CheckReturn (status);
}
/// <summary>
/// Returns true if the status includes the in-progress flag.
/// </summary>
/// <param name="status">The status to check.</param>
/// <returns>True if the status includes the in-progress flag.
/// </returns>
public static bool IsInProgress(NavStatus status)
{
return (status & NavStatus.InProgress) != 0;
}
/// <summary>
/// Returns true if the status includes the success flag.
/// </summary>
/// <param name="status">The status to check.</param>
/// <returns>True if the status includes the success flag.</returns>
public static bool Succeeded(NavStatus status)
{
return (status & NavStatus.Sucess) != 0;
}
/// <summary>
/// Returns true if the status includes the failure flag.
/// </summary>
/// <param name="status">The status to check.</param>
/// <returns>True if the status includes the failure flag.</returns>
public static bool Failed(NavStatus status)
{
return (status & NavStatus.Failure) != 0;
}
