BVB Source Codes

CRYENGINE Show MNMPathfinder.cpp Source code

Return Download CRYENGINE: download MNMPathfinder.cpp Source code - Download CRYENGINE Source code - Type:.cpp
  1. // Copyright 2001-2016 Crytek GmbH / Crytek Group. All rights reserved.
  2.  
  3. #include "StdAfx.h"
  4.  
  5. #include "MNMPathfinder.h"
  6. #include "Navigation/NavigationSystem/NavigationSystem.h"
  7. #include "DebugDrawContext.h"
  8. #include "AIBubblesSystem/AIBubblesSystem.h"
  9. #include "Navigation/PathHolder.h"
  10. #include <CryThreading/IJobManager_JobDelegator.h>
  11.  
  12. //#pragma optimize("", off)
  13. //#pragma inline_depth(0)
  14.  
  15. inline Vec3 TriangleCenter(const Vec3& a, const Vec3& b, const Vec3& c)
  16. {
  17.         return (a + b + c) / 3.f;
  18. }
  19.  
  20. //////////////////////////////////////////////////////////////////////////
  21.  
  22. void MNM::PathfinderUtils::QueuedRequest::SetupDangerousLocationsData()
  23. {
  24.         FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  25.  
  26.         dangerousAreas.clear();
  27.         if (requestParams.dangersToAvoidFlags == eMNMDangers_None)
  28.                 return;
  29.  
  30.         // The different danger types need a different setup
  31.         if (requestParams.dangersToAvoidFlags & eMNMDangers_AttentionTarget)
  32.         {
  33.                 SetupAttentionTargetDanger();
  34.         }
  35.  
  36.         if (requestParams.dangersToAvoidFlags & eMNMDangers_Explosive)
  37.         {
  38.                 SetupExplosiveDangers();
  39.         }
  40.  
  41.         if (requestParams.dangersToAvoidFlags & eMNMDangers_GroupMates)
  42.         {
  43.                 SetupGroupMatesAvoidance();
  44.         }
  45. }
  46.  
  47. void MNM::PathfinderUtils::QueuedRequest::SetupAttentionTargetDanger()
  48. {
  49.         if (dangerousAreas.size() >= MNM::max_danger_amount)
  50.                 return;
  51.  
  52.         if (pRequester)
  53.         {
  54.                 CAIActor* pActor = CastToCAIActorSafe(pRequester->GetPathAgentEntity()->GetAI());
  55.                 if (pActor)
  56.                 {
  57.                         IAIObject* pAttTarget = pActor->GetAttentionTarget();
  58.                         if (pAttTarget)
  59.                         {
  60.                                 const IEntity* pEntity = pAttTarget->GetEntity();
  61.                                 const float effectRange = 0.0f; // Effect over all the world
  62.                                 const Vec3& dangerPosition = pEntity ? pEntity->GetPos() : pAttTarget->GetPos();
  63.                                 MNM::DangerAreaConstPtr info;
  64.                                 info.reset(new MNM::DangerAreaT<MNM::eWCT_Direction>(dangerPosition, effectRange, gAIEnv.CVars.PathfinderDangerCostForAttentionTarget));
  65.                                 dangerousAreas.push_back(info);
  66.                         }
  67.                 }
  68.         }
  69. }
  70. // Predicate to find if an explosive danger is already in the list of the stored dangers
  71. struct DangerAlreadyStoredInRangeFromPositionPred
  72. {
  73.         DangerAlreadyStoredInRangeFromPositionPred(const Vec3& _pos, const float _rangeSq)
  74.                 : pos(_pos)
  75.                 , rangeSq(_rangeSq)
  76.         {}
  77.  
  78.         bool operator()(const MNM::DangerAreaConstPtr dangerInfo)
  79.         {
  80.                 return Distance::Point_PointSq(dangerInfo->GetLocation(), pos) < rangeSq;
  81.         }
  82.  
  83. private:
  84.         const Vec3  pos;
  85.         const float rangeSq;
  86. };
  87. #undef GetObject
  88. void MNM::PathfinderUtils::QueuedRequest::SetupExplosiveDangers()
  89. {
  90.         const float maxDistanceSq = sqr(gAIEnv.CVars.PathfinderExplosiveDangerMaxThreatDistance);
  91.         const float maxDistanceSqToMergeExplosivesThreat = 3.0f;
  92.  
  93.         std::vector<Vec3> grenadesLocations;
  94.         AutoAIObjectIter grenadesIterator(gAIEnv.pAIObjectManager->GetFirstAIObject(OBJFILTER_TYPE, AIOBJECT_GRENADE));
  95.  
  96.         while (dangerousAreas.size() < MNM::max_danger_amount && grenadesIterator->GetObject())
  97.         {
  98.                 const Vec3& pos = grenadesIterator->GetObject()->GetPos();
  99.                 if (Distance::Point_Point2DSq(pos, pRequester->GetPathAgentEntity()->GetPos()) < maxDistanceSq)
  100.                 {
  101.                         MNM::DangerousAreasList::const_iterator it = std::find_if(dangerousAreas.begin(), dangerousAreas.end(),
  102.                                                                                   DangerAlreadyStoredInRangeFromPositionPred(pos, maxDistanceSqToMergeExplosivesThreat));
  103.                         if (it == dangerousAreas.end())
  104.                         {
  105.                                 MNM::DangerAreaConstPtr info;
  106.                                 info.reset(new MNM::DangerAreaT<MNM::eWCT_Range>(pos, gAIEnv.CVars.PathfinderExplosiveDangerRadius,
  107.                                                                                  gAIEnv.CVars.PathfinderDangerCostForExplosives));
  108.                                 dangerousAreas.push_back(info);
  109.                         }
  110.                 }
  111.  
  112.                 grenadesIterator->Next();
  113.         }
  114. }
  115.  
  116. void MNM::PathfinderUtils::QueuedRequest::SetupGroupMatesAvoidance()
  117. {
  118.         IF_UNLIKELY (!pRequester)
  119.         {
  120.                 return;
  121.         }
  122.  
  123.         IAIObject* requesterAIObject = pRequester->GetPathAgentEntity()->GetAI();
  124.         IF_UNLIKELY (!requesterAIObject)
  125.         {
  126.                 return;
  127.         }
  128.  
  129.         const int requesterGroupId = requesterAIObject->GetGroupId();
  130.         AutoAIObjectIter groupMemberIterator(gEnv->pAISystem->GetAIObjectManager()->GetFirstAIObject(OBJFILTER_GROUP, requesterGroupId));
  131.         while (dangerousAreas.size() < MNM::max_danger_amount && groupMemberIterator->GetObject())
  132.         {
  133.                 IAIObject* groupMemberAIObject = groupMemberIterator->GetObject();
  134.                 if (requesterAIObject->GetEntityID() != groupMemberAIObject->GetEntityID())
  135.                 {
  136.                         MNM::DangerAreaConstPtr dangerArea;
  137.                         dangerArea.reset(new MNM::DangerAreaT<MNM::eWCT_Range>(
  138.                                            groupMemberAIObject->GetPos(),
  139.                                            gAIEnv.CVars.PathfinderGroupMatesAvoidanceRadius,
  140.                                            gAIEnv.CVars.PathfinderAvoidanceCostForGroupMates
  141.                                            ));
  142.                         dangerousAreas.push_back(dangerArea);
  143.                 }
  144.                 groupMemberIterator->Next();
  145.         }
  146. }
  147.  
  148. //////////////////////////////////////////////////////////////////////////
  149. //////////////////////////////////////////////////////////////////////////
  150. #undef max
  151. #undef min
  152.  
  153. void ProcessPathRequestJob(MNM::PathfinderUtils::ProcessingContext* pProcessingContext)
  154. {
  155.         gAIEnv.pMNMPathfinder->ProcessPathRequest(*pProcessingContext);
  156. }
  157. DECLARE_JOB("PathProcessing", PathfinderProcessingJob, ProcessPathRequestJob);
  158.  
  159. void ConstructPathIfWayWasFoundJob(MNM::PathfinderUtils::ProcessingContext* pProcessingContext)
  160. {
  161.         gAIEnv.pMNMPathfinder->ConstructPathIfWayWasFound(*pProcessingContext);
  162. }
  163. DECLARE_JOB("PathConstruction", PathConstructionJob, ConstructPathIfWayWasFoundJob);
  164.  
  165. CMNMPathfinder::CMNMPathfinder()
  166. {
  167.         m_pathfindingFailedEventsToDispatch.reserve(gAIEnv.CVars.MNMPathfinderConcurrentRequests);
  168.         m_pathfindingCompletedEventsToDispatch.reserve(gAIEnv.CVars.MNMPathfinderConcurrentRequests);
  169. }
  170.  
  171. CMNMPathfinder::~CMNMPathfinder()
  172. {
  173. }
  174.  
  175. void CMNMPathfinder::Reset()
  176. {
  177.         WaitForJobsToFinish();
  178.  
  179.         // send failed to all requested paths before we clear this queue and leave all our callers with dangling ids that will be reused!
  180.         while (!m_requestedPathsQueue.empty())
  181.         {
  182.                 PathRequestFailed(m_requestedPathsQueue.front_id(), m_requestedPathsQueue.front());
  183.                 m_requestedPathsQueue.pop_front();
  184.         }
  185.  
  186.         m_processingContextsPool.Reset();
  187.         m_pathfindingFailedEventsToDispatch.clear();
  188.         m_pathfindingCompletedEventsToDispatch.clear();
  189. }
  190.  
  191. MNM::QueuedPathID CMNMPathfinder::RequestPathTo(const IAIPathAgent* pRequester, const MNMPathRequest& request)
  192. {
  193.         //////////////////////////////////////////////////////////////////////////
  194.         // Validate requester
  195.         if (!pRequester || (pRequester->GetPathAgentType() != AIOBJECT_ACTOR && pRequester->GetPathAgentType() != AIOBJECT_VEHICLE))
  196.         {
  197.                 AIWarning("[CMNMPathfinder::QueuePathRequest] No agent specified. A NavigationTypeID is needed to find the appropriate mesh.");
  198.                 return MNM::Constants::eQueuedPathID_InvalidID;
  199.         }
  200.  
  201.         //////////////////////////////////////////////////////////////////////////
  202.         // Validate Agent Type
  203.         const char* actorName = pRequester->GetPathAgentName();
  204.  
  205.         const IEntity* pEntity = pRequester->GetPathAgentEntity();
  206.         EntityId entityId = pEntity ? pEntity->GetId() : 0;
  207.  
  208.         if (!request.agentTypeID)
  209.         {
  210.                 AIWarning("[CMNMPathfinder::QueuePathRequest] Request from agent %s has no NavigationType defined.", actorName);
  211.                 return MNM::Constants::eQueuedPathID_InvalidID;
  212.         }
  213.  
  214.         //////////////////////////////////////////////////////////////////////////
  215.         // Validate callback
  216.         if (!request.resultCallback)
  217.         {
  218.                 AIWarning("[CMNMPathfinder::QueuePathRequest] Agent %s does not provide a result Callback", actorName);
  219.                 return 0;
  220.         }
  221.  
  222.         //////////////////////////////////////////////////////////////////////////
  223.         // Validate start/end locations
  224.         const NavigationMeshID meshID = gAIEnv.pNavigationSystem->GetEnclosingMeshID(request.agentTypeID, request.startLocation);
  225.  
  226.         if (!meshID)
  227.         {
  228.                 AIQueueBubbleMessage("CMNMPathfinder::RequestPathTo-NoMesh", entityId,
  229.                                      "I m not inside a Navigation area for my navigation type.",
  230.                                      eBNS_LogWarning);
  231.                 return 0;
  232.         }
  233.  
  234.         return m_requestedPathsQueue.push_back(MNM::PathfinderUtils::QueuedRequest(pRequester, request.agentTypeID, request));
  235. }
  236.  
  237. void CMNMPathfinder::CancelPathRequest(MNM::QueuedPathID requestId)
  238. {
  239.         m_processingContextsPool.CancelPathRequest(requestId);
  240.  
  241.         CancelResultDispatchingForRequest(requestId);
  242.  
  243.         //Check if in the queue
  244.         if (m_requestedPathsQueue.has(requestId))
  245.         {
  246.                 m_requestedPathsQueue.erase(requestId);
  247.         }
  248. }
  249.  
  250. void CMNMPathfinder::CancelResultDispatchingForRequest(MNM::QueuedPathID requestId)
  251. {
  252.         m_pathfindingFailedEventsToDispatch.try_remove_and_erase_if(MNM::PathfinderUtils::IsPathfindingFailedEventRelatedToRequest(requestId));
  253.         m_pathfindingCompletedEventsToDispatch.try_remove_and_erase_if(MNM::PathfinderUtils::IsPathfindingCompletedEventRelatedToRequest(requestId));
  254. }
  255.  
  256. bool CMNMPathfinder::CheckIfPointsAreOnStraightWalkableLine(const NavigationMeshID& meshID, const Vec3& source, const Vec3& destination, float heightOffset) const
  257. {
  258.         if (meshID == 0)
  259.                 return false;
  260.  
  261.         const NavigationMesh& mesh = gAIEnv.pNavigationSystem->GetMesh(meshID);
  262.         const MNM::CNavMesh& navMesh = mesh.navMesh;
  263.  
  264.         const Vec3 raiseUp(0.0f, 0.0f, heightOffset);
  265.         Vec3 raisedSource = source + raiseUp;
  266.  
  267.         MNM::vector3_t startLoc = MNM::vector3_t(MNM::real_t(raisedSource.x), MNM::real_t(raisedSource.y), MNM::real_t(raisedSource.z));
  268.         MNM::vector3_t endLoc = MNM::vector3_t(MNM::real_t(destination.x), MNM::real_t(destination.y), MNM::real_t(destination.z));
  269.  
  270.         const MNM::real_t verticalRange(2.0f);
  271.         MNM::TriangleID triStart = navMesh.GetTriangleAt(startLoc, verticalRange, verticalRange);
  272.         MNM::TriangleID triEnd = navMesh.GetTriangleAt(endLoc, verticalRange, verticalRange);
  273.  
  274.         if (!triStart || !triEnd)
  275.                 return false;
  276.  
  277.         MNM::CNavMesh::RayCastRequest<512> raycastRequest;
  278.  
  279.         if (navMesh.RayCast(startLoc, triStart, endLoc, triEnd, raycastRequest) != MNM::CNavMesh::eRayCastResult_NoHit)
  280.                 return false;
  281.  
  282.         return true;
  283. }
  284.  
  285. void CMNMPathfinder::SetupNewValidPathRequests()
  286. {
  287.         const size_t freeAvailableSlotsToProcessNewRequests = m_processingContextsPool.GetFreeSlotsCount();
  288.  
  289.         for (size_t i = 0; (i < freeAvailableSlotsToProcessNewRequests) && !m_requestedPathsQueue.empty(); ++i)
  290.         {
  291.                 MNM::QueuedPathID idQueuedRequest = m_requestedPathsQueue.front_id();
  292.                 MNM::PathfinderUtils::QueuedRequest requestToServe = m_requestedPathsQueue.front();
  293.                 m_requestedPathsQueue.pop_front();
  294.  
  295.                 MNM::PathfinderUtils::ProcessingContextsPool::PoolIterator pProcessingContext;
  296.                 MNM::PathfinderUtils::ProcessingContextId id;
  297.                 id = m_processingContextsPool.GetFirstAvailableContextId();
  298.                 if (id != MNM::PathfinderUtils::kInvalidProcessingContextId)
  299.                 {
  300.                         MNM::PathfinderUtils::ProcessingContext& processingContext = m_processingContextsPool.GetContextAtPosition(id);
  301.                         assert(processingContext.status == MNM::PathfinderUtils::ProcessingContext::Reserved);
  302.                         processingContext.workingSet.aStarOpenList.SetFrameTimeQuota(gAIEnv.CVars.MNMPathFinderQuota);
  303.  
  304.                         bool hasSetupSucceeded = SetupForNextPathRequest(idQueuedRequest, requestToServe, processingContext);
  305.                         if (!hasSetupSucceeded)
  306.                         {
  307.                                 m_processingContextsPool.ReleaseContext(id);
  308.  
  309.                                 MNM::PathfinderUtils::PathfindingFailedEvent failedEvent(idQueuedRequest, requestToServe);
  310.                                 m_pathfindingFailedEventsToDispatch.push_back(failedEvent);
  311.                         }
  312.                         else
  313.                         {
  314.                                 processingContext.status = MNM::PathfinderUtils::ProcessingContext::InProgress;
  315.                         }
  316.                 }
  317.                 else
  318.                 {
  319.                         CRY_ASSERT_MESSAGE(0, "Trying to setup new requests while not having available slots in the pool.");
  320.                 }
  321.         }
  322. }
  323.  
  324. void CMNMPathfinder::SpawnJobs()
  325. {
  326.         const bool isPathfinderMultithreaded = gAIEnv.CVars.MNMPathfinderMT != 0;
  327.         if (isPathfinderMultithreaded)
  328.         {
  329.                 m_processingContextsPool.ExecuteFunctionOnElements(functor(*this, &CMNMPathfinder::SpawnAppropriateJobIfPossible));
  330.         }
  331.         else
  332.         {
  333.                 m_processingContextsPool.ExecuteFunctionOnElements(functor(*this, &CMNMPathfinder::ExecuteAppropriateOperationIfPossible));
  334.         }
  335. }
  336.  
  337. void CMNMPathfinder::SpawnAppropriateJobIfPossible(MNM::PathfinderUtils::ProcessingContext& processingContext)
  338. {
  339.         switch (processingContext.status)
  340.         {
  341.         case MNM::PathfinderUtils::ProcessingContext::InProgress:
  342.                 {
  343.                         SpawnPathfinderProcessingJob(processingContext);
  344.                         break;
  345.                 }
  346.         case MNM::PathfinderUtils::ProcessingContext::FindWayCompleted:
  347.                 {
  348.                         SpawnPathConstructionJob(processingContext);
  349.                         break;
  350.                 }
  351.         }
  352. }
  353.  
  354. void CMNMPathfinder::ExecuteAppropriateOperationIfPossible(MNM::PathfinderUtils::ProcessingContext& processingContext)
  355. {
  356.         switch (processingContext.status)
  357.         {
  358.         case MNM::PathfinderUtils::ProcessingContext::InProgress:
  359.                 {
  360.                         ProcessPathRequest(processingContext);
  361.                         break;
  362.                 }
  363.         case MNM::PathfinderUtils::ProcessingContext::FindWayCompleted:
  364.                 {
  365.                         ConstructPathIfWayWasFound(processingContext);
  366.                         break;
  367.                 }
  368.         }
  369. }
  370.  
  371. void CMNMPathfinder::SpawnPathfinderProcessingJob(MNM::PathfinderUtils::ProcessingContext& processingContext)
  372. {
  373.         CRY_ASSERT_MESSAGE(processingContext.status == MNM::PathfinderUtils::ProcessingContext::InProgress, "PathfinderProcessingJob is spawned even if the process is not 'InProgress'.");
  374.  
  375.         CRY_ASSERT_MESSAGE(!processingContext.jobState.IsRunning(), "The job is still running and we are spawning a new one.");
  376.  
  377.         PathfinderProcessingJob job(&processingContext);
  378.         job.RegisterJobState(&processingContext.jobState);
  379.         job.SetPriorityLevel(JobManager::eRegularPriority);
  380.         job.Run();
  381. }
  382.  
  383. void CMNMPathfinder::WaitForJobsToFinish()
  384. {
  385.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  386.         m_processingContextsPool.ExecuteFunctionOnElements(functor(*this, &CMNMPathfinder::WaitForJobToFinish));
  387. }
  388.  
  389. void CMNMPathfinder::WaitForJobToFinish(MNM::PathfinderUtils::ProcessingContext& processingContext)
  390. {
  391.         gEnv->GetJobManager()->WaitForJob(processingContext.jobState);
  392. }
  393.  
  394. void CMNMPathfinder::DispatchResults()
  395. {
  396.         {
  397.                 MNM::PathfinderUtils::PathfindingFailedEvent failedEvent;
  398.                 while (m_pathfindingFailedEventsToDispatch.try_pop_back(failedEvent))
  399.                 {
  400.                         PathRequestFailed(failedEvent.requestId, failedEvent.request);
  401.                 }
  402.         }
  403.  
  404.         {
  405.                 MNM::PathfinderUtils::PathfindingCompletedEvent succeeded;
  406.                 while (m_pathfindingCompletedEventsToDispatch.try_pop_back(succeeded))
  407.                 {
  408.                         succeeded.callback(succeeded.requestId, succeeded.eventData);
  409.                 }
  410.         }
  411. }
  412.  
  413. void CMNMPathfinder::Update()
  414. {
  415.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  416.  
  417.         if (gAIEnv.CVars.MNMPathFinderDebug)
  418.         {
  419.                 DebugAllStatistics();
  420.         }
  421.  
  422.         DispatchResults();
  423.  
  424.         m_processingContextsPool.CleanupFinishedRequests();
  425.  
  426.         SetupNewValidPathRequests();
  427.  
  428.         SpawnJobs();
  429. }
  430.  
  431. void CMNMPathfinder::OnNavigationMeshChanged(const NavigationMeshID meshId, const MNM::TileID tileId)
  432. {
  433.         const size_t maximumAmountOfSlotsToUpdate = m_processingContextsPool.GetOccupiedSlotsCount();
  434.         for (size_t i = 0; i < maximumAmountOfSlotsToUpdate; ++i)
  435.         {
  436.                 MNM::PathfinderUtils::ProcessingContext& processingContext = m_processingContextsPool.GetContextAtPosition(i);
  437.                 MNM::PathfinderUtils::ProcessingRequest& processingRequest = processingContext.processingRequest;
  438.  
  439.                 if (!processingRequest.IsValid())
  440.                         return;
  441.  
  442.                 if (processingRequest.meshID != meshId)
  443.                         return;
  444.  
  445.                 if (!processingContext.workingSet.aStarOpenList.TileWasVisited(tileId))
  446.                 {
  447.                         bool neighbourTileWasVisited = false;
  448.  
  449.                         const NavigationMesh& mesh = gAIEnv.pNavigationSystem->GetMesh(meshId);
  450.                         const MNM::vector3_t meshCoordinates = mesh.navMesh.GetTileContainerCoordinates(tileId);
  451.  
  452.                         for (size_t side = 0; side < MNM::CNavMesh::SideCount; ++side)
  453.                         {
  454.                                 const MNM::TileID neighbourTileId = mesh.navMesh.GetNeighbourTileID(meshCoordinates.x.as_int(), meshCoordinates.y.as_int(), meshCoordinates.z.as_int(), side);
  455.  
  456.                                 if (processingContext.workingSet.aStarOpenList.TileWasVisited(neighbourTileId))
  457.                                 {
  458.                                         neighbourTileWasVisited = true;
  459.                                         break;
  460.                                 }
  461.                         }
  462.  
  463.                         if (!neighbourTileWasVisited)
  464.                                 return;
  465.                 }
  466.  
  467.                 //////////////////////////////////////////////////////////////////////////
  468.                 /// Re-start current request for next update
  469.  
  470.                 // Copy onto the stack to call function to avoid self delete.
  471.                 MNM::QueuedPathID requestId = processingRequest.queuedID;
  472.                 MNM::PathfinderUtils::QueuedRequest requestParams = processingRequest.data;
  473.  
  474.                 if (!SetupForNextPathRequest(requestId, requestParams, processingContext))
  475.                 {
  476.                         PathRequestFailed(requestId, requestParams);
  477.                 }
  478.         }
  479. }
  480.  
  481. bool CMNMPathfinder::SetupForNextPathRequest(MNM::QueuedPathID requestID, MNM::PathfinderUtils::QueuedRequest& request, MNM::PathfinderUtils::ProcessingContext& processingContext)
  482. {
  483.         MNM::PathfinderUtils::ProcessingRequest& processingRequest = processingContext.processingRequest;
  484.         processingRequest.Reset();
  485.  
  486.         //////////////////////////////////////////////////////////////////////////
  487.         // Validate start/end locations
  488.         const NavigationMeshID meshID = gAIEnv.pNavigationSystem->GetEnclosingMeshID(request.agentTypeID, request.requestParams.startLocation);
  489.  
  490.         if (!meshID)
  491.         {
  492.                 AIWarning("[CMNMPathfinder::SetupForNextPathRequest] Agent %s is not inside a navigation volume.", request.pRequester->GetPathAgentName());
  493.                 return false;
  494.         }
  495.  
  496.         const NavigationMesh& mesh = gAIEnv.pNavigationSystem->GetMesh(meshID);
  497.         const MNM::CNavMesh& navMesh = mesh.navMesh;
  498.  
  499.         const MNM::CNavMesh::SGridParams& gridParams = navMesh.GetGridParams();
  500.         const MNM::vector3_t origin = MNM::vector3_t(MNM::real_t(gridParams.origin.x), MNM::real_t(gridParams.origin.y), MNM::real_t(gridParams.origin.z));
  501.  
  502.         const uint16 agentRadiusUnits = gAIEnv.pNavigationSystem->GetAgentRadiusInVoxelUnits(request.agentTypeID);
  503.         const uint16 agentHeightUnits = gAIEnv.pNavigationSystem->GetAgentHeightInVoxelUnits(request.agentTypeID);
  504.  
  505.         const MNM::vector3_t startLocation(MNM::real_t(request.requestParams.startLocation.x), MNM::real_t(request.requestParams.startLocation.y),
  506.                                            MNM::real_t(request.requestParams.startLocation.z));
  507.         const MNM::vector3_t endLocation(MNM::real_t(request.requestParams.endLocation.x), MNM::real_t(request.requestParams.endLocation.y),
  508.                                          MNM::real_t(request.requestParams.endLocation.z));
  509.         const Vec3 voxelSize = navMesh.GetGridParams().voxelSize;
  510.         const MNM::real_t horizontalRange = MNMUtils::CalculateMinHorizontalRange(agentRadiusUnits, voxelSize.x);
  511.         const MNM::real_t verticalRange = MNMUtils::CalculateMinVerticalRange(agentHeightUnits, voxelSize.z);
  512.  
  513.         AgentType agentTypeProperties;
  514.         const bool arePropertiesValid = gAIEnv.pNavigationSystem->GetAgentTypeProperties(request.agentTypeID, agentTypeProperties);
  515.         assert(arePropertiesValid);
  516.         const uint16 zOffsetMultiplier = min(uint16(2), agentTypeProperties.settings.heightVoxelCount);
  517.         const MNM::real_t verticalUpwardRange = arePropertiesValid ? MNM::real_t(zOffsetMultiplier * agentTypeProperties.settings.voxelSize.z) : MNM::real_t(.0f);
  518.  
  519.         Vec3 safeStartLocation(request.requestParams.startLocation);
  520.         MNM::TriangleID triangleStartID;
  521.         if (!(triangleStartID = navMesh.GetTriangleAt(startLocation - origin, verticalRange, verticalUpwardRange)))
  522.         {
  523.                 MNM::vector3_t closest;
  524.                 if (!(triangleStartID = navMesh.GetClosestTriangle(startLocation - origin, verticalRange, horizontalRange, NULL, &closest)))
  525.                 {
  526.                         AIWarning("Navigation system couldn't find NavMesh triangle at path start point (%.2f, %2f, %2f) for agent '%s'.",
  527.                                   request.requestParams.startLocation.x, request.requestParams.startLocation.y, request.requestParams.startLocation.z,
  528.                                   request.pRequester->GetPathAgentName());
  529.                         return false;
  530.                 }
  531.                 else
  532.                 {
  533.                         safeStartLocation = closest.GetVec3();
  534.                 }
  535.         }
  536.  
  537.         Vec3 safeEndLocation(request.requestParams.endLocation);
  538.         MNM::TriangleID triangleEndID = navMesh.GetTriangleAt(endLocation - origin, verticalRange, verticalRange);
  539.         if (!triangleEndID)
  540.         {
  541.                 MNM::vector3_t closest;
  542.                 triangleEndID = navMesh.GetClosestTriangle(endLocation - origin, verticalRange, horizontalRange, NULL, &closest);
  543.                 if (triangleEndID)
  544.                 {
  545.                         safeEndLocation = closest.GetVec3();
  546.                 }
  547.                 else
  548.                 {
  549.                         AIWarning("Navigation system couldn't find NavMesh triangle at path destination point (%.2f, %.2f, %.2f) for agent '%s'.",
  550.                                   request.requestParams.endLocation.x, request.requestParams.endLocation.y, request.requestParams.endLocation.z,
  551.                                   request.pRequester->GetPathAgentName());
  552.                         return false;
  553.                 }
  554.         }
  555.  
  556.         // The data for MNM are good until this point so we can set up the path finding
  557.  
  558.         processingRequest.pRequester = request.pRequester;
  559.         processingRequest.meshID = meshID;
  560.         processingRequest.fromTriangleID = triangleStartID;
  561.         processingRequest.toTriangleID = triangleEndID;
  562.         processingRequest.queuedID = requestID;
  563.  
  564.         processingRequest.data = request;
  565.         processingRequest.data.requestParams.startLocation = safeStartLocation;
  566.         processingRequest.data.requestParams.endLocation = safeEndLocation;
  567.  
  568.         const MNM::real_t startToEndDist = (endLocation - startLocation).lenNoOverflow();
  569.         processingContext.workingSet.aStarOpenList.SetUpForPathSolving(mesh.navMesh.GetTriangleCount(), triangleStartID, startLocation, startToEndDist);
  570.  
  571.         return true;
  572. }
  573.  
  574. void CMNMPathfinder::ProcessPathRequest(MNM::PathfinderUtils::ProcessingContext& processingContext)
  575. {
  576.         if (processingContext.status != MNM::PathfinderUtils::ProcessingContext::InProgress)
  577.                 return;
  578.  
  579.         processingContext.workingSet.aStarOpenList.ResetConsumedTimeDuringCurrentFrame();
  580.  
  581.         MNM::PathfinderUtils::ProcessingRequest& processingRequest = processingContext.processingRequest;
  582.         assert(processingRequest.IsValid());
  583.  
  584.         const NavigationMesh& mesh = gAIEnv.pNavigationSystem->GetMesh(processingRequest.meshID);
  585.         const MNM::CNavMesh& navMesh = mesh.navMesh;
  586.         const MNM::CNavMesh::SGridParams& gridParams = navMesh.GetGridParams();
  587.         const OffMeshNavigationManager* offMeshNavigationManager = gAIEnv.pNavigationSystem->GetOffMeshNavigationManager();
  588.         assert(offMeshNavigationManager);
  589.         const MNM::OffMeshNavigation& meshOffMeshNav = offMeshNavigationManager->GetOffMeshNavigationForMesh(processingRequest.meshID);
  590.  
  591.         MNM::CNavMesh::WayQueryRequest inputParams(processingRequest.pRequester, processingRequest.fromTriangleID,
  592.                                                    processingRequest.data.requestParams.startLocation - gridParams.origin, processingRequest.toTriangleID,
  593.                                                    processingRequest.data.requestParams.endLocation - gridParams.origin, meshOffMeshNav, *offMeshNavigationManager,
  594.                                                    processingRequest.data.GetDangersInfos());
  595.  
  596.         if (navMesh.FindWay(inputParams, processingContext.workingSet, processingContext.queryResult) == MNM::CNavMesh::eWQR_Continuing)
  597.                 return;
  598.  
  599.         processingContext.status = MNM::PathfinderUtils::ProcessingContext::FindWayCompleted;
  600.         return;
  601. }
  602.  
  603. bool CMNMPathfinder::ConstructPathFromFoundWay(
  604.   const MNM::CNavMesh::WayQueryResult& way,
  605.   const MNM::CNavMesh& navMesh,
  606.   const OffMeshNavigationManager* pOffMeshNavigationManager,
  607.   const Vec3& startLocation,
  608.   const Vec3& endLocation,
  609.   CPathHolder<PathPointDescriptor>& outputPath)
  610. {
  611.         const MNM::WayTriangleData* pWayData = way.GetWayData();
  612.         const size_t waySize = way.GetWaySize();
  613.  
  614.         const MNM::CNavMesh::SGridParams& gridParams = navMesh.GetGridParams();
  615.         const MNM::vector3_t origin = MNM::vector3_t(gridParams.origin);
  616.  
  617.         // NOTE: waypoints are in reverse order
  618.         for (size_t i = 0; i < waySize; ++i)
  619.         {
  620.                 // Using the edge-midpoints of adjacent triangles to build the path.
  621.                 if (i > 0)
  622.                 {
  623.                         Vec3 edgeMidPoint;
  624.                         if (navMesh.CalculateMidEdge(pWayData[i - 1].triangleID, pWayData[i].triangleID, edgeMidPoint))
  625.                         {
  626.                                 PathPointDescriptor pathPoint(IAISystem::NAV_UNSET, edgeMidPoint + origin.GetVec3());
  627.                                 pathPoint.iTriId = pWayData[i].triangleID;
  628.                                 outputPath.PushFront(pathPoint);
  629.                         }
  630.                 }
  631.  
  632.                 if (pWayData[i].offMeshLinkID)
  633.                 {
  634.                         // Grab off-mesh link object
  635.                         const MNM::OffMeshLink* pOffMeshLink = pOffMeshNavigationManager->GetOffMeshLink(pWayData[i].offMeshLinkID);
  636.                         if (pOffMeshLink == nullptr)
  637.                         {
  638.                                 // Link can no longer be found; this path is now invalid
  639.                                 return false;
  640.                         }
  641.  
  642.                         const bool isOffMeshLinkSmartObject = pOffMeshLink->GetLinkType() == MNM::OffMeshLink::eLinkType_SmartObject;
  643.                         IAISystem::ENavigationType type = isOffMeshLinkSmartObject ? IAISystem::NAV_SMARTOBJECT : IAISystem::NAV_CUSTOM_NAVIGATION;
  644.  
  645.                         // Add Entry/Exit points
  646.                         PathPointDescriptor pathPoint(IAISystem::NAV_UNSET);
  647.  
  648.                         // Add Exit point
  649.                         pathPoint.vPos = pOffMeshLink->GetEndPosition();
  650.                         CRY_ASSERT_MESSAGE(i > 0, "Path contains offmesh link without exit waypoint");
  651.                         if (i > 0)
  652.                         {
  653.                                 pathPoint.iTriId = pWayData[i - 1].triangleID;
  654.                         }
  655.                         outputPath.PushFront(pathPoint);
  656.  
  657.                         // Add Entry point
  658.                         pathPoint.navType = type;
  659.                         pathPoint.vPos = pOffMeshLink->GetStartPosition();
  660.                         pathPoint.offMeshLinkData.offMeshLinkID = pWayData[i].offMeshLinkID;
  661.                         pathPoint.iTriId = pWayData[i].triangleID;
  662.                         outputPath.PushFront(pathPoint);
  663.                 }
  664.         }
  665.  
  666.         PathPointDescriptor navPathStart(IAISystem::NAV_UNSET, startLocation);
  667.         PathPointDescriptor navPathEnd(IAISystem::NAV_UNSET, endLocation);
  668.  
  669.         // Assign triangleID of start and end points (waypoints are in reverse order)
  670.         navPathEnd.iTriId = pWayData[0].triangleID;
  671.         navPathStart.iTriId = pWayData[waySize - 1].triangleID;
  672.  
  673.         //Insert start/end locations in the path
  674.         outputPath.PushBack(navPathEnd);
  675.         outputPath.PushFront(navPathStart);
  676.  
  677.         return true;
  678. }
  679.  
  680. void CMNMPathfinder::ConstructPathIfWayWasFound(MNM::PathfinderUtils::ProcessingContext& processingContext)
  681. {
  682.         if (processingContext.status != MNM::PathfinderUtils::ProcessingContext::FindWayCompleted)
  683.                 return;
  684.  
  685.         MNM::PathfinderUtils::ProcessingRequest& processingRequest = processingContext.processingRequest;
  686.  
  687.         const NavigationMesh& mesh = gAIEnv.pNavigationSystem->GetMesh(processingRequest.meshID);
  688.         const MNM::CNavMesh& navMesh = mesh.navMesh;
  689.         const OffMeshNavigationManager* offMeshNavigationManager = gAIEnv.pNavigationSystem->GetOffMeshNavigationManager();
  690.         assert(offMeshNavigationManager);
  691.  
  692.         const bool bPathFound = (processingContext.queryResult.GetWaySize() != 0);
  693.         bool bPathConstructed = false;
  694.         CPathHolder<PathPointDescriptor> outputPath;
  695.         if (bPathFound)
  696.         {
  697.                 bPathConstructed = ConstructPathFromFoundWay(
  698.                   processingContext.queryResult,
  699.                   navMesh,
  700.                   offMeshNavigationManager,
  701.                   processingRequest.data.requestParams.startLocation,
  702.                   processingRequest.data.requestParams.endLocation,
  703.                   *&outputPath);
  704.  
  705.                 if (bPathConstructed)
  706.                 {
  707.                         if (processingRequest.data.requestParams.beautify && gAIEnv.CVars.BeautifyPath)
  708.                         {
  709.                                 outputPath.PullPathOnNavigationMesh(navMesh, gAIEnv.CVars.PathStringPullingIterations);
  710.                         }
  711.                 }
  712.         }
  713.  
  714.         MNM::PathfinderUtils::PathfindingCompletedEvent successEvent;
  715.         MNMPathRequestResult& resultData = successEvent.eventData;
  716.         if (bPathConstructed)
  717.         {
  718.                 INavPathPtr navPath = resultData.pPath;
  719.                 resultData.result = eMNMPR_Success;
  720.                 navPath->Clear("CMNMPathfinder::ProcessPathRequest");
  721.  
  722.                 outputPath.FillNavPath(*navPath);
  723.  
  724.                 const MNMPathRequest& requestParams = processingRequest.data.requestParams;
  725.                 SNavPathParams pparams(requestParams.startLocation, requestParams.endLocation, Vec3(0.0f, 0.0f, 0.0f), requestParams.endDirection, requestParams.forceTargetBuildingId,
  726.                                        requestParams.allowDangerousDestination, requestParams.endDistance);
  727.                 pparams.meshID = processingRequest.meshID;
  728.  
  729.                 navPath->SetParams(pparams);
  730.                 navPath->SetEndDir(requestParams.endDirection);
  731.         }
  732.         else
  733.         {
  734.                 resultData.result = eMNMPR_NoPathFound;
  735.         }
  736.  
  737.         successEvent.callback = processingRequest.data.requestParams.resultCallback;
  738.         successEvent.requestId = processingRequest.queuedID;
  739.  
  740.         processingRequest.Reset();
  741.         processingContext.workingSet.aStarOpenList.PathSolvingDone();
  742.  
  743.         m_pathfindingCompletedEventsToDispatch.push_back(successEvent);
  744.  
  745.         processingContext.status = MNM::PathfinderUtils::ProcessingContext::Completed;
  746.         return;
  747. }
  748.  
  749. void CMNMPathfinder::SpawnPathConstructionJob(MNM::PathfinderUtils::ProcessingContext& processingContext)
  750. {
  751.         CRY_ASSERT_MESSAGE(processingContext.status == MNM::PathfinderUtils::ProcessingContext::FindWayCompleted, "PathConstructionJob spawned even if the status is not 'FindWayCompleted'");
  752.  
  753.         CRY_ASSERT_MESSAGE(!processingContext.jobState.IsRunning(), "The job is still running and we are spawning a new one.");
  754.  
  755.         PathConstructionJob job(&processingContext);
  756.         job.RegisterJobState(&processingContext.jobState);
  757.         job.SetPriorityLevel(JobManager::eRegularPriority);
  758.         job.Run();
  759. }
  760.  
  761. void CMNMPathfinder::PathRequestFailed(MNM::QueuedPathID requestID, const MNM::PathfinderUtils::QueuedRequest& request)
  762. {
  763.         MNMPathRequestResult result;
  764.         request.requestParams.resultCallback(requestID, result);
  765. }
  766.  
  767. void CMNMPathfinder::DebugAllStatistics()
  768. {
  769.         float y = 40.0f;
  770.         IRenderAuxText::Draw2dLabel(100.f, y, 1.4f, Col_White, false, "Currently we have %" PRISIZE_T " queued requests in the MNMPathfinder", m_requestedPathsQueue.size());
  771.  
  772.         y += 100.0f;
  773.         const size_t maximumAmountOfSlotsToUpdate = m_processingContextsPool.GetMaxSlots();
  774.         for (size_t i = 0; i < maximumAmountOfSlotsToUpdate; ++i)
  775.         {
  776.                 MNM::PathfinderUtils::ProcessingContext& processingContext = m_processingContextsPool.GetContextAtPosition(i);
  777.                 DebugStatistics(processingContext, (y + (i * 100.f)));
  778.         }
  779. }
  780.  
  781. void CMNMPathfinder::DebugStatistics(MNM::PathfinderUtils::ProcessingContext& processingContext, const float textY)
  782. {
  783.         stack_string text;
  784.  
  785.         MNM::AStarContention::ContentionStats stats = processingContext.workingSet.aStarOpenList.GetContentionStats();
  786.  
  787.         text.Format(
  788.           "MNMPathFinder - Frame time quota (%f ms) - EntityId: %d - Status: %s\n"
  789.           "---------\n"
  790.           "AStar steps: Average - %d / Maximum - %d\n"
  791.           "AStar time:  Average - %.4f ms / Maximum - %.4f ms",
  792.           stats.frameTimeQuota,
  793.           (uint32)processingContext.processingRequest.data.requestParams.agentTypeID,
  794.           processingContext.GetStatusAsString(),
  795.           stats.averageSearchSteps,
  796.           stats.peakSearchSteps,
  797.           stats.averageSearchTime,
  798.           stats.peakSearchTime);
  799.  
  800.         IRenderAuxText::Draw2dLabel(100.f, textY, 1.4f, Col_White, false, "%s", text.c_str());
  801. }
  802.  
downloadMNMPathfinder.cpp Source code - Download CRYENGINE Source code
Related Source Codes/Software:
postal - 2017-06-11
reactide - Reactide is the first dedicated IDE for React web ... 2017-06-11
rkt - rkt is a pod-native container engine for Linux. It... 2017-06-11
uWebSockets - Tiny WebSockets https://for... 2017-06-11
realworld - TodoMVC for the RealWorld - Exemplary fullstack Me... 2017-06-11
CRYENGINE - CRYENGINE is a powerful real-time game development... 2017-06-11
goreplay - GoReplay is an open-source tool for capturing and ... 2017-06-10
pyenv - Simple Python version management 2017-06-10
redux-saga - An alternative side effect model for Redux apps ... 2017-06-10
angular-starter - 2017-06-10

 Back to top