/// <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();
}
double atm(double[] x, double t, double alf, double del, ref double s0, ref double h, int[] it)
{
double hh = StaticExtensionVector3D.Normalize(x, y, 0);
h = hh - 6378.140 * (1.0 - 0.335282E-2 * y[2] * y[2]);
//int N10=0;
// int i, j, k, j1, k1;
/*for(i=0;i<6;i++)
* if(ifa[0]==if1[i]) break; else N10++;
* //N10++;
* if(h<=180) //N10=1;
* for(j=0;j<21;j++)
* {
* j1=N10*21+j;
* f1[j]=f0[j1];
* }
* else
* for(k=0;k<21;k++)
* {
* k1=N10*21+k;
* f1[k]=f01[k1];
* }
*/
//f1[12]=.611;
if (h <= 180)
{
f1 = ff0[N10];
}
else
{
f1 = ff1[N10];
}
int N3 = it[1] - 1;
double a2, dat2;
if (N3 <= 0)
{
a2 = (double)it[0] / 10;
}
else
{
dat2 = (double)it[1] / 4;
if (Math.Abs(Math.Floor(dat2 + .00001) - dat2) < .0001)
{
a2 = (mac[N3 - 1] + 1 + it[0]) / 10.0;
}
else
{
a2 = (mac[N3 - 1] + it[0]) / 10.0;
}
}
int N2 = (int)Math.Floor(a2);
double a3 = a2 - N2;
N2++;
double ad1 = ad[N2 - 1] + (ad[N2] - ad[N2 - 1]) * a3;
double gam = alf + f1[12] - s0 - ome * (t - 10800.0);
double cosfi = y[2] * Math.Sin(del) + Math.Cos(del) * (y[0] * Math.Cos(gam) +
y[1] * Math.Sin(gam));
double xk4 = 1 + (f1[16] + f1[17] * h + f1[18] * h * h) *
Math.Log(ifa[1] / f1[20] + f1[19]);
double xk3 = 1 + (f1[13] + f1[14] * h + f1[15] * h * h) * ad1;
double cosfi2 = Math.Abs((1.0 + cosfi) / 2.0);
double xk2 = 1 + (f1[6] + f1[7] * h + f1[8] * Math.Exp(-(h + f1[9]) / f1[10]
* (h + f1[9]) / f1[10])) * Math.Pow(cosfi2, f1[11] / 2);
double xk1 = 1.0 + (f1[3] + f1[4] * h + f1[5] * h * h) * (ifa[2] - ifa[0]) / ifa[0];
double roh = Math.Exp(f1[0] - f1[1] * Math.Sqrt(h - f1[2]));
return(roh * xk1 * xk2 * xk3 * xk4);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="pericenterDistance">Pericenter Distance</param>
/// <param name="eccentricity">Eccentricity</param>
/// <param name="inclination">Inclination</param>
/// <param name="ascendingNode">Ascending Node</param>
/// <param name="argOfPeriapsis">Argument of Periapsis</param>
/// <param name="meanAnomalyAtEpoch">Mean Anomaly at Epoch</param>
/// <param name="period">Period</param>
/// <param name="epoch">Epoch</param>
public EllipticalOrbit(double pericenterDistance,
double eccentricity,
double inclination,
double ascendingNode,
double argOfPeriapsis,
double meanAnomalyAtEpoch,
double period,
double epoch)
{
this.pericenterDistance = pericenterDistance;
this.eccentricity = eccentricity;
this.inclination = inclination;
this.ascendingNode = ascendingNode;
this.argOfPeriapsis = argOfPeriapsis;
this.meanAnomalyAtEpoch = meanAnomalyAtEpoch;
this.period = period;
this.epoch = epoch;
double[] angles = new double[] { ascendingNode, inclination, argOfPeriapsis };
double[][] buffer = new double[][] { new double[4], new double[4] };
double[] buff = new double[4];
meanMotion = 2.0 * Math.PI / period;
StaticExtensionVector3D.Rotate(angles, axis, quaternion, buffer, buff);
quaternion.QuaternionToMatrix(orbitPlaneRotation, qq);
if (eccentricity < 1.0)
{
positionAtEFunc = (double E, double[] x) =>
{
double a = pericenterDistance / (1.0 - eccentricity);
x[0] = a * (Math.Cos(E) - eccentricity);
x[1] = a * Math.Sqrt(1 - (eccentricity * eccentricity)) * Math.Sin(E);
};
velocityAtEFunc = (double E, double[] x) =>
{
double a = pericenterDistance / (1.0 - eccentricity);
double sinE = Math.Sin(E);
double cosE = Math.Cos(E);
x[0] = -a * sinE;
x[1] = a * Math.Sqrt(1 - (eccentricity * eccentricity)) * cosE;
double mm = 2.0 * Math.PI / period;
double edot = mm / (1 - eccentricity * cosE);
x[0] *= edot;
x[1] *= edot;
};
}
else if (eccentricity > 1.0)
{
positionAtEFunc = (double E, double[] x) =>
{
double a = pericenterDistance / (1.0 - eccentricity);
x[0] = -a * (eccentricity - Math.Cosh(E));
x[1] = -a *Math.Sqrt((eccentricity *eccentricity) - 1) * Math.Sinh(E);
};
velocityAtEFunc = (double E, double[] x) =>
{
double a = pericenterDistance / (1.0 - eccentricity);
x[0] = -a * (eccentricity - Math.Cosh(E));
x[1] = -a *Math.Sqrt((eccentricity *eccentricity) - 1) * Math.Sinh(E);
};
}
else
{
// TODO: Handle parabolic orbits
positionAtEFunc = (double E, double[] x) =>
{
x[0] = 0;
x[1] = 0;
};
positionAtEFunc = (double E, double[] x) =>
{
x[0] = 0;
x[1] = 0;
};
}
meanAnomalyFunc = (double t) =>
{
return(meanAnomalyAtEpoch + meanMotion * t);
};
double err = 0;
if (eccentricity == 0)
{
ecc = () => { return(meanAnomaly); };
}
else if (eccentricity < 0.2)
{
Func <double, double> f = SolveKeplerFunc1;
ecc = () =>
{
return(f.SolveIterationFixed(10, meanAnomaly, out err));
};
}
else if (eccentricity < 0.9)
{
// Higher eccentricity elliptical orbit; use a more complex but
// much faster converging iteration.
Func <double, double> f = SolveKeplerFunc2;
ecc = () =>
{
return(f.SolveIterationFixed(6, meanAnomaly, out err));
};
}
else if (eccentricity < 1.0)
{
// Extremely stable Laguerre-Conway method for solving Kepler's
// equation. Only use this for high-eccentricity orbits, as it
// requires more calcuation.
Func <double, double> f = SolveKeplerLaguerreConway;
ecc = () =>
{
E = meanAnomaly + 0.85 * eccentricity * Math.Sign(Math.Sin(meanAnomaly));
return(f.SolveIterationFixed(9, meanAnomaly, out err));
};
}
else if (eccentricity == 1.0)
{
// Nearly parabolic orbit; very common for comets
// TODO: handle this
ecc = () => { return(meanAnomaly); };
}
else
{
// Laguerre-Conway method for hyperbolic (ecc > 1) orbits.
Func <double, double> f = SolveKeplerLaguerreConwayHyp;
ecc = () =>
{
E = Math.Log(2 * meanAnomaly / eccentricity + 1.85);
return(f.SolveIterationFixed(30, meanAnomaly, out err));
};
}
}
static public void StateVectorToOrbit(
double[] position, double[] v, double gravityParameter, double time,
out double pericenterDistance,
out double eccentricity,
out double inclination,
out double ascendingNode,
out double argOfPeriapsis,
out double meanAnomalyAtEpoch,
out double period)
{
//Vec3d R = position - Point3d(0.0, 0.0, 0.0);
StaticExtensionVector3D.VectorPoduct(position, v, L);
double magR = position.Norm();
double magL = L.Norm();
double magV = v.Norm();
L.Multiply(1 / magL);
StaticExtensionVector3D.VectorPoduct(L, position, W);
W.Multiply(1 / magR);
/* double G = Astro.G * 1e-9; // convert from meters to kilometers
* double GM = G *
*
* gravityParameter;*/
// Compute the semimajor axis
double a = 1.0 / (2.0 / magR - (magV * magV) / gravityParameter);
// Compute the eccentricity
double p = (magL * magL) / gravityParameter;
double q = position.ScalarProduct(v);
double ex = 1.0 - magR / a;
double ey = q / Math.Sqrt(a * gravityParameter);
eccentricity = Math.Sqrt(ex * ex + ey * ey);
// Compute the mean anomaly
double E = Math.Atan2(ey, ex);
meanAnomalyAtEpoch = E - eccentricity * Math.Sin(E);
double cosi = L[1];
inclination = 0.0;
if (cosi < 1.0)
{
inclination = Math.Acos(cosi);
}
// Compute the longitude of ascending node
ascendingNode = Math.Atan2(L[0], L[2]);
// Compute the argument of pericenter
Array.Copy(position, U, 3);
U.Multiply(1 / magR);
double s_nu = StaticExtensionVector3D.ScalarProduct(v, U) * Math.Sqrt(p / gravityParameter);
double c_nu = StaticExtensionVector3D.ScalarProduct(v, W) * Math.Sqrt(p / gravityParameter) - 1;
s_nu /= eccentricity;
c_nu /= eccentricity;
double P = U[1] * c_nu - W[1] * s_nu;
double Q = U[1] * s_nu + W[1] * s_nu;
argOfPeriapsis = Math.Atan2(P, Q);
// Compute the period
period = 2 * Math.PI * Math.Sqrt((a * a * a) / gravityParameter);
pericenterDistance = a * (1 - eccentricity);///, e, i, Om, om, M, T, t);
}
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;
}
}
