TAPsFEMMesh_withSparseMatrix.cpp

Go to the documentation of this file.

/******************************************************************************
SUKITTI PUNAK   (04/05/2010)
UPDATE          (05/01/2010)
******************************************************************************/

BEGIN_NAMESPACE_TAPs__FEM
//=============================================================================
// Simulation
//-----------------------------------------------------------------------------
template <typename T>
void Mesh<T>::AdvanceSimulationStatic_woFixedDisplacementNodes ()
{
    #ifdef  TAPs_ADD_INTERACTIONS
        ApplyAllLoads();
    #endif//TAPs_ADD_INTERACTIONS

    // Without the update statement below, m_u is always zero.
    // m_u is used as an initial estimation for displacements.
    // m_u will not be modified (or affected) by the call of MPCGSolver function below.
    //UpdateDisplacementVector();

    MPCGSolver( Mstiff, m_u, m_Forces, m_u_out, m_Filters, m_Precond, m_iIterationLimit, m_tErrorThreshold );
    UpdateDeformedMeshNodes();
}

//-----------------------------------------------------------------------------
#ifdef  TAPs_SIM_DYNAMIC_SYSTEM
//-------------------------------------
    //---------------------------------------------------------------
    template <typename T>
    void Mesh<T>::AdvanceSimulationDynamic_woFixedDisplacementNodes ()
    {
        #ifdef  TAPs_ADD_INTERACTIONS
            ApplyAllLoads();
        #endif//TAPs_ADD_INTERACTIONS

        UpdateDisplacementVector();
        UpdateAandB ( Mstiff );

        // Copy current velocities to m_u
        for ( unsigned int i = 0; i < GetNumOfNodes(); ++i ) {
            m_u[i] = m_vNodeVelocities[m_currVelIdx][i];
        }

        MPCGSolver( m_A, m_u, m_b, m_vNodeVelocities[m_nextVelIdx], m_Filters, m_Precond, m_iIterationLimit, m_tErrorThreshold );

        // Update new positions
        for ( unsigned int i = 0; i < GetNumOfNodes(); ++i ) {
            m_vDeformedMeshNodes[i][0] += m_vNodeVelocities[m_nextVelIdx][i][0]*m_tTimeStep*m_Filters[i][0];
            m_vDeformedMeshNodes[i][1] += m_vNodeVelocities[m_nextVelIdx][i][1]*m_tTimeStep*m_Filters[i][1];
            m_vDeformedMeshNodes[i][2] += m_vNodeVelocities[m_nextVelIdx][i][2]*m_tTimeStep*m_Filters[i][2];
        }

        // Swap indices for velocities
        unsigned char tmp = m_nextVelIdx;
        m_nextVelIdx = m_currVelIdx;
        m_currVelIdx = tmp;
    }
//-------------------------------------
#endif//TAPs_SIM_DYNAMIC_SYSTEM
//-----------------------------------------------------------------------------




//-----------------------------------------------------------------------------
template <typename T>
void Mesh<T>::AdvanceSimulationStatic ()
{
    #ifdef  TAPs_ADD_INTERACTIONS
        ApplyAllLoads();
    #endif//TAPs_ADD_INTERACTIONS

    // Modify stiffness matrix for calculating forces generated by fixed displacement nodes
    // The rows associated with fixed nodes will be diagonalized to one.
    Mstiff_2.SetTo( Mstiff );
    // Set m_u for calculating forces generated by fixed displacement nodes
    for ( unsigned int i = 0; i < GetNumOfNodes(); ++i ) {
        if ( m_FixedNodes[i] ) {
            Mstiff_2.DiagonalizeRow( i, 1 );    // diagonalize the row i (due to node i is fixed)
            m_u[i] = m_FixedDisplacements[i];   // adjust the displacement caused by the displacement of node i
        }
        else {
            m_u[i].Clear(); // the displacement for unfixed node is set to zero
        }
    }
    // Calculating forces generated by fixed displacement nodes
    Mstiff_2.MulWith( m_u, m_b );
    // Set force vector 
    for ( unsigned int i = 0; i < GetNumOfNodes(); ++i ) {
        if ( m_FixedNodes[i] == false ) {
            m_b[i] *= -1;
            m_b[i] += m_Forces[i];
        }
    }

    // Modify stiffness matrix before solving the system of equations
    // Diagonalize the columns of fixed nodes to one
    Mstiff_2.DiagonalizeCols( m_FixedNodes, 1 );

    MPCGSolver( Mstiff_2, m_u, m_b, m_u_out, m_Filters, m_Precond, m_iIterationLimit, m_tErrorThreshold );
    //MPCGSolver_wFixedNodes( m_FixedNodes, Mstiff_2, m_u, m_b, m_u_out, m_Filters, m_Precond, m_iIterationLimit, m_tErrorThreshold );

    UpdateDeformedMeshNodes();
}




