camclay.h

/*************************************************************************************
 * MechSys - A C++ library to simulate (Continuum) Mechanical Systems                *
 * Copyright (C) 2005 Dorival de Moraes Pedroso <dorival.pedroso at gmail.com>       *
 * Copyright (C) 2005 Raul Dario Durand Farfan  <raul.durand at gmail.com>           *
 *                                                                                   *
 * This file is part of MechSys.                                                     *
 *                                                                                   *
 * MechSys is free software; you can redistribute it and/or modify it under the      *
 * terms of the GNU General Public License as published by the Free Software         *
 * Foundation; either version 2 of the License, or (at your option) any later        *
 * version.                                                                          *
 *                                                                                   *
 * MechSys is distributed in the hope that it will be useful, but WITHOUT ANY        *
 * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A   *
 * PARTICULAR PURPOSE. See the GNU General Public License for more details.          *
 *                                                                                   *
 * You should have received a copy of the GNU General Public License along with      *
 * MechSys; if not, write to the Free Software Foundation, Inc., 51 Franklin Street, *
 * Fifth Floor, Boston, MA 02110-1301, USA                                           *
 *************************************************************************************/

#ifndef MECHSYS_CAMCLAY_H
#define MECHSYS_CAMCLAY_H

#ifdef HAVE_CONFIG_H
  #include "config.h"
#else
  #ifndef REAL
    #define REAL double
  #endif
#endif

#include "models/equilibmodel.h"
#include "tensors/tensors.h"
#include "tensors/operators.h"
#include "tensors/functions.h"
#include "util/util.h"
#include "numerical/autostepme.h"

using Tensors::Tensor2;
using Tensors::Tensor4;
using Tensors::Stress_p_q; // Mean and deviatoric Cambridge stress invariants
using Tensors::Reduce;     // Used in: Phi <- V:De:r - y0*HH0  and  dLam <- V:De:dEps/Phi
using Tensors::AddScaled;  // Z = a*X + b*Y
using Tensors::GerX;       // Used in: Dep <- (-1/Phi)(De:r)dy(V:De) + De
using Tensors::Psd;        // P symmetric deviatoric
using Tensors::IdyI;       // I dyadic I
using Tensors::I;          // Second order identity tensor

class CamClay : public EquilibModel
{
public:
    // Constructor
    CamClay(Array<REAL> const & Prms, Array<REAL> const & IniData);

    // Derived Methods
    String Name         () const { return "CamClay"; }
    void   TgStiffness  (Tensors::Tensor4 & D) const;
    void   StressUpdate (Tensors::Tensor2 const & DEps, Tensors::Tensor2 & DSig);
    void   Actualize    (Tensors::Tensor2 const & DSig, Tensors::Tensor2 & DEps);
    void   BackupState  ();
    void   RestoreState ();

    // Access Methods
    Tensors::Tensor2 const & Sig() const { return _sig; }
    Tensors::Tensor2 const & Eps() const { return _eps; }
    int  nInternalStateValues ()   const { return 2;    }
    void InternalStateValues  (Array<REAL>   & IntStateVals ) const { IntStateVals .resize(2);  IntStateVals[0]=_z0;  IntStateVals[1]=_v;   }
    void InternalStateNames   (Array<String> & IntStateNames) const { IntStateNames.resize(2); IntStateNames[0]="z0"; IntStateNames[1]="v"; }

    static size_t NPRMS;
    static size_t NIDAT;

private:
    // Numerator of dLam
    REAL _num_dLam;
    REAL _bkp_num_dLam;

    // Internal State Values
    REAL _z0;
    REAL _v;
    REAL _z0_bkp;
    REAL _v_bkp;

    // Parameters
    REAL _lam;
    REAL _kap;
    REAL _phics;
    REAL _G;

    // Derived constants
    REAL _Mcs;

    // Private methods
    void _calc_dF_dSig(Tensor2 const & Sig, REAL const & z0, Tensor2 & V) const;

