two_fluid_vms.h

Go to the documentation of this file.

//    |  /           |
//    ' /   __| _` | __|  _ \   __|
//    . \  |   (   | |   (   |\__ \.
//   _|\_\_|  \__,_|\__|\___/ ____/
//                   Multi-Physics
//
//  License:         BSD License
//                   Kratos default license: kratos/license.txt
//
//  Main authors:    Riccardo Rossi
//                   Jordi Cotela
//
 
#if !defined(KRATOS_TWO_FLUID_VMS_H_INCLUDED )
#define  KRATOS_TWO_FLUID_VMS_H_INCLUDED
// System includes
#include <string>
#include <iostream>
// External includes
// Project includes
#include "containers/array_1d.h"
#include "includes/define.h"
#include "custom_elements/vms.h"
#include "includes/serializer.h"
#include "utilities/geometry_utilities.h"
#include "utilities/math_utils.h"
#include "utilities/split_tetrahedra.h"
// #include "utilities/enrichment_utilities.h"
#include "utilities/enrichment_utilities_duplicate_dofs.h"
// Application includes
#include "fluid_dynamics_application_variables.h"
#include "vms.h"
 
namespace Kratos
{
 
template< unsigned int TDim,
          unsigned int TNumNodes = TDim + 1 >
class TwoFluidVMS : public VMS<TDim, TNumNodes>
{
public:
    KRATOS_CLASS_INTRUSIVE_POINTER_DEFINITION(TwoFluidVMS);
    typedef IndexedObject BaseType;
    typedef VMS<TDim, TNumNodes> ElementBaseType;
    typedef Node NodeType;
    typedef Properties PropertiesType;
    typedef Geometry<NodeType> GeometryType;
    typedef Geometry<NodeType>::PointsArrayType NodesArrayType;
    typedef Vector VectorType;
    typedef typename ElementBaseType::MatrixType MatrixType;
    typedef std::size_t IndexType;
    typedef std::size_t SizeType;
    typedef std::vector<std::size_t> EquationIdVectorType;
    typedef std::vector< Dof<double>::Pointer > DofsVectorType;
    typedef PointerVectorSet<Dof<double>, IndexedObject> DofsArrayType;
    //Constructors.
 
    TwoFluidVMS(IndexType NewId = 0) :
        ElementBaseType(NewId)
    {
    }
 
    TwoFluidVMS(IndexType NewId, const NodesArrayType& ThisNodes) :
        ElementBaseType(NewId, ThisNodes)
    {
    }
 
    TwoFluidVMS(IndexType NewId, GeometryType::Pointer pGeometry) :
        ElementBaseType(NewId, pGeometry)
    {
    }
 
    TwoFluidVMS(IndexType NewId, GeometryType::Pointer pGeometry, PropertiesType::Pointer pProperties) :
        ElementBaseType(NewId, pGeometry, pProperties)
    {
    }
    ~TwoFluidVMS() override
    {
    }
 
