OpenSteer plugin for social forces model for crowd behavior

SocialForces.cpp

// ----------------------------------------------------------------------------
//
//
// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//
//
// ----------------------------------------------------------------------------
//
// Simple crowd simulation model based on Dr. Stephen Guy's model. All credit to him.
// Q: when deciding if the wall or the non-intersection force, which takes precedence?
//
// TODO:
// 1. Port all internal variables for computing forces [ DONE ]
// 2. Understand how the plugins update their internal position [ DONE ]
// 3. Euler integrate current force to find what we need to add to velocity [ DONE ]
// 4. Make sure code for boundaries works
// 5. Add code for choosing target destinations [ DONE ]
// 6. Add code for aiming velocity at the target destination [ DONE ]
// 7. Add code for randomly assigning position and target goals
//
//
// (contributed on Feb 22, 2017)
//
// ----------------------------------------------------------------------------


#include <iomanip>
#include <sstream>
#include <math.h>
#include "OpenSteer/SimpleVehicle.h"
#include "OpenSteer/OpenSteerDemo.h"
#include "OpenSteer/Draw.h"
#include "OpenSteer/Color.h"
#include "OpenSteer/UnusedParameter.h"
#include "OpenSteer/Proximity.h"

// for the second two constants, see the paper by Guy on
// a universal power law for crowd dynamics.
#define NEIGHBORHOOD_DIST 10.0f
#define RELAXATION_TIME 0.54f
#define POWER_LAW_EXP 2.0f
#define EXP_CUTOFF 3.0f
#define K 1.5f
#define _EPSILON 0.00001f
#define TOTAL_PLAYERS 10
// the total number of bins in each dimension when using the proximity database queries
#define TOTAL_DIVISIONS 10

namespace {
    
    using namespace OpenSteer;
    
    // our connection to the optimized toolkit for proximity queries
    typedef OpenSteer::AbstractProximityDatabase<AbstractVehicle*> ProximityDatabase;
    typedef OpenSteer::AbstractTokenForProximityDatabase<AbstractVehicle*> ProximityToken;
    
    
    Vec3 playerPosition[9] = {
        Vec3(4,0,0),
        Vec3(7,0,-5),
        Vec3(7,0,5),
        Vec3(10,0,-3),
        Vec3(10,0,3),
        Vec3(15,0, -8),
        Vec3(15,0,0),
        Vec3(15,0,8),
        Vec3(4,0,0)
    };
    
    // ----------------------------------------------------------------------------
    
    // a box object for the constrained field
    class AABBox{
    public:
        AABBox(Vec3 &min, Vec3& max): m_min(min), m_max(max){}
        AABBox(Vec3 min, Vec3 max): m_min(min), m_max(max){}
        bool	InsideX(const Vec3 p){if(p.x < m_min.x || p.x > m_max.x)	return false; return true;}
        bool	InsideZ(const Vec3 p){if(p.z < m_min.z || p.z > m_max.z)	return false; return true;}
        void	draw(){
            Vec3 b,c;
            b = Vec3(m_min.x, 0, m_max.z);
            c = Vec3(m_max.x, 0, m_min.z);
            Color color(1.0f,1.0f,0.0f);
            drawLineAlpha(m_min, b, color, 1.0f);
            drawLineAlpha(b, m_max, color, 1.0f);
            drawLineAlpha(m_max, c, color, 1.0f);
            drawLineAlpha(c,m_min, color, 1.0f);
        }
    private:
        Vec3 m_min;
        Vec3 m_max;
    };
    
   
    
    class Player : public SimpleVehicle
    {
    public:
        
        // constructor
        Player (ProximityDatabase& pd) {
            reset ();
        
            // register ourselves with the main database
            m_proximityToken = pd.allocateToken(this);
        }
        