    void _calc_grads_hards(REAL    const & v  ,        // In:  Specific volume
                           Tensor2 const & Sig,        // In:  Stress tensor
                           REAL    const & z0 ,        // In:  Internal variable
                           Tensor2       & r  ,        // Out: Plastic strain direction
                           Tensor2       & V  ,        // Out: Derivative of f ("loading" surface) w.r.t Sig (df_dSig)
                           REAL          & y0 ,        // Out: Derivative of f ("loading" surface) w.r.t z
                           REAL          & HH0,        // Out: Hardening modulae
                           REAL          & hp ) const; // Out: Plastic coeficient

    void _calc_De(REAL const & v, Tensor2 const & Sig, Tensor4 & De) const;
    void _calc_Ce(REAL const & v, Tensor2 const & Sig, Tensor4 & Ce) const;

    int _Dep_times_dEps(Tensor2 const & Eps,        // In: (actual) stress tensor
                        Tensor2 const & Sig,        // In: (actual) strain tensor
                        REAL    const & z0,         // In: (actual) internal variables
                        Tensor2 const & dEps,       // In: Stress increment
                        Tensor2       & dSig,       // Out: Strain increment
                        REAL          & dz0) const; // Out: Internal variables increment

    int _Cep_times_dSig(Tensor2 const & Sig,        // In: (actual) stress tensor
                        Tensor2 const & Eps,        // In: (actual) strain tensor
                        REAL    const & z0,         // In: (actual) internal variables
                        Tensor2 const & dSig,       // In: Stress increment
                        Tensor2       & dEps,       // Out: Strain increment
                        REAL          & dz0) const; // Out: Internal variables increment

    REAL _calc_yield_func(Tensor2 const & Sig, REAL const & z0) const;

    REAL _find_intersection(Tensor2 const & Sig, Tensor2 const & DSig_tr, Tensor2 & Sig_int) const;

    REAL _local_error(Tensor2 const & Ey    ,         // In: Stress or Strain error between low and high order evaluation (FE and ME)
                      Tensor2 const & y_high,         // In: Stress or Strain high order evaluation - "correct" (ME)
                      REAL    const & Ez0   ,         // In: Error on internal variables
                      REAL    const & z0_high) const; // In: Higher evaluation of internal variables

}; // class CamClay

size_t CamClay::NPRMS = 4;
size_t CamClay::NIDAT = 5;



inline CamClay::CamClay(Array<REAL> const & Prms, Array<REAL> const & IniData) // {{{
    : _num_dLam(-1.0/*elastic*/) // deve ser negativo, pois no inicio Dep eh indeterminado
{
    // ##################################################################### Setup Parameters

    /* example.mat
     * #-------------------------------
     * #            0      1     2   3
     * #          lam    kap phics   G
     * CamClay 0.0891 0.0196  31.6 200
     */

    // Check number of parameters
    const size_t n_prms = 4;
    if (Prms.size()!=n_prms)
        throw new Fatal(_("CamClay::Constructor: The number of parameters (%d) is incorrect. It must be equal to %d"), Prms.size(), n_prms);

    // Parameters
    _lam   = Prms[0];
    _kap   = Prms[1];
    _phics = Prms[2];
    _G     = Prms[3];

    // Derived constants
    REAL sin_phi = sin(Util::ToRad(_phics));
    _Mcs = 6.0*sin_phi/(3.0-sin_phi);
    
    // ##################################################################### Setup Initial State

    // Check number of initial data
    if (IniData.size()!=5) // Layered analysis
        throw new Fatal(_("CamClay::Constructor: The number of initial data is not sufficiet (it must be equal to 5). { SigX SigY SigZ vini OCR }"));

    // Read initial data
    REAL sig_x = IniData[0]; // X stress component
    REAL sig_y = IniData[1]; // Y stress component
    REAL sig_z = IniData[2]; // Z stress component
            _v = IniData[3]; // Initial specific volume
    REAL   OCR = IniData[4]; // Over consolidation rate (not used here)

    // Fill stress and strain tensors
    _sig = sig_x, sig_y, sig_z, 0.0*Util::Sq2(), 0.0*Util::Sq2(), 0.0*Util::Sq2(); // Mandel representation
    _eps = 0.0,0.0,0.0,0.0*Util::Sq2(),0.0*Util::Sq2(),0.0*Util::Sq2();

    // Calculate internal variables
    REAL p = (_sig(0)+_sig(1)+_sig(2))/3.0;
    _z0 = OCR*p;
    
} // }}}

