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 dkdx(Matrix & dk,const Matrix & Dv,const Matrix & dx,double h)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 dkdp(Matrix & dk,const Matrix & Dv,const Matrix & dx,const double dv[3],double h)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 // ------------------------------------------------------------------------- Transform(const TFreeFormTransformation * v,double & x,double & y,double & z,double t1,double t2,double dt)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 // ------------------------------------------------------------------------- Jacobian(const TFreeFormTransformation * v,Matrix & jac,double & x,double & y,double & z,double t1,double t2,double dt)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 // ------------------------------------------------------------------------- JacobianDOFs(const TFreeFormTransformation * v,Matrix & jac,int ci,int cj,int ck,int cl,double & x,double & y,double & z,double t1,double t2,double dt)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 // ------------------------------------------------------------------------- JacobianDOFs(const TFreeFormTransformation * v,TransformationJacobian & jac,double & x,double & y,double & z,double t1,double t2,double dt)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 // ------------------------------------------------------------------------- Transform(const TFreeFormTransformation * v,double & x,double & y,double & z,double t1,double t2,double mindt,double maxdt,double tol)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 // ------------------------------------------------------------------------- Jacobian(const TFreeFormTransformation * v,Matrix & jac,double & x,double & y,double & z,double t1,double t2,double mindt,double maxdt,double tol)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 // ------------------------------------------------------------------------- JacobianDOFs(const TFreeFormTransformation * v,Matrix & jac,int ci,int cj,int ck,int cl,double & x,double & y,double & z,double t1,double t2,double mindt,double maxdt,double tol)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 // ------------------------------------------------------------------------- JacobianDOFs(const TFreeFormTransformation * v,TransformationJacobian & jac,double & x,double & y,double & z,double t1,double t2,double mindt,double maxdt,double tol)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 824