/********************************************************************
Crytek Source File.
Copyright (C), Crytek Studios, 2001-2004.
-------------------------------------------------------------------------
File name:   NavPath.cpp
$Id$
Description: 

-------------------------------------------------------------------------
History:
- 13:1:2005   15:53 : Created by Kirill Bulatsev

*********************************************************************/

#include "StdAfx.h"
#include "NavPath.h"
#include "AILog.h"
#include "CAISystem.h"
#include "ISerialize.h"
#include "ObjectTracker.h"
#include "AICollision.h"
#include "Puppet.h"

#include "ISystem.h"
#include "IConsole.h"
#include "IRenderer.h"
#include "IRenderAuxGeom.h"
#include "AIDebugDrawHelpers.h"

#include <numeric>
#include <algorithm>

//====================================================================
// CNavPath
//====================================================================
CNavPath::CNavPath(void)
: m_endDir(ZERO)
, m_currentFrac(0.0f)
, m_stuckTime(0.0f)
, m_pathEndIsAsRequested(true)
, m_fDiscardedPathLength(0)
{
}

//====================================================================
// ~CNavPath
//====================================================================
CNavPath::~CNavPath(void)
{
}

//====================================================================
// Advance
//====================================================================
bool CNavPath::Advance(PathPointDescriptor& nextPathPoint)
{
  if (m_pathPoints.size() <= 1)
    return false;
  ++m_version.v;
  m_pathPoints.erase(m_pathPoints.begin());
  if (m_pathPoints.size() == 1)
  {
    m_currentFrac = 0.0f;
    return false;
  }
  nextPathPoint = *GetNextPathPoint();
  return true;
}

template <class F> 
ILINE bool IsEquivalent2D(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, f32 epsilon=VEC_EPSILON )
{
  return  ((fabs_tpl(v0.x-v1.x) <= epsilon) &&	(fabs_tpl(v0.y-v1.y) <= epsilon));	
}


//====================================================================
// push_front
//====================================================================
void CNavPath::PushFront(const PathPointDescriptor& newPathPoint, bool force)
{
  ++m_version.v;
  // Don't allow null length segments!
  IAISystem::ENavigationType newType = newPathPoint.navType;
  IAISystem::ENavigationType curType = m_pathPoints.front().navType;
  if(!force && !m_pathPoints.empty())
  {
    bool twoD = (curType & (IAISystem::NAV_TRIANGULAR | IAISystem::NAV_WAYPOINT_HUMAN)) != 0 ||
      (newType & (IAISystem::NAV_TRIANGULAR | IAISystem::NAV_WAYPOINT_HUMAN)) != 0;

    if(twoD ? IsEquivalent2D(newPathPoint.vPos, m_pathPoints.front().vPos) : IsEquivalent(newPathPoint.vPos, m_pathPoints.front().vPos))
    {
      if (newType != IAISystem::NAV_SMARTOBJECT && curType != IAISystem::NAV_SMARTOBJECT)
        return;
    }
  }
  m_pathPoints.insert(m_pathPoints.begin(), newPathPoint);
}

//====================================================================
// PushBack
//====================================================================
void CNavPath::PushBack(const PathPointDescriptor& newPathPoint, bool force)
{
  ++m_version.v;
  // Don't allow null length segments!
  IAISystem::ENavigationType newType = newPathPoint.navType;
  IAISystem::ENavigationType curType = m_pathPoints.back().navType;
  if(!force && !m_pathPoints.empty() )
  {
    bool twoD = (curType & (IAISystem::NAV_TRIANGULAR | IAISystem::NAV_WAYPOINT_HUMAN)) != 0 ||
      (newType & (IAISystem::NAV_TRIANGULAR | IAISystem::NAV_WAYPOINT_HUMAN)) != 0;

    if(twoD ? IsEquivalent2D(newPathPoint.vPos, m_pathPoints.back().vPos) : IsEquivalent(newPathPoint.vPos, m_pathPoints.back().vPos))
    {
      if (newType != IAISystem::NAV_SMARTOBJECT && curType != IAISystem::NAV_SMARTOBJECT)
        return;
    }
  }
  m_pathPoints.push_back(newPathPoint);
}

//====================================================================
// GetLength
//====================================================================
float CNavPath::GetPathLength(bool twoD) const
{
  float length = 0.0f;
  Vec3 pos;
  if (!GetPosAlongPath(pos, 0.0f, false, false))
    return 0.0f;

  if (m_pathPoints.size() > 1)
  {
    TPathPoints::const_iterator li = m_pathPoints.begin();
    for (++li; li!=m_pathPoints.end(); ++li)
    {
      length += twoD ? (li->vPos-pos).GetLength2D() : (li->vPos-pos).GetLength();
      pos = li->vPos;
    }
  }
  return length;
}

//====================================================================
// Draw
//====================================================================
void CNavPath::Draw(IRenderer *pRenderer, const Vec3& drawOffset) const
{
  bool useTerrain = false;

  if (!m_pathPoints.empty())
  {
    TPathPoints::const_iterator li,linext;
    li = m_pathPoints.begin();
    linext = li;
    ++linext;
    ColorF col(0.75, 0.5, 0.25);
    while (linext!=m_pathPoints.end())
    {
      Vec3 p0 = li->vPos;
      Vec3 p1 = linext->vPos;
      if (li->navType == IAISystem::NAV_TRIANGULAR)
        useTerrain = true;
      p0.z = GetDebugDrawZ(p0, li->navType == IAISystem::NAV_TRIANGULAR);
      p1.z = GetDebugDrawZ(p1, li->navType == IAISystem::NAV_TRIANGULAR);
      pRenderer->GetIRenderAuxGeom()->DrawLine(p0, col, p1, col );
      pRenderer->GetIRenderAuxGeom()->DrawSphere(p1, 0.05f, col);
      li=linext;
      ++linext;
    }
  }

  std::list<SDebugLine>::const_iterator	dli;
  for( dli = m_debugLines.begin(); dli != m_debugLines.end(); ++dli )
  {
    SDebugLine line = (*dli);
    line.P0.z = GetDebugDrawZ(line.P0, useTerrain);
    line.P1.z = GetDebugDrawZ(line.P1, useTerrain);
    pRenderer->GetIRenderAuxGeom()->DrawLine( line.P0, line.col, line.P1, line.col );
  }
  std::list<SDebugSphere>::const_iterator	dsi;
  for( dsi = m_debugSpheres.begin(); dsi != m_debugSpheres.end(); ++dsi )
  {
    SDebugSphere sphere = (*dsi);
    sphere.pos.z = GetDebugDrawZ(sphere.pos, useTerrain);
    pRenderer->GetIRenderAuxGeom()->DrawSphere(sphere.pos, sphere.r, sphere.col);
  }
}

//====================================================================
// empty
//====================================================================
bool CNavPath::Empty() const
{
  return m_pathPoints.size() < 2;
}

//====================================================================
// Clear
//====================================================================
void CNavPath::Clear(const char *dbgString) 
{
  ++m_version.v;
  if (GetAISystem()->m_cvDebugPathFinding->GetIVal() && !m_pathPoints.empty())
  {
    const PathPointDescriptor &ppd = m_pathPoints.back();
    AILogAlways("CNavPath::Clear old path end is (%5.2f, %5.2f, %5.2f) %s",
      ppd.vPos.x, ppd.vPos.y, ppd.vPos.z, dbgString);
  }
  m_pathPoints.clear();
  m_debugLines.clear();
  m_debugSpheres.clear();
  m_currentFrac = 0.0f;
  m_stuckTime = 0.0f;
  m_pathEndIsAsRequested = true;
  m_fDiscardedPathLength = 0;
}

//====================================================================
// SetPreviousPoint
//====================================================================
void CNavPath::SetPreviousPoint(const PathPointDescriptor& previousPoint)
{
  if (m_pathPoints.empty())
    return;
  ++m_version.v;
  m_pathPoints.front() = previousPoint;
}