    Element::Pointer Create(IndexType NewId, NodesArrayType const& ThisNodes,
                            PropertiesType::Pointer pProperties) const override
    {
        return Kratos::make_intrusive< TwoFluidVMS >(NewId, (this->GetGeometry()).Create(ThisNodes), pProperties);
    }
    Element::Pointer Create(IndexType NewId,
                           GeometryType::Pointer pGeom,
                           PropertiesType::Pointer pProperties) const override
    {
        return Kratos::make_intrusive< TwoFluidVMS >(NewId, pGeom, pProperties);
    }
 
 
    void CalculateRightHandSide(VectorType& rRightHandSideVector,
                                const ProcessInfo& rCurrentProcessInfo) override
    {
        const unsigned int local_size = (TDim+1)*(TDim+1);
        Matrix tmp(local_size,local_size);
        CalculateLocalSystem(tmp,rRightHandSideVector,rCurrentProcessInfo);
    }
 
 
    void CalculateMassMatrix(MatrixType& rMassMatrix, const ProcessInfo& rCurrentProcessInfo) override
    {
        KRATOS_THROW_ERROR(std::logic_error,"MassMatrix function shall not be called when using this type of element","");
    }
 
 
        void CalculateLocalSystem(MatrixType& rLeftHandSideMatrix,
                                          VectorType& rRightHandSideVector,
                                          const ProcessInfo& rCurrentProcessInfo) override
    {
        const unsigned int LocalSize = (TDim + 1) * TNumNodes;
 
        const ProcessInfo& rConstProcessInfo = rCurrentProcessInfo; // Taking const reference for thread safety
 
        //****************************************************
        // Resize and set to zero the RHS
        if(rRightHandSideVector.size() != LocalSize)
            rRightHandSideVector.resize(LocalSize,false);
        noalias(rRightHandSideVector) = ZeroVector(LocalSize);
 
        // Resize and set to zero the LHS
        if (rLeftHandSideMatrix.size1() != LocalSize)
            rLeftHandSideMatrix.resize(LocalSize, LocalSize, false);
        noalias(rLeftHandSideMatrix) = ZeroMatrix(LocalSize, LocalSize);
 
       Matrix MassMatrix = ZeroMatrix(LocalSize, LocalSize);
 
       //****************************************************
        //Get Vector of BDF coefficients
        const Vector& BDFVector = rConstProcessInfo[BDF_COEFFICIENTS];
 
       //****************************************************
        // Get this element's geometric properties
        double Area;
        array_1d<double, TNumNodes> N;
        BoundedMatrix<double, TNumNodes, TDim> DN_DX;
        GeometryUtils::CalculateGeometryData(this->GetGeometry(), DN_DX, N, Area);
 
 
        //estimate a minimal h
        /*double h=0.0;
        if constexpr (TDim == 3) h = pow(6.0*Area, 1.0/3.0);
        else h = sqrt(2.0*Area);*/
 
 
 
        //input data for enrichment function
        Vector distances(TNumNodes);
        Matrix coords(TNumNodes, TDim);
 
        //output data for enrichment function
        Matrix Nenriched;
        Vector volumes;
        Matrix Ngauss;
        Vector signs(6);
        std::vector< Matrix > gauss_gradients;
 
 
        //fill coordinates
        for (unsigned int i = 0; i < TNumNodes; i++)
        {
            const array_1d<double, 3 > & xyz = this->GetGeometry()[i].Coordinates();
//             volumes[i] = 0.0;
            distances[i] = this->GetGeometry()[i].FastGetSolutionStepValue(DISTANCE);
            for (unsigned int j = 0; j < TDim; j++)
                coords(i, j) = xyz[j];
        }
 
/*        for (unsigned int i = 0; i < 6; i++)
            gauss_gradients[i].resize(1, TDim, false);
*/
//         unsigned int ndivisions = EnrichmentUtilities::CalculateTetrahedraEnrichedShapeFuncions(coords, DN_DX, distances, volumes, Ngauss, signs, gauss_gradients, Nenriched);
         unsigned int ndivisions = EnrichmentUtilitiesDuplicateDofs::CalculateTetrahedraEnrichedShapeFuncions(coords, DN_DX, distances, volumes, Ngauss, signs, gauss_gradients, Nenriched);
        const unsigned int nenrichments = Nenriched.size2();
 
        Matrix enrichment_terms_vertical   = ZeroMatrix(LocalSize,nenrichments);
        Matrix enrichment_terms_horizontal = ZeroMatrix(nenrichments,LocalSize);
 
        Matrix enrichment_diagonal = ScalarMatrix(nenrichments,nenrichments,0.0);
        Vector enriched_rhs(nenrichments,0.0);
        array_1d<double,3> bf = ZeroVector(3);
 
        double positive_volume = 0.0;
        double negative_volume = 0.0;
 
        if(ndivisions == 1) //compute gauss points for exact integration of a tetra element
        {
            const GeometryType::IntegrationPointsArrayType& IntegrationPoints = this->GetGeometry().IntegrationPoints(GeometryData::IntegrationMethod::GI_GAUSS_2);
 
            Ngauss = this->GetGeometry().ShapeFunctionsValues(GeometryData::IntegrationMethod::GI_GAUSS_2);
 
            volumes.resize(IntegrationPoints.size(),false);
 
            for (unsigned int g = 0; g < this->GetGeometry().IntegrationPointsNumber(GeometryData::IntegrationMethod::GI_GAUSS_2); g++)
        {
                volumes[g] = 6.0*Area * IntegrationPoints[g].Weight();
                signs[g] = signs[0];
        }
 
        }
//         else
//         {
//             Matrix aux = Ngauss;
//             Ngauss.resize(ndivisions,Ngauss.size2(),false);
//             for(unsigned int i=0; i<ndivisions; i++)
//                 for(unsigned int k=0; k<Ngauss.size2(); k++)
//                     Ngauss(i,k) = aux(i,k);
//         }
/*  KRATOS_WATCH(this->Id());
KRATOS_WATCH(volumes);
KRATOS_WATCH(Ngauss);  */
 
        for (unsigned int igauss = 0; igauss < Ngauss.size1(); igauss++)
        {
            double wGauss = volumes[igauss];
 
 
            if(signs[igauss] > 0) //check positive and negative volume
                positive_volume += wGauss;
            else
                negative_volume += wGauss;
        }
 
 
        const double min_area_ratio = 1e-6;
//         if(positive_volume/Area < min_area_ratio)
//         {
//             for (unsigned int igauss = 0; igauss < Ngauss.size1(); igauss++)
//             {
//                  if(signs[igauss] > 0) //check positive and negative volume
//                     volumes[igauss] =0.0;
//             }
//         }
//
//         if(negative_volume/Area < min_area_ratio)
//         {
//             for (unsigned int igauss = 0; igauss < Ngauss.size1(); igauss++)
//             {
//                  if(signs[igauss] < 0) //check positive and negative volume
//                     volumes[igauss] =0.0;
//             }
//         }
 
        // Porous media losses
        const Properties& r_properties = this->GetProperties();
        const double c1 = r_properties[LIN_DARCY_COEF];
        const double c2 = r_properties[NONLIN_DARCY_COEF];
 
 
        //****************************************************
        //compute LHS and RHS + first part of mass computation
        for (unsigned int igauss = 0; igauss < Ngauss.size1(); igauss++)
        {
            //assigning the gauss data
            for (unsigned int k = 0; k < TNumNodes; k++)
                N[k] = Ngauss(igauss, k);
            double wGauss = volumes[igauss];
 
 
//             if(signs[igauss] > 0) //check positive and negative volume
//                 positive_volume += wGauss;
//             else
//                 negative_volume += wGauss;
 
            //****************************************************
            // Calculate this element's fluid properties
            double Density;
            this->EvaluateInPoint(Density, DENSITY, N);
 
            double ElemSize = this->ElementSize(Area);
            double Viscosity = this->EffectiveViscosity(Density,N,DN_DX,ElemSize,rCurrentProcessInfo);
 
            //compute RHS contributions
            this->AddMomentumRHS(rRightHandSideVector, Density, N, wGauss);
 
            // Get Advective velocity
            array_1d<double, 3 > AdvVel;
            this->GetAdvectiveVel(AdvVel, N);
            const double VelNorm = MathUtils<double>::Norm3(AdvVel);
 
            const double DarcyTerm = this->CalculateDarcyTerm(Density, Viscosity, c1, c2, N);
            // Calculate stabilization parameters
            double TauOne, TauTwo;
 
            //compute stabilization parameters
            this->CalculateStabilizationTau(TauOne, TauTwo, VelNorm, ElemSize, Density, Viscosity, DarcyTerm, rCurrentProcessInfo);
 
            this->AddIntegrationPointVelocityContribution(rLeftHandSideMatrix, rRightHandSideVector, Density, Viscosity, AdvVel, DarcyTerm, TauOne, TauTwo, N, DN_DX, wGauss);
 
            //compute mass matrix - terms related to real mass
            this->AddConsistentMassMatrixContribution(MassMatrix, N, Density, wGauss);
 
 
            //****************************************************
            //enrichment variables
            if (ndivisions > 1)
            {
//                 KRATOS_WATCH(Nenriched);
                this->EvaluateInPoint(bf, BODY_FORCE, N);
 
                //note that here we compute only a part of the acceleration term
                //this is done like this since the velocity*BDFVector[0] is treated implicitly
                array_1d<double,3> OldAcceleration = ZeroVector(3);
                for(unsigned int step=1; step<BDFVector.size(); step++)
                {
                    for(unsigned int jjj=0; jjj<(this->GetGeometry()).size(); jjj++)
                        OldAcceleration += N[jjj] * BDFVector[step] * (this->GetGeometry())[jjj].FastGetSolutionStepValue(VELOCITY,step);
                }
 
                for(unsigned int enriched_id = 0; enriched_id < nenrichments; enriched_id++)
                {
                    const Matrix& enriched_grad = gauss_gradients[igauss];
//                     KRATOS_WATCH(enriched_grad);
 
 
                    //compute enrichment terms contribution
                    for (unsigned int inode = 0; inode < TNumNodes; inode++)
                    {
                        int base_index = (TDim + 1) * inode;
                        array_1d<double,TNumNodes> AGradN = ZeroVector(TNumNodes);
                        this->GetConvectionOperator(AGradN,AdvVel,DN_DX);
                        //momentum term
                        for (unsigned int k = 0; k < TDim; k++)
                        {
                            double convection_stab = wGauss * TauOne * enriched_grad(enriched_id,k)* Density * AGradN[inode];
                            double darcy_stab = wGauss * TauOne * enriched_grad(enriched_id,k) * DarcyTerm * N[inode];
 
                            //                      enrichment_terms_vertical[base_index + k] += velocity_stab + wGauss*N[inode]*enriched_grad(0, k);
                            enrichment_terms_vertical(base_index + k,enriched_id) += convection_stab - darcy_stab - wGauss * DN_DX(inode, k) * Nenriched(igauss, enriched_id);
                            enrichment_terms_horizontal(enriched_id,base_index + k) += convection_stab + darcy_stab + wGauss * DN_DX(inode, k) * Nenriched(igauss, enriched_id);
    //                             enrichment_terms_vertical[base_index + k] +=wGauss*N[inode]*enriched_grad(0, k); //-= wGauss * DN_DX(inode, k) * Nenriched(igauss, 0);
    //                            enrichment_terms_horizontal[base_index + k] -=Density*wGauss*N[inode]*enriched_grad(0, k); //   += Density*wGauss * DN_DX(inode, k) * Nenriched(igauss, 0);
                        }
                        //pressure term
                        for (unsigned int k = 0; k < TDim; k++)
                        {
                            double temp =  wGauss * TauOne* DN_DX(inode, k) * enriched_grad(enriched_id, k);
                            enrichment_terms_vertical(base_index + TDim,enriched_id) += temp;
                            enrichment_terms_horizontal(enriched_id,base_index + TDim) += temp;
                        }
                        //add acceleration enrichment term
                        for (unsigned int k = 0; k < TDim; k++)
                        {
                            double coeff = wGauss * TauOne *Density *  enriched_grad(enriched_id,k)*N[inode] * BDFVector[0];
                            enrichment_terms_horizontal(enriched_id,base_index + k) += coeff;
                            //i believe this shall not be here!! enriched_rhs += coeff * (old_vnode[k]);
    //                             enrichment_terms_vertical[base_index + k] +=wGauss*N[inode]*gauss_gradients[igauss](0, k); //-= wGauss * DN_DX(inode, k) * Nenriched(igauss, 0);
    //                            enrichment_terms_horizontal[base_index + k] -=Density*wGauss*N[inode]*gauss_gradients[igauss](0, k); //   += Density*wGauss * DN_DX(inode, k) * Nenriched(igauss, 0);
                        }
                    }
                    //compute diagonal enrichment terms
 
                    for (unsigned int k = 0; k < TDim; k++)
                    {
                        for(unsigned int lll=0; lll<nenrichments; lll++)
                        {
                            enrichment_diagonal(enriched_id,lll) += wGauss  * TauOne * enriched_grad(enriched_id, k) * enriched_grad(lll,k);
                        }
 
 
                        enriched_rhs[enriched_id] += wGauss * TauOne *Density * enriched_grad(enriched_id,k)*(bf[k]-OldAcceleration[k]); //changed the sign of the acc term
                    }
                }
            }
        }
 
 
//         if (ndivisions > 1)
//         {
//             KRATOS_WATCH(this->Id());
//             KRATOS_WATCH(ndivisions);
//         KRATOS_WATCH( (positive_volume+negative_volume )/Area);
//         }
//    KRATOS_WATCH("line 438");
        //lump mass matrix
        this->LumpMassMatrix(MassMatrix);
 
        //add mass matrix stabilization contributions
        for (unsigned int igauss = 0; igauss < Ngauss.size1(); igauss++)
        {
            //assigning the gauss data
            for (unsigned int k = 0; k < TNumNodes; k++)
                N[k] = Ngauss(igauss, k);
            double wGauss = volumes[igauss];
            // Calculate this element's fluid properties
            double Density;
            this->EvaluateInPoint(Density, DENSITY, N);
 
            double ElemSize = this->ElementSize(Area);
            double Viscosity = this->EffectiveViscosity(Density,N,DN_DX,ElemSize,rCurrentProcessInfo);
 
            // Get Advective velocity
            array_1d<double, 3 > AdvVel;
            this->GetAdvectiveVel(AdvVel, N);
            const double VelNorm = MathUtils<double>::Norm3(AdvVel);
 
            const double DarcyTerm = this->CalculateDarcyTerm(Density, Viscosity, c1, c2, N);
 
            double TauOne,TauTwo;
            this->CalculateStabilizationTau(TauOne, TauTwo, VelNorm, ElemSize, Density, Viscosity, DarcyTerm, rCurrentProcessInfo);
 
            // Add dynamic stabilization terms ( all terms involving a delta(u) )
            this->AddMassStabTerms(MassMatrix, Density, AdvVel, DarcyTerm, TauOne, N, DN_DX, wGauss);
 
        }
 
        //****************************************************
        //consider contributions of mass to LHS and RHS
        //add Mass Matrix to the LHS with the correct coefficient
        noalias(rLeftHandSideMatrix) += BDFVector[0]*MassMatrix;
 
        //do RHS -= MassMatrix*(BDFVector[1]*un + BDFVector[2]*u_(n-1))
        //note that the term related to BDFVector[0] is included in the LHS
        array_1d<double,LocalSize> aaa = ZeroVector(LocalSize);
        for(unsigned int k = 0; k<TNumNodes; k++)
        {
            unsigned int base=k*(TDim+1);
            for(unsigned int step=1; step<BDFVector.size(); step++)
            {
                const array_1d<double,3>& u = this->GetGeometry()[k].FastGetSolutionStepValue(VELOCITY,step);
                const double& bdf_coeff = BDFVector[step];
                aaa[base]   += bdf_coeff*u[0];
                aaa[base+1] += bdf_coeff*u[1];
                aaa[base+2] += bdf_coeff*u[2];
            }
        }
        noalias(rRightHandSideVector) -= prod(MassMatrix,aaa);
 
        //****************************************************
        //finalize computation of the residual
        // Now calculate an additional contribution to the residual: r -= rLeftHandSideMatrix * (u,p)
        VectorType U = ZeroVector(LocalSize);
        int LocalIndex = 0;
        for (unsigned int iNode = 0; iNode < TNumNodes; ++iNode)
        {
            array_1d< double, 3 > & rVel = this->GetGeometry()[iNode].FastGetSolutionStepValue(VELOCITY);
            for (unsigned int d = 0; d < TDim; ++d) // Velocity Dofs
            {
                U[LocalIndex] = rVel[d];
                ++LocalIndex;
            }
            U[LocalIndex] = this->GetGeometry()[iNode].FastGetSolutionStepValue(PRESSURE); // Pressure Dof
            ++LocalIndex;
        }
        noalias(rRightHandSideVector) -= prod(rLeftHandSideMatrix, U);
//                KRATOS_WATCH("line 517");
 
        //****************************************************
        //finalize computation of enrichment terms
        //(do static condensation) of enrichment terms
        //note that it each step we assume that the enrichment starts from 0
        if (ndivisions > 1)
        {
            //We only apply enrichment if we do not have an almost empty/full element
            if (positive_volume / Area > min_area_ratio && negative_volume / Area > min_area_ratio) {
                //finalize the computation of the rhs
                noalias(enriched_rhs) -= prod(enrichment_terms_horizontal,U);
 
                double max_diag = 0.0;
                for(unsigned int k=0; k<TDim+1; k++)
                    if(fabs(enrichment_diagonal(k,k) ) > max_diag) max_diag = fabs(enrichment_diagonal(k,k) );
                if(max_diag == 0) max_diag = 1.0;
 
                for (unsigned int i = 0; i < TDim; i++)
                {
                    const double di = fabs(distances[i]);
 
                    for (unsigned int j = i + 1; j < TDim + 1; j++)
                    {
                        const double dj = fabs(distances[j]);
 
                        if (distances[i] * distances[j] < 0.0) //cut edge
                        {
                            double sum_d = di + dj;
                            double Ni = dj / sum_d;
                            double Nj = di / sum_d;
 
                            double penalty_coeff = max_diag * 0.001; // h/BDFVector[0];
                            enrichment_diagonal(i, i) += penalty_coeff * Ni*Ni;
                            enrichment_diagonal(i, j) -= penalty_coeff * Ni*Nj;
                            enrichment_diagonal(j, i) -= penalty_coeff * Nj*Ni;
                            enrichment_diagonal(j, j) += penalty_coeff * Nj*Nj;
 
                        }
                    }
                }
 
                Matrix inverse_diag(nenrichments, nenrichments);
                double det;
                MathUtils<double>::InvertMatrix(enrichment_diagonal, inverse_diag, det);
 
                Matrix tmp = prod(inverse_diag, enrichment_terms_horizontal);
                noalias(rLeftHandSideMatrix) -= prod(enrichment_terms_vertical, tmp);
 
                Vector tmp2 = prod(inverse_diag, enriched_rhs);
                noalias(rRightHandSideVector) -= prod(enrichment_terms_vertical, tmp2);
            }
 
/*                for (unsigned int i = 0; i < LocalSize; i++)
                for (unsigned int j = 0; j < LocalSize; j++)
                    rLeftHandSideMatrix(i, j) -= inverse_diag_term * enrichment_terms_vertical[i] * enrichment_terms_horizontal[j];
            noalias(rRightHandSideVector) -= (inverse_diag_term*enriched_rhs )*enrichment_terms_vertical;
    */      }
 
//       KRATOS_WATCH("finished elem")
    }
 
