#pragma once #include #include #include #include #include struct SafetyPoint3D { double x = 0.0; double y = 0.0; double z = 0.0; }; struct SafetyAabb { SafetyPoint3D minimum; SafetyPoint3D maximum; bool isValid() const; SafetyAabb inflated(double clearanceMm) const; bool contains(const SafetyPoint3D& point, double tolerance = 1e-9) const; }; struct SafetyPathResult { bool valid = false; bool obstacleDetected = false; bool pathBlocked = false; bool avoidanceRequired = false; QString statusCode; QString message; double safeZMm = -1.0; SafetyAabb obstacle; SafetyAabb safeBox; QVector waypoints; }; class ObstaclePlanner { public: static bool detectRedObstacle(const QImage& image, QRect& pixelBox, double minimumAreaPx = 500.0); static SafetyAabb pixelBoxToKnownCuboid( const QRect& pixelBox, const QSize& imageSize, double workspaceXMinMm, double workspaceXMaxMm, double workspaceYMinMm, double workspaceYMaxMm, double obstacleSizeXmm, double obstacleSizeYmm, double obstacleSizeZmm, double tableZmm = 0.0); static bool segmentIntersectsAabb(const SafetyPoint3D& start, const SafetyPoint3D& goal, const SafetyAabb& box); static SafetyPathResult planLiftOverPath( const SafetyPoint3D& start, const SafetyPoint3D& goal, const SafetyAabb* obstacle, double clearanceMm = 50.0, double overheadMm = 80.0, double maxWorkspaceZmm = 800.0); static QImage createDemoScene( bool obstaclePresent, double obstacleCenterXmm, double obstacleCenterYmm, double workspaceXMinMm, double workspaceXMaxMm, double workspaceYMinMm, double workspaceYMaxMm, double obstacleSizeXmm, double obstacleSizeYmm, const QSize& imageSize = QSize(960, 600)); static bool selfTest(QString* report = nullptr); };