        // reset state
        void reset (void)
        {
            SimpleVehicle::reset (); // reset the vehicle
            setSpeed (0.0f);         // speed along Forward direction.
            setMaxForce (3000.7f);      // steering force is clipped to this magnitude
            setMaxSpeed (10);         // velocity is clipped to this magnitude
            
            // seed a random initial position
            setPosition(RandomUnitVectorOnXZPlane() * 10);
            
            // seed a random destination
            m_goalTarget = RandomUnitVectorOnXZPlane() * 20;
            
            m_proximityToken->updateForNewPosition(position());
            clearTrailHistory ();    // prevent long streaks due to teleportation
            setTrailParameters (10, 60);
        }
        
        void computeForces() {

            
            // anticipatory forces from static obstacles
            for (unsigned int i=0; i < simEngine->getObstacles().size(); ++i)
            {
                const LineObstacle* obstacle = simEngine->getObstacle(i);
                Vector2D n_w = closestPointLineSegment(obstacle->p1(), obstacle->p2(), _position) - _position;
                float d_w = n_w.lengthSqr();
                
                if (_velocity * n_w <0 || d_w == Sqr(_radius) || d_w > Sqr(_neighborDist)) // Agent is moving away from obstacle, already colliding or obstacle too far away
                    continue;
                
                const float radius = d_w < Sqr(_radius)? sqrtf(d_w):_radius; // correct the radius, if the agent is already colliding
                
                const float a= _velocity*_velocity;
                bool discCollision = false, segmentCollision = false;
                float t_min = _INFTY;
                
                float c, b, discr;
                float b_temp, discr_temp, c_temp, D_temp;
                Vector2D w_temp, w, o1_temp, o2_temp, o_temp, o, w_o;
                
                
                // time-to-collision with disc_1 of the capped rectangle (capsule)
                w_temp = obstacle->p1() - _position;
                b_temp = w_temp*_velocity;
                c_temp = w_temp*w_temp - (radius*radius);
                discr_temp = b_temp*b_temp - a*c_temp;
                if (discr_temp > .0f && (a<-_EPSILON || a>_EPSILON))
                {
                    discr_temp = sqrtf(discr_temp);
                    const float t = (b_temp - discr_temp) / a;
                    if (t>0){
                        t_min = t;
                        b = b_temp;
                        discr = discr_temp;
                        w = w_temp;
                        c = c_temp;
                        discCollision = true;
                    }
                }
                
                // time-to-collision with disc_2 of the capsule
                w_temp = obstacle->p2() - _position;
                b_temp = w_temp*_velocity;
                c_temp = w_temp*w_temp - (radius*radius);
                discr_temp = b_temp*b_temp - a*c_temp;
                if (discr_temp > .0f && (a<-_EPSILON || a>_EPSILON))
                {
                    discr_temp = sqrtf(discr_temp);
                    const float t = (b_temp - discr_temp) / a;
                    if (t>0 && t < t_min){
                        t_min = t;
                        b = b_temp;
                        discr = discr_temp;
                        w = w_temp;
                        c = c_temp;
                        discCollision = true;
                    }
                }
                
                // time-to-collision with segment_1 of the capsule
                o1_temp = obstacle->p1() + radius * obstacle->normal();
                o2_temp = obstacle->p2() + radius * obstacle->normal();
                o_temp = o2_temp - o1_temp;
                
                D_temp = det(_velocity, o_temp);
                if (D_temp != 0)   
                {
                    float inverseDet = 1.0f/D_temp;	
                    float t = det(o_temp, _position - o1_temp) * inverseDet;
                    float s = det(_velocity,_position - o1_temp) * inverseDet;
                    if (t>0 && s>=0 && s <=1 && t<t_min)
                    {
                        t_min = t;
                        o = o_temp;
                        w_o = _position - o1_temp;
                        discCollision = false;
                        segmentCollision = true;
                    }
                }
                
                // time-to-collision with segment_2 of the capsule
                o1_temp = obstacle->p1() - radius * obstacle->normal();
                o2_temp = obstacle->p2() - radius * obstacle->normal();
                o_temp = o2_temp - o1_temp;
                
                D_temp = det(_velocity, o_temp);
                if (D_temp != 0)   
                {
                    float inverseDet = 1.0f/D_temp;	
                    float t = det(o_temp, _position - o1_temp) * inverseDet;
                    float s = det(_velocity,_position - o1_temp) * inverseDet;
                    if (t>0 && s>=0 && s <=1 && t<t_min)
                    {
                        t_min = t;
                        o = o_temp;
                        w_o = _position - o1_temp;
                        discCollision = false;
                        segmentCollision = true;
                    }
                }
                
                if (discCollision)
                {
                    _F += -_k*exp(-t_min/_t0)*(_velocity - (b*_velocity - a*w)/discr)/(a*powf(t_min,_m))*(_m/t_min + 1/_t0);		
                }
                else if (segmentCollision)
                {
                    _F += _k*exp(-t_min/_t0)/(powf(t_min,_m)*det(_velocity,o))*(_m/t_min + 1/_t0)*Vector2D(-o.y, o.x);
                }
                
            }
        }
        
