BVB Source Codes

CRYENGINE Show SmartPathFollower.cpp Source code

Return Download CRYENGINE: download SmartPathFollower.cpp Source code - Download CRYENGINE Source code - Type:.cpp
  1. // Copyright 2001-2016 Crytek GmbH / Crytek Group. All rights reserved.
  2.  
  3. /********************************************************************
  4.    CryGame Source File.
  5.    ---------------------------------------------------------------------
  6.    File name:   SmartPathFollower.cpp
  7.    Version:     v1.00
  8.    Description: PathFollower implementaion based on tracking the
  9.    furthest reachable position on the path.
  10.  
  11.    ---------------------------------------------------------------------
  12.    History:
  13.    - 16 Feb 2010  : Will Wilson: Created.
  14.  
  15.  *********************************************************************/
  16.  
  17. #include "StdAfx.h"
  18.  
  19. #include "SmartPathFollower.h"
  20. #include "CAISystem.h"
  21. #include "AILog.h"
  22. #include "NavPath.h"
  23. #include "NavRegion.h"
  24. #include "Walkability/WalkabilityCacheManager.h"
  25. #include "DebugDrawContext.h"
  26. #include <CryGame/IGameFramework.h>
  27.  
  28. #include "Navigation/NavigationSystem/NavigationSystem.h"
  29.  
  30. //#pragma optimize("", off)
  31. //#pragma inline_depth(0)
  32.  
  33. /*static*/ const float InterpolatedPath::k_defaultClosestFindSegmentToleranceZ = 0.5f;
  34.  
  35. float InterpolatedPath::FindNextSegmentIndex(size_t startIndex) const
  36. {
  37.         float addition = .0f;
  38.         if (!m_points.empty())
  39.         {
  40.                 TPoints::const_iterator iter(m_points.begin());
  41.  
  42.                 // Move index to start of search
  43.                 std::advance(iter, startIndex);
  44.                 float previousDistance = iter->distance;
  45.                 if (++iter != m_points.end())
  46.                 {
  47.                         addition = 1.0f;
  48.                 }
  49.         }
  50.         return startIndex + addition;
  51. }
  52.  
  53. // Search for a segment index based on the distance along the path, startIndex is optional to allow search optimization
  54. float InterpolatedPath::FindSegmentIndexAtDistance(float requestedDistance, size_t startIndex) const
  55. {
  56.         // Default to returning the last index (or -1 if empty)
  57.         float result = static_cast<float>(m_points.size()) - 1.0f;
  58.  
  59.         // Clamp to limits
  60.         requestedDistance = clamp_tpl(requestedDistance, 0.0f, m_totalDistance);
  61.  
  62.         if (!m_points.empty())
  63.         {
  64.                 TPoints::const_iterator iter(m_points.begin());
  65.  
  66.                 // Move index to start of search
  67.                 std::advance(iter, startIndex);
  68.  
  69.                 float prevDistance = iter->distance;
  70.                 AIAssert(prevDistance <= requestedDistance + 0.01f);
  71.  
  72.                 for (; iter != m_points.end(); ++iter)
  73.                 {
  74.                         const SPathControlPoint2& point = *iter;
  75.                         const float pointDistance = point.distance;
  76.  
  77.                         // If waypoint at requested distance
  78.                         if (pointDistance >= requestedDistance)
  79.                         {
  80.                                 float end = static_cast<float>(std::distance(m_points.begin(), iter));
  81.                                 float start = (iter != m_points.begin()) ? end - 1.0f : 0.0f;
  82.  
  83.                                 float distanceWithinSegment = requestedDistance - prevDistance;
  84.                                 float segmentDistance = pointDistance - prevDistance;
  85.                                 AIAssert(distanceWithinSegment <= segmentDistance);
  86.  
  87.                                 float delta = (segmentDistance > 0.001f) ? distanceWithinSegment / segmentDistance : 0.0f;
  88.  
  89.                                 AIAssert(delta >= 0.0f && delta <= 1.0f);
  90.  
  91.                                 result = Lerp(start, end, delta);
  92.                                 break;
  93.                         }
  94.  
  95.                         prevDistance = pointDistance;
  96.                 }
  97.         }
  98.  
  99.         return result;
  100. }
  101.  
  102. /*
  103.    struct TestPathFollower
  104.    {
  105.    TestPathFollower()
  106.    {
  107.    InterpolatedPath p;
  108.  
  109.    p.push_back( SPathControlPoint2( IAISystem::NAV_UNSET, Vec3(784.01086, 764.79883, 72.698608), 0 ) );
  110.    p.push_back( SPathControlPoint2( IAISystem::NAV_UNSET, Vec3(783.87500, 764.75000, 72.750000), 0 ) );
  111.  
  112.    const float r = p.FindClosestSegmentIndex( Vec3(786.97339, 751.88739, 72.055885), 0, 5 );
  113.    const float s = p.FindClosestSegmentIndex( Vec3(786.97339, 751.88739, 72.600000), 0, 5 );
  114.    }
  115.    } g__;
  116.  */
  117.  
  118. // Returns the segment index of the closest point on the path (fractional part provides exact position if required)
  119. float InterpolatedPath::FindClosestSegmentIndex(const Vec3& testPoint, const float startDistance,
  120.                                                 const float endDistance, const float toleranceZ /* = 0.5f */, const bool bUse2D /* = true */) const
  121. {
  122.         AIAssert(startDistance <= endDistance);
  123.  
  124.         float bestIndex = -1.0f;
  125.  
  126.         const size_t pointCount = m_points.size();
  127.         if (pointCount != 0)
  128.         {
  129.                 // If the point is this close or closer, consider it on the segment and stop searching
  130.                 static const float MIN_DISTANCE_TO_SEGMENT = 0.01f;
  131.  
  132.                 float bestDistSq = std::numeric_limits<float>::max();
  133.  
  134.                 const float startIndex = FindSegmentIndexAtDistance(startDistance);
  135.                 const size_t startIndexInt = static_cast<size_t>(startIndex);
  136.                 const float endIndex = FindSegmentIndexAtDistance(endDistance, startIndexInt);
  137.                 const size_t endIndexInt = static_cast<size_t>(ceil(endIndex));
  138.  
  139.                 const Vec3 startPos(GetPositionAtSegmentIndex(startIndex));
  140.                 const Vec3 endPos(GetPositionAtSegmentIndex(endIndex));
  141.  
  142.                 // Move index to start of search
  143.                 TPoints::const_iterator iter(m_points.begin());
  144.                 std::advance(iter, startIndexInt);
  145.                 // If we have more than one point, we skip the [startPos,startPos] segment as the check will be included in [startPos,secondPos]
  146.                 const bool evaluatingSinglePoint = (startIndex == endIndex);
  147.                 std::advance(iter, (evaluatingSinglePoint) ? 0 : 1);
  148.  
  149.                 const TPoints::const_iterator finalIter(m_points.begin() + endIndexInt + 1);
  150.  
  151.                 float segmentStartIndex = startIndex;
  152.                 Vec3 segmentStartPos(startPos);
  153.  
  154.                 const float testZ = testPoint.z;
  155.  
  156.                 // TODO: Do the search backwards so that we make sure that, if wierd stuff goes on and we're facing a worst possible scenario, we end up with the
  157.                 // that is furthest away from the start.
  158.                 for (; iter != finalIter; ++iter)
  159.                 {
  160.                         const SPathControlPoint2& point = *iter;
  161.  
  162.                         const size_t segmentEndIndexInt = std::distance(m_points.begin(), iter);
  163.                         const float segmentEndIndex = min(static_cast<float>(segmentEndIndexInt), endIndex);
  164.                         const Vec3 segmentEndPos = (segmentEndIndexInt != endIndexInt) ? point.pos : endPos;
  165.  
  166.                         const Lineseg segment(segmentStartPos, segmentEndPos);
  167.  
  168.                         // Check segment at the same approx. height as the test point
  169.                         if (toleranceZ == FLT_MAX ||
  170.                             ((testZ >= min(segment.start.z, segment.end.z) - toleranceZ) &&
  171.                              (testZ <= max(segment.start.z, segment.end.z) + toleranceZ)))
  172.                         {
  173.                                 float segmentDelta;
  174.                                 const float distToSegmentSq =
  175.                                   bUse2D
  176.                                   ? Distance::Point_Lineseg2DSq(testPoint, segment, segmentDelta)
  177.                                   : Distance::Point_LinesegSq(testPoint, segment, segmentDelta);
  178.  
  179.                                 // If this is a new best
  180.                                 if (distToSegmentSq < bestDistSq)
  181.                                 {
  182.                                         bestDistSq = distToSegmentSq;
  183.  
  184.                                         // Calculate length of the segment (may not be 1 at start and end)
  185.                                         const float segmentLength = segmentEndIndex - segmentStartIndex;
  186.                                         // Calculate the segment index by incorporating the delta returned by the segment test
  187.                                         bestIndex = segmentStartIndex + (segmentDelta * segmentLength);
  188.  
  189.                                         // Early out if point on line - it can't get any closer
  190.                                         if (bestDistSq <= square(MIN_DISTANCE_TO_SEGMENT))
  191.                                                 break;
  192.                                 }
  193.                         }
  194.  
  195.                         segmentStartPos = segmentEndPos;
  196.                         segmentStartIndex = segmentEndIndex;
  197.                 }
  198.         }
  199.  
  200.         return bestIndex;
  201. }
  202.  
  203. /// True if the path section between the start and endIndex deviates from the line by no more than maxDeviation.
  204. bool InterpolatedPath::IsParrallelTo(const Lineseg& line, float startIndex, float endIndex, float maxDeviation) const
  205. {
  206.         AIAssert(startIndex <= endIndex);
  207.  
  208.         // Move along path between start point (inclusive) and end point (exclusive) testing each control point
  209.         for (float index = startIndex; index < endIndex; index = floorf(index) + 1.0f)
  210.         {
  211.                 Vec3 testPos(GetPositionAtSegmentIndex(index));
  212.  
  213.                 float delta;
  214.                 if (Distance::Point_Lineseg2DSq(testPos, line, delta) > maxDeviation)
  215.                         return false;
  216.         }
  217.  
  218.         // Finally test the end position
  219.         Vec3 endPos(GetPositionAtSegmentIndex(endIndex));
  220.  
  221.         float delta;
  222.         return (Distance::Point_Lineseg2DSq(endPos, line, delta) <= maxDeviation);
  223. }
  224.  
  225. Vec3 InterpolatedPath::GetPositionAtSegmentIndex(float index) const
  226. {
  227.         AIAssert(!m_points.empty());
  228.  
  229.         // Bound index to minimum
  230.         index = max(index, 0.0f);
  231.  
  232.         float delta = fmodf(index, 1.0f);
  233.  
  234.         size_t startIndex = static_cast<size_t>(index);
  235.         AIAssert(startIndex < m_points.size());
  236.         if (startIndex >= m_points.size())
  237.                 startIndex = m_points.size() - 1;
  238.         size_t endIndex = startIndex + 1;
  239.  
  240.         Vec3 startPos(m_points[startIndex].pos);
  241.         Vec3 endPos((endIndex < m_points.size()) ? m_points[endIndex].pos : startPos);
  242.  
  243.         return Lerp(startPos, endPos, delta);
  244. }
  245.  
  246. float InterpolatedPath::GetDistanceAtSegmentIndex(float index) const
  247. {
  248.         AIAssert(!m_points.empty());
  249.  
  250.         // Bound index to minimum
  251.         index = max(index, 0.0f);
  252.  
  253.         float delta = fmodf(index, 1.0f);
  254.  
  255.         size_t startIndex = static_cast<size_t>(index);
  256.         AIAssert(startIndex < m_points.size());
  257.         if (startIndex >= m_points.size())
  258.                 startIndex = m_points.size() - 1;
  259.         size_t endIndex = startIndex + 1;
  260.  
  261.         float startDist = m_points[startIndex].distance;
  262.         float endDist = (endIndex < m_points.size()) ? m_points[endIndex].distance : startDist;
  263.  
  264.         return Lerp(startDist, endDist, delta);
  265. }
  266.  
  267. void InterpolatedPath::GetLineSegment(float startIndex, float endIndex, Lineseg& segment) const
  268. {
  269.         segment.start = GetPositionAtSegmentIndex(startIndex);
  270.         segment.end = GetPositionAtSegmentIndex(endIndex);
  271. }
  272.  
  273. // Returns the index of the first navigation type transition after index or end of path
  274. size_t InterpolatedPath::FindNextNavTypeSectionIndexAfter(size_t index) const
  275. {
  276.         // Default is to return the last valid index (or ~0 if empty)
  277.         size_t changeIndex = m_points.size() - 1;
  278.  
  279.         // If index safely on path
  280.         if (index < changeIndex)
  281.         {
  282.                 TPoints::const_iterator iter(m_points.begin() + index);
  283.                 const IAISystem::ENavigationType prevNavType = iter->navType;
  284.  
  285.                 while (++iter != m_points.end())
  286.                 {
  287.                         // If the nav type changes
  288.                         if (iter->navType != prevNavType)
  289.                         {
  290.                                 changeIndex = std::distance<TPoints::const_iterator>(m_points.begin(), iter);
  291.                                 break;
  292.                         }
  293.                 }
  294.         }
  295.  
  296.         return changeIndex;
  297. }
  298.  
  299. // Returns the next index on the path that deviates from a straight line by the deviation specified.
  300. float InterpolatedPath::FindNextInflectionIndex(float startIndex, float maxDeviation /*= 0.1f*/, const bool bUse2D /*= true*/) const
  301. {
  302.         // Start at start pos and generate ray based on next segment start point
  303.         Lineseg testLine;
  304.         testLine.start = GetPositionAtSegmentIndex(startIndex);
  305.         testLine.end = testLine.start;
  306.  
  307.         const float maxDeviationSq = square(maxDeviation);
  308.  
  309.         float searchStartIndex = ceilf(startIndex);
  310.         float searchStartDist = GetDistanceAtSegmentIndex(searchStartIndex);
  311.         float searchEndDist = min(searchStartDist + 3.0f, m_totalDistance);
  312.  
  313.         const float distIncrement = 0.1f;
  314.         float lastSafeIndex = searchStartIndex;
  315.         size_t maxSteps = 64;
  316.  
  317.         for (float dist = searchStartDist; maxSteps && (dist < searchEndDist); dist += distIncrement, --maxSteps)
  318.         {
  319.                 // Update line
  320.                 const float index = FindSegmentIndexAtDistance(dist, static_cast<size_t>(startIndex));
  321.                 testLine.end = GetPositionAtSegmentIndex(index);
  322.  
  323.                 const size_t subIndexEnd = static_cast<size_t>(ceilf(index));
  324.  
  325.                 // Test deviation against test range of the path
  326.                 for (size_t subIndex = static_cast<size_t>(searchStartIndex) + 1; subIndex < subIndexEnd; ++subIndex)
  327.                 {
  328.                         float delta;
  329.                         float deviationSq =
  330.                           bUse2D
  331.                           ? Distance::Point_Lineseg2DSq(m_points[subIndex].pos, testLine, delta)
  332.                           : Distance::Point_LinesegSq(m_points[subIndex].pos, testLine, delta);
  333.                         if (deviationSq > maxDeviationSq)
  334.                         {
  335.                                 // This point is no longer safe, use the last safe index
  336.                                 return lastSafeIndex;
  337.                         }
  338.                 }
  339.  
  340.                 lastSafeIndex = index;
  341.         }
  342.  
  343.         return lastSafeIndex;
  344. }
  345.  
  346. // Shortens the path to the specified endIndex
  347. void InterpolatedPath::ShortenToIndex(float endIndex)
  348. {
  349.         // If cut is actually on the path
  350.         if (endIndex >= 0.0f && endIndex < m_points.size())
  351.         {
  352.                 // Create the cut index based on the next integer index after the endIndex
  353.                 size_t cutIndex = static_cast<size_t>(endIndex) + 1;
  354.  
  355.                 float delta = fmodf(endIndex, 1.0f);
  356.  
  357.                 // Create a new point based on the original
  358.                 SPathControlPoint2 newEndPoint(m_points[static_cast<size_t>(endIndex)]);
  359.                 newEndPoint.pos = GetPositionAtSegmentIndex(endIndex);
  360.                 newEndPoint.distance = GetDistanceAtSegmentIndex(endIndex);
  361.                 // NOTE: New end point copies other values from the point immediately before it. Safe assumption?
  362.  
  363.                 // Erase removed section of the path
  364.                 m_points.erase(m_points.begin() + cutIndex, m_points.end());
  365.  
  366.                 m_points.push_back(newEndPoint);
  367.         }
  368. }
  369.  
  370. void InterpolatedPath::push_back(const SPathControlPoint2& point)
  371. {
  372.         float distance = 0.0f;
  373.  
  374.         if (!m_points.empty())
  375.                 distance = m_points.back().pos.GetDistance(point.pos);
  376.  
  377.         m_points.push_back(point);
  378.  
  379.         float cumulativeDistance = m_totalDistance + distance;
  380.         m_points.back().distance = cumulativeDistance;
  381.  
  382.         m_totalDistance = cumulativeDistance;
  383. }
  384.  
  385. //===================================================================
  386. // CSmartPathFollower
  387. //===================================================================
  388. CSmartPathFollower::CSmartPathFollower(const PathFollowerParams& params, const IPathObstacles& pathObstacles)
  389.         : m_params(params),
  390.         m_pathVersion(-2),
  391.         m_followTargetIndex(),
  392.         m_inflectionIndex(),
  393.         m_pNavPath(),
  394.         m_pathObstacles(pathObstacles),
  395.         m_curPos(ZERO),
  396.         m_followNavType(IAISystem::NAV_UNSET),
  397.         m_lookAheadEnclosingAABB(AABB::RESET),
  398.         m_allowCuttingCorners(true)
  399.         //  m_safePathOptimal(false),
  400.         //  m_reachTestCount()
  401. {
  402.         Reset();
  403. }
  404.  
  405. //===================================================================
  406. // CSmartPathFollower
  407. //===================================================================
  408. CSmartPathFollower::~CSmartPathFollower()
  409. {
  410. }
  411. /*
  412.    // Attempts to find a reachable target between startIndex (exclusive) & endIndex (inclusive).
  413.    // Successful forward searches return the last reachable target found advancing up the path.
  414.    // Successful reverse searches return the first reachable target found reversing back down the path.
  415.    bool CSmartPathFollower::FindReachableTarget(float startIndex, float endIndex, float& reachableIndex) const
  416.    {
  417.    FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  418.  
  419.    // Default fail value
  420.    reachableIndex = -1.0f;
  421.  
  422.    float startDistance = m_path.GetDistanceAtSegmentIndex(startIndex);
  423.    float endDistance = m_path.GetDistanceAtSegmentIndex(endIndex);
  424.  
  425.    // This can be negative if searching backwards along path
  426.    float lengthToTest = endDistance - startDistance;
  427.    float lengthToTestAbs = fabsf(lengthToTest);
  428.  
  429.    // Use this as the starting offset distance
  430.    const float minTestDist = 0.4f;      // TODO: Parameterize
  431.  
  432.    // Direction of search determines the termination logic
  433.    const bool lookingAhead = (lengthToTest >= 0.0f);
  434.  
  435.    // This is used to ease calculation of path distance without worrying about direction of search
  436.    const float dirMultiplier = (lookingAhead) ? 1.0f : -1.0f;
  437.  
  438.    float advanceDist = minTestDist;
  439.    // Move along the path starting at minTestDist
  440.    for (float testOffsetDist = minTestDist; testOffsetDist < lengthToTestAbs; testOffsetDist += advanceDist)
  441.    {
  442.    float testDist = startDistance + (testOffsetDist * dirMultiplier);
  443.    float testIndex = m_path.FindSegmentIndexAtDistance(testDist);                       // TODO: Optimize, needs range look-up support
  444.  
  445.    if (CanReachTarget(m_curPos, testIndex))
  446.    {
  447.     reachableIndex = testIndex;
  448.  
  449.     // If looking behind, return the first reachable index found
  450.     if (!lookingAhead)
  451.     {
  452.     //m_safePathOptimal = true;
  453.     return true;
  454.     }
  455.    }
  456.    else // Can't reach target at this index
  457.    {
  458.     // If looking ahead & there is a previous valid index
  459.     if (lookingAhead && reachableIndex >= 0.0f)
  460.     {
  461.     // Indicate the safe path is now optimal (continue using it until)
  462.     //m_safePathOptimal = true;
  463.     return true;
  464.     }
  465.     // Otherwise, keep looking (expensive but copes with weird situations better)
  466.    }
  467.  
  468.    // Increase the advance distance
  469.    advanceDist *= 1.5f;
  470.    }
  471.  
  472.    // Finally, always test the endIndex (to support start/end of path)
  473.    if (CanReachTarget(m_curPos, endIndex))
  474.    {
  475.    reachableIndex = endIndex;
  476.    }
  477.  
  478.    return (reachableIndex >= 0.0f);
  479.    }*/
  480.  
  481. // Attempts to find a reachable target between startIndex (exclusive) & endIndex (inclusive).
  482. // Successful forward searches return the last reachable target found advancing up the path.
  483. // Successful reverse searches return the first reachable target found reversing back down the path.
  484. bool CSmartPathFollower::FindReachableTarget(float startIndex, float endIndex, float& reachableIndex) const
  485. {
  486.         FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  487.  
  488.         reachableIndex = -1.0f;
  489.  
  490.         if (startIndex < endIndex)
  491.         {
  492.                 float nextIndex = 0.0f;
  493.                 float dist = endIndex - startIndex;
  494.  
  495.                 // early out for reachable ends
  496.                 if (CanReachTarget(endIndex))
  497.                 {
  498.                         reachableIndex = endIndex;
  499.  
  500.                         return true;
  501.                 }
  502.  
  503.                 // early out if we can even reach anything
  504.                 //if (!CanReachTarget(startIndex + 0.35f))
  505.                 //{
  506.                 //return false;
  507.                 //}
  508.  
  509.                 // now search with a decreasing step size
  510.                 float stepSize = 4.0f;
  511.  
  512.                 do
  513.                 {
  514.                         nextIndex = reachableIndex >= 0.0f ? reachableIndex : floor_tpl(startIndex + stepSize);
  515.                         if (dist >= stepSize)
  516.                                 CanReachTargetStep(stepSize, endIndex, nextIndex, reachableIndex);
  517.  
  518.                         if (reachableIndex >= endIndex)
  519.                                 return true;
  520.  
  521.                 }
  522.                 while ((stepSize *= 0.5f) >= 0.5f);
  523.  
  524.                 if (reachableIndex >= 0.0f)
  525.                 {
  526.                         if (gAIEnv.CVars.DrawPathFollower > 1)
  527.                                 GetAISystem()->AddDebugSphere(m_path.GetPositionAtSegmentIndex(nextIndex), 0.25f, 255, 0, 0, 5.0f);
  528.                 }
  529.  
  530.                 if (reachableIndex >= endIndex)
  531.                         return true;
  532.         }
  533.         else // Search backwards along the path incrementally
  534.         {
  535.                 assert(startIndex >= endIndex);
  536.  
  537.                 const float StartingAdvanceDistance = 0.5f;
  538.  
  539.                 float advance = StartingAdvanceDistance;
  540.                 float start = m_path.GetDistanceAtSegmentIndex(startIndex);
  541.                 float end = m_path.GetDistanceAtSegmentIndex(endIndex);
  542.  
  543.                 do
  544.                 {
  545.                         start -= advance;
  546.                         if (start < end)
  547.                                 start = end;
  548.  
  549.                         float test = m_path.FindSegmentIndexAtDistance(start);
  550.  
  551.                         if (CanReachTarget(test))
  552.                         {
  553.                                 reachableIndex = test;
  554.                                 break;
  555.                         }
  556.  
  557.                         advance = min(1.5f, advance * 1.5f);
  558.                 }
  559.                 while (start > end);
  560.  
  561.         }
  562.  
  563.         return reachableIndex >= 0.0f;
  564. }
  565.  
  566. bool CSmartPathFollower::CanReachTargetStep(float step, float endIndex, float nextIndex, float& reachableIndex) const
  567. {
  568.         if (nextIndex > endIndex)
  569.                 nextIndex = endIndex;
  570.  
  571.         while (CanReachTarget(nextIndex))
  572.         {
  573.                 if (gAIEnv.CVars.DrawPathFollower > 1)
  574.                         GetAISystem()->AddDebugSphere(m_path.GetPositionAtSegmentIndex(nextIndex), 0.25f, 0, 255, 0, 5.0f);
  575.  
  576.                 reachableIndex = nextIndex;
  577.  
  578.                 if (nextIndex >= endIndex)
  579.                         break;
  580.  
  581.                 nextIndex = min(nextIndex + step, endIndex);
  582.         }
  583.  
  584.         return reachableIndex >= 0.0f;
  585. }
  586.  
  587. // True if the test position can be reached by the agent.
  588. bool CSmartPathFollower::CanReachTarget(float testIndex) const
  589. {
  590.         FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  591.  
  592.         const Vec3 startPos(m_curPos);
  593.  
  594.         // Assume safe until proved otherwise
  595.         bool safeLineToTarget = false;
  596.  
  597.         //////////////////////////////////////////////////////////////////////////
  598.         /// Navigation Mesh Handling
  599.  
  600.         assert(testIndex >= 0.0f);
  601.         if (testIndex < 0.0f)
  602.                 return false;
  603.  
  604.         const SPathControlPoint2& segStartPoint = m_path[static_cast<size_t>(testIndex)];
  605.         Vec3 testPos(m_path.GetPositionAtSegmentIndex(testIndex));
  606.  
  607.         {
  608.                 if (NavigationMeshID meshID = m_pNavPath->GetMeshID())
  609.                 {
  610.                         const Vec3 raiseUp(0.0f, 0.0f, 0.2f);
  611.                         const Vec3 raisedStartPos = startPos + raiseUp;
  612.                         const Vec3 raisedTestPos = testPos + raiseUp;
  613.  
  614.                         const NavigationMesh& mesh = gAIEnv.pNavigationSystem->GetMesh(meshID);
  615.                         const MNM::CNavMesh& navMesh = mesh.navMesh;
  616.  
  617.                         const MNM::CNavMesh::SGridParams& gridParams = navMesh.GetGridParams();
  618.  
  619.                         const MNM::real_t horizontalRange(5.0f);
  620.                         const MNM::real_t verticalRange(2.0f);
  621.  
  622.                         MNM::vector3_t startLocationInMeshCoordinates(raisedStartPos - gridParams.origin);
  623.                         MNM::vector3_t endLocationInMeshCoordinates(raisedTestPos - gridParams.origin);
  624.  
  625.                         MNM::TriangleID triangleStartID = navMesh.GetTriangleAt(startLocationInMeshCoordinates, verticalRange, verticalRange);
  626.                         if (!triangleStartID)
  627.                         {
  628.                                 MNM::vector3_t closestStartLocation, triangleCenter;
  629.                                 triangleStartID = navMesh.GetClosestTriangle(startLocationInMeshCoordinates, verticalRange, horizontalRange, nullptr, &closestStartLocation);
  630.                                 navMesh.PushPointInsideTriangle(triangleStartID, closestStartLocation, MNM::real_t(.05f));
  631.                                 startLocationInMeshCoordinates = closestStartLocation;
  632.                         }
  633.  
  634.                         MNM::TriangleID triangleEndID = navMesh.GetTriangleAt(endLocationInMeshCoordinates, verticalRange, verticalRange);
  635.                         if (!triangleEndID)
  636.                         {
  637.                                 // Couldn't find a triangle for the end position. Pick the closest one.
  638.                                 MNM::vector3_t closestEndLocation;
  639.                                 triangleEndID = navMesh.GetClosestTriangle(endLocationInMeshCoordinates, verticalRange, horizontalRange, nullptr, &closestEndLocation);
  640.                                 navMesh.PushPointInsideTriangle(triangleEndID, closestEndLocation, MNM::real_t(.05f));
  641.                                 endLocationInMeshCoordinates = closestEndLocation;
  642.                         }
  643.  
  644.                         if (!triangleStartID || !triangleEndID)
  645.                                 return false;
  646.  
  647.                         MNM::CNavMesh::RayCastRequest<512> wayRequest;
  648.  
  649.                         if (navMesh.RayCast(startLocationInMeshCoordinates, triangleStartID, endLocationInMeshCoordinates, triangleEndID, wayRequest))
  650.                                 return false;
  651.  
  652.                         //Check against obstacles...
  653.                         if (m_pathObstacles.IsPathIntersectingObstacles(m_pNavPath->GetMeshID(), raisedStartPos, raisedTestPos, m_params.passRadius))
  654.                                 return false;
  655.  
  656.                         return true;
  657.                 }
  658.         }
  659.  
  660.         return false;
  661. }
  662.  
  663. //===================================================================
  664. // DistancePointPoint
  665. //===================================================================
  666. float CSmartPathFollower::DistancePointPoint(const Vec3 pt1, const Vec3 pt2) const
  667. {
  668.         return m_params.use2D ? Distance::Point_Point2D(pt1, pt2) : Distance::Point_Point(pt1, pt2);
  669. }
  670.  
  671. //===================================================================
  672. // DistancePointPointSq
  673. //===================================================================
  674. float CSmartPathFollower::DistancePointPointSq(const Vec3 pt1, const Vec3 pt2) const
  675. {
  676.         return m_params.use2D ? Distance::Point_Point2DSq(pt1, pt2) : Distance::Point_PointSq(pt1, pt2);
  677. }
  678.  
  679. void CSmartPathFollower::Reset()
  680. {
  681.         // NOTE: m_params is left unaltered
  682.         m_pathVersion = -2;
  683.  
  684.         m_validatedStartPos.zero();
  685.         m_followTargetIndex = 0.0f;
  686.         m_followNavType = IAISystem::NAV_UNSET;
  687.         m_inflectionIndex = 0.0f;
  688.  
  689.         stl::free_container(m_path);
  690. }
  691.  
  692. //===================================================================
  693. // AttachToPath
  694. //===================================================================
  695. void CSmartPathFollower::AttachToPath(INavPath* pNavPath)
  696. {
  697.         Reset();
  698.         m_pNavPath = pNavPath;
  699. }
  700.  
  701. //===================================================================
  702. // ProcessPath
  703. //===================================================================
  704. void CSmartPathFollower::ProcessPath()
  705. {
  706.         AIAssert(m_pNavPath);
  707.         m_pathVersion = m_pNavPath->GetVersion();
  708.  
  709.         // Reset cached data
  710.         m_path.clear();
  711.         m_followTargetIndex = 0.0f;
  712.         m_followNavType = IAISystem::NAV_UNSET;
  713.         m_inflectionIndex = 0.0f;
  714.  
  715.         const CNavPath* pNavPath = static_cast<CNavPath*>(m_pNavPath);
  716.         const TPathPoints& pathPts = pNavPath->GetPath();
  717.  
  718.         if (pathPts.size() < 2)
  719.                 return;
  720.  
  721.         const TPathPoints::const_iterator itEnd = pathPts.end();
  722.  
  723.         SPathControlPoint2 pathPoint;
  724.         pathPoint.distance = 0.0f;
  725.  
  726.         // For each existing path segment
  727.         for (TPathPoints::const_iterator it = pathPts.begin(); it != itEnd; ++it)
  728.         {
  729.                 // Distance used to look ahead and behind (magic number)
  730.                 const PathPointDescriptor& ppd = *it;
  731.  
  732.                 pathPoint.navType = ppd.navType;
  733.                 pathPoint.pos = ppd.vPos;
  734.  
  735.                 m_path.push_back(pathPoint);
  736.         }
  737.  
  738.         // Ensure end point is at ground level (very often it isn't)
  739.         if (m_params.snapEndPointToGround)
  740.         {
  741.                 SPathControlPoint2& endPoint = m_path.back();
  742.  
  743.                 // FIXME: GetFloorPos() is failing in some cases
  744.                 //    Vec3 floorPos;
  745.                 //    if (GetFloorPos(floorPos,                                                 // Found floor position (if successful)
  746.                 //                    endPoint.pos + Vec3(0.0f, 0.0f, 0.5f),                                    // Existing end position
  747.                 //                    0.0f,                                                                     // Up distance (already included)
  748.                 //                    2.0f,                                                                     // End below floor position
  749.                 //                    m_params.passRadius,      // Test radius (avoid falling through cracks)
  750.                 //                    AICE_STATIC))                                     // Only consider static obstacles
  751.                 //    {
  752.                 //      endPoint.pos = floorPos;
  753.                 //    }
  754.                 //    else
  755.                 //    {
  756.                 //      // Can't find the floor!!
  757.                 //      IPersistantDebug* pDebug = gEnv->pGameFramework->GetIPersistantDebug();
  758.                 //      pDebug->AddSphere(endPoint.pos, 0.6f, ColorF(1.0f, 0, 0), 5.0f);
  759.                 //      //AIAssert(false);
  760.                 //    }
  761.  
  762.                 // Snap to previous point height - assuming it is safe
  763.                 if (m_path.size() > 1)
  764.                 {
  765.                         const SPathControlPoint2& prevPoint = m_path[m_path.size() - 2];
  766.                         endPoint.pos.z = min(prevPoint.pos.z + 0.1f, endPoint.pos.z);
  767.                 }
  768.         }
  769.  
  770.         // Now cut the path short if we should stop before the end
  771.         if (m_params.endDistance > 0.0f)
  772.         {
  773.                 float newDistance = m_path.TotalDistance() - m_params.endDistance;
  774.                 float endIndex = m_path.FindSegmentIndexAtDistance(newDistance);
  775.                 m_path.ShortenToIndex(endIndex);
  776.         }
  777.  
  778.         AIAssert(m_path.size() >= 2);
  779. }
  780.  
  781. // Attempts to advance the follow target along the path as far as possible while ensuring the follow
  782. // target remains reachable. Returns true if the follow target is reachable, false otherwise.
  783. bool CSmartPathFollower::Update(PathFollowResult& result, const Vec3& curPos, const Vec3& curVel, float dt)
  784. {
  785.         FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  786.  
  787.         bool targetReachable = true;
  788.         //m_reachTestCount = 0;
  789.  
  790.         CAISystem* pAISystem = GetAISystem();
  791.  
  792.         // If path has changed
  793.         const bool bPathHasChanged = (m_pathVersion != m_pNavPath->GetVersion());
  794.         if (bPathHasChanged)
  795.         {
  796.                 // Duplicate, fix end height and optionally shorten path
  797.                 ProcessPath();
  798.         }
  799.  
  800.         // Set result defaults (ensure no undefined data is passed back to the caller)
  801.         {
  802.                 // This is used to vaguely indicate if the FT has reached path end and so has the agent
  803.                 result.reachedEnd = false;
  804.  
  805.                 // Don't generate predicted states (obsolete)
  806.                 if (result.predictedStates)
  807.                         result.predictedStates->resize(0);
  808.  
  809.                 result.followTargetPos = curPos;
  810.                 result.inflectionPoint = curPos;
  811.  
  812.                 result.velocityOut.zero();
  813.         }
  814.  
  815.         // Cope with empty path (should not occur but best to be safe)
  816.         if (m_path.empty() || m_path.TotalDistance() < FLT_EPSILON)
  817.         {
  818.                 // Indicate we've reached the "end" of path, either because it's empty or has a total distance of 0.
  819.                 result.reachedEnd = true;
  820.                 return true;
  821.         }
  822.  
  823.         m_curPos = curPos;
  824.  
  825.         // Record the current navigation type for the follow target (allows detection of transitions)
  826.         if (m_followNavType == IAISystem::NAV_UNSET)
  827.         {
  828.                 size_t segmentIndex = static_cast<size_t>(m_followTargetIndex);
  829.                 m_followNavType = (segmentIndex < m_path.size()) ? m_path[segmentIndex].navType : IAISystem::NAV_UNSET;
  830.         }
  831.  
  832.         Vec3 followTargetPos = m_path.GetPositionAtSegmentIndex(m_followTargetIndex);
  833.         Vec3 inflectionPoint = m_path.GetPositionAtSegmentIndex(m_inflectionIndex);
  834.  
  835.         // TODO: Optimize case where very little deviation from path (check max distances of intervening points to line)
  836.  
  837.         bool recalculateTarget = false;
  838.         bool onSafeLine = false;
  839.  
  840.         // fraction of the current path segment that needs to be covered before target is recalculated
  841.         const float recalculationFraction = 0.25f;
  842.  
  843.         // If safe path not optimal or progress has been made along the path
  844.         if (/*!m_safePathOptimal ||*/ m_followTargetIndex > 0.0f)
  845.         {
  846.                 // Generate the safe line previously calculated
  847.                 Lineseg safeLine(m_validatedStartPos, followTargetPos);
  848.  
  849.                 float delta;
  850.                 const float distToSafeLineSq =
  851.                   m_params.use2D
  852.                   ? Distance::Point_Lineseg2DSq(curPos, safeLine, delta)
  853.                   : Distance::Point_LinesegSq(curPos, safeLine, delta);
  854.  
  855.                 onSafeLine = distToSafeLineSq < sqr(0.15f);   // TODO: Parameterize & perhaps scale by proximity to target?
  856.  
  857.                 // Are we still effectively on the safe line?
  858.                 if (onSafeLine)
  859.                 {
  860.                         // Have we moved significantly along safe line?
  861.                         if (m_allowCuttingCorners && (delta > recalculationFraction))
  862.                         {
  863.                                 // Is the more path left to advance the FT?
  864.                                 if (m_followTargetIndex < m_path.size() - 1.001f)
  865.                                         recalculateTarget = true;
  866.                         }
  867.                 }
  868.                 else  // Deviated too far from safe line
  869.                 {
  870.                         recalculateTarget = true;
  871.                 }
  872.         }
  873.         else  // No target yet calculated (or attempting to get to start)
  874.         {
  875.                 recalculateTarget = true;
  876.         }
  877.  
  878.         const bool isInsideObstacles = m_pathObstacles.IsPointInsideObstacles(m_curPos);
  879.         bool isAllowedToShortcut;
  880.  
  881.         if (gAIEnv.CVars.SmartPathFollower_useAdvancedPathShortcutting == 0)
  882.         {
  883.                 isAllowedToShortcut = isInsideObstacles ? false : m_params.isAllowedToShortcut;
  884.         }
  885.         else
  886.         {
  887.                 if (isInsideObstacles || !m_params.isAllowedToShortcut)
  888.                 {
  889.                         isAllowedToShortcut = false;
  890.                 }
  891.                 else
  892.                 {
  893.                         isAllowedToShortcut = true; // will change below
  894.  
  895.                         // inspect the path segments from our current position up to the look-ahead position:
  896.                         // if any of them is close to an obstacle, then the path may NOT be shortcut
  897.  
  898.                         float indexAtCurrentPos;
  899.                         float indexAtLookAheadPos;
  900.  
  901.                         if (m_followTargetIndex > 0.0f)
  902.                         {
  903.                                 indexAtCurrentPos = m_followTargetIndex;
  904.                         }
  905.                         else
  906.                         {
  907.                                 // we're at the start of the path
  908.                                 const float startDist = 0.0f;
  909.                                 const float endDist = 5.0f;       // 5m tolerance
  910.                                 const float toleranceZ = FLT_MAX; // using a FLT_MAX z-tolerance in case we're currently traversing a SmartObject that will make us end up at a "much higher" (or "much lower") position than we started at the time of the traveral
  911.                                 indexAtCurrentPos = m_path.FindClosestSegmentIndex(curPos, startDist, endDist, toleranceZ, m_params.use2D);
  912.                         }
  913.  
  914.                         const float currentDistance = m_path.GetDistanceAtSegmentIndex(indexAtCurrentPos);
  915.                         indexAtLookAheadPos = m_path.FindSegmentIndexAtDistance(currentDistance + gAIEnv.CVars.SmartPathFollower_LookAheadDistance);
  916.  
  917.                         for (float index = indexAtCurrentPos; index <= indexAtLookAheadPos; index += 1.0f)
  918.                         {
  919.                                 float indexAtStartOfSegment = floorf(index);
  920.                                 float indexAtEndOfSegment = indexAtStartOfSegment + 1.0f;
  921.  
  922.                                 // make sure the index at the end of the segment doesn't go beyond the end of the path
  923.                                 indexAtEndOfSegment = std::min(indexAtEndOfSegment, static_cast<float>(m_path.size()) - 1.0f);
  924.  
  925.                                 Lineseg lineseg;
  926.                                 m_path.GetLineSegment(indexAtStartOfSegment, indexAtEndOfSegment, lineseg);
  927.  
  928.                                 const float maxDistanceToObstaclesToConsiderTooClose = 0.5f;
  929.                                 if (m_pathObstacles.IsLineSegmentIntersectingObstaclesOrCloseToThem(lineseg, maxDistanceToObstaclesToConsiderTooClose))
  930.                                 {
  931. #ifndef _RELEASE
  932.                                         if (gAIEnv.CVars.SmartPathFollower_useAdvancedPathShortcutting_debug != 0)
  933.                                         {
  934.                                                 gEnv->pGameFramework->GetIPersistantDebug()->Begin("SmartPathFollower_useAdvancedPathShortcutting_debug", false);
  935.                                                 gEnv->pGameFramework->GetIPersistantDebug()->AddLine(lineseg.start + Vec3(0.0, 0.0f, 1.5f), lineseg.end + Vec3(0.0f, 0.0f, 1.5f), ColorF(1.0f, 0.0f, 0.0f), 1.0f);
  936.                                         }
  937. #endif
  938.                                         isAllowedToShortcut = false;
  939.                                         break;
  940.                                 }
  941.  
  942. #ifndef _RELEASE
  943.                                 if (gAIEnv.CVars.SmartPathFollower_useAdvancedPathShortcutting_debug != 0)
  944.                                 {
  945.                                         gEnv->pGameFramework->GetIPersistantDebug()->Begin("SmartPathFollower_useAdvancedPathShortcutting_debug", false);
  946.                                         gEnv->pGameFramework->GetIPersistantDebug()->AddLine(lineseg.start + Vec3(0.0, 0.0f, 1.5f), lineseg.end + Vec3(0.0f, 0.0f, 1.5f), ColorF(0.0f, 1.0f, 0.0f), 1.0f);
  947.                                 }
  948. #endif
  949.                         }
  950.                 }
  951.         }
  952.  
  953.         if (recalculateTarget && isAllowedToShortcut)
  954.         {
  955.                 float currentIndex;
  956.                 float lookAheadIndex;
  957.  
  958.                 const float LookAheadDistance = gAIEnv.CVars.SmartPathFollower_LookAheadDistance;
  959.  
  960.                 // Generate a look-ahead range based on the current FT index.
  961.                 if (m_followTargetIndex > 0.0f)
  962.                 {
  963.                         currentIndex = m_followTargetIndex;
  964.  
  965.                         const float currentDistance = m_path.GetDistanceAtSegmentIndex(m_followTargetIndex);
  966.                         lookAheadIndex = m_path.FindSegmentIndexAtDistance(currentDistance + LookAheadDistance);
  967.                 }
  968.                 else
  969.                 {
  970.                         const float startDist = 0.0f;
  971.                         const float endDist = 5.0f;       // We're at the start of the path, so we should be within 5m
  972.                         const float toleranceZ = FLT_MAX; // using a FLT_MAX z-tolerance in case we're currently traversing a SmartObject that will make us end up at a "much higher" (or "much lower") position than we started at the time of the traveral
  973.  
  974.                         currentIndex = m_path.FindClosestSegmentIndex(curPos, startDist, endDist, toleranceZ, m_params.use2D);
  975.  
  976.                         const float currentDistance = m_path.GetDistanceAtSegmentIndex(currentIndex);
  977.                         lookAheadIndex = m_path.FindSegmentIndexAtDistance(currentDistance + LookAheadDistance);
  978.                 }
  979.  
  980.                 float newTargetIndex = -1.0f;
  981.  
  982.                 // query all entities in this path as well as their bounding boxes from physics
  983.                 m_lookAheadEnclosingAABB.Reset();
  984.                 m_lookAheadEnclosingAABB.Add(m_curPos);
  985.                 m_lookAheadEnclosingAABB.Add(m_path.GetPositionAtSegmentIndex(lookAheadIndex));
  986.  
  987.                 for (float current = currentIndex, end = lookAheadIndex; current <= end; current += 1.0f)
  988.                         m_lookAheadEnclosingAABB.Add(m_path.GetPositionAtSegmentIndex(current));
  989.                 m_lookAheadEnclosingAABB.Expand(Vec3(m_params.passRadius + 0.005f, m_params.passRadius + 0.005f, 0.5f));
  990.  
  991.                 // 1. Search forward to find the next corner from current position.
  992.                 if (!FindReachableTarget(currentIndex, lookAheadIndex, newTargetIndex))
  993.                 {
  994.                         // 2. Try original target.
  995.                         if (onSafeLine || CanReachTarget(m_followTargetIndex))
  996.                         {
  997.                                 newTargetIndex = m_followTargetIndex;
  998.                         }
  999.                         else  // Old target inaccessible
  1000.                         {
  1001.                                 // 3. Find closest point on path before previous FT - use it to bound look-behind search
  1002.                                 // NOTE: Should be safe even if path loops and gives inaccessible closest as search is done backwards.
  1003.                                 float lookBehindIndex = 0.0f;
  1004.  
  1005.                                 // 4. Search backwards from previous FT - start small and increase until beyond closet point on path.
  1006.                                 if (!FindReachableTarget(m_followTargetIndex, lookBehindIndex, newTargetIndex))
  1007.                                 {
  1008.                                         // 5. Admit defeat and inform caller. The caller probably needs to regenerate path.
  1009.                                         targetReachable = false;
  1010.  
  1011. #ifndef _RELEASE
  1012.                                         if (gAIEnv.CVars.DrawPathFollower > 0)
  1013.                                         {
  1014.                                                 CDebugDrawContext dc;
  1015.                                                 dc->Draw3dLabel(m_curPos, 1.6f, "Failed PathFollower!");
  1016.                                         }
  1017. #endif
  1018.                                 }
  1019.                         }
  1020.                 }
  1021.  
  1022.                 // If valid target was found
  1023.                 if (newTargetIndex >= 0.0f)
  1024.                 {
  1025.                         m_validatedStartPos = curPos;
  1026.  
  1027.                         // If the index has actually changed (avoids recalculating inflection point)
  1028.                         if (m_followTargetIndex != newTargetIndex)
  1029.                         {
  1030.                                 // Update the target
  1031.                                 m_followTargetIndex = newTargetIndex;
  1032.                                 followTargetPos = m_path.GetPositionAtSegmentIndex(newTargetIndex);
  1033.  
  1034.                                 // Update inflection point
  1035.                                 m_inflectionIndex = m_path.FindNextInflectionIndex(newTargetIndex, m_params.pathRadius * 0.5f, m_params.use2D);   // TODO: Parameterize max deviation
  1036.                                 inflectionPoint = m_path.GetPositionAtSegmentIndex(m_inflectionIndex);
  1037.  
  1038.                                 // Update the cached follow target navigation-type
  1039.                                 size_t segmentIndex = static_cast<size_t>(m_followTargetIndex);
  1040.                                 m_followNavType = m_path[segmentIndex].navType;
  1041.                         }
  1042.                 }
  1043.         }
  1044.  
  1045.         if (recalculateTarget && !isAllowedToShortcut)
  1046.         {
  1047.                 // If we need to stick to follow a path we don't need to try to cut
  1048.                 // and find the shortest way to reach the final target position.
  1049.                 const float predictionTime = GetPredictionTimeForMovingAlongPath(isInsideObstacles, curVel.GetLengthSquared2D());
  1050.                 const float kIncreasedZToleranceForFindingClosestSegment = FLT_MAX; // We basically don't care about the z tolerance
  1051.  
  1052.                 float currentIndex = m_path.FindClosestSegmentIndex(m_curPos, .0f, m_path.TotalDistance(), kIncreasedZToleranceForFindingClosestSegment, m_params.use2D);
  1053.                 if (currentIndex < 0.0f)
  1054.                 {
  1055.                         assert(currentIndex >= 0.0f);
  1056.                         return false;
  1057.                 }
  1058.  
  1059.                 if (m_followTargetIndex == .0f)
  1060.                 {
  1061.                         m_followTargetIndex = currentIndex;
  1062.                         const float kMinDistanceSqToFirstClosestPointInPath = sqr(0.7f);
  1063.                         const Vec3 pathStartPos = m_path.GetPositionAtSegmentIndex(currentIndex);
  1064.                         const float distanceSqToClosestPointOfPath = DistancePointPointSq(pathStartPos, m_curPos);
  1065.                         // Update follow target
  1066.                         if (distanceSqToClosestPointOfPath < kMinDistanceSqToFirstClosestPointInPath)
  1067.                         {
  1068.                                 m_followTargetIndex = m_path.FindNextSegmentIndex(static_cast<size_t>(m_followTargetIndex));
  1069.                         }
  1070.  
  1071.                         followTargetPos = m_path.GetPositionAtSegmentIndex(m_followTargetIndex);
  1072.  
  1073.                         // Update inflection point/index
  1074.                         m_inflectionIndex = m_path.FindNextSegmentIndex(static_cast<size_t>(m_followTargetIndex));
  1075.                         inflectionPoint = m_path.GetPositionAtSegmentIndex(m_inflectionIndex);
  1076.                 }
  1077.                 else
  1078.                 {
  1079.                         const float currentDistance = m_path.GetDistanceAtSegmentIndex(currentIndex);
  1080.                         const Vec3 localNextPos = curVel * predictionTime;
  1081.                         const float kMinLookAheadDistanceAllowed = 1.0f;
  1082.                         const float lookAheadDistance = max(kMinLookAheadDistanceAllowed, localNextPos.len());
  1083.                         const float lookAheadIndex = m_path.FindSegmentIndexAtDistance(currentDistance + lookAheadDistance);
  1084.                         const float nextFollowIndex = m_path.FindNextSegmentIndex(static_cast<size_t>(lookAheadIndex));
  1085.  
  1086.                         if (m_followTargetIndex < nextFollowIndex)
  1087.                         {
  1088.                                 m_followTargetIndex = nextFollowIndex;
  1089.                                 followTargetPos = m_path.GetPositionAtSegmentIndex(lookAheadIndex);
  1090.  
  1091.                                 m_inflectionIndex = m_path.FindNextSegmentIndex(static_cast<size_t>(nextFollowIndex));
  1092.                                 inflectionPoint = m_path.GetPositionAtSegmentIndex(m_inflectionIndex);
  1093.                         }
  1094.                 }
  1095.                 // Update the starting position
  1096.                 m_validatedStartPos = m_curPos;
  1097.         }
  1098.  
  1099.         // Generate results
  1100.         {
  1101.                 // Pass the absolute data (will eventually replace the old velocity based data below)
  1102.                 result.followTargetPos = followTargetPos;
  1103.                 result.inflectionPoint = inflectionPoint;
  1104.  
  1105.                 // TODO: The following is deprecated. Passing motion requests using velocity only is imprecise,
  1106.                 // unstable with frame time (due to interactions between animation and physics) and requires
  1107.                 // hacks and magic numbers to make it work semi-reliably.
  1108.                 if (bool allowMovement = true)
  1109.                 {
  1110.                         Vec3 velocity = followTargetPos - curPos;
  1111.                         if (m_params.use2D)
  1112.                                 velocity.z = 0.0f;
  1113.                         velocity.NormalizeSafe();
  1114.                         float distToEnd = GetDistToEnd(&curPos);
  1115.  
  1116.                         float speed = m_params.normalSpeed;
  1117.  
  1118.                         // See if target has reached end of path
  1119.                         const float MinDistanceToEnd = m_params.endAccuracy;
  1120.  
  1121.                         if (m_params.stopAtEnd)
  1122.                         {
  1123.                                 if (distToEnd < MinDistanceToEnd)
  1124.                                 {
  1125.                                         result.reachedEnd = true;
  1126.                                         speed = 0.0f;
  1127.                                 }
  1128.                                 else
  1129.                                 {
  1130.                                         float slowDownDist = 1.2f;
  1131.                                         const float decelerationMultiplier = m_params.isVehicle ? gAIEnv.CVars.SmartPathFollower_decelerationVehicle : gAIEnv.CVars.SmartPathFollower_decelerationHuman;
  1132.                                         speed = min(speed, decelerationMultiplier * distToEnd / slowDownDist);
  1133.                                 }
  1134.                         }
  1135.                         else
  1136.                         {
  1137.                                 // This is used to vaguely indicate if the FT has reached path end and so has the agent
  1138.                                 const float MaxTimeStep = 0.5f;
  1139.                                 result.reachedEnd = distToEnd < max(MinDistanceToEnd, speed * min(dt, MaxTimeStep));
  1140.                         }
  1141.  
  1142.                         // TODO: Stability might be improved by detecting and reducing velocities pointing backwards along the path.
  1143.  
  1144.                         // TODO: Curvature speed control
  1145.  
  1146.                         // limit speed & acceleration/deceleration
  1147.                         if (bPathHasChanged)
  1148.                         {
  1149.                                 // take over component of curVel along new movement direction
  1150.                                 // (keep current speed if continuing in the same direction,
  1151.                                 // slow down otherwise; going to zero when moving at right or higher angles)
  1152.                                 Vec3 velDir = curVel;
  1153.                                 Vec3 moveDir = (followTargetPos - curPos);
  1154.                                 if (m_params.use2D)
  1155.                                 {
  1156.                                         velDir.z = 0.0f;
  1157.                                         moveDir.z = 0.0f;
  1158.                                 }
  1159.  
  1160.                                 float curSpeed = velDir.NormalizeSafe();
  1161.                                 moveDir.NormalizeSafe();
  1162.  
  1163.                                 float dot = velDir.Dot(moveDir);
  1164.                                 Limit(dot, 0.0f, 1.0f);
  1165.  
  1166.                                 m_lastOutputSpeed = curSpeed * dot;
  1167.                         }
  1168.                         Limit(m_lastOutputSpeed, m_params.minSpeed, m_params.maxSpeed);
  1169.                         float maxOutputSpeed = min(m_lastOutputSpeed + dt * m_params.maxAccel, m_params.maxSpeed);
  1170.                         float minOutputSpeed = m_params.stopAtEnd ? 0.0f : max(m_lastOutputSpeed - dt * m_params.maxDecel, m_params.minSpeed);
  1171.  
  1172.                         Limit(speed, minOutputSpeed, maxOutputSpeed);
  1173.                         m_lastOutputSpeed = speed;
  1174.  
  1175.                         // Integrate speed
  1176.                         velocity *= speed;
  1177.  
  1178.                         assert(velocity.len() <= (m_params.maxSpeed + 0.001f));
  1179.  
  1180.                         result.velocityOut = velocity;
  1181.                 }
  1182.         }
  1183.  
  1184.         AIAssert(result.velocityOut.IsValid());
  1185.  
  1186. #ifndef _RELEASE
  1187.         if (gAIEnv.CVars.DrawPathFollower > 0)
  1188.         {
  1189.                 // Draw path
  1190.                 Draw();
  1191.  
  1192.                 Vec3 up(0.0f, 0.0f, 0.2f);
  1193.  
  1194.                 // Draw the safe line & follow target
  1195.                 CDebugDrawContext dc;
  1196.                 ColorB debugColor = recalculateTarget ? ColorB(255, 0, 0) : ColorB(0, 255, 0);
  1197.  
  1198.                 if (result.reachedEnd)
  1199.                 {
  1200.                         debugColor.g = result.velocityOut.IsZeroFast() ? 128 : 0;
  1201.                         debugColor.b = 255;
  1202.                 }
  1203.  
  1204.                 dc->DrawSphere(followTargetPos + up, 0.2f, debugColor);
  1205.                 dc->DrawCapsuleOutline(m_validatedStartPos + up, followTargetPos + up, m_params.passRadius, debugColor);
  1206.  
  1207.                 // Draw inflection point
  1208.                 dc->DrawSphere(inflectionPoint + up, 0.2f, ColorB(0, 0, 255));
  1209.                 dc->DrawCapsuleOutline(followTargetPos + up, inflectionPoint + up, m_params.passRadius, ColorB(0, 0, 255));
  1210.  
  1211.                 dc->DrawArrow(m_curPos + up, result.velocityOut, 0.2f, ColorB(230, 200, 180));
  1212.  
  1213.                 //s_passibilityCheckMaxCount = max(s_passibilityCheckMaxCount, m_reachTestCount);
  1214.                 //dc->Draw3dLabel(m_curPos + up, 1.5f, "%d/%d (%d)", m_reachTestCount, s_passibilityCheckMaxCount, s_cheapTestSuccess);
  1215.         }
  1216. #endif      // !defined _RELEASE
  1217.  
  1218.         return targetReachable;
  1219. }
  1220. //===================================================================
  1221. // GetPredictionTimeForMovingAlongPath
  1222. //===================================================================
  1223. float CSmartPathFollower::GetPredictionTimeForMovingAlongPath(const bool isInsideObstacles,
  1224.                                                               const float currentSpeedSq)
  1225. {
  1226.         float predictionTime = gAIEnv.CVars.SmartPathFollower_LookAheadPredictionTimeForMovingAlongPathWalk;
  1227.         if (isInsideObstacles)
  1228.         {
  1229.                 // Heuristic time to look ahead on the path when the agent moves inside the shape
  1230.                 // of dynamic obstacles. We try to don't look ahead too much to stick more to the path
  1231.                 predictionTime = 0.2f;
  1232.         }
  1233.         else
  1234.         {
  1235.                 const float minSpeedToBeConsideredRunningOrSprintingSq = sqr(2.0f);
  1236.                 if (currentSpeedSq > minSpeedToBeConsideredRunningOrSprintingSq)
  1237.                 {
  1238.                         predictionTime = gAIEnv.CVars.SmartPathFollower_LookAheadPredictionTimeForMovingAlongPathRunAndSprint;
  1239.                 }
  1240.         }
  1241.  
  1242.         return predictionTime;
  1243. }
  1244.  
  1245. //===================================================================
  1246. // GetDistToEnd
  1247. //===================================================================
  1248. float CSmartPathFollower::GetDistToEnd(const Vec3* pCurPos) const
  1249. {
  1250.         float distanceToEnd = 0.0f;
  1251.  
  1252.         if (!m_path.empty())
  1253.         {
  1254.                 distanceToEnd = m_path.TotalDistance() - m_path.GetDistanceAtSegmentIndex(m_followTargetIndex);
  1255.                 if (pCurPos)
  1256.                 {
  1257.                         Vec3 followTargetPos(m_path.GetPositionAtSegmentIndex(m_followTargetIndex));
  1258.                         distanceToEnd += DistancePointPoint(*pCurPos, followTargetPos);
  1259.                 }
  1260.         }
  1261.  
  1262.         return distanceToEnd;
  1263. }
  1264.  
  1265. //===================================================================
  1266. // Serialize
  1267. //===================================================================
  1268. void CSmartPathFollower::Serialize(TSerialize ser)
  1269. {
  1270.         ser.Value("m_params", m_params);
  1271.  
  1272.         ser.Value("m_followTargetIndex", m_followTargetIndex);
  1273.         ser.Value("m_inflectionIndex", m_inflectionIndex);
  1274.         ser.Value("m_validatedStartPos", m_validatedStartPos);
  1275.         ser.Value("m_allowCuttingCorners", m_allowCuttingCorners);
  1276.  
  1277.         if (ser.IsReading())
  1278.         {
  1279.                 // NOTE: It's assumed the path will be rebuilt as we don't serialize the path version.
  1280.                 // If we're reading, we need to rebuild the cached path by creating an "invalid" path version.
  1281.                 m_pathVersion = -2;
  1282.         }
  1283. }
  1284.  
  1285. //===================================================================
  1286. // Draw
  1287. //===================================================================
  1288. void CSmartPathFollower::Draw(const Vec3& drawOffset) const
  1289. {
  1290.         CDebugDrawContext dc;
  1291.         size_t pathSize = m_path.size();
  1292.         for (size_t i = 0; i < pathSize; ++i)
  1293.         {
  1294.                 Vec3 prevControlPoint(m_path.GetPositionAtSegmentIndex(static_cast<float>(i) - 1.0f));
  1295.                 Vec3 thisControlPoint(m_path.GetPositionAtSegmentIndex(static_cast<float>(i)));
  1296.  
  1297.                 dc->DrawLine(prevControlPoint, ColorB(0, 0, 0), thisControlPoint, ColorB(0, 0, 0));
  1298.                 dc->DrawSphere(thisControlPoint, 0.05f, ColorB(0, 0, 0));
  1299.                 dc->Draw3dLabel(thisControlPoint, 1.5f, "%" PRISIZE_T, i);
  1300.         }
  1301. }
  1302.  
  1303. //===================================================================
  1304. // GetDistToSmartObject
  1305. //===================================================================
  1306. float CSmartPathFollower::GetDistToSmartObject() const
  1307. {
  1308.         return GetDistToNavType(IAISystem::NAV_SMARTOBJECT);
  1309. }
  1310.  
  1311. float CSmartPathFollower::GetDistToNavType(IAISystem::ENavigationType navType) const
  1312. {
  1313.         // NOTE: This function is used by the Trace Op to detect movement through straight SO's (those that define no actions).
  1314.         // It's used to preclude path regeneration once the SO is within 1m (magic number).
  1315.         // This whole thing is poorly formed & ideally needs to be replaced by FT waiting to ensure accurate positioning.
  1316.  
  1317.         // TODO: Replace this *hack* with wait system.
  1318.  
  1319.         if (!m_path.empty())
  1320.         {
  1321.                 size_t nPts = m_path.size();
  1322.  
  1323.                 // If at end of path
  1324.                 if (m_followTargetIndex + 1 >= nPts)
  1325.                 {
  1326.                         if (navType == m_path.back().navType)
  1327.                                 return Distance::Point_Point(m_path.back().pos, m_curPos);
  1328.  
  1329.                         return std::numeric_limits<float>::max();
  1330.                 }
  1331.  
  1332.                 // FIXME: Stupid and expensive (but the original behavior)
  1333.                 const float closestIndex = m_path.FindClosestSegmentIndex(
  1334.                   m_curPos,
  1335.                   0.0f,
  1336.                   m_path.GetDistanceAtSegmentIndex(m_followTargetIndex),
  1337.                   InterpolatedPath::k_defaultClosestFindSegmentToleranceZ,
  1338.                   m_params.use2D);
  1339.  
  1340.                 if (closestIndex < 0.0f)
  1341.                         return std::numeric_limits<float>::max();
  1342.  
  1343.                 const size_t closestIndexInt = static_cast<size_t>(closestIndex);
  1344.  
  1345.                 // Work out segment index for agent position
  1346.                 float curDist = DistancePointPoint(m_curPos, m_path[closestIndexInt].pos);
  1347.                 float totalDist = 0.0f;
  1348.                 if (closestIndexInt + 1 < nPts)
  1349.                 {
  1350.                         totalDist = DistancePointPoint(m_path[closestIndexInt].pos, m_path[closestIndexInt + 1].pos);
  1351.  
  1352.                         float curFraction = (totalDist > 0.0f) ? (curDist / totalDist) : 0.0f;
  1353.  
  1354.                         // If current segment is of the selected nav type and equal customID.
  1355.                         if ((m_path[closestIndexInt].navType == navType) &&
  1356.                             (m_path[closestIndexInt + 1].navType == navType) &&
  1357.                             (m_path[closestIndexInt].customId == m_path[closestIndexInt + 1].customId))
  1358.                         {
  1359.                                 // If over half-way through segment - we're there! Otherwise not...
  1360.                                 return (curFraction < 0.5f) ? 0.0f : std::numeric_limits<float>::max();
  1361.                         }
  1362.                 }
  1363.                 else if ((closestIndexInt + 1 == nPts) && (m_path[closestIndexInt].navType == navType))
  1364.                 {
  1365.                         return DistancePointPoint(m_curPos, m_path[closestIndexInt].pos);
  1366.                 }
  1367.                 else
  1368.                         return std::numeric_limits<float>::max();
  1369.  
  1370.                 // Go through all segments looking for navType
  1371.                 float dist = 0.0f;
  1372.                 Vec3 lastPos = m_curPos;
  1373.                 for (unsigned int i = closestIndexInt + 1; i < nPts; ++i)
  1374.                 {
  1375.                         dist += DistancePointPoint(lastPos, m_path[i].pos);
  1376.                         if (i + 1 < nPts)
  1377.                         {
  1378.                                 if ((m_path[i].navType == navType) &&
  1379.                                     (m_path[i + 1].navType == navType) &&
  1380.                                     (m_path[i].customId == m_path[i + 1].customId))
  1381.                                         return dist;
  1382.                         }
  1383.                         else
  1384.                         {
  1385.                                 if (m_path[i].navType == navType)
  1386.                                         return dist;
  1387.                         }
  1388.                         lastPos = m_path[i].pos;
  1389.                 }
  1390.         }
  1391.  
  1392.         // Not found
  1393.         return std::numeric_limits<float>::max();
  1394. }
  1395.  
  1396. //===================================================================
  1397. // GetDistToCustomNav
  1398. //===================================================================
  1399. float CSmartPathFollower::GetDistToCustomNav(const InterpolatedPath& controlPoints, uint32 curLASegmentIndex, const Vec3& curLAPos) const
  1400. {
  1401.         size_t nPts = controlPoints.size();
  1402.  
  1403.         float dist = std::numeric_limits<float>::max();
  1404.  
  1405.         if (curLASegmentIndex + 1 < nPts)
  1406.         {
  1407.  
  1408.                 dist = 0.0f;
  1409.  
  1410.                 if (controlPoints[curLASegmentIndex].navType != IAISystem::NAV_CUSTOM_NAVIGATION ||
  1411.                     controlPoints[curLASegmentIndex + 1].navType != IAISystem::NAV_CUSTOM_NAVIGATION)
  1412.                 {
  1413.                         Vec3 lastPos = curLAPos;
  1414.  
  1415.                         for (uint32 i = curLASegmentIndex + 1; i < nPts; ++i)
  1416.                         {
  1417.                                 dist += DistancePointPoint(lastPos, m_path[i].pos);
  1418.                                 if (i + 1 < nPts)
  1419.                                 {
  1420.                                         if (controlPoints[i].navType == IAISystem::NAV_CUSTOM_NAVIGATION &&
  1421.                                             controlPoints[i + 1].navType == IAISystem::NAV_CUSTOM_NAVIGATION)
  1422.                                         {
  1423.                                                 break;
  1424.                                         }
  1425.                                 }
  1426.                                 else
  1427.                                 {
  1428.                                         if (controlPoints[i].navType == IAISystem::NAV_CUSTOM_NAVIGATION)
  1429.                                         {
  1430.                                                 break;
  1431.                                         }
  1432.                                 }
  1433.  
  1434.                                 lastPos = controlPoints[i].pos;
  1435.                         }
  1436.                 }
  1437.         }
  1438.  
  1439.         return dist;
  1440. }
  1441.  
  1442. //===================================================================
  1443. // GetPathPointAhead
  1444. //===================================================================
  1445. Vec3 CSmartPathFollower::GetPathPointAhead(float requestedDist, float& actualDist) const
  1446. {
  1447.         Vec3 followTargetPos(m_path.GetPositionAtSegmentIndex(m_followTargetIndex));
  1448.  
  1449.         float posDist = m_curPos.GetDistance(followTargetPos);
  1450.         if (requestedDist <= posDist)
  1451.         {
  1452.                 actualDist = requestedDist;
  1453.                 return Lerp(m_curPos, followTargetPos, (posDist > 0.0f) ? (requestedDist / posDist) : 1.0f);
  1454.         }
  1455.  
  1456.         // Remove the safe line distance
  1457.         requestedDist -= posDist;
  1458.  
  1459.         float ftDist = m_path.GetDistanceAtSegmentIndex(m_followTargetIndex);
  1460.  
  1461.         // Find the end distance along the path
  1462.         float endDist = ftDist + requestedDist;
  1463.         if (endDist > m_path.TotalDistance())
  1464.                 endDist = m_path.TotalDistance();
  1465.  
  1466.         // Actual distance ahead = (distance along line) + distance from FT to agent position
  1467.         actualDist = (endDist - ftDist) + posDist;
  1468.  
  1469.         float endIndex = m_path.FindSegmentIndexAtDistance(endDist);
  1470.  
  1471.         return m_path.GetPositionAtSegmentIndex(endIndex);
  1472. }
  1473.  
  1474. //===================================================================
  1475. // CheckWalkability
  1476. //===================================================================
  1477. bool CSmartPathFollower::CheckWalkability(const Vec2* path, const size_t length) const
  1478. {
  1479.         // assumes m_curPos is set and valid (which is only the case when following a path)
  1480.         if (!m_path.empty())
  1481.         {
  1482.                 if (NavigationMeshID meshID = m_pNavPath->GetMeshID())
  1483.                 {
  1484.                         const NavigationMesh& mesh = gAIEnv.pNavigationSystem->GetMesh(meshID);
  1485.                         const MNM::CNavMesh& navMesh = mesh.navMesh;
  1486.  
  1487.                         const Vec3 raiseUp(0.0f, 0.0f, 0.2f);
  1488.                         const MNM::real_t verticalRange(2.0f);
  1489.  
  1490.                         Vec3 startLoc = m_curPos + raiseUp;
  1491.  
  1492.                         MNM::vector3_t mnmStartLoc = MNM::vector3_t(MNM::real_t(startLoc.x), MNM::real_t(startLoc.y), MNM::real_t(startLoc.z));
  1493.                         MNM::TriangleID triStart = navMesh.GetTriangleAt(mnmStartLoc, verticalRange, verticalRange);
  1494.                         IF_UNLIKELY (!triStart)
  1495.                                 return false;
  1496.  
  1497.                         float currentZ = startLoc.z;
  1498.                         for (size_t i = 0; i < length; ++i)
  1499.                         {
  1500.                                 const Vec2& endLoc2D = path[i];
  1501.                                 const Vec3 endLoc(endLoc2D.x, endLoc2D.y, currentZ);
  1502.  
  1503.                                 const MNM::vector3_t mnmEndLoc = MNM::vector3_t(MNM::real_t(endLoc.x), MNM::real_t(endLoc.y), MNM::real_t(endLoc.z));
  1504.  
  1505.                                 const MNM::TriangleID triEnd = navMesh.GetTriangleAt(mnmEndLoc, verticalRange, verticalRange);
  1506.  
  1507.                                 if (!triEnd)
  1508.                                         return false;
  1509.  
  1510.                                 MNM::CNavMesh::RayCastRequest<512> raycastRequest;
  1511.  
  1512.                                 if (navMesh.RayCast(mnmStartLoc, triStart, mnmEndLoc, triEnd, raycastRequest) != MNM::CNavMesh::eRayCastResult_NoHit)
  1513.                                         return false;
  1514.  
  1515.                                 if (m_pathObstacles.IsPathIntersectingObstacles(m_pNavPath->GetMeshID(), startLoc, endLoc, m_params.passRadius))
  1516.                                         return false;
  1517.  
  1518.                                 MNM::vector3_t v0, v1, v2;
  1519.                                 const bool success = mesh.navMesh.GetVertices(triEnd, v0, v1, v2);
  1520.                                 CRY_ASSERT(success);
  1521.                                 const MNM::vector3_t closest = MNM::ClosestPtPointTriangle(mnmEndLoc, v0, v1, v2);
  1522.                                 currentZ = closest.GetVec3().z;
  1523.  
  1524.                                 startLoc = endLoc;
  1525.                                 mnmStartLoc = mnmEndLoc;
  1526.                                 triStart = triEnd;
  1527.                         }
  1528.                 }
  1529.  
  1530.                 return true;
  1531.         }
  1532.  
  1533.         return false;
  1534. }
  1535.  
  1536. //===================================================================
  1537. // GetAllowCuttingCorners
  1538. //===================================================================
  1539. bool CSmartPathFollower::GetAllowCuttingCorners() const
  1540. {
  1541.         return m_allowCuttingCorners;
  1542. }
  1543.  
  1544. //===================================================================
  1545. // SetAllowCuttingCorners
  1546. //===================================================================
  1547. void CSmartPathFollower::SetAllowCuttingCorners(const bool allowCuttingCorners)
  1548. {
  1549.         m_allowCuttingCorners = allowCuttingCorners;
  1550. }
  1551.  
  1552. //===================================================================
  1553. // IsRemainingPathOverlappingWithNavMeshTileBounds
  1554. //===================================================================
  1555. bool CSmartPathFollower::IsRemainingPathOverlappingWithNavMeshTileBounds(const NavigationMeshID affectedMeshID, const MNM::TileID affectedTileID) const
  1556. {
  1557.         AIAssert(!m_path.empty());
  1558.         AIAssert(m_pNavPath->GetMeshID() == affectedMeshID);
  1559.  
  1560.         AABB remainingPathAABB;
  1561.  
  1562.         // add our current position to the path's AABB (our position is very likely somewhere between 2 path points)
  1563.         const float indexOfCurrentPos = m_path.FindClosestSegmentIndex(m_curPos, 0.0f, FLT_MAX, FLT_MAX, m_params.use2D);
  1564.         remainingPathAABB.min = remainingPathAABB.max = m_path.GetPositionAtSegmentIndex(indexOfCurrentPos);
  1565.  
  1566.         // add all succeeding points to the path's AABB
  1567.         for (size_t i = static_cast<size_t>(indexOfCurrentPos) + 1, n = m_path.size(); i < n; ++i)
  1568.         {
  1569.                 remainingPathAABB.Add(m_path[i].pos);
  1570.         }
  1571.  
  1572.         // extend the path's AABB a bit as the path is usually very close to the ground and might therefore slightly slip under the tile's AABB
  1573.         remainingPathAABB.Expand(Vec3(0.0f, 0.0f, 0.5f));
  1574.  
  1575.         AABB tileAABB;
  1576.         gAIEnv.pNavigationSystem->GetTileBoundsForMesh(affectedMeshID, affectedTileID, tileAABB);
  1577.  
  1578.         return remainingPathAABB.IsIntersectBox(tileAABB);
  1579. }
  1580.  
  1581. //===================================================================
  1582. // IsRemainingPathTraversableOnNavMesh
  1583. //===================================================================
  1584. bool CSmartPathFollower::IsRemainingPathTraversableOnNavMesh() const
  1585. {
  1586.         AIAssert(!m_path.empty());
  1587.  
  1588.         if (const NavigationMeshID meshIDUsedByPath = m_pNavPath->GetMeshID())
  1589.         {
  1590.                 const MNM::CNavMesh& navMeshUsedByPath = gAIEnv.pNavigationSystem->GetMesh(meshIDUsedByPath).navMesh;
  1591.  
  1592.                 float index1 = m_path.FindClosestSegmentIndex(m_curPos, 0.0f, FLT_MAX, FLT_MAX, m_params.use2D);
  1593.                 float index2 = m_path.FindNextSegmentIndex(static_cast<size_t>(index1));
  1594.  
  1595.                 // - perform raycasts along all segments of the remaining path
  1596.                 // - as soon as a raycast fails we know that the segment can no longer be used to move along
  1597.                 for (size_t i = static_cast<size_t>(index1) + 1; i < m_path.size(); i++)
  1598.                 {
  1599.                         const Vec3 segmentPos1 = m_path.GetPositionAtSegmentIndex(index1);
  1600.                         const Vec3 segmentPos2 = m_path.GetPositionAtSegmentIndex(index2);
  1601.  
  1602.                         const MNM::real_t verticalRange(2.0f);
  1603.  
  1604.                         const MNM::vector3_t mnmStartLoc = MNM::vector3_t(segmentPos1);
  1605.                         const MNM::vector3_t mnmEndLoc = MNM::vector3_t(segmentPos2);
  1606.                         const MNM::TriangleID triStart = navMeshUsedByPath.GetTriangleAt(mnmStartLoc, verticalRange, verticalRange);
  1607.                         const MNM::TriangleID triEnd = navMeshUsedByPath.GetTriangleAt(mnmEndLoc, verticalRange, verticalRange);
  1608.  
  1609.                         if (!triStart || !triEnd)
  1610.                         {
  1611.                                 return false;
  1612.                         }
  1613.  
  1614.                         if (triStart)
  1615.                         {
  1616.                                 MNM::CNavMesh::RayCastRequest<512> raycastRequest;
  1617.                                 MNM::CNavMesh::ERayCastResult raycastResult = navMeshUsedByPath.RayCast(mnmStartLoc, triStart, mnmEndLoc, triEnd, raycastRequest);
  1618.  
  1619.                                 if (raycastResult != MNM::CNavMesh::eRayCastResult_NoHit)
  1620.                                 {
  1621.                                         return false;
  1622.                                 }
  1623.                         }
  1624.  
  1625.                         index1 = index2;
  1626.                         index2 = m_path.FindNextSegmentIndex(i);
  1627.                 }
  1628.         }
  1629.         return true;
  1630. }
  1631.  
  1632. //===================================================================
  1633. // IsRemainingPathAffectedByNavMeshChange
  1634. //===================================================================
  1635. bool CSmartPathFollower::IsRemainingPathAffectedByNavMeshChange(const NavigationMeshID affectedMeshID, const MNM::TileID affectedTileID) const
  1636. {
  1637.         if (const NavigationMeshID meshIDUsedByPath = m_pNavPath->GetMeshID())
  1638.         {
  1639.                 if (affectedMeshID == meshIDUsedByPath)
  1640.                 {
  1641.                         if (!m_path.empty())
  1642.                         {
  1643.                                 if (IsRemainingPathOverlappingWithNavMeshTileBounds(affectedMeshID, affectedTileID))
  1644.                                 {
  1645.                                         if (!IsRemainingPathTraversableOnNavMesh())
  1646.                                         {
  1647.                                                 return true;
  1648.                                         }
  1649.                                 }
  1650.                         }
  1651.                 }
  1652.         }
  1653.         return false;
  1654. }
  1655.  
downloadSmartPathFollower.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