khanat-code-old/code/ryzom/tools/reynolds/track.cpp

546 lines
12 KiB
C++
Raw Normal View History

2015-03-07 16:01:14 +00:00
// 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);
}