//====================================================================
// DebugLine
//====================================================================
void CNavPath::DebugLine( const Vec3& P0, const Vec3& P1, const ColorF& col ) const
{
  if (GetAISystem()->m_cvDebugDraw->GetIVal() == 0)
    return;
  SDebugLine	line;
  line.P0 = P0;
  line.P1 = P1;
  line.col = col;
  m_debugLines.push_back( line );
}

void CNavPath::DebugSphere( const Vec3& pos, float r, const ColorF& col ) const
{
  if (GetAISystem()->m_cvDebugDraw->GetIVal() == 0)
    return;
  SDebugSphere sphere;
  sphere.pos = pos;
  sphere.r = r;
  sphere.col = col;
  m_debugSpheres.push_back( sphere );
}

//===================================================================
// Serialize
//===================================================================
void PathPointDescriptor::SmartObjectNavData::Serialize(TSerialize ser)
{
  GetAISystem()->GetGraph()->SerializeNodePointer(ser, "pFrom", fromIndex);
  GetAISystem()->GetGraph()->SerializeNodePointer(ser, "pTo", toIndex);
}


//====================================================================
// Serialize
//====================================================================
void PathPointDescriptor::Serialize(TSerialize ser)
{
  ser.Value("vPos", vPos);
  ser.EnumValue("navType", navType, IAISystem::NAV_UNSET, IAISystem::NAV_MAX_VALUE);
  ser.EnumValue("navSOMethod", navSOMethod, nSOmNone, nSOmLast);

  if(ser.IsWriting())
  {
    if(ser.BeginOptionalGroup("pSONavData", pSONavData!=NULL))
    {
      pSONavData->Serialize(ser);
      ser.EndGroup();
    }
  }
  else
  {
    pSONavData = NULL;
    if(ser.BeginOptionalGroup("pSONavData", true))
    {
      pSONavData = new SmartObjectNavData;
      pSONavData->Serialize(ser);
      ser.EndGroup();
    }
  }
}

//===================================================================
// Serialize
//===================================================================
void SNavPathParams::Serialize(TSerialize ser, class CObjectTracker& objectTracker)
{
  ser.Value("start", start);
  ser.Value("end", end);
  ser.Value("startDir", startDir);
  ser.Value("endDir", endDir);
  ser.Value("nForceBuildingID", nForceBuildingID);
  ser.Value("allowDangerousDestination", allowDangerousDestination);
  ser.Value("precalculatedPath", precalculatedPath);
  ser.Value("inhibitPathRegeneration", inhibitPathRegeneration);
  ser.Value("continueMovingAtEnd", continueMovingAtEnd);
	ser.Value("isDirectional", isDirectional);
}

//====================================================================
// Serialize
//====================================================================
void CNavPath::Serialize(TSerialize ser, class CObjectTracker& objectTracker)
{
  ser.BeginGroup("CNavPath");

  unsigned pathSize = m_pathPoints.size();
  ser.Value("pathSize", pathSize);

  if (ser.IsReading())
    m_pathPoints.resize(pathSize);

  if (pathSize > 0)
  {
    ser.Value( "PathPoints",m_pathPoints );
    //for (TPathPoints::iterator it = m_pathPoints.begin() ; it != m_pathPoints.end() ; ++it)
    //it->Serialize(ser);
  }

  ser.Value("m_endDir", m_endDir);
  m_debugLines.clear();
  m_debugSpheres.clear();
  ser.Value("m_currentFrac", m_currentFrac);
  ser.Value("m_stuckTime", m_stuckTime);
  ser.Value("m_pathEndIsAsRequested", m_pathEndIsAsRequested);
  m_params.Serialize(ser, objectTracker);
  objectTracker.SerializeObjectPointerContainer(ser, "m_objectsAdjustedFor", m_objectsAdjustedFor, false);
  ser.Value("m_fDiscardedPathLength", m_fDiscardedPathLength);
  ser.Value("m_version", m_version);

  ser.EndGroup();
}


//====================================================================
// GetPath
//====================================================================
void CNavPath::GetPath(PATHPOINTVECTOR& pathVector) const
{
  pathVector.resize(0);
  for(TPathPoints::const_reverse_iterator itr(m_pathPoints.rbegin()); itr!=m_pathPoints.rend(); ++itr)
    pathVector.push_back( (*itr).vPos );

  if (!pathVector.empty())
    pathVector.pop_back();

  if(!pathVector.empty())
    pathVector.front().vDir = m_endDir;
}


//====================================================================
// UpdatePathPosition
// For each segment from the current one, if the agentPos lies beyond
// the end of the segment, use this segment and get the iterator position
// by projecting onto the segment and clamping.
// "beyond the end of the segment" is done by calculating two planes that
// divide up this segment from the next
//====================================================================
float CNavPath::UpdatePathPosition(Vec3 agentPos, float lookAhead, bool twoD, bool allowPathToFinish)
{
  float totalLen = GetPathLength(twoD);

  if (twoD)
    agentPos.z = 0.0f;

  while (m_pathPoints.size() >= 2)
  {
    TPathPoints::const_iterator pathIt = m_pathPoints.begin();
    TPathPoints::const_iterator pathItNext = pathIt;
    ++pathItNext;

    // start and end of this segment
    Vec3 segStart = pathIt->vPos;
    Vec3 segEnd = pathItNext->vPos;

    float segLen = twoD ? (segStart - segEnd).GetLength2D() : (segStart - segEnd).GetLength();
    if ( segLen > 0.1f)
    {
      // the end of the next segment (may be faked)
      Vec3 segEndNext;

      TPathPoints::const_iterator pathItNextNext = pathItNext;
      ++pathItNextNext;
      if (pathItNextNext == m_pathPoints.end())
        segEndNext = segEnd + (segEnd - segStart);
      else
        segEndNext = pathItNextNext->vPos;

      if (twoD)
        segStart.z = segEnd.z = segEndNext.z = 0.0f;

      Vec3 segDir = (segEnd - segStart).GetNormalizedSafe();

      // don't advance until we're within the path width of the segment end
      Plane planeSeg = Plane::CreatePlane(segDir, segEnd);
      float planeSegDist = planeSeg.DistFromPlane(agentPos);
      if (planeSegDist < -0.6f * lookAhead)
      {
        // This segment is the one - calculate the fraction and then
        // clamp it - but don't go backwards!
        float newFrac = m_currentFrac;
        // all z values are already 0
        Distance::Point_LinesegSq(agentPos, Lineseg(segStart, segEnd), newFrac);
        if (newFrac > 1.0f)
          m_currentFrac = 1.0f;
        else if (newFrac > m_currentFrac)
          m_currentFrac = newFrac;
        return totalLen;
      }

      // past the divider between this and the next?
      Vec3 nextSegDir = (segEndNext - segEnd).GetNormalizedSafe(segDir);
      Vec3 avDir = (segDir + nextSegDir).GetNormalizedSafe(segDir);

      Plane planeSegDiv = Plane::CreatePlane(avDir, segEnd);
      float planeSegDivDist = planeSegDiv.DistFromPlane(agentPos);

      if (planeSegDivDist <= 0.0f)
      {
        // This segment is the one - calculate the fraction and then
        // clamp it - but don't go backwards!
        float newFrac = m_currentFrac;
        // all z values are already 0
        Distance::Point_LinesegSq(agentPos, Lineseg(segStart, segEnd), newFrac);
        if (newFrac > 1.0f)
          m_currentFrac = 1.0f;
        else if (newFrac > m_currentFrac)
          m_currentFrac = newFrac;

        return totalLen;
      }
    } // trivial segment length

    if (!allowPathToFinish && m_pathPoints.size() == 2)
    {
      m_currentFrac = 0.99f;
      return totalLen;
    }

    // moving onto the next segment
    PathPointDescriptor junk;
    Advance(junk);
    m_currentFrac = 0.0;
  }
  // got to the end
  m_currentFrac = 0.0f;
  return totalLen;
}

