1 /*=========================================================================
2  *
3  *  Copyright Insight Software Consortium
4  *
5  *  Licensed under the Apache License, Version 2.0 (the "License");
6  *  you may not use this file except in compliance with the License.
7  *  You may obtain a copy of the License at
8  *
9  *         http://www.apache.org/licenses/LICENSE-2.0.txt
10  *
11  *  Unless required by applicable law or agreed to in writing, software
12  *  distributed under the License is distributed on an "AS IS" BASIS,
13  *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14  *  See the License for the specific language governing permissions and
15  *  limitations under the License.
16  *
17  *=========================================================================*/
18 #ifndef itkFEMSolverCrankNicolson_hxx
19 #define itkFEMSolverCrankNicolson_hxx
20 
21 #include "itkFEMSolverCrankNicolson.h"
22 
23 #include "itkFEMLoadNode.h"
24 #include "itkFEMLoadElementBase.h"
25 #include "itkFEMLoadBC.h"
26 #include "itkFEMLoadBCMFC.h"
27 #include "itkFEMLoadLandmark.h"
28 #include "itkMath.h"
29 
30 namespace itk
31 {
32 namespace fem
33 {
34 #define TOTE
35 
36 
37 template <unsigned int VDimension>
38 SolverCrankNicolson<VDimension>
SolverCrankNicolson()39 ::SolverCrankNicolson()
40 {
41   m_TimeStep = 0.5;
42   m_Rho = 1.;
43   m_Alpha = 0.5;
44   // BUG FIXME NOT SURE IF SOLVER IS USING VECTOR INDEX 1 FOR BCs
45   m_ForceTIndex = 0;                      // vector
46   m_ForceTMinus1Index = 2;                // vector
47   m_SolutionVectorTMinus1Index = 3;       // vector
48   m_DiffMatrixBySolutionTMinus1Index = 4; // vector
49   m_ForceTotalIndex = 5;                  // vector
50   m_SolutionTIndex = 0;                   // solution
51   m_TotalSolutionIndex = 1;               // solution
52   m_SolutionTMinus1Index = 2;             // solution
53   m_SumMatrixIndex = 0;                   // matrix
54   m_DifferenceMatrixIndex = 1;            // matrix
55   m_CurrentMaxSolution = 1.0;
56   m_UseMassMatrix = true;
57   m_Iterations = 0;
58 }
59 
60 template <unsigned int VDimension>
61 void
62 SolverCrankNicolson<VDimension>
InitializeForSolution()63 ::InitializeForSolution()
64 {
65   this->m_LinearSystem->SetSystemOrder(this->m_NGFN + this->m_NMFC);
66   this->m_LinearSystem->SetNumberOfVectors(6);
67   this->m_LinearSystem->SetNumberOfSolutions(3);
68   this->m_LinearSystem->SetNumberOfMatrices(2);
69   this->m_LinearSystem->InitializeMatrix(m_SumMatrixIndex);
70   this->m_LinearSystem->InitializeMatrix(m_DifferenceMatrixIndex);
71   this->m_LinearSystem->InitializeVector(m_ForceTIndex);
72   this->m_LinearSystem->InitializeVector(m_ForceTotalIndex);
73   this->m_LinearSystem->InitializeVector(m_ForceTMinus1Index);
74   this->m_LinearSystem->InitializeVector(m_SolutionVectorTMinus1Index);
75   this->m_LinearSystem->InitializeVector(m_DiffMatrixBySolutionTMinus1Index);
76   this->m_LinearSystem->InitializeSolution(m_SolutionTIndex);
77   this->m_LinearSystem->InitializeSolution(m_TotalSolutionIndex);
78   this->m_LinearSystem->InitializeSolution(m_SolutionTMinus1Index);
79 }
80 
81 template <unsigned int VDimension>
82 void
83 SolverCrankNicolson<VDimension>
AssembleKandM()84 ::AssembleKandM()
85 {
86   // If no DOFs exist in a system, we have nothing to do
87   if( this->m_NGFN <= 0 )
88     {
89     return;
90     }
91 
92   Float lhsval;
93   Float rhsval;
94   this->m_NMFC = 0; // number of MFC in a system
95 
96   // Temporary storage for pointers to LoadBCMFC objects
97   using MFCArray = std::vector<LoadBCMFC::Pointer>;
98   MFCArray mfcLoads;
99 
100   //
101   // Before we can start the assembly procedure, we need to know,
102   // how many boundary conditions (MFCs) there are in a system.
103   //
104   mfcLoads.clear();
105 
106   int numLoads = this->m_FEMObject->GetLoadContainer()->Size();
107   for( int l = 0; l < numLoads; l++ )
108     {
109     if( LoadBCMFC::Pointer l1 = dynamic_cast<LoadBCMFC *>( this->m_FEMObject->GetLoad(l).GetPointer() ) )
110       {
111       // Store the index of an LoadBCMFC object for later
112       l1->SetIndex(this->m_NMFC);
113       mfcLoads.push_back(l1);
114       // Increase the number of MFC
115       this->m_NMFC++;
116       }
117     }
118   //
119   // Now we can assemble the master stiffness matrix
120   // from element stiffness matrices.
121   //
122   InitializeForSolution();
123 
124   //
125   // Step over all elements
126   //
127   int numElements = this->m_FEMObject->GetElementContainer()->Size();
128   for( int e = 0; e < numElements;  e++ )
129     {
130     vnl_matrix<Float> Ke;
131     // Copy the element stiffness matrix for faster access.
132     this->m_FEMObject->GetElement(e)->GetStiffnessMatrix(Ke);
133     vnl_matrix<Float> Me;
134     // Copy the element mass matrix for faster access.
135     this->m_FEMObject->GetElement(e)->GetMassMatrix(Me);
136     // ... same for element DOF
137     int Ne = this->m_FEMObject->GetElement(e)->GetNumberOfDegreesOfFreedom();
138 
139     Me = Me * m_Rho;
140     // Step over all rows in in element matrix
141     for( int j = 0; j < Ne; j++ )
142       {
143       // Step over all columns in in element matrix
144       for( int k = 0; k < Ne; k++ )
145         {
146         // Error checking. all GFN should be =>0 and <NGFN
147         if( this->m_FEMObject->GetElement(e)->GetDegreeOfFreedom(j) >= this->m_NGFN
148             || this->m_FEMObject->GetElement(e)->GetDegreeOfFreedom(k) >= this->m_NGFN )
149           {
150           throw FEMExceptionSolution(__FILE__, __LINE__, "SolverCrankNicolson::AssembleKandM()", "Illegal GFN!");
151           }
152 
153         // Here we finally update the corresponding element
154         // in the master stiffness matrix. We first check if
155         // element in Ke is zero, to prevent zeros from being
156         // allocated in sparse matrix.
157         //
158         if( Math::NotExactlyEquals(Ke(j, k), Float(0.0)) || Math::NotExactlyEquals(Me(j, k), Float(0.0)) )
159           {
160           // Left hand side matrix
161           lhsval = ( Me(j, k) + m_Alpha * m_TimeStep * Ke(j, k) );
162           this->m_LinearSystem->AddMatrixValue( this->m_FEMObject->GetElement(e)->GetDegreeOfFreedom(j),
163                                       this->m_FEMObject->GetElement(e)->GetDegreeOfFreedom(k),
164                                       lhsval, m_SumMatrixIndex );
165           // Right hand side matrix
166           rhsval = ( Me(j, k) - ( 1. - m_Alpha ) * m_TimeStep * Ke(j, k) );
167           this->m_LinearSystem->AddMatrixValue( this->m_FEMObject->GetElement(e)->GetDegreeOfFreedom(j),
168                                       this->m_FEMObject->GetElement(e)->GetDegreeOfFreedom(k),
169                                       rhsval, m_DifferenceMatrixIndex );
170           }
171         }
172       }
173     }
174   //
175   // Step over all the loads to add the landmark contributions to the
176   // appropriate place in the stiffness matrix
177   //
178   // int numLoads = m_FEMObject->GetLoadContainer()->Size();
179   for( int l2 = 0; l2 < numLoads; l2++ )
180     {
181     if( LoadLandmark::Pointer l3 = dynamic_cast<LoadLandmark *>( this->m_FEMObject->GetLoad(l2).GetPointer() ) )
182       {
183       Element::ConstPointer ep = (l3->GetElementArray()[0]);
184       Element::MatrixType   Le;
185       ep->GetLandmarkContributionMatrix(l3->GetEta(), Le);
186       int Ne = ep->GetNumberOfDegreesOfFreedom();
187       // Step over all rows in element matrix
188       for( int j = 0; j < Ne; j++ )
189         {
190         // Step over all columns in element matrix
191         for( int k = 0; k < Ne; k++ )
192           {
193           // error checking, all GFN should be >=0 and < NGFN
194           if( ep->GetDegreeOfFreedom(j) >= this->m_NGFN
195               || ep->GetDegreeOfFreedom(k) >= this->m_NGFN )
196             {
197             throw FEMExceptionSolution(__FILE__, __LINE__, "SolverCrankNicolson::AssembleKandM()", "Illegal GFN!");
198             }
199 
200           // Now update the corresponding element in the master
201           // stiffness matrix and omit the zeros for the sparseness
202           if( Math::NotExactlyEquals(Le(j, k), Float(0.0)) )
203             {
204             // lhs matrix
205             lhsval = m_Alpha * m_TimeStep * Le(j, k);
206             this->m_LinearSystem->AddMatrixValue(ep->GetDegreeOfFreedom(j),
207                                        ep->GetDegreeOfFreedom(k),
208                                        lhsval, m_SumMatrixIndex);
209             // rhs matrix
210             rhsval = ( 1. - m_Alpha ) * m_TimeStep * Le(j, k);
211             this->m_LinearSystem->AddMatrixValue(ep->GetDegreeOfFreedom(j),
212                                        ep->GetDegreeOfFreedom(k),
213                                        rhsval, m_DifferenceMatrixIndex);
214             }
215           }
216         }
217       }
218     }
219 
220   // Step over all types of BCs
221   this->ApplyBC(); // BUG -- are BCs applied appropriately to the problem?
222 }
223 
224 template <unsigned int VDimension>
225 void
226 SolverCrankNicolson<VDimension>
AssembleFforTimeStep(int dim)227 ::AssembleFforTimeStep(int dim)
228 {
229   // If no DOFs exist in a system, we have nothing to do
230   if( this->m_NGFN <= 0 )
231     {
232     return;
233     }
234 
235   this->AssembleF(dim); // assuming assemblef uses index 0 in vector!
236 
237   using BCTermType = std::map<Element::DegreeOfFreedomIDType, Float>;
238   BCTermType bcterm;
239 
240   int numLoads = this->m_FEMObject->GetLoadContainer()->Size();
241   for( int l2 = 0; l2 < numLoads; l2++ )
242     {
243     if( LoadBC::Pointer l1 = dynamic_cast<LoadBC *>( this->m_FEMObject->GetLoad(l2).GetPointer()  ) )
244       {
245       bcterm[l1->GetElement()->GetDegreeOfFreedom( l1->GetDegreeOfFreedom() )] =
246         l1->GetValue()[dim];
247       }
248     } // end for LoadArray::iterator l
249   // Now set the solution t_minus1 vector to fit the BCs
250   for(auto & q : bcterm)
251     {
252     this->m_LinearSystem->SetVectorValue(q.first, 0.0, m_SolutionVectorTMinus1Index); // FIXME?
253     this->m_LinearSystem->SetSolutionValue(q.first, 0.0, m_SolutionTMinus1Index);     // FIXME?
254     this->m_LinearSystem->SetSolutionValue(q.first, 0.0, m_TotalSolutionIndex);
255     }
256 
257   this->m_LinearSystem->MultiplyMatrixVector(m_DiffMatrixBySolutionTMinus1Index,
258                                    m_DifferenceMatrixIndex, m_SolutionVectorTMinus1Index);
259   for( unsigned int index = 0; index < this->m_NGFN; index++ )
260     {
261     RecomputeForceVector(index);
262     }
263   // Now set the solution and force vector to fit the BCs
264   for(auto & q : bcterm)
265     {
266     this->m_LinearSystem->SetVectorValue(q.first, q.second, m_ForceTIndex);
267     }
268 }
269 
270 template <unsigned int VDimension>
271 void
272 SolverCrankNicolson<VDimension>
RecomputeForceVector(unsigned int index)273 ::RecomputeForceVector(unsigned int index)
274 {     //
275   Float ft   = this->m_LinearSystem->GetVectorValue(index, m_ForceTIndex);
276   Float ftm1 = this->m_LinearSystem->GetVectorValue(index, m_ForceTMinus1Index);
277   Float utm1 = this->m_LinearSystem->GetVectorValue(index, m_DiffMatrixBySolutionTMinus1Index);
278   Float f = m_TimeStep * ( m_Alpha * ft + ( 1. - m_Alpha ) * ftm1 ) + utm1;
279 
280   this->m_LinearSystem->SetVectorValue(index, f, m_ForceTIndex);
281 }
282 
283 // ----------------------------------------------------------------------------
284 template <unsigned int VDimension>
285 void
286 SolverCrankNicolson<VDimension>
GenerateData()287 ::GenerateData()
288 {
289   // Call Solver
290   this->RunSolver();
291 }
292 
293 template <unsigned int VDimension>
294 void
295 SolverCrankNicolson<VDimension>
RunSolver()296 ::RunSolver()
297 {
298   if( m_Iterations == 0 )
299     {
300     if( m_UseMassMatrix )
301       {
302       this->AssembleKandM();
303       }
304     else
305       {
306       this->InitializeForSolution();
307       this->AssembleK();
308       }
309     }
310 
311   if( m_UseMassMatrix )
312     {
313     this->AssembleFforTimeStep();
314     }
315   else
316     {
317     this->AssembleF();
318     }
319 
320   // FIXME - must verify that this is correct use of wrapper
321   // FIXME Initialize the solution vector
322   this->m_LinearSystem->InitializeSolution(m_SolutionTIndex);
323   this->m_LinearSystem->Solve();
324 
325   m_Iterations++;
326   // call this externally    AddToDisplacements();
327 }
328 
329 template <unsigned int VDimension>
330 void
331 SolverCrankNicolson<VDimension>
FindBracketingTriplet(Float * a,Float * b,Float * c)332 ::FindBracketingTriplet(Float *a, Float *b, Float *c)
333 {
334   // In 1-D domain, we want to find a < b < c , s.t.  f(b) < f(a) && f(b) < f(c)
335   // See Numerical Recipes
336 
337   Float Gold = 1.618034;
338   Float Glimit = 100.0;
339   Float Tiny = 1.e-20;
340 
341   Float ax, bx, cx;
342 
343   ax = 0.0; bx = 1.;
344   Float fc;
345   Float fa = std::fabs( EvaluateResidual(ax) );
346   Float fb = std::fabs( EvaluateResidual(bx) );
347 
348   Float ulim, u, r, q, fu, dum;
349 
350   if( fb > fa )
351     {
352     dum = ax; ax = bx; bx = dum;
353     dum = fb; fb = fa; fa = dum;
354     }
355 
356   cx = bx + Gold * ( bx - ax );  // first guess for c - the 3rd pt needed to
357                                  // bracket the min
358   fc = std::fabs( EvaluateResidual(cx) );
359 
360   while( fb > fc /*&& std::fabs(ax) < 3. && std::fabs(bx) < 3. && std::fabs(cx) <
361                     3.*/)
362     {
363     r = ( bx - ax ) * ( fb - fc );
364     q = ( bx - cx ) * ( fb - fa );
365     Float denom = ( 2.0 * GSSign(GSMax(std::fabs(q - r), Tiny), q - r) );
366     u = ( bx ) - ( ( bx - cx ) * q - ( bx - ax ) * r ) / denom;
367     ulim = bx + Glimit * ( cx - bx );
368     if( ( bx - u ) * ( u - cx ) > 0.0 )
369       {
370       fu = std::fabs( EvaluateResidual(u) );
371       if( fu < fc )
372         {
373         ax = bx;
374         bx = u;
375         *a = ax; *b = bx; *c = cx;
376         return;
377         }
378       else if( fu > fb )
379         {
380         cx = u;
381         *a = ax; *b = bx; *c = cx;
382         return;
383         }
384 
385       u = cx + Gold * ( cx - bx );
386       fu = std::fabs( EvaluateResidual(u) );
387       }
388     else if( ( cx - u ) * ( u - ulim ) > 0.0 )
389       {
390       fu = std::fabs( EvaluateResidual(u) );
391       if( fu < fc )
392         {
393         bx = cx; cx = u; u = cx + Gold * ( cx - bx );
394         fb = fc; fc = fu; fu = std::fabs( EvaluateResidual(u) );
395         }
396       }
397     else if( ( u - ulim ) * ( ulim - cx ) >= 0.0 )
398       {
399       u = ulim;
400       fu = std::fabs( EvaluateResidual(u) );
401       }
402     else
403       {
404       u = cx + Gold * ( cx - bx );
405       fu = std::fabs( EvaluateResidual(u) );
406       }
407 
408     ax = bx; bx = cx; cx = u;
409     fa = fb; fb = fc; fc = fu;
410     }
411 
412   if( std::fabs(ax) > 1.e3  || std::fabs(bx) > 1.e3 || std::fabs(cx) > 1.e3 )
413     {
414     ax = -2.0;  bx = 1.0;  cx = 2.0;
415     } // to avoid crazy numbers caused by bad bracket (u goes nuts)
416 
417   *a = ax; *b = bx; *c = cx;
418 }
419 
420 template <unsigned int VDimension>
421 Element::Float
422 SolverCrankNicolson<VDimension>
BrentsMethod(Float tol,unsigned int MaxIters)423 ::BrentsMethod(Float tol, unsigned int MaxIters)
424 {
425   // We should now have a, b and c, as well as f(a), f(b), f(c),
426   // where b gives the minimum energy position;
427 
428   Float CGOLD = 0.3819660;
429   Float ZEPS = 1.e-10;
430 
431   Float ax = 0.0, bx = 1.0, cx = 2.0;
432 
433   FindBracketingTriplet(&ax, &bx, &cx);
434 
435   Float xmin;
436 
437   unsigned int iter;
438 
439   Float a, b, d = 0., etemp, fu, fv, fw, fx, p, q, r, tol1, tol2, u, v, w, x, xm;
440 
441   Float e = 0.0;  // the distance moved on the step before last;
442 
443   a = ( ( ax  < cx ) ? ax : cx );
444   b = ( ( ax  > cx ) ? ax : cx );
445 
446   x = w = v = bx;
447   fw = fv = fx = std::fabs( EvaluateResidual(x) );
448   for( iter = 1; iter <= MaxIters; iter++ )
449     {
450     xm = 0.5 * ( a + b );
451     tol2 = 2.0 * ( tol1 = tol * std::fabs(x) + ZEPS );
452     if( std::fabs(x - xm) <= ( tol2 - 0.5 * ( b - a ) ) )
453       {
454       xmin = x;
455       SetEnergyToMin(xmin);
456       return fx;
457       }
458 
459     if( std::fabs(e) > tol1 )
460       {
461       r = ( x - w ) * ( fx - fv );
462       q = ( x - v ) * ( fx - fw );
463       p = ( x - v ) * q - ( x - w ) * r;
464       q = 2.0 * ( q - r );
465       if( q > 0.0 )
466         {
467         p = -1. * p;
468         }
469       q = std::fabs(q);
470       etemp = e;
471       e = d;
472       if( std::fabs(p) >= std::fabs(0.5 * q * etemp) || p <= q * ( a - x ) || p >= q * ( b - x ) )
473         {
474         d = CGOLD * ( e = ( x >= xm ? a - x : b - x ) );
475         }
476       else
477         {
478         if( q == 0.0 )
479           {
480           q = q + ZEPS;
481           }
482         d = p / q;
483         u = x + d;
484         if( u - a < tol2 || b - u < tol2 )
485           {
486           d = GSSign(tol1, xm - x);
487           }
488         }
489       }
490     else
491       {
492       d = CGOLD * ( e = ( x >= xm ? a - x : b - x ) );
493       }
494 
495     u = ( std::fabs(d) >= tol1 ? x + d : x + GSSign(tol1, d) );
496     fu = std::fabs( EvaluateResidual(u) );
497     if( fu <= fx )
498       {
499       if( u >= x )
500         {
501         a = x;
502         }
503       else
504         {
505         b = x;
506         }
507       v = w; w = x; x = u;
508       fv = fw; fw = fx; fx = fu;
509       }
510     else
511       {
512       if( u < x )
513         {
514         a = u;
515         }
516       else
517         {
518         b = u;
519         }
520       if( fu <= fw || Math::ExactlyEquals(w, x) )
521         {
522         v = w;
523         w = u;
524         fv = fw;
525         fw = fu;
526         }
527       else if( fu <= fv || Math::ExactlyEquals(v, x) || Math::ExactlyEquals(v, w) )
528         {
529         v = u;
530         fv = fu;
531         }
532       }
533     }
534   xmin = x;
535   SetEnergyToMin(xmin);
536   return fx;
537 }
538 
539 template <unsigned int VDimension>
540 Element::Float
541 SolverCrankNicolson<VDimension>
GoldenSection(Float tol,unsigned int MaxIters)542 ::GoldenSection(Float tol, unsigned int MaxIters)
543 {
544   // We should now have a, b and c, as well as f(a), f(b), f(c),
545   // where b gives the minimum energy position;
546 
547   Float ax, bx, cx;
548 
549   FindBracketingTriplet(&ax, &bx, &cx);
550   Float xmin, fmin;
551 
552   Float f1, f2, x0, x1, x2, x3;
553 
554   Float R = 0.6180339;
555   Float C = ( 1.0 - R );
556 
557   x0 = ax;
558   x3 = cx;
559   if( std::fabs(cx - bx) > std::fabs(bx - ax) )
560     {
561     x1 = bx;
562     x2 = bx + C * ( cx - bx );
563     }
564   else
565     {
566     x2 = bx;
567     x1 = bx - C * ( bx - ax );
568     }
569   f1 = std::fabs( EvaluateResidual(x1) );
570   f2 = std::fabs( EvaluateResidual(x2) );
571   unsigned int iters = 0;
572   while( std::fabs(x3 - x0) > tol * ( std::fabs(x1) + std::fabs(x2) ) && iters < MaxIters )
573     {
574     iters++;
575     if( f2 < f1 )
576       {
577       x0 = x1; x1 = x2; x2 = R * x1 + C * x3;
578       f1 = f2; f2 = std::fabs( EvaluateResidual(x2) );
579       }
580     else
581       {
582       x3 = x2; x2 = x1; x1 = R * x2 + C * x0;
583       f2 = f1; f1 = std::fabs( EvaluateResidual(x1) );
584       }
585     }
586 
587   if( f1 < f2 )
588     {
589     xmin = x1;
590     fmin = f1;
591     }
592   else
593     {
594     xmin = x2;
595     fmin = f2;
596     }
597 
598   SetEnergyToMin(xmin);
599   return fmin;
600 }
601 
602 template <unsigned int VDimension>
603 void
604 SolverCrankNicolson<VDimension>
SetEnergyToMin(Float xmin)605 ::SetEnergyToMin(Float xmin)
606 {
607   for( unsigned int j = 0; j < this->m_NGFN; j++ )
608     {
609     Float SolVal;
610     Float FVal;
611 #ifdef LOCE
612     SolVal = xmin * this->m_LinearSystem->GetSolutionValue(j, m_SolutionTIndex)
613       + ( 1. - xmin ) * this->m_LinearSystem->GetSolutionValue(j, m_SolutionTMinus1Index);
614 
615     FVal = xmin * this->m_LinearSystem->GetVectorValue(j, m_ForceTIndex)
616       + ( 1. - xmin ) * this->m_LinearSystem->GetVectorValue(j, m_ForceTMinus1Index);
617 #endif
618 #ifdef TOTE
619     SolVal = xmin * this->m_LinearSystem->GetSolutionValue(j, m_SolutionTIndex); // FOR TOT E
620     FVal = xmin * this->m_LinearSystem->GetVectorValue(j, m_ForceTIndex);
621 #endif
622     this->m_LinearSystem->SetSolutionValue(j, SolVal, m_SolutionTIndex);
623     this->m_LinearSystem->SetVectorValue(j, FVal, m_ForceTIndex);
624     }
625 }
626 
627 template <unsigned int VDimension>
628 Element::Float
629 SolverCrankNicolson<VDimension>
GetDeformationEnergy(Float t)630 ::GetDeformationEnergy(Float t)
631 {
632   Float DeformationEnergy = 0.0;
633   Float iSolVal, jSolVal;
634   for( unsigned int i = 0; i < this->m_NGFN; i++ )
635     {
636 // forming  U^T F
637 #ifdef LOCE
638     iSolVal = t * ( this->m_LinearSystem->GetSolutionValue(i, m_SolutionTIndex) )
639       + ( 1. - t ) * this->m_LinearSystem->GetSolutionValue(i, m_SolutionTMinus1Index);
640 #endif
641 #ifdef TOTE
642     iSolVal = t * ( this->m_LinearSystem->GetSolutionValue(i, m_SolutionTIndex) );
643 #endif
644 // forming U^T K U
645     Float TempRowVal = 0.0;
646     for( unsigned int j = 0; j < this->m_NGFN; j++ )
647       {
648 #ifdef LOCE
649       jSolVal = t * ( this->m_LinearSystem->GetSolutionValue(j, m_SolutionTIndex) )
650         + ( 1. - t ) * this->m_LinearSystem->GetSolutionValue(j, m_SolutionTMinus1Index);
651 #endif
652 #ifdef TOTE
653       jSolVal = t * ( this->m_LinearSystem->GetSolutionValue(j, m_SolutionTIndex) )
654         + this->m_LinearSystem->GetSolutionValue(j, m_TotalSolutionIndex);         // FOR TOT E
655 #endif
656       TempRowVal += this->m_LinearSystem->GetMatrixValue(i, j, m_SumMatrixIndex) * jSolVal;
657       }
658     DeformationEnergy += iSolVal * TempRowVal;
659     }
660   return DeformationEnergy;
661 }
662 
663 template <unsigned int VDimension>
664 Element::Float
665 SolverCrankNicolson<VDimension>
EvaluateResidual(Float t)666 ::EvaluateResidual(Float t)
667 {
668   Float ForceEnergy = 0.0, FVal = 0.0;
669   Float DeformationEnergy = 0.0;
670   Float iSolVal, jSolVal;
671   for( unsigned int i = 0; i < this->m_NGFN; i++ )
672     {
673 // forming  U^T F
674 #ifdef LOCE
675     iSolVal = t * ( this->m_LinearSystem->GetSolutionValue(i, m_SolutionTIndex) )
676       + ( 1. - t ) * this->m_LinearSystem->GetSolutionValue(i, m_SolutionTMinus1Index);
677     FVal = this->m_LinearSystem->GetVectorValue(i, m_ForceTIndex);
678     FVal = t * FVal + ( 1. - t ) * this->m_LinearSystem->GetVectorValue(i, m_ForceTMinus1Index);
679 
680     ForceEnergy += iSolVal * FVal;
681 #endif
682 #ifdef TOTE
683     FVal = FVal + 0.0;
684     iSolVal = t * ( this->m_LinearSystem->GetSolutionValue(i, m_SolutionTIndex) )
685       + this->m_LinearSystem->GetSolutionValue(i, m_TotalSolutionIndex);         // FOR TOT E
686     ForceEnergy += iSolVal * ( this->m_LinearSystem->GetVectorValue(i, m_ForceTotalIndex)
687                                + t * this->m_LinearSystem->GetVectorValue(i, m_ForceTIndex) ); //
688     // FOR
689     // TOT
690     // E
691 #endif
692 // forming U^T K U
693     Float TempRowVal = 0.0;
694     for( unsigned int j = 0; j < this->m_NGFN; j++ )
695       {
696 #ifdef LOCE
697       jSolVal = t * ( this->m_LinearSystem->GetSolutionValue(j, m_SolutionTIndex) )
698         + ( 1. - t ) * this->m_LinearSystem->GetSolutionValue(j, m_SolutionTMinus1Index);
699 #endif
700 #ifdef TOTE
701       jSolVal = t * ( this->m_LinearSystem->GetSolutionValue(j, m_SolutionTIndex) )
702         + this->m_LinearSystem->GetSolutionValue(j, m_TotalSolutionIndex);         // FOR TOT E
703 #endif
704       TempRowVal += this->m_LinearSystem->GetMatrixValue(i, j, m_SumMatrixIndex) * jSolVal;
705       }
706     DeformationEnergy += iSolVal * TempRowVal;
707     }
708   auto Energy = (Float)std::fabs(DeformationEnergy - ForceEnergy);
709   return Energy;
710 }
711 
712 template <unsigned int VDimension>
713 void
714 SolverCrankNicolson<VDimension>
AddToDisplacements(Float optimum)715 ::AddToDisplacements(Float optimum)
716 {
717   //
718   // Copy the resulting displacements from
719   // solution vector back to node objects.
720   //
721   Float maxs = 0.0, CurrentTotSolution, CurrentSolution, CurrentForce;
722   Float mins2 = 0.0, maxs2 = 0.0;
723   Float absmax = 0.0;
724   for( unsigned int i = 0; i < this->m_NGFN; i++ )
725     {
726 #ifdef TOTE
727     CurrentSolution = this->m_LinearSystem->GetSolutionValue(i, m_SolutionTIndex);
728 #endif
729     if( CurrentSolution < mins2 )
730       {
731       mins2 = CurrentSolution;
732       }
733     else if( CurrentSolution > maxs2 )
734       {
735       maxs2 = CurrentSolution;
736       }
737     if( std::fabs(CurrentSolution) > absmax )
738       {
739       absmax = std::fabs(CurrentSolution);
740       }
741 
742 //  note: set rather than add - i.e. last solution of system not total solution
743 #ifdef LOCE
744     CurrentSolution = optimum * this->m_LinearSystem->GetSolutionValue(i, m_SolutionTIndex)
745       + ( 1. - optimum ) * this->m_LinearSystem->GetVectorValue(i, m_SolutionVectorTMinus1Index);
746     CurrentForce = optimum * this->m_LinearSystem->GetVectorValue(i, m_ForceTIndex)
747       + ( 1. - optimum ) * this->m_LinearSystem->GetVectorValue(i, m_ForceTMinus1Index);
748     this->m_LinearSystem->SetVectorValue(i, CurrentSolution, m_SolutionVectorTMinus1Index);
749     this->m_LinearSystem->SetSolutionValue(i, CurrentSolution, m_SolutionTMinus1Index);
750     this->m_LinearSystem->SetVectorValue(i, CurrentForce, m_ForceTMinus1Index);  // now set t
751     // minus one
752     // force vector
753     // correctly
754 #endif
755 #ifdef TOTE
756     CurrentSolution = optimum * CurrentSolution;
757     CurrentForce = optimum * this->m_LinearSystem->GetVectorValue(i, m_ForceTIndex);
758     this->m_LinearSystem->SetVectorValue(i, CurrentSolution, m_SolutionVectorTMinus1Index); // FOR
759     // TOT
760     // E
761     this->m_LinearSystem->SetSolutionValue(i, CurrentSolution, m_SolutionTMinus1Index);     // FOR
762     // TOT
763     // E
764     this->m_LinearSystem->SetVectorValue(i, CurrentForce, m_ForceTMinus1Index);
765 #endif
766 
767     this->m_LinearSystem->AddSolutionValue(i, CurrentSolution, m_TotalSolutionIndex);
768     this->m_LinearSystem->AddVectorValue(i, CurrentForce, m_ForceTotalIndex);
769     CurrentTotSolution = this->m_LinearSystem->GetSolutionValue(i, m_TotalSolutionIndex);
770 
771     if( std::fabs(CurrentTotSolution) > maxs )
772       {
773       maxs = std::fabs(CurrentTotSolution);
774       }
775     }
776 
777   m_CurrentMaxSolution = absmax;
778 }
779 
780 template <unsigned int VDimension>
781 void
782 SolverCrankNicolson<VDimension>
PrintMinMaxOfSolution()783 ::PrintMinMaxOfSolution()
784 {
785   //
786   // Copy the resulting displacements from
787   // solution vector back to node objects.
788   //
789   Float mins = 0.0, maxs = 0.0;
790   Float mins2 = 0.0, maxs2 = 0.0;
791   for( unsigned int i = 0; i < this->m_NGFN; i++ )
792     {
793     Float CurrentSolution = this->m_LinearSystem->GetSolutionValue(i, m_SolutionTIndex);
794     if( CurrentSolution < mins2 )
795       {
796       mins2 = CurrentSolution;
797       }
798     else if( CurrentSolution > maxs2 )
799       {
800       maxs2 = CurrentSolution;
801       }
802     CurrentSolution = this->m_LinearSystem->GetSolutionValue(i, m_TotalSolutionIndex);
803     if( CurrentSolution < mins )
804       {
805       mins = CurrentSolution;
806       }
807     else if( CurrentSolution > maxs )
808       {
809       maxs = CurrentSolution;
810       }
811     }
812 }
813 
814 template <unsigned int VDimension>
815 void
816 SolverCrankNicolson<VDimension>
AverageLastTwoDisplacements(Float t)817 ::AverageLastTwoDisplacements(Float t)
818 {
819   Float maxs = 0.0;
820   for( unsigned int i = 0; i < this->m_NGFN; i++ )
821     {
822     Float temp = this->m_LinearSystem->GetSolutionValue(i, m_SolutionTIndex);
823     Float temp2 = this->m_LinearSystem->GetSolutionValue(i, m_SolutionTMinus1Index);
824     Float newsol = t * ( temp ) + ( 1. - t ) * temp2;
825     this->m_LinearSystem->SetSolutionValue(i, newsol, m_SolutionTMinus1Index);
826     this->m_LinearSystem->SetVectorValue(i, newsol, m_SolutionVectorTMinus1Index);
827     this->m_LinearSystem->SetSolutionValue(i, newsol, m_SolutionTIndex);
828     if( newsol > maxs )
829       {
830       maxs = newsol;
831       }
832     }
833 }
834 
835 template <unsigned int VDimension>
836 void
837 SolverCrankNicolson<VDimension>
ZeroVector(int which)838 ::ZeroVector(int which)
839 {
840   for( unsigned int i = 0; i < this->m_NGFN; i++ )
841     {
842     this->m_LinearSystem->SetVectorValue(i, 0.0, which);
843     }
844 }
845 
846 template <unsigned int VDimension>
847 void
848 SolverCrankNicolson<VDimension>
PrintDisplacements()849 ::PrintDisplacements()
850 {
851   std::cout <<  " printing current displacements " << std::endl;
852   for( unsigned int i = 0; i < this->m_NGFN; i++ )
853     {
854     std::cout << this->m_LinearSystem->GetSolutionValue(i, m_TotalSolutionIndex) << std::endl;
855     }
856 }
857 
858 template <unsigned int VDimension>
859 void
860 SolverCrankNicolson<VDimension>
PrintForce()861 ::PrintForce()
862 {
863   std::cout <<  " printing current forces " << std::endl;
864   for( unsigned int i = 0; i < this->m_NGFN; i++ )
865     {
866     std::cout << this->m_LinearSystem->GetVectorValue(i, m_ForceTIndex) << std::endl;
867     }
868 }
869 
870 }  // end namespace fem
871 }  // end namespace itk
872 
873 #endif
874