Task: Orbital elements

C: #include <stdio.h>
#include <math.h>

typedef struct {
    double x, y, z;
} vector;

vector add(vector v, vector w) {
    return (vector){v.x + w.x, v.y + w.y, v.z + w.z};
}

vector mul(vector v, double m) {
    return (vector){v.x * m, v.y * m, v.z * m};
}

vector div(vector v, double d) {
    return mul(v, 1.0 / d);
}

double vabs(vector v) {
    return sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
}

vector mulAdd(vector v1, vector v2, double x1, double x2) {
    return add(mul(v1, x1), mul(v2, x2)); 
}

void vecAsStr(char buffer[], vector v) {
    sprintf(buffer, "(%.17g, %.17g, %.17g)", v.x, v.y, v.z);
}

void rotate(vector i, vector j, double alpha, vector ps[]) {
    ps[0] = mulAdd(i, j, cos(alpha), sin(alpha));
    ps[1] = mulAdd(i, j, -sin(alpha), cos(alpha));
}
 
void orbitalStateVectors(
    double semimajorAxis, double eccentricity, double inclination,
    double longitudeOfAscendingNode, double argumentOfPeriapsis,
    double trueAnomaly, vector ps[]) {

    vector i = {1.0, 0.0, 0.0};
    vector j = {0.0, 1.0, 0.0};
    vector k = {0.0, 0.0, 1.0};
    double l = 2.0, c, s, r, rprime;
    vector qs[2];
    rotate(i, j, longitudeOfAscendingNode, qs);
    i = qs[0]; j = qs[1];
    rotate(j, k, inclination, qs);
    j = qs[0];
    rotate(i, j, argumentOfPeriapsis, qs);
    i = qs[0]; j = qs[1];
    if (eccentricity != 1.0)  l = 1.0 - eccentricity * eccentricity;
    l *= semimajorAxis;
    c = cos(trueAnomaly);
    s = sin(trueAnomaly);
    r = l / (1.0 + eccentricity * c);
    rprime = s * r * r / l;
    ps[0] = mulAdd(i, j, c, s);
    ps[0] = mul(ps[0], r);
    ps[1] = mulAdd(i, j, rprime * c - r * s, rprime * s + r * c);
    ps[1] = div(ps[1], vabs(ps[1]));
    ps[1] = mul(ps[1], sqrt(2.0 / r - 1.0 / semimajorAxis));
}

int main() {
    double longitude = 355.0 / (113.0 * 6.0);
    vector ps[2];
    char buffer[80];
    orbitalStateVectors(1.0, 0.1, 0.0, longitude, 0.0, 0.0, ps);
    vecAsStr(buffer, ps[0]);
    printf("Position : %s\n", buffer);
    vecAsStr(buffer, ps[1]);
    printf("Speed    : %s\n", buffer);
    return 0;
}











Ada: with Ada.Text_IO;   use Ada.Text_IO;
with Ada.Numerics.Generic_Real_Arrays;
with Ada.Numerics.Generic_Elementary_Functions;

procedure Orbit is

   type Real is new Long_Float;

   package Real_Arrays is
      new Ada.Numerics.Generic_Real_Arrays (Real => Real);
   use Real_Arrays;

   package Math is
      new Ada.Numerics.Generic_Elementary_Functions (Float_Type => Real);

   subtype Vector_3D is Real_Vector (1 .. 3);

   procedure Put (V : Vector_3D) is
      package Real_IO is
        new Ada.Text_Io.Float_IO (Num => Real);
   begin
      Put ("(");
      Real_IO.Put (V (1), Exp => 0, Aft => 6);  Put (",");
      Real_IO.Put (V (2), Exp => 0, Aft => 6);  Put (",");
      Real_IO.Put (V (3), Exp => 0, Aft => 6);  Put (")");
   end Put;

   function Mul_Add (V1 : Vector_3D;
                     X1 : Real;
                     V2 : Vector_3D;
                     X2 : Real)
                    return Vector_3D is
   begin
      return V1 * X1 + V2 * X2;
   end Mul_Add;

   procedure Rotate (R1    : out Vector_3D;
                     R2    : out Vector_3D;
                     I     :     Vector_3D;
                     J     :     Vector_3D;
                     Alpha :     Real) is
   begin
      R1 := Mul_Add (I, +Math.Cos (Alpha), J, Math.Sin (Alpha));
      R2 := Mul_Add (I, -Math.Sin (Alpha), J, Math.Cos (Alpha));
   end Rotate;

   type Orbital_State_Vectors is record
      Position : Vector_3D;
      Speed    : Vector_3D;
   end record;

   function Calculate_Orbital_State
      (Semimajor_Axis             : Real;
      Eccentricity                : Real;
      Inclination                 : Real;
      Longitude_Of_Ascending_Node : Real;
      Argument_Of_Periapsis       : Real;
      True_Anomaly                : Real)
     return Orbital_State_Vectors
   is
      I : Vector_3D := (1.0, 0.0, 0.0);
      J : Vector_3D := (0.0, 1.0, 0.0);
      K : constant Vector_3D := (0.0, 0.0, 1.0);
      P_R1, P_R2 : Vector_3D;
      State    : Orbital_State_Vectors;
      Position : Vector_3D renames State.Position;
      Speed    : Vector_3D renames State.Speed;
   begin
      Rotate (P_R1, P_R2, I, J, Longitude_Of_Ascending_Node);
      I := P_R1; J := P_R2;
      Rotate (P_R1, P_R2, J, K, Inclination);
      J := P_R1;
      Rotate (P_R1, P_R2, I, J, Argument_Of_Periapsis);
      I := P_R1; J := P_R2;
      declare
         L : constant Real :=
           Semimajor_Axis * (if (Eccentricity = 1.0) then 2.0
           else (1.0 - Eccentricity * Eccentricity));
         C : constant Real := Math.Cos (True_Anomaly);
         S : constant Real := Math.Sin (True_Anomaly);
         R : constant Real := L / (1.0 + Eccentricity * C);
         Rprime   : constant Real      := S * R * R / L;
      begin
         Position := Mul_Add (I, C, J, S) * R;
         Speed    := Mul_Add (I, Rprime * C - R * S,
                              J, Rprime * S + R * C);
         Speed := Speed / abs (Speed);
         Speed := Speed * Math.Sqrt (2.0 / R - 1.0 / Semimajor_Axis);
      end;
      return State;
   end Calculate_Orbital_State;

   Longitude : constant Real := 355.000 / (113.000 * 6.000);
   State     : constant Orbital_State_Vectors :=

     Calculate_Orbital_State
       (Semimajor_Axis              => 1.000,
        Eccentricity                => 0.100,
        Inclination                 => 0.000,
        Longitude_Of_Ascending_Node => Longitude,
        Argument_Of_Periapsis       => 0.000,
        True_Anomaly                => 0.000);
begin
   Put ("Position : "); Put (State.Position); New_Line;
   Put ("Speed    : "); Put (State.Speed);    New_Line;
end Orbit;