/// <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);
}
}
}
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>
/// Constructor
/// </summary>
/// <param name="aggregate">Root aggregate</param>
protected MechanicalAggregateEquation(AggregableWrapper aggregate)
{
wrapper = aggregate;
this.aggregate = wrapper.Aggregate;
PreInit();
Init();
}
/// <summary>
/// Creates aggregate equation
/// </summary>
/// <param name="aggregate">Root aggregate</param>
/// <returns>Aggregate equation</returns>
internal static MechanicalAggregateEquation CreateAggregateEquation(AggregableWrapper aggregate)
{
if (IsConstant(aggregate.Aggregate))
{
return(new RigidMechanicalAggregateEquation(aggregate));
}
return(new MechanicalAggregateEquation(aggregate));
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="aggregate">Root aggregate</param>
internal RigidMechanicalAggregateEquation(AggregableWrapper aggregate)
: base(aggregate)
{
CalculateMatrixes();
if (matrix.GetLength(0) == 0)
{
return;
}
RealMatrixProcessor.RealMatrix.Invert(matrix, invertedMatrix);
RealMatrixProcessor.RealMatrix.Multiply(forcesToAccelerations, invertedMatrix, finalMatrix);
}
/// <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>
/// 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>
/// Root aggegates of desktop
/// </summary>
/// <param name="desktop">The desktop</param>
/// <returns>Root aggregates</returns>
public static ICollection <AggregableWrapper> GetRootAggregates(IEnumerable <object> desktop)
{
IEnumerable <object> objs = desktop.GetObjectsAndArrows();
List <AggregableWrapper> l = new List <AggregableWrapper>();
foreach (object o in objs)
{
if (!(o is AggregableWrapper))
{
continue;
}
AggregableWrapper agg = o as AggregableWrapper;
if (agg.Aggregate.Parent == null)
{
if (!l.Contains(agg))
{
l.Add(agg);
}
}
}
return(l);
}
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;
}
}
