Created
November 16, 2022 15:33
-
-
Save FaZeRs/089ffde49f7e8d5d1492d5509b3f05a2 to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #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; | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment