hoopdehoop

#include <armadillo>
#include <iostream>
#include <vector>
#include <cmath>

constexpr double g       = 9.81;
constexpr double M       = 1.;
constexpr double m       = 1.*M;
constexpr double R       = 1.;
constexpr double dt      = 0.000001;

class Entity {
public:
  double m{0.0};
  arma::vec x{0., 0.};
  arma::vec v{0., 0.};
  arma::vec a{0., 0.};
};

class System {
public:
       System();
  void takeSteps(unsigned int N) noexcept;
  void printState() const noexcept;
private:
  void realStep() noexcept;
  void trialStep() const noexcept;
  void getConstraints() noexcept;
  arma::vec evalConstraints() const noexcept;
private:
  std::vector<Entity> currentState;
  mutable std::vector<Entity> trialState;
  arma::vec constraints{0., 0.};
};

System::System() {
  currentState.resize(3);
  auto& ball1 = currentState[0];
  auto& ball2 = currentState[1];
  auto& loop  = currentState[2];

  ball1.x[1] =  R;
  ball2.x[1] =  R;
  ball1.v[0] = -0.01*R;
  ball2.v[0] =  0.01*R;
  ball1.m    =  m;
  ball2.m    =  m;
  loop.m     =  M;

  trialState = currentState;
}

void System::takeSteps(unsigned int N) noexcept {
  for (int i = 0; i < N; ++i) {
    getConstraints();
    realStep();
  }
}

void System::printState() const noexcept {
  auto& ball1 = currentState[0];
  auto& ball2 = currentState[1];
  auto& loop  = currentState[2];
  std::cout << ball1.x[0] << " " << ball1.x[1] << " "
            << ball2.x[0] << " " << ball2.x[1] << " "
            << loop.x[1]  << " " << loop.a[1]  << std::endl;
}

void System::realStep() noexcept {
  trialStep();
  using std::swap;
  swap(currentState, trialState);
}

void System::trialStep() const noexcept {
  auto& ball1T = trialState[0];
  auto& ball2T = trialState[1];
  auto& loopT  = trialState[2];

  auto& ball1 = currentState[0];
  auto& ball2 = currentState[1];
  auto& loop  = currentState[2];

  auto lbd1   = constraints[0];
  auto lbd2   = constraints[1];

  ball1T.a[0] =  (2.*lbd1 *  ball1.x[0])/ball1.m;
  ball1T.a[1] =  (2.*lbd1 * (ball1.x[1]-loop.x[1]) - ball1.m*g)/ball1.m;
  ball2T.a[0] =  (2.*lbd2 *  ball2.x[0])/ball2.m;
  ball2T.a[1] =  (2.*lbd2 * (ball2.x[1]-loop.x[1]) - ball2.m*g)/ball2.m;
  loopT.a[1]  = (-2.*lbd1 * (ball1.x[1]-loop.x[1])
                 -2.*lbd2 * (ball2.x[1]-loop.x[1]) - loop.m*g)/loop.m;

  auto itTrial = trialState.begin();
  for (auto it = currentState.cbegin(); it != currentState.cend(); ++it) {
    (*itTrial).v = (*it).v + .5*dt*((*it).a + (*itTrial).a);
    (*itTrial).x = (*it).x + dt*(*itTrial).v + .5*dt*dt*(*itTrial).a;
    ++itTrial;
  }

  if (loopT.x[1] < 0.) {
    loopT.x[1] = 0.;
    if (loopT.v[1] < 0.)
      loopT.v[1] *= -1;
  }
}

void System::getConstraints() noexcept {
  constexpr double eps = 0.01;

  for (int i = 0; i < 1000; ++i) {
    auto cc = constraints;
    auto kk = -evalConstraints();
    arma::mat J = arma::zeros<arma::mat>(constraints.size(), constraints.size());

    for (int j = 0; j < constraints.size(); ++j) {
      constraints[j] = cc[j] + eps;
      auto jcol = evalConstraints();
      constraints[j] = cc[j] - eps;
      jcol -= evalConstraints();
      constraints[j] = cc[j];
      J.col(j) = jcol/(2.*eps);
    }

    J = inv(J);
    arma::vec newcc = J*kk;

    if (arma::norm(newcc) < 1e-4*arma::norm(cc))
      break;

    cc += newcc;
    constraints = cc;
  }
}

arma::vec System::evalConstraints() const noexcept {
  trialStep();
  arma::vec val{0., 0.};
  for (int i = 0; i < 2; ++i)
    val[i] = arma::norm(trialState[i].x - trialState[2].x) - R;
  return val;
}

int main() {
  System kk;
  for (int i = 0; i < 6000; ++i) {
    kk.takeSteps(1000);
    kk.printState();
  }
}