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