creep-flight-distance-calculation/include/voxel_distance_calculator.hpp

#ifndef VOXEL_DISTANCE_CALCULATOR_H
#define VOXEL_DISTANCE_CALCULATOR_H

#include <vector>
#include <unordered_set>
#include <unordered_map>
#include <cmath>
#include <limits>
#include <array>
#include <cstdint>
#include "vector3.hpp"
#include "geometry_utils.hpp"

namespace VoxelDistance {

// ============================================
// 基础数据结构
// ============================================

struct Voxel {
    int x, y, z;

    Voxel() : x(0), y(0), z(0) {}
    Voxel(int _x, int _y, int _z) : x(_x), y(_y), z(_z) {}

    bool operator==(const Voxel& other) const {
        return x == other.x && y == other.y && z == other.z;
    }

    struct Hash {
        size_t operator()(const Voxel& v) const {
            return ((size_t)v.x * 73856093) ^
                   ((size_t)v.y * 19349663) ^
                   ((size_t)v.z * 83492791);
        }
    };
};

using VoxelSet = std::unordered_set<Voxel, Voxel::Hash>;
using VoxelPath = std::vector<Voxel>;

// ============================================
// 计算结果
// ============================================

struct DistanceResult {
    bool success;           // 是否找到路径
    double distance;        // 最短距离
    Voxel start_point;      // 在部件A上的起点
    Voxel end_point;        // 在部件B上的终点
    VoxelPath path;         // 完整路径（包含起点和终点）
    bool exceeded_threshold; // 是否超出阈值
    bool ub_only = false; // 是否是上界（即粗略路径，未细化）

    // 新增：几何端点（世界坐标，在零件表面）
    Vector3 geom_start;  // 接触 compA 的几何点
    Vector3 geom_end;    // 接触 compB 的几何点

	std::vector<Vector3> geom_path;  // 几何路径（世界坐标）

    // ===== 新增：用于触发细化的 coarse 初始化诊断 =====
    // 当 coarse 初始化时 open_A/open_B 被障碍过滤空了，置 true
    bool coarse_init_no_free_seed_A = false;
    bool coarse_init_no_free_seed_B = false;

    // Mixed MR search raw candidates for API-side full postprocess.
    bool has_astar_candidate = false;
    bool has_theta_candidate = false;
    double astar_candidate_distance = std::numeric_limits<double>::infinity();
    double theta_candidate_distance = std::numeric_limits<double>::infinity();
    VoxelPath astar_candidate_path;
    VoxelPath theta_candidate_path;

    DistanceResult()
        : success(false)
        , distance(std::numeric_limits<double>::infinity())
        , start_point(0, 0, 0)
        , end_point(0, 0, 0)
        , exceeded_threshold(false)
        , geom_start(0, 0, 0), geom_end(0, 0, 0)
    {}
};

// ============================================
// 邻域定义
// ============================================

struct Neighbor {
    Voxel voxel;
    double distance;
};

class VoxelNeighborhood {
public:
    // 获取26邻域
    static std::vector<Neighbor> Get26Neighbors(const Voxel& center);

    // 获取6面邻域
    static std::vector<Voxel> Get6FaceNeighbors(const Voxel& center);

private:
    static constexpr double FACE_DIST = 1.0;          // 面邻居距离
    static constexpr double EDGE_DIST = 1.414213562;  // √2
    static constexpr double CORNER_DIST = 1.732050808; // √3
};

// ============================================
// 辅助函数
// ============================================

class VoxelUtils {
public:
    // 提取部件边界体素
    static VoxelSet ExtractBoundary(const VoxelSet& component);

    // 提取障碍表面体素
    static VoxelSet ExtractSurface(const VoxelSet& obstacles);

    // 计算点到体素集的最小欧氏距离
    static double MinEuclideanDistance(const Voxel& voxel, const VoxelSet& target_set);

    // 从bool数组转换为VoxelSet
    static VoxelSet FromBoolArray(const bool* array, int xsize, int ysize, int zsize);

    // 从坐标数组转换为VoxelSet
    static VoxelSet FromCoordArray(const int* coords, int count);

    // 检查体素是否在范围内
    static bool IsInBounds(const Voxel& v, int xsize, int ysize, int zsize);

    // 从 std::vector<bool> 转换
    static VoxelSet FromBoolVector(const std::vector<bool>& vec, int xsize, int ysize, int zsize) {
        VoxelSet result;

        for (int z = 0; z < zsize; ++z) {
            for (int y = 0; y < ysize; ++y) {
                for (int x = 0; x < xsize; ++x) {
                    int idx = x + y * xsize + z * xsize * ysize;
                    if (vec[idx]) {
                        result.insert(Voxel(x, y, z));
                    }
                }
            }
        }

        return result;
    }

};

// ============================================
// 飞行距离计算器
// ============================================

class FlightDistanceCalculator {
public:
    static DistanceResult Calculate(
        const VoxelSet& component_A,
        const VoxelSet& component_B,
        const VoxelSet& obstacles,
        int xsize, int ysize, int zsize,
        double threshold = std::numeric_limits<double>::infinity());

