V6
fk524(V6 v)
{
v = m6v6(M, v);
/* restore e-terms */
#ifdef ETERMS
{
double m; /* modulus of position vector */
V3 s0;
V3 s1, s1dot;
V3 r0, r0dot;
V3 r1, r1dot;
/* cache the modulus */
m = v6mod(v);
/* convert the state vector back to a unit vector */
v6GetPos(v) = v3scale(v6GetPos(v), 1/m);
v6GetVel(v) = v3scale(v6GetVel(v), CB/(m*as2r(1)));
/* now proceed with the standard treatment */
r1 = v6GetPos(v);
r1dot = v6GetVel(v);
s1 = v3unit(r1);
s0 = s1;
r0 = v3sum(s1, v3diff(A, v3scale(s0, v3dot(s0, A))));
#ifdef ETERMS_ITERATE
s0 = v3unit(r0);
r0 = v3sum(s1, v3diff(A, v3scale(s0, v3dot(s0, A))));
#endif
v6SetPos(v, r0);
#ifdef ETERMS_VEL
s1dot = v3scale(r1dot, 1/v3mod(r1));
r0dot = v3sum(s1dot, v3diff(Adot, v3scale(s0, v3dot(s0, Adot))));
v6SetVel(v, r0dot);
#endif
/* convert the unit vector back into a state vector */
v6GetPos(v) = v3scale(v6GetPos(v), m);
v6GetVel(v) = v3scale(v6GetVel(v), (m*as2r(1))/CB);
}
#endif
return(v);
}
V6
fk425(V6 v)
{
/* ensure cartesian vectors */
v = v6s2c(v);
#ifdef ETERMS
{
double m; /* modulus of position vector */
V3 u0, u0dot;
V3 u1, u1dot;
/* cache the modulus */
m = v6mod(v);
/* convert the state vector back to a unit vector */
v6GetPos(v) = v3scale(v6GetPos(v), 1/m);
v6GetVel(v) = v3scale(v6GetVel(v), CB/(m*as2r(1)));
/* now proceed with the standard treatment */
u0 = v6GetPos(v);
u1 = v3diff(u0, v3diff(A, v3scale(u0, v3dot(u0, A))));
v6SetPos(v, u1);
#ifdef ETERMS_VEL
u0dot = v6GetVel(v);
u1dot = v3diff(u0dot, v3diff(Adot, v3scale(u0, v3dot(u0, Adot))));
v6SetVel(v, u1dot);
#endif
/* convert the unit vector back into a state vector */
v6GetPos(v) = v3scale(v6GetPos(v), m);
v6GetVel(v) = v3scale(v6GetVel(v), (m*as2r(1))/CB);
}
#endif
v = m6v6(M, v);
return(v);
}
int main(){
V3 v3_1, v3_2;
V6 v6, v6s;
/* Create a V6 vector, using 2 V3 vectors.*/
v3_1 = v3init(CARTESIAN);
v3_2 = v3init(CARTESIAN);
v6 = v6init(CARTESIAN);
v3SetType(v3_1, CARTESIAN);
v3SetX(v3_1, 1123.4556);
v3SetY(v3_1, 4556.1123);
v3SetZ(v3_1, 9876.1267);
v3SetType(v3_2, CARTESIAN);
v3SetX(v3_2, 2.3456);
v3SetY(v3_2, 6.7891);
v3SetZ(v3_2, 7.8912);
v6SetPos(v6, v3_1);
v6SetVel(v6, v3_2);
v6SetType(v6, CARTESIAN);
v6s = v6c2s(v6);
printf("X %f \tY %f \tZ %f \nXDOT %f \tYDOT %f \tZDOT %f\n", \
v6GetX(v6), v6GetY(v6), v6GetZ(v6), v6GetXDot(v6),
v6GetYDot(v6), v6GetZDot(v6));
printf("R %.8f \tAlpha %.8f \tDelta %.8f \nRDOT %.8f \tAlphaDOT %.8f \tDeltaDOT %.8f\n", \
v6GetR(v6s), v6GetAlpha(v6s), v6GetDelta(v6s), v6GetRDot(v6s),
v6GetAlphaDot(v6s), v6GetDeltaDot(v6s));
return 0;
}