You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
301 lines
12 KiB
301 lines
12 KiB
2 days ago
|
#pragma once
|
||
|
|
#include <macros.h>
|
||
|
|
#include <vector>
|
||
|
|
#include <limits>
|
||
|
|
#include <math/eigen_alias.hpp>
|
||
|
|
#include <math/math_defs.hpp>
|
||
|
|
#include <primitive_descriptor.h>
|
||
|
|
|
||
|
|
using aabb_t = Eigen::AlignedBox<double, 3>;
|
||
|
|
|
||
|
|
namespace internal
|
||
|
|
{
|
||
|
|
|
||
|
|
template <int Dim>
|
||
|
|
using aabb_t_dim = Eigen::AlignedBox<double, Dim>;
|
||
|
|
|
||
|
|
constexpr double EPSILON = std::numeric_limits<double>::epsilon();
|
||
|
|
|
||
|
|
// 辅助函数：类型转换
|
||
|
|
namespace detail
|
||
|
|
{
|
||
|
|
static inline Eigen::Vector3d to_eigen(const vector3d& v) { return Eigen::Vector3d(v.x, v.y, v.z); }
|
||
|
|
|
||
|
|
} // namespace detail
|
||
|
|
|
||
|
|
// 辅助函数：坐标转换
|
||
|
|
namespace geometry_utils
|
||
|
|
{
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief 投影转换函数
|
||
|
|
* @detail 将3D点投影到由 x_dir 和 y_dir 定义的平面上，并加上偏移量 offset，使得投影后的坐标系原点位于 offset 位置。
|
||
|
|
* @param point 3D点坐标
|
||
|
|
* @param x_dir 投影平面X轴方向
|
||
|
|
* @param y_dir 投影平面Y轴方向
|
||
|
|
* @param offset 投影后的偏移量
|
||
|
|
* @return 2D投影坐标
|
||
|
|
*/
|
||
|
|
static inline auto manually_projection(const vector3d& point,
|
||
|
|
const Eigen::Ref<const Eigen::Vector3d>& x_dir,
|
||
|
|
const Eigen::Ref<const Eigen::Vector3d>& y_dir,
|
||
|
|
const Eigen::Ref<const Eigen::Vector2d>& offset)
|
||
|
|
{
|
||
|
|
Eigen::Vector3d p = Eigen::Map<const Eigen::Vector3d>(&point.x);
|
||
|
|
return Eigen::Vector2d{p.dot(x_dir), p.dot(y_dir)} + offset;
|
||
|
|
}
|
||
|
|
|
||
|
|
static inline auto manually_projection(vector3d&& point,
|
||
|
|
const Eigen::Ref<const Eigen::Vector3d>& x_dir,
|
||
|
|
const Eigen::Ref<const Eigen::Vector3d>& y_dir,
|
||
|
|
const Eigen::Ref<const Eigen::Vector2d>& offset)
|
||
|
|
{
|
||
|
|
Eigen::Vector3d p = Eigen::Map<const Eigen::Vector3d>(&point.x);
|
||
|
|
return Eigen::Vector2d{p.dot(x_dir), p.dot(y_dir)} + offset;
|
||
|
|
}
|
||
|
|
} // namespace geometry_utils
|
||
|
|
|
||
|
|
// ==================== Polyline 几何数据 ====================
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Polyline 路径的几何计算数据
|
||
|
|
* @details 用于构造拉伸体的 profile 和 axis，支持直线段和圆弧段
|
||
|
|
*/
|
||
|
|
EXTERN_C struct PE_API polyline_geometry_data {
|
||
|
|
// ===== 几何数据 =====
|
||
|
|
std::vector<Eigen::Vector2d> vertices; // 所有顶点（包括圆弧的圆心和中间点）
|
||
|
|
std::vector<uint8_t> start_indices; // 每个段的起始索引
|
||
|
|
std::vector<double> thetas; // 每个段的角度（直线为0，圆弧为实际角度）
|
||
|
|
bool is_axis = false; // 标记是否为 Axis
|
||
|
|
|
||
|
|
// 最近点查询结果
|
||
|
|
struct line_closest_param_t {
|
||
|
|
Eigen::Vector3d point; // 最近点的3D坐标
|
||
|
|
double t; // 参数化坐标 [0, segment_count]
|
||
|
|
bool is_peak_value; // 是否是极值点（用于符号判定）
|
||
|
|
};
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief 作为 Axis 构建
|
||
|
|
* @param desc Polyline 描述符
|
||
|
|
* @param profile_ref_normal Profile 的参考法向量
|
||
|
|
* @param axis_to_world 输出的坐标系变换矩阵
|
||
|
|
* @param aabb 输出的3D AABB（世界坐标系）
|
||
|
|
*/
|
||
|
|
void build_as_axis(const polyline_descriptor_t& desc,
|
||
|
|
const vector3d& profile_ref_normal,
|
||
|
|
Eigen::Transform<double, 3, Eigen::AffineCompact>& axis_to_world,
|
||
|
|
aabb_t& aabb);
|
||
|
|
|
||
|
|
void build_as_axis(polyline_descriptor_t&& desc,
|
||
|
|
const vector3d& profile_ref_normal,
|
||
|
|
Eigen::Transform<double, 3, Eigen::AffineCompact>& axis_to_world,
|
||
|
|
aabb_t& aabb);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief 作为 Profile 构建
|
||
|
|
* @param desc Polyline 描述符
|
||
|
|
* @param proj_x_dir 投影平面X轴方向（世界坐标系）
|
||
|
|
* @param proj_y_dir 投影平面Y轴方向（世界坐标系）
|
||
|
|
* @param proj_origin 投影平面原点（世界坐标系）
|
||
|
|
* @param aabb 输出的2D AABB（投影平面坐标系）
|
||
|
|
*/
|
||
|
|
void build_as_profile(const polyline_descriptor_t& desc,
|
||
|
|
const Eigen::Vector3d& proj_x_dir,
|
||
|
|
const Eigen::Vector3d& proj_y_dir,
|
||
|
|
const Eigen::Vector3d& proj_origin,
|
||
|
|
aabb_t_dim<2>& aabb);
|
||
|
|
|
||
|
|
void build_as_profile(polyline_descriptor_t&& desc,
|
||
|
|
const Eigen::Vector3d& proj_x_dir,
|
||
|
|
const Eigen::Vector3d& proj_y_dir,
|
||
|
|
const Eigen::Vector3d& proj_origin,
|
||
|
|
aabb_t_dim<2>& aabb);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief 计算参数 t 处的切线向量
|
||
|
|
* @param t 参数化坐标 [0, segment_count]
|
||
|
|
* @return 单位切线向量（2D）
|
||
|
|
*/
|
||
|
|
[[nodiscard]] Eigen::Vector2d calculate_tangent(double t) const;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief 计算参数 t 处的法线向量（指向多段线内侧）
|
||
|
|
* @details 对 CCW 缠绕的多段线，内法向定义为切向量的逆时针旋转 90°。
|
||
|
|
* 直线段：inward = CCW_rot(tangent)
|
||
|
|
* 圆弧段：通过顶点帧的叉积符号判断 bulge 正负，确保结果统一指向内侧。
|
||
|
|
* @param t 参数化坐标 [0, segment_count]
|
||
|
|
* @return 法线向量（2D，圆弧段可能未归一化）
|
||
|
|
*/
|
||
|
|
[[nodiscard]] Eigen::Vector2d calculate_normal(double t) const;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief 计算点到 polyline 的最近点
|
||
|
|
* @param p 查询点（3D坐标，z分量用于距离计算）
|
||
|
|
* @return 最近点信息（位置、参数、是否极值）
|
||
|
|
*/
|
||
|
|
[[nodiscard]] std::pair<line_closest_param_t, double> calculate_closest_param(
|
||
|
|
const Eigen::Ref<const Eigen::Vector3d>& p) const;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Point Membership Classification (PMC)
|
||
|
|
* @details Determines which side of the polyline boundary a point lies on,
|
||
|
|
* using the INWARD normal returned by calculate_normal().
|
||
|
|
*
|
||
|
|
* Let v = p - q (from closest boundary point q toward query point p).
|
||
|
|
* v · n_inward < 0 → p opposes inward direction → p is OUTSIDE → return true
|
||
|
|
* v · n_inward ≥ 0 → p aligns with inward direction → p is INSIDE → return false
|
||
|
|
*
|
||
|
|
* Convention (matches SDF sign rule):
|
||
|
|
* true = point is OUTSIDE the polyline (SDF > 0)
|
||
|
|
* false = point is INSIDE the polyline (SDF < 0)
|
||
|
|
*
|
||
|
|
* @param p 查询点（2D）
|
||
|
|
* @param closest_param 最近点信息（由 calculate_closest_param 返回）
|
||
|
|
* @return true 若点在 polyline 外部
|
||
|
|
* @return false 若点在 polyline 内部
|
||
|
|
*/
|
||
|
|
[[nodiscard]] bool pmc(const Eigen::Ref<const Eigen::Vector2d>& p, const line_closest_param_t& closest_param) const;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief 判断参数 t 是否在端点处
|
||
|
|
* @param t 参数化坐标
|
||
|
|
* @return true 如果在起点或终点
|
||
|
|
*/
|
||
|
|
[[nodiscard]] bool isEnd(double t) const;
|
||
|
|
|
||
|
|
private:
|
||
|
|
/**
|
||
|
|
* @brief 内部通用构建逻辑
|
||
|
|
* @param desc Polyline 描述符
|
||
|
|
* @param project_func 投影函数（3D点 → 2D点）
|
||
|
|
* @param aabb 2D AABB（可选）
|
||
|
|
*/
|
||
|
|
void build_internal(const polyline_descriptor_t& desc,
|
||
|
|
std::function<Eigen::Vector2d(const vector3d&)> project_func,
|
||
|
|
aabb_t_dim<2>* aabb = nullptr);
|
||
|
|
};
|
||
|
|
|
||
|
|
// ==================== Helixline 几何数据 ====================
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Helixline（螺旋线）的几何计算数据
|
||
|
|
* @details 用于螺旋拉伸体的 axis
|
||
|
|
*/
|
||
|
|
EXTERN_C struct PE_API helixline_geometry_data {
|
||
|
|
// ===== 几何参数 =====
|
||
|
|
double radius; // 螺旋半径
|
||
|
|
double height; // 轴向高度
|
||
|
|
double total_theta; // 总旋转角度
|
||
|
|
bool is_righthanded; // 是否为右旋
|
||
|
|
|
||
|
|
// ===== 最近点查询结果 =====
|
||
|
|
struct line_closest_param_t {
|
||
|
|
Eigen::Vector3d point; // 最近点的3D坐标
|
||
|
|
double t; // 参数化坐标
|
||
|
|
bool is_peak_value; // 是否是极值点
|
||
|
|
};
|
||
|
|
|
||
|
|
// ===== 核心方法 =====
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief 从 descriptor 构建几何数据
|
||
|
|
*/
|
||
|
|
void build_from_descriptor(const helixline_descriptor_t& desc,
|
||
|
|
Eigen::Transform<double, 3, Eigen::AffineCompact, Eigen::DontAlign>& axis_to_world,
|
||
|
|
aabb_t& aabb);
|
||
|
|
|
||
|
|
|
||
|
|
void build_from_descriptor(helixline_descriptor_t&& desc,
|
||
|
|
Eigen::Transform<double, 3, Eigen::AffineCompact, Eigen::DontAlign>& axis_to_world,
|
||
|
|
aabb_t& aabb);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief 计算参数 t 处的切线向量
|
||
|
|
* @param t 参数化坐标 [0, 1]
|
||
|
|
* @return 单位切线向量（3D）
|
||
|
|
*/
|
||
|
|
[[nodiscard]] Eigen::Vector3d calculate_tangent(double t) const;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief 计算参数 t 处的法线向量（径向）
|
||
|
|
* @param t 参数化坐标 [0, 1]
|
||
|
|
* @return 法线向量（3D）
|
||
|
|
*/
|
||
|
|
[[nodiscard]] Eigen::Vector3d calculate_normal(double t) const;
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief 计算点到螺旋线的最近点（使用峰值搜索 + 牛顿迭代）
|
||
|
|
* @param p 查询点（3D坐标）
|
||
|
|
* @return 最近点信息
|
||
|
|
*/
|
||
|
|
[[nodiscard]] line_closest_param_t calculate_closest_param(const Eigen::Vector3d& p) const;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief 判断参数 t 是否在端点处
|
||
|
|
*/
|
||
|
|
[[nodiscard]] bool isEnd(double t) const;
|
||
|
|
};
|
||
|
|
|
||
|
|
// ==================== TBN 坐标系构建 ====================
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief 构建 TBN 坐标系矩阵（复现旧架构）
|
||
|
|
* @param tangent 切线方向（T）
|
||
|
|
* @param normal 法线方向（N）
|
||
|
|
* @param origin 坐标系原点
|
||
|
|
* @return TBN 变换矩阵
|
||
|
|
*/
|
||
|
|
|
||
|
|
// 返回完整的 paired_model_matrix
|
||
|
|
inline paired_model_matrix build_TBN_paired_matrix(const Eigen::Vector3d& tangent,
|
||
|
|
const Eigen::Vector3d& normal,
|
||
|
|
const Eigen::Vector3d& origin)
|
||
|
|
{
|
||
|
|
paired_model_matrix result;
|
||
|
|
|
||
|
|
// 构建 local_to_world
|
||
|
|
Eigen::Vector3d T = tangent.normalized();
|
||
|
|
Eigen::Vector3d N = normal.normalized();
|
||
|
|
Eigen::Vector3d B = N.cross(T).normalized();
|
||
|
|
|
||
|
|
result.local_to_world.matrix().col(0).head<3>() = B;
|
||
|
|
result.local_to_world.matrix().col(1).head<3>() = N;
|
||
|
|
result.local_to_world.matrix().col(2).head<3>() = T;
|
||
|
|
result.local_to_world.matrix().col(3).head<3>() = origin;
|
||
|
|
// result.local_to_world.matrix()(3, 3) = 1.0;
|
||
|
|
|
||
|
|
// 自动计算逆矩阵
|
||
|
|
result.world_to_local = result.local_to_world.inverse();
|
||
|
|
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
// 针对 Polyline 的专用函数
|
||
|
|
inline paired_model_matrix build_polyline_cap_matrix(const Eigen::Vector3d& axis_point,
|
||
|
|
const Eigen::Vector3d& axis_tangent,
|
||
|
|
const Eigen::Vector3d& axis_normal,
|
||
|
|
bool is_top_cap = false)
|
||
|
|
{
|
||
|
|
//Eigen::Vector3d tangent = is_top_cap ? -axis_tangent : axis_tangent;
|
||
|
|
//return build_TBN_paired_matrix(tangent, axis_normal, axis_point);
|
||
|
|
|
||
|
|
(void)is_top_cap; // both caps use the same +tangent direction; mark controls sign
|
||
|
|
|
||
|
|
paired_model_matrix result;
|
||
|
|
const Eigen::Vector3d T = axis_tangent.normalized();
|
||
|
|
const Eigen::Vector3d N = axis_normal.normalized();
|
||
|
|
// Gram-Schmidt: make N orthogonal to T
|
||
|
|
const Eigen::Vector3d N_orth = (N - N.dot(T) * T).normalized();
|
||
|
|
const Eigen::Vector3d B_orth = T.cross(N_orth);
|
||
|
|
|
||
|
|
result.local_to_world.matrix().col(0).head<3>() = T; // plane normal = axis tangent
|
||
|
|
result.local_to_world.matrix().col(1).head<3>() = N_orth;
|
||
|
|
result.local_to_world.matrix().col(2).head<3>() = B_orth;
|
||
|
|
result.local_to_world.matrix().col(3).head<3>() = axis_point;
|
||
|
|
result.world_to_local = result.local_to_world.inverse();
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
} // namespace internal
|