// Take a set of thrust vectors at positions 0, dt, 2*dt, etc... // and return a higher fidelity track of positions and thrust directions: r, rrr // at intervals of simdt public void Simulate(double a_T, ThrustVectorTime [] thrusts, Vector3d r0, Vector3d v0, Vector3d g, double a_dt) { if (thrusts == null) { return; } T = a_T; List <float> thrust_times = new List <float>(); for (int i = 0; i < thrusts.Length; i++) { thrust_times.Add(thrusts[i].t); } // Simulate dt = a_dt; Vector3d cr = r0; Vector3d cv = v0; int start = 0; if (r != null) { start = r.Length; } int j = start; int N = thrusts.Length; int M = (int)(T / dt + 1); double t = 0; if (r == null) { r = new Vector3d[M]; v = new Vector3d[M]; a = new Vector3d[M]; } else { Array.Resize(ref r, start + M); Array.Resize(ref v, start + M); Array.Resize(ref a, start + M); } while (j < start + M) { Vector3d ca = Vector3d.zero; cr = r0; cv = v0; double [] wr; double [] wv; // TODO: This is slow and expensive in RVWeightsToTime but accurate and // avoid accumulation of error when computing forwards only Solve.RVWeightsToTime(t, dt, thrust_times, out wr, out wv); double [] w = Solve.BasisWeights(t, thrust_times); for (int i = 0; i < N; i++) { ca += w[i] * thrusts[i].v; cr += wr[i] * thrusts[i].v; cv += wv[i] * thrusts[i].v; } cr = cr + v0 * t + 0.5 * t * t * g; cv = cv + g * t; a[j] = ca; r[j] = cr; v[j] = cv; j++; t += dt; } }