Simple example of the RATTLE algorithm

double2.h

#ifndef DOUBLE2_H
#define DOUBLE2_H

#include <cmath>

#include <ostream>
#include <initializer_list>

struct double2 {
  union {
    double array[2];
    struct { double x, y; };
  };

  double2() : x(0.0), y(0.0) {}

  double2(double x_, double y_) : x(x_), y(y_) {}

  double2(std::initializer_list<double> list) {
    std::copy(list.begin(), list.end(), array);
  }
  
  inline double& operator[](size_t i) {
    return array[i];
  }
  
  inline const double& operator[](size_t i) const {
    return array[i];
  }

  inline double2& operator+=(const double2& d) {
    x += d.x;
    y += d.y;
    return *this;
  }

  inline double2& operator-=(const double2& d) {
    x -= d.x;
    y -= d.y;
    return *this;
  }

  inline double2& operator*=(const double2& d) {
    x *= d.x;
    y *= d.y;
    return *this;
  }

  inline double2& operator/=(const double2& d) {
    x /= d.x;
    y /= d.y;
    return *this;
  }

  inline double2& operator*=(const double s) {
    x *= s;
    y *= s;
    return *this;
  }

  inline double2& operator/=(const double s) {
    x /= s;
    y /= s;
    return *this;
  }

  inline double norm2() const {
    return x*x + y*y;
  }

  inline double norm() const {
    return sqrt(norm2());
  }
  
  friend std::ostream& operator<<(std::ostream& stream, const double2& d) {
    return stream << d.x << " " << d.y;
  }
};

inline double2 operator+(const double2& a, const double2& b) {
  double2 c = a;
  return c += b;
}

inline double2 operator-(const double2& a, const double2& b) {
  double2 c = a;
  return c -= b;
}

inline double2 operator-(const double2& a) {
  double2 b = a;
  return b *= -1.0;
}

inline double2 operator*(const double2& a, const double2& b) {
  double2 c = a;
  return c *= b;
}

inline double2 operator*(const double s, const double2& v) {
  double2 w = v;
  return w *= s;
}

inline double2 operator*(const double2& v, const double s) {
  double2 w = v;
  return w *= s;
}

inline double2 operator/(const double2& a, const double2& b) {
  double2 c = a;
  return c /= b;
}

inline double2 operator/(const double s, const double2& v) {
  double2 w = v;
  return w /= s;
}

inline double2 operator/(const double2& v, const double s) {
  double2 w = v;
  return w /= s;
}

inline double dot(const double2& u, const double2& v) {
  const double2 d = u * v;
  return d.x + d.y;
}

inline double dist(const double2& u, const double2& v) {
  const double2 r = u - v;
  return r.norm();
}

#endif

rattle.cpp

// 
// Compile with: g++ -Wall -std=c++11 -o rattle rattle.cpp
// 

#include <cstdlib>
#include <cmath>
#include <cassert>

#include <iostream>
#include <iomanip>

#include "double2.h"

using namespace std;

const double K = 0.5;
const double tol = 1e-15;
const size_t max_iters = 1e6;

double dt;

// Constraint.
double g(const double2& r) {
  return K * r.x * r.x + r.y * r.y - 1.0;
}

// Gradient of constraint.
double2 G(const double2& r) {
  return double2(2.0 * K * r.x, 2.0 * r.y);
}

void rattle(double2& q, double2& p, double& lambda, double h) {
  // Declare auxiliary constants.
  const double2 Gqprev = G(q);
    
  // Deal with constraint on the configuration manifold.
  q += h * p;
  double lambda_r = 0.0;
  // Solve using Newton's method.
  for (size_t k = 1; k <= max_iters; k++) {
    if (k == max_iters)
      abort();

    const double2 r = q - lambda_r * Gqprev;
    const double phi = g(r);
    const double dphi_dl = -dot(G(r), Gqprev);
    const double update = phi / dphi_dl;
      
    if (fabs(phi) < tol && fabs(update) < tol)
      break;
      
    lambda_r -= update;
  }

  q -= lambda_r * Gqprev;
  p -= lambda_r / h * Gqprev;

  // Deal with constraint on the tangent space.
  const double2 Gq = G(q);
  double lambda_v = dot(Gq, p) / dot(Gq, Gq);
  p -= lambda_v * Gq;

  lambda = (lambda_r + lambda_v) / 2.0;
  
  assert(fabs(g(q)) < tol && fabs(dot(G(q), p)) < tol);
}

int main(int argc, char* argv[]) {
  if (argc < 3) {
    cerr << "Usage: " << argv[0] << " delta-t max-steps" << endl;
    return EXIT_FAILURE;
  }

  dt = strtod(argv[1], nullptr);
  const unsigned max_steps = unsigned(atof(argv[2]));
  
  double2 q = { 0.0, 1.0 };
  double2 p = { -q.y, q.x };
  double lambda = 0.0;

  assert(g(q) < tol && fabs(dot(G(q), p)) < tol);
  
  std::cout << std::setprecision(8);
  
  for (size_t k = 0; k < max_steps; k++) {
    rattle(q, p, lambda, dt);
    
    const double time = k * dt;
    const double total_energy = dot(p, p) / 2.0 + lambda * g(q);
    std::cout << time << " " << q << " " << p << " " << total_energy << std::endl;
  }
  
  return EXIT_SUCCESS;
}