itkfemsolvercranknicolson.cxx

DTMK软件开发包,此为开源软件,是一款很好的医学图像开发资源.

/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    $RCSfile: itkFEMSolverCrankNicolson.cxx,v $
  Language:  C++
  Date:      $Date: 2006-03-19 04:37:20 $
  Version:   $Revision: 1.38 $

  Copyright (c) Insight Software Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even 
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

// disable debug warnings in MS compiler
#ifdef _MSC_VER
#pragma warning(disable: 4786)
#endif

#include "itkFEMSolverCrankNicolson.h"

#include "itkFEMLoadNode.h"
#include "itkFEMLoadElementBase.h"
#include "itkFEMLoadBC.h"
#include "itkFEMLoadBCMFC.h"
#include "itkFEMLoadLandmark.h"

namespace itk {
namespace fem {

#define TOTE

void SolverCrankNicolson::InitializeForSolution() 
{
  m_ls->SetSystemOrder(NGFN+NMFC);
  m_ls->SetNumberOfVectors(6);
  m_ls->SetNumberOfSolutions(3);
  m_ls->SetNumberOfMatrices(2);
  m_ls->InitializeMatrix(SumMatrixIndex);
  m_ls->InitializeMatrix(DifferenceMatrixIndex);
  m_ls->InitializeVector(ForceTIndex);
  m_ls->InitializeVector(ForceTotalIndex);
  m_ls->InitializeVector(ForceTMinus1Index);
  m_ls->InitializeVector(SolutionVectorTMinus1Index);
  m_ls->InitializeVector(DiffMatrixBySolutionTMinus1Index);
  m_ls->InitializeSolution(SolutionTIndex);
  m_ls->InitializeSolution(TotalSolutionIndex);
  m_ls->InitializeSolution(SolutionTMinus1Index);
}

/*
 * Assemble the master stiffness matrix (also apply the MFCs to K)
 */  
void SolverCrankNicolson::AssembleKandM() 
{

  // if no DOFs exist in a system, we have nothing to do
  if (NGFN<=0) return;

  Float lhsval;
  Float rhsval;
  NMFC=0;  // number of MFC in a system

  // temporary storage for pointers to LoadBCMFC objects
  typedef std::vector<LoadBCMFC::Pointer> MFCArray;
  MFCArray mfcLoads;

  /*
   * Before we can start the assembly procedure, we need to know,
   * how many boundary conditions (MFCs) there are in a system.
   */
  mfcLoads.clear();
  // search for MFC's in Loads array, because they affect the master stiffness matrix
  for(LoadArray::iterator l=load.begin(); l!=load.end(); l++) {
    if ( LoadBCMFC::Pointer l1=dynamic_cast<LoadBCMFC*>( &(*(*l))) ) {
      // store the index of an LoadBCMFC object for later
      l1->Index=NMFC;
      // store the pointer to a LoadBCMFC object for later
      mfcLoads.push_back(l1);
      // increase the number of MFC
      NMFC++;
    }
  }
 
  /*
   * Now we can assemble the master stiffness matrix
   * from element stiffness matrices
   */
  InitializeForSolution(); 
  
 
  /*
   * Step over all elements
   */
  for(ElementArray::iterator e=el.begin(); e!=el.end(); e++)
  {
    vnl_matrix<Float> Ke;
    (*e)->GetStiffnessMatrix(Ke);  /*Copy the element stiffness matrix for faster access. */
    vnl_matrix<Float> Me;
    (*e)->GetMassMatrix(Me);  /*Copy the element mass matrix for faster access. */
    int Ne=(*e)->GetNumberOfDegreesOfFreedom();          /*... same for element DOF */

    Me=Me*m_rho;

    /* step over all rows in in element matrix */
    for(int j=0; j<Ne; j++)
    {
      /* step over all columns in in element matrix */
      for(int k=0; k<Ne; k++) 
      {
        /* error checking. all GFN should be =>0 and <NGFN */
        if ( (*e)->GetDegreeOfFreedom(j) >= NGFN ||
             (*e)->GetDegreeOfFreedom(k) >= NGFN  )
        {
          throw FEMExceptionSolution(__FILE__,__LINE__,"SolverCrankNicolson::AssembleKandM()","Illegal GFN!");
        }
        
        /* Here we finaly update the corresponding element
         * in the master stiffness matrix. We first check if 
         * element in Ke is zero, to prevent zeros from being 
         * allocated in sparse matrix.
         */
        if ( Ke(j,k)!=Float(0.0) || Me(j,k) != Float(0.0) )
        {
          // left hand side matrix
          lhsval=(Me(j,k) + m_alpha*m_deltaT*Ke(j,k));
          m_ls->AddMatrixValue( (*e)->GetDegreeOfFreedom(j) , 
                    (*e)->GetDegreeOfFreedom(k), 
                    lhsval, SumMatrixIndex );
          // right hand side matrix
          rhsval=(Me(j,k) - (1.-m_alpha)*m_deltaT*Ke(j,k));
          m_ls->AddMatrixValue( (*e)->GetDegreeOfFreedom(j) , 
                    (*e)->GetDegreeOfFreedom(k), 
                    rhsval, DifferenceMatrixIndex );
//          if (k == j ) std::cout << " ke " << Ke(j,k) << " me " << Me(j,k) << std::endl;
        }
      }

    }

  }

  /*
   * Step over all the loads to add the landmark contributions to the
   * appropriate place in the stiffness matrix
   */
  for(LoadArray::iterator l2=load.begin(); l2!=load.end(); l2++) {
    if ( LoadLandmark::Pointer l3=dynamic_cast<LoadLandmark*>( &(*(*l2))) ) {
      Element::Pointer ep = const_cast<Element*>( l3->el[0] );

      Element::MatrixType Le;
      ep->GetLandmarkContributionMatrix( l3->eta, Le );
      
      int Ne = ep->GetNumberOfDegreesOfFreedom();
      
      // step over all rows in element matrix
      for (int j=0; j<Ne; j++) { 
        // step over all columns in element matrix
          for (int k=0; k<Ne; k++) {
            // error checking, all GFN should be >=0 and < NGFN
            if ( ep->GetDegreeOfFreedom(j) >= NGFN ||
                 ep->GetDegreeOfFreedom(k) >= NGFN ) {
                throw FEMExceptionSolution(__FILE__,__LINE__,"SolverCrankNicolson::AssembleKandM()","Illegal GFN!");
            }
    
          // Now update the corresponding element in the master
          // stiffness matrix and omit the zeros for the sparseness
          if ( Le(j,k)!=Float(0.0) ) {
            // lhs matrix
            lhsval = m_alpha*m_deltaT*Le(j,k);
            m_ls->AddMatrixValue( ep->GetDegreeOfFreedom(j),
                                ep->GetDegreeOfFreedom(k),
                                lhsval, SumMatrixIndex );
            //rhs matrix
            rhsval = (1.-m_alpha)*m_deltaT*Le(j,k);
            m_ls->AddMatrixValue( ep->GetDegreeOfFreedom(j),
                                ep->GetDegreeOfFreedom(k),
                                rhsval, DifferenceMatrixIndex );
            }
        }
      }
    }
  }

  /* step over all types of BCs */
  this->ApplyBC();  // BUG  -- are BCs applied appropriately to the problem?
}


/*
 * Assemble the master force vector
 */
void SolverCrankNicolson::AssembleFforTimeStep(int dim) {
/* if no DOFs exist in a system, we have nothing to do */
  if (NGFN<=0) return;
  AssembleF(dim); // assuming assemblef uses index 0 in vector!

  typedef std::map<Element::DegreeOfFreedomIDType,Float> BCTermType;
  BCTermType bcterm;

   /* Step over all Loads */
  for(LoadArray::iterator l=load.begin(); l!=load.end(); l++)
  {
    Load::Pointer l0=*l;
    if ( LoadBC::Pointer l1=dynamic_cast<LoadBC*>(&*l0) )
    {
      bcterm[ l1->m_element->GetDegreeOfFreedom(l1->m_dof) ]=l1->m_value[dim];
    }
  } // end for LoadArray::iterator l

  // Now set the solution t_minus1 vector to fit the BCs
  for( BCTermType::iterator q=bcterm.begin(); q!=bcterm.end(); q++)
  { 
    m_ls->SetVectorValue(q->first,0.0,SolutionVectorTMinus1Index); //FIXME? 
    m_ls->SetSolutionValue(q->first,0.0,SolutionTMinus1Index); //FIXME? 
    m_ls->SetSolutionValue(q->first,0.0,TotalSolutionIndex);
  }

  m_ls->MultiplyMatrixVector(DiffMatrixBySolutionTMinus1Index,
                             DifferenceMatrixIndex,SolutionVectorTMinus1Index);
   
  for (unsigned int index=0; index<NGFN; index++) RecomputeForceVector(index);

  // Now set the solution and force vector to fit the BCs
  for( BCTermType::iterator q=bcterm.begin(); q!=bcterm.end(); q++)
  { 
    m_ls->SetVectorValue(q->first,q->second,ForceTIndex); 
  }
}



void  SolverCrankNicolson::RecomputeForceVector(unsigned int index)
{// 
  Float ft   = m_ls->GetVectorValue(index,ForceTIndex);
  Float ftm1 = m_ls->GetVectorValue(index,ForceTMinus1Index);
  Float utm1 = m_ls->GetVectorValue(index,DiffMatrixBySolutionTMinus1Index);
  Float f=m_deltaT*(m_alpha*ft+(1.-m_alpha)*ftm1)+utm1;
  m_ls->SetVectorValue(index , f, ForceTIndex);
}



/*
 * Solve for the displacement vector u
 */  
void SolverCrankNicolson::Solve() 
{
 /* FIXME - must verify that this is correct use of wrapper */
  /* FIXME Initialize the solution vector */
  m_ls->InitializeSolution(SolutionTIndex);
  m_ls->Solve();  
// call this externally    AddToDisplacements(); 
}


void SolverCrankNicolson::FindBracketingTriplet(Float* a, Float* b, Float* c)
{
 // in 1-D domain, we want to find a < b < c , s.t.  f(b) < f(a) && f(b) < f(c)
 //  see Numerical Recipes
 
  Float Gold=1.618034;
  Float Glimit=100.0;
  Float Tiny=1.e-20;
  
  Float ax, bx,cx;
  ax=0.0; bx=1.;
  Float fc;
  Float fa=vcl_fabs(EvaluateResidual(ax));
  Float fb=vcl_fabs(EvaluateResidual(bx));
  
  Float ulim,u,r,q,fu,dum;

  if ( fb > fa ) 
  {
     dum=ax; ax=bx; bx=dum;
     dum=fb; fb=fa; fa=dum;
  }

  cx=bx+Gold*(bx-ax);  // first guess for c - the 3rd pt needed to bracket the min
  fc=vcl_fabs(EvaluateResidual(cx));

  
  while (fb > fc  /*&& vcl_fabs(ax) < 3. && vcl_fabs(bx) < 3. && vcl_fabs(cx) < 3.*/)
    {
    r=(bx-ax)*(fb-fc);
    q=(bx-cx)*(fb-fa);
    Float denom=(2.0*GSSign(GSMax(fabs(q-r),Tiny),q-r));
    u=(bx)-((bx-cx)*q-(bx-ax)*r)/denom;
    ulim=bx + Glimit*(cx-bx);
    if ((bx-u)*(u-cx) > 0.0)
      {
      fu=vcl_fabs(EvaluateResidual(u));
      if (fu < fc)
        {
        ax=bx;
        bx=u;
        *a=ax; *b=bx; *c=cx;
        return;
        }
      else if (fu > fb)
        {
        cx=u;
        *a=ax; *b=bx; *c=cx;
        return;
        }
      
        u=cx+Gold*(cx-bx);
        fu=vcl_fabs(EvaluateResidual(u));
        
      }
    else if ( (cx-u)*(u-ulim) > 0.0)
      {
      fu=vcl_fabs(EvaluateResidual(u));
      if (fu < fc)
        {
        bx=cx; cx=u; u=cx+Gold*(cx-bx);
        fb=fc; fc=fu; fu=vcl_fabs(EvaluateResidual(u));
        }
      
      }
    else if ( (u-ulim)*(ulim-cx) >= 0.0)
      {
      u=ulim;
      fu=vcl_fabs(EvaluateResidual(u));
      }
    else
      {
      u=cx+Gold*(cx-bx);
      fu=vcl_fabs(EvaluateResidual(u));
      }
    
    ax=bx; bx=cx; cx=u;
    fa=fb; fb=fc; fc=fu;
    
  }

  if ( vcl_fabs(ax) > 1.e3  || vcl_fabs(bx) > 1.e3 || vcl_fabs(cx) > 1.e3)
  { ax=-2.0;  bx=1.0;  cx=2.0; } // to avoid crazy numbers caused by bad bracket (u goes nuts)
  
  *a=ax; *b=bx; *c=cx;
}

Element::Float SolverCrankNicolson::BrentsMethod(Float tol,unsigned int MaxIters)
{
  // We should now have a, b and c, as well as f(a), f(b), f(c), 
  // where b gives the minimum energy position;

  Float CGOLD = 0.3819660;
  Float ZEPS = 1.e-10;

  Float ax=0.0, bx=1.0, cx=2.0;

  FindBracketingTriplet(&ax, &bx, &cx);

  Float xmin;