diff --git a/lckernel/cad/math/intersectionhandler.cpp b/lckernel/cad/math/intersectionhandler.cpp index 8863fa0b4..6420eb110 100644 --- a/lckernel/cad/math/intersectionhandler.cpp +++ b/lckernel/cad/math/intersectionhandler.cpp @@ -7,16 +7,26 @@ std::vector Intersection::LineLine(const Equation& l1, std::vector ret; const auto &m1 = l1.Matrix(); const auto &m2 = l2.Matrix(); - std::vector> ce = { - // D, E, F - {m1(2,0) + m1(0,2), m1(2,1) + m1(1,2), -m1(2,2)}, - {m2(2,0) + m2(0,2), m2(2,1) + m2(1,2), -m2(2,2)} - }; - - std::vector sn(2, 0.); - if (Math::linearSolver(ce, sn)) { - ret.emplace_back(sn[0], sn[1]); - } +// std::vector> ce = { +// // D, E, F +// {m1(2,0) + m1(0,2), m1(2,1) + m1(1,2), -m1(2,2)}, +// {m2(2,0) + m2(0,2), m2(2,1) + m2(1,2), -m2(2,2)} +// }; + +// std::vector sn(2, 0.); +// if (Math::linearSolver(ce, sn)) { +// ret.emplace_back(sn[0], sn[1]); +// } +// return ret; + Eigen::Matrix2d M; + Eigen::Vector2d V; + + M << m1(2,0) + m1(0,2), m1(2,1) + m1(1,2), + m2(2,0) + m2(0,2), m2(2,1) + m2(1,2); + V << -m1(2,2), -m2(2,2); + + Eigen::Vector2d R = M.colPivHouseholderQr().solve(V); + ret.emplace_back(R[0], R[1]); return ret; } diff --git a/lckernel/cad/math/intersectionhandler.h b/lckernel/cad/math/intersectionhandler.h index 865f7b040..adfd21c0b 100644 --- a/lckernel/cad/math/intersectionhandler.h +++ b/lckernel/cad/math/intersectionhandler.h @@ -2,6 +2,7 @@ #include "cad/geometry/geocoordinate.h" #include "cad/math/equation.h" +#include "eigen3/Eigen/Dense" namespace lc { namespace maths { diff --git a/unittest/testmatrices.h b/unittest/testmatrices.h index f5d1bd08c..bc8038add 100644 --- a/unittest/testmatrices.h +++ b/unittest/testmatrices.h @@ -2,6 +2,7 @@ #include "cad/math/quadratic_math.h" #include "cad/math/intersectionhandler.h" #include "cad/geometry/geovector.h" +#include "cad/math/lcmath.h" #include using namespace std; @@ -116,3 +117,18 @@ TEST(QM, LineQuad) { EXPECT_DOUBLE_EQ(x[i].y(), y[i].y()) << "Y differs at index " << i; } } + +TEST(Maths, LinearSolver) { + std::vector> a {{1,-1, -1}, {3,1, 9}}; + std::vector res1; + Eigen::Matrix2d M; + Eigen::Vector2d V; + + M << 1, -1, 3, 1; + V << -1, 9; + lc::Math::linearSolver(a, res1); + Eigen::Vector2d res2 = M.colPivHouseholderQr().solve(V); + + EXPECT_DOUBLE_EQ(res1[0], res2[0]) << "X differs"; + EXPECT_DOUBLE_EQ(res1[1], res2[1]) << "Y differs"; +}