/****************************************************************************
*
*                                 M U E S L I   v 1.8
*
*
*     Copyright 2020 IMDEA Materials Institute, Getafe, Madrid, Spain
*     Contact: muesli.materials@imdea.org
*     Author: Ignacio Romero (ignacio.romero@imdea.org)
*
*     This file is part of MUESLI.
*
*     MUESLI 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 3 of the License, or
*     (at your option) any later version.
*
*     MUESLI 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 MUESLI.  If not, see <http://www.gnu.org/licenses/>.
*
****************************************************************************/


#ifndef WITHEIGEN

#include <iostream>
#include <iomanip>
#include <algorithm>
#include <stdexcept>
#include <utility>
#include <cmath>
#include <cassert>
#include <cstdlib>
#include <vector>
#include "muesli/Math/mtensor.h"


#define INIT2ZERO

static double randomUniform(const double a, const double b);

double randomUniform(const double low, const double up)
{
    static bool initialized = false;
    
    if (!initialized)
    {
        srand((unsigned)time(0));
        initialized = true;
    }
    
    int     random_int = rand();
    double  random_dou = ( static_cast<double_t>(random_int))/ static_cast<double_t>(RAND_MAX);
    
    return random_dou * (up-low) + low;
}



ivector::ivector()
{
#ifdef INIT2ZERO
    x[0] = x[1] = x[2] = 0.0;
#endif
}




ivector::ivector(const double* a)
{
    x[0] = a[0];
    x[1] = a[1];
    x[2] = a[2];
}




ivector::ivector(const double alpha, const double beta, const double gamma)
{
    x[0] = alpha;
    x[1] = beta;
    x[2] = gamma;
}




ivector::ivector(const std::vector<double>& v, const size_t start)
{
    x[0] = v[start];
    x[1] = v[start+1];
    x[2] = v[start+2];
}




std::ostream& operator<<(std::ostream &os, const ivector &v)
{
    os  << std::setprecision(10)
        << v.x[0] << " " << v.x[1] << " " << v.x[2];

    return os;
}



double ivector::angleWith(const ivector& v2) const
{
    return ( dot(v2) / ( norm() * v2.norm() ) );
}




void ivector::changeSign()
{
    x[0] = -x[0];
    x[1] = -x[1];
    x[2] = -x[2];
}




ivector ivector::cross(const ivector &w) const
{
    return ivector(x[1]*w[2] - x[2]*w[1],
                   x[2]*w[0] - x[0]*w[2],
                   x[0]*w[1] - x[1]*w[0]);
}




void ivector::extractFrom(const iquaternion& quat)
{
    double qnorm  = sqrt(quat.x()*quat.x() + quat.y()*quat.y() + quat.z()*quat.z());
    double mm     = (1.0 < qnorm) ? 1.0 : qnorm;
    //double rotnr2 = asin( mm );
    double rotnr2 = atan2( mm, quat.w() );
    double rotnrm = rotnr2 * 2.0;
    
    double rotfac = (qnorm > 1.0e-10) ? rotnrm / qnorm : 2.0;
    
    x[0] = rotfac * quat.x();
    x[1] = rotfac * quat.y();
    x[2] = rotfac * quat.z();
}




void ivector::extractFrom(const irotation& r)
{
    extractFrom( iquaternion(r) );
}




double ivector::max() const
{
    return std::max<double>( (*this)[0], std::max<double>((*this)[1],(*this)[2]));
}




double ivector::min() const
{
    return std::min<double>( (*this)[0], std::min<double>((*this)[1],(*this)[2]));
}



void ivector::normalize()
{
    (*this) *= (1.0/norm());
}




ivector ivector::periodicProjection(const double side) const
{
    return ivector(x[0] - side * round(x[0]/side),
                   x[1] - side * round(x[1]/side),
                   x[2] - side * round(x[2]/side));
}




void ivector::print(std::ostream &of) const
{
    of << *this;
}




// a random vector with components in [0,1]
void ivector::setRandom()
{
    const double dm = 1.0/static_cast<double>(RAND_MAX);
    x[0] = static_cast<double>(rand()) * dm;
    x[1] = static_cast<double>(rand()) * dm;
    x[2] = static_cast<double>(rand()) * dm;
}




void ivector::setRandomUnit()
{
    // random number in [-1,1] and [0,2Pi]
    const double dm = 1.0/static_cast<double>(RAND_MAX);
    double r1 = static_cast<double>(rand()) * dm;
    double r2 = static_cast<double>(rand()) * dm * M_PI + M_PI;
    
    x[0] = cos(r2) * sqrt(1.0 - r1*r1);
    x[1] = sin(r2) * sqrt(1.0 - r1*r1);
    x[2] = r1;
}




void ivector::setZero()
{
    x[0] = x[1] = x[2] = 0.0;
}




double ivector::tripleProduct(const ivector& v1, const ivector &v2, const ivector &v3)
{
    const ivector tmp(v1.cross(v2));
    return v3.dot(tmp);
}




itensor::itensor()
{
#ifdef INIT2ZERO
    a[0][0] = a[0][1] = a[0][2] = 0.0;
    a[1][0] = a[1][1] = a[1][2] = 0.0;
    a[2][0] = a[2][1] = a[2][2] = 0.0;
#endif
}




itensor::itensor(const itensor &t)
{
    a[0][0] = t.a[0][0]; a[0][1] = t.a[0][1]; a[0][2] = t.a[0][2];
    a[1][0] = t.a[1][0]; a[1][1] = t.a[1][1]; a[1][2] = t.a[1][2];
    a[2][0] = t.a[2][0]; a[2][1] = t.a[2][1]; a[2][2] = t.a[2][2];
}




itensor::itensor(const ivector& col1, const ivector& col2, const ivector& col3)
{
    const int ndm(3);
    for (size_t i=0; i<ndm; i++)
    {
        a[i][0] = col1[i];
        a[i][1] = col2[i];
        a[i][2] = col3[i];
    }
}




itensor::itensor(const double a00, const double a01, const double a02,
                 const double a10, const double a11, const double a12,
                 const double a20, const double a21, const double a22)
{
    a[0][0] = a00;  a[0][1] = a01;  a[0][2] = a02;
    a[1][0] = a10;  a[1][1] = a11;  a[1][2] = a12;
    a[2][0] = a20;  a[2][1] = a21;  a[2][2] = a22;
}




itensor::itensor(const double m[3][3])
{
    const int ndm(3);
    for (size_t i=0; i<ndm; i++)
    {
        for (size_t j=0; j<ndm; j++)
        {
            a[i][j] = m[i][j];
        }
    }
}




void itensor::addDyadic(const ivector &m, const ivector &n)
{
    a[0][0] += m[0]*n[0];
    a[0][1] += m[0]*n[1];
    a[0][2] += m[0]*n[2];
    a[1][0] += m[1]*n[0];
    a[1][1] += m[1]*n[1];
    a[1][2] += m[1]*n[2];
    a[2][0] += m[2]*n[0];
    a[2][1] += m[2]*n[1];
    a[2][2] += m[2]*n[2];
}




void itensor::addSymmetrizedDyadic(const ivector &m, const ivector &n)
{
    double tmp;
    
    a[0][0] +=  m[0]*n[0];
    a[1][1] +=  m[1]*n[1];
    
    tmp      =  0.5*(m[0]*n[1]+m[1]*n[0]);
    a[0][1] +=  tmp;
    a[1][0] +=  tmp;
    
    tmp      = 0.5*(m[0]*n[2]+m[2]*n[0]);
    a[0][2] += tmp;
    a[2][0] += tmp;
    
    tmp      = 0.5*(m[1]*n[2]+m[2]*n[1]);
    a[1][2] += tmp;
    a[2][1] += tmp;
    
    a[2][2] += m[2]*n[2];
}




// linearized exponential map dexp(psi)
/*
 *  Notes:      d = norm(psi)
 *              n = psi/d
 *              nn= n otimes n
 *
 *              h = nn + sin(d)/d (eye-nn) + (1-cos(d))/d hatn,
 */
void itensor::beDexp(const ivector &psi)
{
    double s, c, d, z;
    
    double b = psi.dot(psi);
    double q;
    
    // limit case
    if (b < 1.0e-12)
    {
        s = (1.0 - 0.05*b)/6.0;
        c =  0.5 - b*(1.0 - b/30.0)/24.0;
        q =  1.0 - b*s;
    }
    else
    {
        d = sqrt(b);
        z = sin(d)/d;
        c = (1.0 - cos(d))/b;
        s = (1.0-z)/b;
        q = z;
    }
    
    skewtensor hn(psi);
    for (size_t i=0; i<3; i++)
    {
        for (size_t j=0; j<3; j++)
        {
            a[i][j] = s*psi[i]*psi[j] + c*hn(i,j);
        }
        a[i][i] +=  q;
    }
}