    // 最终可用版本：带 uint16 voxel_types（用于bit7判细化）、带 Global mesh（用于3x子体素判障）
    static DistanceResult CalculateEx(
        const VoxelSet& component_A,
        const VoxelSet& component_B,
        const VoxelSet& obstacles_pure,   // 仍然用于 coarse A*
        int xsize, int ysize, int zsize,
        double threshold_voxel_units,
        const uint16_t* voxel_types_u16,  // size = xsize*ysize*zsize，bit7来自这里（按你实际编码）
        const TriangleMesh* sceneMesh,    // Global mesh，允许为 nullptr
        const TriangleMesh* MeshA,
        const TriangleMesh* MeshB,
        const Vector3& offset_vec,
        float voxel_size,
        int g_refine_scale,              // 子体素优化的细化程度
        int threshold_length,           
        int refine_dilate_rings );      // 细化区域：零件外扩圈数（1~2）    

    static DistanceResult CalculateEx(
        const VoxelSet& component_A,
        const VoxelSet& component_B,
        const VoxelSet& obstacles_pure,
        int xsize, int ysize, int zsize,
        double threshold_voxel_units,
        const uint16_t* voxel_types_u16,
        const TriangleMesh* sceneMesh,
        const TriangleMesh* MeshA,
        const TriangleMesh* MeshB,
        const Vector3& offset_vec,
        float voxel_size,
        int g_refine_scale,
        int threshold_length,
        int refine_dilate_rings,
        bool use_search_aabb,
        int search_minx,
        int search_miny,
        int search_minz,
        int search_maxx,
        int search_maxy,
        int search_maxz);

private:
    struct SearchNode {
        Voxel voxel;
        double g_cost;
        double f_cost;
        Voxel parent;
        bool from_A;  // 标记是从A侧还是B侧扩展的

        bool operator>(const SearchNode& other) const {
            return f_cost > other.f_cost;
        }
    };

    static VoxelPath ReconstructPath(
        const std::unordered_map<Voxel, Voxel, Voxel::Hash>& parents_A,
        const std::unordered_map<Voxel, Voxel, Voxel::Hash>& parents_B,
        const Voxel& meeting_point);
};

// ============================================
// 爬行距离计算器
// ============================================

class CreepDistanceCalculator {
public:
    static DistanceResult Calculate(
        const VoxelSet& component_A,
        const VoxelSet& component_B,
        const VoxelSet& obstacles,
        int xsize, int ysize, int zsize,
        double threshold = std::numeric_limits<double>::infinity());

private:
    struct SearchNode {
        Voxel voxel;
        double cost;
        Voxel parent;

        bool operator>(const SearchNode& other) const {
            return cost > other.cost;
        }
    };

    static VoxelPath ReconstructPath(
        const std::unordered_map<Voxel, Voxel, Voxel::Hash>& parents_A,
        const std::unordered_map<Voxel, Voxel, Voxel::Hash>& parents_B,
        const Voxel& meeting_point);
};

// ============================================
// 快速估算器
// ============================================

class FastEstimator {
public:
    static double EstimateDistance(
        const VoxelSet& component_A,
        const VoxelSet& component_B);

private:
    static Voxel FindClosestVoxel(const Voxel& seed, const VoxelSet& targets);
};


// 路径优化工具
struct PathUtils {
    // 简单线段碰撞检测：两体素之间直线是否穿过障碍物
    static bool IsLineCollisionFree(
        const Voxel& a,
        const Voxel& b,
        const VoxelSet& obstacles,
        int xsize, int ysize, int zsize,
        int samples = 50);

    // 比 strict 稍微宽松：允许“端点附近的少数采样点”在障碍里
    static bool PathUtils::IsLineCollisionFreeRelaxedForShortcut(
        const Voxel& a,
        const Voxel& b,
        const VoxelSet& obstacles,
        int xsize, int ysize, int zsize,
        int samples,
        int allow_end_samples);

    static bool PathUtils::IsLineCollisionFreeAllowEndVoxel(
        const Voxel& a,
        const Voxel& b,
        const VoxelSet& obstacles,
        int xsize, int ysize, int zsize,
        int samples);

    static   bool PathUtils::IsLineCollisionFreeStrict(
        const Voxel& a,
        const Voxel& b,
        const VoxelSet& obstacles,
        int xsize, int ysize, int zsize,
        int samples);

