// 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;
}
}
