Commit d2f18b68 authored by stevet's avatar stevet
Browse files

waypointtolist agnostic to row dimension

parent f9e558da
...@@ -56,12 +56,13 @@ linear_variable_t* wayPointsToLists(const bezier_linear_variable_t& self) { ...@@ -56,12 +56,13 @@ linear_variable_t* wayPointsToLists(const bezier_linear_variable_t& self) {
const t_point& wps = self.waypoints(); const t_point& wps = self.waypoints();
// retrieve num variables. // retrieve num variables.
std::size_t dim = wps[0].B().cols(); std::size_t dim = wps[0].B().cols();
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrices(dim, wps.size() * 3); std::size_t dimRows = wps[0].c().rows();
Eigen::Matrix<real, Eigen::Dynamic, 1> vectors = Eigen::Matrix<real, Eigen::Dynamic, 1>::Zero(3 * wps.size()); Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrices(dim, wps.size() * dimRows);
Eigen::Matrix<real, Eigen::Dynamic, 1> vectors = Eigen::Matrix<real, Eigen::Dynamic, 1>::Zero(dimRows * wps.size());
int i = 0; int i = 0;
for (cit_point cit = wps.begin(); cit != wps.end(); ++cit, ++i) { for (cit_point cit = wps.begin(); cit != wps.end(); ++cit, ++i) {
matrices.block(0, i * 3, dim, 3) = cit->B().transpose(); matrices.block(0, i * dimRows, dim, dimRows) = cit->B().transpose();
vectors.segment<3>(i * 3) = cit->c(); vectors.segment(i * dimRows, dimRows) = cit->c();
} }
return new linear_variable_t(matrices.transpose(), vectors.transpose()); return new linear_variable_t(matrices.transpose(), vectors.transpose());
} }
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment