diff --git a/CMakeLists.txt b/CMakeLists.txt index 0fa1f06bbcd83e28cc6b1d0c7a8a256eee7c06e4..6bec6ea439fcf7903d5425bf346dd9e905466492 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,6 +8,7 @@ target_sources(csg PRIVATE src/csg/impl/levelset_3d.cpp src/csg/impl/levelset_2d.cpp src/csg/impl/csg.cpp + src/csg/impl/matrix_functions.cpp ) target_include_directories(csg PRIVATE diff --git a/src/csg/impl/csg_types.hpp b/src/csg/impl/csg_types.hpp index e804c104713f3e585eb404a8d07cfc5eb9531ee0..c1725fe98ec5318eeb5af4e1f7d56bba6293472b 100644 --- a/src/csg/impl/csg_types.hpp +++ b/src/csg/impl/csg_types.hpp @@ -1,6 +1,8 @@ #ifndef CSG_TYPES_H_ #define CSG_TYPES_H_ +#include "matrix_functions.hpp" + #include #include #include @@ -86,15 +88,47 @@ template struct Difference { }; template <> struct Mulmatrix { - std::array, 3> rotation; +private: + matrix::Mat3d m_rotation; + matrix::Mat3d m_rotation_inv; + +public: std::array translation; Union group; + + Mulmatrix(const std::array &rot_row0, + const std::array &rot_row1, + const std::array &rot_row2) { + m_rotation[0] = rot_row0; + m_rotation[1] = rot_row1; + m_rotation[2] = rot_row2; + m_rotation_inv = matrix::inverse(m_rotation); + } + + const matrix::Mat3d &rotation() const { return m_rotation; } + + const matrix::Mat3d &rotation_inv() const { return m_rotation_inv; } }; template <> struct Mulmatrix { - std::array, 2> rotation; +private: + matrix::Mat2d m_rotation; + matrix::Mat2d m_rotation_inv; + +public: std::array translation; Union group; + + Mulmatrix(const std::array &rot_row0, + const std::array &rot_row1) { + m_rotation[0] = rot_row0; + m_rotation[1] = rot_row1; + m_rotation_inv = matrix::inverse(m_rotation); + } + + const matrix::Mat2d &rotation() const { return m_rotation; } + + const matrix::Mat2d &rotation_inv() const { return m_rotation_inv; } }; struct LinearExtrude { diff --git a/src/csg/impl/levelset_2d.cpp b/src/csg/impl/levelset_2d.cpp index 32f593c76803522cf50c8bf1112a786b242f8d1d..3eff4182f47d7b40e8a62fee470264a365895aa9 100644 --- a/src/csg/impl/levelset_2d.cpp +++ b/src/csg/impl/levelset_2d.cpp @@ -55,12 +55,11 @@ double signed_distance_2d(const Difference2D &group, double xx, double yy) { } double signed_distance_2d(const Mulmatrix2D &mm, double xx, double yy) { - // TODO: Invert non-orthogonal matrices auto XX = xx - mm.translation[0]; auto YY = yy - mm.translation[1]; - return signed_distance_2d(mm.group, - mm.rotation[0][0] * XX + mm.rotation[1][0] * YY, - mm.rotation[0][1] * XX + mm.rotation[1][1] * YY); + auto ri = mm.rotation_inv(); + return signed_distance_2d(mm.group, ri[0][0] * XX + ri[0][1] * YY, + ri[1][0] * XX + ri[1][1] * YY); } double signed_distance_2d(const Square &sq, double xx, double yy) { diff --git a/src/csg/impl/levelset_3d.cpp b/src/csg/impl/levelset_3d.cpp index e3498d3ecf3d973c9455a05c9dfd9c5a9f9353a6..32161382828be434ef26c10eba42d5b325bfe140 100644 --- a/src/csg/impl/levelset_3d.cpp +++ b/src/csg/impl/levelset_3d.cpp @@ -64,15 +64,14 @@ double signed_distance_3d(const Difference3D &group, double xx, double yy, double signed_distance_3d(const Mulmatrix3D &mm, double xx, double yy, double zz) { - // TODO: Invert non-orthogonal matrices auto XX = xx - mm.translation[0]; auto YY = yy - mm.translation[1]; auto ZZ = zz - mm.translation[2]; - return signed_distance_3d( - mm.group, - mm.rotation[0][0] * XX + mm.rotation[1][0] * YY + mm.rotation[2][0] * ZZ, - mm.rotation[0][1] * XX + mm.rotation[1][1] * YY + mm.rotation[2][1] * ZZ, - mm.rotation[0][2] * XX + mm.rotation[1][2] * YY + mm.rotation[2][2] * ZZ); + auto ri = mm.rotation_inv(); + return signed_distance_3d(mm.group, + ri[0][0] * XX + ri[0][1] * YY + ri[0][2] * ZZ, + ri[1][0] * XX + ri[1][1] * YY + ri[1][2] * ZZ, + ri[2][0] * XX + ri[2][1] * YY + ri[2][2] * ZZ); } double signed_distance_3d(const Cone &cone, double xx, double yy, double zz) { diff --git a/src/csg/impl/matrix_functions.cpp b/src/csg/impl/matrix_functions.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4c0159d5d988a54290b85fe47e415b1f64ea4f9d --- /dev/null +++ b/src/csg/impl/matrix_functions.cpp @@ -0,0 +1,47 @@ +#include "matrix_functions.hpp" + +namespace { +double determinant(const matrix::Mat2d &m) { + return m[0][0] * m[1][1] - m[1][0] * m[1][1]; +} + +double determinant(const matrix::Mat3d &m) { + return m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]) + + m[0][1] * (m[1][2] * m[2][0] - m[2][2] * m[1][0]) + + m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]); +} + +} // namespace + +namespace matrix { + +Mat2d inverse(const Mat2d &m) { + Mat2d res; + double d = 1 / determinant(m); + + res[0][0] = d * m[1][1]; + res[0][1] = -1.0 * d * m[0][1]; + res[1][0] = -1.0 * d * m[1][0]; + res[1][1] = d * m[0][0]; + + return res; +} + +Mat3d inverse(const Mat3d &m) { + Mat3d res; + double d = 1 / determinant(m); + + res[0][0] = d * (m[1][1] * m[2][2] - m[2][1] * m[1][2]); + res[0][1] = d * (m[0][2] * m[2][1] - m[0][1] * m[2][2]); + res[0][2] = d * (m[0][1] * m[1][2] - m[0][2] * m[1][1]); + res[1][0] = d * (m[1][2] * m[2][0] - m[1][0] * m[2][2]); + res[1][1] = d * (m[0][0] * m[2][2] - m[0][2] * m[2][0]); + res[1][2] = d * (m[1][0] * m[0][2] - m[0][0] * m[1][2]); + res[2][0] = d * (m[1][0] * m[2][1] - m[2][0] * m[1][1]); + res[2][1] = d * (m[2][0] * m[0][1] - m[0][0] * m[2][1]); + res[2][2] = d * (m[0][0] * m[1][1] - m[1][0] * m[0][1]); + + return res; +} + +} // namespace matrix diff --git a/src/csg/impl/matrix_functions.hpp b/src/csg/impl/matrix_functions.hpp new file mode 100644 index 0000000000000000000000000000000000000000..b500cd49eacbfd98175d943aafbef33fef02454f --- /dev/null +++ b/src/csg/impl/matrix_functions.hpp @@ -0,0 +1,14 @@ +#ifndef MATRIX_FUNCTIONS_H_ +#define MATRIX_FUNCTIONS_H_ + +#include + +namespace matrix { +using Mat2d = std::array, 2>; +using Mat3d = std::array, 3>; + +Mat2d inverse(const Mat2d &m); +Mat3d inverse(const Mat3d &m); +} // namespace matrix + +#endif diff --git a/src/csg/impl/parser.cpp b/src/csg/impl/parser.cpp index d2454deb2f4732f6a2fb561a9b98615071bd30ec..22417da9231f6743a42ad16c5e8e56851a82d8e2 100644 --- a/src/csg/impl/parser.cpp +++ b/src/csg/impl/parser.cpp @@ -383,12 +383,9 @@ template <> struct action> { template static void apply(const Input &in, parser_state &st) { std::stringstream ss(in.string()); - auto mulmat = csg::Mulmatrix2D(); auto mat = st.current_matrices.back(); - // clang-format off - mulmat.rotation[0] = {mat[0][0], mat[0][1]}; - mulmat.rotation[1] = {mat[1][0], mat[1][1]}; - // clang-format on + auto mulmat = + csg::Mulmatrix2D({{mat[0][0], mat[0][1]}}, {{mat[1][0], mat[1][1]}}); mulmat.translation = { mat[0][3], @@ -407,13 +404,10 @@ template <> struct action> { template static void apply(const Input &in, parser_state &st) { std::stringstream ss(in.string()); - auto mulmat = csg::Mulmatrix3D(); auto mat = st.current_matrices.back(); - // clang-format off - mulmat.rotation[0] = {mat[0][0], mat[0][1], mat[0][2]}; - mulmat.rotation[1] = {mat[1][0], mat[1][1], mat[1][2]}; - mulmat.rotation[2] = {mat[2][0], mat[2][1], mat[2][2]}; - // clang-format on + auto mulmat = csg::Mulmatrix3D({{mat[0][0], mat[0][1], mat[0][2]}}, + {{mat[1][0], mat[1][1], mat[1][2]}}, + {{mat[2][0], mat[2][1], mat[2][2]}}); mulmat.translation = { mat[0][3], diff --git a/src/csg/meson.build b/src/csg/meson.build index 228b6e95480f9d6b4e8f2bb0a84bb76a4fca0fb1..f7081965816f3963ab816627beb90138dc138b3f 100644 --- a/src/csg/meson.build +++ b/src/csg/meson.build @@ -4,6 +4,7 @@ lib_csg_parser = static_library( 'impl/levelset_3d.cpp', 'impl/levelset_2d.cpp', 'impl/parser.cpp', + 'impl/matrix_functions.cpp', include_directories: tao_inc, install : true) diff --git a/src/csg/tests/levelset/extrude.t.cpp b/src/csg/tests/levelset/extrude.t.cpp index 11769ce0715ebc98f4d2f0130fe3693efc199e19..5938db2f533bfdf520a8bb6d547e91e23ee99f54 100644 --- a/src/csg/tests/levelset/extrude.t.cpp +++ b/src/csg/tests/levelset/extrude.t.cpp @@ -2,6 +2,7 @@ #include #include +#include namespace { @@ -11,9 +12,7 @@ TEST_CASE("two shape linear", "[Levelset Extrude]") { csg::Circle my_cir{.name = std::nullopt, .radius = 1}; my_lin_ext.group.objs.push_back(my_cir); - auto my_mat = csg::Mulmatrix2D(); - my_mat.rotation[0] = std::array{{1, 0}}; - my_mat.rotation[1] = std::array{{0, 1}}; + auto my_mat = csg::Mulmatrix2D({{1, 0}}, {{0, 1}}); my_mat.translation = {4, 0}; my_mat.group.objs.push_back(my_cir); my_lin_ext.group.objs.push_back(my_mat); @@ -41,9 +40,7 @@ TEST_CASE("simple torus", "[Levelset Extrude]") { auto my_rot_ext = csg::RotateExtrude(); csg::Circle my_cir{.name = std::nullopt, .radius = 1}; - auto my_mat = csg::Mulmatrix2D(); - my_mat.rotation[0] = std::array{{1, 0}}; - my_mat.rotation[1] = std::array{{0, 1}}; + auto my_mat = csg::Mulmatrix2D({{1, 0}}, {{0, 1}}); my_mat.translation = {2, 0}; my_mat.group.objs.push_back(my_cir); my_rot_ext.group.objs.push_back(my_mat); diff --git a/src/csg/tests/levelset/transform.t.cpp b/src/csg/tests/levelset/transform.t.cpp index af6c818b6b713655d3348094ca0588ad57cf1e2f..0123c6cd6d88134aab75f1444c5fb352a153b8e4 100644 --- a/src/csg/tests/levelset/transform.t.cpp +++ b/src/csg/tests/levelset/transform.t.cpp @@ -4,6 +4,7 @@ #include #include +#include namespace { @@ -12,10 +13,7 @@ TEST_CASE("Translation", "[Levelset Transform]") { csg::Cylinder my_cyl{ .name = std::nullopt, .radius = 2, .height = 20, .center = true}; - auto my_mat = csg::Mulmatrix3D(); - my_mat.rotation[0] = std::array{{1, 0, 0}}; - my_mat.rotation[1] = std::array{{0, 1, 0}}; - my_mat.rotation[2] = std::array{{0, 0, 1}}; + auto my_mat = csg::Mulmatrix3D({{1, 0, 0}}, {{0, 1, 0}}, {{0, 0, 1}}); my_mat.translation = {100, 200, 300}; my_mat.group.objs.push_back(my_cyl); @@ -48,10 +46,7 @@ TEST_CASE("90° Rotation around y-axis, rotating z-axis into x-axis", csg::Cylinder my_cyl{ .name = std::nullopt, .radius = 2, .height = 20, .center = false}; - auto my_mat = csg::Mulmatrix3D(); - my_mat.rotation[0] = std::array{{0, 0, 1}}; - my_mat.rotation[1] = std::array{{0, 1, 0}}; - my_mat.rotation[2] = std::array{{-1, 0, 0}}; + auto my_mat = csg::Mulmatrix3D({{0, 0, 1}}, {{0, 1, 0}}, {{-1, 0, 0}}); my_mat.translation = {0, 0, 0}; my_mat.group.objs.push_back(my_cyl); @@ -88,18 +83,14 @@ TEST_CASE("90° Rotation around y-axis, rotating z-axis into x-axis", } } -TEST_CASE("Orthogonal rotation + translation of cylinder", - "[Levelset Transform]") { +TEST_CASE("90° rotation + translation of cylinder", "[Levelset Transform]") { double radius = 4.5, height = 10; double Cx = 10, Cy = 5, Cz = 5; csg::Cylinder my_cyl{ .name = std::nullopt, .radius = radius, .height = height, .center = true}; - auto my_mat = csg::Mulmatrix3D(); - my_mat.rotation[0] = std::array{{0, 0, 1}}; - my_mat.rotation[1] = std::array{{0, 1, 0}}; - my_mat.rotation[2] = std::array{{-1, 0, 0}}; + auto my_mat = csg::Mulmatrix3D({{0, 0, 1}}, {{0, 1, 0}}, {{-1, 0, 0}}); my_mat.translation = {Cx, Cy, Cz}; my_mat.group.objs.push_back(my_cyl); @@ -140,4 +131,108 @@ TEST_CASE("Orthogonal rotation + translation of cylinder", } } +TEST_CASE("45° Rotation around y-axis", "[Levelset Transform]") { + + double side = 20; + csg::Cube my_cub{ + .name = std::nullopt, .size = {side, side, side}, .center = true}; + + auto my_mat = csg::Mulmatrix3D({{0.707107, 0, 0.707107}}, {{0, 1, 0}}, + {{-0.707107, 0, 0.707107}}); + my_mat.translation = {0, 0, 0}; + my_mat.group.objs.push_back(my_cub); + + auto my_tree = std::make_shared(); + my_tree->top.objs.push_back(my_mat); + csg::CsgIF my_levelset(my_tree); + + SECTION("along x axis") { + CHECK_FALSE(0 < my_levelset(-14.15, 0, 0)); + CHECK(0 < my_levelset(-14.13, 0, 0)); + + CHECK_FALSE(0 < my_levelset(14.15, 0, 0)); + CHECK(0 < my_levelset(14.13, 0, 0)); + + CHECK_FALSE(0 < my_levelset(14.13, 0, 1)); + CHECK(0 < my_levelset(12, 0, 1)); + + CHECK_FALSE(0 < my_levelset(-14.13, 0, 1)); + CHECK(0 < my_levelset(-12, 0, 1)); + + CHECK_FALSE(0 < my_levelset(-14.13, 0, -1)); + CHECK(0 < my_levelset(-12, 0, -1)); + + CHECK_FALSE(0 < my_levelset(14.15, 1, 0)); + CHECK(0 < my_levelset(14.13, 1, 0)); + } + SECTION("along y axis") { + CHECK_FALSE(0 < my_levelset(0, -10.01, 0)); + CHECK(0 < my_levelset(0, -9.99, 0)); + + CHECK_FALSE(0 < my_levelset(0, 10.01, 0)); + CHECK(0 < my_levelset(0, 9.99, 0)); + + CHECK_FALSE(0 < my_levelset(0, 10.01, 1)); + CHECK(0 < my_levelset(0, 9.99, 1)); + + CHECK_FALSE(0 < my_levelset(0, -10.01, -1)); + CHECK(0 < my_levelset(0, -9.99, -1)); + + CHECK_FALSE(0 < my_levelset(1, 10.01, 0)); + CHECK(0 < my_levelset(1, 9.99, 0)); + } + SECTION("along z axis") { + CHECK_FALSE(0 < my_levelset(0, 0, -14.15)); + CHECK(0 < my_levelset(0, 0, -14.13)); + + CHECK_FALSE(0 < my_levelset(0, 0, 14.15)); + CHECK(0 < my_levelset(0, 0, 14.13)); + + CHECK_FALSE(0 < my_levelset(0, 1, 14.15)); + CHECK(0 < my_levelset(0, 1, 14.13)); + + CHECK_FALSE(0 < my_levelset(0, -1, -14.15)); + CHECK(0 < my_levelset(0, -1, -14.13)); + + CHECK_FALSE(0 < my_levelset(1, 0, 14.13)); + CHECK(0 < my_levelset(1, 0, 12)); + } +} + +TEST_CASE("Skewed with shear y along z", "[Levelset Transform]") { + double radius = 10, height = 10; + double skew_yz = 0.7; + + csg::Cylinder my_cyl{.name = std::nullopt, + .radius = radius, + .height = height, + .center = false}; + + auto my_mat = csg::Mulmatrix3D({{1, 0, 0}}, {{0, 1, 0.7}}, {{0, 0, 1}}); + my_mat.translation = {0, 0, 0}; + my_mat.group.objs.push_back(my_cyl); + + auto my_tree = std::make_shared(); + my_tree->top.objs.push_back(my_mat); + csg::CsgIF my_levelset(my_tree); + + auto check = [radius, &my_levelset](double Cx, double Cy, double Cz) { + CHECK_FALSE(0 < my_levelset(Cx + radius * 1.01, Cy, Cz)); + CHECK(0 < my_levelset(Cx + radius * 0.99, Cy, Cz)); + + CHECK_FALSE(0 < my_levelset(Cx, Cy + 1.01 * radius, Cz)); + CHECK(0 < my_levelset(Cx, Cy + radius * 0.99, Cz)); + + CHECK_FALSE(0 < my_levelset(Cx, Cy + 1.01 * radius, Cz)); + CHECK(0 < my_levelset(Cx, Cy + radius * 0.99, Cz)); + + CHECK_FALSE(0 < my_levelset(Cx, Cy - 1.01 * radius, Cz)); + CHECK(0 < my_levelset(Cx, Cy - radius * 0.99, Cz)); + }; + + SECTION("near lower end") { check(0, 0, 0.01); } + SECTION("near middle") { check(0, skew_yz * 4.99, 4.99); } + SECTION("near upper end") { check(0, skew_yz * 9.99, 9.99); } +} + } // namespace