StatisticsPoint.hcc

Go to the documentation of this file.

//Copyright (c) 2013-2014, The MercuryDPM Developers Team. All rights reserved.
//For the list of developers, see <http://www.MercuryDPM.org/Team>.
//
//Redistribution and use in source and binary forms, with or without
//modification, are permitted provided that the following conditions are met:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name MercuryDPM nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
//ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
//WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
//DISCLAIMED. IN NO EVENT SHALL THE MERCURYDPM DEVELOPERS TEAM BE LIABLE FOR ANY
//DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
//ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
//(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

std::ostream& operator<<(std::ostream& os, const CG S)
{
    if (S == HeavisideSphere)
        os << "HeavisideSphere";
    else if (S == Gaussian)
        os << "Gaussian";
    else if (S == Polynomial)
        os << "Polynomial";
    return os;
}

template<StatType T>
StatisticsVector<T>* StatisticsPoint<T>::gb;

template<StatType T>
void StatisticsPoint<T>::set_zero()
{
    Nu = 0.0;
    Density = 0.0;
    Momentum.setZero();
    DisplacementMomentum.setZero();
    Displacement.setZero();
    MomentumFlux.setZero();
    DisplacementMomentumFlux.setZero();
    EnergyFlux.setZero();
    TangentialStress.setZero();
    NormalStress.setZero();
    TangentialStress.setZero();
    NormalTraction.setZero();
    TangentialTraction.setZero();
    Fabric.setZero();
    CollisionalHeatFlux.setZero();
    Dissipation = 0.0;
    Potential = 0.0;
    LocalAngularMomentum.setZero();
    LocalAngularMomentumFlux.setZero();
    ContactCoupleStress.setZero();
}

template<StatType T>
StatisticsPoint<T> StatisticsPoint<T>::getSquared()
{
    StatisticsPoint < T > P;
    P.Nu = mathsFunc::square(Nu);
    P.Density = mathsFunc::square(Density);
    P.Momentum = Vec3D::square(Momentum);
    P.DisplacementMomentum = Vec3D::square(DisplacementMomentum);
    P.Displacement = MatrixSymmetric3D::square(Displacement);
    P.MomentumFlux = MatrixSymmetric3D::square(MomentumFlux);
    P.DisplacementMomentumFlux = MatrixSymmetric3D::square(DisplacementMomentumFlux);
    P.EnergyFlux = Vec3D::square(EnergyFlux);
    P.NormalStress = Matrix3D::square(NormalStress);
    P.TangentialStress = Matrix3D::square(TangentialStress);
    P.NormalTraction = Vec3D::square(NormalTraction);
    P.TangentialTraction = Vec3D::square(TangentialTraction);
    P.Fabric = MatrixSymmetric3D::square(Fabric);
    P.CollisionalHeatFlux = Vec3D::square(CollisionalHeatFlux);
    P.Dissipation = mathsFunc::square(Dissipation);
    P.Potential = mathsFunc::square(Potential);
    P.LocalAngularMomentum = Vec3D::square(LocalAngularMomentum);
    P.LocalAngularMomentumFlux = Matrix3D::square(LocalAngularMomentumFlux);
    P.ContactCoupleStress = Matrix3D::square(ContactCoupleStress);
    return P;
}

template<StatType T>
StatisticsPoint<T>& StatisticsPoint<T>::operator=(const StatisticsPoint<T> &P)
{
    Nu = P.Nu;
    Density = P.Density;
    Momentum = P.Momentum;
    DisplacementMomentum = P.DisplacementMomentum;
    Displacement = P.Displacement;
    MomentumFlux = P.MomentumFlux;
    DisplacementMomentumFlux = P.DisplacementMomentumFlux;
    EnergyFlux = P.EnergyFlux;
    NormalStress = P.NormalStress;
    TangentialStress = P.TangentialStress;
    NormalTraction = P.NormalTraction;
    TangentialTraction = P.TangentialTraction;
    Fabric = P.Fabric;
    CollisionalHeatFlux = P.CollisionalHeatFlux;
    Dissipation = P.Dissipation;
    Potential = P.Potential;
    LocalAngularMomentum = P.LocalAngularMomentum;
    LocalAngularMomentumFlux = P.LocalAngularMomentumFlux;
    ContactCoupleStress = P.ContactCoupleStress;
    return *this;
}

template<StatType T>
StatisticsPoint<T>& StatisticsPoint<T>::operator+=(const StatisticsPoint<T> &P)
{
    Nu += P.Nu;
    Density += P.Density;
    Momentum += P.Momentum;
    DisplacementMomentum += P.DisplacementMomentum;
    Displacement += P.Displacement;
    MomentumFlux += P.MomentumFlux;
    DisplacementMomentumFlux += P.DisplacementMomentumFlux;
    EnergyFlux += P.EnergyFlux;
    NormalStress += P.NormalStress;
    TangentialStress += P.TangentialStress;
    NormalTraction += P.NormalTraction;
    TangentialTraction += P.TangentialTraction;
    Fabric += P.Fabric;
    CollisionalHeatFlux += P.CollisionalHeatFlux;
    Dissipation += P.Dissipation;
    Potential += P.Potential;
    LocalAngularMomentum += P.LocalAngularMomentum;
    LocalAngularMomentumFlux += P.LocalAngularMomentumFlux;
    ContactCoupleStress += P.ContactCoupleStress;
    return *this;
}

template<StatType T>
StatisticsPoint<T>& StatisticsPoint<T>::operator-=(const StatisticsPoint<T> &P)
{
    Nu -= P.Nu;
    Density -= P.Density;
    Momentum -= P.Momentum;
    DisplacementMomentum -= P.DisplacementMomentum;
    Displacement -= P.Displacement;
    MomentumFlux -= P.MomentumFlux;
    DisplacementMomentumFlux -= P.DisplacementMomentumFlux;
    EnergyFlux -= P.EnergyFlux;
    NormalStress -= P.NormalStress;
    TangentialStress -= P.TangentialStress;
    NormalTraction -= P.NormalTraction;
    TangentialTraction -= P.TangentialTraction;
    Fabric -= P.Fabric;
    CollisionalHeatFlux -= P.CollisionalHeatFlux;
    Dissipation -= P.Dissipation;
    Potential -= P.Potential;
    LocalAngularMomentum -= P.LocalAngularMomentum;
    LocalAngularMomentumFlux -= P.LocalAngularMomentumFlux;
    ContactCoupleStress -= P.ContactCoupleStress;
    return *this;
}

template<StatType T>
StatisticsPoint<T>& StatisticsPoint<T>::operator/=(const Mdouble a)
{
    Nu /= a;
    Density /= a;
    Momentum /= a;
    DisplacementMomentum /= a;
    Displacement /= a;
    MomentumFlux /= a;
    DisplacementMomentumFlux /= a;
    EnergyFlux /= a;
    NormalStress /= a;
    TangentialStress /= a;
    NormalTraction /= a;
    TangentialTraction /= a;
    Fabric /= a;
    CollisionalHeatFlux /= a;
    Dissipation /= a;
    Potential /= a;
    LocalAngularMomentum /= a;
    LocalAngularMomentumFlux /= a;
    ContactCoupleStress /= a;
    return *this;
}

//in the first average, one timestep in Displacement is missing
template<StatType T>
void StatisticsPoint<T>::firstTimeAverage(const int n)
{
    Nu /= n;
    Density /= n;
    Momentum /= n;
    if (n == 1)
    {
        DisplacementMomentum.setZero();
        Displacement.setZero();
        DisplacementMomentumFlux.setZero();
    }
    else
    {
        DisplacementMomentum /= n - 1;
        Displacement /= n - 1;
        DisplacementMomentumFlux /= n - 1;
    }
    MomentumFlux /= n;
    EnergyFlux /= n;
    NormalStress /= n;
    TangentialStress /= n;
    NormalTraction /= n;
    TangentialTraction /= n;
    Fabric /= n;
    CollisionalHeatFlux /= n;
    Dissipation /= n;
    Potential /= n;
    LocalAngularMomentum /= n;
    LocalAngularMomentumFlux /= n;
    ContactCoupleStress /= n;
}

template<StatType T>
Mdouble StatisticsPoint<T>::getDistanceSquaredNonAveraged(const Vec3D &P)
{
    return mathsFunc::square(P.X - Position.X) + mathsFunc::square(P.Y - Position.Y) + mathsFunc::square(P.Z - Position.Z);
}

template<StatType T>
Vec3D StatisticsPoint<T>::crossNonAveraged(Vec3D P, Vec3D &Q)
{
    return Vec3D(P.Y * Q.Z - P.Z * Q.Y, P.Z * Q.X - P.X * Q.Z, P.X * Q.Y - P.Y * Q.X);
}