// inverse of the linearized exp operator
void itensor::beDexpinv(const ivector &theta)
{
    double f, c;
    double norm2 = theta.dot(theta);
    
    if (norm2 < 1e-12)
    {
        // new expansion
        f = 1.0 - norm2/12.0;
        c = 1.0/12.0 + norm2/720.0;
    }
    else
    {
        double norm = sqrt(norm2);
        f    = 0.5*norm/(tan(0.5*norm));
        c    = (1.0 - f)/norm2;
    }
    
    skewtensor htheta(theta);
    (*this) = c * itensor::dyadic(theta, theta) - 0.5*htheta + f * itensor::identity();
}




/* antisymmetrize a tensor
 */
void itensor::beSkew()
{
    a[0][0] = 0.0;
    a[1][1] = 0.0;
    a[2][2] = 0.0;
    
    a[0][1] = 0.5*(a[0][1]-a[1][0]);
    a[0][2] = 0.5*(a[0][2]-a[2][0]);
    a[1][2] = 0.5*(a[1][2]-a[2][1]);
    
    a[1][0] = -a[0][1];
    a[2][0] = -a[0][2];
    a[2][1] = -a[1][2];
}




ivector itensor::col(const size_t n) const
{
    return ivector( a[0][n], a[1][n], a[2][n] );
}




double itensor::contract(const itensor &U) const
{
    double d=0.0;
    const int ndm(3);
    
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            d += a[i][j] * U.a[i][j];
    return d;
}




double itensor::determinant() const
{
    double d;
    
    d = +   a[0][0]*a[1][1]*a[2][2]
    +   a[1][0]*a[2][1]*a[0][2]
    +   a[2][0]*a[0][1]*a[1][2]
    -   a[0][2]*a[1][1]*a[2][0]
    -   a[0][1]*a[1][0]*a[2][2]
    -   a[0][0]*a[2][1]*a[1][2];
    
    return d;
}



double itensor::dot(const itensor &U) const
{
    return contract(U);
}




const itensor itensor::dyadic(const ivector&a, const ivector& b)
{
    return itensor (a(0)*b(0), a(0)*b(1), a(0)*b(2),
                    a(1)*b(0), a(1)*b(1), a(1)*b(2),
                    a(2)*b(0), a(2)*b(1), a(2)*b(2));
}




const itensor itensor::identity()
{
    return itensor(1.0, 0.0, 0.0,
                   0.0, 1.0, 0.0,
                   0.0, 0.0, 1.0);
}




double itensor::invariant1() const
{
    return trace();
}




double itensor::invariant2() const
{
    itensor tt( (*this)*(*this));
    double  z( trace() );
    return 0.5*( z*z - tt.trace());
}




double itensor::invariant3() const
{
    return determinant();
}




itensor itensor::inverse() const
{
    const double det = determinant();
    itensor m;
    
    if (det == 0.0)
        std::cout << "\n ERROR in tensor inverse. Tensor is singular" << std::endl;
    else
    {
        const double idet = 1.0/det;
        
        m.a[0][0] = idet*(a[1][1]*a[2][2] - a[2][1]*a[1][2]);
        m.a[0][1] = idet*(a[0][2]*a[2][1] - a[0][1]*a[2][2]);
        m.a[0][2] = idet*(a[0][1]*a[1][2] - a[0][2]*a[1][1]);
        
        m.a[1][0] = idet*(a[1][2]*a[2][0] - a[1][0]*a[2][2]);
        m.a[1][1] = idet*(a[0][0]*a[2][2] - a[0][2]*a[2][0]);
        m.a[1][2] = idet*(a[1][0]*a[0][2] - a[0][0]*a[1][2]);
        
        m.a[2][0] = idet*(a[1][0]*a[2][1] - a[1][1]*a[2][0]);
        m.a[2][1] = idet*(a[0][1]*a[2][0] - a[0][0]*a[2][1]);
        m.a[2][2] = idet*(a[0][0]*a[1][1] - a[0][1]*a[1][0]);
    }
    return m;
}




double itensor::invert()
{
    double det = determinant();
    itensor m;
    
    if (det == 0.0) std::cout << "\n ERROR in tensor inverse. Tensor is singular" << std::endl;
    else
    {
        double idet = 1.0/det;
        
        m.a[0][0] = idet*(a[1][1]*a[2][2] - a[2][1]*a[1][2]);
        m.a[0][1] = idet*(a[0][2]*a[2][1] - a[0][1]*a[2][2]);
        m.a[0][2] = idet*(a[0][1]*a[1][2] - a[0][2]*a[1][1]);
        
        m.a[1][0] = idet*(a[1][2]*a[2][0] - a[1][0]*a[2][2]);
        m.a[1][1] = idet*(a[0][0]*a[2][2] - a[0][2]*a[2][0]);
        m.a[1][2] = idet*(a[1][0]*a[0][2] - a[0][0]*a[1][2]);
        
        m.a[2][0] = idet*(a[1][0]*a[2][1] - a[1][1]*a[2][0]);
        m.a[2][1] = idet*(a[0][1]*a[2][0] - a[0][0]*a[2][1]);
        m.a[2][2] = idet*(a[0][0]*a[1][1] - a[0][1]*a[1][0]);
    }
    *this = m;
    
    return det;
}




itensor itensor::leftContract(const itensor4 &T)
{
    itensor ret;
	ret.setZero();
    const int ndm(3);

    for (unsigned i=0; i<ndm; i++)
        for (unsigned j=0; j<ndm; j++)
            for (unsigned k=0; k<ndm; k++)
                for (unsigned l=0; l<ndm; l++)
                    ret(i,j) += a[k][l]*T(k,l,i,j);
    return ret;
}




double itensor::J2() const
{
    itensor tt((*this)*(*this));
    return 0.5*tt.trace();
}




double itensor::norm() const
{
    return sqrt(contract(*this));
}




ivector itensor::row(const size_t n) const
{
    return ivector( a[n][0], a[n][1], a[n][2] );
}




void itensor::setZero()
{
    const int ndm(3);
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            a[i][j] = 0.0;
}




itensor& itensor::operator=(const itensor &t)
{
    const int ndm(3);
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            a[i][j] = t.a[i][j];
    
    return *this;
}




itensor itensor::operator-() const
{
    const int ndm(3);
    itensor ret;
    
    for (size_t i=0; i<ndm; i++)
    {
        for (size_t j=0; j<ndm; j++)
        {
            ret(i,j) = -a[i][j];
        }
    }
    return ret;
}




itensor& itensor::operator+=(const itensor &t)
{
    const int ndm(3);
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            a[i][j] += t.a[i][j];
    return *this;
}




itensor& itensor::operator-=(const itensor &t)
{
    a[0][0] -= t(0,0);
    a[0][1] -= t(0,1);
    a[0][2] -= t(0,2);
    
    a[1][0] -= t(1,0);
    a[1][1] -= t(1,1);
    a[1][2] -= t(1,2);
    
    a[2][0] -= t(2,0);
    a[2][1] -= t(2,1);
    a[2][2] -= t(2,2);
    
    return *this;
}




itensor& itensor::operator*=(const double alpha)
{
    a[0][0] *= alpha;
    a[0][1] *= alpha;
    a[0][2] *= alpha;
    a[1][0] *= alpha;
    a[1][1] *= alpha;
    a[1][2] *= alpha;
    a[2][0] *= alpha;
    a[2][1] *= alpha;
    a[2][2] *= alpha;
    return *this;
}




std::ostream& operator<<(std::ostream &os, const itensor &t)
{
    os  << std::setw(12) << std::setprecision(8)
    << "[ " << t.a[0][0] << " , " << t.a[0][1]  << " , " << t.a[0][2] << " ]\n";
    
    os  << std::setw(12) << std::setprecision(8)
    << "[ " << t.a[1][0] << " , " << t.a[1][1]  << " , " << t.a[1][2] << " ]\n";
    
    os  << std::setw(12) << std::setprecision(8)
    << "[ " << t.a[2][0] << " , " << t.a[2][1]  << " , " << t.a[2][2] << " ]\n";
    
    return os;
}




const itensor itensor::scaledIdentity(const double a)
{
    return itensor(a, 0.0, 0.0,
                   0.0, a, 0.0,
                   0.0, 0.0, a);
}




void itensor::setRandom()
{
    for (size_t i=0; i<3; i++)
        for (size_t j=0; j<3; j++)
            a[i][j] = randomUniform(-1.0,1.0);
}




double itensor::trace() const
{
    return a[0][0] + a[1][1] + a[2][2];
}




itensor itensor::transpose() const
{
    itensor m;
    const int ndm(3);
    
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            m.a[i][j] = a[j][i];
    
    return m;
}




