gctl/lib/maths/mathfunc_t.h

/********************************************************
 *  ██████╗  ██████╗████████╗██╗
 * ██╔════╝ ██╔════╝╚══██╔══╝██║
 * ██║  ███╗██║        ██║   ██║
 * ██║   ██║██║        ██║   ██║
 * ╚██████╔╝╚██████╗   ██║   ███████╗
 *  ╚═════╝  ╚═════╝   ╚═╝   ╚══════╝
 * Geophysical Computational Tools & Library (GCTL)
 *
 * Copyright (c) 2023  Yi Zhang (yizhang-geo@zju.edu.cn)
 *
 * GCTL is distributed under a dual licensing scheme. You can redistribute 
 * it and/or modify it under the terms of the GNU Lesser General Public 
 * License as published by the Free Software Foundation, either version 2 
 * of the License, or (at your option) any later version. You should have 
 * received a copy of the GNU Lesser General Public License along with this 
 * program. If not, see <http://www.gnu.org/licenses/>.
 * 
 * If the terms and conditions of the LGPL v.2. would prevent you from using 
 * the GCTL, please consider the option to obtain a commercial license for a 
 * fee. These licenses are offered by the GCTL's original author. As a rule, 
 * licenses are provided "as-is", unlimited in time for a one time fee. Please 
 * send corresponding requests to: yizhang-geo@zju.edu.cn. Please do not forget 
 * to include some description of your company and the realm of its activities. 
 * Also add information on how to contact you by electronic and paper mail.
 ******************************************************/

#ifndef _GCTL_MATHFUNC_TEMPLATE_H
#define _GCTL_MATHFUNC_TEMPLATE_H

#include "../core/enum.h"
#include "../core/array.h"
#include "../core/matrix.h"
#include "../core/exceptions.h"
#include "cmath"

namespace gctl
{
    template <typename T>
    inline int sign(T d)
    {
        return (T(0) < d) - (d < T(0));
    }

    template <typename T>
    inline T arc(T deg)
    {
        return deg*GCTL_Pi/180.0;
    }

    template <typename T>
    inline T deg(T arc)
    {
        return arc*180.0/GCTL_Pi;
    }

    template <typename T>
    inline T sind(T deg)
    {
        return sin(deg*GCTL_Pi/180.0);
    }

    template <typename T>
    inline T cosd(T deg)
    {
        return cos(deg*GCTL_Pi/180.0);
    }

    template <typename T>
    inline T tand(T deg)
    {
        return tan(deg*GCTL_Pi/180.0);
    }

	template <typename T>
	inline T power2(T in)
	{
		return (in)*(in);
	}

	template <typename T>
	inline T power3(T in)
	{
		return (in)*(in)*(in);
	}

	template <typename T>
	inline T power4(T in)
	{
		return (in)*(in)*(in)*(in);
	}

	template <typename T>
	inline T power5(T in)
	{
		return (in)*(in)*(in)*(in)*(in);
	}

    template <typename T>
    inline T jacoby2(T x00, T x01, T x10, T x11)
    {
        return x00*x11-x01*x10;
    }

    template <typename T>
    inline T jacoby3(T x00, T x01, T x02, T x10, T x11, 
        T x12, T x20, T x21, T x22)
    {
        return x00*x11*x22+x01*x12*x20+x10*x21*x02
            -x02*x11*x20-x01*x10*x22-x00*x12*x21;
    }

    /**
     * @brief Calculate the inverse matrix of a 3x3 matrix
     * 
     * @tparam T Type name
     * @param A  Pointer of the input matrix, must be stored in an array of the nine coefficients in a row-major fashion
     * @param invA  Pointer of the output matrix, must be stored in an array of the nine coefficients in a row-major fashion
     * @return true Success
     * @return false Fail
     */
    template <typename T>
    bool inverse3x3(T *A, T *invA)
    {
        T det = jacoby3(A[0], A[1], A[2], A[3], A[4], A[5], A[6], A[7], A[8]);
        if (det <= GCTL_ZERO && det >= -1*GCTL_ZERO) return false;

        invA[0] = jacoby2(A[4], A[7], A[5], A[8])/det;
        invA[1] = -1.0*jacoby2(A[1], A[7], A[2], A[8])/det;
        invA[2] = jacoby2(A[1], A[4], A[2], A[5])/det;
        invA[3] = -1.0*jacoby2(A[3], A[6], A[5], A[8])/det;
        invA[4] = jacoby2(A[0], A[6], A[2], A[8])/det;
        invA[5] = -1.0*jacoby2(A[0], A[3], A[2], A[5])/det;
        invA[6] = jacoby2(A[3], A[6], A[4], A[7])/det;
        invA[7] = -1.0*jacoby2(A[0], A[6], A[1], A[7])/det;
        invA[8] = jacoby2(A[0], A[3], A[1], A[4])/det;
        return true;
    }

