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

#ifndef SEGMENT_GENERATOR_HPP
#define SEGMENT_GENERATOR_HPP

#include "busbar_segment.hpp"
#include "busbar_config.hpp"
#include "space_analyzer.hpp"
#include <memory>
#include <unordered_set>
#include <unordered_map>
#include <queue>
#include <mutex>
#include <cstdint>

// 性能统计函数
void printGeneratorStats();
void resetGeneratorStats();

// 段候选
struct SegmentCandidate {
    SegmentEndState end_state;
    std::shared_ptr<BusbarSegment> segment;
    float cost;

    SegmentCandidate(const SegmentEndState& end,
        std::shared_ptr<BusbarSegment> seg,
        float c)
        : end_state(end), segment(seg), cost(c) {}
};

// 段生成器
class SegmentGenerator {
private:
    struct PrimitiveTemplateKey {
        int type = 0;
        int w10 = 0;
        int h10 = 0;

        int dx = 0, dy = 0, dz = 0;
        int nx = 0, ny = 0, nz = 0;
        int enx = 0, eny = 0, enz = 0;
        int tx = 0, ty = 0, tz = 0;

        int cdx = 0, cdy = 0, cdz = 0;
        int aux0 = 0;  // angle_millirad / bool flag
        int aux1 = 0;  // extra flag

        bool operator==(const PrimitiveTemplateKey& other) const {
            return type == other.type &&
                w10 == other.w10 && h10 == other.h10 &&
                dx == other.dx && dy == other.dy && dz == other.dz &&
                nx == other.nx && ny == other.ny && nz == other.nz &&
                enx == other.enx && eny == other.eny && enz == other.enz &&
                tx == other.tx && ty == other.ty && tz == other.tz &&
                cdx == other.cdx && cdy == other.cdy && cdz == other.cdz &&
                aux0 == other.aux0 && aux1 == other.aux1;
        }
    };

    struct PrimitiveTemplateKeyHasher {
        size_t operator()(const PrimitiveTemplateKey& k) const {
            size_t seed = std::hash<int>()(k.type);
            auto combine = [&seed](int v) {
                seed ^= std::hash<int>()(v) + 0x9e3779b97f4a7c15ULL + (seed << 6) + (seed >> 2);
            };
            combine(k.w10); combine(k.h10);
            combine(k.dx); combine(k.dy); combine(k.dz);
            combine(k.nx); combine(k.ny); combine(k.nz);
            combine(k.enx); combine(k.eny); combine(k.enz);
            combine(k.tx); combine(k.ty); combine(k.tz);
            combine(k.cdx); combine(k.cdy); combine(k.cdz);
            combine(k.aux0); combine(k.aux1);
            return seed;
        }
    };

    struct PrimitiveTemplateData {
        std::vector<Point> offsets;         // full precise template
        std::vector<Point> sparse_offsets;  // coarse precheck template
        Point min_offset;                   // AABB min (relative to segment start)
        Point max_offset;                   // AABB max (relative to segment start)
    };

    const BusbarConfig& config_;
    const SpaceAnalyzer& analyzer_;
    Vector3 start_platform_normal_;
    Point start_platform_point_;
    Vector3 goal_platform_normal_;
    Point goal_platform_point_;

    // ===== 新增：平台体素集合 =====
    std::unordered_set<int64_t> start_platform_voxels_;
    std::unordered_set<int64_t> goal_platform_voxels_;
    // 外扩 clearance 范围的平台体素集合（用于快速判断是否靠近平台）
    std::unordered_set<int64_t> start_platform_expanded_;
    std::unordered_set<int64_t> goal_platform_expanded_;
    int64_t start_platform_level_;
    int64_t goal_platform_level_;
    bool platforms_initialized_;

    mutable std::unordered_map<PrimitiveTemplateKey, std::shared_ptr<const PrimitiveTemplateData>,
        PrimitiveTemplateKeyHasher> primitive_template_cache_;
    mutable std::mutex primitive_template_cache_mutex_;