itensor operator+(const itensor &t1, const itensor &t2)
{
    itensor ret(t1);
    ret += t2;
    return ret;
}




itensor operator-(const itensor &t1, const itensor &t2)
{
    itensor ret(t1);
    ret -= t2;
    return ret;
}




itensor operator*(const itensor &T, const itensor &U)
{
    itensor ret;
    
    // this is a function that is very frequently called, accounting for
    // a large percentage of CPU time, so we make the loops explicit
    /*
     const size_t ndm(3);
     
     for (size_t i=0; i<ndm; i++)
     for (size_t j=0; j<ndm; j++)
     {
     ret.a[i][j] += T.a[i][0] * U.a[0][j] + T.a[i][1] * U.a[1][j] + T.a[i][2] * U.a[2][j];
     }
     */
    
    ret(0,0) = T.a[0][0]*U.a[0][0] + T.a[0][1]*U.a[1][0] + T.a[0][2]*U.a[2][0];
    ret(0,1) = T.a[0][0]*U.a[0][1] + T.a[0][1]*U.a[1][1] + T.a[0][2]*U.a[2][1];
    ret(0,2) = T.a[0][0]*U.a[0][2] + T.a[0][1]*U.a[1][2] + T.a[0][2]*U.a[2][2];
    
    ret(1,0) = T.a[1][0]*U.a[0][0] + T.a[1][1]*U.a[1][0] + T.a[1][2]*U.a[2][0];
    ret(1,1) = T.a[1][0]*U.a[0][1] + T.a[1][1]*U.a[1][1] + T.a[1][2]*U.a[2][1];
    ret(1,2) = T.a[1][0]*U.a[0][2] + T.a[1][1]*U.a[1][2] + T.a[1][2]*U.a[2][2];
    
    ret(2,0) = T.a[2][0]*U.a[0][0] + T.a[2][1]*U.a[1][0] + T.a[2][2]*U.a[2][0];
    ret(2,1) = T.a[2][0]*U.a[0][1] + T.a[2][1]*U.a[1][1] + T.a[2][2]*U.a[2][1];
    ret(2,2) = T.a[2][0]*U.a[0][2] + T.a[2][1]*U.a[1][2] + T.a[2][2]*U.a[2][2];
    
    
    return ret;
}




ivector operator*(const itensor &t , const ivector &v)
{
    ivector w;
    const int ndm(3);
    
    for (size_t i=0; i<ndm; i++)
    {
        w[i] = 0.0;
        for (size_t j=0; j<ndm; j++)
        {
            w[i] += t.a[i][j] * v[j];
        }
    }
    return w;
}




itensor operator*(const itensor &t , const double alpha)
{
    itensor ret(t);
    ret *= alpha;
    return ret;
}




itensor operator*(const double alpha, const itensor &t)
{
    return t*alpha;
}




itensor operator/(const itensor &t, const double alpha)
{
    return t * (1.0/alpha);
}




/* implementation of symmetric, second order tensor. This implementation should be
 changed in the future, to operate only with the upper or lower part of the tensor. For the
 moment, I just copy the itensor implementation and make it work
 */

istensor::istensor() : itensor()
{
}




istensor::istensor(const double t00, const double t11, const double t22,
                     const double t12, const double t02, const double t01)
{
    a[0][0] = t00;
    a[1][1] = t11;
    a[2][2] = t22;
    a[1][2] = a[2][1] = t12;
    a[2][0] = a[0][2] = t02;
    a[0][1] = a[1][0] = t01;
}




// for this constructor, we assume that T is symmetric
istensor::istensor(const istensor &T)
{
    const size_t ndm(3);
    
    for (size_t i=0; i<ndm; i++)
    {
        a[i][i] = T(i,i);
        for (size_t j=i+1; j<ndm; j++)
            a[i][j] = a[j][i] = T(i,j);
    }
}




// for this constructor, we assume that T is symmetric
istensor::istensor(const itensor &T)
{
    *this = symmetricPartOf(T);
}




// this function is used a lot so it is optimized
void istensor::addScaledVdyadicV(const double alpha, const ivector& V)
{
    double va;
    
    va      = V[0]*alpha;
    a[0][0] += va * V[0];
    a[0][1] += va * V[1];
    a[1][0] += va * V[1];
    a[0][2] += va * V[2];
    a[2][0] += va * V[2];
    
    va      = V[1]*alpha;
    a[1][1] += va * V[1];
    a[1][2] += va * V[2];
    a[2][1] += va * V[2];
    
    a[2][2] += alpha * V[2] * V[2];
}




const istensor istensor::identity()
{
    istensor t;
    t.setZero();
    t(0,0) = t(1,1) = t(2,2) = 1.0;
    return t;
}




istensor istensor::inverse() const
{
    double det = determinant();
    istensor m;
    
    if (det == 0.0)
        std::cout << "\n ERROR in tensor inverse. Tensor is singular" << std::endl;
    else
    {
        double idet = 1.0/det;
        
        m.a[0][0] = idet*(a[1][1]*a[2][2] - a[2][1]*a[1][2]);
        m.a[0][1] = idet*(a[0][2]*a[2][1] - a[0][1]*a[2][2]);
        m.a[0][2] = idet*(a[0][1]*a[1][2] - a[0][2]*a[1][1]);
        
        m.a[1][1] = idet*(a[0][0]*a[2][2] - a[0][2]*a[2][0]);
        m.a[1][2] = idet*(a[1][0]*a[0][2] - a[0][0]*a[1][2]);
        
        m.a[2][2] = idet*(a[0][0]*a[1][1] - a[0][1]*a[1][0]);
        
        m(1,0) = m(0,1);
        m(2,0) = m(0,2);
        m(2,1) = m(1,2);
    }
    
    return m;
}




istensor istensor::leftContract(const itensor4 &T)
{
    istensor ret; ret.setZero();
    const int ndm(3);

    for (unsigned i=0; i<ndm; i++)
        for (unsigned j=0; j<ndm; j++)
            for (unsigned k=0; k<ndm; k++)
                for (unsigned l=0; l<ndm; l++)
                    ret(i,j) += a[k][l]*T(k,l,i,j);
    return ret;
}




// compute the three eigenvalues, and take the maximum.
// Since the matrix is symmetric, the three roots of the 3rd order
// polynomial are real
double istensor::maxEigenvalue() const
{
    ivector eval = (*this).eigenvalues();
    return eval(2);
}




//eigenvalues are sorted from smallest to largest
ivector istensor::eigenvalues() const
{
    ivector eval;
    
    
    // polynomial is x^3 + a x^2  + b  x + c  = 0
    //              -l^3 + I1 x^2 - I2 x + I3 = 0
    const double a = -invariant1();
    const double b =  invariant2();
    const double c = -invariant3();
    
    if ( std::abs(a) + std::abs(b) + std::abs(c) <= 1e-8)
    {
        eval.setZero();
    }
    else
    {
        const double  pi  = M_PI;
        double oot = 1.0/3.0;
        double opf = 1.5;
        
        double p = (3.0*b-a*a)*oot;
        double q = c + 2.0*a*a*a/27.0-a*b*oot;
        
        // three real roots
        double  ap     = std::abs(p);
        double  cosphi = -0.5*q/std::pow(ap*oot, opf);
        double  phi;
        if (cosphi >= 1.0)
            phi = 0.0;
        else if (cosphi <= -1.0)
            phi = pi;
        else
            phi = std::acos(cosphi);
        
        if ( std::isnan(cosphi) )
            phi = pi;
        
        double  cf = 2.0*std::sqrt(ap*oot);
        
        double l0, l1, l2;
        l0 = -a*oot + cf*std::cos(     phi*oot);
        l1 = -a*oot - cf*std::cos((phi-pi)*oot);
        l2 = -a*oot - cf*std::cos((phi+pi)*oot);
        
        if (l0>l1) std::swap(l0, l1);
        if (l1>l2) std::swap(l1, l2);
        if (l0>l1) std::swap(l0, l1);
        
        eval(0) = l0;
        eval(1) = l1;
        eval(2) = l2;
    }
    
    return eval;
}