//====================================================================
// GetPosAlongPath
//====================================================================
bool CNavPath::GetPosAlongPath(Vec3 &posOut, float dist, bool twoD, bool extrapolateBeyondEnd) const
{
  if (m_pathPoints.empty())
    return false;

  if (m_pathPoints.size() == 1)
  {
    posOut = m_pathPoints.front().vPos;
    return true;
  }

  Vec3 thisPt = GetNextPathPoint()->vPos;
  Vec3 prevPt = m_currentFrac * thisPt + (1.0f - m_currentFrac) * GetPrevPathPoint()->vPos;
  const TPathPoints::const_iterator itEnd = m_pathPoints.end();
  for (TPathPoints::const_iterator it = ++m_pathPoints.begin(); it != itEnd ; ++it)
  {
    thisPt = it->vPos;
    Vec3 delta = thisPt - prevPt;
    float deltaLen = twoD ? delta.GetLength2D() : delta.GetLength();
    if (dist <= deltaLen)
    {
      float frac = deltaLen > 0.0f ? dist/deltaLen : 0.0f;
      posOut = frac * thisPt + (1.0f - frac) * prevPt;
      return true;
    }
    dist -= deltaLen;
    prevPt = thisPt;
  }
  if (!extrapolateBeyondEnd)
  {
    posOut = thisPt;
    return true;
  }

  Vec3 delta = thisPt - prevPt;
  float deltaLen = twoD ? delta.GetLength2D() : delta.GetLength();
  float frac = deltaLen > 0.0f ? dist/deltaLen : 0.0f;
  posOut = thisPt + frac * delta;
  return true;
}

//====================================================================
// GetPathProperties
//====================================================================
bool CNavPath::GetPathPropertiesAhead(const float distAhead, const bool twoD, Vec3 &posOut, Vec3 &dirOut, 
                                      float *invRcOut, float &lowestPathDotOut, bool scaleOutputWithDist) const
{
  float dist = distAhead;
  if (invRcOut)
    *invRcOut = 0.0f;
  lowestPathDotOut = 1.0f;
  bool extrapolateBeyondEnd = false;

  if (m_pathPoints.size() < 2)
    return false;

  Vec3 thisPt = GetNextPathPoint()->vPos;
  Vec3 currentPos = m_currentFrac * thisPt + (1.0f - m_currentFrac) * GetPrevPathPoint()->vPos;
  Vec3 prevPt = currentPos;

  Vec3 firstSegDir = thisPt - currentPos;
  if (twoD)
    firstSegDir.z = 0.0f;
  firstSegDir.NormalizeSafe(Vec3Constants<float>::fVec3_OneY);
  dirOut = firstSegDir; // ensure it's not unset

  const TPathPoints::const_iterator itEnd = m_pathPoints.end();
  TPathPoints::const_iterator it;
  for (it = ++m_pathPoints.begin(); it != itEnd ; ++it)
  {
    thisPt = it->vPos;
    Vec3 delta = thisPt - prevPt;
    float deltaLen = twoD ? delta.GetLength2D() : delta.GetLength();

    Vec3 currentSegDir = delta;
    if (twoD) 
      currentSegDir.z = 0.0f;
    float segLen = currentSegDir.NormalizeSafe();
    if (segLen < 0.001f)
      continue;
    dirOut = currentSegDir;
    float pathDirDot = currentSegDir.Dot(firstSegDir);
    if (scaleOutputWithDist)
    {
      float frac = dist / distAhead;
      pathDirDot = frac * pathDirDot + (1.0f - frac) * 1.0f;
    }

    if (pathDirDot < lowestPathDotOut)
      lowestPathDotOut = pathDirDot;

    // calculate posOut here so it is correct when we exit the loop - will
    // adjust if necessary if we didn't need to extrapolate
    float frac = deltaLen > 0.0f ? dist/deltaLen : 1.0f;
    posOut = frac * thisPt + (1.0f - frac) * prevPt;

    if (dist <= deltaLen)
      break;
    dist -= deltaLen;
    prevPt = thisPt;
  }

  if (it == itEnd && !extrapolateBeyondEnd)
  {
    // fallen off the end
    posOut = thisPt;
  }

  if (invRcOut)
  {
    // estimate the radius of curvature by comparing the direct distance with the path distance, and assuming that 
    // the path goes through a single point offset by h (in the middle) from the direct path
    float directDistance = twoD ? Distance::Point_Point2D(posOut, currentPos) : Distance::Point_Point(posOut, currentPos);

    if (directDistance < distAhead)
    {
      float d = 0.5f * directDistance;
      static float hScale = 0.5f; // approx scaling since the angled path isn't a circle
      float h = cry_sqrtf(square(0.5f * distAhead) - square(d));
      h *= hScale;
      float R = 0.5f * (square(h) + square(d) ) / h;
      *invRcOut = 1.0f / R;
    }
    else
    {
      *invRcOut = 0.0f;
    }
  }
  return true;
}

//====================================================================
// DoesTargetSegPosExceedPathSeg
// returns true if targetSeg hits the inside of the infinite cylinder
// represented by pathSeg
//====================================================================
bool WouldTargetPosExceedPathSeg(const Vec3& targetPos, const Lineseg& pathSeg,
                                 float radius, bool twoD)
{
  if (twoD)
  {
    Vec3 segDir = (pathSeg.end - pathSeg.start);
    segDir.z = 0.0f;
    segDir.NormalizeSafe();
    Vec3 delta = (targetPos - pathSeg.start);
    delta.z = 0.0f;
    Vec3 perp = delta - segDir * delta.Dot(segDir);
    float distSq = perp.GetLengthSquared2D();
    return (distSq > square(radius));
  }
  else
  {
    Vec3 segDir = (pathSeg.end - pathSeg.start).GetNormalizedSafe();
    Vec3 delta = (targetPos - pathSeg.start);
    Vec3 perp = delta - segDir * delta.Dot(segDir);
    float distSq = perp.GetLengthSquared();
    return (distSq > square(radius));
  }
}

