155 lines
4.5 KiB
C++
155 lines
4.5 KiB
C++
/*****************************************************************/
|
|
/* NAME: Michael Benjamin and John Leonard */
|
|
/* ORGN: NAVSEA Newport RI and MIT Cambridge MA */
|
|
/* FILE: AOF_SimpleWaypoint.cpp */
|
|
/* DATE: Feb 22th 2009 */
|
|
/*****************************************************************/
|
|
|
|
#ifdef _WIN32
|
|
#pragma warning(disable : 4786)
|
|
#pragma warning(disable : 4503)
|
|
#endif
|
|
#include <math.h>
|
|
#include "AOF_SimpleWaypoint.h"
|
|
#include "AngleUtils.h"
|
|
#include "GeomUtils.h"
|
|
|
|
using namespace std;
|
|
|
|
//----------------------------------------------------------
|
|
// Procedure: Constructor
|
|
|
|
AOF_SimpleWaypoint::AOF_SimpleWaypoint(IvPDomain g_domain) : AOF(g_domain)
|
|
{
|
|
// Unitialized cache values for later use in evalBox calls
|
|
m_min_speed = 0;
|
|
m_max_speed = 0;
|
|
m_angle_to_wpt = 0;
|
|
|
|
// Initialization parameters
|
|
m_osx = 0; // ownship x-position
|
|
m_osy = 0; // ownship y-position
|
|
m_ptx = 0; // waypoint x-position
|
|
m_pty = 0; // waypoint y-position
|
|
m_desired_spd = 0;
|
|
|
|
// Initialization parameter flags
|
|
m_osy_set = false;
|
|
m_osx_set = false;
|
|
m_pty_set = false;
|
|
m_ptx_set = false;
|
|
m_desired_spd_set = false;
|
|
}
|
|
|
|
//----------------------------------------------------------------
|
|
// Procedure: setParam
|
|
|
|
bool AOF_SimpleWaypoint::setParam(const string& param, double param_val)
|
|
{
|
|
if(param == "osy") {
|
|
m_osy = param_val;
|
|
m_osy_set = true;
|
|
return(true);
|
|
}
|
|
else if(param == "osx") {
|
|
m_osx = param_val;
|
|
m_osx_set = true;
|
|
return(true);
|
|
}
|
|
else if(param == "pty") {
|
|
m_pty = param_val;
|
|
m_pty_set = true;
|
|
return(true);
|
|
}
|
|
else if(param == "ptx") {
|
|
m_ptx = param_val;
|
|
m_ptx_set = true;
|
|
return(true);
|
|
}
|
|
else if(param == "desired_speed") {
|
|
m_desired_spd = param_val;
|
|
m_desired_spd_set = true;
|
|
return(true);
|
|
}
|
|
else
|
|
return(false);
|
|
}
|
|
|
|
//----------------------------------------------------------------
|
|
// Procedure: initialize
|
|
|
|
bool AOF_SimpleWaypoint::initialize()
|
|
{
|
|
// Check for failure conditions
|
|
if(!m_osy_set || !m_osx_set || !m_pty_set || !m_ptx_set || !m_desired_spd_set)
|
|
return(false);
|
|
if(!m_domain.hasDomain("speed") || !m_domain.hasDomain("course"))
|
|
return(false);
|
|
|
|
// Initialize local variables to cache intermediate calculations
|
|
m_angle_to_wpt = relAng(m_osx, m_osy, m_ptx, m_pty);;
|
|
m_min_speed = m_domain.getVarLow("speed");
|
|
m_max_speed = m_domain.getVarHigh("speed");
|
|
|
|
return(true);
|
|
}
|
|
|
|
//----------------------------------------------------------------
|
|
// Procedure: evalPoint
|
|
// Purpose: Evaluate a candidate point in the decision space
|
|
|
|
double AOF_SimpleWaypoint::evalPoint(const vector<double>& point) const
|
|
{
|
|
// Determine the course and speed being evaluated
|
|
double eval_crs = extract("course", point);
|
|
double eval_spd = extract("speed", point);
|
|
|
|
// Calculate the first score, score_roc, based on rate of closure
|
|
double angle_diff = angle360(eval_crs - m_angle_to_wpt);
|
|
double rad_diff = degToRadians(angle_diff);
|
|
double rate_of_closure = cos(rad_diff) * eval_spd;
|
|
|
|
double roc_range = 2 * m_max_speed;
|
|
double roc_diff = (m_desired_spd - rate_of_closure);
|
|
if(roc_diff < 0)
|
|
roc_diff *= -0.5; // flip the sign, cut the penalty for being over
|
|
if(roc_diff > roc_range)
|
|
roc_diff = roc_range;
|
|
|
|
double pct = (roc_diff / roc_range);
|
|
double score_roc = (1.0 - pct) * 100;
|
|
|
|
// Calculate the second score, score_rod, based on rate of detour
|
|
double angle_180 = angle180(angle_diff);
|
|
if(angle_180 < 0)
|
|
angle_180 *= -1;
|
|
if(eval_spd < 0)
|
|
eval_spd = 0;
|
|
double rate_of_detour = (angle_180 * eval_spd);
|
|
|
|
double rod_range = (m_max_speed * 180);
|
|
double rod_pct = (rate_of_detour / rod_range);
|
|
double score_rod = (1.0 - rod_pct) * 100;
|
|
|
|
// Calculate the third score, score_spd, based on ideal speed.
|
|
double spd_range = m_max_speed - m_desired_spd;
|
|
if((m_desired_spd - m_min_speed) > spd_range)
|
|
spd_range = m_desired_spd - m_min_speed;
|
|
double spd_diff = m_desired_spd - eval_spd;
|
|
if(spd_diff < 0)
|
|
spd_diff *= -1;
|
|
if(spd_diff > spd_range)
|
|
spd_diff = spd_range;
|
|
double spd_pct = (spd_diff / spd_range);
|
|
double score_spd = (1.0 - spd_pct) * 100;
|
|
|
|
// CALCULATE THE COMBINED SCORE
|
|
double combined_score = 0;
|
|
combined_score += (0.75 * score_roc);
|
|
combined_score += (0.2 * score_rod);
|
|
combined_score += (0.05 * score_spd);
|
|
|
|
return(combined_score);
|
|
}
|
|
|