ryzom/server/src/ai_service/path_behaviors.cpp

   1 // Ryzom - MMORPG Framework <http://dev.ryzom.com/projects/ryzom/>
   2 // Copyright (C) 2010  Winch Gate Property Limited
   3 //
   4 // This program is free software: you can redistribute it and/or modify
   5 // it under the terms of the GNU Affero General Public License as
   6 // published by the Free Software Foundation, either version 3 of the
   7 // License, or (at your option) any later version.
   8 //
   9 // This program is distributed in the hope that it will be useful,
  10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 // GNU Affero General Public License for more details.
  13 //
  14 // You should have received a copy of the GNU Affero General Public License
  15 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
  16
  17 #include "stdpch.h"
  18 #include "path_behaviors.h"
  19 #include "ais_actions.h"
  20 #include "continent.h"
  21 #include <typeinfo>
  22
  23 extern bool simulateBug(int bugId);
  24
  25 using namespace RYAI_MAP_CRUNCH;
  26 using namespace CAISActionEnums;
  27
  28
  29 NLMISC::CVariable<bool> ActivateStraightRepulsion("ai", "ActivateStraightRepulsion", "Activate the straight repulsion for follow route (only available with Ring shards for moment).", true, 0, true);
  30 NLMISC::CVariable<bool> LogTimeConsumingAStar("ai", "LogTimeConsumingAStar", "Activate logging of time consuming path finding operations", false, 0, true);
  31
  32
  33 //////////////////////////////////////////////////////////////////////////////
  34 // Actions used when parsing primitives                                     //
  35 //////////////////////////////////////////////////////////////////////////////
  36
  37 DEFINE_ACTION(ContextGlobal,SETNOGO)
  38 {
  39         // get hold of the parameters and check their validity
  40         float x,y;
  41         if (!getArgs(args,name(),x,y))
  42                 return;
  43
  44         const   RYAI_MAP_CRUNCH::CMapPosition   pos(CAIVector   (x,y));
  45
  46         const RYAI_MAP_CRUNCH::CWorldMap        &worldMap=CWorldContainer::getWorldMap(/*0*/);
  47         const CSuperCell        *superCell=worldMap.getSuperCellCst(pos);
  48
  49         if (!superCell)
  50         {
  51                 nlwarning("Unable to set flags at this position %.3f %.3f",x,y);
  52                 return;
  53         }
  54 }
  55
  56 DEFINE_ACTION(ContextGlobal,SAFEZONE)
  57 {
  58         // get hold of the parameters and check their validity
  59         float x,y,r;
  60
  61         if (!getArgs(args,name(),x,y,r))
  62                 return;
  63
  64         const   RYAI_MAP_CRUNCH::CMapPosition   pos(CAIVector   (x,y));
  65         CWorldContainer::getWorldMapNoCst().setFlagOnPosAndRadius(pos,r,1);
  66 }
  67
  68 //////////////////////////////////////////////////////////////////////////////
  69 // CFollowPathContext                                                       //
  70 //////////////////////////////////////////////////////////////////////////////
  71
  72 CFollowPathContext::CFollowPathContext(const char* contextName, uint32 maxSearchDepth, bool forceMaxDepth)
  73 {
  74         if (contextName==NULL)
  75         {
  76 //              nldebug("CFollowPathContext: NULL : first init");
  77                 _PrevContext= NULL;
  78                 _ContextName= "TopOfStack";
  79                 _MaxSearchDepth= maxSearchDepth;
  80                 _NextContext= NULL;
  81         }
  82         else
  83         {
  84 //              nldebug("CFollowPathContext: %s",contextName);
  85                 _PrevContext= CFollowPath::getInstance()->_TopFollowPathContext;
  86                 nlassert(_PrevContext!=NULL);
  87                 CFollowPath::getInstance()->_TopFollowPathContext= this;
  88                 _PrevContext->_NextContext= this;
  89                 _ContextName= contextName;
  90                 _MaxSearchDepth= (forceMaxDepth)? maxSearchDepth: std::min(maxSearchDepth,_PrevContext->_MaxSearchDepth);
  91                 _NextContext= NULL;
  92         }
  93 }
  94
  95 CFollowPathContext::~CFollowPathContext()
  96 {
  97         // if we're not the bottom of stack then fixup the previous stack entry
  98         if (_PrevContext!=NULL)
  99         {
 100                 _PrevContext->_NextContext= _NextContext;
 101         }
 102
 103         // if we're not at the top of stack then fixup the next stack entry
 104         // otherwise fixup the stack top pointer in the CFollowPath singleton
 105         if (_NextContext!=NULL)
 106         {
 107                 _NextContext->_PrevContext= _PrevContext;
 108         }
 109         else
 110         {
 111                 CFollowPath::getInstance()->_TopFollowPathContext= _PrevContext;
 112         }
 113 }
 114
 115 void CFollowPathContext::buildContextName(std::string &result) const
 116 {
 117         // concatenate previous stack entries
 118         if (_PrevContext!=NULL)
 119         {
 120                 _PrevContext->buildContextName(result);
 121                 result+=':';
 122         }
 123         else
 124         {
 125                 result.clear();
 126         }
 127
 128         // add our name to the result
 129         result+= _ContextName;
 130 }
 131
 132
 133 //////////////////////////////////////////////////////////////////////////////
 134 // CFollowPath                                                              //
 135 //////////////////////////////////////////////////////////////////////////////
 136
 137 CFollowPath* CFollowPath::_Instance = NULL;
 138
 139 CFollowPath* CFollowPath::getInstance()
 140 {
 141         if (_Instance==NULL)
 142         {
 143                 _Instance = new CFollowPath;
 144         }
 145         return _Instance;
 146 }
 147
 148 void CFollowPath::destroyInstance()
 149 {
 150         if (_Instance!=NULL)
 151         {
 152                 delete _Instance;
 153                 _Instance = NULL;
 154         }
 155 }
 156
 157 CFollowPath::CFollowPath()
 158 : _LastReason(NO_REASON)
 159 , _LastFIASPReason(CWorldMap::FIASPR_NO_REASON)
 160 {
 161         // initialise the _TopFollowPathContext stack
 162         _TopFollowPathContext= new CFollowPathContext(NULL);
 163 }
 164
 165 NLMISC::CVariable<uint> AStarNbStepsLogThreshold( "ais", "AStarNbStepsLogThreshold", "", 1000, 0, true );
 166 namespace RYAI_MAP_CRUNCH
 167 {
 168         extern uint LastAStarNbSteps;
 169 }
 170
 171 inline void logTimeConsumingAStar(CModEntityPhysical* bot, float dist, CPathCont& pathCont, CAIVector* realDestination )
 172 {
 173         if (!LogTimeConsumingAStar.get())
 174                 return;
 175
 176         if (RYAI_MAP_CRUNCH::LastAStarNbSteps >= AStarNbStepsLogThreshold.get() )
 177         {
 178                 nlinfo( "ASTAR: Bot %s (type %s) used %u steps src=%s dest=%s dist=%g a*flag=%s context=%s",
 179                         bot ? bot->getPersistent().getOneLineInfoString().c_str() : "-",
 180                         bot ? typeid(*bot).name() : "-",
 181                         RYAI_MAP_CRUNCH::LastAStarNbSteps,
 182                         bot ? bot->pos().toString().c_str() : "-",
 183                         realDestination ? realDestination->toString().c_str() : pathCont.getDestination().toString().c_str(),
 184                         dist,
 185                         bot ? RYAI_MAP_CRUNCH::toString(bot->getAStarFlag()).c_str() : "-",
 186                         CFollowPath::getInstance()->getContextName() );
 187         }
 188 }
 189
 190 NLMISC::CMustConsume<CFollowPath::TFollowStatus> CFollowPath::followPath(
 191                 CModEntityPhysical* bot,
 192                 CPathPosition& pathPos,
 193                 CPathCont& pathCont,
 194                 float dist,
 195                 float modecalageDist,
 196                 float angleAmort,
 197                 bool focusOnTargetDirection,
 198                 CAIVector* realDestination,
 199                 float repulsSpeed,
 200                 bool updateOrient)
 201 {
 202         H_AUTO(followPath);
 203
 204         if      (!bot->canMove())
 205                 return FOLLOW_BLOCKED;
 206
 207         TFollowStatus returnStatus = FOLLOWING;
 208         CAngle motionAngle;
 209         bool isMotionAngleComputed = false;
 210         CAIVector add;
 211         CAIVector addCorrection(0.0,0.0);
 212         CWorldMap const& worldMap = CWorldContainer::getWorldMap();
 213
 214         CAIPos const startPos = CAIPos(bot->pos());
 215 #ifdef NL_DEBUG
 216         nlassert(angleAmort>=0.f && angleAmort<=1.f);
 217 #endif
 218         if (!bot->wpos().isValid())
 219                 return FOLLOWING;
 220
 221         bool haveRestart = false;
 222
 223         switch (pathPos._PathState)
 224         {
 225                 case CPathPosition::NOT_INITIALIZED:
 226                 {
 227                         pathPos._Angle=bot->theta();
 228
 229 reStartFollowTopo:
 230                         if      (!pathCont.getCalcPathForSource (pathPos,bot->wpos()))
 231                         {
 232                                 pathPos._PathState = CPathPosition::NOT_INITIALIZED;
 233                                 returnStatus = FOLLOW_NO_PATH;
 234                                 _LastReason = FNP_NOT_INITIALIZED;
 235                                 break;
 236                         }
 237                         logTimeConsumingAStar(bot, dist, pathCont, realDestination);
 238                         pathPos._PathState = CPathPosition::FOLLOWING_TOPO;
 239                 }
 240                 // No break
 241
 242                 case CPathPosition::FOLLOWING_TOPO:
 243                 {
 244                         if (pathCont._TimeTopoChanged!=pathPos._TimeTopoChanged)
 245                         {
 246                                 goto reStartFollowTopo;
 247                         }
 248
 249                         // If we have changed our current topology, we have to take the next furthest topo in the same 'cell'.
 250                         CTopology::TTopologyRef botTopology(bot->wpos().getTopologyRef());
 251                         while ( pathPos.haveNextTopology(2)
 252                                 &&      botTopology==pathPos.getNextTopology())
 253                         {
 254                                 pathPos.nextTopology();
 255                         }
 256
 257                         if (!pathPos.isFinished())
 258                         {
 259                                 CGridDirectionLayer const* layer = worldMap.getGridDirectionLayer(bot->wpos(),pathPos.getNextTopology());
 260
 261                                 if (!layer) // We are now to far to have a layer to lead us to the next topo.
 262                                 {
 263                                         goto reStartFollowTopo;
 264                                 }
 265
 266                                 CDirection motion = layer->getDirection(bot->wpos());
 267                                 if (motion.isValid())
 268                                 {
 269                                         motionAngle = motion.getAngle();
 270                                         isMotionAngleComputed=true;
 271                                         break;
 272                                 }
 273                         }
 274                         pathPos._PathState = CPathPosition::FOLLOWING_INSIDE_TOPO;
 275                 }
 276                 // No break
 277
 278                 case CPathPosition::FOLLOWING_INSIDE_TOPO:
 279                 {
 280                         if (pathCont._TimeTopoChanged!=pathPos._TimeTopoChanged)
 281                         {
 282                                 goto reStartFollowTopo;
 283                         }
 284
 285                         if (pathCont._TimeDestChanged!=pathPos._TimeDestChanged)
 286                         {
 287                                 pathPos._InsidePath=NULL;
 288                         }
 289
 290                         if (pathPos._InsidePath.isNull())
 291                         {
 292                                 CInsidePath* insidePath = new CInsidePath();
 293                                 pathPos._InsidePath = insidePath;
 294
 295                                 if (!worldMap.findInsideAStarPath(bot->wpos(), pathCont.getDestPos(), insidePath->_DirectionPath, pathCont.denyFlags()))
 296                                 {
 297                                         // If findInsideAStarPath returned false we have a topology change
 298                                         pathPos._InsidePath=NULL;
 299
 300                                 #ifdef NL_DEBUG
 301                                         nlassert(!haveRestart);
 302                                 #endif
 303                                         if (!haveRestart)
 304                                         {
 305                                                 haveRestart = true;
 306                                                 goto reStartFollowTopo;
 307                                         }
 308                                         returnStatus = FOLLOW_NO_PATH;
 309                                         _LastReason = FNP_NO_INSIDE_ASTAR_PATH;
 310                                         _LastFIASPReason = worldMap._LastFIASPReason;
 311                                         break;
 312                                 }
 313
 314                                 pathPos._InsideIndex = 0;
 315
 316                                 pathPos._TimeDestChanged = pathCont._TimeDestChanged;
 317                                 pathPos._TimeTopoChanged = pathCont._TimeTopoChanged;
 318
 319                                 // If we reached path end
 320                                 if (pathPos._InsideIndex >= insidePath->_DirectionPath.size())
 321                                 {
 322                                         returnStatus = FOLLOW_ARRIVED;
 323                                         pathPos._InsidePath = NULL;
 324                                 }
 325                                 else
 326                                 {
 327                                         pathPos._InsideStartPos = bot->wpos();
 328                                         CDirection      direction = insidePath->_DirectionPath[pathPos._InsideIndex];
 329                                         pathPos._InsideDestPos = pathPos._InsideStartPos.step(direction.dx(), direction.dy());
 330                                 }
 331                         }
 332
 333                         if (!pathPos._InsidePath.isNull())
 334                         {
 335                                 CInsidePath* insidePath = pathPos._InsidePath;
 336                                 CMapPosition botWpos = bot->wpos();
 337
 338                                 while (pathPos._InsideIndex < insidePath->_DirectionPath.size())
 339                                 {
 340                                         if (botWpos == pathPos._InsideStartPos)
 341                                                 break;
 342
 343                                         if (botWpos == pathPos._InsideDestPos)
 344                                         {
 345                                                 ++pathPos._InsideIndex;
 346                                                 if (pathPos._InsideIndex >= insidePath->_DirectionPath.size())
 347                                                 {
 348                                                         returnStatus = FOLLOW_ARRIVED;
 349                                                         pathPos._InsidePath = NULL;
 350                                                 }
 351                                                 else
 352                                                 {
 353                                                         pathPos._InsideStartPos = botWpos;
 354                                                         CDirection      direction = insidePath->_DirectionPath[pathPos._InsideIndex];
 355                                                         pathPos._InsideDestPos = pathPos._InsideStartPos.step(direction.dx(), direction.dy());
 356                                                 }
 357                                                 break;
 358                                         }
 359                                         else
 360                                         {
 361                                                 pathPos._InsideIndex++;
 362                                         }
 363                                 }
 364
 365                                 if (returnStatus!=FOLLOW_ARRIVED && pathPos._InsideIndex >= insidePath->_DirectionPath.size())
 366                                 {
 367                                         pathPos._InsidePath = NULL;
 368                                 }
 369                                 else
 370                                 {
 371                                         if (returnStatus!=FOLLOW_ARRIVED)
 372                                         {
 373                                                 motionAngle = pathPos._InsidePath->_DirectionPath[pathPos._InsideIndex].getAngle();
 374                                                 isMotionAngleComputed = true;
 375                                         }
 376                                         else
 377                                         {
 378                                                 pathPos._PathState = CPathPosition::REACH_END;
 379                                         }
 380                                 }
 381                         }
 382                 }
 383                 break;
 384
 385                 case CPathPosition::REACH_END:
 386                 {
 387                         if (pathCont._TimeDestChanged!=pathPos._TimeDestChanged)
 388                         {
 389                                 goto reStartFollowTopo;
 390                         }
 391                         else
 392                         {
 393                                 returnStatus=FOLLOW_ARRIVED;
 394                         }
 395                 }
 396                 break;
 397
 398                 case CPathPosition::NO_PATH:
 399                 break;
 400
 401                 default:
 402                 break;
 403         }
 404
 405         if (isMotionAngleComputed)
 406         {
 407                 CAngle thisAngle = pathPos._Angle;
 408
 409                 CAIVector deltaToDest = (realDestination!=NULL)?*realDestination:pathCont.getDestination();
 410                 deltaToDest -= CAIVector(bot->pos());
 411
 412                 CAngle idealAngle = deltaToDest.asAngle();
 413
 414                 sint32 absDeltaIdealAngle = abs((sint16)(idealAngle.asRawSint16()-motionAngle.asRawSint16()));
 415                 if (absDeltaIdealAngle>=32768)
 416                 {
 417                         absDeltaIdealAngle = abs((sint16)absDeltaIdealAngle-65536);
 418                 }
 419
 420                 bool idealIsValid = true;
 421
 422                 if (absDeltaIdealAngle>(32768*67.5/180))        // /*97*/ if a difference of slightly more than 95 degrees.
 423                 {
 424                         idealIsValid = false;
 425                 }
 426                 else
 427                 {
 428                         //////////////////////////////////////////////////////////////////////////
 429                         // verify that the direction is valid in the map at our position (very slow, to be optimized).
 430                         CDirection      dir0, dir1;
 431                         CDirection::getDirectionAround(idealAngle, dir0, dir1);
 432                         CWorldPosition  testPos(bot->wpos());
 433                         dir1.addStep(CDirection::HALF_TURN_RIGHT);
 434                         idealIsValid=worldMap.customCheckDiagMove(testPos, dir1, pathCont.denyFlags());
 435                 }
 436
 437                 if (idealIsValid)
 438                 {
 439                         motionAngle = idealAngle;
 440                 }
 441
 442                 {
 443                         sint16 deltaAngle = motionAngle.asRawSint16()-thisAngle.asRawSint16();
 444                         sint32 absDeltaAngle = abs(deltaAngle);
 445                         // Take the smallest angle must be add in CAngle.
 446                         if (absDeltaAngle >= 32768)
 447                         {
 448                                 if (deltaAngle > 0)
 449                                         deltaAngle = (sint16)deltaAngle-65536;
 450                                 else
 451                                         deltaAngle = (sint16)65536-deltaAngle;
 452                         }
 453
 454                         // Only if significative.
 455                         if (absDeltaAngle>32)
 456                         {
 457                                 deltaAngle = (sint16)((float)deltaAngle*angleAmort);
 458                                 thisAngle += deltaAngle;
 459                         }
 460                         else
 461                         {
 462                                 thisAngle = motionAngle;
 463                         }
 464                 }
 465
 466                 pathPos._Angle = thisAngle;
 467
 468                 // Verify that dist is not too high !
 469                 {
 470                         float reachDist = (float)(bot->pos().quickDistTo(pathCont.getDestination())*0.5+0.1);
 471                         if (dist > reachDist)
 472                                 dist = reachDist;
 473                 }
 474
 475                 add = CAIVector(thisAngle.asVector2d()*dist);
 476                 if      (idealIsValid)
 477                 {
 478                         deltaToDest.normalize(dist*1000);
 479                         addCorrection = deltaToDest-add;
 480                 }
 481         }
 482
 483         CAIVector moveDecalage=bot->moveDecalage();
 484         bot->resetDecalage();
 485         moveDecalage.normalize(modecalageDist*1000);
 486
 487         CAIVector rAdd;
 488         if (simulateBug(8))
 489         {
 490                 rAdd = add;
 491                 if (IsRingShard.get() && ActivateStraightRepulsion.get())
 492                 {
 493                         // straight repulsion
 494                         CAIVector repulsion;
 495                         if (bot->calcStraightRepulsion(startPos+rAdd, repulsion))
 496                         {
 497                                 rAdd += moveDecalage;
 498                                 rAdd += repulsion;
 499                         }
 500                         else
 501                         {
 502                                 return FOLLOW_BLOCKED;
 503                         }
 504                 }
 505                 else
 506                 {
 507                         // repulsion
 508                         CAIVector repulsion = bot->calcRepulsion(startPos+rAdd);
 509                         rAdd += moveDecalage;
 510                         repulsion.normalize((float)(std::max((float)rAdd.norm(),repulsSpeed)*710));     // minimum of 0.25m/s.
 511                         rAdd += repulsion;
 512                 }
 513         }
 514         else
 515         {
 516                 rAdd = add + moveDecalage;
 517                 if (IsRingShard.get() && ActivateStraightRepulsion.get())
 518                 {
 519                         // straight repulsion
 520                         CAIVector repulsion;
 521                         if (bot->calcStraightRepulsion(startPos+rAdd, repulsion))
 522                         {
 523                                 rAdd += repulsion;
 524                         }
 525                         else
 526                         {
 527                                 return FOLLOW_BLOCKED;
 528                         }
 529                 }
 530                 else
 531                 {
 532                         // repulsion
 533                         CAIVector repulsion = bot->calcRepulsion(startPos+rAdd);
 534                         repulsion.normalize((float)(std::max((float)rAdd.norm(),repulsSpeed)*710));     // minimum of 0.25m/s.
 535                         rAdd += repulsion;
 536                 }
 537         }
 538
 539         if (!rAdd.isNull() && !bot->moveBy(rAdd, pathCont.denyFlags()))
 540         {
 541                 if (rAdd==add || !bot->moveBy(add, pathCont.denyFlags())) // Try without correction / repulsion, etc .. (if there's any).
 542                 {
 543                         if (!isMotionAngleComputed || pathPos._Angle==motionAngle)
 544                         {
 545                                 // try a 1 meter step
 546                                 add = add.normalize()*1000;
 547                                 if (!bot->moveBy(add, pathCont.denyFlags()))
 548                                 return FOLLOW_BLOCKED;
 549                         }
 550
 551                         if (isMotionAngleComputed)
 552                         {
 553                                 pathPos._Angle = motionAngle;
 554                                 if (updateOrient)
 555                                         bot->setTheta(motionAngle);
 556                         }
 557                         return  returnStatus;
 558                 }
 559                 else
 560                 {
 561                         if (updateOrient)
 562                                 bot->setTheta(pathPos._Angle);
 563                 }
 564         }
 565         else
 566         {
 567                 if (updateOrient && !rAdd.isNull())
 568                         bot->setTheta(rAdd.asAngle());
 569         }
 570         return returnStatus;
 571 }
 572
 573 const char* CFollowPath::getContextName() const
 574 {
 575         static std::string name;
 576
 577         // the following should never happen but it's quite unimportant
 578         if (_TopFollowPathContext==NULL)
 579                 return "<no context stack>";
 580
 581         // delegate to the top context to build the contextname
 582         _TopFollowPathContext->buildContextName(name);
 583
 584         // we're allowed to return this value because 'name' is a static
 585         return name.c_str();
 586 }
 587
 588
 589 //////////////////////////////////////////////////////////////////////////////
 590 // CPathCont                                                                //
 591 //////////////////////////////////////////////////////////////////////////////
 592
 593 CPathCont::CPathCont(RYAI_MAP_CRUNCH::TAStarFlag denyFlags)
 594 : _TimeTopoChanged(~0)
 595 , _TimeDestChanged(~0)
 596 , _denyFlags(denyFlags)
 597 {
 598 }
 599
 600 CPathCont::CPathCont(CPathCont const& pathCont)
 601 : _denyFlags(pathCont._denyFlags)
 602 {
 603 }
 604
 605 void CPathCont::clearPaths()
 606 {
 607         _SourcePaths.clear();
 608 }
 609
 610 void CPathCont::setDestination(RYAI_MAP_CRUNCH::CWorldPosition const& destPos)
 611 {
 612         RYAI_MAP_CRUNCH::CMapPosition mapPos(destPos);
 613
 614         if (_DestinationMapPos==mapPos)
 615                 return;
 616
 617         _DestinationMapPos = mapPos;
 618         _Destination = destPos;
 619         _VerticalPos = AITYPES::vp_auto;
 620         uint32 gameCycle = CTimeInterface::gameCycle();
 621
 622         RYAI_MAP_CRUNCH::CWorldPosition tmpPos = destPos;
 623
 624         if (tmpPos!=_DestPos)
 625         {
 626                 _DestPos = tmpPos;
 627                 _TimeDestChanged = gameCycle;
 628
 629                 if (!_DestPos.isValid())
 630                 {
 631                         clearPaths();
 632                 }
 633                 else
 634                 {
 635                         RYAI_MAP_CRUNCH::CTopology::TTopologyRef newTopo = _DestPos.getTopologyRef();
 636                         if (((RYAI_MAP_CRUNCH::CTopology::TTopologyId)newTopo)!=_TopoRef )
 637                         {
 638                                 _TimeTopoChanged = gameCycle;
 639                                 clearPaths();
 640                                 _TopoRef = newTopo;
 641                         }
 642                 }
 643         }
 644 }
 645
 646 void CPathCont::setDestination(AITYPES::TVerticalPos verticalPos, CAIVector const& destPos)
 647 {
 648         RYAI_MAP_CRUNCH::CMapPosition mapPos(destPos);
 649
 650         if (_DestinationMapPos==mapPos)
 651                 return;
 652
 653         _DestinationMapPos = mapPos;
 654         _Destination = destPos;
 655         _VerticalPos = verticalPos;
 656         uint32 gameCycle = CTimeInterface::gameCycle();
 657
 658         RYAI_MAP_CRUNCH::CWorldPosition tmpPos;
 659         CWorldContainer::getWorldMap().setWorldPosition(verticalPos, tmpPos,destPos);
 660
 661         if (tmpPos!=_DestPos)
 662         {
 663                 _DestPos = tmpPos;
 664                 _TimeDestChanged = gameCycle;
 665
 666                 if (!_DestPos.isValid())
 667                 {
 668                         clearPaths      ();
 669                 }
 670                 else
 671                 {
 672                         RYAI_MAP_CRUNCH::CTopology::TTopologyRef newTopo = _DestPos.getTopologyRef();
 673                         if (((RYAI_MAP_CRUNCH::CTopology::TTopologyId)newTopo)!=_TopoRef )
 674                         {
 675                                 _TimeTopoChanged = gameCycle;
 676                                 clearPaths();
 677                                 _TopoRef = newTopo;
 678                         }
 679                 }
 680         }
 681 }
 682
 683 bool CPathCont::getPathForSource(CPathPosition& pathPos, RYAI_MAP_CRUNCH::CWorldPosition const& startPos) const
 684 {
 685         H_AUTO(getPathForSource);
 686         RYAI_MAP_CRUNCH::CTopology::TTopologyRef const startTopo = startPos.getTopologyRef();
 687
 688         std::vector<NLMISC::CSmartPtr<CAIPath> >::const_iterator it = _SourcePaths.begin();
 689         const std::vector<NLMISC::CSmartPtr<CAIPath> >::const_iterator itEnd = _SourcePaths.end();
 690
 691         while (it!=itEnd)
 692         {
 693                 CAIPath *path=*(it);
 694
 695                 std::vector<RYAI_MAP_CRUNCH::CTopology::TTopologyRef>::const_iterator const topoItBegin=path->topologiesPath().begin();
 696                 std::vector<RYAI_MAP_CRUNCH::CTopology::TTopologyRef>::const_iterator const topoItEnd=path->topologiesPath().end();
 697                 std::vector<RYAI_MAP_CRUNCH::CTopology::TTopologyRef>::const_iterator const topoItFind = std::find(topoItBegin,topoItEnd,startTopo);
 698
 699                 if (topoItFind!=topoItEnd)
 700                 {
 701                         pathPos._Index = (uint)(topoItFind-topoItBegin);
 702                         pathPos._Path = *it;
 703                         return true;
 704                 }
 705                 ++it;
 706         }
 707         pathPos._Path = NULL;
 708         return false; // No path found.
 709 }
 710
 711 bool CPathCont::calcPathForSource(CPathPosition& pathPos, RYAI_MAP_CRUNCH::CWorldPosition const& startPos)
 712 {
 713         H_AUTO(calcPathForSource);
 714
 715         RYAI_MAP_CRUNCH::CWorldMap const& worldMap = CWorldContainer::getWorldMap();
 716
 717         {
 718                 // Get a free path.
 719                 std::vector<NLMISC::CSmartPtr<CAIPath> >::iterator it, itEnd = _SourcePaths.end();
 720                 for (it = _SourcePaths.begin(); it!=itEnd; ++it)
 721                         if ((*it)->getRefCount()==1) // one refcount is reserved for the current list.
 722                                 break;
 723
 724                 // If we didn't found a free path we have to extend the vector..
 725                 if (it == itEnd)
 726                 {
 727                         _SourcePaths.push_back(new CAIPath());
 728                         pathPos._Path = _SourcePaths.back();
 729                 }
 730                 else
 731                 {
 732                         pathPos._Path = (*it);
 733                 }
 734         }
 735
 736         pathPos._Index = 0;     //      current topology index.
 737
 738         CAIPath& pathRef = *(pathPos._Path);
 739
 740         // Check start position validity
 741         if (!startPos.isValid())
 742         {
 743                 RYAI_MAP_CRUNCH::CWorldPosition newpos;
 744                 CWorldContainer::calcNearestWPosFromPosAnRadius(_VerticalPos, newpos, startPos, 6, 100, CWorldContainer::CPosValidatorDefault());
 745         #ifdef NL_DEBUG
 746                 if (newpos.isValid() && LogAcceptablePos)
 747                                 nlwarning("Path pos error at position %s: an acceptable position could be %s", startPos.toString().c_str(), newpos.toString().c_str() );
 748                 if (!newpos.isValid())
 749                         nlwarning("Path pos error at position %s: no acceptable position found around", startPos.toString().c_str());
 750         #endif
 751                 pathRef.setStartPos(newpos);
 752         }
 753         else
 754         {
 755                 pathRef.setStartPos(startPos);
 756         }
 757
 758         // Check dest position validity
 759         if (!_DestPos.isValid())
 760         {
 761                 RYAI_MAP_CRUNCH::CWorldPosition newpos;
 762                 CWorldContainer::calcNearestWPosFromPosAnRadius(_VerticalPos, newpos, _DestPos, 6, 100, CWorldContainer::CPosValidatorDefault());
 763         #ifdef NL_DEBUG
 764                 if (newpos.isValid() && LogAcceptablePos)
 765                         nlwarning("Path pos error at position %s, an acceptable position could be %s", _DestPos.toString().c_str(), newpos.toString().c_str() );
 766                 if (!newpos.isValid())
 767                         nlwarning("Path pos error at position %s, no acceptable position found around", _DestPos.toString().c_str());
 768         #endif
 769                 _DestPos = newpos;
 770         }
 771
 772         pathRef.setEndPos(_DestPos);
 773
 774         if (!worldMap.findAStarPath(pathRef.getStartPos(), _DestPos, pathRef.topologiesPathForCalc(), _denyFlags))
 775         {
 776                 pathPos._Path = NULL;
 777                 return false;
 778         }
 779         return true;
 780 }
 781
 782 bool CPathCont::getCalcPathForSource(CPathPosition& pathPos, RYAI_MAP_CRUNCH::CWorldPosition const& startPos)
 783 {
 784         H_AUTO(GetCalcPathForSource);
 785         static uint32 getCount = 0;
 786         static uint32 calcCount = 0;
 787         pathPos._TimeDestChanged = _TimeDestChanged;
 788         pathPos._TimeTopoChanged = _TimeTopoChanged;
 789
 790         if (getPathForSource(pathPos, startPos))
 791         {
 792                 ++getCount;
 793                 return true;
 794         }
 795         if (calcPathForSource(pathPos, startPos))
 796         {
 797                 ++calcCount;
 798                 return true;
 799         }
 800         return false;
 801 }
 802
 803 //////////////////////////////////////////////////////////////////////////////
 804 // CPathPosition                                                            //
 805 //////////////////////////////////////////////////////////////////////////////
 806
 807 CPathPosition::CPathPosition(CAngle const& angle)
 808 : _Angle(angle)
 809 , _Path(NULL)
 810 , _PathState(NOT_INITIALIZED)
 811 {
 812 }
 813
 814 bool CPathPosition::isPathValid() const
 815 {
 816         return !_Path.isNull();
 817 }
 818
 819 RYAI_MAP_CRUNCH::CTopology::TTopologyRef const& CPathPosition::getTopology() const
 820 {
 821 #ifdef NL_DEBUG
 822         nlassert(_Index<_Path->topologiesPath().size());
 823 #endif
 824         return _Path->topologiesPath()[_Index];
 825 }
 826
 827 RYAI_MAP_CRUNCH::CTopology::TTopologyRef const& CPathPosition::getNextTopology() const
 828 {
 829 #ifdef NL_DEBUG
 830         nlassert((_Index+1)<_Path->topologiesPath().size());
 831 #endif
 832         return _Path->topologiesPath()[_Index+1];
 833 }
 834
 835 bool CPathPosition::isFinished() const
 836 {
 837         uint32 size = (uint32)_Path->topologiesPath().size();
 838         return (size==0 || _Index==size-1);
 839 }
 840
 841 bool CPathPosition::nextTopology()
 842 {
 843         ++_Index;
 844         return _Index<_Path->topologiesPath().size();
 845 }
 846
 847 bool CPathPosition::haveNextTopology(uint nbTopo)
 848 {
 849         return (_Index+nbTopo)<_Path->topologiesPath().size();
 850 }
 851
 852 //////////////////////////////////////////////////////////////////////////////
 853 //////////////////////////////////////////////////////////////////////////////
 854 //////////////////////////////////////////////////////////////////////////////
 855