/*public override double[,] RelativeMatrix
* {
* get
* {
* return base.RelativeMatrix;
* }
* set
* {
* base.RelativeMatrix = value;
* / for (int i = 0; i < 4; i++)
* {
* initialConditions[i + 6] = relativeQuaternion[i];
* }
* }
* }*/
#endregion
#region IDifferentialEquationSolver Members
void IDifferentialEquationSolver.CalculateDerivations()
{
//IReferenceFrame f = this;
//ReferenceFrame frame = f.Own;
SetAliases();
IDataConsumer cons = this;
int i = 0;
cons.Reset();
cons.UpdateChildrenData();
//Filling of massive of forces and moments using results of calculations of formula trees
for (i = 0; i < 12; i++)
{
forces[i] = (double)measures[i].Parameter();
}
//Filling the part, responding to derivation of radius-vector
/*for (i = 0; i < 3; i++)
* {
* result[i, 1] = result[3 + i, 0];
* }*/
int k = 0;
for (i = 0; i < 3; i++)
{
for (int j = i; j < 3; j++)
{
IMeasurement min = inertia[k];
++k;
if (min != null)
{
double jin = (double)min.Parameter();
J[i, j] = jin;
J[j, i] = jin;
}
}
}
IMeasurement mm = inertia[6];
if (mm != null)
{
unMass = (double)mm.Parameter();
unMass = 1 / unMass;
}
//Filling the part, responding to derivation of linear velocity
for (i = 0; i < 3; i++)
{
linAccAbsolute[i] = forces[6 + i] * unMass;
}
double[,] T = Relative.Matrix;
RealMatrix.Multiply(linAccAbsolute, T, aux);
for (i = 0; i < 3; i++)
{
linAccAbsolute[i] = forces[i] * unMass + aux[i];
}
RealMatrix.Multiply(J, omega, aux);
StaticExtensionVector3D.VectorPoduct(omega, aux, aux1);
RealMatrix.Add(aux1, 0, forces, 9, aux, 0, 3);
Array.Copy(forces, 3, aux1, 0, 3);
RealMatrix.Multiply(aux1, T, aux2);
RealMatrix.Add(aux2, 0, forces, 9, aux1, 0, 3);
RealMatrix.Add(aux1, 0, aux, 0, aux2, 0, 3);
RealMatrix.Multiply(L, aux2, epsRelative);
aux4d[0] = 0;
Array.Copy(omega, 0, aux4d, 1, 3);
StaticExtensionVector3D.QuaternionInvertMultiply(relativeQuaternion, aux4d, quaternionDervation);
for (i = 0; i < 3; i++)
{
quaternionDervation[i] *= 0.5;
}
SetRelative();
Update();
}
// public bool getTyp() { return getFunkcjaCelu().getTypFunkcji(); }
public RealMatrix getMtx()
{
RealMatrix temp = new RealMatrix();
foreach(Funkcja e in ograniczenia){
RealVector t = new RealVector();
for (int i = 0; i <e.getWspZmiennych().Count; i++) {
t.Add(double.Parse(e.getWspZmiennych()[i]));
}
temp.Add(t);
}
return temp;
}
