FreeFormTransformationRungeKutta.h
1 /*
2  * Medical Image Registration ToolKit (MIRTK)
3  *
4  * Copyright 2008-2015 Imperial College London
5  * Copyright 2013-2015 Andreas Schuh
6  *
7  * Licensed under the Apache License, Version 2.0 (the "License");
8  * you may not use this file except in compliance with the License.
9  * You may obtain a copy of the License at
10  *
11  * http://www.apache.org/licenses/LICENSE-2.0
12  *
13  * Unless required by applicable law or agreed to in writing, software
14  * distributed under the License is distributed on an "AS IS" BASIS,
15  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
16  * See the License for the specific language governing permissions and
17  * limitations under the License.
18  */
19 
20 // TODO: Change velocities to be given in lattice units rather than world units.
21 // This reduces the number of conversions from world to lattice coordinates
22 // during the numerical integration.
23 // -as12312
24 
25 #ifndef MIRTK_FreeFormTransformationRungeKutta_H
26 #define MIRTK_FreeFormTransformationRungeKutta_H
27 
28 #include "mirtk/Math.h"
29 #include "mirtk/Matrix.h"
30 #include "mirtk/Vector3D.h"
31 #include "mirtk/TransformationJacobian.h"
32 
33 
34 namespace mirtk {
35 
36 
37 // ============================================================================
38 // Base class of Runge-Kutta integration methods
39 // ============================================================================
40 
41 /**
42  * Base class of integration methods used by FFDs which are parameterized by a
43  * velocity field to compute the displacements.
44  */
45 class FreeFormTransformationRungeKutta
46 {
47 protected:
48 
49  // -------------------------------------------------------------------------
50  /// Helper for computation of Jacobian w.r.t spatial coordinates
51  static void dkdx(Matrix &dk, const Matrix &Dv, const Matrix &dx, double h)
52  {
53  dk(0, 0) = (Dv(0, 0) * dx(0, 0) + Dv(0, 1) * dx(1, 0) + Dv(0, 2) * dx(2, 0)) * h;
54  dk(0, 1) = (Dv(0, 0) * dx(0, 1) + Dv(0, 1) * dx(1, 1) + Dv(0, 2) * dx(2, 1)) * h;
55  dk(0, 2) = (Dv(0, 0) * dx(0, 2) + Dv(0, 1) * dx(1, 2) + Dv(0, 2) * dx(2, 2)) * h;
56  dk(1, 0) = (Dv(1, 0) * dx(0, 0) + Dv(1, 1) * dx(1, 0) + Dv(1, 2) * dx(2, 0)) * h;
57  dk(1, 1) = (Dv(1, 0) * dx(0, 1) + Dv(1, 1) * dx(1, 1) + Dv(1, 2) * dx(2, 1)) * h;
58  dk(1, 2) = (Dv(1, 0) * dx(0, 2) + Dv(1, 1) * dx(1, 2) + Dv(1, 2) * dx(2, 2)) * h;
59  dk(2, 0) = (Dv(2, 0) * dx(0, 0) + Dv(2, 1) * dx(1, 0) + Dv(2, 2) * dx(2, 0)) * h;
60  dk(2, 1) = (Dv(2, 0) * dx(0, 1) + Dv(2, 1) * dx(1, 1) + Dv(2, 2) * dx(2, 1)) * h;
61  dk(2, 2) = (Dv(2, 0) * dx(0, 2) + Dv(2, 1) * dx(1, 2) + Dv(2, 2) * dx(2, 2)) * h;
62  }
63 
64  // -------------------------------------------------------------------------
65  /// Helper for computation of Jacobian w.r.t control point
66  static void dkdp(Matrix &dk, const Matrix &Dv, const Matrix &dx, const double dv[3], double h)
67  {
68  dk(0, 0) = (Dv(0, 0) * dx(0, 0) + Dv(0, 1) * dx(1, 0) + Dv(0, 2) * dx(2, 0) + dv[0]) * h;
69  dk(0, 1) = (Dv(0, 0) * dx(0, 1) + Dv(0, 1) * dx(1, 1) + Dv(0, 2) * dx(2, 1) ) * h;
70  dk(0, 2) = (Dv(0, 0) * dx(0, 2) + Dv(0, 1) * dx(1, 2) + Dv(0, 2) * dx(2, 2) ) * h;
71  dk(1, 0) = (Dv(1, 0) * dx(0, 0) + Dv(1, 1) * dx(1, 0) + Dv(1, 2) * dx(2, 0) ) * h;
72  dk(1, 1) = (Dv(1, 0) * dx(0, 1) + Dv(1, 1) * dx(1, 1) + Dv(1, 2) * dx(2, 1) + dv[1]) * h;
73  dk(1, 2) = (Dv(1, 0) * dx(0, 2) + Dv(1, 1) * dx(1, 2) + Dv(1, 2) * dx(2, 2) ) * h;
74  dk(2, 0) = (Dv(2, 0) * dx(0, 0) + Dv(2, 1) * dx(1, 0) + Dv(2, 2) * dx(2, 0) ) * h;
75  dk(2, 1) = (Dv(2, 0) * dx(0, 1) + Dv(2, 1) * dx(1, 1) + Dv(2, 2) * dx(2, 1) ) * h;
76  dk(2, 2) = (Dv(2, 0) * dx(0, 2) + Dv(2, 1) * dx(1, 2) + Dv(2, 2) * dx(2, 2) + dv[2]) * h;
77  }
78 
79 }; // FreeFormTransformationRungeKutta
80 
81 // ============================================================================
82 // Generic implementation of explicit Runge-Kutta
83 // ============================================================================
84 
85 /**
86  * Explicit Runge-Kutta integration method for FFD parameterized by velocity field.
87  *
88  * The first template argument is the type of the FFD which has to implement
89  * the following methods:
90  *
91  * - Evaluate
92  * - EvaluateJacobianWorld
93  * - EvaluateJacobianDOFs
94  *
95  * The second template argument is a struct of the respective Butcher tableau
96  * of the Runge-Kutta method which has been defined using the macro
97  * MIRTK_DEFINE_FFDRK_EXPLICIT.
98  *
99  * \sa BSplineFreeFormTransformationTD
100  */
101 template <class TFreeFormTransformation, class TButcherTableau>
102 class FreeFormTransformationExplicitRungeKutta
103 :
104  public FreeFormTransformationRungeKutta
105 {
106 public:
107 
108  typedef TButcherTableau BT; ///< Short-hand for Butcher tableau template argument
109 
110  // -------------------------------------------------------------------------
111  static void Transform(const TFreeFormTransformation *v,
112  double &x, double &y, double &z,
113  double t1, double t2, double dt)
114  {
115  if (t1 == t2) return;
116 
117  Vector3D<double> k[BT::s]; // Intermediate evaluations
118  const double d = copysign(1.0, t2 - t1); // Direction of integration
119  double h = d * abs(dt); // Initial step size
120  int i, j; // Butcher tableau indices
121  double l; // Temporal lattice coordinate
122 
123  // Integrate from t=t1 to t=t2
124  double t = t1;
125  while (d * t < d * t2) {
126  // Ensure that last step ends at t2
127  if (d * (t + h) > d * t2) h = t2 - t;
128  // Evaluate velocities at intermediate steps
129  if (BT::fsal && t != t1) k[0] = k[BT::s - 1], i = 1;
130  else i = 0;
131  for (/*i = 0|1*/; i < BT::s; i++) {
132  // Lattice coordinates of current intermediate step
133  k[i]._x = x, k[i]._y = y, k[i]._z = z;
134  for (j = 0; j < i; j++) k[i] += k[j] * BT::a[i][j];
135  v->WorldToLattice(k[i]._x, k[i]._y, k[i]._z);
136  l = v->TimeToLattice(t + BT::c[i] * h);
137  // Evaluate velocity at intermediate point
138  v->Evaluate(k[i]._x, k[i]._y, k[i]._z, l);
139  k[i] *= h;
140  }
141  // Perform step
142  for (i = 0; i < BT::s; i++) {
143  x += k[i]._x * BT::b[i];
144  y += k[i]._y * BT::b[i];
145  z += k[i]._z * BT::b[i];
146  }
147  t += h;
148  }
149  }
150 
151  // -------------------------------------------------------------------------
152  static void Jacobian(const TFreeFormTransformation *v,
153  Matrix &jac,
154  double &x, double &y, double &z,
155  double t1, double t2, double dt)
156  {
157  if (t1 == t2) return;
158 
159  Vector3D<double> k [BT::s]; // Intermediate evaluations
160  Matrix dk[BT::s]; // Derivative of k_i
161  Matrix dx(3, 3); // Derivative of intermediate location
162  Matrix Dv(3, 3); // Partial derivative of velocity field
163  const double d = copysign(1.0, t2 - t1); // Direction of integration
164  double h = d * abs(dt); // Initial step size
165  int i, j; // Butcher tableau indices
166  double l; // Temporal lattice coordinate
167 
168  for (i = 0; i < BT::s; ++i) dk[i].Initialize(3, 3);
169 
170  // Integrate from t=t1 to t=t2
171  double t = t1;
172  while (d * t < d * t2) {
173  // Ensure that last step ends at t2
174  if (d * (t + h) > d * t2) h = t2 - t;
175  // Evaluate at intermediate steps
176  if (BT::fsal && t != t1) {
177  k [0] = k [BT::s-1];
178  dk[0] = dk[BT::s-1];
179  i = 1;
180  } else {
181  i = 0;
182  }
183  for (/*i = 0|1*/; i < BT::s; i++) {
184  // Lattice coordinates
185  k[i]._x = x, k[i]._y = y, k[i]._z = z;
186  for (j = 0; j < i; j++) k[i] += k[j] * BT::a[i][j];
187  v->WorldToLattice(k[i]._x, k[i]._y, k[i]._z);
188  l = v->TimeToLattice(t + BT::c[i] * h);
189  // Evaluate partial derivatives of velocity
190  v->EvaluateJacobianWorld(Dv, k[i]._x, k[i]._y, k[i]._z, l);
191  // Evaluate velocity
192  v->Evaluate(k[i]._x, k[i]._y, k[i]._z, l);
193  k[i] *= h;
194  // Calculate derivatives of k_i
195  dx.Ident();
196  for (j = 0; j < i; j++) dx += dk[j] * BT::a[i][j];
197  dkdx(dk[i], Dv, dx, h); // dk_i = Dv dx h
198  }
199  // Perform step with local extrapolation
200  dx.Ident();
201  for (i = 0; i < BT::s; i++) {
202  x += k[i]._x * BT::b[i];
203  y += k[i]._y * BT::b[i];
204  z += k[i]._z * BT::b[i];
205  dx += dk[i] * BT::b[i];
206  }
207  jac = dx * jac;
208  t += h;
209  }
210  }
211 
212  // -------------------------------------------------------------------------
213  static void JacobianDOFs(const TFreeFormTransformation *v,
214  Matrix &jac, int ci, int cj, int ck, int cl,
215  double &x, double &y, double &z,
216  double t1, double t2, double dt)
217  {
218  if (t1 == t2) return;
219 
220  Vector3D<double> k [BT::s]; // Intermediate evaluations
221  Matrix dk[BT::s]; // Derivative of k_i w.r.t control point
222  Matrix dx; // Derivative of intermediate location w.r.t control point
223  Matrix Dv; // Partial derivative of velocity field w.r.t spatial coordinates
224  double dv[3]; // Partial derivative of velocity field w.r.t control point
225  const double d = copysign(1.0, t2 - t1); // Direction of integration
226  double h = d * abs(dt); // Initial step size
227  int i, j; // Butcher tableau indices
228  double l; // Temporal lattice coordinate
229 
230  for (i = 0; i < BT::s; i++) dk[i].Initialize(3, 3);
231 
232  // Integrate from t=t1 to t=t2
233  double t = t1;
234  while (d * t < d * t2) {
235  // Ensure that last step ends at t2
236  if (d * (t + h) > d * t2) h = t2 - t;
237  // Evaluate at intermediate steps
238  if (BT::fsal && t != t1) {
239  k [0] = k [BT::s-1];
240  dk[0] = dk[BT::s-1];
241  i = 1;
242  } else {
243  i = 0;
244  }
245  for (/*i = 0|1*/; i < BT::s; i++) {
246  // Lattice coordinates
247  k[i]._x = x, k[i]._y = y, k[i]._z = z;
248  for (j = 0; j < i; j++) k[i] += k[j] * BT::a[i][j];
249  v->WorldToLattice(k[i]._x, k[i]._y, k[i]._z);
250  l = v->TimeToLattice(t + BT::c[i] * h);
251  // Evaluate partial derivatives of velocity
252  v->EvaluateJacobianWorld(Dv, k[i]._x, k[i]._y, k[i]._z, l);
253  v->EvaluateJacobianDOFs (dv, ci, cj, ck, cl, k[i]._x, k[i]._y, k[i]._z, l);
254  // Evaluate velocity
255  v->Evaluate(k[i]._x, k[i]._y, k[i]._z, l);
256  k[i] *= h;
257  // Jacobian at intermediate step
258  dx = jac;
259  for (j = 0; j < i; j++) dx += dk[j] * BT::a[i][j];
260  // Calculate derivatives of k_i
261  dkdp(dk[i], Dv, dx, dv, h); // dk = (Dv * dx + dv) * h
262  }
263  // Perform step with local extrapolation
264  for (i = 0; i < BT::s; i++) {
265  x += k[i]._x * BT::b[i];
266  y += k[i]._y * BT::b[i];
267  z += k[i]._z * BT::b[i];
268  jac += dk[i] * BT::b[i];
269  }
270  t += h;
271  }
272  }
273 
274  // -------------------------------------------------------------------------
275  static void JacobianDOFs(const TFreeFormTransformation *v,
276  TransformationJacobian &jac,
277  double &x, double &y, double &z,
278  double t1, double t2, double dt)
279  {
280  if (t1 == t2) return;
281 
282  Vector3D<double> k [BT::s]; // Intermediate evaluations
283  TransformationJacobian dk[BT::s]; // Derivative of k_i w.r.t transformation parameters
284  // and temporarily used to store partial derivative of
285  // velocity field w.r.t control points
286  TransformationJacobian dx; // Current derivative w.r.t transformation parameters
287  Matrix Dv; // Partial derivative of velocity field w.r.t spatial coordinates
288  const double d = copysign(1.0, t2 - t1); // Direction of integration
289  double h = d * abs(dt); // Initial step size
290  int i, j; // Butcher tableau indices
291  double l; // Temporal lattice coordinate
292 
293  // Integrate from t=t1 to t=t2
294  double t = t1;
295  while (d * t < d * t2) {
296  // Ensure that last step ends at t2
297  if (d * (t + h) > d * t2) h = t2 - t;
298  // Evaluate at intermediate steps
299  if (BT::fsal && t != t1) {
300  k [0] = k [BT::s - 1];
301  dk[0] = dk[BT::s - 1];
302  i = 1;
303  } else {
304  i = 0;
305  }
306  for (/*i = 0|1*/; i < BT::s; i++) {
307  // Lattice coordinates
308  k[i]._x = x, k[i]._y = y, k[i]._z = z;
309  for (j = 0; j < i; j++) k[i] += k[j] * BT::a[i][j];
310  v->WorldToLattice(k[i]._x, k[i]._y, k[i]._z);
311  l = v->TimeToLattice(t + BT::c[i] * h);
312  // Evaluate partial derivatives of velocity
313  v->EvaluateJacobianWorld(Dv, k[i]._x, k[i]._y, k[i]._z, l);
314  v->EvaluateJacobianDOFs (dk[i]/*=dv*/, k[i]._x, k[i]._y, k[i]._z, l);
315  // Evaluate velocity
316  v->Evaluate(k[i]._x, k[i]._y, k[i]._z, l);
317  k[i] *= h;
318  // Jacobian at intermediate step
319  dx = jac;
320  for (j = 0; j < i; j++) dx.add(dk[j], BT::a[i][j]);
321  // Calculate derivatives of k_i
322  dx *= (Dv *= h); // += Dv * dx * h
323  dx.add(dk[i], h); // += dv * h
324  dk[i] = dx; // dk = (Dv * dx + dv) * h
325  }
326  // Perform step with local extrapolation
327  for (i = 0; i < BT::s; i++) {
328  x += k[i]._x * BT::b[i];
329  y += k[i]._y * BT::b[i];
330  z += k[i]._z * BT::b[i];
331  jac.add(dk[i], BT::b[i]);
332  }
333  t += h;
334  }
335  }
336 
337 }; // FreeFormTransformationExplicitRungeKutta
338 
339 // ===========================================================================
340 // Generic implementation of embedded Runge-Kutta
341 // ===========================================================================
342 
343 /**
344  * Embedded Runge-Kutta integration method for FFD parameterized by velocity field.
345  *
346  * The first template argument is the type of the FFD which has to implement
347  * the following methods:
348  *
349  * - Evaluate
350  * - EvaluateJacobianWorld
351  * - EvaluateJacobianDOFs
352  *
353  * The second template argument is a struct of the respective Butcher tableau
354  * of the Runge-Kutta method which has been defined using the macro
355  * MIRTK_DEFINE_FFDRK_EMBEDDED.
356  *
357  * \sa BSplineFreeFormTransformationTD
358  */
359 template <class TFreeFormTransformation, class TButcherTableau>
360 class FreeFormTransformationEmbeddedRungeKutta
361 :
362  public FreeFormTransformationRungeKutta
363 {
364 public:
365 
366  typedef TButcherTableau BT; ///< Short-hand for Butcher tableau template argument
367 
368  // -------------------------------------------------------------------------
369  static void Transform(const TFreeFormTransformation *v,
370  double &x, double &y, double &z,
371  double t1, double t2, double mindt, double maxdt, double tol)
372  {
373  if (t1 == t2) return;
374 
375  Vector3D<double> k[BT::s]; // k_i = h * v(t + c_i * h, x + sum_{j=0}^{i-1} a_j * k_j)
376  Vector3D<double> tmp; // Solution of order p-1
377  Vector3D<double> next; // Solution of order p
378  double error; // Local error estimate
379  double h = t2 - t1; // Initial step size
380  double hnext; // Next step size
381  const double d = copysign(1.0, h); // Direction of integration
382  const double e = 1.0 / static_cast<double>(BT::p); // Exponent for step size scaling factor
383  int i, j; // Butcher tableau indices
384  double l; // Temporal lattice coordinate
385 
386  // Decrease initial step size if necessary
387  if (abs(h) > maxdt) h = copysign(maxdt, d);
388 
389  // Integrate from t=t1 to t=t2
390  double t = t1;
391  while (d * t < d * t2) {
392  // Ensure that last step ends at t2
393  if (d * (t + h) > d * t2) h = t2 - t;
394  // Evaluate velocities at intermediate steps
395  if (BT::fsal && t != t1) k[0] = k[BT::s - 1], i = 1;
396  else i = 0;
397  for (/*i = 0|1*/; i < BT::s; i++) {
398  k[i]._x = x, k[i]._y = y, k[i]._z = z;
399  for (j = 0; j < i; j++) k[i] += k[j] * BT::a[i][j];
400  v->WorldToLattice(k[i]._x, k[i]._y, k[i]._z);
401  l = v->TimeToLattice(t + BT::c[i] * h);
402  v->Evaluate(k[i]._x, k[i]._y, k[i]._z, l);
403  k[i] *= h;
404  }
405  // Calculate solution of order p
406  next._x = x, next._y = y, next._z = z;
407  for (i = 0; i < BT::s; i++) next += k[i] * BT::b[1][i];
408  // Adapt step size
409  if (mindt < maxdt) {
410  // Calculate solution of order p-1
411  tmp._x = x, tmp._y = y, tmp._z = z;
412  for (i = 0; i < BT::s; i++) tmp += k[i] * BT::b[0][i];
413  // Estimate local error
414  error = max(max(abs(next._x - tmp._x),
415  abs(next._y - tmp._y)),
416  abs(next._z - tmp._z));
417  // If local error exceeds tolerance...
418  if (abs(h) > mindt && error > tol) {
419  // ...decrease step size
420  h *= 0.8 * pow(tol / error, e);
421  if (abs(h) < mindt) h = copysign(mindt, d);
422  // ...and redo current step
423  continue;
424  // Otherwise, increase step size
425  } else {
426  hnext = 0.8 * pow(tol / error, e) * h;
427  if (abs(hnext) < mindt) hnext = copysign(mindt, d);
428  else if (abs(hnext) > maxdt) hnext = copysign(maxdt, d);
429  }
430  } else {
431  hnext = h;
432  }
433  // Perform step with local extrapolation
434  x = next._x;
435  y = next._y;
436  z = next._z;
437  t += h;
438  // Update step size
439  h = hnext;
440  }
441  }
442 
443  // -------------------------------------------------------------------------
444  static void Jacobian(const TFreeFormTransformation *v,
445  Matrix &jac, double &x, double &y, double &z,
446  double t1, double t2, double mindt, double maxdt, double tol)
447  {
448  if (t1 == t2) return;
449 
450  Vector3D<double> k [BT::s]; // Intermediate evaluations
451  Matrix dk[BT::s]; // Derivative of k_i
452  Matrix dx(3, 3); // Derivative of intermediate location
453  Matrix Dv(3, 3); // Partial derivative of velocity field
454  double h = t2 - t1; // Initial step size
455  double hnext; // Next step size
456  const double d = copysign(1.0, h); // Direction of integration
457  Vector3D<double> tmp; // Solution of order p-1
458  Vector3D<double> next; // Solution of order p
459  double error; // Local error estimate
460  const double e = 1.0 / static_cast<double>(BT::p); // Exponent for step size scaling factor
461  int i, j; // Butcher tableau indices
462  double l; // Temporal lattice coordinate
463 
464  for (i = 0; i < BT::s; i++) dk[i].Initialize(3, 3);
465 
466  // Decrease initial step size if necessary
467  if (abs(h) > maxdt) h = copysign(maxdt, d);
468 
469  // Integrate from t=t1 to t=t2
470  double t = t1;
471  while (d * t < d * t2) {
472  // Ensure that last step ends at t2
473  if (d * (t + h) > d * t2) h = t2 - t;
474  // Evaluate at intermediate steps
475  if (BT::fsal && t != t1) {
476  k [0] = k [BT::s-1];
477  dk[0] = dk[BT::s-1];
478  i = 1;
479  } else {
480  i = 0;
481  }
482  for (/*i = 0|1*/; i < BT::s; i++) {
483  // Lattice coordinates
484  k[i]._x = x, k[i]._y = y, k[i]._z = z;
485  for (j = 0; j < i; j++) k[i] += k[j] * BT::a[i][j];
486  v->WorldToLattice(k[i]._x, k[i]._y, k[i]._z);
487  l = v->TimeToLattice(t + BT::c[i] * h);
488  // Evaluate partial derivatives of velocity
489  v->EvaluateJacobianWorld(Dv, k[i]._x, k[i]._y, k[i]._z, l);
490  // Evaluate velocity
491  v->Evaluate(k[i]._x, k[i]._y, k[i]._z, l);
492  k[i] *= h;
493  // Jacobian at current intermediate step
494  dx.Ident();
495  for (j = 0; j < i; j++) dx += dk[j] * BT::a[i][j];
496  // Calculate derivatives of k_i
497  dkdx(dk[i], Dv, dx, h); // dk = Dv * dx * h
498  }
499  // Calculate solution of order p
500  next._x = x, next._y = y, next._z = z;
501  for (i = 0; i < BT::s; i++) next += k[i] * BT::b[1][i];
502  // Adapt step size
503  if (mindt < maxdt) {
504  // Calculate solution of order p-1
505  tmp._x = x, tmp._y = y, tmp._z = z;
506  for (i = 0; i < BT::s; i++) tmp += k[i] * BT::b[0][i];
507  // Estimate local error
508  error = max(max(abs(next._x - tmp._x),
509  abs(next._y - tmp._y)),
510  abs(next._z - tmp._z));
511  // If local error exceeds tolerance...
512  if (abs(h) > mindt && error > tol) {
513  // ...decrease step size
514  h *= 0.8 * pow(tol / error, e);
515  if (abs(h) < mindt) h = copysign(mindt, d);
516  // ...and redo current step
517  continue;
518  // Otherwise, increase step size
519  } else {
520  hnext = 0.8 * pow(tol / error, e) * h;
521  if (abs(hnext) < mindt) hnext = copysign(mindt, d);
522  else if (abs(hnext) > maxdt) hnext = copysign(maxdt, d);
523  }
524  } else {
525  hnext = h;
526  }
527  // Update Jacobian
528  dx.Ident();
529  for (i = 0; i < BT::s; i++) dx += dk[i] * BT::b[1][i];
530  jac = dx * jac;
531  // Perform step with local extrapolation
532  x = next._x;
533  y = next._y;
534  z = next._z;
535  t += h;
536  // Update step size
537  h = hnext;
538  }
539  }
540 
541  // -------------------------------------------------------------------------
542  static void JacobianDOFs(const TFreeFormTransformation *v,
543  Matrix &jac, int ci, int cj, int ck, int cl,
544  double &x, double &y, double &z, double t1, double t2,
545  double mindt, double maxdt, double tol)
546  {
547  if (t1 == t2) return;
548 
549  Vector3D<double> k [BT::s]; // Intermediate evaluations
550  Matrix dk[BT::s]; // Derivative of k_i w.r.t control point
551  Matrix dx; // Derivative of intermediate location w.r.t control point
552  Matrix Dv; // Partial derivative of velocity field w.r.t spatial coordinates
553  double dv[3]; // Partial derivative of velocity field w.r.t control point
554  double h = t2 - t1; // Initial step size
555  double hnext; // Next step size
556  const double d = copysign(1.0, h); // Direction of integration
557  Vector3D<double> tmp; // Solution of order p-1
558  Vector3D<double> next; // Solution of order p
559  double error; // Local error estimate
560  const double e = 1.0 / static_cast<double>(BT::p); // Exponent for step size scaling factor
561  int i, j; // Butcher tableau indices
562  double l; // Temporal lattice coordinate
563 
564  for (i = 0; i < BT::s; i++) dk[i].Initialize(3, 3);
565 
566  // Decrease initial step size if necessary
567  if (abs(h) > maxdt) h = copysign(maxdt, d);
568 
569  // Integrate from t=t1 to t=t2
570  double t = t1;
571  while (d * t < d * t2) {
572  // Ensure that last step ends at t2
573  if (d * (t + h) > d * t2) h = t2 - t;
574  // Evaluate at intermediate steps
575  if (BT::fsal && t != t1) {
576  k [0] = k [BT::s-1];
577  dk[0] = dk[BT::s-1];
578  i = 1;
579  } else {
580  i = 0;
581  }
582  for (/*i = 0|1*/; i < BT::s; i++) {
583  // Lattice coordinates
584  k[i]._x = x, k[i]._y = y, k[i]._z = z;
585  for (j = 0; j < i; j++) k[i] += k[j] * BT::a[i][j];
586  v->WorldToLattice(k[i]._x, k[i]._y, k[i]._z);
587  l = v->TimeToLattice(t + BT::c[i] * h);
588  // Evaluate partial derivatives of velocity
589  v->EvaluateJacobianWorld(Dv, k[i]._x, k[i]._y, k[i]._z, l);
590  v->EvaluateJacobianDOFs (dv, ci, cj, ck, cl, k[i]._x, k[i]._y, k[i]._z, l);
591  // Evaluate velocity
592  v->Evaluate(k[i]._x, k[i]._y, k[i]._z, l);
593  k[i] *= h;
594  // Jacobian at current intermediate step
595  dx = jac;
596  for (j = 0; j < i; j++) dx += dk[j] * BT::a[i][j];
597  // Calculate derivatives of k_i
598  dkdp(dk[i], Dv, dx, dv, h); // dk = (Dv * dx + dv) * h
599  }
600  // Calculate solution of order p
601  next._x = x, next._y = y, next._z = z;
602  for (i = 0; i < BT::s; i++) next += k[i] * BT::b[1][i];
603  // Adapt step size
604  if (mindt < maxdt) {
605  // Calculate solution of order p-1
606  tmp._x = x, tmp._y = y, tmp._z = z;
607  for (i = 0; i < BT::s; i++) tmp += k[i] * BT::b[0][i];
608  // Estimate local error
609  error = max(max(abs(next._x - tmp._x),
610  abs(next._y - tmp._y)),
611  abs(next._z - tmp._z));
612  // If local error exceeds tolerance...
613  if (abs(h) > mindt && error > tol) {
614  // ...decrease step size
615  h *= 0.8 * pow(tol / error, e);
616  if (abs(h) < mindt) h = copysign(mindt, d);
617  // ...and redo current step
618  continue;
619  // Otherwise, increase step size
620  } else {
621  hnext = 0.8 * pow(tol / error, e) * h;
622  if (abs(hnext) < mindt) hnext = copysign(mindt, d);
623  else if (abs(hnext) > maxdt) hnext = copysign(maxdt, d);
624  }
625  } else {
626  hnext = h;
627  }
628  // Update Jacobian
629  for (i = 0; i < BT::s; i++) jac += dk[i] * BT::b[1][i];
630  // Perform step with local extrapolation
631  x = next._x;
632  y = next._y;
633  z = next._z;
634  t += h;
635  // Update step size
636  h = hnext;
637  }
638  }
639 
640  // -------------------------------------------------------------------------
641  static void JacobianDOFs(const TFreeFormTransformation *v,
642  TransformationJacobian &jac,
643  double &x, double &y, double &z,
644  double t1, double t2, double mindt, double maxdt, double tol)
645  {
646  if (t1 == t2) return;
647 
648  Vector3D<double> k [BT::s]; // Intermediate evaluations
649  TransformationJacobian dk[BT::s]; // Derivative of k_i w.r.t transformation parameters
650  // and temporarily used to store partial derivative of
651  // velocity field w.r.t control points
652  TransformationJacobian dx; // Current derivative w.r.t transformation parameters
653  Matrix Dv; // Partial derivative of velocity field w.r.t spatial coordinates
654  double h = t2 - t1; // Initial step size
655  const double d = copysign(1.0, h); // Direction of integration
656  double hnext; // Next step size
657  Vector3D<double> tmp; // Solution of order p-1
658  Vector3D<double> next; // Solution of order p
659  double error; // Local error estimate
660  const double e = 1.0 / double(BT::p); // Exponent for step size scaling factor
661  int i, j; // Butcher tableau indices
662  double l; // Temporal lattice coordinate
663 
664  // Decrease initial step size if necessary
665  if (abs(h) > maxdt) h = copysign(maxdt, d);
666 
667  // Integrate from t=t1 to t=t2
668  double t = t1;
669  while (d * t < d * t2) {
670  // Ensure that last step ends at t2
671  if (d * (t + h) > d * t2) h = t2 - t;
672  // Evaluate at intermediate steps
673  if (BT::fsal && t != t1) {
674  k [0] = k [BT::s-1];
675  dk[0] = dk[BT::s-1];
676  i = 1;
677  } else {
678  i = 0;
679  }
680  for (/*i = 0|1*/; i < BT::s; i++) {
681  // Lattice coordinates
682  k[i]._x = x, k[i]._y = y, k[i]._z = z;
683  for (j = 0; j < i; j++) k[i] += k[j] * BT::a[i][j];
684  v->WorldToLattice(k[i]._x, k[i]._y, k[i]._z);
685  l = v->TimeToLattice(t + BT::c[i] * h);
686  // Evaluate partial derivatives of velocity
687  v->EvaluateJacobianWorld(Dv, k[i]._x, k[i]._y, k[i]._z, l);
688  v->EvaluateJacobianDOFs (dk[i]/*=dv*/, k[i]._x, k[i]._y, k[i]._z, l);
689  // Evaluate velocity
690  v->Evaluate(k[i]._x, k[i]._y, k[i]._z, l);
691  k[i] *= h;
692  // Jacobian at intermediate step
693  dx = jac;
694  for (j = 0; j < i; j++) dx.add(dk[j], BT::a[i][j]);
695  // Calculate derivatives of k_i
696  dx *= (Dv *= h); // += Dv * dx * h
697  dx.add(dk[i], h); // += dv * h
698  dk[i] = dx; // dk = (Dv * dx + dv) * h
699  }
700  // Calculate solution of order p
701  next._x = x, next._y = y, next._z = z;
702  for (i = 0; i < BT::s; i++) next += k[i] * BT::b[1][i];
703  // Adapt step size
704  if (mindt < maxdt) {
705  // Calculate solution of order p-1
706  tmp._x = x, tmp._y = y, tmp._z = z;
707  for (i = 0; i < BT::s; i++) tmp += k[i] * BT::b[0][i];
708  // Estimate local error
709  error = max(max(abs(next._x - tmp._x),
710  abs(next._y - tmp._y)),
711  abs(next._z - tmp._z));
712  // If local error exceeds tolerance...
713  if (abs(h) > mindt && error > tol) {
714  // ...decrease step size
715  h *= 0.8 * pow(tol / error, e);
716  if (abs(h) < mindt) h = copysign(mindt, d);
717  // ...and redo current step
718  continue;
719  // Otherwise, increase step size
720  } else {
721  hnext = 0.8 * pow(tol / error, e) * h;
722  if (abs(hnext) < mindt) hnext = copysign(mindt, d);
723  else if (abs(hnext) > maxdt) hnext = copysign(maxdt, d);
724  }
725  } else {
726  hnext = h;
727  }
728  // Update Jacobian
729  for (i = 0; i < BT::s; i++) jac.add(dk[i], BT::b[1][i]);
730  // Perform step with local extrapolation
731  x = next._x;
732  y = next._y;
733  z = next._z;
734  t += h;
735  // Update step size
736  h = hnext;
737  }
738  }
739 
740 }; // FreeFormTransformationEmbeddedRungeKutta
741 
742 // =============================================================================
743 // Auxiliary Macros
744 // =============================================================================
745 
746 // -----------------------------------------------------------------------------
747 /// Declare explicit Runge-Kutta method
748 #define MIRTK_DECLARE_FFDRK_EXPLICIT(NAME, S) \
749  struct FreeFormTransformationButcherTableau##NAME \
750  { \
751  static const int s = S; \
752  static const int p; \
753  static const bool fsal; \
754  static const double a[S][S]; \
755  static const double b[S]; \
756  static const double c[S]; \
757  }; \
758  \
759  template <class TFreeFormTransformation> \
760  class FreeFormTransformationIntegration##NAME \
761  : public mirtk::FreeFormTransformationExplicitRungeKutta \
762  <TFreeFormTransformation, FreeFormTransformationButcherTableau##NAME> \
763  { }
764 
765 // -----------------------------------------------------------------------------
766 /// Declare embedded Runge-Kutta method
767 #define MIRTK_DECLARE_FFDRK_EMBEDDED(NAME, S) \
768  struct FreeFormTransformationButcherTableau##NAME \
769  { \
770  static const int s = S; \
771  static const int p; \
772  static const bool fsal; \
773  static const double a[S][S]; \
774  static const double b[2][S]; \
775  static const double c[S]; \
776  }; \
777  \
778  template <class TFreeFormTransformation> \
779  class FreeFormTransformationIntegration##NAME \
780  : public mirtk::FreeFormTransformationEmbeddedRungeKutta \
781  <TFreeFormTransformation, FreeFormTransformationButcherTableau##NAME> \
782  { }
783 
784 // -----------------------------------------------------------------------------
785 /// Define explicit Runge-Kutta method
786 #define MIRTK_DEFINE_FFDRK_EXPLICIT(NAME, S, P, FSAL, C, A, B) \
787  /*const int FreeFormTransformationButcherTableau##NAME::s = S;*/ \
788  const int FreeFormTransformationButcherTableau##NAME::p = P; \
789  const bool FreeFormTransformationButcherTableau##NAME::fsal = FSAL; \
790  const double FreeFormTransformationButcherTableau##NAME::a[S][S] = A; \
791  const double FreeFormTransformationButcherTableau##NAME::b[S] = B; \
792  const double FreeFormTransformationButcherTableau##NAME::c[S] = C
793 
794 // -----------------------------------------------------------------------------
795 /// Define embedded Runge-Kutta method
796 #define MIRTK_DEFINE_FFDRK_EMBEDDED(NAME, S, P, FSAL, C, A, BY, BZ) \
797  /*const int FreeFormTransformationButcherTableau##NAME::s = S;*/ \
798  const int FreeFormTransformationButcherTableau##NAME::p = P; \
799  const bool FreeFormTransformationButcherTableau##NAME::fsal = FSAL; \
800  const double FreeFormTransformationButcherTableau##NAME::a[S][S] = A; \
801  const double FreeFormTransformationButcherTableau##NAME::b[2][S] = {BY, BZ}; \
802  const double FreeFormTransformationButcherTableau##NAME::c[S] = C
803 
804 // ============================================================================
805 // Runge-Kutta integration methods
806 // ============================================================================
807 
808 // name steps description
809 // ----------------------------------------------------------------------------
810 MIRTK_DECLARE_FFDRK_EXPLICIT(RKE1, 1); ///< Forward Euler method
811 MIRTK_DECLARE_FFDRK_EMBEDDED(RKEH12, 2); ///< Euler-Heun method of order 2(1)
812 MIRTK_DECLARE_FFDRK_EXPLICIT(RKE2, 2); ///< Modified Euler method (Heun's method, explicit midpoint rule)
813 MIRTK_DECLARE_FFDRK_EXPLICIT(RKH2, 2); ///< Improved Euler method (Heun's method, explicit trapezoidal rule)
814 MIRTK_DECLARE_FFDRK_EMBEDDED(RKBS23, 4); ///< Bogacki-Shampine method of order 3(2)
815 MIRTK_DECLARE_FFDRK_EXPLICIT(RK4, 4); ///< Classical Runge-Kutta method
816 MIRTK_DECLARE_FFDRK_EMBEDDED(RKF45, 6); ///< Fehlberg method of order 5(4)
817 MIRTK_DECLARE_FFDRK_EMBEDDED(RKCK45, 6); ///< Cash-Karp method of order 5(4)
818 MIRTK_DECLARE_FFDRK_EMBEDDED(RKDP45, 7); ///< Dormand–Prince method of order 5(4)
819 
820 
821 } // namespace mirtk
822 
823 #endif // MIRTK_FreeFormTransformationRungeKutta_H
mirtk::Matrix::Ident
Matrix & Ident()
Set to identity matrix.
mirtk
Definition: IOConfig.h:41
mirtk::FreeFormTransformationRungeKutta::dkdp
static void dkdp(Matrix &dk, const Matrix &Dv, const Matrix &dx, const double dv[3], double h)
Helper for computation of Jacobian w.r.t control point.
Definition: FreeFormTransformationRungeKutta.h:66
mirtk::FreeFormTransformationRungeKutta::dkdx
static void dkdx(Matrix &dk, const Matrix &Dv, const Matrix &dx, double h)
Helper for computation of Jacobian w.r.t spatial coordinates.
Definition: FreeFormTransformationRungeKutta.h:51
mirtk::Vector3D::_z
T _z
The z component.
Definition: Vector3D.h:60
mirtk::Vector3D::_y
T _y
The y component.
Definition: Vector3D.h:59
mirtk::TransformationJacobian::add
TransformationJacobian & add(const TransformationJacobian &, double)
Add scaled transformation Jacobian to this Jacobian matrix.
Definition: TransformationJacobian.h:362
mirtk::FreeFormTransformationExplicitRungeKutta::BT
TButcherTableau BT
Short-hand for Butcher tableau template argument.
Definition: FreeFormTransformationRungeKutta.h:108
mirtk::Vector3D::_x
T _x
The x component.
Definition: Vector3D.h:58
mirtk::FreeFormTransformationEmbeddedRungeKutta::BT
TButcherTableau BT
Short-hand for Butcher tableau template argument.
Definition: FreeFormTransformationRungeKutta.h:366
mirtk::Transform
void Transform(Array< T > &values, UnaryOperation op)
Apply unary operation for each array element in-place.
Definition: Algorithm.h:91
© 2013-2018 Imperial College London, Andreas Schuh.