| Directory: | ./ |
|---|---|
| File: | python/ndcurves/python_variables.cpp |
| Date: | 2025-03-05 17:18:30 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 12 | 45 | 26.7% |
| Branches: | 16 | 78 | 20.5% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | #include "python_variables.h" | ||
| 2 | |||
| 3 | #include <Eigen/Core> | ||
| 4 | |||
| 5 | #include "ndcurves/python/python_definitions.h" | ||
| 6 | |||
| 7 | namespace ndcurves { | ||
| 8 | ✗ | std::vector<linear_variable_t> matrix3DFromEigenArray( | |
| 9 | const point_list3_t& matrices, const point_list3_t& vectors) { | ||
| 10 | ✗ | if (vectors.cols() * 3 != matrices.cols()) { | |
| 11 | ✗ | throw std::invalid_argument("vectors.cols() * 3 != matrices.cols()"); | |
| 12 | } | ||
| 13 | ✗ | std::vector<linear_variable_t> res; | |
| 14 | ✗ | for (int i = 0; i < vectors.cols(); ++i) { | |
| 15 | ✗ | res.push_back( | |
| 16 | ✗ | linear_variable_t(matrices.block<3, 3>(0, i * 3), vectors.col(i))); | |
| 17 | } | ||
| 18 | ✗ | return res; | |
| 19 | } | ||
| 20 | |||
| 21 | ✗ | linear_variable_t fillWithZeros(const linear_variable_t& var, | |
| 22 | const std::size_t totalvar, | ||
| 23 | const std::size_t i) { | ||
| 24 | linear_variable_t::matrix_x_t B( | ||
| 25 | ✗ | linear_variable_t::matrix_x_t::Zero(dim, totalvar * dim)); | |
| 26 | ✗ | B.block(0, dim * i, dim, dim) = var.B(); | |
| 27 | ✗ | return linear_variable_t(B, var.c()); | |
| 28 | } | ||
| 29 | |||
| 30 | ✗ | std::vector<linear_variable_t> computeLinearControlPoints( | |
| 31 | const point_list3_t& matrices, const point_list3_t& vectors) { | ||
| 32 | ✗ | std::vector<linear_variable_t> res; | |
| 33 | std::vector<linear_variable_t> variables = | ||
| 34 | ✗ | matrix3DFromEigenArray(matrices, vectors); | |
| 35 | // now need to fill all this with zeros... | ||
| 36 | ✗ | std::size_t totalvar = variables.size(); | |
| 37 | ✗ | for (std::size_t i = 0; i < totalvar; ++i) | |
| 38 | ✗ | res.push_back(fillWithZeros(variables[i], totalvar, i)); | |
| 39 | ✗ | return res; | |
| 40 | } | ||
| 41 | |||
| 42 | /*linear variable control points*/ | ||
| 43 | ✗ | bezier_linear_variable_t* wrapBezierLinearConstructor( | |
| 44 | const point_list3_t& matrices, const point_list3_t& vectors) { | ||
| 45 | std::vector<linear_variable_t> asVector = | ||
| 46 | ✗ | computeLinearControlPoints(matrices, vectors); | |
| 47 | ✗ | return new bezier_linear_variable_t(asVector.begin(), asVector.end()); | |
| 48 | } | ||
| 49 | |||
| 50 | ✗ | bezier_linear_variable_t* wrapBezierLinearConstructorBounds( | |
| 51 | const point_list3_t& matrices, const point_list3_t& vectors, | ||
| 52 | const real T_min, const real T_max) { | ||
| 53 | std::vector<linear_variable_t> asVector = | ||
| 54 | ✗ | computeLinearControlPoints(matrices, vectors); | |
| 55 | ✗ | return new bezier_linear_variable_t(asVector.begin(), asVector.end(), T_min, | |
| 56 | ✗ | T_max); | |
| 57 | } | ||
| 58 | |||
| 59 | ✗ | Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> cost_t_quad( | |
| 60 | const quadratic_variable_t& p) { | ||
| 61 | ✗ | Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> A = p.A(); | |
| 62 | ✗ | return A; | |
| 63 | } | ||
| 64 | ✗ | Eigen::Matrix<real, Eigen::Dynamic, 1> cost_t_linear( | |
| 65 | const quadratic_variable_t& p) { | ||
| 66 | ✗ | Eigen::Matrix<real, Eigen::Dynamic, 1> b = p.b(); | |
| 67 | ✗ | return b; | |
| 68 | } | ||
| 69 | ✗ | real cost_t_constant(const quadratic_variable_t& p) { return p.c(); } | |
| 70 | |||
| 71 | 1 | linear_variable_t* wayPointsToLists(const bezier_linear_variable_t& self) { | |
| 72 | typedef typename bezier_linear_variable_t::t_point_t t_point; | ||
| 73 | typedef | ||
| 74 | typename bezier_linear_variable_t::t_point_t::const_iterator cit_point; | ||
| 75 | 1 | const t_point& wps = self.waypoints(); | |
| 76 | // retrieve num variables. | ||
| 77 | 1 | std::size_t dim = wps[0].B().cols(); | |
| 78 | 1 | std::size_t dimRows = wps[0].c().rows(); | |
| 79 | Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrices( | ||
| 80 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | dim, wps.size() * dimRows); |
| 81 | Eigen::Matrix<real, Eigen::Dynamic, 1> vectors = | ||
| 82 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | Eigen::Matrix<real, Eigen::Dynamic, 1>::Zero(dimRows * wps.size()); |
| 83 | 1 | int i = 0; | |
| 84 |
2/2✓ Branch 3 taken 4 times.
✓ Branch 4 taken 1 times.
|
5 | for (cit_point cit = wps.begin(); cit != wps.end(); ++cit, ++i) { |
| 85 |
3/6✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
|
4 | matrices.block(0, i * dimRows, dim, dimRows) = cit->B().transpose(); |
| 86 |
2/4✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
|
4 | vectors.segment(i * dimRows, dimRows) = cit->c(); |
| 87 | } | ||
| 88 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | return new linear_variable_t(matrices.transpose(), vectors.transpose()); |
| 89 | 1 | } | |
| 90 | |||
| 91 | } // namespace ndcurves | ||
| 92 |