BVB Source Codes

CRYENGINE Show Graph.cpp Source code

Return Download CRYENGINE: download Graph.cpp Source code - Download CRYENGINE Source code - Type:.cpp
  1. // Copyright 2001-2016 Crytek GmbH / Crytek Group. All rights reserved.
  2.  
  3. /********************************************************************
  4.    CryGame Source File.
  5.    -------------------------------------------------------------------------
  6.    File name:   Graph.cpp
  7.    Version:     v1.00
  8.    Description: Implementation of the CGraph class.
  9.  
  10.    -------------------------------------------------------------------------
  11.    History:
  12.    - ?
  13.    - 4 May 2009  : Evgeny Adamenkov: Removed IRenderer
  14.  
  15.  *********************************************************************/
  16.  
  17. #include "StdAfx.h"
  18.  
  19. #include <limits>
  20. #include <algorithm>
  21.  
  22. #include <CrySystem/ISystem.h>
  23. #include <CryRenderer/IRenderAuxGeom.h>
  24. #include <CrySystem/ILog.h>
  25. #include <Cry3DEngine/I3DEngine.h>
  26. #include <CrySystem/ITimer.h>
  27. #include <CrySystem/IConsole.h>
  28. #include <CryPhysics/IPhysics.h>
  29. #include <CrySystem/File/CryFile.h>
  30.  
  31. #include "Graph.h"
  32. #include "CAISystem.h"
  33. #include "AILog.h"
  34. #include <CryMath/Cry_Math.h>
  35. #include "AIObject.h"
  36. #include "VertexList.h"
  37. #include "NavRegion.h"
  38.  
  39. #include "GraphLinkManager.h"
  40. #include "GraphNodeManager.h"
  41.  
  42. #define BAI_TRI_FILE_VERSION 54
  43.  
  44. // identifier so links can be marked as impassable, then restored
  45. //static const float RADIUS_FOR_BROKEN_LINKS = -121314.0f;
  46.  
  47. const float AStarSearchNode::fInvalidCost = 999999.0f;
  48.  
  49. Vec3 CObstacleRef::GetPos() const
  50. {
  51.         CCCPOINT(CObstacleRef_GetPos);
  52.  
  53.         if (m_refAnchor.IsValid())
  54.         {
  55.                 return m_refAnchor.GetAIObject()->GetPos();
  56.         }
  57.         else if (m_pNode)
  58.         {
  59.                 return m_pNode->GetPos();
  60.         }
  61.         else if (m_pSmartObject && m_pRule)
  62.         {
  63.                 return m_pRule->pObjectHelper ? m_pSmartObject->GetHelperPos(m_pRule->pObjectHelper) : m_pSmartObject->GetPos();
  64.         }
  65.         else
  66.         {
  67.                 assert(false);
  68.                 return Vec3(ZERO);
  69.         }
  70. }
  71.  
  72. float CObstacleRef::GetApproxRadius() const
  73. {
  74.         assert(false);
  75.         return 0.0f;
  76. }
  77.  
  78. //====================================================================
  79. // ValidateNode
  80. //====================================================================
  81. inline bool CGraph::ValidateNode(unsigned nodeIndex, bool fullCheck) const
  82. {
  83.         const GraphNode* pNode = GetNodeManager().GetNode(nodeIndex);
  84.  
  85.         if (!nodeIndex)
  86.                 return false;
  87.         if (!m_allNodes.DoesNodeExist(nodeIndex))
  88.                 return false;
  89.         if (!fullCheck)
  90.                 return true;
  91.         else
  92.                 return ValidateNodeFullCheck(pNode);
  93. }
  94.  
  95. //====================================================================
  96. // ValidateNodeFullCheck
  97. //====================================================================
  98. bool CGraph::ValidateNodeFullCheck(const GraphNode* pNode) const
  99. {
  100.         bool result = true;
  101.         AIAssert(pNode);
  102.         int nNonRoadLinks = 0;
  103.         unsigned nTriLinks = 0;
  104.         for (unsigned linkId = pNode->firstLinkIndex; linkId; linkId = GetLinkManager().GetNextLink(linkId))
  105.         {
  106.                 unsigned nextNodeIndex = GetLinkManager().GetNextNode(linkId);
  107.                 const GraphNode* next = GetNodeManager().GetNode(nextNodeIndex);
  108.                 if (!CGraph::ValidateNode(nextNodeIndex, false))
  109.                         result = false;
  110.                 if (next->navType != IAISystem::NAV_ROAD)
  111.                         ++nNonRoadLinks;
  112.                 if (next->navType == IAISystem::NAV_TRIANGULAR)
  113.                         ++nTriLinks;
  114.         }
  115.         if (nNonRoadLinks > 50)
  116.         {
  117.                 AIWarning("Too many non-road links (%d) from node %p type %d at (%5.2f, %5.2f, %5.2f)", nNonRoadLinks, pNode, pNode->navType,
  118.                           pNode->GetPos().x, pNode->GetPos().y, pNode->GetPos().z);
  119.         }
  120.         if (pNode->navType == IAISystem::NAV_TRIANGULAR && nTriLinks != 3)
  121.         {
  122.                 // NAV_TRIANGULAR is replaced by MNM
  123.                 assert(false);
  124.                 result = false;
  125.         }
  126.         return result;
  127. }
  128.  
  129. //////////////////////////////////////////////////////////////////////
  130. // Construction/Destruction
  131. //////////////////////////////////////////////////////////////////////
  132.  
  133. //====================================================================
  134. // CGraph
  135. //====================================================================
  136. CGraph::CGraph()
  137.         : m_pGraphLinkManager(new CGraphLinkManager),
  138.         m_pGraphNodeManager(new CGraphNodeManager),
  139.         m_allNodes(*m_pGraphNodeManager),
  140.         m_triangularBBox(AABB::RESET)
  141. {
  142.  
  143.         m_safeFirstIndex = CreateNewNode(IAISystem::NAV_UNSET, Vec3(ZERO), 0);
  144.         m_firstIndex = m_safeFirstIndex;
  145.         m_pFirst = GetNodeManager().GetNode(m_safeFirstIndex);
  146.         m_pSafeFirst = m_pFirst;
  147.         m_pCurrent = m_pFirst;
  148.         m_currentIndex = m_firstIndex;
  149.         m_pCurrent->firstLinkIndex = 0;
  150. }
  151.  
  152. //====================================================================
  153. // ~CGraph
  154. //====================================================================
  155. CGraph::~CGraph()
  156. {
  157.         //Takes a long time and shouldn't be needed
  158.         //Clear(IAISystem::NAVMASK_ALL);
  159.  
  160.         delete m_pGraphNodeManager;
  161.         delete m_pGraphLinkManager;
  162.  
  163.         GraphNode::ClearFreeIDs();
  164. }
  165.  
  166. //====================================================================
  167. // Clear
  168. //====================================================================
  169. void CGraph::Clear(IAISystem::tNavCapMask navTypeMask)
  170. {
  171.         ClearMarks();
  172.         DeleteGraph(navTypeMask);
  173.         if (navTypeMask & (IAISystem::NAV_WAYPOINT_HUMAN | IAISystem::NAV_WAYPOINT_3DSURFACE))
  174.         {
  175.                 EntranceMap::iterator next;
  176.                 for (EntranceMap::iterator it = m_mapEntrances.begin(); it != m_mapEntrances.end(); it = next)
  177.                 {
  178.                         next = it;
  179.                         ++next;
  180.                         GraphNode* pNode = GetNodeManager().GetNode(it->second);
  181.                         if (pNode->navType & navTypeMask)
  182.                                 m_mapEntrances.erase(it);
  183.                 }
  184.                 for (EntranceMap::iterator it = m_mapExits.begin(); it != m_mapExits.end(); it = next)
  185.                 {
  186.                         next = it;
  187.                         ++next;
  188.                         GraphNode* pNode = GetNodeManager().GetNode(it->second);
  189.                         if (pNode->navType & navTypeMask)
  190.                                 m_mapExits.erase(it);
  191.                 }
  192.         }
  193.  
  194.         GetNodeManager().Clear(navTypeMask);
  195.  
  196.         if (m_pSafeFirst)
  197.         {
  198.                 Disconnect(m_safeFirstIndex, false);
  199.         }
  200.         else
  201.         {
  202.                 m_safeFirstIndex = CreateNewNode(IAISystem::NAV_UNSET, Vec3(ZERO), 0);
  203.                 m_pSafeFirst = GetNodeManager().GetNode(m_safeFirstIndex);
  204.         }
  205.         m_pFirst = m_pSafeFirst;
  206.         m_firstIndex = m_safeFirstIndex;
  207.         m_pCurrent = m_pFirst;
  208.         m_currentIndex = m_firstIndex;
  209. }
  210.  
  211. //====================================================================
  212. // ConnectInCm
  213. //====================================================================
  214. void CGraph::ConnectInCm(unsigned oneNodeIndex, unsigned twoNodeIndex,
  215.                          int16 radiusOneToTwoCm, int16 radiusTwoToOneCm,
  216.                          unsigned* pLinkOneTwo, unsigned* pLinkTwoOne)
  217. {
  218.         GraphNode* one = GetNodeManager().GetNode(oneNodeIndex);
  219.         GraphNode* two = GetNodeManager().GetNode(twoNodeIndex);
  220.  
  221.         if (pLinkOneTwo)
  222.                 *pLinkOneTwo = 0;
  223.         if (pLinkTwoOne)
  224.                 *pLinkTwoOne = 0;
  225.  
  226.         if (one == two)
  227.                 return;
  228.  
  229.         if (!one || !two)
  230.                 return;
  231.  
  232.         if (!CGraph::ValidateNode(oneNodeIndex, false) || !CGraph::ValidateNode(twoNodeIndex, false))
  233.         {
  234.                 AIError("CGraph::Connect Attempt to connect nodes that aren't created [Code bug]");
  235.                 return;
  236.         }
  237.  
  238.         if ((one == m_pSafeFirst || two == m_pSafeFirst) && m_pSafeFirst->firstLinkIndex)
  239.         {
  240.                 AIWarning("Second link being made to safe/first node");
  241.                 return;
  242.         }
  243.  
  244. #ifdef CRYAISYSTEM_DEBUG
  245.         extern std::vector<const GraphNode*> g_DebugGraphNodesToDraw;
  246.         g_DebugGraphNodesToDraw.clear();
  247. #endif //CRYAISYSTEM_DEBUG
  248.  
  249.         // handle case where they're already connected
  250.         unsigned linkIndexOne = one->GetLinkTo(GetNodeManager(), GetLinkManager(), two);
  251.         unsigned linkIndexTwo = two->GetLinkTo(GetNodeManager(), GetLinkManager(), one);
  252.  
  253.         if ((linkIndexOne == 0) != (linkIndexTwo == 0))
  254.         {
  255.                 AIWarning("Trying to connect links but one is already connected, other isn't");
  256.                 return;
  257.         }
  258.  
  259.         // Check that if both links have bidirectional data, then it is the same.
  260.         assert(linkIndexOne == 0 || linkIndexTwo == 0 || (linkIndexOne & ~1) == (linkIndexTwo & ~1));
  261.  
  262.         // Create new bidirectional data if necessary.
  263.         unsigned linkIndex = linkIndexOne;
  264.         if (!linkIndex && linkIndexTwo)
  265.                 linkIndex = linkIndexTwo ^ 1;
  266.         if (!linkIndex)
  267.                 linkIndex = m_pGraphLinkManager->CreateLink();
  268.  
  269.         Vec3 midPos = 0.5f * (one->GetPos() + two->GetPos());
  270.  
  271.         if (!linkIndexOne)
  272.         {
  273.                 // [1/3/2007 MichaelS] Should be possible to push link on front, but I don't want to run the risk
  274.                 // of breaking code that relies on the link order being preserved, so we add it to the end.
  275.                 //one->links.push_back(linkIndex);
  276.                 if (!one->firstLinkIndex)
  277.                         one->firstLinkIndex = linkIndex;
  278.                 else
  279.                 {
  280.                         unsigned lastLink, nextLink;
  281.                         for (lastLink = one->firstLinkIndex; nextLink = m_pGraphLinkManager->GetNextLink(lastLink); lastLink = nextLink)
  282.                                 ;
  283.                         m_pGraphLinkManager->SetNextLink(lastLink, linkIndex);
  284.                 }
  285.  
  286.                 linkIndexOne = linkIndex;
  287.                 GetLinkManager().SetNextNode(linkIndex, twoNodeIndex);
  288.                 GetLinkManager().SetRadiusInCm(linkIndex, two == m_pSafeFirst ? -100 : radiusOneToTwoCm);
  289.                 GetLinkManager().GetEdgeCenter(linkIndex) = midPos;
  290.                 two->AddRef();
  291.         }
  292.         else
  293.         {
  294.                 if (radiusOneToTwoCm != 0)
  295.                         GetLinkManager().ModifyRadiusInCm(linkIndexOne, radiusOneToTwoCm);
  296.         }
  297.         if (!linkIndexTwo)
  298.         {
  299.                 // [1/3/2007 MichaelS] Should be possible to push link on front, but I don't want to run the risk
  300.                 // of breaking code that relies on the link order being preserved, so we add it to the end.
  301.                 //one->links.push_back(linkIndex);
  302.                 if (!two->firstLinkIndex)
  303.                         two->firstLinkIndex = linkIndex ^ 1;
  304.                 else
  305.                 {
  306.                         unsigned lastLink, nextLink;
  307.                         for (lastLink = two->firstLinkIndex; nextLink = m_pGraphLinkManager->GetNextLink(lastLink); lastLink = nextLink)
  308.                                 ;
  309.                         m_pGraphLinkManager->SetNextLink(lastLink, linkIndex ^ 1);
  310.                 }
  311.  
  312.                 linkIndexTwo = linkIndex ^ 1;
  313.                 GetLinkManager().SetNextNode(linkIndex ^ 1, oneNodeIndex);
  314.                 GetLinkManager().SetRadiusInCm(linkIndex ^ 1, one == m_pSafeFirst ? -100 : radiusTwoToOneCm);
  315.                 GetLinkManager().GetEdgeCenter(linkIndex ^ 1) = midPos;
  316.                 one->AddRef();
  317.         }
  318.         else
  319.         {
  320.                 if (radiusTwoToOneCm != 0)
  321.                         GetLinkManager().ModifyRadiusInCm(linkIndexTwo, radiusTwoToOneCm);
  322.         }
  323.  
  324.         if (pLinkOneTwo && linkIndexOne)
  325.                 *pLinkOneTwo = linkIndexOne;
  326.         if (pLinkTwoOne && linkIndexTwo)
  327.                 *pLinkTwoOne = linkIndexTwo;
  328.  
  329.         if (m_pSafeFirst->firstLinkIndex)
  330.                 return;
  331.  
  332.         if (one->navType == IAISystem::NAV_TRIANGULAR)
  333.         {
  334.                 ConnectInCm(m_safeFirstIndex, oneNodeIndex, 10000, -100);
  335.                 m_pFirst = one;
  336.                 m_firstIndex = oneNodeIndex;
  337.         }
  338.         else if (two->navType == IAISystem::NAV_TRIANGULAR)
  339.         {
  340.                 ConnectInCm(m_safeFirstIndex, twoNodeIndex, 10000, -100);
  341.                 m_pFirst = two;
  342.                 m_firstIndex = twoNodeIndex;
  343.         }
  344.         // may have incurred a reallocation
  345.         // [1/3/2007 MichaelS] Should no longer be necessary since we use indices instead of pointers, but leaving it here for now.
  346.         if (pLinkOneTwo && linkIndexOne)
  347.                 *pLinkOneTwo = linkIndexOne;
  348.         if (pLinkTwoOne && linkIndexTwo)
  349.                 *pLinkTwoOne = linkIndexTwo;
  350. }
  351.  
  352. /// Connects (two-way) two nodes, optionally returning pointers to the new links
  353. void CGraph::Connect(unsigned oneIndex, unsigned twoIndex, float radiusOneToTwo, float radiusTwoToOne,
  354.                      unsigned* pLinkOneTwo, unsigned* pLinkTwoOne)
  355. {
  356.         int16 radiusOneToTwoInCm = NavGraphUtils::InCentimeters(radiusOneToTwo);
  357.         int16 radiusTwoToOneInCm = NavGraphUtils::InCentimeters(radiusTwoToOne);
  358.  
  359.         ConnectInCm(oneIndex, twoIndex, radiusOneToTwoInCm, radiusTwoToOneInCm, pLinkOneTwo, pLinkTwoOne);
  360. }
  361.  
  362. //====================================================================
  363. // DeleteGraph
  364. //====================================================================
  365. void CGraph::DeleteGraph(IAISystem::tNavCapMask navTypeMask)
  366. {
  367.         std::vector<unsigned> nodesToDelete;
  368.         CAllNodesContainer::Iterator it(m_allNodes, navTypeMask);
  369.         while (unsigned nodeIndex = it.Increment())
  370.         {
  371.                 GraphNode* pNode = GetNodeManager().GetNode(nodeIndex);
  372.                 AIAssert(pNode->navType & navTypeMask);
  373.                 nodesToDelete.push_back(nodeIndex);
  374.         }
  375.  
  376.         for (unsigned i = 0; i < nodesToDelete.size(); ++i)
  377.         {
  378.                 GraphNode* pNode = GetNodeManager().GetNode(nodesToDelete[i]);
  379.                 Disconnect(nodesToDelete[i]);
  380.                 if (pNode == m_pSafeFirst)
  381.                 {
  382.                         m_pSafeFirst = 0;
  383.                         m_safeFirstIndex = 0;
  384.                 }
  385.         }
  386.  
  387.         m_allNodes.Compact();
  388. }
  389.  
  390. //====================================================================
  391. // Disconnect
  392. //====================================================================
  393. void CGraph::Disconnect(unsigned nodeIndex, unsigned linkId)
  394. {
  395.         GraphNode* pNode = m_pGraphNodeManager->GetNode(nodeIndex);
  396.  
  397.         if (!CGraph::ValidateNode(nodeIndex, false))
  398.         {
  399.                 AIError("CGraph::Disconnect Attempt to disconnect link from node that isn't created [Code bug]");
  400.                 return;
  401.         }
  402.         AIAssert(linkId);
  403.         unsigned otherNodeIndex = GetLinkManager().GetNextNode(linkId);
  404.         GraphNode* pOtherNode = GetNodeManager().GetNode(otherNodeIndex);
  405.         if (!CGraph::ValidateNode(otherNodeIndex, false))
  406.         {
  407.                 AIError("CGraph::Disconnect Attempt to disconnect link from other node that isn't created [Code bug]");
  408.                 return;
  409.         }
  410.  
  411.         pNode->RemoveLinkTo(GetLinkManager(), otherNodeIndex);
  412.         pNode->Release();
  413.  
  414.         pOtherNode->RemoveLinkTo(GetLinkManager(), nodeIndex);
  415.         pOtherNode->Release();
  416.  
  417.         GetLinkManager().DestroyLink(linkId);
  418. }
  419.  
  420. //====================================================================
  421. // Disconnect
  422. //====================================================================
  423. void CGraph::Disconnect(unsigned nodeIndex, bool bDelete)
  424. {
  425.         GraphNode* pDisconnected = m_pGraphNodeManager->GetNode(nodeIndex);
  426.  
  427.         if (!CGraph::ValidateNode(nodeIndex, false))
  428.         {
  429.                 AIError("CGraph::Disconnect Attempt to disconnect node that isn't created [Code bug]");
  430.                 return;
  431.         }
  432.  
  433. #ifdef CRYAISYSTEM_DEBUG
  434.         extern std::vector<const GraphNode*> g_DebugGraphNodesToDraw;
  435.         g_DebugGraphNodesToDraw.clear();
  436. #endif //CRYAISYSTEM_DEBUG
  437.  
  438.         // if the node we are disconnecting is the current node, move the current
  439.         // to one of his links, or the root if it has no links
  440.         if (pDisconnected == m_pCurrent)
  441.         {
  442.                 if (pDisconnected->firstLinkIndex)
  443.                 {
  444.                         m_currentIndex = GetLinkManager().GetNextNode(pDisconnected->firstLinkIndex);
  445.                         m_pCurrent = GetNodeManager().GetNode(m_currentIndex);
  446.                 }
  447.                 else
  448.                 {
  449.                         m_currentIndex = m_safeFirstIndex;
  450.                         m_pCurrent = m_pSafeFirst;
  451.                 }
  452.         }
  453.  
  454.         // if its the root that is being disconnected, move it
  455.         if (m_pFirst == pDisconnected)
  456.         {
  457.                 if (pDisconnected->firstLinkIndex)
  458.                 {
  459.                         m_firstIndex = GetLinkManager().GetNextNode(pDisconnected->firstLinkIndex);
  460.                         m_pFirst = GetNodeManager().GetNode(m_firstIndex);
  461.                 }
  462.                 else
  463.                 {
  464.                         if (m_pFirst != m_pSafeFirst)
  465.                         {
  466.                                 m_pFirst = m_pSafeFirst;
  467.                                 m_firstIndex = m_safeFirstIndex;
  468.                         }
  469.                         else
  470.                         {
  471.                                 m_pFirst = 0;
  472.                                 m_firstIndex = 0;
  473.                         }
  474.                 }
  475.         }
  476.  
  477.         // now disconnect this node from its links
  478.         for (unsigned link = pDisconnected->firstLinkIndex, nextLink; link; link = nextLink)
  479.         {
  480.                 nextLink = GetLinkManager().GetNextLink(link);
  481.  
  482.                 unsigned nextNodeIndex = GetLinkManager().GetNextNode(link);
  483.                 GraphNode* pNextNode = GetNodeManager().GetNode(nextNodeIndex);
  484.                 pNextNode->RemoveLinkTo(GetLinkManager(), nodeIndex);
  485.                 pNextNode->Release();
  486.                 pDisconnected->Release();
  487.                 GetLinkManager().DestroyLink(link);
  488.         }
  489.  
  490.         pDisconnected->firstLinkIndex = 0;
  491.  
  492.         if (pDisconnected->nRefCount != 1)
  493.                 AIWarning("Node reference count is not 1 after disconnecting");
  494.  
  495.         if (bDelete)
  496.                 DeleteNode(nodeIndex);
  497.  
  498.         if (!m_pSafeFirst)
  499.                 return;
  500.  
  501.         if (pDisconnected != m_pSafeFirst && !m_pSafeFirst->firstLinkIndex)
  502.         {
  503.                 unsigned firstIndex = m_firstIndex;
  504.                 // we have disconnected the link to the dummy safe node - relink it to any outdoor node of the graph
  505.                 if (firstIndex == m_safeFirstIndex)
  506.                 {
  507.                         if (m_currentIndex == m_safeFirstIndex)
  508.                         {
  509.                                 // try any entrance
  510.                                 if (!m_mapEntrances.empty())
  511.                                         firstIndex = (m_mapEntrances.begin()->second);
  512.                                 else
  513.                                         return; // m_pSafeFirst links will stay empty
  514.  
  515.                         }
  516.                         else
  517.                         {
  518.                                 firstIndex = m_currentIndex;
  519.                         }
  520.                 }
  521.  
  522.                 if (firstIndex)
  523.                 {
  524.                         GraphNode* pFirst = GetNodeManager().GetNode(firstIndex);
  525.                         if (pFirst->navType == IAISystem::NAV_TRIANGULAR)
  526.                                 ConnectInCm(m_safeFirstIndex, firstIndex, 10000, -100);
  527.                         else if (pFirst->navType == IAISystem::NAV_WAYPOINT_HUMAN)
  528.                         {
  529.                                 GraphNode* pEntrance = GetEntrance(pFirst->GetWaypointNavData()->nBuildingID, Vec3(0, 0, 0));
  530.                                 if (pEntrance)
  531.                                 {
  532.                                         for (unsigned link = pEntrance->firstLinkIndex; link; link = GetLinkManager().GetNextLink(link))
  533.                                         {
  534.                                                 unsigned nextNodeIndex = GetLinkManager().GetNextNode(link);
  535.                                                 GraphNode* pNextNode = GetNodeManager().GetNode(nextNodeIndex);
  536.                                                 if (pNextNode->navType == IAISystem::NAV_WAYPOINT_HUMAN)
  537.                                                 {
  538.                                                         ConnectInCm(m_safeFirstIndex, nextNodeIndex, 10000, -100);
  539.                                                         break;
  540.                                                 }
  541.                                         }
  542.                                 }
  543.                         }
  544.                 }
  545.         }
  546. }
  547.  
  548. bool operator<(const Vec3r& v1, const Vec3r& v2)
  549. {
  550.         if (v1.x < v2.x)
  551.                 return true;
  552.         else if (v1.x > v2.y)
  553.                 return false;
  554.         if (v1.y < v2.y)
  555.                 return true;
  556.         else if (v1.y > v2.y)
  557.                 return false;
  558.         if (v1.z < v2.z)
  559.                 return true;
  560.         else if (v1.z > v2.z)
  561.                 return false;
  562.         return false;
  563. }
  564.  
  565. //====================================================================
  566. // PointInTriangle
  567. // Check whether a position is within a node's triangle
  568. //====================================================================
  569. bool PointInTriangle(const Vec3& pos, GraphNode* pNode)
  570. {
  571.         // NAV_TRIANGULAR is replaced by MNM
  572.         assert(false);
  573.         return false;
  574. }
  575.  
  576. //#define VALIDATEINMARK
  577.  
  578. //====================================================================
  579. // MarkNode
  580. // uses mark for internal graph operation without disturbing the pathfinder
  581. //====================================================================
  582. void CGraph::MarkNode(unsigned nodeIndex) const
  583. {
  584. #ifdef VALIDATEINMARK
  585.         if (!CGraph::ValidateNode(nodeIndex))
  586.         {
  587.                 AIError("CGraph::MarkNode Unable to validate/mark node %p [Code bug]", pNode);
  588.                 return;
  589.         }
  590. #endif
  591.         GetNodeManager().GetNode(nodeIndex)->mark = 1;
  592.         m_markedNodes.push_back(nodeIndex);
  593. }
  594.  
  595. //====================================================================
  596. // ClearMarks
  597. // clears the marked nodes
  598. //====================================================================
  599. void CGraph::ClearMarks() const
  600. {
  601.         while (!m_markedNodes.empty())
  602.         {
  603. #ifdef VALIDATEINMARK
  604.                 if (!CGraph::ValidateNode(m_markedNodes.back()))
  605.                         AIError("CGraph::ClearMarks Unable to validate/clear mark node %p [Code bug]", m_markedNodes.back());
  606.                 else
  607. #endif
  608.                 GetNodeManager().GetNode(m_markedNodes.back())->mark = 0;
  609.                 m_markedNodes.pop_back();
  610.         }
  611. }
  612.  
  613. //====================================================================
  614. // ReadFromFile
  615. // Reads the AI graph from a specified file
  616. //====================================================================
  617. bool CGraph::ReadFromFile(const char* szName)
  618. {
  619.         CCryFile file;
  620.         ;
  621.         if (file.Open(szName, "rb"))
  622.                 return ReadNodes(file);
  623.         else
  624.                 return false;
  625. }
  626.  
  627. //====================================================================
  628. // ReadNodes
  629. // reads all the nodes in a map
  630. //====================================================================
  631. bool CGraph::ReadNodes(CCryFile& file)
  632. {
  633.         NodeDescBuffer nodeDescs;
  634.  
  635.         //AIAssert(_heapchk()==_HEAPOK);
  636.         int iNumber;
  637.         Vec3 mins, maxs;
  638.  
  639.         AILogLoading("Verifying BAI file version");
  640.         file.ReadType(&iNumber);
  641.         if (iNumber != BAI_TRI_FILE_VERSION)
  642.         {
  643.                 AIError("CGraph::ReadNodes Wrong triangulation BAI file version - found %d expected %d: Regenerate triangulation in the editor [Design bug]", iNumber, BAI_TRI_FILE_VERSION);
  644.                 return false;
  645.         }
  646.  
  647.         AILogLoading("Reading BBOX");
  648.         file.ReadType(&mins);
  649.         file.ReadType(&maxs);
  650.  
  651.         SetBBox(mins, maxs);
  652.  
  653.         AILogLoading("Reading node descriptors");
  654.  
  655.         file.ReadType(&iNumber);
  656.  
  657.         if (iNumber > 0)
  658.         {
  659.                 nodeDescs.resize(iNumber);
  660.                 file.ReadType(&nodeDescs[0], iNumber);
  661.         }
  662.  
  663.         AILogLoading("Verifying graph nodes");
  664.         bool bFoundFirstNode = false;
  665.  
  666.         for (NodeDescBuffer::const_iterator ni = nodeDescs.begin(), end = nodeDescs.end(); ni != end; ++ni)
  667.         {
  668.                 if (ni->ID == 1)
  669.                 {
  670.                         bFoundFirstNode = true;
  671.                         break;
  672.                 }
  673.         }
  674.  
  675.         if (bFoundFirstNode)
  676.         {
  677.                 // m_pSafeFirstIndex overlaps an ID we've read in. We need to disconnect m_pSafeFirst
  678.                 // before we create a node with the same ID to avoid accidentally reusing the ID.
  679.                 if (m_pSafeFirst)
  680.                         Disconnect(m_safeFirstIndex);
  681.                 m_pSafeFirst = NULL;
  682.                 m_safeFirstIndex = 0;
  683.         }
  684.         else
  685.         {
  686.                 AIError("CGraph::ReadNodes First node not found - navigation loading failed [code bug]");
  687.                 gEnv->pLog->UpdateLoadingScreen(" ");
  688.                 return false;
  689.         }
  690.  
  691.         AILogLoading("Creating graph nodes");
  692.  
  693.         typedef std::map<unsigned, unsigned> ReadNodesMap;
  694.         ReadNodesMap mapReadNodes;
  695.  
  696.         I3DEngine* pEngine = gEnv->p3DEngine;
  697.         NodeDescBuffer::iterator ni;
  698.         int index = 0;
  699.  
  700.         if (/*!gEnv->IsEditor() && */ iNumber)
  701.         {
  702.                 //int nodeCounts[IAISystem::NAV_TYPE_COUNT] = {0};
  703.  
  704.                 NodeDescBuffer::iterator end = nodeDescs.end();
  705.                 //for (ni=nodeDescs.begin();ni != end;++ni,++index)
  706.                 //{
  707.                 //      NodeDescriptor desc = (*ni);
  708.                 //      IAISystem::ENavigationType type = (IAISystem::ENavigationType) desc.navType;
  709.  
  710.                 //      int typeIndex = TypeIndexFromType(type);
  711.  
  712.                 //      // in editor waypoint nodes get added by the editor
  713.                 //      // MichaelS - removed check to improve stats consistency.
  714.                 //      if (typeIndex < 0 ||
  715.                 //              (/*type != IAISystem::NAV_TRIANGULAR || */type != IAISystem::NAV_UNSET && gEnv->IsEditor()))
  716.                 //              continue;
  717.  
  718.                 //      ++nodeCounts[typeIndex];
  719.                 //}
  720.  
  721.                 for (ni = nodeDescs.begin(); ni != end; ++ni, ++index)
  722.                 {
  723.                         NodeDescriptor desc = (*ni);
  724.                         IAISystem::ENavigationType type = (IAISystem::ENavigationType) desc.navType;
  725.  
  726.                         unsigned int nodeIndex = 0;
  727.                         GraphNode* pNode = 0;
  728.                         nodeIndex = CreateNewNode(type, desc.pos, desc.ID);
  729.                         pNode = GetNodeManager().GetNode(nodeIndex);
  730.  
  731.                         switch (type)
  732.                         {
  733.                         case IAISystem::NAV_UNSET:
  734.                                 break;
  735.                         case IAISystem::NAV_TRIANGULAR:
  736.                                 {
  737.                                         // NAV_TRIANGULAR is replaced by MNM
  738.                                         assert(false);
  739.                                 }
  740.                                 break;
  741.                         case IAISystem::NAV_WAYPOINT_3DSURFACE:
  742.                         case IAISystem::NAV_WAYPOINT_HUMAN:
  743.                                 {
  744.                                         pNode->GetWaypointNavData()->type = (EWaypointNodeType) desc.type;
  745.  
  746.                                         // building id isn't preserved
  747.                                         pNode->GetWaypointNavData()->nBuildingID = -1;
  748.                                         pNode->GetWaypointNavData()->pArea = 0;
  749.                                         SpecialArea::EType areaType = pNode->navType == IAISystem::NAV_WAYPOINT_HUMAN ? SpecialArea::TYPE_WAYPOINT_HUMAN : SpecialArea::TYPE_WAYPOINT_3DSURFACE;
  750.                                         const SpecialArea* sa = gAIEnv.pNavigation->GetSpecialArea(pNode->GetPos(), areaType);
  751.                                         if (sa)
  752.                                         {
  753.                                                 pNode->GetWaypointNavData()->nBuildingID = sa->nBuildingID;
  754.                                                 I3DEngine* p3dEngine = gEnv->p3DEngine;
  755.                                                 pNode->GetWaypointNavData()->pArea = p3dEngine->GetVisAreaFromPos(pNode->GetPos());
  756.                                         }
  757.  
  758.                                         if (desc.type == WNT_ENTRYEXIT)
  759.                                                 m_mapEntrances.insert(EntranceMap::iterator::value_type(pNode->GetWaypointNavData()->nBuildingID, nodeIndex));
  760.  
  761.                                         if (desc.type == WNT_ENTRYEXIT || desc.type == WNT_EXITONLY)
  762.                                                 m_mapExits.insert(EntranceMap::iterator::value_type(pNode->GetWaypointNavData()->nBuildingID, nodeIndex));
  763.  
  764.                                         // NOTE Oct 15, 2009: <pvl> removable nodes have been unused for
  765.                                         // a very long time now, removing support altogether
  766.                                         assert(!desc.bRemovable);
  767.  
  768.                                         pNode->GetWaypointNavData()->dir = desc.dir;
  769.                                         pNode->GetWaypointNavData()->up = desc.up;
  770.                                 }
  771.                                 break;
  772.                         case IAISystem::NAV_FLIGHT:
  773.                                 AIAssert(!"flight nav should be loaded separately");
  774.                                 pNode->GetFlightNavData()->nSpanIdx = desc.index;
  775.                                 break;
  776.                         case IAISystem::NAV_VOLUME:
  777.                                 AIAssert(!"volume nav should be loaded separately");
  778.                                 pNode->GetVolumeNavData()->nVolimeIdx = desc.index;
  779.                                 break;
  780.                         case IAISystem::NAV_ROAD:
  781.                                 AIAssert(!"road nav should be loaded separately");
  782.                                 pNode->GetRoadNavData()->fRoadWidth = desc.dir.x;
  783.                                 /*pNode->GetRoadNavData()->fRoadOffset = desc.dir.y;*/
  784.                                 break;
  785.                         case IAISystem::NAV_SMARTOBJECT:
  786.                                 AIAssert(!"smart object nav should be loaded separately");
  787.                                 break;
  788.                         default:
  789.                                 AIError("CGraph::ReadNodes Unhandled nav type %d", pNode->navType);
  790.                         }
  791.                         mapReadNodes.insert(ReadNodesMap::value_type(desc.ID, nodeIndex));
  792.  
  793.                 }
  794.  
  795.         }
  796.  
  797.         AILogLoading("Reading links");
  798.  
  799.         LinkDescBuffer linkDescs;
  800.  
  801.         file.ReadType(&iNumber);
  802.         if (iNumber > 0)
  803.         {
  804.                 linkDescs.resize(iNumber);
  805.                 file.ReadType(&linkDescs[0], iNumber);
  806.         }
  807.  
  808.         ReadNodesMap::iterator ei, link;
  809.  
  810.         ei = mapReadNodes.find(1);
  811.         AIAssert(ei != mapReadNodes.end());
  812.         m_safeFirstIndex = (ei->second);
  813.         m_pSafeFirst = GetNodeManager().GetNode(m_safeFirstIndex);
  814.         m_pFirst = m_pSafeFirst;
  815.         m_firstIndex = m_safeFirstIndex;
  816.         m_pCurrent = m_pSafeFirst;
  817.         m_currentIndex = m_safeFirstIndex;
  818.  
  819.         float fStartTime = gEnv->pTimer->GetAsyncCurTime();
  820.         AILogLoading("Reconnecting links");
  821.  
  822.         if (!linkDescs.empty())
  823.         {
  824.                 LinkDescBuffer::iterator iend = linkDescs.end();
  825.                 for (LinkDescBuffer::iterator ldbi = linkDescs.begin(); ldbi != iend; ++ldbi)
  826.                 {
  827.                         LinkDescriptor& ldb = (*ldbi);
  828.  
  829.                         ei = mapReadNodes.find((unsigned)ldb.nSourceNode);
  830.                         if (ei == mapReadNodes.end())
  831.                         {
  832. #if defined __GNUC__
  833.                                 AIWarning("NodeId %lld not found in node map", (long long)ldb.nSourceNode);
  834. #else
  835.                                 AIWarning("NodeId %I64d not found in node map", (int64)ldb.nSourceNode);
  836. #endif
  837.                         }
  838.                         else
  839.                         {
  840.                                 unsigned nodeIndex = ei->second;
  841.                                 GraphNode* pNode = GetNodeManager().GetNode(nodeIndex);
  842.                                 link = mapReadNodes.find((unsigned int)ldb.nTargetNode);
  843.                                 if (link == mapReadNodes.end() || ei == mapReadNodes.end())
  844.                                 {
  845.                                         AIError("CGraph::ReadNodes Read a link to a node which could not be found [Code bug]");
  846.                                 }
  847.                                 else
  848.                                 {
  849.                                         if (pNode != m_pSafeFirst && link->second != m_safeFirstIndex)
  850.                                         {
  851.                                                 unsigned linkOneTwo = 0;
  852.                                                 unsigned linkTwoOne = 0;
  853.                                                 // incoming link gets set when the other way is read
  854.                                                 Connect(nodeIndex, link->second, ldb.fMaxPassRadius, 0.0f, &linkOneTwo, &linkTwoOne);
  855.  
  856.                                                 if (linkOneTwo)
  857.                                                 {
  858.                                                         unsigned nextNodeIndex = link->second;
  859.                                                         GraphNode* pNextNode = GetNodeManager().GetNode(nextNodeIndex);
  860.                                                         AIAssert(GetLinkManager().GetNextNode(linkOneTwo) == nextNodeIndex);
  861.  
  862.                                                         float radius = ldb.fMaxPassRadius;
  863.  
  864.                                                         // Marcio: Hax for Crysis 2 Critters
  865.                                                         // ... but only if the link is passable - hence condition (radius > 0.f) [2/1/2011 evgeny]
  866.                                                         if ((radius > 0.f) && (pNode->navType == IAISystem::NAV_WAYPOINT_HUMAN) && (pNextNode->navType == IAISystem::NAV_WAYPOINT_HUMAN))
  867.                                                         {
  868.                                                                 const SpecialArea* sa1 = gAIEnv.pNavigation->GetSpecialArea(pNode->GetWaypointNavData()->nBuildingID);
  869.                                                                 if (sa1->bCritterOnly)
  870.                                                                         radius = 0.150001f;
  871.                                                                 else
  872.                                                                 {
  873.                                                                         const SpecialArea* sa2 = gAIEnv.pNavigation->GetSpecialArea(pNextNode->GetWaypointNavData()->nBuildingID);
  874.                                                                         if (sa2->bCritterOnly)
  875.                                                                                 radius = 0.150001f;
  876.                                                                 }
  877.                                                         }
  878.  
  879.                                                         if (GetLinkManager().GetNextNode(linkOneTwo) == link->second)
  880.                                                         {
  881.                                                                 GetLinkManager().SetRadius(linkOneTwo, radius);
  882.                                                                 GetLinkManager().SetStartIndex(linkOneTwo, ldb.nStartIndex);
  883.                                                                 GetLinkManager().SetEndIndex(linkOneTwo, ldb.nEndIndex);
  884.                                                                 GetLinkManager().GetEdgeCenter(linkOneTwo) = ldb.vEdgeCenter; // TODO: Don't read shared value twice
  885.                                                                 GetLinkManager().SetExposure(linkOneTwo, ldb.fExposure);      // TODO: Don't read shared value twice
  886.                                                                 GetLinkManager().SetMaxWaterDepth(linkOneTwo, ldb.fMaxWaterDepth);
  887.                                                                 GetLinkManager().SetMinWaterDepth(linkOneTwo, ldb.fMinWaterDepth);
  888.                                                                 GetLinkManager().SetSimple(linkOneTwo, ldb.bSimplePassabilityCheck);
  889.                                                         }
  890.                                                         if (linkTwoOne)
  891.                                                                 GetLinkManager().RestoreLink(linkTwoOne);
  892.                                                 }
  893.                                         }
  894.                                 }
  895.                         }
  896.                 }
  897.         }
  898.  
  899.         AILogLoading("Finished Reconnecting links in %6.3f sec",
  900.                      gEnv->pTimer->GetAsyncCurTime() - fStartTime);
  901.  
  902.         mapReadNodes.clear();
  903.         //AIAssert(_heapchk()==_HEAPOK);
  904.  
  905.         return true;
  906. }
  907.  
  908. //====================================================================
  909. // SetBBox
  910. // defines bounding rectangle of this graph
  911. //====================================================================
  912. void CGraph::SetBBox(const Vec3& min, const Vec3& max)
  913. {
  914.         m_triangularBBox.min = min;
  915.         m_triangularBBox.max = max;
  916. }
  917.  
  918. //====================================================================
  919. // InsideOfBBox
  920. //====================================================================
  921. bool CGraph::InsideOfBBox(const Vec3& pos) const
  922. {
  923.         return pos.x > m_triangularBBox.min.x && pos.x < m_triangularBBox.max.x && pos.y > m_triangularBBox.min.y && pos.y < m_triangularBBox.max.y;
  924. }
  925.  
  926. unsigned CGraph::CreateNewNode(IAISystem::tNavCapMask type, const Vec3& pos, unsigned ID)
  927. {
  928.         MEMSTAT_CONTEXT_FMT(EMemStatContextTypes::MSC_Navigation, 0, "Graph Node (%s)", StringFromTypeIndex(TypeIndexFromType(type)));
  929.         //GraphNode *pNode = new GraphNode(type, pos, ID);
  930.         //GraphNode *pNode = NodesPool.AddGraphNode(type, pos, ID);
  931.         unsigned nodeIndex = m_pGraphNodeManager->CreateNode(type, pos, ID);
  932.         GraphNode* pNode = m_pGraphNodeManager->GetNode(nodeIndex);
  933.         pNode->AddRef();
  934.         m_allNodes.AddNode(nodeIndex);
  935.  
  936.         if (type != IAISystem::NAV_UNSET)
  937.         {
  938.                 CNavRegion* pNavRegion = gAIEnv.pNavigation->GetNavRegion(pNode->navType, this);
  939.                 if (pNavRegion)
  940.                         pNavRegion->NodeCreated(nodeIndex);
  941.         }
  942.  
  943.         return nodeIndex;
  944. }
  945.  
  946. //====================================================================
  947. // GetNode
  948. //====================================================================
  949. GraphNode* CGraph::GetNode(unsigned index)
  950. {
  951.         return GetNodeManager().GetNode(index);
  952. }
  953.  
  954. const GraphNode* CGraph::GetNode(unsigned index) const
  955. {
  956.         return GetNodeManager().GetNode(index);
  957. }
  958.  
  959. //====================================================================
  960. // MoveNode
  961. //====================================================================
  962. void CGraph::MoveNode(unsigned nodeIndex, const Vec3& newPos)
  963. {
  964.         GraphNode* pNode = GetNodeManager().GetNode(nodeIndex);
  965.         if (pNode->GetPos().IsEquivalent(newPos))
  966.                 return;
  967.         m_allNodes.RemoveNode(nodeIndex);
  968.         pNode->SetPos(newPos);
  969.         m_allNodes.AddNode(nodeIndex);
  970.  
  971.         CNavRegion* pNavRegion = gAIEnv.pNavigation->GetNavRegion(pNode->navType, this);
  972.         if (pNavRegion)
  973.                 pNavRegion->NodeMoved(nodeIndex);
  974. }
  975.  
  976. struct SNodeFinder
  977. {
  978.         SNodeFinder(unsigned nodeIndex) : nodeIndex(nodeIndex) {}
  979.         bool operator()(const EntranceMap::value_type& val) const { return val.second == nodeIndex; }
  980.         unsigned nodeIndex;
  981. };
  982.  
  983. //====================================================================
  984. // DeleteNode
  985. //====================================================================
  986. void CGraph::DeleteNode(unsigned nodeIndex)
  987. {
  988.         GraphNode* pNode = GetNodeManager().GetNode(nodeIndex);
  989.  
  990.         if (!CGraph::ValidateNode(nodeIndex, false))
  991.         {
  992.                 AIError("CGraph::DeleteNode Attempting to delete node that doesn't exist %p [Code bug]", pNode);
  993.                 return;
  994.         }
  995.  
  996.         if (pNode->firstLinkIndex)
  997.         {
  998.                 AIWarning("Deleting node %p but it is still connected - disconnecting", pNode);
  999.                 Disconnect(nodeIndex, false);
  1000.         }
  1001.  
  1002.         if (pNode->Release())
  1003.         {
  1004.                 CNavRegion* pNavRegion = gAIEnv.pNavigation ? gAIEnv.pNavigation->GetNavRegion(pNode->navType, this) : NULL;
  1005.                 if (pNavRegion)
  1006.                 {
  1007.                         pNavRegion->NodeAboutToBeDeleted(pNode);
  1008.                 }
  1009.  
  1010.                 m_allNodes.RemoveNode(nodeIndex);
  1011.  
  1012.                 VectorConstNodeIndices::iterator it;
  1013.                 it = std::remove(m_taggedNodes.begin(), m_taggedNodes.end(), nodeIndex);
  1014.                 m_taggedNodes.erase(it, m_taggedNodes.end());
  1015.                 it = std::remove(m_markedNodes.begin(), m_markedNodes.end(), nodeIndex);
  1016.                 m_markedNodes.erase(it, m_markedNodes.end());
  1017.  
  1018.                 EntranceMap::iterator entranceIt;
  1019.                 entranceIt = std::find_if(m_mapEntrances.begin(), m_mapEntrances.end(), SNodeFinder(nodeIndex));
  1020.                 if (entranceIt != m_mapEntrances.end())
  1021.                         m_mapEntrances.erase(entranceIt);
  1022.                 entranceIt = std::find_if(m_mapExits.begin(), m_mapExits.end(), SNodeFinder(nodeIndex));
  1023.                 if (entranceIt != m_mapExits.end())
  1024.                         m_mapExits.erase(entranceIt);
  1025.  
  1026.                 m_pGraphNodeManager->DestroyNode(nodeIndex);
  1027.         }
  1028. }
  1029.  
  1030. struct SVolumeHideSpotFinder
  1031. {
  1032.         SVolumeHideSpotFinder(ListObstacles& obs) : obs(obs) {}
  1033.  
  1034.         void operator()(const SVolumeHideSpot& hs, float)
  1035.         {
  1036.                 obs.push_back(ObstacleData(hs.pos, hs.dir));
  1037.         }
  1038.  
  1039. private:
  1040.         ListObstacles& obs;
  1041. };
  1042.  
  1043. //===================================================================
  1044. // FindNodesWithinRange
  1045. //===================================================================
  1046. void CGraph::FindNodesWithinRange(MapConstNodesDistance& result, float curDistT, float maxDist,
  1047.                                   const GraphNode* pStartNode, float passRadius, const class CAIObject* pRequester) const
  1048. {
  1049.         FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  1050.  
  1051.         bool checkWaterDepth = false;
  1052.         int16 minWaterDepthInCm = 0;
  1053.         int16 maxWaterDepthInCm = 0;
  1054.  
  1055.         if (pRequester && pRequester->CastToCAIActor())
  1056.         {
  1057.                 const CAIActor* pActor = pRequester->CastToCAIActor();
  1058.                 if (pActor)
  1059.                 {
  1060.                         minWaterDepthInCm = NavGraphUtils::InCentimeters(pActor->m_movementAbility.pathfindingProperties.minWaterDepth);
  1061.                         maxWaterDepthInCm = NavGraphUtils::InCentimeters(pActor->m_movementAbility.pathfindingProperties.maxWaterDepth);
  1062.                         checkWaterDepth = true;
  1063.                 }
  1064.         }
  1065.  
  1066.         const CGraphLinkManager& linkManager = GetLinkManager();
  1067.         const CGraphNodeManager& nodeManager = GetNodeManager();
  1068.  
  1069.         typedef std::multimap<float, const GraphNode*, std::less<float>> OpenListMap;
  1070.  
  1071.         static OpenListMap openList;
  1072.         openList.clear();
  1073.  
  1074.         pStartNode->mark = 1;
  1075.         openList.insert(std::make_pair(0.0f, pStartNode));
  1076.         //      openList[0.0f] = pStartNode;
  1077.  
  1078.         result[pStartNode] = 0.0f;
  1079.  
  1080.         while (!openList.empty())
  1081.         {
  1082.                 OpenListMap::iterator front = openList.begin();
  1083.                 const GraphNode* pNode = front->second;
  1084.                 float curDist = front->first;
  1085.                 openList.erase(front);
  1086.                 pNode->mark = 0;
  1087.  
  1088.                 for (unsigned link = pNode->firstLinkIndex; link; link = linkManager.GetNextLink(link))
  1089.                 {
  1090.                         float fCostMultiplier = 1.0f;
  1091.                         unsigned int nextNodeIndex = linkManager.GetNextNode(link);
  1092.                         const GraphNode* pNext = nodeManager.GetNode(nextNodeIndex);
  1093.  
  1094.                         if (!(pNext->navType & (IAISystem::NAV_SMARTOBJECT | IAISystem::NAV_WAYPOINT_HUMAN | IAISystem::NAV_WAYPOINT_3DSURFACE | IAISystem::NAV_TRIANGULAR | IAISystem::NAV_VOLUME)))
  1095.                                 continue;
  1096.  
  1097.                         if (pRequester && pNode->navType == IAISystem::NAV_SMARTOBJECT && pNext->navType == IAISystem::NAV_SMARTOBJECT)
  1098.                         {
  1099.                                 const GraphNode* nodes[2] = { pNode, pNext };
  1100.                                 float resFactor = gAIEnv.pSmartObjectManager->GetSmartObjectLinkCostFactor(nodes, pRequester, &fCostMultiplier);
  1101.                                 if (resFactor < 0.0f)
  1102.                                         continue;
  1103.                         }
  1104.                         else if (linkManager.GetRadius(link) < passRadius)
  1105.                         {
  1106.                                 continue;
  1107.                         }
  1108.                         else if (checkWaterDepth)
  1109.                         {
  1110.                                 int16 wd = linkManager.GetMaxWaterDepthInCm(link);
  1111.                                 if (wd > maxWaterDepthInCm)
  1112.                                         continue;
  1113.  
  1114.                                 if (wd < minWaterDepthInCm)
  1115.                                         continue;
  1116.                         }
  1117.  
  1118.                         float linkLen = 0.01f + fCostMultiplier * Distance::Point_Point(pNode->GetPos(), pNext->GetPos()); // bias to prevent loops
  1119.  
  1120.                         float totalDist = curDist + linkLen;
  1121.                         if (totalDist <= maxDist)
  1122.                         {
  1123.                                 // If we've already processed pNext and had a lower range value before then don't continue
  1124.                                 MapConstNodesDistance::iterator it = result.find(pNext);
  1125.                                 if (it != result.end())
  1126.                                 {
  1127.                                         if (totalDist >= it->second)
  1128.                                                 continue;
  1129.                                         // Smaller distance, update value.
  1130.                                         it->second = totalDist;
  1131.                                 }
  1132.                                 else
  1133.                                 {
  1134.                                         result[pNext] = totalDist;
  1135.                                 }
  1136.  
  1137.                                 // Update open list.
  1138.                                 OpenListMap::iterator found = openList.end();
  1139.                                 if (pNext->mark == 1)
  1140.                                 {
  1141.                                         for (OpenListMap::iterator it2 = openList.begin(), end = openList.end(); it2 != end; ++it2)
  1142.                                         {
  1143.                                                 if (it2->second == pNext)
  1144.                                                 {
  1145.                                                         found = it2;
  1146.                                                         break;
  1147.                                                 }
  1148.                                         }
  1149.                                 }
  1150.  
  1151.                                 if (found != openList.end())
  1152.                                 {
  1153.                                         // Replace value in open list.
  1154.                                         if (totalDist < found->first)
  1155.                                         {
  1156.                                                 openList.erase(found);
  1157.                                                 openList.insert(std::make_pair(totalDist, pNext));
  1158.                                         }
  1159.                                 }
  1160.                                 else
  1161.                                 {
  1162.                                         // Add to the open list
  1163.                                         pNext->mark = 1;
  1164.                                         openList.insert(std::make_pair(totalDist, pNext));
  1165.                                 }
  1166.                         }
  1167.                 }
  1168.         }
  1169.         openList.clear();
  1170. }
  1171.  
  1172. //====================================================================
  1173. // GetNodesInRange
  1174. //====================================================================
  1175. MapConstNodesDistance& CGraph::GetNodesInRange(MapConstNodesDistance& result, const Vec3& startPos,
  1176.                                                float maxDist, IAISystem::tNavCapMask navCapMask, float passRadius,
  1177.                                                unsigned startNodeIndex, const class CAIObject* pRequester)
  1178. {
  1179.         FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  1180.         result.clear();
  1181.  
  1182.         GraphNode* pStart = GetNodeManager().GetNode(startNodeIndex);
  1183.  
  1184.         const CAllNodesContainer& allNodes = GetAllNodes();
  1185.         const CAllNodesContainer::Iterator it(allNodes, IAISystem::NAV_TRIANGULAR | IAISystem::NAV_VOLUME);
  1186.         if (!it.GetNode())
  1187.                 return result; // no navigation
  1188.  
  1189.         unsigned nodeIndex = 0;
  1190.         if (pStart && !allNodes.DoesNodeExist(startNodeIndex))
  1191.         {
  1192.                 startNodeIndex = 0;
  1193.                 pStart = 0;
  1194.         }
  1195.  
  1196.         if (pStart)
  1197.         {
  1198.                 if ((pStart->navType & navCapMask) && startPos.IsEquivalent(pStart->GetPos(), 0.01f))
  1199.                 {
  1200.                         nodeIndex = startNodeIndex;
  1201.                 }
  1202.         }
  1203.  
  1204.         if (!nodeIndex)
  1205.         {
  1206.                 return result;
  1207.         }
  1208.  
  1209.         // don't add the distance to the current node since that
  1210.         float curDist = 0.0f; //Distance::Point_Point(startPos, pNode->GetPos());
  1211.         FindNodesWithinRange(result, curDist, maxDist, GetNodeManager().GetNode(nodeIndex), passRadius, pRequester);
  1212.         return result;
  1213. }
  1214.  
  1215. //====================================================================
  1216. // Reset
  1217. //====================================================================
  1218. void CGraph::Reset()
  1219. {
  1220.         ClearMarks();
  1221.         RestoreAllNavigation();
  1222. }
  1223.  
  1224. void CGraph::ResetIDs()
  1225. {
  1226.         GraphNode::ResetIDs(GetNodeManager(), m_allNodes, m_pSafeFirst);
  1227. }
  1228.  
  1229. //====================================================================
  1230. // GetEntrance
  1231. //====================================================================
  1232. GraphNode* CGraph::GetEntrance(int nBuildingID, const Vec3& pos)
  1233. {
  1234.         GraphNode* pEntrance = 0;
  1235.         float mindist = 1000000;
  1236.         EntranceMap::iterator ei = m_mapEntrances.find(nBuildingID);
  1237.         if (ei != m_mapEntrances.end())
  1238.         {
  1239.                 pEntrance = GetNodeManager().GetNode(ei->second);
  1240.                 if (m_mapEntrances.count(nBuildingID) > 1)
  1241.                 {
  1242.                         mindist = (pEntrance->GetPos() - pos).GetLengthSquared();
  1243.                         for (; ei != m_mapEntrances.end(); ++ei)
  1244.                         {
  1245.                                 if (ei->first != nBuildingID)
  1246.                                         break;
  1247.                                 float curr_dist = (GetNodeManager().GetNode(ei->second)->GetPos() - pos).GetLengthSquared();
  1248.                                 if (curr_dist <= mindist)
  1249.                                 {
  1250.                                         pEntrance = GetNodeManager().GetNode(ei->second);
  1251.                                         mindist = curr_dist;
  1252.                                 }
  1253.                         }
  1254.                 }
  1255.         }
  1256.  
  1257.         ei = m_mapExits.find(nBuildingID);
  1258.         if (ei != m_mapExits.end())
  1259.         {
  1260.                 pEntrance = GetNodeManager().GetNode(ei->second);
  1261.                 if (m_mapExits.count(nBuildingID) > 1)
  1262.                 {
  1263.                         mindist = (pEntrance->GetPos() - pos).GetLengthSquared();
  1264.                         for (; ei != m_mapExits.end(); ++ei)
  1265.                         {
  1266.                                 if (ei->first != nBuildingID)
  1267.                                         break;
  1268.                                 float curr_dist = (GetNodeManager().GetNode(ei->second)->GetPos() - pos).GetLengthSquared();
  1269.                                 if (curr_dist <= mindist)
  1270.                                 {
  1271.                                         pEntrance = GetNodeManager().GetNode(ei->second);
  1272.                                         mindist = curr_dist;
  1273.                                 }
  1274.                         }
  1275.                 }
  1276.         }
  1277.  
  1278.         return pEntrance;
  1279. }
  1280.  
  1281. //====================================================================
  1282. // GetEntrances
  1283. //====================================================================
  1284. bool CGraph::GetEntrances(int nBuildingID, const Vec3& pos, std::vector<unsigned>& nodes)
  1285. {
  1286.         EntranceMap::iterator it1 = m_mapEntrances.lower_bound(nBuildingID);
  1287.         EntranceMap::iterator it2 = m_mapEntrances.upper_bound(nBuildingID);
  1288.         for (EntranceMap::iterator it = it1; it != it2; ++it)
  1289.                 nodes.push_back(it->second);
  1290.         it1 = m_mapExits.lower_bound(nBuildingID);
  1291.         it2 = m_mapExits.upper_bound(nBuildingID);
  1292.         for (EntranceMap::iterator it = it1; it != it2; ++it)
  1293.                 nodes.push_back(it->second);
  1294.         return !nodes.empty();
  1295. }
  1296.  
  1297. //====================================================================
  1298. // RestoreAllNavigation
  1299. // for all removable nodes restore links passibility etc
  1300. //====================================================================
  1301. void CGraph::RestoreAllNavigation()
  1302. {
  1303.         CAllNodesContainer::Iterator it(m_allNodes, IAISystem::NAVMASK_ALL);
  1304.         while (GraphNode* node = GetNodeManager().GetNode(it.Increment()))
  1305.         {
  1306.                 for (unsigned link = node->firstLinkIndex; link; link = GetLinkManager().GetNextLink(link))
  1307.                 {
  1308.                         GetLinkManager().RestoreLink(link);
  1309.                 }
  1310.         }
  1311. }
  1312.  
  1313. //====================================================================
  1314. // CheckForEmpty
  1315. //====================================================================
  1316. bool CGraph::CheckForEmpty(IAISystem::tNavCapMask navTypeMask) const
  1317. {
  1318.         unsigned count = 0;
  1319.         CAllNodesContainer::Iterator it(m_allNodes, navTypeMask);
  1320.         while (const GraphNode* node = GetNodeManager().GetNode(it.Increment()))
  1321.         {
  1322.                 ++count;
  1323.                 AILogEvent("Unexpected Node %p, type = %d, pos = (%5.2f %5.2f %5.2f)", node, node->navType,
  1324.                            node->GetPos().x, node->GetPos().y, node->GetPos().z);
  1325.         }
  1326.         if (count)
  1327.                 AIWarning("Detected %d unexpected nodes whilst checking types %u", count, (unsigned)navTypeMask);
  1328.         return (count == 0);
  1329. }
  1330.  
  1331. //====================================================================
  1332. // Validate
  1333. //====================================================================
  1334. bool CGraph::Validate(const char* msg, bool checkPassable) const
  1335. {
  1336. #ifdef _DEBUG
  1337.         return true;
  1338. #endif
  1339.  
  1340. #ifdef CRYAISYSTEM_VERBOSITY
  1341.         if (!AIGetWarningErrorsEnabled())
  1342.         {
  1343.                 AILogEvent("CGraph::Validate Skipping: %s", msg);
  1344.                 return true;
  1345.         }
  1346.  
  1347.         AILogProgress("CGraph::Validate Starting: %s", msg);
  1348.  
  1349.         // Danny todo tweak this so that if !checkPassable then we only clear
  1350.         // errors that are not to do with the passable checks...
  1351.         if (checkPassable)
  1352.                 mBadGraphData.clear();
  1353.  
  1354.         if (!m_pFirst)
  1355.                 return false;
  1356.  
  1357.         if (!m_pSafeFirst->firstLinkIndex)
  1358.                 return true;
  1359.  
  1360.         // the first safe node is different - expect only one link
  1361.         if (GetLinkManager().GetNextLink(m_pSafeFirst->firstLinkIndex) != 0)
  1362.         {
  1363.                 AIWarning("CGraph::Validate Expect only 1 link to the first safe node");
  1364.                 return false;
  1365.         }
  1366.  
  1367.         int badOutdoorNodes = 0;
  1368.         int badIndoorNodes = 0;
  1369.         int badLinks = 0;
  1370.         int badNumOutsideLinks = 0;
  1371.         int badLinkPassability = 0;
  1372.  
  1373.         static int maxBadImassabilityWarnings = 10;
  1374.  
  1375.         // nodes containing vertices with identical indices
  1376.         int indexDegenerates = 0;
  1377.         // nodes containing vertices that have different indices, but essentially
  1378.         // the same position.
  1379.         int posDegenerates = 0;
  1380.  
  1381.         CAllNodesContainer::Iterator it(m_allNodes, IAISystem::NAVMASK_ALL);
  1382.         while (unsigned nodeIndex = it.Increment())
  1383.         {
  1384.                 const GraphNode* node = GetNodeManager().GetNode(nodeIndex);
  1385.                 if (!CGraph::ValidateNode(nodeIndex, true))
  1386.                         AIWarning("CGraph::Validate Node validation failed: %p", node);
  1387.  
  1388.                 if (node->navType == IAISystem::NAV_WAYPOINT_HUMAN)
  1389.                 {
  1390.                         if (node->GetWaypointNavData()->nBuildingID == -1)
  1391.                         {
  1392.                                 AIWarning("Node at (%5.2f, %5.2f, %5.2f) is not in a human waypoint nav modifier [design bug]",
  1393.                                           node->GetPos().x, node->GetPos().y, node->GetPos().z);
  1394.                                 ++badIndoorNodes;
  1395.                         }
  1396.                 }
  1397.                 else if (node->navType == IAISystem::NAV_WAYPOINT_3DSURFACE)
  1398.                 {
  1399.                         if (node->GetWaypointNavData()->nBuildingID == -1)
  1400.                         {
  1401.                                 AIWarning("Node at (%5.2f, %5.2f, %5.2f) is not in a 3d surface nav modifier [design bug]",
  1402.                                           node->GetPos().x, node->GetPos().y, node->GetPos().z);
  1403.                                 ++badIndoorNodes;
  1404.                         }
  1405.                 }
  1406.                 else if (node->navType == IAISystem::NAV_TRIANGULAR)
  1407.                 {
  1408.                         // NAV_TRIANGULAR is replaced by MNM
  1409.                         assert(false);
  1410.                 }
  1411.         }
  1412.  
  1413.         if (badOutdoorNodes + badIndoorNodes + badNumOutsideLinks + indexDegenerates + posDegenerates + badLinkPassability > 0)
  1414.         {
  1415.                 AIError("CGraph::Validate AI graph contains invalid: %d outdoor, %d indoor, %d outdoor links, %d degenerate index, %d degenerate pos, %d passable: %s: Regenerate triangulation in editor [Design bug]",
  1416.                         badOutdoorNodes, badIndoorNodes, badNumOutsideLinks, indexDegenerates, posDegenerates, badLinkPassability, msg);
  1417.         }
  1418.  
  1419.         AILogProgress("CGraph::Validate Finished graph validation: %s", msg);
  1420.         return (0 == badOutdoorNodes + badIndoorNodes);
  1421. #else
  1422.         return true;
  1423. #endif
  1424. }
  1425.  
  1426. //===================================================================
  1427. // ResetIDs
  1428. //===================================================================
  1429. void GraphNode::ResetIDs(CGraphNodeManager& nodeManager, class CAllNodesContainer& allNodes, GraphNode* pNodeForID1)
  1430. {
  1431.         AIAssert(pNodeForID1);
  1432.  
  1433.         CAllNodesContainer::Iterator itAll(allNodes, 0xffffffff);
  1434.  
  1435.         freeIDs.clear();
  1436. #ifdef DEBUG_GRAPHNODE_IDS
  1437.         usedIds.clear();
  1438. #endif
  1439.  
  1440.         maxID = 1;
  1441.         while (GraphNode* pCurrent = nodeManager.GetNode(itAll.Increment()))
  1442.         {
  1443.                 if (pCurrent == pNodeForID1)
  1444.                         pCurrent->ID = 1;
  1445.                 else
  1446.                         pCurrent->ID = ++maxID;
  1447.  
  1448. #ifdef DEBUG_GRAPHNODE_IDS
  1449.                 usedIds.push_back(pCurrent->ID);
  1450. #endif
  1451.         }
  1452. }
  1453.  
downloadGraph.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