avoidancesolver.h

Go to the documentation of this file.

/*
* Copyright 2015 Autodesk, Inc. All rights reserved.
* Use of this software is subject to the terms of the Autodesk license agreement and any attachments or Appendices thereto provided at the time of installation or download,
* or which otherwise accompanies this software in either electronic or hard copy form, or which is signed by you and accepted by Autodesk.
*/

// primary contact: MUBI - secondary contact: LAPA
#ifndef Navigation_AvoidanceSolver_H
#define Navigation_AvoidanceSolver_H

#include "gwnavruntime/pathfollower/iavoidancesolver.h"
#include "gwnavruntime/pathfollower/avoidanceconfig.h"
#include "gwnavruntime/pathfollower/iavoidancecomputer.h"
#include "gwnavruntime/world/bot.h"
#include "gwnavruntime/channel/raycastinchannel.h"
#include "gwnavruntime/queries/raycastquery.h"
#include "gwnavruntime/queries/diskcollisionquery.h"
#include "gwnavruntime/pathfollower/localcostawaretraverselogic.h"


namespace Kaim
{

class CosAndSinAngle;

class NormalizedUtilityFunctionInfluenceRatios
{
public:
    NormalizedUtilityFunctionInfluenceRatios(const AvoidanceConfig* avoidanceConfig)
    {
        const Kaim::AvoidanceUtilityFunctionConfig& utilityconfig = avoidanceConfig->m_utilityFunctionConfig;
        KyFloat32 influenceSum = 
            utilityconfig.m_avoidanceInfluence +
            utilityconfig.m_desiredVelocityInfluence +
            utilityconfig.m_previousVelocityInfluence;

        KyFloat32 influenceSumInv = 1.0f / influenceSum;

        m_avoidanceInfluence        = influenceSumInv * utilityconfig.m_avoidanceInfluence;
        m_desiredVelocityInfluence  = influenceSumInv * utilityconfig.m_desiredVelocityInfluence;
        m_previousVelocityInfluence = influenceSumInv * utilityconfig.m_previousVelocityInfluence;
    }
    
    KyFloat32 m_avoidanceInfluence;
    KyFloat32 m_desiredVelocityInfluence;
    KyFloat32 m_previousVelocityInfluence;
};

typedef RayCastQueryResult(*PerformRaycastFromBotPositionCallback) 
    (const AvoidanceSolverConfig& config, const Vec2f& direction, KyFloat32 speed, KyFloat32 time,
    LocalCostAwareTraverseLogicData& localCostAwareTraverseLogicData, Vec3f& collisionPos);

typedef DiskCollisionQueryResult(*PerformDiskCollisionFromBotPositionCallback) 
    (const AvoidanceSolverConfig& config, KyFloat32 speed, KyFloat32 time, LocalCostAwareTraverseLogicData& localCostAwareTraverseLogicData);

class VelocitySample
{
public:

    VelocitySample()
        : m_normalizedDirection      (0.0f, 0.0f)
        , m_type                     (Avoidance_NoAvoidance)
        , m_avoidanceScore           (0.0f)
        , m_desiredVelocityScore     (0.0f)
        , m_previousVelocityScore    (0.0f)
        , m_collisionTime            (0.0f)
        , m_colliderIndex            (-1)
    {}

    Vec2f           m_normalizedDirection;
    AvoidanceResult m_type;

    KyFloat32 m_avoidanceScore;
    KyFloat32 m_desiredVelocityScore;
    KyFloat32 m_previousVelocityScore;
    KyFloat32 m_collisionTime;
    KyInt32   m_colliderIndex; // if -1 no colliders, or static one (i.e. navmesh or channel borders)

    KyFloat32 ComputeFinalScore(const NormalizedUtilityFunctionInfluenceRatios& utilityFunctionRatios) const
    {
        return ((m_avoidanceScore         * utilityFunctionRatios.m_avoidanceInfluence) + 
                (m_desiredVelocityScore   * utilityFunctionRatios.m_desiredVelocityInfluence) + 
                (m_previousVelocityScore  * utilityFunctionRatios.m_previousVelocityInfluence));
    }

