zhangyiss/gctl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity
39eed90abb
gctl/lib/geometry/point3c.h

704 lines
18 KiB
C
Raw Normal View History

initial upload
2024-09-10 15:45:07 +08:00
/********************************************************
* ██████╗ ██████╗████████╗██╗
* ██╔════╝ ██╔════╝╚══██╔══╝██║
* ██║ ███╗██║ ██║ ██║
* ██║ ██║██║ ██║ ██║
* ╚██████╔╝╚██████╗ ██║ ███████╗
* ╚═════╝ ╚═════╝ ╚═╝ ╚══════╝
* 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_POINT3C_H
#define _GCTL_POINT3C_H
#include "../core/macro.h"
#include "../core/exceptions.h"
#include "../core/array.h"
#include "point3s.h"
#include "iostream"
#include "string"
#include "cmath"
#include "iomanip"
#include "regex"
namespace gctl
{
template <typename T> struct point3c;
template <typename T> struct point3s;
typedef point3c<double> point3dc;
typedef point3c<float> point3fc;
#ifndef IO_PSN
#define IO_PSN
// static variable for controlling the IO process
static int io_psn = 6;
#endif // IO_PSN
/**
* @brief 直角坐标系内的实点
*/
template <typename T>
struct point3c
{
T x, y, z;
/**
* Constructor
*/
point3c();
/**
* @brief Constructor with initial parameters
*
* @param[in] a x coordinate
* @param[in] b y coordinate
* @param[in] c z coordinate
*/
point3c(T a, T b, T c);
/**
* @brief Copy constructor
*
* @param[in] b the original point
*/
point3c(const point3c<T> &b);
/**
* @brief De-constructor
*/
virtual ~point3c(){}
/**
* @brief If the point has valid coordinates
*
* @return true for valid, otherwise return false
*/
bool valid() const;
/**
* @brief Set coordinate values
*
* @param[in] a x coordinate
* @param[in] b y coordinate
* @param[in] c z coordinate
*/
void set(T a, T b, T c);
/**
* @brief Set coordinates
*
* @param[in] b the original point
*/
void set(const point3c<T> &b);
/**
* @brief Set the point/vector's module to the given value.
*
* @param[in] mod The new module
* @param[in] cut_off The round value to zero
*/
void set2module(T mod, T cut_off = GCTL_ZERO);
/**
* @brief Return vector's module
*
* @return module
*/
T module() const;
/**
* @brief Return the corresponding spherical coordinates
*
* @return spherical coordinates point
*/
point3s<T> c2s() const;
/**
* @brief Return the normal vector
*
* @return normal vector
*/
point3c<T> normal() const;
/**
* @brief Parse the coordinates from a string
*
* @note String format: (x, y, z)
*
* @param[in] str The input string
*/
void str(std::string str);
/**
* @brief Sets the i/o precision of the type.
*
* @param[in] psn The desired precision
*/
void set_io_precision(int psn);
/**
* @brief 计算三维空间矢量与坐标轴的夹角（弧度），以引用的形式返回。
*
* @param angle_x 矢量与 x 轴的夹角
* @param angle_y 矢量与 y 轴的夹角
* @param angle_z 矢量与 z 轴的夹角
*/
void axis_angle(T &angle_x, T &angle_y, T &angle_z);
/**
* @brief 绕x y z轴旋转point3dc
*
* @param[in] angx 绕x轴逆时针旋转的角度（单位为弧度）。
* @param[in] angy 绕y轴逆时针旋转的角度（单位为弧度）。
* @param[in] angz 绕z轴逆时针旋转的角度（单位为弧度）。
*
* @return 旋转后的矢量。
*/
point3c<T> rotate(T angx, T angy, T angz);
/**
* @brief 绕通过原点的旋转轴旋转point3dc
*
* @param[in] axis 旋转轴
* @param[in] ang 绕旋转轴逆时针旋转的角度（单位为弧度）。
*
* @return 返回的矢量。
*/
point3c<T> rotate(const point3c<T> &axis, double ang);
/**
* @brief 输出位置
*
* @param os 输出流
*/
void out_loc(std::ostream &os, char deli) const;
/**
* @brief 输入位置
*
* @param os 输入流
*/
void in_loc(std::istream &os);
/**
* @brief 返回位置
*
* @return 位置
*/
point3c<T> get_loc() const;
};
template <typename T>
gctl::point3c<T>::point3c()
{
x = y = z = NAN;
}
template <typename T>
gctl::point3c<T>::point3c(T a, T b, T c)
{
set(a, b, c);
}
template <typename T>
gctl::point3c<T>::point3c(const point3c<T> &b)
{
set(b);
}
template <typename T>
bool gctl::point3c<T>::valid() const
{
if (std::isnan(x) || std::isnan(y) || std::isnan(z)) return false;
if (std::isinf(x) || std::isinf(y) || std::isinf(z)) return false;
return true;
}
template <typename T>
void gctl::point3c<T>::set(T a, T b, T c)
{
if (std::isnan(a) || std::isnan(b) || std::isnan(c) ||
std::isinf(a) || std::isinf(b) || std::isinf(c))
{
throw invalid_argument("Invalid value detected. From point3c::set(...)");
}
x = a; y = b; z = c;
return;
}
template <typename T>
void gctl::point3c<T>::set(const point3c<T> &b)
{
if (std::isnan(b.x) || std::isnan(b.y) || std::isnan(b.z) ||
std::isinf(b.x) || std::isinf(b.y) || std::isinf(b.z))
{
throw invalid_argument("Invalid value detected. From point3c::set(...)");
}
x = b.x; y = b.y; z = b.z;
return;
}
template <typename T>
void gctl::point3c<T>::set2module(T mod, T cut_off)
{
if (cut_off <= 0.0)
{
throw invalid_argument("Invalid cut-off value. From point3c::set2module(...)");
}
if (mod < cut_off && mod > -1.0*cut_off)
{
x = y = z = 0.0;
return;
}
T old_mod = module();
if (old_mod < cut_off)
{
throw length_error("Zero module. From point3c::set2module(...)");
}
x = x*mod/old_mod;
y = y*mod/old_mod;
z = z*mod/old_mod;
return;
}
template <typename T>
T gctl::point3c<T>::module() const
{
return sqrt(x*x + y*y + z*z);
}
template <typename T>
gctl::point3s<T> gctl::point3c<T>::c2s() const
{
point3s<T> outs;
outs.rad = module();
if (outs.rad <= GCTL_ZERO) //点距离原点极近 将点置于原点
{
throw runtime_error("The point is at the origin. From point3c::c2s()");
}
outs.lat = 90.0 - acos(z/outs.rad)*180.0/GCTL_Pi;
outs.lon = atan2(y, x)*180.0/GCTL_Pi;
return outs;
}
template <typename T>
gctl::point3c<T> gctl::point3c<T>::normal() const
{
point3c<T> nor_v;
double length = module();
if (length <= GCTL_ZERO) //如果矢量为0则令向量与z轴重合
{
throw runtime_error("The point is at the origin. From point3c::normal()");
}
nor_v.x = x/length;
nor_v.y = y/length;
nor_v.z = z/length;
return nor_v;
}
template <typename T>
void gctl::point3c<T>::str(std::string str)
{
std::smatch ret;
std::regex pattern("\\((-?\\d*\\.?\\d+?),[ ]*(-?\\d*\\.?\\d+?),[ ]*(-?\\d*\\.?\\d+?)\\)");
if (regex_search(str, ret, pattern))
{
x = atof(std::string(ret[1]).c_str());
y = atof(std::string(ret[2]).c_str());
z = atof(std::string(ret[3]).c_str());
return;
}
throw runtime_error("Fail to parse the input string: " + str + ". From point3c::str(...)");
}
template <typename T>
void gctl::point3c<T>::set_io_precision(int psn)
{
if (psn < 0)
{
throw invalid_argument("Invalid precision. From point3c::set_io_precision(...)");
}
io_psn = psn;
return;
}
template <typename T>
void gctl::point3c<T>::axis_angle(T &angle_x, T &angle_y, T &angle_z)
{
double length = module();
if (length <= GCTL_ZERO)
{
throw runtime_error("The point is at the origin. From point3c::axis_angle(...)");
}
else
{
angle_x = acos(x/length);
angle_y = acos(y/length);
angle_z = acos(z/length);
return;
}
}
template <typename T>
gctl::point3c<T> gctl::point3c<T>::rotate(T angx, T angy, T angz)
{
gctl::point3c<T> cp, temp_cp;
// 绕x轴旋转
cp.x = x;
cp.y = cos(angx)*y - sin(angx)*z;
cp.z = sin(angx)*y + cos(angx)*z;
// 绕y轴旋转
temp_cp.x = cos(angy)*cp.x + sin(angy)*cp.z;
temp_cp.y = cp.y;
temp_cp.z = -1.0*sin(angy)*cp.x + cos(angy)*cp.z;
// 绕z轴旋转
cp.x = cos(angz)*temp_cp.x - sin(angz)*temp_cp.y;
cp.y = sin(angz)*temp_cp.x + cos(angz)*temp_cp.y;
cp.z = temp_cp.z;
return cp;
}
template <typename T>
gctl::point3c<T> gctl::point3c<T>::rotate(const point3c<T> &axis, double ang)
{
point3c<T> cp;
double A[3][3], A2[3][3], A3[3][3], M[3][3];
point3c<T> nor_axis = axis.normal();
A[0][0] = nor_axis.x*nor_axis.x; A[0][1] = nor_axis.x*nor_axis.y; A[0][2] = nor_axis.x*nor_axis.z;
A[1][0] = nor_axis.y*nor_axis.x; A[1][1] = nor_axis.y*nor_axis.y; A[1][2] = nor_axis.y*nor_axis.z;
A[2][0] = nor_axis.z*nor_axis.x; A[2][1] = nor_axis.z*nor_axis.y; A[2][2] = nor_axis.z*nor_axis.z;
A2[0][0] = 1.0-nor_axis.x*nor_axis.x; A2[0][1] = nor_axis.x*nor_axis.y; A2[0][2] = nor_axis.x*nor_axis.z;
A2[1][0] = nor_axis.y*nor_axis.x; A2[1][1] = 1.0-nor_axis.y*nor_axis.y; A2[1][2] = nor_axis.y*nor_axis.z;
A2[2][0] = nor_axis.z*nor_axis.x; A2[2][1] = nor_axis.z*nor_axis.y; A2[2][2] = 1.0-nor_axis.z*nor_axis.z;
A3[0][0] = 0.0; A3[0][1] = -1.0*nor_axis.z; A3[0][2] = nor_axis.y;
A3[1][0] = nor_axis.z; A3[1][1] = 0.0; A3[1][2] = -1.0*nor_axis.x;
A3[2][0] = -1.0*nor_axis.y; A3[2][1] = nor_axis.x; A3[2][2] = 0.0;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
M[i][j] = A[i][j] + cos(ang)*A2[i][j] + sin(ang)*A3[i][j];
}
}
cp.x = x*M[0][0] + y*M[1][0] + z*M[2][0];
cp.y = x*M[0][1] + y*M[1][1] + z*M[2][1];
cp.z = x*M[0][2] + y*M[1][2] + z*M[2][2];
return cp;
}
template <typename T>
void gctl::point3c<T>::out_loc(std::ostream &os, char deli) const
{
os << std::setprecision(io_psn) << x << deli << y << deli << z;
return;
}
template <typename T>
void gctl::point3c<T>::in_loc(std::istream &os)
{
os >> x >> y >> z;
return;
}
template <typename T>
gctl::point3c<T> gctl::point3c<T>::get_loc() const
{
return point3c<T>(x, y, z);
}
template <typename T>
bool operator ==(const point3c<T> &a, const point3c<T> &b)
{
if( fabs(a.x - b.x) < GCTL_ZERO &&
fabs(a.y - b.y) < GCTL_ZERO &&
fabs(a.z - b.z) < GCTL_ZERO)
{
return true;
}
else return false;
}
template <typename T>
bool operator !=(const point3c<T> &a, const point3c<T> &b)
{
if( fabs(a.x - b.x) >= GCTL_ZERO ||
fabs(a.y - b.y) >= GCTL_ZERO ||
fabs(a.z - b.z) >= GCTL_ZERO)
{
return true;
}
else return false;
}
template <typename T>
point3c<T> operator -(const point3c<T> &a, const point3c<T> &b)
{
point3c<T> m;
m.x = a.x-b.x;
m.y = a.y-b.y;
m.z = a.z-b.z;
return m;
}
template <typename T>
point3c<T> operator +(const point3c<T> &a, const point3c<T> &b)
{
gctl::point3c<T> m;
m.x = a.x+b.x;
m.y = a.y+b.y;
m.z = a.z+b.z;
return m;
}
template <typename T>
point3c<T> operator *(int sign, const point3c<T> &b)
{
gctl::point3c<T> m;
m.x = sign*b.x;
m.y = sign*b.y;
m.z = sign*b.z;
return m;
}
template <typename T>
point3c<T> operator *(float sign, const point3c<T> &b)
{
gctl::point3c<T> m;
m.x = sign*b.x;
m.y = sign*b.y;
m.z = sign*b.z;
return m;
}
template <typename T>
point3c<T> operator *(double sign, const point3c<T> &b)
{
gctl::point3c<T> m;
m.x = sign*b.x;
m.y = sign*b.y;
m.z = sign*b.z;
return m;
}
template <typename T>
point3c<T> operator *(const point3c<T> &b, int sign)
{
gctl::point3c<T> m;
m.x = sign*b.x;
m.y = sign*b.y;
m.z = sign*b.z;
return m;
}
template <typename T>
point3c<T> operator *(const point3c<T> &b, float sign)
{
gctl::point3c<T> m;
m.x = sign*b.x;
m.y = sign*b.y;
m.z = sign*b.z;
return m;
}
template <typename T>
point3c<T> operator *(const point3c<T> &b, double sign)
{
point3c<T> m;
m.x = sign*b.x;
m.y = sign*b.y;
m.z = sign*b.z;
return m;
}
template <typename T>
std::ostream &operator <<(std::ostream & os, const point3c<T> &a)
{
os << std::setprecision(io_psn) << a.x << " " << a.y << " " << a.z;
return os;
}
template <typename T>
std::istream &operator >>(std::istream & os, point3c<T> &a)
{
os >> a.x >> a.y >> a.z;
return os;
}
/**
* @brief 两个三维空间向量的点积
*
* @param[in] a 三维空间内的一个向量或实点a。
* @param[in] b 三维空间内的一个向量或实点b。
*
* @return 点积
*/
template <typename T>
double dot(const point3c<T> &a, const point3c<T> &b)
{
return a.x*b.x+a.y*b.y+a.z*b.z;
}
/**
* @brief 矢量叉乘 注意矢量的叉乘是有顺序的 这里默认为第一个矢量叉乘第二个矢量
*
* @param[in] a 向量a
* @param[in] b 向量b
* @param[in] cut_off 截断误差。当叉乘向量的任一方向的坐标值的绝对值小于截断误差时将直接置为0。
* 截断误差越小则对输入向量的精度要求越高，在实际计算中适当放开截断误差有助于使算法稳定。这个参数有默认值，
* 为 gctl_macro.h 中预设的 GCTL_ZERO 值。
*
* @return 两个向量的叉乘
*/
template <typename T>
point3c<T> cross(const point3c<T> &a, const point3c<T> &b, double cut_off = GCTL_ZERO)
{
gctl::point3c<T> v;
v.x = a.y*b.z-a.z*b.y;
v.y = a.z*b.x-a.x*b.z;
v.z = a.x*b.y-a.y*b.x;
if (fabs(v.x) <= cut_off) v.x = 0;
if (fabs(v.y) <= cut_off) v.y = 0;
if (fabs(v.z) <= cut_off) v.z = 0;
return v;
}
/**
* @brief 两个矢量之间的距离
*
* @param[in] a 三维空间内的一个向量或实点a。
* @param[in] b 三维空间内的一个向量或实点b。
*
* @return 两点之间的距离
*/
template <typename T>
double distance(const point3c<T> &a, const point3c<T> &b)
{
return (a - b).module();
}
/**
* @brief 函数判断两个point3dc类型是否等于
*
* @param[in] a 三维空间内的一个向量或实点a。
* @param[in] b 三维空间内的一个向量或实点b。
* @param[in] cut_off 截断误差，默认值为 gctl_macro.h 中预设的 GCTL_ZERO 值，可在调用时改变。
* 注意比较的精度为 cut_off 减一位，比如 cut_off 为1e-10则比较的精度为小数点后9位。
*
* @return 是否相等。
*/
template <typename T>
bool isequal(const point3c<T> &a, const point3c<T> &b, double cut_off = GCTL_ZERO)
{
if( fabs(a.x - b.x) < cut_off &&
fabs(a.y - b.y) < cut_off &&
fabs(a.z - b.z) < cut_off)
{
return true;
}
else return false;
}
/**
* @brief 函数判断两个point3dc类型是否不等于
*
* @param[in] a 三维空间内的一个向量或实点a。
* @param[in] b 三维空间内的一个向量或实点b。
* @param[in] cut_off 截断误差，默认值为 gctl_macro.h 中预设的 GCTL_ZERO 值，可在调用时改变。
*
* @return 是否不相等。
*/
template <typename T>
bool notequal(const point3c<T> &a, const point3c<T> &b, double cut_off = GCTL_ZERO)
{
if( fabs(a.x - b.x) >= cut_off ||
fabs(a.y - b.y) >= cut_off ||
fabs(a.z - b.z) >= cut_off)
{
return true;
}
else return false;
}
/**
* @brief 计算一个矢量的单位矢量相对一个标量（如顶点半径）的偏导数矢量
*
* @param[in] n 该矢量
* @param[in] n_rj 该矢量对于这个标量的偏导数矢量
*
* @return 偏导数矢量
*/
template <typename T>
point3c<T> nr_dr(const point3c<T> &v, const point3c<T> &v_rj)
{
point3c<T> out, n;
double moduel_v = v.module();
n.x = v.x/moduel_v;
n.y = v.y/moduel_v;
n.z = v.z/moduel_v;
double temp = dot(v, v_rj)/moduel_v;
out.x = (v_rj.x - n.x*temp)/moduel_v;
out.y = (v_rj.y - n.y*temp)/moduel_v;
out.z = (v_rj.z - n.z*temp)/moduel_v;
return out;
}
/**
* @brief 生成直角坐标系的点数组
*
* @param out_ps 返回的点数组
* @param[in] xmin x最小值
* @param[in] xmax x最大值
* @param[in] ymin y最小值
* @param[in] ymax y最大值
* @param[in] dx x间隔
* @param[in] dy y间隔
* @param[in] ele 高程值
*/
template <typename T>
void get_grid_point3c(array<point3c<T>> &out_ps, T xmin, T xmax, T ymin,
T ymax, T dx, T dy, T ele)
{
if (xmin >= xmax || ymin >= ymax || xmin+dx>xmax || ymin+dy>ymax)
{
throw invalid_argument("Invalid range parameters. From get_grid_point3c(...)");
}
if (dx <= 0 || dy <= 0)
{
throw invalid_argument("Invalid interval parameters. From get_grid_point3c(...)");
}
int xnum = floor((xmax-xmin)/dx) + 1;
int ynum = floor((ymax-ymin)/dy) + 1;
out_ps.resize(xnum*ynum);
for (int j = 0; j < ynum; j++)
{
for (int i = 0; i < xnum; i++)
{
out_ps.at(j*xnum+i).x = xmin + dx*i;
out_ps.at(j*xnum+i).y = ymin + dy*j;
out_ps.at(j*xnum+i).z = ele;
}
}
return;
}
}
#endif // _GCTL_POINT3C_H
Reference in New Issue Copy Permalink