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

#ifndef BUSBAR_SEGMENT_HPP
#define BUSBAR_SEGMENT_HPP

#include "point.hpp"
#include "vector3.hpp"
#include "busbar_types.hpp"
#include <vector>
#include <memory>

constexpr float kBendLengthCostFactor = 1.25f;

struct OccupiedVoxelInfo {
    Point point;
    Vector3 tangent;
    Vector3 normal;
    int j_layer = 0;  // cross-section position in normal direction (lower_t = bottom)
};

// ============================================================================
// BusbarSegment 基类
// ============================================================================
class BusbarSegment {
protected:
    SegmentType type_;
    Point start_pos_;
    Vector3 start_normal_;
    Point end_pos_;
    Vector3 end_normal_;
    float width_;
    float thickness_;

public:
    BusbarSegment(SegmentType type,
        const Point& start, const Vector3& start_normal,
        const Point& end, const Vector3& end_normal,
        float width, float thickness)
        : type_(type), start_pos_(start), start_normal_(start_normal),
        end_pos_(end), end_normal_(end_normal),
        width_(width), thickness_(thickness) {}

    virtual ~BusbarSegment() = default;

    // 纯虚函数
    virtual std::vector<OccupiedVoxelInfo> getOccupiedVoxels() const = 0;
    virtual float getLength() const = 0;

    // 弯段返回角度，直段返回0
    virtual float getBendAngle() const { return 0.0f; }

    virtual float getCost() const { return getLength(); }
    virtual Vector3 getStartTangent() const = 0;
    virtual Vector3 getEndTangent() const = 0;

    // 访问器
    SegmentType type() const { return type_; }
    Point startPos() const { return start_pos_; }
    Point endPos() const { return end_pos_; }
    Vector3 startNormal() const { return start_normal_; }
    Vector3 endNormal() const { return end_normal_; }
    float width() const { return width_; }
    float thickness() const { return thickness_; }
};

// ============================================================================
// StraightSegment (直段)
// ============================================================================
class StraightSegment : public BusbarSegment {
private:
    Vector3 tangent_;  // 存储切向，避免从坐标计算导致的精度问题

public:
    StraightSegment(const Point& start, const Vector3& start_normal,
        const Point& end, const Vector3& end_normal,
        float width, float thickness)
        : BusbarSegment(SegmentType::STRAIGHT, start, start_normal,
            end, end_normal, width, thickness),
        tangent_((end.toVector3() - start.toVector3()).normalized()) {}

    // 带切向的构造函数
    StraightSegment(const Point& start, const Vector3& start_normal,
        const Point& end, const Vector3& end_normal,
        float width, float thickness,
        const Vector3& tangent)
        : BusbarSegment(SegmentType::STRAIGHT, start, start_normal,
            end, end_normal, width, thickness),
        tangent_(tangent) {}

    std::vector<OccupiedVoxelInfo> getOccupiedVoxels() const override;
    float getLength() const override;
    Vector3 getStartTangent() const override { return tangent_; }
    Vector3 getEndTangent() const override { return tangent_; }
    Vector3 tangent() const { return tangent_; }
};

// ============================================================================
// FlatBendSegment (平弯段)
// - 从水平方向弯向垂直方向(或反向)
// - 固定90度弯曲
// - 弯曲半径 = f * H
// - 法向改变90度
// - 会导致Z轴变化
// ============================================================================
class FlatBendSegment : public BusbarSegment {
private:
    Point center_;     // 圆心位置
    float radius_;     // 弯曲半径
    bool upward_;      // true=向上弯, false=向下弯

public:
    FlatBendSegment(const Point& start, const Vector3& start_normal,
        const Point& end, const Vector3& end_normal,
        float width, float thickness,
        const Point& center, float radius, bool upward)
        : BusbarSegment(SegmentType::FLAT_BEND, start, start_normal,
            end, end_normal, width, thickness),
        center_(center), radius_(radius), upward_(upward) {}

    std::vector<OccupiedVoxelInfo> getOccupiedVoxels() const override;
    float getCost() const override { return getLength() * kBendLengthCostFactor; }
    float getLength() const override { return radius_ * M_PI / 2.0f; }  // 90度弧长
    Vector3 getStartTangent() const override;
    Vector3 getEndTangent() const override;
    float getBendAngle() const override { return M_PI / 2.0f; }
    Point center() const { return center_; }
    float radius() const { return radius_; }
    bool isUpward() const { return upward_; }
};

