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