template<StatType T> Matrix3D StatisticsPoint<T>::matrixCrossNonAveraged(Vec3D P, Matrix3D &Q)
{
    return Matrix3D(
            P.Y * Q.ZX - P.Z * Q.YX, P.Z * Q.XX - P.X * Q.ZX, P.X * Q.YX - P.Y * Q.XX,
            P.Y * Q.ZY - P.Z * Q.YY, P.Z * Q.XY - P.X * Q.ZY, P.X * Q.YY - P.Y * Q.XY,
            P.Y * Q.ZZ - P.Z * Q.YZ, P.Z * Q.XZ - P.X * Q.ZZ, P.X * Q.YZ - P.Y * Q.XZ);
}

template<StatType T>
Mdouble StatisticsPoint<T>::dotNonAveraged(const Vec3D &P, const Vec3D &Q)
{
    return P.X * Q.X + P.Y * Q.Y + P.Z * Q.Z;
}

template<StatType T>
Mdouble StatisticsPoint<T>::CG_function(const Vec3D &PI)
{
    Mdouble dist2 = getDistanceSquaredNonAveraged(PI);
    if (getCGShape() == HeavisideSphere)
    {
        return (getCGWidthSquared() < dist2) ? 0.0 : getCGInverseVolume();
    }
    else if (getCGShape() == Gaussian)
    {
        return (getCutoff2() < dist2) ? 0.0 : getCGInverseVolume() * exp(-dist2 / (2.0 * getCGWidthSquared()));
    }
    else if (getCGShape() == Polynomial)
    {
        return (getCutoff2() < dist2) ? 0.0 : getCGInverseVolume() * evaluatePolynomial(sqrt(dist2) / getCutoff());
    }
    else
    {
        std::cerr << "error in CG_function" << std::endl;
        exit(-1);
    }
}

template<StatType T>
Mdouble StatisticsPoint<T>::CG_function_2D(const Vec3D &PI)
{
    Mdouble dist2 = getDistanceSquaredNonAveraged(PI);
    if (getCGShape() == HeavisideSphere)
    {
        if (getCGWidthSquared() < dist2)
            return 0.0;
        else
        {
            Mdouble wn = sqrt(getCGWidthSquared() - dist2);
            return getCGInverseVolume() * 2.0 * wn;
            //return getCGInverseVolume()*(std::min(wn,gb->getObjectDistanceLeft()) +std::min(wn,gb->getObjectDistanceRight()));    
        }
    }
    else if (getCGShape() == Gaussian)
    {
        return (getCutoff2() < dist2) ? 0.0 : getCGInverseVolume() * exp(-dist2 / (2.0 * getCGWidthSquared()));
    }
    else if (getCGShape() == Polynomial)
    {
        return (getCutoff2() < dist2) ? 0.0 : getCGInverseVolume() * evaluatePolynomial(sqrt(dist2) / getCutoff());
    }
    else
    {
        std::cerr << "error in CG_function_2D" << std::endl;
        exit(-1);
    }
}

template<StatType T>
Mdouble StatisticsPoint<T>::CG_function_1D(const Vec3D &PI)
{
    Mdouble dist2 = getDistanceSquaredNonAveraged(PI);
    if (getCGShape() == HeavisideSphere)
    {
        return (getCGWidthSquared() < dist2) ? 0.0 : (getCGInverseVolume() * constants::pi * (getCGWidthSquared() - dist2));
    }
    else if (getCGShape() == Gaussian)
    {
        return (getCutoff2() < dist2) ? 0.0 : getCGInverseVolume() * exp(-dist2 / (2.0 * getCGWidthSquared()));
    }
    else if (getCGShape() == Polynomial)
    {
        return (getCutoff2() < dist2) ? 0.0 : getCGInverseVolume() * evaluatePolynomial(sqrt(dist2) / getCutoff());
    }
    else
    {
        std::cerr << "error in CG_function_1D" << std::endl;
        exit(-1);
    }
}

template<StatType T>
Vec3D StatisticsPoint<T>::CG_gradient(const Vec3D &P, const Mdouble phi)
{
    //CG_type_todo
    if (getCGShape() == Gaussian)
    {
        return (P - Position) * (phi / getCGWidthSquared());
    }
    else if (getCGShape() == Polynomial)
    {
        Mdouble r = Vec3D::getLength(Position - P) / getCGWidth();
        return getCGInverseVolume() * evaluatePolynomialGradient(r) / r / getCGWidthSquared() * (Position - P);
    }
    else
    {
        std::cerr << "error in CG_gradient" << std::endl;
        exit(-1);
    }
}

template<StatType T>
Vec3D StatisticsPoint<T>::CG_integral_gradient(Vec3D &P1, Vec3D &P2, Vec3D &P1_P2_normal, Mdouble P1_P2_distance)
{
    Vec3D P_P1 = Position - P1;
    Vec3D P_P2 = Position - P2;
    Mdouble a = dotNonAveraged(P_P1, P1_P2_normal);
    Mdouble b = dotNonAveraged(P_P2, P1_P2_normal);
    Vec3D tangential = P_P1 - a * P1_P2_normal;
    //CG_type_todo
    if (getCGShape() == Gaussian)
    {
        Mdouble wsq2 = sqrt(2 * getCGWidthSquared());
        Mdouble f = sqrt(2 * constants::pi * getCGWidthSquared());
        return Vec3D(0.0, 0.0, (exp(-mathsFunc::square(a / wsq2)) - exp(-mathsFunc::square(b / wsq2))) / f / (P2.Z - P1.Z));
    }
    else if (getCGShape() == Polynomial)
    {
        double wn2 = getCGWidthSquared() - dotNonAveraged(tangential, tangential);
        if ((wn2 <= 0) | (a * fabs(a) >= wn2) | (b * fabs(b) <= -wn2))
        {
            return Vec3D(0, 0, 0);
        }
        else
        {
            double wn = std::sqrt(wn2);
            double w = getCGWidth();
            double delta = w * 1e-3;
            double I = getCGInverseVolume() * evaluateIntegral(std::max(a, -wn) / w, std::min(b, wn) / w, tangential.getLength() / w) * w / P1_P2_distance;
            Vec3D delta_P_P1 = P_P1 + Vec3D(delta, 0, 0);
            Vec3D delta_P_P2 = P_P2 + Vec3D(delta, 0, 0);
            a = dotNonAveraged(delta_P_P1, P1_P2_normal);
            b = dotNonAveraged(delta_P_P2, P1_P2_normal);
            tangential = delta_P_P1 - a * P1_P2_normal;
            wn = std::sqrt(getCGWidthSquared() - dotNonAveraged(tangential, tangential));
            double Ix = getCGInverseVolume() * evaluateIntegral(std::max(a, -wn) / w, std::min(b, wn) / w, tangential.getLength() / w) * w / P1_P2_distance;
            delta_P_P1 = P_P1 + Vec3D(0, delta, 0);
            delta_P_P2 = P_P2 + Vec3D(0, delta, 0);
            a = dotNonAveraged(delta_P_P1, P1_P2_normal);
            b = dotNonAveraged(delta_P_P2, P1_P2_normal);
            tangential = delta_P_P1 - a * P1_P2_normal;
            wn = std::sqrt(getCGWidthSquared() - dotNonAveraged(tangential, tangential));
            double Iy = getCGInverseVolume() * evaluateIntegral(std::max(a, -wn) / w, std::min(b, wn) / w, tangential.getLength() / w) * w / P1_P2_distance;
            delta_P_P1 = P_P1 + Vec3D(0, 0, delta);
            delta_P_P2 = P_P2 + Vec3D(0, 0, delta);
            a = dotNonAveraged(delta_P_P1, P1_P2_normal);
            b = dotNonAveraged(delta_P_P2, P1_P2_normal);
            tangential = delta_P_P1 - a * P1_P2_normal;
            wn = std::sqrt(getCGWidthSquared() - dotNonAveraged(tangential, tangential));
            double Iz = getCGInverseVolume() * evaluateIntegral(std::max(a, -wn) / w, std::min(b, wn) / w, tangential.getLength() / w) * w / P1_P2_distance;
            return Vec3D(Ix - I, Iy - I, Iz - I) / delta;
        }
    }
    else
    {
        return Vec3D(0, 0, 0);
    }
    //~ if (getCGShape()==Gaussian) return Vec3D(0.0,0.0,0.0);
    //~ else { std::cerr << "error in CG_function" << std::endl; exit(-1); }
}

