Behavior.cpp

Behavior.cpp

#include "StdAfx.h"
#include "Behavior.h"
#include <math.h>

//Adjust the gain constants to tune the controller response

float Behavior::g_fMaxSpeed = 10.0f;
float Behavior::g_fMaxAccel = 10.0f;
float Behavior::g_fKNeighborhood = 100.0f;
float Behavior::g_fOriKv = 1.0;  // Orientation
float Behavior::g_fOriKp = 1.0; 
float Behavior::g_fVelKv = 1.0;  // Velocity 
float Behavior::g_fAgentRadius = 2.0;


Behavior::Behavior(const char* name) : m_name(name)
{
}

Behavior::Behavior(const Behavior& orig) : m_name(orig.m_name)
{
}

const std::string& Behavior::GetName() const
{
    return m_name;
}

// Given an actor and desired velocity, calculate a corresponding force
vec3 Behavior::CalculateForce(Actor& actor, const vec3& dvel)
{
    vec3 trueDvel = CalculateDesiredVelocity(actor);
	return actor.mass * (trueDvel - actor.linearVelocity);
}

// Given an actor and desired velocity, calculate a corresponding torque
vec3 Behavior::CalculateTorque(Actor &actor, const vec3& dvel)
{    
	// 1. Get current rotation matrix
    mat3 matCurrentRotation(actor.globalOrientation);
        
	// 2. Construct desired rotation matrix 
    // (This corresponds to facing in the direction of our desired velocity)
	// Note: Z points forwards; Y Points UP; and X points left
    vec3 y(0,1,0), z(dvel), x = y.Cross(z);
    
    mat3 matDesiredRotation(x, y, dvel);
    matDesiredRotation = matDesiredRotation.Transpose();

	//// 3. Compute the change in rotation matrix that will
	//// rotate the actor towards our desired rotation
    
    mat3 matDeltaRotation = matDesiredRotation * matCurrentRotation.Transpose();
        
	//mat3 axisAngleMatrix = desiredRotation * currentRotation.Transpose();
	//
	//// 4. Construct quaternion to get axis and angle from dr
    vec3 axis; float angleRad;
    Quaternion quatDeltaRotation = matDeltaRotation.ToQuaternion();

    quatDeltaRotation.ToAxisAngle(axis, angleRad);
    
    
	//// find torque
    vec3 w = actor.angularVelocity;
    mat3 I = actor.globalInertialTensor;
    vec3 Iw = I * w;
    vec3 diff = g_fOriKp * axis - g_fOriKv * w;

    vec3 torque = w.Cross(Iw) + (I * diff);    
    	
	return torque;
}