    void Calculate(const Variable<array_1d<double, 3 > >& rVariable,
                           array_1d<double, 3 > & rOutput,
                           const ProcessInfo& rCurrentProcessInfo) override
    {
 
    }
 
    int Check(const ProcessInfo& rCurrentProcessInfo) const override
    {
        KRATOS_TRY
        // Perform basic element checks
        int ErrorCode = Kratos::Element::Check(rCurrentProcessInfo);
        if (ErrorCode != 0) return ErrorCode;
 
        // Checks on nodes
        // Check that the element's nodes contain all required SolutionStepData and Degrees of freedom
        for (unsigned int i = 0; i<this->GetGeometry().size(); ++i)
        {
            if (this->GetGeometry()[i].SolutionStepsDataHas(DISTANCE) == false)
                KRATOS_THROW_ERROR(std::invalid_argument, "missing DISTANCE variable on solution step data for node ", this->GetGeometry()[i].Id());
            if (this->GetGeometry()[i].SolutionStepsDataHas(VELOCITY) == false)
                KRATOS_THROW_ERROR(std::invalid_argument, "missing VELOCITY variable on solution step data for node ", this->GetGeometry()[i].Id());
            if (this->GetGeometry()[i].SolutionStepsDataHas(PRESSURE) == false)
                KRATOS_THROW_ERROR(std::invalid_argument, "missing PRESSURE variable on solution step data for node ", this->GetGeometry()[i].Id());
            if (this->GetGeometry()[i].SolutionStepsDataHas(MESH_VELOCITY) == false)
                KRATOS_THROW_ERROR(std::invalid_argument, "missing MESH_VELOCITY variable on solution step data for node ", this->GetGeometry()[i].Id());
            if (this->GetGeometry()[i].HasDofFor(VELOCITY_X) == false ||
                    this->GetGeometry()[i].HasDofFor(VELOCITY_Y) == false ||
                    this->GetGeometry()[i].HasDofFor(VELOCITY_Z) == false)
                KRATOS_THROW_ERROR(std::invalid_argument, "missing VELOCITY component degree of freedom on node ", this->GetGeometry()[i].Id());
            if (this->GetGeometry()[i].HasDofFor(PRESSURE) == false)
                KRATOS_THROW_ERROR(std::invalid_argument, "missing PRESSURE component degree of freedom on node ", this->GetGeometry()[i].Id());
        }
 
        // If this is a 2D problem, check that nodes are in XY plane
        if (this->GetGeometry().WorkingSpaceDimension() == 2)
        {
            for (unsigned int i = 0; i<this->GetGeometry().size(); ++i)
            {
                if (this->GetGeometry()[i].Z() != 0.0)
                    KRATOS_THROW_ERROR(std::invalid_argument, "Node with non-zero Z coordinate found. Id: ", this->GetGeometry()[i].Id());
            }
        }
        return 0;
        KRATOS_CATCH("");
    }
    std::string Info() const override
    {
        std::stringstream buffer;
        buffer << "TwoFluidVMS #" << this->Id();
        return buffer.str();
    }
    void PrintInfo(std::ostream& rOStream) const override
    {
        rOStream << "TwoFluidVMS" << TDim << "D";
    }
    //        /// Print object's data.
    //        virtual void PrintData(std::ostream& rOStream) const;
protected:
 