template<StatType T>
Mdouble StatisticsPoint<T>::CG_integral_gradient_1D(Vec3D &P1, Vec3D &P2, Vec3D &P1_P2_normal, Mdouble P1_P2_distance)
{
    Vec3D P_P1 = Position - P1;
    Vec3D P_P2 = Position - P2;
    Mdouble a = dotNonAveraged(P_P1, P1_P2_normal);
    Mdouble b = dotNonAveraged(P_P2, P1_P2_normal);
    Vec3D tangential = P_P1 - a * P1_P2_normal;
    //CG_type_todo
    if (getCGShape() == Gaussian)
    {
        Mdouble wsq2 = sqrt(2 * getCGWidthSquared());
        Mdouble f = sqrt(2 * constants::pi * getCGWidthSquared());
        return (exp(-mathsFunc::square(a / wsq2)) - exp(-mathsFunc::square(b / wsq2))) / f / (P2.Z - P1.Z);
    }
    else if (getCGShape() == Polynomial)
    {
        double wn2 = getCGWidthSquared() - dotNonAveraged(tangential, tangential);
        if ((wn2 <= 0) | (a * fabs(a) >= wn2) | (b * fabs(b) <= -wn2))
        {
            return 0;
        }
        else if ((P1_P2_distance < 1e-12) | (Vec3D::getLengthSquared(tangential) < 1e-24))
        {
            //if the normal is parallel to the averaging direction
            std::cout << "normal is parallel to the averaging direction: "
                    << " P1_P2_distance " << P1_P2_distance
                    << " tangential " << tangential << std::endl;
            return 0; //CG_gradient_1D(P1,CG_function(P1));
        }
        else
        {
            double wn = sqrt(wn2);
            double w = getCGWidth();
            double delta = w * 3e-6;
            //double I =  getCGInverseVolume()*evaluateIntegral(std::max(a,-wn)/w,std::min(b,wn)/w,dot(tangential,tangential)/w)*w / P1_P2_distance;
            Vec3D delta_P_P1 = P_P1 + Vec3D(delta, delta, delta);
            Vec3D delta_P_P2 = P_P2 + Vec3D(delta, delta, delta);
            a = dotNonAveraged(delta_P_P1, P1_P2_normal);
            b = dotNonAveraged(delta_P_P2, P1_P2_normal);
            tangential = delta_P_P1 - a * P1_P2_normal;
            wn = sqrt(getCGWidthSquared() - dotNonAveraged(tangential, tangential));
            double I2 = getCGInverseVolume() * evaluateIntegral(std::max(a, -wn) / w, std::min(b, wn) / w, dotNonAveraged(tangential, tangential) / w) * w / P1_P2_distance;
            
            delta_P_P1 = P_P1 - Vec3D(delta, delta, delta);
            delta_P_P2 = P_P2 - Vec3D(delta, delta, delta);
            a = dotNonAveraged(delta_P_P1, P1_P2_normal);
            b = dotNonAveraged(delta_P_P2, P1_P2_normal);
            tangential = delta_P_P1 - a * P1_P2_normal;
            wn = sqrt(getCGWidthSquared() - dotNonAveraged(tangential, tangential));
            double I1 = getCGInverseVolume() * evaluateIntegral(std::max(a, -wn) / w, std::min(b, wn) / w, dotNonAveraged(tangential, tangential) / w) * w / P1_P2_distance;
            
            return (I2 - I1) / (2. * delta);
        }
    }
    else
        return 0;
}

template <StatType T>
Mdouble StatisticsPoint<T>::CG_integral(Vec3D &P1, Vec3D &P2, Vec3D &P1_P2_normal, Mdouble P1_P2_distance, Vec3D& rpsi)
{   
    //apply the cutoff in normal direction:
    Vec3D P_P1 = Position - P1;
    Mdouble a = dotNonAveraged(P_P1,P1_P2_normal);
    if ( a > getCutoff() ) return 0.0;
    Vec3D P_P2 = Position - P2;
    Mdouble b = dotNonAveraged(P_P2,P1_P2_normal);
    if ( -b > getCutoff() ) return 0.0;
    //apply the cutoff in tangential direction:
    Vec3D tangential = P_P1 - a * P1_P2_normal;
    if (dotNonAveraged(tangential,tangential)>getCutoff2()) return 0.0;
    //evaluate:
    if (getCGShape()==HeavisideSphere)
    {   
        Mdouble wn2 = getCGWidthSquared() - dotNonAveraged(tangential,tangential);
        if ((wn2<=0) | (a*fabs(a)>=wn2) | (b*fabs(b)<=-wn2))
        {   
            return 0;
        }
        else
        {   
            Mdouble wn = sqrt(wn2);
            return getCGInverseVolume()*( std::min(b,wn)-std::max(a,-wn) ) / P1_P2_distance;
        }
    }
    else if (getCGShape()==Gaussian)
    { //Gaussian
        static Mdouble InvVolumeExp = compute_Gaussian_invvolume(gb->getSystemDimensions()-1);
        static Mdouble w_sqrt_2 = constants::sqrt_2*getCGWidth();
        static Mdouble P1_P2_distance_for_cutoff=-1, InvVolumeErf=-1;
        if (P1_P2_distance_for_cutoff!=P1_P2_distance)
        {   
            P1_P2_distance_for_cutoff = P1_P2_distance;
            Mdouble amax = -getCutoff();
            Mdouble bmax = getCutoff()+P1_P2_distance;
            InvVolumeErf = P1_P2_distance/(
                    erf(bmax/w_sqrt_2)*bmax+w_sqrt_2/constants::sqrt_pi*exp(-bmax*bmax/(w_sqrt_2*w_sqrt_2))
                    -erf(amax/w_sqrt_2)*amax-w_sqrt_2/constants::sqrt_pi*exp(-amax*amax/(w_sqrt_2*w_sqrt_2)));
            // std::cout << "InvVolumeErf" << InvVolumeErf << " InvVolumeExp" << InvVolumeExp << " f" << getCGInverseVolume() * sqrt(2*constants::pi*getCGWidthSquared()) << std::endl;
        }
        Mdouble psi=exp(-dotNonAveraged(tangential,tangential)/(2.0*getCGWidthSquared())) * InvVolumeExp
        * ( erf(b/w_sqrt_2) - erf(a/w_sqrt_2) ) * InvVolumeErf /2./P1_P2_distance;
        rpsi=Position*psi;
        return psi;
    }
    else if (getCGShape()==Polynomial)
    {   
        double wn2 = getCGWidthSquared() - dotNonAveraged(tangential,tangential);
        if ((wn2<=0) | (a*fabs(a)>=wn2) | (b*fabs(b)<=-wn2))
        {   
            return 0;
        }
        else
        {   
            double wn = sqrt(wn2);
            double w = getCGWidth();
            return getCGInverseVolume()*evaluateIntegral(std::max(a,-wn)/w,std::min(b,wn)/w,tangential.getLength()/w)*w / P1_P2_distance;
        }
        //CG_type_todo
    }
    else
    {   std::cerr << "error in CG_integral" << std::endl; exit(-1);}
}