bool istensor::maxPrincipalDirection(ivector& v) const
{
    istensor copy(*this);
    itensor  evec;
    double   eval[3];
    int nrot;
    bool ret(true);
    size_t i;
    
    double tresh,theta,tau,t,sm,s,h,g,c;
    
    int n = 3;
    double b[3], z[3];
    
    evec = itensor::identity();
    
    for (size_t ip=0; ip<n; ip++)
    {
        b[ip] = eval[ip] = copy.a[ip][ip];
        z[ip] = 0.0;
    }
    
    nrot=0;
    for (i=1;i<=50;i++)
    {
        sm=0.0;
        for (size_t ip=0;ip<n-1;ip++)
        {
            for (size_t iq=ip+1;iq<n;iq++)
                sm += fabs(copy.a[ip][iq]);
        }
        
        if (sm == 0.0)
            break;
        
        if (i < 4)
            tresh = 0.2*sm/(n*n);
        
        else
            tresh = 0.0;
        
        for (size_t ip=0; ip<n-1; ip++)
        {
            for (size_t iq=ip+1; iq<n; iq++)
            {
                g = 100.0*fabs(copy.a[ip][iq]);
                if (i > 4 && (fabs(eval[ip])+g) == fabs(eval[ip]) && (fabs(eval[iq])+g) == fabs(eval[iq]))
                    copy.a[ip][iq] = 0.0;
                else if (fabs(copy.a[ip][iq]) > tresh)
                {
                    h=eval[iq]-eval[ip];
                    if ((fabs(h)+g) == fabs(h))
                        t=(copy.a[ip][iq])/h;
                    else
                    {
                        theta=0.5*h/(copy.a[ip][iq]);
                        t=1.0/(fabs(theta)+sqrt(1.0+theta*theta));
                        if (theta < 0.0) t = -t;
                    }
                    c=1.0/sqrt(1+t*t);
                    s=t*c;
                    tau=s/(1.0+c);
                    h=t*copy.a[ip][iq];
                    z[ip] -= h;
                    z[iq] += h;
                    eval[ip] -= h;
                    eval[iq] += h;
                    copy.a[ip][iq]=0.0;
                    for (size_t j=0; j<ip; j++)             copy.jacobi_rot(s, tau, j, ip, j, iq);
                    for (size_t j=ip+1; j<iq; j++)          copy.jacobi_rot(s, tau, ip, j, j, iq);
                    for (size_t j=iq+1; j<n; j++)           copy.jacobi_rot(s, tau, ip, j, iq, j);
                    for (size_t j=0; j<n; j++)              evec.jacobi_rot(s, tau, j, ip, j, iq);
                    ++nrot;
                }
            }
        }
        
        for (size_t ip=0;ip<n;ip++)
        {
            b[ip]    += z[ip];
            eval[ip]  = b[ip];
            z[ip]     = 0.0;
        }
    }
    if (i == 50) ret = false;

    // extract the maximum direction by comparing the three eigenvalues
    double max(eval[0]);
    i = 0;
    
    if ( eval[1] > eval[0] ) {max = eval[1]; i=1;}
    if ( eval[2] > max     ) {i=2;}
    
    v(0) = evec(0, i);
    v(1) = evec(1, i);
    v(2) = evec(2, i);
    
    return ret;
}





void itensor::jacobi_rot(const double s, const double tau, const size_t i, const size_t j, const size_t k, const size_t l)
{
    double g,h;
    
    g = a[i][j];
    h = a[k][l];
    a[i][j] = g - s*(h+g*tau);
    a[k][l] = h + s*(g-h*tau);
}




const istensor istensor::scaledIdentity(const double a)
{
    return istensor(a, a, a, 0.0, 0.0, 0.0);
}




itensor3::itensor3()
{
#ifdef INIT2ZERO
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                a[i][j][k] = 0.0;
#endif
}




itensor3::itensor3(const itensor3 &T1)
{
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                a[i][j][k] = T1.a[i][j][k];
}




void itensor3::addDyadic(const ivector& v1, const ivector& v2, const ivector& v3)
{
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                a[i][j][k] += v1[i]*v2[j]*v3[k];
}




void itensor3::setZero()
{
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                a[i][j][k] = 0.0;
}




const itensor3 itensor3::alternatingSymbol()
{
    itensor3 epsilon;
    ivector e[3];
    e[0] = ivector(1.0, 0.0, 0.0);
    e[1] = ivector(0.0, 1.0, 0.0);
    e[2] = ivector(0.0, 0.0, 1.0);

    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                epsilon(i,j,k) = ivector::tripleProduct(e[i], e[j], e[k]);

    return epsilon;
}




itensor3 itensor3::internalProduct(const itensor& T, const ivector& v) // IP_{ijk} = T_{ik} v_j
{
    itensor3 H;
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                H(i,j,k) = T(i,k)*v(j);

    return H;
}




itensor3 itensor3::internalProduct(const ivector& v, const itensor& T) // IP_{ijk} = v_i T_{jk}
{
    itensor3 H;
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                H(i,j,k) = v(i)*T(j,k);

    return H;
}




double itensor3::operator()(const ivector& vi, const ivector& vj, const ivector& vk) const
{
    double s = 0.0;
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                s += a[i][j][k] * vi[i] * vj[j] * vk[k];

    return s;
}




ivector itensor3::operator()(const istensor& t)
{
    ivector v(0.0, 0.0, 0.0);

    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                v(i) += a[i][j][k] * t(j,k);

    return v;
}




itensor3& itensor3::operator+=(const itensor3 &T1)
{
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                a[i][j][k] += T1(i,j,k);

    return *this;
}




itensor3& itensor3::operator-=(const itensor3 &T1)
{
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                a[i][j][k] *= T1(i,j,k);

    return *this;

}




itensor3& itensor3::operator*=(const double alpha)
{
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                a[i][j][k] *= alpha;

    return *this;
}




itensor3& itensor3::operator=(const itensor3 &T1)
{
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                a[i][j][k] = T1(i,j,k);

    return *this;
}




std::ostream& operator<<(std::ostream &os, const itensor3 &t)
{
    const unsigned ndm = 3;
    unsigned a = 0, b = 0;
    os << "\n";
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            for (size_t k=0; k<ndm; k++)
                os << t(i,j,k) << (++a % 3 == 0 ? "\n" : "\t")
                    << ( ++b % 9 == 0 ? "\n" : "");

    return os;
}




itensor3 operator+(const itensor3 &T1, const itensor3 &T2)
{
    itensor3 S;

    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                S(i,j,k) = T1(i,j,k) + T2(i,j,k);
    return S;
}




itensor3 operator-(const itensor3 &T1, const itensor3 &T2)
{
    itensor3 S;

    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                S(i,j,k) = T1(i,j,k) - T2(i,j,k);
    return S;
}




itensor3 operator*(const itensor3 &T1, const double a)
{
    itensor3 S;

    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                S(i,j,k) = T1(i,j,k)*a;
    return S;
}




itensor3 operator*(const double a, const itensor3 &T1)
{
    itensor3 S;

    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                S(i,j,k) = T1(i,j,k)*a;
    return S;
}




itensor operator*(const ivector& v, const itensor3& T)
{
    itensor S;
    S.setZero();

    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                   S(j,k) += v(i)*T(i,j,k);

    return S;
}




itensor operator*(const itensor3& T, const ivector& v)
{
    itensor S;
    S.setZero();

    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                S(i,j) += T(i,j,k)*v(k);;

    return S;
}




itensor4::itensor4()
{
#ifdef INIT2ZERO
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                for (unsigned l=0; l<3; l++)
                    a[i][j][k][l] = 0.0;
#endif
}




itensor4::itensor4(const itensor4 &T1)
{
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                for (unsigned l=0; l<3; l++)
                    a[i][j][k][l] = T1.a[i][j][k][l];
}




void itensor4::setZero()
{
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                for (unsigned l=0; l<3; l++)
                    a[i][j][k][l] = 0.0;
}




double itensor4::norm() const
{
    return sqrt(this->squaredNorm());
}




double itensor4::squaredNorm() const
{
    double n = 0.0;
    double x;

    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                for (unsigned l=0; l<3; l++)
                {
                    x = a[i][j][k][l];
                    n += x*x;
                }

    return n;
}




void itensor4::symmetrize()
{
    itensor4 ns = (*this);

    for (unsigned i=0; i<3; i++)
        for (unsigned j=i; j<3; j++)
            for (unsigned k=j; k<3; k++)
                for (unsigned l=k; l<3; l++)
                {
                    double t = 0.125 * (  ns(i,j,k,l) + ns(i,j,l,k) + ns(j,i,k,l) + ns(j,i,l,k)
                                        + ns(k,l,i,j) + ns(k,l,j,i) + ns(l,k,i,j) + ns(l,k,j,i));

                    a[i][j][k][l] = a[j][i][k][l] = a[i][j][l][k] = a[k][l][i][j] = a[l][k][i][j] = a[k][l][j][i] = t;
                }
}




void itensor4::addDyadic(const istensor &m, const istensor &n)
{
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                for (unsigned l=0; l<3; l++)
                    a[i][j][k][l] += m(i,j)*n(k,l);
}




