Time.hpp

Go to the documentation of this file.

/*
 * Copyright (c) 2022, Jose Luis Cercos-Pita <jlc@core-marine.com>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
 
#pragma once
 
#include "Misc.hpp"
#include "IO.hpp"
#include "State.hpp"
#include "Line.hpp"
#include "Point.hpp"
#include "Rod.hpp"
#include "Body.hpp"
#include "Waves.hpp"
#include <vector>
#include <string>
 
namespace moordyn {
 
namespace time {
 
class Scheme : public io::IO
{
  public:
    virtual ~Scheme() {}
 
    inline void SetGround(Body* obj) { ground = obj; }
 
    virtual void AddLine(Line* obj)
    {
        if (std::find(lines.begin(), lines.end(), obj) != lines.end()) {
            LOGERR << "The line " << obj->number << " was already registered"
                   << endl;
            throw moordyn::invalid_value_error("Repeated object");
        }
        lines.push_back(obj);
    }
 
    virtual unsigned int RemoveLine(Line* obj)
    {
        auto it = std::find(lines.begin(), lines.end(), obj);
        if (it == lines.end()) {
            LOGERR << "The line " << obj->number << " was not registered"
                   << endl;
            throw moordyn::invalid_value_error("Missing object");
        }
        const unsigned int i = std::distance(lines.begin(), it);
        lines.erase(it);
        return i;
    }
 
    virtual void AddPoint(Point* obj)
    {
        if (std::find(points.begin(), points.end(), obj) != points.end()) {
            LOGERR << "The point " << obj->number << " was already registered"
                   << endl;
            throw moordyn::invalid_value_error("Repeated object");
        }
        points.push_back(obj);
    }
 
    virtual unsigned int RemovePoint(Point* obj)
    {
        auto it = std::find(points.begin(), points.end(), obj);
        if (it == points.end()) {
            LOGERR << "The point " << obj->number << " was not registered"
                   << endl;
            throw moordyn::invalid_value_error("Missing object");
        }
        const unsigned int i = std::distance(points.begin(), it);
        points.erase(it);
        return i;
    }
 
    virtual void AddRod(Rod* obj)
    {
        if (std::find(rods.begin(), rods.end(), obj) != rods.end()) {
            LOGERR << "The rod " << obj->number << " was already registered"
                   << endl;
            throw moordyn::invalid_value_error("Repeated object");
        }
        rods.push_back(obj);
    }
 
    virtual unsigned int RemoveRod(Rod* obj)
    {
        auto it = std::find(rods.begin(), rods.end(), obj);
        if (it == rods.end()) {
            LOGERR << "The rod " << obj->number << " was not registered"
                   << endl;
            throw moordyn::invalid_value_error("Missing object");
        }
        const unsigned int i = std::distance(rods.begin(), it);
        rods.erase(it);
        return i;
    }
 
    virtual void AddBody(Body* obj)
    {
        if (std::find(bodies.begin(), bodies.end(), obj) != bodies.end()) {
            LOGERR << "The body " << obj->number << " was already registered"
                   << endl;
            throw moordyn::invalid_value_error("Repeated object");
        }
        bodies.push_back(obj);
    }
 
    virtual unsigned int RemoveBody(Body* obj)
    {
        auto it = std::find(bodies.begin(), bodies.end(), obj);
        if (it == bodies.end()) {
            LOGERR << "The body " << obj->number << " was not registered"
                   << endl;
            throw moordyn::invalid_value_error("Missing object");
        }
        const unsigned int i = std::distance(bodies.begin(), it);
        bodies.erase(it);
        return i;
    }
 
    inline std::string GetName() const { return name; }
 
    inline real GetTime() const { return t; }
 
    inline void SetTime(const real& time)
    {
        t = time;
        Next();
    }
 
    inline real GetCFL() const { return cfl; }
 
    inline void SetCFL(const real& cfl) { this->cfl = cfl; }
 
    inline void Next()
    {
        t_local = 0.0;
        for (unsigned int i = 0; i < lines.size(); i++) {
            lines[i]->updateUnstretchedLength();
        }
    }
 
    virtual void Init() = 0;
 
    virtual void Step(real& dt) { t_local += dt; };
 
    virtual moordyn::state::State GetState(unsigned int i = 0) = 0;
 
    inline virtual void SetState(const moordyn::state::State& state,
                                 unsigned int i = 0) {};
 
    inline virtual void SaveState(const std::string filepath,
                                  unsigned int i = 0)
    {
    }
 
    inline virtual void LoadState(const std::string filepath,
                                  unsigned int i = 0)
    {
    }
 
  protected:
    Scheme(moordyn::Log* log)
      : io::IO(log)
      , name("None")
      , t(0.0)
    {
    }
 
    Body* ground;
 
    std::vector<Line*> lines;
 
    std::vector<Point*> points;
 
    std::vector<Rod*> rods;
 
    std::vector<Body*> bodies;
 
    std::string name;
 
    real t;
    real t_local;
 
    real cfl;
};
 
#define AS_STATE(R) ((moordyn::state::State*)R)
 
template<unsigned int NSTATE, unsigned int NDERIV>
class SchemeBase : public Scheme
{
  public:
    virtual ~SchemeBase()
    {
        for (unsigned int substep = 0; substep < NSTATE; substep++) {
            delete _r[substep];
        }
        for (unsigned int substep = 0; substep < NDERIV; substep++) {
            delete _rd[substep];
        }
    }
 
    virtual void AddLine(Line* obj)
    {
        try {
            Scheme::AddLine(obj);
        } catch (...) {
            throw;
        }
        for (unsigned int i = 0; i < NSTATE; i++)
            AS_STATE(_r[i])->addInstance(obj);
        for (unsigned int i = 0; i < NDERIV; i++)
            AS_STATE(_rd[i])->addInstance(obj);
        // Add the mask value
        _calc_mask.lines.push_back(true);
    }
 
    // virtual void AddLine(Line* obj)
    // {
    //  try {
    //      TimeScheme::AddLine(obj);
    //  } catch (...) {
    //      throw;
    //  }
    //  // Build up the states and states derivatives
    //  unsigned int n = obj->getN() - 1;
    //  LineState state;
    //  LineState mstate; // misc state stuff
    //  state.pos.assign(n, vec::Zero());
    //  state.vel.assign(n, vec::Zero());
    //  mstate.pos.assign(n+2, vec::Zero());
    //  mstate.vel.assign(n+2, vec::Zero());
    //  for (unsigned int i = 0; i < r.size(); i++) {
    //      r[i].lines.push_back(state);
    //      r[i].misc.push_back(mstate);
    //  }
    //  DLineStateDt dstate;
    //  DLineStateDt mdstate; // misc state stuff
    //  dstate.vel.assign(n, vec::Zero());
    //  dstate.acc.assign(n, vec::Zero());
    //  mdstate.vel.assign(n+2, vec::Zero());
    //  mdstate.acc.assign(n+2, vec::Zero());
    //  for (unsigned int i = 0; i < rd.size(); i++) {
    //      rd[i].lines.push_back(dstate);
    //      rd[i].misc.push_back(mdstate);
    //  }
    //  // Add the mask value
    //  _calc_mask.lines.push_back(true);
    // }
 
    virtual unsigned int RemoveLine(Line* obj)
    {
        unsigned int i;
        try {
            i = Scheme::RemoveLine(obj);
        } catch (...) {
            throw;
        }
        for (unsigned int i = 0; i < NSTATE; i++)
            AS_STATE(_r[i])->removeInstance(obj);
        for (unsigned int i = 0; i < NDERIV; i++)
            AS_STATE(_rd[i])->removeInstance(obj);
        _calc_mask.lines.erase(_calc_mask.lines.begin() + i);
        return i;
    }
 
    // virtual unsigned int RemoveLine(Line* obj)
    // {
    //  unsigned int i;
    //  try {
    //      i = TimeScheme::RemoveLine(obj);
    //  } catch (...) {
    //      throw;
    //  }
    //  for (unsigned int i = 0; i < r.size(); i++) {
    //      r[i].lines.erase(r[i].lines.begin() + i);
    //      r[i].misc.erase(r[i].misc.begin() + i);
    //  }
    //  for (unsigned int i = 0; i < rd.size(); i++) {
    //      rd[i].lines.erase(rd[i].lines.begin() + i);
    //      rd[i].misc.erase(rd[i].misc.begin() + i);
    //  }
    //  _calc_mask.lines.erase(_calc_mask.lines.begin() + i);
    //  return i;
    // }
 
    virtual void AddPoint(Point* obj)
    {
        try {
            Scheme::AddPoint(obj);
        } catch (...) {
            throw;
        }
        for (unsigned int i = 0; i < NSTATE; i++)
            AS_STATE(_r[i])->addInstance(obj);
        for (unsigned int i = 0; i < NDERIV; i++)
            AS_STATE(_rd[i])->addInstance(obj);
        // Add the mask value
        _calc_mask.points.push_back(true);
    }
 
    virtual unsigned int RemovePoint(Point* obj)
    {
        unsigned int i;
        try {
            i = Scheme::RemovePoint(obj);
        } catch (...) {
            throw;
        }
        for (unsigned int i = 0; i < NSTATE; i++)
            AS_STATE(_r[i])->removeInstance(obj);
        for (unsigned int i = 0; i < NDERIV; i++)
            AS_STATE(_rd[i])->removeInstance(obj);
        _calc_mask.points.erase(_calc_mask.points.begin() + i);
        return i;
    }
 
    virtual void AddRod(Rod* obj)
    {
        try {
            Scheme::AddRod(obj);
        } catch (...) {
            throw;
        }
        for (unsigned int i = 0; i < NSTATE; i++)
            AS_STATE(_r[i])->addInstance(obj);
        for (unsigned int i = 0; i < NDERIV; i++)
            AS_STATE(_rd[i])->addInstance(obj);
        // Add the mask value
        _calc_mask.rods.push_back(true);
    }
 
    virtual unsigned int RemoveRod(Rod* obj)
    {
        unsigned int i;
        try {
            i = Scheme::RemoveRod(obj);
        } catch (...) {
            throw;
        }
        for (unsigned int i = 0; i < NSTATE; i++)
            AS_STATE(_r[i])->removeInstance(obj);
        for (unsigned int i = 0; i < NDERIV; i++)
            AS_STATE(_rd[i])->removeInstance(obj);
        _calc_mask.rods.erase(_calc_mask.rods.begin() + i);
        return i;
    }
 
    virtual void AddBody(Body* obj)
    {
        try {
            Scheme::AddBody(obj);
        } catch (...) {
            throw;
        }
        for (unsigned int i = 0; i < NSTATE; i++)
            AS_STATE(_r[i])->addInstance(obj);
        for (unsigned int i = 0; i < NDERIV; i++)
            AS_STATE(_rd[i])->addInstance(obj);
        // Add the mask value
        _calc_mask.bodies.push_back(true);
    }
 
    virtual unsigned int RemoveBody(Body* obj)
    {
        unsigned int i;
        try {
            i = Scheme::RemoveBody(obj);
        } catch (...) {
            throw;
        }
        for (unsigned int i = 0; i < NSTATE; i++)
            AS_STATE(_r[i])->removeInstance(obj);
        for (unsigned int i = 0; i < NDERIV; i++)
            AS_STATE(_rd[i])->removeInstance(obj);
        _calc_mask.bodies.erase(_calc_mask.bodies.begin() + i);
        return i;
    }
 
    virtual void Init()
    {
        // NOTE: Probably is best to populate all the entities to the time
        // integrator, no matter if they are free or not. Thus they can change
        // types (mutate) without needing to micromanage them in the time
        // scheme
        for (unsigned int i = 0; i < bodies.size(); i++) {
            // Only fully coupled bodies are intialized in MD2.cpp
            if ((bodies[i]->type != Body::FREE) &&
                (bodies[i]->type != Body::CPLDPIN))
                continue;
            bodies[i]->initialize(AS_STATE(_r[0])->get(bodies[i]));
            for (unsigned int j = 0; j < NDERIV; j++)
                AS_STATE(_rd[j])->get(bodies[i]).setZero();
        }
 
        for (unsigned int i = 0; i < rods.size(); i++) {
            if ((rods[i]->type != Rod::FREE) && (rods[i]->type != Rod::PINNED))
                continue;
            rods[i]->initialize(AS_STATE(_r[0])->get(rods[i]));
            for (unsigned int j = 0; j < NDERIV; j++)
                AS_STATE(_rd[j])->get(rods[i]).setZero();
        }
 
        for (unsigned int i = 0; i < points.size(); i++) {
            if (points[i]->type != Point::FREE)
                continue;
            points[i]->initialize(AS_STATE(_r[0])->get(points[i]));
            for (unsigned int j = 0; j < NDERIV; j++)
                AS_STATE(_rd[j])->get(points[i]).setZero();
        }
 
        for (unsigned int i = 0; i < lines.size(); i++) {
            lines[i]->initialize(AS_STATE(_r[0])->get(lines[i]));
            for (unsigned int j = 0; j < NDERIV; j++)
                AS_STATE(_rd[j])->get(lines[i]).setZero();
        }
    }
 
    virtual void Step(real& dt) { Scheme::Step(dt); };
 
    inline state::State* r(unsigned int i = 0)
    {
        if (i >= NSTATE) {
            LOGERR << "State " << i << " cannot be got on a '" << name
                   << "' scheme that has " << NSTATE << "states" << endl;
            throw moordyn::invalid_value_error("Invalid state");
        }
        return _r[i];
    }
 
    inline state::State GetState(unsigned int i = 0) { return *r(i); }
    inline virtual void SetState(const state::State& state, unsigned int i = 0)
    {
        if (i >= NSTATE) {
            LOGERR << "State " << i << " cannot be setted on a '" << name
                   << "' scheme that has " << NSTATE << " states" << endl;
            throw moordyn::invalid_value_error("Invalid state");
        }
        *_r[i] = state;
    }
 
    inline virtual void SaveState(const std::string filepath,
                                  unsigned int i = 0)
    {
        auto f = MakeFile(filepath);
        if (i >= NSTATE) {
            LOGERR << "State " << i << " cannot be saved on a '" << name
                   << "' scheme that has " << NSTATE << "states" << endl;
            throw moordyn::invalid_value_error("Invalid state");
        }
        std::vector<uint64_t> data = AS_STATE(_r[i])->Serialize();
        const uint64_t size = data.size();
        f.write((char*)&size, sizeof(uint64_t));
        for (auto v : data) {
            f.write((char*)&v, sizeof(uint64_t));
        }
        f.close();
    }
 
    inline virtual void LoadState(const std::string filepath,
                                  unsigned int i = 0)
    {
        auto [length, data] = LoadFile(filepath);
        if (i >= NSTATE) {
            LOGERR << "State " << i << " cannot be loaded on a '" << name
                   << "' scheme that has " << NSTATE << "states" << endl;
            throw moordyn::invalid_value_error("Invalid state");
        }
        const uint64_t* end = AS_STATE(_r[i])->Deserialize(data);
        if (data + length != end) {
            const uint64_t l = end - data;
            LOGERR << l * sizeof(uint64_t) << " bytes (vs. "
                   << length * sizeof(uint64_t)
                   << " bytes expected) unpacked from '" << filepath << "'"
                   << endl;
            throw moordyn::mem_error("Mismatching data size");
        }
 
        free(data);
    }
 
    inline state::State* rd(unsigned int i = 0)
    {
        if (i >= NDERIV) {
            LOGERR << "State derivative " << i << " cannot be got on a '"
                   << name << "' scheme that has " << NDERIV
                   << "state derivatives" << endl;
            throw moordyn::invalid_value_error("Invalid state derivative");
        }
        return _rd[i];
    }
 
    virtual std::vector<uint64_t> Serialize(void)
    {
        std::vector<uint64_t> data, subdata;
 
        data.push_back(io::IO::Serialize(t));
 
        // We do not need to save the number of states or derivatives, since
        // that information is already known by each specific time scheme.
        // Along the same line, we do not need to same information about the
        // number of lines, rods and so on. That information is already
        // collected from the definition file
        for (unsigned int substep = 0; substep < NSTATE; substep++) {
            subdata = _r[substep]->Serialize();
            data.insert(data.end(), subdata.begin(), subdata.end());
        }
        for (unsigned int substep = 0; substep < NDERIV; substep++) {
            subdata = _rd[substep]->Serialize();
            data.insert(data.end(), subdata.begin(), subdata.end());
        }
 
        return data;
    }
 
    virtual uint64_t* Deserialize(const uint64_t* data)
    {
        uint64_t* ptr = (uint64_t*)data;
        ptr = io::IO::Deserialize(ptr, t);
 
        // We did not save the number of states or derivatives, since that
        // information is already known by each specific time scheme.
        // Along the same line, we did not save information about the number of
        // lines, rods and so on
        for (unsigned int substep = 0; substep < NSTATE; substep++) {
            ptr = _r[substep]->Deserialize(ptr);
        }
        for (unsigned int substep = 0; substep < NDERIV; substep++) {
            ptr = _rd[substep]->Deserialize(ptr);
        }
 
        return ptr;
    }
 
  protected:
    SchemeBase(moordyn::Log* log, moordyn::WavesRef waves)
      : Scheme(log)
      , waves(waves)
    {
        // Register some basic variables that we know are always present
        // The position, and its associated derivative, have several different
        // types depending on the instance (e.g. moordyn::vec for lines and
        // points, moordyn::XYZQuat for rods and bodies), so better using lists
        // that we can resize on demand
        for (unsigned int substep = 0; substep < NSTATE; substep++) {
            _r[substep] = new moordyn::state::State(log);
        }
        for (unsigned int substep = 0; substep < NDERIV; substep++) {
            _rd[substep] = new moordyn::state::State(log);
        }
    }
 
    void Update(real t_local, unsigned int substep = 0);
 
    void CalcStateDeriv(unsigned int substep = 0);
 
    std::array<moordyn::state::State*, NSTATE> _r;
 
    std::array<moordyn::state::State*, NDERIV> _rd;
 
    std::shared_ptr<Waves> waves;
 
    typedef struct _mask
    {
        std::vector<bool> lines;
        std::vector<bool> points;
        std::vector<bool> rods;
        std::vector<bool> bodies;
    } mask;
 
    mask _calc_mask;
};
 
class StationaryScheme final : public SchemeBase<2, 1>
{
  public:
    StationaryScheme(moordyn::Log* log, WavesRef waves);
 
    ~StationaryScheme() {}
 
    void Step(real& dt);
 
    inline real Error() const { return _error; }
 
    inline size_t NStates() const
    {
        size_t n =
            bodies.size() + rods.size() +
            points.size(); // one row (state) per object here. <objects>.size()
                           // gives the length of the list, i.e. the number of
                           // that kind of object
        for (size_t i = 0; i < lines.size(); i++)
            n += lines[i]->stateN();
        return n;
    }
 
  private:
    void MakeStationary(real& dt, unsigned int i = 0, unsigned int id = 0);
 
    real _error;
 
    real _booster;
};
 
class EulerScheme final : public SchemeBase<1, 1>
{
  public:
    EulerScheme(moordyn::Log* log, WavesRef waves);
 
    virtual ~EulerScheme() {}
 
    virtual void Step(real& dt);
};
 
template<unsigned int NSTATE, unsigned int NDERIV>
class LocalSchemeBase : public SchemeBase<NSTATE, NDERIV>
{
  public:
    virtual ~LocalSchemeBase() {}
 
    inline void Init()
    {
        SchemeBase<NSTATE, NDERIV>::Init();
        ComputeDt();
    }
 
    inline virtual void SetState(const state::State& state, unsigned int i = 0)
    {
        SchemeBase<NSTATE, NDERIV>::SetState(state, i);
        ComputeDt();
    }
 
  protected:
    LocalSchemeBase(moordyn::Log* log, moordyn::WavesRef waves)
      : SchemeBase<NSTATE, NDERIV>(log, waves)
    {
    }
 
    void SetCalcMask(real& dt);
 
  private:
    real ComputeDt();
 
    typedef struct _sdeltat
    {
        std::vector<real> lines;
        std::vector<real> points;
        std::vector<real> rods;
        std::vector<real> bodies;
    } deltat;
 
    deltat _dt0;
 
    deltat _dt;
};
 
class LocalEulerScheme final : public LocalSchemeBase<1, 1>
{
  public:
    LocalEulerScheme(moordyn::Log* log, WavesRef waves);
 
    ~LocalEulerScheme() {}
 
    void Step(real& dt);
};
 
class HeunScheme final : public SchemeBase<1, 2>
{
  public:
    HeunScheme(moordyn::Log* log, WavesRef waves);
 
    ~HeunScheme() {}
 
    virtual void Step(real& dt);
};
 
class RK2Scheme final : public SchemeBase<2, 2>
{
  public:
    RK2Scheme(moordyn::Log* log, WavesRef waves);
 
    ~RK2Scheme() {}
 
    virtual void Step(real& dt);
};
 
class RK4Scheme final : public SchemeBase<5, 4>
{
  public:
    RK4Scheme(moordyn::Log* log, WavesRef waves);
 
    ~RK4Scheme() {}
 
    virtual void Step(real& dt);
};
 
template<unsigned int order, bool local>
class ABScheme final : public LocalSchemeBase<1, 5>
{
  public:
    ABScheme(moordyn::Log* log, WavesRef waves);
 
    ~ABScheme() {}
 
    virtual void Step(real& dt);
 
    virtual std::vector<uint64_t> Serialize(void)
    {
        std::vector<uint64_t> data = SchemeBase::Serialize();
        // We append the number of available steps
        data.push_back(io::IO::Serialize((uint64_t)n_steps));
 
        return data;
    }
 
    virtual uint64_t* Deserialize(const uint64_t* data)
    {
        uint64_t* ptr = SchemeBase::Deserialize(data);
        uint64_t n;
        ptr = io::IO::Deserialize(ptr, n);
        n_steps = n;
 
        return ptr;
    }
 
  private:
    unsigned int n_steps;
 
    inline void shift(unsigned int org)
    {
        const unsigned int dst = org + 1;
        for (unsigned int i = 0; i < lines.size(); i++) {
            if (!_calc_mask.lines[i])
                continue;
            AS_STATE(rd(dst))->get(lines[i]) = AS_STATE(rd(org))->get(lines[i]);
        }
 
        for (unsigned int i = 0; i < points.size(); i++) {
            if (!_calc_mask.points[i] && (points[i]->type == Point::FREE))
                continue;
            AS_STATE(rd(dst))->get(points[i]) =
                AS_STATE(rd(org))->get(points[i]);
        }
 
        for (unsigned int i = 0; i < rods.size(); i++) {
            if (!_calc_mask.rods[i] && ((rods[i]->type != Rod::FREE) ||
                                        (rods[i]->type != Rod::PINNED)))
                continue;
            AS_STATE(rd(dst))->get(rods[i]) = AS_STATE(rd(org))->get(rods[i]);
        }
 
        for (unsigned int i = 0; i < bodies.size(); i++) {
            if (!_calc_mask.bodies[i] && (bodies[i]->type == Body::FREE))
                continue;
            AS_STATE(rd(dst))->get(bodies[i]) =
                AS_STATE(rd(org))->get(bodies[i]);
        }
    }
 
    inline void shift()
    {
        for (unsigned int i = 0; i < _rd.size() - 1; i++)
            shift(i);
    }
};
 
template<unsigned int NSTATE, unsigned int NDERIV>
class ImplicitSchemeBase : public SchemeBase<NSTATE, NDERIV>
{
  public:
    ImplicitSchemeBase(moordyn::Log* log,
                       WavesRef waves,
                       unsigned int iters = 10);
 
    virtual ~ImplicitSchemeBase() {}
 
  protected:
    inline unsigned int iters() const { return _iters; }
 
    inline real c0() const { return _c0; }
 
    inline void c0(const real c) { _c0 = c; }
 
    inline real c1() const { return _c1; }
 
    inline void c1(const real c) { _c1 = c; }
 
    real Relax(const unsigned int& iter);
 
  private:
    unsigned int _iters;
 
    real _c0;
 
    real _c1;
};
 
class ImplicitEulerScheme final : public ImplicitSchemeBase<2, 2>
{
  public:
    ImplicitEulerScheme(moordyn::Log* log,
                        WavesRef waves,
                        unsigned int iters = 10,
                        real dt_factor = 0.5);
 
    virtual ~ImplicitEulerScheme() {}
 
    virtual void Step(real& dt);
 
  private:
    real _dt_factor;
};
 
class ImplicitNewmarkScheme final : public ImplicitSchemeBase<2, 3>
{
  public:
    ImplicitNewmarkScheme(moordyn::Log* log,
                          WavesRef waves,
                          unsigned int iters = 10,
                          real gamma = 0.5,
                          real beta = 0.25);
 
    ~ImplicitNewmarkScheme() {}
 
    virtual void Step(real& dt);
 
  private:
    void MakeNewmark(const real& dt);
 
    real _gamma;
    real _beta;
};
 
class ImplicitWilsonScheme final : public ImplicitSchemeBase<2, 3>
{
  public:
    ImplicitWilsonScheme(moordyn::Log* log,
                         WavesRef waves,
                         unsigned int iters = 10,
                         real theta = 1.37);
 
    ~ImplicitWilsonScheme() {}
 
    virtual void Step(real& dt);
 
  private:
    void MakeWilson(const real& tau, const real& dt);
 
    real _theta;
};
 
Scheme*
create_time_scheme(const std::string& name, moordyn::Log* log, WavesRef waves);
 
} // ::time
 
} // ::moordyn