Skip to content

Commit

Permalink
Merge pull request #1153 from MuMech/lcg/force-coasts
Browse files Browse the repository at this point in the history 
PVG: force coasts
  • Loading branch information
@lamont-granquist
lamont-granquist committed Mar 20, 2019
2 parents 700b92d + 6365d86 commit 90a0085
Show file tree
Hide file tree
Showing 2 changed files with 59 additions and 101 deletions.
113 changes: 31 additions & 82 deletions MechJeb2/Pontryagin/PontryaginBase.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -896,27 +896,17 @@ public void optimizationFunction(double[] y0, double[] z, object o)
int index = arcIndex(arcs, i, parameters: true);
if (total_bt_bar > y0[0] || (i == arcs.Count -1))
{
if (arcs[i].coast_after_jettison)
{
z[n] = y0[index];
n++;
z[n] = y0[index+1];
n++;
}
else
{
z[n] = y0[index];
n++;
z[n] = y0[index+1];
n++;
}
z[n] = y0[index];
n++;
z[n] = y0[index+1];
n++;
}
else
{
Vector3d r = new Vector3d(y0[index+2], y0[index+3], y0[index+4]);
Vector3d v = new Vector3d(y0[index+5], y0[index+6], y0[index+7]);
Vector3d pv = new Vector3d(y0[index+8], y0[index+9], y0[index+10]);
Vector3d pr = new Vector3d(y0[index+11], y0[index+12], y0[index+13]);
//Vector3d v = new Vector3d(y0[index+5], y0[index+6], y0[index+7]);
//Vector3d pv = new Vector3d(y0[index+8], y0[index+9], y0[index+10]);
//Vector3d pr = new Vector3d(y0[index+11], y0[index+12], y0[index+13]);
double rm = r.magnitude;

Vector3d r2 = new Vector3d(yf[i*13+0], yf[i*13+1], yf[i*13+2]);
Expand All @@ -925,62 +915,12 @@ public void optimizationFunction(double[] y0, double[] z, object o)
Vector3d pr2 = new Vector3d(yf[i*13+9], yf[i*13+10], yf[i*13+11]);
double r2m = r2.magnitude;

double H0t1 = Vector3d.Dot(pr, v) - Vector3d.Dot(pv, r) / (rm * rm * rm);
//double H0t1 = Vector3d.Dot(pr, v) - Vector3d.Dot(pv, r) / (rm * rm * rm);
double H0t2 = Vector3d.Dot(pr2, v2) - Vector3d.Dot(pv2, r2) / (r2m * r2m * r2m);
if (arcs[i].coast_after_jettison)
{
//z[n] = y0[index];
z[n] = ( 1.0 - 1.0 / (y0[index]/1.0e-2 + 1.0) ) * H0t2 * ( 1.0 + 1.0 / ( ( y0[index] - MAX_COAST_TAU ) / 1e-2 - 1.0 ) );

//if (y0[index] <= 0 || y0[index] >= 2)
// z[n] = 0;
//else
// z[n] = H0t2;

// z[n] = H0t2;


//z[n] = y0[index];
// z[n] = pv2.magnitude - pv.magnitude;
}
else
{
/* H0 at the end of the coast = 0 */
z[n] = H0t2;
}

/*
if (i == 0 || i == arcs.Count - 1)
{
// first or last coast
z[n] = Vector3d.Dot(pr2, v2) - Vector3d.Dot(pv2, r2) / (r2m * r2m * r2m);
}
else
{
// interior coasts
z[n] = pv2.magnitude - pv.magnitude;
}
*/

z[n] = H0t2;
n++;

if (arcs[i].coast_after_jettison)
{
//z[n] = ( y0[index] < 0 ) ? y0[index] - y0[index+1] * y0[index+1] : y0[index+1] * y0[index+1];
//z[n] = y0[index] - Math.Abs(y0[index+1]); // * y0[index+1];
z[n] = y0[index+1];
//n++;
//z[n] = ( y0[index] > 2 ) ? y0[index] - 2 + y0[index+2] * y0[index+2] : y0[index+2] * y0[index+2];
//z[n] = y0[index] - 2 + Math.Abs(y0[index+2]); // * y0[index+2];
//z[n] = y0[index+2];
}
else
{
//if ( y0[index] < 0 )
// z[n-1] = 0;
z[n] = y0[index+1];
// z[n] = y0[index] - y0[index+1] * y0[index+1];
}
z[n] = y0[index+1];
n++;
}
}
Expand Down Expand Up @@ -1042,16 +982,6 @@ public bool runOptimizer(List<Arc> arcs)

