BVB Source Codes

CRYENGINE Show PathFollower.cpp Source code

Return Download CRYENGINE: download PathFollower.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:   PathFollower.cpp
  7.    Version:     v1.00
  8.    Description:
  9.  
  10.    -------------------------------------------------------------------------
  11.    History:
  12.    - ?
  13.    - 4 May 2009  : Evgeny Adamenkov: Replaced IRenderer with CDebugDrawContext
  14.  
  15.  *********************************************************************/
  16.  
  17. #include "StdAfx.h"
  18.  
  19. #include "PathFollower.h"
  20. #include "CAISystem.h"
  21. #include "AILog.h"
  22. #include "NavPath.h"
  23. #include "DebugDrawContext.h"
  24.  
  25. //===================================================================
  26. // Serialize
  27. //===================================================================
  28. void PathFollowerParams::Serialize(TSerialize ser)
  29. {
  30.         ser.Value("normalSpeed", normalSpeed);
  31.         ser.Value("pathRadius", pathRadius);
  32.         ser.Value("pathLookAheadDist", pathLookAheadDist);
  33.         ser.Value("maxAccel", maxAccel);
  34.         ser.Value("maxDecel", maxDecel);
  35.         ser.Value("minSpeed", minSpeed);
  36.         ser.Value("maxSpeed", maxSpeed);
  37.         ser.Value("endAccuracy", endAccuracy);
  38.         ser.Value("endDistance", endDistance);
  39.         ser.Value("stopAtEnd", stopAtEnd);
  40.         ser.Value("use2D", use2D);
  41.         ser.Value("snapEndPointToGround", snapEndPointToGround);
  42.         ser.Value("isVehicle", isVehicle);
  43. }
  44.  
  45. //===================================================================
  46. // CPathFollower
  47. //===================================================================
  48. CPathFollower::CPathFollower(const PathFollowerParams& params)
  49.         : m_params(params),
  50.         m_pathVersion(-2),
  51.         m_curLASegmentIndex(0),
  52.         m_curLAPos(ZERO),
  53.         m_lastOutputSpeed(0.0f),
  54.         m_navPath(0),
  55.         m_CurPos(ZERO),
  56.         m_CurIndex(0)
  57. {
  58. }
  59.  
  60. //===================================================================
  61. // CPathFollower
  62. //===================================================================
  63. CPathFollower::~CPathFollower()
  64. {
  65. }
  66.  
  67. //===================================================================
  68. // DistancePointPoint
  69. //===================================================================
  70. float CPathFollower::DistancePointPoint(const Vec3 pt1, const Vec3 pt2) const
  71. {
  72.         FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  73.  
  74.         return m_params.use2D ? Distance::Point_Point2D(pt1, pt2) : Distance::Point_Point(pt1, pt2);
  75. }
  76.  
  77. //===================================================================
  78. // DistancePointPointSq
  79. //===================================================================
  80. float CPathFollower::DistancePointPointSq(const Vec3 pt1, const Vec3 pt2) const
  81. {
  82.         return m_params.use2D ? Distance::Point_Point2DSq(pt1, pt2) : Distance::Point_PointSq(pt1, pt2);
  83. }
  84.  
  85. void CPathFollower::Reset()
  86. {
  87.         m_pathVersion = -2;
  88.         m_curLASegmentIndex = 0;
  89.         m_curLAPos = ZERO;
  90.         m_lastOutputSpeed = 0.0f;
  91.  
  92.         m_pathControlPoints.resize(0);
  93. }
  94.  
  95. //===================================================================
  96. // AttachToPath
  97. //===================================================================
  98. void CPathFollower::AttachToPath(INavPath* pNavPath)
  99. {
  100.         m_navPath = pNavPath;
  101. }
  102.  
  103. //===================================================================
  104. // GetPathPtAhead
  105. //===================================================================
  106. static Vec3 GetPathPointAhead(const TPathPoints& pathPts, TPathPoints::const_iterator it, float dist)
  107. {
  108.         FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  109.  
  110.         AIAssert(!pathPts.empty());
  111.         if (dist >= 0.0f)
  112.         {
  113.                 float curDist = 0.0f;
  114.                 Vec3 oldPos = it->vPos;
  115.                 while (++it != pathPts.end())
  116.                 {
  117.                         Vec3 pos = it->vPos;
  118.                         float segDist = Distance::Point_Point(oldPos, pos);
  119.  
  120.                         float newDist = segDist + curDist;
  121.                         if (newDist > dist && segDist > 0.0f)
  122.                         {
  123.                                 float frac = (dist - curDist) / segDist;
  124.                                 Vec3 pt = frac * pos + (1.0f - frac) * oldPos;
  125.                                 return pt;
  126.                         }
  127.  
  128.                         curDist = newDist;
  129.                         oldPos = pos;
  130.                 }
  131.                 return pathPts.back().vPos;
  132.         }
  133.         else
  134.         {
  135.                 TPathPoints pathPtsRev(pathPts);
  136.                 std::reverse(pathPtsRev.begin(), pathPtsRev.end());
  137.                 int n = (int)std::distance(pathPts.begin(), it);
  138.                 TPathPoints::iterator itRev = pathPtsRev.begin();
  139.                 std::advance(itRev, (int)(pathPtsRev.size() - (n + 1)));
  140.                 return ::GetPathPointAhead(pathPtsRev, itRev, -dist);
  141.         }
  142. }
  143.  
  144. //===================================================================
  145. // ProcessPath
  146. //===================================================================
  147. void CPathFollower::ProcessPath()
  148. {
  149.         FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  150.  
  151.         IAISystem::tNavCapMask swingOutTypes = IAISystem::NAV_TRIANGULAR | IAISystem::NAV_WAYPOINT_HUMAN | IAISystem::NAV_VOLUME;
  152.  
  153.         AIAssert(m_navPath);
  154.         m_pathVersion = m_navPath->GetVersion();
  155.  
  156.         m_pathControlPoints.clear();
  157.  
  158.         // TODO(marcio): fix
  159.         CNavPath* pNavPath = static_cast<CNavPath*>(m_navPath);
  160.         const TPathPoints& pathPts = pNavPath->GetPath();
  161.  
  162.         if (pathPts.size() < 2)
  163.                 return;
  164.  
  165.         const TPathPoints::const_iterator itEnd = pathPts.end();
  166.  
  167.         float totalLength = 0.0f;
  168.         Vec3 prevPos = pathPts.front().vPos;
  169.         for (TPathPoints::const_iterator it = pathPts.begin(); it != itEnd; ++it)
  170.         {
  171.                 Vec3 delta = it->vPos - prevPos;
  172.                 totalLength += m_params.use2D ? delta.GetLength2D() : delta.GetLength();
  173.                 prevPos = it->vPos;
  174.         }
  175.  
  176.         static bool useSwingout = true;
  177.  
  178.         float distFromStart = 0.0f;
  179.         float distFromEnd = totalLength;
  180.  
  181.         static float startSwingoutDist = 1.0f;
  182.         static float endSwingoutDist = 1.0f;
  183.         // amount of swingout for 90 deg corner
  184.         static float swingoutAmount = 1.0f;
  185.  
  186.         // add extra points this far in front of/behind each original point
  187.         // only swingout if path seg dot product < this
  188.         static float criticalDot = 0.9f;
  189.  
  190.         Vec3 oldPos = pathPts.front().vPos;
  191.         for (TPathPoints::const_iterator it = pathPts.begin(); it != itEnd; ++it)
  192.         {
  193.                 static float pathCurveDist = 2.0f;
  194.                 const PathPointDescriptor& ppd = *it;
  195.  
  196.                 float prevSegLen = m_params.use2D ? Distance::Point_Point2D(ppd.vPos, oldPos) : Distance::Point_Point(ppd.vPos, oldPos);
  197.                 distFromStart += prevSegLen;
  198.                 distFromEnd -= prevSegLen;
  199.  
  200.                 if (useSwingout && distFromStart > startSwingoutDist && distFromEnd > endSwingoutDist && (ppd.navType & swingOutTypes))
  201.                 {
  202.                         // current pt
  203.                         {
  204.                                 Vec3 prevPos2 = ::GetPathPointAhead(pathPts, it, -pathCurveDist);
  205.                                 Vec3 nextPos = ::GetPathPointAhead(pathPts, it, pathCurveDist);
  206.  
  207.                                 Vec3 swingOutDir = -(nextPos + prevPos2 - 2.0f * ppd.vPos) / square(pathCurveDist);
  208.                                 float diff2Len = swingOutDir.NormalizeSafe();
  209.  
  210.                                 static float swingoutScale = 1.0f;
  211.                                 float swingout = diff2Len * swingoutScale;
  212.                                 Limit(swingout, 0.0f, 1.0f);
  213.  
  214.                                 Vec3 prevDir = ppd.vPos - oldPos;
  215.                                 if (m_params.use2D) prevDir.z = 0.0f;
  216.                                 prevDir.NormalizeSafe();
  217.  
  218.                                 TPathPoints::const_iterator itNext = it;
  219.                                 ++itNext;
  220.                                 Vec3 nextPt = (itNext == itEnd) ? ppd.vPos : itNext->vPos;
  221.                                 Vec3 nextDir = nextPt - ppd.vPos;
  222.                                 if (m_params.use2D) nextDir.z = 0.0f;
  223.                                 nextDir.NormalizeSafe();
  224.  
  225.                                 Vec3 prevOutDir(prevDir.y, -prevDir.x, 0.0f);
  226.                                 Vec3 nextOutDir(nextDir.y, -nextDir.x, 0.0f);
  227.  
  228.                                 if (prevOutDir.Dot(swingOutDir) < 0.0f)
  229.                                         prevOutDir *= -1.0f;
  230.                                 if (nextOutDir.Dot(swingOutDir) < 0.0f)
  231.                                         nextOutDir *= -1.0f;
  232.  
  233.                                 if (prevDir.Dot(nextDir) > criticalDot)
  234.                                 {
  235.                                         m_pathControlPoints.push_back(SPathControlPoint(ppd.navType, ppd.vPos, (prevOutDir + nextOutDir).GetNormalizedSafe(), swingout * swingoutAmount));
  236.                                         m_pathControlPoints.back().customId = ppd.navTypeCustomId;
  237.                                 }
  238.                                 else
  239.                                 {
  240.                                         m_pathControlPoints.push_back(SPathControlPoint(ppd.navType, ppd.vPos, prevOutDir, swingout * swingoutAmount));
  241.                                         m_pathControlPoints.back().customId = ppd.navTypeCustomId;
  242.                                         m_pathControlPoints.push_back(SPathControlPoint(ppd.navType, ppd.vPos, nextOutDir, swingout * swingoutAmount));
  243.                                         m_pathControlPoints.back().customId = ppd.navTypeCustomId;
  244.                                 }
  245.                         }
  246.                 }
  247.                 else
  248.                 {
  249.                         m_pathControlPoints.push_back(SPathControlPoint(ppd.navType, ppd.vPos, ZERO, 0.0f));
  250.                         m_pathControlPoints.back().customId = ppd.navTypeCustomId;
  251.                 }
  252.                 oldPos = ppd.vPos;
  253.         }
  254.  
  255.         // Now cut the path short if we should stop before the end
  256.         if (m_params.endDistance > 0.0f)
  257.         {
  258.                 float cutDist = 0.0f; // total amount we have cut
  259.                 for (int iPt = ((int) m_pathControlPoints.size()) - 2; iPt >= 0; --iPt)
  260.                 {
  261.                         int iNext = iPt + 1;
  262.                         Vec3 pt = GetPathControlPoint(iPt);
  263.                         Vec3 next = GetPathControlPoint(iNext);
  264.                         float dist = DistancePointPoint(pt, next);
  265.                         if (m_pathControlPoints.size() > 2 && (dist < 0.01f || cutDist + dist < m_params.endDistance))
  266.                         {
  267.                                 m_pathControlPoints.pop_back();
  268.                                 cutDist += dist;
  269.                         }
  270.                         else
  271.                         {
  272.                                 float frac = clamp_tpl(1.0f - (m_params.endDistance - cutDist) / dist, 0.001f, 1.0f);
  273.                                 m_pathControlPoints.back().pos = pt + (next - pt) * frac;
  274.                                 break;
  275.                         }
  276.                 }
  277.         }
  278.  
  279.         AIAssert(m_pathControlPoints.size() >= 2);
  280.  
  281.         Lineseg lastSeg;
  282.         lastSeg.end = m_pathControlPoints.back().pos;
  283.         lastSeg.start = m_pathControlPoints.front().pos;
  284.         for (int iPt = ((int) m_pathControlPoints.size()) - 1; iPt >= 0; --iPt)
  285.         {
  286.                 Vec3 pt = m_pathControlPoints[iPt].pos;
  287.                 float dist = DistancePointPoint(pt, lastSeg.end);
  288.                 if (dist > 0.01f)
  289.                 {
  290.                         lastSeg.start = pt;
  291.                         break;
  292.                 }
  293.         }
  294.         if (m_params.use2D)
  295.                 lastSeg.end.z = lastSeg.start.z;
  296.  
  297.         // replicate the last point, but advance it in the last segment direction
  298.         m_pathControlPoints.push_back(m_pathControlPoints.back());
  299.         Vec3 dir = (lastSeg.end - lastSeg.start).GetNormalizedSafe(Vec3Constants<float>::fVec3_OneX);
  300.         m_pathControlPoints.back().offsetDir = dir;
  301.         m_pathControlPoints.back().offsetAmount = m_params.stopAtEnd ? 0.01f : 10.0f;
  302.  
  303.         m_curLASegmentIndex = 0;
  304.         m_curLAPos = GetPathControlPoint(0);
  305. }
  306.  
  307. //===================================================================
  308. // StartFollowing
  309. //===================================================================
  310. void CPathFollower::StartFollowing(const Vec3& curPos, const Vec3& curVel)
  311. {
  312.         float bestPathDistSq = std::numeric_limits<float>::max();
  313.         int nPts = m_pathControlPoints.size();
  314.         for (int i = 0; i < nPts - 1; ++i)
  315.         {
  316.                 float t;
  317.                 Lineseg lineseg(GetPathControlPoint(i), GetPathControlPoint(i + 1));
  318.                 float distSq = Distance::Point_Lineseg2DSq(curPos, lineseg, t);
  319.                 if (distSq < bestPathDistSq)
  320.                 {
  321.                         bestPathDistSq = distSq;
  322.                         m_curLAPos = lineseg.GetPoint(t);
  323.                         m_curLASegmentIndex = i;
  324.                 }
  325.         }
  326.  
  327.         Vec3 newLAPos;
  328.         int newLASegIndex;
  329.         // the large time passed just means that the initial lookahead should not be limited how
  330.         // far it goes - except by the LA distance.
  331.         GetNewLookAheadPos(newLAPos, newLASegIndex, m_curLAPos, m_curLASegmentIndex, curPos, curVel, 100.0f);
  332.         m_curLAPos = newLAPos;
  333.         m_curLASegmentIndex = newLASegIndex;
  334.  
  335.         // only start with curSpeed if we will move exactly aligned with the current velocity
  336.         Vec3 velDir = curVel;
  337.         Vec3 moveDir = (m_curLAPos - curPos);
  338.         if (m_params.use2D)
  339.         {
  340.                 velDir.z = 0.0f;
  341.                 moveDir.z = 0.0f;
  342.         }
  343.         velDir.NormalizeSafe();
  344.         moveDir.NormalizeSafe();
  345.         float dot = velDir.Dot(moveDir);
  346.         Limit(dot, 0.0f, 1.0f);
  347.         float curSpeed = m_params.use2D ? curVel.GetLength2D() : curVel.GetLength();
  348.         m_lastOutputSpeed = curSpeed * dot;
  349. }
  350.  
  351. //===================================================================
  352. // GetNewLookAheadPos
  353. // push the lookahead as far as possible without taking it beyond the lookahead
  354. // distance from curPos
  355. //===================================================================
  356. void CPathFollower::GetNewLookAheadPos(Vec3& newLAPos, int& newLASegIndex, const Vec3& curLAPos, int curLASegIndex,
  357.                                        const Vec3& curPos, const Vec3& curVel, float dt) const
  358. {
  359.         FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  360.  
  361.         newLASegIndex = curLASegIndex;
  362.         newLAPos = curLAPos;
  363.  
  364.         int nPts = m_pathControlPoints.size();
  365.         if (nPts < 2)
  366.                 return;
  367.  
  368.         // Danny todo ultimately do a binary search, or calculate this exactly. For now just probe ahead -
  369.         // extremely slow but easy to test with and easy to optimise later
  370.         float probeDist = m_params.pathLookAheadDist * 0.01f;
  371.  
  372.         // limit the total amount we probe ahead
  373.         static float maxDistScale = 3.0f;
  374.         float maxProbeDist = maxDistScale * dt * m_params.maxSpeed;
  375.         float totalProbeDist = 0.0f; // how far probe moved in total
  376.  
  377.         float curDist;
  378.  
  379.         Vec3 prevPt, nextPt;
  380.         int nextPtIndex;
  381.         float segLen, distToNext;
  382.  
  383.         // need to execute this when newLASegIndex changes
  384. #define CALC_SEGMENT                           \
  385.   prevPt = GetPathControlPoint(newLASegIndex); \
  386.   nextPtIndex = newLASegIndex + 1;             \
  387.   if (nextPtIndex == nPts)                     \
  388.   {                                            \
  389.     newLAPos = prevPt;                         \
  390.     return;                                    \
  391.   }                                            \
  392.   nextPt = GetPathControlPoint(nextPtIndex);   \
  393.   segLen = DistancePointPoint(prevPt, nextPt); \
  394.   distToNext = DistancePointPoint(newLAPos, nextPt)
  395.  
  396.         CALC_SEGMENT;
  397.  
  398.         while ((curDist = DistancePointPoint(newLAPos, curPos)) < m_params.pathLookAheadDist)
  399.         {
  400.                 // rather than calculating exactly how far along the next segment - just go to the start of it
  401.                 float thisProbeDist = max(probeDist, m_params.pathLookAheadDist - curDist);
  402.                 bool returnEarly = false;
  403.                 if (thisProbeDist + totalProbeDist > maxProbeDist)
  404.                 {
  405.                         thisProbeDist = maxProbeDist - totalProbeDist;
  406.                         returnEarly = true;
  407.                 }
  408.  
  409.                 if (thisProbeDist > distToNext)
  410.                 {
  411.                         newLAPos = nextPt;
  412.                         ++newLASegIndex;
  413.                         totalProbeDist += distToNext;
  414.                         if (newLASegIndex > nPts - 2)
  415.                         {
  416.                                 newLASegIndex = nPts - 2;
  417.                                 return;
  418.                         }
  419.                         CALC_SEGMENT;
  420.                 }
  421.                 else
  422.                 {
  423.                         float distFromNext = distToNext - thisProbeDist;
  424.                         float frac = (distFromNext / segLen);
  425.                         newLAPos = frac * prevPt + (1.0f - frac) * nextPt;
  426.                         totalProbeDist += thisProbeDist;
  427.                         if (returnEarly)
  428.                                 return;
  429.                         distToNext -= thisProbeDist;
  430.                 }
  431.         }
  432. }
  433.  
  434. //===================================================================
  435. // UseLookAheadPos
  436. //===================================================================
  437. void CPathFollower::UseLookAheadPos(Vec3& velocity, bool& reachedEnd, const Vec3 LAPos, const Vec3 curPos, const Vec3 curVel,
  438.                                     float dt, int curLASegmentIndex, float& lastOutputSpeed) const
  439. {
  440.         FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  441.  
  442.         velocity = LAPos - curPos;
  443.         if (m_params.use2D)
  444.                 velocity.z = 0.0f;
  445.         float dist = velocity.NormalizeSafe();
  446.         float speed = m_params.normalSpeed * dist / m_params.pathLookAheadDist;
  447.  
  448.         // speed control depending on curvature
  449.         static bool doCurvatureSpeedControl = true;
  450.         if (doCurvatureSpeedControl)
  451.         {
  452.                 float actualPathDist;
  453.                 static float pathDist = 3.0f;
  454.                 Vec3 posAhead = GetPathPointAhead(pathDist, actualPathDist, curLASegmentIndex, LAPos);
  455.                 if (actualPathDist > 0.1f)
  456.                 {
  457.                         float distToPosAhead = DistancePointPoint(posAhead, LAPos);
  458.                         float ratio = distToPosAhead / actualPathDist;
  459.                         static float minSpeedScale = 0.5f;
  460.                         static float multiplier = 3.0f;
  461.                         float speedScale = 1 + (ratio - 1) * multiplier;
  462.                         Limit(speedScale, minSpeedScale, 1.0f);
  463.                         speed *= speedScale;
  464.                 }
  465.         }
  466.  
  467.         // speed control based on slowing down to stop at the end of the path
  468.         // at distance s = vt - 0.5 * a * t^2
  469.         // and time is given by t = v / a
  470.         // so s = v^2 / a - 0.5 * v^2 / a
  471.         // so v = sqrt(2 * a * s )
  472.         // only do it if it's worth it
  473.  
  474.         // Marcio: Only do this if we want to stop at the end
  475.         if (m_params.stopAtEnd)
  476.         {
  477.                 float cutoffDist = 0.5f * square(speed) / min(3.0f, m_params.maxDecel);
  478.                 float directDistToEnd = DistancePointPoint(curPos, GetPathControlPoint(m_pathControlPoints.size() - 1));
  479.                 if (directDistToEnd < cutoffDist)
  480.                 {
  481.                         float distToEnd = GetDistToEnd(&curPos, curLASegmentIndex, LAPos);
  482.                         float maxSpeed = sqrtf(2.0f * m_params.maxDecel * distToEnd);
  483.                         if (speed > maxSpeed)
  484.                                 speed = maxSpeed;
  485.                 }
  486.         }
  487.  
  488.         // Allow to slow down to full stop at the end of the path.
  489.         /*      const float slowDownDist = 3.0f;
  490.            const float slowDownSpeed = 5.0f;
  491.            const float minSpeedEndOfPath = 0.5f + slowDownSpeed * (directDistToEnd / slowDownDist);
  492.            float minSpeed = min(m_params.minSpeed, minSpeedEndOfPath);*/
  493.  
  494.         float maxOutputSpeed = min(lastOutputSpeed + dt * m_params.maxAccel, m_params.maxSpeed);
  495.         float minOutputSpeed = m_params.stopAtEnd ? 0.0f : max(lastOutputSpeed - dt * m_params.maxDecel, m_params.minSpeed);
  496.  
  497.         Limit(speed, minOutputSpeed, maxOutputSpeed);
  498.         velocity *= speed;
  499.         lastOutputSpeed = speed;
  500.  
  501.         // only finish when we go just beyond the start of the last segment
  502.         if (curLASegmentIndex >= (int)m_pathControlPoints.size() - 2)
  503.         {
  504.                 Vec3 segStart = GetPathControlPoint(m_pathControlPoints.size() - 2);
  505.                 Vec3 segEnd = GetPathControlPoint(m_pathControlPoints.size() - 1);
  506.  
  507.                 float distSq = (curPos - segStart).GetLengthSquared2D();
  508.  
  509.                 if (m_params.stopAtEnd)
  510.                         reachedEnd = distSq < 0.05 * 0.05f;
  511.                 else
  512.                         reachedEnd = distSq < 0.25f * 0.25f;
  513.         }
  514.         else
  515.                 reachedEnd = false;
  516. }
  517.  
  518. uint32 CPathFollower::GetIndex(const Vec3& pos) const
  519. {
  520.         FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  521.  
  522.         float bestPathDistSq = std::numeric_limits<float>::max();
  523.         int nPts = m_pathControlPoints.size();
  524.  
  525.         uint32 ret = 0;
  526.  
  527.         for (int i = 0; i < nPts - 1; ++i)
  528.         {
  529.                 float t;
  530.                 Lineseg lineseg(GetPathControlPoint(i), GetPathControlPoint(i + 1));
  531.                 float distSq = Distance::Point_Lineseg2DSq(pos, lineseg, t);
  532.                 if (distSq < bestPathDistSq)
  533.                 {
  534.                         bestPathDistSq = distSq;
  535.                         ret = i;
  536.                 }
  537.         }
  538.  
  539.         return ret;
  540. }
  541.  
  542. //===================================================================
  543. // Update
  544. // Method is to advance the lookahead as far as possible without exceeding
  545. // the lookahead distance. Then move towards it.
  546. // For the moment do no speed control
  547. //===================================================================
  548. bool CPathFollower::Update(PathFollowResult& result, const Vec3& curPos, const Vec3& curVel, float dt)
  549. {
  550.         FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  551.  
  552.         float curSpeed = m_params.use2D ? curVel.GetLength2D() : curVel.GetLength();
  553.  
  554.         if (m_pathVersion != m_navPath->GetVersion())
  555.         {
  556.                 ProcessPath();
  557.                 StartFollowing(curPos, curVel);
  558.         }
  559.  
  560.         // clamp the last speed to +/- speedClampTol of the actual speed. This is mainly to
  561.         // catch the case where the entity is limited by animation. We can't use the physics
  562.         // speed exactly because there's no guarantee it is what we asked for last time
  563.         // and using it can stop us accelerating
  564.         static float speedClampTol = 0.5f;
  565.         if (m_lastOutputSpeed > curSpeed + speedClampTol)
  566.                 m_lastOutputSpeed = curSpeed + speedClampTol;
  567.  
  568.         result.velocityOut.zero();
  569.         if (result.predictedStates)
  570.                 result.predictedStates->resize(0);
  571.         result.reachedEnd = true;
  572.  
  573.         // an extra point got added during preprocessing, so a real path will always be more than
  574.         // two points
  575.         if (m_pathControlPoints.size() < 2)
  576.                 return false;
  577.  
  578.         Vec3 newLAPos;
  579.         int newLASegIndex;
  580.         GetNewLookAheadPos(newLAPos, newLASegIndex, m_curLAPos, m_curLASegmentIndex, curPos, curVel, dt);
  581.  
  582.         m_CurPos = curPos;
  583.         m_CurIndex = GetIndex(curPos);
  584.  
  585.         m_curLAPos = newLAPos;
  586.         m_curLASegmentIndex = newLASegIndex;
  587.  
  588.         UseLookAheadPos(result.velocityOut, result.reachedEnd, m_curLAPos, curPos, curVel, dt, m_curLASegmentIndex, m_lastOutputSpeed);
  589.  
  590.         if (result.reachedEnd)
  591.         {
  592.                 result.velocityOut.zero();
  593.                 return true;
  594.         }
  595.  
  596.         if (result.predictedStates)
  597.         {
  598.                 result.predictedStates->resize(0);
  599.                 int nDesiredPredictions = (int)(result.desiredPredictionTime / result.predictionDeltaTime + 0.5f);
  600.  
  601.                 // Danny todo having a very small value here improves the smoothness of the prediction, but
  602.                 // it's expensive too.
  603.                 static float idealDt = 0.05f;
  604.                 int stepsPerPrediction = (int) (result.predictionDeltaTime / idealDt);
  605.                 float actualDt = result.predictionDeltaTime / stepsPerPrediction;
  606.  
  607.                 // copy the variables so we use/modify them for prediction
  608.                 Vec3 predCurLAPos = m_curLAPos;
  609.                 int predCurLASegmentIndex = m_curLASegmentIndex;
  610.                 Vec3 predCurPos = curPos;
  611.                 Vec3 predCurVel = curVel;
  612.                 float lastOutputSpeed = m_lastOutputSpeed;
  613.  
  614.                 for (int iPrediction = 0; iPrediction != nDesiredPredictions; ++iPrediction)
  615.                 {
  616.                         for (int iStep = 0; iStep < stepsPerPrediction; ++iStep)
  617.                         {
  618.                                 Vec3 newLAPos2;
  619.                                 int newLASegIndex2;
  620.                                 GetNewLookAheadPos(newLAPos2, newLASegIndex2, predCurLAPos, predCurLASegmentIndex, predCurPos, predCurVel, actualDt);
  621.  
  622.                                 predCurLAPos = newLAPos2;
  623.                                 predCurLASegmentIndex = newLASegIndex2;
  624.  
  625.                                 Vec3 velocity;
  626.                                 bool reachedEnd;
  627.                                 UseLookAheadPos(velocity, reachedEnd, predCurLAPos, predCurPos, predCurVel, actualDt, predCurLASegmentIndex, lastOutputSpeed);
  628.  
  629.                                 if (reachedEnd && m_params.stopAtEnd)
  630.                                 {
  631.                                         predCurVel.zero();
  632.                                         predCurPos = GetPathControlPoint(m_pathControlPoints.size() - 2);
  633.                                 }
  634.                                 else
  635.                                 {
  636.                                         // now assume physics would do what it's told
  637.                                         predCurPos += predCurVel * actualDt; // predCurVel seems slightly better than using velocity, or even midpoint method
  638.                                         predCurVel = velocity;
  639.                                 }
  640.                         }
  641.  
  642.                         result.predictedStates->push_back(PathFollowResult::SPredictedState(predCurPos, predCurVel));
  643.                 }
  644.         }
  645.  
  646. #ifndef _RELEASE
  647.         if (gAIEnv.CVars.DrawPathFollower == 1)
  648.         {
  649.                 Draw();
  650.         }
  651. #endif
  652.  
  653.         // This path follower always *assumes* the follow target is always reachable.
  654.         return true;
  655. }
  656.  
  657. //===================================================================
  658. // GetDistToEnd
  659. //===================================================================
  660. float CPathFollower::GetDistToEnd(const Vec3* pCurPos, int curLASegmentIndex, Vec3 curLAPos) const
  661. {
  662.         if (m_pathControlPoints.empty())
  663.                 return 0.0f;
  664.  
  665.         float dist = 0.0f;
  666.         int nPts = m_pathControlPoints.size();
  667.         for (int i = curLASegmentIndex + 1; i < nPts - 2; ++i)
  668.                 dist += DistancePointPoint(GetPathControlPoint(i), GetPathControlPoint(i + 1));
  669.  
  670.         dist += DistancePointPoint(curLAPos, GetPathControlPoint(min(curLASegmentIndex + 1, nPts - 1)));
  671.         if (pCurPos)
  672.                 dist += DistancePointPoint(*pCurPos, curLAPos);
  673.         return dist;
  674. }
  675.  
  676. //===================================================================
  677. // GetDistToEnd
  678. //===================================================================
  679. float CPathFollower::GetDistToEnd(const Vec3* pCurPos) const
  680. {
  681.         return GetDistToEnd(pCurPos, m_curLASegmentIndex, m_curLAPos);
  682. }
  683.  
  684. //===================================================================
  685. // Serialize
  686. //===================================================================
  687. void CPathFollower::Serialize(TSerialize ser)
  688. {
  689.         ser.Value("m_params", m_params);
  690.         ser.Value("m_curLAPos", m_curLAPos);
  691.         ser.Value("m_curLASegmentIndex", m_curLASegmentIndex);
  692.         ser.Value("m_lastOutputSpeed", m_lastOutputSpeed);
  693. }
  694.  
  695. //===================================================================
  696. // Draw
  697. //===================================================================
  698. void CPathFollower::Draw(const Vec3& drawOffset) const
  699. {
  700.         bool useTerrain = false;
  701.  
  702.         if (!m_pathControlPoints.empty() && m_pathControlPoints.front().navType == IAISystem::NAV_TRIANGULAR)
  703.                 useTerrain = true;
  704.  
  705.         CDebugDrawContext dc;
  706.  
  707.         Sphere LASphere(m_curLAPos, 0.2f);
  708.         LASphere.center.z = dc->GetDebugDrawZ(LASphere.center, useTerrain);
  709.         dc->DrawSphere(LASphere.center, LASphere.radius, ColorB(255, 0, 0));
  710.  
  711.         int nPts = m_pathControlPoints.size();
  712.         for (int i = 1; i < nPts; ++i)
  713.         {
  714.                 dc->DrawLine(GetPathControlPoint(i - 1), ColorB(0, 0, 0), GetPathControlPoint(i), ColorB(0, 0, 0));
  715.                 dc->DrawSphere(GetPathControlPoint(i - 1), 0.05f, ColorB(0, 0, 0));
  716.         }
  717. }
  718.  
  719. //===================================================================
  720. // GetDistToSmartObject
  721. //===================================================================
  722. float CPathFollower::GetDistToSmartObject() const
  723. {
  724.         return GetDistToNavType(IAISystem::NAV_SMARTOBJECT);
  725. }
  726.  
  727. float CPathFollower::GetDistToNavType(IAISystem::ENavigationType navType) const
  728. {
  729.         uint32 nPts = m_pathControlPoints.size();
  730.  
  731.         if (m_CurIndex + 1 >= nPts)
  732.                 return std::numeric_limits<float>::max();
  733.  
  734.         float curDist = DistancePointPoint(m_CurPos, m_pathControlPoints[m_CurIndex].pos);
  735.         float totalDist = DistancePointPoint(m_pathControlPoints[m_CurIndex].pos, m_pathControlPoints[m_CurIndex + 1].pos);
  736.         float curFraction = (totalDist > 0.0f) ? (curDist / totalDist) : 0.0f;
  737.  
  738.         if ((m_pathControlPoints[m_CurIndex].navType == navType) &&
  739.             (m_pathControlPoints[m_CurIndex + 1].navType == navType) &&
  740.             (m_pathControlPoints[m_CurIndex].customId == m_pathControlPoints[m_CurIndex + 1].customId))
  741.                 return (curFraction < 0.5f) ? 0.0f : std::numeric_limits<float>::max();
  742.  
  743.         float dist = 0.0f;
  744.         Vec3 lastPos = m_CurPos;
  745.         for (uint32 i = m_CurIndex + 1; i < nPts; ++i)
  746.         {
  747.                 dist += DistancePointPoint(lastPos, m_pathControlPoints[i].pos);
  748.                 if (i + 1 < nPts)
  749.                 {
  750.                         if ((m_pathControlPoints[i].navType == navType) &&
  751.                             (m_pathControlPoints[i + 1].navType == navType) &&
  752.                             (m_pathControlPoints[i].customId == m_pathControlPoints[i + 1].customId))
  753.                                 return dist;
  754.                 }
  755.                 else
  756.                 {
  757.                         if (m_pathControlPoints[i].navType == navType)
  758.                                 return dist;
  759.                 }
  760.                 lastPos = m_pathControlPoints[i].pos;
  761.         }
  762.         return std::numeric_limits<float>::max();
  763. }
  764.  
  765. //===================================================================
  766. // GetDistToCustomNav
  767. //===================================================================
  768. float CPathFollower::GetDistToCustomNav(const TPathControlPoints& controlPoints, uint32 curLASegmentIndex, const Vec3& curLAPos) const
  769. {
  770.         int nPts = controlPoints.size();
  771.  
  772.         float dist = std::numeric_limits<float>::max();
  773.  
  774.         if (curLASegmentIndex + 1 < (uint32)nPts)
  775.         {
  776.  
  777.                 dist = 0.0f;
  778.  
  779.                 if (controlPoints[curLASegmentIndex].navType != IAISystem::NAV_CUSTOM_NAVIGATION ||
  780.                     controlPoints[curLASegmentIndex + 1].navType != IAISystem::NAV_CUSTOM_NAVIGATION)
  781.                 {
  782.                         Vec3 lastPos = curLAPos;
  783.  
  784.                         for (int i = curLASegmentIndex + 1; i < nPts; ++i)
  785.                         {
  786.                                 dist += DistancePointPoint(lastPos, m_pathControlPoints[i].pos);
  787.                                 if (i + 1 < nPts)
  788.                                 {
  789.                                         if (controlPoints[i].navType == IAISystem::NAV_CUSTOM_NAVIGATION &&
  790.                                             controlPoints[i + 1].navType == IAISystem::NAV_CUSTOM_NAVIGATION)
  791.                                         {
  792.                                                 break;
  793.                                         }
  794.                                 }
  795.                                 else
  796.                                 {
  797.                                         if (controlPoints[i].navType == IAISystem::NAV_CUSTOM_NAVIGATION)
  798.                                         {
  799.                                                 break;
  800.                                         }
  801.                                 }
  802.  
  803.                                 lastPos = controlPoints[i].pos;
  804.                         }
  805.                 }
  806.         }
  807.  
  808.         return dist;
  809. }
  810.  
  811. //===================================================================
  812. // GetPathPointAhead
  813. //===================================================================
  814. Vec3 CPathFollower::GetPathPointAhead(float dist, float& actualDist, int curLASegmentIndex, Vec3 curLAPos) const
  815. {
  816.         Vec3 pt = curLAPos;
  817.         actualDist = 0.0f;
  818.         const int nPts1 = m_pathControlPoints.size() - 1;
  819.         while (curLASegmentIndex++ < nPts1)
  820.         {
  821.                 Vec3 nextPathPt = GetPathControlPoint(curLASegmentIndex);
  822.                 float distToNext = DistancePointPoint(pt, nextPathPt);
  823.  
  824.                 float newDist = actualDist + distToNext;
  825.                 if (newDist > dist && (newDist - actualDist) > 0.0f)
  826.                 {
  827.                         float frac = (dist - actualDist) / (newDist - actualDist);
  828.                         pt = frac * nextPathPt + (1.0f - frac) * pt;
  829.                         actualDist = dist;
  830.                         return pt;
  831.                 }
  832.                 actualDist = newDist;
  833.                 pt = nextPathPt;
  834.         }
  835.         return pt;
  836. }
  837.  
  838. //===================================================================
  839. // GetPathPointAhead
  840. //===================================================================
  841. Vec3 CPathFollower::GetPathPointAhead(float dist, float& actualDist) const
  842. {
  843.         return GetPathPointAhead(dist, actualDist, m_CurIndex, m_CurPos);
  844. }
  845.  
  846. //===================================================================
  847. // Advance
  848. //===================================================================
  849. void CPathFollower::Advance(float distance)
  850. {
  851.         float distLeft = distance;
  852.         while (distLeft > 0.0f && m_curLASegmentIndex < (int) m_pathControlPoints.size() - 1)
  853.         {
  854.                 Vec3 nextPt = GetPathControlPoint(m_curLASegmentIndex + 1);
  855.                 float distToNext = DistancePointPoint(m_curLAPos, nextPt);
  856.                 if (distToNext > distLeft && distToNext > 0.0f)
  857.                 {
  858.                         float frac = distLeft / distToNext;
  859.                         m_curLAPos = frac * nextPt + (1.0f - frac) * m_curLAPos;
  860.                         return;
  861.                 }
  862.                 distLeft -= distToNext;
  863.                 m_curLAPos = nextPt;
  864.                 ++m_curLASegmentIndex;
  865.         }
  866. }
  867.  
  868. //===================================================================
  869. // CheckWalkability
  870. //===================================================================
  871. bool CPathFollower::CheckWalkability(const Vec2* path, const size_t length) const
  872. {
  873.         CRY_ASSERT(false); // not implemented
  874.         return false;
  875. }
  876.  
  877. //===================================================================
  878. // GetAllowCuttingCorners
  879. //===================================================================
  880. bool CPathFollower::GetAllowCuttingCorners() const
  881. {
  882.         return true;
  883. }
  884.  
  885. //===================================================================
  886. // SetAllowCuttingCorners
  887. //===================================================================
  888. void CPathFollower::SetAllowCuttingCorners(const bool allowCuttingCorners)
  889. {
  890.         CRY_ASSERT(false); // not implemented
  891. }
  892.  
  893. //===================================================================
  894. // IsRemainingPathAffectedByNavMeshChange
  895. //===================================================================
  896. bool CPathFollower::IsRemainingPathAffectedByNavMeshChange(const NavigationMeshID affectedMeshID, const MNM::TileID affectedTileID) const
  897. {
  898.         CRY_ASSERT(false);  // not implemented
  899.         return false;
  900. }
  901.  
downloadPathFollower.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