/// <summary>
/// Return the orbital velocity in the orbit's reference frame at the
/// specified time (TDB). Units are kilometers per day. If the method
/// is not overridden, the velocity will be computed by differentiation
/// of position.
/// </summary>
/// <param name="time">Time</param>
/// <param name="velocity">Orbital velocity in the orbit's reference</param>
public override void velocityAtTime(double time, double[] velocity)
{
double E = eccentricAnomaly(time);
velocityAtEFunc(E, v);
RealMatrix.Multiply(orbitPlaneRotation, v, velocity);
}
/// <summary>
/// Return the position in the orbit's reference frame at the specified
/// time (TDB). Units are kilometers.
/// </summary>
/// <param name="jd">Time</param>
/// <param name="position">Position</param>
public override void positionAtTime(double jd, double[] position)
{
double E = eccentricAnomaly(jd);
positionAtEFunc(E, x);
RealMatrix.Multiply(orbitPlaneRotation, x, position);
}
object IObjectOperation.this[object[] x]
{
get
{
double[,] a = x[0] as double[, ];
double[,] b = x[1] as double[, ];
RealMatrix.Multiply(a, b, buffer);
return(buffer);
}
}
/// <summary>
/// Return the orbital velocity in the orbit's reference frame at the
/// specified time (TDB). Units are kilometers per day. If the method
/// is not overridden, the velocity will be computed by differentiation
/// of position.
/// </summary>
/// <param name="time">Time</param>
/// <param name="vector">Orbital state in the orbit's reference</param>
public override void vectorAtTime(double time, double[] vector)
{
double E = eccentricAnomaly(time); // Eccentric anomaly
positionAtEFunc(E, x); // Position
RealMatrix.Multiply(orbitPlaneRotation, x, x1);
Array.Copy(x1, vector, 3);
velocityAtEFunc(E, v); // Velocity
RealMatrix.Multiply(orbitPlaneRotation, v, v1);
Array.Copy(v1, 0, vector, 3, 3);
}
/// <summary>
/// Sets state
/// </summary>
/// <param name="baseFrame">Base frame</param>
/// <param name="relative">Relative frame</param>
public override void Set(ReferenceFrame baseFrame, ReferenceFrame relative)
{
base.Set(baseFrame, relative);
IAcceleration ab = baseFrame as IAcceleration;
IAcceleration ar = relative as IAcceleration;
IAngularAcceleration arn = relative as IAngularAcceleration;
IVelocity vb = baseFrame as IVelocity;
IVelocity vr = relative as IVelocity;
IAngularVelocity anb = baseFrame as IAngularVelocity;
IAngularVelocity anr = relative as IAngularVelocity;
double[] rp = Position;
double[,] m = Matrix;
double[] omr = anr.Omega;
StaticExtensionVector3D.VectorPoduct(omr, vr.Velocity, tempV);
double om2 = StaticExtensionVector3D.Square(omr);
double[] eps = arn.AngularAcceleration;
StaticExtensionVector3D.VectorPoduct(eps, rp, temp);
for (int i = 0; i < 3; i++)
{
tempV[i] *= 2;
tempV[i] += om2 * rp[i] + relativeAcceleration[i] + temp[i];
}
RealMatrix.Multiply(m, tempV, acceleration);
double[] omb = anb.Omega;
IOrientation orr = relative as IOrientation;
double[,] mrr = orr.Matrix;
RealMatrix.Multiply(omb, mrr, temp);
StaticExtensionVector3D.VectorPoduct(temp, omr, tempV);
for (int i = 0; i < 3; i++)
{
temp[i] = eps[i] + tempV[i];
}
RealMatrix.Multiply(temp, m, angularAcceleration);
}
/*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();
}
/// <summary>
/// Performs iteration
/// </summary>
/// <returns>Residue</returns>
public double Iterate()
{
PrepareIteration();
double sigma = 0;
List <IIterator> iterators;
if (ownIterators.Count != 0)
{
iterators = ownIterators;
}
else
{
iterators = this.iterators;
}
if (iterators.Count == 0)
{
return(1);
}
foreach (IIterator it in iterators)
{
it.Reset();
}
for (int i = 0; i < a.GetLength(0); i++)
{
for (int j = 0; j < a.GetLength(1); j++)
{
a[i, j] = d[i, j];
}
}
for (int i = 0; i < z.Length; i++)
{
z[i] = 0;
}
while (true)
{
consumer.Reset();
try
{
consumer.UpdateChildrenData();
}
catch (Exception ex)
{
ex.ShowError(10);
goto cycle;
}
for (int i = 0; i < y.Length; i++)
{
object o = left[i].Parameter();
if (o == null)
{
goto cycle;
}
y[i] = (double)o;
o = right[i].Parameter();
if (o == null | o is DBNull)
{
goto cycle;
}
double res = (double)o - y[i];
yr[i] = res;
sigma += res * res;
}
for (int i = 0; i < aliases.Length; i++)
{
IAliasName a = aliases[i];
double delta = dx[i];
SetDelta(a, delta);
consumer.Reset();
consumer.UpdateChildrenData();
for (int j = 0; j < y.Length; j++)
{
object obj = left[j].Parameter();
if (obj == null)
{
SetDelta(a, -delta);
goto cycle;
}
ht[i, j] = ((double)obj - y[j]) / delta;
}
SetDelta(a, -delta);
}
for (int i = 0; i < y.Length; i++)
{
for (int j = 0; j <= i; j++)
{
mr[i, j] = (double)r[i, j].Parameter();
mr[j, i] = mr[i, j];
}
}
RealMatrix.Invert(mr, mr1);
RealMatrix.Multiply(ht, mr1, htr);
for (int i = 0; i < a.GetLength(0); i++)
{
for (int k = 0; k < htr.GetLength(1); k++)
{
z[i] += htr[i, k] * yr[k];
for (int j = 0; j < a.GetLength(1); j++)
{
a[i, j] += htr[i, k] * ht[j, k];
}
}
}
cycle:
foreach (IIterator it in iterators)
{
if (!it.Next())
{
goto m;
}
}
}
m:
RealMatrix.Solve(a, z, indxa);
for (int i = 0; i < z.Length; i++)
{
SetDelta(aliases[i], z[i]);
}
return(sigma);
}
/// <summary>
/// Performs full iteration
/// </summary>
/// <returns>Residue</returns>
public double FullIterate()
{
PrepareIteration();
for (int i = 0; i < a.GetLength(0); i++)
{
for (int j = 0; j < a.GetLength(1); j++)
{
a[i, j] = d[i, j];
}
}
for (int i = 0; i < z.Length; i++)
{
z[i] = 0;
}
currentSigma = 0;
List <double[]> c = Calculate();
double s = currentSigma;
List <double[]>[] ll = new List <double[]> [aliases.Length];
double[,] h = new double[aliases.Length, c.Count];
for (int i = 0; i < aliases.Length; i++)
{
IAliasName alias = aliases[i];
double delta = dx[i];
SetDelta(alias, delta);
ll[i] = Calculate();
SetDelta(alias, -delta);
}
for (int i = 0; i < y.Length; i++)
{
for (int j = 0; j <= i; j++)
{
mr[i, j] = (double)r[i, j].Parameter();
mr[j, i] = mr[i, j];
}
}
RealMatrix.Invert(mr, mr1);
for (int im = 0; im < c.Count; im++)
{
double[] y0 = c[im];
for (int i = 0; i < aliases.Length; i++)
{
double[] y = ll[i][im];
for (int j = 0; j < y.Length; j++)
{
ht[i, j] = (y[j] - y0[j]) / dx[i];
}
}
RealMatrix.Multiply(ht, mr1, htr);
for (int i = 0; i < a.GetLength(0); i++)
{
for (int k = 0; k < htr.GetLength(1); k++)
{
z[i] -= htr[i, k] * y0[k];
for (int j = 0; j < a.GetLength(1); j++)
{
a[i, j] += htr[i, k] * ht[j, k];
}
}
}
}
RealMatrix.Solve(a, z, indxa);
for (int i = 0; i < z.Length; i++)
{
SetDelta(aliases[i], z[i]);
}
return(s);
}