const itensor4 itensor4::deviatoricIdentity()
{
    // fourth order projection tensor (Refer to Holzapfel 2000 Non linear solid Mechanics - Wiley & Sons: eq. 1.164)
    itensor4 ret;
    itensor4 idSymm = itensor4::identitySymm();
    itensor4 ioi;
    ioi.setZero();
    ioi.addDyadic(istensor::identity(), istensor::identity());

    const double third = 1.0/3.0;
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                for (unsigned l=0; l<3; l++)
                    ret(i,j,k,l) = idSymm(i,j,k,l) - third * ioi(i,j,k,l);
    
    return ret;
}




// I_{ijkl} = delta_{ik} delta_{jl}
const itensor4 itensor4::identity()
{
    // fourth order unit tensor (Holzapfel 2000, eq. 1.161)
    const istensor id = istensor::identity();
    itensor4 ret;

    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            for (unsigned k=0; k<3; k++)
                for (unsigned l=0; l<3; l++)
                    ret(i,j,k,l) = id(i,k)*id(j,l);

    return ret;
}




const itensor4 itensor4::identitySymm()
{
    // Symmetric fourth order unit tensor (Holzapfel 2000 eq. 1.164)
    itensor4 ret;
    const int ndm(3);
    const istensor id = istensor::identity();
    
    for (unsigned i=0; i<ndm; i++)
        for (unsigned j=0; j<ndm; j++)
            for (unsigned k=0; k<ndm; k++)
                for (unsigned l=0; l<ndm; l++)
                    ret(i,j,k,l) = 0.5 * ( id(i,k)*id(j,l) + id(i,l)*id(j,k) );
    
    return ret;
}




itensor4& itensor4::operator+=(const itensor4 &T1)
{
    const int ndm(3);
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            for (size_t k=0; k<ndm; k++)
                for (size_t l=0; l<ndm; l++)
                    a[i][j][k][l] += T1.a[i][j][k][l];
    return *this;
}




itensor4& itensor4::operator-=(const itensor4 &T1)
{
    const int ndm(3);
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            for (size_t k=0; k<ndm; k++)
                for (size_t l=0; l<ndm; l++)
                    a[i][j][k][l] -= T1.a[i][j][k][l];
    return *this;
}




itensor4& itensor4::operator*=(double alpha)
{
    const int ndm(3);
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            for (size_t k=0; k<ndm; k++)
                for (size_t l=0; l<ndm; l++)
                    a[i][j][k][l] *= alpha;
    return *this;
}




istensor itensor4::operator*(const istensor& s) const
{
    istensor cs;
    cs.setZero();
    const int ndm(3);
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            for (size_t k=0; k<ndm; k++)
                for (size_t l=0; l<ndm; l++)
                    cs(i,j) += a[i][j][k][l]*s(k,l);
    return cs;
}



itensor4& itensor4::operator=(const itensor4 &T1)
{
    const int ndm(3);
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            for (size_t k=0; k<ndm; k++)
                for (size_t l=0; l<ndm; l++)
                    a[i][j][k][l] = T1.a[i][j][k][l];
    
    return *this;
}




double itensor4::operator()(const ivector& vi, const ivector& vj,
                              const ivector& vk, const ivector& vl) const
{
    double r = 0.0;
    const unsigned ndm = 3;
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            for (size_t k=0; k<ndm; k++)
                for (size_t l=0; l<ndm; l++)
                    r += a[i][j][k][l]*vi(i)*vj(j)*vk(k)*vl(l);

    return r;
}




istensor itensor4::operator()(const istensor& t)
{
    istensor ret;
    ret.setZero();
    const int ndm(3);
    
    for (unsigned i=0; i<ndm; i++)
        for (unsigned j=0; j<ndm; j++)
            for (unsigned k=0; k<ndm; k++)
                for (unsigned l=0; l<ndm; l++)
                    ret(i,j) += a[i][j][k][l]*t(k,l);
    
    return ret;
}




std::ostream& operator<<(std::ostream &os, const itensor4 &t)
{
    const unsigned ndm = 3;
    unsigned a = 0, b = 0;
    os << "\n";
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            for (size_t k=0; k<ndm; k++)
                for (size_t l=0; l<ndm; l++)
                    os << t(i,j,k,l) << (++a % 3 == 0 ? "\n" : "\t")
                        << ( ++b % 9 == 0 ? "\n" : "");

    return os;
}




itensor4 operator+(const itensor4 &T1, const itensor4 &T2)
{
    itensor4 ret(T1);
    ret += T2;
    return ret;
}




itensor4 operator-(const itensor4 &T1, const itensor4 &T2)
{
    itensor4 ret(T1);
    ret -= T2;
    return ret;
}




itensor4 operator*(const itensor4 &T1 , const double alpha)
{
    itensor4 ret(T1);
    ret *= alpha;
    return ret;
}




itensor4  operator*(const double alpha, const itensor4 &T1)
{
    return T1*alpha;
}




itensor4 operator*(const itensor4 &T1, const itensor4 &T2)
{
    itensor4 ret;
    ret.setZero();
    const int ndm(3);
    
    for (unsigned i=0; i<ndm; i++)
        for (unsigned j=0; j<ndm; j++)
            for (unsigned k=0; k<ndm; k++)
                for (unsigned l=0; l<ndm; l++)
                    for (unsigned m=0; m<ndm; m++)
                        for (unsigned n=0; n<ndm; n++)
                            ret(i,j,k,l) += T1(i,j,m,n)*T2(m,n,k,l);
    return ret;
}




/* Eigen decomposition code for symmetric 3x3 matrices, copied from the public
 domain Java Matrix library JAMA. */
#include <math.h>

#ifdef MAX
#undef MAX
#endif

#define MAX(a, b) ((a)>(b)?(a):(b))


static double hypot2(double x, double y)
{
    return sqrt(x*x+y*y);
}




// Symmetric Householder reduction to tridiagonal form.
static void tred2(double V[3][3], double d[3], double e[3])
{
    const int n=3;
    
    //  This is derived from the Algol procedures tred2 by
    //  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
    //  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
    //  Fortran subroutine in EISPACK.
    
    for (size_t j = 0; j < n; j++)
    {
        d[j] = V[n-1][j];
    }
    
    // Householder reduction to tridiagonal form.
    
    for (size_t i = n-1; i > 0; i--)
    {
        
        // Scale to avoid under/overflow.
        
        double scale = 0.0;
        double h = 0.0;
        for (size_t k = 0; k < i; k++) {
            scale = scale + fabs(d[k]);
        }
        if (scale == 0.0) {
            e[i] = d[i-1];
            for (size_t j = 0; j < i; j++) {
                d[j] = V[i-1][j];
                V[i][j] = 0.0;
                V[j][i] = 0.0;
            }
        } else
        {
            // Generate Householder vector.
            for (size_t k = 0; k < i; k++) {
                d[k] /= scale;
                h += d[k] * d[k];
            }
            double f = d[i-1];
            double g = sqrt(h);
            if (f > 0) {
                g = -g;
            }
            e[i] = scale * g;
            h = h - f * g;
            d[i-1] = f - g;
            for (size_t j = 0; j < i; j++) {
                e[j] = 0.0;
            }
            
            // Apply similarity transformation to remaining columns.
            
            for (size_t j = 0; j < i; j++) {
                f = d[j];
                V[j][i] = f;
                g = e[j] + V[j][j] * f;
                for (size_t k = j+1; k <= i-1; k++) {
                    g += V[k][j] * d[k];
                    e[k] += V[k][j] * f;
                }
                e[j] = g;
            }
            f = 0.0;
            for (size_t j = 0; j < i; j++) {
                e[j] /= h;
                f += e[j] * d[j];
            }
            double hh = f / (h + h);
            for (size_t j = 0; j < i; j++) {
                e[j] -= hh * d[j];
            }
            for (size_t j = 0; j < i; j++) {
                f = d[j];
                g = e[j];
                for (size_t k = j; k <= i-1; k++) {
                    V[k][j] -= (f * e[k] + g * d[k]);
                }
                d[j] = V[i-1][j];
                V[i][j] = 0.0;
            }
        }
        d[i] = h;
    }
    
    // Accumulate transformations.
    
    for (size_t i = 0; i < n-1; i++)
    {
        V[n-1][i] = V[i][i];
        V[i][i] = 1.0;
        double h = d[i+1];
        if (h != 0.0)
        {
            for (size_t k = 0; k <= i; k++)
            {
                d[k] = V[k][i+1] / h;
            }
            for (size_t j = 0; j <= i; j++)
            {
                double g = 0.0;
                for (size_t k = 0; k <= i; k++)
                {
                    g += V[k][i+1] * V[k][j];
                }
                for (size_t k = 0; k <= i; k++)
                {
                    V[k][j] -= g * d[k];
                }
            }
        }
        for (size_t k = 0; k <= i; k++)
        {
            V[k][i+1] = 0.0;
        }
    }
    for (size_t j = 0; j < n; j++)
    {
        d[j] = V[n-1][j];
        V[n-1][j] = 0.0;
    }
    V[n-1][n-1] = 1.0;
    e[0] = 0.0;
}




