V6
v6s2c(V6 vs)
{
double cos_a;
double cos_d;
double sin_a;
double sin_d;
V6 vc;
if (v6GetType(vs) == CARTESIAN) {
return(vs);
}
vc = v6init(CARTESIAN);
cos_a = cos(A);
cos_d = cos(D);
sin_a = sin(A);
sin_d = sin(D);
/* the standard transformation */
v6SetX(vc, R*cos_d*cos_a);
v6SetY(vc, R*cos_d*sin_a);
v6SetZ(vc, R*sin_d);
/* the first derivative of the standard transformation */
v6SetXDot(vc, -R*(cos_d*sin_a*ADOT + sin_d*cos_a*DDOT) + RDOT*cos_d*cos_a);
v6SetYDot(vc, R*(cos_d*cos_a*ADOT - sin_d*sin_a*DDOT) + RDOT*cos_d*sin_a);
v6SetZDot(vc, R*cos_d*DDOT + RDOT*sin_d);
return(vc);
}
double
v6dot(V6 v1, V6 v2)
{
double x = 0;
if (v6GetType(v1) == SPHERICAL) {
v1 = v6s2c(v1);
}
if (v6GetType(v2) == SPHERICAL) {
v2 = v6s2c(v2);
}
x += v6GetX(v1) * v6GetX(v2);
x += v6GetY(v1) * v6GetY(v2);
x += v6GetZ(v1) * v6GetZ(v2);
return(x);
}
V6
v6diff(V6 v1, V6 v2)
{
if (v6GetType(v1) == SPHERICAL) {
v1 = v6s2c(v1);
}
if (v6GetType(v2) == SPHERICAL) {
v2 = v6s2c(v2);
}
v6DecX(v1, v6GetX(v2));
v6DecY(v1, v6GetY(v2));
v6DecZ(v1, v6GetZ(v2));
v6DecXDot(v1, v6GetXDot(v2));
v6DecYDot(v1, v6GetYDot(v2));
v6DecZDot(v1, v6GetZDot(v2));
return(v1);
}
V6
m3v6(M3 m, V6 v1)
{
int row, col;
V6 v2;
if (v6GetType(v1) == SPHERICAL) {
v1 = v6s2c(v1);
}
v2 = v6init(CARTESIAN);
for (row = 0; row < 3; row++) {
for (col = 0; col < 3; col++) {
v2.v[POS].v[row] += m.m[row][col] * v1.v[POS].v[col];
v2.v[VEL].v[row] += m.m[row][col] * v1.v[VEL].v[col];
}
}
return(v2);
}
