以下代码片段是 RATTLE 在具有约束的系统上的实现$g(x, y) = K x^2 + y^2 - 1 = 0$.

// 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);
}

我使用Simulating Hamiltonian Dynamics一书中第 7 章的符号。

为了您的方便，我上传了一个独立的程序，您可以在https://gist.github.com/jmbr/9857614#file-rattle-cpp上编译、运行和使用 该程序产生的输出可用于绘图下图（示例轨迹及其对应的总能量随时间变化的图）。