// Symmetric tridiagonal QL algorithm.
static void tql2(double V[3][3], double d[3], double e[3])
{
    const int n=3;
    
    //  This is derived from the Algol procedures tql2, by
    //  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
    //  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
    //  Fortran subroutine in EISPACK.
    for (int i = 1; i < n; i++)
    {
        e[i-1] = e[i];
    }
    e[n-1] = 0.0;
    
    double f = 0.0;
    double tst1 = 0.0;
    double eps = pow(2.0,-52.0);
    for (int l = 0; l < n; l++)
    {
        
        // Find small subdiagonal element
        
        tst1 = MAX(tst1,fabs(d[l]) + fabs(e[l]));
        int m = l;
        while (m < n)
        {
            if (fabs(e[m]) <= eps*tst1)
            {
                break;
            }
            m++;
        }
        
        // If m == l, d[l] is an eigenvalue,
        // otherwise, iterate.
        
        if (m > l)
        {
            size_t iter = 0;
            do {
                iter = iter + 1;  // (Could check iteration count here.)
                
                // Compute implicit shift
                
                double g = d[l];
                double p = (d[l+1] - g) / (2.0 * e[l]);
                double r = hypot2(p,1.0);
                if (p < 0)
                {
                    r = -r;
                }
                d[l] = e[l] / (p + r);
                d[l+1] = e[l] * (p + r);
                double dl1 = d[l+1];
                double h = g - d[l];
                for (int i = l+2; i < n; i++)
                {
                    d[i] -= h;
                }
                f = f + h;
                
                // Implicit QL transformation.
                
                p = d[m];
                double c = 1.0;
                double c2 = c;
                double c3 = c;
                double el1 = e[l+1];
                double s = 0.0;
                double s2 = 0.0;
                for (int i = m-1; i >= l; i--) {
                    c3 = c2;
                    c2 = c;
                    s2 = s;
                    g = c * e[i];
                    h = c * p;
                    r = hypot2(p,e[i]);
                    e[i+1] = s * r;
                    s = e[i] / r;
                    c = p / r;
                    p = c * d[i] - s * g;
                    d[i+1] = h + s * (c * g + s * d[i]);
                    
                    // Accumulate transformation.
                    
                    for (int k = 0; k < n; k++)
                    {
                        h = V[k][i+1];
                        V[k][i+1] = s * V[k][i] + c * h;
                        V[k][i] = c * V[k][i] - s * h;
                    }
                }
                p = -s * s2 * c3 * el1 * e[l] / dl1;
                e[l] = s * p;
                d[l] = c * p;
                
                // Check for convergence.
                
            } while (fabs(e[l]) > eps*tst1);
        }
        d[l] = d[l] + f;
        e[l] = 0.0;
    }
    
    // Sort eigenvalues and corresponding vectors.
    for (size_t i = 0; i < n-1; i++)
    {
        size_t k = i;
        double p = d[i];
        for (size_t j = i+1; j < n; j++)
        {
            if (d[j] < p)
            {
                k = j;
                p = d[j];
            }
        }
        if (k != i)
        {
            d[k] = d[i];
            d[i] = p;
            for (size_t j = 0; j < n; j++)
            {
                p = V[j][i];
                V[j][i] = V[j][k];
                V[j][k] = p;
            }
        }
    }
}




void istensor::setRandom()
{
    for (size_t i=0; i<3; i++)
    {
        a[i][i] = randomUniform(-1.0,1.0);
        for (size_t j=i+1; j<3; j++)
        {
            a[i][j] = a[j][i] = randomUniform(-1.0,1.0);
        }
    }
}




void istensor::spectralDecomposition(ivector evectors[3], ivector &evalues) const
{
    double e[3], d[3], V[3][3];
    for (size_t i = 0; i<3; i++)
        for (size_t j = 0; j <3; j++)
            V[i][j] = (*this)(i,j);
    
    tred2(V, d, e);
    tql2(V, d, e);
    
    for (unsigned i=0; i<3; i++)
    {
        evalues[i] = d[i];
        for (size_t j = 0; j<3; j++)
        {
            evectors[i][j] = V[j][i];
        }
        evectors[i].normalize();
    }
    
    
    // make sure the eigenvector triad is right-handed.
    // if not, reverse the last eigenvector
    if ( ivector::tripleProduct(evectors[0], evectors[1], evectors[2]) < 0.0)
    {
        for (size_t i=0; i<3; i++) evectors[2][i] = -evectors[2][i];
    }
}




istensor istensor::squared() const
{
    istensor SS;
    
    for (unsigned i=0; i<3; i++)
    {
        for (unsigned j=0; j<3; j++)
        {
            SS(i,j) = 0.0;
            for (unsigned k=0; k<3; k++)
            {
                SS(i,j) += a[i][k]*a[k][j];
            }
        }
    }
    return SS;
}




const istensor istensor::deviatoricPart(const istensor& t)
{
    istensor dev(t);
    const double theta3 = t.trace()/3.0;
    
    dev(0,0) -= theta3;
    dev(1,1) -= theta3;
    dev(2,2) -= theta3;
    
    return dev;
}




const istensor istensor::symmetricPartOf(const itensor &t)
{
    istensor s;
    for (size_t i=0; i<3; i++)
    {
        s(i,i) = t(i,i);
        for (size_t j=i+1; j<3; j++)
        {
            s(i,j) = s(j,i) = 0.5*(t(i,j)+t(j,i));
        }
    }
    
    return s;
}




istensor& istensor::operator=(const istensor &t)
{
    a[0][0] = t.a[0][0];
    a[0][1] = t.a[0][1];
    a[0][2] = t.a[0][2];
    a[1][0] = t.a[1][0];
    a[1][1] = t.a[1][1];
    a[1][2] = t.a[1][2];
    a[2][0] = t.a[2][0];
    a[2][1] = t.a[2][1];
    a[2][2] = t.a[2][2];
    
    return *this;
}




istensor& istensor::operator=(const itensor &t)
{
    *this = symmetricPartOf(t);
    return *this;
}




istensor istensor::operator-()
{
    const int ndm(3);
    istensor ret;
    
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            ret(i,j) = -a[i][j];
    return ret;
}




istensor& istensor::operator+=(const istensor &t)
{
    const int ndm(3);
    
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            a[i][j] += t.a[i][j];
    return *this;
}




istensor& istensor::operator-=(const istensor &t)
{
    const int ndm(3);
    
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            a[i][j] -= t.a[i][j];
    return *this;
}




istensor& istensor::operator*=(const double alpha)
{
    const int ndm(3);
    
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            a[i][j] *= alpha;
    return *this;
}




istensor operator+(const istensor &t1, const istensor &t2)
{
    istensor ret(t1);
    ret += t2;
    return ret;
}


itensor operator+(const istensor &t1, const itensor &t2)
{
    itensor tt(t1);
    return tt+t2;
}



itensor operator+(const itensor &t1, const istensor &t2)
{
    return t2+t1;
}




istensor operator-(const istensor &t1, const istensor &t2)
{
    istensor ret(t1);
    ret -= t2;
    return ret;
}




itensor operator*(const itensor &t1, const istensor &t2)
{
    itensor ret;
    ret.setZero();
    const int ndm(3);
    
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            for (size_t k=0; k<ndm; k++)
                ret(i,j) += t1(i,k) * t2.a[k][j];
    
    return ret;
}


itensor operator*(const istensor &t1, const itensor &t2)
{
    itensor ret;
    
    /*const int ndm(3);
     
     for (size_t i=0; i<ndm; i++)
     for (size_t j=0; j<ndm; j++)
     for (size_t k=0; k<ndm; k++)
     ret(i,j) += t1(i,k) * t2(k,j);
     */
    
    ret(0,0) = t1(0,0)*t2(0,0) + t1(0,1)*t2(1,0) + t1(0,2)*t2(2,0);
    ret(0,1) = t1(0,0)*t2(0,1) + t1(0,1)*t2(1,1) + t1(0,2)*t2(2,1);
    ret(0,2) = t1(0,0)*t2(0,2) + t1(0,1)*t2(1,2) + t1(0,2)*t2(2,2);
    
    ret(1,0) = t1(1,0)*t2(0,0) + t1(1,1)*t2(1,0) + t1(1,2)*t2(2,0);
    ret(1,1) = t1(1,0)*t2(0,1) + t1(1,1)*t2(1,1) + t1(1,2)*t2(2,1);
    ret(1,2) = t1(1,0)*t2(0,2) + t1(1,1)*t2(1,2) + t1(1,2)*t2(2,2);
    
    ret(2,0) = t1(2,0)*t2(0,0) + t1(2,1)*t2(1,0) + t1(2,2)*t2(2,0);
    ret(2,1) = t1(2,0)*t2(0,1) + t1(2,1)*t2(1,1) + t1(2,2)*t2(2,1);
    ret(2,2) = t1(2,0)*t2(0,2) + t1(2,1)*t2(1,2) + t1(2,2)*t2(2,2);
    
    return ret;
}



