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.
83 lines
3.4 KiB
83 lines
3.4 KiB
#pragma once
|
|
|
|
#include <base/subface.hpp>
|
|
|
|
namespace internal
|
|
{
|
|
// local: cylinder face x^2+y^2-1=0
|
|
struct cylinder_face_t final : subface {
|
|
static constexpr uint64_t max_degree = 2;
|
|
static constexpr equation_system_type eq_sys_type = equation_system_type::implicit;
|
|
|
|
std::function<double(Eigen::Vector3d)> fetch_sdf_evaluator() const override;
|
|
std::function<Eigen::Vector3d(Eigen::Vector3d)> fetch_sdf_grad_evaluator() const override;
|
|
|
|
// u: planar angle from x-axis to z-axis
|
|
// v: depth/height from xz-plane to y-axis
|
|
std::function<Eigen::Vector4d(double, double)> fetch_point_by_param_evaluator() const override;
|
|
|
|
std::function<constraint_curve_intermediate(double)> fetch_curve_constraint_evaluator(parameter_u_t constraint_var_type,
|
|
double u) const override;
|
|
std::function<constraint_curve_intermediate(double)> fetch_curve_constraint_evaluator(parameter_v_t constraint_var_type,
|
|
double v) const override;
|
|
std::function<equation_intermediate_t(constraint_curve_intermediate &&)> fetch_solver_evaluator() const override;
|
|
};
|
|
|
|
struct cylinder_paired_model_matrix {
|
|
internal::paired_model_matrix *data{};
|
|
};
|
|
} // namespace internal
|
|
|
|
namespace detail
|
|
{
|
|
template <>
|
|
struct hasher<internal::cylinder_paired_model_matrix> {
|
|
size_t operator()(const internal::cylinder_paired_model_matrix &block) const
|
|
{
|
|
// 使用 world_to_local 矩阵（4x4 齐次变换）
|
|
const auto& mat = block.data->world_to_local.matrix(); // 4x4
|
|
|
|
// 提取线性部分 A (3x3) 和平移部分 b (3x1)
|
|
Eigen::Matrix3d A = mat.block<3,3>(0,0);
|
|
Eigen::Vector3d b = mat.block<3,1>(0,3);
|
|
|
|
// ✅ 直接使用 A 的前两行作为 R（不需要逆）
|
|
// 因为 A 就是 world_to_local 的线性部分，row(0)=x, row(1)=y
|
|
Eigen::Matrix<double, 2, 3> R;
|
|
R.row(0) = A.row(0);
|
|
R.row(1) = A.row(1);
|
|
|
|
// ✅ 归一化 R 的行向量（消除缩放影响）
|
|
double norm0 = R.row(0).norm();
|
|
double norm1 = R.row(1).norm();
|
|
if (norm0 > 1e-8) R.row(0) /= norm0;
|
|
if (norm1 > 1e-8) R.row(1) /= norm1;
|
|
|
|
// G = R^T * R 编码横截面方向（已归一化，对旋转/缩放鲁棒）
|
|
Eigen::Matrix3d G = R.transpose() * R;
|
|
|
|
// ❌ 原始 R*b 依赖局部坐标系旋转
|
|
// ✅ 改为：只使用 ||R*b||^2（旋转不变）或投影长度平方
|
|
Eigen::Vector2d rp = R * b;
|
|
double radial_proj_sq = rp.squaredNorm(); // 旋转不变量
|
|
|
|
// 构造哈希键：G (3x3) + radial_proj_sq (1 double)
|
|
// 使用紧凑方式避免结构体
|
|
Eigen::Matrix<double, 10, 1> hash_key;
|
|
hash_key.head<9>() = Eigen::Map<const Eigen::Matrix<double, 9, 1>>(G.data());
|
|
hash_key(9) = radial_proj_sq;
|
|
|
|
return XXH3_64bits(hash_key.data(), sizeof(double) * 10);
|
|
}
|
|
};
|
|
|
|
template <>
|
|
struct default_elem_ctor<internal::cylinder_paired_model_matrix, internal::cylinder_face_t> {
|
|
internal::cylinder_face_t operator()(const internal::cylinder_paired_model_matrix &k) const
|
|
{
|
|
internal::cylinder_face_t res{};
|
|
res.model_matrices = const_cast<internal::paired_model_matrix *>(k.data);
|
|
return res;
|
|
}
|
|
};
|
|
} // namespace detail
|