public static Quaternion Proud(Quaternion l, Quaternion m)
{
Quaternion n = new Quaternion();
n.ScalarPart = l.ScalarPart * m.ScalarPart - MathExpansion.ScalarProud(l.VectorPart, m.VectorPart);
for (int i = 0; i < 3; i++)
{
n.VectorPart[i] = l.ScalarPart * m.VectorPart[i] + m.ScalarPart * l.VectorPart[i]
+ MathExpansion.VectorProud(l.VectorPart, m.VectorPart)[i];
}
return(n);
}
public void Go(double[] nTheta, double[] nV, double nT, double tEx, List <ConsumerParameters> consParms)
{
double[] g = GravitationalAcceleratioVector(R_vec);
double[] vScecified = MathExpansion.Sum(nTheta,
MathExpansion.Multiplication(1 / 2, MathExpansion.VectorProud(nTheta, nV)));
double[] v1 = new double[3]; //3-х мерный вектор абсолютной скорости с предыдущего такта
for (int i = 0; i < v1.Length; i++)
{
v1[i] = V[i];
}
double[] nTheta1 = MathExpansion.Multiplication(nT, Omega);
Omega = MathExpansion.Division(nTheta, nT);
Quaternion s = Quaternion.Proud(n0, L);
double[] deltaV = MathExpansion.Sum(Quaternion.InverseBasisTransform(s, vScecified),
MathExpansion.Multiplication(nT, g));
V = MathExpansion.Sum(V, deltaV);
R_vec = MathExpansion.Sum(R_vec, MathExpansion.Multiplication(nT,
MathExpansion.Multiplication(1 / 2, MathExpansion.Sum(v1, V))));
double dl_0 = 1 - MathExpansion.Module(nTheta) * MathExpansion.Module(nTheta) / 8;
double[] dl = MathExpansion.Multiplication(1 / 2, nTheta);
dl = MathExpansion.Sum(dl, MathExpansion.Multiplication(-MathExpansion.Module(nTheta)
* MathExpansion.Module(nTheta) / 48, nTheta));
dl = MathExpansion.Sum(dl, MathExpansion.Multiplication(-1 / 24, MathExpansion.VectorProud(nTheta, nTheta1)));
Quaternion deltaL = new Quaternion(dl_0, dl);
deltaL.Multiplication(1 / Math.Sqrt(dl_0 * dl_0 + MathExpansion.ScalarProud(dl, dl)));
L = Quaternion.Proud(L, deltaL);
T += nT;
ConsumerParameters result = new ConsumerParameters();
ConsumerParametersCalculation cpCalc = new ConsumerParametersCalculation();
result = cpCalc.Calculate(T, R_vec, V, n0, L);
consParms.Add(result);
if (T < tEx)
{
Go(nTheta, nV, nT, tEx, consParms);
}
}
double[] GravitationalAcceleratioVector(double[] r)
{
const double m = 3.9860009e14; //геоцентрическая гравитационная постоянная Земли
const double c = -1082.63e-6; //коэф. при второй зональной гармонике разложения геопотенциала в ряд
//по сферическим ф-циям
double modR = Math.Sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]);
double f = 5 * r[2] / (modR * modR);
double[] g = new double[3];
g = MathExpansion.Multiplication(-m / (modR * modR * modR), r);
double[] temp = { (3 - f) * r[0], (1 - f) * r[1], (1 - f) * r[2] };
g = MathExpansion.Sum(g, MathExpansion.Multiplication((3 / 2) * c * m
* MathExpansion.a * MathExpansion.a / Math.Pow(modR, 5), temp));
return(g);
}