template<StatType T>
Mdouble StatisticsPoint<T>::CG_integral_2D(Vec3D &P1, Vec3D &P2, Vec3D &P1_P2_normal, Mdouble P1_P2_distance, Mdouble& rpsi_scalar)
{
    //apply the cutoff in normal direction:
    Vec3D P_P1 = Position - P1;
    Mdouble a = dotNonAveraged(P_P1, P1_P2_normal);
    if (a > getCutoff())
        return 0.0;
    Vec3D P_P2 = Position - P2;
    Mdouble b = dotNonAveraged(P_P2, P1_P2_normal);
    if (-b > getCutoff())
        return 0.0;
    //apply the cutoff in tangential direction:
    Vec3D tangential = P_P1 - a * P1_P2_normal;
    if (dotNonAveraged(tangential, tangential) > getCutoff2())
        return 0.0;
    if (getCGShape() == HeavisideSphere)
    {
        Mdouble wn2 = getCGWidthSquared() - dotNonAveraged(tangential, tangential);
        if ((wn2 <= 0) || (a * fabs(a) >= wn2) || (b * fabs(b) <= -wn2))
        {
            return 0;
        }
        else
        {
            Mdouble wn = sqrt(wn2);
            //if the normal is parallel to the averaging direction
            if (std::max(fabs(a), fabs(b)) < 1e-20)
            {
                return getCGInverseVolume() * 2 * wn;
            }
            return getCGInverseVolume() / P1_P2_distance
                    * (((b >= wn) ? (constants::pi * wn2 / 2.0) : (b * sqrt(wn2 - mathsFunc::square(b)) + wn2 * asin(b / wn)))
                            + ((a <= -wn) ? (constants::pi * wn2 / 2.0) : (-a * sqrt(wn2 - mathsFunc::square(a)) - wn2 * asin(a / wn))));
        }
    }
    else if (getCGShape() == Gaussian)
    { //Gaussian
        if (dotNonAveraged(P1_P2_normal, P1_P2_normal) < 1e-20)
        {
            //since we average parallel to the force line, the CG integral is shaped like the CG function
            return getCGInverseVolume() * exp(-dotNonAveraged(P_P1, P_P1) / (2.0 * getCGWidthSquared()));
        }
        static Mdouble InvVolumeExp = compute_Gaussian_invvolume(gb->getSystemDimensions() - 2);
        static Mdouble w_sqrt_2 = constants::sqrt_2 * getCGWidth();
        static Mdouble P1_P2_distance_for_cutoff = -1, InvVolumeErf = -1;
        if (P1_P2_distance_for_cutoff != P1_P2_distance)
        {
            P1_P2_distance_for_cutoff = P1_P2_distance;
            Mdouble amax = -getCutoff();
            Mdouble bmax = getCutoff() + P1_P2_distance;
            InvVolumeErf = P1_P2_distance / (
                    erf(bmax / w_sqrt_2) * bmax + w_sqrt_2 / constants::sqrt_pi * exp(-bmax * bmax / (w_sqrt_2 * w_sqrt_2))
                            - erf(amax / w_sqrt_2) * amax - w_sqrt_2 / constants::sqrt_pi * exp(-amax * amax / (w_sqrt_2 * w_sqrt_2))
                    ) / averagingVolume();
            //std::cout << "InvVolumeErf" << InvVolumeErf << " InvVolumeExp" << InvVolumeExp << " f" << getCGInverseVolume() * sqrt(2*constants::pi*getCGWidthSquared()) << std::endl;
        }
        //Note: use dot(t,t),  GetLength2(t), as dot only works on non-averaged directions
        //we also define rpsi
        Mdouble psi = exp(-dotNonAveraged(tangential, tangential) / (2.0 * getCGWidthSquared())) * InvVolumeExp
                * (erf(b / w_sqrt_2) - erf(a / w_sqrt_2)) * InvVolumeErf / 2. / P1_P2_distance;
        Mdouble phi1 = getCGInverseVolume() * exp(-dotNonAveraged(P_P1, P_P1) / (2.0 * getCGWidthSquared()));
        Mdouble phi2 = getCGInverseVolume() * exp(-dotNonAveraged(P_P2, P_P2) / (2.0 * getCGWidthSquared()));
        rpsi_scalar = -a / P1_P2_distance * psi + getCGWidthSquared() / P1_P2_distance / P1_P2_distance * (phi1 - phi2);
        return psi;
    }
    else if (getCGShape() == Polynomial)
    {
        double wn2 = getCGWidthSquared() - dotNonAveraged(tangential, tangential);
        if ((wn2 <= 0) | (a * fabs(a) >= wn2) | (b * fabs(b) <= -wn2))
        {
            return 0;
        }
        else if (P1_P2_distance < 1e-20)
        {
            //if the normal is parallel to the averaging direction
            return CG_function_1D(P_P1);
        }
        else
        {
            double wn = sqrt(wn2);
            double w = getCGWidth();
            return getCGInverseVolume() * evaluateIntegral(std::max(a, -wn) / w, std::min(b, wn) / w, tangential.getLength() / w) * w / P1_P2_distance;
        }
        //CG_type_todo
    }
    else
    {
        std::cerr << "error in CG_integral_2D" << std::endl;
        exit(-1);
    }
}

template<StatType T>
Mdouble StatisticsPoint<T>::CG_integral_1D(Vec3D &P1, Vec3D &P2, Vec3D &P1_P2_normal, Mdouble P1_P2_distance, Mdouble& rpsi_scalar)
{
    //apply the cutoff in normal direction:
    Vec3D P_P1 = Position - P1;
    Mdouble a = dotNonAveraged(P_P1, P1_P2_normal);
    if (a > getCutoff())
        return 0.0;
    Vec3D P_P2 = Position - P2;
    Mdouble b = dotNonAveraged(P_P2, P1_P2_normal);
    if (-b > getCutoff())
        return 0.0;
    //apply the cutoff in tangential direction:
    Vec3D tangential = P_P1 - a * P1_P2_normal;
    if (dotNonAveraged(tangential, tangential) > getCutoff2())
        return 0.0;
    if (getCGShape() == HeavisideSphere)
    {
        Mdouble wn2 = getCGWidthSquared() - dotNonAveraged(tangential, tangential);
        if ((wn2 <= 0) | (a * fabs(a) >= wn2) | (b * fabs(b) <= -wn2))
        {
            return 0;
        }
        else
        {
            //if the normal is parallel to the averaging direction
            if (std::max(fabs(a), fabs(b)) < 1e-20)
            {
                return getCGInverseVolume() * constants::pi * wn2;
            }
            Mdouble wn = sqrt(wn2);
            return getCGInverseVolume() / P1_P2_distance
                    * (((b >= wn) ? (2.0 / 3.0 * constants::pi * wn2 * wn) : (b * constants::pi * (wn2 - mathsFunc::square(b) / 3.0)))
                            - ((a <= -wn) ? (-2.0 / 3.0 * constants::pi * wn2 * wn) : (a * constants::pi * (wn2 - mathsFunc::square(a) / 3.0))));
        }
    }
    else if (getCGShape() == Gaussian)
    { //Gaussian
        if (fabs(b - a) < 1e-20)
        {
            return getCGInverseVolume() * exp(-dotNonAveraged(P_P1, P_P1) / (2.0 * getCGWidthSquared()));
        }
        static Mdouble w_sqrt_2 = constants::sqrt_2 * getCGWidth();
        static Mdouble P1_P2_distance_for_cutoff = -1, InvVolumeErf = -1;
        if (P1_P2_distance_for_cutoff != P1_P2_distance)
        {
            P1_P2_distance_for_cutoff = P1_P2_distance;
            Mdouble amax = -getCutoff();
            Mdouble bmax = getCutoff() + P1_P2_distance;
            InvVolumeErf = P1_P2_distance / (
                    erf(bmax / w_sqrt_2) * bmax + w_sqrt_2 / constants::sqrt_pi * exp(-bmax * bmax / (w_sqrt_2 * w_sqrt_2))
                            - erf(amax / w_sqrt_2) * amax - w_sqrt_2 / constants::sqrt_pi * exp(-amax * amax / (w_sqrt_2 * w_sqrt_2))
                    ) / averagingVolume();
            //std::cout << "InvVolumeErf" << InvVolumeErf << " InvVolumeExp" << InvVolumeExp << " f" << getCGInverseVolume() * sqrt(2*constants::pi*getCGWidthSquared()) << std::endl;
        }
        //Note: use dot(t,t),  GetLength2(t), as dot only works on non-averaged directions
        Mdouble psi = (erf(b / w_sqrt_2) - erf(a / w_sqrt_2)) * InvVolumeErf / 2. / P1_P2_distance;
        Mdouble phi1 = getCGInverseVolume() * exp(-dotNonAveraged(P_P1, P_P1) / (2.0 * getCGWidthSquared()));
        Mdouble phi2 = getCGInverseVolume() * exp(-dotNonAveraged(P_P2, P_P2) / (2.0 * getCGWidthSquared()));
        rpsi_scalar = -a / P1_P2_distance * psi + getCGWidthSquared() / P1_P2_distance / P1_P2_distance * (phi1 - phi2);
        return psi;
    }
    else if (getCGShape() == Polynomial)
    {
        //~ std::cout << setprecision(12) << P1_P2_normal << std::endl;
        double wn2 = getCGWidthSquared() - dotNonAveraged(tangential, tangential);
        if ((wn2 <= 0) | (a * fabs(a) >= wn2) | (b * fabs(b) <= -wn2))
        {
            return 0;
        }
        else if ((P1_P2_distance < 1e-12) | (Vec3D::getLengthSquared(tangential) < 1e-24))
        {
            //~ } else if (fabs(b-a)<1e-12) { 
            //if the normal is parallel to the averaging direction
            //~ std::cout << "normal is parallel to the averaging direction: "
            //~ << " P_P1 " << P1_P2_distance
            //~ << " psi= " << CG_function_1D(P1) << std::endl;
            return CG_function_1D(P1);
        }
        else
        {
            double wn = sqrt(wn2);
            double w = getCGWidth();
            return getCGInverseVolume() * evaluateIntegral(std::max(a, -wn) / w, std::min(b, wn) / w, dotNonAveraged(tangential, tangential) / w) * w / P1_P2_distance;
        }
    }
    else
    {
        std::cerr << "error in CG_integral_1D" << std::endl;
        exit(-1);
    }
    std::cout << "eind testje" << rpsi_scalar << std::endl;
}