//====================================================================
// CalculateTargetPos
//====================================================================
Vec3 CNavPath::CalculateTargetPos(Vec3 agentPos, float lookAhead, float minLookAheadAlongPath, float pathRadius, bool twoD) const
{
  if (m_pathPoints.empty())
    return agentPos;
  if (m_pathPoints.size() == 1)
    return m_pathPoints.front().vPos;

  float origLookAhead = lookAhead;

  Vec3 pathPos = GetPrevPathPoint()->vPos * (1.0f - m_currentFrac) + GetNextPathPoint()->vPos * m_currentFrac;

  DebugLine(agentPos, pathPos, ColorF(0, 1, 0));
  DebugSphere(pathPos, 0.3f, ColorF(0, 1, 0));

  float distAgentToPathPos = twoD ? (pathPos - agentPos).GetLength2D() : (pathPos - agentPos).GetLength();
  Vec3 pathDelta = GetNextPathPoint()->vPos - pathPos;
  float distPathPosToCurrent = twoD ? pathDelta.GetLength2D() : pathDelta.GetLength();

  lookAhead -= distAgentToPathPos;

  if (lookAhead < minLookAheadAlongPath)
    lookAhead = minLookAheadAlongPath;

  if (lookAhead < distPathPosToCurrent)
  {
    Vec3 targetPos = pathPos + lookAhead * pathDelta / distPathPosToCurrent;
    return targetPos;
  }

  if (m_pathPoints.size() == 2)
  {
    lookAhead -= distPathPosToCurrent;

    Vec3 delta = GetNextPathPoint()->vPos - GetPrevPathPoint()->vPos;
    if (twoD)
      delta.z = 0.0f;
    float deltaLen = delta.NormalizeSafe();
    if (deltaLen < 0.01f)
    {
      return GetNextPathPoint()->vPos;
    }
    else
    {
      delta *= lookAhead;
      return GetNextPathPoint()->vPos + delta;
    }
  }
  // walk forward through the remaining path until we either reach lookAhead, or we (Danny todo) deviate from
  // the path

  // Binary search to find a suitable target position that doesn't intersect the path width.
  Vec3 targetPos;
  if (false == GetPosAlongPath(targetPos, lookAhead, twoD, true))
    return GetNextPathPoint()->vPos;

  Lineseg curSeg(GetPrevPathPoint()->vPos, GetNextPathPoint()->vPos);
  // if almost on the next seg use that one to allow the lookahead to progress
  if (const PathPointDescriptor *pNextNext = GetNextNextPathPoint())
  {
    Lineseg nextSeg(curSeg.end, pNextNext->vPos);
    float junk;
    float distToNextSegSq = twoD ? Distance::Point_Lineseg2DSq(agentPos, nextSeg, junk) : Distance::Point_LinesegSq(agentPos, nextSeg, junk);
    if (distToNextSegSq < square(pathRadius))
      curSeg = nextSeg;
  }

  if (WouldTargetPosExceedPathSeg(targetPos, curSeg, pathRadius, twoD))
  {
    lookAhead = minLookAheadAlongPath + 0.5f * (lookAhead - minLookAheadAlongPath);
    float delta = lookAhead * 0.5f;
    for (int i = 0 ; i < 8 ; ++i)
    {
      if (false == GetPosAlongPath(targetPos, lookAhead, twoD, true))
        return GetNextPathPoint()->vPos;
      if (WouldTargetPosExceedPathSeg(targetPos, curSeg, pathRadius, twoD))
        lookAhead -= delta;
      else
        lookAhead += delta;
      delta *= 0.5f;
    }
  }
  return targetPos;
}

//===================================================================
// GetPathDeviationDistance
//===================================================================
float CNavPath::GetPathDeviationDistance(Vec3 &deviationOut, float criticalDeviation, bool twoD)
{
  deviationOut.zero();
  if (m_pathPoints.size() < 3)
    return 0.0f;

  TPathPoints::const_iterator it = m_pathPoints.begin();
  Lineseg thisSeg;
  thisSeg.start = (it++)->vPos;
  thisSeg.end = (it++)->vPos;

  float distOut = twoD ? (1.0f - m_currentFrac) * Distance::Point_Point2D(thisSeg.start, thisSeg.end) : 
    m_currentFrac * Distance::Point_Point(thisSeg.start, thisSeg.end);
  Vec3 lastPt = thisSeg.end;
  float lastDeviation = 0.0f;
  for ( ; it != m_pathPoints.end() ; ++it)
  {
    Vec3 pt = it->vPos;
    float	t;
    float deviation = twoD ? Distance::Point_Lineseg2D(pt, Lineseg(thisSeg.start, thisSeg.end), t) : 
      Distance::Point_Lineseg(pt, Lineseg(thisSeg.start, thisSeg.end), t);
    float segLen = twoD ? Distance::Point_Point2D(lastPt, pt) : Distance::Point_Point(lastPt, pt);
    if (deviation > criticalDeviation)
    {
      float frac = 1.0f - (deviation - criticalDeviation) / (deviation - lastDeviation);
      distOut += frac * segLen;
      Vec3 exitPt = frac * pt + (1.0f - frac) * lastPt;

      Vec3 exitDelta = exitPt - thisSeg.start;
      Vec3 segDir = (thisSeg.end - thisSeg.start).GetNormalizedSafe();
      deviationOut = exitDelta - exitDelta.Dot(segDir) * segDir;
      if (twoD)
        deviationOut.z = 0.0f;
      return distOut;
    }
    distOut += segLen;
    lastPt = pt;
    lastDeviation = deviation;
  }

  return distOut;
}

//====================================================================
// Dump
//====================================================================
void CNavPath::Dump(const char *name) const
{
  AILogAlways("Path for %s", name);
  int i = 0;
  for (TPathPoints::const_iterator it = m_pathPoints.begin() ; it != m_pathPoints.end() ; ++it, ++i)
  {
    const PathPointDescriptor &ppd = *it;
    AILogAlways("pt %4d: %5.2f %5.2f %5.2f", i, ppd.vPos.x, ppd.vPos.y, ppd.vPos.z);
  }
}


//====================================================================
// Update
// Procedure is this:
// 1. Calculate a working position based on the input position, velocity 
// and response time.
// 2. Calculate a new path position by advancing the old one.
// 3. Find a new target position by advancing from the iterator
// position
// 4. Adjust the target position to stop it taking the agent too far
// off the path
// 5. Calculate the path curvature and distance to destination
//====================================================================
bool CNavPath::UpdateAndSteerAlongPath(Vec3& dirOut, float& distToEndOut, float &distToPathOut, bool& isResolvingStickingOut,
                                       Vec3 &pathDirOut, Vec3 &pathAheadDirOut, Vec3 &pathAheadPosOut,
                                       Vec3 currentPos, const Vec3& currentVel, 
                                       float lookAhead, float pathRadius, float dt, bool resolveSticking, bool twoD)
{
  FUNCTION_PROFILER(GetAISystem()->m_pSystem, PROFILE_AI);
  m_debugLines.clear();
  m_debugSpheres.clear();

  if (m_pathPoints.size() < 2)
    return false;

  /*
  Vec3 deviation;
  float maxDeviation = 0.5f * pathRadius;
  float dist = GetPathDeviationDistance(deviation, maxDeviation, twoD);
  float frac = dist < lookAhead ? dist / lookAhead : 1.0f - (dist - lookAhead)/5.0f;
  if (frac > 0.0f)
  currentPos += deviation * frac;
  */

  // use a kind-of mid-point method
  const Vec3 workingPos = currentPos;// + currentVel * (0.5f * dt);

  Vec3 origPathPosition(currentPos);
  origPathPosition = GetPrevPathPoint()->vPos * (1.0f - m_currentFrac) + GetNextPathPoint()->vPos * m_currentFrac;

  // update the path position.
  distToEndOut = UpdatePathPosition(workingPos, lookAhead, twoD, true);
  distToPathOut = 0.0f;

  if (m_pathPoints.size() < 2)
    return false;

  float workingLookAhead = lookAhead;
  isResolvingStickingOut = false;
  if (resolveSticking && distToEndOut > lookAhead)
  {
    // prevent sticking if the path position isn't moving and the agent is some way from the path
    Vec3 newPathPosition = GetPrevPathPoint()->vPos * (1.0f - m_currentFrac) + GetNextPathPoint()->vPos * m_currentFrac;
    float dist = (newPathPosition - origPathPosition).GetLength();
    float expectedDist = twoD ? dt * currentVel.GetLength2D() : dt * currentVel.GetLength();

    distToPathOut = twoD ? (currentPos - newPathPosition).GetLength2D() : (currentPos - newPathPosition).GetLength();

    static float stickFrac = 0.1f;
    if (dist < stickFrac * expectedDist && distToPathOut > 0.5f * pathRadius)
      m_stuckTime += dt;
    else
      m_stuckTime -= min(m_stuckTime, dt);

    static float minStickTime = 0.2f;
    static float maxStickTime = 1.0f;
    static float stickTimeRate = 1.0f;
    if (m_stuckTime > minStickTime)
    {
      isResolvingStickingOut = true;
      workingLookAhead *= 1.0f / ( 1.0f + stickTimeRate * (m_stuckTime - minStickTime) );
    }
    if (m_stuckTime > maxStickTime)
      m_stuckTime = maxStickTime;
  }
  else
  {
    Vec3 newPathPosition = GetPrevPathPoint()->vPos * (1.0f - m_currentFrac) + GetNextPathPoint()->vPos * m_currentFrac;
    distToPathOut = twoD ? (currentPos - newPathPosition).GetLength2D() : (currentPos - newPathPosition).GetLength();
  }

  distToEndOut += distToPathOut;

  // Calculate where the agent should head towards - makes sure this heading
  // doesn't deviate too much from the path even with a big look-ahead
  static float minLookAheadFrac = 0.1f;
  float minLookAheadAlongPath = minLookAheadFrac * lookAhead;
  pathAheadPosOut = CalculateTargetPos(workingPos, workingLookAhead, minLookAheadAlongPath, pathRadius, twoD);

  DebugLine(currentPos, pathAheadPosOut, ColorF(1, 0, 0));
  DebugSphere(pathAheadPosOut, 0.3f, ColorF(1, 0, 0));

  dirOut = (pathAheadPosOut - currentPos).GetNormalizedSafe(Vec3Constants<float>::fVec3_OneX);

  pathDirOut = (GetNextPathPoint()->vPos - GetPrevPathPoint()->vPos).GetNormalizedSafe();
  pathAheadDirOut = pathDirOut;

  // Sometimes even if the agent reached the steering point it wouldn't advance the path position
  // (especially for vehicles) so if the agent has already progressed up to the steer point then
  // gradually advance the current position.
  if ((currentPos - pathAheadPosOut).Dot(pathAheadPosOut - workingPos) > 0.0f)
  {
    float advanceDist = twoD ? (pathAheadPosOut - workingPos).GetLength2D() : (pathAheadPosOut - workingPos).GetLength();
    Vec3 pathNextPoint;
    if (GetPosAlongPath(pathNextPoint, advanceDist, true, false))
      UpdatePathPosition(pathNextPoint, 100.0f, true, false);
  }

  return true;
}

