Skip to content

Instantly share code, notes, and snippets.

@jmbr

jmbr/double2.h

Created Mar 29, 2014
Embed
What would you like to do?
Simple example of the RATTLE algorithm
#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
//
// 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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.