public static CoordLocator[,] GenerateCoordLocators(float[,] values)
{
int width = values.GetUpperBound(0);
int height = values.GetUpperBound(1);
CoordLocator[,] coordLocators = new CoordLocator[width + 1, height + 1];
for (int i = 0; i <= width; i++)
{
for (int j = 0; j <= height; j++)
{
coordLocators[i, j] = new CoordLocator(i, j, values[i, j]);
}
}
return(coordLocators);
}
private void UpdateGraphs()
{
float bottom = currentConditions.lowerBoundAltitude;
float top = currentConditions.upperBoundAltitude;
float left = currentConditions.lowerBoundSpeed;
float right = currentConditions.upperBoundSpeed;
Func <EnvelopePoint, float> scale = (pt) => 1;
if (WindTunnelSettings.UseCoefficients)
{
scale = (pt) => 1 / pt.dynamicPressure;
}
((SurfGraph)graphables["Excess Thrust"]).SetValues(envelopePoints.SelectToArray(pt => pt.Thrust_excess), left, right, bottom, top);
((SurfGraph)graphables["Excess Acceleration"]).SetValues(envelopePoints.SelectToArray(pt => pt.Accel_excess), left, right, bottom, top);
((SurfGraph)graphables["Thrust Available"]).SetValues(envelopePoints.SelectToArray(pt => pt.Thrust_available), left, right, bottom, top);
((SurfGraph)graphables["Level AoA"]).SetValues(envelopePoints.SelectToArray(pt => pt.AoA_level * Mathf.Rad2Deg), left, right, bottom, top);
((SurfGraph)graphables["Max Lift AoA"]).SetValues(envelopePoints.SelectToArray(pt => pt.AoA_max * Mathf.Rad2Deg), left, right, bottom, top);
((SurfGraph)graphables["Max Lift"]).SetValues(envelopePoints.SelectToArray(pt => pt.Lift_max), left, right, bottom, top);
((SurfGraph)graphables["Lift/Drag Ratio"]).SetValues(envelopePoints.SelectToArray(pt => pt.LDRatio), left, right, bottom, top);
((SurfGraph)graphables["Drag"]).SetValues(envelopePoints.SelectToArray(pt => pt.drag * scale(pt)), left, right, bottom, top);
((SurfGraph)graphables["Lift Slope"]).SetValues(envelopePoints.SelectToArray(pt => pt.dLift / pt.dynamicPressure), left, right, bottom, top);
((SurfGraph)graphables["Pitch Input"]).SetValues(envelopePoints.SelectToArray(pt => pt.pitchInput), left, right, bottom, top);
((SurfGraph)graphables["Fuel Burn Rate"]).SetValues(envelopePoints.SelectToArray(pt => pt.fuelBurnRate), left, right, bottom, top);
//((SurfGraph)graphables["Stability Derivative"]).SetValues(envelopePoints.SelectToArray(pt => pt.stabilityDerivative), left, right, bottom, top);
//((SurfGraph)graphables["Stability Range"]).SetValues(envelopePoints.SelectToArray(pt => pt.stabilityRange), left, right, bottom, top);
//((SurfGraph)graphables["Stability Score"]).SetValues(envelopePoints.SelectToArray(pt => pt.stabilityScore), left, right, bottom, top);
float[,] economy = envelopePoints.SelectToArray(pt => pt.fuelBurnRate / pt.speed * 1000 * 100);
int stallpt = CoordLocator.GenerateCoordLocators(envelopePoints.SelectToArray(pt => pt.Thrust_excess)).First(0, 0, c => c.value >= 0);
SurfGraph toModify = (SurfGraph)graphables["Fuel Economy"];
toModify.SetValues(economy, left, right, bottom, top);
float minEconomy = economy[stallpt, 0] / 3;
toModify.ZMax = minEconomy;
((OutlineMask)graphables["Envelope Mask"]).SetValues(envelopePoints.SelectToArray(pt => pt.Thrust_excess), left, right, bottom, top);
}
private static List <AscentPathPoint> GetOptimalPath(Conditions conditions, float exitSpeed, float exitAlt, float initialSpeed, float initialAlt, CostIncreaseFunction costIncreaseFunc, Predicate <float> neighborPredicate, float[,] predicateData, CostIncreaseFunction timeFunc)
{
System.Diagnostics.Stopwatch stopwatch = new System.Diagnostics.Stopwatch();
System.Diagnostics.Stopwatch profileWatch = new System.Diagnostics.Stopwatch();
long[] sections = new long[3];
exitSpeed = Math.Min(exitSpeed, conditions.upperBoundSpeed);
exitAlt = Math.Min(exitAlt, conditions.upperBoundAltitude);
initialSpeed = Math.Max(initialSpeed, conditions.lowerBoundSpeed);
initialAlt = Math.Max(initialAlt, conditions.lowerBoundAltitude);
stopwatch.Start();
EnvelopePointExtensions.UniqueQueue <PathSolverCoords> queue = new EnvelopePointExtensions.UniqueQueue <PathSolverCoords>(500);
PathSolverCoords baseCoord = new PathSolverCoords(conditions.lowerBoundSpeed, conditions.lowerBoundAltitude, conditions.stepSpeed, conditions.stepAltitude, conditions.XResolution, conditions.YResolution);
float rangeX = (conditions.upperBoundSpeed - conditions.lowerBoundSpeed) - conditions.stepSpeed;
float rangeY = (conditions.upperBoundAltitude - conditions.lowerBoundAltitude) - conditions.stepAltitude;
profileWatch.Start();
float[,] costMatrix = new float[baseCoord.width, baseCoord.height];
costMatrix.SetAll(float.MaxValue);
baseCoord = new PathSolverCoords(exitSpeed, exitAlt, baseCoord);
if (!neighborPredicate(predicateData.Lerp2(exitSpeed / rangeX, exitAlt / rangeY)))
{
IEnumerator <CoordLocator> exitCoordFinder = CoordLocator.GenerateCoordLocators(predicateData).GetTaxicabNeighbors(baseCoord.xi, baseCoord.yi, -1, Linq2.Quadrant.II, Linq2.Quadrant.III, Linq2.Quadrant.IV);
while (exitCoordFinder.MoveNext() && !neighborPredicate(exitCoordFinder.Current.value))
{
}
baseCoord = new PathSolverCoords(exitCoordFinder.Current.x, exitCoordFinder.Current.y, baseCoord);
exitSpeed = baseCoord.x; exitAlt = baseCoord.y;
}
costMatrix[baseCoord.xi, baseCoord.yi] = 0;
foreach (PathSolverCoords c in baseCoord.GetNeighbors(neighborPredicate, predicateData))
{
queue.Enqueue(c);
}
PathSolverCoords coord;
while (queue.Count > 0)
{
coord = queue.Dequeue();
List <PathSolverCoords> neighbors = coord.GetNeighbors(neighborPredicate, predicateData);
PathSolverCoords bestNeighbor = neighbors[0];
float bestCost = costMatrix[bestNeighbor.xi, bestNeighbor.yi];
for (int i = neighbors.Count - 1; i >= 1; i--)
{
if (costMatrix[neighbors[i].xi, neighbors[i].yi] < bestCost)
{
bestNeighbor = neighbors[i];
bestCost = costMatrix[bestNeighbor.xi, bestNeighbor.yi];
}
}
float newCost = bestCost + costIncreaseFunc(coord, bestNeighbor);
if (newCost < costMatrix[coord.xi, coord.yi])
{
costMatrix[coord.xi, coord.yi] = newCost;
neighbors.Remove(bestNeighbor);
for (int i = neighbors.Count - 1; i >= 0; i--)
{
if (costMatrix[neighbors[i].xi, neighbors[i].yi] > newCost)
{
queue.Enqueue(neighbors[i]);
}
}
}
}
profileWatch.Stop();
sections[0] = profileWatch.ElapsedMilliseconds;
profileWatch.Reset();
profileWatch.Start();
float[,] gradientx = new float[baseCoord.width - 1, baseCoord.height - 1];
float[,] gradienty = new float[baseCoord.width - 1, baseCoord.height - 1];
for (int i = baseCoord.width - 2; i >= 0; i--)
{
for (int j = baseCoord.height - 2; j >= 0; j--)
{
gradientx[i, j] = ((costMatrix[i + 1, j] - costMatrix[i, j]) + (costMatrix[i + 1, j + 1] - costMatrix[i, j + 1])) / 2 / conditions.stepAltitude;
gradienty[i, j] = ((costMatrix[i, j + 1] - costMatrix[i, j]) + (costMatrix[i + 1, j + 1] - costMatrix[i + 1, j])) / 2 / conditions.stepSpeed;
}
}
profileWatch.Stop();
sections[1] = profileWatch.ElapsedMilliseconds;
profileWatch.Reset();
profileWatch.Start();
/*new Graphing.SurfGraph(costMatrix, conditions.lowerBoundSpeed, conditions.upperBoundSpeed, conditions.lowerBoundAltitude, conditions.upperBoundAltitude).
* WriteToFile("costMatrix");
* new Graphing.SurfGraph(gradientx, conditions.lowerBoundSpeed + conditions.stepSpeed / 2, conditions.upperBoundSpeed - conditions.stepSpeed / 2, conditions.lowerBoundAltitude + conditions.stepAltitude / 2, conditions.upperBoundAltitude - conditions.stepAltitude / 2).
* WriteToFile("gradientX");
* new Graphing.SurfGraph(gradienty, conditions.lowerBoundSpeed + conditions.stepSpeed / 2, conditions.upperBoundSpeed - conditions.stepSpeed / 2, conditions.lowerBoundAltitude + conditions.stepAltitude / 2, conditions.upperBoundAltitude - conditions.stepAltitude / 2).
* WriteToFile("gradientY");*/
List <AscentPathPoint> result = new List <AscentPathPoint>(300);
int iter = -1;
coord = new PathSolverCoords(initialSpeed, initialAlt, baseCoord);
coord = new PathSolverCoords(costMatrix.First(0, 0, f => f >= initialSpeed && f < float.MaxValue / 100) + 1, coord.yi, baseCoord);
PathSolverCoords lastCoord = coord;
float lastCost = 0, lastTime = 0;
while (true)
{
iter++;
if (iter % 10 == 0)
{
//result.Add(new EnvelopePoint(vessel, conditions.body, coord.y, coord.x));
if (result.Count > 0)
{
lastCost = result[result.Count - 1].cost;
lastTime = result[result.Count - 1].time;
}
result.Add(new AscentPathPoint(coord.x, coord.y, costIncreaseFunc(coord, lastCoord) + lastCost, (coord.y - lastCoord.y) / timeFunc(coord, lastCoord), timeFunc(coord, lastCoord) + lastTime));
float dx = (coord.x - lastCoord.x) / rangeX;
float dy = (coord.y - lastCoord.y) / rangeY;
float r = Mathf.Sqrt(dx * dx + dy * dy);
//Debug.LogFormat("{0}\t{1}\t{2}", coord.x, coord.y, r);
lastCoord = coord;
if (r < 0.00001f && iter > 0)
{
break;
}
}
float xW = (coord.x - conditions.stepSpeed / 2) / rangeX;
float yW = (coord.y - conditions.stepAltitude / 2) / rangeY;
if (Math.Abs(coord.x - exitSpeed) < 10 && Math.Abs(coord.y - exitAlt) < 100)
{
break;
}
try
{
Vector2 d = new Vector2(gradientx.Lerp2(xW, yW), gradienty.Lerp2(xW, yW));
if (d.sqrMagnitude <= 0)
{
break;
}
float step = 5 / Mathf.Sqrt(d.x * d.x + (d.y * conditions.stepSpeed / conditions.stepAltitude) * (d.y * conditions.stepSpeed / conditions.stepAltitude));
if (coord.y + -d.y * step < 0)
{
coord = coord.Offset(-d.x * step, -coord.y);
}
else
{
coord = coord.Offset(-d * step);
}
}
catch (Exception)
{
Debug.Log("Exception in gradient finding.");
Debug.Log(iter);
Debug.Log("xW: " + xW + " yW: " + yW);
throw;
}
}
coord = new PathSolverCoords(exitSpeed, exitAlt, lastCoord);
if (result.Count > 0)
{
lastCost = result[result.Count - 1].cost;
lastTime = result[result.Count - 1].time;
}
result.Add(new AscentPathPoint(coord.x, coord.y, costIncreaseFunc(coord, lastCoord) + lastCost, (coord.y - lastCoord.y) / timeFunc(coord, lastCoord), timeFunc(coord, lastCoord) + lastTime));
profileWatch.Stop();
stopwatch.Stop();
Debug.Log("Time: " + stopwatch.ElapsedMilliseconds + " Iterations: " + iter);
Debug.Log("Costing: " + sections[0] + " Gradients: " + sections[1] + " Minimizing: " + profileWatch.ElapsedMilliseconds);
return(result);
}
