Rough cut MCPI ODE IVP Solver

mcpi.m

% implementation from:
% - https://oaktrust.library.tamu.edu/bitstream/handle/1969.1/ETD-TAMU-2010-08-8240/BAI-DISSERTATION.pdf
% - https://link.springer.com/article/10.1007/BF03321534
function [xnew, iter] = MCPI(ode, xold, tmin, tmax, N, tol, varargin)
  x0 = zeros(size(xold));
  x0(1,:) = xold(1,:) * 2

  tau = cos((0:N).*pi./N);

  k1 = ( tmax + tmin ) / 2;
  k2 = ( tmax - tmin ) / 2;

  t = k2 * tau + k1;

  % V
  V = repmat(2/N,[N+1 1]);
  V(1) = V(1)/2;
  V(end) = V(end)/2;
  V = diag(V)

  % T
  T = chebyshevT(repmat((0:N),N+1,1),repmat(tau', 1, N+1))

  % Cx
  Cx = T;
  Cx(:,1) = Cx(:,1)*0.5

  % R
  R(1) = 1;
  for f=1:N; R(f+1) = 1/(2*f); end
  R = diag(R)

  % S
  S = diag(ones(1,N),-1) + diag(-1*ones(1,N),1);
  S(1,1) = 1;
  S(1,2) = -1/2;
  % r is weirdly zero-indexed in Bai+Judkins
  for r=2:N-1; S(1,r+1) = realpow(-1, r+1) * (1/(r-1)-1/(r+1)); end
  S(1,N+1) = realpow(-1,N+1) * (1/N-1)

  % Ca
  Ca = R * S * T' * V

  eold = 10 * tol;
  iter = 0;
  maxiter = 300;

  while true
    g = k2 * ode(t, xold, varargin{:});
    beta = Ca * g + x0;
    xnew = Cx * beta;
    enew = max(abs(xnew - xold), [], 'all');

    iter = iter + 1;

    if ( enew < tol && eold < tol ) || iter >= maxiter
      break
    end

    xold = xnew;
    eold = enew;
  end

  xnew
  iter
end

test.m

mu = 1.0;

indexes.r = 1:3;
indexes.v = 4:6;

N = 20;

r0 = [ 1 0 0 ];
v0 = [ 0 1 0 ];

x0 = repmat([ r0 v0 ], N+1, 1);

mcpi(@centralforce, x0, 0, 2*pi, N, 1e-8, indexes, mu);

function dxdt = centralforce(t, x, indexes, mu)
  r = x(:, indexes.r)
  rmag = vecnorm(r, 2, 2)

  vdot = - mu ./ rmag.^3 .* r

  dxdt = [ x(:, indexes.v) vdot ]
end