inline void CamClay::TgStiffness(Tensors::Tensor4 & D) const // {{{
{
    if (_num_dLam<0) // Elastic tangent stiffness
    {
        _calc_De(_v, _sig, D); // D <- De
    }
    else // Elastoplastic tangent stiffness
    {
        // Gradients and Hardening
        Tensor2 r;
        Tensor2 V;
        REAL    y0;
        REAL    HH0;
        REAL    hp;
        _calc_grads_hards(_v, _sig, _z0, r, V, y0, HH0, hp);

        // Dep and Phi
        Tensors::Tensor4 De;
        _calc_De(_v, _sig, De);
        REAL Phi = Reduce(V,De,r)+hp;   // Phi = V:De:r - y0*HH0
        Tensors::GerX(-1.0/Phi,De,r,V,De,De, D); // D <- Dep = (-1/Phi) * (De:r)dy(V:De) + De
    }
    
} // }}}

inline void CamClay::Actualize(Tensors::Tensor2 const & DSig, Tensors::Tensor2 & DEps) // P{{{
{
    // Initial _eps
    Tensor2 eps_ini = _eps;

    // Stress invariants of the increment of stress
    Tensor2 dsig_tr = DSig;
    Tensor2  sig_tr; sig_tr=_sig+dsig_tr;

    // Yield function values
    REAL F    = _calc_yield_func(_sig,  _z0);
    REAL F_tr = _calc_yield_func(sig_tr,_z0);

    // Check loading condition
    const REAL FTOL=1.0e-3;
    enum Situation {EL, EL_EPL, EPL}; // EL: Elastic-Loading
    Situation sit;                    // EL_EPL: Elastic-Loading + ElastoPlastic-Loading
    if (F<0.0)                        // EPL :ElastoPlastic-Loading
    {
        if (F_tr<=0.0) sit=EL;
        else           sit=EL_EPL;
    }
    else if (F>=0.0 && F/_z0<=FTOL)
    {
        if (F_tr<=0.0) sit=EL;
        else
        {
            // Gradients
            Tensor2 dF_dSig;
            _calc_dF_dSig(_sig, _z0, dF_dSig);
            REAL scalar = Tensors::Dot(dF_dSig,dsig_tr);
            if (scalar>=0.0) sit=EPL;
            else             sit=EL_EPL;
        }
    }
    else // F>FTOL
        throw new Fatal(_("CamClay::Actualize: F/z0 (=%e) >FTOL (%e)"),F/_z0,FTOL);

    // Elastic loading
    if (sit==EL)
    {
        // Elastic tensors
        Tensor4 Ce;
        _calc_Ce(_v, _sig, Ce);

        // DEps
        Tensor2 DEps;
        Tensors::Dot(Ce,DSig, DEps);

        // Actualize
        _sig += DSig;
        _eps += DEps;
        // _z stay unchanged
    }
    else // sit==EL_EPL || sit==EPL
    {
        // Auxiliar tensor
        Tensor2 dsig = DSig;

        // Find interpolation
        if (sit==EL_EPL)
        {
            //std::cout << "(Actualize): F = " << F << ", F_tr = " << F_tr << std::endl;

            // Find intersection
            Tensor2 sig_int;
            _find_intersection(_sig,dsig_tr, sig_int);

            // Elastic and Elastoplastic parts of stress
            Tensor2 dsig_el,dsig_epl;
             dsig_el = sig_int - _sig;
            dsig_epl = dsig_tr - dsig_el;

            // Elastic tensors
            Tensor4 Ce;
            _calc_Ce(_v, _sig, Ce);

            // DEps
            Tensor2 DEps;
            Tensors::Dot(Ce,dsig_el, DEps);

            // Actualize
            _sig += dsig_el;
            _eps += DEps;
            // _z stay unchanged

            // Continue with elastoplastic part (fill augmented vectors)
            dsig = dsig_epl;
        }

        // Forward-Euler
        int n_div=_isc.FE_ndiv();
        dsig = dsig/n_div;
        for (int i=0; i<n_div; ++i)
        {
            REAL dz0 = 0.0;
            _Cep_times_dSig(_sig, _eps, _z0, dsig, DEps, dz0);
            _sig += dsig;
            _eps += DEps;
            _z0  += dz0;
        }
    }

    // Calculate the total (secant) increment of strain
    DEps = _eps - eps_ini;

    // Update internal state (secant)
    _v += -(DEps(0)+DEps(1)+DEps(2))*_v; // dv=-dEv*v

} // }}}