template<StatType T>
std::string StatisticsPoint<T>::write_variable_names()
{
    std::stringstream ss;
    ss << "Nu "
            << "Density "
            << "MomentumX "
            << "MomentumY "
            << "MomentumZ "
            << "DisplacementMomentumX "
            << "DisplacementMomentumY "
            << "DisplacementMomentumZ "
            << "DisplacementXX "
            << "DisplacementXY "
            << "DisplacementXZ "
            << "DisplacementYY "
            << "DisplacementYZ "
            << "DisplacementZZ "
            << "MomentumFluxXX "
            << "MomentumFluxXY "
            << "MomentumFluxXZ "
            << "MomentumFluxYY "
            << "MomentumFluxYZ "
            << "MomentumFluxZZ "
            << "DisplacementMomentumFluxXX "
            << "DisplacementMomentumFluxXY "
            << "DisplacementMomentumFluxXZ "
            << "DisplacementMomentumFluxYY "
            << "DisplacementMomentumFluxYZ "
            << "DisplacementMomentumFluxZZ "
            << "EnergyFluxX "
            << "EnergyFluxY "
            << "EnergyFluxZ "
            << "NormalStressXX "
            << "NormalStressXY "
            << "NormalStressXZ "
            << "NormalStressYX "
            << "NormalStressYY "
            << "NormalStressYZ "
            << "NormalStressZX "
            << "NormalStressZY "
            << "NormalStressZZ "
            << "TangentialStressXX "
            << "TangentialStressXY "
            << "TangentialStressXZ "
            << "TangentialStressYX "
            << "TangentialStressYY "
            << "TangentialStressYZ "
            << "TangentialStressZX "
            << "TangentialStressZY "
            << "TangentialStressZZ "
            << "NormalTractionX "
            << "NormalTractionY "
            << "NormalTractionZ "
            << "TangentialTractionX "
            << "TangentialTractionY "
            << "TangentialTractionZ "
            << "FabricXX "
            << "FabricXY "
            << "FabricXZ "
            << "FabricYY "
            << "FabricYZ "
            << "FabricZZ "
            << "CollisionalHeatFluxX "
            << "CollisionalHeatFluxY "
            << "CollisionalHeatFluxZ "
            << "Dissipation "
            << "Potential "
            << "LocalAngularMomentumX "
            << "LocalAngularMomentumY "
            << "LocalAngularMomentumZ "
            << "LocalAngularMomentumFluxXX "
            << "LocalAngularMomentumFluxXY "
            << "LocalAngularMomentumFluxXZ "
            << "LocalAngularMomentumFluxYX "
            << "LocalAngularMomentumFluxYY "
            << "LocalAngularMomentumFluxYZ "
            << "LocalAngularMomentumFluxZX "
            << "LocalAngularMomentumFluxZY "
            << "LocalAngularMomentumFluxZZ "
            << "ContactCoupleStressXX "
            << "ContactCoupleStressXY "
            << "ContactCoupleStressXZ "
            << "ContactCoupleStressYX "
            << "ContactCoupleStressYY "
            << "ContactCoupleStressYZ "
            << "ContactCoupleStressZX "
            << "ContactCoupleStressZY "
            << "ContactCoupleStressZZ ";
    return ss.str();
}

template<StatType T>
std::string StatisticsPoint<T>::print() const
{
    std::stringstream ss;
    ss << "Nu " << Nu
            << ", Density " << Density
            << ",\n Momentum " << Momentum
            << ",\n DisplacementMomentum " << DisplacementMomentum
            << ",\n Displacement " << Displacement
            << ",\n MomentumFlux " << MomentumFlux
            << ",\n DisplacementMomentumFlux " << DisplacementMomentumFlux
            << ",\n EnergyFlux " << EnergyFlux
            << ",\n NormalStress " << NormalStress
            << ",\n TangentialStress " << TangentialStress
            << ",\n NormalTraction " << NormalTraction
            << ",\n TangentialTraction " << TangentialTraction
            << ",\n Fabric " << Fabric
            << ",\n CollisionalHeatFlux " << CollisionalHeatFlux
            << ",\n Dissipation " << Dissipation
            << ",\n Potential " << Potential
            << ",\n LocalAngularMomentum " << LocalAngularMomentum
            << ",\n LocalAngularMomentumFlux " << LocalAngularMomentumFlux
            << ",\n ContactCoupleStress " << ContactCoupleStress;
    return ss.str();
}

template<StatType T>
std::string StatisticsPoint<T>::print_sqrt() const
{
    std::stringstream ss;
    ss << "Nu " << std::sqrt(Nu)
            << ", Density " << std::sqrt(Density)
            << ", Momentum " << Vec3D::sqrt(Momentum)
            << ", DisplacementMomentum " << Vec3D::sqrt(DisplacementMomentum)
            << ", Displacement " << MatrixSymmetric3D::sqrt(Displacement)
            << ", MomentumFlux " << MatrixSymmetric3D::sqrt(MomentumFlux)
            << ", DisplacementMomentumFlux " << MatrixSymmetric3D::sqrt(DisplacementMomentumFlux)
            << ", EnergyFlux " << Vec3D::sqrt(EnergyFlux)
            << ", NormalStress " << Matrix3D::sqrt(NormalStress)
            << ", TangentialStress " << Matrix3D::sqrt(TangentialStress)
            << ", NormalTraction " << Vec3D::sqrt(NormalTraction)
            << ", TangentialTraction " << Vec3D::sqrt(TangentialTraction)
            << ", Fabric " << MatrixSymmetric3D::sqrt(Fabric)
            << ", CollisionalHeatFlux " << Vec3D::sqrt(CollisionalHeatFlux)
            << ", Dissipation " << std::sqrt(Dissipation)
            << ", Potential " << std::sqrt(Potential)
            << ", LocalAngularMomentum " << Vec3D::sqrt(LocalAngularMomentum)
            << ", LocalAngularMomentumFlux " << Matrix3D::sqrt(LocalAngularMomentumFlux)
            << ", ContactCoupleStress " << Matrix3D::sqrt(ContactCoupleStress);
    
    return ss.str();
}

template<StatType T>
std::string StatisticsPoint<T>::write() const
{
    std::stringstream ss;
    ss.precision(16);
    ss << Nu
            << " " << Density
            << " " << Momentum
            << " " << DisplacementMomentum
            << " " << Displacement
            << " " << MomentumFlux
            << " " << DisplacementMomentumFlux
            << " " << EnergyFlux
            << " " << NormalStress
            << " " << TangentialStress
            << " " << NormalTraction
            << " " << TangentialTraction
            << " " << Fabric
            << " " << CollisionalHeatFlux
            << " " << Dissipation
            << " " << Potential
            << " " << LocalAngularMomentum
            << " " << LocalAngularMomentumFlux
            << " " << ContactCoupleStress;
    return ss.str();
}

template<StatType T>
void StatisticsPoint<T>::set_Gaussian_invvolume(int dim)
{
    CG_invvolume = compute_Gaussian_invvolume(dim);
}

template<StatType T>
double StatisticsPoint<T>::compute_Gaussian_invvolume(int dim)
{
    if (dim == 0)
    {
        return 1.;
    }
    
    //this is the prefactor 1/V of phi(r)=1/V*exp(-|r|^2/w^2) for dim=1
    double CG_invvolume_computed = 1. / (constants::sqrt_2 * constants::sqrt_pi * getCGWidth());
    //take into account the cutoff radius and the dimension of the problem
    if (dim == 3)
    {
        CG_invvolume_computed = mathsFunc::cubic(CG_invvolume_computed);
        //Wolfram alpha: erf(c/(sqrt(2) w))-(sqrt(2/pi) c e^(-c^2/(2 w^2)))/w
        CG_invvolume_computed /= erf(getCutoff() / (constants::sqrt_2 * getCGWidth()))
                - constants::sqrt_2 / constants::sqrt_pi * getCutoff() / getCGWidth() * exp(-getCutoff2() / (2 * getCGWidthSquared()));
    }
    else if (dim == 2)
    {
        CG_invvolume_computed = mathsFunc::square(CG_invvolume_computed);
        //Wolfram alpha: integrate(x*exp(-x^2/(2w^2)),{x,0,c})/integrate(x*exp(-x^2/(2w^2)),{x,0,inf})=1-e^(-c^2/(2 w^2))
        CG_invvolume_computed /= 1 - exp(-getCutoff2() / (2 * getCGWidthSquared()));
    }
    else
    {
        CG_invvolume_computed /= erf(getCutoff() / (constants::sqrt_2 * getCGWidth()));
    }
    return CG_invvolume_computed;
}

template<StatType T>
void StatisticsPoint<T>::set_Heaviside_invvolume()
{
    CG_invvolume = 1.0 / (4.0 / 3.0 * constants::pi * sqrt(getCGWidthSquared()) * getCGWidthSquared());
}

template<StatType T>
void StatisticsPoint<T>::set_Polynomial_invvolume(int dim)
{
    if (dim == 3)
        CG_invvolume = 1.0 / (sqrt(getCGWidthSquared()) * getCGWidthSquared());
    else if (dim == 2)
        CG_invvolume = 1.0 / getCGWidthSquared();
    else
        CG_invvolume = 1.0 / sqrt(getCGWidthSquared());
}

template<StatType T> void StatisticsPoint<T>::setCGInverseVolume()
{
    if (getCGShape() == HeavisideSphere)
    {
        set_Heaviside_invvolume();
    }
    else if (getCGShape() == Gaussian)
    {
        set_Gaussian_invvolume (nonaveragedDim());
        } else if (getCGShape()==Polynomial)
        {   
            set_Polynomial_invvolume(nonaveragedDim());
        }
        else
        {   std::cerr << "error in CG_function" << std::endl; exit(-1);}
        CG_invvolume /= averagingVolume();
    }