//===================================================================
// PreparePathNavigationalSmartObjects
/// Iterates over the path and fills the navigation methods for the path segments.
/// Cuts the path at the path point where the next navSO animation should be played.
/// The trace goalop will follow the path and play the animation at the target location
/// and then regenerate the path to the target.
//===================================================================
void CNavPath::PrepareNavigationalSmartObjects(CPipeUser* pUser)
{
  PathPointDescriptor* pPoint = NULL;
  TPathPoints::iterator it, itEnd = m_pathPoints.end();
  for ( it = m_pathPoints.begin(); it != itEnd; ++it )
  {
    if ( it->navType == IAISystem::NAV_SMARTOBJECT )
    {
      // NavSOs always come in pairs.
      pPoint = &*it;
      ++it;
			GraphNode* pTo = NULL;
			// Only cut at animation navSOs
			if(!pPoint->pSONavData)
			{
				// Can be null only at the end of the path.
				AIAssert(it == itEnd);
				return;
			}
			{
				GraphNode* pFrom = GetAISystem()->GetNodeGraph()->GetNode(pPoint->pSONavData->fromIndex);
				pTo = GetAISystem()->GetNodeGraph()->GetNode(pPoint->pSONavData->toIndex);
				pPoint->navSOMethod = (ENavSOMethod)GetAISystem()->GetNavigationalSmartObjectActionType(pUser, pFrom, pTo);
			}
			if(pPoint->navSOMethod == nSOmStraight)
			{
				if (it == itEnd)
					return;
				continue;
			}

			pUser->SetLastSOExitHelperPos( pTo && (pPoint->navSOMethod != nSOmNone) ? pTo->GetPos() : ZERO);

      ++m_version.v;
      if (it != itEnd)
      {
        AIAssert( it->navType == IAISystem::NAV_SMARTOBJECT );
        AIAssert( pPoint->pSONavData );

        // compute the length of the discarded path segment
//        m_fDiscardedPathLength = 0;
        Vec3 current = pPoint->vPos;
        for( TPathPoints::const_iterator itTemp = it; itTemp != itEnd; ++itTemp )
        {
          m_fDiscardedPathLength += Distance::Point_Point( current, itTemp->vPos );
          current = itTemp->vPos;
        }

        m_pathPoints.erase( it, itEnd );
        return;
      }
      else
      {
        m_pathPoints.pop_back();
        return;
      }
    }
  }
  return;
}

//===================================================================
// MovePathEndsOutOfObstacles
//===================================================================
void CNavPath::TrimPath(float trimLength, bool twoD)
{
	if (m_pathPoints.size() < 2)
		return;

	float initialLenght = GetPathLength(twoD);

	if (trimLength > 0.0f)
		trimLength = initialLenght - trimLength;
	else
		trimLength = -trimLength;

	if (trimLength > initialLenght)
		return;

	if (trimLength < 0.01f)
	{
		// Trim the whole path.
		++m_version.v;
		PathPointDescriptor pt = m_pathPoints.back();
		m_pathPoints.clear();
		m_pathPoints.push_back(pt);
		m_pathPoints.push_back(pt);
		return;
	}

	IAISystem::ENavigationType lastSafeNavType = IAISystem::NAV_UNSET;
	float runLength = 0.0;
	const TPathPoints::iterator itPathEnd = m_pathPoints.end();
	for (TPathPoints::iterator it = m_pathPoints.begin() ; it != itPathEnd ; ++it)
	{
		TPathPoints::iterator itNext = it;
		++itNext;
		if (itNext == itPathEnd)
			break;

		PathPointDescriptor &start = *it;
		PathPointDescriptor &end = *itNext;

		if (start.navType != IAISystem::NAV_SMARTOBJECT)
			lastSafeNavType = start.navType;

		float segmentLength;
		if (twoD)
			segmentLength = Distance::Point_Point2D(start.vPos, end.vPos);
		else
			segmentLength = Distance::Point_Point(start.vPos, end.vPos);

		if (trimLength >= runLength && trimLength < (runLength + segmentLength))
		{
			// If end point is navso, reset it.
			if (start.navType == IAISystem::NAV_SMARTOBJECT && end.navType == IAISystem::NAV_SMARTOBJECT)
			{
				// The cut would happen at a navso segment, allow to pass it.
				// The navso cutter will handle this case.
				return;
/*				start.navType = lastSafeNavType;
				end.navType = start.navType;
				end.pSONavData = 0;
				end.navSOMethod = nSOmNone;*/
			}
			else
			{
				if (end.navType == IAISystem::NAV_SMARTOBJECT)
				{
					end.navType = start.navType;
					end.pSONavData = 0;
					end.navSOMethod = nSOmNone;
				}

				float u = (trimLength - runLength) / segmentLength;
				end.vPos = start.vPos + u * (end.vPos - start.vPos);

				++itNext;
			}

			// Remove the end of the path.
			if (itNext != itPathEnd)
				m_pathPoints.erase(itNext, itPathEnd);

			++m_version.v;

			return;
		}

		runLength += segmentLength;
	}
}

//===================================================================
// MovePathEndsOutOfObstacles
//===================================================================
void CNavPath::MovePathEndsOutOfObstacles(const CPathObstacles &obstacles)
{
  if (m_pathPoints.empty())
    return;
  ++m_version.v;
  Vec3 &pathStart = m_pathPoints.front().vPos;
  Vec3 &pathEnd = m_pathPoints.back().vPos;
  const PathPointDescriptor &ppdStart = m_pathPoints.front();
  const PathPointDescriptor &ppdEnd = m_pathPoints.back();
  static float extra = 0.1f;
  // Danny todo actually walk/back/forward moving all points out?
  if (ppdStart.navType & (IAISystem::NAV_TRIANGULAR | IAISystem::NAV_ROAD))
    pathStart = obstacles.GetPointOutsideObstacles(pathStart, extra);
  if (ppdEnd.navType & (IAISystem::NAV_TRIANGULAR | IAISystem::NAV_ROAD))
    pathEnd = obstacles.GetPointOutsideObstacles(pathEnd, extra);
}