inline void CamClay::StressUpdate(Tensors::Tensor2 const & DEps, Tensors::Tensor2 & DSig) // P{{{
{
    // Initial _sig
    Tensor2 sig_ini = _sig;

    // De
    Tensor4 De;
    _calc_De(_v,_sig, De);

    // Trial state
    Tensor2 dsig_tr; Tensors::Dot(De,DEps, dsig_tr);
    Tensor2  sig_tr; sig_tr=_sig+dsig_tr;

    // Yield function values
    REAL F    = _calc_yield_func(_sig,  _z0);
    REAL F_tr = _calc_yield_func(sig_tr,_z0);

    // Check loading condition
    const REAL FTOL=1.0e-1;
    enum Situation {EL, EL_EPL, EPL}; // EL: Elastic-Loading
    Situation sit;                    // EL_EPL: Elastic-Loading + ElastoPlastic-Loading
    if (F<0.0)                        // EPL :ElastoPlastic-Loading
    {
        if (F_tr<=0.0) sit=EL;
        else           sit=EL_EPL;
    }
    else if (F>=0.0 && F<=FTOL)
    {
        if (F_tr<=0.0) sit=EL;
        else
        {
            // Calculate the numerator of dLam
            Tensor2 V; // df_dsig
            _calc_dF_dSig(_sig,_z0, V);
            _num_dLam = Tensors::Dot(V,dsig_tr); // dLam = (V:De:deps)/Phi
            if (_num_dLam>=0.0) sit=EPL;
            else                sit=EL_EPL;
        }
    }
    else // F>FTOL
        throw new Fatal("CamClay::StressUpdate: F>0.0");

    // Elastic loading
    if (sit==EL)
    {
        // Set the numerator of dLam so that it may be used in TgStiffness
        _num_dLam = -1.0;

        // DSig
        DSig = dsig_tr;

        // Actualize
        _sig += DSig;
        _eps += DEps;
        // _z stay unchanged
    }
    else // sit==EL_EPL || sit==EPL
    {
        // Set the numerator of dLam so that it may be used in TgStiffness
        _num_dLam = +1.0;

        // Auxiliar tensor
        Tensor2 deps = DEps;

        // Find interpolation
        if (sit==EL_EPL)
        {
            //std::cout << "(StressUpdate): F = " << F << ", F_tr = " << F_tr << std::endl;

            // Find intersection
            Tensor2 sig_int;
            _find_intersection(_sig,dsig_tr, sig_int);

            // Ce
            Tensor4 Ce;
            _calc_Ce(_v,_sig, Ce);

            // Elastic and Elastoplastic parts of stress ans strain
            Tensor2 dsig_el = sig_int - _sig;
            Tensor2 deps_el;
            Tensors::Dot(Ce,dsig_el, deps_el);

            // Actualize
            _sig += dsig_el;
            _eps += deps_el;
            // _z stay unchanged

            // Continue with elastoplastic part
            deps = DEps - deps_el;
        }



        if (EquilibModel::_isc.Type()==IntegSchemesCtes::FE)
        {
            // Forward-Euler - Update (Elastoplastic)
            int n_div=_isc.FE_ndiv();
            deps = deps/n_div;
            for (int i=0; i<n_div; ++i)
            {
                REAL dz0;
                _Dep_times_dEps(_eps, _sig, _z0, deps, DSig, dz0);
                _sig += DSig;
                _eps += deps;
                _z0  += dz0;
            }
        }
        else
        {
            // TimeInteg
            AutoStepME<Tensor2,Tensor2,REAL,CamClay> TI(EquilibModel::_isc);

            // Update stress (_sig), internal variables (_z) and strain (_eps) state
            int nss = TI.Solve(this,                      // Address of this instance of GenericEP class
                               &CamClay::_Dep_times_dEps, // Pointer to a function that calculates {dSig}=[Dep]:{dEps}
                               &CamClay::_local_error,    // Pointer to a function that calculates local error on dEps
                               _eps, _sig, _z0, deps);
            // Check num_sub_steps
            if (nss==-1)
                throw new Fatal(_("CamClay::StressUpdate: (Loading) Number of max sub-steps (%d) was not sufficient for AutoStepME."), EquilibModel::_isc.ME_maxSS());
        }
    }

    // Calculate the total (secant) increment of stress
    DSig = _sig - sig_ini;

    // Update internal state
    _v += -(DEps(0)+DEps(1)+DEps(2))*_v; // dv=-dEv*v

} // }}}

