setVpx accept now all VPX using ; as delimiter

[ryzomcore.git] / ryzom / tools / reynolds / track.cpp

// 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);
}