        Vec3 computeForceWithOtherPlayer(Player *other) {
            // if agents are actually colliding use their separation distance
            if (distanceSq < radiusSq)
                radiusSq  = Sqr(other->radius() + _radius - sqrtf(distanceSq));
            
            const Vec3 w = position() - other->position();
            const Vector2D v = velocity() - other->velocity();
            
            // here we compute the pieces of the formula
            const float a = v*v;
            const float b = w*v;
            const float c = w*w - radiusSq;
            float discr = b*b - a*c;
            
            if (discr > .0f && (a < -_EPSILON || a > _EPSILON))
            {
                discr = sqrtf(discr);
                
                // the time until collision (key parameter). Stephen Guy's paper demonstrates this is
                // the key parameter.
                const float t = (b - discr) / a;
                
                if (t > 0){
                    // now we compute the spatial derivative of the exponential potential function
                    return -K * exp(-t / EXP_CUTOFF)*(v - (b*v - a * w) / discr)/(a * powf(t, POWER_LAW_EXP)) * (POWER_LAW_EXP / t + 1 / EXP_CUTOFF);
                }
            }
        }
        
        // generates a repulsive potential force for the close neighbors
        Vec3 avoidCloseNeighbors(const float neighborhoodDist) {
            // we find close neighbors and compute the "social repulsive forces"
            proximityToken->findNeighbors(position(), neighborhoodDist, neighbors);
            
            Vec3 totalAvoidanceForce;
            
            for (AVIterator other = neighbors.begin(); other != neighbors.end(); other++) {
                // we need double de-reference because neighbors is an iterator of pointers.
                totalAvoidanceForce += computeForceWithOtherPlayer(**other);
            }
            
            return totalAvoidanceForce;
        }
        
        // generates a repulsive potential force for the walls
        Vec3 avoidWalls() {
            
        }
        
        // per frame simulation update
        // (parameter names commented out to prevent compiler warning from "-W")
        void update (const float /*currentTime*/, const float elapsedTime)
        {
            // try to avoid others via social force model (we override this method)
            Vec3 avoidCollideForce = avoidCloseNeighbors(NEIGHBORHOOD_DIST);
            
            // TODO: I worry that 'applySteeringForce' is doing too much post processing on the force.
            // first priority is to avoid running into people
            if (avoidCollideForce != Vec3::zero) {
                applySteeringForce(avoidCollideForce, elapsedTime);
                
            } else {
                // avoid the wall force
                Vec3 avoidWallForce = avoidWallForce();
                
                if (avoidWallForce != Vec3::zero) {
                    applySteeringForce(avoidWallForce, elapsedTime);
                } else {
                    // second priority is to aim for our target destination
                    // TODO: I'm not sure *why* we divide by the relaxation time
                    Vec3 goalForce = xxxsteerForSeek(m_goalTarget) / RELAXATION_TIME;
                    applySteeringForce(goalForce, elapsedTime);
                }
                
            }
            
            m_proximityToken->updateForNewPosition(position());
            
        }
        
