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);
}
}
private void Add(IAggregableMechanicalObject source, IAggregableMechanicalObject target, int sc, int tc, int tn)
{
double[] fs = source.GetConnectionForce(sc);
double[,] mt = target.GetForcesMatrix(tc);
double[] intt = target.InternalAcceleration;
Add(mt, fs, intt, vector, tn);
}
/// <summary>
/// Solves equation
/// </summary>
protected virtual void Solve()
{
int k = 0;
for (int i = 0; i < aggrWrappres.Length; i++)
{
AggregableWrapper frame = aggrWrappres[i];
IAggregableMechanicalObject agg = frame.Aggregate;
double[] intacc = agg.InternalAcceleration;
int l = intacc.Length;
Array.Copy(intacc, 0, vector, k, l);
k += l;
}
if (matrix.GetLength(0) != 0)
{
CalculateLinkAccelerations();
RealMatrixProcessor.RealMatrix.PlusEqual(vector, addAcceleration);
for (int ln = 0; ln < links.Count; ln++)
{
MechanicalAggregateLink ml = links[ln];
IAggregableMechanicalObject s = ml.SourceObject;
IAggregableMechanicalObject t = ml.TargetObject;
int sc = ml.SourceConnection;
int tc = ml.TargetConnection;
int sn = numbers[s];
int tn = numbers[t];
Add(s, t, sc, tc, tn);
Add(t, s, tc, sc, sn);
}
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="aggregate">Root aggregate</param>
protected MechanicalAggregateEquation(AggregableWrapper aggregate)
{
wrapper = aggregate;
this.aggregate = wrapper.Aggregate;
PreInit();
Init();
}
internal VelocityMeasurememt(IAggregableMechanicalObject aggregate, int i, string name)
{
this.aggregate = aggregate;
this.name = name;
this.i = i;
par = velocity;
}
/// <summary>
/// Deserialization construcror
/// </summary>
/// <param name="info">Serialization info</param>
/// <param name="context">Streaming context</param>
protected AggregableWrapper(SerializationInfo info, StreamingContext context)
{
byte[] b = info.GetValue("Buffer", typeof(byte[])) as byte[];
MemoryStream stream = new MemoryStream(b);
BinaryFormatter bf = new BinaryFormatter();
aggregate = bf.Deserialize(stream) as IAggregableMechanicalObject;
prepare();
}
internal CoordinateMeasurement(IAggregableMechanicalObject aggregate, int i, int j, string name)
{
this.aggregate = aggregate;
this.name = name;
this.i = i;
this.j = j;
par = coord;
derivation = new Measurement(deriv, "");
}
/// <summary>
/// Constuctor
/// </summary>
/// <param name="aggregate">Prototype</param>
public AggregableWrapper(IAggregableMechanicalObject aggregate)
{
if (!(aggregate is ISerializable) &
((aggregate as CategoryTheory.IAssociatedObject).GetObject<IPropertiesEditor>() == null))
{
throw new Exception();
}
this.aggregate = aggregate;
prepare();
}
private static void Reset(IAggregableMechanicalObject aggregate)
{
double[] intacc = aggregate.InternalAcceleration;
if (intacc != null)
{
for (int i = 0; i < intacc.Length; i++)
{
intacc[i] = 0;
}
}
foreach (IAggregableMechanicalObject ao in aggregate.Children.Keys)
{
Reset(ao);
}
}
/// <summary>
/// Checks whether aggregate and all its children are constants
/// </summary>
/// <param name="aggregate">The aggregate</param>
/// <returns>Result of checking</returns>
static bool IsConstant(IAggregableMechanicalObject aggregate)
{
if (!aggregate.IsConstant)
{
return(false);
}
foreach (IAggregableMechanicalObject ob in aggregate.Children.Keys)
{
if (!IsConstant(ob))
{
return(false);
}
}
return(true);
}
/// <summary>
/// Calculates residues of accelerations
/// </summary>
protected void CalculateResidues()
{
for (int ln = 0; ln < links.Count; ln++)
{
int k = ln * 6;
MechanicalAggregateLink ml = links[ln];
IAggregableMechanicalObject s = ml.SourceObject;
IAggregableMechanicalObject t = ml.TargetObject;
int sc = ml.SourceConnection;
int tc = ml.TargetConnection;
double[] sa = s.GetInternalAcceleration(sc);
double[] ta = t.GetInternalAcceleration(tc);
for (int i = 0; i < 6; i++)
{
connectionResidues[i + k] = sa[i] - ta[i];
}
}
}
/// <summary>
/// Creates dictionary of aggregate
/// </summary>
/// <param name="aggregate">The aggregate</param>
/// <param name="dic">The dictionary</param>
/// <param name="deg">Degrees of freedom</param>
/// <param name="add">Additional variables</param>
/// <param name="acc">Akseleration number</param>
private void CreateDictionary(AggregableWrapper aggregate,
Dictionary <AggregableWrapper, Dictionary <AggregableWrapper, int[]> > dic,
ref int deg, ref int add, ref int acc)
{
IAggregableMechanicalObject aggr = aggregate.Aggregate;
numbers[aggr] = acc;
int kd = add;
int d0 = deg;
deg += aggr.Dimension;
list.Add(aggregate);
lm.Add(aggr);
acc += GetAcceleationDimension(aggr);
Dictionary <IAggregableMechanicalObject, int[]> ch = aggr.Children;
if (ch == null)
{
return;
}
if (ch.Count == 0)
{
return;
}
Dictionary <AggregableWrapper, int[]> d = new Dictionary <AggregableWrapper, int[]>();
Dictionary <AggregableWrapper, int> dn = new Dictionary <AggregableWrapper, int>();
Dictionary <IAggregableMechanicalObject, int> dna = new Dictionary <IAggregableMechanicalObject, int>();
dictn[aggregate] = dn;
dictna[aggregate.Aggregate] = dna;
dic[aggregate] = d;
foreach (IAggregableMechanicalObject agg in ch.Keys)
{
AggregableWrapper aw = FindWrapper(agg);
int[] nc = ch[agg];
d[aw] = nc;
dn[aw] = add;
dna[agg] = add;
++add;
linkNumbers.Add(new int[] { kd, add, nc[1], nc[0], d0, deg });
CreateDictionary(aw, dic, ref deg, ref add, ref acc);
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="label">Arrow label</param>
public FormAggregateLink(IArrowLabel label)
: this()
{
this.label = label;
UpdateFormUI();
link = label.Arrow as MechanicalAggregateLink;
try
{
pictureBoxParent.Image = NamedComponent.GetImage(label.Target);
pictureBoxChild.Image = NamedComponent.GetImage(label.Source);
}
catch (Exception ex)
{
ex.ShowError(10);
}
source = link.SourceObject;
target = link.TargetObject;
int nt = target.NumberOfConnections;
int ns = source.NumberOfConnections;
if (nt == 0)
{
numericUpDownChild.Enabled = false;
}
else
{
numericUpDownChild.Minimum = 1;
numericUpDownChild.Maximum = source.NumberOfConnections;
numericUpDownChild.Value = link.SourceConnection + 1;
}
if (nt == 0)
{
numericUpDownParent.Enabled = false;
}
else
{
numericUpDownParent.Minimum = 1;
numericUpDownParent.Maximum = target.NumberOfConnections;
numericUpDownParent.Value = link.TargetConnection + 1;
}
first = false;
}
/// <summary>
/// Normalizes aggregate
/// </summary>
/// <param name="aggregate">The aggregate for normalizing</param>
protected static void Normalize(IAggregableMechanicalObject aggregate)
{
if (aggregate is INormalizable)
{
INormalizable n = aggregate as INormalizable;
n.Normalize();
}
Dictionary <IAggregableMechanicalObject, int[]> d = aggregate.Children;
if (d == null)
{
return;
}
foreach (IAggregableMechanicalObject agg in d.Keys)
{
int[] n = d[agg];
agg[n[0]] = aggregate[n[1]];
Normalize(agg);
}
}
/// <summary>
/// Finds wrapper of aggregable mechanical object
/// </summary>
/// <param name="obj">The aggregable mechanical object</param>
/// <returns>The wrapper</returns>
protected AggregableWrapper FindWrapper(IAggregableMechanicalObject obj)
{
IAssociatedObject ao = obj as IAssociatedObject;
IObjectLabel l = ao.Object as IObjectLabel;
IDesktop d = l.Desktop;
IEnumerable <ICategoryObject> objs = d.CategoryObjects;
foreach (ICategoryObject ob in objs)
{
if (ob is AggregableWrapper)
{
AggregableWrapper wr = ob as AggregableWrapper;
if (wr.Aggregate == obj)
{
return(wr);
}
}
}
return(null);
}
void IStateDoubleVariables.Set(double[] input, int offset, int length)
{
int n = 0;
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++)
{
state[j] = input[n + offset];
++n;
if (n >= length)
{
return;
}
}
}
}
/// <summary>
/// Calculates matrixes
/// </summary>
protected void CalculateMatrixes()
{
int n = vector.Length;
int conn = connectionForces.Length;
for (int i = 0; i < conn; i++)
{
for (int j = 0; j < conn; j++)
{
matrix[i, j] = 0;
}
for (int j = 0; j < n; j++)
{
forcesToAccelerations[j, i] = 0;
accelerationTransition[i, j] = 0;
}
}
for (int ln = 0; ln < links.Count; ln++)
{
int k = ln * 6;
MechanicalAggregateLink ml = links[ln];
IAggregableMechanicalObject s = ml.SourceObject;
IAggregableMechanicalObject t = ml.TargetObject;
int sc = ml.SourceConnection;
int tc = ml.TargetConnection;
int sn = numbers[s];
int tn = numbers[t];
int ss = ml.SourceConnection;
int tt = ml.TargetConnection;
Fill(accelerationTransition, s.GetAccelerationMatrix(ss), k, sn);
FillMinus(accelerationTransition, t.GetAccelerationMatrix(tt), k, tn);
Fill(forcesToAccelerations, s.GetForcesMatrix(ss), sn, k);
FillMinus(forcesToAccelerations, t.GetForcesMatrix(tt), tn, k);
}
RealMatrixProcessor.RealMatrix.Multiply(accelerationTransition, forcesToAccelerations, matrix);
}
/// <summary>
/// Gets dimension of aggregate acelerations
/// </summary>
/// <param name="aggeregate">The aggregate</param>
/// <returns>Dimension of aceleration vector</returns>
public static int GetAcceleationDimension(IAggregableMechanicalObject aggeregate)
{
return((aggeregate.Dimension - 1) / 2);
}
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;
}
}
