Avoid.cpp

Avoid.cpp

#include "StdAfx.h"
#include "Avoid.h"
#include "Arrival.h"

float Avoid::g_fKAvoid = 1000.0f;
float Avoid::g_fTAvoid = 5.0f;
float Avoid::g_fDistanceAvoid = 10.0f;

Avoid::Avoid(vec3 target, const std::vector<Obstacle>& obstacles) : 
	Behavior("avoid"), m_pTarget(target), m_pObstacles(obstacles)
{
}

Avoid::Avoid(const Avoid& orig) : 
	Behavior(orig), m_pTarget(orig.m_pTarget), m_pObstacles(orig.m_pObstacles)
{
}

Behavior* Avoid::Clone() const
{
    return new Avoid(*this);
}

Avoid::~Avoid()
{
	 m_pTarget = vec3(0,0,0);
}

vec3 Avoid::CalculateDesiredVelocity(Actor& actor) {
    return CalculateDesiredVelocityTangential(actor);
}

// Given the actor, return a desired velocity in world coordinates
// If an actor is near an obstacle, avoid adds either a tangential or
// normal response velocity
vec3 Avoid::CalculateDesiredVelocityTangential(Actor& actor)
{
    Arrival arrival(m_pTarget);
    vec3 desired;
    vec3 v_arrival = arrival.CalculateDesiredVelocity(actor);
    
    float r_B = 1.2; // radius of the actor
    
    // makes actors stop right at the target
    if(v_arrival.Length() < 5.0f) return v_arrival;

    std::vector<Obstacle>::const_iterator it;
    for(it = m_pObstacles.begin(); it != m_pObstacles.end(); ++it)
	{
		Obstacle obstacle = *it;
        
        vec3 d_world = obstacle.globalPosition - actor.globalPosition;
        vec3 d_local = actor.globalOrientation * d_world; // @todo change this
        float L_B = g_fTAvoid * actor.linearVelocity.Length();
                
        vec3 d_x(d_local[VX], 0, 0); 
        vec3 d_y(0, d_local[VY], 0);
        vec3 nhat_avoid_part = (L_B * d_x / d_x.Length()) - d_local;
        vec3 nhat_avoid = nhat_avoid_part / nhat_avoid_part.Length();
        
        vec3 khat_avoid = (actor.linearVelocity.Cross(d_local)) / 
                          (actor.linearVelocity.Length() * d_local.Length());
        
        vec3 that_avoid = khat_avoid.Cross(nhat_avoid);
        
        float r_0 = obstacle.radius;
        
        // the projected position of the actor 
        vec3 actorNewPosition = actor.globalPosition + actor.linearVelocity;
        
        float rb_r0 = r_B + r_0;

        // default the maginitude at 0
        float v_avoid_mag = 0;
        
        v_avoid_mag = (g_fKAvoid * g_fMaxSpeed) / 
                      (1 + pow((d_local.Length() - rb_r0), 2));
                      
        vec3 v_avoid = v_avoid_mag * that_avoid;
        
        desired += v_avoid;
    
	}
	
	return 0.3 * clippedVelocity(desired) + 0.7 * v_arrival;
}