Commit d73eefad authored by Steve Tonneau's avatar Steve Tonneau Committed by Pierre Fernbach
Browse files

implemented binomial

parent d6d7d100
......@@ -7,6 +7,8 @@
#include <iostream>
#include <fstream>
#include <cmath>
#include <cassert>
#include <Eigen/Dense>
#include <Eigen/src/Core/util/Macros.h>
......@@ -136,6 +138,63 @@ namespace centroidal_dynamics
std::string getDateAndTimeAsString();
/**
* Computes a binomal coefficient
* @return n!/((n–k)! k!).
*/
value_type nchoosek(const int n, const int k);
template < typename DerivedV, typename DerivedU>
void doCombs(Eigen::Matrix<typename DerivedU::Scalar,1,Eigen::Dynamic>& running,
int& running_i, int& running_j, Eigen::PlainObjectBase<DerivedU> & U, const Eigen::MatrixBase<DerivedV> & V,
int offset, int k)
{
int N = (int)(V.size());
if(k==0)
{
U.row(running_i) = running;
running_i++;
return;
}
for (int i = offset; i <= N - k; ++i)
{
running(running_j) = V(i);
running_j++;
doCombs(running, running_i, running_j, U, V, i+1,k-1);
running_j--;
}
}
/**
* Computes a matrix C containing all possible combinations of the elements of vector v taken k at a time.
* Matrix C has k columns and n!/((n–k)! k!) rows, where n is length(v).
* @param V n-long vector of elements
* @param k size of sub-set to consider
* @param U result matrix
* @return nchoosek by k long matrix where each row is a unique k-size
* the first one, with all zeros.
*/
template < typename DerivedV, typename DerivedU>
void nchoosek(
const Eigen::MatrixBase<DerivedV> & V,
const int k,
Eigen::PlainObjectBase<DerivedU> & U)
{
using namespace Eigen;
if(V.size() == 0)
{
U.resize(0,k);
return;
}
assert((V.cols() == 1 || V.rows() == 1) && "V must be a vector");
U.resize(nchoosek((int)(V.size()),k),k);
int running_i = 0;
int running_j = 0;
Matrix<typename DerivedU::Scalar,1,Dynamic> running(1,k);
/* const std::function<void(int,int)> doCombs =
[&running,&N,&doCombs,&running_i,&running_j,&U,&V](int offset, int k)*/;
doCombs(running, running_i, running_j, U, V,0,k);
}
} //namespace centroidal_dynamics
#endif //_CENTROIDAL_DYNAMICS_LIB_UTIL_HH
......@@ -521,12 +521,41 @@ LP_status Equilibrium::findExtremumInDirection(Cref_vector3 direction, Ref_vecto
SEND_ERROR_MSG("findExtremumInDirection not implemented yet");
return LP_STATUS_ERROR;
}
///
/// \brief Computes factorial of a number
///
int fact(const int n)
{
assert(n>=0);
int res = 1;
for (int i=2 ; i <= n ; ++i)
res *= i;
return res;
}
///
/// \brief Computes a binomal coefficient
///
int choosenk(const int n, const int k)
{
return fact(n) / (fact(k) * fact(n - k));
}
bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
{
int n = (int)(v.rows());
int m = (int)(v.cols());
// todo: for the moment using ad hoc upper bound = 500 N
int n = (int)v.rows();
int m = (int)v.cols();
if (n>m)
{
SEND_ERROR_MSG("V has more lines that columns, this case is not handled!");
return false;
}
MatrixXX test = v;
VectorX vec;
nchoosek(vec,6,test);
//int I = I=nchoosek(1:m,n-1);*/
// getProfiler().start("eigen_to_cdd");
dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix);
// getProfiler().stop("eigen_to_cdd");
......
......@@ -165,6 +165,51 @@ std::string getDateAndTimeAsString()
strftime(buffer,80,"%Y%m%d_%I%M%S",timeinfo);
return std::string(buffer);
}
/*
int fact(const int n)
{
assert(n>=0);
int res = 1;
for (int i=2 ; i <= n ; ++i)
res *= i;
return res;
}
int choosenk(const int n, const int k)
{
return fact(n) / (fact(k) * fact(n - k));
}*/
/* is this faster ?
value_type choosenk(const int n, const int k)
{
if(k>n/2)
return nchoosek(n,n-k);
else if(k==1)
return n;
else
{
double c = 1;
for(int i = 1;i<=k;i++)
c *= (((double)n-k+i)/((double)i));
return std::round(c);
}
}*/
value_type nchoosek(const int n, const int k)
{
if(k>n/2)
return nchoosek(n,n-k);
else if(k==1)
return n;
else
{
value_type c = 1;
for(int i = 1;i<=k;i++)
c *= (((value_type)n-k+i)/((value_type)i));
return round(c);
}
}
} //namespace centroidal_dynamics
......
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