    void LumpMassMatrix(MatrixType& rMassMatrix)
    {
        for (unsigned int i = 0; i < rMassMatrix.size1(); ++i)
        {
            double diag_factor = 0.0;
            for (unsigned int j = 0; j < rMassMatrix.size2(); ++j)
            {
                diag_factor += rMassMatrix(i, j);
                rMassMatrix(i, j) = 0.0;
            }
            rMassMatrix(i, i) = diag_factor;
        }
    }
 
    virtual void AddPointContribution(double& rResult,
                                      const Variable< double >& rVariable,
                                      const array_1d< double, TNumNodes >& rShapeFunc,
                                      const double Weight = 1.0)
    {
        double temp = 0.0;
        this->EvaluateInPoint(temp, rVariable, rShapeFunc);
        rResult += Weight*temp;
    }
 
    void EvaluateInPoint(double& rResult,
                                 const Variable< double >& rVariable,
                                 const array_1d< double, TNumNodes >& rShapeFunc) override
    {
        //compute sign of distance on gauss point
        double dist = 0.0;
        for (unsigned int i = 0; i < TNumNodes; i++)
            dist += rShapeFunc[i] * this->GetGeometry()[i].FastGetSolutionStepValue(DISTANCE);
 
        double navg = 0.0;
        double value = 0.0;
        for (unsigned int i = 0; i < TNumNodes; i++)
        {
            if ( (dist * this->GetGeometry()[i].FastGetSolutionStepValue(DISTANCE)) > 0.0)
            {
                navg += 1.0;
                value += this->GetGeometry()[i].FastGetSolutionStepValue(rVariable);
            }
        }
        if(navg != 0)
            value /= navg;
        else
            ElementBaseType::EvaluateInPoint(value,rVariable,rShapeFunc);
        rResult = value;
    }
 