        // draw this character/vehicle into the scene
        void draw (void)
        {
            // TODO: remove that annoying radius around each guy
            drawBasic2dCircularVehicle(*this, Color(1.0f, 0.0f, 0.0f));
        }

        // the box we must stay inside
        AABBox *m_boundingBox;
        
        // the target we are trying to pursue
        Vec3	m_goalTarget;
        
        // closest neighbors
        AVGroup m_neighbors;
        
        // our token for searching for nearest neighbors
        ProximityToken *m_proximityToken;
    };
    
    
    
    // ----------------------------------------------------------------------------
    // PlugIn for OpenSteer demo
    class SocialForcePlugin : public PlugIn
    {
    public:
        
        const char* name (void) {return "Social Force";}
        
        // float selectionOrderSortKey (void) {return 0.06f;}
        
        // bool requestInitialSelection() { return true;}
        
        // be more "nice" to avoid a compiler warning
        virtual SocialForcePlugin() {}
        
        void open (void)
        {
            // Make a field (note that y is the vertical axis)
            // you only specify the two corners of the box
            m_bbox = new AABBox(Vec3(-20,0,-10), Vec3(20,0,10));
            
            // construct a database for tracking close particles
            const Vec3 center;
            const Vec3 dimensions();
            const Vec3 divisions(TOTAL_DIVISIONS, TOTAL_DIVISIONS, TOTAL_DIVISIONS);
            
            m_proximityDatabase = new LQProximityDatabase<Player*>(center, dimensions, divisions);

            for (unsigned int i = 0; i < TOTAL_PLAYERS; i++)
            {
                Player *player = new Player(m_proximityDatabase);
                OpenSteerDemo::selectedVehicle = player;
                m_allPlayers.push_back(player);
            
            }
            
            
            // initialize camera
            OpenSteerDemo::init2dCamera (*OpenSteerDemo::selectedVehicle);
            OpenSteerDemo::camera.setPosition (10, OpenSteerDemo::camera2dElevation, 10);
            OpenSteerDemo::camera.fixedPosition.set (40, 40, 40);
            OpenSteerDemo::camera.mode = Camera::cmFixed;

        }
        
        void update (const float currentTime, const float elapsedTime)
        {
            

            // update simulation of test vehicle
            for(Player* player = m_allPlayers.begin(); player != m_allPlayers.end(); player++) {
                player->update(currentTime, elapsedTime);
            }
            
        }
        
        void redraw (const float currentTime, const float elapsedTime)
        {
            // draw test vehicle
            for (Player *player = m_allPlayers.begin(); player != m_allPlayers.end(); player++) {
                player->draw();
            }
            
            m_bbox->draw();
            
            // update camera, tracking test vehicle
            OpenSteerDemo::updateCamera (currentTime, elapsedTime, *OpenSteerDemo::selectedVehicle);
            
            // draw "ground plane"
            OpenSteerDemo::gridUtility (Vec3(0,0,0));
        }
        
        void close (void)
        {
            for (Player *player = m_allPlayers.begin(); player != m_allPlayers.end(); player++) {
                delete player[i];
            }
            
            m_allPlayers.clear();
        }
        
        void reset (void)
        {
            for (Player *player = m_allPlayers.begin(); player != m_allPlayers.end(); player++) {
                player->reset();
            }

        }
        
        const AVGroup& allVehicles (void) {
            return (const AVGroup&) m_allPlayers;
        }
        
        // a list of all players
        std::vector<Player*> m_allPlayers;
        
        // the field bounding box
        AABBox	*m_bbox;
        
        // the database we use for proximity queries between players
        ProximityDatabase *m_proximityDatabase;
    };
    
    
    SocialForcePlugin pSocialForcePlugin;
    
    
    
    
    // ----------------------------------------------------------------------------
    
} // anonymous namespace