ryzom/tools/reynolds/track.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
  18
  19 #include "stdpch.h"
  20
  21 #include "track.h"
  22 #include "reynolds_manager.h"
  23
  24 using namespace std;
  25 using namespace NLMISC;
  26 using namespace NLPACS;
  27
  28
  29 // ------------------------------------
  30 // Attraction/Repulsion functions
  31
  32 // strong repulsion limited
  33 inline double   softPointing(double dist, double strength, double amp = 1.0)
  34 {
  35         return amp*atan(strength*dist)/1.5707963268;
  36 }
  37
  38 // strong repulsion limited
  39 inline double   softTargetting(double dist, double moderateDist, double strength, double amp = 1.0)
  40 {
  41         return amp*( atan(strength*(dist-moderateDist-0.2)) + atan(strength*(dist-moderateDist+0.2)) )/3.1415927;
  42 }
  43
  44 // strong repulsion limited
  45 inline double   softRepulse(double dist, double moderateDist, double strength, double amp = 1.0)
  46 {
  47         return amp*(0.5 - atan(strength*(dist-moderateDist)-1.0)/3.1415927);
  48 }
  49
  50
  51
  52 // Required target spacing
  53 double                  CTrack::TargetSpacing = 0.5;
  54
  55 // Target attraction strength
  56 double                  CTrack::TargetAttraction = 3.0;
  57
  58 // Target attraction amplification
  59 double                  CTrack::TargetAmp = 2.0;
  60
  61 // Fast obstacle exclusion distance
  62 double                  CTrack::ObstacleExcludeDistance = 6.0;
  63
  64 // Required obstacle spacing
  65 double                  CTrack::ObstacleSpacing = 0.5;
  66
  67 // Obstacle repulsion strength
  68 double                  CTrack::ObstacleRepulsion = 3.0;
  69
  70 // Obstacle repulsion amplification
  71 double                  CTrack::ObstacleAmp = 2.0;
  72
  73
  74 // Minimum motion distance
  75 double                  CTrack::MinimumMotion = 0.2;
  76
  77 // Lock distance threshold
  78 double                  CTrack::LockThreshold = 0.1;
  79
  80 // Lose distance threshold
  81 double                  CTrack::LoseThreshold = 2.0;
  82
  83 // Stabilise cycle
  84 uint32                  CTrack::StabiliseCycle = 5;
  85
  86
  87 // The default sheet
  88 CTrack::CSheet  CTrack::CSheet::DefaultSheet;
  89
  90
  91
  92
  93
  94 // ------------------------------------
  95 // Destructor
  96 CTrack::~CTrack()
  97 {
  98         nldebug("ReynoldsLib:CTrack:~CTrack(): Delete Track %s", _Id.toString().c_str());
  99
 100         // remove this track from the manager map
 101         CReynoldsManager::removeTrackFromMap(_Id);
 102
 103         // delete move primitive
 104         deleteMovePrimitive();
 105
 106         nldebug("ReynoldsLib:CTrack:~CTrack(): Track %s deleted", _Id.toString().c_str());
 107 }
 108
 109 // ------------------------------------
 110 // Init track
 111 void    CTrack::setId(const NLMISC::CEntityId &id, const NLMISC::CSheetId &sheet)
 112 {
 113         _Id = id;
 114         _SheetId = sheet;
 115         _HasId = true;
 116         _IdRequested = false;
 117
 118         _Sheet = CReynoldsManager::lookup(_SheetId);
 119         if (_Sheet == NULL)
 120                 _Sheet = &(CSheet::DefaultSheet);
 121 }
 122
 123
 124 // ------------------------------------
 125 // Follow
 126 void    CTrack::follow(CTrack *followed)
 127 {
 128         acquireControl();
 129         acquireVision();
 130
 131         _Followed = followed;
 132 }
 133
 134 // ------------------------------------
 135 // Leave
 136 void    CTrack::leave()
 137 {
 138         releaseControl();
 139         releaseVision();
 140
 141         _Followed = NULL;
 142
 143         // warn reynolds manager the track left its target
 144         CReynoldsManager::trackStop(this);
 145 }
 146
 147
 148
 149
 150 // ------------------------------------
 151 // Update
 152 void    CTrack::update(double dt)
 153 {
 154         // do not update not owned tracks
 155         if (!_OwnControl)
 156                 return;
 157
 158         // check if target is forced to leave
 159         CTrack  *target = (CTrack*)_Followed;
 160         if (target == NULL || target->_ForceRelease)
 161         {
 162                 leave();
 163                 return;
 164         }
 165
 166         // check if entity has id
 167         if (!hasId() || !hasPosition())
 168                 return;
 169
 170         // check if has a move primitive
 171         if (_MovePrimitive == NULL)
 172         {
 173                 createMovePrimitive();
 174                 // if failed, just leave
 175                 if (_MovePrimitive == NULL)
 176                         leave();
 177                 return;
 178         }
 179
 180         // if target hasn't position and is not owned by manager, request for position updates
 181         if (!target->isValid())
 182                 return;
 183
 184         // motion
 185         CVectorD        motion(CVectorD::Null);
 186         CVectorD        heading;
 187
 188
 189         // --------------------------
 190         // move toward target
 191         double  targetObjective;
 192         if (target->isStatic())
 193         {
 194                 CVectorD        vdist;
 195                 double          ddist;
 196
 197                 ddist = rawDistance(target, vdist);
 198                 heading = vdist;
 199
 200                 targetObjective = ddist;
 201
 202                 double          strength = softPointing(ddist, TargetAttraction, TargetAmp);
 203
 204                 motion += (vdist * (_Sheet->RunSpeed*dt*strength/(ddist+0.01)));
 205         }
 206         else
 207         {
 208                 CVectorD        vdist;
 209                 double          ddist;
 210
 211                 double          cdist = contactDistance(target, vdist, ddist);
 212
 213                 targetObjective = cdist-TargetSpacing;
 214
 215                 double          strength = softTargetting(cdist, TargetSpacing, TargetAttraction, TargetAmp);
 216
 217                 motion += (vdist * (_Sheet->RunSpeed*dt*strength/(ddist+0.01)));
 218         }
 219
 220
 221         // --------------------------
 222         // check target not lost
 223         if (_State == TargetLocked &&
 224                 targetObjective > LoseThreshold)
 225         {
 226                 _State = TargetLost;
 227                 CReynoldsManager::stateChanged(_Id, _State);
 228                 leave();
 229                 return;
 230         }
 231
 232
 233         // --------------------------
 234         // check target reachable
 235         const double    SmoothFactor = 0.7;
 236         double                  tddelta = (_LastTargetDistance<0) ? _SmoothedTargetDistanceDelta : _LastTargetDistance-targetObjective;
 237         _SmoothedTargetDistanceDelta = _SmoothedTargetDistanceDelta*SmoothFactor + tddelta*(1.0-SmoothFactor);
 238         _LastTargetDistance = targetObjective;
 239
 240         // if actor seems not to move fast enough, leave
 241         if (_State == MovingTowardsTarget &&
 242                 _SmoothedTargetDistanceDelta < 0.1 &&
 243                 targetObjective > 1.0)
 244         {
 245                 _State = TargetUnreachable;
 246                 CReynoldsManager::stateChanged(_Id, _State);
 247                 leave();
 248                 return;
 249         }
 250
 251
 252
 253         // --------------------------
 254         // avoid obstacles in vision
 255         TVision::iterator       itv;
 256         for (itv=_Vision.begin(); itv!=_Vision.end(); )
 257         {
 258                 CTrack  *obstacle = (CTrack*)((*itv).second);
 259
 260                 // if obstacle is forced to be release, delete it
 261                 if (obstacle->_ForceRelease)
 262                 {
 263                         TVision::iterator       itr = itv++;
 264                         _Vision.erase(itr);
 265                         continue;
 266                 }
 267
 268                 // if obstacle not yet ready, don't avoid it
 269                 if (!obstacle->isValid())
 270                         continue;
 271
 272                 // don't avoid static obstacles (virtual tracks)
 273                 if (!obstacle->isStatic())
 274                 {
 275                         CVectorD        vdist;
 276                         double          ddist;
 277
 278                         ddist = rawDistance(obstacle, vdist);
 279
 280                         if (ddist > ObstacleExcludeDistance)
 281                                 continue;
 282
 283                         double          cdist = contactDistanceWithRawDistance(obstacle, vdist, ddist);
 284                         double          strength = softRepulse(cdist, ObstacleSpacing, ObstacleRepulsion, ObstacleAmp);
 285
 286                         motion -= (vdist * (_Sheet->WalkSpeed*dt*strength/(ddist+0.01)));
 287                 }
 288
 289                 ++itv;
 290         }
 291
 292         // --------------------------
 293         // avoid walls
 294         CVectorD        front = motion.normed();
 295         CVectorD        lateral(-front.y, front.x, 0.0);
 296         CVectorD        normal;
 297         float           thetaProbe = frand(3.1415926535f) - 1.570796f;
 298
 299         if (!_MoveContainer->testMove(_MovePrimitive, front*cos(thetaProbe)*2.0 + lateral*sin(thetaProbe)*1.0, 1, 0, &normal))
 300                 motion += normal*_Sheet->WalkSpeed*dt;
 301
 302
 303
 304
 305
 306         // --------------------------
 307         // other motion applied by user
 308         CReynoldsManager::applyUserMotion(this, motion);
 309
 310
 311
 312
 313
 314         // --------------------------
 315         // Do move
 316         double  motionNorm = motion.norm();
 317
 318         // check if enough motion
 319         if (motionNorm < MinimumMotion)
 320         {
 321                 if (targetObjective < LockThreshold &&
 322                         _State == MovingTowardsTarget &&
 323                         CReynoldsManager::getCycle()-_LastMoveCycle > StabiliseCycle)
 324                 {
 325                         _State = TargetLocked;
 326                         CReynoldsManager::stateChanged(_Id, _State);
 327                 }
 328                 return;
 329         }
 330
 331         // reset last moving cycle
 332         _LastMoveCycle = CReynoldsManager::getCycle();
 333
 334         // renorm motion if needed
 335         if (motionNorm > dt*_Sheet->RunSpeed)
 336                 motion *= dt*_Sheet->RunSpeed/motionNorm;
 337
 338         // eval move
 339         _MovePrimitive->move(motion, 0);
 340         _MoveContainer->evalNCPrimitiveCollision(1, _MovePrimitive, 0);
 341
 342         // store new position/heading
 343         _Heading = (float)atan2(heading.y, heading.x);
 344         _Position = _MovePrimitive->getFinalPosition(0);
 345 }
 346
 347
 348
 349 // ------------------------------------
 350 // Update vision
 351 void    CTrack::updateVision(const std::vector<NLMISC::CEntityId> &in, const std::vector<NLMISC::CEntityId> &out)
 352 {
 353         uint    i;
 354
 355         // add new in vision
 356         for (i=0; i<in.size(); ++i)
 357         {
 358                 CTrack  *newin = CReynoldsManager::createTrack(in[i]);
 359                 if (newin == NULL)
 360                         continue;
 361
 362                 _Vision.insert(make_pair<CEntityId, CSmartPtr<CTrack> >(in[i], newin));
 363         }
 364
 365         // remove old of vision
 366         for (i=0; i<out.size(); ++i)
 367         {
 368                 _Vision.erase(out[i]);
 369         }
 370 }
 371
 372 // ------------------------------------
 373 // Update vision
 374 void    CTrack::updateVision(const std::vector<NLMISC::CEntityId> &vision)
 375 {
 376         uint    i;
 377
 378         // add new in vision
 379         TVision copy = _Vision;
 380         _Vision.clear();
 381         for (i=0; i<vision.size(); ++i)
 382         {
 383                 CTrack  *newin = CReynoldsManager::createTrack(vision[i]);
 384                 if (newin == NULL)
 385                         continue;
 386
 387                 _Vision.insert(make_pair<CEntityId, CSmartPtr<CTrack> >(vision[i], newin));
 388         }
 389 }
 390
 391
 392
 393
 394
 395
 396 // ------------------------------------
 397 // Acquire Control
 398 void    CTrack::acquireControl()
 399 {
 400         // set state to moving
 401         _State = MovingTowardsTarget;
 402         CReynoldsManager::stateChanged(_Id, _State);
 403
 404         if (_OwnControl)
 405                 return;
 406
 407         // unrequest for position updates if necessary
 408         if (_PositionUpdatesRequested)
 409         {
 410                 CReynoldsManager::unrequestPositionUpdates(_Id);
 411                 _PositionUpdatesRequested = false;
 412         }
 413
 414         // invalidate position
 415         invalidPosition();
 416
 417         // and request for valid position
 418         CReynoldsManager::requestPosition(_Id);
 419
 420         // init user motion
 421         CReynoldsManager::initUserMotion(this);
 422
 423         _OwnControl = true;
 424
 425         // init last move cycle
 426         _LastMoveCycle = CReynoldsManager::getCycle();
 427 }
 428
 429 // ------------------------------------
 430 // Release Control
 431 void    CTrack::releaseControl()
 432 {
 433         // invalidate position
 434         invalidPosition();
 435
 436         // remove primitive
 437         deleteMovePrimitive();
 438
 439         // release user motion
 440         CReynoldsManager::releaseUserMotion(this);
 441
 442         // set state to moving
 443         _State = Idle;
 444         CReynoldsManager::stateChanged(_Id, _State);
 445
 446         _OwnControl = false;
 447 }
 448
 449
 450 // ------------------------------------
 451 // Acquire vision
 452 void    CTrack::acquireVision()
 453 {
 454         if (_ReceiveVision)
 455                 return;
 456
 457         _ReceiveVision = true;
 458         CReynoldsManager::requestVision(_Id);
 459 }
 460
 461 // ------------------------------------
 462 // Release vision
 463 void    CTrack::releaseVision()
 464 {
 465         if (!_ReceiveVision)
 466                 return;
 467
 468         _ReceiveVision = false;
 469         CReynoldsManager::unrequestVision(_Id);
 470 }
 471
 472
 473
 474
 475 // ------------------------------------
 476 // Create Move primitive
 477 void    CTrack::createMovePrimitive()
 478 {
 479         if (!hasPosition())
 480         {
 481                 nlwarning("ReynoldsLib:CTrack:createMovePrimitive(): Can't create move primitive, Track %s has no valid position", _Id.toString().c_str());
 482                 return;
 483         }
 484
 485         if (!hasId())
 486         {
 487                 nlwarning("ReynoldsLib:CTrack:createMovePrimitive(): Can't create move primitive, Track %s has no valid id", _Id.toString().c_str());
 488                 return;
 489         }
 490
 491         deleteMovePrimitive();
 492
 493         CReynoldsManager::createMovePrimitive(_Position, _MovePrimitive, _MoveContainer);
 494
 495         _MovePrimitive->setPrimitiveType(UMovePrimitive::_2DOrientedCylinder);
 496         _MovePrimitive->setReactionType(UMovePrimitive::Slide);
 497         _MovePrimitive->setTriggerType(UMovePrimitive::NotATrigger);
 498         _MovePrimitive->setCollisionMask(0);
 499         _MovePrimitive->setOcclusionMask(0);
 500         _MovePrimitive->setObstacle(false);
 501         _MovePrimitive->setDontSnapToGround(false);
 502         _MovePrimitive->setRadius(_Sheet->Radius);
 503         _MovePrimitive->setHeight(_Sheet->Height);
 504
 505         _MovePrimitive->insertInWorldImage(0);
 506         _MovePrimitive->setGlobalPosition(_Position, 0);
 507         _MovePrimitive->setOrientation(_Heading, 0);
 508         _MoveContainer->evalCollision(1, 0);
 509 }
 510
 511 // ------------------------------------
 512 // Delete Move primitive
 513 void    CTrack::deleteMovePrimitive()
 514 {
 515         if (_MovePrimitive != NULL)
 516         {
 517                 if (_MoveContainer == NULL)
 518                 {
 519                         nlwarning("ReynoldsLib:CTrack:deleteMovePrimitive(): Track %s has a MovePrimitive, but MoveContainer not set", _Id.toString().c_str());
 520                 }
 521                 else
 522                 {
 523                         _MoveContainer->removePrimitive(_MovePrimitive);
 524                 }
 525         }
 526
 527         _MovePrimitive = NULL;
 528         _MoveContainer = NULL;
 529 }
 530
 531 // ------------------------------------
 532 // Request Id
 533 void    CTrack::requestId()
 534 {
 535         _IdRequested = true;
 536         CReynoldsManager::requestSheet(_Id);
 537 }
 538
 539 // ------------------------------------
 540 // Request Position
 541 void    CTrack::requestPositionUpdates()
 542 {
 543         _PositionUpdatesRequested = true;
 544         CReynoldsManager::requestPositionUpdates(_Id);
 545 }