sub orb2eci(mu, oev(), r(), v())

' convert classical orbital elements to state vector

' input

' mu = gravitational constant (km**3/sec**2)
' oev(1) = semimajor axis (kilometers)
' oev(2) = orbital eccentricity (non-dimensional)
' (0 <= eccentricity < 1)
' oev(3) = orbital inclination (radians)
' (0 <= inclination <= pi)
' oev(4) = argument of perigee (radians)
' (0 <= argument of perigee <= 2 pi)
' oev(5) = right ascension of ascending node (radians)
' (0 <= raan <= 2 pi)
' oev(6) = true anomaly (radians)
' (0 <= true anomaly <= 2 pi)

' output

' r() = position vector (kilometers)
' v() = velocity vector (kilometers/second)

''''''''''''''''''''''''''''''''''''''''''''

local sma, ecc, inc, argper, raan, tanom

local slr, rm, arglat, sarglat, carglat

local c4, c5, c6, sinc, cinc, sraan, craan

' unload orbital elements array

sma = oev(1)
ecc = oev(2)
inc = oev(3)
argper = oev(4)
raan = oev(5)
tanom = oev(6)

slr = sma * (1.0 - ecc * ecc)

rm = slr / (1.0 + ecc * cos(tanom))

arglat = argper + tanom

sarglat = sin(arglat)
carglat = cos(arglat)

c4 = sqr(mu / slr)
c5 = ecc * cos(argper) + carglat
c6 = ecc * sin(argper) + sarglat

sinc = sin(inc)
cinc = cos(inc)

sraan = sin(raan)
craan = cos(raan)

' position vector

r(1) = rm * (craan * carglat - sraan * cinc * sarglat)
r(2) = rm * (sraan * carglat + cinc * sarglat * craan)
r(3) = rm * sinc * sarglat

' velocity vector

v(1) = -c4 * (craan * c6 + sraan * cinc * c5)
v(2) = -c4 * (sraan * c6 - craan * cinc * c5)
v(3) = c4 * c5 * sinc

end sub

// convert a float to a string including scientific notation if necessary
// p is the buffer to store the string
// f is the number
// m is the nbr of chars before the decimal point (if negative print the + sign)
// n is the nbr chars after the point
// ch is the leading pad char
// def specifies to use default formatting
void FloatToStr(char *p, long double f, int m, int n, unsigned char ch, int def) {
int i, exp, trim = false;
long double rounding;
long double tens[] ={1.0L,10.0L,100.0L,1000.0L,10000.0L,100000.0L,1000000.0L,10000000.0L,100000000.0L,1000000000.0L,10000000000.0L,10000000 0000.0L,1000000000000.0L,10000000000000.0L,100000000000000.0L,1000000000000000.0L,10000000000000000.0L};
char *pp;
long long int a;
ch &= 0x7f; // make sure that ch is an ASCII char
exp = floor(log10(fabs(f))); // get the exponent part
if(((fabs(f) >= 1000000.0L || fabs(f) < 0.0001L) && f != 0.0L && def) || n<0) {
// we must use scientific notation
if(f==0.0L){
pp=p;
*pp++='0';
*pp++='.';
for(i=0;i>n;i--)*pp++='0';
*pp++='e';
*pp++='+';
*pp++='0';
*pp++='0';
*pp++=0;
} else {
if(n<0)n=-n;
f /= pow(10.0L, exp); // scale the number to 1.2345
FloatToStr(p, f, m, n, ch, 0); // recursively call ourself to convert that to a string
p = p + strlen(p);
*p++ = 'e'; // add the exponent
if(exp >= 0) {
*p++ = '+';
IntToStrPad(p, exp, '0', 2, 10); // add a positive exponent
} else {
*p++ = '-';
IntToStrPad(p, exp * -1, '0', 2, 10); // add a negative exponent
}
}
} else {
// we can treat it as a normal number

// first figure out how many decimal places we want.
// def == 1 means that we should automatically determine the precision
if(def) {
trim = true;
n = 5 - exp;
if(n < 0) n = 0;
}

// add rounding to hide the vagarities of floating point
if(n > 0)
rounding = 0.5L/pow(10.0L, n);
else
rounding = 0.5L;

if(f > 0) f += rounding; // add rounding for positive numbers
if(f < 0) f -= rounding; // add rounding for negative numbers

// convert the digits before the decimal point
if((long long int)f == 0 && f < 0)
IntToStrPad(p, 0, -ch, m, 10); // convert -0 incl padding if necessary
else
IntToStrPad(p, (long long int)f, ch, m, 10); // convert the integer incl padding if necessary
p += strlen(p); // point to the end of the integer
pp = p;

// convert the digits after the decimal point
if(f < 0) f = -f; // make the number positive
if(n > 0) { // if we need to have a decimal point and following digits
*pp++ = '.'; // add the decimal point
f -= (long long int)f; // get just the fractional part
f*=tens[n];
a=(long long int)f;
IntToStrPad(pp, a, '0', n, 10);
pp+=n;
// if we do not have a fixed number of decimal places step backwards removing trailing zeros and the decimal point if necessary
while(trim && pp > p) {
pp--;
if(*pp == '.') break;
if(*pp != '0') { pp++; break; }
}
}
*pp = 0;
}
}

version elapsed time statements/sec
4.5D 6.752 7127
5.0 4.942 9737
5.1 4.968 9686
5.2 5.021 9584
5.03.01 5.402 8908
5.03.02 5.402 8908
5.03.03 5.402 8908
5.03.04 5.402 8908
5.04.01 5.403 8906
5.04.04 5.403 8906
5.04.05 4.873 9875

For I=2 To 6: SetPin I, DOUT: Next I
Start = Timer

For I=1 To 5000
Pin(2)=1
Pin(3)=1
Pin(4)=1
Pin(5)=1
Pin(6)=1
Pin(2)=0
Pin(3)=0
Pin(4)=0
Pin(5)=0
Pin(6)=0
Next I

Elapsed = (Timer-Start)/1000
Print 55000/Elapsed " I/O lines per second"

>
> run
19434.6 I/O lines per second
>