    template <typename T>
    T arctg(T v)
    {
        T ang;
        if(v>=0) ang=atan(v);
        else if(v<0) ang=atan(v)+GCTL_Pi;
        return ang; 
    }

    template <typename T>
    T arctg2(T v, T f)
    {
        T ang;
        if(f>=0)
        {
            if(atan(v)>0) ang=atan(v);
            else ang=atan(v) + GCTL_Pi;
        }
        else if(f<0)
        {
            if(atan(v)<0) ang=atan(v);
            else ang=atan(v) - GCTL_Pi;
        }
        return ang;
    }
}

#endif //_GCTL_MATHFUNC_TEMPLATE_H

/*
    template <typename T>
    void matrix_vector(const matrix<T> &mat, const array<T> &vec, array<T> &ret, T zero = 0, matrix_layout_e layout = NoTrans)
    {
        static_assert(std::is_arithmetic<T>::value, "gctl::matrix_vector(...) could only be used with an arithmetic type.");

        if (mat.empty() || vec.empty())
        {
            throw runtime_error("Empty matrix or vector. From matrix_vector(...)");
        }

        if ((layout == NoTrans && mat.col_size() != vec.size()) || (layout == Trans && mat.row_size() != vec.size()))
        {
            throw runtime_error("Incompatible sizes of the matrix and the vector. From matrix_vector(...)");
        }

        if (layout == Trans)
        {
            ret.resize(mat.col_size(), zero);
            for (int j = 0; j < mat.col_size(); ++j)
            {
                for (int i = 0; i < mat.row_size(); ++i)
                {
                    ret[j] = ret[j] + mat[i][j]*vec[i];
                }
            }
            return;
        }

        ret.resize(mat.row_size(), zero);
        for (int i = 0; i < mat.row_size(); ++i)
        {
            for (int j = 0; j < mat.col_size(); ++j)
            {
                ret[i] = ret[i] + mat[i][j]*vec[j];
            }
        }
        return;
    }

    template <typename T>
    void matrix_matrix(const matrix<T> &mat, const matrix<T> &mat2, matrix<T> &ret, T zero = 0)
    {
        static_assert(std::is_arithmetic<T>::value, "gctl::matrix_matrix(...) could only be used with an arithmetic type.");

        if (mat.empty() || mat2.empty())
        {
            throw runtime_error("Empty matrix(s). From matrix_matrix(...)");
        }

        if (mat.col_size() != mat2.row_size())
        {
            throw runtime_error("Incompatible matrix sizes. From matrix_vector(...)");
        }

        ret.resize(mat.row_size(), mat2.col_size(), zero);
        for (int i = 0; i < mat.row_size(); ++i)
        {
            for (int j = 0; j < mat2.col_size(); ++j)
            {
                for (int k = 0; k < mat.col_size(); ++k)
                {
                    ret[i][j] = ret[i][j] + mat[i][k]*mat2[k][j];
                }
            }
        }
        return;
    }

    template <typename T>
    void linespace(const T &start, const T &end, unsigned int size, std::vector<T> &out_vec)
    {
        if (size < 1)
            throw invalid_argument("Invalid vector size. From linespace(...)");

        out_vec.resize(size);

        if (size == 1)
        {
            out_vec[0] = 0.5*(start + end);
            return;
        }

        T space = 1.0/(size-1)*(end - start);
        for (int i = 0; i < size; i++)
        {
            out_vec[i] = start + i*space;
        }
        return;
    }

    template <typename T>
    void gridspace(const T &xs, const T &xe, const T &ys, const T &ye, unsigned int xn, 
        unsigned int yn, std::vector<T> &out_vec)
    {
        if (xn < 1 || yn < 1)
            throw invalid_argument("Invalid grid size. From gridspace(...)");

        std::vector<T> out_x, out_y;
        linespace(xs, xe, xn, out_x);
        linespace(ys, ye, yn, out_y);

        out_vec.resize(xn*yn);
        for (int i = 0; i < yn; ++i)
        {
            for (int j = 0; j < xn; ++j)
            {
                out_vec[j+i*xn] = out_x[j] + out_y[i];
            }
        }
        return;
    }

    template <typename T>
    void meshspace(const T &xs, const T &xe, const T &ys, const T &ye, const T &zs, const T &ze, 
        unsigned int xn, unsigned int yn, unsigned int zn, std::vector<T> &out_vec)
    {
        if (xn < 1 || yn < 1 || zn < 1)
            throw invalid_argument("Invalid grid size. From meshspace(...)");

        array<T> out_x, out_y, out_z;
        linespace(xs, xe, xn, out_x);
        linespace(ys, ye, yn, out_y);
        linespace(zs, ze, zn, out_z);

        out_vec.resize(xn*yn*zn);
        for (int i = 0; i < zn; ++i)
        {
            for (int j = 0; j < yn; ++j)
            {
                for (int k = 0; k < xn; ++k)
                {
                    out_vec[k+j*xn+i*xn*yn] = out_x[k] + out_y[j] + out_z[i];
                }
            }
        }
        return;
    }

    template <typename T>
    T average(T *val_ptr, int size, const T &zero = 0)
    {
        if (val_ptr == nullptr)
            throw domain_error("Invalid pointer. From average(...)");

        if (size <= 0)
            throw invalid_argument("Invalid object size. From average(...)");

        T mn = zero;
        for (int i = 0; i < size; i++)
        {
            mn = mn + val_ptr[i];
        }

        return 1.0/size*mn;
    }

    template <typename T>
    T average(const std::vector<T> &val_arr, const T &zero = 0)
    {
        if (val_arr.empty())
            throw domain_error("Invalid object size. From average(...)");

        int size = val_arr.size();

        T mn = zero;
        for (int i = 0; i < size; i++)
        {
            mn = mn + val_arr[i];
        }

        return 1.0/size*mn;
    }

    template <typename T>
    T deviation(T *val_ptr, int size, const T &zero = 0, bool STD = false)
    {
        T mn = average(val_ptr, size);

        T deviation = zero;
        for (int i = 0; i < size; i++)
        {
            deviation = deviation + (val_ptr[i] - mn)*(val_ptr[i] - mn);
        }
        deviation = 1.0/size*deviation;

        if (STD) return sqrt(deviation);
        else return deviation;
    }

    template <typename T>
    T deviation(const std::vector<T> &val_arr, const T &zero = 0, bool STD = false)
    {
        T mn = average(val_arr);
        int size = val_arr.size();

        T deviation = zero;
        for (int i = 0; i < size; i++)
        {
            deviation = deviation + (val_arr[i] - mn)*(val_arr[i] - mn);
        }
        deviation = 1.0/size*deviation;

        if (STD) return sqrt(deviation);
        else return deviation;
    }

    template <typename T>
    T root_mn_square(T *val_ptr, int size, const T &zero = 0)
    {
        if (val_ptr == nullptr)
            throw domain_error("Invalid pointer. From root_mn_square(...)");

        if (size <= 0)
            throw invalid_argument("Invalid object size. From root_mn_square(...)");

        T mn = zero;
        for (int i = 0; i < size; i++)
        {
            mn = mn + val_ptr[i]*val_ptr[i];
        }
        return sqrt(1.0/size*mn);
    }

    template <typename T>
    T root_mn_square(const std::vector<T> &val_arr, const T &zero = 0)
    {
        if (val_arr.empty())
            throw domain_error("Invalid object size. From root_mn_square(...)");

        int size = val_arr.size();

        T mn = zero;
        for (int i = 0; i < size; i++)
        {
            mn = mn + val_arr[i]*val_arr[i];
        }
        return sqrt(1.0/size*mn);
    }

    template <typename T>
    void random(T p1, T p2, T *val_ptr, int size, random_type_e mode = RdNormal)
    {
        if (val_ptr == nullptr)
            throw domain_error("Invalid pointer. From random(...)");

        if (size <= 0)
            throw invalid_argument("Invalid size. From random(...)");

        unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();

        std::default_random_engine generator(seed);
        if (mode == RdNormal)
        {
            //添加高斯分布的随机值
            std::normal_distribution<T> dist(p1, p2);
            for (int i = 0; i < size; i++)
            {
                val_ptr[i] = dist(generator);
            }
            return;
        }

        //添加均匀分布的随机值
        std::uniform_real_distribution<T> dist(p1, p2);
        for (int i = 0; i < size; i++)
        {
            val_ptr[i] = dist(generator);
        }
        return;
    }

    template <typename T>
    void random(T p1, T p2, std::vector<T> &val_vec, random_type_e mode = RdNormal)
    {
        size_t size = val_vec.size();

        if (size <= 0)
            throw invalid_argument("Invalid size. From random(...)");

        unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();

        std::default_random_engine generator(seed);
        if (mode == RdNormal)
        {
            //添加高斯分布的随机值
            std::normal_distribution<T> dist(p1, p2);
            for (int i = 0; i < size; i++)
            {
                val_vec[i] = dist(generator);
            }
            return;
        }

        //添加均匀分布的随机值
        std::uniform_real_distribution<T> dist(p1, p2);
        for (int i = 0; i < size; i++)
        {
            val_vec[i] = dist(generator);
        }
        return;
    }

    template <typename T>
    T normalize(array<T> &in_arr, T eps = 1e-8)
    {
        T norm_val = norm(in_arr, L2);
        if (norm_val < eps)
            return 0.0;

        for (int i = 0; i < in_arr.size(); i++)
        {
            in_arr[i] /= norm_val;
        }
        return norm_val;
    }
*/