TAPsFEMMesh_withSparseSymmetricMatrix.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 ()
{
    // 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();

    Mstiff.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 ()
{
    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];
    }

    Mstiff.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_wFixedDisplacementNodes ()
{
    // Modify stiffness matrix for calculating forces generated by fixed displacement nodes
    // The rows associated with fixed nodes will be diagonalized to one.
    Mstiff_2 = 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
        }
        m_b[i].Clear(); // clear m_b
    }
    // Calculating forces generated by fixed displacement nodes
    // The statements below is similar to MOD(Mstiff_2) * m_u = m_b 
    // where MOD(Mstiff2) diagonalizes rows of Mstiff2 (associated with fixed nodes) to 1.
    // REMARK: MOD(Mstiff2) is on longer a symmetric matrix.
    std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator it;
    for ( unsigned int i = 0; i < GetNumOfNodes(); ++i ) {
        if ( m_FixedNodes[i] ) {
            // skip fixed nodes
        }
        else {
            unsigned int r = 0;
            // row 0 to row (i-1)
            for ( r = 0; r < i; ++r ) {
                it = Mstiff_2.DirectAccess(r)->RefToElements().begin();
                // find symmetric element to row i
                while ( it != Mstiff_2.DirectAccess(r)->RefToElements().end() && it->idx < i ) {
                    ++it;
                }
                if ( it != Mstiff_2.DirectAccess(r)->RefToElements().end() && it->idx == i ) {
                    m_b[i] += it->M * m_u[i];
                }
            }
            // row i
            it = Mstiff_2.DirectAccess(r)->RefToElements().begin();
            while ( it != Mstiff_2.DirectAccess(r)->RefToElements().end() ) {
                m_b[i] += it->M * m_u[it->idx];
                ++it;
            }
        }
    }
    // 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];
        }
        else {
            m_b[i] = m_FixedDisplacements[i];
        }
        //std::cout << "FFFS m_b["<<i<<"]: " << m_b[i] << "\n";
    }


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

    //std::cout << Mstiff_2 << "TESTMMM\n";

    //Mstiff_2.MPCGSolverSpecificForFEM( m_FixedNodes, Mstiff_2, m_u, m_b, m_u_out, m_Filters, m_Precond, m_iIterationLimit, m_tErrorThreshold );
    Mstiff_2.MPCGSolver( 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_wFixedDisplacementNodes ()
{

    /*
    // TEST
    SparseMatrix_Matrix3x3<T> K;
    K = Mstiff;
    std::cout << Mstiff << "AAA\n";
    std::cout << K << "BBB\n";
    //*/

    // Modify stiffness matrix for calculating forces generated by fixed displacement nodes
    // The rows associated with fixed nodes will be diagonalized to one.
    Mstiff_2 = 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
        }
        m_b[i].Clear(); // clear m_b
    }
    // Calculating forces generated by fixed displacement nodes
    // The statements below is similar to MOD(Mstiff_2) * m_u = m_b 
    // where MOD(Mstiff2) diagonalizes rows of Mstiff2 (associated with fixed nodes) to 1.
    // REMARK: MOD(Mstiff2) is on longer a symmetric matrix.
    std::list< SparseVector_Matrix3x3_ElementData<T> >::iterator it;
    for ( unsigned int i = 0; i < GetNumOfNodes(); ++i ) {
        if ( m_FixedNodes[i] ) {
            // skip fixed nodes
        }
        else {
            unsigned int r = 0;
            // row 0 to row (i-1)
            for ( r = 0; r < i; ++r ) {
                it = Mstiff_2.DirectAccess(r)->RefToElements().begin();
                // find symmetric element to row i
                while ( it != Mstiff_2.DirectAccess(r)->RefToElements().end() && it->idx < i ) {
                    ++it;
                }
                if ( it != Mstiff_2.DirectAccess(r)->RefToElements().end() && it->idx == i ) {
                    m_b[i] += it->M * m_u[i];
                }
            }
            // row i
            it = Mstiff_2.DirectAccess(r)->RefToElements().begin();
            while ( it != Mstiff_2.DirectAccess(r)->RefToElements().end() ) {
                m_b[i] += it->M * m_u[it->idx];
                ++it;
            }
        }
    }
    // 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];
        }
        else {
            m_b[i] = m_FixedDisplacements[i];
        }
        //std::cout << "FFFS m_b["<<i<<"]: " << m_b[i] << "\n";
    }

    UpdateDisplacementVector_wFixedDisplacementNodes();
    UpdateAandB_wFixedDisplacementNodes( Mstiff_2 );

    // Modify stiffness matrix for before solving the system of equations
    // Diagonalize the rows and columns of fixed nodes to one
    Mstiff_2.DiagonalizeRowsCols( 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];
        }
        //else {
        //  m_vNodeVelocities[m_currVelIdx][i].Clear();
        //}
    }

    Mstiff_2.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 ) {

        //std::cout << "m_vNodeVelocities[m_currVelIdx]["<<i<<"]: " << m_vNodeVelocities[m_currVelIdx][i] << "\n";
        //std::cout << "m_vNodeVelocities[m_nextVelIdx]["<<i<<"]: " << m_vNodeVelocities[m_nextVelIdx][i] << "\n";
        //std::cout << "m_tTimeStep: " << m_tTimeStep << "\n";
        //std::cout << "m_vDeformedMeshNodes[i]: (bef) " << m_vDeformedMeshNodes[i] << "\n";

        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];
        }

        //std::cout << "m_vDeformedMeshNodes[i]: (aft) DF " << m_vDeformedMeshNodes[i] << "\n";
        //std::cout << i << " (aft) DF " << m_vDeformedMeshNodes[i] - m_vUndeformedMeshNodes[i] << "\n";
    }

    //UpdateDeformedMeshNodes();

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

    //std::cout << "END: AdvanceSimulationDynamic\n\n";
    //*/
}












//-------------------------------------
template <typename T>
void Mesh<T>::UpdateAandB ( SparseSymmetricMatrix_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];
    }
}

//-------------------------------------
template <typename T>
void Mesh<T>::UpdateAandB_wFixedDisplacementNodes ( SparseSymmetricMatrix_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_wFixedDisplacementNodes(), 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 ) {
        if ( m_FixedNodes[i] == false ) {
            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];
        }
        else {
            m_b[i] = m_u[i];
        }
    }
}


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