//-----------------------------------------------------------------------------
#ifdef  TAPs_SIM_DYNAMIC_SYSTEM
//-------------------------------------

    //---------------------------------------------------------------
    template <typename T>
    void Mesh<T>::AdvanceSimulationDynamic ()
    {
        //std::cout << "TAPs_SIM_DYNAMIC_SYSTEM is defined and AdvanceSimulationDynamic() is called\n";

        #ifdef  TAPs_ADD_INTERACTIONS
            ApplyAllLoads();
        #endif//TAPs_ADD_INTERACTIONS

        // Modify stiffness matrix for calculating forces generated by fixed displacement nodes
        // The rows associated with fixed nodes will be diagonalized to one.
        Mstiff_2.SetTo( Mstiff );
        // Set m_u for calculating forces generated by fixed displacement nodes
        for ( unsigned int i = 0; i < GetNumOfNodes(); ++i ) {
            if ( m_FixedNodes[i] ) {
                Mstiff_2.DiagonalizeRow( i, 1 );    // diagonalize the row i (due to node i is fixed)
                m_u[i] = m_FixedDisplacements[i];   // adjust the displacement caused by the displacement of node i
            }
            else {
                m_u[i].Clear(); // the displacement for unfixed node is set to zero
            }
        }
        // Calculating forces generated by fixed displacement nodes
        Mstiff_2.MulWith( m_u, m_b );

        // Set force vector 
        for ( unsigned int i = 0; i < GetNumOfNodes(); ++i ) {
            if ( m_FixedNodes[i] == false ) {
                m_b[i] *= -1;
                m_b[i] += m_Forces[i];
            }
        }

        // After Mstiff has rows associated with fixed nodes diagonalized to one,
        // update the displacement vector and update A and B
        UpdateDisplacementVector_wFixedDisplacementNodes();
        UpdateAandB( Mstiff_2 );

        // Modify stiffness matrix for before solving the system of equations
        // Diagonalize the columns of fixed nodes to one
        Mstiff_2.DiagonalizeCols( m_FixedNodes, 1 );

        // Copy current velocities to m_u
        for ( unsigned int i = 0; i < GetNumOfNodes(); ++i ) {
            if ( m_FixedNodes[i] == false ) {
                m_u[i] = m_vNodeVelocities[m_currVelIdx][i];
            }
        }

        m_A.DiagonalizeCols( m_FixedNodes );

        MPCGSolver( m_A, m_u, m_b, m_vNodeVelocities[m_nextVelIdx], m_Filters, m_Precond, m_iIterationLimit, m_tErrorThreshold );
        //MPCGSolver_wFixedNodes( m_FixedNodes, m_A, m_u, m_b, m_vNodeVelocities[m_nextVelIdx], m_Filters, m_Precond, m_iIterationLimit, m_tErrorThreshold );

        // Update new positions
        for ( unsigned int i = 0; i < GetNumOfNodes(); ++i ) {
            if ( m_FixedNodes[i] == false ) {
                m_vDeformedMeshNodes[i][0] += m_vNodeVelocities[m_nextVelIdx][i][0]*m_tTimeStep*m_Filters[i][0];
                m_vDeformedMeshNodes[i][1] += m_vNodeVelocities[m_nextVelIdx][i][1]*m_tTimeStep*m_Filters[i][1];
                m_vDeformedMeshNodes[i][2] += m_vNodeVelocities[m_nextVelIdx][i][2]*m_tTimeStep*m_Filters[i][2];
            }
            else {
                m_vDeformedMeshNodes[i] = m_u[i] + m_vUndeformedMeshNodes[i];
            }
        }

        // Swap indices for velocities
        unsigned char tmp = m_nextVelIdx;
        m_nextVelIdx = m_currVelIdx;
        m_currVelIdx = tmp;
    }


    //---------------------------------------------------------------
    template <typename T>
    void Mesh<T>::UpdateAandB ( SparseMatrix_Matrix3x3<T> & Mstiff )
    {
        SparseVector_Matrix3x3<T> * K;
        SparseVector_Matrix3x3<T> * R;
        Matrix3x3<T> * diagR;
        std::list< SparseVector_Matrix3x3_ElementData<T> >::const_iterator itK;
        std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator itR;

        // since C = alpha*M + beta*K
        // and A := M + h*C + h*h*K 
        //        = M + h*alpha*M + h*beta*K + h*h*K
        //        = (1 + h*alpha)*M + h*(beta + h)*K
        T scaleB = (m_tNodeDamping_Beta + m_tTimeStep) * m_tTimeStep;
        T scaleA = 1 + m_tTimeStep*m_tNodeDamping_Alpha;
        // therefore, R  = h*(beta+h) * K  = K * scaleB
        //            R += (1+h*alpha) * M = M * scaleA
        for ( unsigned int i = 0; i < Mstiff.GetNumOfRows(); ++i ) {
            K = Mstiff.DirectAccess( i );
            R = m_A.DirectAccess( i );
            itK = K->RefToElements().begin();
            itR = R->RefToElements().begin();

            while ( itK != K->RefToElements().end() ) {
                // R  = K * scaleB
                itR->M = itK->M * scaleB;
                ++itK;
                ++itR;
            }

            // R += M * scaleA
            diagR= R->ReturnMatrix3x3( i );
            (*diagR)[0] += m_vNodeMass[i][0] * scaleA;
            (*diagR)[4] += m_vNodeMass[i][1] * scaleA;
            (*diagR)[8] += m_vNodeMass[i][2] * scaleA;
        }

        // b := M*v_curr - h*( K*(x_def - x_orig) - f )
        // after UpdateDisplacementVector(), m_u = x_def - x_orig
        // therefore, m_b  = K*m_u
        //            m_b -= f
        //            m_b *= -h
        //            m_b += M*v_curr
        Mstiff.MulWith( m_u, m_b );
        for ( unsigned int i = 0; i < GetNumOfNodes(); ++i ) {
            m_b[i] -= m_Forces[i];
            m_b[i] *= -m_tTimeStep;
            m_b[i][0] += m_vNodeMass[i][0] * m_vNodeVelocities[m_currVelIdx][i][0];
            m_b[i][1] += m_vNodeMass[i][1] * m_vNodeVelocities[m_currVelIdx][i][1];
            m_b[i][2] += m_vNodeMass[i][2] * m_vNodeVelocities[m_currVelIdx][i][2];
        }
    }