//===================================================================
// CheckPath
// Currently just checks against forbidden edges, but should also check
// other things (depending on the navCapMask too). radius also needs to
// get used.
//===================================================================
bool CNavPath::CheckPath(const TPathPoints &pathList, float radius) const
{
  Vec3 junk;
  for (TPathPoints::const_iterator it = pathList.begin() ; it != pathList.end() ; ++it)
  {
    TPathPoints::const_iterator itNext = it;
    if (++itNext == pathList.end())
      return true;
    Vec3 from = it->vPos;
    Vec3 to = itNext->vPos;
    if (GetAISystem()->IntersectsForbidden(from, to, junk))
      return false;
  }
  return true;
}

//===================================================================
// AdjustPathAroundObstacleShape2D
//===================================================================
bool CNavPath::AdjustPathAroundObstacleShape2D(const SPathObstacleShape2D &obstacle, IAISystem::tNavCapMask navCapMask)
{
  if (!(navCapMask & (IAISystem::NAV_TRIANGULAR | IAISystem::NAV_ROAD)))
    return true;

  if (m_pathPoints.empty())
    return true;

  const TPathPoints::iterator itPathEnd = m_pathPoints.end();
  // the path point before entering the shape
  TPathPoints::iterator itCutEntrance = itPathEnd;
  // the path point before exiting the shape
  TPathPoints::iterator itCutExit = itPathEnd;
  // the shape entrance/exit points
  Vec3 entrancePoint(ZERO);
  Vec3 exitPoint(ZERO);
  // counters to make sure we record the correct entrance/exit points
  float entranceCounter = std::numeric_limits<float>::max();
  float exitCounter = -std::numeric_limits<float>::max();
  // entrance/exit indices (and fractions) so that the entrance/exit points lie on the segment between
  // these indices and the index+1
  int entranceShapeIndex = -1;
  int exitShapeIndex = -1;
  float entranceShapeFrac = -1;
  float exitShapeFrac = -1;

  const unsigned nPts = obstacle.pts.size();

  int segCounter = 0;
  for (TPathPoints::iterator it = m_pathPoints.begin() ; it != itPathEnd ; ++it, ++segCounter)
  {
    TPathPoints::iterator itNext = it;
    ++itNext;
    if (itNext == itPathEnd)
      break;

    PathPointDescriptor &start = *it;
    PathPointDescriptor &end = *itNext;

    // Danny todo - problem if we start in tri, skip some indoor, then end in tri?
    if ( !(start.navType & (IAISystem::NAV_TRIANGULAR | IAISystem::NAV_ROAD)) || 
      !(end.navType   & (IAISystem::NAV_TRIANGULAR | IAISystem::NAV_ROAD)) )
      continue;

    Lineseg seg(start.vPos, end.vPos);
    Vec3 intersectPos;

    // if the seg intersection is for the entrance then this gets set so we know
    // whether to over-ride it
    float entranceFrac = -1.0f;

    float biggestPtsFrac = std::numeric_limits<float>::max();
    float smallestPtsFrac = -std::numeric_limits<float>::max();

    for (unsigned iPt = 0 ; iPt < nPts ; ++iPt)
    {
      unsigned iPtNext = (iPt + 1) % nPts;
      Lineseg ptsSeg(obstacle.pts[iPt], obstacle.pts[iPtNext]);

      float segFrac, ptsFrac;
      if (!Intersect::Lineseg_Lineseg2D(seg, ptsSeg, segFrac, ptsFrac))
        continue;

      if ((segCounter+segFrac) < entranceCounter)
      {
        entranceCounter = segCounter + segFrac;
        itCutEntrance = it;
        entrancePoint = seg.GetPoint(segFrac);
        entranceShapeIndex = iPt;
        entranceShapeFrac = ptsFrac;
      }
      if ((segCounter+segFrac) > exitCounter)
      {
        exitCounter = segCounter + segFrac;
        itCutExit = it;
        exitPoint = seg.GetPoint(segFrac);
        exitShapeIndex = iPt;
        exitShapeFrac = ptsFrac;
      }
    }
  }

  if (itCutEntrance == itPathEnd || itCutExit == itPathEnd)
    return true;

  ++m_version.v;

  AIAssert(entranceShapeIndex >= 0 && entranceShapeIndex < nPts);
  AIAssert(exitShapeIndex >= 0 && exitShapeIndex < nPts);

  TPathPoints::iterator itCutExitAfter = itCutExit;
  ++itCutExitAfter;
  AIAssert(itCutExitAfter != itPathEnd);
  AIAssert(exitCounter >= entranceCounter);

  // First remove any intermediate points in the path list.
  for (TPathPoints::iterator it = itCutEntrance ; it != itCutExitAfter ; )
  {
    if (it != itCutEntrance)
      it = m_pathPoints.erase(it);
    else
      ++it;
  }

  // now insert the extra points before itCutExitAfter
  IAISystem::ENavigationType navType = itCutEntrance->navType;

  float distFwd = 0.0f;
  float distBwd = 0.0f;
  TPathPoints pathFwd;
  TPathPoints pathBwd;

  pathFwd.push_back(PathPointDescriptor(navType, entrancePoint));
  pathBwd.push_back(PathPointDescriptor(navType, entrancePoint));

  // if we enter/exit on the same edge then we need to make sure that we don't cut
  // through a forbidden edge - so still calculate the two routes (but fake the distances
  // for convenience)
  if (entranceShapeIndex == exitShapeIndex) 
  {
    bool shortestRouteIsFwd = entranceShapeFrac < exitShapeFrac;
    distFwd = shortestRouteIsFwd ? 0.0f : 1.0f;
    distBwd = shortestRouteIsFwd ? 1.0f : 0.0f;

    if (shortestRouteIsFwd)
    {
      bool doingFirst = true;
      Vec3 lastPt = entrancePoint;
      for (int iPt = entranceShapeIndex ; doingFirst || iPt != exitShapeIndex ; )
      {
        doingFirst = false;
        Vec3 aheadPt = obstacle.pts[iPt];
        aheadPt.z = entrancePoint.z;
        pathBwd.push_back(PathPointDescriptor(navType, aheadPt));
        lastPt = aheadPt;
        if (--iPt < 0) 
          iPt = nPts - 1;
      }
    }
    else
    {
      bool doingFirst = true;
      Vec3 lastPt = entrancePoint;
      for (int iPt = entranceShapeIndex ; doingFirst || iPt != exitShapeIndex ; )
      {
        doingFirst = false;
        int iAheadPt = (iPt + 1) % nPts;
        Vec3 aheadPt = obstacle.pts[iAheadPt];
        aheadPt.z = entrancePoint.z;
        pathFwd.push_back(PathPointDescriptor(navType, aheadPt));
        lastPt = aheadPt;
        if (++iPt >= nPts) 
          iPt = 0;
      }
    }    
  }
  else
  {
    // Generate paths around the object in both directions to (a) choose the shortest
    // and (b) be able to choose one that is less problematic
    distFwd = (obstacle.pts[exitShapeIndex] - exitPoint).GetLength2D();
    distBwd = (obstacle.pts[(exitShapeIndex+1)%nPts] - exitPoint).GetLength2D();

    // fwd
    Vec3 lastPt = entrancePoint;
    for (int iPt = entranceShapeIndex ; iPt != exitShapeIndex ; )
    {
      int iAheadPt = (iPt + 1) % nPts;
      Vec3 aheadPt = obstacle.pts[iAheadPt];
      aheadPt.z = entrancePoint.z;
      pathFwd.push_back(PathPointDescriptor(navType, aheadPt));
      distFwd += (aheadPt - lastPt).GetLength2D();
      lastPt = aheadPt;
      if (++iPt >= nPts) 
        iPt = 0;
    }
    // bwd
    lastPt = entrancePoint;
    for (int iPt = entranceShapeIndex ; iPt != exitShapeIndex ; )
    {
      Vec3 aheadPt = obstacle.pts[iPt];
      aheadPt.z = entrancePoint.z;
      pathBwd.push_back(PathPointDescriptor(navType, aheadPt));
      distBwd += (aheadPt - lastPt).GetLength2D();
      lastPt = aheadPt;
      if (--iPt < 0) 
        iPt = nPts - 1;
    }
  }

  pathFwd.push_back(PathPointDescriptor(navType, exitPoint));
  pathBwd.push_back(PathPointDescriptor(navType, exitPoint));

  float checkRadius = 0.3f;
  if (distFwd < distBwd)
  {
    if (CheckPath(pathFwd, checkRadius))
      m_pathPoints.insert(itCutExitAfter, pathFwd.begin(), pathFwd.end());
    else if (CheckPath(pathBwd, checkRadius))
      m_pathPoints.insert(itCutExitAfter, pathBwd.begin(), pathBwd.end());
    else
      return false;
    return true;
  }
  else
  {
    if (CheckPath(pathBwd, checkRadius))
      m_pathPoints.insert(itCutExitAfter, pathBwd.begin(), pathBwd.end());
    else if (CheckPath(pathFwd, checkRadius))
      m_pathPoints.insert(itCutExitAfter, pathFwd.begin(), pathFwd.end());
    else
      return false;
    return true;
  }
}

