FaZeRs · November 16, 2022 15:33 · Nov 16, 2022
diff --git a/main.cpp b/main.cpp
@@ -0,0 +1,257 @@
+#include <opencv2/opencv.hpp>
+
+#include <uc/logger.h>
+#include <uc/command_line.h>
+#include <uc/timer.h>
+#include <uc/math_common.h>
+#include <uc/dimensions.h>
+#include <uc/math_geometric.h>
+
+#include "draw_utils.h"
+
+using namespace dimensions::literals;
+
+std::vector<std::array<math::Vector3d, 4>> planes(const std::vector<math::Vector2d>& vector,
+                                                  const dimensions::Length& dist_to_ceiling,
+                                                  const dimensions::Length& dist_to_floor) {
+  std::vector<std::array<math::Vector3d, 4>> output_planes;
+  for (size_t i = 0; i < vector.size(); ++i) {
+    const auto& p0 = vector[i == 0 ? vector.size() - 1 : i - 1];
+    const auto& p1 = vector[i];
+    output_planes.emplace_back(std::array<math::Vector3d, 4>{{{p0.x, p0.y, dist_to_ceiling.m()},
+                                                              {p1.x, p1.y, dist_to_ceiling.m()},
+                                                              {p1.x, p1.y, dist_to_floor.m()},
+                                                              {p0.x, p0.y, dist_to_floor.m()}}});
+  }
+  return output_planes;
+}
+
+struct Portal {
+  Portal() = default;
+  Portal(const math::Vector2d& top_left, const math::Vector2d& bottom_right) :
+      top_left(top_left), bottom_right(bottom_right) {}
+  math::Vector2d top_left;
+  math::Vector2d bottom_right;
+
+  math::Vector2d layoutTopLeft(const math::Vector2d& layout_size) const {
+    return {top_left.x * layout_size.x, top_left.y * layout_size.y};
+  }
+  math::Vector2d layoutBottomRight(const math::Vector2d& layout_size) const {
+    return {bottom_right.x * layout_size.x, bottom_right.y * layout_size.y};
+  }
+
+  std::array<math::Vector3d, 4> plane(const std::vector<std::array<math::Vector3d, 4>>& planes,
+                                      const math::Vector2d& layout_size) {
+    const auto layout_tl = layoutTopLeft(layout_size);
+    const auto layout_br = layoutBottomRight(layout_size);
+    auto tl_plane = layoutPointOnPlane(layout_tl, planes, layout_size);
+    auto br_plane = layoutPointOnPlane(layout_br, planes, layout_size);
+    math::Vector3d tr_plane = {br_plane.x, br_plane.y, tl_plane.z};
+    math::Vector3d bl_plane = {tl_plane.x, tl_plane.y, br_plane.z};
+    return {tl_plane, tr_plane, br_plane, bl_plane};
+  }
+
+  math::Vector4d rect(const std::array<math::Vector3d, 4>& wall, const math::Vector2d& layout_size) {
+    const auto layout_tl = layoutTopLeft(layout_size);
+    const auto layout_br = layoutBottomRight(layout_size);
+    const auto plane = planeFromTriangle(wall[0], wall[1], wall[2]);
+    const auto plane_size = planeSizeFromTriangle(wall[0], wall[1], wall[2]);
+    const auto tl_plane = math::Vector2d(pix2PointOnPlane(plane, plane_size, layout_tl, layout_size));
+    const auto br_plane = math::Vector2d(pix2PointOnPlane(plane, plane_size, layout_br, layout_size));
+    return {tl_plane.x, tl_plane.y, fabs(br_plane.x - tl_plane.x), fabs(br_plane.y - tl_plane.y)};
+  }
+};
+
+math::Vector2d getRectOverlapOffset(const math::Vector4d& rect1, const math::Vector4d& rect2) {
+  // fl, fw, fr, ft, fh, fb: First.Left, Width, Right, Top, Height, Bottom
+  // s* - similar parameters for the second rectangle
+  const auto fl = rect1.x;
+  const auto fw = rect1.z;
+  const auto fr = fl + fw;
+  const auto ft = rect1.y;
+  const auto fb = ft + rect1.w;
+  const auto fh = rect1.w;
+
+  const auto sl = rect2.x;
+  const auto sw = rect2.z;
+  const auto sr = sl + sw;
+  const auto st = rect2.y;
+  const auto sb = st + rect2.w;
+  const auto sh = rect2.w;
+
+  const auto dcy = (sb + st) - (fb + ft); // doubled center y-difference
+  const auto dcx = (sl + sr) - (fl + fr);
+  if (std::abs(dcx) >= fw + sw || std::abs(dcy) >= fh + sh) { // no intersection
+    return {0, 0};
+  }
+
+  auto dx = fw + sw - std::abs(dcx); // doubled  needed x-shift absolute value
+  auto dy = fh + sh - std::abs(dcy);
+
+  if (dx > dy) {
+    if (dcy > 0) {
+      dy = -dy / 2;
+    } else {
+      dy = dy / 2; // needed y-shift accounting for direction
+    }
+    dx = 0;
+  } else {
+    if (dcy > 0) {
+      dx = -dx / 2;
+    } else {
+      dx = dx / 2;
+    }
+    dy = 0;
+  }
+
+  // Result: dx, dy pair to shift the second rectangle
+  return {dx, dy};
+}
+
+std::array<math::Vector2d, 4> rectToArray(const math::Vector4d& rect) {
+  return {{{rect.x, rect.y}, {rect.x + rect.z, rect.y}, {rect.x + rect.z, rect.y + rect.w}, {rect.x, rect.y + rect.w}}};
+}
+
+int main(int argc, char** argv) {
+  CMDLine cmd;
+
+  if (cmd.parse(argc, argv) == false) {
+    cmd.help();
+    return 0;
+  }
+
+  math::Vector2d layout_size{2000, 1000};
+
+  dimensions::Length ceiling = 1.0598699949175356_m;
+  dimensions::Length floor = -1.2196247597843446_m;
+
+  std::vector<math::Vector2d> contour = {{-1.2792625427246094, -1.5333192348480225},
+                                         {1.8199341297149658, -1.491775393486023},
+                                         {1.785354495048523, 1.7804408073425293},
+                                         {-1.3336228132247925, 1.73493230342865}};
+  const auto contour_planes = planes(contour, ceiling, floor);
+
+  Portal portal_one({0.10979843974529249, 0.39079923902773445}, {0.15709831606137092, 0.7091479793600469});
+  Portal portal_two({0.15201005025125622, 0.35050251256281406}, {0.20351758793969862, 0.7316167535108522});
+  Portal portal_three({0.12836970474967904, 0.3427471116816431}, {0.23876765083440324, 0.35686777920410784});
+
+  auto output = drawPanRoom(contour_planes, layout_size);
+
+  cv::imshow("Output", output);
+  cv::waitKey(0);
+
+  const auto portal_one_tl = portal_one.layoutTopLeft(layout_size);
+  const auto portal_one_br = portal_one.layoutBottomRight(layout_size);
+  const auto portal_two_tl = portal_two.layoutTopLeft(layout_size);
+  const auto portal_two_br = portal_two.layoutBottomRight(layout_size);
+  const auto portal_three_tl = portal_two.layoutTopLeft(layout_size);
+  const auto portal_three_br = portal_two.layoutBottomRight(layout_size);
+
+  std::array<math::Vector3d, 4> wall_plane;
+  bool portal_plane_found =
+      closestPlaneAtLayoutLine(wall_plane, portal_one_tl, portal_one_br, contour_planes, layout_size);
+  if (portal_plane_found) {
+    drawPanPlane(output, wall_plane, layout_size, {255, 0, 0});
+  }
+  cv::imshow("Output", output);
+  cv::waitKey(0);
+
+  cv::Scalar portal_one_color{0, 255, 0};
+  cv::Scalar portal_two_color{127, 127, 0};
+  cv::Scalar portal_three_color{127, 127, 200};
+
+  const auto portal_one_plane = portal_one.plane(contour_planes, layout_size);
+  const auto portal_two_plane = portal_two.plane(contour_planes, layout_size);
+  const auto portal_three_plane = portal_three.plane(contour_planes, layout_size);
+  drawPanPlane(output, portal_one_plane, layout_size, portal_one_color);
+  drawPanPlane(output, portal_two_plane, layout_size, portal_two_color);
+  drawPanPlane(output, portal_three_plane, layout_size, portal_three_color);
+  cv::imshow("Output", output);
+  cv::waitKey(0);
+
+  cv::Mat plane_output(1000, 1000, CV_8UC3);
+  plane_output = cv::Scalar(255, 255, 255);
+
+  const auto portal_one_rect = portal_one.rect(wall_plane, layout_size);
+  const auto portal_two_rect = portal_two.rect(wall_plane, layout_size);
+  const auto portal_three_rect = portal_three.rect(wall_plane, layout_size);
+
+  drawRect(plane_output, portal_one_rect, portal_one_color, 300);
+  drawRect(plane_output, portal_two_rect, portal_two_color, 300);
+  drawRect(plane_output, portal_three_rect, portal_three_color, 300);
+
+  cv::imshow("Plane Output", plane_output);
+  cv::waitKey(0);
+
+  const auto portal_one_array = rectToArray(portal_one_rect);
+  const auto plane = planeFromTriangle(wall_plane[0], wall_plane[1], wall_plane[2]);
+
+  math::Vector4d portal_two_new_rect;
+  bool portals_two_overlap = isRectangleIntersection(portal_one_rect, portal_two_rect);
+  if (portals_two_overlap) {
+    const auto two_overlap_offset = getRectOverlapOffset(portal_one_rect, portal_two_rect);
+    const auto portal_two_array = rectToArray(portal_two_rect);
+    bool change_tl = true;
+    portal_two_new_rect.z = portal_two_rect.z - two_overlap_offset.x;
+    portal_two_new_rect.w = portal_two_rect.w - two_overlap_offset.y;
+    portal_two_new_rect.x = change_tl ? portal_two_rect.x + two_overlap_offset.x : portal_two_rect.x;
+    portal_two_new_rect.y = change_tl ? portal_two_rect.y + two_overlap_offset.y : portal_two_rect.y;
+
+    drawRect(plane_output, portal_two_new_rect, {0, 0, 255}, 300, 1);
+
+    cv::imshow("Plane Output", plane_output);
+    cv::waitKey(0);
+
+    const auto portal_two_tl_new =
+        pointOnPlane2Pix(plane, {portal_two_new_rect.x, portal_two_new_rect.y, 0}, layout_size);
+    const auto portal_two_br_new = pointOnPlane2Pix(
+        plane, {portal_two_new_rect.x + portal_two_new_rect.z, portal_two_new_rect.y + portal_two_new_rect.w, 0},
+        layout_size);
+    Portal portal_two_new(portal_two_tl_new / layout_size, portal_two_br_new / layout_size);
+    const auto portal_two_new_plane = portal_two_new.plane(contour_planes, layout_size);
+    drawPanPlane(output, portal_two_new_plane, layout_size, {0, 0, 255}, 1);
+    cv::imshow("Output", output);
+    cv::waitKey(0);
+  }
+
+  math::Vector4d portal_three_new_rect;
+  bool portals_three_overlap = isRectangleIntersection(portal_one_rect, portal_three_rect);
+  if (portals_three_overlap) {
+    const auto three_overlap_offset = getRectOverlapOffset(portal_one_rect, portal_three_rect);
+    const auto portal_three_array = rectToArray(portal_three_rect);
+
+    bool change_tl = false;
+    const auto tl_dist_to_rect = pointToRectangleDistance(portal_three_array[0], portal_one_rect);
+    const auto tr_dist_to_rect = pointToRectangleDistance(portal_three_array[1], portal_one_rect);
+    const auto br_dist_to_rect = pointToRectangleDistance(portal_three_array[2], portal_one_rect);
+    const auto bl_dist_to_rect = pointToRectangleDistance(portal_three_array[3], portal_one_rect);
+
+    if (tl_dist_to_rect < br_dist_to_rect || bl_dist_to_rect > tr_dist_to_rect) {
+      change_tl = true;
+    }
+
+    portal_three_new_rect.z = portal_three_rect.z - three_overlap_offset.x;
+    portal_three_new_rect.w = portal_three_rect.w - three_overlap_offset.y;
+    portal_three_new_rect.x = change_tl ? portal_three_rect.x + three_overlap_offset.x : portal_three_rect.x;
+    portal_three_new_rect.y = change_tl ? portal_three_rect.y + three_overlap_offset.y : portal_three_rect.y;
+
+    drawRect(plane_output, portal_three_new_rect, {0, 0, 255}, 300, 1);
+
+    cv::imshow("Plane Output", plane_output);
+    cv::waitKey(0);
+
+    const auto portal_three_tl_new =
+        pointOnPlane2Pix(plane, {portal_three_new_rect.x, portal_three_new_rect.y, 0}, layout_size);
+    const auto portal_three_br_new = pointOnPlane2Pix(
+        plane,
+        {portal_three_new_rect.x + portal_three_new_rect.z, portal_three_new_rect.y + portal_three_new_rect.w, 0},
+        layout_size);
+    Portal portal_three_new(portal_three_tl_new / layout_size, portal_three_br_new / layout_size);
+    const auto portal_three_new_plane = portal_three_new.plane(contour_planes, layout_size);
+    drawPanPlane(output, portal_three_new_plane, layout_size, {0, 0, 255}, 1);
+    cv::imshow("Output", output);
+    cv::waitKey(0);
+  }
+
+  return 0;
+}
No results found