5 #ifndef HPP_CENTROIDAL_DYNAMICS_UTIL_HH
6 #define HPP_CENTROIDAL_DYNAMICS_UTIL_HH
10 #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,
72 typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3, Eigen::ColMajor>
74 typedef Eigen::Matrix<
value_type, Eigen::Dynamic, Eigen::Dynamic,
83 template <
class Matrix>
85 std::ofstream out(filename.c_str(),
86 std::ios::out | std::ios::binary | std::ios::trunc);
87 if (!out.is_open())
return false;
88 typename Matrix::Index rows = matrix.rows(), cols = matrix.cols();
89 out.write((
char*)(&rows),
sizeof(
typename Matrix::Index));
90 out.write((
char*)(&cols),
sizeof(
typename Matrix::Index));
91 out.write((
char*)matrix.data(),
92 rows * cols *
sizeof(
typename Matrix::Scalar));
100 template <
class Matrix>
102 std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
103 if (!in.is_open())
return false;
104 typename Matrix::Index rows = 0, cols = 0;
105 in.read((
char*)(&rows),
sizeof(
typename Matrix::Index));
106 in.read((
char*)(&cols),
sizeof(
typename Matrix::Index));
107 matrix.resize(rows, cols);
108 in.read((
char*)matrix.data(), rows * cols *
sizeof(
typename Matrix::Scalar));
121 const bool canonicalize =
false);
154 template <
typename DerivedV,
typename DerivedU>
156 Eigen::Matrix<typename DerivedU::Scalar, 1, Eigen::Dynamic>& running,
157 int& running_i,
int& running_j, Eigen::PlainObjectBase<DerivedU>& U,
158 const Eigen::MatrixBase<DerivedV>& V,
int offset,
int k) {
159 int N = (int)(V.size());
161 U.row(running_i) = running;
165 for (
int i = offset; i <= N - k; ++i) {
166 running(running_j) = V(i);
168 doCombs(running, running_i, running_j, U, V, i + 1, k - 1);
183 template <
typename DerivedV,
typename DerivedU>
184 void nchoosek(
const Eigen::MatrixBase<DerivedV>& V,
const int k,
185 Eigen::PlainObjectBase<DerivedU>& U) {
186 using namespace Eigen;
191 assert((V.cols() == 1 || V.rows() == 1) &&
"V must be a vector");
192 U.resize(
nchoosek((
int)(V.size()), k), k);
195 Matrix<typename DerivedU::Scalar, 1, Dynamic> running(1, k);
196 doCombs(running, running_i, running_j, U, V, 0, k);
200 #endif // HPP_CENTROIDAL_DYNAMICS_UTIL_HH