//===================================================================
// AdjustPathAroundObstacle
//===================================================================
bool CNavPath::AdjustPathAroundObstacle(const CPathObstacle &obstacle, IAISystem::tNavCapMask navCapMask)
{
  switch (obstacle.GetType())
  {
  case CPathObstacle::ePOT_Shape2D:
    return AdjustPathAroundObstacleShape2D(obstacle.GetShape2D(), navCapMask);
  default:
    AIError("CNavPath::AdjustPathAroundObstacle unhandled type %d", obstacle.GetType());
  }
  return false;
}

//===================================================================
// AdjustPathAroundObstacles
//===================================================================
bool CNavPath::AdjustPathAroundObstacles(const CPathObstacles &obstacles, IAISystem::tNavCapMask navCapMask)
{
  MovePathEndsOutOfObstacles(obstacles);

  for (TPathObstacles::const_iterator it = obstacles.GetCombinedObstacles().begin() ; it != obstacles.GetCombinedObstacles().end() ; ++it)
  {
    const CPathObstacle &obstacle = **it;
    if (false == AdjustPathAroundObstacle(obstacle, navCapMask))
      return false;
  }
  return true;
}

//===================================================================
// GetAABB
//===================================================================
AABB CNavPath::GetAABB(float dist) const
{
  float distLeft = dist;

  AABB result(AABB::RESET);

  if (m_pathPoints.size() < 2)
    return result;

  Vec3 thisPt;
  if (false == GetPosAlongPath(thisPt, 0.0f, false, false))
    return result;

  result.Add(thisPt);

  // remaining path
  for (TPathPoints::const_iterator it = ++m_pathPoints.begin() ; it != m_pathPoints.end() ; ++it)
  {
    const PathPointDescriptor &ppd = *it;

    Vec3 delta = ppd.vPos - thisPt;
    float deltaLen = delta.NormalizeSafe();
    if (deltaLen > distLeft)
    {
      result.Add(thisPt + distLeft * delta);
      return result;
    }
    thisPt = ppd.vPos;
    result.Add(thisPt);
    distLeft -= deltaLen;
  }
  return result;
}

//===================================================================
// GetDistToPath
//===================================================================
float CNavPath::GetDistToPath(Vec3 &pathPosOut, float &distAlongPathOut, const Vec3 &pos, const float dist, const bool twoD) const
{
  float distOut = std::numeric_limits<float>::max();

  float traversedDist = 0.0f;
  const TPathPoints::const_iterator pathEndIt = m_pathPoints.end();
  for (TPathPoints::const_iterator pathIt = m_pathPoints.begin() ; pathIt != pathEndIt ; ++pathIt)
  {
    TPathPoints::const_iterator pathNextIt = pathIt;
    ++pathNextIt;
    if (pathNextIt == pathEndIt)
      break;

    Lineseg seg(pathIt->vPos, pathNextIt->vPos);

    float segLen = twoD ? Distance::Point_Point2D(seg.start, seg.end) : Distance::Point_Point(seg.start, seg.end);
    if (segLen < 0.001f)
      continue;
    if (segLen + traversedDist > dist)
    {
      float frac = (dist - traversedDist) / (segLen + traversedDist);
      seg.end = frac * seg.end + (1.0f - frac) * seg.start;
    }

    float t;
    float thisDist = twoD ? Distance::Point_Lineseg2D(pos, seg, t) : Distance::Point_Lineseg(pos, seg, t);
    if (thisDist < distOut)
    {
      distOut = thisDist;
      pathPosOut = seg.GetPoint(t);
      distAlongPathOut = traversedDist + t * segLen;
    }

    traversedDist += segLen;
    if (traversedDist > dist)
      break;
  }
  return distOut < std::numeric_limits<float>::max() ? distOut : -1.0f;
}

static float criticalMinDist = 0.5f;
static float criticalMaxDist = 4.0f;

//===================================================================
// CanTargetPointBeReached
//===================================================================
ETriState CNavPath::CanTargetPointBeReached(CTargetPointRequest &request, const class CPuppet *pPuppet, bool twoD) const
{
  request.result = eTS_false;

  // Allow to check for the trivial cases even if there is inly one point on path.
  if (m_pathPoints.empty())
    return eTS_false;

  const PathPointDescriptor &lastPathPoint = m_pathPoints.back();
  const Vec3 &curEndPt = lastPathPoint.vPos;

  request.pathID = m_version.v;
  request.itIndex = 0;
  request.itBeforeIndex = 1;
  request.splitPoint = curEndPt;

  Vec3 delta = request.targetPoint - curEndPt;
  if (twoD)
    delta.z = 0.0f;
  float dist = delta.GetLength();

  // blindly allow very small changes
  if (dist < criticalMinDist)
  {
    AILogComment("CNavPath::CanTargetPointBeReached trivial distance for %s", pPuppet->GetName());
    return request.result = eTS_true;
  }

  // forbid huge changes
  if (dist > criticalMaxDist)
  {
    AILogComment("CNavPath::CanTargetPointBeReached excessive distance for %s", pPuppet->GetName());
    return request.result = eTS_maybe;
  }

  // The remaining checks require at least one segment on the path.
  if (m_pathPoints.size() < 2)
    return eTS_false;

// this wouldn't be right to do here. it would break the entering into vehicles
// since the entry point is sometimes inside the obstacle shape of the vehicle.
//
//  const CPathObstacles &pathAdjustmentObstacles = pPuppet->GetLastPathAdjustmentObstacles();
//  if (pathAdjustmentObstacles.IsPointInsideObstacles(request.targetPoint))
//    return eTS_false;
//

  // walk back dist along the path from the end. If it's possible to walk from this point to the 
  // requested end point then cut the end of the path off

  TPathPoints::const_reverse_iterator it = m_pathPoints.rbegin();
  TPathPoints::const_reverse_iterator itBefore = it;
  float distLeft = dist;
  float lastFrac = 1.0f; // at the end this is the fraction between it and itBefore
  Vec3 candidatePt = curEndPt;
  for (++itBefore ; it != m_pathPoints.rend() && itBefore != m_pathPoints.rend() ; ++it, ++itBefore)
  {
    const Vec3 &pos = it->vPos;
    const Vec3 &posBefore = itBefore->vPos;
    float thisSegLen = twoD ? (pos - posBefore).GetLength2D() : (pos - posBefore).GetLength();
    if (thisSegLen <= 0.0001f)
      continue;
    if (thisSegLen < distLeft)
    {
      distLeft -= thisSegLen;
      candidatePt = posBefore;
    }
    else
    {
      lastFrac = distLeft / thisSegLen;
      candidatePt = lastFrac * posBefore + (1.0f - lastFrac) * pos;
      break;
    }
  }

  // already checked that there's more than 2 points, so itBefore should hit rend first
  AIAssert(it != m_pathPoints.rend());

  request.itIndex = std::distance(m_pathPoints.rbegin(), it);
  request.itBeforeIndex = std::distance(m_pathPoints.rbegin(), itBefore);
  request.splitPoint = candidatePt;

  Vec3 offset(0, 0, 1.0f); 
  if (twoD)
  {
    if (!CheckWalkability(candidatePt, request.targetPoint+Vec3(0,0,0.5f), 0.0f, false, ListPositions(), AICE_ALL, 0, 0))
    {
      AILogComment("CNavPath::CanTargetPointBeReached no 2D walkability for %s", pPuppet->GetName());
      return request.result = eTS_false;
    }
  }
  else
  {
    float radius = pPuppet->m_Parameters.m_fPassRadius;
    if (OverlapCapsule(Lineseg(candidatePt, request.targetPoint), radius, AICE_ALL))
    {
      AILogComment("CNavPath::CanTargetPointBeReached no 3D passability for %s", pPuppet->GetName());
      return request.result = eTS_false;
    }
  }

  AILogComment("CNavPath::CanTargetPointBeReached Accepting new path target point (%5.2f, %5.2f, %5.2f) for %s", 
    request.targetPoint.x, request.targetPoint.y, request.targetPoint.z, pPuppet->GetName());

  return request.result = eTS_true;
}

