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.
191 lines
7.5 KiB
191 lines
7.5 KiB
#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
|
|
|