itensor operator*(const istensor &t1, const istensor &t2)
{
    itensor ret;
    ret.setZero();
    const int ndm(3);
    
    for (size_t i=0; i<ndm; i++)
        for (size_t j=0; j<ndm; j++)
            for (size_t k=0; k<ndm; k++)
                ret(i,j) += t1(i,k) * t2.a[k][j];
    
    return ret;
}



ivector operator*(const istensor &t, const ivector &v)
{
    return ivector(t.a[0][0]*v[0] + t.a[0][1]*v[1] + t.a[0][2]*v[2],
                   t.a[1][0]*v[0] + t.a[1][1]*v[1] + t.a[1][2]*v[2],
                   t.a[2][0]*v[0] + t.a[2][1]*v[1] + t.a[2][2]*v[2]);
}




istensor operator*(const istensor &t , const double alpha)
{
    istensor ret(t);
    ret *= alpha;
    return ret;
}




istensor operator*(const double alpha, const istensor &t)
{
    return t*alpha;
}


istensor operator/(const istensor &t , const double alpha)
{
    return t * (1.0/alpha);
}




skewtensor::skewtensor()
{

#ifdef INIT2ZERO
    for (size_t i=0; i<3; i++)
        for (size_t j=0; j<3; j++)
            a[i][j] = 0.0;
#endif
}




skewtensor::skewtensor(const skewtensor& t)
{
    for (size_t i=0; i<3; i++)
        for (size_t j=0; j<3; j++)
            a[i][j] = t(i,j);
}




skewtensor& skewtensor::operator=(const skewtensor &t)
{
    for (size_t i=0; i<3; i++)
        for (size_t j=0; j<3; j++)
            a[i][j] = t(i,j);
    
    return *this;
}



skewtensor skewtensor::skewpart(const itensor& t)
{
    itensor     sp = 0.5*(t - t.transpose());
    skewtensor  s;
    
    for (size_t i=0; i<3; i++)
        for (size_t j=0; j<3; j++)
            s(i,j) = sp(i,j);
    
    return s;
}




skewtensor::skewtensor(const ivector& v)
{
    skewtensor& vhat = *this;
    vhat(0,0) =  0.0;
    vhat(0,1) = -v(2);
    vhat(0,2) =  v(1);
    vhat(1,0) =  v(2);
    vhat(1,1) =  0.0;
    vhat(1,2) = -v(0);
    vhat(2,0) = -v(1);
    vhat(2,1) =  v(0);
    vhat(2,2) =  0.0;
}




ivector skewtensor::axialVector() const
{
    const skewtensor& s = *this;
    return ivector( -s(1,2), -s(2,0), -s(0,1) );
}




void skewtensor::setRandom()
{
    ivector v; v.setRandom();
    *this = skewtensor(v);
}




const istensor istensor::squareRoot(const istensor& t)
{
    ivector  eval, evec[3];
    t.spectralDecomposition(evec, eval);
    
    itensor r;
    r.setZero();
    for (size_t a=0; a<3; a++)
    {
        assert( eval[a] >= 0.0);
        r.addDyadic(sqrt(eval[a])*evec[a], evec[a]);
    }
    
    return r;
}




const istensor istensor::tensorTimesTensorTransposed(const itensor& F)
{
    istensor FFt;
    
    for (size_t i=0; i<3; i++)
    {
        for (size_t j=0; j<3; j++)
        {
            FFt(i,j) = 0.0;
            for (size_t k=0; k<3; k++)
            {
                FFt(i,j) += F(i,k) * F(j,k);
            }
        }
    }
    return FFt;
}




const istensor istensor::tensorTransposedTimesTensor(const itensor& F)
{
    istensor FtF;
    
    for (size_t i=0; i<3; i++)
    {
        for (size_t j=0; j<3; j++)
        {
            FtF(i,j) = 0.0;
            for (size_t k=0; k<3; k++)
            {
                FtF(i,j) += F(k,i) * F(k,j);
            }
        }
    }
    return FtF;
}




const istensor istensor::FtCF(const itensor& F, const istensor& C)
{
    itensor CF;
    
    for (size_t i=0; i<3; i++)
    {
        for (size_t j=0; j<3; j++)
        {
            CF(i,j) = 0.0;
            for (size_t k=0; k<3; k++)
            {
                CF(i,j) += C(i,k) * F(k,j);
            }
        }
    }
    
    istensor mFtCF;
    for (size_t i=0; i<3; i++)
    {
        for (size_t j=0; j<3; j++)
        {
            mFtCF(i,j) = 0.0;
            for (size_t k=0; k<3; k++)
            {
                mFtCF(i,j) += F(k,i) * CF(k,j);
            }
        }
    }
    
    return mFtCF;
}




const istensor istensor::FSFt(const itensor& F, const istensor& S)
{
    itensor SFt;
    
    for (size_t i=0; i<3; i++)
    {
        for (size_t j=0; j<3; j++)
        {
            SFt(i,j) = 0.0;
            for (size_t k=0; k<3; k++)
            {
                SFt(i,j) += S(i,k) * F(j,k);
            }
        }
    }
    
    istensor mFSFt;
    for (size_t i=0; i<3; i++)
    {
        for (size_t j=0; j<3; j++)
        {
            mFSFt(i,j) = 0.0;
            for (size_t k=0; k<3; k++)
            {
                mFSFt(i,j) += F(i,k) * SFt(k,j);
            }
        }
    }
    
    return mFSFt;
}





irotation::irotation()
{
#ifdef INIT2ZERO
    a[0][0] = a[1][1] = a[2][2] = 1.0;
    a[0][1] = a[1][0] = 0.0;
    a[0][2] = a[2][0] = 0.0;
    a[1][2] = a[2][1] = 0.0;
#endif
}




ivector irotation::operator()(const ivector& v) const
{
    return (*this)*v;
}




irotation::irotation(const double a, const double b, const double c)
{
    *this = irotation( ivector(a,b,c) );
}




// rotation initialization via exponential map
irotation::irotation(const ivector& theta)
{
    double norm, f1, f2, f3, sn;
    
    norm = theta.norm();
    f1   = cos(norm);
    
    if (norm < 1.0e-6)
    {
        norm *= norm;
        f2 = 1.0 - norm*(1.0/6.0  - norm*(1.0/120.0 - norm/5040.0));
        f3 = 0.5 - norm*(1.0/24.0 - norm*(1.0/720.0 - norm/40320.0));
    }
    else
    {
        f2    = sin(norm)/norm;
        norm *= 0.5;
        sn    = sin(norm);
        f3    = 0.5 * sn * sn /(norm*norm);
    }
    
    a[0][0] =           f1 + f3*theta[0]*theta[0];
    a[0][1] = -theta[2]*f2 + f3*theta[0]*theta[1];
    a[0][2] =  theta[1]*f2 + f3*theta[0]*theta[2];
    
    a[1][0] =  theta[2]*f2 + f3*theta[1]*theta[0];
    a[1][1] =           f1 + f3*theta[1]*theta[1];
    a[1][2] = -theta[0]*f2 + f3*theta[1]*theta[2];
    
    a[2][0] = -theta[1]*f2 + f3*theta[2]*theta[0];
    a[2][1] =  theta[0]*f2 + f3*theta[2]*theta[1];
    a[2][2] =           f1 + f3*theta[2]*theta[2];
}




irotation::irotation(const iquaternion &quat)
{
    computeFrom(quat);
}




irotation::irotation(const itensor& t)
{
    for (size_t i = 0; i<3; i++)
        for (size_t j=0; j<3; j++)
            this->a[i][j] = t(i,j);
}




void irotation::beRotationWithoutDrill(const ivector& u)
{
    double f;
    
    if (u[2] > 0.0)
    {
        f   =   1.0 / (1.0 + u[2]);
        a[0][0] =   u[2] + f * u[1] * u[1];
        a[0][1] =        - f * u[1] * u[0];
        a[1][0] =        - f * u[0] * u[1];
        a[1][1] =   u[2] + f * u[0] * u[0];
        a[2][0] = - u[0];
        a[2][1] = - u[1];
    }
    else
    {
        f         =   1.0 / (1.0 - u[2]);
        a[0][0] = - u[2] + f * u[1] * u[1];
        a[0][1] =          f * u[1] * u[0];
        a[1][0] =        - f * u[0] * u[1];
        a[1][1] =   u[2] - f * u[0] * u[0];
        a[2][0] =   u[0];
        a[2][1] = - u[1];
    }
    
    a[0][2] = u[0];
    a[1][2] = u[1];
    a[2][2] = u[2];
}




