/// <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); }