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.
105 lines
3.8 KiB
105 lines
3.8 KiB
#ifndef SPACE_ANALYZER_HPP
|
|
#define SPACE_ANALYZER_HPP
|
|
|
|
#include "busbar_config.hpp"
|
|
#include <vector>
|
|
#include <cstdint>
|
|
#include <cmath>
|
|
#include <unordered_set>
|
|
|
|
class SpaceAnalyzer {
|
|
private:
|
|
const BusbarConfig& config_;
|
|
const std::vector<bool>& obstacles_; // 原有障碍物 (flag=0)
|
|
const std::vector<bool>& collision_voxels_; // 会碰撞的空闲体素 (flag=1)
|
|
uint64_t xsize_, ysize_, zsize_;
|
|
|
|
// 预计算的检测半径，避免每次hasSpace调用时重复计算
|
|
int half_w_;
|
|
int half_t_;
|
|
|
|
inline size_t getIndex(int64_t x, int64_t y, int64_t z) const {
|
|
return static_cast<size_t>(x + y * xsize_ + z * xsize_ * ysize_);
|
|
}
|
|
|
|
public:
|
|
// 新构造函数：支持两种类型的体素
|
|
SpaceAnalyzer(const BusbarConfig& config,
|
|
const std::vector<bool>& obstacles,
|
|
const std::vector<bool>& collision_voxels,
|
|
uint64_t xsize, uint64_t ysize, uint64_t zsize)
|
|
: config_(config), obstacles_(obstacles), collision_voxels_(collision_voxels),
|
|
xsize_(xsize), ysize_(ysize), zsize_(zsize),
|
|
half_w_(static_cast<int>(std::ceil(config.clearance()))),
|
|
half_t_(static_cast<int>(std::ceil(config.clearance()))) {}
|
|
|
|
// 内联热点函数：检查坐标是否在边界内
|
|
inline bool isInBounds(int64_t x, int64_t y, int64_t z) const {
|
|
return x >= 0 && y >= 0 && z >= 0 &&
|
|
static_cast<uint64_t>(x) < xsize_ &&
|
|
static_cast<uint64_t>(y) < ysize_ &&
|
|
static_cast<uint64_t>(z) < zsize_;
|
|
}
|
|
|
|
inline bool isInBounds(const Point& p) const {
|
|
return isInBounds(p.x(), p.y(), p.z());
|
|
}
|
|
|
|
// 内联热点函数：检查体素是否为原有障碍物 (flag=0)
|
|
inline bool isObstacle(int64_t x, int64_t y, int64_t z) const {
|
|
if (!isInBounds(x, y, z)) return true;
|
|
return obstacles_[getIndex(x, y, z)];
|
|
}
|
|
|
|
inline bool isObstacle(const Point& p) const {
|
|
return isObstacle(p.x(), p.y(), p.z());
|
|
}
|
|
|
|
// 内联热点函数：检查体素是否畅通（既不是障碍物，也不是会碰撞的空闲体素）
|
|
inline bool isVoxelFree(const Point& p) const {
|
|
if (!isInBounds(p)) return false;
|
|
size_t idx = getIndex(p.x(), p.y(), p.z());
|
|
return !obstacles_[idx] && !collision_voxels_[idx];
|
|
}
|
|
|
|
// 检查体素是否会碰撞（包括障碍物和会碰撞的空闲体素）
|
|
inline bool isCollision(int64_t x, int64_t y, int64_t z) const {
|
|
if (!isInBounds(x, y, z)) return true;
|
|
size_t idx = getIndex(x, y, z);
|
|
return obstacles_[idx] || collision_voxels_[idx];
|
|
}
|
|
|
|
inline bool isCollision(const Point& p) const {
|
|
return isCollision(p.x(), p.y(), p.z());
|
|
}
|
|
|
|
// 检查体素是否为防护间隙体素（flag=1，不含障碍物）
|
|
inline bool isCollisionVoxel(const Point& p) const {
|
|
if (!isInBounds(p)) return false;
|
|
size_t idx = getIndex(p.x(), p.y(), p.z());
|
|
return collision_voxels_[idx];
|
|
}
|
|
|
|
// 检测参数
|
|
bool isEndpointValid(const Point& p) const;
|
|
|
|
// 检查某个点在给定姿态下是否有足够空间
|
|
bool hasSpace(const Point& p, const Vector3& tangent,
|
|
const Vector3& normal) const;
|
|
|
|
// 优化版：检查 clearance 外壳，使用去重集合避免重复检查
|
|
// 返回 false 如果有碰撞，true 如果通过
|
|
bool checkClearanceShell(const Point& p, const Vector3& tangent,
|
|
const Vector3& normal, std::unordered_set<int64_t>& checked) const;
|
|
|
|
// 获取地图尺寸
|
|
uint64_t xsize() const { return xsize_; }
|
|
uint64_t ysize() const { return ysize_; }
|
|
uint64_t zsize() const { return zsize_; }
|
|
|
|
// 获取预计算的检测半径
|
|
int halfWidth() const { return half_w_; }
|
|
int halfThickness() const { return half_t_; }
|
|
};
|
|
|
|
#endif
|