public static FindPathResult <T> FindPath <T>(iExplorer <T> explorer, T start, List <T> goal)
where T : PathNodeFPQN
{
var result = new FindPathResult <T>(c_MaxElementsInQueue, c_MaxElementsInQueue);
if (start == null || goal == null || explorer == null)
{
return(result);
}
var closedSet = result.m_ClosedSet;
var openSet = result.m_OpenSet;
start.PathLengthFromStart = 0.0f;
start.PathLengthEstimated = implGetShortestPossiblePath(explorer, start, goal);
openSet.Enqueue(start, 0);
while (closedSet.Count < c_MaxElementsInQueue && openSet.Count > 0)
{
var currentNode = openSet.Dequeue();
foreach (var n in goal)
{
if (currentNode == n)
{
implGetPathForNode <T>(currentNode, result.m_Path);
return(result);
}
}
closedSet.Enqueue(currentNode, currentNode.PathLengthEstimated + currentNode.PathLengthFromStart);
foreach (var neighbourNode in explorer.iGetNeighbours(currentNode as T))
{
if (closedSet.Contains(neighbourNode)) // skip if already checked
{
continue;
}
var pathLengthFromStart = currentNode.PathLengthFromStart + explorer.iGetPathCost(currentNode as T, neighbourNode);
if (openSet.Contains(neighbourNode))
{ // if presented and part is shorter then reset his parent and cost
if (neighbourNode.PathLengthFromStart > pathLengthFromStart)
{
neighbourNode.СameFrom = currentNode;
neighbourNode.PathLengthFromStart = pathLengthFromStart;
openSet.UpdatePriority(neighbourNode, neighbourNode.PathLengthEstimated + neighbourNode.PathLengthFromStart);
}
}
else
{ // if not presented add as wariant
neighbourNode.СameFrom = currentNode;
neighbourNode.PathLengthFromStart = pathLengthFromStart;
neighbourNode.PathLengthEstimated = implGetShortestPossiblePath(explorer, neighbourNode as T, goal);
openSet.Enqueue(neighbourNode, neighbourNode.PathLengthEstimated + neighbourNode.PathLengthFromStart);
}
}
}
return(null);
}
/// <summary>
/// Attempts to optimize the path using a local area search. (Partial
/// replanning.)
///
/// Inaccurate locomotion or dynamic obstacle avoidance can force the agent
/// position significantly outside the original corridor. Over time this can
/// result in the formation of a non-optimal corridor. This function will use
/// a local area path search to try to re-optimize the corridor.
///
/// The more inaccurate the agent movement, the more beneficial this function
/// becomes. Simply adjust the frequency of the call to match the needs to
/// the agent.
/// </summary>
/// <param name="navquery"> The query object used to build the corridor. </param>
/// <param name="filter"> The filter to apply to the operation.
/// </param>
internal virtual bool optimizePathTopology(NavMeshQuery navquery, QueryFilter filter)
{
if (m_path.Count < 3)
{
return(false);
}
const int MAX_ITER = 32;
navquery.initSlicedFindPath(m_path[0], m_path[m_path.Count - 1], m_pos, m_target, filter, 0);
navquery.updateSlicedFindPath(MAX_ITER);
FindPathResult fpr = navquery.finalizeSlicedFindPathPartial(m_path);
if (fpr.Status.Success && fpr.Refs.Count > 0)
{
m_path = mergeCorridorStartShortcut(m_path, fpr.Refs);
return(true);
}
return(false);
}
/// <summary>
/// Retourne le chemin complet de la route et domaine qui match
/// </summary>
/// <param name="input"></param>
/// <returns></returns>
public async Task <FindPathResult> FindDomainPathAsync(FindPathInput input)
{
var findPathResult = new FindPathResult();
findPathResult.IsSuccess = false;
if (_routeProvider.Domains == null)
{
return(findPathResult);
}
var datas = new Dictionary <string, string>();
var datasDomain = new Dictionary <string, string>();
foreach (var data in input.Datas)
{
var key = data.Key;
datas.Add(key, data.Value);
}
//Pour les données de nom de domaine, on persite les valeurs s'il nont pas été redéfinie
if (input.DomainDatas != null)
{
foreach (var domainData in input.DomainDatas)
{
var key = domainData.Key;
if (!datasDomain.ContainsKey(key))
{
datasDomain.Add(key, domainData.Value.ToString(CultureInfo.InvariantCulture));
}
}
}
var domainMasterId = input.MasterDomainId;
if (string.IsNullOrEmpty(domainMasterId))
{
domainMasterId = (from d in _routeProvider.Domains
where input.DomainId == d.Id
select d.DomainMasterId).FirstOrDefault();
}
if (string.IsNullOrEmpty(domainMasterId))
{
return(findPathResult);
}
Route route;
var routePath = GetRoutePath(_routeProvider, datas, input.DomainDatas, _logger, out route);
Domain domain;
var isSecure = false;
if (!string.IsNullOrEmpty(input.DomainId))
{
domain = (from d in _routeProvider.Domains
where
d.Id == input.DomainId
select d).FirstOrDefault();
isSecure = domain.SecureMode == SecureMode.Secure;
if (domain == null)
{
return(findPathResult);
}
}
else
{
if (input.IsSecure.HasValue)
{
var secureMode = input.IsSecure.Value ? SecureMode.Secure : SecureMode.NoSecure;
isSecure = input.IsSecure.Value;
domain = (from d in _routeProvider.Domains
where
d.DomainMasterId == domainMasterId && d.SecureMode == secureMode &&
IsSameDomainType(datasDomain, d)
select d).FirstOrDefault();
if (domain == null)
{
return(findPathResult);
}
}
else
{
domain = (from d in _routeProvider.Domains
where
d.DomainMasterId == domainMasterId &&
IsSameDomainType(datasDomain, d)
select d).FirstOrDefault();
if (domain == null)
{
return(findPathResult);
}
isSecure = domain.SecureMode == SecureMode.Secure;
}
}
var domainPath = GetPath(domain.Path, datasDomain, null);
var domainSplitted = domainPath.Split('/');
var domainLenght = domainSplitted.Length;
if (domainLenght <= 0)
{
return(findPathResult);
}
var requestDomain = domainSplitted[0];
var preUrl = domainPath.ReplaceIgnoreCase(requestDomain, string.Empty);
var applicationPath = string.Empty;
if (datasDomain.ContainsKey("application_path"))
{
applicationPath = UrlHelper.RemoveSeparator(datasDomain["application_path"]);
preUrl = UrlHelper.RemoveSeparator(preUrl.ReplaceFirstIgnoreCase(applicationPath, string.Empty));
}
findPathResult.RoutePathWithoutHomePage = routePath;
// Gestion de la page d'accueil
var isHomePage = await IsHomePage(domain, datasDomain, datas);
if (isHomePage)
{
routePath = string.Empty;
}
var path = GetPath(isSecure, requestDomain, applicationPath, preUrl, input.Port,
routePath, _routeProvider.ProtocolSecure);
findPathResult.PreUrl = preUrl;
findPathResult.RequestDomain = requestDomain;
findPathResult.Path = path;
findPathResult.Route = route;
findPathResult.IsSuccess = true;
findPathResult.RoutePath = routePath;
var protocole = isSecure ? _routeProvider.ProtocolSecure : _routeProvider.ProtocolDefault;
findPathResult.FullUrl = string.Concat(protocole, "://", UrlHelper.Concat(requestDomain, path));
findPathResult.BaseUrl = string.Concat(protocole, "://", requestDomain);
findPathResult.IsSecure = isSecure;
return(findPathResult);
}
public static extern IntPtr navmesh_query_get_smooth_path(IntPtr startPos, DtPolyRef startRef, IntPtr endPos, ref FindPathResult path, IntPtr filter, IntPtr navMesh, IntPtr navQuery);
public static FindPathResult <T> FindPath <T>(this iExplorer <T> explorer, T start, T goal, CancellationToken cancellationToken, int maxChecks = c_MaxChecks, int bufferSize = c_BufferSize)
{
var result = new FindPathResult <T>();
var startNode = new PathNode <T>(start);
if (start == null || goal == null || explorer == null || explorer.iReachable(start, goal) == false)
{
return(result);
}
var closedSet = result.m_ClosedSet;
var openSet = new FastPriorityQueue <PathNode <T> >(bufferSize);
result.m_OpenSet = openSet;
startNode.СameFrom = null;
startNode.PathCost = 0.0f;
startNode.PathCostEstimated = explorer.iGetShortestPossiblePath(start, goal);
startNode.Cost = 0.0f;
openSet.Enqueue(startNode, startNode.Cost);
// do while has variants
while (openSet.Count > 0 && closedSet.Count < maxChecks && openSet.Count < bufferSize)
{
// cancellation check
if (cancellationToken.IsCancellationRequested)
{
return(result);
}
// get next node
var currentNode = openSet.First();
openSet.Remove(currentNode);
// goal check
if (currentNode.Master.Equals(goal))
{
implGetPathForNode(currentNode, result.m_Path);
return(result);
}
// close current
closedSet.Add(currentNode.Master);
// proceed connections
foreach (var neighborNode in explorer.iGetNeighbours(currentNode.Master))
{
if (closedSet.Contains(neighborNode)) // skip if already checked
{
continue;
}
var pathCost = currentNode.PathCost + explorer.iGetPathCost(currentNode.Master, neighborNode);
// can use Dictionary instead FirstOrDefault
var openNode = openSet.FirstOrDefault(n => n.Master.Equals(neighborNode));
if (openNode != null)
{ // if presented and part is shorter then reset his parent and cost
if (openNode.PathCost > pathCost)
{
openNode.СameFrom = currentNode;
openNode.PathCost = pathCost;
// update priority
openNode.Cost = openNode.PathCostEstimated + openNode.PathCost;
openSet.UpdatePriority(openNode, openNode.Cost);
}
}
else
{ // if not presented add as variant
var pathNode = new PathNode <T>(neighborNode);
pathNode.СameFrom = currentNode;
pathNode.PathCost = pathCost;
pathNode.PathCostEstimated = explorer.iGetShortestPossiblePath(pathNode.Master, goal);
pathNode.Cost = pathNode.PathCostEstimated + pathNode.PathCost;
openSet.Enqueue(pathNode, pathNode.Cost);
}
}
}
return(result);
}
public SmoothPathResult FindSmoothPath(NavMeshQuery navMeshQuery, NavMesh navMesh, FindPathResult pathResult, PolyPointResult a, PolyPointResult b)
{
var filter = RecastLibrary.dtQueryFilter_create();
var aPointer = Marshal.AllocHGlobal(3 * 4);
Marshal.Copy(a.point, 0, aPointer, 3);
var bPointer = Marshal.AllocHGlobal(3 * 4);
Marshal.Copy(b.point, 0, bPointer, 3);
var pathResultPointer = RecastLibrary.navmesh_query_get_smooth_path(aPointer, a.polyRef, bPointer,
ref pathResult, filter, navMesh.DangerousGetHandle(), navMeshQuery.DangerousGetHandle());
Marshal.FreeHGlobal(aPointer);
Marshal.FreeHGlobal(bPointer);
RecastLibrary.dtQueryFilter_delete(filter);
var smoothPathResult = Marshal.PtrToStructure(pathResultPointer, typeof(SmoothPathResult));
RecastLibrary.smooth_path_result_delete(pathResultPointer);
return((SmoothPathResult)smoothPathResult);
}