inline void CamClay::_calc_dF_dSig(Tensor2 const & Sig, REAL const & z0, Tensor2 & V) const // {{{
{
    // Invariants
    REAL p,q;
    Stress_p_q(Sig,p,q);
    Tensor2 S;
    S = Sig - p*I;

    // Gradients
    REAL MM = _Mcs*_Mcs;
          V = (MM*(2.0*p-z0)/3.0)*I + 3.0*S;
} // }}}

inline void CamClay::_calc_grads_hards(REAL    const & v  ,       // In:  Specific volume // {{{
                                       Tensor2 const & Sig,       // In:  Stress tensor
                                       REAL    const & z0 ,       // In:  Internal variable
                                       Tensor2       & r  ,       // Out: Plastic strain direction
                                       Tensor2       & V  ,       // Out: Derivative of f ("loading" surface) w.r.t Sig (df_dSig)
                                       REAL          & y0 ,       // Out: Derivative of f ("loading" surface) w.r.t z
                                       REAL          & HH0,       // Out: Hardening modulae
                                       REAL          & hp ) const // Out: Plastic coeficient
{
    // Invariants
    REAL p,q;
    Stress_p_q(Sig,p,q);
    Tensor2 S;
    S = Sig - p*I;

    // Gradients
    REAL MM = _Mcs*_Mcs;
          V = (MM*(2.0*p-z0)/3.0)*I + 3.0*S;
          r = V;
         y0 = -MM*p;

    // Hardening
    REAL tr_r = r(0)+r(1)+r(2);
    REAL  chi = (_lam - _kap)/v;
          HH0 = z0*tr_r/chi;

    // Plastic coefficient
    hp = -y0*HH0;

} // }}}

inline void CamClay::_calc_De(REAL const & v, Tensor2 const & Sig, Tensor4 & De) const // {{{
{
    // Invariants
    REAL p = (Sig(0)+Sig(1)+Sig(2))/3.0;

    // Calculate Bulk modulus
    REAL K = p*v/_kap;

    // Calc De
    Tensors::AddScaled(2.0*_G,Tensors::Psd, K,Tensors::IdyI, De);

} // }}}

inline void CamClay::_calc_Ce(REAL const & v, Tensor2 const & Sig, Tensor4 & Ce) const // {{{
{
    // Invariants
    REAL p = (Sig(0)+Sig(1)+Sig(2))/3.0;

    // Calculate Bulk modulus
    REAL K = p*v/_kap;

    // Calc Ce
    AddScaled(1.0/(2.0*_G), Psd, 1.0/(9.0*K), IdyI,  Ce);
} // }}}

