// Ryzom - MMORPG Framework <http://dev.ryzom.com/projects/ryzom/> // Copyright (C) 2010 Winch Gate Property Limited // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU Affero General Public License as // published by the Free Software Foundation, either version 3 of the // License, or (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU Affero General Public License for more details. // // You should have received a copy of the GNU Affero General Public License // along with this program. If not, see <http://www.gnu.org/licenses/>. #include "stdpch.h" #include "track.h" #include "reynolds_manager.h" using namespace std; using namespace NLMISC; using namespace NLPACS; // ------------------------------------ // Attraction/Repulsion functions // strong repulsion limited inline double softPointing(double dist, double strength, double amp = 1.0) { return amp*atan(strength*dist)/1.5707963268; } // strong repulsion limited inline double softTargetting(double dist, double moderateDist, double strength, double amp = 1.0) { return amp*( atan(strength*(dist-moderateDist-0.2)) + atan(strength*(dist-moderateDist+0.2)) )/3.1415927; } // strong repulsion limited inline double softRepulse(double dist, double moderateDist, double strength, double amp = 1.0) { return amp*(0.5 - atan(strength*(dist-moderateDist)-1.0)/3.1415927); } // Required target spacing double CTrack::TargetSpacing = 0.5; // Target attraction strength double CTrack::TargetAttraction = 3.0; // Target attraction amplification double CTrack::TargetAmp = 2.0; // Fast obstacle exclusion distance double CTrack::ObstacleExcludeDistance = 6.0; // Required obstacle spacing double CTrack::ObstacleSpacing = 0.5; // Obstacle repulsion strength double CTrack::ObstacleRepulsion = 3.0; // Obstacle repulsion amplification double CTrack::ObstacleAmp = 2.0; // Minimum motion distance double CTrack::MinimumMotion = 0.2; // Lock distance threshold double CTrack::LockThreshold = 0.1; // Lose distance threshold double CTrack::LoseThreshold = 2.0; // Stabilise cycle uint32 CTrack::StabiliseCycle = 5; // The default sheet CTrack::CSheet CTrack::CSheet::DefaultSheet; // ------------------------------------ // Destructor CTrack::~CTrack() { nldebug("ReynoldsLib:CTrack:~CTrack(): Delete Track %s", _Id.toString().c_str()); // remove this track from the manager map CReynoldsManager::removeTrackFromMap(_Id); // delete move primitive deleteMovePrimitive(); nldebug("ReynoldsLib:CTrack:~CTrack(): Track %s deleted", _Id.toString().c_str()); } // ------------------------------------ // Init track void CTrack::setId(const NLMISC::CEntityId &id, const NLMISC::CSheetId &sheet) { _Id = id; _SheetId = sheet; _HasId = true; _IdRequested = false; _Sheet = CReynoldsManager::lookup(_SheetId); if (_Sheet == NULL) _Sheet = &(CSheet::DefaultSheet); } // ------------------------------------ // Follow void CTrack::follow(CTrack *followed) { acquireControl(); acquireVision(); _Followed = followed; } // ------------------------------------ // Leave void CTrack::leave() { releaseControl(); releaseVision(); _Followed = NULL; // warn reynolds manager the track left its target CReynoldsManager::trackStop(this); } // ------------------------------------ // Update void CTrack::update(double dt) { // do not update not owned tracks if (!_OwnControl) return; // check if target is forced to leave CTrack *target = (CTrack*)_Followed; if (target == NULL || target->_ForceRelease) { leave(); return; } // check if entity has id if (!hasId() || !hasPosition()) return; // check if has a move primitive if (_MovePrimitive == NULL) { createMovePrimitive(); // if failed, just leave if (_MovePrimitive == NULL) leave(); return; } // if target hasn't position and is not owned by manager, request for position updates if (!target->isValid()) return; // motion CVectorD motion(CVectorD::Null); CVectorD heading; // -------------------------- // move toward target double targetObjective; if (target->isStatic()) { CVectorD vdist; double ddist; ddist = rawDistance(target, vdist); heading = vdist; targetObjective = ddist; double strength = softPointing(ddist, TargetAttraction, TargetAmp); motion += (vdist * (_Sheet->RunSpeed*dt*strength/(ddist+0.01))); } else { CVectorD vdist; double ddist; double cdist = contactDistance(target, vdist, ddist); targetObjective = cdist-TargetSpacing; double strength = softTargetting(cdist, TargetSpacing, TargetAttraction, TargetAmp); motion += (vdist * (_Sheet->RunSpeed*dt*strength/(ddist+0.01))); } // -------------------------- // check target not lost if (_State == TargetLocked && targetObjective > LoseThreshold) { _State = TargetLost; CReynoldsManager::stateChanged(_Id, _State); leave(); return; } // -------------------------- // check target reachable const double SmoothFactor = 0.7; double tddelta = (_LastTargetDistance<0) ? _SmoothedTargetDistanceDelta : _LastTargetDistance-targetObjective; _SmoothedTargetDistanceDelta = _SmoothedTargetDistanceDelta*SmoothFactor + tddelta*(1.0-SmoothFactor); _LastTargetDistance = targetObjective; // if actor seems not to move fast enough, leave if (_State == MovingTowardsTarget && _SmoothedTargetDistanceDelta < 0.1 && targetObjective > 1.0) { _State = TargetUnreachable; CReynoldsManager::stateChanged(_Id, _State); leave(); return; } // -------------------------- // avoid obstacles in vision TVision::iterator itv; for (itv=_Vision.begin(); itv!=_Vision.end(); ) { CTrack *obstacle = (CTrack*)((*itv).second); // if obstacle is forced to be release, delete it if (obstacle->_ForceRelease) { TVision::iterator itr = itv++; _Vision.erase(itr); continue; } // if obstacle not yet ready, don't avoid it if (!obstacle->isValid()) continue; // don't avoid static obstacles (virtual tracks) if (!obstacle->isStatic()) { CVectorD vdist; double ddist; ddist = rawDistance(obstacle, vdist); if (ddist > ObstacleExcludeDistance) continue; double cdist = contactDistanceWithRawDistance(obstacle, vdist, ddist); double strength = softRepulse(cdist, ObstacleSpacing, ObstacleRepulsion, ObstacleAmp); motion -= (vdist * (_Sheet->WalkSpeed*dt*strength/(ddist+0.01))); } ++itv; } // -------------------------- // avoid walls CVectorD front = motion.normed(); CVectorD lateral(-front.y, front.x, 0.0); CVectorD normal; float thetaProbe = frand(3.1415926535f) - 1.570796f; if (!_MoveContainer->testMove(_MovePrimitive, front*cos(thetaProbe)*2.0 + lateral*sin(thetaProbe)*1.0, 1, 0, &normal)) motion += normal*_Sheet->WalkSpeed*dt; // -------------------------- // other motion applied by user CReynoldsManager::applyUserMotion(this, motion); // -------------------------- // Do move double motionNorm = motion.norm(); // check if enough motion if (motionNorm < MinimumMotion) { if (targetObjective < LockThreshold && _State == MovingTowardsTarget && CReynoldsManager::getCycle()-_LastMoveCycle > StabiliseCycle) { _State = TargetLocked; CReynoldsManager::stateChanged(_Id, _State); } return; } // reset last moving cycle _LastMoveCycle = CReynoldsManager::getCycle(); // renorm motion if needed if (motionNorm > dt*_Sheet->RunSpeed) motion *= dt*_Sheet->RunSpeed/motionNorm; // eval move _MovePrimitive->move(motion, 0); _MoveContainer->evalNCPrimitiveCollision(1, _MovePrimitive, 0); // store new position/heading _Heading = (float)atan2(heading.y, heading.x); _Position = _MovePrimitive->getFinalPosition(0); } // ------------------------------------ // Update vision void CTrack::updateVision(const std::vector<NLMISC::CEntityId> &in, const std::vector<NLMISC::CEntityId> &out) { uint i; // add new in vision for (i=0; i<in.size(); ++i) { CTrack *newin = CReynoldsManager::createTrack(in[i]); if (newin == NULL) continue; _Vision.insert(make_pair<CEntityId, CSmartPtr<CTrack> >(in[i], newin)); } // remove old of vision for (i=0; i<out.size(); ++i) { _Vision.erase(out[i]); } } // ------------------------------------ // Update vision void CTrack::updateVision(const std::vector<NLMISC::CEntityId> &vision) { uint i; // add new in vision TVision copy = _Vision; _Vision.clear(); for (i=0; i<vision.size(); ++i) { CTrack *newin = CReynoldsManager::createTrack(vision[i]); if (newin == NULL) continue; _Vision.insert(make_pair<CEntityId, CSmartPtr<CTrack> >(vision[i], newin)); } } // ------------------------------------ // Acquire Control void CTrack::acquireControl() { // set state to moving _State = MovingTowardsTarget; CReynoldsManager::stateChanged(_Id, _State); if (_OwnControl) return; // unrequest for position updates if necessary if (_PositionUpdatesRequested) { CReynoldsManager::unrequestPositionUpdates(_Id); _PositionUpdatesRequested = false; } // invalidate position invalidPosition(); // and request for valid position CReynoldsManager::requestPosition(_Id); // init user motion CReynoldsManager::initUserMotion(this); _OwnControl = true; // init last move cycle _LastMoveCycle = CReynoldsManager::getCycle(); } // ------------------------------------ // Release Control void CTrack::releaseControl() { // invalidate position invalidPosition(); // remove primitive deleteMovePrimitive(); // release user motion CReynoldsManager::releaseUserMotion(this); // set state to moving _State = Idle; CReynoldsManager::stateChanged(_Id, _State); _OwnControl = false; } // ------------------------------------ // Acquire vision void CTrack::acquireVision() { if (_ReceiveVision) return; _ReceiveVision = true; CReynoldsManager::requestVision(_Id); } // ------------------------------------ // Release vision void CTrack::releaseVision() { if (!_ReceiveVision) return; _ReceiveVision = false; CReynoldsManager::unrequestVision(_Id); } // ------------------------------------ // Create Move primitive void CTrack::createMovePrimitive() { if (!hasPosition()) { nlwarning("ReynoldsLib:CTrack:createMovePrimitive(): Can't create move primitive, Track %s has no valid position", _Id.toString().c_str()); return; } if (!hasId()) { nlwarning("ReynoldsLib:CTrack:createMovePrimitive(): Can't create move primitive, Track %s has no valid id", _Id.toString().c_str()); return; } deleteMovePrimitive(); CReynoldsManager::createMovePrimitive(_Position, _MovePrimitive, _MoveContainer); _MovePrimitive->setPrimitiveType(UMovePrimitive::_2DOrientedCylinder); _MovePrimitive->setReactionType(UMovePrimitive::Slide); _MovePrimitive->setTriggerType(UMovePrimitive::NotATrigger); _MovePrimitive->setCollisionMask(0); _MovePrimitive->setOcclusionMask(0); _MovePrimitive->setObstacle(false); _MovePrimitive->setDontSnapToGround(false); _MovePrimitive->setRadius(_Sheet->Radius); _MovePrimitive->setHeight(_Sheet->Height); _MovePrimitive->insertInWorldImage(0); _MovePrimitive->setGlobalPosition(_Position, 0); _MovePrimitive->setOrientation(_Heading, 0); _MoveContainer->evalCollision(1, 0); } // ------------------------------------ // Delete Move primitive void CTrack::deleteMovePrimitive() { if (_MovePrimitive != NULL) { if (_MoveContainer == NULL) { nlwarning("ReynoldsLib:CTrack:deleteMovePrimitive(): Track %s has a MovePrimitive, but MoveContainer not set", _Id.toString().c_str()); } else { _MoveContainer->removePrimitive(_MovePrimitive); } } _MovePrimitive = NULL; _MoveContainer = NULL; } // ------------------------------------ // Request Id void CTrack::requestId() { _IdRequested = true; CReynoldsManager::requestSheet(_Id); } // ------------------------------------ // Request Position void CTrack::requestPositionUpdates() { _PositionUpdatesRequested = true; CReynoldsManager::requestPositionUpdates(_Id); }