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); } }
/// <summary> /// Creates frame /// </summary> protected virtual void CreateFrame() { if (IsAcceleration) { relative = new Motion6DAcceleratedFrame(); owp = new Motion6DAcceleratedFrame(); } else if (IsVelocity & IsAngularVelocity) { relative = new Motion6DFrame(); owp = new Motion6DFrame(); } else if (IsAngularVelocity) { relative = new RotatedFrame(); owp = new RotatedFrame(); } else if (IsVelocity) { relative = new MovedFrame(); owp = new MovedFrame(); } else { relative = new ReferenceFrame(); owp = new ReferenceFrame(); } }
void IDifferentialEquationSolver.CalculateDerivations() { Normalize(aggregate); foreach (AggregableWrapper aw in aggrWrappres) { IAggregableMechanicalObject obj = aw.Aggregate; if (obj is Diagram.UI.IUpdatableObject) { Diagram.UI.IUpdatableObject uo = obj as Diagram.UI.IUpdatableObject; if (uo.Update != null) { uo.Update(); } } } Solve(); int n = 0; int kv = 0; for (int i = 0; i < aggrWrappres.Length; i++) { AggregableWrapper wrapper = aggrWrappres[i]; IAggregableMechanicalObject agg = wrapper.Aggregate; Motion6DAcceleratedFrame frame = wrapper.OwnFrame; IVelocity vel = frame; IAcceleration acc = frame; IPosition pos = frame; double[] state = agg.State; double[] p = pos.Position; double[] v = vel.Velocity; for (int j = 0; j < 3; j++) { p[j] = state[j]; derivations[n + j, 0] = state[j]; double a = state[j + 3]; v[j] = a; derivations[n + j, 1] = a; derivations[n + 3 + j, 0] = a; derivations[n + 3 + j, 1] = vector[kv]; ++kv; } IOrientation or = frame; double[] q = or.Quaternion; for (int j = 0; j < 4; j++) { double a = state[j + 6]; quater[j] = a; q[j] = a; } IAngularVelocity av = frame; double[] om = av.Omega; for (int j = 0; j < 3; j++) { double a = state[j + 10]; omega[j] = a; om[j] = a; } StaticExtensionVector3D.CalculateQuaternionDerivation(quater, omega, der, auxQuaternion); for (int j = 0; j < 4; j++) { derivations[n + 6 + j, 0] = quater[j]; derivations[n + 6 + j, 1] = der[j]; } for (int j = 0; j < 3; j++) { derivations[n + 10 + j, 0] = omega[j]; derivations[n + 10 + j, 1] = vector[kv]; ++kv; } int kk = n + 13; int stk = kk; int stv = 6; int sk = 13; for (int j = 13; j < agg.Dimension; j++) { derivations[kk, 0] = state[sk]; double a = state[sk + 1]; derivations[kk, 1] = a; ++kk; ++stk; ++sk; derivations[kk, 0] = a; derivations[kk, 1] = vector[kv]; ++sk; ++kv; ++stv; ++kk; ++j; } n += agg.Dimension; } }