    virtual void AddPointContribution(array_1d< double, 3 > & rResult,
                                      const Variable< array_1d< double, 3 > >& rVariable,
                                      const array_1d< double, TNumNodes>& rShapeFunc,
                                      const double Weight = 1.0)
    {
        array_1d<double, 3 > temp = ZeroVector(3);
        this->EvaluateInPoint(temp, rVariable, rShapeFunc);
        rResult += Weight*temp;
    }
 
 
    void EvaluateInPoint(array_1d< double, 3 > & rResult,
                                 const Variable< array_1d< double, 3 > >& rVariable,
                                 const array_1d< double, TNumNodes >& rShapeFunc) override
    {
        //compute sign of distance on gauss point
        double dist = 0.0;
        for (unsigned int i = 0; i < TNumNodes; i++)
            dist += rShapeFunc[i] * this->GetGeometry()[i].FastGetSolutionStepValue(DISTANCE);
        double navg = 0.0;
        array_1d< double, 3 > value = ZeroVector(3);
        for (unsigned int i = 0; i < TNumNodes; i++)
        {
            if (dist * this->GetGeometry()[i].FastGetSolutionStepValue(DISTANCE) > 0.0)
            {
                navg += 1.0;
                value += this->GetGeometry()[i].FastGetSolutionStepValue(rVariable);
            }
        }
        if(navg != 0)
            value /= navg;
        else
            ElementBaseType::EvaluateInPoint(value,rVariable,rShapeFunc);
        rResult = value;
    }
 
 
    void AddIntegrationPointVelocityContribution(MatrixType& rDampingMatrix,
            VectorType& rDampRHS,
            const double Density,
            const double Viscosity,
            const array_1d< double, 3 > & rAdvVel,
            const double ReactionTerm,
            const double TauOne,
            const double TauTwo,
            const array_1d< double, TNumNodes >& rShapeFunc,
            const BoundedMatrix<double, TNumNodes, TDim >& rShapeDeriv,
            const double Weight)
    {
        const unsigned int BlockSize = TDim + 1;
 
        // If we want to use more than one Gauss point to integrate the convective term, this has to be evaluated once per integration point
        array_1d<double, TNumNodes> AGradN;
        this->GetConvectionOperator(AGradN, rAdvVel, rShapeDeriv); // Get a * grad(Ni)
 
        // Build the local matrix and RHS
        unsigned int FirstRow(0), FirstCol(0); // position of the first term of the local matrix that corresponds to each node combination
        double K, PDivV, L, qF; // Temporary results
 
        array_1d<double,3> BodyForce = ZeroVector(3);
        this->EvaluateInPoint(BodyForce,BODY_FORCE,rShapeFunc);
        BodyForce *= Density;
 
        array_1d<double, TNumNodes> StabilizationOperator = Density*AGradN;
        noalias(StabilizationOperator) -= ReactionTerm * rShapeFunc;
        StabilizationOperator *= TauOne;
 
        for (unsigned int i = 0; i < TNumNodes; ++i) // iterate over rows
        {
            for (unsigned int j = 0; j < TNumNodes; ++j) // iterate over columns
            {
                // Convection + Reaction: v *( a*Grad(u) + sigma*u )
                // For Darcy: sigma = A + B|u|
                K = rShapeFunc[i] * ( Density*AGradN[j] + ReactionTerm*rShapeFunc[j] );
                // Stabilization: (a * Grad(v) - sigma * N) * TauOne *( a*Grad(u) + sigma*u )
                K += StabilizationOperator[i] * ( Density*AGradN[j] + ReactionTerm*rShapeFunc[j] );
                K *= Weight;
 
                // q-p stabilization block (reset result)
                L = 0;
 
                const array_1d<double,3>& OldVel = this->GetGeometry()[j].FastGetSolutionStepValue(VELOCITY,1);
 
                for (unsigned int m = 0; m < TDim; ++m) // iterate over v components (vx,vy[,vz])
                {
                    // v-p block (pressure gradient)
                    double div_v_p = rShapeDeriv(i, m) * rShapeFunc[j];
                    double stab_grad_p = StabilizationOperator[i] * rShapeDeriv(j,m);
                    rDampingMatrix(FirstRow + m, FirstCol + TDim) += Weight * (stab_grad_p - div_v_p);
 
                    // q-u block (velocity divergence)
                    double q_div_u = rShapeFunc[i] * rShapeDeriv(j,m);
                    double stab_div_u = TauOne*rShapeDeriv(i,m)* ( Density*AGradN[j] + ReactionTerm * rShapeFunc[j] );
                    rDampingMatrix(FirstRow + TDim, FirstCol + m) += Weight * ( q_div_u + stab_div_u );
 
                    PDivV = rShapeDeriv(i, m) * rShapeFunc[j]; // Div(v) * p
                    rDampRHS[FirstCol + TDim] -=  Weight * PDivV*OldVel[m];
 
                    // q-p stabilization block
                    L += rShapeDeriv(i, m) * rShapeDeriv(j, m); // Stabilization: Grad(q) * TauOne * Grad(p)
 
                    for (unsigned int n = 0; n < TDim; ++n) // iterate over u components (ux,uy[,uz])
                    {
                        // Velocity block
                        rDampingMatrix(FirstRow + m, FirstCol + n) += Weight * TauTwo * rShapeDeriv(i, m) * rShapeDeriv(j, n); // Stabilization: Div(v) * TauTwo * Div(u)
                    }
                }
 
                // Write remaining terms to velocity block
                for (unsigned int d = 0; d < TDim; ++d)
                    rDampingMatrix(FirstRow + d, FirstCol + d) += K;
 
                // Write q-p stabilization block
                rDampingMatrix(FirstRow + TDim, FirstCol + TDim) += Weight * TauOne * L;
 
                // Update reference column index for next iteration
                FirstCol += BlockSize;
            }
 
            // Operate on RHS
            qF = 0.0;
            for (unsigned int d = 0; d < TDim; ++d)
            {
                rDampRHS[FirstRow + d] += Weight * StabilizationOperator[i] * BodyForce[d]; // (a * Grad(v) - sigma * N) * TauOne * (Density * BodyForce)
                qF += rShapeDeriv(i, d) * BodyForce[d];
            }
            rDampRHS[FirstRow + TDim] += Weight * TauOne * qF; // Grad(q) * TauOne * (Density * BodyForce)
 
            // Update reference indices
            FirstRow += BlockSize;
            FirstCol = 0;
        }
 
        this->AddViscousTerm(rDampingMatrix,rShapeDeriv,Viscosity*Weight);
    }
 
