Merge in the er7_utils integrators

Taking in the latest er7_utils directory and adding it into Trick in the
same location it was as an external repository.  Made one change to
the files_to_ICG.hh file in the repository to remove the CheckpointHelper
header files.  Those go in the Trick files_to_ICG.hh file.

refs #180

Conflicts:
	Makefile
	autoconf/configure.ac
	configure
	include/trick/files_to_ICG.hh
	share/trick/makefiles/Makefile.common
	share/trick/makefiles/config_user.mk.in
	trick_source/sim_services/Integrator/Makefile
	trick_source/trick_swig/Makefile

Makefile

@@ -60,7 +60,6 @@ SIM_SERV_DIRS = \
 SIM_SERV_OBJS = $(addsuffix /object_$(TRICK_HOST_CPU)/*.o ,$(SIM_SERV_DIRS))
 
 ER7_UTILS_DIRS = \
-	${ER7_UTILS_HOME}/CheckpointHelper \
 	${ER7_UTILS_HOME}/integration/abm4 \
 	${ER7_UTILS_HOME}/integration/beeman \
 	${ER7_UTILS_HOME}/integration/core \
@@ -80,6 +79,9 @@ ER7_UTILS_DIRS = \
 	${ER7_UTILS_HOME}/interface \
 	${ER7_UTILS_HOME}/math \
 	${ER7_UTILS_HOME}/trick/integration
+ifeq ($(USE_ER7_UTILS_CHECKPOINTHELPER), 1)
+ER7_UTILS_DIRS += ${ER7_UTILS_HOME}/CheckpointHelper
+endif
 ER7_UTILS_OBJS = $(addsuffix /object_$(TRICK_HOST_CPU)/*.o ,$(ER7_UTILS_DIRS))
 
 UTILS_DIRS = \
@@ -110,7 +112,7 @@ UNIT_TEST_DIRS := \
     $(wildcard ${TRICK_HOME}/trick_source/sim_services/*/test) \
     $(wildcard ${TRICK_HOME}/trick_source/trick_utils/*/test) \
     ${TRICK_HOME}/trick_source/data_products/DPX/test/unit_test
-ifeq ($(USE_ER7_UTILS_INTEGRATORS), 0)
+ifeq ($(USE_ER7_UTILS), 0)
   UNIT_TEST_DIRS := $(filter-out %Integrator/test,$(UNIT_TEST_DIRS))
 endif
 
@@ -163,7 +165,7 @@ no_dp: $(TRICK_LIB) $(TRICK_SWIG_LIB) $(TRICK_LIB_DIR)/master.o
 	@ echo ; echo "Trick libs compiled:" ; date
 
 # 1.1.1 Build libTrick.a
-ifeq ($(USE_ER7_UTILS_INTEGRATORS), 1)
+ifeq ($(USE_ER7_UTILS), 1)
 $(TRICK_LIB): $(SIM_SERV_DIRS) $(ER7_UTILS_DIRS) $(UTILS_DIRS) | $(TRICK_LIB_DIR)
 	ar crs $@ $(SIM_SERV_OBJS) $(ER7_UTILS_OBJS) $(UTILS_OBJS)
 else
@@ -193,7 +195,7 @@ make_er7_makefiles:
 	   $(CP) ${TRICK_HOME}/trick_source/sim_services/Executive/Makefile $$i; \
 	done
 
-ifeq ($(USE_ER7_UTILS_INTEGRATORS), 1)
+ifeq ($(USE_ER7_UTILS), 1)
 icg_sim_serv: | make_er7_makefiles
 endif
 
@@ -286,7 +288,7 @@ clean: clean_sim_serv clean_utils clean_swig clean_dp clean_ICG clean_java
 	@/bin/rm -rf $(TRICK_BIN_DIR)
 	@/bin/rm -rf $(TRICK_LIB_DIR)
 
-ifeq ($(USE_ER7_UTILS_INTEGRATORS), 1)
+ifeq ($(USE_ER7_UTILS), 1)
 clean: clean_er7_utils 
 endif
 
@@ -310,7 +312,7 @@ clean_swig:
 	   $(MAKE) -C $$i real_clean ; \
 	done
 
-ifeq ($(USE_ER7_UTILS_INTEGRATORS), 1)
+ifeq ($(USE_ER7_UTILS), 1)
 clean_swig: make_er7_makefiles 
 endif
 
@@ -380,7 +382,7 @@ ${PREFIX}/trick/trick-$(TRICK_VERSION) :
 
 copy_trick_source: copy_codegen copy_sim_objects copy_sim_serv_dirs copy_utils_dirs copy_swig
 
-ifeq ($(USE_ER7_UTILS_INTEGRATORS), 1)
+ifeq ($(USE_ER7_UTILS), 1)
 copy_trick_source: copy_er7_utils_dirs
 endif
 

configure

@@ -33,6 +33,7 @@ tprocte=""
 bc635=""
 java_min_ver=1.6.0
 no_create=0
+er7utils=1
 dev=1
 help=0
 lsb_release=/usr/bin/lsb_release
@@ -304,6 +305,8 @@ do
            python=$optarg ;;
        --no_create)
            no_create=1 ;;
+       --no-er7utils)
+           er7utils=0 ;;
        --swig)
            prev=swig ;;
        --swig=*)
@@ -825,6 +828,8 @@ GTEST_HOME = $gtest
 LIBXML = $libxml
 MOTIF_HOME = $motif
 
+USE_ER7_UTILS = $er7utils
+
 # experimental, default to 0
 HAVE_ZEROCONF = 0
 

makefiles/Makefile.common

@@ -78,12 +78,15 @@ TRICK_CFLAGS += -DHAVE_ZEROCONF
 TRICK_CXXFLAGS += -DHAVE_ZEROCONF
 endif
 
-export USE_ER7_UTILS_INTEGRATORS := 0
-ifneq ("$(wildcard $(TRICK_HOME)/trick_source/er7_utils)","")  # if the er7_utils directory exists
+ifeq ($(USE_ER7_UTILS), 1)
   ER7_UTILS_HOME := $(TRICK_HOME)/trick_source/er7_utils
-  USE_ER7_UTILS_INTEGRATORS := 1
   TRICK_CFLAGS += -DUSE_ER7_UTILS_INTEGRATORS
   TRICK_CXXFLAGS += -DUSE_ER7_UTILS_INTEGRATORS
+ifneq ($(wildcard ${ER7_UTILS_HOME}/CheckpointHelper),)
+  USE_ER7_UTILS_CHECKPOINTHELPER = 1
+  TRICK_SYSTEM_CFLAGS += -DUSE_ER7_UTILS_CHECKPOINTHELPER
+  TRICK_SYSTEM_CXXFLAGS += -DUSE_ER7_UTILS_CHECKPOINTHELPER
+endif
 endif
 
 endif #TRICK_MAKE_COMMON

trick_source/.gitignore

@@ -1 +0,0 @@
-er7_utils

trick_source/er7_utils/CheckpointHelper/.gitignore

@@ -0,0 +1 @@
+libSTLHelper.a

trick_source/er7_utils/CheckpointHelper/Alloc.cpp

@@ -0,0 +1,198 @@
+/*
+ * Alloc.cpp
+ *
+ *  Created on: Aug 24, 2015
+ *      Author: tbrain
+ */
+
+#include "Alloc.hh"
+
+namespace CheckpointHelper
+{
+#if (defined TRICK_VER) && (TRICK_VER >= 10)
+std::string treatAllocName(const std::string & nameIn)
+{
+   std::string treatedName(nameIn);
+
+   // Always remove "&"
+   std::size_t found = treatedName.find('&');
+   if (found != std::string::npos)
+   {
+      treatedName.erase(0, 1);
+   }
+
+   found = treatedName.find('.');
+   while (found != std::string::npos)
+   {
+      treatedName.replace(found, 1, "_");
+      found = treatedName.find('.');
+   }
+   found = treatedName.find('[');
+   while (found != std::string::npos)
+   {
+      treatedName.replace(found, 1, "_");
+      found = treatedName.find('[');
+   }
+   found = treatedName.find(']');
+   while (found != std::string::npos)
+   {
+      treatedName.replace(found, 1, "_");
+      found = treatedName.find(']');
+   }
+   found = treatedName.find(' ');
+   while (found != std::string::npos)
+   {
+      treatedName.replace(found, 1, "_");
+      found = treatedName.find(' ');
+   }
+   found = treatedName.find('+');
+   while (found != std::string::npos)
+   {
+      treatedName.replace(found, 1, "_");
+      found = treatedName.find('+');
+   }
+   found = treatedName.find(':');
+   while (found != std::string::npos)
+   {
+      treatedName.replace(found, 1, "_");
+      found = treatedName.find(':');
+   }
+   found = treatedName.find('(');
+   while (found != std::string::npos)
+   {
+      treatedName.replace(found, 1, "_");
+      found = treatedName.find('(');
+   }
+   found = treatedName.find(')');
+   while (found != std::string::npos)
+   {
+      treatedName.replace(found, 1, "_");
+      found = treatedName.find(')');
+   }
+
+   // Check for linked-list, we want to prevent very long names
+   found = treatedName.rfind("AUTOLIST");
+   if (found == std::string::npos)
+   {
+      found = treatedName.rfind("_0__");
+      if (found != std::string::npos)
+      {
+         size_t newParamNamePos = found + 4;
+         std::string newParamName = treatedName.substr(newParamNamePos);
+         size_t remChars = treatedName.length() - newParamNamePos;
+         if (remChars <= found)
+         {
+            std::string prevParamName = treatedName.substr(found - remChars, remChars);
+            if (prevParamName.compare(newParamName) == 0)
+            {
+               treatedName = treatedName.substr(0, found);
+               treatedName += "_AUTOLIST1";
+            }
+         }
+      }
+   }
+   else
+   {
+      size_t afterListPos = treatedName.rfind("_0__");
+      std::stringstream iss(treatedName.substr(found + 8, afterListPos));
+      int currIdx = 0;
+      iss >> currIdx;
+      ++currIdx;
+      iss.str("");
+      iss << currIdx;
+      treatedName = treatedName.substr(0, found + 8);
+      treatedName += iss.str();
+   }
+
+   return treatedName;
+}
+
+bool updateAllocName(void * parentPtr, void * ptr, const std::string & name, const std::string & funcName,
+      bool printError)
+{
+   bool success = false;
+   if (parentPtr != 0x0)
+   {
+      ALLOC_INFO * info = trick_MM->get_alloc_info_of(parentPtr);
+      if (info != 0x0)
+      {
+         if (info->name != 0x0)
+         {
+            std::string pathName = trick_MM->ref_name_from_address(parentPtr);
+            pathName = treatAllocName(pathName);
+            std::stringstream ss;
+            std::string treatedName = treatAllocName(name);
+            ss << pathName << "_" << treatedName;
+            trick_MM->set_name_at(ptr, ss.str().c_str());
+            success = true;
+         }
+         else
+         {
+            if (printError)
+            {
+               if (ptr == 0x0)
+               {
+                  std::stringstream ss;
+                  ss << "Could not name an allocation with name " << name << " because the allocated ptr is NULL. "
+                        << "Allocation call invoked from " << funcName << std::endl;
+                  message_publish(1, ss.str().c_str());
+               }
+               else
+               {
+                  std::stringstream ss;
+                  ss << "Could not name the allocation at " << ptr << " because it's parent data structure at "
+                        << parentPtr
+                        << " has no name registered with Trick MemoryManager. Allocation call invoked from " << funcName
+                        << std::endl;
+                  message_publish(1, ss.str().c_str());
+               }
+            }
+         }
+      }
+      else
+      {
+         if (printError)
+         {
+            if (ptr == 0x0)
+            {
+               std::stringstream ss;
+               ss << "Could not name an allocation with name " << name << " because the allocated ptr is NULL. "
+                     << "Allocation call invoked from " << funcName << std::endl;
+               message_publish(1, ss.str().c_str());
+            }
+            else
+            {
+               std::stringstream ss;
+               ss << "Could not name the allocation at " << ptr << " because it's parent data structure at "
+                     << parentPtr << " was not found by Trick MemoryManagerAllocation call invoked from " << funcName
+                     << std::endl;
+               message_publish(1, ss.str().c_str());
+            }
+         }
+      }
+   }
+   else
+   {
+      if (printError)
+      {
+         if (ptr == 0x0)
+         {
+            std::stringstream ss;
+            ss << "Could not name an allocation with name  " << name
+                  << " because the parent and allocated pointers are NULL. " << "Allocation call invoked from "
+                  << funcName << std::endl;
+            message_publish(1, ss.str().c_str());
+         }
+         else
+         {
+            std::stringstream ss;
+            ss << "Could not name the allocation at " << ptr << " because it's parent pointer is NULL. "
+                  << "Allocation call invoked from " << funcName << std::endl;
+            message_publish(1, ss.str().c_str());
+         }
+      }
+   }
+   return success;
+}
+#endif
+}

trick_source/er7_utils/CheckpointHelper/Alloc.hh

@@ -0,0 +1,472 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (alloc template for allocating and reallocating elements.)
+
+ LIBRARY DEPENDENCY:
+ ((Alloc.cpp))
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (May 2013) (--)))
+
+ *******************************************************************************/
+
+#ifndef CHECKPOINTHELPER_alloc_HH_
+#define CHECKPOINTHELPER_alloc_HH_
+#if (defined TRICK_VER) && (TRICK_VER >= 10)
+#include "sim_services/MemoryManager/include/MemoryManager.hh"
+#include "sim_services/Message/include/message_proto.h"
+#endif
+#include <string>
+#include <sstream>
+#include <algorithm>
+#include "Manager.hh"
+
+namespace CheckpointHelper
+{
+
+extern Manager * helperManager;
+
+#if (defined TRICK_VER) && (TRICK_VER >= 10)
+typedef TRICK_TYPE MM_TYPE;
+
+std::string treatAllocName(const std::string & nameIn);
+bool updateAllocName(void * parentPtr, void * ptr, const std::string & name, const std::string & funcName,
+      bool printError);
+
+template<class T>
+void delete_alloc(T ** x, const std::string & whereAt, bool external)
+{
+   if (!external)
+   {
+      if (trick_MM->is_alloced(*x))
+      {
+         trick_MM->delete_var((void *) *x);
+         if (helperManager != 0x0)
+         {
+            helperManager->removeConstructorAlloc(*x);
+         }
+         *x = 0x0;
+      }
+      else if (*x != 0x0)
+      {
+         std::stringstream ss;
+         ss << "CheckpointHelper::delete_alloc ERROR attempting to delete ptr at " << whereAt
+               << " that is not allocated.\n";
+         message_publish(MSG_WARNING, ss.str().c_str());
+         *x = 0x0;
+      }
+   }
+   else
+   {
+      if (trick_MM->is_alloced(*x))
+      {
+         trick_MM->delete_extern_var((void *) *x);
+         if (helperManager != 0x0)
+         {
+            helperManager->removeConstructorAlloc(*x);
+            size_t numEntries = helperManager->getExternNum(*x);
+            helperManager->removeExternAlloc(*x);
+            for (int ii = numEntries - 1; ii >= 0; --ii)
+            {
+               (*x)[ii].~T();
+            }
+            free(*x);
+            *x = 0x0;
+         }
+      }
+      else if (*x != 0x0)
+      {
+         std::stringstream ss;
+         ss << "CheckpointHelper::delete_alloc ERROR attempting to delete external ptr at " << whereAt
+               << " that is not allocated.\n";
+         message_publish(MSG_WARNING, ss.str().c_str());
+         *x = 0x0;
+      }
+   }
+}
+
+template<class U, class T>
+void operatornew_alloc(void * parentPtr, T ** x, int n, const std::string & TYPE, const std::string & name,
+      const std::string & funcNameIn)
+{
+   if (n)
+   {
+      if (trick_MM->is_alloced(*x))
+      {
+         int orig_size = trick_MM->get_size((char *) *x);
+         if (n > orig_size)
+         {
+            helperManager->removeConstructorAlloc(*x);
+            *x = (U *) trick_MM->resize_array((void *) *x, n);
+            helperManager->addConstructorAlloc(parentPtr, *x, name, funcNameIn);
+         }
+      }
+      else
+      {
+         ALLOC_INFO * info = trick_MM->get_alloc_info_of(x);
+         if (info != 0x0)
+         {
+            if (info->name != 0x0)
+            {
+               *x = static_cast<U *>(trick_MM->declare_operatornew_var(TYPE, sizeof(U) * n, sizeof(U)));
+               std::string pathName = trick_MM->ref_name_from_address(x);
+               pathName = treatAllocName(pathName);
+               trick_MM->set_name_at(*x, pathName.c_str());
+            }
+            else
+            {
+               *x = static_cast<U *>(trick_MM->declare_operatornew_var(TYPE, sizeof(U) * n, sizeof(U)));
+               if (helperManager != 0x0)
+               {
+                  helperManager->addConstructorAlloc(parentPtr, *x, name, funcNameIn);
+               }
+            }
+         }
+         else
+         {
+            *x = static_cast<U *>(trick_MM->declare_operatornew_var(TYPE, sizeof(U) * n, sizeof(U)));
+            if (helperManager != 0x0)
+            {
+               helperManager->addConstructorAlloc(parentPtr, *x, name, funcNameIn);
+            }
+         }
+      }
+   }
+   else
+   {
+      *x = 0;
+   }
+}
+
+template<class U, class T>
+void alloc(void * parentPtr, T ** x, TRICK_TYPE type, const std::string & TYPE_NAME, int n_stars,
+      const std::string & name, int n_cdims, int cdims[8], bool external, const std::string & funcNameIn)
+{
+   if (cdims[0])
+   {
+      if (trick_MM->is_alloced(*x))
+      {
+         int orig_size = trick_MM->get_size((char *) *x);
+         if (cdims[0] > orig_size)
+         {
+            helperManager->removeConstructorAlloc(*x);
+            *x = (U *) trick_MM->resize_array((void *) *x, cdims[0]);
+            helperManager->addConstructorAlloc(parentPtr, *x, name, funcNameIn);
+         }
+      }
+      else
+      {
+         ALLOC_INFO * info = trick_MM->get_alloc_info_of(x);
+         if (info != 0x0)
+         {
+            if (info->name != 0x0)
+            {
+               std::string pathName = trick_MM->ref_name_from_address(x);
+               pathName = treatAllocName(pathName);
+               if (external)
+               {
+                  if (*x == 0x0)
+                  {
+                     *x = reinterpret_cast<U *>(calloc(cdims[0], sizeof(U)));
+                     new (*x) U[cdims[0]]();
+                     if (helperManager != 0x0)
+                     {
+                        helperManager->addExternAlloc(*x, cdims[0]);
+                     }
+                  }
+                  trick_MM->declare_extern_var(*x, type, TYPE_NAME, n_stars, pathName, n_cdims, cdims);
+               }
+               else
+               {
+                  *x = (U *) trick_MM->declare_var(type, TYPE_NAME, n_stars, pathName, n_cdims, cdims);
+               }
+            }
+            else
+            {
+               if (external)
+               {
+                  if (*x == 0x0)
+                  {
+                     *x = reinterpret_cast<U *>(calloc(cdims[0], sizeof(U)));
+                     new (*x) U[cdims[0]]();
+                     if (helperManager != 0x0)
+                     {
+                        helperManager->addExternAlloc(*x, cdims[0]);
+                     }
+                  }
+                  trick_MM->declare_extern_var(*x, type, TYPE_NAME, n_stars, "", n_cdims, cdims);
+               }
+               else
+               {
+                  *x = (U *) trick_MM->declare_var(type, TYPE_NAME, n_stars, "", n_cdims, cdims);
+               }
+               if (helperManager != 0x0)
+               {
+                  helperManager->addConstructorAlloc(parentPtr, *x, name, funcNameIn);
+               }
+            }
+         }
+         else
+         {
+            if (external)
+            {
+               if (*x == 0x0)
+               {
+                  *x = reinterpret_cast<U *>(calloc(cdims[0], sizeof(U)));
+                  new (*x) U[cdims[0]]();
+                  if (helperManager != 0x0)
+                  {
+                     helperManager->addExternAlloc(*x, cdims[0]);
+                  }
+               }
+               trick_MM->declare_extern_var(*x, type, TYPE_NAME, n_stars, "", n_cdims, cdims);
+            }
+            else
+            {
+               *x = (U *) trick_MM->declare_var(type, TYPE_NAME, n_stars, "", n_cdims, cdims);
+            }
+            if (helperManager != 0x0)
+            {
+               helperManager->addConstructorAlloc(parentPtr, *x, name, funcNameIn);
+            }
+         }
+      }
+   }
+   else
+   {
+      *x = 0;
+   }
+}
+
+template<class U, class T>
+void alloc(void * parentPtr, T ** x, int n, const std::string & TYPE, const std::string & name, bool external,
+      const std::string & funcNameIn)
+{
+   int cdims[8] =
+   { n, 0, 0, 0, 0, 0, 0, 0 };
+   std::string treatedTYPE = TYPE;
+   size_t ret;
+   while ((ret = treatedTYPE.find(' ')) != std::string::npos)
+   {
+      treatedTYPE.erase(ret);
+   }
+   while ((ret = treatedTYPE.find('*')) != std::string::npos)
+   {
+      treatedTYPE.erase(ret);
+   }
+   CheckpointHelper::alloc<U>(parentPtr, x, TRICK_OPAQUE_TYPE, treatedTYPE, 0, name, 1, cdims, external, funcNameIn);
+}
+
+template<class U, class T>
+void alloc(void * parentPtr, T *** x, int n, const std::string & TYPE, const std::string & name, bool external,
+      const std::string & funcNameIn)
+{
+   int cdims[8] =
+   { n, 0, 0, 0, 0, 0, 0, 0 };
+   std::string treatedTYPE = TYPE;
+   size_t ret;
+   while ((ret = treatedTYPE.find(' ')) != std::string::npos)
+   {
+      treatedTYPE.erase(ret);
+   }
+   while ((ret = treatedTYPE.find('*')) != std::string::npos)
+   {
+      treatedTYPE.erase(ret);
+   }
+   CheckpointHelper::alloc<U>(parentPtr, x, TRICK_OPAQUE_TYPE, treatedTYPE, 1, name, 1, cdims, external, funcNameIn);
+}
+
+template<class U, class T>
+void alloc(void * parentPtr, T **** x, int n, const std::string & TYPE, const std::string & name, bool external,
+      const std::string & funcNameIn)
+{
+   int cdims[8] =
+   { n, 0, 0, 0, 0, 0, 0, 0 };
+   std::string treatedTYPE = TYPE;
+   size_t ret;
+   while ((ret = treatedTYPE.find(' ')) != std::string::npos)
+   {
+      treatedTYPE.erase(ret);
+   }
+   while ((ret = treatedTYPE.find('*')) != std::string::npos)
+   {
+      treatedTYPE.erase(ret);
+   }
+   CheckpointHelper::alloc<U>(parentPtr, x, TRICK_OPAQUE_TYPE, treatedTYPE, 2, name, 1, cdims, external, funcNameIn);
+}
+
+#define CHECKPOINTHELPER_PRIM_ALLOC_TEMPS( primType, trickType ) \
+template<> \
+inline void alloc<primType>(void * parentPtr, primType ** x, int n, const std::string & TYPE, const std::string & name, \
+      bool external, const std::string & funcNameIn) \
+{ \
+   int cdims[8] = { n, 0, 0, 0, 0, 0, 0, 0 }; \
+   CheckpointHelper::alloc<primType>(parentPtr, x, trickType, "", 0, name, 1, cdims, external, funcNameIn); \
+} \
+\
+template<> \
+inline void alloc<primType *, primType>(void * parentPtr, primType *** x, int n, const std::string & TYPE, const std::string & name, \
+      bool external, const std::string & funcNameIn) \
+{ \
+   int cdims[8] = { n, 0, 0, 0, 0, 0, 0, 0 }; \
+   CheckpointHelper::alloc<primType *>(parentPtr, x, trickType, "", 1, name, 1, cdims, external, funcNameIn); \
+} \
+\
+template<> \
+inline void alloc<primType **, primType>(void * parentPtr, primType **** x, int n, const std::string & TYPE, const std::string & name, \
+      bool external, const std::string & funcNameIn) \
+{ \
+   int cdims[8] = { n, 0, 0, 0, 0, 0, 0, 0 }; \
+   CheckpointHelper::alloc<primType **>(parentPtr, x, trickType, "", 2, name, 1, cdims, external, funcNameIn); \
+}
+
+CHECKPOINTHELPER_PRIM_ALLOC_TEMPS(double, TRICK_DOUBLE)
+CHECKPOINTHELPER_PRIM_ALLOC_TEMPS(char, TRICK_CHARACTER)
+CHECKPOINTHELPER_PRIM_ALLOC_TEMPS(unsigned char, TRICK_UNSIGNED_CHARACTER)
+CHECKPOINTHELPER_PRIM_ALLOC_TEMPS(std::string, TRICK_STRING)
+CHECKPOINTHELPER_PRIM_ALLOC_TEMPS(int, TRICK_INTEGER)
+CHECKPOINTHELPER_PRIM_ALLOC_TEMPS(unsigned int, TRICK_UNSIGNED_INTEGER)
+CHECKPOINTHELPER_PRIM_ALLOC_TEMPS(long, TRICK_LONG)
+CHECKPOINTHELPER_PRIM_ALLOC_TEMPS(unsigned long, TRICK_UNSIGNED_LONG)
+CHECKPOINTHELPER_PRIM_ALLOC_TEMPS(float, TRICK_FLOAT)
+CHECKPOINTHELPER_PRIM_ALLOC_TEMPS(long long, TRICK_LONG_LONG)
+CHECKPOINTHELPER_PRIM_ALLOC_TEMPS(unsigned long long, TRICK_UNSIGNED_LONG_LONG)
+CHECKPOINTHELPER_PRIM_ALLOC_TEMPS(bool, TRICK_BOOLEAN)
+CHECKPOINTHELPER_PRIM_ALLOC_TEMPS(wchar_t, TRICK_WCHAR)
+
+#else
+
+typedef int MM_TYPE;
+
+template<class T>
+void alloc( void * parentPtr, T ** x, int n, std::string TYPE, std::string name )
+{
+   (void) parentPtr;
+   (void) x;
+   (void) n;
+   (void) TYPE;
+   (void) name;
+
+   *x = new T[n];
+}
+
+template<class T>
+void delete_alloc(T ** x)
+{
+   if (x != 0x0)
+   {
+      if(*x != 0x0)
+      {
+         delete[] *x;
+      }
+   }
+}
+#endif
+
+#ifndef CHECKPOINTHELPER_STRINGIFY
+#define CHECKPOINTHELPER_STRINGIFY(x) #x
+#endif
+#ifndef CHECKPOINTHELPER_TOSTRING
+#define CHECKPOINTHELPER_TOSTRING(x) CHECKPOINTHELPER_STRINGIFY(x)
+#endif
+
+#ifndef CHECKPOINTHELPER_FILELINE
+#define CHECKPOINTHELPER_FILELINE __FILE__ ":" CHECKPOINTHELPER_TOSTRING(__LINE__)
+#endif
+
+// get number of arguments with __NARG__
+#define __NARG__(...)  __NARG_I_(__VA_ARGS__,__RSEQ_N())
+#define __NARG_I_(...) __ARG_N(__VA_ARGS__)
+#define __ARG_N( \
+      _1, _2, _3, _4, _5, _6, _7, _8, _9,_10, \
+     _11,_12,_13,_14,_15,_16,_17,_18,_19,_20, \
+     _21,_22,_23,_24,_25,_26,_27,_28,_29,_30, \
+     _31,_32,_33,_34,_35,_36,_37,_38,_39,_40, \
+     _41,_42,_43,_44,_45,_46,_47,_48,_49,_50, \
+     _51,_52,_53,_54,_55,_56,_57,_58,_59,_60, \
+     _61,_62,_63,N,...) N
+#define __RSEQ_N() \
+     63,62,61,60,                   \
+     59,58,57,56,55,54,53,52,51,50, \
+     49,48,47,46,45,44,43,42,41,40, \
+     39,38,37,36,35,34,33,32,31,30, \
+     29,28,27,26,25,24,23,22,21,20, \
+     19,18,17,16,15,14,13,12,11,10, \
+     9,8,7,6,5,4,3,2,1,0
+
+// general definition for any function name
+#define _VFUNC_(name, n) name##n
+#define _VFUNC(name, n) _VFUNC_(name, n)
+#define VFUNC(func, ...) _VFUNC(func, __NARG__(__VA_ARGS__)) (__VA_ARGS__)
+
+// definition our overloaded macros
+#define CHECKPOINTHELPER_ALLOC_WCSTR6(parent, item, num, type, name, cstr) \
+   CheckpointHelper::operatornew_alloc<type>(parent, &item, num, #type, name, CHECKPOINTHELPER_FILELINE); \
+   for(int ii = 0; ii < num; ++ii) { \
+      new (&item[ii]) type cstr; \
+   }
+
+#define CHECKPOINTHELPER_ALLOC_WCSTR5(item, num, type, name, cstr) \
+   CheckpointHelper::operatornew_alloc<type>(this, &item, num, #type, name, CHECKPOINTHELPER_FILELINE); \
+   for(int ii = 0; ii < num; ++ii) { \
+      new (&item[ii]) type cstr; \
+   }
+
+#define CHECKPOINTHELPER_ALLOC_WCSTR4(item, num, type, cstr) \
+   CheckpointHelper::operatornew_alloc<type>(this, &item, num, #type, #item, CHECKPOINTHELPER_FILELINE); \
+   for(int ii = 0; ii < num; ++ii) { \
+      new (&item[ii]) type cstr; \
+   }
+
+#define CHECKPOINTHELPER_ALLOC_WCSTR3(item, type, cstr) \
+   CheckpointHelper::operatornew_alloc<type>(this, &item, 1, #type, #item, CHECKPOINTHELPER_FILELINE); \
+   new (item) type cstr;
+
+#define CHECKPOINTHELPER_ALLOC5(parent, item, num, type, name) CheckpointHelper::alloc<type>(parent, &item, num, #type, name, false, CHECKPOINTHELPER_FILELINE)
+#define CHECKPOINTHELPER_ALLOC4(item, num, type, name) CheckpointHelper::alloc<type>(this, &item, num, #type, name, false, CHECKPOINTHELPER_FILELINE)
+#define CHECKPOINTHELPER_ALLOC3(item, num, type) CheckpointHelper::alloc<type>(this, &item, num, #type, #item, false, CHECKPOINTHELPER_FILELINE)
+#define CHECKPOINTHELPER_ALLOC2(item, type) CheckpointHelper::alloc<type>(this, &item, 1, #type, #item, false, CHECKPOINTHELPER_FILELINE)
+
+#define CHECKPOINTHELPER_EXTERN_ALLOC_WCSTR6(parent, item, num, type, name, cstr)  \
+   item = reinterpret_cast<type *>(calloc(num, sizeof(type))); \
+   for(int ii = 0; ii < num; ++ii) { \
+      new (&item[ii]) type cstr; \
+   } \
+   CheckpointHelper::alloc<type>(parent, &item, num, #type, name, true, CHECKPOINTHELPER_FILELINE)
+
+#define CHECKPOINTHELPER_EXTERN_ALLOC_WCSTR5(item, num, type, name, cstr) \
+   item = reinterpret_cast<type *>(calloc(num, sizeof(type))); \
+   for(int ii = 0; ii < num; ++ii) { \
+      new (&item[ii]) type cstr; \
+   } \
+   CheckpointHelper::alloc<type>(this, &item, num, #type, name, true, CHECKPOINTHELPER_FILELINE)
+
+#define CHECKPOINTHELPER_EXTERN_ALLOC_WCSTR4(item, num, type, cstr) \
+   item = reinterpret_cast<type *>(calloc(num, sizeof(type))); \
+   for(int ii = 0; ii < num; ++ii) { \
+      new (&item[ii]) type cstr; \
+   } \
+   CheckpointHelper::alloc<type>(this, &item, num, #type, #item, true, CHECKPOINTHELPER_FILELINE)
+
+#define CHECKPOINTHELPER_EXTERN_ALLOC_WCSTR3(item, type, cstr) \
+   item = reinterpret_cast<type *>(calloc(1, sizeof(type))); \
+   new (item) type cstr; \
+   CheckpointHelper::alloc<type>(this, &item, 1, #type, #item, true, CHECKPOINTHELPER_FILELINE)
+
+#define CHECKPOINTHELPER_EXTERN_ALLOC5(parent, item, num, type, name) CheckpointHelper::alloc<type>(parent, &item, num, #type, name, true, CHECKPOINTHELPER_FILELINE)
+#define CHECKPOINTHELPER_EXTERN_ALLOC4(item, num, type, name) CheckpointHelper::alloc<type>(this, &item, num, #type, name, true, CHECKPOINTHELPER_FILELINE)
+#define CHECKPOINTHELPER_EXTERN_ALLOC3(item, num, type) CheckpointHelper::alloc<type>(this, &item, num, #type, #item, true, CHECKPOINTHELPER_FILELINE)
+#define CHECKPOINTHELPER_EXTERN_ALLOC2(item, type) CheckpointHelper::alloc<type>(this, &item, 1, #type, #item, true, CHECKPOINTHELPER_FILELINE)
+
+#define CHECKPOINTHELPER_ALLOC_WCSTR(...) VFUNC(CHECKPOINTHELPER_ALLOC_WCSTR, __VA_ARGS__)
+#define CHECKPOINTHELPER_ALLOC(...) VFUNC(CHECKPOINTHELPER_ALLOC, __VA_ARGS__)
+#define CHECKPOINTHELPER_EXTERN_ALLOC_WCSTR(...) VFUNC(CHECKPOINTHELPER_EXTERN_ALLOC_WCSTR, __VA_ARGS__)
+#define CHECKPOINTHELPER_EXTERN_ALLOC(...) VFUNC(CHECKPOINTHELPER_EXTERN_ALLOC, __VA_ARGS__)
+
+#define CHECKPOINTHELPER_DELETE(item) CheckpointHelper::delete_alloc(&item, CHECKPOINTHELPER_FILELINE " " #item, false)
+#define CHECKPOINTHELPER_EXTERN_DELETE(item) CheckpointHelper::delete_alloc(&item, CHECKPOINTHELPER_FILELINE " " #item, true)
+
+}
+
+#endif /* STLHELPER_alloc_HH_ */

trick_source/er7_utils/CheckpointHelper/CheckpointHelper.sm

@@ -0,0 +1,24 @@
+##include "er7_utils/CheckpointHelper/Manager.hh"
+##include "er7_utils/CheckpointHelper/CheckpointItem.hh"
+##include "er7_utils/CheckpointHelper/InputAllocsChkptRestart.hh"
+
+class CheckpointHelperSimObject : public Trick::SimObject {
+  public:
+     CheckpointHelper::Manager manager;
+     CheckpointHelper::InputAllocsChkptRestart inputAllocs;
+
+     CheckpointHelperSimObject() {
+        P1 ("initialization") manager.processConstructorAllocs();
+
+        ("checkpoint") manager.checkpoint();
+        ("checkpoint") inputAllocs.writeChkptFile();
+
+        P59000 ("preload_checkpoint") inputAllocs.loadChkptFile();
+        ("preload_checkpoint") manager.processConstructorAllocs();
+        ("restart") manager.restart();
+     }
+};
+
+CheckpointHelperSimObject checkpointHelper;
+
+

trick_source/er7_utils/CheckpointHelper/CheckpointItem.cpp

@@ -0,0 +1,39 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (CheckpointItem is the abstract class to be used for automatic registration and
+ checkpointing of items. This package provides STL container equivalents deriving
+ from this classn.)
+
+ LIBRARY DEPENDENCY:
+ ((CheckpointItem.cpp)
+ (Manager.cpp))
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (May 2013) (--)))
+
+ *******************************************************************************/
+
+#include "Manager.hh"
+#include "CheckpointItem.hh"
+
+namespace CheckpointHelper
+{
+
+CheckpointItem::CheckpointItem()
+{
+   if (helperManager != NULL)
+   {
+      helperManager->addItem(*this);
+   }
+}
+
+CheckpointItem::~CheckpointItem()
+{
+   if (helperManager != NULL)
+   {
+      helperManager->removeItem(*this);
+   }
+}
+
+}

trick_source/er7_utils/CheckpointHelper/CheckpointItem.hh

@@ -0,0 +1,287 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (CheckpointItem is the abstract class to be used for automatic registration and
+ checkpointing of items. This package provides STL container equivalents deriving
+ from this classn.)
+
+ LIBRARY DEPENDENCY:
+ ((CheckpointItem.cpp))
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (May 2013) (--)))
+
+ *******************************************************************************/
+
+#ifndef CHECKPOINTHELPER_CHECKPOINTITEM_HH_
+#define CHECKPOINTHELPER_CHECKPOINTITEM_HH_
+
+#include <queue>
+#include <stack>
+#include "Alloc.hh"
+
+namespace CheckpointHelper
+{
+
+class Manager;
+
+extern Manager * helperManager;
+
+class CheckpointItem
+{
+   friend class InputProcessor;
+   friend void init_attrCheckpointHelper__CheckpointItem();
+public:
+   CheckpointItem();
+   virtual ~CheckpointItem();
+
+   virtual void checkpoint() = 0;
+   virtual void restart() = 0;
+};
+
+#if (defined TRICK_VER) && (TRICK_VER >= 10)
+template<typename U, class V, class T>
+void chkpt_seq(T & obj)
+{
+   int numObjs = obj.size();
+   if (numObjs)
+   {
+      CheckpointHelper::alloc<V>(0x0, obj.chkpt, obj.size(), obj.mmstr, "", false, CHECKPOINTHELPER_FILELINE);
+      int ii = 0;
+      for (U it = obj.begin(), end = obj.end(); it != end; ++it, ++ii)
+      {
+         (*obj.chkpt)[ii] = *it;
+      }
+   }
+}
+
+template<class V, class T>
+void chkpt_unsafevec(T & obj)
+{
+   int numObjs = obj.size();
+   if (numObjs)
+   {
+      CheckpointHelper::alloc<V>(0x0, obj.chkpt, obj.size(), obj.mmstr, "", false, CHECKPOINTHELPER_FILELINE);
+      for (int ii = 0; ii != numObjs; ++ii)
+      {
+         (*obj.chkpt)[ii] = obj[ii];
+      }
+   }
+}
+
+template<typename U, class T>
+void restart_seq(T & obj)
+{
+   obj.clear();
+   if ((*obj.chkpt) != NULL)
+   {
+      int numObjs = trick_MM->get_size((char *) (*obj.chkpt));
+      for (int ii = 0; ii < numObjs; ++ii)
+      {
+         U it = obj.end();
+         obj.insert(it, (*obj.chkpt)[ii]);
+      }
+   }
+}
+
+template<class T>
+void restart_unsafevec(T & obj)
+{
+   obj.clear();
+   if ((*obj.chkpt) != 0x0)
+   {
+      int numObjs = trick_MM->get_size((char *) (*obj.chkpt));
+      for (int ii = 0; ii != numObjs; ++ii)
+      {
+         obj.addElement((*obj.chkpt)[ii]);
+      }
+   }
+}
+
+template<typename U, class V, class T>
+void chkpt_queue(T & obj)
+{
+   U temp_queue(obj);
+   int numObjs = temp_queue.size();
+   if (numObjs)
+   {
+      CheckpointHelper::alloc<V>(0x0, obj.chkpt, temp_queue.size(), obj.mmstr, "", false, CHECKPOINTHELPER_FILELINE);
+      for (int ii = 0, end = temp_queue.size(); ii != end; ++ii)
+      {
+         (*obj.chkpt)[ii] = temp_queue.front();
+         temp_queue.pop();
+      }
+   }
+}
+
+template<class T>
+void restart_queue(T & obj)
+{
+   while (!obj.empty())
+   {
+      obj.pop();
+   }
+   if ((*obj.chkpt) != NULL)
+   {
+      int numObjs = trick_MM->get_size((char *) (*obj.chkpt));
+      for (int ii = 0; ii < numObjs; ++ii)
+      {
+         obj.push((*obj.chkpt)[ii]);
+      }
+   }
+}
+
+template<typename U, class V, class T>
+void chkpt_stack(T & obj)
+{
+   U temp_stack(obj);
+   int numObjs = temp_stack.size();
+   if (numObjs)
+   {
+      CheckpointHelper::alloc<V>(0x0, obj.chkpt, temp_stack.size(), obj.mmstr, "", false, CHECKPOINTHELPER_FILELINE);
+      for (int ii = 0, end = temp_stack.size(); ii != end; ++ii)
+      {
+         (*obj.chkpt)[ii] = temp_stack.top();
+         temp_stack.pop();
+      }
+   }
+}
+
+template<class T>
+void restart_stack(T & obj)
+{
+   while (!obj.empty())
+   {
+      obj.pop();
+   }
+   if ((*obj.chkpt) != NULL)
+   {
+      int numObjs = trick_MM->get_size((char *) (*obj.chkpt));
+      for (int ii = numObjs - 1; ii != -1; --ii)
+      {
+         obj.push((*obj.chkpt)[ii]);
+      }
+   }
+}
+
+template<class T>
+void chkpt_pair(T & obj)
+{
+   (*obj.chkptFirst) = obj.first;
+   (*obj.chkptSecond) = obj.second;
+}
+
+template<class T>
+void restart_pair(T & obj)
+{
+   obj.first = (*obj.chkptFirst);
+   obj.second = (*obj.chkptSecond);
+}
+
+template<typename U, class V, class X, class T>
+void chkpt_map(T &obj)
+{
+   int numObjs = obj.size();
+   if (numObjs)
+   {
+      CheckpointHelper::alloc<V>(0x0, obj.chkptKeys, obj.size(), obj.mmstrKey, "", false, CHECKPOINTHELPER_FILELINE);
+      CheckpointHelper::alloc<X>(0x0, obj.chkptValues, obj.size(), obj.mmstrValue, "", false,
+      CHECKPOINTHELPER_FILELINE);
+      int ii = 0;
+      for (U it = obj.begin(), end = obj.end(); it != end; ++it, ++ii)
+      {
+         (*obj.chkptKeys)[ii] = it->first;
+         (*obj.chkptValues)[ii] = it->second;
+      }
+   }
+}
+
+template<typename U, class T>
+void restart_map(T &obj)
+{
+   obj.clear();
+   if ((*obj.chkptKeys) != NULL)
+   {
+      int numObjs = trick_MM->get_size((char*) (*obj.chkptKeys));
+      for (int ii = 0; ii < numObjs; ++ii)
+      {
+         obj.insert(std::make_pair((*obj.chkptKeys)[ii], (*obj.chkptValues)[ii]));
+      }
+   }
+}
+#else
+template<typename U, class V, class T>
+void chkpt_seq( T & obj )
+{
+   (void) obj;
+}
+
+template<class V, class T>
+void chkpt_unsafevec( T & obj )
+{
+   (void) obj;
+}
+
+template<typename U, class T>
+void restart_seq( T & obj )
+{
+   (void) obj;
+}
+
+template<class T>
+void restart_unsafevec( T & obj )
+{
+   (void) obj;
+}
+
+template<typename U, class V, class T>
+void chkpt_queue( T & obj )
+{
+   (void) obj;
+}
+
+template<class T>
+void restart_queue( T & obj )
+{
+   (void) obj;
+}
+
+template<typename U, class V, class T>
+void chkpt_stack( T & obj )
+{
+   (void) obj;
+}
+
+template<class T>
+void restart_stack( T & obj )
+{
+   (void) obj;
+}
+
+template<class T>
+void chkpt_pair( T & obj )
+{
+   (void) obj;
+}
+
+template<class T>
+void restart_pair( T & obj )
+{
+   (void) obj;
+}
+
+template<typename U, class V, class X, class T>
+void chkpt_map( T & obj )
+{
+   (void) obj;
+}
+
+template<typename U, class T>
+void restart_map( T &obj )
+{
+   (void) obj;
+}
+#endif
+}
+
+#endif /* STLHELPER_STL_HH_ */

trick_source/er7_utils/CheckpointHelper/Deque.hh

@@ -0,0 +1,78 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (List for checkpointable deque container.)
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (May 2013) (--)))
+
+ *******************************************************************************/
+
+#ifndef CHECKPOINTHELPER_DEQUE_HH_
+#define CHECKPOINTHELPER_DEQUE_HH_
+
+#include "CheckpointItem.hh"
+#include <deque>
+
+namespace CheckpointHelper
+{
+
+template<class T>
+class deque: public CheckpointItem, public std::deque<T>
+{
+public:
+   T ** chkpt;
+   std::string mmstr;
+
+   deque(T ** chkptIn, std::string mmstrIn) :
+         chkpt(chkptIn), mmstr(mmstrIn)
+   {
+      *chkpt = 0x0;
+   }
+
+   deque<T>& operator=(const std::deque<T>& pr)
+   {
+      std::deque<T>::operator=(pr);
+      return *this;
+   }
+
+   virtual ~deque()
+   {
+   }
+
+   virtual void checkpoint()
+   {
+      chkpt_seq<typename std::deque<T>::iterator, T>(*this);
+   }
+   virtual void restart()
+   {
+      restart_seq<typename std::deque<T>::iterator>(*this);
+   }
+};
+
+}
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#define STLDEQUE( type, varName ) \
+   type* _##varName; \
+   CheckpointHelper::deque<type> varName
+#else
+#define STLDEQUE( type, varName ) \
+   type* _##varName
+#endif
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#ifndef STL_CSTR1
+#define STL_CSTR1( varName, type  ) \
+   varName( &_##varName, #type )
+#endif
+#else
+#ifndef STL_CSTR1
+#define STL_CSTR1( varName, type  ) \
+   _##varName()
+#endif
+#endif
+
+#define STLDEQUECSTR STL_CSTR1
+
+#endif /* STLHELPER_DEQUE_HH_ */

trick_source/er7_utils/CheckpointHelper/DoublePtrCollect.hh

@@ -0,0 +1,54 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (DoublePtrCollect is a checkpointable dynamic collect to replace Trick's
+ collect mechanism for collected "double *"'s.)
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (Aug 2012) (--)))
+
+ *******************************************************************************/
+
+#ifndef CHECKPOINTHELPER_CHECKPOINTABLECOLLECT_HH_
+#define CHECKPOINTHELPER_CHECKPOINTABLECOLLECT_HH_
+#include "Vector.hh"
+#include <algorithm>
+
+namespace CheckpointHelper
+{
+
+class DoublePtrCollect
+{
+public:
+   DoublePtrCollect() : STLVECTORCSTR( collect, double *)
+   {
+   }
+
+   virtual ~DoublePtrCollect()
+   {
+   }
+
+   void add_collect(double * vIn)
+   {
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+      collect.push_back(vIn);
+#endif
+   }
+
+   void remove_collect(double * vIn)
+   {
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+      std::vector<double *>::iterator it = std::find(collect.begin(), collect.end(), vIn);
+      if (it != collect.end())
+      {
+         collect.erase(it);
+      }
+#endif
+   }
+
+   STLVECTOR( double *, collect);
+};
+
+}
+
+#endif /* STLHELPER_CHECKPOINTABLECOLLECT_HH_ */

trick_source/er7_utils/CheckpointHelper/InputAllocsChkptRestart.hh

@@ -0,0 +1,65 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (Jobs to call python routine to checkpoint/restart input file allocations.)
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (March 2014) (--)))
+
+ *******************************************************************************/
+
+#ifndef CHECKPOINTHELPER_INPUTALLOCSCHKPTRESTART_HH_
+#define CHECKPOINTHELPER_INPUTALLOCSCHKPTRESTART_HH_
+
+#include <sstream>
+
+#include "sim_services/InputProcessor/include/InputProcessor.hh"
+#include "sim_services/CheckPointRestart/include/CheckPointRestart.hh"
+
+extern Trick::InputProcessor * the_ip;
+extern Trick::CheckPointRestart * the_cpr;
+
+namespace CheckpointHelper
+{
+
+class InputAllocsChkptRestart
+{
+public:
+   InputAllocsChkptRestart()
+   {
+   }
+   virtual ~InputAllocsChkptRestart()
+   {
+   }
+
+   void writeChkptFile()
+   {
+      std::string cFile = the_cpr->get_output_file();
+      cFile += "_inputAllocs.py";
+      std::stringstream ss;
+      ss << "if \"externAllocs\" in globals():" << std::endl;
+      ss << "  externAllocs.writeChkptFile(\"" << cFile << "\")" << std::endl;
+      ss << "  trick.message_publish(1, \"Dumped input allocations checkpoint " << cFile << ".\\n\")" << std::endl;
+      ss << "else:" << std::endl;
+      ss << "  trick.message_publish(1, \"externAllocs not defined: skipping input allocations checkpoint\\n\")"
+            << std::endl;
+      the_ip->parse(ss.str().c_str());
+   }
+
+   void loadChkptFile()
+   {
+      std::string cFile = the_cpr->get_load_file();
+      cFile += "_inputAllocs.py";
+      std::stringstream ss;
+      ss << "try:" << std::endl;
+      ss << "  execfile(\"" << cFile << "\")" << std::endl;
+      ss << "  trick.message_publish(1, \"Loaded " << cFile << " input allocations checkpoint.\\n\")" << std::endl;
+      ss << "except IOError:" << std::endl;
+      ss << "  trick.message_publish(1, \"" << cFile << " was not found. Skipping input allocations restoration.\\n\")"
+            << std::endl;
+      the_ip->parse(ss.str().c_str());
+   }
+};
+
+} /* namespace STLHelper */
+#endif /* STLHELPER_INPUTALLOCSCHKPTRESTART_HH_ */

trick_source/er7_utils/CheckpointHelper/List.hh

@@ -0,0 +1,78 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (List for checkpointable list container.)
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (May 2013) (--)))
+
+ *******************************************************************************/
+
+#ifndef CHECKPOINTHELPER_LIST_HH_
+#define CHECKPOINTHELPER_LIST_HH_
+
+#include "CheckpointItem.hh"
+#include <list>
+
+namespace CheckpointHelper
+{
+
+template<class T>
+class list: public CheckpointItem, public std::list<T>
+{
+public:
+   T ** chkpt;
+   std::string mmstr;
+
+   list(T ** chkptIn, std::string mmstrIn) :
+         chkpt(chkptIn), mmstr(mmstrIn)
+   {
+      *chkpt = 0x0;
+   }
+
+   list<T>& operator=(const std::list<T>& pr)
+   {
+      std::list<T>::operator=(pr);
+      return *this;
+   }
+
+   virtual ~list()
+   {
+   }
+
+   virtual void checkpoint()
+   {
+      chkpt_seq<typename std::list<T>::iterator, T>(*this);
+   }
+   virtual void restart()
+   {
+      restart_seq<typename std::list<T>::iterator>(*this);
+   }
+};
+
+}
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#define STLLIST( type, varName ) \
+   type* _##varName; \
+   CheckpointHelper::list<type> varName
+#else
+#define STLLIST( type, varName ) \
+   type* _##varName
+#endif
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#ifndef STL_CSTR1
+#define STL_CSTR1( varName, type  ) \
+   varName( &_##varName, #type )
+#endif
+#else
+#ifndef STL_CSTR1
+#define STL_CSTR1( varName, type  ) \
+   _##varName()
+#endif
+#endif
+
+#define STLLISTCSTR STL_CSTR1
+
+#endif /* STLHELPER_LIST_HH_ */

trick_source/er7_utils/CheckpointHelper/Manager.cpp

@@ -0,0 +1,236 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (Manager for globally calling checkpoint and restart methods of STLs)
+
+ LIBRARY DEPENDENCY:
+ ((Manager.cpp)
+ (CheckpointItem.cpp))
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (May 2013) (--)))
+
+ *******************************************************************************/
+
+#include "Alloc.hh"
+#include "Manager.hh"
+#if (defined TRICK_VER) && (TRICK_VER >= 10)
+#include "sim_services/Message/include/message_proto.h"
+#endif
+#include "CheckpointItem.hh"
+#include <algorithm>
+#include <iostream>
+
+namespace CheckpointHelper
+{
+
+Manager * helperManager = 0x0;
+
+Manager::Manager()
+{
+   _checkpointObjects = NULL;
+   if (helperManager == 0x0)
+   {
+      helperManager = this;
+   }
+   else
+   {
+      std::cerr << "WARNING: Only one instance of CheckpointHelper::Manager is allowed. Invalid Manager instance at  "
+            << this << std::endl;
+   }
+   pthread_mutex_init(&allocQueueMutex, 0x0);
+   pthread_mutex_init(&objListMutex, 0x0);
+   pthread_mutex_init(&externMapMutex, 0x0);
+}
+
+Manager::~Manager()
+{
+   if (helperManager != 0x0)
+   {
+      helperManager = 0x0;
+   }
+}
+#if (defined TRICK_VER) && (TRICK_VER >= 10)
+
+void Manager::checkpoint()
+{
+   processConstructorAllocs();
+   pthread_mutex_lock(&objListMutex);
+   int numObjs = checkpointObjects.size();
+   if (numObjs)
+   {
+      alloc<CheckpointItem *>(this, &_checkpointObjects, numObjs, "CheckpointHelper::CheckpointItem", "", false,
+      CHECKPOINTHELPER_FILELINE);
+      int ii = 0;
+      for (std::list<CheckpointItem *>::iterator it = checkpointObjects.begin(), end = checkpointObjects.end();
+            it != end; ++it, ++ii)
+      {
+         CheckpointItem * item = *it;
+         item->checkpoint();
+         _checkpointObjects[ii] = item;
+      }
+   }
+   pthread_mutex_unlock(&objListMutex);
+}
+void Manager::restart()
+{
+   pthread_mutex_lock(&allocQueueMutex);
+   constructorAllocs.clear();
+   pthread_mutex_unlock(&allocQueueMutex);
+   pthread_mutex_lock(&objListMutex);
+   checkpointObjects.clear();
+   if (_checkpointObjects != NULL)
+   {
+      int numObjs = trick_MM->get_size((char *) _checkpointObjects);
+      for (int ii = 0; ii < numObjs; ++ii)
+      {
+         CheckpointItem * item = _checkpointObjects[ii];
+         checkpointObjects.push_back(_checkpointObjects[ii]);
+         item->restart();
+      }
+   }
+   pthread_mutex_unlock(&objListMutex);
+}
+
+#else
+void Manager::checkpoint()
+{
+}
+void Manager::restart()
+{
+}
+
+#endif
+
+void Manager::addItem(CheckpointItem & stlIn)
+{
+   pthread_mutex_lock(&objListMutex);
+   std::list<CheckpointItem *>::iterator it = find(checkpointObjects.begin(), checkpointObjects.end(), &stlIn);
+   if (it == checkpointObjects.end())
+   {
+      checkpointObjects.push_back(&stlIn);
+   }
+   pthread_mutex_unlock(&objListMutex);
+}
+
+void Manager::removeItem(CheckpointItem & stlIn)
+{
+   pthread_mutex_lock(&objListMutex);
+   std::list<CheckpointItem *>::iterator it = find(checkpointObjects.begin(), checkpointObjects.end(), &stlIn);
+   if (it != checkpointObjects.end())
+   {
+      checkpointObjects.erase(it);
+   }
+   pthread_mutex_unlock(&objListMutex);
+}
+
+Manager::AllocEntry::AllocEntry(void * parentPtrIn, void * ptrIn, const std::string nameIn,
+      const std::string funcNameIn) :
+      parentPtr(parentPtrIn), ptr(ptrIn), name(nameIn), funcName(funcNameIn)
+{
+}
+
+void Manager::addConstructorAlloc(void* parentPtr, void* ptr, const std::string name, const std::string funcNameIn)
+{
+   pthread_mutex_lock(&allocQueueMutex);
+   constructorAllocs.push_back(AllocEntry(parentPtr, ptr, name, funcNameIn));
+   pthread_mutex_unlock(&allocQueueMutex);
+}
+
+void Manager::removeConstructorAlloc(void * ptrIn)
+{
+   pthread_mutex_lock(&allocQueueMutex);
+   std::deque<AllocEntry>::iterator it = std::find(constructorAllocs.begin(), constructorAllocs.end(), ptrIn);
+   if (it != constructorAllocs.end())
+   {
+      constructorAllocs.erase(it);
+   }
+   pthread_mutex_unlock(&allocQueueMutex);
+}
+
+void Manager::processConstructorAllocs()
+{
+   pthread_mutex_lock(&allocQueueMutex);
+   std::deque<AllocEntry> preQueue;
+   std::deque<AllocEntry> postQueue;
+   while (!constructorAllocs.empty())
+   {
+#if (defined TRICK_VER) && (TRICK_VER >= 10)
+      AllocEntry & entry = constructorAllocs.front();
+
+      if (!updateAllocName(entry.parentPtr, entry.ptr, entry.name, entry.funcName, false))
+      {
+         postQueue.push_back(AllocEntry(entry.parentPtr, entry.ptr, entry.name, entry.funcName));
+      }
+#endif
+      constructorAllocs.pop_front();
+   }
+   pthread_mutex_unlock(&allocQueueMutex);
+
+   size_t preSize = preQueue.size();
+   size_t postSize = postQueue.size();
+
+   while (preSize != postSize)
+   {
+      preQueue = postQueue;
+      preSize = preQueue.size();
+      postQueue.clear();
+      while (!preQueue.empty())
+      {
+#if (defined TRICK_VER) && (TRICK_VER >= 10)
+         AllocEntry & entry = preQueue.front();
+
+         if (!updateAllocName(entry.parentPtr, entry.ptr, entry.name, entry.funcName, false))
+         {
+            postQueue.push_back(AllocEntry(entry.parentPtr, entry.ptr, entry.name, entry.funcName));
+         }
+#endif
+         preQueue.pop_front();
+      }
+      postSize = postQueue.size();
+   }
+
+   while (!postQueue.empty())
+   {
+#if (defined TRICK_VER) && (TRICK_VER >= 10)
+      AllocEntry & entry = postQueue.front();
+
+      updateAllocName(entry.parentPtr, entry.ptr, entry.name, entry.funcName, true);
+#endif
+      postQueue.pop_front();
+   }
+}
+
+void Manager::addExternAlloc(void* ptr, size_t numElems)
+{
+   pthread_mutex_lock(&externMapMutex);
+   externMemAllocs[ptr] = numElems;
+   pthread_mutex_unlock(&externMapMutex);
+}
+
+void Manager::removeExternAlloc(void* ptr)
+{
+   pthread_mutex_lock(&externMapMutex);
+   externMemAllocs.erase(ptr);
+   pthread_mutex_unlock(&externMapMutex);
+}
+
+size_t Manager::getExternNum(void* ptr)
+{
+   pthread_mutex_lock(&externMapMutex);
+   size_t num;
+   std::map<void *, size_t>::iterator it = externMemAllocs.find(ptr);
+   if (it != externMemAllocs.end())
+   {
+      num = it->second;
+   }
+   else
+   {
+      num = 0;
+   }
+   pthread_mutex_unlock(&externMapMutex);
+   return num;
+}
+
+}
+

trick_source/er7_utils/CheckpointHelper/Manager.hh

@@ -0,0 +1,78 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (Manager for globally calling checkpoint and restart methods of STLs)
+
+ LIBRARY DEPENDENCY:
+ ((Manager.cpp))
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (May 2013) (--)))
+
+ *******************************************************************************/
+
+#ifndef CHECKPOINTHELPER_MANAGER_HH_
+#define CHECKPOINTHELPER_MANAGER_HH_
+
+#include <deque>
+#include <string>
+#include <map>
+#include <list>
+#include <pthread.h>
+
+namespace CheckpointHelper
+{
+
+class CheckpointItem;
+
+class Manager
+{
+   friend class InputProcessor;
+   friend void init_attrCheckpointHelper__Manager();
+public:
+   Manager();
+   virtual ~Manager();
+
+   class AllocEntry
+   {
+   public:
+      AllocEntry(void * parentPtrIn, void * ptrIn, const std::string nameIn, const std::string funcNameIn);
+      void * parentPtr;
+      void * ptr;
+      std::string name;
+      std::string funcName;
+
+      bool operator==(void * ptrComp)
+      {
+         return this->ptr == ptrComp;
+      }
+   };
+
+   void addConstructorAlloc(void * parentPtr, void * ptr, const std::string name, const std::string funcNameIn);
+   void removeConstructorAlloc(void * ptr);
+   void processConstructorAllocs();
+
+   void checkpoint();
+   void restart();
+
+   void addItem(CheckpointItem & stlIn);
+   void removeItem(CheckpointItem & stlIn);
+
+   void addExternAlloc(void * ptr, size_t numElems);
+   void removeExternAlloc(void * ptr);
+   size_t getExternNum(void * ptr);
+
+protected:
+   std::list<CheckpointItem *> checkpointObjects;
+   CheckpointItem ** _checkpointObjects;
+   std::deque<AllocEntry> constructorAllocs;
+   pthread_mutex_t allocQueueMutex; /* ** Do not checkpoint */
+   pthread_mutex_t objListMutex; /* ** Do not checkpoint */
+   pthread_mutex_t externMapMutex; /* ** Do not checkpoint */
+   std::map<void *, size_t> externMemAllocs;
+
+};
+
+}
+
+#endif /* STLHELPER_MANAGER_HH_ */

trick_source/er7_utils/CheckpointHelper/Map.hh

@@ -0,0 +1,121 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (Map for checkpointable map container.)
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (May 2013) (--)))
+
+ *******************************************************************************/
+
+#ifndef CHECKPOINTHELPER_MAP_HH_
+#define CHECKPOINTHELPER_MAP_HH_
+
+#include "CheckpointItem.hh"
+#include <map>
+
+namespace CheckpointHelper
+{
+
+template<class T, class U>
+class map: public CheckpointItem, public std::map<T, U>
+{
+public:
+   T ** chkptKeys;
+   U ** chkptValues;
+   std::string mmstrKey;
+   std::string mmstrValue;
+
+   map(T ** chkptKeysIn, U **chkptValuesIn, std::string mmstrKeyIn, std::string mmstrValueIn) :
+         chkptKeys(chkptKeysIn), chkptValues(chkptValuesIn), mmstrKey(mmstrKeyIn), mmstrValue(mmstrValueIn)
+   {
+      *chkptKeys = 0x0;
+      *chkptValues = 0x0;
+   }
+
+   map<T, U>& operator=(const std::map<T, U>& pr)
+   {
+      std::map<T, U>::operator=(pr);
+      return *this;
+   }
+
+   virtual ~map()
+   {
+   }
+
+   virtual void checkpoint()
+   {
+      chkpt_map<typename std::map<T, U>::iterator, T, U>(*this);
+   }
+   virtual void restart()
+   {
+      restart_map<typename std::map<T, U>::iterator>(*this);
+   }
+};
+
+template<class T, class U>
+class multimap: public CheckpointHelper::CheckpointItem, public std::multimap<T, U>
+{
+public:
+   T ** chkptKeys;
+   U ** chkptValues;
+   std::string mmstrKey;
+   std::string mmstrValue;
+
+   multimap(T ** chkptKeysIn, U **chkptValuesIn, std::string mmstrKeyIn, std::string mmstrValueIn) :
+         chkptKeys(chkptKeysIn), chkptValues(chkptValuesIn), mmstrKey(mmstrKeyIn), mmstrValue(mmstrValueIn)
+   {
+      *chkptKeys = 0x0;
+      *chkptValues = 0x0;
+   }
+
+   virtual ~multimap()
+   {
+   }
+
+   virtual void checkpoint()
+   {
+      chkpt_map<typename std::multimap<T, U>::iterator, T, U>(*this);
+   }
+   virtual void restart()
+   {
+      restart_map<typename std::multimap<T, U>::iterator>(*this);
+   }
+};
+
+}
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#define STLMAP( type1, type2, varName ) \
+   type1* _##varName##T1; \
+   type2* _##varName##T2; \
+   CheckpointHelper::map<type1, type2> varName
+#define STLMULTIMAP( type1, type2, varName ) \
+   type1* _##varName##T1; \
+   type2* _##varName##T2; \
+   CheckpointHelper::multimap<type1, type2> varName
+#else
+#define STLMAP( type1, type2,  varName ) \
+   type1* _##varName##T1; \
+   type2* _##varName##T2;
+#define STLMULTIMAP( type1, type2, varName ) \
+   type1* _##varName##T1; \
+   type2* _##varName##T2;
+#endif
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#ifndef STL_CSTR2
+#define STL_CSTR2( varName, type1, type2 ) \
+   varName( &_##varName##T1, &_##varName##T2, #type1, #type2 )
+#endif
+#else
+#ifndef STL_CSTR2
+#define STL_CSTR2( varName, type1, type2 ) \
+   _##varName##T1(), _##varName##T2()
+#endif
+#endif
+
+#define STLMAPCSTR STL_CSTR2
+#define STLMULTIMAPCSTR STL_CSTR2
+
+#endif /* STLHELPER_MAP_HH_ */

trick_source/er7_utils/CheckpointHelper/Pair.hh

@@ -0,0 +1,83 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (Pair for checkpointable pair container.)
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (May 2013) (--)))
+
+ *******************************************************************************/
+
+#ifndef CHECKPOINTHELPER_PAIR_HH_
+#define CHECKPOINTHELPER_PAIR_HH_
+
+#include "CheckpointItem.hh"
+#include <utility>
+
+namespace CheckpointHelper
+{
+
+template<class T, class U>
+class pair: public CheckpointItem, public std::pair<T, U>
+{
+public:
+   T * chkptFirst;
+   U * chkptSecond;
+   std::string mmstrKey;
+   std::string mmstrValue;
+
+   pair(T * chkptKeysIn, U * chkptValuesIn, std::string mmstrKeyIn, std::string mmstrValueIn) :
+         chkptFirst(chkptKeysIn), chkptSecond(chkptValuesIn), mmstrKey(mmstrKeyIn), mmstrValue(mmstrValueIn)
+   {
+      *chkptFirst = 0x0;
+      *chkptSecond = 0x0;
+   }
+
+   pair<T, U>& operator=(const std::pair<T, U>& pr)
+   {
+      std::pair<T, U>::operator=(pr);
+      return *this;
+   }
+
+   virtual ~pair()
+   {
+   }
+
+   virtual void checkpoint()
+   {
+      chkpt_pair(*this);
+   }
+   virtual void restart()
+   {
+      restart_pair(*this);
+   }
+};
+
+}
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#define STLPAIR( type1, type2, varName ) \
+   type1 _##varName##T1; \
+   type2 _##varName##T2; \
+   CheckpointHelper::pair<type1, type2> varName
+#else
+#define STLPAIR( type1, type2, varName ) \
+   type1 _##varName##T1; \
+   type2 _##varName##T2;
+#endif
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#ifndef STL_CSTR2
+#define STL_CSTR2( varName, type1, type2 ) \
+   varName( &_##varName##T1, &_##varName##T2, #type1, #type2 )
+#endif
+#else
+#ifndef STL_CSTR2
+#define STL_CSTR2( varName, type1, type2 ) \
+   _##varName##T1(), _##varName##T2()
+#endif
+#endif
+
+#define STLPAIRCSTR STL_CSTR2
+
+#endif /* STLHELPER_PAIR_HH_ */

trick_source/er7_utils/CheckpointHelper/Queue.hh

@@ -0,0 +1,78 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (Queue for checkpointable queue container.)
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (May 2013) (--)))
+
+ *******************************************************************************/
+
+#ifndef CHECKPOINTHELPER_QUEUE_HH_
+#define CHECKPOINTHELPER_QUEUE_HH_
+
+#include "CheckpointItem.hh"
+#include <queue>
+
+namespace CheckpointHelper
+{
+
+template<class T>
+class queue: public CheckpointItem, public std::queue<T>
+{
+public:
+   T ** chkpt;
+   std::string mmstr;
+
+   queue(T ** chkptIn, std::string mmstrIn) :
+         chkpt(chkptIn), mmstr(mmstrIn)
+   {
+      *chkpt = 0x0;
+   }
+
+   queue<T>& operator=(const std::queue<T>& pr)
+   {
+      std::queue<T>::operator=(pr);
+      return *this;
+   }
+
+   virtual ~queue()
+   {
+   }
+
+   virtual void checkpoint()
+   {
+      chkpt_queue<typename std::queue<T>, T>(*this);
+   }
+   virtual void restart()
+   {
+      restart_queue(*this);
+   }
+};
+
+}
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#define STLQUEUE( type, varName ) \
+   type* _##varName; \
+   CheckpointHelper::queue<type> varName
+#else
+#define STLQUEUE( type, varName ) \
+   type* _##varName
+#endif
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#ifndef STL_CSTR1
+#define STL_CSTR1( varName, type  ) \
+   varName( &_##varName, #type )
+#endif
+#else
+#ifndef STL_CSTR1
+#define STL_CSTR1( varName, type  ) \
+   _##varName()
+#endif
+#endif
+
+#define STLQUEUECSTR STL_CSTR1
+
+#endif /* STLHELPER_QUEUE_HH_ */

trick_source/er7_utils/CheckpointHelper/Set.hh

@@ -0,0 +1,117 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (Set for checkpointable set container.)
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (May 2013) (--)))
+
+ *******************************************************************************/
+
+#ifndef CHECKPOINTHELPER_SET_HH_
+#define CHECKPOINTHELPER_SET_HH_
+
+#include "CheckpointItem.hh"
+#include <set>
+
+namespace CheckpointHelper
+{
+
+template<class T>
+class set: public CheckpointItem, public std::set<T>
+{
+public:
+   T ** chkpt;
+   std::string mmstr;
+
+   set(T ** chkptIn, std::string mmstrIn) :
+         chkpt(chkptIn), mmstr(mmstrIn)
+   {
+      *chkpt = 0x0;
+   }
+
+   set<T>& operator=(const std::set<T>& pr)
+   {
+      std::set<T>::operator=(pr);
+      return *this;
+   }
+
+   virtual ~set()
+   {
+   }
+
+   virtual void checkpoint()
+   {
+      chkpt_seq<typename std::set<T>::iterator, T>(*this);
+   }
+   virtual void restart()
+   {
+      restart_seq<typename std::set<T>::iterator>(*this);
+   }
+};
+
+template<class T>
+class multiset: public CheckpointHelper::CheckpointItem, public std::multiset<T>
+{
+public:
+   T ** chkpt;
+   std::string mmstr;
+
+   multiset(T ** chkptIn, std::string mmstrIn) :
+         chkpt(chkptIn), mmstr(mmstrIn)
+   {
+      *chkpt = 0x0;
+   }
+
+   multiset<T>& operator=(const std::multiset<T>& pr)
+   {
+      std::multiset<T>::operator=(pr);
+      return *this;
+   }
+
+   virtual ~multiset()
+   {
+   }
+
+   virtual void checkpoint()
+   {
+      chkpt_seq<typename std::multiset<T>::iterator, T>(*this);
+   }
+   virtual void restart()
+   {
+      restart_seq<typename std::multiset<T>::iterator>(*this);
+   }
+};
+
+}
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#define STLSET( type, varName ) \
+   type* _##varName; \
+   CheckpointHelper::set<type> varName
+#define STLMULTISET( type, varName ) \
+   type* _##varName; \
+   CheckpointHelper::multiset<type> varName
+#else
+#define STLSET( type, varName ) \
+   type* _##varName
+#define STLMULTISET( type, varName ) \
+   type* _##varName;
+#endif
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#ifndef STL_CSTR1
+#define STL_CSTR1( varName, type  ) \
+   varName( &_##varName, #type )
+#endif
+#else
+#ifndef STL_CSTR1
+#define STL_CSTR1( varName, type  ) \
+   _##varName()
+#endif
+#endif
+
+#define STLSETCSTR STL_CSTR1
+#define STLMULTISETCSTR STL_CSTR1
+
+#endif /* STLHELPER_SET_HH_ */

trick_source/er7_utils/CheckpointHelper/Stack.hh

@@ -0,0 +1,78 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (Stack for checkpointable stack container.)
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (May 2013) (--)))
+
+ *******************************************************************************/
+
+#ifndef CHECKPOINTHELPER_STACK_HH_
+#define CHECKPOINTHELPER_STACK_HH_
+
+#include "CheckpointItem.hh"
+#include <stack>
+
+namespace CheckpointHelper
+{
+
+template<class T>
+class stack: public CheckpointItem, public std::stack<T>
+{
+public:
+   T ** chkpt;
+   std::string mmstr;
+
+   stack(T ** chkptIn, std::string mmstrIn) :
+         chkpt(chkptIn), mmstr(mmstrIn)
+   {
+      *chkpt = 0x0;
+   }
+
+   stack<T>& operator=(const std::stack<T>& pr)
+   {
+      std::stack<T>::operator=(pr);
+      return *this;
+   }
+
+   virtual ~stack()
+   {
+   }
+
+   virtual void checkpoint()
+   {
+      chkpt_stack<typename std::stack<T>, T>(*this);
+   }
+   virtual void restart()
+   {
+      restart_stack(*this);
+   }
+};
+
+}
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#define STLSTACK( type, varName ) \
+   type* _##varName; \
+   CheckpointHelper::stack<type> varName
+#else
+#define STLSTACK( type, varName ) \
+   type* _##varName
+#endif
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#ifndef STL_CSTR1
+#define STL_CSTR1( varName, type  ) \
+   varName( &_##varName, #type )
+#endif
+#else
+#ifndef STL_CSTR1
+#define STL_CSTR1( varName, type  ) \
+   _##varName()
+#endif
+#endif
+
+#define STLSTACKCSTR STL_CSTR1
+
+#endif /* STLHELPER_STACK_HH_ */

trick_source/er7_utils/CheckpointHelper/UnsafeChkptVector.hh

@@ -0,0 +1,83 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (Definition of Unsafevector container class. Minimal protections and
+ forced-inlining for this container to acheive maximum performance..)
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (Apr 2014) (--)))
+
+ ICG:(No.)
+
+ *******************************************************************************/
+
+#ifndef CHECKPOINTHELPER_UNSAFECHKPTVECTOR_HH_
+#define CHECKPOINTHELPER_UNSAFECHKPTVECTOR_HH_
+#include "Unsafevector.hh"
+#include "CheckpointItem.hh"
+
+#define stlhelper_inline inline __attribute__((always_inline))
+#define stlhelper_force_optimize __attribute__((optimize("-O3")))
+
+//#define stlhelper_inline inline
+//#define stlhelper_force_optimize
+
+namespace CheckpointHelper
+{
+
+template<class T>
+class UnsafeChkptVector: public Unsafevector<T>, public CheckpointItem
+{
+public:
+   T ** chkpt;
+   std::string mmstr;
+
+   stlhelper_inline UnsafeChkptVector(T ** chkptIn, std::string mmstrIn) :
+         chkpt(chkptIn), mmstr(mmstrIn)
+   {
+      *chkpt = 0x0;
+   }
+
+   stlhelper_inline UnsafeChkptVector(const UnsafeChkptVector<T> & other) :
+         Unsafevector<T>(other), chkpt(other.chkpt), mmstr(other.mmstrIn)
+   {
+      *chkpt = 0x0;
+   }
+
+   virtual void checkpoint()
+   {
+      chkpt_unsafevec<T>(*this);
+   }
+
+   virtual void restart()
+   {
+      restart_unsafevec(*this);
+   }
+};
+
+}
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#define STLUNSAFECHKPTVECTOR( type, varName ) \
+   type* _##varName; \
+   CheckpointHelper::UnsafeChkptVector<type> varName
+#else
+#define STLUNSAFECHKPTVECTOR( type, varName ) \
+   type* _##varName
+#endif
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#ifndef STL_CSTR1
+#define STL_CSTR1( varName, type  ) \
+   varName( &_##varName, #type )
+#endif
+#else
+#ifndef STL_CSTR1
+#define STL_CSTR1( varName, type  ) \
+   _##varName()
+#endif
+#endif
+
+#define STLUNSAFECHKPTVECTORCSTR STL_CSTR1
+
+#endif /* UNSAFEVECTOR_HH_ */

trick_source/er7_utils/CheckpointHelper/Unsafemap.hh

@@ -0,0 +1,173 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (Definition of Unsafemap container class. Minimal protections and
+ forced-inlining for this container to acheive maximum performance..)
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (Apr 2014) (--)))
+
+ ICG:(No.)
+
+ *******************************************************************************/
+
+#ifndef CHECKPOINTHELPER_UNSAFEMAP_HH_
+#define CHECKPOINTHELPER_UNSAFEMAP_HH_
+#include <stdlib.h>
+#include "Unsafevector.hh"
+
+namespace CheckpointHelper
+{
+
+template<class U, class T>
+class Unsafemap
+{
+public:
+   stlhelper_inline Unsafemap()
+   {
+   }
+
+   stlhelper_inline ~Unsafemap()
+   {
+      int vsize = m.size();
+      for (int ii = 0; ii < vsize; ++ii)
+      {
+         internal * it = m[ii];
+         delete it;
+      }
+   }
+
+   class internal
+   {
+   public:
+      stlhelper_inline internal() :
+            first(), second()
+      {
+      }
+      stlhelper_inline internal(const U & firstIn) :
+            first(firstIn), second()
+      {
+      }
+
+      U first;
+      T second;
+   };
+
+   Unsafevector<internal *> m;
+
+   stlhelper_inline static int comparekeys(const void * a, const void * b)
+   {
+      U & key = *((U*) a);
+      internal * bPtr = *((internal **) b);
+      if (key < bPtr->first)
+         return -1;
+      if (key == bPtr->first)
+         return 0;
+      return 1;
+   }
+
+   stlhelper_inline static int compareelems(const void * a, const void * b)
+   {
+      internal * aPtr = *((internal **) a);
+      internal * bPtr = *((internal **) b);
+      if (aPtr->first < bPtr->first)
+         return -1;
+      if (aPtr->first == bPtr->first)
+         return 0;
+      return 1;
+   }
+
+   stlhelper_inline int size()
+   {
+      return m.size();
+   }
+
+   stlhelper_inline void clear()
+   {
+      for (int ii = 0, end = m.size(); ii != end; ++ii)
+      {
+         internal * Elem = m[ii];
+         delete Elem;
+      }
+      m.clear();
+   }
+
+   stlhelper_inline T & getElement(int idx)
+   {
+      return m.v[idx]->second;
+   }
+
+#define compareKeys (int (*)( const void *, const void * )) Unsafemap<U, T>::comparekeys
+#define compareElems (int (*)( const void *, const void * )) Unsafemap<U, T>::compareelems
+
+   stlhelper_inline internal * find(const U & keyIn)
+   {
+      int res = searchForIndex(&keyIn);
+      if (res == -1)
+      {
+         return 0x0;
+      }
+      else
+      {
+         return m[res];
+      }
+   }
+
+   stlhelper_inline T & operator[](const U & keyIn)
+   {
+      int res = searchForIndex(&keyIn);
+      if (res == -1)
+      {
+         internal * Elem = new internal(keyIn);
+         m.addElement(Elem);
+         qsort(m.v, m.size(), sizeof(internal *), compareElems);
+         return Elem->second;
+      }
+      else
+      {
+         return m[res]->second;
+      }
+   }
+
+   stlhelper_inline void remove(const U & keyIn)
+   {
+      int res = searchForIndex(&keyIn);
+      if (res != -1)
+      {
+         m.removeElement(res);
+      }
+   }
+
+protected:
+   stlhelper_inline int searchForIndex(const U * keyIn)
+   {
+      size_t lowerIdx, upperIdx, currIdx;
+      const void *currAddr;
+      int comparResult;
+
+      lowerIdx = 0;
+      upperIdx = m.size();
+      while (lowerIdx < upperIdx)
+      {
+         currIdx = (lowerIdx + upperIdx) / 2;
+         currAddr = (void *) (((const char *) m.v) + (currIdx * sizeof(internal *)));
+         comparResult = (*compareKeys )(keyIn, currAddr);
+         if (comparResult < 0)
+            upperIdx = currIdx;
+         else if (comparResult > 0)
+            lowerIdx = currIdx + 1;
+         else
+         {
+            return currIdx;
+         }
+      }
+      return -1;
+   }
+
+#undef compareKeys
+#undef compareElems
+};
+
+}
+
+#endif /* UNSAFEMAP_HH_ */

trick_source/er7_utils/CheckpointHelper/Unsafevector.hh

@@ -0,0 +1,516 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (Definition of Unsafevector container class. Minimal protections and
+ forced-inlining for this container to acheive maximum performance..)
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (Apr 2014) (--)))
+
+ ICG:(No.)
+
+ *******************************************************************************/
+
+#ifndef CHECKPOINTHELPER_UNSAFEVECTOR_HH_
+#define CHECKPOINTHELPER_UNSAFEVECTOR_HH_
+
+#include <cstring>
+
+#ifndef DEBUG
+#define stlhelper_inline inline __attribute__((always_inline))
+#define stlhelper_force_optimize __attribute__((optimize("-O3")))
+#else
+#define stlhelper_inline
+#define stlhelper_force_optimize
+#endif
+
+//#define stlhelper_inline inline
+//#define stlhelper_force_optimize
+
+#include <vector>
+#include <cstdlib>
+
+namespace CheckpointHelper
+{
+
+static const int minSize = 16;
+
+template<class T>
+class Unsafevector
+{
+
+public:
+
+   stlhelper_inline Unsafevector() :
+         v(new T[minSize]), numElems(0), capacity(minSize)
+   {
+   }
+
+   stlhelper_inline Unsafevector(int newNumElems) :
+         numElems(0), capacity(minSize)
+   {
+      while (capacity <= newNumElems)
+      {
+         capacity *= 2;
+      }
+      v = new T[capacity];
+   }
+
+   stlhelper_inline Unsafevector(const Unsafevector<T> & other) :
+         numElems(other.numElems), capacity(other.numElems)
+   {
+      v = new T[other.numElems];
+      for (int ii = 0; ii < other.numElems; ++ii)
+      {
+         v[ii] = other.v[ii];
+      }
+   }
+
+   stlhelper_inline Unsafevector<T> & operator=(const Unsafevector<T> & other)
+   {
+      reserve_nocopy(other.numElems);
+      for (int ii = 0; ii < other.numElems; ++ii)
+      {
+         v[ii] = other.v[ii];
+      }
+      capacity = other.numElems;
+      numElems = other.numElems;
+      return *this;
+   }
+
+   stlhelper_inline Unsafevector<T> & operator=(const std::vector<T> & other)
+   {
+      int vSize = other.size();
+      reserve_nocopy(vSize);
+      for (int ii = 0; ii < vSize; ++ii)
+      {
+         v[ii] = other[ii];
+      }
+      numElems = vSize;
+      return *this;
+   }
+
+   stlhelper_inline ~Unsafevector()
+   {
+      delete[] v;
+   }
+
+   stlhelper_inline void reserve(int newSize)
+   {
+      if (newSize > capacity)
+      {
+         while (capacity <= newSize)
+         {
+            capacity *= 2;
+         }
+         T * newPtr = new T[capacity];
+         for (int ii = 0; ii < numElems; ++ii)
+         {
+            newPtr[ii] = v[ii];
+         }
+         delete[] v;
+         v = newPtr;
+      }
+   }
+
+   stlhelper_inline void addVectorNoCheck(std::vector<T> & vecIn)
+   {
+      int vecInSize = vecIn.size();
+      for (int ii = numElems, jj = 0; jj < vecInSize; ++ii, ++jj)
+      {
+         v[ii] = vecIn[jj];
+      }
+      numElems += vecInSize;
+   }
+
+   stlhelper_inline void addVector(std::vector<T> & vecIn)
+   {
+      int vecInSize = vecIn.size();
+      reserve(numElems + vecInSize);
+      for (int ii = numElems, jj = 0; jj < vecInSize; ++ii, ++jj)
+      {
+         v[ii] = vecIn[jj];
+      }
+      numElems += vecInSize;
+   }
+
+   stlhelper_inline void addVectorNoCheck(Unsafevector<T> & vecIn)
+   {
+      int vecInSize = vecIn.size();
+      for (int ii = numElems, jj = 0; jj < vecInSize; ++ii, ++jj)
+      {
+         v[ii] = vecIn[jj];
+      }
+      numElems += vecInSize;
+   }
+
+   stlhelper_inline void addVector(Unsafevector<T> & vecIn)
+   {
+      int vecInSize = vecIn.size();
+      reserve(numElems + vecInSize);
+      for (int ii = numElems, jj = 0; jj < vecInSize; ++ii, ++jj)
+      {
+         v[ii] = vecIn[jj];
+      }
+      numElems += vecInSize;
+   }
+
+   stlhelper_inline void addElementNoCheck(const T & elemIn)
+   {
+      v[numElems++] = elemIn;
+   }
+
+   stlhelper_inline void addElement(const T & elemIn)
+   {
+      if (numElems >= capacity)
+      {
+         reserve(capacity * 2);
+      }
+      v[numElems++] = elemIn;
+   }
+
+   stlhelper_inline void removeElement(int idx)
+   {
+      if ((numElems - 1) != idx)
+      {
+         for (int ii = idx, end = numElems; ii != end; ++ii)
+         {
+            v[ii] = v[ii + 1];
+         }
+         --numElems;
+      }
+      else
+      {
+         --numElems;
+      }
+   }
+
+   stlhelper_inline int find(const T & testElem)
+   {
+      for (int ii = 0, end = numElems; ii != end; ++ii)
+      {
+         if (v[ii] == testElem)
+         {
+            return ii;
+         }
+      }
+      return -1;
+   }
+
+   stlhelper_inline void clear()
+   {
+      numElems = 0;
+   }
+
+   stlhelper_inline int size()
+   {
+      return numElems;
+   }
+
+   stlhelper_inline bool isEmpty()
+   {
+      return (numElems == 0);
+   }
+
+   stlhelper_inline T & operator[](int idx)
+   {
+      return v[idx];
+   }
+   stlhelper_inline T operator[](int idx) const
+   {
+      return v[idx];
+   }
+
+   stlhelper_inline void swap(Unsafevector<T> & other)
+   {
+      std::swap(v, other.v);
+      std::swap(numElems, other.numElems);
+      std::swap(capacity, other.capacity);
+   }
+
+   T * v;
+   int numElems;
+   int capacity;
+
+protected:
+
+   stlhelper_inline void reserve_nocopy(int newSize)
+   {
+      if (newSize > capacity)
+      {
+         delete[] v;
+         v = new T[newSize];
+         capacity = newSize;
+      }
+   }
+};
+
+template<class T>
+class Unsafevector<T *>
+{
+public:
+
+   stlhelper_inline Unsafevector() :
+         v(new T *[minSize]), numElems(0), capacity(minSize)
+   {
+   }
+
+   stlhelper_inline Unsafevector(int newNumElems) :
+         numElems(0), capacity(minSize)
+   {
+      while (capacity <= newNumElems)
+      {
+         capacity *= 2;
+      }
+      v = new T *[capacity];
+   }
+
+   stlhelper_inline Unsafevector(const Unsafevector<T *> & other)
+   {
+      v = new T *[other.numElems];
+      memcpy(v, other.v, sizeof(T *) * other.numElems);
+      capacity = other.numElems;
+      numElems = other.numElems;
+   }
+
+   stlhelper_inline Unsafevector<T *> & operator=(const Unsafevector<T *> & other)
+   {
+      reserve_nocopy(other.numElems);
+      memcpy(v, other.v, sizeof(T *) * other.numElems);
+      capacity = other.numElems;
+      numElems = other.numElems;
+      return *this;
+   }
+
+   stlhelper_inline Unsafevector<T *> & operator=(const std::vector<T *> & other)
+   {
+      int vSize = other.size();
+      reserve_nocopy(vSize);
+      memcpy(v, &other[0], sizeof(T *) * vSize);
+      numElems = vSize;
+      return *this;
+   }
+
+   stlhelper_inline ~Unsafevector()
+   {
+      delete[] v;
+   }
+
+   stlhelper_inline void reserve(int newSize)
+   {
+      if (newSize > capacity)
+      {
+         while (capacity <= newSize)
+         {
+            capacity *= 2;
+         }
+         T ** newPtr = new T *[capacity];
+         memcpy(newPtr, v, sizeof(T *) * numElems);
+         delete[] v;
+         v = newPtr;
+      }
+   }
+
+   stlhelper_inline void addVectorNoCheck(std::vector<T *> & vecIn)
+   {
+      int vecInSize = vecIn.size();
+      if (vecInSize)
+      {
+         memcpy(&v[numElems], &vecIn[0], sizeof(T *) * vecInSize);
+         numElems += vecInSize;
+      }
+   }
+
+   stlhelper_inline void addVector(std::vector<T *> & vecIn)
+   {
+      int vecInSize = vecIn.size();
+      if (vecInSize)
+      {
+         reserve(numElems + vecInSize);
+         memcpy(&v[numElems], &vecIn[0], sizeof(T *) * vecInSize);
+         numElems += vecInSize;
+      }
+   }
+
+   stlhelper_inline void addVectorNoCheck(Unsafevector<T *> & vecIn)
+   {
+      int vecInSize = vecIn.size();
+      if (vecInSize)
+      {
+         memcpy(&v[numElems], vecIn.v, sizeof(T *) * vecInSize);
+         numElems += vecInSize;
+      }
+   }
+
+   stlhelper_inline void addVector(Unsafevector<T *> & vecIn)
+   {
+      int vecInSize = vecIn.size();
+      if (vecInSize)
+      {
+         reserve(numElems + vecInSize);
+         memcpy(&v[numElems], vecIn.v, sizeof(T *) * vecInSize);
+         numElems += vecInSize;
+      }
+   }
+
+   stlhelper_inline void addElementNoCheck(T * elemIn)
+   {
+      v[numElems++] = elemIn;
+   }
+
+   stlhelper_inline void addElement(T * elemIn)
+   {
+      if (numElems >= capacity)
+      {
+         reserve(capacity * 2);
+      }
+      v[numElems++] = elemIn;
+   }
+
+   void addElementSortedUnique(T * elemIn)
+   {
+      for (int ii = 0; ii < numElems; ++ii)
+      {
+         if (elemIn == v[ii])
+         {
+            return;
+         }
+      }
+      addElement(elemIn);
+      sortSelf();
+   }
+
+   stlhelper_inline void removeElement(int idx)
+   {
+      memmove(&v[idx], &v[idx + 1], sizeof(T*) * (numElems - idx - 1));
+      --numElems;
+   }
+
+   stlhelper_inline int find(const T * testElem)
+   {
+      for (int ii = 0, end = numElems; ii != end; ++ii)
+      {
+         if (v[ii] == testElem)
+         {
+            return ii;
+         }
+      }
+      return -1;
+   }
+
+   stlhelper_inline void clear()
+   {
+      numElems = 0;
+   }
+
+   stlhelper_inline int size()
+   {
+      return numElems;
+   }
+
+   stlhelper_inline bool isEmpty()
+   {
+      return (numElems == 0);
+   }
+
+   stlhelper_inline T * & operator[](int idx)
+   {
+      return v[idx];
+   }
+   stlhelper_inline T * operator[](int idx) const
+   {
+      return v[idx];
+   }
+
+   stlhelper_inline void swap(Unsafevector<T *> & other)
+   {
+      std::swap(v, other.v);
+      std::swap(numElems, other.numElems);
+      std::swap(capacity, other.capacity);
+   }
+
+   stlhelper_inline static int compareelems(const void * a, const void * b)
+   {
+      T * aPtr = *((T **) a);
+      T * bPtr = *((T **) b);
+      if (aPtr < bPtr)
+         return -1;
+      if (aPtr == bPtr)
+         return 0;
+      return 1;
+   }
+
+   stlhelper_inline static int comparekey(const void * a, const void * b)
+   {
+      T * aPtr = (T *) a;
+      T * bPtr = *((T **) b);
+      if (aPtr < bPtr)
+         return -1;
+      if (aPtr == bPtr)
+         return 0;
+      return 1;
+   }
+
+   stlhelper_inline bool contains(const T & keyIn)
+   {
+      return (search(&keyIn) != 0x0);
+   }
+
+#define compareElems (int (*)( const void *, const void * )) Unsafevector<T *>::compareelems
+#define compareKey (int (*)( const void *, const void * )) Unsafevector<T *>::comparekey
+
+   stlhelper_inline void sortSelf()
+   {
+      std::qsort(v, numElems, sizeof(T *), compareElems);
+   }
+
+   stlhelper_inline void sortSelf(int (*compareIn)(const void *, const void *))
+   {
+      std::qsort(v, numElems, sizeof(T *), compareIn);
+   }
+
+   T ** v;
+   int numElems;
+   int capacity;
+
+protected:
+   stlhelper_inline T ** search(const T * keyIn)
+   {
+      size_t lowerIdx, upperIdx, currIdx;
+      const void *currAddr;
+      int comparResult;
+
+      lowerIdx = 0;
+      upperIdx = numElems;
+      while (lowerIdx < upperIdx)
+      {
+         currIdx = (lowerIdx + upperIdx) / 2;
+         currAddr = (void *) (((const char *) v) + (currIdx * sizeof(T *)));
+         comparResult = (*compareKey )(keyIn, currAddr);
+         if (comparResult < 0)
+            upperIdx = currIdx;
+         else if (comparResult > 0)
+            lowerIdx = currIdx + 1;
+         else
+            return (T **) currAddr;
+      }
+
+      return NULL;
+   }
+
+   stlhelper_inline void reserve_nocopy(int newSize)
+   {
+      if (newSize > capacity)
+      {
+         delete[] v;
+         v = new T *[newSize];
+         capacity = newSize;
+      }
+   }
+#undef compareElems
+#undef compareKey
+};
+
+}
+
+#endif /* UNSAFEVECTOR_HH_ */

trick_source/er7_utils/CheckpointHelper/Vector.hh

@@ -0,0 +1,79 @@
+/*******************************************************************************
+
+ PURPOSE:
+ (Vector for checkpointable vector container.)
+
+ PROGRAMMERS:
+ (((Thomas Brain) (Metecs) (May 2013) (--)))
+
+ *******************************************************************************/
+
+#ifndef CHECKPOINTHELPER_VECTOR_HH_
+#define CHECKPOINTHELPER_VECTOR_HH_
+
+#include "CheckpointItem.hh"
+#include "Unsafevector.hh"
+#include <vector>
+
+namespace CheckpointHelper
+{
+
+template<class T>
+class vector: public CheckpointItem, public std::vector<T>
+{
+public:
+   T ** chkpt;
+   std::string mmstr;
+
+   vector(T ** chkptIn, std::string mmstrIn) :
+         chkpt(chkptIn), mmstr(mmstrIn)
+   {
+      *chkpt = 0x0;
+   }
+
+   vector<T>& operator=(const std::vector<T>& pr)
+   {
+      std::vector<T>::operator=(pr);
+      return *this;
+   }
+
+   virtual ~vector()
+   {
+   }
+
+   virtual void checkpoint()
+   {
+      chkpt_seq<typename std::vector<T>::iterator, T>(*this);
+   }
+   virtual void restart()
+   {
+      restart_seq<typename std::vector<T>::iterator>(*this);
+   }
+};
+
+}
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#define STLVECTOR( type, varName ) \
+   type* _##varName; \
+   CheckpointHelper::vector<type> varName
+#else
+#define STLVECTOR( type, varName ) \
+   type* _##varName
+#endif
+
+#if  (!defined(SWIG) && !defined(TRICK_ICG))
+#ifndef STL_CSTR1
+#define STL_CSTR1( varName, type  ) \
+   varName( &_##varName, #type )
+#endif
+#else
+#ifndef STL_CSTR1
+#define STL_CSTR1( varName, type  ) \
+   _##varName()
+#endif
+#endif
+
+#define STLVECTORCSTR STL_CSTR1
+
+#endif /* STLHELPER_VECTOR_HH_ */

trick_source/er7_utils/CheckpointHelper/doxygen/index.html

@@ -0,0 +1,6 @@
+<!-- $Id$ -->
+
+<head>
+<meta HTTP-EQUIV="REFRESH" content="0; url=html/index.html">
+</head>
+

trick_source/er7_utils/CheckpointHelper/trick_mem_utils.py

@@ -0,0 +1,94 @@
+def create_new_sim_object( simObjName, typeName, ctrString = "" ):
+  global externAllocs
+  TRICK_DEFAULT_PHASE = 60000
+  externAllocs.simObjectAllocs.append((simObjName, typeName, ctrString))
+
+  if not ctrString:
+    ctrString = "trick." + typeName + "()"
+  else:
+    ctrString = "trick." + typeName + ctrString
+     
+  cmdStr = simObjName + " = " + ctrString + "\n"
+  cmdStr += "globals()[\'" + simObjName + "\'] = " + simObjName + "\n"
+
+  if "DRAscii" in typeName or "DRBinary" in typeName or "DRHDF5" in typeName:
+     cmdStr += simObjName + ".declare_memory()\n"
+  else:
+     cmdStr += "trick.exec_add_sim_object(" + simObjName + ", \'" + simObjName + "\' )\n"
+     cmdStr += "trick.TMM_declare_ext_var(" + simObjName + ", trick.TRICK_STRUCTURED, \'" + typeName + "\', 0, \'" + simObjName + "\', 0, None)\n"
+
+  cmdStr += "newSO = " + simObjName + "\n"
+  exec(cmdStr)
+  return newSO
+
+def create_new_instance( objectName, typeName, ctrString = "", mmName = ""):
+  global externAllocs
+  externAllocs.objectAllocs.append((objectName, typeName, ctrString, mmName))
+
+  if not ctrString:
+    ctrString = "trick." + typeName + "()"
+  else:
+    ctrString = "trick." + typeName + ctrString
+
+  cmdStr = objectName + " = " + ctrString + "\n"
+  cmdStr += "globals()[\'" + objectName + "\'] = " + objectName + "\n"
+  if not mmName:
+     cmdStr += "trick.TMM_declare_ext_var(" + objectName + ", trick.TRICK_STRUCTURED, \'" + typeName + "\', 0, \'" + objectName + "\', 0, None)\n"
+  else:
+     cmdStr += "trick.TMM_declare_ext_var(" + objectName + ", trick.TRICK_STRUCTURED, \'" + mmName + "\', 0, \'" + objectName + "\', 0, None)\n"
+  cmdStr += "newObj = " + objectName + "\n"
+  exec(cmdStr)
+  return newObj
+
+class ExternAlloc():
+  def __init__(self):
+     self.simObjectAllocs = []
+     self.objectAllocs = []
+
+  def printEntries(self):
+     for alloc in self.simObjectAllocs:
+        print "SimObject", alloc[0], "of type", alloc[1], "allocated in input file"
+     for alloc in self.objectAllocs:
+        print "Object", alloc[0], "of type", alloc[1], "allocated in input file"
+     return
+  
+  def writeChkptFile(self, chkpt_file):
+     path = chkpt_file
+     file = open(path, 'w')
+     file.write("oldAllocs = ExternAlloc()\n")
+     file.write("oldAllocs.simObjectAllocs = list(externAllocs.simObjectAllocs)\n")
+     file.write("oldAllocs.objectAllocs = list(externAllocs.objectAllocs)\n")
+     file.write("newAllocs = ExternAlloc()\n")
+     for alloc in self.simObjectAllocs:
+        file.write("newAllocs.simObjectAllocs.append((\"" + alloc[0] + "\",\"" + alloc[1] + "\",\"" + alloc[2] +"\"))\n")
+     for alloc in self.objectAllocs:
+        file.write("newAllocs.objectAllocs.append((\"" + alloc[0] + "\",\"" + alloc[1] + "\",\"" + alloc[2] +"\", \"" + alloc[3] + "\"))\n")
+     file.write("for alloc in newAllocs.simObjectAllocs:\n")
+     file.write("  if alloc not in oldAllocs.simObjectAllocs:\n")
+     file.write("     create_new_sim_object( alloc[0], alloc[1], alloc[2])\n")
+     file.write("     print \"Allocated \" + alloc[0]\n")
+     file.write("  else:\n")
+     file.write("     oldAllocs.simObjectAllocs.remove( alloc )\n")
+     file.write("for alloc in newAllocs.objectAllocs:\n")
+     file.write("  if alloc not in oldAllocs.objectAllocs:\n")
+     file.write("     create_new_instance( alloc[0], alloc[1], alloc[2], alloc[3])\n")
+     file.write("     print \"Allocated \" + alloc[0]\n")
+     file.write("  else:\n")
+     file.write("     oldAllocs.objectAllocs.remove( alloc )\n")
+     file.write("for alloc in oldAllocs.simObjectAllocs:\n")
+     file.write("     exec(\"ptr = \" + alloc[0])\n")
+     file.write("     trick.exec_remove_sim_object( ptr ) \n")
+     file.write("     trick.TMM_delete_extern_var_a( ptr )\n")
+     file.write("     del globals()[alloc[0]]\n")
+     file.write("     print \"Deleting \" + alloc[0]\n")
+     file.write("     externAllocs.simObjectAllocs.remove( alloc )\n")
+     file.write("for alloc in oldAllocs.objectAllocs:\n")
+     file.write("     trick.TMM_delete_extern_var_n( alloc[0] )\n")
+     file.write("     del globals()[alloc[0]]\n")
+     file.write("     print \"Deleting \" + alloc[0]\n")
+     file.write("     externAllocs.objectAllocs.remove( alloc )\n")
+     return
+
+if "externAllocs" not in globals():
+  global externAllocs
+  externAllocs = ExternAlloc()

trick_source/er7_utils/bin/er7_utils_cm/doxygenate.csh

@@ -0,0 +1,23 @@
+#!/bin/tcsh -f
+
+set er7_docs = $ER7_UTILS_HOME/er7_utils/docs
+
+chdir $ER7_UTILS_HOME
+
+if (-d html) rm -rf html
+if (-d doxygen) rm -rf doxygen
+mkdir doxygen
+
+set doxygen_version = `doxygen --version | cut -d'.' -f -3`
+if (-f $er7_docs/doxygen/html_config.dox.$doxygen_version) then
+   set doxygen_config = $er7_docs/doxygen/html_config.dox.$doxygen_version
+else
+   echo "Configuration file for doxygen verion $doxygen_version not found"
+   exit 1
+endif
+
+(doxygen $doxygen_config >> doxygen/doxygen.out) >>& doxygen/doxygen.err
+
+mkdir html
+mv doxygen/html html/er7_utils
+

trick_source/er7_utils/bin/er7_utils_cm/doxygenate_prep.csh

@@ -0,0 +1,47 @@
+#!/bin/tcsh -f
+
+set install_target = $cwd/doxygen
+if (! -d $install_target) then
+   echo "Missing installation target $install_target"
+   exit 1
+endif
+
+set er7_scripts = $ER7_UTILS_HOME/er7_utils/bin/er7_utils_cm
+set pp = "$er7_scripts/er7_utils_pp -mod -dox -prefix $install_target/er7_utils"
+
+chdir $ER7_UTILS_HOME/er7_utils
+
+set skip = '/(verif|\.svn|unit_test|data)/'
+
+set files = ()
+foreach dir (integration interface jeod trick)
+   set dirfiles = `find $dir -name '*.[ch][ch]' | grep -v -E $skip`
+   set files = ($files $dirfiles)
+end
+
+if (-d $install_target/er7_utils) then
+   rm -rf $install_target/er7_utils
+endif
+mkdir $install_target/er7_utils
+
+set missing = ()
+foreach file ($files)
+   (perl $pp $file >> $install_target/doxygen.out) \
+      >>& $install_target/doxygen.err
+   if (! -f $install_target/er7_utils/$file) then
+      set missing = ($missing $file)
+   endif
+end
+
+if ($#missing > 0) then
+   printf "\nCould not process the following ER7 numerical utilities files:\n" \
+      >> $install_target/doxygen.err
+   foreach file ($missing)
+      printf "$file\n" >> $install_target/doxygen.err
+   end
+endif
+
+cp docs/doxygen/groups.txt $install_target/er7_utils
+
+printf "End of doxygen processing of ER7 numerical utilities\n\n" \
+   >> $install_target/doxygen.err

trick_source/er7_utils/docs/doxygen/groups.txt

@@ -0,0 +1,92 @@
+/**
+@defgroup Er7Utils Er7Utils
+The ER7 numerical utilities package.
+
+@addtogroup Er7Utils
+@{
+
+/**
+ * Namespace in which the entirety of the
+ * ER7 numerical utilities package is embedded.
+ */
+namespace er7_utils {
+}
+
+
+@defgroup Jeod Jeod
+The ER7 numerical utilities side of the er7_utils/JEOD interface.
+
+@addtogroup Jeod
+@{
+
+namespace er7_utils {
+
+/**
+ * Namespace in which the er7_utils/JEOD interface is embedded.
+ */
+namespace jeod_interface {
+}
+}
+
+@}
+
+
+@defgroup Trick Trick
+The ER7 numerical utilities side of the er7_utils/Trick interface.
+
+
+@defgroup Interface Interface
+Low-level interface utilities used internally within the
+ER7 numerical utilities package.
+
+@defgroup Integration Integration
+Numerical integration techniques.
+
+@addtogroup Integration
+@{
+
+@defgroup Abm4 Abm4
+Fourth order Adams Bashforth Moulton integration.
+
+@defgroup Beeman Beeman
+Beeman's method.
+
+@defgroup Core Core
+Common integration infrastructure.
+
+@defgroup Euler Euler
+Euler's method.
+
+@defgroup Mm4 Mm4
+Modified midpoint 4.
+
+@defgroup Nl2 Nl2
+Nystrom-Lear 2 integration.
+
+@defgroup Rk2Heun Rk2Heun
+Heun's method.
+
+@defgroup Rk2Midpoint Rk2Midpoint
+Midpoint method.
+
+@defgroup Rk4 Rk4
+Canonical fourth order Runge Kutta.
+
+@defgroup Rkf45 Rkf45
+Runge Kutta Fehberg 4/5.
+
+@defgroup Rkf78 Rkf78
+Runge Kutta Fehberg 7/8.
+
+@defgroup Rkg4 Rkg4
+Fourth order Runge Kutta Gill.
+
+@defgroup SymplecticEuler SymplecticEuler
+Symplectic Euler integration, aka Euler-Cromer.
+
+@defgroup VelocityVerlet VelocityVerlet
+Velocity verlet method.
+
+@}
+@}
+*/

trick_source/er7_utils/docs/doxygen/html_config.dox.1.7.5

@@ -0,0 +1,294 @@
+# Doxyfile 1.7.5
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+DOXYFILE_ENCODING      = UTF-8
+PROJECT_NAME           = ER7_UTILS
+PROJECT_NUMBER         = 0.9
+PROJECT_BRIEF          =
+PROJECT_LOGO           =
+OUTPUT_DIRECTORY       = doxygen
+CREATE_SUBDIRS         = YES
+OUTPUT_LANGUAGE        = English
+BRIEF_MEMBER_DESC      = YES
+REPEAT_BRIEF           = YES
+ABBREVIATE_BRIEF       =
+ALWAYS_DETAILED_SEC    = NO
+INLINE_INHERITED_MEMB  = NO
+FULL_PATH_NAMES        = NO
+STRIP_FROM_PATH        =
+STRIP_FROM_INC_PATH    =
+SHORT_NAMES            = NO
+JAVADOC_AUTOBRIEF      = YES
+QT_AUTOBRIEF           = NO
+MULTILINE_CPP_IS_BRIEF = NO
+INHERIT_DOCS           = YES
+SEPARATE_MEMBER_PAGES  = NO
+TAB_SIZE               = 3
+ALIASES                =
+OPTIMIZE_OUTPUT_FOR_C  = YES
+OPTIMIZE_OUTPUT_JAVA   = NO
+OPTIMIZE_FOR_FORTRAN   = NO
+OPTIMIZE_OUTPUT_VHDL   = NO
+EXTENSION_MAPPING      =
+# BUILTIN_STL_SUPPORT    = YES
+BUILTIN_STL_SUPPORT    = NO
+CPP_CLI_SUPPORT        = NO
+SIP_SUPPORT            = NO
+IDL_PROPERTY_SUPPORT   = YES
+DISTRIBUTE_GROUP_DOC   = NO
+SUBGROUPING            = YES
+INLINE_GROUPED_CLASSES = NO
+INLINE_SIMPLE_STRUCTS  = NO
+TYPEDEF_HIDES_STRUCT   = NO
+SYMBOL_CACHE_SIZE      = 0
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+EXTRACT_ALL            = YES
+EXTRACT_PRIVATE        = YES
+EXTRACT_STATIC         = YES
+EXTRACT_LOCAL_CLASSES  = YES
+EXTRACT_LOCAL_METHODS  = NO
+EXTRACT_ANON_NSPACES   = NO
+HIDE_UNDOC_MEMBERS     = NO
+HIDE_UNDOC_CLASSES     = NO
+HIDE_FRIEND_COMPOUNDS  = NO
+HIDE_IN_BODY_DOCS      = NO
+INTERNAL_DOCS          = YES
+CASE_SENSE_NAMES       = YES
+HIDE_SCOPE_NAMES       = NO
+SHOW_INCLUDE_FILES     = YES
+FORCE_LOCAL_INCLUDES   = NO
+INLINE_INFO            = YES
+SORT_MEMBER_DOCS       = YES
+SORT_BRIEF_DOCS        = NO
+SORT_MEMBERS_CTORS_1ST = NO
+SORT_GROUP_NAMES       = NO
+SORT_BY_SCOPE_NAME     = NO
+STRICT_PROTO_MATCHING  = NO
+GENERATE_TODOLIST      = YES
+GENERATE_TESTLIST      = YES
+GENERATE_BUGLIST       = YES
+GENERATE_DEPRECATEDLIST= YES
+ENABLED_SECTIONS       =
+MAX_INITIALIZER_LINES  = 30
+SHOW_USED_FILES        = YES
+SHOW_DIRECTORIES       = YES
+SHOW_FILES             = YES
+SHOW_NAMESPACES        = YES
+FILE_VERSION_FILTER    =
+LAYOUT_FILE            =
+CITE_BIB_FILES         =
+#---------------------------------------------------------------------------
+# configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+QUIET                  = NO
+WARNINGS               = YES
+WARN_IF_UNDOCUMENTED   = YES
+WARN_IF_DOC_ERROR      = YES
+WARN_NO_PARAMDOC       = NO
+WARN_FORMAT            = "$file:$line: $text"
+WARN_LOGFILE           =
+#---------------------------------------------------------------------------
+# configuration options related to the input files
+#---------------------------------------------------------------------------
+INPUT                  =
+INPUT_ENCODING         = UTF-8
+FILE_PATTERNS          = *.cc \
+                         *.hh \
+                         *.txt \
+                         *.c \
+                         *.h \
+                         *.d \
+                         *.py \
+                         S_define
+RECURSIVE              = YES
+EXCLUDE                = */verif \
+                         */Old
+EXCLUDE_SYMLINKS       = NO
+EXCLUDE_PATTERNS       =
+EXCLUDE_SYMBOLS        =
+EXAMPLE_PATH           = .
+EXAMPLE_PATTERNS       =
+EXAMPLE_RECURSIVE      = NO
+IMAGE_PATH             =
+INPUT_FILTER           =
+FILTER_PATTERNS        =
+FILTER_SOURCE_FILES    = NO
+FILTER_SOURCE_PATTERNS =
+#---------------------------------------------------------------------------
+# configuration options related to source browsing
+#---------------------------------------------------------------------------
+SOURCE_BROWSER         = YES
+INLINE_SOURCES         = NO
+STRIP_CODE_COMMENTS    = YES
+REFERENCED_BY_RELATION = YES
+REFERENCES_RELATION    = YES
+REFERENCES_LINK_SOURCE = NO
+USE_HTAGS              = NO
+VERBATIM_HEADERS       = YES
+#---------------------------------------------------------------------------
+# configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+ALPHABETICAL_INDEX     = YES
+COLS_IN_ALPHA_INDEX    = 5
+IGNORE_PREFIX          =
+#---------------------------------------------------------------------------
+# configuration options related to the HTML output
+#---------------------------------------------------------------------------
+GENERATE_HTML          = YES
+HTML_OUTPUT            = html
+HTML_FILE_EXTENSION    = .html
+HTML_HEADER            =
+HTML_FOOTER            =
+HTML_STYLESHEET        =
+HTML_EXTRA_FILES       =
+HTML_COLORSTYLE_HUE    = 220
+HTML_COLORSTYLE_SAT    = 100
+HTML_COLORSTYLE_GAMMA  = 80
+HTML_TIMESTAMP         = NO
+HTML_ALIGN_MEMBERS     = YES
+HTML_DYNAMIC_SECTIONS  = NO
+GENERATE_DOCSET        = NO
+DOCSET_FEEDNAME        = "Doxygen generated docs"
+DOCSET_BUNDLE_ID       = org.doxygen.Project
+DOCSET_PUBLISHER_ID    = org.doxygen.Publisher
+DOCSET_PUBLISHER_NAME  = Publisher
+GENERATE_HTMLHELP      = NO
+CHM_FILE               =
+HHC_LOCATION           =
+GENERATE_CHI           = NO
+CHM_INDEX_ENCODING     =
+BINARY_TOC             = NO
+TOC_EXPAND             = NO
+GENERATE_QHP           = NO
+QCH_FILE               =
+QHP_NAMESPACE          = org.doxygen.Project
+QHP_VIRTUAL_FOLDER     = doc
+QHP_CUST_FILTER_NAME   =
+QHP_CUST_FILTER_ATTRS  =
+QHP_SECT_FILTER_ATTRS  =
+QHG_LOCATION           =
+GENERATE_ECLIPSEHELP   = NO
+ECLIPSE_DOC_ID         = org.doxygen.Project
+DISABLE_INDEX          = NO
+ENUM_VALUES_PER_LINE   = 4
+GENERATE_TREEVIEW      = NO
+USE_INLINE_TREES       = YES
+TREEVIEW_WIDTH         = 250
+EXT_LINKS_IN_WINDOW    = NO
+FORMULA_FONTSIZE       = 10
+FORMULA_TRANSPARENT    = YES
+USE_MATHJAX            = NO
+MATHJAX_RELPATH        = http://www.mathjax.org/mathjax
+MATHJAX_EXTENSIONS     =
+SEARCHENGINE           = NO
+SERVER_BASED_SEARCH    = NO
+#---------------------------------------------------------------------------
+# configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+GENERATE_LATEX         = NO
+LATEX_OUTPUT           = latex
+LATEX_CMD_NAME         = latex
+MAKEINDEX_CMD_NAME     = makeindex
+COMPACT_LATEX          = NO
+PAPER_TYPE             = a4wide
+EXTRA_PACKAGES         =
+LATEX_HEADER           =
+LATEX_FOOTER           =
+PDF_HYPERLINKS         = YES
+USE_PDFLATEX           = YES
+LATEX_BATCHMODE        = NO
+LATEX_HIDE_INDICES     = NO
+LATEX_SOURCE_CODE      = NO
+LATEX_BIB_STYLE        = plain
+#---------------------------------------------------------------------------
+# configuration options related to the RTF output
+#---------------------------------------------------------------------------
+GENERATE_RTF           = NO
+RTF_OUTPUT             = rtf
+COMPACT_RTF            = NO
+RTF_HYPERLINKS         = NO
+RTF_STYLESHEET_FILE    =
+RTF_EXTENSIONS_FILE    =
+#---------------------------------------------------------------------------
+# configuration options related to the man page output
+#---------------------------------------------------------------------------
+GENERATE_MAN           = YES
+MAN_OUTPUT             = man
+MAN_EXTENSION          = .3
+MAN_LINKS              = NO
+#---------------------------------------------------------------------------
+# configuration options related to the XML output
+#---------------------------------------------------------------------------
+GENERATE_XML           = NO
+XML_OUTPUT             = xml
+XML_SCHEMA             =
+XML_DTD                =
+XML_PROGRAMLISTING     = YES
+#---------------------------------------------------------------------------
+# configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+GENERATE_AUTOGEN_DEF   = NO
+#---------------------------------------------------------------------------
+# configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+GENERATE_PERLMOD       = NO
+PERLMOD_LATEX          = NO
+PERLMOD_PRETTY         = YES
+PERLMOD_MAKEVAR_PREFIX =
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+ENABLE_PREPROCESSING   = YES
+MACRO_EXPANSION        = NO
+EXPAND_ONLY_PREDEF     = YES
+SEARCH_INCLUDES        = YES
+INCLUDE_PATH           = $(ER7_UTILS_HOME)/doxygen/models
+INCLUDE_FILE_PATTERNS  = *.hh
+PREDEFINED             = TRICK_VER=10 TRICK_MINOR=5
+EXPAND_AS_DEFINED      = 
+SKIP_FUNCTION_MACROS   = YES
+#---------------------------------------------------------------------------
+# Configuration::additions related to external references
+#---------------------------------------------------------------------------
+TAGFILES               =
+GENERATE_TAGFILE       =
+ALLEXTERNALS           = NO
+EXTERNAL_GROUPS        = YES
+PERL_PATH              = /usr/bin/perl
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+CLASS_DIAGRAMS         = YES
+MSCGEN_PATH            =
+HIDE_UNDOC_RELATIONS   = YES
+HAVE_DOT               = YES
+DOT_NUM_THREADS        = 0
+DOT_FONTNAME           = FreeSans
+DOT_FONTSIZE           = 10
+DOT_FONTPATH           =
+CLASS_GRAPH            = YES
+COLLABORATION_GRAPH    = YES
+GROUP_GRAPHS           = YES
+UML_LOOK               = NO
+TEMPLATE_RELATIONS     = NO
+INCLUDE_GRAPH          = YES
+INCLUDED_BY_GRAPH      = YES
+CALL_GRAPH             = NO
+CALLER_GRAPH           = NO
+GRAPHICAL_HIERARCHY    = YES
+DIRECTORY_GRAPH        = YES
+DOT_IMAGE_FORMAT       = png
+INTERACTIVE_SVG        = NO
+DOT_PATH               =
+DOTFILE_DIRS           =
+MSCFILE_DIRS           =
+DOT_GRAPH_MAX_NODES    = 500
+MAX_DOT_GRAPH_DEPTH    = 0
+DOT_TRANSPARENT        = NO
+DOT_MULTI_TARGETS      = YES
+GENERATE_LEGEND        = YES
+DOT_CLEANUP            = YES

trick_source/er7_utils/docs/doxygen/mainpage.txt

@@ -0,0 +1,21 @@
+/**
+@mainpage ER7 Numerical Utilities
+These pages present the ER7 numerical utilities 0.9 API in HTML format.
+
+@n Click on
+ - "Main Page" to display this page.
+ - "Related Page" to display related pages.
+ - "Modules" to see a hierarchical presentation of the ER7 utilities package.
+ - "Data Structures" to see a clickable list of all the ER7 utilities classes.
+ - "Files" to see a clickable list of the source and header files.
+
+@section code_caveat Caveat on Displayed Source Code
+
+These doxygen pages include facsimiles of the ER7 utilities source code.
+A word of warning: These are facsimiles.
+Some differences do exist between the true source code and the source presented
+in this API documentation. In particular,
+  - The line numbers differ between the displayed source and the true source.
+  - The combination of the preparation of files for doxygen and the processing
+    by doxygen strips some comments from the displayed source.
+*/

trick_source/er7_utils/integration/abm4/include/abm4_first_order_ode_integrator.hh

@@ -0,0 +1,168 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class ABM4FirstOrderODEIntegrator, which integrates a monolithic
+ * state as a first order ODE.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_ABM4_ONE_STATE_INTEGRATOR_HH
+#define ER7_UTILS_ABM4_ONE_STATE_INTEGRATOR_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/priming_first_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+class IntegratorConstructor;
+
+
+/**
+ * Advance state using the fourth order Adams-Bashforth-Moulton method.
+ * This includes
+ *   - Initial priming of the recent history of state derivatives,
+ *   - Using the ABM-4 method once primed, and
+ *   - Clearing the history when needed (e.g., time change or state change).
+ */
+class ABM4FirstOrderODEIntegrator : public PrimingFirstOrderODEIntegrator {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(ABM4FirstOrderODEIntegrator)
+
+public:
+
+   // Constructors and destructor.
+
+   /**
+    * ABM4FirstOrderODEIntegrator default constructor.
+    */
+   ABM4FirstOrderODEIntegrator (void);
+
+   /**
+    * ABM4FirstOrderODEIntegrator copy constructor.
+    * @param[in] src  Item to be copied.
+    */
+   ABM4FirstOrderODEIntegrator (
+      const ABM4FirstOrderODEIntegrator & src);
+
+   /**
+    * ABM4FirstOrderODEIntegrator non-default constructor.
+    * This is the constructor used by the integrator constructor.
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     size                State size
+    * @param[in,out] controls_in         Integration controls
+    */
+   ABM4FirstOrderODEIntegrator (
+      const IntegratorConstructor & primer_constructor,
+      unsigned int size,
+      IntegrationControls & controls_in);
+
+   /**
+    * ABM4FirstOrderODEIntegrator destructor.
+    */
+   virtual ~ABM4FirstOrderODEIntegrator (void);
+
+   /**
+    * ABM4FirstOrderODEIntegrator assignment operator.
+    * @param src  Item to be copied.
+    */
+   ABM4FirstOrderODEIntegrator & operator=(
+      ABM4FirstOrderODEIntegrator src)
+   {
+      swap (src);
+      return *this;
+   }
+
+   /**
+    * Create a copy of 'this' ABM4FirstOrderODEIntegrator object.
+    * @return Clone of 'this'.
+    */
+   virtual ABM4FirstOrderODEIntegrator * create_copy () const;
+
+
+protected:
+
+   // Member functions.
+
+   /**
+    * Non-throwing swap.
+    * @param other  Item with which contents are to be swapped.
+    */
+   void swap (ABM4FirstOrderODEIntegrator & other);
+
+   using PrimingFirstOrderODEIntegrator::swap;
+
+   /**
+    * Save derivatives during priming.
+    * @param[in] countdown     The prime_counter data member.
+    * @param[in] position      Generalized position vector.
+    * @param[in] velocity      Generalized velocity vector.
+    */
+   virtual void technique_save_derivatives (
+      int countdown,
+      const double * ER7_UTILS_RESTRICT velocity,
+      const double * ER7_UTILS_RESTRICT position);
+
+   /**
+    * Propagate state via fourth order Adams Bashforth Moulton.
+    * @param[in]     dyn_dt        Integration step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult technique_integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      const double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position);
+
+
+   // Member data
+
+   double * init_state; /*!< trick_units(--) @n
+      The state at the start of an integration cycle. */
+
+   double * deriv_hist[4]; /*!< trick_units(--) @n
+      The state derivatives at the start of current and at the starts of the
+      three previous integration cycles.
+
+      This array is rotated so that deriv_hist[0] contains the state derivatives
+      at the start of the current cycle, deriv_hist[1] the derivatives at the
+      start of the previous cycle, and so on. */
+};
+
+}
+
+
+#ifdef ER7_UTILS_NEED_AUX_INCLUDES
+#include "er7_utils/integration/core/include/integration_controls.hh"
+#endif
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/abm4/include/abm4_integrator_constructor.hh

@@ -0,0 +1,219 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class ABM4IntegratorConstructor, which constructs integrators
+ * that use ABM4 integration.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_ABM4_INTEGRATOR_CONSTRUCTOR_HH
+#define ER7_UTILS_ABM4_INTEGRATOR_CONSTRUCTOR_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/priming_integrator_constructor.hh"
+#include "er7_utils/integration/rk4/include/rk4_integrator_constructor.hh"
+
+
+namespace er7_utils {
+
+/**
+ * Create state and time integrators that propagate using fourth order
+ * Adams-Bashforth-Moulton.
+ */
+class ABM4IntegratorConstructor : public PrimingIntegratorConstructor {
+ER7_UTILS_MAKE_SIM_INTERFACES(ABM4IntegratorConstructor)
+
+public:
+   // Static member functions.
+
+   /**
+    * Named constructor; create an ABM4IntegratorConstructor instance.
+    * The caller is responsible for deleting the returned object.
+    * @return Newly created ABM4IntegratorConstructor instance.
+    */
+   static IntegratorConstructor* create_constructor (void);
+
+
+   // Constructors and assignment operator
+   // Note that the destructor is not declared.
+   // The implicitly defined destructor is correct.
+
+   /**
+    * ABM4 default constructor.
+    * Note that by default ABM4 uses RK4 for priming.
+    */
+   ABM4IntegratorConstructor (void)
+   :
+      Er7UtilsDeletable (),
+      PrimingIntegratorConstructor (RK4IntegratorConstructor ())
+   {}
+
+   /**
+    * ABM4IntegratorConstructor copy constructor.
+    */
+   ABM4IntegratorConstructor (const ABM4IntegratorConstructor & src)
+   :
+      Er7UtilsDeletable (),
+      PrimingIntegratorConstructor (src)
+   {}
+
+   /**
+    * ABM4IntegratorConstructor assignment operator.
+    */
+   ABM4IntegratorConstructor & operator= (ABM4IntegratorConstructor src)
+   {
+      swap (src);
+      return *this;
+   }
+
+
+   // Member functions.
+
+   /**
+    * Return the class name.
+    */
+   virtual const char * get_class_name (void) const
+   { return "ABM4IntegratorConstructor"; }
+
+   /**
+    * ABM4 currently does not implement a second order generalized step
+    * integrator.
+    */
+   virtual bool implements (Integration::ODEProblemType problem_type) const
+   {
+      return (problem_type != Integration::GeneralizedStepSecondOrderODE);
+   }
+
+   /**
+    * ABM4 currently does not provide a second order generalized step
+    * integrator.
+    */
+   virtual bool provides (Integration::ODEProblemType problem_type) const
+   {
+      return (problem_type != Integration::GeneralizedStepSecondOrderODE);
+   }
+
+   /**
+    * Create a duplicate of the constructor.
+    * The caller is responsible for deleting the returned object.
+    * @return Duplicated constructor.
+    */
+   virtual IntegratorConstructor * create_copy (void) const;
+
+   /**
+    * Create an integration controls that guides the ABM4 integration process.
+    * The caller is responsible for deleting the created object.
+    * @return Integration controls object
+    */
+   virtual IntegrationControls * create_integration_controls (void) const;
+
+   /**
+    * Create an ABM4 state integrator for a first order ODE.
+    * The caller is responsible for deleting the created object.
+    * @return State integrator
+    * @param[in]     size      State size
+    * @param[in,out] controls  Integration controls
+    */
+   virtual FirstOrderODEIntegrator * create_first_order_ode_integrator (
+      unsigned int size,
+      IntegrationControls & controls) const;
+
+   /**
+    * Create an ABM4 state integrator for a simple second order ODE.
+    * The caller is responsible for deleting the created object.
+    * @return State integrator
+    * @param[in]     size      State size
+    * @param[in,out] controls  Integration controls
+    */
+   virtual SecondOrderODEIntegrator * create_second_order_ode_integrator (
+      unsigned int size,
+      IntegrationControls & controls) const;
+
+   /**
+    * Create an ABM4 state integrator for a generalized second order ODE
+    * where generalized position is advanced with the use of the
+    * position derivative function.
+    * The caller is responsible for deleting the created object.
+    * @return State integrator
+    * @param[in]     position_size  Size of the generalized position
+    * @param[in]     velocity_size  Size of the generalized velocity
+    * @param[in]     deriv_funs     Position derivative functions container
+    * @param[in,out] controls       Integration controls
+    */
+   virtual SecondOrderODEIntegrator *
+   create_generalized_deriv_second_order_ode_integrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionDerivativeFunctions & deriv_funs,
+      IntegrationControls & controls) const;
+
+#if 0
+   /**
+    * Create an ABM4 state integrator for a generalized second order ODE
+    * where generalized position is advanced with the use of the
+    * position step function.
+    * The caller is responsible for deleting the created object.
+    * @return State integrator
+    * @param[in]     position_size  Size of the generalized position
+    * @param[in]     velocity_size  Size of the generalized velocity
+    * @param[in]     step_funs      Position step functions container
+    * @param[in,out] controls       Integration controls
+    */
+   virtual SecondOrderODEIntegrator *
+   create_generalized_step_second_order_ode_integrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionStepFunctions & step_funs,
+      IntegrationControls & controls) const;
+#endif
+
+
+   /**
+    * ABM4 uses two steps per cycle once primed.
+    * @return Always returns 2.
+    */
+   virtual unsigned int get_transition_table_size (void) const
+   { return 2; }
+
+   /**
+    * ABM4 needs a history of four derivatives.
+    * @return Always returns 4.
+    */
+   virtual unsigned int get_history_length (void) const
+   { return 4; }
+};
+
+}
+
+
+#ifdef ER7_UTILS_NEED_AUX_INCLUDES
+#include "er7_utils/integration/core/include/priming_integration_controls.hh"
+#include "abm4_first_order_ode_integrator.hh"
+#include "abm4_second_order_ode_integrator.hh"
+#endif
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/abm4/include/abm4_second_order_ode_integrator.hh

@@ -0,0 +1,523 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class ABM4SecondOrderODEIntegrator, which integrates a state
+ * comprising a zeroth derivative / first derivative pair via the
+ * fourth order Adams Bashforth Moulton method.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_ABM4_TWO_STATE_INTEGRATOR_HH
+#define ER7_UTILS_ABM4_TWO_STATE_INTEGRATOR_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/priming_second_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+class IntegratorConstructor;
+
+/**
+ * Advance state using the fourth order Adams-Bashforth-Moulton method.
+ * This includes
+ *   - Initial priming of the recent history of state derivatives,
+ *   - Using the ABM-4 method once primed, and
+ *   - Clearing the history when needed (e.g., time change or state change).
+ */
+class ABM4SecondOrderODEIntegrator : public PrimingSecondOrderODEIntegrator {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(ABM4SecondOrderODEIntegrator)
+
+
+   // Note:
+   //  This is an abstract class.
+   //  - The constructors for this class are protected.
+   //  - The assignment operator for this class is private / unimplemented.
+
+
+public:
+
+   /**
+    * ABM4SecondOrderODEIntegrator destructor.
+    */
+   virtual ~ABM4SecondOrderODEIntegrator (void);
+
+
+protected:
+
+   // Constructors.
+
+   /**
+    * ABM4SecondOrderODEIntegrator default constructor.
+    */
+   ABM4SecondOrderODEIntegrator (void);
+
+   /**
+    * ABM4SecondOrderODEIntegrator copy constructor.
+    * @param[in] src  Object to be copied.
+    */
+   ABM4SecondOrderODEIntegrator (const ABM4SecondOrderODEIntegrator & src);
+
+   /**
+    * ABM4SecondOrderODEIntegrator non-default constructor
+    * for a simple second order ODE.
+    * This constructor is used by the integrator constructor.
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     size                State size
+    * @param[in,out] controls            Integration controls
+    */
+   ABM4SecondOrderODEIntegrator (
+      const IntegratorConstructor & primer_constructor,
+      unsigned int size,
+      IntegrationControls & controls);
+
+   /**
+    * ABM4SecondOrderODEIntegrator non-default constructor
+    * for a generalized second order ODE in which position is advanced
+    * using the position derivative function.
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     position_size       Size of the generalized position
+    * @param[in]     velocity_size       Size of the generalized velocity
+    * @param[in]     deriv_funs          Position derivative functions container
+    * @param[in,out] controls            Integration controls
+    */
+   ABM4SecondOrderODEIntegrator (
+      const IntegratorConstructor & primer_constructor,
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionDerivativeFunctions & deriv_funs,
+      IntegrationControls & controls);
+
+// The current implementation of ABM4GeneralizedStepSecondOrderODEIntegrator
+// is not ready for prime time.
+#if 0
+   /**
+    * ABM4SecondOrderODEIntegrator non-default constructor
+    * for a generalized second order ODE in which position is advanced
+    * using the position step function.
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     position_size       Size of the generalized position
+    * @param[in]     velocity_size       Size of the generalized velocity
+    * @param[in]     step_funs           Position step functions container
+    * @param[in,out] controls            Integration controls
+    */
+   ABM4SecondOrderODEIntegrator (
+      const IntegratorConstructor & primer_constructor,
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionStepFunctions & step_funs,
+      IntegrationControls & controls);
+#endif
+
+
+   // Member functions.
+
+   /**
+    * Non-throwing swap.
+    * @param other  Item with which contents are to be swapped.
+    */
+   void swap (ABM4SecondOrderODEIntegrator & other);
+
+   using PrimingSecondOrderODEIntegrator::swap;
+
+
+   // Member data.
+
+   double * init_pos; /**< trick_units(--) @n
+      Position at the start of an integration cycle. */
+
+   double * init_vel; /**< trick_units(--) @n
+      Velocity at the start of an integration cycle. */
+
+   double * posdot; /**< trick_units(--) @n
+      Position derivative at the current time step. */
+
+#if 0
+   double * dtheta; /**< trick_units(--) @n
+      Product of delta t and weighted sum of generalized velocities. */
+#endif
+
+   double * posdot_hist[4]; /**< trick_units(--) @n
+      Position derivatives at each step in the integration cycle. */
+
+   double * posdot_hist_anchor; /**< trick_units(--) @n
+   Cached pointer to the data needed for deletion after PDH is rotated */
+
+   double * veldot_hist[4]; /**< trick_units(--) @n
+      Velocity derivatives at each step in the integration cycle. */
+
+   double * veldot_hist_anchor; /**< trick_units(--) @n
+   Cached pointer to the data needed for deletion after VDH is rotated */
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   ABM4SecondOrderODEIntegrator & operator= (
+      const ABM4SecondOrderODEIntegrator &);
+};
+
+
+/**
+ * Specialization of ABM4SecondOrderODEIntegrator for the case of
+ * generalized velocity being the time derivative of generalized position.
+ */
+class ABM4SimpleSecondOrderODEIntegrator :
+   public ABM4SecondOrderODEIntegrator {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(ABM4SimpleSecondOrderODEIntegrator)
+
+public:
+
+   // Constructors and destructor.
+
+   /**
+    * ABM4SimpleSecondOrderODEIntegrator default constructor.
+    */
+   ABM4SimpleSecondOrderODEIntegrator (void)
+   :
+      Er7UtilsDeletable(),
+      ABM4SecondOrderODEIntegrator()
+   {}
+
+   /**
+    * ABM4SimpleSecondOrderODEIntegrator copy constructor.
+    * @param[in] src  Object to be copied.
+    */
+   ABM4SimpleSecondOrderODEIntegrator (
+      const ABM4SimpleSecondOrderODEIntegrator & src)
+   :
+      Er7UtilsDeletable(),
+      ABM4SecondOrderODEIntegrator(src)
+   {}
+
+   /**
+    * ABM4SimpleSecondOrderODEIntegrator non-default constructor.
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     size                Size of the position, velocity vectors
+    * @param[in,out] controls_in         Integration controls
+    */
+   ABM4SimpleSecondOrderODEIntegrator (
+      const IntegratorConstructor & primer_constructor,
+      unsigned int size,
+      IntegrationControls & controls_in)
+   :
+      Er7UtilsDeletable(),
+      ABM4SecondOrderODEIntegrator(primer_constructor, size, controls_in)
+   {}
+
+   /**
+    * ABM4SimpleSecondOrderODEIntegrator destructor.
+    */
+   virtual ~ABM4SimpleSecondOrderODEIntegrator (void)
+   {}
+
+
+   // Member functions.
+
+   /**
+    * ABM4SimpleSecondOrderODEIntegrator assignment operator.
+    * @param[in] src  Object to be copied.
+    */
+   ABM4SimpleSecondOrderODEIntegrator & operator= (
+      ABM4SimpleSecondOrderODEIntegrator src)
+   {
+      swap (src);
+      return *this;
+   }
+
+   /**
+    * Create a copy of 'this' ABM4SimpleSecondOrderODEIntegrator object.
+    * @return Clone of 'this'.
+    */
+   virtual ABM4SimpleSecondOrderODEIntegrator * create_copy () const;
+
+
+protected:
+
+   /**
+    * Save derivatives, called during priming.
+    * @param countdown  The priming countdown.
+    * @param accel      Time derivative of the generalized velocity.
+    * @param velocity   Generalized velocity vector.
+    * @param position   Generalized position vector.
+    */
+   virtual void technique_save_derivatives (
+      int countdown,
+      double const * ER7_UTILS_RESTRICT accel,
+      double const * ER7_UTILS_RESTRICT velocity,
+      double const * ER7_UTILS_RESTRICT position);
+
+   /**
+    * Propagate state using fourth order Adams Bashforth Moulton,
+    * excluding priming.
+    * @param[in]     dyn_dt        Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     accel         Time derivative of the generalized velocity.
+    * @param[in,out] velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult technique_integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      double const * ER7_UTILS_RESTRICT accel,
+      double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position);
+};
+
+
+/**
+ * Specialization of ABM4SecondOrderODEIntegrator for the case of
+ * the time derivative of generalized position being some function of
+ * the generalized position and generalized velocity.
+ */
+class ABM4GeneralizedDerivSecondOrderODEIntegrator :
+   public ABM4SecondOrderODEIntegrator {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(ABM4GeneralizedDerivSecondOrderODEIntegrator)
+
+public:
+
+   // Constructors and destructor.
+
+   /**
+    * ABM4GeneralizedDerivSecondOrderODEIntegrator default constructor.
+    */
+   ABM4GeneralizedDerivSecondOrderODEIntegrator (void)
+   :
+      Er7UtilsDeletable(),
+      ABM4SecondOrderODEIntegrator()
+   {}
+
+   /**
+    * ABM4GeneralizedDerivSecondOrderODEIntegrator copy constructor.
+    * @param[in] src  Object to be copied.
+    */
+   ABM4GeneralizedDerivSecondOrderODEIntegrator (
+      const ABM4GeneralizedDerivSecondOrderODEIntegrator & src)
+   :
+      Er7UtilsDeletable(),
+      ABM4SecondOrderODEIntegrator(src)
+   {}
+
+   /**
+    * ABM4GeneralizedDerivSecondOrderODEIntegrator non-default constructor.
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     position_size       Size of the generalized position
+    * @param[in]     velocity_size       Size of the generalized velocity
+    * @param[in]     deriv_funs          Position derivative functions container
+    * @param[in,out] controls_in         Integration controls
+    */
+   ABM4GeneralizedDerivSecondOrderODEIntegrator (
+      const IntegratorConstructor & primer_constructor,
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionDerivativeFunctions & deriv_funs,
+      IntegrationControls & controls_in)
+   :
+      Er7UtilsDeletable(),
+      ABM4SecondOrderODEIntegrator (primer_constructor,
+                                    position_size, velocity_size,
+                                    deriv_funs, controls_in)
+   {}
+
+   /**
+    * ABM4GeneralizedDerivSecondOrderODEIntegrator destructor.
+    */
+   virtual ~ABM4GeneralizedDerivSecondOrderODEIntegrator (void)
+   {}
+
+   /**
+    * ABM4GeneralizedDerivSecondOrderODEIntegrator assignment operator.
+    * @param[in] src  Object to be copied.
+    */
+   ABM4GeneralizedDerivSecondOrderODEIntegrator & operator= (
+      ABM4GeneralizedDerivSecondOrderODEIntegrator src)
+   {
+      swap (src);
+      return *this;
+   }
+
+   /**
+    * Create a copy of 'this' ABM4GeneralizedDerivSecondOrderODEIntegrator.
+    * @return Clone of 'this'.
+    */
+   virtual ABM4GeneralizedDerivSecondOrderODEIntegrator * create_copy () const;
+
+
+protected:
+
+   /**
+    * Save derivatives, called during priming.
+    * @param countdown  The priming countdown.
+    * @param accel      Time derivative of the generalized velocity.
+    * @param velocity   Generalized velocity vector.
+    * @param position   Generalized position vector.
+    */
+   virtual void technique_save_derivatives (
+      int countdown,
+      double const * ER7_UTILS_RESTRICT accel,
+      double const * ER7_UTILS_RESTRICT velocity,
+      double const * ER7_UTILS_RESTRICT position);
+
+   /**
+    * Propagate state using fourth order Adams Bashforth Moulton,
+    * excluding priming.
+    * @param[in]     dyn_dt        Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     accel         Time derivative of the generalized velocity.
+    * @param[in,out] velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult technique_integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      double const * ER7_UTILS_RESTRICT accel,
+      double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position);
+};
+
+
+// The current implementation of ABM4GeneralizedStepSecondOrderODEIntegrator
+// is not ready for prime time.
+#if 0
+/**
+ * Specialization of ABM4SecondOrderODEIntegrator for the case of
+ * the generalized position being constrained to lie on a manifold.
+ * The generalized position step function makes a non-linear position step that
+ * keeps the generalized position on the manifold.
+ */
+class ABM4GeneralizedStepSecondOrderODEIntegrator :
+   public ABM4SecondOrderODEIntegrator {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(ABM4GeneralizedStepSecondOrderODEIntegrator)
+
+public:
+
+   // Constructors and destructor.
+
+   // Default constructor, exists because the memory manager needs one.
+   ABM4GeneralizedStepSecondOrderODEIntegrator (void)
+   :
+      ABM4SecondOrderODEIntegrator()
+   {}
+
+   /**
+    * ABM4GeneralizedStepSecondOrderODEIntegrator non-default constructor.
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     position_size       Size of the generalized position
+    * @param[in]     velocity_size       Size of the generalized velocity
+    * @param[in]     step_funs           Position step functions container
+    * @param[in,out] controls_in         Integration controls
+    */
+   ABM4GeneralizedStepSecondOrderODEIntegrator (
+      const IntegratorConstructor & primer_constructor,
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionStepFunctions & step_funs,
+      IntegrationControls & controls_in)
+   :
+      ABM4SecondOrderODEIntegrator (primer_constructor,
+                                    position_size, velocity_size,
+                                    step_funs, controls_in)
+   {}
+
+   /**
+    * ABM4GeneralizedStepSecondOrderODEIntegrator destructor.
+    */
+   virtual ~ABM4GeneralizedStepSecondOrderODEIntegrator (void)
+   {}
+
+
+protected:
+
+   /**
+    * Save derivatives, called during priming.
+    * @param countdown  The priming countdown.
+    * @param accel      Time derivative of the generalized velocity.
+    * @param velocity   Generalized velocity vector.
+    * @param position   Generalized position vector.
+    */
+   virtual void technique_save_derivatives (
+      int countdown,
+      double const * ER7_UTILS_RESTRICT accel,
+      double const * ER7_UTILS_RESTRICT velocity,
+      double const * ER7_UTILS_RESTRICT position);
+
+   /**
+    * Propagate state using fourth order Adams Bashforth Moulton,
+    * excluding priming.
+    * @param[in]     dyn_dt        Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     accel         Time derivative of the generalized velocity.
+    * @param[in,out] velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult technique_integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      double const * ER7_UTILS_RESTRICT accel,
+      double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position);
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   ABM4GeneralizedStepSecondOrderODEIntegrator (
+      const ABM4GeneralizedStepSecondOrderODEIntegrator &);
+
+   /**
+    * Not implemented.
+    */
+   ABM4GeneralizedStepSecondOrderODEIntegrator & operator= (
+      const ABM4GeneralizedStepSecondOrderODEIntegrator &);
+};
+#endif
+
+}
+
+
+#ifdef ER7_UTILS_NEED_AUX_INCLUDES
+#include "er7_utils/integration/core/include/integration_controls.hh"
+#endif
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/abm4/src/abm4_first_order_ode_integrator.cc

@@ -0,0 +1,202 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the class ABM4FirstOrderODEIntegrator.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+#include <algorithm>
+#include <cstddef>
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/abm_utils.hh"
+#include "er7_utils/integration/core/include/integ_utils.hh"
+#include "er7_utils/integration/core/include/integrator_constructor.hh"
+#include "er7_utils/integration/core/include/integration_controls.hh"
+
+// Model includes
+#include "../include/abm4_first_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+// Default constructor.
+ABM4FirstOrderODEIntegrator::ABM4FirstOrderODEIntegrator (
+   void)
+:
+   Er7UtilsDeletable (),
+   PrimingFirstOrderODEIntegrator (),
+   init_state (NULL)
+{
+   alloc::initialize_2D_array<double, 4> (deriv_hist);
+}
+
+// Copy constructor.
+ABM4FirstOrderODEIntegrator::ABM4FirstOrderODEIntegrator (
+   const ABM4FirstOrderODEIntegrator & src)
+:
+   Er7UtilsDeletable (),
+   PrimingFirstOrderODEIntegrator (src),
+   init_state (NULL)
+{
+   // Replicate the source's contents if they exist.
+   if (src.init_state != NULL) {
+      init_state = alloc::replicate_array<double> (state_size, src.init_state);
+      alloc::replicate_2D_array<double, 4> (state_size, src.deriv_hist,
+                                            deriv_hist);
+   }
+   else {
+      alloc::initialize_2D_array<double, 4> (deriv_hist);
+   }
+}
+
+
+// Non-default constructor for a ABM4FirstOrderODEIntegrator.
+ABM4FirstOrderODEIntegrator::ABM4FirstOrderODEIntegrator (
+   const IntegratorConstructor & primer_constructor,
+   unsigned int size,
+   IntegrationControls & controls_in)
+:
+   Er7UtilsDeletable (),
+   PrimingFirstOrderODEIntegrator (4, primer_constructor, size, controls_in),
+   init_state(NULL)
+{
+   // Allocate storage for the ABM method.
+   init_state = alloc::allocate_array<double> (state_size);
+   alloc::allocate_2D_array<double, 4> (state_size, deriv_hist);
+}
+
+
+// ABM4FirstOrderODEIntegrator destructor.
+ABM4FirstOrderODEIntegrator::~ABM4FirstOrderODEIntegrator (
+   void)
+{
+   // Free ABM storage memory.
+   alloc::deallocate_array<double> (init_state);
+   alloc::deallocate_2D_array<double, 4> (deriv_hist);
+}
+
+
+// Clone a ABM4FirstOrderODEIntegrator.
+ABM4FirstOrderODEIntegrator *
+ABM4FirstOrderODEIntegrator::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Non-throwing swap.
+void
+ABM4FirstOrderODEIntegrator::swap (
+   ABM4FirstOrderODEIntegrator & other)
+{
+   PrimingFirstOrderODEIntegrator::swap (other);
+   std::swap (init_state, other.init_state);
+   std::swap (deriv_hist[0], other.deriv_hist[0]);
+   std::swap (deriv_hist[1], other.deriv_hist[1]);
+   std::swap (deriv_hist[2], other.deriv_hist[2]);
+   std::swap (deriv_hist[3], other.deriv_hist[3]);
+}
+
+
+//Save derivatives during priming.
+void
+ABM4FirstOrderODEIntegrator::technique_save_derivatives (
+   int,
+   const double * ER7_UTILS_RESTRICT velocity,
+   const double *)
+{
+   abm::rotate_history<4> (deriv_hist);
+   integ_utils::copy_array (velocity, state_size, deriv_hist[0]);
+}
+
+
+// Integrate state using ABM4 proper.
+IntegratorResult
+ABM4FirstOrderODEIntegrator::technique_integrate (
+   double dt,
+   unsigned int target_stage,
+   const double * ER7_UTILS_RESTRICT velocity,
+   double * ER7_UTILS_RESTRICT position)
+{
+   static const double wscale = 1.0/24.0;
+   static const double predictor_weights[4] = {55.0, -59.0, 37.0, -9.0};
+   static const double corrector_weights[4] = {9.0, 19.0, -5.0, 1.0};
+
+   /**
+    * ### Overview
+    *
+    * ABM4 proper requires four sets of start of cycle derivatives.
+    * This means that ABM4 needs to be "primed" by some other integration
+    * technique. The primer is run through three complete cycles to generate
+    * the requisite four sets.
+    *
+    * ABM4 itself is used once the integrator has been primed. ABM4 integrates
+    * state by using fourth order Adams Bashforth as a predictor and third order
+    * Adams Moulton as a corrector.
+    *
+    * ### Algorithm
+    */
+
+   /**
+    * Integrate state via ABM4 once priming is complete.
+    * Advance state based on the target stage number.
+    */
+   switch (target_stage) {
+
+   /**
+    * - ABM4 target stage 1 advances state to the end of the integration
+    *   interval using fourth order Adams-Bashforth as a predictor.
+    *   + Rotate the derivatives history. This rotation moves the oldest
+    *     set of derivatives to the zeroth position.
+    *   + Integrate state via fourth order Adams Bashforth,
+    *     saving derivatives in the zeroth element of the derivatives
+    *     history.
+    */
+   case 1:
+      abm::rotate_history<4> (deriv_hist);
+
+      abm::predictor_step<4> (velocity, predictor_weights,
+                              wscale, dt, state_size,
+                              deriv_hist, init_state, position);
+      break;
+
+
+   /**
+    * - ABM4 target stage 2 advances the state to the end of the integration
+    *   interval using the third order Adams-Moulton method as a corrector.
+    */
+   case 2:
+
+      abm::corrector_step<4> (init_state, velocity, deriv_hist,
+                              corrector_weights, wscale, dt, state_size,
+                              position);
+      break;
+   }
+
+   return 1.0;
+}
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/abm4/src/abm4_integrator_constructor.cc

@@ -0,0 +1,133 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the class ABM4IntegratorConstructor.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/integrator_constructor_utils.hh"
+#include "er7_utils/integration/core/include/priming_integration_controls.hh"
+#include "er7_utils/integration/rk4/include/rk4_integrator_constructor.hh"
+
+// Model includes
+#include "../include/abm4_integrator_constructor.hh"
+#include "../include/abm4_first_order_ode_integrator.hh"
+#include "../include/abm4_second_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+// Named constructor; create an ABM4IntegratorConstructor.
+IntegratorConstructor*
+ABM4IntegratorConstructor::create_constructor (
+   void)
+{
+   return alloc::allocate_object<ABM4IntegratorConstructor> ();
+}
+
+
+// Create a duplicate of the constructor.
+IntegratorConstructor *
+ABM4IntegratorConstructor::create_copy (
+   void)
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Create an ABM4 integration controls.
+IntegrationControls *
+ABM4IntegratorConstructor::create_integration_controls (
+   void)
+const
+{
+   return integ_utils::allocate_controls<PrimingIntegrationControls> (
+             *primer_constructor, 4, 2);
+}
+
+
+// Create an ABM4 state integrator for a first order ODE.
+FirstOrderODEIntegrator *
+ABM4IntegratorConstructor::create_first_order_ode_integrator (
+   unsigned int size,
+   IntegrationControls & controls)
+const
+{
+   return integ_utils::allocate_integrator<ABM4FirstOrderODEIntegrator> (
+             *primer_constructor, size, controls);
+}
+
+
+// Create an ABM4 state integrator for a second order ODE.
+SecondOrderODEIntegrator *
+ABM4IntegratorConstructor::create_second_order_ode_integrator (
+   unsigned int size,
+   IntegrationControls & controls)
+const
+{
+   return integ_utils::allocate_integrator<ABM4SimpleSecondOrderODEIntegrator> (
+             *primer_constructor, size, controls);
+}
+
+
+// Create an ABM4 state integrator for a second order ODE.
+SecondOrderODEIntegrator *
+ABM4IntegratorConstructor::
+create_generalized_deriv_second_order_ode_integrator (
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionDerivativeFunctions & deriv_funs,
+   IntegrationControls & controls)
+const
+{
+   return integ_utils::allocate_integrator<
+                ABM4GeneralizedDerivSecondOrderODEIntegrator> (
+             *primer_constructor,
+             position_size, velocity_size, deriv_funs, controls);
+}
+
+
+#if 0
+// Create an ABM4 state integrator for a second order ODE.
+SecondOrderODEIntegrator *
+ABM4IntegratorConstructor::
+create_generalized_step_second_order_ode_integrator (
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionStepFunctions & step_funs,
+   IntegrationControls & controls)
+const
+{
+   return integ_utils::allocate_integrator<
+                ABM4GeneralizedStepSecondOrderODEIntegrator> (
+             *primer_constructor,
+             position_size, velocity_size, step_funs, controls);
+}
+#endif
+
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/abm4/src/abm4_second_order_ode_integrator.cc

@@ -0,0 +1,516 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the class ABM4SecondOrderODEIntegrator.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+#include <algorithm>
+#include <cstddef>
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/abm_utils.hh"
+#include "er7_utils/integration/core/include/integ_utils.hh"
+#include "er7_utils/integration/core/include/integrator_constructor.hh"
+#include "er7_utils/integration/core/include/integration_controls.hh"
+
+// Model includes
+#include "../include/abm4_second_order_ode_integrator.hh"
+
+
+namespace {
+
+   /**
+    * Scale factor on weights so that the weights can be integral.
+    */
+   const double wscale = 1.0/24.0;
+
+   /**
+    * Fourth order Adams Bashforth weights.
+    */
+   const double predictor_weights[4] = {55.0, -59.0, 37.0, -9.0};
+
+   /**
+    * Fourth order Adams Bashforth weights, scaled.
+    */
+   const double scaled_predictor_weights[4] = {
+      55.0/24.0, -59.0/24.0, 37.0/24.0, -9.0/24.0};
+
+   /**
+    * Third order Adams Moulton weights.
+    */
+   const double corrector_weights[4] = { 9.0,  19.0, -5.0,  1.0};
+
+   /**
+    * Third order Adams Moulton weights, scaled.
+    */
+   const double scaled_corrector_weights[4] = {
+      9.0/24.0,  19.0/24.0, -5.0/24.0,  1.0/24.0};
+}
+
+
+namespace er7_utils {
+
+
+
+// ABM4SecondOrderODEIntegrator default constructor.
+ABM4SecondOrderODEIntegrator::ABM4SecondOrderODEIntegrator (
+   void)
+:
+   Er7UtilsDeletable (),
+   PrimingSecondOrderODEIntegrator (),
+   init_pos (NULL),
+   init_vel (NULL),
+   posdot (NULL),
+   posdot_hist_anchor (NULL),
+   veldot_hist_anchor (NULL)
+#if 0
+   dtheta (NULL)
+#endif
+{
+   // Initialize memory used by ABM4 algorithm.
+   alloc::initialize_2D_array<double, 4> (posdot_hist);
+   posdot_hist_anchor = posdot_hist[0];
+   alloc::initialize_2D_array<double, 4> (veldot_hist);
+   veldot_hist_anchor = veldot_hist[0];
+}
+
+
+// Copy constructor.
+ABM4SecondOrderODEIntegrator::ABM4SecondOrderODEIntegrator (
+   const ABM4SecondOrderODEIntegrator & src)
+:
+   Er7UtilsDeletable (),
+   PrimingSecondOrderODEIntegrator (src),
+   init_pos (NULL),
+   init_vel (NULL),
+   posdot (NULL),
+   posdot_hist_anchor (NULL),
+   veldot_hist_anchor (NULL)
+#if 0
+   dtheta (NULL)
+#endif
+{
+   // Replicate the source's contents if they exist.
+#if 0
+   if (problem_type == Integration::GeneralizedStepSecondOrderODE) {
+      // Fill this in if/when this technique is revitalized.
+   }
+   else
+#endif
+   if (problem_type == Integration::GeneralizedDerivSecondOrderODE) {
+      init_pos = alloc::replicate_array (state_size[0], src.init_pos);
+      init_vel = alloc::replicate_array (state_size[1], src.init_vel);
+      posdot = alloc::replicate_array (state_size[0], src.posdot);
+      alloc::replicate_2D_array<double, 4> (state_size[0], src.posdot_hist,
+                                            posdot_hist);
+      posdot_hist_anchor = posdot_hist[0];
+      alloc::replicate_2D_array<double, 4> (state_size[1], src.veldot_hist,
+                                            veldot_hist);
+      veldot_hist_anchor = veldot_hist[0];
+   }
+
+   else if (src.init_pos != NULL) {
+      init_pos = alloc::replicate_array (state_size[0], src.init_pos);
+      init_vel = alloc::replicate_array (state_size[1], src.init_vel);
+      alloc::replicate_2D_array<double, 4> (state_size[0], src.posdot_hist,
+                                            posdot_hist);
+      posdot_hist_anchor = posdot_hist[0];
+      alloc::replicate_2D_array<double, 4> (state_size[1], src.veldot_hist,
+                                            veldot_hist);
+      veldot_hist_anchor = veldot_hist[0];
+   }
+
+   else {
+      alloc::initialize_2D_array<double, 4> (posdot_hist);
+      alloc::initialize_2D_array<double, 4> (veldot_hist);
+   }
+}
+
+
+
+// ABM4SecondOrderODEIntegrator non-default constructor,
+// simple second order ODE.
+ABM4SecondOrderODEIntegrator::ABM4SecondOrderODEIntegrator (
+   const IntegratorConstructor & primer_constructor,
+   unsigned int size,
+   IntegrationControls & controls_in)
+:
+   Er7UtilsDeletable (),
+   PrimingSecondOrderODEIntegrator (4, primer_constructor, size, controls_in),
+   init_pos (NULL),
+   init_vel (NULL),
+   posdot (NULL),
+   posdot_hist_anchor (NULL),
+   veldot_hist_anchor (NULL)
+#if 0
+   dtheta (NULL)
+#endif
+{
+   // Allocate memory used by ABM4.
+   init_pos = alloc::allocate_array<double> (size);
+   init_vel = alloc::allocate_array<double> (size);
+   alloc::allocate_2D_array<double, 4> (size, posdot_hist);
+   posdot_hist_anchor = posdot_hist[0];
+   alloc::allocate_2D_array<double, 4> (size, veldot_hist);
+   veldot_hist_anchor = veldot_hist[0];
+}
+
+
+// ABM4SecondOrderODEIntegrator non-default constructor,
+// generalized second order ODE with position advanced internally.
+ABM4SecondOrderODEIntegrator::ABM4SecondOrderODEIntegrator (
+   const IntegratorConstructor & primer_constructor,
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionDerivativeFunctions & deriv_funs,
+   IntegrationControls & controls_in)
+:
+   Er7UtilsDeletable (),
+   PrimingSecondOrderODEIntegrator (4, primer_constructor,
+                                    position_size, velocity_size,
+                                    deriv_funs, controls_in),
+   init_pos (NULL),
+   init_vel (NULL),
+   posdot (NULL),
+   posdot_hist_anchor (NULL),
+veldot_hist_anchor (NULL)
+#if 0
+   dtheta (NULL)
+#endif
+{
+   // Allocate memory used by ABM4.
+   init_pos = alloc::allocate_array<double> (position_size);
+   init_vel = alloc::allocate_array<double> (velocity_size);
+   posdot = alloc::allocate_array<double> (position_size);
+   alloc::allocate_2D_array<double, 4> (position_size, posdot_hist);
+   posdot_hist_anchor = posdot_hist[0];
+   alloc::allocate_2D_array<double, 4> (velocity_size, veldot_hist);
+   veldot_hist_anchor = veldot_hist[0];
+}
+
+
+#if 0
+// ABM4SecondOrderODEIntegrator non-default constructor,
+// generalized second order ODE with position advanced externally.
+ABM4SecondOrderODEIntegrator::ABM4SecondOrderODEIntegrator (
+   const IntegratorConstructor & primer_constructor,
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionStepFunctions & step_funs,
+   IntegrationControls & controls_in)
+:
+   PrimingSecondOrderODEIntegrator (4, primer_constructor,
+                                    position_size, velocity_size,
+                                    step_funs, controls_in),
+   init_pos (NULL),
+   init_vel (NULL),
+   posdot (NULL),
+   posdot_hist_anchor (NULL),
+   veldot_hist_anchor (NULL),
+   dtheta (NULL)
+{
+   // Allocate memory used by ABM4.
+   init_pos = alloc::allocate_array<double> (position_size);
+   init_vel = alloc::allocate_array<double> (velocity_size);
+   posdot = alloc::allocate_array<double> (position_size);
+   dtheta = alloc::allocate_array<double> (position_size);
+   alloc::allocate_2D_array<double, 4> (position_size, posdot_hist);
+   posdot_hist_anchor = posdot_hist[0];
+   alloc::allocate_2D_array<double, 4> (velocity_size, veldot_hist);
+   veldot_hist_anchor = veldot_hist[0];
+}
+#endif
+
+
+// ABM4SecondOrderODEIntegrator destructor.
+ABM4SecondOrderODEIntegrator::~ABM4SecondOrderODEIntegrator (
+   void)
+{
+   alloc::deallocate_array<double> (init_pos);
+   alloc::deallocate_array<double> (init_vel);
+   alloc::deallocate_array<double> (posdot);
+#if 0
+   alloc::deallocate_array<double> (dtheta);
+#endif
+   // Restore the cached pointers so that the arrays can be deleted properly.
+   posdot_hist[0] = posdot_hist_anchor;
+   veldot_hist[0] = veldot_hist_anchor;
+   alloc::deallocate_2D_array<double, 4> (posdot_hist);
+   alloc::deallocate_2D_array<double, 4> (veldot_hist);
+}
+
+
+// Non-throwing swap.
+void
+ABM4SecondOrderODEIntegrator::swap (
+   ABM4SecondOrderODEIntegrator & other)
+{
+   PrimingSecondOrderODEIntegrator::swap (other);
+
+   std::swap (init_pos, other.init_pos);
+   std::swap (init_vel, other.init_vel);
+   std::swap (posdot, other.posdot);
+#if 0
+   std::swap (dtheta, other.dtheta);
+#endif
+   for (int ii = 0; ii < 4; ++ii) {
+      std::swap (posdot_hist[ii], other.posdot_hist[ii]);
+      std::swap (veldot_hist[ii], other.veldot_hist[ii]);
+   }
+      std::swap (posdot_hist_anchor, other.posdot_hist_anchor);
+      std::swap (veldot_hist_anchor, other.veldot_hist_anchor);
+}
+
+
+// Clone a ABM4SimpleSecondOrderODEIntegrator.
+ABM4SimpleSecondOrderODEIntegrator *
+ABM4SimpleSecondOrderODEIntegrator::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Clone a ABM4GeneralizedDerivSecondOrderODEIntegrator.
+ABM4GeneralizedDerivSecondOrderODEIntegrator *
+ABM4GeneralizedDerivSecondOrderODEIntegrator::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Save derivatives during priming
+void
+ABM4SimpleSecondOrderODEIntegrator::technique_save_derivatives (
+   int,
+   double const * ER7_UTILS_RESTRICT accel,
+   double const * ER7_UTILS_RESTRICT velocity,
+   double const *)
+{
+   abm::rotate_history<4> (posdot_hist, veldot_hist);
+   integ_utils::two_state_copy_array (
+      velocity, accel, state_size[0],
+      posdot_hist[0], veldot_hist[0]);
+}
+
+
+// Propagate state via ABM4, simple second order ODE.
+IntegratorResult
+ABM4SimpleSecondOrderODEIntegrator::technique_integrate (
+   double dyn_dt,
+   unsigned int target_stage,
+   double const * ER7_UTILS_RESTRICT accel,
+   double * ER7_UTILS_RESTRICT velocity,
+   double * ER7_UTILS_RESTRICT position)
+{
+   switch (target_stage) {
+
+   // Target stage 1 represents the ABM4 predictor stage.
+   // Save derivatives in the circular derivative buffer and advance state
+   // propagate state to the end of the integration cycle using the
+   // fourth order Adams-Bashforth method as a predictor.
+   case 1:
+      // Rotate the history so that what was the oldest set of saved
+      // derivatives will be overwritten with the incoming derivatives.
+      abm::rotate_history<4> (posdot_hist, veldot_hist);
+
+      // Perform the predictor step, saving derivatives.
+      abm::predictor_step<4> (accel,
+                              predictor_weights, wscale,
+                              dyn_dt, state_size[0],
+                              posdot_hist, veldot_hist,
+                              init_pos, init_vel,
+                              position, velocity);
+      break;
+
+   // Target stage 2 represents the ABM4 corrector stage.
+   // Advance state using third order Adams-Moulton method as a corrector.
+   case 2:
+      // Perform the corrector step.
+      abm::corrector_step<4> (init_pos, init_vel, accel,
+                              posdot_hist, veldot_hist,
+                              corrector_weights, wscale,
+                              dyn_dt, state_size[0],
+                              position, velocity);
+      break;
+   }
+
+   return 1.0;
+}
+
+
+// Save derivatives during priming
+void
+ABM4GeneralizedDerivSecondOrderODEIntegrator::technique_save_derivatives (
+   int,
+   double const * ER7_UTILS_RESTRICT accel,
+   double const * ER7_UTILS_RESTRICT velocity,
+   double const * ER7_UTILS_RESTRICT position)
+{
+   abm::rotate_history<4> (posdot_hist, veldot_hist);
+
+   // Compute time derivative of canonical position,
+   // saving the result in the position derivative history.
+   compute_posdot (position, velocity, posdot_hist[0]);
+
+   // Save the acceleration in the velocity derivative history.
+   integ_utils::copy_array (accel, state_size[1], veldot_hist[0]);
+}
+
+
+// Propagate state for the general case of the generalized position derivative
+// being a function of generalized position and generalized velocity.
+IntegratorResult
+ABM4GeneralizedDerivSecondOrderODEIntegrator::technique_integrate (
+   double dyn_dt,
+   unsigned int target_stage,
+   double const * ER7_UTILS_RESTRICT accel,
+   double * ER7_UTILS_RESTRICT velocity,
+   double * ER7_UTILS_RESTRICT position)
+{
+   switch (target_stage) {
+   // Target stage 1 represents the ABM4 predictor stage.
+   // Save derivatives in the circular derivative buffer and advance state
+   // propagate state to the end of the integration cycle using the
+   // fourth order Adams-Bashforth method as a predictor.
+   case 1:
+      // Rotate the history.
+      abm::rotate_history<4> (posdot_hist, veldot_hist);
+
+      // Compute the time derivative of the generalized position.
+      compute_posdot (position, velocity, posdot);
+
+      // Perform the predictor step.
+      abm::predictor_step<4> (posdot, accel,
+                              predictor_weights, wscale, dyn_dt, state_size,
+                              posdot_hist, veldot_hist,
+                              init_pos, init_vel,
+                              position, velocity);
+      break;
+
+   // Target stage 2 represents the ABM4 corrector stage.
+   // Advance state using third order Adams-Moulton method as a corrector.
+   case 2:
+      // Compute the time derivative of the generalized position.
+      compute_posdot (position, velocity, posdot);
+
+      // Perform the corrector step.
+      abm::corrector_step<4> (init_pos, init_vel, posdot, accel,
+                              posdot_hist, veldot_hist,
+                              corrector_weights, wscale,
+                              dyn_dt, state_size,
+                              position, velocity);
+      break;
+   }
+
+   return 1.0;
+}
+
+
+#if 0
+// Save derivatives during priming
+void
+ABM4GeneralizedStepSecondOrderODEIntegrator::technique_save_derivatives (
+   int,
+   double const * ER7_UTILS_RESTRICT accel,
+   double const * ER7_UTILS_RESTRICT velocity,
+   double const * ER7_UTILS_RESTRICT position)
+{
+   abm::rotate_history<4> (posdot_hist, veldot_hist);
+   integ_utils::two_state_copy_array (
+      velocity, accel, state_size[1],
+      posdot_hist[0], veldot_hist[0]);
+}
+
+
+// Propagate state for the general case of the generalized position derivative
+// being a function of generalized position and generalized velocity.
+IntegratorResult
+ABM4GeneralizedStepSecondOrderODEIntegrator::technique_integrate (
+   double dyn_dt,
+   unsigned int target_stage,
+   double const * ER7_UTILS_RESTRICT accel,
+   double * ER7_UTILS_RESTRICT velocity,
+   double * ER7_UTILS_RESTRICT position)
+{
+   int vel_size = state_size[1];
+
+   switch (target_stage) {
+   // Target stage 1 represents the ABM4 predictor stage.
+   // Save derivatives in the circular derivative buffer and advance state
+   // propagate state to the end of the integration cycle using the
+   // fourth order Adams-Bashforth method as a predictor.
+   case 1:
+      // Rotate the history.
+      abm::rotate_history<4> (posdot_hist, veldot_hist);
+
+      // Save the initial position and current velocity.
+      integ_utils::copy_array (position, state_size[0], init_pos);
+      integ_utils::copy_array (velocity, vel_size, posdot_hist[0]);
+
+      // Update position via Crouch-Grossman.
+      for (int ii = 3; ii >= 0; --ii) {
+         integ_utils::scale_array (
+            posdot_hist[ii], scaled_predictor_weights[ii]*dyn_dt, vel_size,
+            dtheta);
+         compute_expmap_position_step (position, dtheta, position);
+      }
+
+      // Update velocity.
+      abm::predictor_step<4> (accel, predictor_weights,
+                              wscale*dyn_dt, vel_size,
+                              veldot_hist, init_vel, velocity);
+      break;
+
+   // Target stage 2 represents the ABM4 corrector stage.
+   // Advance state using third order Adams-Moulton method as a corrector.
+   case 2:
+      // Update position via Crouch-Grossman.
+      integ_utils::scale_array (
+         posdot_hist[2], scaled_corrector_weights[3]*dyn_dt, vel_size, dtheta);
+      compute_expmap_position_step (init_pos, dtheta, position);
+      integ_utils::scale_array (
+          posdot_hist[1], scaled_corrector_weights[2]*dyn_dt, vel_size, dtheta);
+      compute_expmap_position_step (position, dtheta, position);
+      integ_utils::scale_array (
+          posdot_hist[0], scaled_corrector_weights[1]*dyn_dt, vel_size, dtheta);
+      compute_expmap_position_step (position, dtheta, position);
+      integ_utils::scale_array (
+          velocity, scaled_corrector_weights[0]*dyn_dt, vel_size, dtheta);
+      compute_expmap_position_step (position, dtheta, position);
+
+      // Update velocity.
+      abm::corrector_step<4> (init_vel, accel, veldot_hist, corrector_weights,
+                              wscale*dyn_dt,  vel_size,
+                              velocity);
+      break;
+   }
+
+   return 1.0;
+}
+#endif
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/beeman/include/beeman_integrator_constructor.hh

@@ -0,0 +1,228 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class BeemanIntegratorConstructor, which constructs integrators
+ * that use Beeman's method.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_BEEMAN_INTEGRATOR_CONSTRUCTOR_HH
+#define ER7_UTILS_BEEMAN_INTEGRATOR_CONSTRUCTOR_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/priming_integrator_constructor.hh"
+#include "er7_utils/integration/rk2_heun/include/rk2_heun_integrator_constructor.hh"
+
+
+namespace er7_utils {
+
+/**
+ * Create state and time integrators that propagate using Beeman's method.
+ */
+class BeemanIntegratorConstructor : public PrimingIntegratorConstructor {
+ER7_UTILS_MAKE_SIM_INTERFACES(BeemanIntegratorConstructor)
+
+public:
+   // Static member functions.
+
+   /**
+    * Named constructor; create a BeemanIntegratorConstructor instance.
+    * The caller is responsible for deleting the returned object.
+    * @return Newly created BeemanIntegratorConstructor instance.
+    */
+   static IntegratorConstructor* create_constructor (void);
+
+
+   // Constructors and assignment operator
+   // Note that the destructor is not declared.
+   // The implicitly defined destructor is correct.
+
+   /**
+    * Beeman default constructor.
+    * Note that by default Beeman uses Heun's method for priming.
+    */
+   BeemanIntegratorConstructor (void)
+   :
+      Er7UtilsDeletable (),
+      PrimingIntegratorConstructor (RK2HeunIntegratorConstructor ())
+   {}
+
+   /**
+    * BeemanIntegratorConstructor copy constructor.
+    */
+   BeemanIntegratorConstructor (const BeemanIntegratorConstructor & src)
+   :
+      Er7UtilsDeletable (),
+      PrimingIntegratorConstructor (src)
+   {}
+
+   /**
+    * BeemanIntegratorConstructor assignment operator.
+    */
+   BeemanIntegratorConstructor & operator= (BeemanIntegratorConstructor src)
+   {
+      swap (src);
+      return *this;
+   }
+
+
+   // Member functions.
+
+   /**
+    * Return the class name.
+    */
+   virtual const char * get_class_name (void) const
+   { return "BeemanIntegratorConstructor"; }
+
+   /**
+    * Beeman is a second order technique; it provides but does not implement
+    * a first order ODE integrator. It currently does not implement or provide
+    * a second order generalized step integrator.
+    */
+   virtual bool implements (Integration::ODEProblemType problem_type) const
+   {
+      return (problem_type != Integration::FirstOrderODE) &&
+             (problem_type != Integration::GeneralizedStepSecondOrderODE);
+   }
+
+   /**
+    * Beeman currently does not provide a second order generalized step
+    * integrator.
+    */
+   virtual bool provides (Integration::ODEProblemType problem_type) const
+   {
+      return (problem_type != Integration::GeneralizedStepSecondOrderODE);
+   }
+
+   /**
+    * Create a duplicate of the constructor.
+    * The caller is responsible for deleting the returned object.
+    * @return Duplicated constructor.
+    */
+   virtual IntegratorConstructor * create_copy (void) const;
+
+   /**
+    * Create an integration controls that guides the Beeman integration process.
+    * The caller is responsible for deleting the created object.
+    * @return Integration controls object
+    */
+   virtual IntegrationControls * create_integration_controls (void) const;
+
+   /**
+    * Create a Beeman state integrator for a first order ODE.
+    * The caller is responsible for deleting the created object.
+    * @return State integrator
+    * @param[in]     size      State size
+    * @param[in,out] controls  Integration controls
+    */
+   virtual FirstOrderODEIntegrator * create_first_order_ode_integrator (
+      unsigned int size,
+      IntegrationControls & controls) const;
+
+   /**
+    * Create a Beeman state integrator for a simple second order ODE.
+    * The caller is responsible for deleting the created object.
+    * @return State integrator
+    * @param[in]     size      State size
+    * @param[in,out] controls  Integration controls
+    */
+   virtual SecondOrderODEIntegrator * create_second_order_ode_integrator (
+      unsigned int size,
+      IntegrationControls & controls) const;
+
+   /**
+    * Create a Beeman state integrator for a generalized second order ODE
+    * where generalized position is advanced with the use of the
+    * position derivative function.
+    * The caller is responsible for deleting the created object.
+    * @return State integrator
+    * @param[in]     position_size  Size of the generalized position
+    * @param[in]     velocity_size  Size of the generalized velocity
+    * @param[in]     deriv_funs     Position derivative functions container
+    * @param[in,out] controls       Integration controls
+    */
+   virtual SecondOrderODEIntegrator *
+   create_generalized_deriv_second_order_ode_integrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionDerivativeFunctions & deriv_funs,
+      IntegrationControls & controls) const;
+
+#if 0
+   /**
+    * Create a Beeman state integrator for a generalized second order ODE
+    * where generalized position is advanced with the use of the
+    * position step function.
+    * The caller is responsible for deleting the created object.
+    * @return State integrator
+    * @param[in]     position_size  Size of the generalized position
+    * @param[in]     velocity_size  Size of the generalized velocity
+    * @param[in]     step_funs      Position step functions container
+    * @param[in,out] controls       Integration controls
+    */
+   virtual SecondOrderODEIntegrator *
+   create_generalized_step_second_order_ode_integrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionStepFunctions & step_funs,
+      IntegrationControls & controls) const;
+#endif
+
+
+   /**
+    * Indicate whether the integration technique explicitly solves a second
+    * order ODE. Beeman's method is such a technique.
+    * @return Always returns true.
+    */
+   virtual bool is_second_order_ode_technique (void) const
+   { return true; }
+
+   /**
+    * Beeman uses two steps per cycle once primed.
+    * @return Always returns 2.
+    */
+   virtual unsigned int get_transition_table_size (void) const
+   { return 2; }
+
+   /**
+    * Beeman needs a history of two derivatives.
+    * @return Always returns 2.
+    */
+   virtual unsigned int get_history_length (void) const
+   { return 2; }
+};
+
+}
+
+
+#ifdef ER7_UTILS_NEED_AUX_INCLUDES
+#include "er7_utils/integration/core/include/priming_integration_controls.hh"
+#include "er7_utils/integration/rk2_heun/include/rk2_heun_first_order_ode_integrator.hh"
+#include "beeman_second_order_ode_integrator.hh"
+#endif
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/beeman/include/beeman_second_order_ode_integrator.hh

@@ -0,0 +1,513 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class BeemanSecondOrderODEIntegrator, which integrates a state
+ * comprising a zeroth derivative / first derivative pair via Beeman's method.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_BEEMAN_TWO_STATE_INTEGRATOR_HH
+#define ER7_UTILS_BEEMAN_TWO_STATE_INTEGRATOR_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/priming_second_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+class IntegratorConstructor;
+
+/**
+ * Advance state using Beeman's method.
+ * This includes
+ *   - Initial priming of the recent history of state derivatives,
+ *   - Using Beeman's method once primed, and
+ *   - Clearing the history when needed (e.g., time change or state change).
+ */
+class BeemanSecondOrderODEIntegrator : public PrimingSecondOrderODEIntegrator {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(BeemanSecondOrderODEIntegrator)
+
+
+   // Note:
+   //  This is an abstract class.
+   //  - The constructors for this class are protected.
+   //  - The assignment operator for this class is private / unimplemented.
+
+
+public:
+
+   /**
+    * BeemanSecondOrderODEIntegrator destructor.
+    */
+   virtual ~BeemanSecondOrderODEIntegrator (void);
+
+
+protected:
+
+   // Constructors.
+
+   /**
+    * BeemanSecondOrderODEIntegrator default constructor.
+    * Notes:
+    *  - This implicitly assumes the time derivative of position is velocity.
+    *  - This is needed so that Trick can checkpoint/restart its integrators.
+    */
+   BeemanSecondOrderODEIntegrator (void);
+
+   /**
+    * BeemanSecondOrderODEIntegrator copy constructor.
+    * @param[in] src  Object to be copied.
+    */
+   BeemanSecondOrderODEIntegrator (const BeemanSecondOrderODEIntegrator & src);
+
+   /**
+    * BeemanSecondOrderODEIntegrator non-default constructor
+    * for a simple second order ODE.
+    * This constructor is used by the integrator constructor.
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     size                State size
+    * @param[in,out] controls            Integration controls
+    */
+   BeemanSecondOrderODEIntegrator (
+      const IntegratorConstructor & primer_constructor,
+      unsigned int size,
+      IntegrationControls & controls);
+
+   /**
+    * BeemanSecondOrderODEIntegrator non-default constructor
+    * for a generalized second order ODE in which position is advanced
+    * using the position derivative function.
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     position_size       Size of the generalized position
+    * @param[in]     velocity_size       Size of the generalized velocity
+    * @param[in]     deriv_funs          Position derivative functions container
+    * @param[in,out] controls            Integration controls
+    */
+   BeemanSecondOrderODEIntegrator (
+      const IntegratorConstructor & primer_constructor,
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionDerivativeFunctions & deriv_funs,
+      IntegrationControls & controls);
+
+// The implementation of BeemanGeneralizedStepSecondOrderODEIntegrator
+// is not ready for prime time.
+#if 0
+   /**
+    * BeemanSecondOrderODEIntegrator non-default constructor
+    * for a generalized second order ODE in which position is advanced
+    * using the position step function.
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     position_size       Size of the generalized position
+    * @param[in]     velocity_size       Size of the generalized velocity
+    * @param[in]     step_funs           Position step functions container
+    * @param[in,out] controls            Integration controls
+    */
+   BeemanSecondOrderODEIntegrator (
+      const IntegratorConstructor & primer_constructor,
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionStepFunctions & step_funs,
+      IntegrationControls & controls);
+#endif
+
+
+   // Member functions.
+
+   /**
+    * Non-throwing swap.
+    * @param other  Item with which contents are to be swapped.
+    */
+   void swap (BeemanSecondOrderODEIntegrator & other);
+
+   using PrimingSecondOrderODEIntegrator::swap;
+
+
+   // Member data.
+
+   double * init_vel; /**< trick_units(--) @n
+      Velocity at the start of an integration cycle. */
+
+   double * mean_vel; /**< trick_units(--) @n
+      Velocity used to update position. */
+
+   double * init_acc; /**< trick_units(--) @n
+      Acceleration at the start of an integration cycle. */
+
+   double * prev_acc; /**< trick_units(--) @n
+      Acceleration at the start of the previous integration cycle. */
+
+   double * posdot; /**< trick_units(--) @n
+      Position derivative at the current time step. */
+
+   double * posddot; /**< trick_units(--) @n
+      Position 2nd derivative at the current time step. */
+
+   double * init_posddot; /**< trick_units(--) @n
+      Position 2nd derivative at the start of an integration cycle. */
+
+   double * prev_posddot; /**< trick_units(--) @n
+      Position 2nd derivative at the start of the previous integration cycle. */
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   BeemanSecondOrderODEIntegrator & operator= (
+      const BeemanSecondOrderODEIntegrator &);
+};
+
+
+/**
+ * Specialization of BeemanSecondOrderODEIntegrator for the case of
+ * generalized velocity being the time derivative of generalized position.
+ */
+class BeemanSimpleSecondOrderODEIntegrator :
+   public BeemanSecondOrderODEIntegrator {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(BeemanSimpleSecondOrderODEIntegrator)
+
+public:
+
+   // Constructors and destructor.
+
+   /**
+    * BeemanSimpleSecondOrderODEIntegrator default constructor.
+    */
+   BeemanSimpleSecondOrderODEIntegrator (void)
+   :
+      Er7UtilsDeletable(),
+      BeemanSecondOrderODEIntegrator()
+   {}
+
+   /**
+    * BeemanSimpleSecondOrderODEIntegrator copy constructor.
+    * @param[in] src  Object to be copied.
+    */
+   BeemanSimpleSecondOrderODEIntegrator (
+      const BeemanSimpleSecondOrderODEIntegrator & src)
+   :
+      Er7UtilsDeletable(),
+      BeemanSecondOrderODEIntegrator(src)
+   {}
+
+   /**
+    * BeemanSimpleSecondOrderODEIntegrator non-default constructor.
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     size                Size of the position, velocity vectors
+    * @param[in,out] controls_in         Integration controls
+    */
+   BeemanSimpleSecondOrderODEIntegrator (
+      const IntegratorConstructor & primer_constructor,
+      unsigned int size,
+      IntegrationControls & controls_in)
+   :
+      Er7UtilsDeletable(),
+      BeemanSecondOrderODEIntegrator(primer_constructor, size, controls_in)
+   {}
+
+   /**
+    * BeemanSimpleSecondOrderODEIntegrator destructor.
+    */
+   virtual ~BeemanSimpleSecondOrderODEIntegrator (void)
+   {}
+
+
+   // Member functions.
+
+   /**
+    * BeemanSimpleSecondOrderODEIntegrator assignment operator.
+    * @param[in] src  Object to be copied.
+    */
+   BeemanSimpleSecondOrderODEIntegrator & operator= (
+      BeemanSimpleSecondOrderODEIntegrator src)
+   {
+      swap (src);
+      return *this;
+   }
+
+   /**
+    * Create a copy of 'this' BeemanSimpleSecondOrderODEIntegrator object.
+    * @return Clone of 'this'.
+    */
+   virtual BeemanSimpleSecondOrderODEIntegrator * create_copy () const;
+
+
+protected:
+
+   /**
+    * Save derivatives, called during priming.
+    * @param countdown  The priming countdown.
+    * @param accel      Time derivative of the generalized velocity.
+    * @param velocity   Generalized velocity vector.
+    * @param position   Generalized position vector.
+    */
+   virtual void technique_save_derivatives (
+      int countdown,
+      double const * ER7_UTILS_RESTRICT accel,
+      double const * ER7_UTILS_RESTRICT velocity,
+      double const * ER7_UTILS_RESTRICT position);
+
+   /**
+    * Propagate state using Beeman's method, excluding priming.
+    * @param[in]     dyn_dt        Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     accel         Time derivative of the generalized velocity.
+    * @param[in,out] velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult technique_integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      double const * ER7_UTILS_RESTRICT accel,
+      double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position);
+};
+
+
+/**
+ * Specialization of BeemanSecondOrderODEIntegrator for the case of
+ * the time derivative of generalized position being some function of
+ * the generalized position and generalized velocity.
+ */
+class BeemanGeneralizedDerivSecondOrderODEIntegrator :
+   public BeemanSecondOrderODEIntegrator {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(BeemanGeneralizedDerivSecondOrderODEIntegrator)
+
+public:
+
+   // Constructors and destructor.
+
+   /**
+    * BeemanGeneralizedDerivSecondOrderODEIntegrator default constructor.
+    */
+   BeemanGeneralizedDerivSecondOrderODEIntegrator (void)
+   :
+      Er7UtilsDeletable(),
+      BeemanSecondOrderODEIntegrator()
+   {}
+
+   /**
+    * BeemanGeneralizedDerivSecondOrderODEIntegrator copy constructor.
+    * @param[in] src  Object to be copied.
+    */
+   BeemanGeneralizedDerivSecondOrderODEIntegrator (
+      const BeemanGeneralizedDerivSecondOrderODEIntegrator & src)
+   :
+      Er7UtilsDeletable(),
+      BeemanSecondOrderODEIntegrator(src)
+   {}
+
+   /**
+    * BeemanGeneralizedDerivSecondOrderODEIntegrator non-default constructor.
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     position_size       Size of the generalized position
+    * @param[in]     velocity_size       Size of the generalized velocity
+    * @param[in]     deriv_funs          Position derivative functions container
+    * @param[in,out] controls_in         Integration controls
+    */
+   BeemanGeneralizedDerivSecondOrderODEIntegrator (
+      const IntegratorConstructor & primer_constructor,
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionDerivativeFunctions & deriv_funs,
+      IntegrationControls & controls_in)
+   :
+      Er7UtilsDeletable (),
+      BeemanSecondOrderODEIntegrator (primer_constructor,
+                                    position_size, velocity_size,
+                                    deriv_funs, controls_in)
+   {}
+
+   /**
+    * BeemanGeneralizedDerivSecondOrderODEIntegrator destructor.
+    */
+   virtual ~BeemanGeneralizedDerivSecondOrderODEIntegrator (void)
+   {}
+
+
+   // Member functions.
+
+   /**
+    * BeemanGeneralizedDerivSecondOrderODEIntegrator assignment operator.
+    * @param[in] src  Object to be copied.
+    */
+   BeemanGeneralizedDerivSecondOrderODEIntegrator & operator= (
+      BeemanGeneralizedDerivSecondOrderODEIntegrator src)
+   {
+      swap (src);
+      return *this;
+   }
+
+   /**
+    * Create a copy of 'this' object.
+    * @return Clone of 'this'.
+    */
+   virtual BeemanGeneralizedDerivSecondOrderODEIntegrator * create_copy ()
+   const;
+
+
+protected:
+
+   /**
+    * Save derivatives, called during priming.
+    * @param countdown  The priming countdown.
+    * @param accel      Time derivative of the generalized velocity.
+    * @param velocity   Generalized velocity vector.
+    * @param position   Generalized position vector.
+    */
+   virtual void technique_save_derivatives (
+      int countdown,
+      double const * ER7_UTILS_RESTRICT accel,
+      double const * ER7_UTILS_RESTRICT velocity,
+      double const * ER7_UTILS_RESTRICT position);
+
+   /**
+    * Propagate state using Beeman's method, excluding priming.
+    * @param[in]     dyn_dt        Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     accel         Time derivative of the generalized velocity.
+    * @param[in,out] velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult technique_integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      double const * ER7_UTILS_RESTRICT accel,
+      double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position);
+};
+
+
+// This old implementation of BeemanGeneralizedStepSecondOrderODEIntegrator
+// is not ready for prime time.
+#if 0
+/**
+ * Specialization of BeemanSecondOrderODEIntegrator for the case of
+ * the generalized position being constrained to lie on a manifold.
+ * The generalized position step function makes a non-linear position step that
+ * keeps the generalized position on the manifold.
+ */
+class BeemanGeneralizedStepSecondOrderODEIntegrator :
+   public BeemanSecondOrderODEIntegrator {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(BeemanGeneralizedStepSecondOrderODEIntegrator)
+
+public:
+
+   // Constructors and destructor.
+
+   // Default constructor, exists because the memory manager needs one.
+   BeemanGeneralizedStepSecondOrderODEIntegrator (void)
+   :
+      Er7UtilsDeletable(),
+      BeemanSecondOrderODEIntegrator()
+   {}
+
+   /**
+    * BeemanGeneralizedStepSecondOrderODEIntegrator non-default constructor.
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     position_size       Size of the generalized position
+    * @param[in]     velocity_size       Size of the generalized velocity
+    * @param[in]     step_funs           Position step functions container
+    * @param[in,out] controls_in         Integration controls
+    */
+   BeemanGeneralizedStepSecondOrderODEIntegrator (
+      const IntegratorConstructor & primer_constructor,
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionStepFunctions & step_funs,
+      IntegrationControls & controls_in)
+   :
+      Er7UtilsDeletable(),
+      BeemanSecondOrderODEIntegrator (primer_constructor,
+                                    position_size, velocity_size,
+                                    step_funs, controls_in)
+   {}
+
+   /**
+    * BeemanGeneralizedStepSecondOrderODEIntegrator destructor.
+    */
+   virtual ~BeemanGeneralizedStepSecondOrderODEIntegrator (void)
+   {}
+
+
+protected:
+
+   /**
+    * Propagate state using Beeman's method, excluding priming.
+    * @param[in]     dyn_dt        Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     accel         Time derivative of the generalized velocity.
+    * @param[in,out] velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult technique_integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      double const * ER7_UTILS_RESTRICT accel,
+      double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position);
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   BeemanGeneralizedStepSecondOrderODEIntegrator (
+      const BeemanGeneralizedStepSecondOrderODEIntegrator &);
+
+   /**
+    * Not implemented.
+    */
+   BeemanGeneralizedStepSecondOrderODEIntegrator & operator= (
+      const BeemanGeneralizedStepSecondOrderODEIntegrator &);
+};
+#endif
+
+}
+
+
+#ifdef ER7_UTILS_NEED_AUX_INCLUDES
+#include "er7_utils/integration/core/include/integration_controls.hh"
+#endif
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/beeman/src/beeman_integrator_constructor.cc

@@ -0,0 +1,133 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the class BeemanIntegratorConstructor.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/integrator_constructor_utils.hh"
+#include "er7_utils/integration/core/include/priming_integration_controls.hh"
+#include "er7_utils/integration/rk2_heun/include/rk2_heun_integrator_constructor.hh"
+
+// Model includes
+#include "../include/beeman_integrator_constructor.hh"
+#include "../include/beeman_second_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+// Named constructor; create an BeemanIntegratorConstructor.
+IntegratorConstructor*
+BeemanIntegratorConstructor::create_constructor (
+   void)
+{
+   return alloc::allocate_object<BeemanIntegratorConstructor> ();
+}
+
+
+// Create a duplicate of the constructor.
+IntegratorConstructor *
+BeemanIntegratorConstructor::create_copy (
+   void)
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Create an Beeman integration controls.
+IntegrationControls *
+BeemanIntegratorConstructor::create_integration_controls (
+   void)
+const
+{
+   return integ_utils::allocate_controls<PrimingIntegrationControls> (
+             *primer_constructor, 2, 2);
+}
+
+
+// Create a Heun's method state integrator for a first order ODE.
+FirstOrderODEIntegrator *
+BeemanIntegratorConstructor::create_first_order_ode_integrator (
+   unsigned int size,
+   IntegrationControls & controls)
+const
+{
+   return integ_utils::allocate_integrator<RK2HeunFirstOrderODEIntegrator> (
+             size, controls);
+}
+
+
+// Create an Beeman state integrator for a second order ODE.
+SecondOrderODEIntegrator *
+BeemanIntegratorConstructor::create_second_order_ode_integrator (
+   unsigned int size,
+   IntegrationControls & controls)
+const
+{
+   return integ_utils::allocate_integrator<
+                BeemanSimpleSecondOrderODEIntegrator> (
+             *primer_constructor, size, controls);
+}
+
+
+// Create an Beeman state integrator for a second order ODE.
+SecondOrderODEIntegrator *
+BeemanIntegratorConstructor::
+create_generalized_deriv_second_order_ode_integrator (
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionDerivativeFunctions & deriv_funs,
+   IntegrationControls & controls)
+const
+{
+   return integ_utils::allocate_integrator<
+                BeemanGeneralizedDerivSecondOrderODEIntegrator> (
+             *primer_constructor,
+             position_size, velocity_size, deriv_funs, controls);
+}
+
+
+#if 0
+// Create an Beeman state integrator for a second order ODE.
+SecondOrderODEIntegrator *
+BeemanIntegratorConstructor::
+create_generalized_step_second_order_ode_integrator (
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionStepFunctions & step_funs,
+   IntegrationControls & controls)
+const
+{
+   return integ_utils::allocate_integrator<
+                BeemanGeneralizedStepSecondOrderODEIntegrator> (
+             *primer_constructor,
+             position_size, velocity_size, step_funs, controls);
+}
+#endif
+
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/beeman/src/beeman_second_order_ode_integrator.cc

@@ -0,0 +1,442 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the class BeemanSecondOrderODEIntegrator.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/abm_utils.hh"
+#include "er7_utils/integration/core/include/integ_utils.hh"
+#include "er7_utils/integration/core/include/integrator_constructor.hh"
+#include "er7_utils/integration/core/include/integration_controls.hh"
+
+// Model includes
+#include "../include/beeman_second_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+
+
+//BeemanSecondOrderODEIntegrator default constructor.
+BeemanSecondOrderODEIntegrator::BeemanSecondOrderODEIntegrator (
+   void)
+:
+   Er7UtilsDeletable (),
+   PrimingSecondOrderODEIntegrator (),
+   init_vel (NULL),
+   mean_vel (NULL),
+   init_acc (NULL),
+   prev_acc (NULL),
+   posdot (NULL),
+   posddot (NULL),
+   init_posddot (NULL),
+   prev_posddot (NULL)
+{
+}
+
+
+//BeemanSecondOrderODEIntegrator copy constructor.
+BeemanSecondOrderODEIntegrator::BeemanSecondOrderODEIntegrator (
+   const BeemanSecondOrderODEIntegrator & src)
+:
+   Er7UtilsDeletable (),
+   PrimingSecondOrderODEIntegrator (src),
+   init_vel (NULL),
+   mean_vel (NULL),
+   init_acc (NULL),
+   prev_acc (NULL),
+   posdot (NULL),
+   posddot (NULL),
+   init_posddot (NULL),
+   prev_posddot (NULL)
+{
+   // Replicate the source's contents if they exist.
+   if (problem_type == Integration::GeneralizedDerivSecondOrderODE) {
+       init_vel = alloc::replicate_array (state_size[1], src.init_vel);
+       mean_vel = alloc::replicate_array (state_size[1], src.mean_vel);
+       init_acc = alloc::replicate_array (state_size[1], src.init_acc);
+       prev_acc = alloc::replicate_array (state_size[1], src.prev_acc);
+       posdot       = alloc::replicate_array (state_size[0], src.posdot);
+       posddot      = alloc::replicate_array (state_size[0], src.posddot);
+       init_posddot = alloc::replicate_array (state_size[0], src.init_posddot);
+       prev_posddot = alloc::replicate_array (state_size[0], src.prev_posddot);
+   }
+
+   else if (src.init_vel != NULL) {
+      init_vel = alloc::replicate_array (state_size[1], src.init_vel);
+      mean_vel = alloc::replicate_array (state_size[1], src.mean_vel);
+      init_acc = alloc::replicate_array (state_size[1], src.init_acc);
+      prev_acc = alloc::replicate_array (state_size[1], src.prev_acc);
+   }
+}
+
+
+
+// BeemanSecondOrderODEIntegrator non-default constructor for a simple
+// second order ODE.
+BeemanSecondOrderODEIntegrator::BeemanSecondOrderODEIntegrator (
+   const IntegratorConstructor & primer_constructor,
+   unsigned int size,
+   IntegrationControls & controls_in)
+:
+   Er7UtilsDeletable (),
+   PrimingSecondOrderODEIntegrator (2, primer_constructor, size, controls_in),
+   init_vel (NULL),
+   mean_vel (NULL),
+   init_acc (NULL),
+   prev_acc (NULL),
+   posdot (NULL),
+   posddot (NULL),
+   init_posddot (NULL),
+   prev_posddot (NULL)
+{
+   // Allocate memory used by Beeman's method.
+   init_vel = alloc::allocate_array (size);
+   mean_vel = alloc::allocate_array (size);
+   init_acc = alloc::allocate_array (size);
+   prev_acc = alloc::allocate_array (size);
+}
+
+
+// BeemanSecondOrderODEIntegrator non-default constructor for a generalized
+// second order ODE in which position is advanced internally using the position
+// derivative computed by the provided derivative function.
+BeemanSecondOrderODEIntegrator::BeemanSecondOrderODEIntegrator (
+   const IntegratorConstructor & primer_constructor,
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionDerivativeFunctions & deriv_funs,
+   IntegrationControls & controls_in)
+:
+   Er7UtilsDeletable (),
+   PrimingSecondOrderODEIntegrator (2, primer_constructor,
+                                    position_size, velocity_size,
+                                    deriv_funs, controls_in),
+   init_vel (NULL),
+   mean_vel (NULL),
+   init_acc (NULL),
+   prev_acc (NULL),
+   posdot (NULL),
+   posddot (NULL),
+   init_posddot (NULL),
+   prev_posddot (NULL)
+{
+   // Allocate memory used by Beeman's method.
+   init_vel = alloc::allocate_array (velocity_size);
+   mean_vel = alloc::allocate_array (velocity_size);
+   init_acc = alloc::allocate_array (velocity_size);
+   prev_acc = alloc::allocate_array (velocity_size);
+   posdot       = alloc::allocate_array (position_size);
+   posddot      = alloc::allocate_array (position_size);
+   init_posddot = alloc::allocate_array (position_size);
+   prev_posddot = alloc::allocate_array (position_size);
+}
+
+
+// BeemanSecondOrderODEIntegrator destructor.
+BeemanSecondOrderODEIntegrator::~BeemanSecondOrderODEIntegrator (
+   void)
+{
+   alloc::deallocate_array (init_vel);
+   alloc::deallocate_array (mean_vel);
+   alloc::deallocate_array (init_acc);
+   alloc::deallocate_array (prev_acc);
+   alloc::deallocate_array (posdot);
+   alloc::deallocate_array (posddot);
+   alloc::deallocate_array (init_posddot);
+   alloc::deallocate_array (prev_posddot);
+}
+
+
+// Non-throwing swap.
+void
+BeemanSecondOrderODEIntegrator::swap (
+   BeemanSecondOrderODEIntegrator & other)
+{
+   PrimingSecondOrderODEIntegrator::swap (other);
+
+   std::swap (init_vel, other.init_vel);
+   std::swap (mean_vel, other.mean_vel);
+   std::swap (init_acc, other.init_acc);
+   std::swap (prev_acc, other.prev_acc);
+   std::swap (posdot, other.posdot);
+   std::swap (posddot, other.posddot);
+   std::swap (init_posddot, other.init_posddot);
+   std::swap (prev_posddot, other.prev_posddot);
+}
+
+
+// Clone a BeemanSimpleSecondOrderODEIntegrator.
+BeemanSimpleSecondOrderODEIntegrator *
+BeemanSimpleSecondOrderODEIntegrator::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Clone a BeemanGeneralizedDerivSecondOrderODEIntegrator.
+BeemanGeneralizedDerivSecondOrderODEIntegrator *
+BeemanGeneralizedDerivSecondOrderODEIntegrator::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Save derivatives during priming
+void
+BeemanSimpleSecondOrderODEIntegrator::technique_save_derivatives (
+   int,
+   double const * ER7_UTILS_RESTRICT accel,
+   double const *,
+   double const *)
+{
+   integ_utils::copy_array (accel, state_size[1], init_acc);
+}
+
+
+// Save derivatives during priming
+void
+BeemanGeneralizedDerivSecondOrderODEIntegrator::technique_save_derivatives (
+   int,
+   double const * ER7_UTILS_RESTRICT accel,
+   double const * ER7_UTILS_RESTRICT velocity,
+   double const * ER7_UTILS_RESTRICT position)
+{
+   integ_utils::copy_array (accel, state_size[1], init_acc);
+   compute_posdotdot (position, velocity, accel, init_posddot);
+}
+
+
+/**
+ * Advance position and velocity for the 1st step of Beeman's method.
+ * @param[in]     accel       Current acceleration
+ * @param[in]     prev_acc    Acceleration at start of previous integ cycle
+ * @param[in]     deltat      Time step
+ * @param[in]     size        State size
+ * @param[out]    init_vel    Saved velocity vector
+ * @param[out]    init_acc    Saved acceleration vector
+ * @param[in,out] position    Updated position vector
+ * @param[in,out] velocity    Updated velocity vector
+ */
+inline void ER7_UTILS_ALWAYS_INLINE
+simple_beeman_step_one (
+   double const * ER7_UTILS_RESTRICT accel,
+   double const * ER7_UTILS_RESTRICT prev_acc,
+   double deltat,
+   int size,
+   double * ER7_UTILS_RESTRICT init_vel,
+   double * ER7_UTILS_RESTRICT init_acc,
+   double * ER7_UTILS_RESTRICT position,
+   double * ER7_UTILS_RESTRICT velocity)
+{
+   static const double one_sixth = 1.0 / 6.0;
+   double dto6 = one_sixth * deltat;
+
+   for (int ii = 0; ii < size; ++ii) {
+      double acc_prev = prev_acc[ii];
+      double acc = accel[ii];
+      double vel = velocity[ii];
+      double vdot = (3.0*acc - acc_prev) * 0.5;
+      double xdot = vel + (4.0*acc - acc_prev) * dto6;
+      init_vel[ii]  = vel;
+      init_acc[ii]  = acc;
+      velocity[ii] += vdot*deltat;
+      position[ii] += xdot*deltat;
+   }
+}
+
+
+/**
+ * Advance velocity for the 1st step of Beeman's method.
+ * @param[in]     accel       Current acceleration
+ * @param[in]     prev_acc    Acceleration at start of previous integ cycle
+ * @param[in]     deltat      Time step
+ * @param[in]     size        State size
+ * @param[out]    init_vel    Saved velocity vector
+ * @param[out]    init_acc    Saved acceleration vector
+ * @param[in,out] velocity    Updated velocity vector
+ */
+inline void ER7_UTILS_ALWAYS_INLINE
+generalized_beeman_velocity_step_one (
+   double const * ER7_UTILS_RESTRICT accel,
+   double const * ER7_UTILS_RESTRICT prev_acc,
+   double deltat,
+   int size,
+   double * ER7_UTILS_RESTRICT init_vel,
+   double * ER7_UTILS_RESTRICT init_acc,
+   double * ER7_UTILS_RESTRICT velocity)
+{
+   for (int ii = 0; ii < size; ++ii) {
+      double acc_prev = prev_acc[ii];
+      double acc = accel[ii];
+      double vel = velocity[ii];
+      double vdot = (3.0*acc - acc_prev) * 0.5;
+      init_vel[ii]  = vel;
+      init_acc[ii]  = acc;
+      velocity[ii] += vdot*deltat;
+   }
+}
+
+
+/**
+ * Advance position for the 1st step of Beeman's method.
+ * @param[in]     init_posddot  Current position 2nd deriv
+ * @param[in]     prev_posddot  Posn 2nd deriv at start of previous integ cycle
+ * @param[in]     deltat        Time step
+ * @param[in]     size          State size
+ * @param[in,out] posdot        Position derivative (tweaked on output)
+ */
+inline void ER7_UTILS_ALWAYS_INLINE
+generalized_beeman_position_step_one (
+   double const * ER7_UTILS_RESTRICT init_posddot,
+   double const * ER7_UTILS_RESTRICT prev_posddot,
+   double deltat,
+   int size,
+   double * ER7_UTILS_RESTRICT posdot)
+{
+   static const double one_sixth = 1.0 / 6.0;
+   double dto6 = one_sixth * deltat;
+
+   for (int ii = 0; ii < size; ++ii) {
+      posdot[ii] += (4.0*init_posddot[ii] - prev_posddot[ii]) * dto6;
+   }
+}
+
+
+/**
+ * Advance velocity for the 2nd step of Beeman's method.
+ * @param[in]     init_vel    Saved velocity vector
+ * @param[in]     accel       Current acceleration
+ * @param[in]     prev_acc    Acceleration at start of previous integ cycle
+ * @param[in]     init_acc    Acceleration at start of current integ cycle
+ * @param[in]     deltat      Time step
+ * @param[in]     size        State size
+ * @param[out]    velocity    Updated velocity vector
+ */
+inline void ER7_UTILS_ALWAYS_INLINE
+beeman_step_two (
+   double const * ER7_UTILS_RESTRICT init_vel,
+   double const * ER7_UTILS_RESTRICT accel,
+   double const * ER7_UTILS_RESTRICT prev_acc,
+   double const * ER7_UTILS_RESTRICT init_acc,
+   double deltat,
+   int size,
+   double * ER7_UTILS_RESTRICT velocity)
+{
+   static const double one_sixth = 1.0 / 6.0;
+   double dto6 = one_sixth * deltat;
+
+   for (int ii = 0; ii < size; ++ii) {
+      velocity[ii] = init_vel[ii] +
+                     (2.0*accel[ii] + 5.0*init_acc[ii] - prev_acc[ii]) * dto6;
+   }
+}
+
+
+// Propagate state for the special case of velocity being the derivative of
+// position.
+IntegratorResult
+BeemanSimpleSecondOrderODEIntegrator::technique_integrate (
+   double dyn_dt,
+   unsigned int target_stage,
+   double const * ER7_UTILS_RESTRICT accel,
+   double * ER7_UTILS_RESTRICT velocity,
+   double * ER7_UTILS_RESTRICT position)
+{
+   // Primed: Integrate state via Beeman's method
+   switch (target_stage) {
+   // Target stage 1 represents the Beeman's method predictor stage.
+   case 1:
+      // Rotate the history.
+      std::swap (init_acc, prev_acc);
+
+      // Perform the predictor step.
+      simple_beeman_step_one (
+         accel, prev_acc, dyn_dt, state_size[0],
+         init_vel, init_acc, position, velocity);
+      break;
+
+   // Target stage 2 represents the Beeman's method corrector stage.
+   case 2:
+      beeman_step_two (
+         init_vel, accel, prev_acc, init_acc, dyn_dt, state_size[0], velocity);
+      break;
+   }
+
+   return 1.0;
+}
+
+
+// Propagate state for the general case of the generalized position derivative
+// being a function of generalized position and generalized velocity.
+IntegratorResult
+BeemanGeneralizedDerivSecondOrderODEIntegrator::technique_integrate (
+   double dyn_dt,
+   unsigned int target_stage,
+   double const * ER7_UTILS_RESTRICT accel,
+   double * ER7_UTILS_RESTRICT velocity,
+   double * ER7_UTILS_RESTRICT position)
+{
+   switch (target_stage) {
+   // Target stage 1 represents the Beeman's method predictor stage.
+   case 1:
+      // Rotate the history.
+      std::swap (init_acc, prev_acc);
+      std::swap (init_posddot, prev_posddot);
+
+      // Compute the position derivatives.
+      compute_posdot (position, velocity, posdot);
+      compute_posdotdot (position, velocity, accel, init_posddot);
+
+      // Modify the position derivative per the Beeman position predictor.
+      generalized_beeman_position_step_one (
+         init_posddot, prev_posddot, dyn_dt, state_size[0], posdot);
+
+      // Advance position as an Euler step with the updated position derivative.
+      integ_utils::euler_step (
+         position, posdot, dyn_dt, state_size[0], position);
+
+      // Advance velocity per the Beeman velocity predictor step.
+      generalized_beeman_velocity_step_one (
+         accel, prev_acc, dyn_dt, state_size[1],
+         init_vel, init_acc, velocity);
+      break;
+
+   // Target stage 2 represents the Beeman's method corrector stage.
+   // Velocity is corrected, but not position.
+   case 2:
+      beeman_step_two (
+         init_vel, accel, prev_acc, init_acc, dyn_dt, state_size[1], velocity);
+      break;
+   }
+
+   return 1.0;
+}
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/abm_utils.hh

@@ -0,0 +1,666 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines free functions used by Adams-Bashforth-Moulton integrators.
+ */
+
+/*
+Purpose: ()
+*/
+
+#ifndef ER7_UTILS_ABM_UTILS_HH
+#define ER7_UTILS_ABM_UTILS_HH
+
+
+// ER7 utilities includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+
+namespace er7_utils {
+
+/**
+ * Utility template functions specific to the ABM family of integrators.
+ */
+namespace abm {
+
+   /**
+    * Circularly rotate the history buffer.
+    * @tparam        hist_len    Size of the history buffer
+    * @param[in,out] deriv_hist  State derivatives history
+    */
+   template<int hist_len>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rotate_history (
+      double * ER7_UTILS_RESTRICT deriv_hist[hist_len])
+   {
+      double * oldest = deriv_hist[hist_len-1];
+      for (int jj = hist_len-1; jj > 0; --jj) {
+         deriv_hist[jj] = deriv_hist[jj-1];
+      }
+      deriv_hist[0] = oldest;
+   }
+
+   /**
+    * Circularly rotate the velocity and acceleration history buffers.
+    * @tparam        hist_len    Size of the history buffer
+    * @param[in,out] posdot_hist  State derivatives history
+    * @param[in,out] veldot_hist  State second derivatives history
+    */
+   template<int hist_len>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rotate_history (
+      double * ER7_UTILS_RESTRICT posdot_hist[hist_len],
+      double * ER7_UTILS_RESTRICT veldot_hist[hist_len])
+   {
+      double * oldest_posdot = posdot_hist[hist_len-1];
+      double * oldest_veldot = veldot_hist[hist_len-1];
+      for (int jj = hist_len-1; jj > 0; --jj) {
+         posdot_hist[jj] = posdot_hist[jj-1];
+         veldot_hist[jj] = veldot_hist[jj-1];
+      }
+      posdot_hist[0] = oldest_posdot;
+      veldot_hist[0] = oldest_veldot;
+   }
+
+
+   /**
+    * Make an Adams-Bashforth predictor step.
+    * @tparam        hist_len    Size of the history buffer
+    * @param[in]     velocity    State derivatives
+    * @param[in]     weights     Weights
+    * @param[in]     wscaled_dt  Time step * common weight scale
+    * @param[in]     size        State size
+    * @param[in,out] deriv_hist  State derivatives history
+    * @param[out]    init_pos    Initial state
+    * @param[in,out] position    State vector (updated in place)
+    */
+   template<int hist_len>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   predictor_step (
+      double const * ER7_UTILS_RESTRICT velocity,
+      double const * ER7_UTILS_RESTRICT weights,
+      double wscaled_dt,
+      int size,
+      double * ER7_UTILS_RESTRICT deriv_hist[hist_len],
+      double * ER7_UTILS_RESTRICT init_pos,
+      double * ER7_UTILS_RESTRICT position)
+   {
+      for (int ii = 0; ii < size; ++ii) {
+         // Save the initial position and velocity.
+         init_pos[ii]      = position[ii];
+         deriv_hist[0][ii] = velocity[ii];
+
+         // Compute the updated position.
+         double weighted_deriv = weights[0] * velocity[ii];
+         for (int jj = 1; jj < hist_len; ++jj) {
+            weighted_deriv += weights[jj] * deriv_hist[jj][ii];
+         }
+         position[ii] += weighted_deriv * wscaled_dt;
+      }
+   }
+
+
+   /**
+    * Make an Adams-Bashforth predictor step.
+    * @tparam        hist_len    Size of the history buffer
+    * @param[in]     position    State vector
+    * @param[in]     velocity    State derivatives
+    * @param[in]     weights     Weights
+    * @param[in]     wscaled_dt  Time step * common weight scale
+    * @param[in]     size        State size
+    * @param[in,out] deriv_hist  State derivatives history
+    * @param[out]    init_pos    Initial state
+    * @param[out]    delta_pos   Change in position
+    */
+   template<int hist_len>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   predictor_weighted_sum (
+      double const * ER7_UTILS_RESTRICT position,
+      double const * ER7_UTILS_RESTRICT velocity,
+      double const * ER7_UTILS_RESTRICT weights,
+      double wscaled_dt,
+      int size,
+      double * ER7_UTILS_RESTRICT deriv_hist[hist_len],
+      double * ER7_UTILS_RESTRICT init_pos,
+      double * ER7_UTILS_RESTRICT delta_pos)
+   {
+      for (int ii = 0; ii < size; ++ii) {
+         // Save the initial position and velocity.
+         init_pos[ii]      = position[ii];
+         deriv_hist[0][ii] = velocity[ii];
+
+         // Compute the change in position.
+         double weighted_deriv = weights[0] * velocity[ii];
+         for (int jj = 1; jj < hist_len; ++jj) {
+            weighted_deriv += weights[jj] * deriv_hist[jj][ii];
+         }
+         delta_pos[ii] = weighted_deriv * wscaled_dt;
+      }
+   }
+
+
+   /**
+    * Make an Adams-Bashforth predictor step.
+    * @tparam        hist_len    Size of the history buffer
+    * @param[in]     velocity    State derivatives
+    * @param[in]     weights     Weights
+    * @param[in]     wscale      Common weight scale factor
+    * @param[in]     deltat      Time step
+    * @param[in]     size        State size
+    * @param[in,out] deriv_hist  State derivatives history
+    * @param[out]    init_pos    Initial state
+    * @param[in,out] position    State vector (updated in place)
+    */
+   template<int hist_len>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   predictor_step (
+      double const * ER7_UTILS_RESTRICT velocity,
+      double const * ER7_UTILS_RESTRICT weights,
+      double wscale,
+      double deltat,
+      int size,
+      double * ER7_UTILS_RESTRICT deriv_hist[hist_len],
+      double * ER7_UTILS_RESTRICT init_pos,
+      double * ER7_UTILS_RESTRICT position)
+   {
+      predictor_step<hist_len> (
+         velocity, weights, wscale*deltat, size,
+         deriv_hist, init_pos, position);
+   }
+
+
+   /**
+    * Make a second order ODE Adams-Bashforth predictor step for the special
+    * case of velocity being the derivative of position.
+    * @tparam        hist_len     Size of the history buffer
+    * @param[in]     accel        Velocity derivatives
+    * @param[in]     weights      Weights
+    * @param[in]     wscale       Common weight scale factor
+    * @param[in]     deltat       Time step
+    * @param[in]     state_size   Size of the position and velocity vectors
+    * @param[in,out] posdot_hist  Generalized position derivatives history
+    * @param[in,out] veldot_hist  Generalized velocity derivatives history
+    * @param[out]    init_pos     Initial position
+    * @param[out]    init_vel     Initial velocity
+    * @param[in,out] position     Position vector (updated in place)
+    * @param[in,out] velocity     Velocity vector (updated in place)
+    */
+   template<int hist_len>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   predictor_step (
+      double const * ER7_UTILS_RESTRICT accel,
+      double const * ER7_UTILS_RESTRICT weights,
+      double wscale,
+      double deltat,
+      int state_size,
+      double * ER7_UTILS_RESTRICT posdot_hist[hist_len],
+      double * ER7_UTILS_RESTRICT veldot_hist[hist_len],
+      double * ER7_UTILS_RESTRICT init_pos,
+      double * ER7_UTILS_RESTRICT init_vel,
+      double * ER7_UTILS_RESTRICT position,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      double wscaled_dt = wscale * deltat;
+      for (int ii = 0; ii < state_size; ++ii) {
+         // Save the initial position, velocity, and their derivatives.
+         init_pos[ii]       = position[ii];
+         init_vel[ii]       = velocity[ii];
+         posdot_hist[0][ii] = velocity[ii];
+         veldot_hist[0][ii] = accel[ii];
+
+         // Compute the updated position.
+         double weighted_posdot = weights[0] * velocity[ii];
+         double weighted_veldot = weights[0] * accel[ii];
+         for (int jj = 1; jj < hist_len; ++jj) {
+            weighted_posdot += weights[jj] * posdot_hist[jj][ii];
+            weighted_veldot += weights[jj] * veldot_hist[jj][ii];
+         }
+         position[ii] += weighted_posdot * wscaled_dt;
+         velocity[ii] += weighted_veldot * wscaled_dt;
+      }
+   }
+
+
+   /**
+    * Make a second order ODE Adams-Bashforth predictor step for a generalized
+    * position and generalized velocity.
+    * @tparam        hist_len     Size of the history buffer
+    * @param[in]     posdot       Position derivatives
+    * @param[in]     veldot       Velocity derivatives
+    * @param[in]     weights      Weights
+    * @param[in]     wscale       Common weight scale factor
+    * @param[in]     deltat       Time step
+    * @param[in]     state_size   Sizes of position, velocity vectors
+    * @param[in,out] posdot_hist  Generalized position derivatives history
+    * @param[in,out] veldot_hist  Generalized velocity derivatives history
+    * @param[out]    init_pos     Initial position
+    * @param[out]    init_vel     Initial velocity
+    * @param[in,out] position     Position vector (updated in place)
+    * @param[in,out] velocity     Velocity vector (updated in place)
+    */
+   template<int hist_len>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   predictor_step (
+      double const * ER7_UTILS_RESTRICT posdot,
+      double const * ER7_UTILS_RESTRICT veldot,
+      double const * ER7_UTILS_RESTRICT weights,
+      double wscale,
+      double deltat,
+      int state_size[2],
+      double * ER7_UTILS_RESTRICT posdot_hist[hist_len],
+      double * ER7_UTILS_RESTRICT veldot_hist[hist_len],
+      double * ER7_UTILS_RESTRICT init_pos,
+      double * ER7_UTILS_RESTRICT init_vel,
+      double * ER7_UTILS_RESTRICT position,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      double wscaled_dt = wscale * deltat;
+
+      predictor_step<hist_len> (
+         posdot, weights, wscaled_dt, state_size[0],
+         posdot_hist, init_pos, position);
+
+      predictor_step<hist_len> (
+         veldot, weights, wscaled_dt, state_size[1],
+         veldot_hist, init_vel, velocity);
+   }
+
+
+   /**
+    * Make a second order ODE Adams-Bashforth predictor step for a generalized
+    * position and generalized velocity, where generalized position will be
+    * advanced externally using the delta_pos computed in this function.
+    * @tparam        hist_len     Size of the history buffer
+    * @param[in]     position     Position
+    * @param[in]     posdot       Position derivatives
+    * @param[in]     veldot       Velocity derivatives
+    * @param[in]     weights      Weights
+    * @param[in]     wscale       Common weight scale factor
+    * @param[in]     deltat       Time step
+    * @param[in]     state_size   Sizes of position, velocity vectors
+    * @param[in,out] posdot_hist  Generalized position derivatives history
+    * @param[in,out] veldot_hist  Generalized velocity derivatives history
+    * @param[out]    init_pos     Initial position
+    * @param[out]    init_vel     Initial velocity
+    * @param[out]    delta_pos    Position vector delta
+    * @param[in,out] velocity     Velocity vector (updated in place)
+    */
+   template<int hist_len>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   predictor_step (
+      double const * ER7_UTILS_RESTRICT position,
+      double const * ER7_UTILS_RESTRICT posdot,
+      double const * ER7_UTILS_RESTRICT veldot,
+      double const * ER7_UTILS_RESTRICT weights,
+      double wscale,
+      double deltat,
+      int state_size[2],
+      double * ER7_UTILS_RESTRICT posdot_hist[hist_len],
+      double * ER7_UTILS_RESTRICT veldot_hist[hist_len],
+      double * ER7_UTILS_RESTRICT init_pos,
+      double * ER7_UTILS_RESTRICT init_vel,
+      double * ER7_UTILS_RESTRICT delta_pos,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      double wscaled_dt = wscale * deltat;
+
+      predictor_weighted_sum<hist_len> (
+         position, posdot, weights, wscaled_dt, state_size[0],
+         posdot_hist, init_pos, delta_pos);
+
+      predictor_step<hist_len> (
+         veldot, weights, wscaled_dt, state_size[1],
+         veldot_hist, init_vel, velocity);
+   }
+
+
+#if 0
+   /**
+    * Make a second order ODE Adams-Bashforth predictor step for generalized
+    * position and velocity, where generalized position is updated afterwards
+    * some a position update function and the mean velocity.
+    * @tparam        hist_len     Size of the history buffer
+    * @param[in]     accel        Velocity derivatives
+    * @param[in]     weights      Weights
+    * @param[in]     wscale       Common weight scale factor
+    * @param[in]     deltat       Time step
+    * @param[in]     state_size   Size of the position and velocity vectors
+    * @param[in,out] posdot_hist  Generalized position derivatives history
+    * @param[in,out] veldot_hist  Generalized velocity derivatives history
+    * @param[out]    init_vel     Initial velocity
+    * @param[in,out] mean_vel     Mean velocity for position update
+    * @param[in,out] velocity     Velocity vector (updated in place)
+    */
+   template<int hist_len>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   predictor_step (
+      double const * ER7_UTILS_RESTRICT accel,
+      double const * ER7_UTILS_RESTRICT weights,
+      double wscale,
+      double deltat,
+      int state_size,
+      double * ER7_UTILS_RESTRICT posdot_hist[hist_len],
+      double * ER7_UTILS_RESTRICT veldot_hist[hist_len],
+      double * ER7_UTILS_RESTRICT init_vel,
+      double * ER7_UTILS_RESTRICT mean_vel,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      double wscaled_dt = wscale * deltat;
+      for (int ii = 0; ii < state_size; ++ii) {
+         // Save the initial position, velocity, and their derivatives.
+         init_vel[ii]       = velocity[ii];
+         posdot_hist[0][ii] = velocity[ii];
+         veldot_hist[0][ii] = accel[ii];
+
+         // Compute the mean velocity and update velocity.
+         double weighted_posdot = weights[0] * velocity[ii];
+         double weighted_veldot = weights[0] * accel[ii];
+         for (int jj = 1; jj < hist_len; ++jj) {
+            weighted_posdot += weights[jj] * posdot_hist[jj][ii];
+            weighted_veldot += weights[jj] * veldot_hist[jj][ii];
+         }
+         mean_vel[ii]  = weighted_posdot * wscale;
+         velocity[ii] += weighted_veldot * wscaled_dt;
+      }
+   }
+#endif
+
+
+   /**
+    * Make an Adams-Moulton corrector step.
+    * @tparam        hist_len    Size of the history buffer
+    * @param[in]     init_pos    Initial state
+    * @param[in]     velocity    State derivatives
+    * @param[in]     deriv_hist  State derivatives vectors
+    * @param[in]     weights     Weights on velocity, deriv_hist
+    * @param[in]     wscaled_dt  Time step * common weight scale
+    * @param[in]     size        State size
+    * @param[out]    position    Updated state vector
+    */
+   template<int hist_len>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   corrector_step (
+      double const * ER7_UTILS_RESTRICT init_pos,
+      double const * ER7_UTILS_RESTRICT velocity,
+      double const * ER7_UTILS_RESTRICT const deriv_hist[hist_len],
+      double const * ER7_UTILS_RESTRICT weights,
+      double wscaled_dt,
+      int size,
+      double * ER7_UTILS_RESTRICT position)
+   {
+      double der_weight = weights[0];
+      double const * ER7_UTILS_RESTRICT hist_weights = weights+1;
+
+      for (int ii = 0; ii < size; ++ii) {
+         double weighted_deriv = der_weight * velocity[ii];
+         for (int jj = 0; jj < hist_len-1; ++jj) {
+            weighted_deriv += hist_weights[jj] * deriv_hist[jj][ii];
+         }
+         position[ii] = init_pos[ii] + weighted_deriv * wscaled_dt;
+      }
+   }
+
+
+   /**
+    * Make an Adams-Bashforth corrector step.
+    * @tparam        hist_len    Size of the history buffer
+    * @param[in]     velocity    State derivatives
+    * @param[in]     weights     Weights
+    * @param[in]     wscaled_dt  Time step * common weight scale
+    * @param[in]     size        State size
+    * @param[in,out] deriv_hist  State derivatives history
+    * @param[out]    delta_pos   Change in position
+    */
+   template<int hist_len>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   corrector_weighted_sum (
+      double const * ER7_UTILS_RESTRICT velocity,
+      double const * ER7_UTILS_RESTRICT const deriv_hist[hist_len],
+      double const * ER7_UTILS_RESTRICT weights,
+      double wscaled_dt,
+      int size,
+      double * ER7_UTILS_RESTRICT delta_pos)
+   {
+      double der_weight = weights[0];
+      double const * ER7_UTILS_RESTRICT hist_weights = weights+1;
+
+      for (int ii = 0; ii < size; ++ii) {
+         double weighted_deriv = der_weight * velocity[ii];
+         for (int jj = 0; jj < hist_len-1; ++jj) {
+            weighted_deriv += hist_weights[jj] * deriv_hist[jj][ii];
+         }
+         delta_pos[ii] = weighted_deriv * wscaled_dt;
+      }
+   }
+
+
+   /**
+    * Make an Adams-Moulton corrector step.
+    * @tparam        hist_len    Size of the history buffer
+    * @param[in]     init_pos    Initial state
+    * @param[in]     velocity    State derivatives
+    * @param[in]     deriv_hist  State derivatives vectors
+    * @param[in]     weights     Weights on velocity, deriv_hist
+    * @param[in]     wscale      Common weight scale factor
+    * @param[in]     deltat      Time step
+    * @param[in]     size        State size
+    * @param[out]    position    Updated state vector
+    */
+   template<int hist_len>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   corrector_step (
+      double const * ER7_UTILS_RESTRICT init_pos,
+      double const * ER7_UTILS_RESTRICT velocity,
+      double const * ER7_UTILS_RESTRICT const deriv_hist[hist_len],
+      double const * ER7_UTILS_RESTRICT weights,
+      double wscale,
+      double deltat,
+      int size,
+      double * ER7_UTILS_RESTRICT position)
+   {
+      corrector_step<hist_len> (
+         init_pos, velocity, deriv_hist, weights, wscale*deltat, size,
+         position);
+   }
+
+
+   /**
+    * Make a second order ODE Adams-Moulton corrector step for the special
+    * case of velocity being the derivative of position.
+    * @tparam        hist_len     Size of the history buffer
+    * @param[in]     init_pos     Initial state
+    * @param[out]    init_vel     Initial velocity
+    * @param[in]     accel        Velocity derivatives
+    * @param[in,out] posdot_hist  Generalized position derivatives history
+    * @param[in,out] veldot_hist  Generalized velocity derivatives history
+    * @param[in]     weights      Weights on velocity, deriv_hist
+    * @param[in]     wscale       Common weight scale factor
+    * @param[in]     deltat       Time step
+    * @param[in]     state_size   State sizes
+    * @param[out]    position     Updated position vector
+    * @param[out]    velocity     Updated velocity vector
+    */
+   template<int hist_len>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   corrector_step (
+      double const * ER7_UTILS_RESTRICT init_pos,
+      double const * ER7_UTILS_RESTRICT init_vel,
+      double const * ER7_UTILS_RESTRICT accel,
+      double const * ER7_UTILS_RESTRICT const posdot_hist[hist_len],
+      double const * ER7_UTILS_RESTRICT const veldot_hist[hist_len],
+      double const * ER7_UTILS_RESTRICT weights,
+      double wscale,
+      double deltat,
+      int state_size,
+      double * ER7_UTILS_RESTRICT position,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      double wscaled_dt = wscale * deltat;
+      double der_weight = weights[0];
+      double const * ER7_UTILS_RESTRICT hist_weights = weights+1;
+
+      for (int ii = 0; ii < state_size; ++ii) {
+         double weighted_posdot = der_weight * velocity[ii];
+         double weighted_veldot = der_weight * accel[ii];
+         for (int jj = 0; jj < hist_len-1; ++jj) {
+            weighted_posdot += hist_weights[jj] * posdot_hist[jj][ii];
+            weighted_veldot += hist_weights[jj] * veldot_hist[jj][ii];
+         }
+         position[ii] = init_pos[ii] + weighted_posdot * wscaled_dt;
+         velocity[ii] = init_vel[ii] + weighted_veldot * wscaled_dt;
+      }
+   }
+
+
+   /**
+    * Make a second order ODE Adams-Moulton corrector step for a generalized
+    * position and generalized velocity.
+    * @tparam        hist_len     Size of the history buffer
+    * @param[in]     init_pos     Initial state
+    * @param[in]     init_vel     Initial velocity
+    * @param[in]     posdot       Position derivatives
+    * @param[in]     veldot       Velocity derivatives
+    * @param[in,out] posdot_hist  Generalized position derivatives history
+    * @param[in,out] veldot_hist  Generalized velocity derivatives history
+    * @param[in]     weights      Weights on velocity, deriv_hist
+    * @param[in]     wscale       Common weight scale factor
+    * @param[in]     deltat       Time step
+    * @param[in]     state_size   State sizes
+    * @param[out]    position     Updated position vector
+    * @param[out]    velocity     Updated velocity vector
+    */
+   template<int hist_len>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   corrector_step (
+      double const * ER7_UTILS_RESTRICT init_pos,
+      double const * ER7_UTILS_RESTRICT init_vel,
+      double const * ER7_UTILS_RESTRICT posdot,
+      double const * ER7_UTILS_RESTRICT veldot,
+      double const * ER7_UTILS_RESTRICT const posdot_hist[hist_len],
+      double const * ER7_UTILS_RESTRICT const veldot_hist[hist_len],
+      double const * ER7_UTILS_RESTRICT weights,
+      double wscale,
+      double deltat,
+      int state_size[2],
+      double * ER7_UTILS_RESTRICT position,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      double wscaled_dt = wscale * deltat;
+
+      corrector_step<hist_len> (
+         init_pos, posdot, posdot_hist, weights, wscaled_dt, state_size[0],
+         position);
+
+      corrector_step<hist_len> (
+         init_vel, veldot, veldot_hist, weights, wscaled_dt, state_size[1],
+         velocity);
+   }
+
+
+   /**
+    * Make a second order ODE Adams-Moulton corrector step for a generalized
+    * position and generalized velocity.
+    * @tparam        hist_len     Size of the history buffer
+    * @param[in]     init_vel     Initial velocity
+    * @param[in]     posdot       Position derivatives
+    * @param[in]     veldot       Velocity derivatives
+    * @param[in,out] posdot_hist  Generalized position derivatives history
+    * @param[in,out] veldot_hist  Generalized velocity derivatives history
+    * @param[in]     weights      Weights on velocity, deriv_hist
+    * @param[in]     wscale       Common weight scale factor
+    * @param[in]     deltat       Time step
+    * @param[in]     state_size   State sizes
+    * @param[out]    delta_pos    Position vector delta
+    * @param[out]    velocity     Updated velocity vector
+    */
+   template<int hist_len>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   corrector_step (
+      double const * ER7_UTILS_RESTRICT init_vel,
+      double const * ER7_UTILS_RESTRICT posdot,
+      double const * ER7_UTILS_RESTRICT veldot,
+      double const * ER7_UTILS_RESTRICT const posdot_hist[hist_len],
+      double const * ER7_UTILS_RESTRICT const veldot_hist[hist_len],
+      double const * ER7_UTILS_RESTRICT weights,
+      double wscale,
+      double deltat,
+      int state_size[2],
+      double * ER7_UTILS_RESTRICT delta_pos,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      double wscaled_dt = wscale * deltat;
+
+      corrector_weighted_sum<hist_len> (
+         posdot, posdot_hist, weights, wscaled_dt, state_size[0], delta_pos);
+
+      corrector_step<hist_len> (
+         init_vel, veldot, veldot_hist, weights, wscaled_dt, state_size[1],
+         velocity);
+   }
+
+
+#if 0
+   /**
+    * Make a second order ODE Adams-Moulton corrector step for the special
+    * case of velocity being the derivative of position.
+    * @tparam        hist_len     Size of the history buffer
+    * @param[out]    init_vel     Initial velocity
+    * @param[in]     accel        Velocity derivatives
+    * @param[in,out] posdot_hist  Generalized position derivatives history
+    * @param[in,out] veldot_hist  Generalized velocity derivatives history
+    * @param[in]     weights      Weights on velocity, deriv_hist
+    * @param[in]     wscale       Common weight scale factor
+    * @param[in]     deltat       Time step
+    * @param[in]     state_size   State sizes
+    * @param[in,out] mean_vel     Mean velocity for position update
+    * @param[out]    velocity     Updated velocity vector
+    */
+   template<int hist_len>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   corrector_step (
+      double const * ER7_UTILS_RESTRICT init_vel,
+      double const * ER7_UTILS_RESTRICT accel,
+      double const * ER7_UTILS_RESTRICT const posdot_hist[hist_len],
+      double const * ER7_UTILS_RESTRICT const veldot_hist[hist_len],
+      double const * ER7_UTILS_RESTRICT weights,
+      double wscale,
+      double deltat,
+      int state_size,
+      double * ER7_UTILS_RESTRICT mean_vel,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      double wscaled_dt = wscale * deltat;
+      double der_weight = weights[0];
+      double const * ER7_UTILS_RESTRICT hist_weights = weights+1;
+
+      for (int ii = 0; ii < state_size; ++ii) {
+         double weighted_posdot = der_weight * velocity[ii];
+         double weighted_veldot = der_weight * accel[ii];
+         for (int jj = 0; jj < hist_len-1; ++jj) {
+            weighted_posdot += hist_weights[jj] * posdot_hist[jj][ii];
+            weighted_veldot += hist_weights[jj] * veldot_hist[jj][ii];
+         }
+         mean_vel[ii] = weighted_posdot * wscale;
+         velocity[ii] = init_vel[ii] + weighted_veldot * wscaled_dt;
+      }
+   }
+#endif
+
+}
+}
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/base_integration_group.hh

@@ -0,0 +1,297 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the extensible class BaseIntegrationGroup, an instance of which is
+ * responsible for integrating the states of a set of DynBody objects.
+ */
+
+/*
+Purpose: ()
+*/
+
+#ifndef ER7_UTILS_BASE_INTEGRATION_GROUP_HH
+#define ER7_UTILS_BASE_INTEGRATION_GROUP_HH
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Local includes
+#include "integration_controls.hh"
+// #include "integrator_interface.hh"
+#include "integrator_result.hh"
+
+
+namespace er7_utils {
+
+// Forward declarations
+class IntegratorConstructor;
+class IntegratorInterface;
+class TimeInterface;
+
+/**
+ * A BaseIntegrationGroup integrates the state of a set of objects over time.
+ * This class is designed for extensibility. Authors of derived classes should
+ * follow the extension notes in the source file.
+ *
+ * Note that an IntegrableObject is not an Er7UtilsDeletable.
+ */
+class BaseIntegrationGroup {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(BaseIntegrationGroup)
+
+
+   // Note:
+   //  This is an abstract class.
+   //  - The constructors for this class are protected.
+   //  - The assignment operator for this class is private / unimplemented.
+
+
+public:
+
+   /**
+    * BaseIntegrationGroup destructor.
+    */
+   virtual ~BaseIntegrationGroup ();
+
+
+   /**
+    * Reset the group's integrators.
+    *
+    * Resets can occur when time changes behavior (call is internal to the
+    * integration process) or when some external event would render an
+    * integrator's history invalid (call to reset comes from outside).
+    * When either happens, integrators that depend on history need to reset
+    * their internal state to indicate that the saved data are invalid.
+    */
+   void reset_integrators ()
+   {
+      // Somehow reset the group's state integrators.
+      reset_body_integrators();
+
+      // Reset the integration controls.
+      integ_controls->reset_integrator ();
+
+      // Reset the integrator interface so derivatives will be calculated.
+      integ_interface->reset_first_step_derivs_flag ();
+   }
+
+
+   /**
+    * Get the integration interface's first_step_derivs_flag.
+    */
+   bool get_first_step_derivs_flag ()
+   {
+      return integ_interface->get_first_step_derivs_flag();
+   }
+
+
+   /**
+    * Integrate to the next stage of the integration process.
+    * This is a simple pass-through to the integration controls object.
+    * @param sim_begtime Start time of integration cycle, in sim engine seconds
+    * @param sim_deltime Duration of integration cycle, in sim engine seconds
+    * @return            Integrator step number, zero = done
+    */
+   int integrate_group (
+      double sim_begtime,
+      double sim_deltime)
+   ER7_UTILS_ALWAYS_INLINE
+   {
+      return integ_controls->integrate (
+                sim_begtime,
+                sim_deltime,
+                *time_interface,
+                *integ_interface,
+                *this);
+   }
+
+
+   /**
+    * Integrate to the next stage of the integration process.
+    * This is a simple pass-through to the integration controls object.
+    * @param sim_begtime Start time of integration cycle, in sim engine seconds
+    * @return            Integrator step number, zero = done
+    */
+   int integrate_group_from (double sim_begtime)
+   ER7_UTILS_ALWAYS_INLINE
+   {
+      return integrate_group (sim_begtime, integ_interface->get_dt());
+   }
+
+
+   /**
+    * Integrate to the next stage of the integration process.
+    * This is a simple pass-through to the integration controls object.
+    * @param sim_endtime End time of integration cycle, in sim engine seconds
+    * @return            Integrator step number, zero = done
+    */
+   int integrate_group_to (double sim_endtime)
+   ER7_UTILS_ALWAYS_INLINE
+   {
+      double dt = integ_interface->get_dt();
+      return integrate_group (sim_endtime-dt, dt);
+   }
+
+
+   // Virtual methods
+
+   /**
+    * Initialize the integration group.
+    * Some integration techniques are configurable by user input, and thus
+    * the creation of the controls and integrators needs to be delayed a bit.
+    */
+   virtual void initialize_group ();
+
+
+   /**
+    * Update the interface with the sim engine to reflect integration status.
+    * @param[in] step_number  The integration step number
+    */
+   virtual void update_integration_interface (
+      unsigned int step_number)
+   {
+      integ_interface->set_step_number (step_number);
+      integ_interface->set_time (integ_controls->get_simtime());
+   }
+
+
+   // Pure virtual methods
+
+   /**
+    * Integrate the states of the integrable objects managed by this group.
+    * This function should only be called by IntegrationControls::integrate
+    * or by an override of that function.
+    * @param[in]     cycle_dyndt   Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult integrate_bodies (
+      double cycle_dyndt,
+      unsigned int target_stage) = 0;
+
+
+   /**
+    * Reset the integrators for the integrable objects managed by this group.
+    * Resets can occur when time changes behavior (call is internal to the
+    * integration process) or when some external event would render an
+    * integrator's history invalid (call comes from outside).
+    * When either happens, integrators that depend on history need to reset
+    * their internal state to indicate that the saved data are invalid.)
+    */
+   virtual void reset_body_integrators () = 0;
+
+
+protected:
+
+   // Constructors
+
+   /**
+    * BaseIntegrationGroup default constructor.
+    * This constructor exists internally for checkpoint restart use
+    * and to support derived classes' default constructors.
+    */
+   BaseIntegrationGroup ();
+
+   /**
+    * BaseIntegrationGroup non-default constructor.
+    * @param[in] integ_cotr   Integrator constructor
+    * @param[in] integ_inter  Integrator interface
+    * @param[in] time_if      Time interface
+    *
+    * The integ_controls member is initialized to a BogusIntegrationControls
+    * instance so that attempts to use a default-initialized group will fail
+    * rather than crash.
+    */
+   BaseIntegrationGroup (
+      IntegratorConstructor & integ_cotr,
+      IntegratorInterface & integ_inter,
+      TimeInterface & time_if);
+
+   /**
+    * BaseIntegrationGroup copy constructor.
+    * @param[in] source       Object to be copied
+    */
+   BaseIntegrationGroup (
+      const BaseIntegrationGroup & source);
+
+
+   /**
+    * Set the interfaces to the simulation engine and the time keeper.
+    * @param[in] integ_inter  Integrator interface
+    * @param[in] time_if      Time interface
+    */
+   void set_interfaces (
+      IntegratorInterface & integ_inter,
+      TimeInterface & time_if)
+   {
+      integ_interface = &integ_inter;
+      time_interface = &time_if;
+   }
+
+
+   // Member functions
+
+   /**
+    * Non-throwing swap function.
+    * Swap contents of 'this' with that of another BaseIntegrationGroup.
+    * @param[in] other  Item with which contents are to be swapped.
+    */
+   virtual void swap (BaseIntegrationGroup & other);
+
+
+   // Member data
+
+   IntegratorConstructor * integ_constructor; /**< trick_units(--) @n
+     The integrator constructor that is used to generate the integration
+     controls for this object and is used to generate the state integrators for
+     each integrated object whose state is integrated by this group. */
+
+   IntegratorInterface * integ_interface; /**< trick_units(--) @n
+     The interface between the integration module and the simulation engine's
+     integration structure. */
+
+   TimeInterface * time_interface; /**< trick_units(--) @n
+     The interface between the integration module and the object that
+     represents time. */
+
+   IntegrationControls * integ_controls; /**< trick_units(--) @n
+     The integration controls object that guides the integration process.
+     This object is created at construction time by calling the
+     integrator constructor's create_integration_controls method. */
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   BaseIntegrationGroup & operator= (const BaseIntegrationGroup &);
+};
+
+
+}
+
+#ifdef ER7_UTILS_NEED_AUX_INCLUDES
+#include "time_interface.hh"
+#include "integrator_constructor.hh"
+#include "integration_controls.hh"
+#include "bogus_integration_controls.hh"
+#endif
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/bogus_integration_controls.hh

@@ -0,0 +1,86 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class BogusIntegrationControls.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_BOGUS_INTEGRATION_CONTROLS_HH
+#define ER7_UTILS_BOGUS_INTEGRATION_CONTROLS_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+#include "er7_utils/interface/include/er7_class.hh"
+#include "er7_utils/interface/include/message_handler.hh"
+
+// Local includes
+#include "../include/integration_controls.hh"
+#include "../include/integration_messages.hh"
+
+
+namespace er7_utils {
+
+/**
+ * This class exists for one purpose and one purpose only, which is to enable
+ * an integration group to always have a non-null integration controls, even
+ * before the group has been properly initialized.
+ * That a group has a non-null controls object means that it is always safe
+ * (from a core dump perspective) to invoke that object's integrate method.
+ * This avoids a run-time check for a null pointer at the integration rate.
+ * The integrate method for a BogusIntegrationControls object dies,
+ * but it does not drop core.
+ */
+class BogusIntegrationControls : public IntegrationControls {
+ER7_UTILS_MAKE_SIM_INTERFACES(BogusIntegrationControls)
+
+public:
+
+   /**
+    * BogusIntegrationControls assignment operator.
+    * @param[in] src  Object to be copied.
+    */
+   BogusIntegrationControls &
+   operator = (BogusIntegrationControls src)
+   {
+      swap (src);
+      return *this;
+   }
+
+   /**
+    * Nominally, integrate.
+    * A BogusIntegrationControls dies instead.
+    */
+   virtual unsigned int integrate (
+      double, double,
+      TimeInterface&, IntegratorInterface&, BaseIntegrationGroup&);
+
+   /**
+    * Create a copy of 'this' BogusIntegrationControls object.
+    * @return Clone of 'this'.
+    */
+   virtual BogusIntegrationControls * create_copy () const;
+};
+
+}
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/first_order_ode_integrator.hh

@@ -0,0 +1,182 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class FirstOrderODEIntegrator, the base class for propagating
+ * state that is conceptually monolithic.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_ONE_STATE_INTEGRATOR_HH
+#define ER7_UTILS_ONE_STATE_INTEGRATOR_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Local includes
+#include "state_integrator.hh"
+#include "integrator_result.hh"
+
+
+namespace er7_utils {
+
+class IntegrationControls;
+
+
+/**
+ * Base class for propagating monolithic states.
+ */
+class FirstOrderODEIntegrator : public StateIntegratorInterface {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(FirstOrderODEIntegrator)
+
+
+   // Note:
+   //  This is an abstract class.
+   //  - The constructors for this class are protected.
+   //  - The assignment operator for this class is private / unimplemented.
+
+
+public:
+
+   /**
+    * FirstOrderODEIntegrator destructor.
+    */
+   virtual ~FirstOrderODEIntegrator (void) { }
+
+
+   // Member functions.
+
+   /**
+    * Create a copy of 'this', a derived FirstOrderODEIntegrator object.
+    * @return Clone of 'this'.
+    */
+   virtual FirstOrderODEIntegrator* create_copy () const = 0;
+
+   /**
+    * Set the controls object that guides this object's integration process.
+    * The default implementation does nothing.
+    * @param[in,out] controls  Integration controls (unused)
+    */
+   virtual void set_controls (
+      IntegrationControls & controls ER7_UTILS_UNUSED)
+   { }
+
+   /**
+    * Propagate state to the specified stage of the integration
+    * process for an overall integration time interval of dyn_dt.
+    *
+    * All integration techniques can assume that the status parameter will
+    * be default-initialized on entry: time scales set to 1.0, failure status
+    * and merge count set to zero. An integration technique only needs to change
+    * the time scales if they are something other than 1.0, the failure mode
+    * if the technique somehow is not successful.
+    *
+    * Note that this is a pure virtual function; instantiable subclasses must
+    * provide an implementation of this method.
+    *
+    * @param[in]     dyn_dt        Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in,out] velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      const double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position) = 0;
+
+
+protected:
+
+   // Constructors.
+
+   /**
+    * FirstOrderODEIntegrator default constructor.
+    */
+   FirstOrderODEIntegrator (void)
+   :
+      Er7UtilsDeletable (),
+      StateIntegratorInterface (),
+      state_size (0)
+   { }
+
+   /**
+    * FirstOrderODEIntegrator copy constructor.
+    * @param[in] src  Object to be copied.
+    */
+   FirstOrderODEIntegrator (const FirstOrderODEIntegrator & src)
+   :
+      Er7UtilsDeletable (),
+      StateIntegratorInterface (src),
+      state_size (src.state_size)
+   { }
+
+   /**
+    * FirstOrderODEIntegrator non-default constructor.
+    * Notes:
+    *  - The provided size is assumed to be non-zero.
+    *    Behavior is undefined if size is zero.
+    *  - The controls parameter exists for derived classes.
+    *    It's unused in the default implementation.
+    * @param[in]     size      State size
+    * @param[in,out] controls  Integration controls (unused)
+    */
+   FirstOrderODEIntegrator (
+      unsigned int size,
+      IntegrationControls & controls ER7_UTILS_UNUSED)
+   :
+      StateIntegratorInterface (),
+      state_size (size)
+   { }
+
+
+   // Member functions.
+
+   /**
+    * Swap contents with that of another.
+    * @param other  Other object with which contents are to be swapped.
+    */
+   virtual void swap (FirstOrderODEIntegrator & other);
+
+
+   // Member data
+
+   unsigned int state_size; /**< trick_units(--) @n
+      The size of the state. */
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   FirstOrderODEIntegrator & operator= (const FirstOrderODEIntegrator &);
+};
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/generalized_position_derivative.hh

@@ -0,0 +1,219 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the classes GeneralizedPositionDerivativeFunctions and
+ * GeneralizedPositionStepFunctions, which define and encapsulate
+ * pointers to various functions needed to support integration of generalized
+ * position.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_GENERALIZED_POSITION_DERIVATIVE_HH
+#define ER7_UTILS_GENERALIZED_POSITION_DERIVATIVE_HH
+
+
+// Interface includes
+#include "er7_utils/interface/include/config.hh"
+
+
+namespace er7_utils {
+
+
+/**
+ * Encapsulates pointers to functions that calculate the first and
+ * second time derivatives of generalized position.
+ */
+class GeneralizedPositionDerivativeFunctions {
+public:
+
+   // Types
+
+   /**
+    * Pointer to a function that computes the time derivative of the generalized
+    * position as a function of generalized position and generalized velocity.
+    *
+    * @param[in]  position             Generalized position vector
+    * @param[in]  velocity             Generalized velocity vector
+    * @param[out] position_derivative  Time derivative of generalized position
+    */
+   typedef void (* FirstDerivative) (
+      const double * ER7_UTILS_RESTRICT position,
+      const double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position_derivative);
+
+   /**
+    * Pointer to a function that computes the second time derivative of
+    * generalized position as a function of generalized position, generalized
+    * velocity, and generalized acceleration (the time derivative of generalized
+    * velocity).
+    *
+    * @param[in]  position                 Generalized position vector
+    * @param[in]  velocity                 Generalized velocity vector
+    * @param[in]  acceleration             Generalized velocity vector
+    * @param[out] position_2nd_derivative  Generalized position 2nd derivative
+    */
+   typedef void (* SecondDerivative) (
+      const double * ER7_UTILS_RESTRICT position,
+      const double * ER7_UTILS_RESTRICT velocity,
+      const double * ER7_UTILS_RESTRICT acceleration,
+      double * ER7_UTILS_RESTRICT position_second_derivative);
+
+
+   // Constructors and destructor
+   // Note: The copy constructor and assignment operator are implicitly defined.
+
+   /**
+    * Default constructor.
+    */
+   GeneralizedPositionDerivativeFunctions ()
+   :
+      first_deriv_fun (0),
+      second_deriv_fun (0)
+   { }
+
+
+   /**
+    * Non-default constructor.
+    * @param first_deriv_fun_in   Generalized position time derivative function
+    * @param second_deriv_fun_in  Generalized position 2nd time deriv function
+    */
+   GeneralizedPositionDerivativeFunctions (
+      FirstDerivative first_deriv_fun_in,
+      SecondDerivative second_deriv_fun_in)
+   :
+      first_deriv_fun (first_deriv_fun_in),
+      second_deriv_fun (second_deriv_fun_in)
+   { }
+
+
+   /**
+    * Destructor.
+    */
+   virtual ~GeneralizedPositionDerivativeFunctions () {}
+
+
+   // Member data
+
+   FirstDerivative first_deriv_fun; /**< trick_io(**) @n
+      Function that computes the first time derivative of
+      generalized position. */
+
+   SecondDerivative second_deriv_fun; /**< trick_io(**) @n
+      Function that computes the second time derivative of
+      generalized position. */
+
+};
+
+
+/**
+ * Encapsulates pointers to functions that are needed by Lie group integrators.
+ */
+class GeneralizedPositionStepFunctions {
+public:
+
+   // Types
+
+
+   /**
+    * Pointer to a function that advances the generalized position as a function
+    * of generalized position, generalized velocity, and delta t.
+    *
+    * @note
+    * A viable candidate must be able to handle the output generalized position
+    * pointing to the same place as the input generalized position and the
+    * output exponential map pointing to the same place as the input map.
+    *
+    * @param[in]  position          Generalized position vector.
+    * @param[in]  dtheta            Product of time and generalized velocity.
+    * @param[out] updated_position  Generalized position after the time step.
+    */
+   typedef void (* ExpMapPositionStep) (
+      const double * position,
+      const double * dtheta,
+      double * updated_position);
+
+
+   /**
+    * Pointer to a function that transforms a generalized velocity per
+    * the dexp inverse operator.
+    *
+    * @note
+    * A viable candidate must be able to handle the output generalized position
+    * pointing to the same place as the input generalized position.
+    *
+    * @param[in]  subject_velocity  Generalized velocity vector is to be
+    *                               transformed.
+    * @param[in]  dtheta            dtheta vector computed during the previous
+    *                               integration step.
+    * @param[out] xform_velocity    Generalized position after the time step.
+    */
+   typedef void (* DexpinvTransformVelocity) (
+      const double * subject_velocity,
+      const double * dtheta,
+      double * xform_velocity);
+
+
+   // Constructors and destructor
+   // Note: The copy constructor and assignment operator are implicitly defined.
+
+   /**
+    * Default constructor.
+    */
+   GeneralizedPositionStepFunctions ()
+   :
+      expmap_step_fun (0),
+      dexpinv_xform_fun (0)
+   { }
+
+
+   /**
+    * Non-default constructor.
+    * @param expmap_step_fun_in    Exponential map step function
+    * @param dexpinv_xform_fun_in  Velocity transformation function
+    */
+   GeneralizedPositionStepFunctions (
+      ExpMapPositionStep expmap_step_fun_in,
+      DexpinvTransformVelocity dexpinv_xform_fun_in)
+   :
+      expmap_step_fun (expmap_step_fun_in),
+      dexpinv_xform_fun (dexpinv_xform_fun_in)
+   { }
+
+
+   /**
+    * Destructor.
+    */
+   virtual ~GeneralizedPositionStepFunctions () {}
+
+
+   // Member data
+
+   ExpMapPositionStep expmap_step_fun; /**< trick_io(**) @n
+      Exponential map step function. */
+
+   DexpinvTransformVelocity dexpinv_xform_fun; /**< trick_io(**) @n
+      Velocity transformation function. */
+
+};
+
+}
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/integrable_object.hh

@@ -0,0 +1,194 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class IntegrableObject.
+ */
+
+/*
+Purpose: ()
+*/
+
+#ifndef ER7_UTILS_INTEGRABLE_BODY_HH
+#define ER7_UTILS_INTEGRABLE_BODY_HH
+
+// System includes
+#include <cstddef>
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+
+namespace er7_utils {
+
+class BaseIntegrationGroup;
+class IntegrationControls;
+class IntegratorConstructor;
+class IntegratorResult;
+class TimeInterface;
+
+/**
+ * An IntegrableObject is managed by an IntegrationGroup and under direction
+ * of that group, the IntegrableObject integrates the state of some object.
+ *
+ * Note that an IntegrableObject is not an Er7UtilsDeletable.
+ */
+class IntegrableObject {
+ER7_UTILS_MAKE_SIM_INTERFACES(IntegrableObject)
+
+
+   // Note:
+   //  This is an abstract class.
+   //  - The constructors for this class are protected.
+   //  - The assignment operator for this class is private / unimplemented.
+
+
+public:
+
+   /**
+    * Destructor.
+    */
+   virtual ~IntegrableObject () {}
+
+   /**
+    * Create the integration model state integrators that the object will
+    * use to integrate state.@n
+    * An implementation will typically call one of IntegratorConstructor's
+    * create_XXX_integrator methods to create the necessary integrators.
+    * @param[in] generator  Integrator constructor to be used to create
+                            state integrators.
+    * @param[in] controls   The integration controls created the integrator
+                            constructor's create_integration_controls method.
+    * @param[in] time_if    Time interface that timestamps the integrated state.
+    */
+   virtual void create_integrators (
+      const IntegratorConstructor & generator,
+      IntegrationControls & controls,
+      const TimeInterface & time_if) = 0;
+
+   /**
+    * Destroy the integrators created by the create_integrators method.
+    */
+   virtual void destroy_integrators (void) = 0;
+
+   /**
+    * Restore the integrators on restart.
+    */
+   virtual void reset_integrators (void) = 0;
+
+   /**
+    * Integrate state by the specified dynamic time interval.
+    *
+    * All IntegrableObjects can assume that the status parameter will
+    * be default-initialized on entry: time scales set to 1.0, failure status
+    * and merge count set to zero. An integration technique only needs to change
+    * the time scales if they are something other than 1.0, the failure mode
+    * if the technique somehow is not successful.
+    *
+    * @param[in]     dyn_dt        Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult integrate (
+      double dyn_dt,
+      unsigned int target_stage)
+   = 0;
+
+
+   /**
+    * Setter for the integration group.
+    * @param[in] integ_group_in New integration group
+    */
+   void set_integration_group (
+      BaseIntegrationGroup & integ_group_in)
+   {
+      if (integ_group != NULL) {
+         destroy_integrators ();
+      }
+      integ_group = &integ_group_in;
+   }
+
+   /**
+    * Reset (clear) the integration group.
+    */
+   void clear_integration_group (void)
+   {
+      if (integ_group != NULL) {
+         destroy_integrators ();
+      }
+      integ_group = NULL;
+   }
+
+   /**
+    * Getter for the integration group.
+    * @return Current integration group
+    */
+   BaseIntegrationGroup * get_integration_group (void)
+   {
+      return integ_group;
+   }
+
+
+protected:
+
+   /**
+    * Default constructor.
+    */
+   IntegrableObject () : integ_group(NULL) {}
+
+   /**
+    * Copy constructor.
+    * Note: The integration group is not copied. A copy of an IntegrableObject
+    * does not belong to the same group as the source object.
+    * It has to be put in some group, explicitly.
+    */
+   IntegrableObject (const IntegrableObject &) : integ_group(NULL) {}
+
+   /**
+    * Non-throwing swap.
+    * @param other  Object whose contents are to be swapped.
+    */
+   void swap (IntegrableObject & other)
+   {
+      BaseIntegrationGroup * temp = integ_group;
+      integ_group = other.integ_group;
+      other.integ_group = temp;
+   }
+
+private:
+
+   // Member data
+
+   BaseIntegrationGroup * integ_group; /**< trick_units(--) @n
+      The integration group that integrates this body, and possibly others. */
+
+
+   // Unimplemented member functions
+
+   /**
+    * Not implemented.
+    */
+   IntegrableObject & operator= (const IntegrableObject &);
+
+};
+
+}
+
+
+#endif
+
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/integration_controls.hh

@@ -0,0 +1,331 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class IntegrationControls, the base class used for controlling
+ * the integration process. Every integration group contains an
+ * IntegrationControls object, created by means of some integration constructor.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_INTEGRATION_CONTROLS_HH
+#define ER7_UTILS_INTEGRATION_CONTROLS_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+#include "er7_utils/interface/include/deletable.hh"
+
+// Local includes
+#include "integrator_interface.hh"
+
+
+namespace er7_utils {
+
+// Forward declarations
+class TimeInterface;
+class BaseIntegrationGroup;
+
+
+
+/**
+ * The class IntegrationControls defines the basics needed for controlling
+ * the integration process.
+ */
+class IntegrationControls : virtual public Er7UtilsDeletable {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(IntegrationControls)
+
+
+   // Note:
+   //  This is an abstract class.
+   //  - The constructors for this class are protected.
+   //  - The assignment operator for this class is private / unimplemented.
+
+
+public:
+
+   /**
+    * IntegrationControls destructor.
+    */
+   virtual ~IntegrationControls (void);
+
+
+   // Member functions.
+
+   /**
+    * Create a copy of 'this', a derived IntegrationControls object.
+    * @return Clone of 'this'.
+    */
+   virtual IntegrationControls * create_copy (void) const = 0;
+
+
+   // Getters for which this is no setter
+
+   /**
+    * Get the current integration time.
+    * @return integ_simtime value.
+    */
+   double ER7_UTILS_ALWAYS_INLINE
+   get_simtime (void) const
+   {
+      return integ_simtime;
+   }
+
+   /**
+    * Get the final_stage data member.
+    * @return final_stage value.
+    */
+   unsigned int ER7_UTILS_ALWAYS_INLINE
+   get_final_stage (void) const
+   {
+      return final_stage;
+   }
+
+   /**
+    * Get the step_number data member.
+    * @return step_number value.
+    */
+   unsigned int ER7_UTILS_ALWAYS_INLINE
+   get_step_number (void) const
+   {
+      return step_number;
+   }
+
+   /**
+    * Determine whether the controller is in the initial cycle of a loop.
+    * @return True if first cycle of a loop, false otherwise.
+    */
+   bool ER7_UTILS_ALWAYS_INLINE
+   in_initial_cycle (void) const
+   {
+      return step_number == 0;
+   }
+
+   /**
+    * Determine the expected step number, assuming integration succeeds.
+    * @return Expected step number.
+    */
+   unsigned int ER7_UTILS_ALWAYS_INLINE
+   get_expected_step_number (void) const
+   {
+      if (transition_table[cycle_stage] == final_stage) {
+         return 0;
+      }
+      else {
+         return step_number + 1;
+      }
+   }
+
+
+   // Paired getters and setters
+
+   /**
+    * Get the reset_needed flag.
+    * @return Value of reset_needed flag.
+    */
+   bool ER7_UTILS_ALWAYS_INLINE
+   get_reset_needed (bool value ER7_UTILS_UNUSED) const
+   {
+      return reset_needed;
+   }
+
+   /**
+    * Set the reset_needed flag.
+    */
+   void ER7_UTILS_ALWAYS_INLINE
+   set_reset_needed ()
+   {
+      reset_needed = true;
+   }
+
+   /**
+    * Get a transition table element.
+    * @param[in] index  Table element to be retrieved.
+    * @return  Transition table element value.
+    */
+   unsigned int
+   get_transition_table_element (unsigned int index) const
+   {
+      return transition_table[index];
+   }
+
+   /**
+    * Set a transition table element.
+    * This method should be used with great caution.
+    * @param[in] index  Table element to be set.
+    * @param[in] value  Value to be inserted at that element.
+    */
+   void
+   set_transition_table_element (
+      unsigned int index,
+      unsigned int value)
+   {
+      transition_table[index] = value;
+   }
+
+
+   // Integration methods
+
+   /**
+    * Make one step in the process that eventually integrates state from
+    * the start_time to start_time+sim_dt.
+    * @return Step number; zero when finished.
+    * @param[in] start_time
+    *    The simulation engine time at the start of the integration tour.
+    * @param[in] sim_dt
+    *    The difference between the simulation time at the end and start of the
+    *    integration tour.
+    * @param[in,out] time_interface
+    *    Object external to the ER7 utilities suite that represents time.
+    * @param[in,out] integ_interface
+    *    Interface with the simulation engine for this integration controls.
+    * @param[in,out] integ_group
+    *    The integration group that contains this integration controls.
+    */
+   virtual unsigned int integrate (
+      double start_time, double sim_dt,
+      TimeInterface & time_interface,
+      IntegratorInterface & integ_interface,
+      BaseIntegrationGroup & integ_group) = 0;
+
+
+   /**
+    * Reset an integration controls for events such as changes in the
+    * nature in time, discontinuities in the derivatives, etc.
+    *
+    * The default implementation does nothing.
+    * Multi-cycle and multi-step integrators that override this default
+    * implementation should respond in some technique-specific way to
+    * that resets an integration controls to its initial state.
+    */
+   virtual void reset_integrator (void) {}
+
+
+protected:
+
+   // Constructors.
+
+   /**
+    * IntegrationControls default constructor.
+    * This constructor is needed for restart.
+    */
+   IntegrationControls (void);
+
+   /**
+    * IntegrationControls copy constructor.
+    * @param[in] source  The IntegrationControls to be copied.
+    */
+   IntegrationControls (const IntegrationControls &);
+
+   /**
+    * IntegrationControls non-default constructor.
+    * This is the constructor invoked by an integrator constructor.
+    * @param[in] number_stages_in  Number of stages in an integration cycle.
+    */
+   explicit IntegrationControls (unsigned int number_stages_in);
+
+
+   // Member functions.
+
+   /**
+    * Non-throwing swap function.
+    * Swap contents of 'this' with that of another IntegrationControls.
+    * @param[in] other  Item with which contents are to be swapped.
+    */
+   virtual void swap (IntegrationControls & other);
+
+
+   // Member data.
+
+   double time_scale_factor; /**< trick_units(--) @n
+      Ratio of dynamic delta time to simulation engine delta time. */
+
+
+   double integ_starttime; /**< trick_units(--) @n
+      The simulation time of the start of the current integration loop as
+      requested by the simulation engine. */
+
+   double integ_simdt; /**< trick_units(--) @n
+      The simulation time span of the current integration loop as
+      requested by the simulation engine. */
+
+   double integ_dyndt; /**< trick_units(--) @n
+      The dynamic time span corresponding to integ_simdt. */
+
+   double integ_time_scale; /**< trick_units(--) @n
+      The time scale factor, with 0.0 representing the start of the
+      integration interval and 1.0 representing the end of the interval. */
+
+   double integ_simtime; /**< trick_units(--) @n
+      The current simulation time. */
+
+
+   unsigned int * transition_table; /**< trick_units(--) @n
+      A number_stages array that directs the transitions from one integration
+      stage to the next.
+      The constructor for this class populates this array with values
+      1 (element 0) to number_stages (element number_stages-1). */
+
+
+   unsigned int step_number; /**< trick_units(--) @n
+      Starts at zero at the start of a tour, increments by one per stage,
+      and resets to zero at the end of a tour. */
+
+   unsigned int cycle_stage; /**< trick_units(--) @n
+      The technique-specific finite state machine state number of the current
+      stage. The cycle_stage starts at zero, nominally incrementing by one for
+      each cycle, until it attains a value equal to the final_stage. At this
+      point the cycle begins anew with a cycle_stage of zero. */
+
+#if 0
+   unsigned int target_stage; /**< trick_units(--) @n
+      The technique-specific finite state machine state number that the TwoState
+      integrators should attempt to reach, nominally cycle_stage+1. Simple
+      integrators will always reach this state. More complex ones may not, in
+      which case they should set attained_stage to indicate the stage that was
+      reached. */
+#endif
+
+   unsigned int final_stage; /**< trick_units(--) @n
+      The stage number at which the cycle_stage resets to zero. */
+
+   unsigned int number_stages; /**< trick_units(--) @n
+      The size of the transition table, nominally final_stage. */
+
+   bool reset_needed; /**< trick_units(--) @n
+      Wen set, indicates that a reset needs to be performed
+      at the start of the next integration cycle.
+      The flag will be cleared when the reset is performed. */
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   IntegrationControls & operator= (const IntegrationControls &);
+};
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/integration_messages.hh

@@ -0,0 +1,94 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class IntegrationMessages, the class that specifies the
+ * message IDs used in the integration model.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_INTEGRATION_MESSAGES_HH
+#define ER7_UTILS_INTEGRATION_MESSAGES_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+
+namespace er7_utils {
+
+/**
+ * Declares messages associated with the integration model.
+ */
+class IntegrationMessages {
+
+   // This is a static class.
+   // The default constructor, copy constructor, destructor, and assignment
+   // operator for this class are private / unimplemented.
+
+public:
+
+   // Static member data
+
+   static char const * unsupported_option; /**< trick_units(--) @n
+      Issued when some user input is invalid. */
+
+   static char const * invalid_item; /**< trick_units(--) @n
+      Issued when an item is somehow invalid; a duplicate entry for example. */
+
+   static char const * internal_error; /**< trick_units(--) @n
+      Issued when the ER7_UTILS programmer messed up. */
+
+   static char const * invalid_request; /**< trick_units(--) @n
+      Issued when a non-ER7_UTILS programmer messed up. */
+
+   static char const * information; /**< trick_units(--) @n
+      Issued in non-error messages. */
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   IntegrationMessages (void);
+
+   /**
+    * Not implemented.
+    */
+   IntegrationMessages (const IntegrationMessages &);
+
+   /**
+    * Not implemented.
+    */
+   ~IntegrationMessages (void);
+
+   /**
+    * Not implemented.
+    */
+   IntegrationMessages & operator= (const IntegrationMessages &);
+
+};
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/integration_technique.hh

@@ -0,0 +1,146 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class Integration, which enumerates the integration techniques
+ * provided by the ER7 utilities.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_INTEGRATION_TECHNIQUE_HH
+#define ER7_UTILS_INTEGRATION_TECHNIQUE_HH
+
+
+namespace er7_utils {
+
+/**
+ * Contains the enums Integration::Technique and Integration::ODEProblemType.
+ * Note: This would be better as a namespace, but swig gets in the way.
+ */
+class Integration {
+
+   // This is a static class.
+   // The default constructor, copy constructor, destructor, and assignment
+   // operator for this class are private / unimplemented.
+
+public:
+
+   /**
+    * Enumerates the integration techniques supported directly by this package.
+    */
+   enum Technique {
+      Unspecified            =   0, //!< Default initial value
+
+      Euler                  =   1, //!< Basic Euler
+      SymplecticEuler        =   2, //!< Symplectic Euler
+
+      Beeman                 =  11, //!< Beeman's algorithm
+      NystromLear2           =  14, //!< Second order Nystrom-Lear
+      PositionVerlet         =  15, //!< Position verlet
+      RK2Heun                =  16, //!< Heun's method
+      RK2Midpoint            =  17, //!< Midpoint method
+      VelocityVerlet         =  19, //!< Velocity verlet
+
+      ModifiedMidpoint4      =  21, //!< Modified midpoint
+
+      AdamsBashforthMoulton4 =  31, //!< Fourth order Adams-Bashforth-Moulton
+      RungeKutta4            =  34, //!< Fourth order Runga Kutta
+      RKGill4                =  35, //!< Fourth order Runga Kutta Gill
+
+      RKNystrom4             =  36, //!< Fourth order Runga Kutta Nystrom
+
+      RKFehlberg45           =  44, //!< Runge-Kutta Fehlberg 4/5
+
+      RKFehlberg78           =  74, //!< Runge-Kutta Fehlberg 7/8
+
+// Gauss Jackson isn't in this alpha release.
+#if 0
+      GaussJackson           = 121, //!< Gauss-Jackson
+#endif
+
+      Unsupported            = 998, //!< Catch-all for unsupported techniques
+      Invalid                = 999  //!< Catch-all for foul ups
+   };
+
+
+   /**
+    * Enumerates the types of problems to be solved.
+    */
+   enum ODEProblemType {
+
+      FirstOrderODE,                   /**<
+         Problem is a first order ODE where an initial state is to be propated
+         in time using the state derivatives. The integrate function takes
+         target stage, delta t, state, and state derivatives as inputs
+         and updates the state in place. */
+
+      SimpleSecondOrderODE,            /**<
+         Problem is a second order ODE in which the time derivative of position
+         is the velocity. An initial position and velocity are to be propated
+         in time using acceleration. The integrate function takes
+         target stage, delta t, position, velocity, and acceleration as inputs
+         and updates position and velocity in place. */
+
+      GeneralizedDerivSecondOrderODE,  /**<
+         Problem is a generalized second order ODE in which the time derivative
+         of generalized position is some function of position and velocity.
+         As with the SimpleSecondOrderODE, an initial position and velocity are
+         to be propated in time using acceleration. The integrate function takes
+         target stage, delta t, position, velocity, and acceleration as inputs
+         and updates position and velocity in place. */
+
+      GeneralizedStepSecondOrderODE    /**<
+         Problem is a generalized second order ODE in which generalized position
+         is advances by some function of generalized position and velocity.
+         As with the SimpleSecondOrderODE, an initial position and velocity are
+         to be propated in time using acceleration. The integrate function takes
+         target stage, delta t, position, velocity, and acceleration as inputs
+         and updates position and velocity in place. */
+
+   };
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   Integration (void);
+
+   /**
+    * Not implemented.
+    */
+   Integration (const Integration &);
+
+   /**
+    * Not implemented.
+    */
+   ~Integration (void);
+
+   /**
+    * Not implemented.
+    */
+   Integration & operator= (const Integration &);
+};
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/integrator_constructor.hh

@@ -0,0 +1,350 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class IntegratorConstructor, the base class used to construct
+ * synchronized integration controls and state integrators.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_INTEGRATOR_CONSTRUCTOR_HH
+#define ER7_UTILS_INTEGRATOR_CONSTRUCTOR_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+#include "er7_utils/interface/include/deletable.hh"
+
+// Integrator includes
+#include "generalized_position_derivative.hh"
+#include "integration_technique.hh"
+
+
+namespace er7_utils {
+
+class IntegrationControls;
+class IntegratorResult;
+class IntegratorResultMerger;
+class FirstOrderODEIntegrator;
+class SecondOrderODEIntegrator;
+
+/**
+ * The base class for all integrator constructors.
+ * An integrator constructor creates time and state integrators that work in
+ * a synchronized fashion. Consider the midpoint method. A midpoint method
+ * state integrator propagates state to the midpoint of the integration time
+ * interval on the first intermediate step of an integration cycle. It then
+ * propagates state to the end of the integration time interval on the second
+ * (and last) intermediate step. The state integrator dictates the behavior
+ * of the midpoint method time integrator.
+
+ * Some other technique will involve a different number of intermediate steps
+ * that jump to different points in time. The idea of an integrator constructor
+ * is to ensure that the method used for propagating time is consistent with
+ * the method used to propagate state.
+ */
+class IntegratorConstructor : virtual public Er7UtilsDeletable {
+ER7_UTILS_MAKE_SIM_INTERFACES(IntegratorConstructor)
+
+
+   // Note:
+   //  This is an abstract class.
+   //  - The constructors for this class are protected.
+   //  - The assignment operator for this class is protected / does nothing.
+
+
+public:
+
+   /**
+    * IntegratorConstructor destructor.
+    *
+    * Note well: This class and its derived classes do allocate resources,
+    * in the form of the created state and time integrators. However,
+    * the burden of deleting those integrators falls upon the caller
+    * rather than on this or derivative classes.
+    */
+   virtual ~IntegratorConstructor (void) {}
+
+
+   /**
+    * Identify the integrator constructor by class name.
+    * Some of the constructors cannot create state integrators for all of
+    * the create_XXX_integrator methods elaborated below.
+    * The default implementation is to croak, identifying the technique by name.
+    */
+   virtual const char * get_class_name (void) const = 0;
+
+
+   /**
+    * Indicates whether the integrator constructor implements the
+    * create_XXX_integrator function corresponding to the problem type:
+    *
+    * Problem Type                   | create_XXX_integrator Function
+    * -------------------------------|-----------------------------------
+    * FirstOrderODE                  | first_order_ode
+    * SimpleSecondOrderODE           | second_order_ode
+    * GeneralizedDerivSecondOrderODE | generalized_deriv_second_order_ode
+    * GeneralizedStepSecondOrderODE  | generalized_step_second_order_ode
+    *
+    * Note that the default is to always return true. Techniques that do
+    * not implement all of the create_XXX_integrator functions should
+    * override this default implementation.
+    *
+    * @param problem_type  Specifies the type of ODE to be solved.
+    * @return True if the constructor provides a means to solve the problem,
+    *         false otherwise.
+    *
+    */
+   virtual bool provides (
+      Integration::ODEProblemType problem_type ER7_UTILS_UNUSED)
+   const
+   { return true; }
+
+
+   /**
+    * Indicates whether the problem type is implemented using the
+    * integration technique corresponding to the integrator constructor.
+    * This is distinct from provides() in that an integrator constructor
+    * can implement the corresponding create_XXX_integrator function by
+    * creating a state integrator from a different but related technique.
+    *
+    * Example: Symplectic Euler, being a second order technique, cannot solve
+    * first order ODE problems. The symplectic Euler integrator constructor
+    * implements its create_first_order_ode_integrator function by creating
+    * a first order Euler state integrator to act as a surrogate.
+    * Thus symplectic Euler returns true for provides(FirstOrderODE) but
+    * returns false for implements(FirstOrderODE).
+    *
+    * Note that the default is to always return true. Techniques that do
+    * not implement all of the create_XXX_integrator functions should
+    * override this default implementation.
+    *
+    * @param problem_type  Specifies the type of ODE to be solved.
+    * @return True if the technique directly implements a solution to the
+    *         specified problem, false otherwise.
+    *
+    */
+   virtual bool implements (
+      Integration::ODEProblemType problem_type ER7_UTILS_UNUSED)
+   const
+   { return true; }
+
+
+   /**
+    * There's no need to call the typically computationally expensive derivative
+    * on a step if the derivatives aren't used in that step. Some of the
+    * implemented techniques do not use the results from these derivative jobs
+    * on the initial step of an integration cycle. This method indicates whether
+    * the integration technique needs the derivative jobs called on the initial
+    * step of an integration cycle.
+    * The default is to return true; all but a small number of the implemented
+    * techniques do use the derivative information on the initial step. Those
+    * techniques that don't make use of the derivative calculations should
+    * override this method.
+    * @return True if the technique needs derivatives on the initial step.
+    */
+   virtual bool need_first_step_derivatives (void) const
+   { return true; }
+
+
+   /**
+    * Indicate whether the integration technique will always takes a
+    * pre-determined number of integration steps per integration cycle.
+    * A fixed-step integrator may not always use the same number of steps
+    * every cycle. For example, a technique that requires priming may use
+    * a different number of steps during priming than it uses once primed.
+    * To qualify as a fixed-step integrator,
+    *  - The number of steps must be determinable at any point in the cycle, and
+    *  - The maximum number of steps must be fixed.@n
+    * The default implementation is to return true; the technique is fixed-step.
+    * Variable step integrators should override this default.
+    * @return True if the technique is a fixed step integrator.
+    */
+   virtual bool is_fixed_step_integrator (void) const
+   { return true; }
+
+
+   /**
+    * Indicate whether the integration technique explicitly solves a second
+    * order ODE.
+    *
+    * The default implementation is to return false. Second order techniques
+    * should override this default.
+    *
+    * @return True if the technique is for a second order ODE,
+    *    false if the technique targets first order ODEs.
+    */
+   virtual bool is_second_order_ode_technique (void) const
+   { return false; }
+
+
+   /**
+    * Return the buffer size needed by engines such as Trick that need to
+    * allocate space separately from the integration technique.@n
+    * The default implementation is to return the transition table size.
+    * @return Largest number of steps in any integration cycle
+    */
+   virtual unsigned int get_buffer_size (void) const
+   { return get_transition_table_size(); }
+
+
+   /**
+    * Return the size of the state transition table.
+    * This pertains to the table needed by the technique itself,
+    * not techniques used for priming.
+    * @return Number of states in the state machine.
+    */
+   virtual unsigned int get_transition_table_size (void) const = 0;
+
+
+   /**
+    * Create a copy of 'this', a derived IntegratorConstructor object.
+    * @return Clone of 'this'.
+    */
+   virtual IntegratorConstructor * create_copy (void) const = 0;
+
+
+   /**
+    * Create an integration controls object that guides the integration process.
+    * @return Constructed integration controls object
+    */
+   virtual IntegrationControls * create_integration_controls (
+      void) const = 0;
+
+
+   /**
+    * Create an integrator results merger object that merges the results from
+    * a single integrator into an overall result.
+    * @return Constructed integrator results merger object.
+    */
+   virtual IntegratorResultMerger * create_integrator_results_merger (
+      void) const;
+
+
+   /**
+    * Create an integrator that propagates a first order ODE.
+    *
+    * Note well: The default implementation complains and returns NULL.
+    * A derived class should override this default implementation and
+    * create a FirstOrderODEIntegrator that implements the technique
+    * corresponding to that derived class.
+    *
+    * @return Constructed state integrator
+    * @param[in]      size     State size
+    * @param [in,out] controls Integration controls to associate with the
+    *    created integrator.
+    */
+   virtual FirstOrderODEIntegrator * create_first_order_ode_integrator (
+      unsigned int size,
+      IntegrationControls & controls) const;
+
+
+   /**
+    * Create an integrator that propagates a second order ODE for which the
+    * time derivative of position is the velocity.
+    *
+    * Note well: The default implementation complains and returns NULL.
+    * A derived class should override this default implementation and
+    * create a SecondOrderODEIntegrator that implements the technique
+    * corresponding to that derived class.
+    *
+    * @return Constructed state integrator
+    * @param[in]      size     State size
+    * @param [in,out] controls Integration controls to associate with the
+    *    created integrator.
+    */
+   virtual SecondOrderODEIntegrator * create_second_order_ode_integrator (
+      unsigned int size,
+      IntegrationControls & controls) const;
+
+
+   /**
+    * Create an integrator that propagates a second order ODE for which the
+    * time derivative of generalized position is a function of generalized
+    * position and generalized velocity.
+    *
+    * Note well: The default implementation complains and returns NULL.
+    * A derived class should override this default implementation and
+    * create a SecondOrderODEIntegrator that implements the technique
+    * corresponding to that derived class.
+    *
+    * @return Constructed state integrator
+    * @param[in]     position_size  Size of the generalized position vector
+    * @param[in]     velocity_size  Size of the generalized velocity vector
+    * @param[in]     deriv_funs     Derivative functions container
+    * @param[in,out] controls       Integration controls to associate with the
+    *    created integrator.
+    */
+   virtual SecondOrderODEIntegrator *
+   create_generalized_deriv_second_order_ode_integrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionDerivativeFunctions & deriv_funs,
+      IntegrationControls & controls) const;
+
+
+   /**
+    * Create an integrator that propagates a second order ODE in which the
+    * generalized position is propagated via the position step function.
+    * Note well: The default implementation complains and returns NULL.
+    * @return Constructed state integrator
+    * @param[in]     position_size  Size of the generalized position vector
+    * @param[in]     velocity_size  Size of the generalized velocity vector
+    * @param[in]     step_funs      Step functions container
+    * @param[in,out] controls       Integration controls to associate with the
+    *    created integrator.
+    */
+   virtual SecondOrderODEIntegrator *
+   create_generalized_step_second_order_ode_integrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionStepFunctions & step_funs,
+      IntegrationControls & controls) const;
+
+
+protected:
+
+   /**
+    * IntegratorConstructor default constructor.
+    */
+   IntegratorConstructor (void)
+   : Er7UtilsDeletable()
+   { }
+
+   /**
+    * IntegratorConstructor copy constructor.
+    */
+   IntegratorConstructor (const IntegratorConstructor &)
+   : Er7UtilsDeletable()
+   { }
+
+   /**
+    * IntegratorConstructor assignment operator; no-op.
+    */
+   IntegratorConstructor & operator= (const IntegratorConstructor &)
+   { return *this; }
+
+};
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/integrator_constructor_factory.hh

@@ -0,0 +1,119 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class IntegratorConstructorFactory, which creates an integrator
+ * constructor based on a Trick integration structure.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_INTEGRATOR_CONSTRUCTOR_FACTORY_HH
+#define ER7_UTILS_INTEGRATOR_CONSTRUCTOR_FACTORY_HH
+
+// System includes
+
+// ER7 utilities includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Model includes
+#include "integration_technique.hh"
+
+
+namespace er7_utils {
+
+class IntegratorConstructor;
+
+
+/**
+ * Factory method design pattern class for constructing an IntegratorConstructor
+ * based on technique specified in a Trick integration structure.
+ */
+class IntegratorConstructorFactory {
+
+   // This is a static class.
+   // The default constructor, copy constructor, destructor, and assignment
+   // operator for this class are private / unimplemented.
+
+public:
+
+
+   // Static member functions
+
+   // create():
+   // This static member function creates an IntegratorConstructor instance
+   // based on the provided technique.
+   static IntegratorConstructor * create (Integration::Technique type);
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   IntegratorConstructorFactory (void);
+
+   /**
+    * Not implemented.
+    */
+   IntegratorConstructorFactory (const IntegratorConstructorFactory &);
+
+   /**
+    * Not implemented.
+    */
+   ~IntegratorConstructorFactory (void);
+
+   /**
+    * Not implemented.
+    */
+   IntegratorConstructorFactory & operator= (
+      const IntegratorConstructorFactory &);
+
+};
+
+}
+
+
+#ifdef ER7_UTILS_NEED_AUX_INCLUDES
+#include "integrator_constructor.hh"
+
+#include "er7_utils/integration/abm4/include/abm4_integrator_constructor.hh"
+#include "er7_utils/integration/beeman/include/beeman_integrator_constructor.hh"
+#include "er7_utils/integration/euler/include/euler_integrator_constructor.hh"
+#if 0
+#include "er7_utils/integration/gauss_jackson/include/gauss_jackson_integrator_constructor.hh"
+#endif
+#include "er7_utils/integration/mm4/include/mm4_integrator_constructor.hh"
+#include "er7_utils/integration/nl2/include/nl2_integrator_constructor.hh"
+#include "er7_utils/integration/position_verlet/include/position_verlet_integrator_constructor.hh"
+#include "er7_utils/integration/rk2_heun/include/rk2_heun_integrator_constructor.hh"
+#include "er7_utils/integration/rk2_midpoint/include/rk2_midpoint_integrator_constructor.hh"
+#include "er7_utils/integration/rk4/include/rk4_integrator_constructor.hh"
+#include "er7_utils/integration/rkf45/include/rkf45_integrator_constructor.hh"
+#include "er7_utils/integration/rkf78/include/rkf78_integrator_constructor.hh"
+#include "er7_utils/integration/rkg4/include/rkg4_integrator_constructor.hh"
+#if 0
+#include "er7_utils/integration/rkn4/include/rkn4_integrator_constructor.hh"
+#endif
+#include "er7_utils/integration/symplectic_euler/include/symplectic_euler_integrator_constructor.hh"
+#include "er7_utils/integration/velocity_verlet/include/velocity_verlet_integrator_constructor.hh"
+#endif
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/integrator_constructor_utils.hh

@@ -0,0 +1,223 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines convenience function templates for use by integrator constructors.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_INTEGRATOR_CONSTRUCTOR_UTILS_HH
+#define ER7_UTILS_INTEGRATOR_CONSTRUCTOR_UTILS_HH
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+
+namespace er7_utils {
+
+class IntegrationControls;
+class IntegratorConstructor;
+class GeneralizedPositionDerivativeFunctions;
+class GeneralizedPositionStepFunctions;
+
+namespace integ_utils {
+
+/**
+ * Create an integration controls instance.
+ * @tparam T        Integration controls derived class
+ * @param  nstages  Number of stages in the integration cycle
+ */
+template<typename T>
+inline T* allocate_controls (
+   unsigned int nstages)
+{
+   return alloc::allocate_object<T, unsigned int> (nstages);
+}
+
+
+/**
+ * Create an integration controls instance.
+ * @tparam T                   Integration controls derived class
+ * @param  primer_constructor  Constructor that creates the primer
+ * @param  history_length      History length
+ * @param  nstages             Number of stages in the integration cycle
+ */
+template<typename T>
+inline T* allocate_controls (
+   const IntegratorConstructor & primer_constructor,
+   unsigned int history_length,
+   unsigned int nstages)
+{
+   return alloc::allocate_object<T,
+                                 const IntegratorConstructor &,
+                                 unsigned int,
+                                 unsigned int> (
+             primer_constructor, history_length, nstages);
+}
+
+
+/**
+ * Create a state integrator instance.
+ * @tparam T         State integrator derived class
+ * @param  size      State size
+ * @param  controls  Integration controls
+ */
+template<typename T>
+inline T* allocate_integrator (
+   unsigned int size,
+   IntegrationControls & controls)
+{
+   return alloc::allocate_object<T,
+                                 unsigned int,
+                                 IntegrationControls &> (
+             size, controls);
+}
+
+
+/**
+ * Create a state integrator instance.
+ * @tparam T                   State integrator derived class
+ * @param  primer_constructor  Constructor that creates the primer
+ * @param  size                State size
+ * @param  controls            Integration controls
+ */
+template<typename T>
+inline T* allocate_integrator (
+   const IntegratorConstructor & primer_constructor,
+   unsigned int size,
+   IntegrationControls & controls)
+{
+   return alloc::allocate_object<T,
+                                 const IntegratorConstructor &,
+                                 unsigned int,
+                                 IntegrationControls &> (
+             primer_constructor, size, controls);
+}
+
+
+/**
+ * Create a state integrator instance.
+ * @tparam T              State integrator derived class
+ * @param  position_size  Generalized position size
+ * @param  velocity_size  Generalized velocity size
+ * @param  deriv_funs     Position derivative functions container
+ * @param  controls       Integration controls
+ */
+template<typename T>
+inline T* allocate_integrator (
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionDerivativeFunctions & deriv_funs,
+   IntegrationControls & controls)
+{
+   return alloc::allocate_object<T,
+                                 unsigned int,
+                                 unsigned int,
+                                 const GeneralizedPositionDerivativeFunctions &,
+                                 IntegrationControls &> (
+             position_size, velocity_size, deriv_funs, controls);
+}
+
+
+/**
+ * Create a state integrator instance.
+ * @tparam T                   State integrator derived class
+ * @param  primer_constructor  Constructor that creates the primer
+ * @param  position_size       Generalized position size
+ * @param  velocity_size       Generalized velocity size
+ * @param  deriv_funs          Position derivative functions container
+ * @param  controls            Integration controls
+ */
+template<typename T>
+inline T* allocate_integrator (
+   const IntegratorConstructor & primer_constructor,
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionDerivativeFunctions & deriv_funs,
+   IntegrationControls & controls)
+{
+   return alloc::allocate_object<T,
+                                 const IntegratorConstructor &,
+                                 unsigned int,
+                                 unsigned int,
+                                 const GeneralizedPositionDerivativeFunctions &,
+                                 IntegrationControls &> (
+             primer_constructor,
+             position_size, velocity_size, deriv_funs, controls);
+}
+
+
+/**
+ * Create a state integrator instance.
+ * @tparam T              State integrator derived class
+ * @param  position_size  Generalized position size
+ * @param  velocity_size  Generalized velocity size
+ * @param  step_funs      Position step functions container
+ * @param  controls       Integration controls
+ */
+template<typename T>
+inline T* allocate_integrator (
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionStepFunctions & step_funs,
+   IntegrationControls & controls)
+{
+   return alloc::allocate_object<T,
+                                 unsigned int,
+                                 unsigned int,
+                                 const GeneralizedPositionStepFunctions &,
+                                 IntegrationControls &> (
+             position_size, velocity_size, step_funs, controls);
+}
+
+
+/**
+ * Create a state integrator instance.
+ * @tparam T                   State integrator derived class
+ * @param  primer_constructor  Constructor that creates the primer
+ * @param  position_size       Generalized position size
+ * @param  velocity_size       Generalized velocity size
+ * @param  step_funs           Position step functions container
+ * @param  controls            Integration controls
+ */
+template<typename T>
+inline T* allocate_integrator (
+   const IntegratorConstructor & primer_constructor,
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionStepFunctions & step_funs,
+   IntegrationControls & controls)
+{
+   return alloc::allocate_object<T,
+                                 const IntegratorConstructor &,
+                                 unsigned int,
+                                 unsigned int,
+                                 const GeneralizedPositionStepFunctions &,
+                                 IntegrationControls &> (
+             primer_constructor,
+             position_size, velocity_size, step_funs, controls);
+}
+
+}
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/integrator_interface.hh

@@ -0,0 +1,134 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines a generic interface for accessing / updating elements of a simulation
+ * integrator object.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_INTEGRATOR_INTERFACE_HH
+#define ER7_UTILS_INTEGRATOR_INTERFACE_HH
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+
+namespace er7_utils {
+
+
+/**
+ * The simulation engine has some kind of mechanism for guiding the integration
+ * process. The IntegratorInterface class represents in a generic sense the
+ * interface to those mechanism.
+ */
+class IntegratorInterface {
+ER7_UTILS_MAKE_SIM_INTERFACES(IntegratorInterface)
+
+
+   // Note:
+   //  This is an abstract class.
+   //  - The constructors for this class are protected.
+   //  - The assignment operator for this class is private / unimplemented.
+
+
+public:
+
+   /**
+    * Destructor.
+    */
+   virtual ~IntegratorInterface () {}
+
+
+   // Getters and setters
+
+   /**
+    * Get the integration cycle time step.
+    * @return                Simulation time delta t@n
+    * Units: s
+    */
+   virtual double get_dt () const = 0;
+
+   /**
+    * Get the flag that tells the simulation engine to compute derivatives
+    * on the initial step of each integration cycle.
+    * @return Value of the first step derivatives flag
+    */
+   virtual bool get_first_step_derivs_flag () const = 0;
+
+   /**
+    * Set the flag that tells the simulation engine to compute derivatives
+    * on the initial step of each integration cycle.
+    * @param[in] value       Value of the first step derivatives flag
+    */
+   virtual void set_first_step_derivs_flag (bool value) = 0;
+
+   /**
+    * Reset the flag that tells the simulation engine to compute derivatives
+    * on the initial step of each integration cycle. Derivatives are always
+    * needed just after a reset.
+    */
+   virtual void reset_first_step_derivs_flag () = 0;
+
+   /**
+    * Restore the flag that tells the simulation engine to compute derivatives
+    * on the initial step of each integration cycle to it's value prior to
+    * the most recent call to reset_first_step_derivs_flag.
+    */
+   virtual void restore_first_step_derivs_flag () = 0;
+
+   /**
+    * Set the step number within an integration cycle.
+    * @param[in] stepno      Step number
+    */
+   virtual void set_step_number (unsigned int stepno) = 0;
+
+   /**
+    * Set the simulation time for the start of the next integration cycle.
+    * @param[in] time        Simulation time, in simulation time seconds
+    */
+   virtual void set_time (double time) = 0;
+
+
+protected:
+
+   /**
+    * IntegratorInterface default constructor.
+    */
+   IntegratorInterface () {}
+
+   /**
+    * IntegratorInterface copy constructor.
+    */
+   IntegratorInterface (const IntegratorInterface &) {}
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   IntegratorInterface & operator = (const IntegratorInterface &);
+};
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/integrator_result.hh

@@ -0,0 +1,203 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class IntegratorResult, which encapsulates the return value
+ * from an integrator.
+ */
+
+/*
+Purpose: ()
+*/
+
+#ifndef ER7_UTILS_INTEGRATOR_RESULT_HH
+#define ER7_UTILS_INTEGRATOR_RESULT_HH
+
+
+// Interface includes
+#include "er7_utils/interface/include/config.hh"
+#include "er7_utils/interface/include/er7_class.hh"
+
+
+namespace er7_utils {
+
+
+/**
+ * Class that encapsulates the return value from an integrator and the results
+ * of merging results from multiple integrators.
+ *
+ * A state integrator returns two values, an indication of whether the
+ * integration attained the target stage, and if successful, the fractional
+ * amount of the integration time step by which time should be advanced.
+ * An IntegratorResult combines these two return values in a single object.
+ *
+ * An IntegratorResult contains one additional field, the number of results
+ * from multiple state integrators that have been merged to form a merged
+ * IntegratorResult result. Some state integrators may disagree on whether
+ * the target stage has been successfully achieved or on how time should be
+ * advanced. A technique-specific IntegratorResultMerger object is responsible
+ * for merging results from multiple state integrators.
+ *
+ * @note This class has a non-virtual destructor. It is a final class.
+ */
+class IntegratorResult {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(IntegratorResult)
+
+public:
+
+
+   // Note: The destructor, copy constructor, and assignment operator are not
+   // declared for this class.
+   // The C++ implicitly-defined versions of these functions are in force.
+
+
+   /**
+    * IntegratorResult default constructor.
+    * The two time scales are initialized to 1.0 as these are the canonical
+    * values. The failure mode is initialized to 0 to indicate success.
+    * The merge count is initialized to 0 or 1 depending on the value of
+    * the provided argument. The default value results in a merge_count of 1.
+    *
+    * @param is_integrated Indicates how the merge_count is to be initialized.
+    *    Integration controls should explicitly use false to create an
+    *    empty IntegratorResult. The default setting is for IntegratorResult
+    *    created just before calling an integrator.
+    */
+   IntegratorResult (bool is_integrated=true) ER7_UTILS_ALWAYS_INLINE
+   :
+      time_scale(1.0),
+      failure_mode(0),
+      merge_count(is_integrated ? 1 : 0)
+   { }
+
+
+   /**
+    * IntegratorResult non-default constructor, intended for use by
+    * simple integrators that always pass.
+    * The failure_mode and merge_count members are set to default values.
+    * The time_scale member is set to the provided value.
+    *
+    * @param step_factor Indicates how the time_scale is to be initialized.
+    */
+   IntegratorResult (double step_factor) ER7_UTILS_ALWAYS_INLINE
+   :
+      time_scale(step_factor),
+      failure_mode(0),
+      merge_count(1)
+   { }
+
+
+   // Getters and setters.
+
+   /**
+    * Get the IntegratorResult's time_scale data member.
+    */
+   double get_time_scale () const ER7_UTILS_ALWAYS_INLINE
+   { return time_scale; }
+
+   /**
+    * Determine whether the integration attained the target stage.
+    */
+   bool get_passed () const ER7_UTILS_ALWAYS_INLINE
+   { return failure_mode == 0; }
+
+   /**
+    * Get the IntegratorResult's failure_mode data member.
+    */
+   int get_failure_mode () const ER7_UTILS_ALWAYS_INLINE
+   { return failure_mode; }
+
+   /**
+    * Get the IntegratorResult's merge_count data member.
+    */
+   int get_merge_count () const ER7_UTILS_ALWAYS_INLINE
+   { return merge_count; }
+
+
+   /**
+    * Set the IntegratorResult's time_scale to the specified value.
+    */
+   void set_time_scale (double value) ER7_UTILS_ALWAYS_INLINE
+   { time_scale = value; }
+
+   /**
+    * Denote that the integration has attained the target stage.
+    */
+   void set_passed () ER7_UTILS_ALWAYS_INLINE
+   { failure_mode = 0; }
+
+   /**
+    * Denote that the integration has failed to attain the target stage.
+    */
+   void set_failed () ER7_UTILS_ALWAYS_INLINE
+   { failure_mode = 1; }
+
+   /**
+    * Set the IntegratorResult's failure_mode to the specified value.
+    */
+   void set_failure_mode (int value) ER7_UTILS_ALWAYS_INLINE
+   { failure_mode = value; }
+
+   /**
+    * Mark an IntegratorResult as containing a true result.
+    * This function should be used when
+    *  - an IntegratorResult was created as an empty object for use by an
+    *    individual integrator, and
+    *  - That integration was performed.
+    */
+   void mark_as_integrated () ER7_UTILS_ALWAYS_INLINE
+   { merge_count = 1; }
+
+   /**
+    * Mark an IntegratorResult as containing a null result.
+    * This function should be used when
+    *  - an IntegratorResult was created as a default object for use by an
+    *    individual integrator, and
+    *  - That integration was not performed.
+    */
+   void mark_as_not_integrated () ER7_UTILS_ALWAYS_INLINE
+   { merge_count = 0; }
+
+   /**
+    * Increment the IntegratorResult's merge_count by the specified value.
+    */
+   void increment_merge_count (int value = 1) ER7_UTILS_ALWAYS_INLINE
+   { merge_count += value; }
+
+private:
+
+   // Member data.
+
+   double time_scale; /**< trick_units(--) @n
+      Fraction of the integration interval time step by which the time should be
+      advanced. A value of 0.0 means time should be set to the start of the
+      integration interval; a value of 1.0 means time should be advanced to
+      the end of the interval. */
+
+   int failure_mode; /**< trick_units(--) @n
+      Zero indicates the integrator successfully reached the target stage.
+      A non-zero value indicates some technique-dependent failure. */
+
+   int merge_count; /**< trick_units(--) @n
+      The number of times another IntegratorResult has been merged into this
+      IntegratorResult. A value of zero means this is a virgin result. */
+};
+
+
+}
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/integrator_result_merger.hh

@@ -0,0 +1,123 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class IntegratorResultMerger, the base class for merging
+ * an IntegratorResult from a state integrator into a merged IntegratorResult.
+ */
+
+/*
+Purpose: ()
+*/
+
+#ifndef ER7_UTILS_INTEGRATOR_RESULT_MERGER_HH
+#define ER7_UTILS_INTEGRATOR_RESULT_MERGER_HH
+
+
+// Interface includes
+#include "er7_utils/interface/include/config.hh"
+#include "er7_utils/interface/include/deletable.hh"
+
+
+// Forward declarations
+namespace er7_utils {
+class IntegratorResult;
+}
+
+
+namespace er7_utils {
+
+
+/**
+ * Class that merges a new integrator result into a combined result.
+ * Note that this class is stateless; it contains no data members.
+ *  Derived classes must also be stateless.
+ */
+class IntegratorResultMerger : public Er7UtilsDeletable {
+public:
+
+
+   // Note: The default constructor, copy constructor, and assignment operator
+   // are not declared for this class.
+   // The C++ implicitly-defined versions of these functions are in force.
+
+
+   /**
+    * IntegratorResultMerger destructor.
+    */
+   virtual ~IntegratorResultMerger() {}
+
+
+   /**
+    * Merge an IntegratorResult into another.
+    * The default implementation pertains to simple, fixed step integration
+    * techniques. Adaptive techniques or techniques that can have a failure
+    * mode other than zero or one must override this default.
+    * @return True if merger was successful, false if some error occurred.
+    * @param[in]      new_result     Size of the generalized position vector
+    * @param[in,out]  merged_result  Size of the generalized position vector
+    */
+   virtual bool merge_integrator_result (
+      const IntegratorResult & new_result,
+      IntegratorResult & merged_result) const;
+
+
+   /**
+    * Create a copy of 'this' IntegratorResultMerger object.
+    * @return Clone of 'this'.
+    */
+   virtual IntegratorResultMerger * create_copy (void) const;
+};
+
+
+/**
+ * Bogus IntegratorResultMerger.
+ * The merge_integrator_result function croaks.
+ */
+class BogusIntegratorResultMerger : public IntegratorResultMerger {
+public:
+
+
+   // Note: The default constructor, copy constructor, and assignment operator
+   // are not declared for this class.
+   // The C++ implicitly-defined versions of these functions are in force.
+
+
+   /**
+    * BogusIntegratorResultMerger destructor.
+    */
+   virtual ~BogusIntegratorResultMerger() {}
+
+
+   /**
+    * Croak.
+    */
+   virtual bool merge_integrator_result (
+      const IntegratorResult &, IntegratorResult &) const;
+
+
+   /**
+    * Create a copy of 'this' BogusIntegratorResultMerger object.
+    * @return Clone of 'this'.
+    */
+   virtual IntegratorResultMerger * create_copy (void) const;
+};
+
+
+}
+
+#endif
+
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/integrator_result_merger_container.hh

@@ -0,0 +1,143 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class IntegratorResultMergerContainer, which encapsulates an
+ * IntegratorResultMerger using the RAII idiom.
+ */
+
+/*
+Purpose: ()
+*/
+
+#ifndef ER7_UTILS_INTEGRATOR_RESULT_MERGER_CONTAINER_HH
+#define ER7_UTILS_INTEGRATOR_RESULT_MERGER_CONTAINER_HH
+
+
+// Local includes
+#include "integrator_result_merger.hh"
+
+// Interface includes
+#include "er7_utils/interface/include/config.hh"
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Forward declarations
+namespace er7_utils {
+class IntegratorConstructor;
+class IntegratorResult;
+}
+
+
+namespace er7_utils {
+
+/**
+ * A IntegratorResultMergerContainer is an RAII class that encapsulates an
+ * IntegratorResultMerger object so that other objects need not worry about
+ * memory management for an IntegratorResultMerger object.
+ */
+class IntegratorResultMergerContainer {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(IntegratorResultMergerContainer)
+
+public:
+
+   /**
+    * IntegratorResultMergerContainer default constructor.
+    */
+   IntegratorResultMergerContainer ();
+
+   /**
+    * IntegratorResultMergerContainer non-default constructor.
+    * @param[in] integ_cotr  Integrator constructor
+    */
+   IntegratorResultMergerContainer (
+      const IntegratorConstructor & integ_cotr);
+
+
+   /**
+    * IntegratorResultMergerContainer copy constructor.
+    * @param[in] src  Object to be copied.
+    */
+   IntegratorResultMergerContainer (
+      const IntegratorResultMergerContainer & src);
+
+
+   /**
+    * IntegratorResultMergerContainer destructor.
+    */
+   ~IntegratorResultMergerContainer ();
+
+
+   /**
+    * Swap.
+    */
+   friend void swap (
+      IntegratorResultMergerContainer & a, IntegratorResultMergerContainer & b);
+
+
+   /**
+    * IntegratorResultMergerContainer assignment operator.
+    * @param src  Object to be copied.
+    */
+   IntegratorResultMergerContainer & operator= (
+      IntegratorResultMergerContainer src)
+   {
+      swap (*this, src);
+      return *this;
+   }
+
+
+   /**
+    * Configure the IntegratorResultMergerContainer for use with a specific
+    * integration technique.
+    * @param integ_cotr  The integrator constructor for the technique.
+    */
+   void configure (
+      const IntegratorConstructor & integ_cotr);
+
+
+   /**
+    * Merge an IntegratorResult into another.
+    * The default implementation pertains to simple, fixed step integration
+    * techniques. Adaptive techniques or techniques that can have a failure
+    * mode other than zero or one must override this default.
+    * @return True if merger was successful, false if some error occurred.
+    * @param[in]      new_result     Size of the generalized position vector
+    * @param[in,out]  merged_result  Size of the generalized position vector
+    */
+   bool merge_integrator_result (
+      const IntegratorResult & new_result,
+      IntegratorResult & merged_result)
+   const ER7_UTILS_ALWAYS_INLINE
+   {
+      return integ_merger->merge_integrator_result (new_result, merged_result);
+   }
+
+
+private:
+
+   /**
+    * The object that merges results from multiple integrators.
+    */
+   er7_utils::IntegratorResultMerger * integ_merger; //!< trick_units(--)
+
+};
+
+}
+
+
+#endif
+
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/left_quaternion_functions.hh

@@ -0,0 +1,199 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the classes
+ * LeftQuaternionGeneralizedPositionDerivativeFunctions,
+ * LeftQuaternionGeneralizedPositionStepFunctions, and
+ * LeftQuaternionGeneralizedPositionFunctions.
+ */
+
+/*
+Purpose: ()
+*/
+
+#ifndef ER7_UTILS_LEFT_QUATERNION_FUNCTIONS_HH
+#define ER7_UTILS_LEFT_QUATERNION_FUNCTIONS_HH
+
+
+// Integration includes
+#include "generalized_position_derivative.hh"
+
+
+namespace er7_utils {
+
+
+/**
+ * Specialization of GeneralizedPositionDerivativeFunctions for the
+ * rotation group SO(3) in which
+ *  - Generalized position is represented by a parent to body
+ *    left transformation quaternion in which the real scalar part is the
+ *    initial (zeroth) element of the quaternion and the imaginary vectorial
+ *    part comprises the remaining three elements, and
+ *  - Generalized velocity is represented by the angular velocity of
+ *    the body frame with respect to the parent frame but expressed
+ *    in body frame coordinates.
+ */
+class LeftQuaternionGeneralizedPositionDerivativeFunctions :
+   public GeneralizedPositionDerivativeFunctions {
+public:
+
+   /**
+    * LeftQuaternionGeneralizedPositionDerivativeFunctions default constructor.
+    */
+   LeftQuaternionGeneralizedPositionDerivativeFunctions()
+   :
+      GeneralizedPositionDerivativeFunctions (derivative, second_derivative)
+   { }
+
+
+   /**
+    * Compute the time derivative of an inertial to body left transformation
+    * quaternion as a function of the quaternion and angular velocity.
+    *
+    * @param[in]  quaternion
+    *             Inertial to body-fixed left transformation quaternion.
+    * @param[in]  angular_vel
+    *             Angular velocity in radians per second of the body frame wrt
+    *             inertial, expressed in body-fixed coordinates.
+    * @param[out] quaternion_deriv
+    *             Time derivative of the input quaternion.
+    */
+   static void derivative (
+      const double * ER7_UTILS_RESTRICT quaternion,
+      const double * ER7_UTILS_RESTRICT angular_vel,
+      double * ER7_UTILS_RESTRICT quaternion_deriv);
+
+
+   /**
+    * Compute the second time derivative of an inertial to body left
+    * transformation quaternion as a function of the quaternion, angular
+    * velocity, and angular acceleration.
+    *
+    * @param[in]  quaternion
+    *             Inertial to body-fixed left transformation quaternion.
+    * @param[in]  angular_vel
+    *             Angular velocity in radians per second of the body frame wrt
+    *             inertial, expressed in body-fixed coordinates.
+    * @param[in]  angular_acc
+    *             Angular acceleration in radians per second^2 of the body frame
+    *             wrt inertial, expressed in body-fixed coordinates.
+    * @param[out] quaternion_second_deriv
+    *             Second time derivative of the input quaternion.
+    */
+   static void second_derivative (
+      const double * ER7_UTILS_RESTRICT quaternion,
+      const double * ER7_UTILS_RESTRICT angular_vel,
+      const double * ER7_UTILS_RESTRICT angular_acc,
+      double * ER7_UTILS_RESTRICT quaternion_second_deriv);
+};
+
+
+/**
+ * Specialization of GeneralizedPositionStepFunctions for the
+ * rotation group SO(3) in which
+ *  - Generalized position is represented by a parent to body
+ *    left transformation quaternion in which the real scalar part is the
+ *    initial (zeroth) element of the quaternion and the imaginary vectorial
+ *    part comprises the remaining three elements, and
+ *  - Generalized velocity is represented by the angular velocity of
+ *    the body frame with respect to the parent frame but expressed
+ *    in body frame coordinates.
+ */
+class LeftQuaternionGeneralizedPositionStepFunctions :
+   public GeneralizedPositionStepFunctions {
+public:
+
+   /**
+    * LeftQuaternionGeneralizedPositionStepFunctions default constructor.
+    */
+   LeftQuaternionGeneralizedPositionStepFunctions()
+   :
+      GeneralizedPositionStepFunctions (expmap, dexpinv)
+   { }
+
+
+   /**
+    * Propagate the quaternion given a change dtheta=omega*dt in the angular
+    * velocity space per the exponential map applied as a left operator:
+    * @f[
+    *   \mathcal{Q}_{\text{new}} =
+    *   \exp(\vec{Delta\theta}) \mathcal{Q}_{\text{old}}
+    * @f]
+    * where @f$\exp(\vec{Delta\theta})@f$ is defined as
+    * @f[
+    *   \exp(\vec{Delta\theta}) \equiv
+    *   \begin{bmatrix}
+    *     \cos(Delta\theta/2) \\
+    *     -\sin(Delta\theta/2) \hat{\Delta\theta}
+    *   \end{bmatrix}
+    * @f]
+    *
+    * @param[in]  quaternion_init
+    *             Initial inertial to body-fixed left transformation quaternion.
+    * @param[in]  dtheta
+    *             Conceptually, product of angular velocity and delta t.
+    * @param[out] quaternion_end
+    *             Propagated quaternion.
+    */
+   static void expmap (
+      const double * quaternion_init,
+      const double * dtheta,
+      double * quaternion_end);
+
+
+   /**
+    * Transform the angular velocity from the tangent space at exp(dtheta)
+    * to the tangent space at identity via dexpinv.
+    *
+    * @param[in]  angular_vel_in
+    *             Angular velocity at exp(dtheta).
+    * @param[in]  dtheta
+    *             Conceptually, product of angular velocity and delta t.
+    * @param[out] angular_vel_out
+    *             Angular velocity at identity.
+    */
+   static void dexpinv (
+      const double * angular_vel_in,
+      const double * dtheta,
+      double * angular_vel_out);
+
+};
+
+
+/**
+ * Convenience class that merges the left quaternions specializations of
+ * GeneralizedPositionDerivativeFunctions and GeneralizedPositionStepFunctions.
+ */
+class LeftQuaternionGeneralizedPositionFunctions :
+   public LeftQuaternionGeneralizedPositionDerivativeFunctions,
+   public LeftQuaternionGeneralizedPositionStepFunctions {
+
+public:
+
+   LeftQuaternionGeneralizedPositionFunctions ()
+   :
+      LeftQuaternionGeneralizedPositionDerivativeFunctions (),
+      LeftQuaternionGeneralizedPositionStepFunctions ()
+   {}
+
+};
+
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/priming_first_order_ode_integrator.hh

@@ -0,0 +1,212 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class PrimingFirstOrderODEIntegrator, which serves as the basis
+ * for those first order ODE integrators that need priming.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_PRIMING_ONE_STATE_INTEGRATOR_HH
+#define ER7_UTILS_PRIMING_ONE_STATE_INTEGRATOR_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/first_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+class IntegratorConstructor;
+
+
+/**
+ * Advance state for a technique that needs priming.
+ * This includes
+ *   - Initial priming of the recent history of state derivatives,
+ *   - Using the technique-specific integration method once primed, and
+ *   - Clearing the history when needed (e.g., time change or state change).
+ */
+class PrimingFirstOrderODEIntegrator : public FirstOrderODEIntegrator {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(PrimingFirstOrderODEIntegrator)
+
+
+   // Note:
+   //  This is an abstract class.
+   //  - The constructors for this class are protected.
+   //  - The assignment operator for this class is private / unimplemented.
+
+
+public:
+
+   /**
+    * PrimingFirstOrderODEIntegrator destructor.
+    */
+   virtual ~PrimingFirstOrderODEIntegrator (void);
+
+
+   // Member functions.
+
+   /**
+    * Set the controls object that guides this object's integration process.
+    * @param[in,out] controls_in  Integration controls
+    */
+   virtual void set_controls (IntegrationControls & controls_in);
+
+   /**
+    * Reset a PrimingFirstOrderODEIntegrator.
+    */
+   virtual void reset_integrator (void)
+   {
+      prime_counter = priming_state_size;
+      primed = false;
+      primer->reset_integrator();
+   }
+
+   /**
+    * Propagate state. This integrator covers priming, but defers
+    * technique-specific integration to the technique.
+    * @param[in]     dyn_dt        Integration step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      const double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position);
+
+
+protected:
+
+   // Constructors.
+
+   /**
+    * PrimingFirstOrderODEIntegrator default constructor.
+    */
+   PrimingFirstOrderODEIntegrator (void);
+
+   /**
+    * PrimingFirstOrderODEIntegrator copy constructor.
+    */
+   PrimingFirstOrderODEIntegrator (
+      const PrimingFirstOrderODEIntegrator &);
+
+   /**
+    * PrimingFirstOrderODEIntegrator non-default constructor.
+    * This is the constructor used by the integrator constructor.
+    * @param[in]     priming_size        Number of required derivatives
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     size                State size
+    * @param[in,out] controls_in         Integration controls
+    */
+   PrimingFirstOrderODEIntegrator (
+      unsigned int priming_size,
+      const IntegratorConstructor & primer_constructor,
+      unsigned int size,
+      IntegrationControls & controls_in);
+
+
+   // Member functions
+
+   /**
+    * Swap contents with that of another.
+    * @param other  Other object with which contents are to be swapped.
+    */
+   virtual void swap (PrimingFirstOrderODEIntegrator & other);
+
+   using FirstOrderODEIntegrator::swap;
+
+   /**
+    * Save derivatives during priming.
+    * @param[in] countdown     The prime_counter data member.
+    * @param[in] velocity      Generalized velocity vector.
+    * @param[in] position      Generalized position vector.
+    */
+   virtual void technique_save_derivatives (
+      int countdown,
+      const double * ER7_UTILS_RESTRICT velocity,
+      const double * ER7_UTILS_RESTRICT position) = 0;
+
+   /**
+    * Propagate state once primed, using the technique-specific algorithm.
+    * @param[in]     dyn_dt        Integration step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult technique_integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      const double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position) = 0;
+
+
+   // Member data
+
+   IntegrationControls * controls; /**< trick_units(--) @n
+      The integration controls object that controls this state integrator. */
+
+   FirstOrderODEIntegrator * primer; /**< trick_units(--) @n
+      The state integrator used to prime this integrator. */
+
+   int priming_state_size; /**< trick_units(--) @n
+      The number of start of cycle derivatives needed by the technique. */
+
+   int prime_counter; /*!< trick_units(--) @n
+      Priming phase countdown. This is set to priming_state_size initially
+      and upon reset. Priming is complete when the counter reaches zero. */
+
+   bool primed; /*!< trick_units(--) @n
+      Indicates whether the object has been primed.
+      This is false initially and upon reset; it is set to true when
+      priming is complete. */
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   PrimingFirstOrderODEIntegrator & operator= (
+      const PrimingFirstOrderODEIntegrator &);
+};
+
+}
+
+
+#ifdef ER7_UTILS_NEED_AUX_INCLUDES
+#include "er7_utils/integration/core/include/integration_controls.hh"
+#endif
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/priming_integration_controls.hh

@@ -0,0 +1,173 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class PrimingIntegrationControls, the class used for controlling
+ * the ABM4 and Beeman integration processes.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_PRIMING_INTEGRATION_CONTROLS_HH
+#define ER7_UTILS_PRIMING_INTEGRATION_CONTROLS_HH
+
+// System includes
+#include <cstddef>
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Model includes
+#include "standard_integration_controls.hh"
+
+
+namespace er7_utils {
+
+class IntegratorConstructor;
+
+/**
+ * A priming integration controls object provides mechanisms for controlling
+ * integrators that need a simple priming process.
+ */
+class PrimingIntegrationControls : public StandardIntegrationControls {
+ER7_UTILS_MAKE_SIM_INTERFACES(PrimingIntegrationControls)
+
+public:
+
+   // Constructors and destructor.
+
+   /**
+    * PrimingIntegrationControls default constructor.
+    */
+   PrimingIntegrationControls (void)
+   :
+      Er7UtilsDeletable (),
+      StandardIntegrationControls (),
+      priming_controls (NULL),
+      priming_count (0),
+      history_length (0)
+   { }
+
+
+   /**
+    * PrimingIntegrationControls copy constructor.
+    * @param[in] src  Object to be copied.
+    * Note that this is a deep rather than shallow copy.
+    */
+   PrimingIntegrationControls (
+      const PrimingIntegrationControls & src)
+   :
+      Er7UtilsDeletable (),
+      StandardIntegrationControls (src),
+      priming_controls (src.priming_controls ?
+                        src.priming_controls->create_copy() : NULL),
+      priming_count (src.priming_count),
+      history_length (src.history_length)
+   {
+      if (priming_controls != NULL) {
+         set_alt_controls (*priming_controls);
+      }
+   }
+
+
+   /**
+    * PrimingIntegrationControls non-default constructor.
+    * @param[in] primer_constructor       Constructor that creates the primer
+    * @param[in] history_buffer_size      History buffer size
+    * @param[in] number_operating_stages  Number operational stages
+    */
+   PrimingIntegrationControls (
+      const IntegratorConstructor & primer_constructor,
+      unsigned int history_buffer_size,
+      unsigned int number_operating_stages);
+
+
+   /**
+    * PrimingIntegrationControls destructor.
+    */
+   virtual ~PrimingIntegrationControls (void);
+
+
+   /**
+    * PrimingIntegrationControls assignment operator.
+    * @param[in] src  Object to be copied.
+    */
+   PrimingIntegrationControls & operator= (PrimingIntegrationControls src)
+   {
+      swap (src);
+      return *this;
+   }
+
+
+   // Additional member functions
+
+   /**
+    * Get the integration controls used during priming.
+    */
+   IntegrationControls & get_priming_controls ()
+   { return *priming_controls; }
+
+   /**
+    * Reset the integration controls.
+    */
+   virtual void reset_integrator (void);
+
+   /**
+    * Prepare for a new integration tour.
+    */
+   virtual void start_integration_tour (void);
+
+   /**
+    * Create a copy of 'this' PrimingIntegrationControls object.
+    * @return Clone of 'this'.
+    */
+   virtual PrimingIntegrationControls * create_copy () const;
+
+
+protected:
+
+   // Member functions.
+
+   /**
+    * Non-throwing swap function.
+    * Swap contents of 'this' with that of another PrimingIntegrationControls.
+    * @param[in] other  Item with which contents are to be swapped.
+    */
+   virtual void swap (PrimingIntegrationControls & other);
+
+   using StandardIntegrationControls::swap;
+
+
+   // Member data.
+
+   IntegrationControls * priming_controls; /**< trick_units(--) @n
+      The integration controls object used to prime the state integrators. */
+
+   int priming_count; /**< trick_units(--) @n
+      The number of priming integration cycles the integrator to go.
+      This is typically reset to history_length-1. */
+
+   unsigned int history_length; /**< trick_units(--) @n
+      The number of items in the state integrator's history buffer. */
+};
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/priming_integrator_constructor.hh

@@ -0,0 +1,155 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class PrimingIntegratorConstructor, which integrators for a
+ * technique that needs to be primed.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_PRIMING_INTEGRATOR_CONSTRUCTOR_HH
+#define ER7_UTILS_PRIMING_INTEGRATOR_CONSTRUCTOR_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Model includes
+#include "integrator_constructor.hh"
+
+
+namespace er7_utils {
+
+/**
+ * Create state and time integrators for techniques that need priming.@n
+ * Note well:@n
+ * The IntegratorConstructor used to prime the PrimingIntegratorConstructor
+ * must not be a PrimingIntegratorConstructor thanks to the nature of the
+ * non-default and copy constructors.
+ */
+class PrimingIntegratorConstructor : public IntegratorConstructor {
+ER7_UTILS_MAKE_SIM_INTERFACES(PrimingIntegratorConstructor)
+
+
+   // Note:
+   //  This is an abstract class.
+   //  - The constructors for this class are protected.
+   //  - The assignment operator for this class is private / unimplemented.
+
+
+public:
+
+   /**
+    * PrimingIntegratorConstructor destructor.
+    */
+   virtual ~PrimingIntegratorConstructor (void);
+
+
+   /**
+    * Getter for the primer_constructor.@n
+    * Note that there is no setter for this member.
+    * @return Const reference to the primer_constructor.
+    */
+   const IntegratorConstructor & get_primer_constructor (void) const
+   {
+      return *primer_constructor;
+   }
+
+   /**
+    * Indicate whether the integration technique will always takes a
+    * pre-determined number of integration steps per integration cycle.@n
+    * This implemenetation assumes the primary technique is a fixed step
+    * integrator, but that the primer might not be.
+    * @return True if the primer is also fixed step, false otherwise.
+    */
+   virtual bool is_fixed_step_integrator (void) const
+   { return primer_constructor->is_fixed_step_integrator(); }
+
+   /**
+    * Return the buffer size needed by engines such as Trick that need to
+    * allocate space separately from the integration technique.@n
+    * This implementation returns the larger of the primer's buffer size and
+    * the primary technique's transition table size.
+    * @return Largest number of steps in any integration cycle
+    */
+   virtual unsigned int get_buffer_size (void) const
+   {
+      unsigned int primer_size  = primer_constructor->get_buffer_size();
+      unsigned int this_table_size = get_transition_table_size();
+      return (primer_size > this_table_size) ? primer_size : this_table_size;
+   }
+
+
+   /**
+    * Get the size of the history buffer needed by the technique.
+    * @return Required history buffer length.
+    */
+   virtual unsigned int get_history_length (void) const = 0;
+
+
+protected:
+
+   // Member functions.
+
+   /**
+    * PrimingIntegratorConstructor default constructor.
+    */
+   PrimingIntegratorConstructor (void);
+
+   /**
+    * PrimingIntegratorConstructor copy constructor.
+    * @param[in] src  Object to be copied.
+    */
+   PrimingIntegratorConstructor (const PrimingIntegratorConstructor & src);
+
+   /**
+    * PrimingIntegratorConstructor non-default constructor.
+    * @param[in] primer  Integrator constructor that creates the primers.
+    */
+   explicit PrimingIntegratorConstructor (const IntegratorConstructor & primer);
+
+   /**
+    * Non-throwing swap.
+    * @param[in,out] src  Object with which contents are to be swapped.
+    */
+   virtual void swap (PrimingIntegratorConstructor & src);
+
+
+   // Member data.
+
+   IntegratorConstructor * primer_constructor; /**< trick_units(--) @n
+      Constructor used to create the primers. */
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   PrimingIntegratorConstructor & operator= (
+      const PrimingIntegratorConstructor &);
+
+};
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/priming_second_order_ode_integrator.hh

@@ -0,0 +1,298 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class PrimingSecondOrderODEIntegrator, which serves as the basis
+ * for those second order ODE integrators that need priming.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_PRIMING_TWO_STATE_INTEGRATOR_HH
+#define ER7_UTILS_PRIMING_TWO_STATE_INTEGRATOR_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/second_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+class IntegratorConstructor;
+
+
+/**
+ * Advance state for a technique that needs priming.
+ * This includes
+ *   - Initial priming of the recent history of state derivatives,
+ *   - Using the technique-specific integration method once primed, and
+ *   - Clearing the history when needed (e.g., time change or state change).
+ */
+class PrimingSecondOrderODEIntegrator : public SecondOrderODEIntegrator {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(PrimingSecondOrderODEIntegrator)
+
+
+   // Note:
+   //  This is an abstract class.
+   //  - The constructors for this class are protected.
+   //  - The assignment operator for this class is private / unimplemented.
+
+
+public:
+
+   /**
+    * PrimingSecondOrderODEIntegrator destructor.
+    */
+   virtual ~PrimingSecondOrderODEIntegrator (void);
+
+
+   // Member functions.
+
+   /**
+    * Set the controls object that guides this object's integration process.
+    * The default implementation does nothing.
+    * @param[in,out] controls_in  Integration controls
+    */
+   virtual void set_controls (IntegrationControls & controls_in);
+
+   /**
+    * Reset a PrimingSecondOrderODEIntegrator.
+    */
+   virtual void reset_integrator (void)
+   {
+      // Mark this object as unprimed.
+      prime_counter = priming_state_size;
+      primed = false;
+
+      // Reset the primer.
+      primer->reset_integrator();
+   }
+
+
+   /**
+    * Reset the position derivative functions.
+    * @param[in] deriv_funs  Position time derivative functions container
+    */
+   virtual void set_position_derivative_functions (
+      const GeneralizedPositionDerivativeFunctions & deriv_funs)
+   {
+      // Set our derivative functions.
+      SecondOrderODEIntegrator::set_position_derivative_functions (deriv_funs);
+
+      // And also that of the primer.
+      primer->set_position_derivative_functions (deriv_funs);
+   }
+
+
+   /**
+    * Reset the position advance functions.
+    * @param[in] step_funs Position step functions container
+    */
+   virtual void set_position_step_functions (
+      const GeneralizedPositionStepFunctions & step_funs)
+   {
+      // Set our step functions.
+      SecondOrderODEIntegrator::set_position_step_functions (step_funs);
+
+      // And also that of the primer.
+      primer->set_position_step_functions (step_funs);
+   }
+
+
+   /**
+    * Propagate state. This integrator covers priming, but defers
+    * technique-specific integration to the technique.
+    * @param[in]     dyn_dt        Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     accel         Time derivative of the generalized velocity.
+    * @param[in,out] velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      double const * ER7_UTILS_RESTRICT accel,
+      double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position);
+
+
+protected:
+
+   // Constructors.
+
+   /**
+    * PrimingSecondOrderODEIntegrator default constructor.
+    */
+   PrimingSecondOrderODEIntegrator (void);
+
+   /**
+    * PrimingSecondOrderODEIntegrator copy constructor.
+    */
+   PrimingSecondOrderODEIntegrator (
+      const PrimingSecondOrderODEIntegrator &);
+
+   /**
+    * PrimingSecondOrderODEIntegrator non-default constructor for a simple
+    * simple second order ODE.
+    * This is the constructor invoked by the constructors of simple second
+    * order ODE integrator classes that derive from this class.
+    * @param[in]     priming_size        Number of required derivatives
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     size                State size
+    * @param[in,out] controls_in         Integration controls
+    */
+   PrimingSecondOrderODEIntegrator (
+      unsigned int priming_size,
+      const IntegratorConstructor & primer_constructor,
+      unsigned int size,
+      IntegrationControls & controls_in);
+
+   /**
+    * PrimingSecondOrderODEIntegrator non-default constructor for a generalized
+    * second order ODE in which the time derivative of position is computed
+    * via the provided derivative function. Position is integrated directly
+    * using the computed derivative.
+    * This is the constructor invoked by the constructors of generalized
+    * second order ODE integrator classes that derive from this class.
+    * @param[in]     priming_size        Number of required derivatives
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     position_size       Size of the generalized position
+    * @param[in]     velocity_size       Size of the generalized velocity
+    * @param[in]     deriv_funs          Position derivative functions container
+    * @param[in,out] controls_in         Integration controls
+    */
+   PrimingSecondOrderODEIntegrator (
+      unsigned int priming_size,
+      const IntegratorConstructor & primer_constructor,
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionDerivativeFunctions & deriv_funs,
+      IntegrationControls & controls_in);
+
+   /**
+    * PrimingSecondOrderODEIntegrator non-default constructor
+    * for a generalized second order ODE in which position is advanced
+    * using the provided position step function.
+    * This is the constructor invoked by the constructors of generalized
+    * second order ODE integrator classes that derive from this class.
+    * @param[in]     priming_size        Number of required derivatives
+    * @param[in]     primer_constructor  Constructor that creates the primer
+    * @param[in]     position_size       Size of the generalized position
+    * @param[in]     velocity_size       Size of the generalized velocity
+    * @param[in]     step_funs           Position step functions container
+    * @param[in,out] controls_in         Integration controls
+    */
+   PrimingSecondOrderODEIntegrator (
+      unsigned int priming_size,
+      const IntegratorConstructor & primer_constructor,
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionStepFunctions & step_funs,
+      IntegrationControls & controls_in);
+
+
+   // Member functions
+
+   /**
+    * Swap contents with that of another.
+    * @param other  Other object with which contents are to be swapped.
+    */
+   virtual void swap (PrimingSecondOrderODEIntegrator & other);
+
+   using SecondOrderODEIntegrator::swap;
+
+   /**
+    * Save derivatives during priming.
+    * @param[in] countdown     The prime_counter data member.
+    * @param[in] accel         Time derivative of the generalized velocity.
+    * @param[in] velocity      Generalized velocity vector.
+    * @param[in] position      Generalized position vector.
+    */
+   virtual void technique_save_derivatives (
+      int countdown,
+      const double * ER7_UTILS_RESTRICT accel,
+      const double * ER7_UTILS_RESTRICT velocity,
+      const double * ER7_UTILS_RESTRICT position) = 0;
+
+   /**
+    * Propagate state once primed, using the technique-specific algorithm.
+    * @param[in]     dyn_dt        Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     accel         Time derivative of the generalized velocity.
+    * @param[in,out] velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult technique_integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      double const * ER7_UTILS_RESTRICT accel,
+      double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position) = 0;
+
+
+   // Member data
+
+   IntegrationControls * controls; /**< trick_units(--) @n
+      The integration controls object that controls this state integrator. */
+
+   SecondOrderODEIntegrator * primer; /**< trick_units(--) @n
+      The state integrator used to prime this integrator. */
+
+   int priming_state_size; /**< trick_units(--) @n
+      The number of start of cycle derivatives needed by the technique. */
+
+   int prime_counter; /**< trick_units(--) @n
+      Priming phase countdown. This is set to priming_state_size initially
+      and upon reset. Priming is complete when the counter reaches zero. */
+
+   bool primed; /**< trick_units(--) @n
+      Indicates whether the object has been primed.
+      This is false initially and upon reset; it is set to true when
+      priming is complete. */
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   PrimingSecondOrderODEIntegrator & operator= (
+      const PrimingSecondOrderODEIntegrator &);
+};
+
+}
+
+
+#ifdef ER7_UTILS_NEED_AUX_INCLUDES
+#include "er7_utils/integration/core/include/integration_controls.hh"
+#endif
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/rk_utils.hh

@@ -0,0 +1,726 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines free functions used by Runge Kutta integrators.
+ */
+
+/*
+Purpose: ()
+*/
+
+#ifndef ER7_UTILS_RK_UTILS_HH
+#define ER7_UTILS_RK_UTILS_HH
+
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+
+namespace er7_utils {
+
+/**
+ * Utilities specific to the Runge-Kutta famility of integrators.
+ */
+namespace rk {
+
+#if 0
+   /**
+    * Make a weighted step of size deltat.
+    * @tparam     nweights   Weights size
+    * @param[in]  old_state  Input state vector
+    * @param[in]  derivs     State derivatives vectors
+    * @param[in]  weights    Weights on deriv1
+    * @param[in]  deltat     Time step
+    * @param[in]  size       State size
+    * @param[out] new_state  Updated state vector
+    */
+   template <int nweights>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   weighted_step (
+      double const * ER7_UTILS_RESTRICT old_state,
+      double const * ER7_UTILS_RESTRICT const * ER7_UTILS_RESTRICT derivs,
+      double const * ER7_UTILS_RESTRICT weights,
+      double deltat,
+      int size,
+      double * ER7_UTILS_RESTRICT new_state)
+   {
+      for (int ii = 0; ii < size; ++ii) {
+         double weighted_deriv = weights[0] * derivs[0][ii];
+         for (int jj = 1; jj < nweights; ++jj) {
+            weighted_deriv += weights[jj] * derivs[jj][ii];
+         }
+         new_state[ii] = old_state[ii] + weighted_deriv * deltat;
+      }
+   }
+
+
+   /**
+    * Make a second order ODE Runge Kutta weighted step for a generalized
+    * position and generalized velocity.
+    * @tparam     nweights  Weights size
+    * @param[in]  init_pos  Initial position
+    * @param[in]  init_vel  Initial velocity
+    * @param[in]  posdot     Position derivatives
+    * @param[in]  veldot     Velocity derivatives
+    * @param[in]  weights    Weights on deriv1
+    * @param[in]  deltat     Time step
+    * @param[in]  size       State size
+    * @param[out] position   Updated position vector
+    * @param[out] velocity   Updated velocity vector
+    */
+   template <int nweights>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   two_state_weighted_step (
+      double const * ER7_UTILS_RESTRICT init_pos,
+      double const * ER7_UTILS_RESTRICT init_vel,
+      double const * ER7_UTILS_RESTRICT const * ER7_UTILS_RESTRICT posdot,
+      double const * ER7_UTILS_RESTRICT const * ER7_UTILS_RESTRICT veldot,
+      double const * ER7_UTILS_RESTRICT weights,
+      double deltat,
+      int size[2],
+      double * ER7_UTILS_RESTRICT position,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      weighted_step<nweights> (init_pos, posdot, weights, deltat, size[0],
+                               position);
+      weighted_step<nweights> (init_vel, veldot, weights, deltat, size[1],
+                               velocity);
+   }
+
+
+   /**
+    * Make a second order ODE Runge Kutta weighted step for the special
+    * case of velocity being the derivative of position.
+    * @tparam     nweights  Weights size
+    * @param[in]  init_pos  Initial position
+    * @param[in]  init_vel  Initial velocity
+    * @param[in]  posdot    Position derivatives
+    * @param[in]  veldot    Velocity derivatives
+    * @param[in]  weights   Weights on derivative elements
+    * @param[in]  deltat    Time step
+    * @param[in]  size      State size
+    * @param[out] position  Updated position vector
+    * @param[out] velocity  Updated velocity vector
+    */
+   template <int nweights>
+   inline void ER7_UTILS_ALWAYS_INLINE
+   two_state_weighted_step (
+      double const * ER7_UTILS_RESTRICT init_pos,
+      double const * ER7_UTILS_RESTRICT init_vel,
+      double const * ER7_UTILS_RESTRICT const * ER7_UTILS_RESTRICT posdot,
+      double const * ER7_UTILS_RESTRICT const * ER7_UTILS_RESTRICT veldot,
+      double const * ER7_UTILS_RESTRICT weights,
+      double deltat,
+      int size,
+      double * ER7_UTILS_RESTRICT position,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+#if 1
+      double weight_j = weights[0];
+      double const * ER7_UTILS_RESTRICT posdot_j = posdot[0];
+      double const * ER7_UTILS_RESTRICT veldot_j = veldot[0];
+      for (int ii = 0; ii < size; ++ii) {
+         position[ii] = weight_j * posdot_j[ii];
+         velocity[ii] = weight_j * veldot_j[ii];
+      }
+      for (int jj = 1; jj < nweights; ++jj) {
+         weight_j = weights[jj];
+         posdot_j = posdot[jj];
+         veldot_j = veldot[jj];
+         for (int ii = 0; ii < size; ++ii) {
+            position[ii] += weight_j * posdot_j[ii];
+            velocity[ii] += weight_j * veldot_j[ii];
+         }
+      }
+
+      for (int ii = 0; ii < size; ++ii) {
+         position[ii] *= deltat;
+         velocity[ii] *= deltat;
+         position[ii] += init_pos[ii];
+         velocity[ii] += init_vel[ii];
+      }
+#else
+      for (int ii = 0; ii < size; ++ii) {
+         double weighted_pos_deriv = 0.0;
+         double weighted_vel_deriv = 0.0;
+         for (int jj = 0; jj < nweights; ++jj) {
+            weighted_pos_deriv += weights[jj] * posdot[jj][ii];
+            weighted_vel_deriv += weights[jj] * veldot[jj][ii];
+         }
+         position[ii] = old_state[0][ii] + weighted_pos_deriv * deltat;
+         velocity[ii] = old_state[1][ii] + weighted_vel_deriv * deltat;
+      }
+#endif
+   }
+#endif
+
+
+   /**
+    * Special case for an initial RK step.@n
+    * All versions update the state in-place.
+    * This version also saves the initial state and initial derivative.
+    * @param[in]     deriv       Input state derivative
+    * @param[in]     deltat      Time step
+    * @param[in]     size        State size
+    * @param[out]    init_state  Initial state vector
+    * @param[out]    init_deriv  Initial deriv vector
+    * @param[in,out] state       Input and updated state vector
+    */
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rk_initial_step (
+      double const * ER7_UTILS_RESTRICT deriv,
+      double deltat,
+      int size,
+      double * ER7_UTILS_RESTRICT init_state,
+      double * ER7_UTILS_RESTRICT init_deriv,
+      double * ER7_UTILS_RESTRICT state)
+   {
+#if 0
+#else
+      double work_state, work_deriv;
+#endif
+      for (int ii = 0; ii < size; ++ii) {
+#if 0
+         init_state[ii] = state[ii];
+         init_deriv[ii] = deriv[ii];
+         state[ii] += deriv[ii]*deltat;
+#else
+         work_state = state[ii];
+         work_deriv = deriv[ii];
+         init_state[ii] = work_state;
+         init_deriv[ii] = work_deriv;
+         state[ii]      = work_state + work_deriv*deltat;
+#endif
+      }
+   }
+
+
+   /**
+    * Special case for an initial RK step.@n
+    * All versions update the state in-place.
+    * This version saves the initial state but not the initial derivative.
+    * @param[in]     deriv       Input state derivative
+    * @param[in]     deltat      Time step
+    * @param[in]     size        State size
+    * @param[out]    init_state  Initial state vector
+    * @param[in,out] state       Input and updated state vector
+    */
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rk_initial_step (
+      double const * ER7_UTILS_RESTRICT deriv,
+      double deltat,
+      int size,
+      double * ER7_UTILS_RESTRICT init_state,
+      double * ER7_UTILS_RESTRICT state)
+   {
+      for (int ii = 0; ii < size; ++ii) {
+         init_state[ii] = state[ii];
+         state[ii] += deriv[ii]*deltat;
+      }
+   }
+
+
+   /**
+    * Second order ODE initial RK step for the general case of generalized
+    * position time derivative passed as an argument.@n
+    * All versions update the position and velocity in-place.
+    * This version also saves the initial position, initial velocity,
+    * initial velocity derivative, and initial position derivative.
+    * @param[in]     posdot       Initial position time derivative
+    * @param[in]     veldot       Initial velocity time derivative
+    * @param[in]     deltat       Time step
+    * @param[in]     size         State sizes
+    * @param[out]    init_pos     Initial position
+    * @param[out]    init_vel     Initial velocity
+    * @param[out]    init_posdot  Initial position time derivative
+    * @param[out]    init_veldot  Initial velocity time derivative
+    * @param[in,out] position     Updated position vector
+    * @param[in,out] velocity     Updated velocity vector
+    */
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rk_two_state_initial_step (
+      double const * ER7_UTILS_RESTRICT posdot,
+      double const * ER7_UTILS_RESTRICT veldot,
+      double deltat,
+      int size[2],
+      double * ER7_UTILS_RESTRICT init_pos,
+      double * ER7_UTILS_RESTRICT init_vel,
+      double * ER7_UTILS_RESTRICT init_posdot,
+      double * ER7_UTILS_RESTRICT init_veldot,
+      double * ER7_UTILS_RESTRICT position,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      rk_initial_step (posdot, deltat, size[0], init_pos,
+                       init_posdot, position);
+      rk_initial_step (veldot, deltat, size[1], init_vel,
+                       init_veldot, velocity);
+   }
+
+
+   /**
+    * Second order ODE initial RK step for the general case of generalized
+    * position time derivative passed as an argument.@n
+    * All versions update the position and velocity in-place.
+    * This version also saves the initial position, initial velocity, and
+    * initial velocity derivative, but not the initial position derivative.
+    * @param[in]     posdot       Initial position time derivative
+    * @param[in]     veldot       Initial velocity time derivative
+    * @param[in]     deltat       Time step
+    * @param[in]     size         State sizes
+    * @param[out]    init_pos     Initial position
+    * @param[out]    init_vel     Initial velocity
+    * @param[out]    init_veldot  Initial velocity time derivative
+    * @param[in,out] position     Updated position vector
+    * @param[in,out] velocity     Updated velocity vector
+    */
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rk_two_state_initial_step (
+      double const * ER7_UTILS_RESTRICT posdot,
+      double const * ER7_UTILS_RESTRICT veldot,
+      double deltat,
+      int size[2],
+      double * ER7_UTILS_RESTRICT init_pos,
+      double * ER7_UTILS_RESTRICT init_vel,
+      double * ER7_UTILS_RESTRICT init_veldot,
+      double * ER7_UTILS_RESTRICT position,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      rk_initial_step (posdot, deltat, size[0],
+                       init_pos, position);
+      rk_initial_step (veldot, deltat, size[1],
+                       init_vel, init_veldot, velocity);
+   }
+
+
+   /**
+    * Second order ODE initial RK step for the special case of generalized
+    * position time derivative being the velocity.@n
+    * All versions update the position and velocity in-place.
+    * This version also saves the initial position, velocity, and acceleration,
+    * an also explicitly saves the initial position derivative.
+    * @param[in]     accel        Initial acceleration
+    * @param[in]     deltat       Time step
+    * @param[in]     size         State size
+    * @param[out]    init_pos     Initial position
+    * @param[out]    init_vel     Initial velocity
+    * @param[out]    init_posdot  Initial position derivative
+    * @param[out]    init_veldot  Initial velocity derivative
+    * @param[in,out] position     Updated position vector
+    * @param[in,out] velocity     Updated velocity vector
+    */
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rk_two_state_initial_step (
+      double const * ER7_UTILS_RESTRICT accel,
+      double deltat,
+      int size,
+      double * ER7_UTILS_RESTRICT init_pos,
+      double * ER7_UTILS_RESTRICT init_vel,
+      double * ER7_UTILS_RESTRICT init_posdot,
+      double * ER7_UTILS_RESTRICT init_veldot,
+      double * ER7_UTILS_RESTRICT position,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      for (int ii = 0; ii < size; ++ii) {
+         init_pos[ii]     = position[ii];
+         init_posdot[ii]  = velocity[ii];
+         position[ii]    += velocity[ii]*deltat;
+         init_vel[ii]     = velocity[ii];
+         init_veldot[ii]  = accel[ii];
+         velocity[ii]    += accel[ii]*deltat;
+      }
+   }
+
+
+   /**
+    * Second order ODE initial RK step for the special case of generalized
+    * position time derivative being the velocity.@n
+    * All versions update the position and velocity in-place.
+    * This version also saves the initial position, velocity, and acceleration,
+    * but does not explicitly save the initial position derivative.
+    * @param[in]     accel     Initial acceleration
+    * @param[in]     deltat    Time step
+    * @param[in]     size      State size
+    * @param[out]    init_pos  Initial position
+    * @param[out]    init_vel  Initial velocity
+    * @param[out]    init_acc  Initial acceleration
+    * @param[in,out] position  Updated position vector
+    * @param[in,out] velocity  Updated velocity vector
+    */
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rk_two_state_initial_step (
+      double const * ER7_UTILS_RESTRICT accel,
+      double deltat,
+      int size,
+      double * ER7_UTILS_RESTRICT init_pos,
+      double * ER7_UTILS_RESTRICT init_vel,
+      double * ER7_UTILS_RESTRICT init_acc,
+      double * ER7_UTILS_RESTRICT position,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      for (int ii = 0; ii < size; ++ii) {
+         init_pos[ii]  = position[ii];
+         position[ii] += velocity[ii]*deltat;
+         init_vel[ii]  = velocity[ii];
+         init_acc[ii]  = accel[ii];
+         velocity[ii] += accel[ii]*deltat;
+      }
+   }
+
+
+   /**
+    * Special case for an intermediate RK step.
+    * @param[in]     init_state   Initial state vector
+    * @param[in]     curr_deriv   Input state derivative
+    * @param[in]     deltat       Time step
+    * @param[in]     size         State size
+    * @param[out]    saved_deriv  Saved deriv vector
+    * @param[in,out] state        Updated state vector
+    */
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rk_intermediate_step (
+      double const * ER7_UTILS_RESTRICT init_state,
+      double const * ER7_UTILS_RESTRICT curr_deriv,
+      double deltat,
+      int size,
+      double * ER7_UTILS_RESTRICT saved_deriv,
+      double * ER7_UTILS_RESTRICT state)
+   {
+      double work_state, work_deriv;
+      for (int ii = 0; ii < size; ++ii) {
+         work_state = init_state[ii];
+         work_deriv = curr_deriv[ii];
+         state[ii]  = work_state + work_deriv * deltat;
+         saved_deriv[ii] = work_deriv;
+      }
+   }
+
+
+   /**
+    * Second order ODE intermediate RK step for the general case of generalized
+    * position time derivative passed as an argument.
+    * @param[in]     init_pos      Initial position
+    * @param[in]     init_vel      Initial velocity
+    * @param[in]     posdot        Current position time derivative
+    * @param[in]     veldot        Current velocity time derivative
+    * @param[in]     deltat        Time step
+    * @param[in]     size          State sizes
+    * @param[out]    saved_veldot  Updated velocity derivative history
+    * @param[in,out] position      Updated position vector
+    * @param[in,out] velocity      Updated velocity vector
+    */
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rk_two_state_intermediate_step (
+      double const * ER7_UTILS_RESTRICT init_pos,
+      double const * ER7_UTILS_RESTRICT init_vel,
+      double const * ER7_UTILS_RESTRICT posdot,
+      double const * ER7_UTILS_RESTRICT veldot,
+      double deltat,
+      int size[2],
+      double * ER7_UTILS_RESTRICT saved_veldot,
+      double * ER7_UTILS_RESTRICT position,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      int lim = size[0];
+      for (int ii = 0; ii < lim; ++ii) {
+         position[ii]     = init_pos[ii] + posdot[ii]*deltat;
+      }
+
+      lim = size[1];
+      for (int ii = 0; ii < lim; ++ii) {
+         saved_veldot[ii] = veldot[ii];
+         velocity[ii]     = init_vel[ii] + veldot[ii]*deltat;
+      }
+   }
+
+
+   /**
+    * Second order ODE intermediate RK step for the special case of generalized
+    * position time derivative being the velocity.
+    * @param[in]     init_pos   Initial position
+    * @param[in]     init_vel   Initial velocity
+    * @param[in]     accel      Current acceleration
+    * @param[in]     deltat     Time step
+    * @param[in]     size       State size
+    * @param[out]    saved_vel  Updated velocity history
+    * @param[out]    saved_acc  Updated acceleration history
+    * @param[in,out] position   Updated position vector
+    * @param[in,out] velocity   Updated velocity vector
+    */
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rk_two_state_intermediate_step (
+      double const * ER7_UTILS_RESTRICT init_pos,
+      double const * ER7_UTILS_RESTRICT init_vel,
+      double const * ER7_UTILS_RESTRICT accel,
+      double deltat,
+      int size,
+      double * ER7_UTILS_RESTRICT saved_vel,
+      double * ER7_UTILS_RESTRICT saved_acc,
+      double * ER7_UTILS_RESTRICT position,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      for (int ii = 0; ii < size; ++ii) {
+         saved_vel[ii] = velocity[ii];
+         saved_acc[ii] = accel[ii];
+         position[ii] = init_pos[ii] + velocity[ii]*deltat;
+         velocity[ii] = init_vel[ii] + accel[ii]*deltat;
+      }
+   }
+
+
+   /**
+    * Special case for Heun's method target stage = 2.
+    * @param[in]  init_state  Initial state vector
+    * @param[in]  deriv1      State derivatives vector
+    * @param[in]  deriv2      State derivatives vector
+    * @param[in]  deltat      Time step
+    * @param[in]  size        State size
+    * @param[out] state       Updated state vector
+    */
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rk2_heun_final_step (
+      double const * ER7_UTILS_RESTRICT init_state,
+      double const * ER7_UTILS_RESTRICT deriv1,
+      double const * ER7_UTILS_RESTRICT deriv2,
+      double deltat,
+      int size,
+      double * ER7_UTILS_RESTRICT state)
+   {
+      double hdt = 0.5 * deltat;
+      for (int ii = 0; ii < size; ++ii) {
+         state[ii] = init_state[ii] + (deriv1[ii] + deriv2[ii]) * hdt;
+      }
+   }
+
+#if 0
+   /**
+    * Second order ODE Heun's method final step for the general
+    * case of generalized position time derivative passed as an argument.
+    * @param[in]     init_pos     Initial position
+    * @param[in]     init_vel     Initial velocity
+    * @param[in]     init_posdot  Initial position time derivative
+    * @param[in]     init_veldot  Initial velocity time derivative
+    * @param[in]     curr_posdot  Current position time derivative
+    * @param[in]     curr_veldot  Current velocity time derivative
+    * @param[in]     deltat       Time step
+    * @param[in]     size         State sizes
+    * @param[in,out] position     Updated position vector
+    * @param[in,out] velocity     Updated velocity vector
+    */
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rk2_heun_two_state_final_step (
+      double const * ER7_UTILS_RESTRICT init_pos,
+      double const * ER7_UTILS_RESTRICT init_vel,
+      double const * ER7_UTILS_RESTRICT init_posdot,
+      double const * ER7_UTILS_RESTRICT init_veldot,
+      double const * ER7_UTILS_RESTRICT curr_posdot,
+      double const * ER7_UTILS_RESTRICT curr_veldot,
+      double deltat,
+      int size[2],
+      double * ER7_UTILS_RESTRICT position,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      double hdt = 0.5 * deltat;
+
+      int lim = size[0];
+      for (int ii = 0; ii < lim; ++ii) {
+         position[ii] = init_pos[ii] +
+                        (init_posdot[ii] + curr_posdot[ii]) * hdt;
+      }
+
+      lim = size[1];
+      for (int ii = 0; ii < lim; ++ii) {
+         velocity[ii] = init_vel[ii] +
+                        (init_veldot[ii] + curr_veldot[ii]) * hdt;
+      }
+   }
+
+
+   /**
+    * Second order ODE Heun's method final step for the special
+    * case of velocity being the derivative of position.
+    * @param[in]     init_pos  Initial position
+    * @param[in]     init_vel  Initial velocity
+    * @param[in]     init_acc  Initial acceleration
+    * @param[in]     accel     Current acceleration
+    * @param[in]     deltat    Time step
+    * @param[in]     size      State size
+    * @param[out]    position  Updated position vector
+    * @param[out]    velocity  Updated velocity vector
+    */
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rk2_heun_two_state_final_step (
+      double const * ER7_UTILS_RESTRICT init_pos,
+      double const * ER7_UTILS_RESTRICT init_vel,
+      double const * ER7_UTILS_RESTRICT init_acc,
+      double const * ER7_UTILS_RESTRICT accel,
+      double deltat,
+      int size,
+      double * ER7_UTILS_RESTRICT position,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      double hdt = 0.5 * deltat;
+      for (int ii = 0; ii < size; ++ii) {
+         position[ii] = init_pos[ii] + (init_vel[ii] + velocity[ii]) * hdt;
+         velocity[ii] = init_vel[ii] + (init_acc[ii] + accel[ii]) * hdt;
+      }
+   }
+#endif
+
+
+   /**
+    * Special case for canonical RK4 target stage = 4.
+    * @param[in]  init_state  Initial state vector
+    * @param[in]  deriv_hist  Intermediate step state derivatives vectors
+    * @param[in]  curr_deriv  Current state derivative
+    * @param[in]  deltat      Time step
+    * @param[in]  size        State size
+    * @param[out] state       Updated state vector
+    */
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rk4_final_step (
+      double const * ER7_UTILS_RESTRICT init_state,
+      double const * ER7_UTILS_RESTRICT const * ER7_UTILS_RESTRICT deriv_hist,
+      double const * ER7_UTILS_RESTRICT curr_deriv,
+      double deltat,
+      int size,
+      double * ER7_UTILS_RESTRICT state)
+   {
+      static const double one_sixth = 1.0 / 6.0;
+      double dto6 = deltat * one_sixth;
+#if 0
+      double state_init, deriv0_i, deriv1_i, deriv2_i, deriv3_i;
+#endif
+#if 1
+      const double * ER7_UTILS_RESTRICT deriv0 = deriv_hist[0];
+      const double * ER7_UTILS_RESTRICT deriv1 = deriv_hist[1];
+      const double * ER7_UTILS_RESTRICT deriv2 = deriv_hist[2];
+#endif
+      for (int ii = 0; ii < size; ++ii) {
+#if 0
+         state_init = init_state[ii];
+         deriv0_i = deriv_hist[0][ii];
+         deriv1_i = deriv_hist[1][ii];
+         deriv2_i = deriv_hist[2][ii];
+         deriv3_i = curr_deriv[ii];
+         state[ii] = init_state[ii] +
+                     (deriv0_i + 2.0*(deriv1_i + deriv2_i) + deriv3_i) * dto6;
+#endif
+#if 1
+         state[ii] = init_state[ii] +
+                     (deriv0[ii] +
+                      2.0*(deriv1[ii] + deriv2[ii]) +
+                      curr_deriv[ii]) * dto6;
+#else
+         state[ii] = init_state[ii] +
+                     (deriv_hist[0][ii] +
+                      2.0*(deriv_hist[1][ii] + deriv_hist[2][ii]) +
+                      curr_deriv[ii]) * dto6;
+#endif
+      }
+   }
+
+
+   /**
+    * Second order ODE final RK step for the special case of generalized
+    * position time derivative being the velocity.
+    * @param[in]     init_pos   Initial position
+    * @param[in]     init_vel   Initial velocity
+    * @param[out]    saved_vel  Velocity history
+    * @param[out]    saved_acc  Acceleration history
+    * @param[in]     posdot     Current position time derivative
+    * @param[in]     veldot     Current velocity time derivative
+    * @param[in]     deltat        Time step
+    * @param[in]     size       State sizes
+    * @param[in,out] position   Updated position vector
+    * @param[in,out] velocity   Updated velocity vector
+    */
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rk4_two_state_final_step (
+      double const * ER7_UTILS_RESTRICT init_pos,
+      double const * ER7_UTILS_RESTRICT init_vel,
+      double const * ER7_UTILS_RESTRICT const * ER7_UTILS_RESTRICT saved_vel,
+      double const * ER7_UTILS_RESTRICT const * ER7_UTILS_RESTRICT saved_acc,
+      double const * ER7_UTILS_RESTRICT posdot,
+      double const * ER7_UTILS_RESTRICT veldot,
+      double deltat,
+      int size[2],
+      double * ER7_UTILS_RESTRICT position,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      static const double one_sixth = 1.0 / 6.0;
+      double dto6 = deltat * one_sixth;
+      int lim = size[0];
+      for (int ii = 0; ii < lim; ++ii) {
+         position[ii] = init_pos[ii] +
+                        (saved_vel[0][ii] +
+                         2.0*(saved_vel[1][ii] + saved_vel[2][ii]) +
+                         posdot[ii]) * dto6;
+      }
+
+      lim = size[1];
+      for (int ii = 0; ii < lim; ++ii) {
+         velocity[ii] = init_vel[ii] +
+                        (saved_acc[0][ii] +
+                         2.0*(saved_acc[1][ii] + saved_acc[2][ii]) +
+                         veldot[ii]) * dto6;
+      }
+   }
+
+
+   /**
+    * Second order ODE final RK step for the special case of generalized
+    * position time derivative being the velocity.
+    * @param[in]     init_pos   Initial position
+    * @param[in]     init_vel   Initial velocity
+    * @param[out]    saved_vel  Velocity history
+    * @param[out]    saved_acc  Acceleration history
+    * @param[in]     accel      Current acceleration
+    * @param[in]     deltat     Time step
+    * @param[in]     size       State size
+    * @param[in,out] position   Updated position vector
+    * @param[in,out] velocity   Updated velocity vector
+    */
+   inline void ER7_UTILS_ALWAYS_INLINE
+   rk4_two_state_final_step (
+      double const * ER7_UTILS_RESTRICT init_pos,
+      double const * ER7_UTILS_RESTRICT init_vel,
+      double const * ER7_UTILS_RESTRICT const * ER7_UTILS_RESTRICT saved_vel,
+      double const * ER7_UTILS_RESTRICT const * ER7_UTILS_RESTRICT saved_acc,
+      double const * ER7_UTILS_RESTRICT accel,
+      double deltat,
+      int size,
+      double * ER7_UTILS_RESTRICT position,
+      double * ER7_UTILS_RESTRICT velocity)
+   {
+      static const double one_sixth = 1.0 / 6.0;
+      double dto6 = deltat * one_sixth;
+      for (int ii = 0; ii < size; ++ii) {
+         position[ii] = init_pos[ii] +
+                        (saved_vel[0][ii] +
+                         2.0*(saved_vel[1][ii] + saved_vel[2][ii]) +
+                         velocity[ii]) * dto6;
+         velocity[ii] = init_vel[ii] +
+                        (saved_acc[0][ii] +
+                         2.0*(saved_acc[1][ii] + saved_acc[2][ii]) +
+                         accel[ii]) * dto6;
+      }
+   }
+
+}
+}
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/second_order_ode_integrator.hh

@@ -0,0 +1,250 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class SecondOrderODEIntegrator, the base class for propagating
+ * state that conceptually comprises a zeroth and first derivative.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_TWO_STATE_INTEGRATOR_HH
+#define ER7_UTILS_TWO_STATE_INTEGRATOR_HH
+
+// System includes
+#include <cstddef>
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Local includes
+#include "state_integrator.hh"
+#include "generalized_position_derivative.hh"
+#include "integration_technique.hh"
+#include "integrator_result.hh"
+
+
+namespace er7_utils {
+
+class IntegrationControls;
+
+/**
+ * Base class for propagating states comprising a zeroth and first derivative.
+ */
+class SecondOrderODEIntegrator : public StateIntegratorInterface {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(SecondOrderODEIntegrator)
+
+
+   // Note:
+   //  This is an abstract class.
+   //  - The constructors for this class are protected.
+   //  - The assignment operator for this class is private / unimplemented.
+
+
+public:
+
+   /**
+    * SecondOrderODEIntegrator destructor.
+    */
+   virtual ~SecondOrderODEIntegrator (void)
+   { }
+
+
+   // Member functions.
+
+   /**
+    * Create a copy of 'this', a derived SecondOrderODEIntegrator object.
+    * @return Clone of 'this'.
+    */
+   virtual SecondOrderODEIntegrator* create_copy () const = 0;
+
+   /**
+    * Set the controls object that guides this object's integration process.
+    * The default implementation does nothing.
+    * @param[in,out] controls  Integration controls (unused)
+    */
+   virtual void set_controls (IntegrationControls & controls ER7_UTILS_UNUSED)
+   { }
+
+   /**
+    * Reset the position derivative functions.
+    * This function exists so that the derivative function pointers can be
+    * restored to their checkpointed value at restart time.
+    * @param[in] deriv_funs  Position derivative functions container
+    */
+   virtual void set_position_derivative_functions (
+      const GeneralizedPositionDerivativeFunctions & deriv_funs);
+
+   /**
+    * Reset the position step functions.
+    * This function exists so that the step function pointers can be
+    * restored to their checkpointed value at restart time.
+    * @param[in] step_funs  Lie integrator step functions container
+    */
+   virtual void set_position_step_functions (
+      const GeneralizedPositionStepFunctions & step_funs);
+
+   /**
+    * Propagate state to the specified stage of the integration
+    * process for an overall integration time interval of dyn_dt.
+    *
+    * All integration techniques can assume that the status parameter will
+    * be default-initialized on entry: time scales set to 1.0, failure status
+    * and merge count set to zero. An integration technique only needs to change
+    * the time scales if they are something other than 1.0, the failure mode
+    * if the technique somehow is not successful.
+    *
+    * Note that this is a pure virtual function; instantiable subclasses must
+    * provide an implementation of this method.
+    *
+    * @param[in]     dyn_dt        Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     accel         Time derivative of the generalized velocity.
+    * @param[in,out] velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      double const * ER7_UTILS_RESTRICT accel,
+      double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position) = 0;
+
+
+protected:
+
+   // Constructors.
+
+   /**
+    * SecondOrderODEIntegrator default constructor.
+    * This is the constructor invoked by the default constructors of classes
+    * that derive from this class.
+    * Note that this default constructor does not create a usable instance.
+    */
+   SecondOrderODEIntegrator (void);
+
+   /**
+    * SecondOrderODEIntegrator copy constructor.
+    * @param[in] src  Object to be copied
+    */
+   SecondOrderODEIntegrator (const SecondOrderODEIntegrator & src);
+
+   /**
+    * SecondOrderODEIntegrator non-default constructor for a simple second
+    * order ODE, one in the velocity is the time derivative of position.
+    * This is the constructor invoked by the constructors of simple second
+    * order ODE integrator classes that derive from this class.
+    * @param[in]     size      State size
+    * @param[in,out] controls  Integration controls (unused)
+    */
+   SecondOrderODEIntegrator (
+      unsigned int size,
+      IntegrationControls & controls);
+
+   /**
+    * SecondOrderODEIntegrator non-default constructor for a generalized
+    * second order ODE in which the time derivative of position is computed
+    * via the provided derivative function. Position is integrated directly
+    * using the computed derivative.
+    * This is the constructor invoked by the constructors of generalized
+    * second order ODE integrator classes that derive from this class.
+    * @param[in]     position_size  Size of the generalized position
+    * @param[in]     velocity_size  Size of the generalized velocity
+    * @param[in]     deriv_funs     Derivative functions container
+    * @param[in,out] controls       Integration controls (unused)
+    */
+   SecondOrderODEIntegrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionDerivativeFunctions & deriv_funs,
+      IntegrationControls & controls);
+
+   /**
+    * SecondOrderODEIntegrator non-default constructor for a generalized
+    * second order ODE in which the time derivative of position is computed
+    * via the provided derivative function. Position is integrated indirectly
+    * using the provided step function.
+    * This is the constructor invoked by the constructors of generalized
+    * second order ODE integrator classes that derive from this class.
+    * @param[in]     position_size  Size of the generalized position
+    * @param[in]     velocity_size  Size of the generalized velocity
+    * @param[in]     step_funs      Step functions container
+    * @param[in,out] controls       Integration controls (unused)
+    */
+   SecondOrderODEIntegrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionStepFunctions & step_funs,
+      IntegrationControls & controls);
+
+
+   // Member functions.
+
+   /**
+    * Swap contents with that of other.
+    * @param other  Other object with which contents are to be swapped.
+    */
+   virtual void swap (SecondOrderODEIntegrator & other);
+
+
+   // Member data.
+
+   GeneralizedPositionDerivativeFunctions::FirstDerivative
+   compute_posdot; /**< trick_io(**) @n
+      External function that computes the time derivative of
+      generalized position. */
+
+   GeneralizedPositionDerivativeFunctions::SecondDerivative
+   compute_posdotdot; /**< trick_io(**) @n
+      External function that computes the second time derivative of
+      generalized position. */
+
+   GeneralizedPositionStepFunctions::ExpMapPositionStep
+   compute_expmap_position_step; /**< trick_io(**) @n
+      External function that advances generalized position. */
+
+   GeneralizedPositionStepFunctions::DexpinvTransformVelocity
+   compute_dexpinv_velocity_transform; /**< trick_io(**) @n
+      External function that transforms velocity per the Lie algebra
+      dexpinv operator. */
+
+   int state_size[2]; /**< trick_units(--) @n
+      The sizes of the generalized position and generalized velocity vectors. */
+
+   Integration::ODEProblemType problem_type; /**< trick_units(--) @n
+      The type of problem to be solved. */
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   SecondOrderODEIntegrator & operator= (const SecondOrderODEIntegrator &);
+};
+
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/single_cycle_integration_controls.hh

@@ -0,0 +1,119 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class SingleCycleIntegrationControls, the class used for
+ * controlling most of the integration techniques.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_SINGLE_CYCLE_INTEGRATION_CONTROLS_HH
+#define ER7_UTILS_SINGLE_CYCLE_INTEGRATION_CONTROLS_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Local includes
+#include "integration_controls.hh"
+
+
+namespace er7_utils {
+
+/**
+   A single cycle integration controls object provides mechanisms for
+    controlling integrators that need a simple single cycle process.
+*/
+class SingleCycleIntegrationControls : public IntegrationControls {
+ER7_UTILS_MAKE_SIM_INTERFACES(SingleCycleIntegrationControls)
+
+public:
+
+   // Constructors and destructor.
+   // Note: This class has no direct data members.
+   // The implicitly-defined copy constructor and destructor
+   // work quite nicely and are not declared.
+
+
+   /**
+    * SingleCycleIntegrationControls default constructor.
+    */
+   SingleCycleIntegrationControls () : IntegrationControls()
+   { }
+
+
+   /**
+    * SingleCycleIntegrationControls non-default constructor.
+    * @param[in] number_stages_in  Number operational stages
+    */
+   explicit SingleCycleIntegrationControls (unsigned int number_stages_in)
+   :
+      IntegrationControls(number_stages_in)
+   { }
+
+
+   // Member functions
+
+   /**
+    * Copy-and-swap SingleCycleIntegrationControls assignment operator.
+    * @param[in] src  The IntegrationControls to be copied.
+    */
+   SingleCycleIntegrationControls &
+   operator= (SingleCycleIntegrationControls src)
+   {
+      swap (src);
+      return *this;
+   }
+
+
+   /**
+    * Create a copy of 'this' SingleCycleIntegrationControls object.
+    * @return Clone of 'this'.
+    */
+   virtual SingleCycleIntegrationControls * create_copy () const;
+
+
+   /**
+    * Perform one step of the integration process.
+    * @return Step number; zero when finished.
+    * @param[in] start_time
+    *    The simulation engine time from which the integrators start.
+    * @param[in] sim_dt
+    *    The difference between the simulation time at the end and start of the
+    *    integration tour.
+    * @param[in,out] time_if
+    *    Object external to the ER7 utilities suite that represents time.
+    * @param[in,out] integ_interface
+    *    Interface with the simulation engine for this integration controls.
+    * @param[in,out] integ_group
+    *    The integration group that contains this integration controls.
+    */
+   virtual unsigned int integrate (
+      double start_time, double sim_dt,
+      TimeInterface & time_if,
+      IntegratorInterface & integ_interface,
+      BaseIntegrationGroup & integ_group);
+};
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/standard_integration_controls.hh

@@ -0,0 +1,284 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class StandardIntegrationControls, the class that defines
+ * the most generic integration controls.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_STANDARD_INTEGRATION_CONTROLS_HH
+#define ER7_UTILS_STANDARD_INTEGRATION_CONTROLS_HH
+
+// System includes
+#include <cstddef>
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Local includes
+#include "integration_controls.hh"
+
+
+namespace er7_utils {
+
+// Forward declarations
+class TimeInterface;
+class BaseIntegrationGroup;
+
+
+/**
+ * The class StandardIntegrationControls extends the base IntegrationControls
+ * class for general purpose operations. Compare with the class
+ * SingleCycleIntegrationControls, which extends IntegrationControls for
+ * integration techniques that always use but one integration tour comprising
+ * but one integration cycle to complete the integration process.
+ */
+class StandardIntegrationControls : public IntegrationControls {
+ER7_UTILS_MAKE_SIM_INTERFACES(StandardIntegrationControls)
+
+public:
+
+   // Constructors and destructor.
+
+   /**
+    * StandardIntegrationControls default constructor.
+    */
+   StandardIntegrationControls (void)
+   :
+      Er7UtilsDeletable (),
+      IntegrationControls (),
+      cycle_starttime (0.0),
+      cycle_simdt (0.0),
+      cycle_dyndt (0.0),
+      multi_cycle (false),
+      /* target_attained (true), */
+      alt_controls (NULL)
+   {}
+
+
+   /**
+    * StandardIntegrationControls non-default constructor.
+    * @param[in] number_stages_in  Number of stages in an integration cycle.
+    */
+   explicit StandardIntegrationControls (unsigned int number_stages_in)
+   :
+      Er7UtilsDeletable (),
+      IntegrationControls (number_stages_in),
+      cycle_starttime (0.0),
+      cycle_simdt (0.0),
+      cycle_dyndt (0.0),
+      multi_cycle (false),
+      /* target_attained (true), */
+      alt_controls (NULL)
+   {}
+
+
+   /**
+    * StandardIntegrationControls copy constructor.
+    * @param[in] src  Object to be copied.
+    * Note that the alt_controls is not copied.
+    * Properly setting this member is the responsibility of a derived class'
+    * copy constructor.
+    */
+   StandardIntegrationControls (const StandardIntegrationControls & src)
+   :
+      Er7UtilsDeletable (),
+      IntegrationControls (src),
+      cycle_starttime (src.cycle_starttime),
+      cycle_simdt (src.cycle_simdt),
+      cycle_dyndt (src.cycle_dyndt),
+      multi_cycle (src.multi_cycle),
+      /* target_attained (src.target_attained), */
+      alt_controls (NULL)
+   {}
+
+
+   /**
+    * StandardIntegrationControls destructor.
+    */
+   virtual ~StandardIntegrationControls (void)
+   {}
+
+
+   // Member functions.
+
+   /**
+    * StandardIntegrationControls assignment operator.
+    */
+   StandardIntegrationControls & operator= (StandardIntegrationControls src)
+   {
+      swap (src);
+      return *this;
+   }
+
+
+   /**
+    * Create a copy of 'this' StandardIntegrationControls object.
+    * @return Clone of 'this'.
+    */
+   virtual StandardIntegrationControls * create_copy () const;
+
+
+   /**
+    * Prepare for the upcoming cycle of integrations.
+    * The integrate function calls this function at the start of a tour.
+    *
+    * The default implementation sets integ_starttime and cycle_starttime to the
+    * provided starttime, integ_simdt and cycle_simdt to the provided simdt.
+    * Multi-cycle and multi-step integrators that override this default
+    * implementation should set integ_starttime and integ_simdt set to the
+    * provided inputs, and should also set
+    *  - cycle_starttime and cycle_simdt based on the corresponding integ
+    *    values and the state of the technique-specific integration control.
+    * The overrides are free to do many do other things such as switching
+    * transition tables, etc.
+    */
+   virtual void start_integration_tour (void)
+   {
+      cycle_starttime  = integ_starttime;
+      cycle_simdt      = integ_simdt;
+      cycle_dyndt      = integ_dyndt;
+      integ_simtime    = integ_starttime;
+      integ_time_scale = 0.0;
+   }
+
+
+   /**
+    * Indicate whether this is the end of a tour.
+    *
+    * The default implementation always returns true.
+    * Integrators that override this method must return true at the end
+    * of a tour, false otherwise. Overrides can also perform any
+    * technique-specific actions that need to be taken at the end of a cycle.
+    */
+   virtual bool end_integration_cycle (void)
+   {
+      return true;
+   }
+
+
+   /**
+    * Perform one step of the integration process.
+    * @return Step number; a value of zero means the integration process
+    *    has been completed.
+    * @param[in] start_time
+    *    The simulation engine time from which the integrators start.
+    *    The integration process must eventually reach starttime+sim_dt.
+    *    This function should return zero when the integration tour has finally
+    *    reached this tour end time, non-zero until then.
+    * @param[in] sim_dt
+    *    The difference between the simulation time at the end and start of the
+    *    integration tour.
+    * @param[in,out] time_if
+    *    Object external to the ER7 utilities suite that represents time.
+    * @param[in,out] integ_interface
+    *    Interface with the simulation engine for this integration controls.
+    * @param[in,out] integ_group
+    *    The integration group that contains this integration controls.
+    *
+    * Note well: The starttime and sim_dt are assumed to be constant during
+    * an integration tour.
+    */
+   virtual unsigned int integrate (
+      double start_time, double sim_dt,
+      TimeInterface & time_if,
+      IntegratorInterface & integ_interface,
+      BaseIntegrationGroup & integ_group);
+
+
+protected:
+
+   // Member functions.
+
+   /**
+    * Set the alternative controller to the provided value.
+    * @param[in] alt_controller  New alternate controller
+    */
+   void set_alt_controls (IntegrationControls & alt_controller)
+   {
+      alt_controls = &alt_controller;
+      multi_cycle = true;
+   }
+
+
+   /**
+    * Clear the alternative controller.
+    * @param[in] is_multi_cycle  New value for multi_cycle
+    */
+   void clear_alt_controls (bool is_multi_cycle = false)
+   {
+      alt_controls = NULL;
+      multi_cycle = is_multi_cycle;
+   }
+
+
+   /**
+    * Non-throwing swap function.
+    * Swap contents of 'this' with that of another StandardIntegrationControls.
+    * @param[in] other  Item with which contents are to be swapped.
+    */
+   virtual void swap (StandardIntegrationControls & other);
+
+   using IntegrationControls::swap;
+
+
+   // Member data
+
+   double cycle_starttime; /**< trick_units(--) @n
+      The simulation engine time of the start of the current integration cycle
+      as determined by start_integration_tour or end_integration_cycle.
+      For single cycle integrators such as RK4, the cycle_starttime and
+      cycle_simdt are equal to the integ_starttime and integ_simdt.
+      Multi-cycle integrators such as Gauss-Jackson compute the
+      cycle_starttime and cycle_simdt. */
+
+   double cycle_simdt; /**< trick_units(--) @n
+      The simulation engine time span of the current integration cycle
+      as determined by start_integration_tour or end_integration_cycle. */
+
+   double cycle_dyndt; /**< trick_units(--) @n
+      The JEOD dynamic time span corresponding to cycle_simdt. */
+
+
+   bool multi_cycle; /**< trick_units(--) @n
+      Always false for single-cycle integrators.
+      Multi-cycle integrators must set this to true to operate in
+      multi-cycle mode. */
+
+#if 0
+   bool target_attained; /**< trick_units(--) @n
+      True if an state integrators attained the target_stage,
+      false otherwise. */
+#endif
+
+private:
+
+   // Member data
+
+   IntegrationControls * alt_controls; /**< trick_units(--) @n
+      An alternate controls, typically for priming, that acts as a stand-in
+      for this integration controller. */
+};
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/state_integrator.hh

@@ -0,0 +1,112 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class StateIntegratorInterface, the base class for propagating
+ * state.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_STATE_INTEGRATOR_HH
+#define ER7_UTILS_STATE_INTEGRATOR_HH
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+#include "er7_utils/interface/include/deletable.hh"
+
+
+namespace er7_utils {
+
+/**
+ * The base class for classes that propagate state.
+ * A state integrator class propagates state and indicates upon return
+ * whether it successfully reached the target and indicates the fractional
+ * amount by which it assumed time is to be advanced.
+ * Note that this interface class does not declare an integrate method.
+ */
+class StateIntegratorInterface : virtual public Er7UtilsDeletable {
+
+
+   // Note:
+   //  This is an abstract class.
+   //  - The constructors for this class are protected.
+   //  - The assignment operator for this class is private / unimplemented.
+
+
+public:
+
+
+   /**
+    * Destructor.
+    */
+   virtual ~StateIntegratorInterface (void) {}
+
+   /**
+    * Called to tell an integrator to reset itself when the historical state has
+    * become invalid because of a change the time step or time direction, or
+    * because of some discrete change in state such as a big thruster firing
+    * or docking/undocking.
+    *
+    * The default implementation, which is to do nothing, is valid for single
+    * step integrators that do not maintain a history. Multistep integrators
+    * such as Adams-Bashforth-Moulton and Gauss-Jackson must override this
+    * default.
+    */
+   virtual void reset_integrator (void) {}
+
+   /**
+    * Every integrator needs to be able to create a copy of itself.
+    * Note: This must be defined in this class until C++11.
+    * This class needs a pure virtual to ensure is_abstract<> true.
+    * @return Clone of 'this'.
+    */
+   virtual StateIntegratorInterface* create_copy () const = 0;
+
+
+protected:
+
+   /**
+    * Default constructor.
+    */
+   StateIntegratorInterface (void)
+   : Er7UtilsDeletable()
+   { }
+
+   /**
+    * Copy constructor.
+    */
+   StateIntegratorInterface (const StateIntegratorInterface &)
+   : Er7UtilsDeletable()
+   { }
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   StateIntegratorInterface & operator= (const StateIntegratorInterface &);
+
+};
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/include/time_interface.hh

@@ -0,0 +1,98 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class TimeInterface.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_TIME_INTERFACE_HH
+#define ER7_UTILS_TIME_INTERFACE_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+
+namespace er7_utils {
+
+
+/**
+ * At the most abstract level, a time interface is something that updates an
+ * internal representation of time based on some simulation time.
+ */
+class TimeInterface {
+ER7_UTILS_MAKE_SIM_INTERFACES(TimeInterface)
+
+
+   // Note:
+   //  This is an abstract class.
+   //  - The constructors for this class are protected.
+   //  - The assignment operator for this class is private / unimplemented.
+
+
+public:
+
+   /**
+    * Destructor.
+    */
+   virtual ~TimeInterface () {}
+
+   /**
+    * Update the time model given the simulation time.
+    * \param[in] sim_time Simulation time
+    */
+   virtual void update_time (double sim_time) = 0;
+
+   /**
+    * Get the current scale factor from simulation time
+    * to dynamic time.
+    * \return Simulation time to dynamic time scale factor
+    */
+   virtual double get_time_scale_factor () const = 0;
+
+
+protected:
+
+   /**
+    * TimeInterface default constructor.
+    */
+   TimeInterface () {}
+
+   /**
+    * TimeInterface copy constructor.
+    */
+   TimeInterface (const TimeInterface &) {}
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   TimeInterface & operator = (const TimeInterface &);
+
+};
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/base_integration_group.cc

@@ -0,0 +1,142 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines BaseIntegrationGroup methods.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+// System includes
+#include <algorithm>
+#include <cstddef>
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+#include "er7_utils/interface/include/message_handler.hh"
+
+// Local includes
+#include "../include/base_integration_group.hh"
+#include "../include/bogus_integration_controls.hh"
+#include "../include/integration_controls.hh"
+#include "../include/integration_messages.hh"
+#include "../include/integrator_constructor.hh"
+#include "../include/integrator_interface.hh"
+#include "../include/time_interface.hh"
+
+
+namespace er7_utils {
+
+// BaseIntegrationGroup default constructor.
+// The integ_controls member is initialized to a BogusIntegrationControls
+// instance so that attempts to use an uninitialized group will fail
+// rather than crash.
+BaseIntegrationGroup::BaseIntegrationGroup (
+   void)
+:
+   integ_constructor (NULL),
+   integ_interface (NULL),
+   time_interface (NULL),
+   integ_controls (BogusIntegrationControls().create_copy())
+{
+}
+
+
+// BaseIntegrationGroup non-default constructor.
+BaseIntegrationGroup::BaseIntegrationGroup (
+   IntegratorConstructor & integ_cotr,
+   IntegratorInterface & integ_inter,
+   TimeInterface & time_if)
+:
+   integ_constructor (&integ_cotr),
+   integ_interface (&integ_inter),
+   time_interface (&time_if),
+   integ_controls (BogusIntegrationControls().create_copy())
+{
+}
+
+
+// BaseIntegrationGroup copy constructor.
+// This is probably bogus.
+BaseIntegrationGroup::BaseIntegrationGroup (
+   const BaseIntegrationGroup & source)
+:
+   integ_constructor (source.integ_constructor),
+   integ_interface (source.integ_interface),
+   time_interface (source.time_interface),
+   integ_controls (source.integ_controls ?
+                   source.integ_controls->create_copy() :
+                   BogusIntegrationControls().create_copy())
+{
+}
+
+
+// Non-throwing swap.
+void
+BaseIntegrationGroup::swap (
+   BaseIntegrationGroup & other)
+{
+   std::swap (other.integ_constructor, integ_constructor);
+   std::swap (other.integ_interface, integ_interface);
+   std::swap (other.time_interface, time_interface);
+   std::swap (other.integ_controls, integ_controls);
+}
+
+
+// BaseIntegrationGroup destructor.
+BaseIntegrationGroup::~BaseIntegrationGroup (
+   void)
+{
+   Er7UtilsDeletable::delete_instance (integ_controls);
+}
+
+
+// Initialize the integration group.
+void
+BaseIntegrationGroup::initialize_group (
+   void)
+{
+   // Protect against a null integ_constructor or integ_interface.
+   // Derived classes should ensure that this can't happen.
+   if ((integ_constructor == NULL) || (integ_interface == NULL)) {
+      MessageHandler::error (
+         __FILE__, __LINE__,
+         IntegrationMessages::invalid_request,
+         "Attempt to initialize an improperly constructed integration group.\n"
+         "No initialization was performed.\n");
+      return;
+   }
+
+   // Destroy the existing (if any) integration controls.
+   Er7UtilsDeletable::delete_instance (integ_controls);
+
+   // Create an integration controls via the integrator constructor.
+   integ_controls = integ_constructor->create_integration_controls();
+
+   // Tell the simulation engine whether to calculate derivatives
+   // on the initial step of an integration cycle.
+   integ_interface->set_first_step_derivs_flag (
+      integ_constructor->need_first_step_derivatives());
+
+   // But we always need derivatives on the very first step.
+   integ_interface->reset_first_step_derivs_flag ();
+}
+
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/bogus_integration_controls.cc

@@ -0,0 +1,68 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the methods for class IntegrationControls.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+#include <cstddef>
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+#include "er7_utils/interface/include/message_handler.hh"
+
+// Local includes
+#include "../include/bogus_integration_controls.hh"
+#include "../include/integration_messages.hh"
+
+
+
+namespace er7_utils {
+
+
+// Die. This class exists for one purpose, which is to die when put to use.
+unsigned int
+BogusIntegrationControls::integrate (
+   double,
+   double,
+   TimeInterface&,
+   IntegratorInterface&,
+   BaseIntegrationGroup&)
+{
+   MessageHandler::fail (
+      __FILE__, __LINE__,
+      IntegrationMessages::internal_error,
+      "Integration group has not been initialized.");
+
+      return 0;
+}
+
+
+// Clone a BogusIntegrationControls.
+BogusIntegrationControls *
+BogusIntegrationControls::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/first_order_ode_integrator.cc

@@ -0,0 +1,42 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the class FirstOrderODEIntegrator.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+#include <algorithm>
+
+// Local includes
+#include "../include/first_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+// Swap
+void
+FirstOrderODEIntegrator::swap (
+   FirstOrderODEIntegrator & other)
+{
+   std::swap (state_size, other.state_size);
+}
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/integration_controls.cc

@@ -0,0 +1,145 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the methods for class IntegrationControls.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+#include <algorithm>
+#include <cstddef>
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Local includes
+#include "../include/integration_controls.hh"
+
+
+namespace er7_utils {
+
+
+// IntegrationControls default constructor.
+IntegrationControls::IntegrationControls (
+   void)
+:
+   Er7UtilsDeletable(),
+   time_scale_factor(0.0),
+   integ_starttime(0.0),
+   integ_simdt(0.0),
+   integ_dyndt(0.0),
+   integ_time_scale(0.0),
+   integ_simtime(0.0),
+   transition_table(NULL),
+   step_number(0),
+   cycle_stage(0),
+   /* target_stage(0), */
+   final_stage(0),
+   number_stages(0),
+   reset_needed(true)
+{ }
+
+
+// IntegrationControls copy constructor.
+IntegrationControls::IntegrationControls (
+   const IntegrationControls & source)
+:
+   Er7UtilsDeletable(),
+   time_scale_factor (source.time_scale_factor),
+   integ_starttime (source.integ_starttime),
+   integ_simdt (source.integ_simdt),
+   integ_dyndt (source.integ_dyndt),
+   integ_time_scale (source.integ_time_scale),
+   integ_simtime (source.integ_simtime),
+   transition_table (NULL),
+   step_number (source.step_number),
+   cycle_stage (source.cycle_stage),
+   /* target_stage (source.target_stage), */
+   final_stage (source.final_stage),
+   number_stages (source.number_stages),
+   reset_needed (source.reset_needed)
+{
+   // Allocate and copy the source's transition table if it has one.
+   if (source.transition_table != NULL) {
+      transition_table = alloc::allocate_array<unsigned int> (number_stages);
+      for (unsigned int ii = 0; ii < number_stages; ++ii) {
+         transition_table[ii] = source.transition_table[ii];
+      }
+   }
+}
+
+
+// IntegrationControls non-default constructor.
+IntegrationControls::IntegrationControls (
+   unsigned int number_stages_in)
+:
+   Er7UtilsDeletable(),
+   time_scale_factor(0.0),
+   integ_starttime(0.0),
+   integ_simdt(0.0),
+   integ_dyndt(0.0),
+   integ_time_scale(0.0),
+   integ_simtime(0.0),
+   transition_table(NULL),
+   step_number(0),
+   cycle_stage(0),
+   /* target_stage(0), */
+   final_stage(number_stages_in),
+   number_stages(number_stages_in),
+   reset_needed(true)
+{
+   // Create the default transition table, state n -> n+1
+   transition_table = alloc::allocate_array<unsigned int> (number_stages);
+   for (unsigned int ii = 0; ii < number_stages; ++ii) {
+      transition_table[ii] = ii+1;
+   }
+}
+
+
+// Non-throwing swap.
+void
+IntegrationControls::swap (
+   IntegrationControls & other)
+{
+   std::swap (other.time_scale_factor, time_scale_factor);
+   std::swap (other.integ_starttime, integ_starttime);
+   std::swap (other.integ_simdt, integ_simdt);
+   std::swap (other.integ_dyndt, integ_dyndt);
+   std::swap (other.integ_time_scale, integ_time_scale);
+   std::swap (other.integ_simtime, integ_simtime);
+   std::swap (other.transition_table, transition_table);
+   std::swap (other.step_number, step_number);
+   std::swap (other.cycle_stage, cycle_stage);
+   /* std::swap (other.target_stage, target_stage); */
+   std::swap (other.final_stage, final_stage);
+   std::swap (other.number_stages, number_stages);
+   std::swap (other.reset_needed, reset_needed);
+}
+
+
+// IntegrationControls destructor.
+IntegrationControls::~IntegrationControls (
+   void)
+{
+   alloc::deallocate_array (transition_table);
+}
+
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/integration_messages.cc

@@ -0,0 +1,47 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Implements the class IntegrationMessages.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+// System includes
+
+// Local includes
+#include "../include/integration_messages.hh"
+
+
+/**
+ * Make a message code in class IntegrationMessages.
+ */
+#define MAKE_MESSAGE_CODE(id) \
+    char const * IntegrationMessages::id = "er7_utils/integration/" #id
+
+
+namespace er7_utils {
+
+MAKE_MESSAGE_CODE(unsupported_option);
+MAKE_MESSAGE_CODE(invalid_item);
+MAKE_MESSAGE_CODE(internal_error);
+MAKE_MESSAGE_CODE(invalid_request);
+MAKE_MESSAGE_CODE(information);
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/integrator_constructor.cc

@@ -0,0 +1,125 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the IntegratorConstructor class.
+ */
+
+/*
+Purpose: ()
+*/
+
+// Local includes
+#include "../include/integrator_constructor.hh"
+
+#include "../include/integration_messages.hh"
+#include "../include/integrator_result.hh"
+#include "../include/integrator_result_merger.hh"
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+#include "er7_utils/interface/include/message_handler.hh"
+
+// System includes
+#include <cmath>
+
+
+namespace er7_utils {
+
+
+// Create an integrator results merger object.
+IntegratorResultMerger *
+IntegratorConstructor::create_integrator_results_merger (
+   void)
+const
+{
+   return alloc::allocate_object<IntegratorResultMerger> ();
+}
+
+
+// Create an integrator for a first order ODE.
+FirstOrderODEIntegrator *
+IntegratorConstructor::create_first_order_ode_integrator (
+   unsigned int,
+   IntegrationControls &)
+const
+{
+   MessageHandler::error (
+      __FILE__, __LINE__,
+      IntegrationMessages::unsupported_option,
+      "Unable to create a first order ODE integrator for %s.",
+      get_class_name());
+
+   return NULL;
+}
+
+
+// Create an integrator for a second order ODE.
+SecondOrderODEIntegrator *
+IntegratorConstructor::create_second_order_ode_integrator (
+   unsigned int,
+   IntegrationControls &)
+const
+{
+   MessageHandler::error (
+      __FILE__, __LINE__,
+      IntegrationMessages::unsupported_option,
+      "Unable to create a simple second order ODE integrator for %s.",
+      get_class_name());
+
+   return NULL;
+}
+
+
+// Create an integrator for a second order ODE.
+SecondOrderODEIntegrator *
+IntegratorConstructor::create_generalized_deriv_second_order_ode_integrator (
+   unsigned int,
+   unsigned int,
+   const GeneralizedPositionDerivativeFunctions &,
+   IntegrationControls &)
+const
+{
+   MessageHandler::error (
+      __FILE__, __LINE__,
+      IntegrationMessages::unsupported_option,
+      "Unable to create a generalized second order ODE integrator for %s.",
+      get_class_name());
+
+   return NULL;
+}
+
+
+// Create an integrator for a second order ODE.
+SecondOrderODEIntegrator *
+IntegratorConstructor::create_generalized_step_second_order_ode_integrator (
+   unsigned int,
+   unsigned int,
+   const GeneralizedPositionStepFunctions &,
+   IntegrationControls &)
+const
+{
+   MessageHandler::error (
+      __FILE__, __LINE__,
+      IntegrationMessages::unsupported_option,
+      "Unable to create a generalized second order ODE integrator for %s.",
+      get_class_name());
+
+   return NULL;
+}
+
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/integrator_constructor_factory.cc

@@ -0,0 +1,167 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the IntegratorConstructorFactory class.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+#include <cstddef>
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+#include "er7_utils/interface/include/message_handler.hh"
+
+// Integration includes
+#include "er7_utils/integration/abm4/include/abm4_integrator_constructor.hh"
+#include "er7_utils/integration/beeman/include/beeman_integrator_constructor.hh"
+#include "er7_utils/integration/euler/include/euler_integrator_constructor.hh"
+#if 0
+#include "er7_utils/integration/gauss_jackson/include/gauss_jackson_integrator_constructor.hh"
+#endif
+#include "er7_utils/integration/mm4/include/mm4_integrator_constructor.hh"
+#include "er7_utils/integration/nl2/include/nl2_integrator_constructor.hh"
+#include "er7_utils/integration/position_verlet/include/position_verlet_integrator_constructor.hh"
+#include "er7_utils/integration/rk2_heun/include/rk2_heun_integrator_constructor.hh"
+#include "er7_utils/integration/rk2_midpoint/include/rk2_midpoint_integrator_constructor.hh"
+#include "er7_utils/integration/rk4/include/rk4_integrator_constructor.hh"
+#include "er7_utils/integration/rkf45/include/rkf45_integrator_constructor.hh"
+#include "er7_utils/integration/rkf78/include/rkf78_integrator_constructor.hh"
+#include "er7_utils/integration/rkg4/include/rkg4_integrator_constructor.hh"
+#if 0
+#include "er7_utils/integration/rkn4/include/rkn4_integrator_constructor.hh"
+#endif
+#include "er7_utils/integration/symplectic_euler/include/symplectic_euler_integrator_constructor.hh"
+#include "er7_utils/integration/velocity_verlet/include/velocity_verlet_integrator_constructor.hh"
+
+// Local includes
+#include "../include/integration_messages.hh"
+#include "../include/integration_technique.hh"
+#include "../include/integrator_constructor_factory.hh"
+
+
+namespace er7_utils {
+
+
+/**
+ * Create an er7_utils IntegratorConstructor based on the input technique.
+ * @param integ_type  Integration technique.
+ * @return Created integrator constructor.
+ */
+IntegratorConstructor *
+IntegratorConstructorFactory::create (
+   Integration::Technique integ_type)
+{
+   IntegratorConstructor * constructor = NULL;
+
+   switch (integ_type) {
+   case Integration::Euler:
+      constructor = EulerIntegratorConstructor::create_constructor();
+      break;
+
+   case Integration::SymplecticEuler:
+      constructor = SymplecticEulerIntegratorConstructor::create_constructor();
+      break;
+
+   case Integration::Beeman:
+      constructor = BeemanIntegratorConstructor::create_constructor();
+      break;
+
+   case Integration::NystromLear2:
+      constructor = NystromLear2IntegratorConstructor::create_constructor();
+      break;
+
+   case Integration::PositionVerlet:
+      constructor = PositionVerletIntegratorConstructor::create_constructor();
+      break;
+
+   case Integration::RK2Heun:
+      constructor = RK2HeunIntegratorConstructor::create_constructor();
+      break;
+
+   case Integration::RK2Midpoint:
+      constructor = RK2MidpointIntegratorConstructor::create_constructor();
+      break;
+
+   case Integration::VelocityVerlet:
+      constructor = VelocityVerletIntegratorConstructor::create_constructor();
+      break;
+
+   case Integration::ModifiedMidpoint4:
+      constructor = MM4IntegratorConstructor::create_constructor();
+      break;
+
+   case Integration::AdamsBashforthMoulton4:
+      constructor = ABM4IntegratorConstructor::create_constructor();
+      break;
+
+   case Integration::RungeKutta4:
+      constructor = RK4IntegratorConstructor::create_constructor();
+      break;
+
+   case Integration::RKGill4:
+      constructor = RKGill4IntegratorConstructor::create_constructor();
+      break;
+
+#if 0
+   case Integration::RKNystrom4:
+      constructor = RKNystrom4IntegratorConstructor::create_constructor();
+      break;
+#endif
+
+   case Integration::RKFehlberg45:
+      constructor = RKFehlberg45IntegratorConstructor::create_constructor();
+      break;
+
+   case Integration::RKFehlberg78:
+      constructor = RKFehlberg78IntegratorConstructor::create_constructor();
+      break;
+
+#if 0
+   case Integration::GaussJackson:
+      constructor = GaussJacksonIntegratorConstructor::create_constructor();
+      break;
+#endif
+
+   // The remaining cases do not construct an integrator constructor.
+   case Integration::Unspecified:
+      MessageHandler::fail (__FILE__, __LINE__,
+                            IntegrationMessages::invalid_request,
+                            "Unspecified integration technique.");
+      break;
+
+   case Integration::Unsupported:
+      MessageHandler::fail (__FILE__, __LINE__,
+                            IntegrationMessages::invalid_request,
+                            "Unsupported integration technique.");
+      break;
+
+   case Integration::Invalid:
+   default:
+      MessageHandler::fail (__FILE__, __LINE__,
+                            IntegrationMessages::invalid_request,
+                            "Invalid integration option %d\n.",
+                            integ_type);
+      break;
+   }
+   return constructor;
+}
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/integrator_result_merger.cc

@@ -0,0 +1,125 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the class IntegratorResultMerger.
+ */
+
+/*
+Purpose: ()
+*/
+
+// Local includes
+#include "../include/integrator_result_merger.hh"
+#include "../include/integrator_result.hh"
+#include "../include/integration_messages.hh"
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+#include "er7_utils/interface/include/message_handler.hh"
+
+// System includes
+#include <cmath>
+
+
+namespace er7_utils {
+
+// Merge an IntegratorResult into another.
+bool
+IntegratorResultMerger::merge_integrator_result (
+   const IntegratorResult & new_result,
+   IntegratorResult & merged_result)
+const
+{
+   bool success = true;
+
+   // Leave the old result as-is if the new result is empty.
+   if (new_result.get_merge_count() == 0) {
+      ; // Empty
+   }
+
+   // Take the new result as-is if the merged result is currently empty.
+   else if (merged_result.get_merge_count() == 0) {
+      merged_result = new_result;
+   }
+
+   // A true merge is needed if something has already been merged in.
+   else {
+
+      // A new failure means the merged result failed.
+      // Note that there is no test for a mismatch; in this simple
+      // implementation failure modes are assumed to be zero or one only.
+      if (new_result.get_failure_mode() != 0) {
+         merged_result.set_failure_mode (new_result.get_failure_mode());
+      }
+
+      // Test for a time scale mismatch.
+      if (std::abs (new_result.get_time_scale() -
+                    merged_result.get_time_scale()) > 1e-15) {
+         MessageHandler::error (
+            __FILE__, __LINE__,
+            IntegrationMessages::invalid_item,
+            "Time scale mismatch between %.16f (new) and %.16f (old).",
+            new_result.get_time_scale(),
+            merged_result.get_time_scale());
+         success = false;
+      }
+
+      // Merge the counts.
+      merged_result.increment_merge_count (new_result.get_merge_count());
+   }
+
+   return success;
+}
+
+
+// Clone a IntegratorResultMerger object.
+IntegratorResultMerger *
+IntegratorResultMerger::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Croak when merging an IntegratorResult into another.
+bool
+BogusIntegratorResultMerger::merge_integrator_result (
+   const IntegratorResult &,
+   IntegratorResult &)
+const
+{
+   MessageHandler::fail (
+      __FILE__, __LINE__,
+      IntegrationMessages::invalid_request,
+      "Integration group has not been properly initialized.");
+
+   return false;
+}
+
+
+// Clone a BogusIntegratorResultMerger object.
+IntegratorResultMerger *
+BogusIntegratorResultMerger::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+}
+
+
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/integrator_result_merger_container.cc

@@ -0,0 +1,95 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the class IntegratorResultMergerContainer.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+// Local includes
+#include "../include/integrator_result_merger_container.hh"
+#include "../include/integrator_constructor.hh"
+
+// System includes
+#include <algorithm>
+#include <cstddef>
+
+
+namespace er7_utils {
+
+// IntegratorResultMergerContainer default constructor.
+IntegratorResultMergerContainer::IntegratorResultMergerContainer ()
+:
+   integ_merger(NULL)
+{
+   integ_merger = BogusIntegratorResultMerger().create_copy();
+}
+
+
+// IntegratorResultMergerContainer non-default constructor.
+IntegratorResultMergerContainer::IntegratorResultMergerContainer (
+   const er7_utils::IntegratorConstructor & integ_cotr)
+:
+   integ_merger(NULL)
+{
+   integ_merger = integ_cotr.create_integrator_results_merger();
+}
+
+
+// IntegratorResultMergerContainer copy constructor.
+IntegratorResultMergerContainer::IntegratorResultMergerContainer (
+   const IntegratorResultMergerContainer & src)
+:
+   integ_merger(NULL)
+{
+   integ_merger = src.integ_merger->create_copy();
+}
+
+
+// IntegratorResultMergerContainer destructor.
+IntegratorResultMergerContainer::~IntegratorResultMergerContainer ()
+{
+   Er7UtilsDeletable::delete_instance (integ_merger);
+}
+
+
+// Swap.
+void
+swap (
+   IntegratorResultMergerContainer & a,
+   IntegratorResultMergerContainer & b)
+{
+   std::swap (a.integ_merger, b.integ_merger);
+}
+
+
+// Configure for use with a specific integration technique.
+void
+IntegratorResultMergerContainer::configure (
+   const er7_utils::IntegratorConstructor & integ_cotr)
+{
+   Er7UtilsDeletable::delete_instance (integ_merger);
+   integ_merger = integ_cotr.create_integrator_results_merger();
+}
+
+
+}
+
+
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/left_quaternion_functions.cc

@@ -0,0 +1,347 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the four member functions of the left quaternion specializations
+ * of the generalized position and generalized step classes.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#include <cmath>
+
+#include <cstddef>
+
+#include "../include/left_quaternion_functions.hh"
+
+
+/**
+ * @internal
+ *
+ * @def ONE_OVER_6
+ *    1/6
+ * @def ONE_OVER_12
+ *    1/12
+ * @def ONE_OVER_20
+ *    1/20
+ * @def ONE_OVER_30
+ *    1/30
+ * @def ONE_OVER_40
+ *    1/40
+ * @def ONE_OVER_42
+ *    1/42
+ * @def ONE_OVER_60
+ *    1/60
+ */
+#define ONE_OVER_6    (1.0 /  6.0)
+#define ONE_OVER_12   (1.0 / 12.0)
+#define ONE_OVER_20   (1.0 / 20.0)
+#define ONE_OVER_30   (1.0 / 30.0)
+#define ONE_OVER_40   (1.0 / 40.0)
+#define ONE_OVER_42   (1.0 / 42.0)
+#define ONE_OVER_60   (1.0 / 60.0)
+
+
+namespace er7_utils {
+
+/**
+ * Truncate a small value to zero.
+ * @param[in] val  Value to be trunctated.
+ * @return 0 if val is small, val otherwise.
+ */
+static inline double ER7_UTILS_ALWAYS_INLINE
+truncate_scalar (
+   double val)
+{
+   return ((val > -1e-36) && (val < 1e-36)) ? 0.0 : val;
+}
+
+
+/**
+ * Truncate elements in a 3-vector that contain small values to zero.
+ * @param[in]  in     Vector to be scale
+ * @param[out] out    Scaled vector
+ */
+static inline void ER7_UTILS_ALWAYS_INLINE
+truncate_vector (
+   const double * ER7_UTILS_RESTRICT in,
+   double * ER7_UTILS_RESTRICT out)
+{
+   out[0] = truncate_scalar (in[0]);
+   out[1] = truncate_scalar (in[1]);
+   out[2] = truncate_scalar (in[2]);
+}
+
+
+/**
+ * Compute the square of the magnitude of a 3-vector.
+ * Note: The input vector should not contain extremely small values.
+ * @param[in] vec  Vector whose square magnitude is to be computed.
+ * @return ||vec||^2
+ */
+static inline double ER7_UTILS_ALWAYS_INLINE
+square_magnitude (
+   const double vec[3])
+{
+   return
+      vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2];
+}
+
+
+/**
+ * Compute the inner product between two vectors.
+ * Note: The input vectors should not contain extremely small values.
+ * @param[in] vec1  First vector
+ * @param[in] vec2  Second vector
+ * @return vec1 . vec2
+ */
+static inline double ER7_UTILS_ALWAYS_INLINE
+inner_product (
+   const double vec1[3],
+   const double vec2[3])
+{
+   return
+      vec1[0]*vec2[0] + vec1[1]*vec2[1] + vec1[2]*vec2[2];
+}
+
+
+/**
+ * Compute the Nth component of the cross product of two vectors.
+ * Note: The input vectors should not contain extremely small values.
+ * @tparam N  Component index (0, 1, or 2)
+ * @param[in] vec1  First vector
+ * @param[in] vec2  Second vector
+ * @return (vec1 x vec2)[N]
+ */
+template<int N>
+static inline double ER7_UTILS_ALWAYS_INLINE
+cross_product_element (
+   const double * vec1,
+   const double * vec2)
+{
+   return vec1[(N+1)%3]*vec2[(N+2)%3] - vec1[(N+2)%3]*vec2[(N+1)%3];
+}
+
+
+/**
+ * Scale a 3-vector by some constant.
+ * @param[in]  in     Vector to be scale
+ * @param[in]  scale  Scale factor
+ * @param[out] out    Scaled vector
+ */
+static inline void ER7_UTILS_ALWAYS_INLINE
+scale_vector (
+   const double * ER7_UTILS_RESTRICT in,
+   const double scale,
+   double * ER7_UTILS_RESTRICT out)
+{
+   out[0] = scale * in[0];
+   out[1] = scale * in[1];
+   out[2] = scale * in[2];
+}
+
+
+/**
+ * Scale a 3-vector by some constant, protecting for off-scale low values.
+ * @param[in]  in     Vector to be scale
+ * @param[in]  scale  Scale factor
+ * @param[out] out    Scaled vector
+ */
+static inline void ER7_UTILS_ALWAYS_INLINE
+scale_and_truncate_vector (
+   const double * ER7_UTILS_RESTRICT in,
+   const double scale,
+   double * ER7_UTILS_RESTRICT out)
+{
+   out[0] = scale * truncate_scalar (in[0]);
+   out[1] = scale * truncate_scalar (in[1]);
+   out[2] = scale * truncate_scalar (in[2]);
+}
+
+}
+
+
+namespace er7_utils {
+
+// Compute the time derivative of an inertial to body left transformation
+// quaternion as a function of the quaternion and angular velocity.
+void
+LeftQuaternionGeneralizedPositionDerivativeFunctions::derivative (
+   const double * ER7_UTILS_RESTRICT quat,
+   const double * ER7_UTILS_RESTRICT ang_vel,
+   double * ER7_UTILS_RESTRICT qdot)
+{
+   double qs = truncate_scalar (quat[0]);
+   double qv[3];      // Imaginary part of quaternion, truncated
+   double mhvel[3];   // -0.5*ang_vel, truncated
+
+   truncate_vector (quat+1, qv);
+   scale_and_truncate_vector (ang_vel, -0.5, mhvel);
+
+   // Compute qdot = { 0, -0.5*ang_vel } * q
+   qdot[0] = -inner_product(mhvel, qv);
+   qdot[1] = qs*mhvel[0] + cross_product_element<0>(mhvel, qv);
+   qdot[2] = qs*mhvel[1] + cross_product_element<1>(mhvel, qv);
+   qdot[3] = qs*mhvel[2] + cross_product_element<2>(mhvel, qv);
+}
+
+
+// Compute the second time derivative of an inertial to body left
+// transformation quaternion as a function of the quaternion, angular
+// velocity, and angular acceleration.
+void
+LeftQuaternionGeneralizedPositionDerivativeFunctions::second_derivative (
+   const double * ER7_UTILS_RESTRICT quat,
+   const double * ER7_UTILS_RESTRICT ang_vel,
+   const double * ER7_UTILS_RESTRICT ang_acc,
+   double * ER7_UTILS_RESTRICT qddot)
+{
+   double qs = truncate_scalar (quat[0]);
+   double qv[3];      // Imaginary part of quaternion, truncated
+   double omega[3];   // angular velocity, truncated
+   double mhacc[3];   // -0.5*ang_acc, truncated
+
+   truncate_vector (quat+1, qv);
+   truncate_vector (ang_vel, omega);
+   scale_and_truncate_vector (ang_acc, -0.5, mhacc);
+
+   double mhwsq = -0.25 * square_magnitude(omega);
+
+   qddot[0] = mhwsq*qs - inner_product(mhacc, qv);
+   qddot[1] = mhwsq*qv[0] + qs*mhacc[0] + cross_product_element<0>(mhacc, qv);
+   qddot[2] = mhwsq*qv[1] + qs*mhacc[1] + cross_product_element<1>(mhacc, qv);
+   qddot[3] = mhwsq*qv[2] + qs*mhacc[2] + cross_product_element<2>(mhacc, qv);
+}
+
+
+// Propagate the quaternion given a change dtheta=omega*dt in the angular
+// velocity space per the exponential map applied as a left operator.
+void
+LeftQuaternionGeneralizedPositionStepFunctions::expmap (
+   double const quat_init[4],
+   double const dtheta[3],
+   double quat_end[4])
+{
+   double hdtheta[3];   // 0.5*dtheta, truncated
+
+   scale_and_truncate_vector (dtheta, 0.5, hdtheta);
+
+   // Compute (0.5 * ||dtheta||)^2
+   double h_dtheta_sq = square_magnitude (hdtheta);
+
+   // Too small a value means no change in position.
+   if (h_dtheta_sq < 1e-34) {
+      quat_end[0] = quat_init[0];
+      quat_end[1] = quat_init[1];
+      quat_end[2] = quat_init[2];
+      quat_end[3] = quat_init[3];
+      return;
+   }
+
+   double h_dtheta_mag = std::sqrt (h_dtheta_sq);
+   double qs_init = truncate_scalar (quat_init[0]);
+   double qv_init[3];
+   double mtheta_hat[3];
+   double quat_norm[4];
+
+   truncate_vector (quat_init+1, qv_init);
+
+
+   // Compute the unit quaternion in the direction of the
+   // quaternion time derivative.
+   scale_vector (hdtheta, -1.0/h_dtheta_mag, mtheta_hat);
+
+   quat_norm[0] = -inner_product (mtheta_hat, qv_init);
+   quat_norm[1] = qs_init*mtheta_hat[0] +
+                  cross_product_element<0>(mtheta_hat, qv_init);
+   quat_norm[2] = qs_init*mtheta_hat[1] +
+                  cross_product_element<1>(mtheta_hat, qv_init);
+   quat_norm[3] = qs_init*mtheta_hat[2] +
+                  cross_product_element<2>(mtheta_hat, qv_init);
+
+   // Use polynomial approximations of 1-cos(h_dtheta) and sin(h_dtheta),
+   // valid for angles up to about 2.35 degrees.
+   if (h_dtheta_sq < 0.001685) {
+      // Approximate 1-cos(0.5*dtheta)
+      double omcos_hwdt = 0.5 * h_dtheta_sq *
+                          (1.0 - ONE_OVER_12*h_dtheta_sq *
+                                 (1.0 - ONE_OVER_30*h_dtheta_sq));
+      // Approximate sin(0.5*dtheta)
+      double sin_hwdt = h_dtheta_mag *
+                        (1.0 - ONE_OVER_6*h_dtheta_sq *
+                               (1.0 - ONE_OVER_20*h_dtheta_sq *
+                                      (1.0 - ONE_OVER_42*h_dtheta_sq)));
+
+      for (int ii = 0; ii < 4; ++ii) {
+         double dq = sin_hwdt*quat_norm[ii] - omcos_hwdt*quat_init[ii];
+         quat_end[ii] = quat_init[ii] + dq;
+      }
+   }
+
+   // Use the exact formulation for large angles.
+   else {
+      double cos_hwdt = std::cos (h_dtheta_mag);
+      double sin_hwdt = std::sin (h_dtheta_mag);
+      for (int ii = 0; ii < 4; ++ii) {
+         quat_end[ii] = cos_hwdt*quat_init[ii] + sin_hwdt*quat_norm[ii];
+      }
+   }
+}
+
+
+// Transform the angular velocity from the tangent space at exp(dtheta)
+// to the tangent space at identity via dexpinv.
+void
+LeftQuaternionGeneralizedPositionStepFunctions::dexpinv (
+   double const angular_vel_in[3],
+   double const dtheta[3],
+   double angular_vel_out[3])
+{
+   double vin[3];
+   double dth[3];
+
+   truncate_vector (angular_vel_in, vin);
+   truncate_vector (dtheta, dth);
+
+   // Compute (||dtheta||)^2
+   double dtheta_sq = square_magnitude(dth);
+
+   // Estimate (1-((||dtheta||/2)/2)*cot((||dtheta||/2)/2))/(||dtheta||)^2
+   double fact = ONE_OVER_12 *
+                 (1.0 + dtheta_sq*ONE_OVER_60 *
+                        (1.0 + dtheta_sq*ONE_OVER_42 *
+                               (1.0 + dtheta_sq*ONE_OVER_40)));
+
+   double bracket[3];
+   double double_bracket[3];
+
+   bracket[0] = cross_product_element<0>(dth, vin);
+   bracket[1] = cross_product_element<1>(dth, vin);
+   bracket[2] = cross_product_element<2>(dth, vin);
+
+   double_bracket[0] = cross_product_element<0>(dth, bracket);
+   double_bracket[1] = cross_product_element<1>(dth, bracket);
+   double_bracket[2] = cross_product_element<2>(dth, bracket);
+
+   angular_vel_out[0] = vin[0] + 0.5*bracket[0] + fact*double_bracket[0];
+   angular_vel_out[1] = vin[1] + 0.5*bracket[1] + fact*double_bracket[1];
+   angular_vel_out[2] = vin[2] + 0.5*bracket[2] + fact*double_bracket[2];
+}
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/priming_first_order_ode_integrator.cc

@@ -0,0 +1,201 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the class PrimingFirstOrderODEIntegrator.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+#include <algorithm>
+#include <cstddef>
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/integrator_constructor.hh"
+#include "er7_utils/integration/core/include/integration_controls.hh"
+
+// Model includes
+#include "../include/priming_first_order_ode_integrator.hh"
+#include "../include/priming_integration_controls.hh"
+
+
+namespace er7_utils {
+
+// Default constructor.
+PrimingFirstOrderODEIntegrator::PrimingFirstOrderODEIntegrator (
+   void)
+:
+   Er7UtilsDeletable (),
+   FirstOrderODEIntegrator (),
+   controls (NULL),
+   primer (NULL),
+   priming_state_size (0),
+   prime_counter (0),
+   primed (false)
+{
+}
+
+// Copy constructor.
+PrimingFirstOrderODEIntegrator::PrimingFirstOrderODEIntegrator (
+   const PrimingFirstOrderODEIntegrator & src)
+:
+   Er7UtilsDeletable (),
+   FirstOrderODEIntegrator (src),
+   controls (NULL),
+   primer (src.primer->create_copy()),
+   priming_state_size (src.priming_state_size),
+   prime_counter (src.prime_counter),
+   primed (src.primed)
+{
+}
+
+
+// Non-default constructor for a PrimingFirstOrderODEIntegrator.
+PrimingFirstOrderODEIntegrator::PrimingFirstOrderODEIntegrator (
+   unsigned int priming_size,
+   const IntegratorConstructor & primer_constructor,
+   unsigned int size,
+   IntegrationControls & controls_in)
+:
+   Er7UtilsDeletable (),
+   FirstOrderODEIntegrator (size, controls_in),
+   controls (&controls_in),
+   primer (NULL),
+   priming_state_size (priming_size),
+   prime_counter (priming_size),
+   primed (false)
+{
+   // Create the state integrator used to prime the integrator.
+   primer = primer_constructor.create_first_order_ode_integrator (
+               size, controls_in);
+}
+
+
+// PrimingFirstOrderODEIntegrator destructor.
+PrimingFirstOrderODEIntegrator::~PrimingFirstOrderODEIntegrator (
+   void)
+{
+   // Delete the primer.
+   alloc::delete_object (primer);
+}
+
+
+// Non-throwing swap.
+void
+PrimingFirstOrderODEIntegrator::swap (
+   PrimingFirstOrderODEIntegrator & other)
+{
+   FirstOrderODEIntegrator::swap (other);
+   std::swap (controls, other.controls);
+   std::swap (primer, other.primer);
+   std::swap (priming_state_size, other.priming_state_size);
+   std::swap (prime_counter, other.prime_counter);
+   std::swap (primed, other.primed);
+}
+
+
+// Set the integration controls.
+void
+PrimingFirstOrderODEIntegrator::set_controls (
+   IntegrationControls & controls_in)
+{
+   controls = &controls_in;
+
+   PrimingIntegrationControls * priming_controls =
+      dynamic_cast<PrimingIntegrationControls*> (controls);
+   if ((priming_controls == NULL) || (primer == NULL)) {
+      // FIXME: Error message
+      return;
+   }
+
+   primer->set_controls (priming_controls->get_priming_controls());
+}
+
+
+// Integrate state
+IntegratorResult
+PrimingFirstOrderODEIntegrator::integrate (
+   double dyn_dt,
+   unsigned int target_stage,
+   const double * ER7_UTILS_RESTRICT velocity,
+   double * ER7_UTILS_RESTRICT position)
+{
+
+   /**
+    * ### Overview
+    *
+    * Multistep integration techniques use a recent history of derivatives
+    * at the start of some number of integration cycles to propagate state.
+    * These techniques need to be primed (bootstrapped) by some other
+    * integration technique to establish that derivative history.
+    *
+    * Suppose some multistep technique needs two start of cycle derivatives.
+    * Because derivatives are provided as input to the integrate function,
+    * the first such derivative is "free". The primer must be driven through
+    * a complete integration cycle so as to obtain the needed second set of
+    * derivatives. In general, a technique that uses a history of N derivatives
+    * needs to have the primer driven through N-1 complete cycles.
+    *
+    * This function controls the integration process, driving the primer through
+    * priming_state_size-1 integration cycles so as to establish the requisite
+    * priming_state_size start of cycle derivatives.
+    * The technique itself is used once the integrator has been primed.
+    *
+    * ### Algorithm
+    */
+
+
+   /**
+    * If priming isn't complete,
+    */
+   if (! primed) {
+      /**
+       *  - At the start of an integration cycle,
+       *    + Decrement the countdown to completion of priming.
+       *    + Save the derivatives if the countdown hasn't reached zero.
+       *    + Priming is finished when the countdown reaches zero.
+       */
+      if (controls->in_initial_cycle()) {
+         --prime_counter;
+         if (prime_counter > 0) {
+            technique_save_derivatives (prime_counter, velocity, position);
+         }
+         else {
+            primed = true;
+         }
+      }
+
+      /**
+       *  - Integrate state via the primer and return if priming isn't complete.
+       */
+      if (! primed) {
+         return primer->integrate (dyn_dt, target_stage, velocity, position);
+      }
+   }
+
+   /**
+    * Use the technique-specific integrator the once priming is complete.
+    */
+   return technique_integrate (dyn_dt, target_stage, velocity, position);
+}
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/priming_integration_controls.cc

@@ -0,0 +1,114 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the methods for class PrimingIntegrationControls.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+#include <cstddef>
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Local includes
+#include "../include/integrator_constructor.hh"
+#include "../include/priming_integration_controls.hh"
+
+
+namespace er7_utils {
+
+
+// PrimingIntegrationControls non-default constructor.
+PrimingIntegrationControls::PrimingIntegrationControls (
+   const IntegratorConstructor & primer_constructor,
+   unsigned int history_buffer_size,
+   unsigned int number_operating_stages)
+:
+   StandardIntegrationControls (number_operating_stages),
+   priming_controls (primer_constructor.create_integration_controls()),
+   priming_count (history_buffer_size),
+   history_length (history_buffer_size)
+{
+   set_alt_controls (*priming_controls);
+}
+
+
+// Destructor.
+PrimingIntegrationControls::~PrimingIntegrationControls (
+   void)
+{
+   alloc::delete_object (priming_controls);
+}
+
+
+// Non-throwing swap.
+void
+PrimingIntegrationControls::swap (
+   PrimingIntegrationControls & other)
+{
+   StandardIntegrationControls::swap (other);
+   std::swap (priming_controls, other.priming_controls);
+   std::swap (priming_count, other.priming_count);
+   std::swap (history_length, other.history_length);
+}
+
+
+// Clone a PrimingIntegrationControls.
+PrimingIntegrationControls *
+PrimingIntegrationControls::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Reset the integration controls.
+void
+PrimingIntegrationControls::reset_integrator (
+   void)
+{
+   priming_controls->reset_integrator();
+   priming_count = history_length;
+   set_alt_controls (*priming_controls);
+}
+
+
+// Start an integration tour.
+void
+PrimingIntegrationControls::start_integration_tour (
+   void)
+{
+   // Cycle and tour are synonomous for this class of integration controls.
+   // Set the cycle start and delta times to their tour counterparts.
+   StandardIntegrationControls::start_integration_tour();
+
+   // Not yet primed: Bump the priming count and check if we are now primed.
+   if (priming_count > 0) {
+      --priming_count;
+      // Newly primed: Adjust the transition table so the transition from
+      // stage zero is to the initial operational stage.
+      if (priming_count == 0) {
+         clear_alt_controls ();
+      }
+   }
+}
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/priming_integrator_constructor.cc

@@ -0,0 +1,90 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the PrimingIntegratorConstructor class.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+#include <algorithm>
+#include <cstddef>
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Local includes
+#include "../include/priming_integrator_constructor.hh"
+
+
+namespace er7_utils {
+
+// PrimingIntegratorConstructor default constructor
+PrimingIntegratorConstructor::PrimingIntegratorConstructor (
+   void)
+:
+   Er7UtilsDeletable (),
+   IntegratorConstructor (),
+   primer_constructor (NULL)
+{
+   // Empty
+}
+
+
+// PrimingIntegratorConstructor non-default constructor.
+PrimingIntegratorConstructor::PrimingIntegratorConstructor (
+   const IntegratorConstructor & primer)
+:
+   Er7UtilsDeletable (),
+   IntegratorConstructor (),
+   primer_constructor (primer.create_copy())
+{
+   // Empty
+}
+
+
+// PrimingIntegratorConstructor copy constructor.
+PrimingIntegratorConstructor::PrimingIntegratorConstructor (
+   const PrimingIntegratorConstructor & src)
+:
+   Er7UtilsDeletable (),
+   IntegratorConstructor (),
+   primer_constructor (src.primer_constructor->create_copy())
+{
+   // Empty
+}
+
+
+// PrimingIntegratorConstructor non-throwing swap.
+void
+PrimingIntegratorConstructor::swap (
+   PrimingIntegratorConstructor & src)
+{
+   std::swap (primer_constructor, src.primer_constructor);
+}
+
+
+// PrimingIntegratorConstructor destructor.
+PrimingIntegratorConstructor::~PrimingIntegratorConstructor (
+   void)
+{
+   alloc::delete_object (primer_constructor);
+}
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/priming_second_order_ode_integrator.cc

@@ -0,0 +1,253 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the class PrimingSecondOrderODEIntegrator.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+#include <cstddef>
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/integrator_constructor.hh"
+#include "er7_utils/integration/core/include/integration_controls.hh"
+
+// Model includes
+#include "../include/priming_second_order_ode_integrator.hh"
+#include "../include/priming_integration_controls.hh"
+
+
+namespace er7_utils {
+
+// Default constructor.
+PrimingSecondOrderODEIntegrator::PrimingSecondOrderODEIntegrator (
+   void)
+:
+   Er7UtilsDeletable (),
+   SecondOrderODEIntegrator (),
+   controls (NULL),
+   primer (NULL),
+   priming_state_size (0),
+   prime_counter (0),
+   primed (false)
+{ }
+
+// Copy constructor.
+PrimingSecondOrderODEIntegrator::PrimingSecondOrderODEIntegrator (
+   const PrimingSecondOrderODEIntegrator & src)
+:
+   Er7UtilsDeletable (),
+   SecondOrderODEIntegrator (src),
+   controls (NULL),
+   primer (src.primer->create_copy()),
+   priming_state_size (src.priming_state_size),
+   prime_counter (src.prime_counter),
+   primed (src.primed)
+{ }
+
+
+// Non-default constructor for a PrimingSecondOrderODEIntegrator.
+PrimingSecondOrderODEIntegrator::PrimingSecondOrderODEIntegrator (
+   unsigned int priming_size,
+   const IntegratorConstructor & primer_constructor,
+   unsigned int size,
+   IntegrationControls & controls_in)
+:
+   Er7UtilsDeletable (),
+   SecondOrderODEIntegrator (size, controls_in),
+   controls (&controls_in),
+   primer (NULL),
+   priming_state_size (priming_size),
+   prime_counter (priming_size),
+   primed (false)
+{
+   // Create the state integrator used to prime the integrator.
+   primer = primer_constructor.create_second_order_ode_integrator (
+               size, controls_in);
+}
+
+
+// Non-default constructor for a PrimingSecondOrderODEIntegrator
+// for generalized position, generalized velocity.
+PrimingSecondOrderODEIntegrator::PrimingSecondOrderODEIntegrator (
+   unsigned int priming_size,
+   const IntegratorConstructor & primer_constructor,
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionDerivativeFunctions & deriv_funs,
+   IntegrationControls & controls_in)
+:
+   Er7UtilsDeletable (),
+   SecondOrderODEIntegrator (position_size, velocity_size,
+                             deriv_funs, controls_in),
+   controls (&controls_in),
+   primer (NULL),
+   priming_state_size (priming_size),
+   prime_counter (priming_size),
+   primed (false)
+{
+   // Create the integrator used to prime the state.
+   primer =
+      primer_constructor.create_generalized_deriv_second_order_ode_integrator (
+         position_size, velocity_size, deriv_funs, controls_in);
+}
+
+
+// Non-default constructor for a PrimingSecondOrderODEIntegrator
+// for generalized position, generalized velocity.
+PrimingSecondOrderODEIntegrator::PrimingSecondOrderODEIntegrator (
+   unsigned int priming_size,
+   const IntegratorConstructor & primer_constructor,
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionStepFunctions & step_funs,
+   IntegrationControls & controls_in)
+:
+   Er7UtilsDeletable (),
+   SecondOrderODEIntegrator (position_size, velocity_size,
+                             step_funs, controls_in),
+   controls (&controls_in),
+   primer (NULL),
+   priming_state_size (priming_size),
+   prime_counter (priming_size),
+   primed (false)
+{
+   // Create the integrator used to prime the state.
+   primer =
+      primer_constructor.create_generalized_step_second_order_ode_integrator (
+         position_size, velocity_size, step_funs, controls_in);
+}
+
+
+// PrimingSecondOrderODEIntegrator destructor.
+PrimingSecondOrderODEIntegrator::~PrimingSecondOrderODEIntegrator (
+   void)
+{
+   // Delete the primer.
+   alloc::delete_object (primer);
+}
+
+
+// Non-throwing swap.
+void
+PrimingSecondOrderODEIntegrator::swap (
+   PrimingSecondOrderODEIntegrator & other)
+{
+   SecondOrderODEIntegrator::swap (other);
+   std::swap (controls, other.controls);
+   std::swap (primer, other.primer);
+   std::swap (priming_state_size, other.priming_state_size);
+   std::swap (prime_counter, other.prime_counter);
+   std::swap (primed, other.primed);
+}
+
+
+// Set the integration controls.
+void
+PrimingSecondOrderODEIntegrator::set_controls (
+   IntegrationControls & controls_in)
+{
+   controls = &controls_in;
+
+   PrimingIntegrationControls * priming_controls =
+      dynamic_cast<PrimingIntegrationControls*> (controls);
+   if ((priming_controls == NULL) || (primer == NULL)) {
+      // FIXME: Error message
+      return;
+   }
+
+   primer->set_controls (priming_controls->get_priming_controls());
+}
+
+
+// Integrate state
+IntegratorResult
+PrimingSecondOrderODEIntegrator::integrate (
+   double dyn_dt,
+   unsigned int target_stage,
+   const double * ER7_UTILS_RESTRICT accel,
+   double * ER7_UTILS_RESTRICT velocity,
+   double * ER7_UTILS_RESTRICT position)
+{
+
+   /**
+    * ### Overview
+    *
+    * Multistep integration techniques use a recent history of derivatives
+    * at the start of some number of integration cycles to propagate state.
+    * These techniques need to be primed (bootstrapped) by some other
+    * integration technique to establish that derivative history.
+    *
+    * Suppose some multistep technique needs two start of cycle derivatives.
+    * Because derivatives are provided as input to the integrate function,
+    * the first such derivative is "free". The primer must be driven through
+    * a complete integration cycle so as to obtain the needed second set of
+    * derivatives. In general, a technique that uses a history of N derivatives
+    * needs to have the primer driven through N-1 complete cycles.
+    *
+    * This function controls the integration process, driving the primer through
+    * priming_state_size-1 integration cycles so as to establish the requisite
+    * priming_state_size start of cycle derivatives.
+    * The technique itself is used once the integrator has been primed.
+    *
+    * ### Algorithm
+    */
+
+
+   /**
+    * If priming isn't complete,
+    */
+   if (! primed) {
+      /**
+       *  - At the start of an integration cycle,
+       *    + Decrement the countdown.
+       *    + Save the derivatives if the countdown hasn't reached zero.
+       *    + Priming is finished when the countdown reaches zero.
+       */
+      if (controls->in_initial_cycle()) {
+         --prime_counter;
+         if (prime_counter > 0) {
+            technique_save_derivatives (
+               prime_counter, accel, velocity, position);
+         }
+         else {
+            primed = true;
+         }
+      }
+
+      /**
+       *  - Integrate state via the primer and return if priming isn't complete.
+       */
+      if (! primed) {
+         return primer->integrate (
+                   dyn_dt, target_stage, accel, velocity, position);
+      }
+   }
+
+   /**
+    * Use the technique-specific integrator the once priming is complete.
+    */
+   return technique_integrate (dyn_dt, target_stage, accel, velocity, position);
+}
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/second_order_ode_integrator.cc

@@ -0,0 +1,207 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the class SecondOrderODEIntegrator.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+#include <algorithm>
+#include <cstddef>
+
+// Interface includes
+#include "er7_utils/interface/include/message_handler.hh"
+
+// Local includes
+#include "../include/integration_messages.hh"
+#include "../include/second_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+// SecondOrderODEIntegrator default constructor.
+SecondOrderODEIntegrator::SecondOrderODEIntegrator (
+   void)
+:
+   Er7UtilsDeletable (),
+   StateIntegratorInterface (),
+   compute_posdot (NULL),
+   compute_posdotdot (NULL),
+   compute_expmap_position_step (NULL),
+   compute_dexpinv_velocity_transform (NULL),
+   problem_type (Integration::SimpleSecondOrderODE)
+{
+   // Initialize the sizes.
+   state_size[0] = state_size[1] = 0;
+}
+
+// SecondOrderODEIntegrator copy constructor.
+SecondOrderODEIntegrator::SecondOrderODEIntegrator (
+   const SecondOrderODEIntegrator & src)
+:
+   Er7UtilsDeletable (),
+   StateIntegratorInterface (src),
+   compute_posdot (src.compute_posdot),
+   compute_posdotdot (src.compute_posdotdot),
+   compute_expmap_position_step (src.compute_expmap_position_step),
+   compute_dexpinv_velocity_transform (src.compute_dexpinv_velocity_transform),
+   problem_type (src.problem_type)
+{
+   // Initialize the sizes.
+   state_size[0] = src.state_size[0];
+   state_size[1] = src.state_size[1];
+}
+
+
+// SecondOrderODEIntegrator non-default constructor.
+SecondOrderODEIntegrator::SecondOrderODEIntegrator (
+   unsigned int size,
+   IntegrationControls &)
+:
+   Er7UtilsDeletable (),
+   StateIntegratorInterface(),
+   compute_posdot (NULL),
+   compute_posdotdot (NULL),
+   compute_expmap_position_step (NULL),
+   compute_dexpinv_velocity_transform (NULL),
+   problem_type (Integration::SimpleSecondOrderODE)
+{
+   // Save the provided sizes.
+   state_size[0] = state_size[1] = size;
+}
+
+
+// SecondOrderODEIntegrator non-default constructor.
+SecondOrderODEIntegrator::SecondOrderODEIntegrator (
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionDerivativeFunctions & deriv_funs,
+   IntegrationControls &)
+:
+   Er7UtilsDeletable (),
+   StateIntegratorInterface (),
+   compute_posdot (deriv_funs.first_deriv_fun),
+   compute_posdotdot (deriv_funs.second_deriv_fun),
+   compute_expmap_position_step (NULL),
+   compute_dexpinv_velocity_transform (NULL),
+   problem_type (Integration::GeneralizedDerivSecondOrderODE)
+{
+   // The derivative functions must not be null.
+   if ((compute_posdot == NULL) || (compute_posdotdot == NULL)) {
+      MessageHandler::fail (
+         __FILE__, __LINE__,
+         IntegrationMessages::invalid_request,
+         "The function pointers provided to\n"
+         "the SecondOrderODEIntegrator constructor must be non-null.");
+   }
+
+   // Save the provided sizes.
+   state_size[0] = position_size;
+   state_size[1] = velocity_size;
+}
+
+
+// SecondOrderODEIntegrator non-default constructor.
+SecondOrderODEIntegrator::SecondOrderODEIntegrator (
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionStepFunctions & step_funs,
+   IntegrationControls &)
+:
+   Er7UtilsDeletable (),
+   StateIntegratorInterface (),
+   compute_posdot (NULL),
+   compute_posdotdot (NULL),
+   compute_expmap_position_step (step_funs.expmap_step_fun),
+   compute_dexpinv_velocity_transform (step_funs.dexpinv_xform_fun),
+   problem_type (Integration::GeneralizedStepSecondOrderODE)
+{
+   // The step functions must not be null.
+   if ((compute_expmap_position_step == NULL) ||
+       (compute_dexpinv_velocity_transform == NULL)) {
+      MessageHandler::fail (
+         __FILE__, __LINE__,
+         IntegrationMessages::invalid_request,
+         "The function pointers provided to\n"
+         "the SecondOrderODEIntegrator constructor must be non-null.");
+   }
+
+   // Save the provided sizes.
+   state_size[0] = position_size;
+   state_size[1] = velocity_size;
+}
+
+
+// Reset the position derivative functions.
+void
+SecondOrderODEIntegrator::set_position_derivative_functions (
+   const GeneralizedPositionDerivativeFunctions & deriv_funs)
+{
+   if ((deriv_funs.first_deriv_fun == NULL) ||
+       (deriv_funs.second_deriv_fun == NULL)) {
+      MessageHandler::fail (
+         __FILE__, __LINE__,
+         IntegrationMessages::invalid_request,
+         "The derivative function pointers provided to\n"
+         "set_position_derivative_functions must be non-null.");
+   }
+
+   compute_posdot = deriv_funs.first_deriv_fun;
+   compute_posdotdot = deriv_funs.second_deriv_fun;
+}
+
+
+// Reset the position step functions.
+void
+SecondOrderODEIntegrator::set_position_step_functions (
+   const GeneralizedPositionStepFunctions & step_funs)
+{
+   if ((step_funs.expmap_step_fun == NULL) ||
+       (step_funs.dexpinv_xform_fun == NULL)) {
+      MessageHandler::fail (
+         __FILE__, __LINE__,
+         IntegrationMessages::invalid_request,
+         "The step function pointers provided to\n"
+         "set_position_step_functions must be non-null.");
+   }
+
+   compute_expmap_position_step = step_funs.expmap_step_fun;
+   compute_dexpinv_velocity_transform = step_funs.dexpinv_xform_fun;
+}
+
+
+// Swap
+void
+SecondOrderODEIntegrator::swap (
+   SecondOrderODEIntegrator & other)
+{
+   std::swap (compute_posdot, other.compute_posdot);
+   std::swap (compute_posdotdot, other.compute_posdotdot);
+   std::swap (compute_expmap_position_step,
+              other.compute_expmap_position_step);
+   std::swap (compute_dexpinv_velocity_transform,
+              other.compute_dexpinv_velocity_transform);
+   std::swap (state_size[0], other.state_size[0]);
+   std::swap (state_size[1], other.state_size[1]);
+}
+
+
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/single_cycle_integration_controls.cc

@@ -0,0 +1,117 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the methods for class IntegrationControls.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Local includes
+#include "../include/base_integration_group.hh"
+#include "../include/integrator_interface.hh"
+#include "../include/time_interface.hh"
+#include "../include/single_cycle_integration_controls.hh"
+
+
+namespace er7_utils {
+
+
+// Clone a SingleCycleIntegrationControls.
+SingleCycleIntegrationControls *
+SingleCycleIntegrationControls::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Perform one step of the integration process.
+unsigned int
+SingleCycleIntegrationControls::integrate (
+   double starttime,
+   double sim_dt,
+   TimeInterface & time_interface,
+   IntegratorInterface & integ_interface,
+   BaseIntegrationGroup & integ_group)
+{
+
+   // Starting a new integration tour/cycle needs special processing.
+   if (cycle_stage == 0) {
+
+      // Reset the integrators, time if the meaning of time has changed.
+      if (reset_needed || (integ_simdt != sim_dt)) {
+
+         // Reset integrators.
+         integ_group.reset_body_integrators ();
+         reset_integrator ();
+
+         // Update timing information.
+         time_scale_factor = time_interface.get_time_scale_factor();
+         integ_simdt = sim_dt;
+         integ_dyndt = sim_dt * time_scale_factor;
+
+         // Mark the reset as handled.
+         reset_needed = false;
+         integ_interface.restore_first_step_derivs_flag ();
+      }
+
+      // Set time to the start of the interval.
+      integ_simtime = integ_starttime = starttime;
+      integ_time_scale = 0.0;
+   }
+
+   // Advance the stage per the transition table.
+   unsigned int target_stage = transition_table[cycle_stage];
+
+   // Integrate the dynamic bodies to the target stage and advance time.
+   // This simple controller assumes integration always succeeds.
+   const IntegratorResult & integ_status =
+      integ_group.integrate_bodies (integ_dyndt, target_stage);
+   double time_scale = integ_status.get_time_scale();
+
+   // Advance time.
+   if (integ_time_scale != time_scale) {
+      integ_time_scale = time_scale;
+      integ_simtime    = integ_starttime + time_scale*integ_simdt;
+      time_interface.update_time (integ_simtime);
+   }
+
+   // Reset cycle_stage, step_number at the end of a cycle.
+   if (target_stage == final_stage) {
+      cycle_stage = 0;
+      step_number = 0;
+      time_interface.update_time(integ_starttime+integ_simdt);
+   }
+   else {
+      cycle_stage = target_stage;
+      step_number++;
+   }
+
+   // Update the sim engine's integration interface structure.
+   integ_group.update_integration_interface (step_number);
+
+   // Return the step number, which is zero upon completion.
+   return step_number;
+}
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/core/src/standard_integration_controls.cc

@@ -0,0 +1,186 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the methods for class IntegrationControls.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+#include <algorithm>
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Local includes
+#include "../include/base_integration_group.hh"
+#include "../include/integrator_interface.hh"
+#include "../include/time_interface.hh"
+#include "../include/standard_integration_controls.hh"
+
+
+namespace er7_utils {
+
+
+// Non-throwing swap.
+void
+StandardIntegrationControls::swap (
+   StandardIntegrationControls & other)
+{
+   IntegrationControls::swap (other);
+   std::swap (cycle_starttime, other.cycle_starttime);
+   std::swap (cycle_simdt, other.cycle_simdt);
+   std::swap (cycle_dyndt, other.cycle_dyndt);
+   std::swap (multi_cycle, other.multi_cycle);
+   /* std::swap (target_attained, other.target_attained); */
+   std::swap (alt_controls, other.alt_controls);
+}
+
+
+// Clone a StandardIntegrationControls.
+StandardIntegrationControls *
+StandardIntegrationControls::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Perform one step of the integration process.
+unsigned int
+StandardIntegrationControls::integrate (
+   double starttime,
+   double sim_dt,
+   TimeInterface & time_interface,
+   IntegratorInterface & integ_interface,
+   BaseIntegrationGroup & integ_group)
+{
+
+   // Starting a new integration tour needs special processing.
+   if (step_number == 0) {
+
+      // Reset the integrators, time if the meaning of time has changed.
+      if (reset_needed || (integ_simdt != sim_dt)) {
+
+         // Reset integrators.
+         integ_group.reset_body_integrators ();
+         reset_integrator ();
+
+         // Update timing information.
+         time_scale_factor = time_interface.get_time_scale_factor();
+         integ_simdt = sim_dt;
+         integ_dyndt = sim_dt * time_scale_factor;
+
+         // Mark the reset as handled.
+         reset_needed = false;
+         integ_interface.restore_first_step_derivs_flag ();
+      }
+
+      // Cache the integration tour start time.
+      integ_starttime = starttime;
+
+      // Perform start of tour operations for a multi-cycle integrator.
+      if (multi_cycle) {
+         start_integration_tour ();
+      }
+
+      // Standard single-cycle integrator: Set the integration cycle times.
+      else {
+         cycle_starttime  = integ_starttime;
+         cycle_simdt      = integ_simdt;
+         cycle_dyndt      = integ_dyndt;
+         integ_simtime    = integ_starttime;
+         integ_time_scale = 0.0;
+      }
+   }
+
+
+   // No alternative integrator:
+   // Control the main body of the integration step.
+   if (alt_controls == NULL) {
+
+      // Set the target_stage based on the transition table and cycle_stage.
+       unsigned int target_stage = transition_table[cycle_stage];
+
+      // Integrate the bodies that comprise the group to the target stage.
+      // The group integrator returns true if all integrators succeeded.
+      const IntegratorResult & integ_status =
+         integ_group.integrate_bodies (integ_dyndt, target_stage);
+      bool target_attained = integ_status.get_passed();
+      double time_scale = integ_status.get_time_scale();
+
+      // Advance time to the next stage.
+      if (integ_time_scale != time_scale) {
+         integ_time_scale = time_scale;
+         integ_simtime    = cycle_starttime + time_scale*cycle_simdt;
+         time_interface.update_time (integ_simtime);
+      }
+
+      // Target attained by all integrators:
+      // Advance the cycle stage to the attained target stage.
+      if (target_attained) {
+         cycle_stage = target_stage;
+      }
+
+      // FUTURE FIXME:
+      // Perform a technique-specific action if the target was not attained.
+      // For now, the else is intentionally empty.
+      else {
+      }
+
+      // At the end of a cycle:
+      // Reset cycle_stage, and advance or reset the step_number.
+      if (cycle_stage == final_stage) {
+
+         cycle_stage = 0;
+
+         // Reset the step number to zero for a single-cycle integrator
+         // or for a multi-cycle integrator that is at the end of its tour.
+         if ((! multi_cycle) || end_integration_cycle()) {
+            step_number = 0;
+         }
+
+         // Multi-cycle integrator that isn't finished: Bump the step number.
+         else {
+            step_number++;
+         }
+      }
+
+      // Not at the end of the cycle: Bump the step number.
+      else {
+         step_number++;
+      }
+
+      // Update the sim engine's integration interface structure.
+      integ_group.update_integration_interface (step_number);
+   }
+
+   // Non-standard: Use the alternate integrator.
+   else {
+      step_number = alt_controls->integrate (
+                       starttime, sim_dt,
+                       time_interface, integ_interface, integ_group);
+   }
+
+   // Return the step number, which is zero upon completion.
+   return step_number;
+
+}
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/euler/include/euler_first_order_ode_integrator.hh

@@ -0,0 +1,117 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class EulerFirstOrderODEIntegrator, which integrates a monolithic
+ * state via the Euler method.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_EULER_ONE_STATE_INTEGRATOR_HH
+#define ER7_UTILS_EULER_ONE_STATE_INTEGRATOR_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/first_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+/**
+ * Propagate state using Euler integration.
+ */
+class EulerFirstOrderODEIntegrator : public FirstOrderODEIntegrator {
+ER7_UTILS_MAKE_SIM_INTERFACES(EulerFirstOrderODEIntegrator)
+
+public:
+
+   // Constructors and destructor.
+
+   /**
+    * EulerFirstOrderODEIntegrator default constructor.
+    */
+   EulerFirstOrderODEIntegrator (void);
+
+   /**
+    * EulerFirstOrderODEIntegrator copy constructor.
+    * @param[in] src  Item to be copied.
+    */
+   EulerFirstOrderODEIntegrator (const EulerFirstOrderODEIntegrator & src);
+
+   /**
+    * EulerFirstOrderODEIntegrator non-default constructor.
+    * @param[in]     size      State size
+    * @param[in,out] controls  Integration controls
+    */
+   EulerFirstOrderODEIntegrator (
+      unsigned int size,
+      IntegrationControls & controls);
+
+   /**
+    * EulerFirstOrderODEIntegrator destructor.
+    */
+   virtual ~EulerFirstOrderODEIntegrator (void);
+
+
+   // Member functions.
+
+   /**
+    * EulerFirstOrderODEIntegrator assignment operator.
+    * @param src  Item to be copied.
+    */
+   EulerFirstOrderODEIntegrator & operator=(
+      EulerFirstOrderODEIntegrator src)
+   {
+      swap (src);
+      return *this;
+   }
+
+   /**
+    * Create a copy of 'this' RK4FirstOrderODEIntegrator object.
+    * @return Clone of 'this'.
+    */
+   virtual EulerFirstOrderODEIntegrator * create_copy () const;
+
+   /**
+    * Propagate state via Euler's method.
+    * @param[in] dyn_dt        Integration interval step, dynamic time seconds.
+    * @param[in] target_stage  The stage of the integration process
+    *                          that the integrator should try to attain.
+    * @param[in,out] velocity  Generalized velocity vector.
+    * @param[in,out] position  Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      const double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position);
+
+};
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/euler/include/euler_integrator_constructor.hh

@@ -0,0 +1,176 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class EulerIntegratorConstructor, which constructs integrators
+ * that use Euler integration.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_EULER_INTEGRATOR_CONSTRUCTOR_HH
+#define ER7_UTILS_EULER_INTEGRATOR_CONSTRUCTOR_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/integrator_constructor.hh"
+
+
+namespace er7_utils {
+
+/**
+ * Create state and time integrators that propagate using standard Euler.
+ */
+class EulerIntegratorConstructor : public IntegratorConstructor {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(EulerIntegratorConstructor)
+
+public:
+   // Static member functions.
+
+   /**
+    * Named constructor; create an EulerIntegratorConstructor instance.
+    * The caller is responsible for deleting the returned object.
+    * @return Newly created EulerIntegratorConstructor instance.
+    */
+   static IntegratorConstructor* create_constructor (void);
+
+
+   // Constructors and destructor.
+
+   // Note: The copy constructor and assignment operator for this
+   // class are not declared. The C++ defaults suffice for this class.
+
+   /**
+    * EulerIntegratorConstructor default constructor.
+    */
+   EulerIntegratorConstructor (void)
+   : IntegratorConstructor ()
+   { }
+
+   /**
+    * EulerIntegratorConstructor destructor.
+    */
+   virtual ~EulerIntegratorConstructor (void)
+   { }
+
+
+   // Member functions.
+
+   /**
+    * Return the class name.
+    */
+   virtual const char * get_class_name (void) const
+   { return "EulerIntegratorConstructor"; }
+
+   /**
+    * Create a duplicate of the constructor.
+    * The caller is responsible for deleting the returned object.
+    * @return Duplicated constructor.
+    */
+   virtual IntegratorConstructor * create_copy (void) const;
+
+   /**
+    * Create an integration controls that guides the Euler integration process.
+    * The caller is responsible for deleting the created object.
+    * @return Integration controls object
+    */
+   virtual IntegrationControls * create_integration_controls (void) const;
+
+   /**
+    * Create an Euler state integrator for a first order ODE.
+    * The caller is responsible for deleting the created object.
+    * @return State integrator
+    * @param[in]     size      State size
+    * @param[in,out] controls  Integration controls
+    */
+   virtual FirstOrderODEIntegrator * create_first_order_ode_integrator (
+      unsigned int size,
+      IntegrationControls & controls) const;
+
+   /**
+    * Create an Euler state integrator for a simple second order ODE.
+    * The caller is responsible for deleting the created object.
+    * @return State integrator
+    * @param[in]     size      State size
+    * @param[in,out] controls  Integration controls
+    */
+   virtual SecondOrderODEIntegrator * create_second_order_ode_integrator (
+      unsigned int size,
+      IntegrationControls & controls) const;
+
+   /**
+    * Create an Euler state integrator for a generalized second order ODE where
+    * generalized position is advanced with the use of the derivative function.
+    * The caller is responsible for deleting the created object.
+    * @return State integrator
+    * @param[in]     position_size  Size of the generalized position
+    * @param[in]     velocity_size  Size of the generalized velocity
+    * @param[in]     deriv_funs     Position derivative functions container
+    * @param[in,out] controls       Integration controls
+    */
+   virtual SecondOrderODEIntegrator *
+   create_generalized_deriv_second_order_ode_integrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionDerivativeFunctions & deriv_funs,
+      IntegrationControls & controls) const;
+
+   /**
+    * Create an Euler state integrator for a generalized second order ODE where
+    * generalized position is advanced with the use of the step function.
+    * The caller is responsible for deleting the created object.
+    * @return State integrator
+    * @param[in]     position_size  Size of the generalized position
+    * @param[in]     velocity_size  Size of the generalized velocity
+    * @param[in]     step_funs      Position step functions container
+    * @param[in,out] controls       Integration controls
+    */
+   virtual SecondOrderODEIntegrator *
+   create_generalized_step_second_order_ode_integrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionStepFunctions & step_funs,
+      IntegrationControls & controls) const;
+
+
+   /**
+    * Euler uses one step per cycle.
+    * @return Always returns 1.
+    */
+   virtual unsigned int get_transition_table_size (void) const
+   { return 1; }
+
+};
+
+}
+
+
+#ifdef ER7_UTILS_NEED_AUX_INCLUDES
+#include "er7_utils/integration/core/include/single_cycle_integration_controls.hh"
+#include "euler_first_order_ode_integrator.hh"
+#include "euler_second_order_ode_integrator.hh"
+#endif
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/euler/include/euler_second_order_ode_integrator.hh

@@ -0,0 +1,440 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class EulerSecondOrderODEIntegrator, which integrates a state
+ * comprising a zeroth derivative / first derivative pair via Euler's method.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_EULER_TWO_STATE_INTEGRATOR_HH
+#define ER7_UTILS_EULER_TWO_STATE_INTEGRATOR_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/second_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+/**
+ * Propagate state using Euler's method.
+ */
+class EulerSecondOrderODEIntegrator : public SecondOrderODEIntegrator {
+ER7_UTILS_MAKE_SIM_INTERFACES(EulerSecondOrderODEIntegrator)
+
+
+   // Note:
+   //  This is an abstract class.
+   //  - The constructors for this class are protected.
+   //  - The assignment operator for this class is private / unimplemented.
+
+
+public:
+
+   /**
+    * EulerSecondOrderODEIntegrator destructor.
+    */
+   virtual ~EulerSecondOrderODEIntegrator (void);
+
+
+protected:
+
+   // Constructors.
+
+   /**
+    * EulerSecondOrderODEIntegrator default constructor.
+    */
+   EulerSecondOrderODEIntegrator (void);
+
+   /**
+    * EulerSecondOrderODEIntegrator copy constructor.
+    * @param[in] src  Object to be copied.
+    */
+   EulerSecondOrderODEIntegrator (const EulerSecondOrderODEIntegrator & src);
+
+   /**
+    * EulerSecondOrderODEIntegrator non-default constructor
+    * for a simple second order ODE.
+    * This constructor is used by the integrator constructor.
+    * @param[in]     size                State size
+    * @param[in,out] controls            Integration controls
+    */
+   EulerSecondOrderODEIntegrator (
+      unsigned int size,
+      IntegrationControls & controls);
+
+
+   /**
+    * EulerSecondOrderODEIntegrator non-default constructor
+    * for a generalized second order ODE in which position is advanced
+    * using the provided derivative function.
+    * This constructor is used by the integrator constructor.
+    * @param[in]     position_size  Size of the generalized position
+    * @param[in]     velocity_size  Size of the generalized velocity
+    * @param[in]     deriv_funs     Position derivative functions container
+    * @param[in,out] controls       Integration controls
+    */
+   EulerSecondOrderODEIntegrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionDerivativeFunctions & deriv_funs,
+      IntegrationControls & controls);
+
+   /**
+    * EulerSecondOrderODEIntegrator non-default constructor
+    * for a generalized second order ODE in which position is advanced
+    * using the provided position step function.
+    * This constructor is used by the integrator constructor.
+    * @param[in]     position_size  Size of the generalized position
+    * @param[in]     velocity_size  Size of the generalized velocity
+    * @param[in]     step_funs      Position step functions container
+    * @param[in,out] controls       Integration controls
+    */
+   EulerSecondOrderODEIntegrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionStepFunctions & step_funs,
+      IntegrationControls & controls);
+
+
+protected:
+
+   // Member functions.
+
+   /**
+    * Non-throwing swap.
+    * @param other  Item with which contents are to be swapped.
+    */
+   void swap (EulerSecondOrderODEIntegrator & other);
+
+   using SecondOrderODEIntegrator::swap;
+
+
+   // Member data.
+
+   double * posdot; /**< trick_units(--) @n
+      Position derivative. */
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   EulerSecondOrderODEIntegrator & operator= (
+      const EulerSecondOrderODEIntegrator &);
+};
+
+
+/**
+ * Specialization of EulerSecondOrderODEIntegrator for the case of
+ * generalized velocity being the time derivative of generalized position.
+ */
+class EulerSimpleSecondOrderODEIntegrator :
+   public EulerSecondOrderODEIntegrator {
+ER7_UTILS_MAKE_SIM_INTERFACES(EulerSimpleSecondOrderODEIntegrator)
+
+public:
+
+   // Constructors and destructor.
+
+   /**
+    * EulerSimpleSecondOrderODEIntegrator default constructor.
+    */
+   EulerSimpleSecondOrderODEIntegrator (void)
+   :
+      Er7UtilsDeletable(),
+      EulerSecondOrderODEIntegrator()
+   {}
+
+   /**
+    * EulerSimpleSecondOrderODEIntegrator copy constructor.
+    * @param[in] src  Object to be copied.
+    */
+   EulerSimpleSecondOrderODEIntegrator (
+      const EulerSimpleSecondOrderODEIntegrator & src)
+   :
+      Er7UtilsDeletable(),
+      EulerSecondOrderODEIntegrator(src)
+   {}
+
+   /**
+    * EulerSimpleSecondOrderODEIntegrator non-default constructor.
+    * @param[in]     size      Size of the position and velocity vectors
+    * @param[in,out] controls  Integration controls
+    */
+   EulerSimpleSecondOrderODEIntegrator (
+      unsigned int size,
+      IntegrationControls & controls)
+   :
+      Er7UtilsDeletable(),
+      EulerSecondOrderODEIntegrator(size, controls)
+   {}
+
+   /**
+    * EulerSimpleSecondOrderODEIntegrator destructor.
+    */
+   virtual ~EulerSimpleSecondOrderODEIntegrator (void)
+   {}
+
+
+   // Member functions.
+
+   /**
+    * EulerSimpleSecondOrderODEIntegrator assignment operator.
+    * @param[in] src  Object to be copied.
+    */
+   EulerSimpleSecondOrderODEIntegrator & operator= (
+      EulerSimpleSecondOrderODEIntegrator src)
+   {
+      swap (src);
+      return *this;
+   }
+
+   /**
+    * Create a copy of 'this' EulerSimpleSecondOrderODEIntegrator object.
+    * @return Clone of 'this'.
+    */
+   virtual EulerSimpleSecondOrderODEIntegrator * create_copy () const;
+
+   /**
+    * Propagate state using Euler's method.
+    * @param[in]     dyn_dt        Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     accel         Time derivative of the generalized velocity.
+    * @param[in,out] velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      double const * ER7_UTILS_RESTRICT accel,
+      double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position);
+};
+
+
+/**
+ * Specialization of EulerSecondOrderODEIntegrator for the case of
+ * the time derivative of generalized position being some function of
+ * the generalized position and generalized velocity.
+ */
+class EulerGeneralizedDerivSecondOrderODEIntegrator :
+   public EulerSecondOrderODEIntegrator {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(EulerGeneralizedDerivSecondOrderODEIntegrator)
+
+public:
+
+   // Constructors and destructor.
+
+   /**
+    * EulerGeneralizedDerivSecondOrderODEIntegrator default constructor.
+    */
+   EulerGeneralizedDerivSecondOrderODEIntegrator (void)
+   :
+      Er7UtilsDeletable(),
+      EulerSecondOrderODEIntegrator()
+   {}
+
+   /**
+    * EulerGeneralizedDerivSecondOrderODEIntegrator copy constructor.
+    * @param[in] src  Object to be copied.
+    */
+   EulerGeneralizedDerivSecondOrderODEIntegrator (
+      const EulerGeneralizedDerivSecondOrderODEIntegrator & src)
+   :
+      Er7UtilsDeletable(),
+      EulerSecondOrderODEIntegrator(src)
+   {}
+
+   /**
+    * EulerGeneralizedDerivSecondOrderODEIntegrator non-default constructor.
+    * @param[in]     position_size  Size of the generalized position
+    * @param[in]     velocity_size  Size of the generalized velocity
+    * @param[in]     deriv_funs     Derivative functions container
+    * @param[in,out] controls       Integration controls
+    */
+   EulerGeneralizedDerivSecondOrderODEIntegrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionDerivativeFunctions & deriv_funs,
+      IntegrationControls & controls)
+   :
+      Er7UtilsDeletable (),
+      EulerSecondOrderODEIntegrator (position_size, velocity_size,
+                                     deriv_funs, controls)
+   {}
+
+   /**
+    * EulerGeneralizedDerivSecondOrderODEIntegrator destructor.
+    */
+   virtual ~EulerGeneralizedDerivSecondOrderODEIntegrator (void)
+   {}
+
+
+   // Member functions.
+
+   /**
+    * EulerGeneralizedDerivSecondOrderODEIntegrator assignment operator.
+    * @param[in] src  Object to be copied.
+    */
+   EulerGeneralizedDerivSecondOrderODEIntegrator & operator= (
+      EulerGeneralizedDerivSecondOrderODEIntegrator src)
+   {
+      swap (src);
+      return *this;
+   }
+
+   /**
+    * Create a copy of 'this' object.
+    * @return Clone of 'this'.
+    */
+   virtual EulerGeneralizedDerivSecondOrderODEIntegrator * create_copy () const;
+
+   /**
+    * Propagate state using Euler's method.
+    * @param[in]     dyn_dt        Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     accel         Time derivative of the generalized velocity.
+    * @param[in,out] velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      double const * ER7_UTILS_RESTRICT accel,
+      double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position);
+};
+
+
+/**
+ * Specialization of EulerSecondOrderODEIntegrator for the case of
+ * the time derivative of generalized position being some function of
+ * the generalized position and generalized velocity.
+ */
+class EulerGeneralizedStepSecondOrderODEIntegrator :
+   public EulerSecondOrderODEIntegrator {
+
+ER7_UTILS_MAKE_SIM_INTERFACES(EulerGeneralizedStepSecondOrderODEIntegrator)
+
+public:
+
+   // Constructors and destructor.
+
+   /**
+    * EulerGeneralizedStepSecondOrderODEIntegrator default constructor.
+    */
+   EulerGeneralizedStepSecondOrderODEIntegrator (void)
+   :
+      Er7UtilsDeletable(),
+      EulerSecondOrderODEIntegrator()
+   {}
+
+   /**
+    * EulerGeneralizedStepSecondOrderODEIntegrator copy constructor.
+    * @param[in] src  Object to be copied.
+    */
+   EulerGeneralizedStepSecondOrderODEIntegrator (
+      const EulerGeneralizedStepSecondOrderODEIntegrator & src)
+   :
+      Er7UtilsDeletable(),
+      EulerSecondOrderODEIntegrator(src)
+   {}
+
+   /**
+    * EulerGeneralizedStepSecondOrderODEIntegrator non-default constructor.
+    * @param[in]     position_size  Size of the generalized position
+    * @param[in]     velocity_size  Size of the generalized velocity
+    * @param[in]     step_funs      Step functions container
+    * @param[in,out] controls       Integration controls
+    */
+   EulerGeneralizedStepSecondOrderODEIntegrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionStepFunctions & step_funs,
+      IntegrationControls & controls)
+   :
+      Er7UtilsDeletable (),
+      EulerSecondOrderODEIntegrator (position_size, velocity_size,
+                                     step_funs, controls)
+   {}
+
+   /**
+    * EulerGeneralizedDerivSecondOrderODEIntegrator destructor.
+    */
+   virtual ~EulerGeneralizedStepSecondOrderODEIntegrator (void)
+   {}
+
+
+   // Member functions.
+
+   /**
+    * EulerGeneralizedStepSecondOrderODEIntegrator assignment operator.
+    * @param[in] src  Object to be copied.
+    */
+   EulerGeneralizedStepSecondOrderODEIntegrator & operator= (
+      EulerGeneralizedStepSecondOrderODEIntegrator src)
+   {
+      swap (src);
+      return *this;
+   }
+
+   /**
+    * Create a copy of 'this' object.
+    * @return Clone of 'this'.
+    */
+   virtual EulerGeneralizedStepSecondOrderODEIntegrator * create_copy () const;
+
+   /**
+    * Propagate state using Euler's method.
+    * @param[in]     dyn_dt        Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     accel         Time derivative of the generalized velocity.
+    * @param[in,out] velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      double const * ER7_UTILS_RESTRICT accel,
+      double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position);
+};
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/euler/src/euler_first_order_ode_integrator.cc

@@ -0,0 +1,107 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the class EulerFirstOrderODEIntegrator.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/integ_utils.hh"
+
+// Model includes
+#include "../include/euler_first_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+// Default constructor for a EulerFirstOrderODEIntegrator.
+EulerFirstOrderODEIntegrator::EulerFirstOrderODEIntegrator (
+   void)
+:
+   Er7UtilsDeletable (),
+   FirstOrderODEIntegrator ()
+{
+   ; /* Empty */
+}
+
+
+// EulerFirstOrderODEIntegrator copy constructor.
+EulerFirstOrderODEIntegrator::EulerFirstOrderODEIntegrator (
+   const EulerFirstOrderODEIntegrator & src)
+:
+   Er7UtilsDeletable (),
+   FirstOrderODEIntegrator (src)
+{
+   ; /* Empty */
+}
+
+
+// Non-default constructor for a EulerFirstOrderODEIntegrator.
+EulerFirstOrderODEIntegrator::EulerFirstOrderODEIntegrator (
+   unsigned int size,
+   IntegrationControls & controls)
+:
+   Er7UtilsDeletable (),
+   FirstOrderODEIntegrator (size, controls)
+{
+   ; /* Empty */
+}
+
+
+// Destructor for a EulerFirstOrderODEIntegrator.
+EulerFirstOrderODEIntegrator::~EulerFirstOrderODEIntegrator (
+   void)
+{
+   ; /* Empty */
+}
+
+
+// Clone a EulerFirstOrderODEIntegrator.
+EulerFirstOrderODEIntegrator *
+EulerFirstOrderODEIntegrator::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Propagate state via Euler's method.
+IntegratorResult
+EulerFirstOrderODEIntegrator::integrate (
+   double dyn_dt,
+   unsigned int target_stage,
+   const double * ER7_UTILS_RESTRICT velocity,
+   double * ER7_UTILS_RESTRICT position)
+{
+
+   // The only valid target_stage is 1.
+   if (target_stage == 1) {
+      integ_utils::inplace_euler_step (velocity, dyn_dt, state_size, position);
+   }
+
+   return 1.0;
+}
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/euler/src/euler_integrator_constructor.cc

@@ -0,0 +1,127 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the class EulerIntegratorConstructor.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/integrator_constructor_utils.hh"
+#include "er7_utils/integration/core/include/single_cycle_integration_controls.hh"
+
+// Model includes
+#include "../include/euler_integrator_constructor.hh"
+#include "../include/euler_first_order_ode_integrator.hh"
+#include "../include/euler_second_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+// Named constructor; create an EulerIntegratorConstructor.
+IntegratorConstructor*
+EulerIntegratorConstructor::create_constructor (
+   void)
+{
+   return alloc::allocate_object<EulerIntegratorConstructor> ();
+}
+
+
+// Create a duplicate of the constructor.
+IntegratorConstructor *
+EulerIntegratorConstructor::create_copy (
+   void)
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Create an integration controls for Euler.
+IntegrationControls *
+EulerIntegratorConstructor::create_integration_controls (
+   void)
+const
+{
+   return integ_utils::allocate_controls<SingleCycleIntegrationControls> (1);
+}
+
+
+// Create an Euler integrator for a first order ODE.
+FirstOrderODEIntegrator *
+EulerIntegratorConstructor::create_first_order_ode_integrator (
+   unsigned int size,
+   IntegrationControls & controls)
+const
+{
+   return integ_utils::allocate_integrator<EulerFirstOrderODEIntegrator> (
+             size, controls);
+}
+
+
+// Create an Euler integrator for a second order ODE.
+SecondOrderODEIntegrator *
+EulerIntegratorConstructor::create_second_order_ode_integrator (
+   unsigned int size,
+   IntegrationControls & controls)
+const
+{
+   return integ_utils::allocate_integrator<
+                EulerSimpleSecondOrderODEIntegrator> (
+             size, controls);
+}
+
+
+// Create an Euler integrator for a second order ODE.
+SecondOrderODEIntegrator *
+EulerIntegratorConstructor::
+create_generalized_deriv_second_order_ode_integrator (
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionDerivativeFunctions & deriv_funs,
+   IntegrationControls & controls)
+const
+{
+   return integ_utils::allocate_integrator<
+                EulerGeneralizedDerivSecondOrderODEIntegrator> (
+             position_size, velocity_size, deriv_funs, controls);
+}
+
+
+// Create an Euler integrator for a second order ODE.
+SecondOrderODEIntegrator *
+EulerIntegratorConstructor::
+create_generalized_step_second_order_ode_integrator (
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionStepFunctions & step_funs,
+   IntegrationControls & controls)
+const
+{
+   return integ_utils::allocate_integrator<
+                EulerGeneralizedStepSecondOrderODEIntegrator> (
+             position_size, velocity_size, step_funs, controls);
+}
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/euler/src/euler_second_order_ode_integrator.cc

@@ -0,0 +1,241 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the class EulerSecondOrderODEIntegrator.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/integ_utils.hh"
+
+// Model includes
+#include "../include/euler_second_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+// EulerSecondOrderODEIntegrator default constructor.
+EulerSecondOrderODEIntegrator::EulerSecondOrderODEIntegrator (
+   void)
+:
+   Er7UtilsDeletable (),
+   SecondOrderODEIntegrator (),
+   posdot (NULL)
+{ }
+
+
+// Copy constructor.
+EulerSecondOrderODEIntegrator::EulerSecondOrderODEIntegrator (
+   const EulerSecondOrderODEIntegrator & src)
+:
+   Er7UtilsDeletable (),
+   SecondOrderODEIntegrator (src),
+   posdot (NULL)
+{
+   // Replicate the source's contents if they exist.
+   if (problem_type == Integration::GeneralizedStepSecondOrderODE) {
+      posdot = alloc::replicate_array (state_size[1], src.posdot);
+   }
+
+   else if (problem_type == Integration::GeneralizedDerivSecondOrderODE) {
+      posdot = alloc::replicate_array (state_size[0], src.posdot);
+   }
+}
+
+
+
+// EulerSecondOrderODEIntegrator non-default constructor for a simple
+// second order ODE.
+EulerSecondOrderODEIntegrator::EulerSecondOrderODEIntegrator (
+   unsigned int size,
+   IntegrationControls & controls)
+:
+   Er7UtilsDeletable (),
+   SecondOrderODEIntegrator (size, controls),
+   posdot (NULL)
+{ }
+
+
+// EulerSecondOrderODEIntegrator non-default constructor for a generalized
+// second order ODE in which position is advanced internally using the position
+// derivative computed by the provided derivative function.
+EulerSecondOrderODEIntegrator::EulerSecondOrderODEIntegrator (
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionDerivativeFunctions & deriv_funs,
+   IntegrationControls & controls)
+:
+   Er7UtilsDeletable (),
+   SecondOrderODEIntegrator (position_size, velocity_size,
+                             deriv_funs, controls),
+   posdot (NULL)
+{
+   // Allocate memory used by Euler integration.
+   posdot = alloc::allocate_array<double> (position_size);
+}
+
+
+// EulerSecondOrderODEIntegrator non-default constructor for a generalized
+// second order ODE in which position is advanced externally by the provided
+// position step function.
+EulerSecondOrderODEIntegrator::EulerSecondOrderODEIntegrator (
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionStepFunctions & step_funs,
+   IntegrationControls & controls)
+:
+   Er7UtilsDeletable (),
+   SecondOrderODEIntegrator (position_size, velocity_size,
+                             step_funs, controls),
+   posdot (NULL)
+{
+   // Allocate memory used by Euler integration.
+   posdot = alloc::allocate_array<double> (velocity_size);
+}
+
+
+// EulerSecondOrderODEIntegrator destructor.
+EulerSecondOrderODEIntegrator::~EulerSecondOrderODEIntegrator (
+   void)
+{
+   alloc::deallocate_array<double> (posdot);
+}
+
+
+// Non-throwing swap.
+void
+EulerSecondOrderODEIntegrator::swap (
+   EulerSecondOrderODEIntegrator & other)
+{
+   SecondOrderODEIntegrator::swap (other);
+
+   std::swap (posdot, other.posdot);
+}
+
+
+// Clone a EulerSimpleSecondOrderODEIntegrator.
+EulerSimpleSecondOrderODEIntegrator *
+EulerSimpleSecondOrderODEIntegrator::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Clone a EulerGeneralizedDerivSecondOrderODEIntegrator.
+EulerGeneralizedDerivSecondOrderODEIntegrator *
+EulerGeneralizedDerivSecondOrderODEIntegrator::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Clone a EulerGeneralizedStepSecondOrderODEIntegrator.
+EulerGeneralizedStepSecondOrderODEIntegrator *
+EulerGeneralizedStepSecondOrderODEIntegrator::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Propagate state for the special case of velocity being the derivative of
+// position.
+IntegratorResult
+EulerSimpleSecondOrderODEIntegrator::integrate (
+   double dyn_dt,
+   unsigned int target_stage,
+   double const * ER7_UTILS_RESTRICT accel,
+   double * ER7_UTILS_RESTRICT velocity,
+   double * ER7_UTILS_RESTRICT position)
+{
+   if (target_stage == 1) {
+
+      // Propagate position using the propagated velocity.
+      integ_utils::inplace_euler_step (velocity, dyn_dt, state_size[0],
+                                       position);
+
+      // Propagate velocity.
+      integ_utils::inplace_euler_step (accel, dyn_dt, state_size[1],
+                                       velocity);
+   }
+
+   return 1.0;
+}
+
+
+// Propagate state for the general case of the generalized position derivative
+// being a function of generalized position and generalized velocity.
+IntegratorResult
+EulerGeneralizedDerivSecondOrderODEIntegrator::integrate (
+   double dyn_dt,
+   unsigned int target_stage,
+   double const * ER7_UTILS_RESTRICT accel,
+   double * ER7_UTILS_RESTRICT velocity,
+   double * ER7_UTILS_RESTRICT position)
+{
+   if (target_stage == 1) {
+
+      // Compute position derivative based on the input velocity
+      // and propagate position.
+      compute_posdot (position, velocity, posdot);
+      integ_utils::inplace_euler_step (posdot, dyn_dt, state_size[0],
+                                       position);
+
+      // Propagate generalized velocity.
+      integ_utils::inplace_euler_step (accel, dyn_dt, state_size[1],
+                                       velocity);
+   }
+
+   return 1.0;
+}
+
+
+// Propagate state for the general case of the generalized position derivative
+// being a function of generalized position and generalized velocity.
+IntegratorResult
+EulerGeneralizedStepSecondOrderODEIntegrator::integrate (
+   double dyn_dt,
+   unsigned int target_stage,
+   double const * ER7_UTILS_RESTRICT accel,
+   double * ER7_UTILS_RESTRICT velocity,
+   double * ER7_UTILS_RESTRICT position)
+{
+   if (target_stage == 1) {
+
+      // Propagate position using the external step function.
+      integ_utils::scale_array (velocity, dyn_dt, state_size[1], posdot);
+      compute_expmap_position_step (position, posdot, position);
+
+      // Propagate generalized velocity as an Euler step.
+      integ_utils::inplace_euler_step (accel, dyn_dt, state_size[1],
+                                       velocity);
+   }
+
+   return 1.0;
+}
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/mm4/include/mm4_integrator_constructor.hh

@@ -0,0 +1,171 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class MM4IntegratorConstructor, which constructs integrators
+ * that use MM4 integration.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_MM4_INTEGRATOR_CONSTRUCTOR_HH
+#define ER7_UTILS_MM4_INTEGRATOR_CONSTRUCTOR_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Model includes
+#include "er7_utils/integration/core/include/integrator_constructor.hh"
+
+
+namespace er7_utils {
+
+/**
+ * Create state and time integrators that propagate using standard MM4.
+ */
+class MM4IntegratorConstructor : public IntegratorConstructor {
+ER7_UTILS_MAKE_SIM_INTERFACES(MM4IntegratorConstructor)
+
+   // Note: The default constructor, copy constructor, assignment operator,
+   // and destructor for this class are not declared.
+   // The implicit C++ defaults suffice for this class.
+
+public:
+   // Static member functions.
+
+   /**
+    * Named constructor; create an MM4IntegratorConstructor instance.
+    * The caller is responsible for deleting the returned object.
+    * @return Newly created MM4IntegratorConstructor instance.
+    */
+   static IntegratorConstructor* create_constructor (void);
+
+
+   // Member functions.
+
+   /**
+    * Return the class name.
+    */
+   virtual const char * get_class_name (void) const
+   { return "MM4IntegratorConstructor"; }
+
+   /**
+    * Modified midpoint 4 is a second order technique; it provides but does not
+    * implement a first order ODE integrator. It neither provides nor implements
+    * a generalized step second order ODE integrator.
+    */
+   virtual bool implements (Integration::ODEProblemType problem_type) const
+   {
+      return (problem_type != Integration::FirstOrderODE) &&
+             (problem_type != Integration::GeneralizedStepSecondOrderODE);
+   }
+
+   /**
+    * Modified midpoint 4 does not provide a generalized step second order
+    * ODE integrator.
+    */
+   virtual bool provides (Integration::ODEProblemType problem_type) const
+   {
+      return (problem_type != Integration::GeneralizedStepSecondOrderODE);
+   }
+
+   /**
+    * Create a duplicate of the constructor.
+    * The caller is responsible for deleting the returned object.
+    * @return Duplicated constructor.
+    */
+   virtual IntegratorConstructor * create_copy (void) const;
+
+   /**
+    * Create an integration controls that guides the MM4 integration process.
+    * The caller is responsible for deleting the created object.
+    * @return Integration controls object
+    */
+   virtual IntegrationControls * create_integration_controls (void) const;
+
+   /**
+    * Create a surrogate integrator to solve a first order ODE.
+    * The caller is responsible for deleting the created object.
+    * @return State integrator
+    * @param[in]     size      State size
+    * @param[in,out] controls  Integration controls
+    */
+   virtual FirstOrderODEIntegrator * create_first_order_ode_integrator (
+      unsigned int size,
+      IntegrationControls & controls) const;
+
+   /**
+    * Create an MM4 state integrator for a simple second order ODE.
+    * The caller is responsible for deleting the created object.
+    * @return State integrator
+    * @param[in]     size      State size
+    * @param[in,out] controls  Integration controls
+    */
+   virtual SecondOrderODEIntegrator * create_second_order_ode_integrator (
+      unsigned int size,
+      IntegrationControls & controls) const;
+
+   /**
+    * Create an MM4 state integrator for a generalized second order ODE
+    * where generalized position is advanced with the use of the
+    * position derivative function.
+    * The caller is responsible for deleting the created object.
+    * @return State integrator
+    * @param[in]     position_size  Size of the generalized position
+    * @param[in]     velocity_size  Size of the generalized velocity
+    * @param[in]     deriv_funs     Position derivative functions container
+    * @param[in,out] controls       Integration controls
+    */
+   virtual SecondOrderODEIntegrator *
+   create_generalized_deriv_second_order_ode_integrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionDerivativeFunctions & deriv_funs,
+      IntegrationControls & controls) const;
+
+   /**
+    * Indicate whether the integration technique explicitly solves a second
+    * order ODE. Modified midpoint 4 is such a technique.
+    * @return Always returns true.
+    */
+   virtual bool is_second_order_ode_technique (void) const
+   { return true; }
+
+
+   /**
+    * MM4 uses three steps per cycle.
+    * \return Always returns 3.
+    */
+   virtual unsigned int get_transition_table_size (void) const
+   { return 3; }
+};
+
+}
+
+
+#ifdef ER7_UTILS_NEED_AUX_INCLUDES
+#include "er7_utils/integration/core/include/single_cycle_integration_controls.hh"
+#include "er7_utils/integration/rk2_midpoint/include/rk2_midpoint_first_order_ode_integrator.hh"
+#include "mm4_second_order_ode_integrator.hh"
+#endif
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/mm4/include/mm4_second_order_ode_integrator.hh

@@ -0,0 +1,330 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines the class MM4SecondOrderODEIntegrator, which integrates a state
+ * comprising a zeroth derivative / first derivative pair via the MM4 method.
+ */
+
+/*
+Purpose: ()
+*/
+
+
+#ifndef ER7_UTILS_MM4_TWO_STATE_INTEGRATOR_HH
+#define ER7_UTILS_MM4_TWO_STATE_INTEGRATOR_HH
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/er7_class.hh"
+
+// Model includes
+#include "er7_utils/integration/core/include/second_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+/**
+ * Propagate state using the modified midpoint 4 method.
+ */
+class MM4SecondOrderODEIntegrator : public SecondOrderODEIntegrator {
+ER7_UTILS_MAKE_SIM_INTERFACES(MM4SecondOrderODEIntegrator)
+
+
+   // Note:
+   //  This is an abstract class.
+   //  - The constructors for this class are protected.
+   //  - The assignment operator for this class is private / unimplemented.
+
+
+public:
+
+   /**
+    * MM4SecondOrderODEIntegrator destructor.
+    */
+   virtual ~MM4SecondOrderODEIntegrator (void);
+
+
+protected:
+
+   // Constructors.
+
+   /**
+    * MM4SecondOrderODEIntegrator default constructor.
+    */
+   MM4SecondOrderODEIntegrator (void);
+
+   /**
+    * MM4SecondOrderODEIntegrator copy constructor.
+    * @param[in] src  Object to be copied.
+    */
+   MM4SecondOrderODEIntegrator (const MM4SecondOrderODEIntegrator & src);
+
+   /**
+    * MM4SecondOrderODEIntegrator non-default constructor
+    * for a simple second order ODE.
+    * This constructor is used by the integrator constructor.
+    * @param[in]     size                State size
+    * @param[in,out] controls            Integration controls
+    */
+   MM4SecondOrderODEIntegrator (
+      unsigned int size,
+      IntegrationControls & controls);
+
+   /**
+    * MM4SecondOrderODEIntegrator non-default constructor
+    * for a generalized second order ODE in which position is advanced
+    * using the position derivative function.
+    * @param[in]     position_size       Size of the generalized position
+    * @param[in]     velocity_size       Size of the generalized velocity
+    * @param[in]     deriv_funs          Position derivative functions container
+    * @param[in,out] controls            Integration controls
+    */
+   MM4SecondOrderODEIntegrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionDerivativeFunctions & deriv_funs,
+      IntegrationControls & controls);
+
+
+   // Member functions.
+
+   /**
+    * Non-throwing swap.
+    * @param other  Item with which contents are to be swapped.
+    */
+   void swap (MM4SecondOrderODEIntegrator & other);
+
+   using SecondOrderODEIntegrator::swap;
+
+
+   // Member data.
+
+   double * pos_hist[3]; /**< trick_units(--) @n
+      Position history, initial plus results from first two steps. */
+
+   double * vel_hist[3]; /**< trick_units(--) @n
+      Position history, initial plus results from first two steps. */
+
+   double * posdot; /**< trick_units(--) @n
+      Derivative of generalized position. */
+
+
+private:
+
+   /**
+    * Not implemented.
+    */
+   MM4SecondOrderODEIntegrator & operator= (
+      const MM4SecondOrderODEIntegrator &);
+};
+
+
+/**
+ * Specialization of MM4SecondOrderODEIntegrator for the case of
+ * generalized velocity being the time derivative of generalized position.
+ */
+class MM4SimpleSecondOrderODEIntegrator :
+   public MM4SecondOrderODEIntegrator {
+ER7_UTILS_MAKE_SIM_INTERFACES(MM4SimpleSecondOrderODEIntegrator)
+
+public:
+
+   // Constructors and destructor.
+
+   /**
+    * MM4SimpleSecondOrderODEIntegrator default constructor.
+    */
+   MM4SimpleSecondOrderODEIntegrator (void)
+   :
+      Er7UtilsDeletable (),
+      MM4SecondOrderODEIntegrator ()
+   {}
+
+   /**
+    * MM4SimpleSecondOrderODEIntegrator copy constructor.
+    * @param[in] src  Object to be copied.
+    */
+   MM4SimpleSecondOrderODEIntegrator (
+      const MM4SimpleSecondOrderODEIntegrator & src)
+   :
+      Er7UtilsDeletable (),
+      MM4SecondOrderODEIntegrator (src)
+   {}
+
+   /**
+    * MM4SimpleSecondOrderODEIntegrator non-default constructor.
+    * @param[in]     size      Size of the position and velocity vectors
+    * @param[in,out] controls  Integration controls
+    */
+   MM4SimpleSecondOrderODEIntegrator (
+      unsigned int size,
+      IntegrationControls & controls)
+   :
+      Er7UtilsDeletable (),
+      MM4SecondOrderODEIntegrator (size, controls)
+   {}
+
+   /**
+    * MM4SimpleSecondOrderODEIntegrator destructor.
+    */
+   virtual ~MM4SimpleSecondOrderODEIntegrator (void)
+   {}
+
+
+   // Member functions.
+
+   /**
+    * MM4SimpleSecondOrderODEIntegrator assignment operator.
+    * @param[in] src  Object to be copied.
+    */
+   MM4SimpleSecondOrderODEIntegrator & operator= (
+      MM4SimpleSecondOrderODEIntegrator src)
+   {
+      swap (src);
+      return *this;
+   }
+
+   /**
+    * Create a copy of 'this' MM4SimpleSecondOrderODEIntegrator object.
+    * @return Clone of 'this'.
+    */
+   virtual MM4SimpleSecondOrderODEIntegrator * create_copy () const;
+
+   /**
+    * Propagate state via MM4 for the special case of velocity being the
+    * derivative of position.
+    * @param[in]     dyn_dt        Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     accel         Time derivative of the generalized velocity.
+    * @param[in,out] velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      double const * ER7_UTILS_RESTRICT accel,
+      double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position);
+};
+
+
+/**
+ * Specialization of MM4SecondOrderODEIntegrator for the case of
+ * the time derivative of generalized position being some function of
+ * the generalized position and generalized velocity.
+ */
+class MM4GeneralizedDerivSecondOrderODEIntegrator :
+   public MM4SecondOrderODEIntegrator {
+ER7_UTILS_MAKE_SIM_INTERFACES(MM4GeneralizedDerivSecondOrderODEIntegrator)
+
+public:
+
+   // Constructors and destructor.
+
+   /**
+    * MM4GeneralizedDerivSecondOrderODEIntegrator default constructor.
+    */
+   MM4GeneralizedDerivSecondOrderODEIntegrator (void)
+   :
+      Er7UtilsDeletable (),
+      MM4SecondOrderODEIntegrator ()
+   {}
+
+   /**
+    * MM4GeneralizedDerivSecondOrderODEIntegrator copy constructor.
+    * @param[in] src  Object to be copied.
+    */
+   MM4GeneralizedDerivSecondOrderODEIntegrator (
+      const MM4GeneralizedDerivSecondOrderODEIntegrator & src)
+   :
+      Er7UtilsDeletable (),
+      MM4SecondOrderODEIntegrator (src)
+   {}
+
+   /**
+    * MM4GeneralizedDerivSecondOrderODEIntegrator non-default constructor.
+    * @param[in]     position_size  Size of the generalized position
+    * @param[in]     velocity_size  Size of the generalized velocity
+    * @param[in]     deriv_funs     Position derivative functions container
+    * @param[in,out] controls       Integration controls
+    */
+   MM4GeneralizedDerivSecondOrderODEIntegrator (
+      unsigned int position_size,
+      unsigned int velocity_size,
+      const GeneralizedPositionDerivativeFunctions & deriv_funs,
+      IntegrationControls & controls)
+   :
+      Er7UtilsDeletable (),
+      MM4SecondOrderODEIntegrator (position_size, velocity_size,
+                                   deriv_funs, controls)
+   {}
+
+   /**
+    * MM4GeneralizedDerivSecondOrderODEIntegrator destructor.
+    */
+   virtual ~MM4GeneralizedDerivSecondOrderODEIntegrator (void)
+   {}
+
+
+   // Member functions.
+
+   /**
+    * MM4GeneralizedDerivSecondOrderODEIntegrator assignment operator.
+    * @param[in] src  Object to be copied.
+    */
+   MM4GeneralizedDerivSecondOrderODEIntegrator & operator= (
+      MM4GeneralizedDerivSecondOrderODEIntegrator src)
+   {
+      swap (src);
+      return *this;
+   }
+
+   /**
+    * Create a copy of 'this' object.
+    * @return Clone of 'this'.
+    */
+   virtual MM4GeneralizedDerivSecondOrderODEIntegrator * create_copy () const;
+
+   /**
+    * Propagate state via MM4 for generalized position and generalized velocity
+    * where generalized position is advanced using the function that yields
+    * the time derivative of generalized position.
+    * @param[in]     dyn_dt        Dynamic time step, in dynamic time seconds.
+    * @param[in]     target_stage  The stage of the integration process
+    *                              that the integrator should try to attain.
+    * @param[in]     accel         Time derivative of the generalized velocity.
+    * @param[in,out] velocity      Generalized velocity vector.
+    * @param[in,out] position      Generalized position vector.
+    *
+    * @return The status (time advance, pass/fail status) of the integration.
+    */
+   virtual IntegratorResult integrate (
+      double dyn_dt,
+      unsigned int target_stage,
+      double const * ER7_UTILS_RESTRICT accel,
+      double * ER7_UTILS_RESTRICT velocity,
+      double * ER7_UTILS_RESTRICT position);
+};
+
+}
+
+
+#endif
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/mm4/src/mm4_integrator_constructor.cc

@@ -0,0 +1,110 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the class MM4IntegratorConstructor.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Model includes
+#include "er7_utils/integration/core/include/integrator_constructor_utils.hh"
+#include "er7_utils/integration/core/include/single_cycle_integration_controls.hh"
+#include "er7_utils/integration/rk2_midpoint/include/rk2_midpoint_first_order_ode_integrator.hh"
+
+// Local includes
+#include "../include/mm4_integrator_constructor.hh"
+#include "../include/mm4_second_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+// Named constructor; create an MM4IntegratorConstructor.
+IntegratorConstructor*
+MM4IntegratorConstructor::create_constructor (
+   void)
+{
+   return alloc::allocate_object<MM4IntegratorConstructor> ();
+}
+
+
+// Create a duplicate of the constructor.
+IntegratorConstructor *
+MM4IntegratorConstructor::create_copy (
+   void)
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Create an MM4 integration controls.
+IntegrationControls *
+MM4IntegratorConstructor::create_integration_controls (
+   void)
+const
+{
+   return integ_utils::allocate_controls<SingleCycleIntegrationControls> (3);
+}
+
+
+// Create an MM4-compatible state integrator for a first order ODE.
+FirstOrderODEIntegrator *
+MM4IntegratorConstructor::create_first_order_ode_integrator (
+   unsigned int size,
+   IntegrationControls & controls)
+const
+{
+   return integ_utils::allocate_integrator<RK2MidpointFirstOrderODEIntegrator> (
+             size, controls);
+}
+
+
+// Create an MM4 state integrator for a second order ODE.
+SecondOrderODEIntegrator *
+MM4IntegratorConstructor::create_second_order_ode_integrator (
+   unsigned int size,
+   IntegrationControls & controls)
+const
+{
+   return integ_utils::allocate_integrator<MM4SimpleSecondOrderODEIntegrator> (
+             size, controls);
+}
+
+
+// Create an MM4 state integrator for a second order ODE.
+SecondOrderODEIntegrator *
+MM4IntegratorConstructor::
+create_generalized_deriv_second_order_ode_integrator (
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionDerivativeFunctions & deriv_funs,
+   IntegrationControls & controls)
+const
+{
+   return integ_utils::allocate_integrator<
+                MM4GeneralizedDerivSecondOrderODEIntegrator> (
+             position_size, velocity_size, deriv_funs, controls);
+}
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */

trick_source/er7_utils/integration/mm4/src/mm4_second_order_ode_integrator.cc

@@ -0,0 +1,450 @@
+/**
+ * @if Er7UtilsUseGroups
+ * @addtogroup Er7Utils
+ * @{
+ * @addtogroup Integration
+ * @{
+ * @endif
+ */
+
+/**
+ * @file
+ * Defines member functions for the class MM4SecondOrderODEIntegrator.
+ */
+
+/*
+Purpose: ()
+*/
+
+// System includes
+
+// Interface includes
+#include "er7_utils/interface/include/alloc.hh"
+
+// Integration includes
+#include "er7_utils/integration/core/include/integ_utils.hh"
+
+// Model includes
+#include "../include/mm4_second_order_ode_integrator.hh"
+
+
+namespace er7_utils {
+
+/**
+ * Advance position and velocity for the 1st step of an MM4 integration cycle.
+ * @param[in]     accel     Acceleration vector
+ * @param[in]     hdt       Half of the integration interval time step
+ * @param[in]     size      State size
+ * @param[out]    pos_hist  Generalized position history
+ * @param[out]    vel_hist  Generalized velocity history
+ * @param[in,out] position  Generalized position
+ * @param[in,out] velocity  Generalized velocity
+ */
+static inline void ER7_UTILS_ALWAYS_INLINE
+second_order_mm4_step_one (
+   double const * ER7_UTILS_RESTRICT accel,
+   double hdt,
+   int size,
+   double * ER7_UTILS_RESTRICT * ER7_UTILS_RESTRICT pos_hist,
+   double * ER7_UTILS_RESTRICT * ER7_UTILS_RESTRICT vel_hist,
+   double * ER7_UTILS_RESTRICT position,
+   double * ER7_UTILS_RESTRICT velocity)
+{
+   double * ER7_UTILS_RESTRICT init_pos = pos_hist[0];
+   double * ER7_UTILS_RESTRICT init_vel = vel_hist[0];
+   double * ER7_UTILS_RESTRICT step_pos = pos_hist[1];
+   double * ER7_UTILS_RESTRICT step_vel = vel_hist[1];
+
+   for (int ii = 0; ii < size; ++ii) {
+      init_pos[ii] = position[ii];
+      init_vel[ii] = velocity[ii];
+      step_pos[ii] = (position[ii] += velocity[ii] * hdt);
+      step_vel[ii] = (velocity[ii] += accel[ii] * hdt);
+   }
+}
+
+
+/**
+ * Advance position and velocity for the 1st step of an MM4 integration cycle.
+ * @param[in]     posdot     Generalized position derivative
+ * @param[in]     veldot     Generalized velocity derivative
+ * @param[in]     hdt        Half of the integration interval time step
+ * @param[in]     size       Sizes of the position and velocity vectors
+ * @param[out]    pos_hist   Generalized position history
+ * @param[out]    vel_hist   Generalized velocity history
+ * @param[in,out] position   Generalized position
+ * @param[in,out] velocity   Generalized velocity
+ */
+static inline void ER7_UTILS_ALWAYS_INLINE
+second_order_mm4_step_one (
+   double const * ER7_UTILS_RESTRICT posdot,
+   double const * ER7_UTILS_RESTRICT veldot,
+   double hdt,
+   int size[2],
+   double * ER7_UTILS_RESTRICT * ER7_UTILS_RESTRICT pos_hist,
+   double * ER7_UTILS_RESTRICT * ER7_UTILS_RESTRICT vel_hist,
+   double * ER7_UTILS_RESTRICT position,
+   double * ER7_UTILS_RESTRICT velocity)
+{
+   double * ER7_UTILS_RESTRICT init_pos = pos_hist[0];
+   double * ER7_UTILS_RESTRICT init_vel = vel_hist[0];
+   double * ER7_UTILS_RESTRICT step_pos = pos_hist[1];
+   double * ER7_UTILS_RESTRICT step_vel = vel_hist[1];
+   int lim;
+
+   lim = size[0];
+   for (int ii = 0; ii < lim; ++ii) {
+      init_pos[ii] = position[ii];
+      step_pos[ii] = (position[ii] += posdot[ii] * hdt);
+   }
+
+   lim = size[1];
+   for (int ii = 0; ii < lim; ++ii) {
+      init_vel[ii] = velocity[ii];
+      step_vel[ii] = (velocity[ii] += veldot[ii] * hdt);
+   }
+}
+
+
+/**
+ * Advance position and velocity for the 2nd step of an MM4 integration cycle.
+ * @param[in]     accel     Acceleration vector
+ * @param[in]     dt        Integration interval time step
+ * @param[in]     size      State size
+ * @param[in,out] pos_hist  Generalized position history
+ * @param[in,out] vel_hist  Generalized velocity history
+ * @param[out]    position  Generalized position
+ * @param[out]    velocity  Generalized velocity
+ */
+static inline void ER7_UTILS_ALWAYS_INLINE
+second_order_mm4_step_two (
+   double const * ER7_UTILS_RESTRICT accel,
+   double dt,
+   int size,
+   double * ER7_UTILS_RESTRICT * ER7_UTILS_RESTRICT pos_hist,
+   double * ER7_UTILS_RESTRICT * ER7_UTILS_RESTRICT vel_hist,
+   double * ER7_UTILS_RESTRICT position,
+   double * ER7_UTILS_RESTRICT velocity)
+{
+   double * ER7_UTILS_RESTRICT init_pos = pos_hist[0];
+   double * ER7_UTILS_RESTRICT init_vel = vel_hist[0];
+   double * ER7_UTILS_RESTRICT step_pos = pos_hist[2];
+   double * ER7_UTILS_RESTRICT step_vel = vel_hist[2];
+
+   for (int ii = 0; ii < size; ++ii) {
+      step_pos[ii] = position[ii] = init_pos[ii] + velocity[ii] * dt;
+      step_vel[ii] = velocity[ii] = init_vel[ii] + accel[ii] * dt;
+   }
+}
+
+
+/**
+ * Advance position and velocity for the 2nd step of an MM4 integration cycle.
+ * @param[in]     posdot    Generalized position derivative
+ * @param[in]     veldot    Generalized velocity derivative
+ * @param[in]     dt        Integration interval time step
+ * @param[in]     size      State size
+ * @param[in,out] pos_hist  Generalized position history
+ * @param[in,out] vel_hist  Generalized velocity history
+ * @param[out]    position  Generalized position
+ * @param[out]    velocity  Generalized velocity
+ */
+static inline void ER7_UTILS_ALWAYS_INLINE
+second_order_mm4_step_two (
+   double const * ER7_UTILS_RESTRICT posdot,
+   double const * ER7_UTILS_RESTRICT veldot,
+   double dt,
+   int size[2],
+   double * ER7_UTILS_RESTRICT * ER7_UTILS_RESTRICT pos_hist,
+   double * ER7_UTILS_RESTRICT * ER7_UTILS_RESTRICT vel_hist,
+   double * ER7_UTILS_RESTRICT position,
+   double * ER7_UTILS_RESTRICT velocity)
+{
+   double * ER7_UTILS_RESTRICT init_pos = pos_hist[0];
+   double * ER7_UTILS_RESTRICT init_vel = vel_hist[0];
+   double * ER7_UTILS_RESTRICT step_pos = pos_hist[2];
+   double * ER7_UTILS_RESTRICT step_vel = vel_hist[2];
+   int lim;
+
+   lim = size[0];
+   for (int ii = 0; ii < lim; ++ii) {
+      step_pos[ii] = position[ii] = init_pos[ii] + posdot[ii] * dt;
+   }
+
+   lim = size[1];
+   for (int ii = 0; ii < lim; ++ii) {
+      step_vel[ii] = velocity[ii] = init_vel[ii] + veldot[ii] * dt;
+   }
+}
+
+
+/**
+ * Advance a state for the final step of an MM4 integration cycle.
+ * @param deriv        Derivatives at final step
+ * @param step1_state  State from end of first MM4 step
+ * @param step2_state  State from end of second MM4 step
+ * @param hdt          Half the integration interval time step
+ * @param size         State size
+ * @param state        Output state
+ */
+static inline void ER7_UTILS_ALWAYS_INLINE
+first_order_mm4_step_three (
+   double const * ER7_UTILS_RESTRICT deriv,
+   double const * ER7_UTILS_RESTRICT step1_state,
+   double const * ER7_UTILS_RESTRICT step2_state,
+   double hdt,
+   int size,
+   double * ER7_UTILS_RESTRICT state)
+{
+   for (int ii = 0; ii < size; ++ii) {
+      state[ii] = 0.5 * (step1_state[ii] + step2_state[ii] +
+                         hdt*deriv[ii]);
+   }
+}
+
+
+/**
+ * Advance a state for the final step of an MM4 integration cycle.
+ * @param accel      Acceleration at final step
+ * @param step1_pos  Position from end of first MM4 step
+ * @param step2_pos  Position from end of second MM4 step
+ * @param step1_vel  Velocity from end of first MM4 step
+ * @param step2_vel  Velocity from end of second MM4 step
+ * @param hdt        Half the integration interval time step
+ * @param size       State size
+ * @param position   Output position
+ * @param velocity   Output velocity
+ */
+static inline void ER7_UTILS_ALWAYS_INLINE
+second_order_mm4_step_three (
+   double const * ER7_UTILS_RESTRICT accel,
+   double const * ER7_UTILS_RESTRICT step1_pos,
+   double const * ER7_UTILS_RESTRICT step2_pos,
+   double const * ER7_UTILS_RESTRICT step1_vel,
+   double const * ER7_UTILS_RESTRICT step2_vel,
+   double hdt,
+   int size,
+   double * ER7_UTILS_RESTRICT position,
+   double * ER7_UTILS_RESTRICT velocity)
+{
+   for (int ii = 0; ii < size; ++ii) {
+      velocity[ii] = 0.5 * (step1_vel[ii] + step2_vel[ii] + hdt*accel[ii]);
+      position[ii] = 0.5 * (step1_pos[ii] + step2_pos[ii] + hdt*velocity[ii]);
+   }
+}
+
+
+// Default constructor for an MM4SecondOrderODEIntegrator.
+MM4SecondOrderODEIntegrator::MM4SecondOrderODEIntegrator (
+   void)
+:
+   Er7UtilsDeletable (),
+   SecondOrderODEIntegrator (),
+   posdot (NULL)
+{
+   // Initialize memory used by MM4 algorithm.
+   alloc::initialize_2D_array<3> (pos_hist);
+   alloc::initialize_2D_array<3> (vel_hist);
+}
+
+
+// Copy constructor.
+MM4SecondOrderODEIntegrator::MM4SecondOrderODEIntegrator (
+   const MM4SecondOrderODEIntegrator & src)
+:
+   Er7UtilsDeletable (),
+   SecondOrderODEIntegrator (src),
+   posdot (NULL)
+{
+   // Replicate the source's contents if they exist.
+   if (src.posdot != NULL) {
+      posdot = alloc::replicate_array (state_size[0], src.posdot);
+      alloc::replicate_2D_array<3> (state_size[0], src.pos_hist, pos_hist);
+      alloc::replicate_2D_array<3> (state_size[1], src.vel_hist, vel_hist);
+   }
+
+   else {
+      alloc::initialize_2D_array<3> (pos_hist);
+      alloc::initialize_2D_array<3> (vel_hist);
+   }
+}
+
+
+// Non-default constructor for an MM4SecondOrderODEIntegrator
+MM4SecondOrderODEIntegrator::MM4SecondOrderODEIntegrator (
+   unsigned int size,
+   IntegrationControls & controls)
+:
+   Er7UtilsDeletable (),
+   SecondOrderODEIntegrator (size, controls),
+   posdot (NULL)
+{
+   // Allocate memory used by MM4 algorithm.
+   alloc::allocate_2D_array<3> (size, pos_hist);
+   alloc::allocate_2D_array<3> (size, vel_hist);
+   posdot = alloc::allocate_array (size);
+}
+
+
+// Non-default constructor for an MM4SecondOrderODEIntegrator
+// for generalized position, generalized velocity.
+MM4SecondOrderODEIntegrator::MM4SecondOrderODEIntegrator (
+   unsigned int position_size,
+   unsigned int velocity_size,
+   const GeneralizedPositionDerivativeFunctions & deriv_funs,
+   IntegrationControls & controls)
+:
+   Er7UtilsDeletable (),
+   SecondOrderODEIntegrator (position_size, velocity_size,
+                             deriv_funs, controls),
+   posdot (NULL)
+{
+   // Allocate memory used by MM4 algorithm.
+   alloc::allocate_2D_array<3> (position_size, pos_hist);
+   alloc::allocate_2D_array<3> (velocity_size, vel_hist);
+   posdot = alloc::allocate_array (position_size);
+}
+
+
+// MM4SecondOrderODEIntegrator destructor.
+MM4SecondOrderODEIntegrator::~MM4SecondOrderODEIntegrator (
+   void)
+{
+   alloc::deallocate_2D_array<3> (pos_hist);
+   alloc::deallocate_2D_array<3> (vel_hist);
+   alloc::deallocate_array (posdot);
+}
+
+
+// Non-throwing swap.
+void
+MM4SecondOrderODEIntegrator::swap (
+   MM4SecondOrderODEIntegrator & other)
+{
+   SecondOrderODEIntegrator::swap (other);
+
+   std::swap (posdot, other.posdot);
+   for (int ii = 0; ii < 4; ++ii) {
+      std::swap (pos_hist[ii], other.pos_hist[ii]);
+      std::swap (vel_hist[ii], other.vel_hist[ii]);
+   }
+}
+
+
+// Clone a MM4SimpleSecondOrderODEIntegrator.
+MM4SimpleSecondOrderODEIntegrator *
+MM4SimpleSecondOrderODEIntegrator::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Clone a MM4GeneralizedDerivSecondOrderODEIntegrator.
+MM4GeneralizedDerivSecondOrderODEIntegrator *
+MM4GeneralizedDerivSecondOrderODEIntegrator::create_copy ()
+const
+{
+   return alloc::replicate_object (*this);
+}
+
+
+// Propagate state for the special case of velocity being the derivative of
+// position.
+IntegratorResult
+MM4SimpleSecondOrderODEIntegrator::integrate (
+   double dyn_dt,
+   unsigned int target_stage,
+   double const * ER7_UTILS_RESTRICT accel,
+   double * ER7_UTILS_RESTRICT velocity,
+   double * ER7_UTILS_RESTRICT position)
+{
+   double step_factor;
+
+   switch (target_stage) {
+   case 1:
+      second_order_mm4_step_one (
+         accel, 0.5*dyn_dt, state_size[0],
+         pos_hist, vel_hist, position, velocity);
+      step_factor = 0.5;
+      break;
+
+   case 2:
+      second_order_mm4_step_two (
+         accel, dyn_dt, state_size[0],
+         pos_hist, vel_hist, position, velocity);
+      step_factor = 1.0;
+      break;
+
+   case 3:
+      second_order_mm4_step_three (
+         accel, pos_hist[1], pos_hist[2], vel_hist[1], vel_hist[2],
+         0.5*dyn_dt, state_size[0],
+         position, velocity);
+      step_factor = 1.0;
+      break;
+
+   default:
+      step_factor = 1.0;
+      break;
+   }
+
+   return step_factor;
+}
+
+
+// Propagate state for the general case of the generalized position derivative
+// being a function of generalized position and generalized velocity.
+IntegratorResult
+MM4GeneralizedDerivSecondOrderODEIntegrator::integrate (
+   double dyn_dt,
+   unsigned int target_stage,
+   double const * ER7_UTILS_RESTRICT accel,
+   double * ER7_UTILS_RESTRICT velocity,
+   double * ER7_UTILS_RESTRICT position)
+{
+   double step_factor;
+
+   switch (target_stage) {
+   case 1:
+      compute_posdot (position, velocity, posdot);
+      second_order_mm4_step_one (
+         posdot, accel, 0.5*dyn_dt, state_size,
+         pos_hist, vel_hist, position, velocity);
+      step_factor = 0.5;
+      break;
+
+   case 2:
+      compute_posdot (position, velocity, posdot);
+      second_order_mm4_step_two (
+         posdot, accel, dyn_dt, state_size,
+         pos_hist, vel_hist, position, velocity);
+      step_factor = 1.0;
+      break;
+
+   case 3: {
+      double hdt = 0.5 * dyn_dt;
+      first_order_mm4_step_three (
+         accel, vel_hist[1], vel_hist[2], hdt, state_size[1], velocity);
+      compute_posdot (position, velocity, posdot);
+      first_order_mm4_step_three (
+         posdot, pos_hist[1], pos_hist[2], hdt, state_size[0], position);
+      step_factor = 1.0;
+      break;
+   }
+
+   default:
+      step_factor = 1.0;
+      break;
+   }
+
+   return step_factor;
+}
+
+}
+/**
+ * @if Er7UtilsUseGroups
+ * @}
+ * @}
+ * @endif
+ */