public FARRungeKutta4(double endTime, double dt, FARMatrix eqns, double[] initCond)
{
// b.Add(0.5f, 0, 1);
// b.Add(0.5f, 1, 2);
// b.Add(1, 2, 3);
this.endTime = endTime;
this.dt = dt;
this.stateEquations = eqns;
this.initCond = initCond;
soln = new double[initCond.Length, (int)Math.Ceiling(endTime / dt)];
time = new double[(int)Math.Ceiling(endTime / dt)];
}
private void RunTransientSimLongitudinal(double endTime, double initDt, double[] InitCond)
{
FARMatrix A = new FARMatrix(4, 4);
FARMatrix B = new FARMatrix(1, 4);
MonoBehaviour.print("Init Dump...");
A.PrintToConsole();
MonoBehaviour.print("Deriv Dump...");
int i = 0;
int j = 0;
int num = 0;
foreach (double f in MOI_stabDerivs)
{
if (num < 3 || num >= 15)
{
num++; //Avoid Ix, Iy, Iz
continue;
}
else
num++;
MonoBehaviour.print(i + "," + j);
if (i <= 2)
if (num == 10)
A.Add(f + u0, i, j);
else
A.Add(f, i, j);
else
B.Add(f, 0, j);
if (j < 2)
j++;
else
{
j = 0;
i++;
}
}
A.Add(-9.81f, 3, 1);
A.Add(1, 2, 3);
A.PrintToConsole(); //We should have an array that looks like this:
B.PrintToConsole();
/* i --------------->
* j [ Z w , Z u , Z q , 0 ]
* | [ X w , X u , X q , -g ]
* | [ M w , M u , M q , 0 ]
* \ / [ 0 , 0 , 1 , 0 ]
* V //And one that looks like this:
*
* [ Z e ]
* [ X e ]
* [ M e ]
* [ 0 ]
*
*
*/
FARRungeKutta4 transSolve = new FARRungeKutta4(endTime, initDt, A, InitCond);
MonoBehaviour.print("Runge-Kutta 4 init...");
transSolve.Solve();
MonoBehaviour.print("Solved...");
graph.Clear();
double[] yVal = transSolve.GetSolution(0);
MonoBehaviour.print("Got 0");
for (int k = 0; k < yVal.Length; k++)
if (yVal[k] > 50)
yVal[k] = 50;
else if (yVal[k] < -50)
yVal[k] = -50;
graph.AddLine("w", transSolve.time, yVal, Color.green);
yVal = transSolve.GetSolution(1);
MonoBehaviour.print("Got 1");
for (int k = 0; k < yVal.Length; k++)
if (yVal[k] > 50)
yVal[k] = 50;
else if (yVal[k] < -50)
yVal[k] = -50;
graph.AddLine("u", transSolve.time, yVal, Color.yellow);
yVal = transSolve.GetSolution(2);
for (int k = 0; k < yVal.Length; k++)
{
yVal[k] = yVal[k] * 180 / Mathf.PI;
if (yVal[k] > 50)
yVal[k] = 50;
else if (yVal[k] < -50)
yVal[k] = -50;
}
MonoBehaviour.print("Got 2");
graph.AddLine("q", transSolve.time, yVal, Color.blue);
yVal = transSolve.GetSolution(3);
for (int k = 0; k < yVal.Length; k++)
{
yVal[k] = yVal[k] * 180 / Mathf.PI;
if (yVal[k] > 50)
yVal[k] = 50;
else if (yVal[k] < -50)
yVal[k] = -50;
}
MonoBehaviour.print("Got 3");
graph.AddLine("θ", transSolve.time, yVal, Color.cyan);
graph.SetBoundaries(0, endTime, -10, 10);
graph.SetGridScaleUsingValues(1, 5);
graph.horizontalLabel = "time";
graph.verticalLabel = "value";
graph.Update();
}
public FARRungeKutta4(double endTime, double dt, FARMatrix eqns, double[] initCond)
{
// b.Add(0.5f, 0, 1);
// b.Add(0.5f, 1, 2);
// b.Add(1, 2, 3);
this.endTime = endTime;
this.dt = dt;
this.stateEquations = eqns;
this.initCond = initCond;
soln = new double[initCond.Length, (int)Math.Ceiling(endTime / dt)];
time = new double[(int)Math.Ceiling(endTime / dt)];
}
private void RunTransientSimLateral(double endTime, double initDt, double[] InitCond)
{
FARMatrix A = new FARMatrix(4, 4);
FARMatrix B = new FARMatrix(1, 4);
MonoBehaviour.print("Init Dump...");
A.PrintToConsole();
MonoBehaviour.print("Deriv Dump...");
int i = 0;
int j = 0;
int num = 0;
double[] Derivs = new double[27];
MOI_stabDerivs.CopyTo(Derivs, 0);
Derivs[15] = Derivs[15] / u0;
Derivs[18] = Derivs[18] / u0;
Derivs[21] = Derivs[21] / u0 - 1;
double Lb = Derivs[16] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Nb = Derivs[17] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Lp = Derivs[19] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Np = Derivs[20] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Lr = Derivs[22] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
double Nr = Derivs[23] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
Derivs[16] = Lb + Derivs[26] / Derivs[0] * Nb;
Derivs[17] = Nb + Derivs[26] / Derivs[2] * Lb;
Derivs[19] = Lp + Derivs[26] / Derivs[0] * Np;
Derivs[20] = Np + Derivs[26] / Derivs[2] * Lp;
Derivs[22] = Lr + Derivs[26] / Derivs[0] * Nr;
Derivs[23] = Nr + Derivs[26] / Derivs[2] * Lr;
foreach (double f in Derivs)
{
if (num < 15)
{
num++; //Avoid Ix, Iy, Iz and long derivs
continue;
}
else
num++;
MonoBehaviour.print(i + "," + j);
if (i <= 2)
A.Add(f, i, j);
if (j < 2)
j++;
else
{
j = 0;
i++;
}
}
A.Add(9.81 * Math.Cos(stable_AoA * FARMathUtil.deg2rad) / u0, 3, 0);
A.Add(1, 1, 3);
A.PrintToConsole(); //We should have an array that looks like this:
B.PrintToConsole();
/* i --------------->
* j [ Yb / u0 , Yp / u0 , -(1 - Yr/ u0) , g Cos(θ0) / u0 ]
* | [ Lb , Lp , Lr , 0 ]
* | [ Nb , Np , Nr , 0 ]
* \ / [ 0 , 1 , 0 , 0 ]
* V //And one that looks like this:
*
* [ Z e ]
* [ X e ]
* [ M e ]
* [ 0 ]
*
*
*/
FARRungeKutta4 transSolve = new FARRungeKutta4(endTime, initDt, A, InitCond);
MonoBehaviour.print("Runge-Kutta 4 init...");
transSolve.Solve();
MonoBehaviour.print("Solved...");
graph.Clear();
double[] yVal = transSolve.GetSolution(0);
MonoBehaviour.print("Got 0");
for (int k = 0; k < yVal.Length; k++)
{
yVal[k] = yVal[k] * 180 / Mathf.PI;
if (yVal[k] > 50)
yVal[k] = 50;
else if (yVal[k] < -50)
yVal[k] = -50;
}
graph.AddLine("β", transSolve.time, yVal, Color.green);
yVal = transSolve.GetSolution(1);
MonoBehaviour.print("Got 1");
for (int k = 0; k < yVal.Length; k++)
{
yVal[k] = yVal[k] * 180 / Mathf.PI;
if (yVal[k] > 50)
yVal[k] = 50;
else if (yVal[k] < -50)
yVal[k] = -50;
}
graph.AddLine("p", transSolve.time, yVal, Color.yellow);
yVal = transSolve.GetSolution(2);
for (int k = 0; k < yVal.Length; k++)
{
yVal[k] = yVal[k] * 180 / Mathf.PI;
if (yVal[k] > 50)
yVal[k] = 50;
else if (yVal[k] < -50)
yVal[k] = -50;
}
MonoBehaviour.print("Got 2");
graph.AddLine("r", transSolve.time, yVal, Color.blue);
yVal = transSolve.GetSolution(3);
for (int k = 0; k < yVal.Length; k++)
{
yVal[k] = yVal[k] * 180 / Mathf.PI;
if (yVal[k] > 50)
yVal[k] = 50;
else if (yVal[k] < -50)
yVal[k] = -50;
}
MonoBehaviour.print("Got 3");
graph.AddLine("φ", transSolve.time, yVal, Color.cyan);
graph.SetBoundaries(0, endTime, -10, 10);
graph.SetGridScaleUsingValues(1, 5);
graph.horizontalLabel = "time";
graph.verticalLabel = "value";
graph.Update();
}