5 #ifndef HPP_CENTROIDAL_DYNAMICS_UTIL_HH
6 #define HPP_CENTROIDAL_DYNAMICS_UTIL_HH
13 #include <Eigen/Dense>
14 #include <Eigen/src/Core/util/Macros.h>
30 typedef Eigen::Matrix<value_type, 2, 1>
Vector2;
31 typedef Eigen::Matrix<value_type, 1, 2>
RVector2;
32 typedef Eigen::Matrix<value_type, 3, 1>
Vector3;
33 typedef Eigen::Matrix<value_type, 1, 3>
RVector3;
34 typedef Eigen::Matrix<value_type, 6, 1>
Vector6;
35 typedef Eigen::Matrix<value_type, Eigen::Dynamic, 1>
VectorX;
36 typedef Eigen::Matrix<value_type, 1, Eigen::Dynamic>
RVectorX;
37 typedef Eigen::Matrix<value_type, 3, 3, Eigen::RowMajor>
Rotation;
38 typedef Eigen::Matrix<value_type, Eigen::Dynamic, 2, Eigen::RowMajor>
MatrixX2;
39 typedef Eigen::Matrix<value_type, 3, 3, Eigen::RowMajor>
Matrix3;
40 typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3, Eigen::RowMajor>
MatrixX3;
41 typedef Eigen::Matrix<value_type, 3, Eigen::Dynamic, Eigen::RowMajor>
Matrix3X;
42 typedef Eigen::Matrix<value_type, 4, 3, Eigen::RowMajor>
Matrix43;
43 typedef Eigen::Matrix<value_type, 6, Eigen::Dynamic, Eigen::RowMajor>
Matrix6X;
44 typedef Eigen::Matrix<value_type, 6, 2, Eigen::RowMajor>
Matrix62;
45 typedef Eigen::Matrix<value_type, 6, 3, Eigen::RowMajor>
Matrix63;
46 typedef Eigen::Matrix<value_type, Eigen::Dynamic, 6, Eigen::RowMajor>
MatrixX6;
47 typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
MatrixXX;
70 typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3, Eigen::ColMajor>
MatrixX3ColMajor;
71 typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
MatrixXXColMajor;
78 template <
class Matrix>
80 std::ofstream out(filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
81 if (!out.is_open())
return false;
82 typename Matrix::Index rows = matrix.rows(), cols = matrix.cols();
83 out.write((
char*)(&rows),
sizeof(
typename Matrix::Index));
84 out.write((
char*)(&cols),
sizeof(
typename Matrix::Index));
85 out.write((
char*)matrix.data(), rows * cols *
sizeof(
typename Matrix::Scalar));
93 template <
class Matrix>
95 std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
96 if (!in.is_open())
return false;
97 typename Matrix::Index rows = 0, cols = 0;
98 in.read((
char*)(&rows),
sizeof(
typename Matrix::Index));
99 in.read((
char*)(&cols),
sizeof(
typename Matrix::Index));
100 matrix.resize(rows, cols);
101 in.read((
char*)matrix.data(), rows * cols *
sizeof(
typename Matrix::Scalar));
141 template <
typename DerivedV,
typename DerivedU>
142 void doCombs(Eigen::Matrix<typename DerivedU::Scalar, 1, Eigen::Dynamic>& running,
int& running_i,
int& running_j,
143 Eigen::PlainObjectBase<DerivedU>& U,
const Eigen::MatrixBase<DerivedV>& V,
int offset,
int k) {
144 int N = (int)(V.size());
146 U.row(running_i) = running;
150 for (
int i = offset; i <= N - k; ++i) {
151 running(running_j) = V(i);
153 doCombs(running, running_i, running_j, U, V, i + 1, k - 1);
167 template <
typename DerivedV,
typename DerivedU>
168 void nchoosek(
const Eigen::MatrixBase<DerivedV>& V,
const int k, Eigen::PlainObjectBase<DerivedU>& U) {
169 using namespace Eigen;
174 assert((V.cols() == 1 || V.rows() == 1) &&
"V must be a vector");
175 U.resize(
nchoosek((
int)(V.size()), k), k);
178 Matrix<typename DerivedU::Scalar, 1, Dynamic> running(1, k);
179 doCombs(running, running_i, running_j, U, V, 0, k);
183 #endif // HPP_CENTROIDAL_DYNAMICS_UTIL_HH