    void AddMassStabTerms(MatrixType& rLHSMatrix,
                          const double Density,
                          const array_1d<double, 3 > & rAdvVel,
                          const double ReactionTerm,
                          const double TauOne,
                          const array_1d<double, TNumNodes>& rShapeFunc,
                          const BoundedMatrix<double, TNumNodes, TDim>& rShapeDeriv,
                          const double Weight)
    {
        const unsigned int BlockSize = TDim + 1;
 
        unsigned int FirstRow(0), FirstCol(0);
        double K; // Temporary results
 
        // If we want to use more than one Gauss point to integrate the convective term, this has to be evaluated once per integration point
        array_1d<double, TNumNodes> AGradN;
        this->GetConvectionOperator(AGradN, rAdvVel, rShapeDeriv); // Get a * grad(Ni)
 
        array_1d<double, TNumNodes> StabilizationOperator = Density*AGradN;
        noalias(StabilizationOperator) -= ReactionTerm * rShapeFunc;
        StabilizationOperator *= TauOne;
 
        // Note: Dof order is (vx,vy,[vz,]p) for each node
        for (unsigned int i = 0; i < TNumNodes; ++i)
        {
            // Loop over columns
            for (unsigned int j = 0; j < TNumNodes; ++j)
            {
                // Delta(u) * TauOne * [ AdvVel * Grad(v) - sigma * N ] in velocity block
                K = Weight * StabilizationOperator[i] * Density * rShapeFunc[j];
 
                for (unsigned int d = 0; d < TDim; ++d) // iterate over dimensions for velocity Dofs in this node combination
                {
                    rLHSMatrix(FirstRow + d, FirstCol + d) += K;
                    // Delta(u) * TauOne * Grad(q) in q * Div(u) block
                    rLHSMatrix(FirstRow + TDim, FirstCol + d) += Weight * TauOne * rShapeDeriv(i, d) * Density * rShapeFunc[j];
                }
                // Update column index
                FirstCol += BlockSize;
            }
            // Update matrix indices
            FirstRow += BlockSize;
            FirstCol = 0;
        }
    }
 