bndl[0] = 0;

for(int i = 0; i < arcs.Count; i++)
{
if (arcs[i].coast_after_jettison)
{
int j = arcIndex(arcs, i, parameters: true);
bndl[j] = 0.0;
bndu[j] = MAX_COAST_TAU;
}
}

alglib.minlmstate state;
alglib.minlmreport rep = new alglib.minlmreport();
alglib.minlmcreatev(y0.Length, y0, lmDiffStep, out state); /* y0.Length must == z.Length returned by the BC function for square problems */
Expand Down Expand Up @@ -1393,6 +1323,18 @@ public bool threadStart(double t0)
}
}

Queue<string> logQueue = new Queue<string>();
Mutex mut = new Mutex();

// lets us Debug.Log events from the computation thread in a thread-safe fashion
//
protected void DebugLog(string s)
{
mut.WaitOne();
logQueue.Enqueue(s);
mut.ReleaseMutex();
}

// need to call this every tick or so in order to dump exceptions to the log from
// the main thread since Debug.Log is not threadsafe in Unity. (this must also
// obviously be kept safe to call every tick and must not update any inputs from
Expand All @@ -1403,20 +1345,27 @@ public void Janitorial()
if ( last_alglib_exception != null )
{
alglib.alglibexception e = last_alglib_exception;
last_alglib_exception = null;
Debug.Log("An exception occurred: " + e.GetType().Name);
Debug.Log("Message: " + e.Message);
Debug.Log("MSG: " + e.msg);
Debug.Log("Stack Trace:\n" + e.StackTrace);
last_alglib_exception = null;
}
if ( last_exception != null )
{
Exception e = last_exception;
last_exception = null;
Debug.Log("An exception occurred: " + e.GetType().Name);
Debug.Log("Message: " + e.Message);
Debug.Log("Stack Trace:\n" + e.StackTrace);
last_exception = null;
}

mut.WaitOne();
while ( logQueue.Count > 0 )
{
Debug.Log(logQueue.Dequeue());
}
mut.ReleaseMutex();
}