    static std::vector<Vector3> ReplanInTube_VisibilityGraph(
        const std::vector<Vector3>& tubePathWorld,   // 体素/MR 得到的几何路径（world）
        const TriangleMesh& sceneMesh,
        const TriangleMesh* meshA,
        const TriangleMesh* meshB,
        double voxel_size,
        double safe_epsilon,
        double tube_margin_voxels,    // 通道AABB外扩多少个体素
        int    max_nodes);

    // 对路径进行拉直（Shortcut），返回优化后的路径
    static std::vector<Voxel> ShortcutPath(
        const std::vector<Voxel>& path,
        const VoxelSet& obstacles,
        int xsize, int ysize, int zsize);

    static std::vector<Vector3> OptimizeWithEndpoints(
        std::vector<Vector3> path,
        const TriangleMesh& sceneMesh,
        const TriangleMesh& meshA,
        const TriangleMesh& meshB,
        double eps);

    static std::vector<Vector3> SnapNearObstaclesToSurface(
        const std::vector<Vector3>& inputPath,
        const TriangleMesh& sceneMesh,
        double voxel_size,
        double max_snap_dist_factor /* = 1.0 */);

    static void OptimizeEndpoint(
        Vector3& endpoint,
        const Vector3& neighbor,
        const TriangleMesh& meshEndpoint,
        const TriangleMesh& sceneMesh,
        double eps);

    static std::vector<Vector3> OptimizePathGlobal(
        const std::vector<Vector3>& path,
        const TriangleMesh& mesh,
        double eps);

    static std::vector<Vector3> ShortcutPathFinal(
        const std::vector<Vector3>& path,
        const VoxelSet& obstacles,
        int xsize, int ysize, int zsize);
    
    static std::vector<Vector3> PathUtils::ShortcutPathFinalGeom(
        const std::vector<Vector3>& worldPath,
        const TriangleMesh& sceneMesh,
        double safe_epsilon);

    static std::vector<Vector3> AttachCornersToMeshFeaturesInTube(
        std::vector<Vector3>& pathWorld,
        const TriangleMesh& sceneMesh,
        const TriangleMesh* meshA,
        const TriangleMesh* meshB,
        double voxel_size,
        std::vector<bool>& isAttached,   // 输出：每个点是否被吸附了
        double tube_margin_voxels,
        int* snap_count);

    static std::vector<Vector3> FinalContinuousShortcutGeom(
        const std::vector<Vector3>& inPath,
        const TriangleMesh& sceneMesh,
        double safe_epsilon);
};

std::vector<Vector3> ReplanInTube_VisibilityGraph(
    const std::vector<Vector3>& tubePathWorld,
    const TriangleMesh& sceneMesh,
    const TriangleMesh* meshA,
    const TriangleMesh* meshB,
    double voxel_size,
    double safe_epsilon,
    double tube_margin_voxels,
    int    max_nodes);

} // namespace VoxelDistance

struct TubeAABB_World {
    Vector3 bmin;
    Vector3 bmax;
};

static TubeAABB_World BuildTubeAABBFromWorldPath(
    const std::vector<Vector3>& pathWorld,
    double voxel_size,
    double margin_voxels)
{
    TubeAABB_World tube;
    if (pathWorld.empty()) {
        tube.bmin = Vector3(0,0,0);
        tube.bmax = Vector3(0,0,0);
        return tube;
    }

    Vector3 mn = pathWorld[0];
    Vector3 mx = pathWorld[0];
    for (const auto& p : pathWorld) {
        mn.setX(std::min(mn.x(), p.x()));
        mn.setY(std::min(mn.y(), p.y()));
        mn.setZ(std::min(mn.z(), p.z()));
        mx.setX(std::max(mx.x(), p.x()));
        mx.setY(std::max(mx.y(), p.y()));
        mx.setZ(std::max(mx.z(), p.z()));
    }

    const double m = margin_voxels * voxel_size;
    tube.bmin = Vector3(float(mn.x() - m), float(mn.y() - m), float(mn.z() - m));
    tube.bmax = Vector3(float(mx.x() + m), float(mx.y() + m), float(mx.z() + m));
    return tube;
}

static bool InTubeAABB_World(const TubeAABB_World& tube, const Vector3& p)
{
    return  p.x() >= tube.bmin.x() && p.x() <= tube.bmax.x() &&
            p.y() >= tube.bmin.y() && p.y() <= tube.bmax.y() &&
            p.z() >= tube.bmin.z() && p.z() <= tube.bmax.z();
}



#endif // VOXEL_DISTANCE_CALCULATOR_H