template<> int StatisticsPoint<XYZ>::nonaveragedDim()
{
    return gb->getSystemDimensions();
}
template<> int StatisticsPoint<YZ>::nonaveragedDim()
{
    return gb->getSystemDimensions() - 1;
}
template<> int StatisticsPoint<XZ>::nonaveragedDim()
{
    return gb->getSystemDimensions() - 1;
}
template<> int StatisticsPoint<XY>::nonaveragedDim()
{
    return 2;
}
template<> int StatisticsPoint<X>::nonaveragedDim()
{
    return 1;
}
template<> int StatisticsPoint<Y>::nonaveragedDim()
{
    return 1;
}
template<> int StatisticsPoint<Z>::nonaveragedDim()
{
    return gb->getSystemDimensions() - 2;
}
template<> int StatisticsPoint<O>::nonaveragedDim()
{
    return 0;
}
template<> int StatisticsPoint<RAZ>::nonaveragedDim()
{
    exit(-1);
}
template<> int StatisticsPoint<AZ>::nonaveragedDim()
{
    exit(-1);
}
template<> int StatisticsPoint<RZ>::nonaveragedDim()
{
    exit(-1);
}
template<> int StatisticsPoint<RA>::nonaveragedDim()
{
    exit(-1);
}
template<> int StatisticsPoint<R>::nonaveragedDim()
{
    exit(-1);
}
template<> int StatisticsPoint<A>::nonaveragedDim()
{
    exit(-1);
}

template<> double StatisticsPoint<XYZ>::averagingVolume()
{
    return 1.0;
}
template<> double StatisticsPoint<XY>::averagingVolume()
{
    return ((gb->getSystemDimensions() != 3) ? (1.0) : (getZMaxStat() - getZMinStat()));
}
template<> double StatisticsPoint<XZ>::averagingVolume()
{
    return (getYMaxStat() - getYMinStat());
}
template<> double StatisticsPoint<YZ>::averagingVolume()
{
    return (getXMaxStat() - getXMinStat());
}
template<> double StatisticsPoint<X>::averagingVolume()
{
    return (getYMaxStat() - getYMinStat())
            * ((gb->getSystemDimensions() != 3) ? (1.0) : (getZMaxStat() - getZMinStat()));
}
template<> double StatisticsPoint<Y>::averagingVolume()
{
    return (getXMaxStat() - getXMinStat())
            * ((gb->getSystemDimensions() != 3) ? (1.0) : (getZMaxStat() - getZMinStat()));
}
template<> double StatisticsPoint<Z>::averagingVolume()
{
    return (getXMaxStat() - getXMinStat())
            * (getYMaxStat() - getYMinStat());
}
template<> double StatisticsPoint<O>::averagingVolume()
{
    return (getXMaxStat() - getXMinStat())
            * (getYMaxStat() - getYMinStat())
            * ((gb->getSystemDimensions() != 3) ? (1.0) : (getZMaxStat() - getZMinStat()));
}
template<> double StatisticsPoint<RAZ>::averagingVolume()
{
    exit(-1);
}
template<> double StatisticsPoint<AZ>::averagingVolume()
{
    exit(-1);
}
template<> double StatisticsPoint<RZ>::averagingVolume()
{
    exit(-1);
}
template<> double StatisticsPoint<RA>::averagingVolume()
{
    exit(-1);
}
template<> double StatisticsPoint<R>::averagingVolume()
{
    exit(-1);
}
template<> double StatisticsPoint<A>::averagingVolume()
{
    exit(-1);
}

template<> Mdouble StatisticsPoint<RA>::CG_function(const Vec3D &PI)
{
    return CG_function_2D(PI);
}
template<> Mdouble StatisticsPoint<RZ>::CG_function(const Vec3D &PI)
{
    Mdouble dist2 = getDistanceSquaredNonAveraged(PI);
    Mdouble R2 = mathsFunc::square(PI.X) + mathsFunc::square(PI.Y);
    Mdouble RI2 = mathsFunc::square(Position.X) + mathsFunc::square(Position.Y);
    Mdouble Z2 = mathsFunc::square(PI.Z - Position.Z);
    if (getCGShape() == Gaussian)
    {
        return (getCutoff2() < dist2) ? 0.0 : getCGInverseVolume()
                                                 * exp(-(R2 + RI2 + Z2) / (2.0 * getCGWidthSquared())) * besselFunc::I0(sqrt(R2 * RI2) / getCGWidthSquared());
    }
    else
    {
        std::cerr << "error in CG_function<RZ>" << std::endl;
        exit(-1);
    }
}
template<> Mdouble StatisticsPoint<AZ>::CG_function(const Vec3D&)
{
    std::cerr << "error in CG_function<AZ>" << std::endl;
    exit(-1);
}
template<> Mdouble StatisticsPoint<R>::CG_function(const Vec3D &PI)
{
    Mdouble dist2 = getDistanceSquaredNonAveraged(PI);
    Mdouble R2 = mathsFunc::square(PI.X) + mathsFunc::square(PI.Y);
    Mdouble RI2 = mathsFunc::square(Position.X) + mathsFunc::square(Position.Y);
    if (getCGShape() == Gaussian)
    {
        return (getCutoff2() < dist2) ? 0.0 : getCGInverseVolume()
                                                 * exp(-(R2 + RI2) / (2.0 * getCGWidthSquared())) * besselFunc::I0(sqrt(R2 * RI2) / getCGWidthSquared());
    }
    else
    {
        std::cerr << "error in CG_function<R>" << std::endl;
        exit(-1);
    }
}
template<> Mdouble StatisticsPoint<A>::CG_function(const Vec3D&)
{
    std::cerr << "error in CG_function<A>" << std::endl;
    exit(-1);
}

template<> Mdouble StatisticsPoint<XY>::CG_function(const Vec3D &PI)
{
    return CG_function_2D(PI);
}
template<> Mdouble StatisticsPoint<XZ>::CG_function(const Vec3D &PI)
{
    return CG_function_2D(PI);
}
template<> Mdouble StatisticsPoint<YZ>::CG_function(const Vec3D &PI)
{
    return CG_function_2D(PI);
}
template<> Mdouble StatisticsPoint<X>::CG_function(const Vec3D &PI)
{
    return CG_function_1D(PI);
}
template<> Mdouble StatisticsPoint<Y>::CG_function(const Vec3D &PI)
{
    return CG_function_1D(PI);
}
template<> Mdouble StatisticsPoint<Z>::CG_function(const Vec3D &PI)
{
    return CG_function_1D(PI);
}
template<> Mdouble StatisticsPoint<O>::CG_function(const Vec3D&)
{
    return getCGInverseVolume();
}

template<> Vec3D StatisticsPoint<YZ>::CG_gradient(const Vec3D &P, const Mdouble phi)
{
    //CG_type_todo
    if (getCGShape() == Gaussian)
    {
        return Vec3D(0.0, P.Y - Position.Y, P.Z - Position.Z) * (phi / getCGWidthSquared());
    }
    else
    {
        std::cerr << "error in CG_gradient<YZ>" << std::endl;
        exit(-1);
    }
}
template<> Vec3D StatisticsPoint<XZ>::CG_gradient(const Vec3D &P, const Mdouble phi)
{
    //CG_type_todo
    if (getCGShape() == Gaussian)
    {
        return Vec3D(P.X - Position.X, 0.0, P.Z - Position.Z) * (phi / getCGWidthSquared());
    }
    else
    {
        std::cerr << "error in CG_gradient<XZ>" << std::endl;
        exit(-1);
    }
}
template<> Vec3D StatisticsPoint<XY>::CG_gradient(const Vec3D &P, const Mdouble phi)
{
    //CG_type_todo
    if (getCGShape() == Gaussian)
    {
        return Vec3D(P.X - Position.X, P.Y - Position.Y, 0.0) * (phi / getCGWidthSquared());
    }
    else
    {
        std::cerr << "error in CG_gradient<XY>" << std::endl;
        exit(-1);
    }
}

template<> Vec3D StatisticsPoint<X>::CG_gradient(const Vec3D &P, const Mdouble phi)
{
    //CG_type_todo
    if (getCGShape() == Gaussian)
    {
        return Vec3D((P.X - Position.X) * (phi / getCGWidthSquared()), 0.0, 0.0);
    }
    else if (getCGShape() == Polynomial)
    {
        Mdouble r = fabs(Position.X - P.X) / getCGWidth();
        return Vec3D(getCGInverseVolume() * evaluatePolynomialGradient(r) / r / getCGWidthSquared() * (Position.X - P.X), 0, 0);
    }
    else
    {
        std::cerr << "error in CG_gradient<X>" << std::endl;
        exit(-1);
    }
}
template<> Vec3D StatisticsPoint<Y>::CG_gradient(const Vec3D &P, const Mdouble phi)
{
    //CG_type_todo
    if (getCGShape() == Gaussian)
    {
        return Vec3D(0.0, (P.Y - Position.Y) * (phi / getCGWidthSquared()), 0.0);
    }
    else if (getCGShape() == Polynomial)
    {
        Mdouble r = fabs(Position.Y - P.Y) / getCGWidth();
        return Vec3D(0, getCGInverseVolume() * evaluatePolynomialGradient(r) / r / getCGWidthSquared() * (Position.Y - P.Y), 0);
    }
    else
    {
        std::cerr << "error in CG_gradient<Y>" << std::endl;
        exit(-1);
    }
}
template<> Vec3D StatisticsPoint<Z>::CG_gradient(const Vec3D &P, const Mdouble phi)
{
    //CG_type_todo
    if (getCGShape() == Gaussian)
    {
        return Vec3D(0.0, 0.0, (P.Z - Position.Z) * (phi / getCGWidthSquared()));
    }
    else if (getCGShape() == Polynomial)
    {
        Mdouble r = fabs(Position.Z - P.Z) / getCGWidth();
        return Vec3D(0, 0, getCGInverseVolume() * evaluatePolynomialGradient(r) / r / getCGWidthSquared() * (Position.Z - P.Z));
    }
    else
    {
        std::cerr << "error in CG_gradient<Z>" << std::endl;
        exit(-1);
    }
}

template<> Vec3D StatisticsPoint<O>::CG_gradient(const Vec3D&, const Mdouble)
{
    return Vec3D(0.0, 0.0, 0.0);
}

//~ template<> Vec3D StatisticsPoint<Z>::CG_integral_gradient(Vec3D &P1, Vec3D &P2, Vec3D &P1_P2_normal, Mdouble P1_P2_distance) {
//~ Vec3D P_P1 = Position - P1;
//~ Vec3D P_P2 = Position - P2;
//~ Mdouble a = dot(P_P1,P1_P2_normal);
//~ Mdouble b = dot(P_P2,P1_P2_normal);
//~ Vec3D tangential = P_P1 - a * P1_P2_normal;
//~ if (getCGShape()==Gaussian) {
//~ Mdouble wsq2 = sqrt(2*getCGWidthSquared());
//~ Mdouble f = sqrt(2*constants::pi*getCGWidthSquared());
//~ return Vec3D(0.0,0.0,(exp(-mathsFunc::square(a/wsq2))-exp(-mathsFunc::square(b/wsq2)))/f/(P2.Z-P1.Z));
//~ }   else { std::cerr << "error in CG_function" << std::endl; exit(-1); }
//~ }

template<> Mdouble StatisticsPoint<RA>::CG_integral(Vec3D &P1, Vec3D &P2, Vec3D &P1_P2_normal, Mdouble P1_P2_distance, Vec3D& rpsi)
{
    Mdouble rpsi_scalar;
    Mdouble psi = CG_integral_2D(P1, P2, P1_P2_normal, P1_P2_distance, rpsi_scalar);
    rpsi = Position * psi;
    return psi;
}
template<> Mdouble StatisticsPoint<RZ>::CG_integral(Vec3D &P1, Vec3D &P2, Vec3D&, Mdouble, Vec3D& rpsi UNUSED)
{   
    Vec3D P=(P1+P2)/2; return CG_function(P);
}
template<> Mdouble StatisticsPoint<AZ>::CG_integral(Vec3D&, Vec3D&, Vec3D&, Mdouble, Vec3D& rpsi UNUSED)
{   
    std::cerr << "error in CG_function<AZ>" << std::endl; exit(-1);
}
template<> Mdouble StatisticsPoint<R>::CG_integral(Vec3D &P1, Vec3D &P2, Vec3D&, Mdouble, Vec3D& rpsi UNUSED)
{   
    Vec3D P=(P1+P2)/2; return CG_function(P);
}
template<> Mdouble StatisticsPoint<A>::CG_integral(Vec3D&, Vec3D&, Vec3D&, Mdouble, Vec3D& rpsi UNUSED)
{   
    std::cerr << "error in CG_function<A>" << std::endl; exit(-1);
}

template<> Mdouble StatisticsPoint<XY>::CG_integral(Vec3D &P1, Vec3D &P2, Vec3D &P1_P2_normal, Mdouble P1_P2_distance, Vec3D& rpsi)
{
    Mdouble rpsi_scalar = 0;
    Mdouble psi = CG_integral_2D(P1, P2, P1_P2_normal, P1_P2_distance, rpsi_scalar);
    rpsi = Vec3D(Position.X * psi, Position.Y * psi, P1.Z * psi - (P1.Z - P2.Z) * rpsi_scalar);
    return psi;
}
template<> Mdouble StatisticsPoint<XZ>::CG_integral(Vec3D &P1, Vec3D &P2, Vec3D &P1_P2_normal, Mdouble P1_P2_distance, Vec3D& rpsi)
{
    Mdouble rpsi_scalar = 0;
    Mdouble psi = CG_integral_2D(P1, P2, P1_P2_normal, P1_P2_distance, rpsi_scalar);
    rpsi = Vec3D(Position.X * psi, P1.Y * psi - (P1.Y - P2.Y) * rpsi_scalar, Position.Z * psi);
    return psi;
}
template<> Mdouble StatisticsPoint<YZ>::CG_integral(Vec3D &P1, Vec3D &P2, Vec3D &P1_P2_normal, Mdouble P1_P2_distance, Vec3D& rpsi)
{
    Mdouble rpsi_scalar = 0;
    Mdouble psi = CG_integral_2D(P1, P2, P1_P2_normal, P1_P2_distance, rpsi_scalar);
    rpsi = Vec3D(P1.X * psi - (P1.X - P2.X) * rpsi_scalar, Position.Y * psi, Position.Z * psi);
    return psi;
}
template<> Mdouble StatisticsPoint<X>::CG_integral(Vec3D &P1, Vec3D &P2, Vec3D &P1_P2_normal, Mdouble P1_P2_distance, Vec3D& rpsi)
{
    Mdouble rpsi_scalar = 0;
    Mdouble psi = CG_integral_1D(P1, P2, P1_P2_normal, P1_P2_distance, rpsi_scalar);
    rpsi = Vec3D(Position.X * psi, P1.Y * psi - (P1.Y - P2.Y) * rpsi_scalar, P1.Z * psi - (P1.Z - P2.Z) * rpsi_scalar);
    return psi;
}
template<> Mdouble StatisticsPoint<Y>::CG_integral(Vec3D &P1, Vec3D &P2, Vec3D &P1_P2_normal, Mdouble P1_P2_distance, Vec3D& rpsi)
{
    Mdouble rpsi_scalar = 0;
    Mdouble psi = CG_integral_1D(P1, P2, P1_P2_normal, P1_P2_distance, rpsi_scalar);
    rpsi = Vec3D(P1.X * psi - (P1.X - P2.X) * rpsi_scalar, Position.Y * psi, P1.Z * psi - (P1.Z - P2.Z) * rpsi_scalar);
    return psi;
}
template<> Mdouble StatisticsPoint<Z>::CG_integral(Vec3D &P1, Vec3D &P2, Vec3D &P1_P2_normal, Mdouble P1_P2_distance, Vec3D& rpsi)
{
    Mdouble rpsi_scalar = 0;
    Mdouble psi = CG_integral_1D(P1, P2, P1_P2_normal, P1_P2_distance, rpsi_scalar);
    rpsi = Vec3D(P1.X * psi - (P1.X - P2.X) * rpsi_scalar, P1.Y * psi - (P1.Y - P2.Y) * rpsi_scalar, Position.Z * psi);
    return psi;
}
template<> Mdouble StatisticsPoint<O>::CG_integral(Vec3D &P1, Vec3D &P2 UNUSED, Vec3D &P1_P2_normal UNUSED, Mdouble P1_P2_distance UNUSED, Vec3D& rpsi)
{   
    Mdouble psi = getCGInverseVolume();
    rpsi=P1*psi; 
    return psi;
}

template<> Vec3D StatisticsPoint<Z>::CG_integral_gradient(Vec3D &P1, Vec3D &P2, Vec3D &P1_P2_normal, Mdouble P1_P2_distance)
{
    return Vec3D(0, 0, CG_integral_gradient_1D(P1, P2, P1_P2_normal, P1_P2_distance));
}

template<StatType T> std::ostream& operator<<(std::ostream& os, const StatisticsPoint<T> &stat)
{
    os.precision(16);
    if (stat.mirrorParticle < 0)
    {
        //only write std particles, not mirrored particles
        os << stat.getPosition() << " " << stat.write() << std::endl;
    }
    return os;
}