public void KillThread()
Expand Down
47 changes: 28 additions & 19 deletions MechJeb2/Pontryagin/PontryaginLaunch.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,18 +13,22 @@ public PontryaginLaunch(MechJebCore core, double mu, Vector3d r0, Vector3d v0, V

double rTm;
double vTm;
double gamma;
double inc;
double gammaT;
double incT;
double smaT;
double eccT;
double LANT;
double ArgPT;
Vector3d hT;
int numStages;

// 5-constraint PEG with fixed LAN
public void flightangle5constraint(double rTm, double vTm, double gamma, Vector3d hT)
public void flightangle5constraint(double rTm, double vTm, double gammaT, Vector3d hT)
{
QuaternionD rot = Quaternion.Inverse(Planetarium.fetch.rotation);
this.rTm = rTm / r_scale;
this.vTm = vTm / v_scale;
this.gamma = gamma;
this.gammaT = gammaT;
this.hT = rot * hT / r_scale / v_scale;
bcfun = flightangle5constraint;
}
Expand All @@ -43,7 +47,7 @@ private void flightangle5constraint(double[] yT, double[] z, bool terminal)
if (!terminal)
{
z[0] = ( rf.magnitude * rf.magnitude - rTm * rTm ) / 2.0;
z[1] = Vector3d.Dot(rf, vf) - rf.magnitude * vf.magnitude * Math.Sin(gamma);
z[1] = Vector3d.Dot(rf, vf) - rf.magnitude * vf.magnitude * Math.Sin(gammaT);
z[2] = hmiss[0];
z[3] = hmiss[1];
z[4] = hmiss[2];
Expand All @@ -66,8 +70,8 @@ public void flightangle4constraint(double rTm, double vTm, double gamma, double
this.vTm = vTm / v_scale;
//Debug.Log("4constraint vTm = " + vTm + " v_scale = " + v_scale + " vTm_bar = " + this.vTm );
//Debug.Log("4constraint rTm = " + rTm + " r_scale = " + r_scale + " rTm_bar = " + this.rTm );
this.gamma = gamma;
this.inc = inc;
this.gammaT = gamma;
this.incT = inc;
bcfun = flightangle4constraint;
}

Expand All @@ -87,15 +91,15 @@ private void flightangle4constraint(double[] yT, double[] z, bool terminal)
{
z[0] = ( rf.magnitude * rf.magnitude - rTm * rTm ) / 2.0;
z[1] = ( vf.magnitude * vf.magnitude - vTm * vTm ) / 2.0;
z[2] = Vector3d.Dot(n, hf) - hf.magnitude * Math.Cos(inc);
z[3] = Vector3d.Dot(rf, vf) - rf.magnitude * vf.magnitude * Math.Sin(gamma);
z[4] = rTm * rTm * ( Vector3d.Dot(vf, prf) - vTm * Math.Sin(gamma) / rTm * Vector3d.Dot(rf, prf) ) -
vTm * vTm * ( Vector3d.Dot(rf, pvf) - rTm * Math.Sin(gamma) / vTm * Vector3d.Dot(vf, pvf) );
z[2] = Vector3d.Dot(n, hf) - hf.magnitude * Math.Cos(incT);
z[3] = Vector3d.Dot(rf, vf) - rf.magnitude * vf.magnitude * Math.Sin(gammaT);
z[4] = rTm * rTm * ( Vector3d.Dot(vf, prf) - vTm * Math.Sin(gammaT) / rTm * Vector3d.Dot(rf, prf) ) -
vTm * vTm * ( Vector3d.Dot(rf, pvf) - rTm * Math.Sin(gammaT) / vTm * Vector3d.Dot(vf, pvf) );
z[5] = Vector3d.Dot(hf, prf) * Vector3d.Dot(hf, rn) + Vector3d.Dot(hf, pvf) * Vector3d.Dot(hf, vn);
}
else
{
double hTm = rTm * vTm * Math.Cos(gamma);
double hTm = rTm * vTm * Math.Cos(gammaT);

z[0] = hf.magnitude - hTm;
z[1] = z[2] = z[3] = z[4] = z[5] = 0.0;
Expand Down Expand Up @@ -195,30 +199,35 @@ public override void Bootstrap(double t0)

//Debug.Log("optimizer done");

new_sol = new Solution(t_scale, v_scale, r_scale, t0);
multipleIntegrate(y0, new_sol, arcs, 10);

//for(int k = 0; k < y0.Length; k++)
// Debug.Log("y0[" + k + "] = " + y0[k]);

if (insertedCoast)
{
if ( new_sol.tgo(new_sol.t0, arcs.Count-2) < 1 )
new_sol = new Solution(t_scale, v_scale, r_scale, t0);
multipleIntegrate(y0, new_sol, arcs, 10);

double coastlen = new_sol.tgo(new_sol.t0, arcs.Count-2); // human seconds

if ( coastlen < 1 )
{
// coast is less than one second, delete it and reconverge
DebugLog("optimum coast of " + coastlen + " seconds was removed from the solution");

RemoveArc(arcs, arcs.Count-2, new_sol);

if ( !runOptimizer(arcs) )
{
Fatal("failed to converge after removing zero length coast");
Fatal("failed to converge after removing negative length coast after jettison");
return;
}

new_sol = new Solution(t_scale, v_scale, r_scale, t0);
multipleIntegrate(y0, new_sol, arcs, 10);
}
}

new_sol = new Solution(t_scale, v_scale, r_scale, t0);
multipleIntegrate(y0, new_sol, arcs, 10);

//for(int k = 0; k < y0.Length; k++)
//Debug.Log("new y0[" + k + "] = " + y0[k]);

Expand Down

0 comments on commit 90a0085

Please sign in to comment.