// ============================================================================
// VerticalBendSegment (立弯段)
// - 在水平面内弯曲
// - Z轴不变(或变化很小)
// - 弯曲半径 = v * W
// - 法向保持不变(始终垂直)
// - 类似贴在地上的圆弧
// ============================================================================
class VerticalBendSegment : public BusbarSegment {
private:
    Point center_;     // 圆心位置，Z坐标与起点相同
    float radius_;     // 弯曲半径
    float angle_;      // 弯曲角度(弧度)，可以是各种角度

public:
    VerticalBendSegment(const Point& start, const Vector3& start_normal,
        const Point& end, const Vector3& end_normal,
        float width, float thickness,
        const Point& center, float radius, float angle)
        : BusbarSegment(SegmentType::VERTICAL_BEND, start, start_normal,
            end, end_normal, width, thickness),
        center_(center), radius_(radius), angle_(angle) {}

    std::vector<OccupiedVoxelInfo> getOccupiedVoxels() const override;
    float getCost() const override { return getLength() * kBendLengthCostFactor; }
    float getLength() const override { return radius_ * std::abs(angle_); }
    Vector3 getStartTangent() const override;
    Vector3 getEndTangent() const override;
    float getBendAngle() const override { return angle_; }
    Point center() const { return center_; }
    float radius() const { return radius_; }
    float angle() const { return angle_; }
};

// ============================================================================
// TwistSegment (扭转段)
// - 沿长度方向扭转
// - 法向旋转90度
// - 扭转长度 = t * W
// ============================================================================
class TwistSegment : public BusbarSegment {
private:
    float twist_angle_;  // 扭转角度(弧度)，目前固定90度

public:
    TwistSegment(const Point& start, const Vector3& start_normal,
        const Point& end, const Vector3& end_normal,
        float width, float thickness,
        float twist_angle)
        : BusbarSegment(SegmentType::TWIST, start, start_normal,
            end, end_normal, width, thickness),
        twist_angle_(twist_angle) {}

    std::vector<OccupiedVoxelInfo> getOccupiedVoxels() const override;
    float getCost() const override { return getLength() * kBendLengthCostFactor; }
    float getLength() const override;
    Vector3 getStartTangent() const override;
    Vector3 getEndTangent() const override;

    float twistAngle() const { return twist_angle_; }
};

// ============================================================================
// Bend45Segment (折叠弯段)
// - 在水平面内转弯，Z轴有高度变化
// - 法向保持不变或旋转
// ============================================================================
class Bend45Segment : public BusbarSegment {
private:
    Point center_;           // 圆心在水平面的投影坐标
    float radius_;           // 弯曲半径 = b * W
    float bend_angle_;       // 水平转角(弧度)
    bool is_upward_;         // true = 向上偏移, false = 向下偏移
    float z_offset_;         // Z轴偏移量 = 2bH + H

public:
    Bend45Segment(const Point& start, const Vector3& start_normal,
        const Point& end, const Vector3& end_normal,
        float width, float thickness,
        const Point& center, float radius,
        float bend_angle, bool is_upward)
        : BusbarSegment(SegmentType::BEND_45, start, start_normal,
            end, end_normal, width, thickness),
        center_(center), radius_(radius),
        bend_angle_(bend_angle), is_upward_(is_upward){
        z_offset_ = (end.toVector3() - start.toVector3()).dot(start_normal);
    }

    std::vector<OccupiedVoxelInfo> getOccupiedVoxels() const override;
    float getCost() const override { return getLength() * kBendLengthCostFactor; }
    float getLength() const override {
        // 弧长 + Z轴偏移距离的估算
        return radius_ * std::abs(bend_angle_) + std::abs(z_offset_);
    }
    Vector3 getStartTangent() const override;
    Vector3 getEndTangent() const override;
    float getBendAngle() const override { return bend_angle_; }
    Point center() const { return center_; }
    float radius() const { return radius_; }
    bool isUpward() const { return is_upward_; }
};

#endif