void irotation::beRotationWithoutDrill(const ivector &from, const ivector& to)
{
    itensor rot;
    double    dd = from.dot(to);
    ivector   cr = from.cross(to);
    skewtensor hatcr(cr);
    
    rot = dd * identity() + hatcr + 1.0/(1.0+dd)* dyadic(cr,cr);
    *this = rot;
}




void irotation::computeFrom(const iquaternion &quat)
{
    double q00, q01, q02, q03, q11, q12, q13, q22, q23, q33;
    
    q00 = quat.w()*quat.w()*2.0 - 1.0;
    q01 = quat.w()*quat.x()*2.0;
    q02 = quat.w()*quat.y()*2.0;
    q03 = quat.w()*quat.z()*2.0;
    q11 = quat.x()*quat.x()*2.0;
    q12 = quat.x()*quat.y()*2.0;
    q13 = quat.x()*quat.z()*2.0;
    q22 = quat.y()*quat.y()*2.0;
    q23 = quat.y()*quat.z()*2.0;
    q33 = quat.z()*quat.z()*2.0;
    
    a[0][0] = q00 + q11;
    a[1][0] = q12 + q03;
    a[2][0] = q13 - q02;
    a[0][1] = q12 - q03;
    a[1][1] = q00 + q22;
    a[2][1] = q23 + q01;
    a[0][2] = q13 + q02;
    a[1][2] = q23 - q01;
    a[2][2] = q00 + q33;
}




itensor irotation::matrixForm() const
{
    itensor m;
    m = *this;
    return m;
}




ivector irotation::rotationVector() const
{
    ivector v;
    v.extractFrom(*this);
    return v;
}




irotation irotation::slerp(const irotation& rot1, const irotation& rot2, double t)
{
    irotation theta = rot2*rot1.transpose();
    ivector   v = theta.rotationVector();
    return    irotation(t*v)*rot1;
}




void irotation::setRandom()
{
    ivector v; v.setRandom();
    (*this) = irotation(v);
}




iquaternion::iquaternion()
{
#ifdef INIT2ZERO
    q[0] = q[1] = q[2] = 0.0;
    q[3] = 1.0;
#endif
}




iquaternion::iquaternion(const double q0, const double q1, const double q2, const double q3)
{
    q[0] = q0;
    q[1] = q1;
    q[2] = q2;
    q[3] = q3;
}




iquaternion::iquaternion(const double* qr)
{
    q[0] = qr[0];
    q[1] = qr[1];
    q[2] = qr[2];
    q[3] = qr[3];
}




iquaternion::iquaternion(const iquaternion& q_)
{
    q[0] = q_.x();
    q[1] = q_.y();
    q[2] = q_.z();
    q[3] = q_.w();
}




iquaternion::iquaternion(const irotation &m)
{
    extractFromRotationMatrix(m);
}




iquaternion::iquaternion(const ivector &theta)
{
    // extract the iquaternion such that expmap[theta] = quat->rotationMatrix
    double thetanr2, rr, fac2;
    
    thetanr2 = theta[0]*theta[0]+theta[1]*theta[1]+theta[2]*theta[2];
    thetanr2 = 0.5*sqrt(thetanr2);
    
    if (thetanr2 < 1e-4)
    {
        rr   = thetanr2*thetanr2;
        fac2 = 0.5 - rr*(840.0 - rr*(42.0- rr))/10080.0;
    }
    else
        fac2 = sin(thetanr2)/thetanr2 * 0.5;
    
    q[3] = cos(thetanr2);
    for (size_t i=0; i<3; i++) q[i] = fac2 * theta[i];
}




// extract iquaternion from a rotation matrix with Spurrier algorithm
void iquaternion::extractFromRotationMatrix(const irotation &rot)
{
    double trace, xm, bigm, dum;
    size_t e[]={0,1,2,0,1};
    size_t ii, j, k, l, ll, i;

    itensor mat;
    mat = rot.matrixForm();
    
    trace = mat.trace();
    ii    = 0;
    xm    = mat(0,0);
    
    if (mat(1,1) > xm)
    {
        xm = mat(1,1);
        ii = 1;
    }
    
    if (mat(2,2) > xm)
    {
        xm = mat(2,2);
        ii  = 2;
    }
    
    bigm = trace > xm ? trace : xm;
    
    if (bigm == trace)
    {
        q[3] = 0.5*sqrt(1.0 + trace);
        dum  = 0.25/ q[3];
        for (i=0; i<3; i++)
        {
            j    = e[i+1];
            k    = e[i+2];
            q[i] = dum*(mat(k,j) - mat(j,k));
        }
    }
    else
    {
        q[ii] = sqrt(0.5*mat(ii,ii) + 0.25*(1.0 - trace));
        dum   = 0.25 / q[ii];
        j     = e[ii+1];
        k     = e[ii+2];
        q[3]  = dum*(mat(k,j) - mat(j,k));
        for (ll=ii+1; ll<=ii+2; ll++)
        {
            l    = e[ll];
            q[l] = dum*(mat(l,ii) + mat(ii,l));
        }
    }
    
    // if, during computations, the iquaternion has lost its orthogonality, normalize it
    double n = norm();
    if ( fabs(n - 1.0) > 1e-6)
    {
        n = 1.0/n;
        for (i=0; i<4; i++) q[i] *= n;
    }
}



iquaternion iquaternion::conjugate() const
{
    return iquaternion(-q[0], -q[1], -q[2], q[3]);
}




iquaternion iquaternion::identity()
{
    return iquaternion(0.0, 0.0, 0.0, 1.0);
}




double iquaternion::norm() const
{
    return sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
}




void iquaternion::normalize()
{
    double n(1.0/norm());
    q[0] *= n;
    q[1] *= n;
    q[2] *= n;
    q[3] *= n;
}




void iquaternion::setRandom()
{
    const double dm = 1.0/static_cast<double>(RAND_MAX);
    q[0] = static_cast<double>(rand()) * dm;
    q[1] = static_cast<double>(rand()) * dm;
    q[2] = static_cast<double>(rand()) * dm;
    q[3] = static_cast<double>(rand()) * dm;

    normalize();
}




void iquaternion::toIdentity()
{
    q[0] = q[1] = q[2] = 0.0;
    q[3] = 1.0;
}




double& iquaternion::x()
{
    return q[0];
}



double& iquaternion::y()
{
    return q[1];
}



double& iquaternion::z()
{
    return q[2];
}



double& iquaternion::w()
{
    return q[3];
}




const double& iquaternion::x() const
{
    return q[0];
}




const double& iquaternion::y() const
{
    return q[1];
}




const double& iquaternion::z() const
{
    return q[2];
}




const double& iquaternion::w() const
{
    return q[3];
}




void vector2::print(std::ostream &of) const
{
    of << x[0] << x[1];
}



    
void vector2::setRandom()
{
    const double dm = 1.0/static_cast<double>(RAND_MAX);
    x[0] = static_cast<double>(rand()) * dm;
    x[1] = static_cast<double>(rand()) * dm;
}




ivector iquaternion::operator()(const ivector& v) const
{
    iquaternion qv;
    qv.x() = v(0);
    qv.y() = v(1);
    qv.z() = v(2);
    qv.w() = 0.0;

    iquaternion tmp = ((*this) * qv) * ((*this).conjugate());
    return ivector( tmp.x(), tmp.y(), tmp.z());
}




iquaternion& iquaternion::operator=(const iquaternion &rhs)
{
    q[0] = rhs.x();
    q[1] = rhs.y();
    q[2] = rhs.z();
    q[3] = rhs.w();
    return *this;
}




iquaternion& iquaternion::operator=(const irotation &rhs)
{
    *this = iquaternion(rhs);
    return *this;
}




iquaternion operator*(const iquaternion &p, const iquaternion &q)
{
    ivector pv(p.x(), p.y(), p.z());
    ivector qv(q.x(), q.y(), q.z());

    double  pqs = p.w()*q.w() - pv.dot(qv);
    ivector pqv = pv.cross(qv) + p.w()*qv + q.w()*pv;

    iquaternion pq( pqv(0), pqv(1), pqv(2), pqs);

    return pq;
}




std::ostream& operator<<(std::ostream &os, const iquaternion &v)
{
    os  << std::setw(12) << std::setprecision(8)
    << "[ " << v.x() << " , " << v.y() << " , " << v.z() << " , " << v.w()
    << " ]" << std::flush;
    
    return os;
}




std::ostream& operator<<(std::ostream &os, const vector2 &v)
{
    os  << std::setw(12) << std::setprecision(8)
    << "[ " << v.x[0] << " , " << v.x[1] << " ]" << std::flush;
    
    return os;
}

#endif