    // VelocitySamplesArray::ScoreForDistanceToDirection(desiredDirection, &VelocitySample::ScoreDesiredVelocity);
    // => (velocitySample.*scoreFunction)(score);
    // ScoreForDistanceToDirection(previousDirection, &VelocitySample::ScorePreviousVelocity);
    void ScoreDesiredVelocity (KyFloat32 score) { m_desiredVelocityScore = score; }
    void ScorePreviousVelocity(KyFloat32 score) { m_previousVelocityScore = score; }
    typedef void(VelocitySample::*ScoreVelocityFunction)(KyFloat32);

    // Debugging functions
    KyFloat32 GetAvoidanceScore        (const NormalizedUtilityFunctionInfluenceRatios& ) const { return m_avoidanceScore; }
    KyFloat32 GetDesiredVelocityScore  (const NormalizedUtilityFunctionInfluenceRatios& ) const { return m_desiredVelocityScore; }
    KyFloat32 GetPreviousVelocityScore (const NormalizedUtilityFunctionInfluenceRatios& ) const { return m_previousVelocityScore; }

    typedef KyFloat32(VelocitySample::*ScoreAccessorFunction)(const NormalizedUtilityFunctionInfluenceRatios&) const;

public: // internal
    void AdjustScoreAgainstChannel(const AvoidanceSolverConfig& config, KyFloat32 movingSpeed, KyFloat32 timeHorizon, RayCastInChannel& rayCastInChannel);
    void AdjustScoreAgainstNavMesh(const AvoidanceSolverConfig& config, KyFloat32 movingSpeed, KyFloat32 timeHorizon,
        LocalCostAwareTraverseLogicData& localCostAwareTraverseLogicData, PerformRaycastFromBotPositionCallback raycastFunction);
private:
    void AdjustScoreToStaticObstacleCollision(const AvoidanceSolverConfig& config, KyFloat32 movingSpeed, KyFloat32 timeHorizon, const Vec2f& botPosition, const Vec2f& collisionPos);
};

typedef KyArrayPOD<VelocitySample, MemStat_PathFollowing, Kaim::ArrayConstPolicy<0, 16, true> > VelocitySampleArrayType;

class VelocitySampleArray
{
public:
    void Clear();
    void ComputeTurningVelocitiesSamples(const AvoidanceSolverConfig& config, const Vec2f& movingDirection, KyFloat32 timeHorizon);
    
    void ScoreForDistanceToDirection(const Vec2f& direction, VelocitySample::ScoreVelocityFunction scoreFunction);
    void ScoreForPreviousDirection(const Vec2f& previousDirection, KyFloat32 previousSpeed);
    KyUInt32 FindClosestSampleToDirection(const Vec2f& direction);
    KyFloat32 SelectBestVelocitySample(const NormalizedUtilityFunctionInfluenceRatios& utilityFunctionRatios, const VelocitySample*& bestSample, const Vec2f& previousDirection);
    
    bool ScoreForAvoidance(const AvoidanceSolverConfig& config, KyFloat32 timeHorizon, KyFloat32 movingSpeed, 
        LocalCostAwareTraverseLogicData& localCostAwareTraverseLogicData, PerformRaycastFromBotPositionCallback raycastFunction,
        PerformDiskCollisionFromBotPositionCallback diskCollisionFunction);

    void DebugDrawSample(ScopedDisplayList& displayList_Samples, const VelocitySample& velocitySample, const Vec3f& rootPosition,
        VelocitySample::ScoreAccessorFunction scoreAccessorFunction, const NormalizedUtilityFunctionInfluenceRatios& utilityFunctionRatios, KyFloat32 movingSpeed);

    void DebugDrawSamples(const char* groupName, const char* displayListName, Kaim::Bot* bot, VelocitySample::ScoreAccessorFunction scoreAccessorFunction,
        const NormalizedUtilityFunctionInfluenceRatios& utilityFunctionRatios, KyFloat32 movingSpeed);

    void DebugDrawSamplesCurve(const char* groupName, const char* displayListName, Kaim::Bot* bot, VelocitySample::ScoreAccessorFunction scoreAccessorFunction,
        const NormalizedUtilityFunctionInfluenceRatios& utilityFunctionRatios, KyFloat32 movingSpeed);

