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