private static void Set(ref IMeasurement[] measures, double[,] derivations)
{
if (measures == null)
{
measures = new IMeasurement[derivations.GetLength(0)];
for (int i = 0; i < measures.Length; i++)
{
measures[i] = new AggergateDerivation(derivations, i);
}
return;
}
IMeasurement[] old = measures;
if (measures.Length != old.Length)
{
measures = new IMeasurement[derivations.Length];
}
for (int i = 0; i < measures.Length; i++)
{
if (i < old.Length)
{
AggergateDerivation d = old[i] as AggergateDerivation;
d.Set(i, derivations);
measures[i] = d;
continue;
}
measures[i] = new AggergateDerivation(derivations, i);
}
}
void IDifferentialEquationSolver.CopyVariablesToSolver(int offset, double[] variables)
{
int n = offset;
for (int i = 0; i < aggrWrappres.Length; i++)
{
AggregableWrapper aw = aggrWrappres[i];
IAggregableMechanicalObject ao = aw.Aggregate;
double[] state = ao.State;
for (int j = 0; j < state.Length; j++)
{
double a = variables[n];
state[j] = a;
if (j < measures.Length)
{
AggergateDerivation der = measures[j] as AggergateDerivation;
der.Set(j, a);
}
++n;
}
Motion6DAcceleratedFrame frame = aw.OwnFrame;
IOrientation or = frame;
Array.Copy(state, 6, or.Quaternion, 0, 4);
IAngularVelocity w = frame;
Array.Copy(state, 10, w.Omega, 0, 3);
}
}