inline int CamClay::_Dep_times_dEps(Tensor2 const & Eps,       // In: (actual) stress tensor {{{
                                    Tensor2 const & Sig,       // In: (actual) strain tensor
                                    REAL    const & z0,        // In: (actual) internal variables
                                    Tensor2 const & dEps,      // In: Stress increment
                                    Tensor2       & dSig,      // Out: Strain increment
                                    REAL          & dz0) const // Out: Internal variables increment
{
    // Gradients and Hardening
    REAL    v=_v; // using (constant) current specific volume
    Tensor2 r;
    Tensor2 V;
    REAL    y0;
    REAL    HH0;
    REAL    hp;
    _calc_grads_hards(v, Sig, z0, r, V, y0, HH0, hp);

    // Dep and Phi
    Tensors::Tensor4 De,Dep;
    _calc_De(v, Sig, De);
    REAL Phi = Reduce(V,De,r)+hp;              // Phi = V:De:r - y0*HH0
    Tensors::GerX(-1.0/Phi,De,r,V,De,De, Dep); // Dep = (-1/Phi) * (De:r)dy(V:De) + De

    // dSig and dLam
    Tensors::Dot(Dep, dEps, dSig);     // dSig = Dep:dEps
    REAL dLam = Reduce(V,De,dEps)/Phi; // dLam = V:De:dEps/Phi

    // dz0
    dz0 = dLam * HH0;

    return 0;

} // }}}

inline int CamClay::_Cep_times_dSig(Tensor2 const & Sig,       // In: (actual) stress tensor {{{
                                    Tensor2 const & Eps,       // In: (actual) strain tensor
                                    REAL    const & z0,        // In: (actual) internal variables
                                    Tensor2 const & dSig,      // In: Stress increment
                                    Tensor2       & dEps,      // Out: Strain increment
                                    REAL          & dz0) const // Out: Internal variables increment
{
    // Stop if excessive deviatoric strains
    REAL ev,ed;
    Tensors::Strain_Ev_Ed(Eps, ev,ed);
    if (ed>EquilibModel::_isc.EdMax()) return 1; // -1 => failed;  0 => continue;  1 => stop

    // Gradients and Hardening
    REAL    v=_v; // using (constant) current specific volume
    Tensor2 r;
    Tensor2 V;
    REAL    y0;
    REAL    HH0;
    REAL    hp;
    _calc_grads_hards(v, Sig, z0, r, V, y0, HH0, hp);

    // Cep
    Tensor4 Cep;
    _calc_Ce(v, Sig, Cep);        // Cep <- Ce
    Tensors::Ger(1.0/hp,r,V,Cep); // Cep <- (1/cp)*(r dyadic V) + Ce

    // dEps and dLam
    Tensors::Dot(Cep, dSig, dEps);       // dEps <- Cep:dSig
    REAL dLam = Tensors::Dot(V,dSig)/hp; // dLam <- V:dSig/cp

    // dz0
    dz0 = dLam * HH0;

    return 0;

} // }}}

inline REAL CamClay::_calc_yield_func(Tensor2 const & Sig, REAL const & z0) const // {{{
{
    REAL p,q;
    Stress_p_q(Sig,p,q);
    REAL F = _Mcs*_Mcs*p*(p-z0)+q*q;
    return F/fabs(z0);
} // }}}