    void CalculateStabilizationTau(
        double& TauOne,
        double& TauTwo,
        const double VelNorm,
        const double ElemSize,
        const double Density,
        const double DynamicViscosity,
        const double ReactionTerm,
        const ProcessInfo& rCurrentProcessInfo)
    {
        const double DynamicTerm = rCurrentProcessInfo[DYNAMIC_TAU] / rCurrentProcessInfo[DELTA_TIME];
        double InvTau = Density * ( DynamicTerm + 2.0*VelNorm / ElemSize ) + 4.0*DynamicViscosity/ (ElemSize * ElemSize) + ReactionTerm;
        TauOne = 1.0 / InvTau;
 
        TauTwo = DynamicViscosity + 0.5 * Density * ElemSize * VelNorm;
    }
 
    virtual double CalculateDarcyTerm(
        const double Density,
        const double DynamicViscosity,
        const double LinearCoefficient,
        const double NonlinearCoefficient,
        const array_1d<double, TNumNodes>& rShapefunctions) {
 
        array_1d<double,3> velocity;
        this->GetAdvectiveVel(velocity, rShapefunctions);
        const double velocity_norm = MathUtils<double>::Norm3(velocity);
 
        return DynamicViscosity * LinearCoefficient + Density * NonlinearCoefficient*velocity_norm;
    }
 
private:
    friend class Serializer;
    void save(Serializer& rSerializer) const override
    {
        KRATOS_SERIALIZE_SAVE_BASE_CLASS(rSerializer, ElementBaseType);
    }
    void load(Serializer& rSerializer) override
    {
        KRATOS_SERIALIZE_LOAD_BASE_CLASS(rSerializer, ElementBaseType);
    }
    TwoFluidVMS & operator=(TwoFluidVMS const& rOther);
    TwoFluidVMS(TwoFluidVMS const& rOther);
}; // Class TwoFluidVMS
template< unsigned int TDim,
          unsigned int TNumNodes >
inline std::istream & operator >>(std::istream& rIStream,
                                  TwoFluidVMS<TDim, TNumNodes>& rThis)
{
    return rIStream;
}
template< unsigned int TDim,
          unsigned int TNumNodes >
inline std::ostream & operator <<(std::ostream& rOStream,
                                  const TwoFluidVMS<TDim, TNumNodes>& rThis)
{
    rThis.PrintInfo(rOStream);
    rOStream << std::endl;
    rThis.PrintData(rOStream);
    return rOStream;
}
} // namespace Kratos.
#endif // KRATOS_TWO_FLUID_VMS_H_INCLUDED  defined