template<> std::ostream& operator<<(std::ostream& os, const StatisticsPoint<RAZ> &stat)
{
    os.precision(16);
    if (stat.mirrorParticle < 0)
        os << stat.getPosition().getCylindricalCoordinates() << " " << stat.write() << std::endl;
    return os;
}
template<> std::ostream& operator<<(std::ostream& os, const StatisticsPoint<RA> &stat)
{
    os.precision(16);
    if (stat.mirrorParticle < 0)
        os << stat.getPosition().getCylindricalCoordinates() << " " << stat.write() << std::endl;
    return os;
}
template<> std::ostream& operator<<(std::ostream& os, const StatisticsPoint<RZ> &stat)
{
    os.precision(16);
    if (stat.mirrorParticle < 0)
        os << stat.getPosition().getCylindricalCoordinates() << " " << stat.write() << std::endl;
    return os;
}
template<> std::ostream& operator<<(std::ostream& os, const StatisticsPoint<AZ> &stat)
{
    os.precision(16);
    if (stat.mirrorParticle < 0)
        os << stat.getPosition().getCylindricalCoordinates() << " " << stat.write() << std::endl;
    return os;
}
template<> std::ostream& operator<<(std::ostream& os, const StatisticsPoint<R> &stat)
{
    os.precision(16);
    if (stat.mirrorParticle < 0)
        os << stat.getPosition().getCylindricalCoordinates() << " " << stat.write() << std::endl;
    return os;
}
template<> std::ostream& operator<<(std::ostream& os, const StatisticsPoint<A> &stat)
{
    os.precision(16);
    if (stat.mirrorParticle < 0)
        os << stat.getPosition().getCylindricalCoordinates() << " " << stat.write() << std::endl;
    return os;
}

template<> Mdouble StatisticsPoint<XY>::getDistanceSquaredNonAveraged(const Vec3D &P)
{
    return mathsFunc::square(P.X - Position.X) + mathsFunc::square(P.Y - Position.Y);
}
template<> Mdouble StatisticsPoint<XZ>::getDistanceSquaredNonAveraged(const Vec3D &P)
{
    return mathsFunc::square(P.X - Position.X) + mathsFunc::square(P.Z - Position.Z);
}
template<> Mdouble StatisticsPoint<YZ>::getDistanceSquaredNonAveraged(const Vec3D &P)
{
    return mathsFunc::square(P.Y - Position.Y) + mathsFunc::square(P.Z - Position.Z);
}
template<> Mdouble StatisticsPoint<X>::getDistanceSquaredNonAveraged(const Vec3D &P)
{
    return mathsFunc::square(P.X - Position.X);
}
template<> Mdouble StatisticsPoint<Y>::getDistanceSquaredNonAveraged(const Vec3D &P)
{
    return mathsFunc::square(P.Y - Position.Y);
}
template<> Mdouble StatisticsPoint<Z>::getDistanceSquaredNonAveraged(const Vec3D &P)
{
    return mathsFunc::square(P.Z - Position.Z);
}
template<> Mdouble StatisticsPoint<O>::getDistanceSquaredNonAveraged(const Vec3D&)
{
    return 0;
}

template<> Mdouble StatisticsPoint<XY>::dotNonAveraged(const Vec3D &P, const Vec3D &Q)
{
    return P.X * Q.X + P.Y * Q.Y;
}
template<> Mdouble StatisticsPoint<XZ>::dotNonAveraged(const Vec3D &P, const Vec3D &Q)
{
    return P.X * Q.X + P.Z * Q.Z;
}
template<> Mdouble StatisticsPoint<YZ>::dotNonAveraged(const Vec3D &P, const Vec3D &Q)
{
    return P.Y * Q.Y + P.Z * Q.Z;
}
template<> Mdouble StatisticsPoint<X>::dotNonAveraged(const Vec3D &P, const Vec3D &Q)
{
    return P.X * Q.X;
}
template<> Mdouble StatisticsPoint<Y>::dotNonAveraged(const Vec3D &P, const Vec3D &Q)
{
    return P.Y * Q.Y;
}
template<> Mdouble StatisticsPoint<Z>::dotNonAveraged(const Vec3D &P, const Vec3D &Q)
{
    return P.Z * Q.Z;
}
template<> Mdouble StatisticsPoint<O>::dotNonAveraged(const Vec3D&, const Vec3D&)
{
    return 0;
}

template<StatType T> Vec3D StatisticsPoint<T>::clearAveragedDirections(Vec3D P)
{
    return P;
}
template<> Vec3D StatisticsPoint<XY>::clearAveragedDirections(Vec3D P)
{
    return Vec3D(P.X, P.Y, 0);
}
template<> Vec3D StatisticsPoint<XZ>::clearAveragedDirections(Vec3D P)
{
    return Vec3D(P.X, 0, P.Z);
}
template<> Vec3D StatisticsPoint<YZ>::clearAveragedDirections(Vec3D P)
{
    return Vec3D(0, P.Y, P.Z);
}
template<> Vec3D StatisticsPoint<X>::clearAveragedDirections(Vec3D P)
{
    return Vec3D(P.X, 0, 0);
}
template<> Vec3D StatisticsPoint<Y>::clearAveragedDirections(Vec3D P)
{
    return Vec3D(0, P.Y, 0);
}
template<> Vec3D StatisticsPoint<Z>::clearAveragedDirections(Vec3D P)
{
    return Vec3D(0, 0, P.Z);
}
template<> Vec3D StatisticsPoint<O>::clearAveragedDirections(Vec3D P UNUSED)
{   
    return Vec3D(0, 0, 0);
}

template<> Vec3D StatisticsPoint<XY>::crossNonAveraged(Vec3D P, Vec3D &Q)
{
    return Vec3D(0, 0, P.X * Q.Y - P.Y * Q.X);
}
template<> Vec3D StatisticsPoint<XZ>::crossNonAveraged(Vec3D P, Vec3D &Q)
{
    return Vec3D(0, P.Z * Q.X - P.X * Q.Z, 0);
}
template<> Vec3D StatisticsPoint<YZ>::crossNonAveraged(Vec3D P, Vec3D &Q)
{
    return Vec3D(P.Y * Q.Z - P.Z * Q.Y, 0, 0);
}
template<> Vec3D StatisticsPoint<X>::crossNonAveraged(Vec3D P UNUSED, Vec3D &Q UNUSED)
{   return Vec3D(0,0,0);}
template<> Vec3D StatisticsPoint<Y>::crossNonAveraged(Vec3D P UNUSED, Vec3D &Q UNUSED)
{   return Vec3D(0,0,0);}
template<> Vec3D StatisticsPoint<Z>::crossNonAveraged(Vec3D P UNUSED, Vec3D &Q UNUSED)
{   return Vec3D(0,0,0);}
template<> Vec3D StatisticsPoint<O>::crossNonAveraged(Vec3D P UNUSED, Vec3D &Q UNUSED)
{   return Vec3D(0,0,0);}

template<> Matrix3D StatisticsPoint<XY>::matrixCrossNonAveraged(Vec3D P, Matrix3D &Q)
{
    return Matrix3D(
            0, 0, P.X * Q.YX - P.Y * Q.XX,
            0, 0, P.X * Q.YY - P.Y * Q.XY,
            0, 0, P.X * Q.YZ - P.Y * Q.XZ);
}
template<> Matrix3D StatisticsPoint<XZ>::matrixCrossNonAveraged(Vec3D P, Matrix3D &Q)
{
    return Matrix3D(
            0, P.Z * Q.XX - P.X * Q.ZX, 0,
            0, P.Z * Q.XY - P.X * Q.ZY, 0,
            0, P.Z * Q.XZ - P.X * Q.ZZ, 0);
}
template<> Matrix3D StatisticsPoint<YZ>::matrixCrossNonAveraged(Vec3D P, Matrix3D &Q)
{
    return Matrix3D(
            P.Y * Q.ZX - P.Z * Q.YX, 0, 0,
            P.Y * Q.ZY - P.Z * Q.YY, 0, 0,
            P.Y * Q.ZZ - P.Z * Q.YZ, 0, 0);
}
template<> Matrix3D StatisticsPoint<X>::matrixCrossNonAveraged(Vec3D P UNUSED, Matrix3D &Q UNUSED)
{
    return Matrix3D(0,0,0,0,0,0,0,0,0);
}
template<> Matrix3D StatisticsPoint<Y>::matrixCrossNonAveraged(Vec3D P UNUSED, Matrix3D &Q UNUSED)
{
    return Matrix3D(0,0,0,0,0,0,0,0,0);
}
template<> Matrix3D StatisticsPoint<Z>::matrixCrossNonAveraged(Vec3D P UNUSED, Matrix3D &Q UNUSED)
{
    return Matrix3D(0,0,0,0,0,0,0,0,0);
}
template<> Matrix3D StatisticsPoint<O>::matrixCrossNonAveraged(Vec3D P UNUSED, Matrix3D &Q UNUSED)
{
    return Matrix3D(0,0,0,0,0,0,0,0,0);
}