485 lines
14 KiB
C++
485 lines
14 KiB
C++
/********************************************************
|
|
* ██████╗ ██████╗████████╗██╗
|
|
* ██╔════╝ ██╔════╝╚══██╔══╝██║
|
|
* ██║ ███╗██║ ██║ ██║
|
|
* ██║ ██║██║ ██║ ██║
|
|
* ╚██████╔╝╚██████╗ ██║ ███████╗
|
|
* ╚═════╝ ╚═════╝ ╚═╝ ╚══════╝
|
|
* 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;
|
|
}
|
|
*/ |