void initMtlOptDM(pMtlOptDM dm){
dm->setTotalFrames(T);
dm->setDamping(ak,am);
dm->fixHeadFrames(fixHead);
dm->fixTailFrames(fixTail);
dm->Z = MatrixXd::Random(rs,T);
dm->Z.leftCols(fixHead).setZero();
dm->Z.rightCols(fixTail).setZero();
dm->initVariables(lambda,rs);
dm->S = MatrixXd::Random(rw,rs);
dm->conFrames.clear();
dm->conNodes.clear();
dm->uc.clear();
VectorXi conF;
conF.resize(2);
for (int f = 0; f < conF.size(); ++f){
conF[f] = fixHead+f;
}
for (int i = 0; i < conF.size(); ++i){
dm->conFrames.push_back(conF[i]);
vector<int> nodes;
for (int j = 0; j < 2; ++j){
nodes.push_back(i+j);
}
dm->conNodes.push_back(nodes);
dm->uc.push_back(VectorXd::Random(nodes.size()*3)*0.0f);
}
}
void ModelParametersLWPR::getParameterVectorMask(const std::set<std::string> selected_values_labels, VectorXi& selected_mask) const
{
selected_mask.resize(getParameterVectorAllSize());
selected_mask.fill(0);
int offset = 0;
int size;
// Centers
size = n_centers_;
if (selected_values_labels.find("centers")!=selected_values_labels.end())
selected_mask.segment(offset,size).fill(1);
offset += size;
// Widths
size = n_widths_;
if (selected_values_labels.find("widths")!=selected_values_labels.end())
selected_mask.segment(offset,size).fill(2);
offset += size;
// Offsets
size = n_offsets_;
if (selected_values_labels.find("offsets")!=selected_values_labels.end())
selected_mask.segment(offset,size).fill(3);
offset += size;
// Slopes
size = n_slopes_;
if (selected_values_labels.find("slopes")!=selected_values_labels.end())
selected_mask.segment(offset,size).fill(4);
offset += size;
assert(offset == getParameterVectorAllSize());
}
void UnifiedModel::getParameterVectorMask(const std::set<std::string> selected_values_labels, VectorXi& selected_mask) const
{
selected_mask.resize(getParameterVectorAllSize());
selected_mask.fill(0);
int offset = 0;
int size;
// Centers
size = centers_.size()*centers_[0].size();
if (selected_values_labels.find("centers")!=selected_values_labels.end())
selected_mask.segment(offset,size).fill(1);
offset += size;
// Widths
size = covars_.size()*covars_[0].cols();
if (selected_values_labels.find("widths")!=selected_values_labels.end())
selected_mask.segment(offset,size).fill(2);
offset += size;
// Offsets
size = offsets_.size();
if (selected_values_labels.find("offsets")!=selected_values_labels.end())
selected_mask.segment(offset,size).fill(3);
offset += size;
// Slopes
size = slopes_.size()*slopes_[0].size();
if (selected_values_labels.find("slopes")!=selected_values_labels.end())
selected_mask.segment(offset,size).fill(4);
offset += size;
assert(offset == getParameterVectorAllSize());
}
void MathUtilities::extractTripletData( const SparseMatrixsc& matrix, VectorXi& rows, VectorXi& cols, VectorXs& vals )
{
rows.resize( matrix.nonZeros() );
cols.resize( matrix.nonZeros() );
vals.resize( matrix.nonZeros() );
int flat_index{ 0 };
for( int outer_index = 0; outer_index < matrix.outerSize(); ++outer_index )
{
for( Eigen::SparseMatrix<double>::InnerIterator it( matrix, outer_index ); it; ++it )
{
rows( flat_index ) = it.row();
cols( flat_index ) = it.col();
vals( flat_index++ ) = it.value();
}
}
assert( flat_index == matrix.nonZeros() );
}
void MathUtilities::extractDataCCS( const SparseMatrixsc& A, VectorXi& col_ptr, VectorXi& row_ind, VectorXs& val )
{
col_ptr.resize( A.cols() + 1 );
row_ind.resize( A.nonZeros() );
val.resize( A.nonZeros() );
col_ptr(0) = 0;
for( int col = 0; col < A.outerSize(); ++col )
{
col_ptr(col+1) = col_ptr(col);
for( SparseMatrixsc::InnerIterator it(A,col); it; ++it )
{
const int row{ it.row() };
val(col_ptr(col+1)) = it.value();
row_ind(col_ptr(col+1)) = row;
++col_ptr(col+1);
}
}
assert( col_ptr( col_ptr.size() - 1 ) == row_ind.size() );
}
void CodeAtlas::FuzzyDependBuilder::buildSubGraphLevel(const SparseMatrix& veMat, const bool* const validMask, int nNodes, VectorXi& level)
{
SparseMatrix subMat; VectorXi subLevel;
if(!GraphUtility::getSubGraph(veMat, validMask, subMat))
{
level.setConstant(nNodes, GraphUtility::s_defaultBaseLevel);
return;
}
GraphUtility::computeVtxLevel(subMat, subLevel, GraphUtility::s_defaultBaseLevel);
level.resize(nNodes);
for (int i = 0, ithNon = 0; i < nNodes; ++i)
{
if (!validMask[i])
level[i] = GraphUtility::s_defaultBaseLevel;
else
level[i] = subLevel[ithNon++];
}
}
void ComponentLayouter::layoutByWord( QVector<Component>& compInfo, float& finalRadius )
{
// qDebug()<< "by word";
int nComp = compInfo.size();
if (nComp == 1)
{
Component& comp = compInfo[0];
comp.m_compPos2D.resize(2);
comp.m_compPos2D[0] = comp.m_compPos2D[1] = 0.f;
finalRadius = comp.m_radius;
return;
}
MatrixXd distMat(nComp, nComp);
for (int i = 0; i < nComp; ++i)
{
distMat(i,i) = 0.f;
for (int j = i+1; j < nComp; ++j)
{
Component& compI = compInfo[i];
Component& compJ = compInfo[j];
double cosVal = compI.m_wordAttr.cosSimilarity(compJ.m_wordAttr);
distMat(i,j) = distMat(j,i) = 1.0 - cosVal;
}
}
VectorXf rVec;
VectorXi hashVec;
rVec.resize(nComp);
hashVec.resize(nComp);
for (int ithComp = 0; ithComp < nComp; ++ithComp)
{
rVec[ithComp] = compInfo[ithComp].m_radius;
hashVec[ithComp] = compInfo[ithComp].m_compHash;
}
MatrixXf finalPos2D;
Layouter::mds(distMat, rVec, hashVec, finalPos2D, finalRadius, m_wordSparseFactor, 0.01f);
for (int ithComp = 0; ithComp < nComp; ++ithComp)
compInfo[ithComp].m_compPos2D = finalPos2D.row(ithComp);
}
void ordering(const int & _first_ordered_node)
{
t_ordering_ = clock();
// full problem ordering
if (_first_ordered_node == 0)
{
// ordering ordering constraints
node_ordering_restrictions_.resize(n_nodes_);
node_ordering_restrictions_ = A_nodes_.bottomRows(1).transpose();
// computing nodes partial ordering_
A_nodes_.makeCompressed();
PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(n_nodes_);
orderer_(A_nodes_, incr_permutation_nodes, node_ordering_restrictions_.data());
// node ordering to variable ordering
PermutationMatrix<Dynamic, Dynamic, int> incr_permutation(A_.cols());
nodePermutation2VariablesPermutation(incr_permutation_nodes, incr_permutation);
// apply partial_ordering orderings
A_nodes_ = (A_nodes_ * incr_permutation_nodes.transpose()).sparseView();
A_ = (A_ * incr_permutation.transpose()).sparseView();
// ACCUMULATING PERMUTATIONS
accumulatePermutation(incr_permutation_nodes);
}
// partial ordering
else
{
int ordered_nodes = n_nodes_ - _first_ordered_node;
int unordered_nodes = n_nodes_ - ordered_nodes;
if (ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones
{
// SUBPROBLEM ORDERING (from first node variable to last one)
//std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1 << std::endl;
SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes);
// _partial_ordering ordering_ constraints
node_ordering_restrictions_.resize(ordered_nodes);
node_ordering_restrictions_ = sub_A_nodes_.bottomRows(1).transpose();
// computing nodes partial ordering_
sub_A_nodes_.makeCompressed();
PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_nodes(ordered_nodes);
orderer_(sub_A_nodes_, partial_permutation_nodes, node_ordering_restrictions_.data());
// node ordering to variable ordering
PermutationMatrix<Dynamic, Dynamic, int> partial_permutation(A_.cols());
nodePermutation2VariablesPermutation(partial_permutation_nodes, partial_permutation);
// apply partial_ordering orderings
int ordered_variables = A_.cols() - nodes_.at(_first_ordered_node).location;
A_nodes_.rightCols(ordered_nodes) = (A_nodes_.rightCols(ordered_nodes) * partial_permutation_nodes.transpose()).sparseView();
A_.rightCols(ordered_variables) = (A_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
R_.rightCols(ordered_variables) = (R_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
// ACCUMULATING PERMUTATIONS
accumulatePermutation(partial_permutation_nodes);
}
}
time_ordering_ += ((double) clock() - t_ordering_) / CLOCKS_PER_SEC;
}
//#define IGL_LINPROG_VERBOSE
IGL_INLINE bool igl::linprog(
const Eigen::VectorXd & c,
const Eigen::MatrixXd & _A,
const Eigen::VectorXd & b,
const int k,
Eigen::VectorXd & x)
{
// This is a very literal translation of
// http://www.mathworks.com/matlabcentral/fileexchange/2166-introduction-to-linear-algebra/content/strang/linprog.m
using namespace Eigen;
using namespace std;
bool success = true;
// number of constraints
const int m = _A.rows();
// number of original variables
const int n = _A.cols();
// number of iterations
int it = 0;
// maximum number of iterations
//const int MAXIT = 10*m;
const int MAXIT = 100*m;
// residual tolerance
const double tol = 1e-10;
const auto & sign = [](const Eigen::VectorXd & B) -> Eigen::VectorXd
{
Eigen::VectorXd Bsign(B.size());
for(int i = 0;i<B.size();i++)
{
Bsign(i) = B(i)>0?1:(B(i)<0?-1:0);
}
return Bsign;
};
// initial (inverse) basis matrix
VectorXd Dv = sign(sign(b).array()+0.5);
Dv.head(k).setConstant(1.);
MatrixXd D = Dv.asDiagonal();
// Incorporate slack variables
MatrixXd A(_A.rows(),_A.cols()+D.cols());
A<<_A,D;
// Initial basis
VectorXi B = igl::colon<int>(n,n+m-1);
// non-basis, may turn out that vector<> would be better here
VectorXi N = igl::colon<int>(0,n-1);
int j;
double bmin = b.minCoeff(&j);
int phase;
VectorXd xb;
VectorXd s;
VectorXi J;
if(k>0 && bmin<0)
{
phase = 1;
xb = VectorXd::Ones(m);
// super cost
s.resize(n+m+1);
s<<VectorXd::Zero(n+k),VectorXd::Ones(m-k+1);
N.resize(n+1);
N<<igl::colon<int>(0,n-1),B(j);
J.resize(B.size()-1);
// [0 1 2 3 4]
// ^
// [0 1]
// [3 4]
J.head(j) = B.head(j);
J.tail(B.size()-j-1) = B.tail(B.size()-j-1);
B(j) = n+m;
MatrixXd AJ;
igl::slice(A,J,2,AJ);
const VectorXd a = b - AJ.rowwise().sum();
{
MatrixXd old_A = A;
A.resize(A.rows(),A.cols()+a.cols());
A<<old_A,a;
}
D.col(j) = -a/a(j);
D(j,j) = 1./a(j);
}else if(k==m)
{
phase = 2;
xb = b;
s.resize(c.size()+m);
// cost function
s<<c,VectorXd::Zero(m);
}else //k = 0 or bmin >=0
{
phase = 1;
xb = b.array().abs();
s.resize(n+m);
// super cost
s<<VectorXd::Zero(n+k),VectorXd::Ones(m-k);
}
while(phase<3)
{
double df = -1;
int t = std::numeric_limits<int>::max();
// Lagrange mutipliers fro Ax=b
VectorXd yb = D.transpose() * igl::slice(s,B);
while(true)
{
if(MAXIT>0 && it>=MAXIT)
{
#ifdef IGL_LINPROG_VERBOSE
cerr<<"linprog: warning! maximum iterations without convergence."<<endl;
#endif
success = false;
break;
}
// no freedom for minimization
if(N.size() == 0)
{
break;
}
// reduced costs
VectorXd sN = igl::slice(s,N);
MatrixXd AN = igl::slice(A,N,2);
VectorXd r = sN - AN.transpose() * yb;
int q;
// determine new basic variable
double rmin = r.minCoeff(&q);
// optimal! infinity norm
if(rmin>=-tol*(sN.array().abs().maxCoeff()+1))
{
break;
}
// increment iteration count
it++;
// apply Bland's rule to avoid cycling
if(df>=0)
{
if(MAXIT == -1)
{
#ifdef IGL_LINPROG_VERBOSE
cerr<<"linprog: warning! degenerate vertex"<<endl;
#endif
success = false;
}
igl::find((r.array()<0).eval(),J);
double Nq = igl::slice(N,J).minCoeff();
// again seems like q is assumed to be a scalar though matlab code
// could produce a vector for multiple matches
(N.array()==Nq).cast<int>().maxCoeff(&q);
}
VectorXd d = D*A.col(N(q));
VectorXi I;
igl::find((d.array()>tol).eval(),I);
if(I.size() == 0)
{
#ifdef IGL_LINPROG_VERBOSE
cerr<<"linprog: warning! solution is unbounded"<<endl;
#endif
// This seems dubious:
it=-it;
success = false;
break;
}
VectorXd xbd = igl::slice(xb,I).array()/igl::slice(d,I).array();
// new use of r
int p;
{
double r;
r = xbd.minCoeff(&p);
p = I(p);
// apply Bland's rule to avoid cycling
if(df>=0)
{
igl::find((xbd.array()==r).eval(),J);
double Bp = igl::slice(B,igl::slice(I,J)).minCoeff();
// idiotic way of finding index in B of Bp
// code down the line seems to assume p is a scalar though the matlab
// code could find a vector of matches)
(B.array()==Bp).cast<int>().maxCoeff(&p);
}
// update x
xb -= r*d;
xb(p) = r;
// change in f
df = r*rmin;
}
// row vector
RowVectorXd v = D.row(p)/d(p);
yb += v.transpose() * (s(N(q)) - d.transpose()*igl::slice(s,B));
d(p)-=1;
// update inverse basis matrix
D = D - d*v;
t = B(p);
B(p) = N(q);
if(t>(n+k-1))
{
// remove qth entry from N
VectorXi old_N = N;
N.resize(N.size()-1);
N.head(q) = old_N.head(q);
N.head(q) = old_N.head(q);
N.tail(old_N.size()-q-1) = old_N.tail(old_N.size()-q-1);
}else
{
N(q) = t;
}
}
// iterative refinement
xb = (xb+D*(b-igl::slice(A,B,2)*xb)).eval();
// must be due to rounding
VectorXi I;
igl::find((xb.array()<0).eval(),I);
if(I.size()>0)
{
// so correct
VectorXd Z = VectorXd::Zero(I.size(),1);
igl::slice_into(Z,I,xb);
}
// B, xb,n,m,res=A(:,B)*xb-b
if(phase == 2 || it<0)
{
break;
}
if(xb.transpose()*igl::slice(s,B) > tol)
{
it = -it;
#ifdef IGL_LINPROG_VERBOSE
cerr<<"linprog: warning, no feasible solution"<<endl;
#endif
success = false;
break;
}
// re-initialize for Phase 2
phase = phase+1;
s*=1e6*c.array().abs().maxCoeff();
s.head(n) = c;
}
x.resize(std::max(B.maxCoeff()+1,n));
igl::slice_into(xb,B,x);
x = x.head(n).eval();
return success;
}
int EMclustering::EM(int k, int *IDX, bool spatial, bool att)
{
clusternum = k;
MatrixXf x;
/*if(spatial)
{
if(att)
{
x.resize(4,dataSize);
for(int i=0;i<dataSize;i++)
{
x(0,i) = dataPos[i][0];
x(1,i) = dataPos[i][1];
x(2,i) = dataPos[i][2];
x(3,i) = dataDen[i];
}
}
else
{
x.resize(6,dataSize);
for(int i=0;i<dataSize;i++)
{
x(0,i) = dataPos[i][0];
x(1,i) = dataPos[i][1];
x(2,i) = dataPos[i][2];
x(3,i) = dataVel[i][0];
x(4,i) = dataVel[i][1];
x(5,i) = dataVel[i][2];
}
}
}
else
{*/
if(att)
{
x.resize(1,dataDen.size());
for(int i=0;i<dataDen.size();i++)
{
x(0,i) = dataDen[i];
}
//cerr<<x;
//cerr<<endl;
if(k>dataDen.size())
return -1;
}
else
{
x.resize(3,dataSize);
for(int i=0;i<dataSize;i++)
{
x(0,i) = dataVel[i][0];//fabs(cos(-PI/4)*dataVel[i][0] - sin(-PI/4)*dataVel[i][1]);
x(1,i) = dataVel[i][1];//fabs(sin(-PI/4)*dataVel[i][0] + cos(-PI/4)*dataVel[i][1]);
x(2,i) = dataVel[i][2];
}
if(k>dataSize)
return -1;
}
//}
//cout<<"EM for Gaussian mixture: running ... "<<endl;
//cerr<<x<<endl;
MatrixXf r =initialization(x,k);// kmeans(x,k);//
//cerr<<"Initialization is Done"<<endl;//cerr<<r<<endl;
VectorXi label(r.rows());
for(int i=0;i<r.rows();i++)
{
int index;
float tmp1 = r.row(i).maxCoeff(&index);
label(i) = index;
}//cerr<<label<<endl;
VectorXi tmpp(label.size());
VectorXi tmp2 = unique(label,tmpp);
int tmpd = tmp2.size(); //cerr<<tmpd<<endl;
MatrixXf tmpr(r.rows(),tmpd);
for(int i=0;i<tmpd;i++)
{
tmpr.col(i) = r.col(tmp2(i));
}//cerr<<"done1"<<endl;
r.resize(r.rows(),tmpd);
r = tmpr;//cerr<<r.cols()<<endl;
float tol = 1e-10;
int max = 300;
double llh = -9e+9;
bool converged = false;
int t = 1;
//cerr<<"done1"<<endl;
//gaussian_model model;
int clusternum_error;
MatrixXf tmpmodel;
while(!converged&&t<max)
{
t = t + 1;
gaussian_model model = maximization(x,r);//cerr<<t<<" "<<"max"<<endl;
float tmpllh = llh;
r = expectation(x,model,llh);//cerr<<t<<" "<<"exp"<<endl;
for(int i=0;i<r.rows();i++)
{
int index;
float tmp1 = r.row(i).maxCoeff(&index);
label(i) = index;
}
VectorXi u = unique(label,tmpp);//cerr<<t<<" "<<u.size()<<" "<<r.cols()<<" "<<r.rows()<<endl;
clusternum_error = clusternum - u.size();
if(r.cols()!=u.size())
{
/* tmpr.resize(r.rows(),u.size());
for(int i=0;i<u.size();i++)
{
tmpr.col(i) = r.col(u(i));
}
r.resize(r.rows(),u.size());
r = tmpr;//cerr<<"r"<<endl;*/
}
else
{
if((llh - tmpllh)<tol*abs(llh))
converged = true;
else
converged = false;
}
//cerr<<"t"<<t<<endl;
//return_model = model;
tmpmodel.resize(model.mu.rows(),model.mu.cols());
//return_model = model.mu;
tmpmodel = model.mu;
u.resize(0);
//cerr<<tmpmodel<<endl;
}
/*ofstream off2("rr");
off2<<r.row(0)<<endl;
for(int i=1;i<r.rows();i++)
if(r.row(i)!=r.row(i-1))
{off2<<x.col(i)<<" ";
off2<<r.row(i)<<endl;}
off2.close();*/
cerr<<clusternum_error<<endl;
return_model = tmpmodel;
//cerr<<label<<endl;
if (converged)
cerr<<"Converged in "<<t-1<<endl;
else
cerr<<max<<endl;
//cerr<<t-1<<endl;
for(int i=0;i<label.size();i++)
{
IDX[i] = label(i);
//cerr<<IDX[i]<<" ";
}//cerr<<endl;
//cerr<<label.size()<<endl;
x.resize(0,0);
r.resize(0,0);
tmpr.resize(0,0);
tmpmodel.resize(0,0);
label.resize(0);
tmpp.resize(0);
tmp2.resize(0);
return clusternum_error;
}
MatrixXf EMclustering::initialization(MatrixXf x, int k)
{
srand (1);
int n = x.cols();
int d = x.rows();
//cerr<<n<<"d "<<d<<endl;
VectorXi idx(k);
//idx.setRandom(k);
//cerr<<idx<<endl;
idx(0) = rand()%n;
for(int i=1;i<k;i++)
{
int random;// = rand()%n;
//cerr<<random<<endl;
bool flag2 = true;
while(flag2)
{
random = rand()%n;
flag2 = false;
for(int j=0;j<i;j++)
{
if(idx(j)==random)
{
flag2 = true;
break;
}
}
}
idx(i) = random;
}
//cerr<<"idx"<<idx<<endl<<endl;
int tmpsize = idx.size();
MatrixXf m(d,tmpsize);
for(int i=0;i<tmpsize;i++)
m.col(i) = x.col(idx(i));
//cerr<<m<<endl<<endl;
VectorXf tmp1(m.cols());
tmp1 = m.cwiseProduct(m).colwise().sum();
//cerr<<tmp1<<endl<<endl;
tmp1 = tmp1.adjoint()/2;
//cerr<<tmp1<<endl<<endl;
MatrixXf tmp2(m.adjoint().rows(),x.cols());
tmp2 = m.adjoint() * x;
//cerr<<tmp2<<endl<<endl;
MatrixXf tmp3(tmp2.rows(),tmp2.cols());
tmp3 = tmp2.colwise() - tmp1;
//cerr<<tmp3<<endl<<endl;
double tmp4;
int tmp5;
int tmpd = tmp3.cols();
VectorXi label(tmpd);
for(int i=0;i<tmpd;i++)
{
tmp4 = tmp3.col(i).maxCoeff(&tmp5);
label(i) = tmp5;
}
//cerr<<label<<endl<<endl;
VectorXi tmp6(label.size());
VectorXi u = unique(label, tmp6);
//cerr<<u<<endl<<endl;
//cerr<<tmp6<<endl<<endl;
int max = 100;
int t=0;
while(k!=u.size()&&t<max)
{
t++;
//cerr<<"-----------"<<endl;
//idx.setRandom(k);
idx(0) = rand()%n;//cerr<<k<<" "<<u.size()<<" "<<idx(0)<<endl;
for(int i=1;i<k;i++)
{
int random;// = rand()%n;
bool flag2 = true;
while(flag2)
{random = rand()%n;
flag2 = false;
for(int j=0;j<i;j++)
{
if(idx(j)==random||x.col(idx(j))==x.col(random))
{
flag2 = true;
break;
}
}
}
idx(i) = random;//cerr<<idx(i)<<" ";
}//cerr<<endl;
//cerr<<"idx"<<idx<<endl<<endl;;
//cerr<<"u"<<u<<endl<<endl;
for(int i=0;i<tmpsize;i++)
m.col(i) = x.col(idx(i));
//cerr<<m<<endl<<endl;
tmp1 = m.cwiseProduct(m).colwise().sum();
//cerr<<tmp1<<endl<<endl;
tmp1 = tmp1.adjoint()/2;
//cerr<<tmp1<<endl<<endl;
tmp2 = m.adjoint() * x;
//cerr<<tmp2<<endl<<endl;
tmp3 = tmp2.colwise() - tmp1;
//cerr<<tmp3<<endl<<endl;
tmpd = tmp3.cols();
for(int i=0;i<tmpd;i++)
{
tmp4 = tmp3.col(i).maxCoeff(&tmp5);
label(i) = tmp5;
}
//cerr<<"label"<<label.size()<<endl<<endl;
//cerr<<"end"<<endl;
u = unique(label, tmp6);
label = tmp6;
//cerr<<"u"<<u<<endl<<endl;
//cerr<<tmp6<<endl<<endl;
}//cerr<<k<<" "<<u.size()<<endl;
//cerr<<t<<endl;
//cerr<<u.size()<<endl;
MatrixXf r(n,k);
r.setZero(n,k);
int j=0;
int tmplabel[k];
for(int i=0;i<k;i++)
tmplabel[i] = 0;
for(int i=0;i<n;i++)
{
r(i,label(j)) = 1;
tmplabel[label(j)] = 1;
j++;
}
//for(int i=0;i<n;i++)
//{
// if(tmplabel[i]==0)
// {
// r(i,0) = 1;
// }
//}
j = 0;
for(int i=0;i<k;i++)
{
if(tmplabel[i]==0)
{
//cerr<<i<<endl;
for(int ii=0;ii<n/k;ii++)
{
int ttt = rand()%n;
//for(int jj=0;jj<k;jj++)
// r(ttt,jj)=0;
r(ttt,i) = 1;
}
//j ++;//cerr<<j<<endl;
}
}
//cerr<<r.cols()<<endl;
//cerr<<r<<endl;
//delete [] tmplabel;
idx.resize(0);
m.resize(0,0);
tmp1.resize(0);
tmp2.resize(0,0);
tmp3.resize(0,0);
tmp6.resize(0);
u.resize(0);
label.resize(0);
return r;
}
void mexFunction(
int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
using namespace std;
using namespace Eigen;
using namespace igl;
using namespace igl::matlab;
igl::matlab::MexStream mout;
std::streambuf *outbuf = cout.rdbuf(&mout);
//mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);
MatrixXd P,V,C,N;
MatrixXi F;
VectorXi I;
VectorXd S;
SignedDistanceType type;
parse_rhs(nrhs,prhs,P,V,F,type);
if(F.rows() > 0)
{
switch(V.cols())
{
case 2:
{
// Persistent data not supported for 2D
signed_distance(P,V,F,type,S,I,C,N);
break;
}
case 3:
{
if(g_sign_type != type || g_V != V || g_F != F)
{
g_V = V;
g_F = F;
g_sign_type = type;
// Clear the tree
g_tree.deinit();
// Prepare distance computation
g_tree.init(V,F);
switch(type)
{
default:
assert(false && "Unknown SignedDistanceType");
case SIGNED_DISTANCE_TYPE_DEFAULT:
case SIGNED_DISTANCE_TYPE_WINDING_NUMBER:
g_hier.set_mesh(V,F);
g_hier.grow();
break;
case SIGNED_DISTANCE_TYPE_PSEUDONORMAL:
// "Signed Distance Computation Using the Angle Weighted Pseudonormal"
// [Bærentzen & Aanæs 2005]
per_face_normals(V,F,g_FN);
per_vertex_normals(V,F,PER_VERTEX_NORMALS_WEIGHTING_TYPE_ANGLE,
g_FN,g_VN);
per_edge_normals(
V,F,PER_EDGE_NORMALS_WEIGHTING_TYPE_UNIFORM,
g_FN,g_EN,g_E,g_EMAP);
break;
}
}
N.resize(P.rows(),3);
S.resize(P.rows(),1);
I.resize(P.rows(),1);
C.resize(P.rows(),3);
//for(int p = 0;p<P.rows();p++)
igl::parallel_for(P.rows(),[&](const int p)
{
const Eigen::RowVector3d q(P(p,0),P(p,1),P(p,2));
double s,sqrd;
Eigen::RowVector3d c;
int i;
switch(type)
{
default:
assert(false && "Unknown SignedDistanceType");
case SIGNED_DISTANCE_TYPE_DEFAULT:
case SIGNED_DISTANCE_TYPE_WINDING_NUMBER:
signed_distance_winding_number(
g_tree,g_V,g_F,g_hier,q,s,sqrd,i,c);
break;
case SIGNED_DISTANCE_TYPE_PSEUDONORMAL:
{
RowVector3d n(0,0,0);
signed_distance_pseudonormal(
g_tree,g_V,g_F,g_FN,g_VN,g_EN,g_EMAP,
q,s,sqrd,i,c,n);
N.row(p) = n;
break;
}
}
I(p) = i;
S(p) = s*sqrt(sqrd);
C.row(p) = c;
},10000);
break;
}
}
}
switch(nlhs)
{
default:
{
mexErrMsgTxt(false,"Too many output parameters.");
}
case 4:
{
prepare_lhs_double(N,plhs+3);
// Fall through
}
case 3:
{
prepare_lhs_double(C,plhs+2);
// Fall through
}
case 2:
{
prepare_lhs_index(I,plhs+1);
// Fall through
}
case 1:
{
prepare_lhs_double(S,plhs+0);
// Fall through
}
case 0: break;
}
// Restore the std stream buffer Important!
std::cout.rdbuf(outbuf);
}
