void OrbitToXYZ(
const OrbitData& orbit,
long when,
float* x,
float* y,
float* z
)
{
float true_anomaly;
float inclination = orbit.OrbitalInclination(when) * deg2rad;
float ascnode = orbit.LongitudeOfAscendingNode(when) * deg2rad;
float peri = orbit.ArgumentOfPeriapsis(when) * deg2rad;
float mean_anomaly = orbit.MeanAnomaly(when) * deg2rad;
float semimajor = orbit.SemiMajorAxis(when);
float eccentricity = orbit.Eccentricity(when);
mean_anomaly = NormalizeRadians(mean_anomaly);
float eccentric_anomaly = kepler(eccentricity, mean_anomaly);
eccentric_anomaly = NormalizeRadians(eccentric_anomaly);
true_anomaly = 2*atan2f(sqrtf((1+eccentricity)/(1-eccentricity)),1.0/tanf(eccentric_anomaly*0.5));
float radius_orbit = semimajor * (1 - eccentricity * eccentricity) / (1 + eccentricity * cosf(true_anomaly));
*x = radius_orbit * (cosf(ascnode) * cosf(peri+true_anomaly) - sinf(ascnode) * cosf(inclination) * sinf(peri+true_anomaly));
*y = radius_orbit * (sinf(ascnode) * cosf(peri+true_anomaly) + cosf(ascnode) * cosf(inclination) * sinf(peri+true_anomaly));
*z = radius_orbit * sinf(inclination) * sinf(peri+true_anomaly);
}
void CompensateAltAz(
const CMatrix& mat,
float real_alt,
float real_az,
float* corr_alt,
float* corr_az
)
{
float altrad = real_alt * deg2rad;
float azrad = real_az * deg2rad;
float x, y, z;
AltAzRadiansToXYZ(altrad, azrad, &x, &y, &z);
float AVals[] = {
x,
y,
z
};
CMatrix A(1,3,AVals);
CMatrix B(1,3);
B = mat * A;
float X = B.Get(0,0);
float Y = B.Get(0,1);
float Z = B.Get(0,2);
*corr_az = NormalizeRadians(atan2f(Y, X)) * rad2deg;
*corr_alt = atan2f(Z, sqrtf(X*X+Y*Y)) * rad2deg;
}
void TiltUpDown(float radians){
mPitch = Clamp(-0.49f*AKJ_PIf, NormalizeRadians(mPitch+radians), 0.49f*AKJ_PIf);
}
void RotateLeftRight(float radians){
mHeading = NormalizeRadians(mHeading - radians);
}
