RVO2.h

#pragma once
// ===========================================================================
// Copyright (c) 2015 Toolchefs Ltd. All rights reserved.
//
// Use of this software is subject to the terms of the Toolchefs license
// agreement provided at the time of installation or download, or which
// otherwise accompanies this software in either electronic or hard copy form.
// ===========================================================================
 
#include <Atoms/Agent.h>
#include <AtomsCore/AtomsMath.h>
 
 
namespace Atoms
{
    namespace RVO2
    {
        inline float distSqPointLineSegment(const AtomsMath::Vector2f& a, const AtomsMath::Vector2f& b, const AtomsMath::Vector2f& c)
        {
            const float r = ((c - a).dot(b - a)) / (b - a).length2();
 
            if (r < 0.0f) {
                return (c - a).length2();
            }
            else if (r > 1.0f) {
                return (c - b).length2();
            }
            else {
                return (c - (a + (b - a) * r)).length2();
            }
        }
 
        inline float distSqPointLineSegment(const AtomsMath::Vector3f& a, const AtomsMath::Vector3f& b, const AtomsMath::Vector3f& c)
        {
            const float r = ((c - a).dot(b - a)) / (b - a).length2();
 
            if (r < 0.0f) {
                return (c - a).length2();
            }
            else if (r > 1.0f) {
                return (c - b).length2();
            }
            else {
                return (c - (a + (b - a) * r)).length2();
            }
        }
 
        inline float det(const AtomsMath::Vector2f& vector1, const AtomsMath::Vector2f& Vector2f)
        {
            return vector1.x * Vector2f.y - vector1.y * Vector2f.x;
        }
 
        inline float leftOf(const AtomsMath::Vector2f& a, const AtomsMath::Vector2f& b, const AtomsMath::Vector2f& c)
        {
            return det(a - c, b - a);
        }
 
        inline float sqr(float a)
        {
            return a * a;
        }
 
        class ATOMS_EXPORT Obstacle;
 
        class ATOMS_EXPORT Line {
        public:
 
            AtomsMath::Vector2f point;
 
            AtomsMath::Vector2f direction;
        };
 
        class ATOMS_EXPORT ObstacleTreeNode {
        public:
 
            ObstacleTreeNode* left;
 
            const Obstacle* obstacle;
 
            ObstacleTreeNode* right;
        };
 
        class ATOMS_EXPORT KdTreeObstacle {
        public:
 
            explicit KdTreeObstacle();
 
            ~KdTreeObstacle();
 
            void buildObstacleTree(std::vector<Obstacle*>& simObstacles);
 
            ObstacleTreeNode* buildObstacleTreeRecursive(std::vector<Obstacle*>& simObstacles, const std::vector<Obstacle*>& copyObstacles);
 
            void deleteObstacleTree(ObstacleTreeNode* node);
 
            void clear();
 
            void queryObstacleTreeRecursive(
                const AtomsMath::Vector3f& agentPosition,
                float rangeSq,
                float heightRange,
                const ObstacleTreeNode* node,
                std::vector<std::pair<float, const Obstacle*>>& obstacleNeighbors) const;
 
            void insertObstacleNeighbor(
                const Obstacle* obstacle,
                const AtomsMath::Vector3f& agentPosition,
                float rangeSq,
                float heightRange,
                std::vector<std::pair<float, const Obstacle*>>& obstacleNeighbors) const;
 
            ObstacleTreeNode* tree();
 
            ObstacleTreeNode* obstacleTree_;
 
            static const size_t MAX_LEAF_SIZE = 10;
        };
 
        class ATOMS_EXPORT Obstacle {
        public:
 
            Obstacle();
 
            enum NearTypeEnum { FIRST, MIDDLE, LAST };
 
            AtomsMath::Vector2f getP1() const;
 
            Obstacle::NearTypeEnum distanceSqToPoint(const AtomsMath::Vector2f& pt, AtomsMath::Vector2f& nearPt, float& distSq) const;
 
            bool isConvex_;
            Obstacle* nextObstacle_;
            AtomsMath::Vector3f pointW_;
            AtomsMath::Vector2f point_;
            Obstacle* prevObstacle_;
            AtomsMath::Vector2f unitDir_;
            AtomsMath::Vector3f unitDirW_;
            float length_;
            float lengthW_;
            size_t id_;
        };
 
        ATOMS_EXPORT size_t computeORCALinesTurning(
            const AtomsCore::Vector2f& position,
            const AtomsCore::Vector2f& velocity,
            const AtomsCore::Vector2f& prefVelocity,
            const std::vector<const Atoms::Agent*>& agentNeighbors,
            const std::vector<std::pair<float, const Atoms::RVO2::Obstacle*>>& obstacleNeighbors,
            float timeHorizon,
            float timeHorizonObst,
            float radius,
            float maxSpeed,
            float timeStep,
            float fps,
            float sceneScale,
            float radiusMultiplier,
            float obstacleRadiusMultiplier,
            float turningBias,
            bool isPedVO,
            int step,
            int steps,
            std::vector<Atoms::RVO2::Line>& orcaLines,
            AtomsMath::Vector2f& optVel,
            AtomsMath::Vector2f& prefDir,
            float& prefSpeed);
 
        ATOMS_EXPORT AtomsCore::Vector2f computeNewVelocity(
            const AtomsCore::Vector2f& position,
            const AtomsCore::Vector2f& velocity,
            AtomsCore::Vector2f prefVelocity,
            const std::vector<const Atoms::Agent*>& neighbors,
            const std::vector<std::pair<float, const Atoms::RVO2::Obstacle*>>& obstacleNeighbors,
            float timeHorizon,
            float timeHorizonObst,
            float radius,
            float maxSpeed,
            float timeStep,
            float fps,
            float sceneScale,
            float radiusMultiplier,
            float obstacleRadiusMultiplier,
            float speedConst,
            float turningBias,
            bool isPedVO,
            int step,
            int steps);
 
        ATOMS_EXPORT void adaptPreferredVelocity(
            AtomsCore::Vector2f& prefVelocity,
            const AtomsCore::Vector2f& position,
            const std::vector<const Atoms::Agent*>& agentNeighbors,
            const std::vector<std::pair<float, const Atoms::RVO2::Obstacle*>>& obstacleNeighbors,
            float sceneScale,
            float speedConst);
 
        ATOMS_EXPORT bool linearProgram1(
            const std::vector<Line>& lines,
            size_t lineNo,
            float radius,
            const AtomsMath::Vector2f& optVelocity,
            bool directionOpt,
            float turnBias,
            bool isPedVO,
            AtomsMath::Vector2f& result);
 
        ATOMS_EXPORT size_t linearProgram2(
            const std::vector<Line>& lines,
            float radius,
            const AtomsMath::Vector2f& optVelocity,
            bool directionOpt,
            float turnBias,
            bool isPedVO,
            AtomsMath::Vector2f& result);
 
        ATOMS_EXPORT void linearProgram3(
            const std::vector<Line>& lines,
            size_t numObstLines,
            size_t beginLine,
            float radius,
            float turnBias,
            bool isPedVO,
            AtomsMath::Vector2f& result);
 
    }
}