    // Collision hierarchy cache (built once after platforms are initialized)
    // 0=free, 1=obstacle, 2=clearance-blocked(not near platform)
    mutable std::vector<uint8_t> blocked_type_grid_;
    // Lower-bound clearance distance in voxels (26-neighbor chessboard metric, truncated).
    mutable std::vector<uint16_t> clearance_lb_grid_;
    // Coarse block occupancy prefix-sum (for fast empty-AABB acceptance).
    mutable std::vector<uint32_t> blocked_block_prefix_;
    mutable int64_t blocked_block_x_ = 0;
    mutable int64_t blocked_block_y_ = 0;
    mutable int64_t blocked_block_z_ = 0;
    mutable bool collision_hierarchy_ready_ = false;
    mutable std::mutex collision_hierarchy_mutex_;
    static constexpr int kCollisionBlockSize = 8;

    // 体素索引计算（空间哈希）
    int64_t voxelToIndex(const Point& p) const {
        return p.x() + p.y() * 100000LL + p.z() * 10000000000LL;
    }

    // BFS计算平台范围
    void computePlatformVoxels(Point& platform_point, const Vector3& normal,
                               std::unordered_set<int64_t>& platform_set,
                               int64_t& detected_level);


    // 生成各类段
    std::vector<SegmentCandidate> generateStraightCandidates(
        const SegmentEndState& current) const;

    std::vector<SegmentCandidate> generateFlatBendCandidates(
        const SegmentEndState& current) const;

    std::vector<SegmentCandidate> generateVerticalBendCandidates(
        const SegmentEndState& current) const;

    std::vector<SegmentCandidate> generateTwistCandidates(
        const SegmentEndState& current) const;

    std::vector<SegmentCandidate> generateBend45Candidates(
        const SegmentEndState& current) const;

    PrimitiveTemplateKey buildPrimitiveTemplateKey(const BusbarSegment& segment) const;
    std::shared_ptr<const PrimitiveTemplateData> getPrimitiveTemplateData(
        const BusbarSegment& segment) const;
    void ensureCollisionHierarchyBuilt() const;

public:
    // 检测段是否有效（碰撞检测）
    bool isSegmentValid(const BusbarSegment& segment) const;

    // ===== 修改：isNearPlatform增加is_start_platform参数 =====
    bool isNearPlatform(const Point& voxel, const Point& platform_point,
                        const Vector3& platform_normal, bool is_start_platform) const;

    // ===== 新增：初始化平台（必须在使用前调用）=====
    void initializePlatforms();

    // 获取平台体素数量
    size_t getStartPlatformSize() const { return start_platform_voxels_.size(); }
    size_t getGoalPlatformSize() const { return goal_platform_voxels_.size(); }

    // 获取检测到的平台表面点和高度
    Point getStartPlatformPoint() const { return start_platform_point_; }
    Point getGoalPlatformPoint() const { return goal_platform_point_; }
    int64_t getStartPlatformLevel() const { return start_platform_level_; }
    int64_t getGoalPlatformLevel() const { return goal_platform_level_; }

    SegmentGenerator(const BusbarConfig& config,
        const SpaceAnalyzer& analyzer,
        const Point& start_platform_point,
        const Vector3& start_platform_normal,
        const Point& goal_platform_point,
        const Vector3& goal_platform_normal)
        : config_(config), analyzer_(analyzer),
        start_platform_point_(start_platform_point),
        start_platform_normal_(start_platform_normal),
        goal_platform_point_(goal_platform_point),
        goal_platform_normal_(goal_platform_normal),
        start_platform_level_(0),
        goal_platform_level_(0),
        platforms_initialized_(false) {}

    // 主接口：生成所有可行的后继段
    std::vector<SegmentCandidate> generateSuccessors(
        const SegmentEndState& current) const;
};

#endif