//===================================================================
// UseTargetPointRequest
// If we accept the request then it becomes impractical to store
// the request and apply it in subsequent path regenerations - so
// path regeneration needs to be disabled
//===================================================================
bool CNavPath::UseTargetPointRequest(const CTargetPointRequest &request, class CPuppet *pPuppet, bool twoD)
{
  if (m_pathPoints.empty())
    return false;

  PathPointDescriptor &lastPathPoint = m_pathPoints.back();
  const Vec3 &curEndPt = lastPathPoint.vPos;
  // assume that the only ppd with a pSONavData is the last one
  PathPointDescriptor::SmartObjectNavDataPtr pSONavData = lastPathPoint.pSONavData;
  IAISystem::ENavigationType lastNavType = lastPathPoint.navType;

  // We (Mikko/Danny) are not sure, but think that this case happens when the guy starts near the
  // exact position destination and he finishes his path before the exact positioning request calls
  // here. So - we need to indicate to exact positioning that we can continue - 
  if (m_pathPoints.size() == 1)
  {
    float dist = twoD ? Distance::Point_Point2D(request.targetPoint, curEndPt) : Distance::Point_Point(request.targetPoint, curEndPt);
    // blindly allow very small changes
    if (dist < criticalMinDist)
    {
      AILogComment("CNavPath::UseTargetPointRequest trivial distance for %s with path size = 1", pPuppet->GetName());
      lastPathPoint.vPos = request.targetPoint;
      m_pathPoints.push_front(PathPointDescriptor(IAISystem::NAV_UNSET, pPuppet->GetPhysicsPos()));
      return true;
    }
    else
    {
      AILogComment("CNavPath::UseTargetPointRequest excessive for %s when path size = 1", pPuppet->GetName());
      return false;
    }
  }

  CTargetPointRequest workingReq = request;
  if (request.pathID != m_version.v || request.result != eTS_true)
  {
    ETriState res = CanTargetPointBeReached(workingReq, pPuppet, twoD);
    if (res != eTS_true)
      return false;
  }

  Limit(workingReq.itIndex, 0, (int) m_pathPoints.size()-1);
  Limit(workingReq.itBeforeIndex, 0, (int) m_pathPoints.size()-1);

  TPathPoints::reverse_iterator it = m_pathPoints.rbegin();
  TPathPoints::reverse_iterator itBefore = m_pathPoints.rbegin();
  std::advance(it, workingReq.itIndex);
  std::advance(itBefore, workingReq.itBeforeIndex);

  if (itBefore == m_pathPoints.rend())
  {
    // replace the whole path with one that goes from the first point to the new point
    TPathPoints::iterator pit = m_pathPoints.begin();
    ++pit;
    m_pathPoints.erase(pit, m_pathPoints.end());
    // clone the first point and modify it.
    m_pathPoints.push_back(m_pathPoints.back());
    m_pathPoints.back().vPos = workingReq.targetPoint;
  }
  else
  {
    TPathPoints::iterator fwdIt = it.base();
    --fwdIt; // now fwdIt points to the same element as it
    // move the last point and delete the rest
    fwdIt->vPos = workingReq.splitPoint;
    ++fwdIt;
    m_pathPoints.erase(fwdIt, m_pathPoints.end());
    // clone the first point and modify it.
    m_pathPoints.push_back(m_pathPoints.back());
    m_pathPoints.back().vPos = workingReq.targetPoint;
    if (lastNavType == IAISystem::NAV_SMARTOBJECT)
      m_pathPoints.back().navType = lastNavType;
  }

  m_pathPoints.back().pSONavData = pSONavData;
  m_pathPoints.back().vPos = workingReq.targetPoint;
  m_params.inhibitPathRegeneration = true;
  m_params.continueMovingAtEnd = (lastNavType == IAISystem::NAV_SMARTOBJECT); // request.continueMovingAtEnd;
  ++m_version.v;
  return true;
}

//===================================================================
// GetDistToSmartObject
//===================================================================
float CNavPath::GetDistToSmartObject(bool twoD) const
{
  if (m_pathPoints.size() < 2)
    return std::numeric_limits<float>::max();

  TPathPoints::const_iterator pathIt = m_pathPoints.begin();
  IAISystem::ENavigationType	typeCur = pathIt->navType;
  ++pathIt;
  IAISystem::ENavigationType	typeNext = pathIt->navType;

  if (typeCur == IAISystem::NAV_SMARTOBJECT && typeNext == IAISystem::NAV_SMARTOBJECT)
    return 0.0f;

  Vec3 curPos;
  GetPosAlongPath(curPos, 0.0f, twoD, false);

  float dist = 0.0f;
  const TPathPoints::const_iterator pathEndIt = m_pathPoints.end();
  for (pathIt ; pathIt != pathEndIt ; ++pathIt)
  {
    Vec3 thisPos = pathIt->vPos;
    dist += twoD ? Distance::Point_Point2D(curPos, pathIt->vPos) : Distance::Point_Point(curPos, pathIt->vPos);

    TPathPoints::const_iterator pathItNext = pathIt;
    ++pathItNext;
    if(pathItNext != pathEndIt)
    {
      if (pathIt->navType == IAISystem::NAV_SMARTOBJECT && pathItNext->navType == IAISystem::NAV_SMARTOBJECT)
        return dist;
    }
    else
    {
      // End of the path
      if (pathIt->navType == IAISystem::NAV_SMARTOBJECT)
        return dist;
    }

    curPos = pathIt->vPos;
  }
  return std::numeric_limits<float>::max();
}

//===================================================================
// GetLastPathPointAnimNavSOData
//===================================================================
PathPointDescriptor::SmartObjectNavDataPtr CNavPath::GetLastPathPointAnimNavSOData() const
{
  if(m_pathPoints.empty() || m_pathPoints.back().navType != IAISystem::NAV_SMARTOBJECT)
    return 0;
  const PathPointDescriptor&	lastPoint = m_pathPoints.back();
  if(lastPoint.navType != IAISystem::NAV_SMARTOBJECT)
    return 0;
  if(lastPoint.navSOMethod == nSOmSignalAnimation || lastPoint.navSOMethod == nSOmActionAnimation)
    return lastPoint.pSONavData;
  return 0;
}

//===================================================================
// Serialize
//===================================================================
void CNavPath::SVersion::Serialize(TSerialize ser)
{
  ser.Value("version", v);
}

