/// <summary>
/// Compute and return fitness for the given parameters.
/// </summary>
/// <param name="x">Candidate solution.</param>
public override double Fitness(double[] x)
{
Vector3 desiredPosition = new Vector3((float)x [0], (float)x [1], (float)x [2]);
foreach (Cost c in costManager.GetCosts())
{
c.SetDesiredPosition(desiredPosition);
}
double cost = costManager.GetTotalCost();
// If it's less than zero it must have wrapped around, set it to max
if (cost < 0)
{
cost = double.MaxValue;
}
return(cost);
}
/**
* This is abstracted out from the objective function so we can debug the cost outside of the Nelson-Mead optimization
* */
private double getCost(Vector3 unconstrainedPosition)
{
// Constrain position to only possible movements with current velocity
// TODO: REMOVE HARDCODING OF VELOCITY
Vector3 desiredPosition = Vector3.MoveTowards(this.sensorModule.gps.position, unconstrainedPosition, 10 * Time.deltaTime);
foreach (Cost c in costManager.GetCosts())
{
c.SetDesiredPosition(desiredPosition);
}
double cost = costManager.GetTotalCost();
// If it's less than zero it must have wrapped around, set it to max
if (cost < 0)
{
cost = double.MaxValue;
}
return(cost);
}
