BVB Source Codes

CRYENGINE Show NavPath.cpp Source code

Return Download CRYENGINE: download NavPath.cpp Source code - Download CRYENGINE Source code - Type:.cpp
  1. // Copyright 2001-2016 Crytek GmbH / Crytek Group. All rights reserved.
  2.  
  3. /********************************************************************
  4.    -------------------------------------------------------------------------
  5.    File name:   NavPath.cpp
  6.    $Id$
  7.    Description:
  8.  
  9.    -------------------------------------------------------------------------
  10.    History:
  11.    - 13:1:2005   15:53 : Created by Kirill Bulatsev
  12.    - 4 May 2009        : Evgeny Adamenkov: Replaced IRenderer with CDebugDrawContext
  13.  
  14.  *********************************************************************/
  15.  
  16. #include "StdAfx.h"
  17. #include "NavPath.h"
  18. #include "AILog.h"
  19. #include "CAISystem.h"
  20. #include <CryNetwork/ISerialize.h>
  21. #include "AICollision.h"
  22. #include "Puppet.h"
  23.  
  24. #include <CrySystem/ISystem.h>
  25. #include <CrySystem/IConsole.h>
  26. #include "DebugDrawContext.h"
  27.  
  28. #include "Navigation/NavigationSystem/OffMeshNavigationManager.h"
  29. #include "Navigation/NavigationSystem/NavigationSystem.h"
  30. #include "Navigation/MNM/OffGridLinks.h"
  31.  
  32. #include <numeric>
  33. #include <algorithm>
  34.  
  35. //====================================================================
  36. // CNavPath
  37. //====================================================================
  38. CNavPath::CNavPath()
  39.         : m_endDir(ZERO)
  40.         , m_currentFrac(0.0f)
  41.         , m_stuckTime(0.0f)
  42.         , m_pathEndIsAsRequested(true)
  43.         , m_fDiscardedPathLength(0)
  44. {
  45. }
  46.  
  47. //====================================================================
  48. // ~CNavPath
  49. //====================================================================
  50. CNavPath::~CNavPath()
  51. {
  52. }
  53.  
  54. NavigationMeshID CNavPath::GetMeshID() const
  55. {
  56.         return m_params.meshID;
  57. }
  58.  
  59. //====================================================================
  60. // Advance
  61. //====================================================================
  62. bool CNavPath::Advance(PathPointDescriptor& nextPathPoint)
  63. {
  64.         if (m_pathPoints.size() <= 1)
  65.                 return false;
  66.         ++m_version.v;
  67.         m_pathPoints.erase(m_pathPoints.begin());
  68.         if (m_pathPoints.size() == 1)
  69.         {
  70.                 m_currentFrac = 0.0f;
  71.                 return false;
  72.         }
  73.         nextPathPoint = *GetNextPathPoint();
  74.         return true;
  75. }
  76.  
  77. template<class F>
  78. ILINE bool IsEquivalent2D(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, f32 epsilon = VEC_EPSILON)
  79. {
  80.         return  ((fabs_tpl(v0.x - v1.x) <= epsilon) && (fabs_tpl(v0.y - v1.y) <= epsilon));
  81. }
  82.  
  83. //====================================================================
  84. // push_front
  85. //====================================================================
  86. void CNavPath::PushFront(const PathPointDescriptor& newPathPoint, bool force)
  87. {
  88.         ++m_version.v;
  89.         if (!force && !m_pathPoints.empty())
  90.         {
  91.                 IAISystem::ENavigationType newType = newPathPoint.navType;
  92.                 IAISystem::ENavigationType curType = m_pathPoints.front().navType;
  93.  
  94.                 bool twoD = (curType & (IAISystem::NAV_TRIANGULAR | IAISystem::NAV_WAYPOINT_HUMAN)) != 0 ||
  95.                             (newType & (IAISystem::NAV_TRIANGULAR | IAISystem::NAV_WAYPOINT_HUMAN)) != 0;
  96.  
  97.                 // Don't allow null length segments!
  98.                 if (twoD ? IsEquivalent2D(newPathPoint.vPos, m_pathPoints.front().vPos) : IsEquivalent(newPathPoint.vPos, m_pathPoints.front().vPos))
  99.                 {
  100.                         if (newType != IAISystem::NAV_SMARTOBJECT && curType != IAISystem::NAV_SMARTOBJECT && newType != IAISystem::NAV_CUSTOM_NAVIGATION && curType != IAISystem::NAV_CUSTOM_NAVIGATION)
  101.                                 return;
  102.                 }
  103.         }
  104.         m_pathPoints.insert(m_pathPoints.begin(), newPathPoint);
  105. }
  106.  
  107. //====================================================================
  108. // PushBack
  109. //====================================================================
  110. void CNavPath::PushBack(const PathPointDescriptor& newPathPoint, bool force)
  111. {
  112.         ++m_version.v;
  113.         // Don't allow null length segments!
  114.         if (!force && !m_pathPoints.empty())
  115.         {
  116.                 IAISystem::ENavigationType newType = newPathPoint.navType;
  117.                 IAISystem::ENavigationType curType = m_pathPoints.back().navType;
  118.                 bool twoD = (curType & (IAISystem::NAV_TRIANGULAR | IAISystem::NAV_WAYPOINT_HUMAN)) != 0 ||
  119.                             (newType & (IAISystem::NAV_TRIANGULAR | IAISystem::NAV_WAYPOINT_HUMAN)) != 0;
  120.  
  121.                 if (twoD ? IsEquivalent2D(newPathPoint.vPos, m_pathPoints.back().vPos) : IsEquivalent(newPathPoint.vPos, m_pathPoints.back().vPos))
  122.                 {
  123.                         if (newType != IAISystem::NAV_SMARTOBJECT && curType != IAISystem::NAV_SMARTOBJECT && newType != IAISystem::NAV_CUSTOM_NAVIGATION && curType != IAISystem::NAV_CUSTOM_NAVIGATION)
  124.                                 return;
  125.                 }
  126.         }
  127.         m_pathPoints.push_back(newPathPoint);
  128. }
  129.  
  130. //====================================================================
  131. // GetLength
  132. //====================================================================
  133. float CNavPath::GetPathLength(bool b2D) const
  134. {
  135.         Vec3 vPos;
  136.         if (!GetPosAlongPath(vPos))
  137.                 return 0.f;
  138.  
  139.         float length = 0.f;
  140.         if (m_pathPoints.size() > 1)
  141.         {
  142.                 TPathPoints::const_iterator li = m_pathPoints.begin();
  143.                 for (++li; li != m_pathPoints.end(); ++li)
  144.                 {
  145.                         const Vec3 vDelta = li->vPos - vPos;
  146.                         length += b2D ? vDelta.GetLength2D() : vDelta.GetLength();
  147.                         vPos = li->vPos;
  148.                 }
  149.         }
  150.         return length;
  151. }
  152.  
  153. //====================================================================
  154. // Draw
  155. //====================================================================
  156. void CNavPath::Draw(const Vec3& drawOffset) const
  157. {
  158.         bool useTerrain = false;
  159.  
  160.         CDebugDrawContext dc;
  161.  
  162.         if (!m_pathPoints.empty())
  163.         {
  164.                 TPathPoints::const_iterator li, linext;
  165.                 li = m_pathPoints.begin();
  166.                 linext = li;
  167.                 ++linext;
  168.                 while (linext != m_pathPoints.end())
  169.                 {
  170.                         Vec3 p0 = li->vPos;
  171.                         Vec3 p1 = linext->vPos;
  172.                         if (li->navType == IAISystem::NAV_TRIANGULAR)
  173.                                 useTerrain = true;
  174.  
  175.                         p0.z = dc->GetDebugDrawZ(p0, li->navType == IAISystem::NAV_TRIANGULAR);
  176.                         p1.z = dc->GetDebugDrawZ(p1, li->navType == IAISystem::NAV_TRIANGULAR);
  177.                         dc->DrawLine(p0, Col_SteelBlue, p1, Col_SteelBlue);
  178.  
  179.                         ColorB color = Col_Grey;
  180.  
  181.                         switch (li->navType)
  182.                         {
  183.                         case IAISystem::NAV_SMARTOBJECT:
  184.                                 color = Col_SlateBlue;
  185.                                 break;
  186.                         case IAISystem::NAV_TRIANGULAR:
  187.                                 color = Col_Brown;
  188.                                 break;
  189.                         case IAISystem::NAV_WAYPOINT_HUMAN:
  190.                                 color = Col_Cyan;
  191.                                 break;
  192.                         default:
  193.                                 color = Col_Grey;
  194.                                 break;
  195.                         }
  196.  
  197.                         li = linext;
  198.                         ++linext;
  199.                 }
  200.         }
  201.  
  202.         if (!m_remainingPathPoints.empty())
  203.         {
  204.                 TPathPoints::const_iterator li, linext;
  205.                 li = m_remainingPathPoints.begin();
  206.                 linext = li;
  207.                 ++linext;
  208.                 while (linext != m_remainingPathPoints.end())
  209.                 {
  210.                         Vec3 p0 = li->vPos;
  211.                         Vec3 p1 = linext->vPos;
  212.                         if (li->navType == IAISystem::NAV_TRIANGULAR)
  213.                                 useTerrain = true;
  214.  
  215.                         p0.z = dc->GetDebugDrawZ(p0, li->navType == IAISystem::NAV_TRIANGULAR);
  216.                         p1.z = dc->GetDebugDrawZ(p1, li->navType == IAISystem::NAV_TRIANGULAR);
  217.                         dc->DrawLine(p0, Col_DarkOrchid, p1, Col_DarkOrchid);
  218.  
  219.                         ColorB color = Col_Grey;
  220.  
  221.                         switch (li->navType)
  222.                         {
  223.                         case IAISystem::NAV_SMARTOBJECT:
  224.                                 color = Col_SlateBlue;
  225.                                 break;
  226.                         case IAISystem::NAV_TRIANGULAR:
  227.                                 color = Col_Brown;
  228.                                 break;
  229.                         case IAISystem::NAV_WAYPOINT_HUMAN:
  230.                                 color = Col_Cyan;
  231.                                 break;
  232.                         default:
  233.                                 color = Col_Grey;
  234.                                 break;
  235.                         }
  236.                         dc->DrawSphere(p1, 0.125f, color);
  237.  
  238.                         li = linext;
  239.                         ++linext;
  240.                 }
  241.         }
  242.  
  243.         std::list<SDebugLine>::const_iterator dli;
  244.         for (dli = m_debugLines.begin(); dli != m_debugLines.end(); ++dli)
  245.         {
  246.                 SDebugLine line = (*dli);
  247.                 line.P0.z = dc->GetDebugDrawZ(line.P0, useTerrain);
  248.                 line.P1.z = dc->GetDebugDrawZ(line.P1, useTerrain);
  249.                 dc->DrawLine(line.P0, line.col, line.P1, line.col);
  250.         }
  251.         std::list<SDebugSphere>::const_iterator dsi;
  252.         for (dsi = m_debugSpheres.begin(); dsi != m_debugSpheres.end(); ++dsi)
  253.         {
  254.                 SDebugSphere sphere = (*dsi);
  255.                 sphere.pos.z = dc->GetDebugDrawZ(sphere.pos, useTerrain);
  256.                 dc->DrawSphere(sphere.pos, sphere.r, sphere.col);
  257.         }
  258. }
  259.  
  260. //====================================================================
  261. // empty
  262. //====================================================================
  263. bool CNavPath::Empty() const
  264. {
  265.         return m_pathPoints.size() < 2;
  266. }
  267.  
  268. //====================================================================
  269. // Clear
  270. //====================================================================
  271. void CNavPath::Clear(const char* dbgString)
  272. {
  273.         ++m_version.v;
  274.         if (gAIEnv.CVars.DebugPathFinding && !m_pathPoints.empty())
  275.         {
  276.                 const PathPointDescriptor& ppd = m_pathPoints.back();
  277.                 AILogAlways("CNavPath::Clear old path end is (%5.2f, %5.2f, %5.2f) %s",
  278.                             ppd.vPos.x, ppd.vPos.y, ppd.vPos.z, dbgString);
  279.         }
  280.         m_params.Clear();
  281.         m_pathPoints.clear();
  282.         m_debugLines.clear();
  283.         m_debugSpheres.clear();
  284.         m_currentFrac = 0.0f;
  285.         m_stuckTime = 0.0f;
  286.         m_pathEndIsAsRequested = true;
  287.         m_fDiscardedPathLength = 0;
  288. }
  289.  
  290. //====================================================================
  291. // SetPreviousPoint
  292. //====================================================================
  293. void CNavPath::SetPreviousPoint(const PathPointDescriptor& previousPoint)
  294. {
  295.         if (m_pathPoints.empty())
  296.                 return;
  297.         ++m_version.v;
  298.         m_pathPoints.front() = previousPoint;
  299. }
  300.  
  301. //====================================================================
  302. // DebugLine
  303. //====================================================================
  304. void CNavPath::DebugLine(const Vec3& P0, const Vec3& P1, const ColorF& col) const
  305. {
  306.         if (gAIEnv.CVars.DebugDraw == 0)
  307.                 return;
  308.         SDebugLine line;
  309.         line.P0 = P0;
  310.         line.P1 = P1;
  311.         line.col = col;
  312.         m_debugLines.push_back(line);
  313. }
  314.  
  315. void CNavPath::DebugSphere(const Vec3& pos, float r, const ColorF& col) const
  316. {
  317.         if (gAIEnv.CVars.DebugDraw == 0)
  318.                 return;
  319.         SDebugSphere sphere;
  320.         sphere.pos = pos;
  321.         sphere.r = r;
  322.         sphere.col = col;
  323.         m_debugSpheres.push_back(sphere);
  324. }
  325.  
  326. unsigned GetSmartObjectNavIndex(EntityId smartObjectEntityId, const string& className, const string& helperName)
  327. {
  328.         CSmartObject* pSmartObject = gAIEnv.pSmartObjectManager->GetSmartObject(smartObjectEntityId);
  329.         AIAssert(pSmartObject);
  330.         if (pSmartObject)
  331.         {
  332.                 SmartObjectHelper* pHelper = gAIEnv.pSmartObjectManager->GetSmartObjectHelper(className.c_str(), helperName.c_str());
  333.                 AIAssert(pHelper);
  334.                 if (pHelper)
  335.                 {
  336.                         Vec3 pos = pHelper ? pSmartObject->GetHelperPos(pHelper) : pSmartObject->GetPos();
  337.                         unsigned navIndex = pSmartObject->GetCorrespondingNavNode(pHelper);
  338.  
  339.                         AIAssert(navIndex);
  340.                         GraphNode* pNode = gAIEnv.pGraph->GetNode(navIndex);
  341.                         AIAssert(pNode);
  342.                         AIAssert(pNode->navType == IAISystem::NAV_SMARTOBJECT);
  343.  
  344.                         return navIndex;
  345.                 }
  346.                 else
  347.                 {
  348.                         AIWarning("[GetSmartObjectNavIndex]: Can't find smart object helper \"%s\" for smart object class \"%s\"", helperName.c_str(), className.c_str());
  349.                 }
  350.         }
  351.         else
  352.         {
  353.                 AIWarning("[GetSmartObjectNavIndex]: Can't find smart object for entity id %i", smartObjectEntityId);
  354.         }
  355.  
  356.         return 0;
  357. }
  358.  
  359. //====================================================================
  360. // Serialize
  361. //====================================================================
  362. void SerializeNavIndex(TSerialize ser, const char* szName, unsigned& navIndex)
  363. {
  364.         EntityId entityId = 0;
  365.         string className;
  366.         string helperName;
  367.  
  368.         ser.BeginGroup(szName);
  369.         if (ser.IsWriting())
  370.         {
  371.                 const GraphNode* pNavNode = gAIEnv.pGraph->GetNodeManager().GetNode(navIndex);
  372.                 if (pNavNode)
  373.                 {
  374.                         const SSmartObjectNavData* pNavData = pNavNode->GetSmartObjectNavData();
  375.                         if ((pNavData != NULL) && pNavData->pSmartObject && pNavData->pClass && pNavData->pHelper)
  376.                         {
  377.                                 entityId = pNavData->pSmartObject->GetEntityId();
  378.                                 className = pNavData->pClass->GetName();
  379.                                 helperName = pNavData->pHelper->name;
  380.                         }
  381.                 }
  382.         }
  383.  
  384.         ser.Value("entityId", entityId);
  385.         ser.Value("className", className);
  386.         ser.Value("helperName", helperName);
  387.  
  388.         if (ser.IsReading())
  389.         {
  390.                 navIndex = GetSmartObjectNavIndex(entityId, className, helperName);
  391.         }
  392.         ser.EndGroup();
  393. }
  394.  
  395. void PathPointDescriptor::SmartObjectNavData::Serialize(TSerialize ser)
  396. {
  397.         SerializeNavIndex(ser, "fromIndex", fromIndex);
  398.         SerializeNavIndex(ser, "toIndex", toIndex);
  399. }
  400.  
  401. //====================================================================
  402. // Serialize
  403. //====================================================================
  404. void PathPointDescriptor::Serialize(TSerialize ser)
  405. {
  406.         ser.Value("vPos", vPos);
  407.         ser.EnumValue("navType", navType, IAISystem::NAV_UNSET, IAISystem::NAV_MAX_VALUE);
  408.         ser.EnumValue("navSOMethod", navSOMethod, nSOmNone, nSOmLast);
  409.  
  410.         if (ser.IsWriting())
  411.         {
  412.                 if (ser.BeginOptionalGroup("pSONavData", pSONavData != NULL))
  413.                 {
  414.                         pSONavData->Serialize(ser);
  415.                         ser.EndGroup();
  416.                 }
  417.         }
  418.         else
  419.         {
  420.                 pSONavData = NULL;
  421.                 if (ser.BeginOptionalGroup("pSONavData", true))
  422.                 {
  423.                         pSONavData = new SmartObjectNavData;
  424.                         pSONavData->Serialize(ser);
  425.                         ser.EndGroup();
  426.                 }
  427.         }
  428. }
  429.  
  430. //====================================================================
  431. // Serialize
  432. //====================================================================
  433. void CNavPath::Serialize(TSerialize ser)
  434. {
  435.         ser.BeginGroup("CNavPath");
  436.  
  437.         unsigned pathSize = m_pathPoints.size();
  438.         ser.Value("pathSize", pathSize);
  439.  
  440.         if (ser.IsReading())
  441.                 m_pathPoints.resize(pathSize);
  442.  
  443.         if (pathSize > 0)
  444.         {
  445.                 ser.Value("PathPoints", m_pathPoints);
  446.         }
  447.  
  448.         ser.Value("m_endDir", m_endDir);
  449.         m_debugLines.clear();
  450.         m_debugSpheres.clear();
  451.         ser.Value("m_currentFrac", m_currentFrac);
  452.         ser.Value("m_stuckTime", m_stuckTime);
  453.         ser.Value("m_pathEndIsAsRequested", m_pathEndIsAsRequested);
  454.         m_params.Serialize(ser);
  455.  
  456.         ser.Value("m_fDiscardedPathLength", m_fDiscardedPathLength);
  457.         ser.Value("m_version", m_version);
  458.  
  459.         ser.EndGroup();
  460. }
  461.  
  462. //====================================================================
  463. // UpdatePathPosition
  464. // For each segment from the current one, if the agentPos lies beyond
  465. // the end of the segment, use this segment and get the iterator position
  466. // by projecting onto the segment and clamping.
  467. // "beyond the end of the segment" is done by calculating two planes that
  468. // divide up this segment from the next
  469. //====================================================================
  470. float CNavPath::UpdatePathPosition(Vec3 agentPos, float lookAhead, bool twoD, bool allowPathToFinish)
  471. {
  472.         float totalLen = GetPathLength(twoD);
  473.  
  474.         if (twoD)
  475.                 agentPos.z = 0.0f;
  476.  
  477.         while (m_pathPoints.size() >= 2)
  478.         {
  479.                 TPathPoints::const_iterator pathIt = m_pathPoints.begin();
  480.                 TPathPoints::const_iterator pathItNext = pathIt;
  481.                 ++pathItNext;
  482.  
  483.                 // start and end of this segment
  484.                 Vec3 segStart = pathIt->vPos;
  485.                 Vec3 segEnd = pathItNext->vPos;
  486.  
  487.                 float segLen = twoD ? (segStart - segEnd).GetLength2D() : (segStart - segEnd).GetLength();
  488.                 if (segLen > 0.1f)
  489.                 {
  490.                         // the end of the next segment (may be faked)
  491.                         Vec3 segEndNext;
  492.  
  493.                         TPathPoints::const_iterator pathItNextNext = pathItNext;
  494.                         ++pathItNextNext;
  495.                         if (pathItNextNext == m_pathPoints.end())
  496.                                 segEndNext = segEnd + (segEnd - segStart);
  497.                         else
  498.                                 segEndNext = pathItNextNext->vPos;
  499.  
  500.                         if (twoD)
  501.                                 segStart.z = segEnd.z = segEndNext.z = 0.0f;
  502.  
  503.                         Vec3 segDir = (segEnd - segStart).GetNormalizedSafe();
  504.  
  505.                         // don't advance until we're within the path width of the segment end
  506.                         Plane planeSeg = Plane::CreatePlane(segDir, segEnd);
  507.                         float planeSegDist = planeSeg.DistFromPlane(agentPos);
  508.                         if (planeSegDist < -0.6f * lookAhead)
  509.                         {
  510.                                 // This segment is the one - calculate the fraction and then
  511.                                 // clamp it - but don't go backwards!
  512.                                 float newFrac = m_currentFrac;
  513.                                 // all z values are already 0
  514.                                 Distance::Point_LinesegSq(agentPos, Lineseg(segStart, segEnd), newFrac);
  515.                                 if (newFrac > 1.0f)
  516.                                         m_currentFrac = 1.0f;
  517.                                 else if (newFrac > m_currentFrac)
  518.                                         m_currentFrac = newFrac;
  519.                                 return totalLen;
  520.                         }
  521.  
  522.                         // past the divider between this and the next?
  523.                         Vec3 nextSegDir = (segEndNext - segEnd).GetNormalizedSafe(segDir);
  524.                         Vec3 avDir = (segDir + nextSegDir).GetNormalizedSafe(segDir);
  525.  
  526.                         Plane planeSegDiv = Plane::CreatePlane(avDir, segEnd);
  527.                         float planeSegDivDist = planeSegDiv.DistFromPlane(agentPos);
  528.  
  529.                         if (planeSegDivDist <= 0.0f)
  530.                         {
  531.                                 // This segment is the one - calculate the fraction and then
  532.                                 // clamp it - but don't go backwards!
  533.                                 float newFrac = m_currentFrac;
  534.                                 // all z values are already 0
  535.                                 Distance::Point_LinesegSq(agentPos, Lineseg(segStart, segEnd), newFrac);
  536.                                 if (newFrac > 1.0f)
  537.                                         m_currentFrac = 1.0f;
  538.                                 else if (newFrac > m_currentFrac)
  539.                                         m_currentFrac = newFrac;
  540.  
  541.                                 return totalLen;
  542.                         }
  543.                 } // trivial segment length
  544.  
  545.                 if (!allowPathToFinish && m_pathPoints.size() == 2)
  546.                 {
  547.                         m_currentFrac = 0.99f;
  548.                         return totalLen;
  549.                 }
  550.  
  551.                 // moving onto the next segment
  552.                 PathPointDescriptor junk;
  553.                 Advance(junk);
  554.                 m_currentFrac = 0.0;
  555.         }
  556.         // got to the end
  557.         m_currentFrac = 0.0f;
  558.         return totalLen;
  559. }
  560.  
  561. //====================================================================
  562. // GetPosAlongPath
  563. //====================================================================
  564. bool CNavPath::GetPosAlongPath(Vec3& posOut, float dist, bool b2D, bool bExtrapolateBeyondEnd, IAISystem::ENavigationType* pNextPointType /*=NULL*/) const
  565. {
  566.         if (m_pathPoints.empty())
  567.                 return false;
  568.  
  569.         if (m_pathPoints.size() == 1)
  570.         {
  571.                 const PathPointDescriptor& pointDesc = m_pathPoints.front();
  572.                 posOut = pointDesc.vPos;
  573.                 if (pNextPointType)
  574.                         *pNextPointType = pointDesc.navType;
  575.                 return true;
  576.         }
  577.  
  578.         // We are at prevPt, a point of m_currentFrac completion of segment ["previous point", thisPt]
  579.         TPathPoints::const_iterator it = ++m_pathPoints.begin();
  580.         Vec3 thisPt = it->vPos;
  581.         Vec3 prevPt = m_currentFrac * thisPt + (1.0f - m_currentFrac) * GetPrevPathPoint()->vPos;
  582.  
  583.         // Some clients want to know only the current position on path [6/3/2010 evgeny]
  584.         if (dist == 0.f)
  585.         {
  586.                 posOut = prevPt;
  587.                 if (pNextPointType)
  588.                         *pNextPointType = it->navType;
  589.                 return true;
  590.         }
  591.  
  592.         const TPathPoints::const_iterator itEnd = m_pathPoints.end();
  593.  
  594.         // No initialization; the loop below will run at least once (we have >= 2 path points here).
  595.         Vec3 vDelta;
  596.         float fDeltaLen = 0.f;  // initializing to make the Static Analyzer happy
  597.         assert(it != itEnd);
  598.  
  599.         float& fDistRemaining = dist;
  600.  
  601.         for (; it != itEnd; ++it)
  602.         {
  603.                 thisPt = it->vPos;
  604.                 vDelta = thisPt - prevPt;
  605.                 fDeltaLen = b2D ? vDelta.GetLength2D() : vDelta.GetLength();
  606.                 if (fDistRemaining <= fDeltaLen)
  607.                 {
  608.                         float frac = fDeltaLen > 0.f ? fDistRemaining / fDeltaLen : 0.f;
  609.                         posOut = frac * thisPt + (1.f - frac) * prevPt;
  610.                         if (pNextPointType)
  611.                                 *pNextPointType = it->navType;
  612.                         return true;
  613.                 }
  614.                 fDistRemaining -= fDeltaLen;
  615.                 prevPt = thisPt;
  616.         }
  617.  
  618.         posOut = thisPt;
  619.  
  620.         if (bExtrapolateBeyondEnd)
  621.         {
  622.                 float frac = fDeltaLen > 0.f ? fDistRemaining / fDeltaLen : 0.f;
  623.                 posOut += frac * vDelta;
  624.         }
  625.  
  626.         if (pNextPointType)
  627.                 *pNextPointType = m_pathPoints.back().navType;
  628.  
  629.         return true;
  630. }
  631.  
  632. //====================================================================
  633. // GetPathProperties
  634. //====================================================================
  635. bool CNavPath::GetPathPropertiesAhead(const float distAhead, const bool twoD, Vec3& posOut, Vec3& dirOut,
  636.                                       float* invRcOut, float& lowestPathDotOut, bool scaleOutputWithDist) const
  637. {
  638.         float dist = distAhead;
  639.         if (invRcOut)
  640.                 *invRcOut = 0.0f;
  641.         lowestPathDotOut = 1.0f;
  642.         bool extrapolateBeyondEnd = false;
  643.  
  644.         if (m_pathPoints.size() < 2)
  645.                 return false;
  646.  
  647.         Vec3 thisPt = GetNextPathPoint()->vPos;
  648.         Vec3 currentPos = m_currentFrac * thisPt + (1.0f - m_currentFrac) * GetPrevPathPoint()->vPos;
  649.         Vec3 prevPt = currentPos;
  650.  
  651.         Vec3 firstSegDir = thisPt - currentPos;
  652.         if (twoD)
  653.                 firstSegDir.z = 0.0f;
  654.         firstSegDir.NormalizeSafe(Vec3Constants<float>::fVec3_OneY);
  655.         dirOut = firstSegDir; // ensure it's not unset
  656.  
  657.         const TPathPoints::const_iterator itEnd = m_pathPoints.end();
  658.         TPathPoints::const_iterator it;
  659.         for (it = ++m_pathPoints.begin(); it != itEnd; ++it)
  660.         {
  661.                 thisPt = it->vPos;
  662.                 Vec3 delta = thisPt - prevPt;
  663.                 float deltaLen = twoD ? delta.GetLength2D() : delta.GetLength();
  664.  
  665.                 Vec3 currentSegDir = delta;
  666.                 if (twoD)
  667.                         currentSegDir.z = 0.0f;
  668.                 float segLen = currentSegDir.NormalizeSafe();
  669.                 if (segLen < 0.001f)
  670.                         continue;
  671.                 dirOut = currentSegDir;
  672.                 float pathDirDot = currentSegDir.Dot(firstSegDir);
  673.                 if (scaleOutputWithDist)
  674.                 {
  675.                         float frac = dist / distAhead;
  676.                         pathDirDot = frac * pathDirDot + (1.0f - frac) * 1.0f;
  677.                 }
  678.  
  679.                 if (pathDirDot < lowestPathDotOut)
  680.                         lowestPathDotOut = pathDirDot;
  681.  
  682.                 // calculate posOut here so it is correct when we exit the loop - will
  683.                 // adjust if necessary if we didn't need to extrapolate
  684.                 float frac = deltaLen > 0.0f ? dist / deltaLen : 1.0f;
  685.                 posOut = frac * thisPt + (1.0f - frac) * prevPt;
  686.  
  687.                 if (dist <= deltaLen)
  688.                         break;
  689.                 dist -= deltaLen;
  690.                 prevPt = thisPt;
  691.         }
  692.  
  693.         if (it == itEnd && !extrapolateBeyondEnd)
  694.         {
  695.                 // fallen off the end
  696.                 posOut = thisPt;
  697.         }
  698.  
  699.         if (invRcOut)
  700.         {
  701.                 // estimate the radius of curvature by comparing the direct distance with the path distance, and assuming that
  702.                 // the path goes through a single point offset by h (in the middle) from the direct path
  703.                 float directDistance = twoD ? Distance::Point_Point2D(posOut, currentPos) : Distance::Point_Point(posOut, currentPos);
  704.  
  705.                 if (directDistance < distAhead)
  706.                 {
  707.                         float d = 0.5f * directDistance;
  708.                         static float hScale = 0.5f; // approx scaling since the angled path isn't a circle
  709.                         float h = sqrt_tpl(square(0.5f * distAhead) - square(d));
  710.                         h *= hScale;
  711.                         float R = 0.5f * (square(h) + square(d)) / h;
  712.                         *invRcOut = 1.0f / R;
  713.                 }
  714.                 else
  715.                 {
  716.                         *invRcOut = 0.0f;
  717.                 }
  718.         }
  719.         return true;
  720. }
  721.  
  722. //====================================================================
  723. // DoesTargetSegPosExceedPathSeg
  724. // returns true if targetSeg hits the inside of the infinite cylinder
  725. // represented by pathSeg
  726. //====================================================================
  727. bool WouldTargetPosExceedPathSeg(const Vec3& targetPos, const Lineseg& pathSeg,
  728.                                  float radius, bool twoD)
  729. {
  730.         if (twoD)
  731.         {
  732.                 Vec3 segDir = (pathSeg.end - pathSeg.start);
  733.                 segDir.z = 0.0f;
  734.                 segDir.NormalizeSafe();
  735.                 Vec3 delta = (targetPos - pathSeg.start);
  736.                 delta.z = 0.0f;
  737.                 Vec3 perp = delta - segDir * delta.Dot(segDir);
  738.                 float distSq = perp.GetLengthSquared2D();
  739.                 return (distSq > square(radius));
  740.         }
  741.         else
  742.         {
  743.                 Vec3 segDir = (pathSeg.end - pathSeg.start).GetNormalizedSafe();
  744.                 Vec3 delta = (targetPos - pathSeg.start);
  745.                 Vec3 perp = delta - segDir * delta.Dot(segDir);
  746.                 float distSq = perp.GetLengthSquared();
  747.                 return (distSq > square(radius));
  748.         }
  749. }
  750.  
  751. //====================================================================
  752. // CalculateTargetPos
  753. //====================================================================
  754. Vec3 CNavPath::CalculateTargetPos(Vec3 agentPos, float lookAhead, float minLookAheadAlongPath, float pathRadius, bool twoD) const
  755. {
  756.         if (m_pathPoints.empty())
  757.                 return agentPos;
  758.         if (m_pathPoints.size() == 1)
  759.                 return m_pathPoints.front().vPos;
  760.  
  761.         Vec3 pathPos = GetPrevPathPoint()->vPos * (1.0f - m_currentFrac) + GetNextPathPoint()->vPos * m_currentFrac;
  762.  
  763.         DebugLine(agentPos, pathPos, ColorF(0, 1, 0));
  764.         DebugSphere(pathPos, 0.3f, ColorF(0, 1, 0));
  765.  
  766.         float distAgentToPathPos = twoD ? (pathPos - agentPos).GetLength2D() : (pathPos - agentPos).GetLength();
  767.         Vec3 pathDelta = GetNextPathPoint()->vPos - pathPos;
  768.         float distPathPosToCurrent = twoD ? pathDelta.GetLength2D() : pathDelta.GetLength();
  769.  
  770.         lookAhead -= distAgentToPathPos;
  771.  
  772.         if (lookAhead < minLookAheadAlongPath)
  773.                 lookAhead = minLookAheadAlongPath;
  774.  
  775.         if (lookAhead < distPathPosToCurrent)
  776.         {
  777.                 Vec3 targetPos = pathPos + lookAhead * pathDelta / distPathPosToCurrent;
  778.                 return targetPos;
  779.         }
  780.  
  781.         if (m_pathPoints.size() == 2)
  782.         {
  783.                 lookAhead -= distPathPosToCurrent;
  784.  
  785.                 Vec3 delta = GetNextPathPoint()->vPos - GetPrevPathPoint()->vPos;
  786.                 if (twoD)
  787.                         delta.z = 0.0f;
  788.                 float deltaLen = delta.NormalizeSafe();
  789.                 if (deltaLen < 0.01f)
  790.                 {
  791.                         return GetNextPathPoint()->vPos;
  792.                 }
  793.                 else
  794.                 {
  795.                         delta *= lookAhead;
  796.                         return GetNextPathPoint()->vPos + delta;
  797.                 }
  798.         }
  799.         // walk forward through the remaining path until we either reach lookAhead, or we (Danny todo) deviate from
  800.         // the path
  801.  
  802.         // Binary search to find a suitable target position that doesn't intersect the path width.
  803.         Vec3 targetPos;
  804.         if (false == GetPosAlongPath(targetPos, lookAhead, twoD, true))
  805.                 return GetNextPathPoint()->vPos;
  806.  
  807.         Lineseg curSeg(GetPrevPathPoint()->vPos, GetNextPathPoint()->vPos);
  808.         // if almost on the next seg use that one to allow the lookahead to progress
  809.         if (const PathPointDescriptor* pNextNext = GetNextNextPathPoint())
  810.         {
  811.                 Lineseg nextSeg(curSeg.end, pNextNext->vPos);
  812.                 float junk;
  813.                 float distToNextSegSq = twoD ? Distance::Point_Lineseg2DSq(agentPos, nextSeg, junk) : Distance::Point_LinesegSq(agentPos, nextSeg, junk);
  814.                 if (distToNextSegSq < square(pathRadius))
  815.                         curSeg = nextSeg;
  816.         }
  817.  
  818.         if (WouldTargetPosExceedPathSeg(targetPos, curSeg, pathRadius, twoD))
  819.         {
  820.                 lookAhead = minLookAheadAlongPath + 0.5f * (lookAhead - minLookAheadAlongPath);
  821.                 float delta = lookAhead * 0.5f;
  822.                 for (int i = 0; i < 8; ++i)
  823.                 {
  824.                         if (false == GetPosAlongPath(targetPos, lookAhead, twoD, true))
  825.                                 return GetNextPathPoint()->vPos;
  826.                         if (WouldTargetPosExceedPathSeg(targetPos, curSeg, pathRadius, twoD))
  827.                                 lookAhead -= delta;
  828.                         else
  829.                                 lookAhead += delta;
  830.                         delta *= 0.5f;
  831.                 }
  832.         }
  833.         return targetPos;
  834. }
  835.  
  836. //===================================================================
  837. // GetPathDeviationDistance
  838. //===================================================================
  839. float CNavPath::GetPathDeviationDistance(Vec3& deviationOut, float criticalDeviation, bool twoD)
  840. {
  841.         deviationOut.zero();
  842.         if (m_pathPoints.size() < 3)
  843.                 return 0.0f;
  844.  
  845.         TPathPoints::const_iterator it = m_pathPoints.begin();
  846.         Lineseg thisSeg;
  847.         thisSeg.start = (it++)->vPos;
  848.         thisSeg.end = (it++)->vPos;
  849.  
  850.         float distOut = twoD ? (1.0f - m_currentFrac) * Distance::Point_Point2D(thisSeg.start, thisSeg.end) :
  851.                         m_currentFrac* Distance::Point_Point(thisSeg.start, thisSeg.end);
  852.         Vec3 lastPt = thisSeg.end;
  853.         float lastDeviation = 0.0f;
  854.         for (; it != m_pathPoints.end(); ++it)
  855.         {
  856.                 Vec3 pt = it->vPos;
  857.                 float t;
  858.                 float deviation = twoD ? Distance::Point_Lineseg2D(pt, Lineseg(thisSeg.start, thisSeg.end), t) :
  859.                                   Distance::Point_Lineseg(pt, Lineseg(thisSeg.start, thisSeg.end), t);
  860.                 float segLen = twoD ? Distance::Point_Point2D(lastPt, pt) : Distance::Point_Point(lastPt, pt);
  861.                 if (deviation > criticalDeviation)
  862.                 {
  863.                         float frac = 1.0f - (deviation - criticalDeviation) / (deviation - lastDeviation);
  864.                         distOut += frac * segLen;
  865.                         Vec3 exitPt = frac * pt + (1.0f - frac) * lastPt;
  866.  
  867.                         Vec3 exitDelta = exitPt - thisSeg.start;
  868.                         Vec3 segDir = (thisSeg.end - thisSeg.start).GetNormalizedSafe();
  869.                         deviationOut = exitDelta - exitDelta.Dot(segDir) * segDir;
  870.                         if (twoD)
  871.                                 deviationOut.z = 0.0f;
  872.                         return distOut;
  873.                 }
  874.                 distOut += segLen;
  875.                 lastPt = pt;
  876.                 lastDeviation = deviation;
  877.         }
  878.  
  879.         return distOut;
  880. }
  881.  
  882. //====================================================================
  883. // Dump
  884. //====================================================================
  885. void CNavPath::Dump(const char* name) const
  886. {
  887.         AILogAlways("Path for %s", name);
  888.         int i = 0;
  889.         for (TPathPoints::const_iterator it = m_pathPoints.begin(); it != m_pathPoints.end(); ++it, ++i)
  890.         {
  891.                 const PathPointDescriptor& ppd = *it;
  892.                 AILogAlways("pt %4d: %5.2f %5.2f %5.2f", i, ppd.vPos.x, ppd.vPos.y, ppd.vPos.z);
  893.         }
  894. }
  895.  
  896. //====================================================================
  897. // Update
  898. // Procedure is this:
  899. // 1. Calculate a working position based on the input position, velocity
  900. // and response time.
  901. // 2. Calculate a new path position by advancing the old one.
  902. // 3. Find a new target position by advancing from the iterator
  903. // position
  904. // 4. Adjust the target position to stop it taking the agent too far
  905. // off the path
  906. // 5. Calculate the path curvature and distance to destination
  907. //====================================================================
  908. bool CNavPath::UpdateAndSteerAlongPath(Vec3& dirOut, float& distToEndOut, float& distToPathOut, bool& isResolvingStickingOut,
  909.                                        Vec3& pathDirOut, Vec3& pathAheadDirOut, Vec3& pathAheadPosOut,
  910.                                        Vec3 currentPos, const Vec3& currentVel,
  911.                                        float lookAhead, float pathRadius, float dt, bool resolveSticking, bool twoD)
  912. {
  913.         FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI);
  914.         m_debugLines.clear();
  915.         m_debugSpheres.clear();
  916.  
  917.         if (m_pathPoints.size() < 2)
  918.                 return false;
  919.  
  920.         /*
  921.            Vec3 deviation;
  922.            float maxDeviation = 0.5f * pathRadius;
  923.            float dist = GetPathDeviationDistance(deviation, maxDeviation, twoD);
  924.            float frac = dist < lookAhead ? dist / lookAhead : 1.0f - (dist - lookAhead)/5.0f;
  925.            if (frac > 0.0f)
  926.            currentPos += deviation * frac;
  927.          */
  928.  
  929.         // use a kind-of mid-point method
  930.         const Vec3 workingPos = currentPos;// + currentVel * (0.5f * dt);
  931.  
  932.         Vec3 origPathPosition(currentPos);
  933.         origPathPosition = GetPrevPathPoint()->vPos * (1.0f - m_currentFrac) + GetNextPathPoint()->vPos * m_currentFrac;
  934.  
  935.         // update the path position.
  936.         distToEndOut = UpdatePathPosition(workingPos, lookAhead, twoD, true);
  937.         distToPathOut = 0.0f;
  938.  
  939.         if (m_pathPoints.size() < 2)
  940.                 return false;
  941.  
  942.         float workingLookAhead = lookAhead;
  943.         isResolvingStickingOut = false;
  944.         if (resolveSticking && distToEndOut > lookAhead)
  945.         {
  946.                 // prevent sticking if the path position isn't moving and the agent is some way from the path
  947.                 Vec3 newPathPosition = GetPrevPathPoint()->vPos * (1.0f - m_currentFrac) + GetNextPathPoint()->vPos * m_currentFrac;
  948.                 float dist = (newPathPosition - origPathPosition).GetLength();
  949.                 float expectedDist = twoD ? dt* currentVel.GetLength2D() : dt* currentVel.GetLength();
  950.  
  951.                 distToPathOut = twoD ? (currentPos - newPathPosition).GetLength2D() : (currentPos - newPathPosition).GetLength();
  952.  
  953.                 static float stickFrac = 0.1f;
  954.                 if (dist < stickFrac * expectedDist && distToPathOut > 0.5f * pathRadius)
  955.                         m_stuckTime += dt;
  956.                 else
  957.                         m_stuckTime -= min(m_stuckTime, dt);
  958.  
  959.                 static float minStickTime = 0.2f;
  960.                 static float maxStickTime = 1.0f;
  961.                 static float stickTimeRate = 1.0f;
  962.                 if (m_stuckTime > minStickTime)
  963.                 {
  964.                         isResolvingStickingOut = true;
  965.                         workingLookAhead *= 1.0f / (1.0f + stickTimeRate * (m_stuckTime - minStickTime));
  966.                 }
  967.                 if (m_stuckTime > maxStickTime)
  968.                         m_stuckTime = maxStickTime;
  969.         }
  970.         else
  971.         {
  972.                 Vec3 newPathPosition = GetPrevPathPoint()->vPos * (1.0f - m_currentFrac) + GetNextPathPoint()->vPos * m_currentFrac;
  973.                 distToPathOut = twoD ? (currentPos - newPathPosition).GetLength2D() : (currentPos - newPathPosition).GetLength();
  974.         }
  975.  
  976.         distToEndOut += distToPathOut;
  977.  
  978.         // Calculate where the agent should head towards - makes sure this heading
  979.         // doesn't deviate too much from the path even with a big look-ahead
  980.         static float minLookAheadFrac = 0.1f;
  981.         float minLookAheadAlongPath = minLookAheadFrac * lookAhead;
  982.         pathAheadPosOut = CalculateTargetPos(workingPos, workingLookAhead, minLookAheadAlongPath, pathRadius, twoD);
  983.  
  984.         DebugLine(currentPos, pathAheadPosOut, ColorF(1, 0, 0));
  985.         DebugSphere(pathAheadPosOut, 0.3f, ColorF(1, 0, 0));
  986.  
  987.         dirOut = (pathAheadPosOut - currentPos).GetNormalizedSafe(Vec3Constants<float>::fVec3_OneX);
  988.  
  989.         pathDirOut = (GetNextPathPoint()->vPos - GetPrevPathPoint()->vPos).GetNormalizedSafe();
  990.         pathAheadDirOut = pathDirOut;
  991.  
  992.         // Sometimes even if the agent reached the steering point it wouldn't advance the path position
  993.         // (especially for vehicles) so if the agent has already progressed up to the steer point then
  994.         // gradually advance the current position.
  995.         if ((currentPos - pathAheadPosOut).Dot(pathAheadPosOut - workingPos) > 0.0f)
  996.         {
  997.                 float advanceDist = twoD ? (pathAheadPosOut - workingPos).GetLength2D() : (pathAheadPosOut - workingPos).GetLength();
  998.                 Vec3 pathNextPoint;
  999.                 if (GetPosAlongPath(pathNextPoint, advanceDist, true, false))
  1000.                         UpdatePathPosition(pathNextPoint, 100.0f, true, false);
  1001.         }
  1002.  
  1003.         return true;
  1004. }
  1005.  
  1006. void CNavPath::PrepareNavigationalSmartObjectsForMNM(IAIPathAgent* pAgent)
  1007. {
  1008.         m_fDiscardedPathLength = 0.0f;
  1009.  
  1010.         // TODO(marcio): Make this work with normal entities.
  1011.         // Perhaps with an ISmartObjectUser interface.
  1012.         IEntity* pEntity = pAgent ? pAgent->GetPathAgentEntity() : NULL;
  1013.         IAIObject* pAIObject = pEntity ? pEntity->GetAI() : NULL;
  1014.         CPipeUser* pPipeUser = pAIObject ? pAIObject->CastToCPipeUser() : NULL;
  1015.  
  1016.         if (!pPipeUser)
  1017.                 return;
  1018.  
  1019.         for (TPathPoints::iterator it = m_pathPoints.begin(), itEnd = m_pathPoints.end(); it != itEnd; ++it)
  1020.         {
  1021.                 if (it->navType == IAISystem::NAV_SMARTOBJECT)
  1022.                 {
  1023.                         //////////////////////////////////////////////////////////////////////////
  1024.                         /// MNM Smart Objects glue code
  1025.                         PathPointDescriptor& pathPointDescriptor = *it;
  1026.                         ++it;
  1027.                         MNM::OffMeshLink* pOffMeshLink = gAIEnv.pNavigationSystem->GetOffMeshNavigationManager()->GetOffMeshLink(pathPointDescriptor.offMeshLinkData.offMeshLinkID);
  1028.                         OffMeshLink_SmartObject* pSOLink = pOffMeshLink ? pOffMeshLink->CastTo<OffMeshLink_SmartObject>() : NULL;
  1029.                         if (pSOLink)
  1030.                         {
  1031.                                 // Only cut at animation navSOs
  1032.                                 pathPointDescriptor.navSOMethod = static_cast<ENavSOMethod>
  1033.                                                                   (gAIEnv.pSmartObjectManager->GetNavigationalSmartObjectActionTypeForMNM(
  1034.                                                                      pPipeUser,
  1035.                                                                      pSOLink->m_pSmartObject,
  1036.                                                                      pSOLink->m_pSmartObjectClass,
  1037.                                                                      pSOLink->m_pFromHelper,
  1038.                                                                      pSOLink->m_pToHelper));
  1039.  
  1040.                                 const ENavSOMethod navSOMethod = pathPointDescriptor.navSOMethod;
  1041.                                 if (navSOMethod == nSOmStraight)
  1042.                                 {
  1043.                                         if (it != itEnd)
  1044.                                         {
  1045.                                                 continue;
  1046.                                         }
  1047.                                         else
  1048.                                         {
  1049.                                                 return;
  1050.                                         }
  1051.                                 }
  1052.  
  1053.                                 ++m_version.v;
  1054.                                 if (it != itEnd)
  1055.                                 {
  1056.                                         // compute the length of the discarded path segment
  1057.                                         //m_fDiscardedPathLength = 0;
  1058.                                         Vec3 vPos = pathPointDescriptor.vPos;
  1059.                                         for (TPathPoints::const_iterator itNext = it; itNext != itEnd; ++itNext)
  1060.                                         {
  1061.                                                 const Vec3& vNextPos = itNext->vPos;
  1062.                                                 m_fDiscardedPathLength += Distance::Point_Point(vPos, vNextPos);
  1063.                                                 vPos = vNextPos;
  1064.                                         }
  1065.  
  1066.                                         // Preserve the remaining path points for later
  1067.                                         m_remainingPathPoints.assign(it, itEnd);
  1068.  
  1069.                                         m_pathPoints.erase(it, itEnd);
  1070.                                         return;
  1071.                                 }
  1072.                                 else
  1073.                                 {
  1074.                                         m_pathPoints.pop_back();
  1075.                                         return;
  1076.                                 }
  1077.                         }
  1078.                         else  // if (pSONavData)
  1079.                         {
  1080.                                 // Can be null only at the end of the path.
  1081.                                 AIAssert(0);
  1082.                                 return;
  1083.                         }
  1084.                 }
  1085.         }
  1086. }
  1087.  
  1088. void CNavPath::ResurrectRemainingPath()
  1089. {
  1090.         if (!m_remainingPathPoints.empty())
  1091.         {
  1092.                 m_fDiscardedPathLength = 0.0f;
  1093.                 m_pathPoints.swap(m_remainingPathPoints);
  1094.                 m_remainingPathPoints.clear();
  1095.                 m_version.v++;
  1096.         }
  1097. }
  1098.  
  1099. //===================================================================
  1100. // MovePathEndsOutOfObstacles
  1101. //===================================================================
  1102. void CNavPath::TrimPath(float trimLength, bool twoD)
  1103. {
  1104.         if (m_pathPoints.size() < 2)
  1105.                 return;
  1106.  
  1107.         float initialLenght = GetPathLength(twoD);
  1108.  
  1109.         if (trimLength > 0.0f)
  1110.                 trimLength = initialLenght - trimLength;
  1111.         else
  1112.                 trimLength = -trimLength;
  1113.  
  1114.         if (trimLength > initialLenght)
  1115.                 return;
  1116.  
  1117.         if (trimLength < 0.01f)
  1118.         {
  1119.                 // Trim the whole path.
  1120.                 ++m_version.v;
  1121.                 PathPointDescriptor pt = m_pathPoints.back();
  1122.                 m_pathPoints.clear();
  1123.                 m_pathPoints.push_back(pt);
  1124.                 m_pathPoints.push_back(pt);
  1125.                 return;
  1126.         }
  1127.  
  1128.         //      IAISystem::ENavigationType lastSafeNavType = IAISystem::NAV_UNSET;
  1129.         float runLength = 0.0;
  1130.         const TPathPoints::iterator itPathEnd = m_pathPoints.end();
  1131.         for (TPathPoints::iterator it = m_pathPoints.begin(); it != itPathEnd; ++it)
  1132.         {
  1133.                 TPathPoints::iterator itNext = it;
  1134.                 ++itNext;
  1135.                 if (itNext == itPathEnd)
  1136.                         break;
  1137.  
  1138.                 PathPointDescriptor& start = *it;
  1139.                 PathPointDescriptor& end = *itNext;
  1140.  
  1141.                 //              if (start.navType != IAISystem::NAV_SMARTOBJECT)
  1142.                 //                      lastSafeNavType = start.navType;
  1143.  
  1144.                 float segmentLength;
  1145.                 if (twoD)
  1146.                         segmentLength = Distance::Point_Point2D(start.vPos, end.vPos);
  1147.                 else
  1148.                         segmentLength = Distance::Point_Point(start.vPos, end.vPos);
  1149.  
  1150.                 if (trimLength >= runLength && trimLength < (runLength + segmentLength))
  1151.                 {
  1152.                         // If end point is navso, reset it.
  1153.                         if (start.navType == IAISystem::NAV_SMARTOBJECT && end.navType == IAISystem::NAV_SMARTOBJECT)
  1154.                         {
  1155.                                 // The cut would happen at a navso segment, allow to pass it.
  1156.                                 // The navso cutter will handle this case.
  1157.                                 return;
  1158.                                 /*                              start.navType = lastSafeNavType;
  1159.                                         end.navType = start.navType;
  1160.                                         end.pSONavData = 0;
  1161.                                         end.navSOMethod = nSOmNone;*/
  1162.                         }
  1163.                         else
  1164.                         {
  1165.                                 if (end.navType == IAISystem::NAV_SMARTOBJECT)
  1166.                                 {
  1167.                                         end.navType = start.navType;
  1168.                                         end.pSONavData = 0;
  1169.                                         end.navSOMethod = nSOmNone;
  1170.                                 }
  1171.  
  1172.                                 float u = (trimLength - runLength) / segmentLength;
  1173.                                 end.vPos = start.vPos + u * (end.vPos - start.vPos);
  1174.  
  1175.                                 ++itNext;
  1176.                         }
  1177.  
  1178.                         // Remove the end of the path.
  1179.                         if (itNext != itPathEnd)
  1180.                                 m_pathPoints.erase(itNext, itPathEnd);
  1181.  
  1182.                         ++m_version.v;
  1183.  
  1184.                         return;
  1185.                 }
  1186.  
  1187.                 runLength += segmentLength;
  1188.         }
  1189. }
  1190.  
  1191. Vec3 GetSafePositionInMesh(const NavigationMesh& mesh, const Vec3& testLocation, const float verticalRange, const float horizontalRange)
  1192. {
  1193.         Vec3 safePosition = testLocation;
  1194.  
  1195.         const MNM::vector3_t testLocationFixedPoint(MNM::real_t(testLocation.x), MNM::real_t(testLocation.y), MNM::real_t(testLocation.z));
  1196.  
  1197.         const MNM::real_t vRange(verticalRange);
  1198.         const MNM::real_t hRange(horizontalRange);
  1199.  
  1200.         if (!mesh.navMesh.GetTriangleAt(testLocationFixedPoint, vRange, vRange))
  1201.         {
  1202.                 MNM::vector3_t closestLocation;
  1203.                 if (mesh.navMesh.GetClosestTriangle(testLocationFixedPoint, vRange, hRange, nullptr, &closestLocation))
  1204.                 {
  1205.                         safePosition = closestLocation.GetVec3();
  1206.                 }
  1207.         }
  1208.  
  1209.         return safePosition;
  1210. }
  1211.  
  1212. //===================================================================
  1213. // MovePathEndsOutOfObstacles
  1214. //===================================================================
  1215. void CNavPath::MovePathEndsOutOfObstacles(const CPathObstacles& obstacles)
  1216. {
  1217.         FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  1218.  
  1219.         if (m_pathPoints.empty())
  1220.                 return;
  1221.  
  1222.         ++m_version.v;
  1223.         Vec3& pathStart = m_pathPoints.front().vPos;
  1224.         Vec3& pathEnd = m_pathPoints.back().vPos;
  1225.         const PathPointDescriptor& ppdStart = m_pathPoints.front();
  1226.         const PathPointDescriptor& ppdEnd = m_pathPoints.back();
  1227.         static float extra = 0.1f;
  1228.  
  1229.         //////////////////////////////////////////////////////////////////////////
  1230.         /// Note: When using MNM moving the points could get those out of the mesh
  1231.  
  1232.         const bool usingMNM = (GetMeshID() != NavigationMeshID(0));
  1233.  
  1234.         if (usingMNM)
  1235.         {
  1236.                 const IAISystem::ENavigationType navTypeFilter = (IAISystem::ENavigationType)(IAISystem::NAV_TRIANGULAR | IAISystem::NAV_ROAD | IAISystem::NAV_UNSET);
  1237.  
  1238.                 const NavigationMesh& mesh = gAIEnv.pNavigationSystem->GetMesh(GetMeshID());
  1239.                 const MNM::CNavMesh::SGridParams& gridParams = mesh.navMesh.GetGridParams();
  1240.  
  1241.                 // Danny todo actually walk/back/forward moving all points out?
  1242.                 if (ppdStart.navType & navTypeFilter)
  1243.                 {
  1244.                         const Vec3 newPosition = obstacles.GetPointOutsideObstacles(pathStart, extra);
  1245.  
  1246.                         //Check if new position is safe
  1247.                         if (!newPosition.IsEquivalent(pathStart))
  1248.                         {
  1249.                                 const float displacement = (newPosition - pathStart).len();
  1250.  
  1251.                                 pathStart = GetSafePositionInMesh(mesh, newPosition - gridParams.origin, 2.0f, displacement / gridParams.voxelSize.x);
  1252.                         }
  1253.                 }
  1254.  
  1255.                 if (ppdEnd.navType & navTypeFilter)
  1256.                 {
  1257.                         Vec3 newPosition = obstacles.GetPointOutsideObstacles(pathEnd, extra);
  1258.  
  1259.                         //Check if new position is safe
  1260.                         if (!newPosition.IsEquivalent(pathEnd))
  1261.                         {
  1262.                                 const float displacement = (newPosition - pathEnd).len();
  1263.  
  1264.                                 pathEnd = GetSafePositionInMesh(mesh, newPosition - gridParams.origin, 2.0f, displacement / gridParams.voxelSize.x);
  1265.                         }
  1266.                 }
  1267.         }
  1268.         else
  1269.         {
  1270.                 const IAISystem::ENavigationType navTypeFilter = (IAISystem::ENavigationType)(IAISystem::NAV_TRIANGULAR | IAISystem::NAV_ROAD);
  1271.  
  1272.                 // Danny todo actually walk/back/forward moving all points out?
  1273.                 if (ppdStart.navType & navTypeFilter)
  1274.                 {
  1275.                         pathStart = obstacles.GetPointOutsideObstacles(pathStart, extra);
  1276.                 }
  1277.  
  1278.                 if (ppdEnd.navType & navTypeFilter)
  1279.                 {
  1280.                         pathEnd = obstacles.GetPointOutsideObstacles(pathEnd, extra);
  1281.                 }
  1282.         }
  1283. }
  1284.  
  1285. //===================================================================
  1286. // CheckPath
  1287. // Currently just checks against forbidden edges, but should also check
  1288. // other things (depending on the navCapMask too). radius also needs to
  1289. // get used.
  1290. //===================================================================
  1291. bool CNavPath::CheckPath(const TPathPoints& pathList, float radius) const
  1292. {
  1293.         const bool usingMNM = (GetMeshID() != NavigationMeshID(0));
  1294.  
  1295.         if (usingMNM)
  1296.         {
  1297.                 const size_t MaxWayTriangleCount = 512;
  1298.                 MNM::TriangleID way[MaxWayTriangleCount] = { 0 };
  1299.  
  1300.                 const NavigationMesh& mesh = gAIEnv.pNavigationSystem->GetMesh(GetMeshID());
  1301.                 const MNM::CNavMesh::SGridParams& gridParams = mesh.navMesh.GetGridParams();
  1302.                 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));
  1303.  
  1304.                 const MNM::real_t verticalRange(3.0f);
  1305.  
  1306.                 MNM::vector3_t startLocation;
  1307.                 MNM::vector3_t endLocation;
  1308.  
  1309.                 MNM::TriangleID triangleStartID(0);
  1310.                 MNM::TriangleID triangleEndID(0);
  1311.  
  1312.                 for (TPathPoints::const_iterator it = pathList.begin(); it != pathList.end(); ++it)
  1313.                 {
  1314.                         TPathPoints::const_iterator itNext = it;
  1315.                         if (++itNext != pathList.end())
  1316.                         {
  1317.                                 const Vec3 from = it->vPos;
  1318.                                 const Vec3 to = itNext->vPos;
  1319.  
  1320.                                 if (triangleEndID)
  1321.                                 {
  1322.                                         startLocation = endLocation;
  1323.                                         endLocation.Set(MNM::real_t(to.x), MNM::real_t(to.y), MNM::real_t(to.z));
  1324.  
  1325.                                         triangleStartID = triangleEndID;
  1326.                                         triangleEndID = mesh.navMesh.GetTriangleAt(endLocation - origin, verticalRange, verticalRange);
  1327.                                 }
  1328.                                 else
  1329.                                 {
  1330.                                         startLocation.Set(MNM::real_t(from.x), MNM::real_t(from.y), MNM::real_t(from.z));
  1331.                                         endLocation.Set(MNM::real_t(to.x), MNM::real_t(to.y), MNM::real_t(to.z));
  1332.  
  1333.                                         triangleStartID = mesh.navMesh.GetTriangleAt(startLocation - origin, verticalRange, verticalRange);
  1334.                                         triangleEndID = mesh.navMesh.GetTriangleAt(endLocation - origin, verticalRange, verticalRange);
  1335.                                 }
  1336.  
  1337.                                 if (!triangleStartID || !triangleEndID)
  1338.                                         return false;
  1339.  
  1340.                                 MNM::CNavMesh::RayCastRequest<512> raycastRequest;
  1341.  
  1342.                                 if (mesh.navMesh.RayCast(startLocation, triangleStartID, endLocation, triangleEndID, raycastRequest) != MNM::CNavMesh::eRayCastResult_NoHit)
  1343.                                         return false;
  1344.                         }
  1345.                 }
  1346.         }
  1347.         else
  1348.         {
  1349.                 Vec3 closestPoint;
  1350.                 for (TPathPoints::const_iterator it = pathList.begin(); it != pathList.end(); ++it)
  1351.                 {
  1352.                         TPathPoints::const_iterator itNext = it;
  1353.                         if (++itNext == pathList.end())
  1354.                                 return true;
  1355.                         Vec3 from = it->vPos;
  1356.                         Vec3 to = itNext->vPos;
  1357.                         if (gAIEnv.pNavigation->IntersectsForbidden(from, to, closestPoint))
  1358.                                 return false;
  1359.                 }
  1360.         }
  1361.  
  1362.         return true;
  1363. }
  1364.  
  1365. //===================================================================
  1366. // AdjustPathAroundObstacleShape2D
  1367. //===================================================================
  1368. bool CNavPath::AdjustPathAroundObstacleShape2D(const SPathObstacleShape2D& obstacle, IAISystem::tNavCapMask navCapMask)
  1369. {
  1370.         FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  1371.  
  1372.         if (!(navCapMask & (IAISystem::NAV_TRIANGULAR | IAISystem::NAV_ROAD)))
  1373.                 return true;
  1374.  
  1375.         if (m_pathPoints.empty() || obstacle.pts.empty())
  1376.                 return true;
  1377.  
  1378.         const TPathPoints::iterator itPathEnd = m_pathPoints.end();
  1379.         // the path point before entering the shape
  1380.         TPathPoints::iterator itCutEntrance = itPathEnd;
  1381.         // the path point before exiting the shape
  1382.         TPathPoints::iterator itCutExit = itPathEnd;
  1383.         // the shape entrance/exit points
  1384.         Vec3 entrancePoint(ZERO);
  1385.         Vec3 exitPoint(ZERO);
  1386.         // counters to make sure we record the correct entrance/exit points
  1387.         float entranceCounter = std::numeric_limits<float>::max();
  1388.         float exitCounter = -std::numeric_limits<float>::max();
  1389.         // entrance/exit indices (and fractions) so that the entrance/exit points lie on the segment between
  1390.         // these indices and the index+1
  1391.         int entranceShapeIndex = -1;
  1392.         int exitShapeIndex = -1;
  1393.         float entranceShapeFrac = -1;
  1394.         float exitShapeFrac = -1;
  1395.  
  1396.         const unsigned pointAmount = obstacle.pts.size();
  1397.         const Vec3 polyCenter = std::accumulate(obstacle.pts.begin(), obstacle.pts.end(), Vec3(ZERO)) / (float)pointAmount;
  1398.  
  1399.         const float offset = 0.1f;
  1400.  
  1401.         const bool usingMNM = (GetMeshID() != NavigationMeshID(0));
  1402.         const IAISystem::ENavigationType startNavTypeFilter = usingMNM ? (IAISystem::ENavigationType)(IAISystem::NAV_TRIANGULAR | IAISystem::NAV_ROAD | IAISystem::NAV_UNSET) : (IAISystem::ENavigationType)(IAISystem::NAV_TRIANGULAR | IAISystem::NAV_ROAD);
  1403.         const IAISystem::ENavigationType endNavTypeFilter = (IAISystem::ENavigationType)(startNavTypeFilter | IAISystem::NAV_SMARTOBJECT);
  1404.  
  1405.         int segCounter = 0;
  1406.         for (TPathPoints::iterator it = m_pathPoints.begin(); it != itPathEnd; ++it, ++segCounter)
  1407.         {
  1408.                 TPathPoints::iterator itNext = it;
  1409.                 ++itNext;
  1410.                 if (itNext == itPathEnd)
  1411.                         break;
  1412.  
  1413.                 PathPointDescriptor& start = *it;
  1414.                 PathPointDescriptor& end = *itNext;
  1415.  
  1416.                 // Danny todo - problem if we start in tri, skip some indoor, then end in tri?
  1417.                 if (!(start.navType & startNavTypeFilter) || !(end.navType & endNavTypeFilter))
  1418.                         continue;
  1419.  
  1420.                 Lineseg pathSegment(start.vPos, end.vPos);
  1421.                 Vec3 intersectPos;
  1422.  
  1423.                 // if the seg intersection is for the entrance then this gets set so we know
  1424.                 // whether to over-ride it
  1425.  
  1426.                 for (unsigned pointIndex = 0; pointIndex < pointAmount; ++pointIndex)
  1427.                 {
  1428.                         unsigned nextPointIndex = (pointIndex + 1) % pointAmount;
  1429.                         Lineseg obstacleSegment(obstacle.pts[pointIndex], obstacle.pts[nextPointIndex]);
  1430.  
  1431.                         float pathSegmentFraction, obstacleSegmentFraction;
  1432.                         if (!Intersect::Lineseg_Lineseg2D(pathSegment, obstacleSegment, pathSegmentFraction, obstacleSegmentFraction))
  1433.                                 continue;
  1434.  
  1435.                         const Vec3 intersectionPoint = pathSegment.GetPoint(pathSegmentFraction);
  1436.  
  1437.                         const float zTolerance = 0.1f;
  1438.                         const bool isObstacleOnTheSameLayerAsThePathSegment = (intersectionPoint.z > obstacle.minZ - zTolerance) && (intersectionPoint.z < obstacle.maxZ + zTolerance);
  1439.                         if (!isObstacleOnTheSameLayerAsThePathSegment)
  1440.                                 continue;
  1441.  
  1442.                         if ((segCounter + pathSegmentFraction) < entranceCounter)
  1443.                         {
  1444.                                 entranceCounter = segCounter + pathSegmentFraction;
  1445.                                 itCutEntrance = it;
  1446.                                 entrancePoint = intersectionPoint;
  1447.                                 entranceShapeIndex = pointIndex;
  1448.                                 entranceShapeFrac = obstacleSegmentFraction;
  1449.                                 entrancePoint = entrancePoint + Vec2(entrancePoint - polyCenter).GetNormalized() * offset;
  1450.                         }
  1451.                         if ((segCounter + pathSegmentFraction) > exitCounter)
  1452.                         {
  1453.                                 exitCounter = segCounter + pathSegmentFraction;
  1454.                                 itCutExit = it;
  1455.                                 exitPoint = intersectionPoint;
  1456.                                 exitShapeIndex = pointIndex;
  1457.                                 exitShapeFrac = obstacleSegmentFraction;
  1458.                                 exitPoint = exitPoint + Vec2(entrancePoint - polyCenter).GetNormalized() * offset;
  1459.                         }
  1460.                 }
  1461.         }
  1462.  
  1463.         if (itCutEntrance == itPathEnd || itCutExit == itPathEnd)
  1464.                 return true;
  1465.  
  1466.         ++m_version.v;
  1467.  
  1468.         AIAssert(entranceShapeIndex >= 0 && entranceShapeIndex < (int)pointAmount);
  1469.         AIAssert(exitShapeIndex >= 0 && exitShapeIndex < (int)pointAmount);
  1470.  
  1471.         TPathPoints::iterator itCutExitAfter = itCutExit;
  1472.         ++itCutExitAfter;
  1473.         AIAssert(itCutExitAfter != itPathEnd);
  1474.         AIAssert(exitCounter >= entranceCounter);
  1475.  
  1476.         // First remove any intermediate points in the path list.
  1477.         for (TPathPoints::iterator it = itCutEntrance; it != itCutExitAfter; )
  1478.         {
  1479.                 if (it != itCutEntrance)
  1480.                         it = m_pathPoints.erase(it);
  1481.                 else
  1482.                         ++it;
  1483.         }
  1484.  
  1485.         // now insert the extra points before itCutExitAfter
  1486.         IAISystem::ENavigationType navType = itCutEntrance->navType;
  1487.  
  1488.         float distFwd = 0.0f;
  1489.         float distBwd = 0.0f;
  1490.         TPathPoints pathFwd;
  1491.         TPathPoints pathBwd;
  1492.  
  1493.         pathFwd.push_back(PathPointDescriptor(navType, entrancePoint));
  1494.         pathBwd.push_back(PathPointDescriptor(navType, entrancePoint));
  1495.  
  1496.         // if we enter/exit on the same edge then we need to make sure that we don't cut
  1497.         // through a forbidden edge - so still calculate the two routes (but fake the distances
  1498.         // for convenience)
  1499.         if (entranceShapeIndex == exitShapeIndex)
  1500.         {
  1501.                 bool shortestRouteIsFwd = entranceShapeFrac < exitShapeFrac;
  1502.                 distFwd = shortestRouteIsFwd ? 0.0f : 1.0f;
  1503.                 distBwd = shortestRouteIsFwd ? 1.0f : 0.0f;
  1504.  
  1505.                 if (shortestRouteIsFwd)
  1506.                 {
  1507.                         bool doingFirst = true;
  1508.                         for (int iPt = entranceShapeIndex; doingFirst || iPt != exitShapeIndex; )
  1509.                         {
  1510.                                 doingFirst = false;
  1511.                                 Vec3 aheadPt = obstacle.pts[iPt];
  1512.                                 aheadPt.z = entrancePoint.z;
  1513.                                 aheadPt = aheadPt + Vec2(aheadPt - polyCenter).GetNormalized() * offset;
  1514.                                 pathBwd.push_back(PathPointDescriptor(navType, aheadPt));
  1515.                                 if (--iPt < 0)
  1516.                                         iPt = pointAmount - 1;
  1517.                         }
  1518.                 }
  1519.                 else
  1520.                 {
  1521.                         bool doingFirst = true;
  1522.                         for (int iPt = entranceShapeIndex; doingFirst || iPt != exitShapeIndex; )
  1523.                         {
  1524.                                 doingFirst = false;
  1525.                                 int iAheadPt = (iPt + 1) % pointAmount;
  1526.                                 Vec3 aheadPt = obstacle.pts[iAheadPt];
  1527.                                 aheadPt.z = entrancePoint.z;
  1528.                                 aheadPt = aheadPt + Vec2(aheadPt - polyCenter).GetNormalized() * offset;
  1529.                                 pathFwd.push_back(PathPointDescriptor(navType, aheadPt));
  1530.                                 if (++iPt >= (int)pointAmount)
  1531.                                         iPt = 0;
  1532.                         }
  1533.                 }
  1534.         }
  1535.         else
  1536.         {
  1537.                 // Generate paths around the object in both directions to (a) choose the shortest
  1538.                 // and (b) be able to choose one that is less problematic
  1539.                 distFwd = (obstacle.pts[exitShapeIndex] - exitPoint).GetLength2D();
  1540.                 distBwd = (obstacle.pts[(exitShapeIndex + 1) % pointAmount] - exitPoint).GetLength2D();
  1541.  
  1542.                 // fwd
  1543.                 Vec3 lastPt = entrancePoint;
  1544.                 for (int iPt = entranceShapeIndex; iPt != exitShapeIndex; )
  1545.                 {
  1546.                         int iAheadPt = (iPt + 1) % pointAmount;
  1547.                         Vec3 aheadPt = obstacle.pts[iAheadPt];
  1548.                         aheadPt.z = entrancePoint.z;
  1549.                         aheadPt = aheadPt + Vec2(aheadPt - polyCenter).GetNormalized() * offset;
  1550.                         pathFwd.push_back(PathPointDescriptor(navType, aheadPt));
  1551.                         distFwd += (aheadPt - lastPt).GetLength2D();
  1552.                         lastPt = aheadPt;
  1553.                         if (++iPt >= (int)pointAmount)
  1554.                                 iPt = 0;
  1555.                 }
  1556.                 // bwd
  1557.                 lastPt = entrancePoint;
  1558.                 for (int iPt = entranceShapeIndex; iPt != exitShapeIndex; )
  1559.                 {
  1560.                         Vec3 aheadPt = obstacle.pts[iPt];
  1561.                         aheadPt.z = entrancePoint.z;
  1562.                         aheadPt = aheadPt + Vec2(aheadPt - polyCenter).GetNormalized() * offset;
  1563.                         pathBwd.push_back(PathPointDescriptor(navType, aheadPt));
  1564.                         distBwd += (aheadPt - lastPt).GetLength2D();
  1565.                         lastPt = aheadPt;
  1566.                         if (--iPt < 0)
  1567.                                 iPt = pointAmount - 1;
  1568.                 }
  1569.         }
  1570.  
  1571.         pathFwd.push_back(PathPointDescriptor(navType, exitPoint));
  1572.         pathBwd.push_back(PathPointDescriptor(navType, exitPoint));
  1573.  
  1574.         float checkRadius = 0.3f;
  1575.         if (distFwd < distBwd)
  1576.         {
  1577.                 if (CheckPath(pathFwd, checkRadius))
  1578.                         m_pathPoints.insert(itCutExitAfter, pathFwd.begin(), pathFwd.end());
  1579.                 else if (CheckPath(pathBwd, checkRadius))
  1580.                         m_pathPoints.insert(itCutExitAfter, pathBwd.begin(), pathBwd.end());
  1581.                 else
  1582.                         return false;
  1583.                 return true;
  1584.         }
  1585.         else
  1586.         {
  1587.                 if (CheckPath(pathBwd, checkRadius))
  1588.                         m_pathPoints.insert(itCutExitAfter, pathBwd.begin(), pathBwd.end());
  1589.                 else if (CheckPath(pathFwd, checkRadius))
  1590.                         m_pathPoints.insert(itCutExitAfter, pathFwd.begin(), pathFwd.end());
  1591.                 else
  1592.                         return false;
  1593.                 return true;
  1594.         }
  1595. }
  1596.  
  1597. //===================================================================
  1598. // AdjustPathAroundObstacle
  1599. //===================================================================
  1600. bool CNavPath::AdjustPathAroundObstacle(const CPathObstacle& obstacle, IAISystem::tNavCapMask navCapMask)
  1601. {
  1602.         switch (obstacle.GetType())
  1603.         {
  1604.         case CPathObstacle::ePOT_Shape2D:
  1605.                 return AdjustPathAroundObstacleShape2D(obstacle.GetShape2D(), navCapMask);
  1606.         default:
  1607.                 AIError("CNavPath::AdjustPathAroundObstacle unhandled type %d", obstacle.GetType());
  1608.         }
  1609.         return false;
  1610. }
  1611.  
  1612. //===================================================================
  1613. // AdjustPathAroundObstacles
  1614. //===================================================================
  1615. bool CNavPath::AdjustPathAroundObstacles(const CPathObstacles& obstacles, IAISystem::tNavCapMask navCapMask)
  1616. {
  1617.         MovePathEndsOutOfObstacles(obstacles);
  1618.  
  1619.         for (TPathObstacles::const_iterator it = obstacles.GetCombinedObstacles().begin(); it != obstacles.GetCombinedObstacles().end(); ++it)
  1620.         {
  1621.                 const CPathObstacle& obstacle = **it;
  1622.                 if (!AdjustPathAroundObstacle(obstacle, navCapMask))
  1623.                         return false;
  1624.         }
  1625.         return true;
  1626. }
  1627.  
  1628. bool CNavPath::AdjustPathAroundObstacles(const Vec3& currentpos, const AgentMovementAbility& movementAbility)
  1629. {
  1630.         m_obstacles.CalculateObstaclesAroundLocation(currentpos, movementAbility, this);
  1631.  
  1632.         MovePathEndsOutOfObstacles(m_obstacles);
  1633.  
  1634.         for (TPathObstacles::const_iterator it = m_obstacles.GetCombinedObstacles().begin(); it != m_obstacles.GetCombinedObstacles().end(); ++it)
  1635.         {
  1636.                 const CPathObstacle& obstacle = **it;
  1637.                 if (!AdjustPathAroundObstacle(obstacle, movementAbility.pathfindingProperties.navCapMask))
  1638.                         return false;
  1639.         }
  1640.         return true;
  1641. }
  1642.  
  1643. //===================================================================
  1644. // GetMaxDistanceFromStart
  1645. //===================================================================
  1646. float CNavPath::GetMaxDistanceFromStart() const
  1647. {
  1648.         if (m_pathPoints.size() < 2)
  1649.                 return .0f;
  1650.  
  1651.         float maxDistanceSq = .0f;
  1652.         TPathPoints::const_iterator start = m_pathPoints.begin();
  1653.         const Vec3& startPathPointPosition = start->vPos;
  1654.         TPathPoints::const_iterator it = ++m_pathPoints.begin();
  1655.         TPathPoints::const_iterator end = m_pathPoints.end();
  1656.         for (; it != end; ++it)
  1657.         {
  1658.                 const PathPointDescriptor& ppd = *it;
  1659.                 const float distanceSq = Distance::Point_PointSq(startPathPointPosition, ppd.vPos);
  1660.                 maxDistanceSq = static_cast<float>(__fsel(maxDistanceSq - distanceSq, maxDistanceSq, distanceSq));
  1661.         }
  1662.  
  1663.         return sqrt_tpl(maxDistanceSq);
  1664. }
  1665.  
  1666. //===================================================================
  1667. // GetAABB
  1668. //===================================================================
  1669. AABB CNavPath::GetAABB(float dist) const
  1670. {
  1671.         float distLeft = dist;
  1672.  
  1673.         AABB result(AABB::RESET);
  1674.  
  1675.         if (m_pathPoints.size() < 2)
  1676.                 return result;
  1677.  
  1678.         Vec3 thisPt;
  1679.         if (!GetPosAlongPath(thisPt))
  1680.                 return result;
  1681.  
  1682.         result.Add(thisPt);
  1683.  
  1684.         // remaining path
  1685.         for (TPathPoints::const_iterator it = ++m_pathPoints.begin(); it != m_pathPoints.end(); ++it)
  1686.         {
  1687.                 const PathPointDescriptor& ppd = *it;
  1688.  
  1689.                 Vec3 delta = ppd.vPos - thisPt;
  1690.                 float deltaLen = delta.NormalizeSafe();
  1691.                 if (deltaLen > distLeft)
  1692.                 {
  1693.                         result.Add(thisPt + distLeft * delta);
  1694.                         return result;
  1695.                 }
  1696.                 thisPt = ppd.vPos;
  1697.                 result.Add(thisPt);
  1698.                 distLeft -= deltaLen;
  1699.         }
  1700.         return result;
  1701. }
  1702.  
  1703. //===================================================================
  1704. // GetDistToPath
  1705. //===================================================================
  1706. float CNavPath::GetDistToPath(Vec3& pathPosOut, float& distAlongPathOut, const Vec3& pos, const float dist, const bool twoD) const
  1707. {
  1708.         float distOut = std::numeric_limits<float>::max();
  1709.  
  1710.         float traversedDist = 0.0f;
  1711.         const TPathPoints::const_iterator pathEndIt = m_pathPoints.end();
  1712.         for (TPathPoints::const_iterator pathIt = m_pathPoints.begin(); pathIt != pathEndIt; ++pathIt)
  1713.         {
  1714.                 TPathPoints::const_iterator pathNextIt = pathIt;
  1715.                 ++pathNextIt;
  1716.                 if (pathNextIt == pathEndIt)
  1717.                         break;
  1718.  
  1719.                 Lineseg seg(pathIt->vPos, pathNextIt->vPos);
  1720.  
  1721.                 float segLen = twoD ? Distance::Point_Point2D(seg.start, seg.end) : Distance::Point_Point(seg.start, seg.end);
  1722.                 if (segLen < 0.001f)
  1723.                         continue;
  1724.                 if (segLen + traversedDist > dist)
  1725.                 {
  1726.                         float frac = (dist - traversedDist) / (segLen + traversedDist);
  1727.                         seg.end = frac * seg.end + (1.0f - frac) * seg.start;
  1728.                 }
  1729.  
  1730.                 float t;
  1731.                 float thisDist = twoD ? Distance::Point_Lineseg2D(pos, seg, t) : Distance::Point_Lineseg(pos, seg, t);
  1732.                 if (thisDist < distOut)
  1733.                 {
  1734.                         distOut = thisDist;
  1735.                         pathPosOut = seg.GetPoint(t);
  1736.                         distAlongPathOut = traversedDist + t * segLen;
  1737.                 }
  1738.  
  1739.                 traversedDist += segLen;
  1740.                 if (traversedDist > dist)
  1741.                         break;
  1742.         }
  1743.         return distOut < std::numeric_limits<float>::max() ? distOut : -1.0f;
  1744. }
  1745.  
  1746. void CNavPath::GetDirectionToPathFromPoint(const Vec3& point, Vec3& dir) const
  1747. {
  1748.         if (m_pathPoints.empty())
  1749.         {
  1750.                 dir.zero();
  1751.         }
  1752.         else if (m_pathPoints.size() == 1)
  1753.         {
  1754.                 dir = m_pathPoints.front().vPos - point;
  1755.         }
  1756.         else
  1757.         {
  1758.                 const Vec3 currentPositionOnPath = GetPrevPathPoint()->vPos * (1.0f - m_currentFrac) + GetNextPathPoint()->vPos * m_currentFrac;
  1759.                 dir = currentPositionOnPath - point;
  1760.         }
  1761. }
  1762.  
  1763. static float criticalMinDist = 0.5f;
  1764. static float criticalMaxDist = 4.0f;
  1765.  
  1766. //===================================================================
  1767. // CanTargetPointBeReached
  1768. //===================================================================
  1769. ETriState CNavPath::CanTargetPointBeReached(CTargetPointRequest& request, const CAIActor* pAIActor, bool twoD) const
  1770. {
  1771.         request.result = eTS_false;
  1772.  
  1773.         // Allow to check for the trivial cases even if there is inly one point on path.
  1774.         if (m_pathPoints.empty())
  1775.                 return eTS_false;
  1776.  
  1777.         const char* szAIActorName = pAIActor->GetName();
  1778.  
  1779.         const PathPointDescriptor& lastPathPoint = m_pathPoints.back();
  1780.         Vec3 curEndPt = lastPathPoint.vPos;
  1781.         curEndPt.z = request.targetPoint.z + 0.5f;
  1782.  
  1783.         request.pathID = m_version.v;
  1784.         request.itIndex = 0;
  1785.         request.itBeforeIndex = 1;
  1786.         request.splitPoint = curEndPt;
  1787.  
  1788.         Vec3 delta = request.targetPoint - curEndPt;
  1789.         if (twoD)
  1790.                 delta.z = 0.0f;
  1791.         float dist = delta.GetLength();
  1792.  
  1793.         // blindly allow very small changes
  1794.         if (dist < criticalMinDist)
  1795.         {
  1796.                 AILogComment("CNavPath::CanTargetPointBeReached trivial distance for %s", szAIActorName);
  1797.                 return request.result = eTS_true;
  1798.         }
  1799.  
  1800.         // forbid huge changes
  1801.         if (dist > criticalMaxDist)
  1802.         {
  1803.                 AILogComment("CNavPath::CanTargetPointBeReached excessive distance for %s", szAIActorName);
  1804.                 return request.result = eTS_maybe;
  1805.         }
  1806.  
  1807.         // The remaining checks require at least one segment on the path.
  1808.         if (m_pathPoints.size() < 2)
  1809.                 return eTS_false;
  1810.  
  1811.         // this wouldn't be right to do here. it would break the entering into vehicles
  1812.         // since the entry point is sometimes inside the obstacle shape of the vehicle.
  1813.         //
  1814.         //  const CPathObstacles &pathAdjustmentObstacles = pPuppet->GetLastPathAdjustmentObstacles();
  1815.         //  if (pathAdjustmentObstacles.IsPointInsideObstacles(request.targetPoint))
  1816.         //    return eTS_false;
  1817.         //
  1818.  
  1819.         // walk back dist along the path from the end. If it's possible to walk from this point to the
  1820.         // requested end point then cut the end of the path off
  1821.  
  1822.         TPathPoints::const_reverse_iterator it = m_pathPoints.rbegin();
  1823.         TPathPoints::const_reverse_iterator itBefore = it;
  1824.         float distLeft = dist;
  1825.         float lastFrac = 1.0f; // at the end this is the fraction between it and itBefore
  1826.         Vec3 candidatePt = curEndPt;
  1827.         for (++itBefore; it != m_pathPoints.rend() && itBefore != m_pathPoints.rend(); ++it, ++itBefore)
  1828.         {
  1829.                 const Vec3& pos = it->vPos;
  1830.                 const Vec3& posBefore = itBefore->vPos;
  1831.                 float thisSegLen = twoD ? (pos - posBefore).GetLength2D() : (pos - posBefore).GetLength();
  1832.                 if (thisSegLen <= 0.0001f)
  1833.                         continue;
  1834.                 if (thisSegLen < distLeft)
  1835.                 {
  1836.                         distLeft -= thisSegLen;
  1837.                         candidatePt = posBefore;
  1838.                 }
  1839.                 else
  1840.                 {
  1841.                         lastFrac = distLeft / thisSegLen;
  1842.                         candidatePt = lastFrac * posBefore + (1.0f - lastFrac) * pos;
  1843.                         break;
  1844.                 }
  1845.         }
  1846.         candidatePt.z = request.targetPoint.z + 0.5f;
  1847.  
  1848.         // already checked that there's more than 2 points, so itBefore should hit rend first
  1849.         AIAssert(it != m_pathPoints.rend());
  1850.  
  1851.         request.itIndex = (int)std::distance(m_pathPoints.rbegin(), it);
  1852.         request.itBeforeIndex = (int)std::distance(m_pathPoints.rbegin(), itBefore);
  1853.         request.splitPoint = candidatePt;
  1854.  
  1855.         Vec3 offset(0, 0, 1.0f);
  1856.         float radius = pAIActor->m_Parameters.m_fPassRadius;
  1857.         if (twoD)
  1858.         {
  1859.                 if (!CheckWalkability(candidatePt, request.targetPoint + Vec3(0, 0, 0.5f), radius))
  1860.                 {
  1861.                         AILogComment("CNavPath::CanTargetPointBeReached no 2D walkability for %s", szAIActorName);
  1862.                         return request.result = eTS_false;
  1863.                 }
  1864.         }
  1865.         else
  1866.         {
  1867.                 if (OverlapCapsule(Lineseg(candidatePt, request.targetPoint), radius, AICE_ALL))
  1868.                 {
  1869.                         AILogComment("CNavPath::CanTargetPointBeReached no 3D passability for %s", szAIActorName);
  1870.                         return request.result = eTS_false;
  1871.                 }
  1872.         }
  1873.  
  1874.         AILogComment("CNavPath::CanTargetPointBeReached Accepting new path target point (%5.2f, %5.2f, %5.2f) for %s",
  1875.                      request.targetPoint.x, request.targetPoint.y, request.targetPoint.z, szAIActorName);
  1876.  
  1877.         return request.result = eTS_true;
  1878. }
  1879.  
  1880. //===================================================================
  1881. // UseTargetPointRequest
  1882. // If we accept the request then it becomes impractical to store
  1883. // the request and apply it in subsequent path regenerations - so
  1884. // path regeneration needs to be disabled
  1885. //===================================================================
  1886. bool CNavPath::UseTargetPointRequest(const CTargetPointRequest& request, CAIActor* pAIActor, bool twoD)
  1887. {
  1888.         if (m_pathPoints.empty())
  1889.                 return false;
  1890.  
  1891.         const char* szAIActorName = pAIActor->GetName();
  1892.  
  1893.         PathPointDescriptor& lastPathPoint = m_pathPoints.back();
  1894.         const Vec3& curEndPt = lastPathPoint.vPos;
  1895.         // assume that the only ppd with a pSONavData is the last one
  1896.         PathPointDescriptor::SmartObjectNavDataPtr pSONavData = lastPathPoint.pSONavData;
  1897.         IAISystem::ENavigationType lastNavType = lastPathPoint.navType;
  1898.  
  1899.         const float dist = twoD ? Distance::Point_Point2D(request.targetPoint, curEndPt) : Distance::Point_Point(request.targetPoint, curEndPt);
  1900.         // blindly allow very small changes
  1901.         if (dist < criticalMinDist)
  1902.         {
  1903.                 AILogComment("CNavPath::UseTargetPointRequest trivial distance for %s (%.3f)", szAIActorName, dist);
  1904.                 lastPathPoint.vPos = request.targetPoint;
  1905.                 m_pathPoints.push_front(PathPointDescriptor(IAISystem::NAV_UNSET, pAIActor->GetPhysicsPos()));
  1906.                 return true;
  1907.         }
  1908.  
  1909.         // We (Mikko/Danny) are not sure, but think that this case happens when the guy starts near the
  1910.         // exact position destination and he finishes his path before the exact positioning request calls
  1911.         // here. So - we need to indicate to exact positioning that we can continue -
  1912.         if (m_pathPoints.size() == 1)
  1913.         {
  1914.                 AILogComment("CNavPath::UseTargetPointRequest excessive for %s when path size = 1", szAIActorName);
  1915.                 return false;
  1916.         }
  1917.  
  1918.         CTargetPointRequest workingReq = request;
  1919.         if (request.pathID != m_version.v || request.result != eTS_true)
  1920.         {
  1921.                 ETriState res = CanTargetPointBeReached(workingReq, pAIActor, twoD);
  1922.                 if (res != eTS_true)
  1923.                         return false;
  1924.         }
  1925.  
  1926.         Limit(workingReq.itIndex, 0, (int) m_pathPoints.size() - 1);
  1927.         Limit(workingReq.itBeforeIndex, 0, (int) m_pathPoints.size() - 1);
  1928.  
  1929.         TPathPoints::reverse_iterator it = m_pathPoints.rbegin();
  1930.         TPathPoints::reverse_iterator itBefore = m_pathPoints.rbegin();
  1931.         std::advance(it, workingReq.itIndex);
  1932.         std::advance(itBefore, workingReq.itBeforeIndex);
  1933.  
  1934.         if (itBefore == m_pathPoints.rend())
  1935.         {
  1936.                 // replace the whole path with one that goes from the first point to the new point
  1937.                 TPathPoints::iterator pit = m_pathPoints.begin();
  1938.                 ++pit;
  1939.                 m_pathPoints.erase(pit, m_pathPoints.end());
  1940.                 // clone the first point and modify it.
  1941.                 m_pathPoints.push_back(m_pathPoints.back());
  1942.                 m_pathPoints.back().vPos = workingReq.targetPoint;
  1943.         }
  1944.         else
  1945.         {
  1946.                 TPathPoints::iterator fwdIt = it.base();
  1947.                 --fwdIt; // now fwdIt points to the same element as it
  1948.                 // move the last point and delete the rest
  1949.                 fwdIt->vPos = workingReq.splitPoint;
  1950.                 ++fwdIt;
  1951.                 m_pathPoints.erase(fwdIt, m_pathPoints.end());
  1952.                 // clone the first point and modify it.
  1953.                 m_pathPoints.push_back(m_pathPoints.back());
  1954.                 m_pathPoints.back().vPos = workingReq.targetPoint;
  1955.                 if (lastNavType == IAISystem::NAV_SMARTOBJECT)
  1956.                         m_pathPoints.back().navType = lastNavType;
  1957.         }
  1958.  
  1959.         m_pathPoints.back().pSONavData = pSONavData;
  1960.         m_pathPoints.back().vPos = workingReq.targetPoint;
  1961.         m_params.inhibitPathRegeneration = true;
  1962.         m_params.continueMovingAtEnd = (lastNavType == IAISystem::NAV_SMARTOBJECT); // request.continueMovingAtEnd;
  1963.         ++m_version.v;
  1964.         return true;
  1965. }
  1966.  
  1967. //===================================================================
  1968. // GetDistToSmartObject
  1969. //===================================================================
  1970. float CNavPath::GetDistToSmartObject(bool b2D) const
  1971. {
  1972.         if (m_pathPoints.size() < 2)
  1973.                 return std::numeric_limits<float>::max();
  1974.  
  1975.         TPathPoints::const_iterator pathIt = m_pathPoints.begin();
  1976.         IAISystem::ENavigationType typeCur = pathIt->navType;
  1977.         ++pathIt;
  1978.         IAISystem::ENavigationType typeNext = pathIt->navType;
  1979.  
  1980.         if (typeCur == IAISystem::NAV_SMARTOBJECT && typeNext == IAISystem::NAV_SMARTOBJECT)
  1981.                 return 0.0f;
  1982.  
  1983.         Vec3 curPos;
  1984.         GetPosAlongPath(curPos);
  1985.  
  1986.         float dist = 0.0f;
  1987.         const TPathPoints::const_iterator pathEndIt = m_pathPoints.end();
  1988.         for (; pathIt != pathEndIt; ++pathIt)
  1989.         {
  1990.                 Vec3 thisPos = pathIt->vPos;
  1991.                 dist += b2D ? Distance::Point_Point2D(curPos, pathIt->vPos) : Distance::Point_Point(curPos, pathIt->vPos);
  1992.  
  1993.                 TPathPoints::const_iterator pathItNext = pathIt;
  1994.                 ++pathItNext;
  1995.                 if (pathItNext != pathEndIt)
  1996.                 {
  1997.                         if (pathIt->navType == IAISystem::NAV_SMARTOBJECT && pathItNext->navType == IAISystem::NAV_SMARTOBJECT)
  1998.                                 return dist;
  1999.                 }
  2000.                 else
  2001.                 {
  2002.                         // End of the path
  2003.                         if (pathIt->navType == IAISystem::NAV_SMARTOBJECT)
  2004.                                 return dist;
  2005.                 }
  2006.  
  2007.                 curPos = pathIt->vPos;
  2008.         }
  2009.         return std::numeric_limits<float>::max();
  2010. }
  2011.  
  2012. //===================================================================
  2013. // GetLastPathPointAnimNavSOData
  2014. //===================================================================
  2015. PathPointDescriptor::SmartObjectNavDataPtr CNavPath::GetLastPathPointAnimNavSOData() const
  2016. {
  2017.         if (!m_pathPoints.empty())
  2018.         {
  2019.                 const PathPointDescriptor& lastPoint = m_pathPoints.back();
  2020.                 if (lastPoint.navType == IAISystem::NAV_SMARTOBJECT)
  2021.                 {
  2022.                         if (lastPoint.navSOMethod == nSOmSignalAnimation || lastPoint.navSOMethod == nSOmActionAnimation)
  2023.                         {
  2024.                                 return lastPoint.pSONavData;
  2025.                         }
  2026.                 }
  2027.         }
  2028.  
  2029.         return 0;
  2030. }
  2031.  
  2032. const PathPointDescriptor::OffMeshLinkData* CNavPath::GetLastPathPointMNNSOData() const
  2033. {
  2034.         if (!m_pathPoints.empty())
  2035.         {
  2036.                 const PathPointDescriptor& lastPoint = m_pathPoints.back();
  2037.                 if (lastPoint.navType == IAISystem::NAV_SMARTOBJECT)
  2038.                 {
  2039.                         if (lastPoint.navSOMethod == nSOmSignalAnimation || lastPoint.navSOMethod == nSOmActionAnimation)
  2040.                         {
  2041.                                 return &lastPoint.offMeshLinkData;
  2042.                         }
  2043.                 }
  2044.         }
  2045.  
  2046.         return NULL;
  2047. }
  2048.  
  2049. //===================================================================
  2050. // Serialize
  2051. //===================================================================
  2052. void CNavPath::SVersion::Serialize(TSerialize ser)
  2053. {
  2054.         ser.Value("version", v);
  2055. }
  2056.  
downloadNavPath.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