//-------------------------------------
#endif//TAPs_SIM_DYNAMIC_SYSTEM
//-----------------------------------------------------------------------------




//-----------------------------------------------------------------------------
template <typename T>
void Mesh<T>::MPCGSolver (
    SparseMatrix_Matrix3x3<T> const &A,     
    std::vector< Vector3<T> > const &x,     
    std::vector< Vector3<T> > const &b,     
    std::vector< Vector3<T> > &x_out,       
    std::vector< Vector3<T> > const &ft,    
    std::vector< Vector3<T> > const &P,     
    int iterationLimit = 100,               
    T errorThreshold = 1E-10                
)
{
    unsigned int size = b.size();
    
    // inputs
    // A, x, b, x_out, ft, P
    // temps
    T D0, Dold, Dnew, alpha;
    //std::vector< Vector3<T> > v( size );
    //std::vector< Vector3<T> > Pinv( size );
    //std::vector< Vector3<T> > r( size );
    //std::vector< Vector3<T> > s( size );
    //std::vector< Vector3<T> > c( size );
    //std::vector< Vector3<T> > q( size );

    // x_out = x
    // D0 = Filter(b)^T.P.Filter(b)
    D0 = 0;
    Vector3<T> filterB;
    for ( unsigned int i = 0; i < size; ++i ) {
        // x_out
        x_out[i][0] = x[i][0];
        x_out[i][1] = x[i][1];
        x_out[i][2] = x[i][2];
        // D0
        filterB[0] = ft[i][0]*b[i][0];
        filterB[1] = ft[i][1]*b[i][1];
        filterB[2] = ft[i][2]*b[i][2];
        D0 += filterB[0]*filterB[0]*P[i][0] + filterB[1]*filterB[1]*P[i][1] + filterB[2]*filterB[2]*P[i][2];
    }
    // r = Filter(b - A.x_out)
    // c = Filter(P^{-1}.r)
    // Dnew = r^{T}.c
    Dnew = 0;
    A.MulWith( x_out, v );      // v = A.x_out
    for ( unsigned int i = 0; i < size; ++i ) {
        // r
        r[i][0] = b[i][0] - v[i][0];
        r[i][1] = b[i][1] - v[i][1];
        r[i][2] = b[i][2] - v[i][2];
        r[i][0] = ft[i][0]*r[i][0];
        r[i][1] = ft[i][1]*r[i][1];
        r[i][2] = ft[i][2]*r[i][2];
        // c
        Pinv[i][0] = T(1) / P[i][0];
        Pinv[i][1] = T(1) / P[i][1];
        Pinv[i][2] = T(1) / P[i][2];
        c[i][0] = Pinv[i][0] * r[i][0];
        c[i][1] = Pinv[i][1] * r[i][1];
        c[i][2] = Pinv[i][2] * r[i][2];
        c[i][0] = ft[i][0]*c[i][0];
        c[i][1] = ft[i][1]*c[i][1];
        c[i][2] = ft[i][2]*c[i][2];
        // Dnew
        Dnew += r[i] * c[i];
    }

    //D0 = Dnew;

    int count = 0;
    T err = 1;
    T ths = errorThreshold * errorThreshold * D0 * D0;
    while ( err > 0 && count < iterationLimit ) {

        // q = Filter(A.c)
        // alpha = Dnew / (c^T.q)
        T tmp = 0;
        A.MulWith( c, v );  // v = A.c
        for ( unsigned int i = 0; i < size; ++i ) {
            // q
            q[i][0] = ft[i][0]*v[i][0];
            q[i][1] = ft[i][1]*v[i][1];
            q[i][2] = ft[i][2]*v[i][2];
            // alpha
            tmp += c[i]*q[i];
        }
        if ( tmp != 0 ) {
            alpha = Dnew / tmp;
        }
        else {
            break;
        }
        // x_out += alpha.c
        // r -= alpha.q
        // s = P^{-1}.r
        for ( unsigned int i = 0; i < size; ++i ) {
            // x_out
            x_out[i][0] += alpha * c[i][0];
            x_out[i][1] += alpha * c[i][1];
            x_out[i][2] += alpha * c[i][2];
            // r
            r[i][0] -= alpha * q[i][0];
            r[i][1] -= alpha * q[i][1];
            r[i][2] -= alpha * q[i][2];
            // s
            s[i][0] = Pinv[i][0]*r[i][0];
            s[i][1] = Pinv[i][1]*r[i][1];
            s[i][2] = Pinv[i][2]*r[i][2];
        }
        // Dold = Dnew
        Dold = Dnew;
        // Dnew = r^T.s
        Dnew = 0;
        for ( unsigned int i = 0; i < size; ++i ) {
            Dnew += r[i] * s[i];
        }
        // c = Filter( s + (Dnew/Dold)*c )
        tmp = Dnew/Dold;
        for ( unsigned int i = 0; i < size; ++i ) {
            c[i][0] = s[i][0] + tmp*c[i][0];
            c[i][1] = s[i][1] + tmp*c[i][1];
            c[i][2] = s[i][2] + tmp*c[i][2];
            c[i][0] = ft[i][0]*c[i][0];
            c[i][1] = ft[i][1]*c[i][1];
            c[i][2] = ft[i][2]*c[i][2];
        }

        // update values for stop conditions
        err = Dnew*Dnew - ths;
        ++count;
    }
}

//=============================================================================
END_NAMESPACE_TAPs__FEM
//34567890123456789012345678901234567890123456789012345678901234567890123456789
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----