inline REAL CamClay::_find_intersection(Tensor2 const & Sig, Tensor2 const & DSig_tr, Tensor2 & Sig_int) const // {{{
{
    Tensor2 Sig_tr;
    Sig_tr = Sig + DSig_tr;

    REAL F     = _calc_yield_func(Sig,    _z0);
    REAL F_tr  = _calc_yield_func(Sig_tr, _z0);
    REAL alpha;
    REAL F_int;
    
    // Find alpha for intersection and the intersection (Sig_int) itself

    //Using Brent’s method, find the root of a function func known to lie between x1 and x2. The
    //root, returned as zbrent, will be refined until its accuracy is tol.

    const int  ITMAX=20;   // Maximum allowed number of iterations.
    const REAL EPS=3.0e-8; // Machine REALing-point precision.
    const REAL tol=1.0e-5;

    REAL a=0.0; // x1
    REAL b=1.0; // x2
    REAL c=1.0; // x2
    REAL d=0,e=0,min1=0,min2=0;
    REAL fa=F;    //(*func)(a);
    REAL fb=F_tr; //(*func)(b);
    REAL fc,p,q,r,s,tol1,xm;
    if ((fa > 0.0 && fb > 0.0) || (fa < 0.0 && fb < 0.0))
    {
        Sig_int = Sig;
        return 0.0;
        //throw new Fatal("CamClay::_find_intersection: Root must be bracketed in zbrent");
    }
    fc=fb;
    int iter;
    for (iter=1;iter<=ITMAX;iter++)
    {
        if ((fb > 0.0 && fc > 0.0) || (fb < 0.0 && fc < 0.0))
        {
            c=a;   //Rename a, b, c and adjust bounding interval
            fc=fa; // d.
            e=d=b-a;
        }
        if (fabs(fc) < fabs(fb))
        {
            a=b;
            b=c;
            c=a;
            fa=fb;
            fb=fc;
            fc=fa;
        }
        tol1=2.0*EPS*fabs(b)+0.5*tol; // Convergence check.
        xm=0.5*(c-b);
        if (fabs(xm) <= tol1 || fb == 0.0)
        {
              alpha = b;
            Sig_int = Sig + alpha*DSig_tr;
            break;
        }
        if (fabs(e) >= tol1 && fabs(fa) > fabs(fb))
        {
            s=fb/fa; // Attempt inverse quadratic interpolation.
            if (a == c)
            {
                p=2.0*xm*s;
                q=1.0-s;
            }
            else
            {
                q=fa/fc;
                r=fb/fc;
                p=s*(2.0*xm*q*(q-r)-(b-a)*(r-1.0));
                q=(q-1.0)*(r-1.0)*(s-1.0);
            }
            if (p > 0.0) q = -q; // Check whether in bounds.
            p=fabs(p);
            min1=3.0*xm*q-fabs(tol1*q);
            min2=fabs(e*q);
            if (2.0*p < (min1 < min2 ? min1 : min2))
            {
                e=d; // Accept interpolation.
                d=p/q;
            }
            else
            {
                d=xm; // Interpolation failed, use bisection.
                e=d;
            }
        }
        else
        { // Bounds decreasing too slowly, use bisection.
            d=xm;
            e=d;
        }
        a=b; // Move last best guess to a.
        fa=fb;
        if (fabs(d) > tol1) // Evaluate new trial root.
            b += d;
        else
            b += Util::Sign(tol1,xm);

        // Calculate function value at intersection
        //fb=(*func)(b);
        // Trial Intersection
          alpha = b;
        Sig_int = Sig + alpha*DSig_tr;
          F_int = _calc_yield_func(Sig_int, _z0);
             fb = F_int;
    }
    if (iter>ITMAX)
        throw new Fatal("CamClay::_find_intersection: Maximum number of iterations exceeded in zbrent");

    return alpha;

} // }}}

inline void CamClay::BackupState() // {{{
{
    _sig_bkp = _sig;
    _eps_bkp = _eps;
      _v_bkp = _v;
     _z0_bkp = _z0;
    _bkp_num_dLam = _num_dLam;
} // }}}

inline void CamClay::RestoreState() // {{{
{
    _sig = _sig_bkp;
    _eps = _eps_bkp;
      _v =   _v_bkp;
     _z0 =  _z0_bkp;
    _num_dLam = _bkp_num_dLam;
} // }}}

inline REAL CamClay::_local_error(Tensor2 const & Ey    ,         // In: Stress or Strain error between low and high order evaluation (FE and ME) {{{
                                  Tensor2 const & y_high,         // In: Stress or Strain high order evaluation - "correct" (ME)
                                  REAL    const & Ez0   ,         // In: Error on internal variables
                                  REAL    const & z0_high) const  // In: Higher evaluation of internal variables
{
    REAL Err_y = Tensors::Norm(Ey)/Tensors::Norm(y_high);
    REAL Err_z0 = fabs(Ez0)/fabs(z0_high);
    return (Err_y>Err_z0 ? Err_y : Err_z0);
} // }}}




// Allocate a new CamClay model
EquilibModel * CamClayMaker(Array<REAL> const & Prms, Array<REAL> const & IniData) // {{{
{
    return new CamClay(Prms, IniData);
} // }}}

// Register an CamClay model into ModelFactory array map
int CamClayRegister() // {{{
{
    EquilibModelFactory["CamClay"] = CamClayMaker;
    return 0;
} // }}}

// Execute the autoregistration
int __CamClay_dummy_int = CamClayRegister();

#endif // MECHSYS_CAMCLAY_H

// vim:fdm=marker

---