501 lines
12 KiB
C++
501 lines
12 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.
|
||
******************************************************/
|
||
|
||
#include "mathfunc.h"
|
||
|
||
bool gctl::isequal(double f, double v, double eps)
|
||
{
|
||
return fabs(f - v) < eps;
|
||
}
|
||
|
||
double gctl::sign(double a)
|
||
{
|
||
if (a > GCTL_ZERO) return 1.0;
|
||
if (a < -1.0*GCTL_ZERO) return -1.0;
|
||
return 0.0;
|
||
}
|
||
|
||
//利用二分法求一个正数的n次方根 注意输入值小于1的情况
|
||
double gctl::sqrtn(double d,int n,double eps)
|
||
{
|
||
double xmin,xmax,halfx;
|
||
if (d == 1)
|
||
{
|
||
return d;
|
||
}
|
||
else if (d > 1)
|
||
{
|
||
xmin = 1;
|
||
xmax = d;
|
||
halfx = 0.5*(xmin+xmax);
|
||
while (fabs(d - pow(halfx,n)) > eps)
|
||
{
|
||
if (pow(halfx,n) > d)
|
||
{
|
||
xmax = halfx;
|
||
halfx = 0.5*(xmin+xmax);
|
||
}
|
||
else
|
||
{
|
||
xmin = halfx;
|
||
halfx = 0.5*(xmin+xmax);
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
xmin = 0;
|
||
xmax = 1;
|
||
halfx = 0.5*(xmin+xmax);
|
||
while (fabs(d - pow(halfx,n)) > eps)
|
||
{
|
||
if (pow(halfx,n) > d)
|
||
{
|
||
xmax = halfx;
|
||
halfx = 0.5*(xmin+xmax);
|
||
}
|
||
else
|
||
{
|
||
xmin = halfx;
|
||
halfx = 0.5*(xmin+xmax);
|
||
}
|
||
}
|
||
}
|
||
return halfx;
|
||
}
|
||
|
||
/**
|
||
* @brief 计算一个椭圆在不同位置的半径
|
||
*
|
||
* @param[in] x_len x方向半径
|
||
* @param[in] y_len y方向半径
|
||
* @param[in] x_arc 椭圆绕原点逆时针旋转的角度 （弧度）
|
||
* @param[in] arc 计算方向与x轴正方向的夹角（弧度） 逆时针为正
|
||
*
|
||
* @return 半径值
|
||
*/
|
||
double gctl::ellipse_radius_2d(double x_len, double y_len, double arc, double x_arc)
|
||
{
|
||
if (fabs(x_len - y_len) < 1e-8) // 就是个圆 直接加
|
||
{
|
||
return 0.5*(x_len + y_len);
|
||
}
|
||
|
||
return sqrt(pow(x_len*cos(arc-x_arc),2) + pow(y_len*sin(arc-x_arc),2));
|
||
}
|
||
|
||
/**
|
||
* @brief 计算已椭圆为基准面的高程数据的绝对坐标位置。
|
||
*
|
||
* 假设高程数据的测量方向（如大地水准面）为椭圆切线的垂直方向（与切点与坐标原点的连线方向不一致）
|
||
* 则高程点的绝对空间位置的维度值与切点的维度值也不一致。此函数可以计算校正后的维度值与球心半径。
|
||
*
|
||
* @param x_len 椭圆的x方向半径（一般为长轴）
|
||
* @param y_len 椭圆的y方向半径（一般为短轴）
|
||
* @param arc 计算方向与x轴正方向的夹角（弧度） 逆时针为正（等于纬度）
|
||
* @param elev 切点的高程值
|
||
* @param out_arc 校正后的维度值
|
||
* @param out_rad 校正后高程点的球心半径
|
||
* @param x_arc x轴正方向绕原点逆时针旋转的角度 （弧度），默认为0
|
||
*/
|
||
void gctl::ellipse_plus_elevation_2d(double x_len, double y_len, double arc, double elev,
|
||
double &out_arc, double &out_rad, double x_arc)
|
||
{
|
||
if (fabs(x_len - y_len) < 1e-8) // 就是个圆 直接加
|
||
{
|
||
out_arc = arc;
|
||
out_rad = 0.5*(x_len + y_len) + elev;
|
||
return;
|
||
}
|
||
|
||
if (fabs(arc) < 1e-8) // 弧度太小 直接加和
|
||
{
|
||
out_arc = arc;
|
||
out_rad = x_len + elev;
|
||
return;
|
||
}
|
||
|
||
if (fabs(fabs(arc) - 0.5*GCTL_Pi) < 1e-8) // 到了弧顶 直接加和
|
||
{
|
||
out_arc = arc;
|
||
out_rad = y_len + elev;
|
||
return;
|
||
}
|
||
|
||
double ellip_rad = ellipse_radius_2d(x_len, y_len, arc, x_arc);
|
||
double alpha = atan(x_len*x_len*sin(arc)/(y_len*y_len*cos(arc)));
|
||
double sin_alpha = sin(alpha);
|
||
double lx = ellip_rad * sin(alpha - arc)/sin_alpha;
|
||
double lr = ellip_rad * sin(arc)/sin_alpha + elev;
|
||
out_rad = sqrt(lx*lx + lr*lr - 2.0*lx*lr*cos(GCTL_Pi - alpha));
|
||
out_arc = acos((lx*lx + out_rad*out_rad - lr*lr)/(2.0*out_rad*lx));
|
||
if (arc < 0.0) out_arc *= -1.0;
|
||
|
||
return;
|
||
}
|
||
|
||
/**
|
||
* @brief 椭球或者球在不同球面位置的半径
|
||
*
|
||
* @param[in] x_len x方向半径
|
||
* @param[in] y_len y方向半径
|
||
* @param[in] z_len z方向半径
|
||
* @param[in] phi 计算方向与x轴正方向的夹角（弧度） 逆时针为正
|
||
* @param[in] theta 计算方向与z轴正方向的夹角（弧度） 逆时针为正
|
||
*
|
||
* @return 半径值
|
||
*/
|
||
double gctl::ellipsoid_radius(double x_len, double y_len, double z_len, double phi, double theta)
|
||
{
|
||
return x_len*y_len*z_len/sqrt(pow(y_len*z_len*sin(theta)*cos(phi),2) +
|
||
pow(x_len*z_len*sin(theta)*sin(phi),2) + pow(x_len*y_len*cos(theta),2));
|
||
}
|
||
|
||
// 使用牛顿迭代法计算一个矩阵的近似逆
|
||
double gctl::newton_inverse(const _2d_matrix &in_mat, _2d_matrix &inverse_mat,
|
||
double epsilon, int iter_times, bool initiate)
|
||
{
|
||
if (in_mat.empty())
|
||
throw runtime_error("The input matrix is empty. Thrown by gctl::newton_inverse(...)");
|
||
|
||
if (in_mat.row_size() != in_mat.col_size())
|
||
throw logic_error("The input matrix is square. Thrown by gctl::newton_inverse(...)");
|
||
|
||
if (iter_times < 0)
|
||
throw invalid_argument("Invalid iteration times. Thrown by gctl::newton_inverse(...)");
|
||
|
||
int m_size = in_mat.row_size();
|
||
|
||
if (initiate)
|
||
{
|
||
// 初始化逆矩阵 使用输入矩阵的对角元素构建初始矩阵
|
||
if (!inverse_mat.empty()) inverse_mat.clear();
|
||
inverse_mat.resize(m_size, m_size, 0.0);
|
||
for (int i = 0; i < m_size; i++)
|
||
{
|
||
inverse_mat[i][i] = 1.0/in_mat[i][i];
|
||
}
|
||
}
|
||
|
||
if (inverse_mat.row_size() != m_size || inverse_mat.col_size() != m_size)
|
||
throw logic_error("Invalid output matrix size. From gctl::newton_inverse(...)");
|
||
|
||
// 迭代的收敛条件 || E - in_mat*inverse_mat ||_inf < 1
|
||
// 计算矩阵的无穷大范数
|
||
double maxi_mod = 0.0, mod, ele;
|
||
for (int i = 0; i < m_size; i++)
|
||
{
|
||
mod = 0.0;
|
||
for (int j = 0; j < m_size; j++)
|
||
{
|
||
ele = 0.0;
|
||
for (int k = 0; k < m_size; k++)
|
||
{
|
||
ele += in_mat[i][k] * inverse_mat[k][j];
|
||
}
|
||
|
||
if (i == j)
|
||
{
|
||
mod += GCTL_FABS(1.0 - ele);
|
||
}
|
||
else
|
||
{
|
||
mod += GCTL_FABS(ele);
|
||
}
|
||
}
|
||
|
||
maxi_mod = GCTL_MAX(maxi_mod, mod);
|
||
}
|
||
|
||
if (maxi_mod >= 1.0)
|
||
{
|
||
GCTL_ShowWhatError("The iteration may not converge. From gctl::newton_inverse(...)",
|
||
GCTL_WARNING_ERROR, 0, 0, 0);
|
||
}
|
||
|
||
array<double> col_ax(m_size, 0.0); // A*X 的列向量
|
||
_2d_matrix tmp_mat(m_size, m_size, 0.0);
|
||
for (int t = 0; t < iter_times; t++)
|
||
{
|
||
if (maxi_mod <= epsilon)
|
||
break;
|
||
|
||
for (int i = 0; i < m_size; i++)
|
||
{
|
||
for (int j = 0; j < m_size; j++)
|
||
{
|
||
tmp_mat[i][j] = inverse_mat[i][j];
|
||
}
|
||
}
|
||
|
||
for (int n = 0; n < m_size; n++)
|
||
{
|
||
for (int i = 0; i < m_size; i++)
|
||
{
|
||
for (int j = 0; j < m_size; j++)
|
||
{
|
||
col_ax[j] = 0.0;
|
||
for (int k = 0; k < m_size; k++)
|
||
{
|
||
col_ax[j] += in_mat[j][k] * tmp_mat[k][i];
|
||
}
|
||
col_ax[j] *= -1.0;
|
||
}
|
||
col_ax[i] += 2.0;
|
||
|
||
ele = 0.0;
|
||
for (int j = 0; j < m_size; j++)
|
||
{
|
||
ele += tmp_mat[n][j] * col_ax[j];
|
||
}
|
||
|
||
inverse_mat[n][i] = ele;
|
||
}
|
||
}
|
||
|
||
maxi_mod = 0.0;
|
||
for (int i = 0; i < m_size; i++)
|
||
{
|
||
mod = 0.0;
|
||
for (int j = 0; j < m_size; j++)
|
||
{
|
||
ele = 0.0;
|
||
for (int k = 0; k < m_size; k++)
|
||
{
|
||
ele += in_mat[i][k] * inverse_mat[k][j];
|
||
}
|
||
|
||
if (i == j)
|
||
{
|
||
mod += GCTL_FABS(1.0 - ele);
|
||
}
|
||
else
|
||
{
|
||
mod += GCTL_FABS(ele);
|
||
}
|
||
}
|
||
|
||
maxi_mod = GCTL_MAX(maxi_mod, mod);
|
||
}
|
||
}
|
||
|
||
return maxi_mod;
|
||
}
|
||
|
||
// 使用牛顿迭代法计算一个矩阵的近似逆
|
||
double gctl::newton_inverse(const spmat<double> &in_mat, _2d_matrix &inverse_mat,
|
||
double epsilon, int iter_times, bool initiate)
|
||
{
|
||
if (in_mat.empty())
|
||
throw runtime_error("The input matrix is empty. Thrown by gctl::newton_inverse(...)");
|
||
|
||
if (in_mat.row_size() != in_mat.col_size())
|
||
throw logic_error("The input matrix is square. Thrown by gctl::newton_inverse(...)");
|
||
|
||
if (iter_times < 0)
|
||
throw invalid_argument("Invalid iteration times. Thrown by gctl::newton_inverse(...)");
|
||
|
||
int m_size = in_mat.row_size();
|
||
|
||
if (initiate)
|
||
{
|
||
// 初始化逆矩阵 使用输入矩阵的对角元素构建初始矩阵
|
||
if (!inverse_mat.empty()) inverse_mat.clear();
|
||
inverse_mat.resize(m_size, m_size, 0.0);
|
||
for (int i = 0; i < m_size; i++)
|
||
{
|
||
inverse_mat[i][i] = 1.0/in_mat.at(i, i);
|
||
}
|
||
}
|
||
|
||
if (inverse_mat.row_size() != m_size || inverse_mat.col_size() != m_size)
|
||
throw logic_error("Invalid output matrix size. From gctl::newton_inverse(...)");
|
||
|
||
// 迭代的收敛条件 || E - in_mat*inverse_mat ||_inf < 1
|
||
// 计算矩阵的无穷大范数
|
||
double maxi_mod = 0.0, mod, ele;
|
||
array<double> tmp_arr(m_size, 0.0); // A*X 的列向量
|
||
for (int i = 0; i < m_size; i++)
|
||
{
|
||
mod = 0.0;
|
||
for (int j = 0; j < m_size; j++)
|
||
{
|
||
for (int k = 0; k < m_size; k++)
|
||
{
|
||
tmp_arr[k] = inverse_mat[k][j];
|
||
}
|
||
|
||
ele = in_mat.multiply_vector(tmp_arr, i);
|
||
|
||
if (i == j)
|
||
{
|
||
mod += GCTL_FABS(1.0 - ele);
|
||
}
|
||
else
|
||
{
|
||
mod += GCTL_FABS(ele);
|
||
}
|
||
}
|
||
|
||
maxi_mod = GCTL_MAX(maxi_mod, mod);
|
||
}
|
||
|
||
if (maxi_mod >= 1.0)
|
||
{
|
||
GCTL_ShowWhatError("The iteration may not converge. From gctl::newton_inverse(...)",
|
||
GCTL_WARNING_ERROR, 0, 0, 0);
|
||
}
|
||
|
||
array<double> col_ax(m_size, 0.0);
|
||
_2d_matrix tmp_mat(m_size, m_size, 0.0);
|
||
for (int t = 0; t < iter_times; t++)
|
||
{
|
||
if (maxi_mod <= epsilon)
|
||
break;
|
||
//else std::cout << "epsilon = " << maxi_mod << std::endl;
|
||
|
||
for (int i = 0; i < m_size; i++)
|
||
{
|
||
for (int j = 0; j < m_size; j++)
|
||
{
|
||
tmp_mat[i][j] = inverse_mat[i][j];
|
||
}
|
||
}
|
||
|
||
for (int n = 0; n < m_size; n++)
|
||
{
|
||
for (int i = 0; i < m_size; i++)
|
||
{
|
||
for (int j = 0; j < m_size; j++)
|
||
{
|
||
for (int k = 0; k < m_size; k++)
|
||
{
|
||
tmp_arr[k] = tmp_mat[k][i];
|
||
}
|
||
col_ax[j] = in_mat.multiply_vector(tmp_arr, j);
|
||
col_ax[j] *= -1.0;
|
||
}
|
||
col_ax[i] += 2.0;
|
||
|
||
ele = 0.0;
|
||
for (int j = 0; j < m_size; j++)
|
||
{
|
||
ele += tmp_mat[n][j] * col_ax[j];
|
||
}
|
||
|
||
inverse_mat[n][i] = ele;
|
||
}
|
||
}
|
||
|
||
maxi_mod = 0.0;
|
||
for (int i = 0; i < m_size; i++)
|
||
{
|
||
mod = 0.0;
|
||
for (int j = 0; j < m_size; j++)
|
||
{
|
||
for (int k = 0; k < m_size; k++)
|
||
{
|
||
tmp_arr[k] = inverse_mat[k][j];
|
||
}
|
||
|
||
ele = in_mat.multiply_vector(tmp_arr, i);
|
||
|
||
if (i == j)
|
||
{
|
||
mod += GCTL_FABS(1.0 - ele);
|
||
}
|
||
else
|
||
{
|
||
mod += GCTL_FABS(ele);
|
||
}
|
||
}
|
||
|
||
maxi_mod = GCTL_MAX(maxi_mod, mod);
|
||
}
|
||
}
|
||
|
||
return maxi_mod;
|
||
}
|
||
|
||
void gctl::schmidt_orthogonal(const array<double> &a, array<double> &e, int a_s)
|
||
{
|
||
if (a.empty())
|
||
throw runtime_error("The input array is empty. Thrown by gctl::schmidt_orthogonal(...)");
|
||
|
||
if (a_s <= 1) // a_s >= 2
|
||
throw invalid_argument("vector size must be bigger than one. Thrown by gctl::schmidt_orthogonal(...)");
|
||
|
||
int t_s = a.size();
|
||
if (t_s%a_s != 0 || t_s <= 3) // t_s >= 4
|
||
throw invalid_argument("incompatible total array size. Thrown by gctl::schmidt_orthogonal(...)");
|
||
|
||
int len = t_s/a_s;
|
||
if (len < a_s)
|
||
throw invalid_argument("the vectors are over-determined. Thrown by gctl::schmidt_orthogonal(...)");
|
||
|
||
e.resize(t_s, 0.0);
|
||
|
||
double ae, ee;
|
||
for (int i = 0; i < a_s; i++)
|
||
{
|
||
for (int l = 0; l < len; l++)
|
||
{
|
||
e[l + i*len] = a[l + i*len];
|
||
}
|
||
|
||
for (int m = 0; m < i; m++)
|
||
{
|
||
ae = ee = 0.0;
|
||
for (int n = 0; n < len; n++)
|
||
{
|
||
ae += a[n + i*len] * e[n + m*len];
|
||
ee += e[n + m*len] * e[n + m*len];
|
||
}
|
||
|
||
for (int n = 0; n < len; n++)
|
||
{
|
||
e[n + i*len] -= e[n + m*len] * ae/ee;
|
||
}
|
||
}
|
||
}
|
||
|
||
for (int i = 0; i < a_s; i++)
|
||
{
|
||
ee = 0.0;
|
||
for (int l = 0; l < len; l++)
|
||
{
|
||
ee += e[l + i*len] * e[l + i*len];
|
||
}
|
||
ee = sqrt(ee);
|
||
|
||
for (int l = 0; l < len; l++)
|
||
{
|
||
e[l + i*len] /= ee;
|
||
}
|
||
}
|
||
return;
|
||
} |