    void DebugDraw(AvoidanceSolverConfig& config, const NormalizedUtilityFunctionInfluenceRatios& utilityFunctionRatios, const Vec2f& previousVelocity,
        const Vec2f& desiredVelocity, const Vec2f& outputVelocity, const VelocitySample& noAvoidanceSample, const VelocitySample& favoriteSample, KyFloat32 movingSpeed);

public:
    VelocitySampleArrayType m_velocitySamples;
};


// what's is point of this AvoidanceSolverImplementation class ?
class AvoidanceSolverImplementation
{
    KY_DEFINE_NEW_DELETE_OPERATORS(MemStat_PathFollowing)

public:
    AvoidanceSolverImplementation() {}

    AvoidanceResult Solve(AvoidanceSolverConfig& config, const Vec2f& previousVelocity, const Vec2f& desiredVelocity,
        Vec2f& outputVelocity, Vec2f& outputFrontDirection, bool& collide,
        PerformRaycastFromBotPositionCallback raycastFunction,
        PerformDiskCollisionFromBotPositionCallback diskCollisionFunction);

    AvoidanceResult ComputeAvoidanceVelocity(
        const Vec2f& previousVelocity, const Vec2f& desiredVelocity, Vec2f& outputVelocity, Vec2f& outputFrontDirection,
        PerformRaycastFromBotPositionCallback raycastFunction,
        PerformDiskCollisionFromBotPositionCallback diskCollisionFunction);

    void ComputeMovingDirection(Vec2f& movingDirection);

    KyFloat32 ComputeTimeHorizon(KyFloat32 movingSpeed);

    bool CapDirectionChangeAngle(const Vec2f& normalizedFrom, const Vec2f& normalizedTo, Vec2f& result, CosAndSinAngle& maxAngle);
    
    void SolveContact(const KyFloat32 maxSpeed, const Vec2f& avoidanceDirection, bool& hasCollided, Vec2f& collideSolveDirection, KyFloat32& finalSpeed);
    
    void CapAvoidanceVelocityChange(
        const KyFloat32 previousSpeed, const bool hasCollided, const Vec2f& previousDirection, const KyFloat32 movingSpeed,
        Vec2f& finalVelocityDirection, KyFloat32& finalSpeed, Vec2f& finalFrontDirection);

    bool IsDirectionValid(const Vec2f& direction, KyFloat32 speed, KyFloat32 time,
        PerformRaycastFromBotPositionCallback raycastFunction,
        LocalCostAwareTraverseLogicData& localCostAwareTraverseLogicData);
    
private:
    VelocitySampleArray             m_velocitySamplesArray;
    LocalCostAwareTraverseLogicData m_localCostAwareTraverseLogicData;

    AvoidanceSolverConfig m_config;
    Vec2f                 m_previousVelocityDirection; 
    KyFloat32             m_previousSpeed; 
    Vec2f                 m_followVelocityDirection;
    KyFloat32             m_followSpeed; 
};

template<class TraverseLogic>
class AvoidanceSolver : public IAvoidanceSolver
{
    KY_DEFINE_NEW_DELETE_OPERATORS(MemStat_PathFollowing)

public:
    // ---------------------------------- Public Member Functions ----------------------------------
    AvoidanceSolver()
    {}

    virtual AvoidanceResult Solve(
        AvoidanceSolverConfig& config, const Vec2f& previousVelocity, const Vec2f& desiredVelocity,
        Vec2f& outputVelocity, Vec2f& outputFrontDirection, bool& collide);

protected:
    static DiskCollisionQueryResult PerformDiskCollisionFromBotPosition(
        const AvoidanceSolverConfig& config, KyFloat32 speed, KyFloat32 time,
        LocalCostAwareTraverseLogicData& localCostAwareTraverseLogicData);
    
    static RayCastQueryResult PerformRaycastFromBotPosition(
        const AvoidanceSolverConfig& config, const Vec2f& direction, KyFloat32 speed, KyFloat32 time,
        LocalCostAwareTraverseLogicData& localCostAwareTraverseLogicData, Vec3f& collisionPos);
    
    AvoidanceSolverImplementation m_impl;
};

} // namespace Kaim

#include "gwnavruntime/pathfollower/avoidancesolver.inl"

#endif // Navigation_AvoidanceSolver_H