Single Orbit Simulation in Python

include/gnc/ode.hpp

@@ -17,22 +17,6 @@
 #ifndef GNC_ODE_HPP_
 #define GNC_ODE_HPP_
 
-#define GNC_ODEX_SINGLE_TEMPLATE(odex, type) \
-    template \
-    void odex<type>(type, type, type const *, type *, unsigned int, type *, \
-        void (* const)(type, type const *, type *))
-
-#define GNC_ODEX_SINGLE_EXTERN_TEMPLATE(odex, type) \
-    extern GNC_ODEX_SINGLE_TEMPLATE(odex, type)
-
-#define GNC_ODEX_MULTI_TEMPLATE(odex, type) \
-    template \
-    void odex<type>(type const *, unsigned int, type **, unsigned int, type *, \
-        void (* const)(type, type const *, type *))
-
-#define GNC_ODEX_MULTI_EXTERN_TEMPLATE(odex, type) \
-    extern GNC_ODEX_MULTI_TEMPLATE(odex, type)
-
 #define GNC_ODEXX_TEMPLATE(odexx, type) \
     template \
     int odexx<type>(type, type, type const *, type *, unsigned int, type *, \
@@ -57,158 +41,6 @@ enum : int {
   ODE_ERR_BAD_INTERVAL = (0b1 << 2)
 };
 
-/** @fn ode1
- *  @param[in]  ti Initial conditions for the independant variable.
- *  @param[in]  dt Integrator step for the independant variable.
- *  @param[in]  yi Initial conditions for the dependant variables.
- *  @param[out] yf Final state of the system (dependant varaibles).
- *  @param[in]  ne Number of dependant variables.
- *  @param[in]  bf Buffer of length (ne).
- *  @param[in]  f  Dependant variable update function.
- *  Integrates the system from (ti, yi) to (ti + dt, yf) using a first order
- *  method. You may pass yi and yf as the same pointer if yi can be overwritten.
- *  NOTE: Template specializations are provided for double and float types. */
-template <typename T>
-void ode1(T ti, T dt, T const *yi, T *yf, unsigned int ne, T *bf,
-    void (*const f)(T, T const *, T *));
-
-GNC_ODEX_SINGLE_EXTERN_TEMPLATE(ode1, float);
-GNC_ODEX_SINGLE_EXTERN_TEMPLATE(ode1, double);
-
-/** @fn ode1
- *  @param[in]     t  Array of independant variables states.
- *  @param[in]     nt Length of the dependant variable array t.
- *  @param[inout]  y  Array of dependant variables states.
- *  @param[in]     ne Number of dependant variables.
- *  @param[in]     bf Buffer of length (ne).
- *  @param[in]     f  Dependant variable update function.
- *  Integrates the system using a first order method through the state sequence
- *  below given t[i] and y[0]:
- *    (t[0], y[0]) -> (t[1], y[1]) -> ... -> (t[nt - 1], y[nt - 1])
- *  Passing y as a list of the same pointer is permissible if intermediate state
- *  information can be overwritten.
- *  NOTE: Template specializations are provided for double and float types. */
-template <typename T>
-void ode1(T const *t, unsigned int nt, T **y, unsigned int ne, T *bf,
-    void (*const f)(T, T const *, T *));
-
-GNC_ODEX_MULTI_EXTERN_TEMPLATE(ode1, float);
-GNC_ODEX_MULTI_EXTERN_TEMPLATE(ode1, double);
-
-/** @fn ode2
- *  @param[in]  ti Initial conditions for the independant variable.
- *  @param[in]  dt Integrator step for the independant variable.
- *  @param[in]  yi Initial conditions for the dependant variables.
- *  @param[out] yf Final state of the system (dependant varaibles).
- *  @param[in]  ne Number of dependant variables.
- *  @param[in]  bf Buffer of length (3 * ne).
- *  @param[in]  f  Dependant variable update function.
- *  Integrates the system from (ti, yi) to (ti + dt, yf) using a second order
- *  method. You may pass yi and yf as the same pointer if yi can be overwritten.
- *  NOTE: Template specializations are provided for double and float types. */
-template <typename T>
-void ode2(T ti, T dt, T const *yi, T *yf, unsigned int ne, T *bf,
-    void (*const f)(T, T const *, T *));
-
-GNC_ODEX_SINGLE_EXTERN_TEMPLATE(ode2, float);
-GNC_ODEX_SINGLE_EXTERN_TEMPLATE(ode2, double);
-
-/** @fn ode2
- *  @param[in]     t  Array of independant variables states.
- *  @param[in]     nt Length of the dependant variable array t.
- *  @param[inout]  y  Array of dependant variables states.
- *  @param[in]     ne Number of dependant variables.
- *  @param[in]     bf Buffer of length (3 * ne).
- *  @param[in]     f  Dependant variable update function.
- *  Integrates the system using a second order method through the state sequence
- *  below given t[i] and y[0]:
- *    (t[0], y[0]) -> (t[1], y[1]) -> ... -> (t[nt - 1], y[nt - 1])
- *  Passing y as a list of the same pointer is permissible if intermediate state
- *  information can be overwritten.
- *  NOTE: Template specializations are provided for double and float types. */
-template <typename T>
-void ode2(T const *t, unsigned int nt, T **y, unsigned int ne, T *bf,
-    void (*const f)(T, T const *, T *));
-
-GNC_ODEX_MULTI_EXTERN_TEMPLATE(ode2, float);
-GNC_ODEX_MULTI_EXTERN_TEMPLATE(ode2, double);
-
-/** @fn ode3
- *  @param[in]  ti Initial conditions for the independant variable.
- *  @param[in]  dt Integrator step for the independant variable.
- *  @param[in]  yi Initial conditions for the dependant variables.
- *  @param[out] yf Final state of the system (dependant varaibles).
- *  @param[in]  ne Number of dependant variables.
- *  @param[in]  bf Buffer of length (4 * ne).
- *  @param[in]  f  Dependant variable update function.
- *  Integrates the system from (ti, yi) to (ti + dt, yf) using a third order
- *  method. You may pass yi and yf as the same pointer if yi can be overwritten.
- *  NOTE: Template specializations are provided for double and float types. */
-template <typename T>
-void ode3(T ti, T dt, T const *yi, T *yf, unsigned int ne, T *bf,
-    void (*const f)(T, T const *, T *));
-
-GNC_ODEX_SINGLE_EXTERN_TEMPLATE(ode3, float);
-GNC_ODEX_SINGLE_EXTERN_TEMPLATE(ode3, double);
-
-/** @fn ode3
- *  @param[in]     t  Array of independant variables states.
- *  @param[in]     nt Length of the dependant variable array t.
- *  @param[inout]  y  Array of dependant variables states.
- *  @param[in]     ne Number of dependant variables.
- *  @param[in]     bf Buffer of length (4 * ne).
- *  @param[in]     f  Dependant variable update function.
- *  Integrates the system using a third order method through the state sequence
- *  below given t[i] and y[0]:
- *    (t[0], y[0]) -> (t[1], y[1]) -> ... -> (t[nt - 1], y[nt - 1])
- *  Passing y as a list of the same pointer is permissible if intermediate state
- *  information can be overwritten.
- *  NOTE: Template specializations are provided for double and float types. */
-template <typename T>
-void ode3(T const *t, unsigned int nt, T **y, unsigned int ne, T *bf,
-    void (*const f)(T, T const *, T *));
-
-GNC_ODEX_MULTI_EXTERN_TEMPLATE(ode3, float);
-GNC_ODEX_MULTI_EXTERN_TEMPLATE(ode3, double);
-
-/** @fn ode4
- *  @param[in]  ti Initial conditions for the independant variable.
- *  @param[in]  dt Integrator step for the independant variable.
- *  @param[in]  yi Initial conditions for the dependant variables.
- *  @param[out] yf Final state of the system (dependant varaibles).
- *  @param[in]  ne Number of dependant variables.
- *  @param[in]  bf Buffer of length (5 * ne).
- *  @param[in]  f  Dependant variable update function.
- *  Integrates the system from (ti, yi) to (ti + dt, yf) using a fourth order
- *  method. You may pass yi and yf as the same pointer if yi can be overwritten.
- *  NOTE: Template specializations are provided for double and float types. */
-template <typename T>
-void ode4(T ti, T dt, T const *yi, T *yf, unsigned int ne, T *bf,
-    void (*const f)(T, T const *, T *));
-
-GNC_ODEX_SINGLE_EXTERN_TEMPLATE(ode4, float);
-GNC_ODEX_SINGLE_EXTERN_TEMPLATE(ode4, double);
-
-/** @fn ode4
- *  @param[in]     t  Array of independant variables states.
- *  @param[in]     nt Length of the dependant variable array t.
- *  @param[inout]  y  Array of dependant variables states.
- *  @param[in]     ne Number of dependant variables.
- *  @param[in]     bf Buffer of length (5 * ne).
- *  @param[in]     f  Dependant variable update function.
- *  Integrates the system using a fourth order method through the state sequence
- *  below given t[i] and y[0]:
- *    (t[0], y[0]) -> (t[1], y[1]) -> ... -> (t[nt - 1], y[nt - 1])
- *  Passing y as a list of the same pointer is permissible if intermediate state
- *  information can be overwritten.
- *  NOTE: Template specializations are provided for double and float types. */
-template <typename T>
-void ode4(T const *t, unsigned int nt, T **y, unsigned int ne, T *bf,
-    void (*const f)(T, T const *, T *));
-
-GNC_ODEX_MULTI_EXTERN_TEMPLATE(ode4, float);
-GNC_ODEX_MULTI_EXTERN_TEMPLATE(ode4, double);
-
 /** @fn ode23
  *  @param[in]  ti       Initial conditions for the independant variable.
  *  @param[in]  tf       Desired final state for the independant variable.

include/gnc/ode1.hpp

@@ -0,0 +1,76 @@
+//
+// MIT License
+//
+// Copyright (c) 2020 Pathfinder for Autonomous Navigation (PAN)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+/** @file gnc/ode1.hpp
+ *  @author Kyle Krol
+ */
+
+#ifndef GNC_ODE1_HPP_
+#define GNC_ODE1_HPP_
+
+#include <lin/core.hpp>
+
+namespace gnc {
+
+/** @brief First order fixed step size integrator.
+ *
+ *  @tparam T Fundamental data type.
+ *  @tparam N Number of state parameters.
+ */
+template <typename T, lin::size_t N>
+class Ode1 {
+ private:
+  lin::Vector<T, N> _k[1];
+
+ public:
+  /** @brief Step a differential equation forward in time by a single timestep.
+   *
+   *  @param[in] ti  Initial time.
+   *  @param[in] dt  Integrator timestep.
+   *  @param[in] xi  Initial state.
+   *  @param[in] ptr Pointer to arbitrary data accesible in the update function.
+   *  @param[in] dx  Differential update function.
+   *
+   *  Used for fixed timestep numerical integration. Implements the first order
+   *  integration scheme known as Euler's method.
+   *
+   *  Reference(s):
+   *   - https://en.wikipedia.org/wiki/Euler_method
+   */
+  lin::Vector<T, N> operator()(
+      T ti, T dt,
+      lin::Vector<T, N> const &xi, void *ptr,
+      lin::Vector<T, N> (*dx)(T t, lin::Vector<T, N> const &x, void *ptr)) {
+    // Scratch buffers
+    auto &k1 = _k[0];
+
+    k1 = dx(ti, xi, ptr);
+
+    // Step forwad in time
+    return (xi + dt * k1).eval();
+  }
+};
+}  // namespace gnc
+
+#endif

include/gnc/ode2.hpp

@@ -0,0 +1,85 @@
+//
+// MIT License
+//
+// Copyright (c) 2020 Pathfinder for Autonomous Navigation (PAN)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+/** @file gnc/ode2.hpp
+ *  @author Kyle Krol
+ */
+
+#ifndef GNC_ODE2_HPP_
+#define GNC_ODE2_HPP_
+
+#include <lin/core.hpp>
+
+namespace gnc {
+
+/** @brief Second order fixed step size integrator.
+ *
+ *  @tparam T Fundamental data type.
+ *  @tparam N Number of state parameters.
+ */
+template <typename T, lin::size_t N>
+class Ode2 {
+ private:
+  lin::Vector<T, N> _k[3];
+
+ public:
+  /** @brief Step a differential equation forward in time by a single timestep.
+   *
+   *  @param[in] ti  Initial time.
+   *  @param[in] dt  Integrator timestep.
+   *  @param[in] xi  Initial state.
+   *  @param[in] ptr Pointer to arbitrary data accesible in the update function.
+   *  @param[in] dx  Differential update function.
+   *
+   *  Used for fixed timestep numerical integration. Implements the second order
+   *  integration scheme known as Heun's method.
+   *
+   *  Reference(s):
+   *   - https://en.wikipedia.org/wiki/List_of_Runge–Kutta_methods#Heun's_method
+   */
+  lin::Vector<T, N> operator()(
+      T ti, T dt,
+      lin::Vector<T, N> const &xi, void *ptr,
+      lin::Vector<T, N> (*dx)(T t, lin::Vector<T, N> const &x, void *ptr)) {
+    // Table of integration constants
+    static constexpr T
+        b1 = 1.0 / 2.0, b2 = 1.0 / 2.0;
+
+    // Scratch buffers
+    auto &ks = _k[0];
+    auto &k1 = _k[1];
+    auto &k2 = _k[2];
+
+    // Step forward
+    k1 = dx(ti, xi, ptr);
+    ks = xi + dt * k1;
+    k2 = dx(ti + dt, ks, ptr);
+
+    // Step forward in time
+    return (xi + dt * (b1 * k1 + b2 * k2)).eval();
+  }
+};
+}  // namespace gnc
+
+#endif