Example #1
0
int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;

  cout<<"Usage:"<<endl;
  cout<<"[space]  toggle showing input mesh, output mesh or slice "<<endl;
  cout<<"         through tet-mesh of convex hull."<<endl;
  cout<<"'.'/','  push back/pull forward slicing plane."<<endl;
  cout<<endl;

  // Load mesh: (V,T) tet-mesh of convex hull, F contains facets of input
  // surface mesh _after_ self-intersection resolution
  igl::readMESH(TUTORIAL_SHARED_PATH "/big-sigcat.mesh",V,T,F);

  // Compute barycenters of all tets
  igl::barycenter(V,T,BC);

  // Compute generalized winding number at all barycenters
  cout<<"Computing winding number over all "<<T.rows()<<" tets..."<<endl;
  igl::winding_number(V,F,BC,W);

  // Extract interior tets
  MatrixXi CT((W.array()>0.5).count(),4);
  {
    size_t k = 0;
    for(size_t t = 0;t<T.rows();t++)
    {
      if(W(t)>0.5)
      {
        CT.row(k) = T.row(t);
        k++;
      }
    }
  }
  // find bounary facets of interior tets
  igl::boundary_facets(CT,G);
  // boundary_facets seems to be reversed...
  G = G.rowwise().reverse().eval();

  // normalize
  W = (W.array() - W.minCoeff())/(W.maxCoeff()-W.minCoeff());

  // Plot the generated mesh
  igl::opengl::glfw::Viewer viewer;
  update_visualization(viewer);
  viewer.callback_key_down = &key_down;
  viewer.launch();
}
Example #2
0
int calculate_column_width(const Eigen::VectorXd& x,
                           const std::string& name,
                           const int sig_figs,
                           std::ios_base::fmtflags& format) {

  int padding = 2;
  
  // Fixed Precision
  size_t fixed_threshold = 8;
  size_t max_fixed_width = 0;
  
  for (int i = 0; i < x.size(); ++i) {
    size_t width = compute_width(x[i], sig_figs);
    max_fixed_width = width > max_fixed_width ? width : max_fixed_width;
  }
  
  if (max_fixed_width + padding < fixed_threshold) {
    format = std::ios_base::fixed;
    max_fixed_width = name.length() > max_fixed_width ? name.length() : max_fixed_width;
    return max_fixed_width + padding;
  }
  
  // Scientific Notation
  size_t scientific_width = sig_figs + 1 + 4; // Decimal place + exponent
  if (x.minCoeff() < 0) ++scientific_width;
  
  scientific_width = name.length() > scientific_width ? name.length() : scientific_width;
  
  format = std::ios_base::scientific;
  return scientific_width + padding;
  
}
Example #3
0
TEST_F(Models_BasicDistributions_Triangle,
  Test_Triangle) {
  populate_chains();
  
  Eigen::VectorXd y = chains->samples(chains->index("y"));

  EXPECT_LE(y.minCoeff(), -0.9)
    << "expecting to get close to the corner";
  EXPECT_GE(y.maxCoeff(), 0.9)
    << "expecting to get close to the corner";
}
Example #4
0
int calculate_size(const Eigen::VectorXd& x, 
                   const std::string& name,
                   const int digits,
                   std::ios_base::fmtflags& format) {
  using std::max;
  using std::ceil;
  using std::log10;
  
  double padding = 0;
  if (digits > 0)
    padding = digits + 1;

  double fixed_size = 0.0;
  if (x.maxCoeff() > 0)
    fixed_size = ceil(log10(x.maxCoeff()+0.001)) + padding;
  if (x.minCoeff() < 0)
    fixed_size = max(fixed_size, ceil(log10(-x.minCoeff()+0.01))+(padding+1));
  format = std::ios_base::fixed;
  if (fixed_size < 7) {
    return max(fixed_size,
               max(name.length(), std::string("-0.0").length())+0.0);
  }

  double scientific_size = 0;
  scientific_size += 4.0;   // "-0.0" has four digits
  scientific_size += 1.0;   // e
  double exponent_size = 0;
  if (x.maxCoeff() > 0)
    exponent_size = ceil(log10(log10(x.maxCoeff())));
  if (x.minCoeff() < 0)
    exponent_size = max(exponent_size,
                        ceil(log10(log10(-x.minCoeff()))));
  scientific_size += fmin(exponent_size, 3);
  format = std::ios_base::scientific;
  return scientific_size;
}
Example #5
0
void do_residuals_stats(const Eigen::VectorXd & r, Eigen::VectorXd &stats, std::string & file_hdr)
{
	file_hdr = "% MAX_ABS_ERR   MIN_ABS_ERR   AVERAGE_ERR   STD_DEV   RMSE    MEDIAN\n";

	const size_t N = r.size();
	stats.resize(6);
	if (!N) return;

	stats[0] = r.maxCoeff();
	stats[1] = r.minCoeff();

	mrpt::math::meanAndStd(r, stats[2], stats[3]);
	// RMSE:
	stats[4] = std::sqrt(  r.array().square().sum() / N );
	
	std::vector<double> v(N);
	for (size_t i=0;i<N;i++) v[i]=r[i];
	nth_element(v.begin(), v.begin()+(N/2), v.end());
	stats[5] = v[N/2];
}
void IRLTest::generateData()
{
  int num_active_features;
  int num_data;
  int num_samples_per_data;
  double cost_noise_stddev;

  num_features_ = 10;
  num_active_features = 2;
  num_data = 10;
  num_samples_per_data = 10;
  cost_noise_stddev = 0.1;

  // generate random weights
  real_weights_ = Eigen::VectorXd::Zero(num_features_);
  real_weights_.head(num_active_features).setRandom();

  data_.resize(num_data);
  for (int i=0; i<num_data; ++i)
  {
    IRLData* d = new IRLData(num_features_);
    Eigen::MatrixXd features = Eigen::MatrixXd::Zero(num_samples_per_data, num_features_);
    Eigen::VectorXd target = Eigen::VectorXd::Zero(num_samples_per_data);
    features.setRandom();

    // compute w^t * phi
    Eigen::VectorXd costs = features*real_weights_;

    // add random noise to costs
    costs += cost_noise_stddev * Eigen::VectorXd::Random(num_samples_per_data);

    // set min cost target to 1
    int min_index;
    double min_cost = costs.minCoeff(&min_index);
    target(min_index) = 1.0;
    d->addSamples(features, target);
    data_[i].reset(d);
  }
  real_weights_exist_ = true;
}
Example #7
0
bool mrpt::vision::pnp::rpnp::compute_pose(Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_)
{
	// selecting an edge $P_{ i1 }P_{ i2 }$ by n random sampling
	int i1 = 0, i2 = 1;
	double lmin = Q(0, i1)*Q(0, i2) + Q(1, i1)*Q(1, i2) + Q(2, i1)*Q(2, i2);

	Eigen::MatrixXi rij (n,2);
    
    R_=Eigen::MatrixXd::Identity(3,3);
    t_=Eigen::Vector3d::Zero();
    
	for (int i = 0; i < n; i++)
		for (int j = 0; j < 2; j++)
			rij(i, j) = rand() % n;

	for (int ii = 0; ii < n; ii++)
	{
		int i = rij(ii, 0), j = rij(ii,1);

		if (i == j)
			continue;

		double l = Q(0, i)*Q(0, j) + Q(1, i)*Q(1, j) + Q(2, i)*Q(2, j);

		if (l < lmin)
		{
			i1 = i;
			i2 = j;
			lmin = l;
		}
	}

	// calculating the rotation matrix of $O_aX_aY_aZ_a$.
	Eigen::Vector3d p1, p2, p0, x, y, z, dum_vec;

	p1 = P.col(i1);
	p2 = P.col(i2);
	p0 = (p1 + p2) / 2;

	x = p2 - p0; x /= x.norm();

	if (abs(x(1)) < abs(x(2)) )
	{
		dum_vec << 0, 1, 0;
		z = x.cross(dum_vec); z /= z.norm();
		y = z.cross(x); y /= y.norm();
	}
	else
	{
		dum_vec << 0, 0, 1;
		y = dum_vec.cross(x); y /= y.norm();
		z = x.cross(y); x /= x.norm();
	}

	Eigen::Matrix3d R0;

	R0.col(0) = x; R0.col(1) =y; R0.col(2) = z;

	for (int i = 0; i < n; i++)
		P.col(i) = R0.transpose() * (P.col(i) - p0);

	// Dividing the n - point set into(n - 2) 3 - point subsets
	// and setting up the P3P equations

	Eigen::Vector3d v1 = Q.col(i1), v2 = Q.col(i2);
	double cg1 = v1.dot(v2);
	double sg1 = sqrt(1 - cg1*cg1);
	double D1 = (P.col(i1) - P.col(i2)).norm();
	Eigen::MatrixXd D4(n - 2, 5);
    
	int j = 0;
    Eigen::Vector3d vi;
    Eigen::VectorXd rowvec(5);
	for (int i = 0; i < n; i++)
	{
		if (i == i1 || i == i2)
			continue;

		vi = Q.col(i);
		double cg2 = v1.dot(vi); 
		double cg3 = v2.dot(vi);
		double sg2 = sqrt(1 - cg2*cg2);
		double D2 = (P.col(i1) - P.col(i)).norm();
		double D3 = (P.col(i) - P.col(i2)).norm();

		// get the coefficients of the P3P equation from each subset.
		
		rowvec = getp3p(cg1, cg2, cg3, sg1, sg2, D1, D2, D3);
		D4.row(j) = rowvec;
		j += 1;
      
        if(j>n-3)
            break;
	}

	Eigen::VectorXd D7(8), dumvec(8), dumvec1(5);
	D7.setZero();
    
	for (int i = 0; i < n-2; i++)
	{
        dumvec1 = D4.row(i);
		dumvec= getpoly7(dumvec1);
		D7 += dumvec;
	}
    
	Eigen::PolynomialSolver<double, 7> psolve(D7.reverse());
	Eigen::VectorXcd comp_roots = psolve.roots().transpose();
	Eigen::VectorXd real_comp, imag_comp;
	real_comp = comp_roots.real();
	imag_comp = comp_roots.imag();

	Eigen::VectorXd::Index max_index;

	double max_real= real_comp.cwiseAbs().maxCoeff(&max_index);

    std::vector<double> act_roots_;

	int cnt=0;
    
	for (int i=0; i<imag_comp.size(); i++ )
	{
		if(abs(imag_comp(i))/max_real<0.001)
		{
				act_roots_.push_back(real_comp(i));
                cnt++;
		}
	}
    
    double* ptr = &act_roots_[0];
    Eigen::Map<Eigen::VectorXd> act_roots(ptr, cnt); 
    
	if (cnt==0)
    {
		return false;
    }
    
    Eigen::VectorXd act_roots1(cnt);
    act_roots1 << act_roots.segment(0,cnt);

	std::vector<Eigen::Matrix3d> R_cum(cnt);
	std::vector<Eigen::Vector3d> t_cum(cnt);
	std::vector<double> err_cum(cnt);
    
	for(int i=0; i<cnt; i++)
	{
		double root = act_roots(i);

		// Compute the rotation matrix

		double d2 = cg1 + root;

		Eigen::Vector3d unitx, unity, unitz;
		unitx << 1,0,0;
		unity << 0,1,0;
		unitz << 0,0,1;
		x = v2*d2 -v1;
		x/=x.norm();
		if (abs(unity.dot(x)) < abs(unitz.dot(x)))
		{
			z = x.cross(unity);z/=z.norm();
			y=z.cross(x); y/y.norm();
		}
		else
		{
			y=unitz.cross(x); y/=y.norm();
			z = x.cross(y); z/=z.norm();
		}
		R.col(0)=x;
		R.col(1)=y;
		R.col(2)=z;

		//calculating c, s, tx, ty, tz

		Eigen::MatrixXd D(2 * n, 6);
		D.setZero();

		R0 = R.transpose();
		Eigen::VectorXd r(Eigen::Map<Eigen::VectorXd>(R0.data(), R0.cols()*R0.rows()));
        
		for (int j = 0; j<n; j++)
		{
			double ui = img_pts(j, 0), vi = img_pts(j, 1), xi = P(0, j), yi = P(1, j), zi = P(2, j);
			D.row(2 * j) << -r(1)*yi + ui*(r(7)*yi + r(8)*zi) - r(2)*zi,
				-r(2)*yi + ui*(r(8)*yi - r(7)*zi) + r(1)*zi,
				-1,
				0,
				ui,
				ui*r(6)*xi - r(0)*xi;
			
			D.row(2 * j + 1) << -r(4)*yi + vi*(r(7)*yi + r(8)*zi) - r(5)*zi,
				-r(5)*yi + vi*(r(8)*yi - r(7)*zi) + r(4)*zi,
				0,
				-1,
				vi,
				vi*r(6)*xi - r(3)*xi;
		}
        
		Eigen::MatrixXd DTD = D.transpose()*D;

		Eigen::EigenSolver<Eigen::MatrixXd> es(DTD);

		Eigen::VectorXd Diag = es.pseudoEigenvalueMatrix().diagonal();

		Eigen::MatrixXd V_mat = es.pseudoEigenvectors();

		Eigen::MatrixXd::Index min_index;

		Diag.minCoeff(&min_index);

		Eigen::VectorXd V = V_mat.col(min_index);

		V /= V(5);

		double c = V(0), s = V(1);
		t << V(2), V(3), V(4);

		//calculating the camera pose by 3d alignment
		Eigen::VectorXd xi, yi, zi;
		xi = P.row(0); 
		yi = P.row(1);
		zi = P.row(2);

		Eigen::MatrixXd XXcs(3, n), XXc(3,n);
		XXc.setZero();

		XXcs.row(0) = r(0)*xi + (r(1)*c + r(2)*s)*yi + (-r(1)*s + r(2)*c)*zi + t(0)*Eigen::VectorXd::Ones(n);
		XXcs.row(1) = r(3)*xi + (r(4)*c + r(5)*s)*yi + (-r(4)*s + r(5)*c)*zi + t(1)*Eigen::VectorXd::Ones(n);
		XXcs.row(2) = r(6)*xi + (r(7)*c + r(8)*s)*yi + (-r(7)*s + r(8)*c)*zi + t(2)*Eigen::VectorXd::Ones(n);

		for (int ii = 0; ii < n; ii++)
			XXc.col(ii) = Q.col(ii)*XXcs.col(ii).norm();

		Eigen::Matrix3d R2;
		Eigen::Vector3d t2;

		Eigen::MatrixXd XXw = obj_pts.transpose();

		calcampose(XXc, XXw, R2, t2);

		R_cum[i] = R2;
		t_cum[i] = t2;

		for (int k = 0; k < n; k++)
			XXc.col(k) = R2 * XXw.col(k) + t2;

		Eigen::MatrixXd xxc(2, n);
		
		xxc.row(0) = XXc.row(0).array() / XXc.row(2).array();
		xxc.row(1) = XXc.row(1).array() / XXc.row(2).array();

		double res = ((xxc.row(0) - img_pts.col(0).transpose()).norm() + (xxc.row(1) - img_pts.col(1).transpose()).norm()) / 2;

		err_cum[i] = res;
	
	}
    
	int pos_cum = std::min_element(err_cum.begin(), err_cum.end()) - err_cum.begin();

	R_ = R_cum[pos_cum];
	t_ = t_cum[pos_cum];
    
	return true;
}
Example #8
0
int Lemke(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q,
          Eigen::VectorXd* _z) {
  int n = _q.size();

  const double zer_tol = 1e-5;
  const double piv_tol = 1e-8;
  int maxiter = 1000;
  int err = 0;

  if (_q.minCoeff() > 0) {
    // LOG(INFO) << "Trivial solution exists.";
    *_z = Eigen::VectorXd::Zero(n);
    return err;
  }

  *_z = Eigen::VectorXd::Zero(2 * n);
  int iter = 0;
  double theta = 0;
  double ratio = 0;
  int leaving  = 0;
  Eigen::VectorXd Be = Eigen::VectorXd::Constant(n, 1);
  Eigen::VectorXd x = _q;
  std::vector<int> bas;
  std::vector<int> nonbas;

  int t = 2 * n + 1;
  int entering = t;

  bas.clear();
  nonbas.clear();

  for (int i = 0; i < n; ++i) {
    bas.push_back(i);
  }

  Eigen::MatrixXd B = -Eigen::MatrixXd::Identity(n, n);

  for (int i = 0; i < bas.size(); ++i) {
    B.col(bas[i]) = _M.col(bas[i]);
  }

  x = -B.partialPivLu().solve(_q);

  Eigen::VectorXd minuxX = -x;
  int lvindex;
  double tval = minuxX.maxCoeff(&lvindex);
  leaving = bas[lvindex];
  bas[lvindex] = t;

  Eigen::VectorXd U = Eigen::VectorXd::Zero(n);
  for (int i = 0; i < n; ++i) {
    if (x[i] < 0)
      U[i] = 1;
  }
  Be = -(B * U);
  x += tval * U;
  x[lvindex] = tval;
  B.col(lvindex) = Be;

  for (iter = 0; iter < maxiter; ++iter) {
    if (leaving == t) {
      break;
    } else if (leaving < n) {
      entering = n + leaving;
      Be = Eigen::VectorXd::Zero(n);
      Be[leaving] = -1;
    } else {
      entering = leaving - n;
      Be = _M.col(entering);
    }

    Eigen::VectorXd d = B.partialPivLu().solve(Be);

    std::vector<int> j;
    for (int i = 0; i < n; ++i) {
      if (d[i] > piv_tol)
        j.push_back(i);
    }
    if (j.empty()) {
      // err = 2;
      break;
    }

    int jSize = static_cast<int>(j.size());
    Eigen::VectorXd minRatio(jSize);
    for (int i = 0; i < jSize; ++i) {
      minRatio[i] = (x[j[i]] + zer_tol) / d[j[i]];
    }
    double theta = minRatio.minCoeff();

    std::vector<int> tmpJ;
    std::vector<double> tmpMinRatio;
    for (int i = 0; i < jSize; ++i) {
      if (x[j[i]] / d[j[i]] <= theta) {
        tmpJ.push_back(j[i]);
        tmpMinRatio.push_back(minRatio[i]);
      }
    }
//    if (tmpJ.empty())
//    {
//      LOG(WARNING) << "tmpJ should never be empty!!!";
//      LOG(WARNING) << "dumping data:";
//      LOG(WARNING) << "theta:" << theta;
//      for (int i = 0; i < jSize; ++i)
//      {
//        LOG(WARNING) << "x(" << j[i] << "): " << x[j[i]] << "d: " << d[j[i]];
//      }
//    }

    j = tmpJ;
    jSize = static_cast<int>(j.size());
    if (jSize == 0) {
      err = 4;
      break;
    }
    lvindex = -1;

    for (int i = 0; i < jSize; ++i) {
      if (bas[j[i]] == t)
        lvindex = i;
    }
    if (lvindex != -1) {
      lvindex = j[lvindex];
    } else {
      theta = tmpMinRatio[0];
      lvindex = 0;

      for (int i = 0; i < jSize; ++i) {
        if (tmpMinRatio[i]-theta > piv_tol) {
          theta = tmpMinRatio[i];
          lvindex = i;
        }
      }
      lvindex = j[lvindex];
    }

    leaving = bas[lvindex];

    ratio = x[lvindex] / d[lvindex];

    bool bDiverged = false;
    for (int i = 0; i < n; ++i) {
      if (isnan(x[i]) || isinf(x[i])) {
        bDiverged = true;
        break;
      }
    }
    if (bDiverged) {
      err = 4;
      break;
    }
    x = x - ratio * d;
    x[lvindex] = ratio;
    B.col(lvindex) = Be;
    bas[lvindex] = entering;
  }

  if (iter >= maxiter && leaving != t)
    err = 1;

  if (err == 0) {
    for (size_t i = 0; i < bas.size(); ++i) {
      if (bas[i] < _z->size()) {
        (*_z)[bas[i]] = x[i];
      }
    }

    Eigen::VectorXd realZ = _z->segment(0, n);
    *_z = realZ;

    if (!validate(_M, *_z, _q)) {
      // _z = VectorXd::Zero(n);
      err = 3;
    }
  } else {
    *_z = Eigen::VectorXd::Zero(n);  // solve failed, return a 0 vector
  }

//  if (err == 1)
//    LOG(ERROR) << "LCP Solver: Iterations exceeded limit";
//  else if (err == 2)
//    LOG(ERROR) << "LCP Solver: Unbounded ray";
//  else if (err == 3)
//    LOG(ERROR) << "LCP Solver: Solver converged with numerical issues. "
//               << "Validation failed.";
//  else if (err == 4)
//    LOG(ERROR) << "LCP Solver: Iteration diverged.";

  return err;
}
Example #9
0
//#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;
}
void propa_2d()
{
    trace.beginBlock("2d propagation");

    typedef DiscreteExteriorCalculus<2, 2, EigenLinearAlgebraBackend> Calculus;
		typedef DiscreteExteriorCalculusFactory<EigenLinearAlgebraBackend> CalculusFactory;

    {
        trace.beginBlock("solving time dependent equation");

        const Calculus::Scalar cc = 8; // px/s
        trace.info() << "cc = " << cc << endl;

        const Z2i::Domain domain(Z2i::Point(0,0), Z2i::Point(29,29));
        const Calculus calculus = CalculusFactory::createFromDigitalSet(generateDiskSet(domain), false);

        //! [time_laplace]
        const Calculus::DualIdentity0 laplace = calculus.laplace<DUAL>() + 1e-8 * calculus.identity<0, DUAL>();
        //! [time_laplace]
        trace.info() << "laplace = " << laplace << endl;

        trace.beginBlock("finding eigen pairs");
        //! [time_eigen]
        typedef Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> EigenSolverMatrix;
        const EigenSolverMatrix eigen_solver(laplace.myContainer);

        const Eigen::VectorXd eigen_values = eigen_solver.eigenvalues();
        const Eigen::MatrixXd eigen_vectors = eigen_solver.eigenvectors();
        //! [time_eigen]
        trace.info() << "eigen_values_range = " << eigen_values.minCoeff() << "/" << eigen_values.maxCoeff() << endl;
        trace.endBlock();

        //! [time_omega]
        const Eigen::VectorXd angular_frequencies = cc * eigen_values.array().sqrt();
        //! [time_omega]

        Eigen::VectorXcd initial_wave = Eigen::VectorXcd::Zero(calculus.kFormLength(0, DUAL));
        //set_initial_phase_null(calculus, initial_wave);
        set_initial_phase_dir_yy(calculus, initial_wave);

        {
            Board2D board;
            board << domain;
            board << CustomStyle("KForm", new KFormStyle2D(-1, 1));
            board << Calculus::DualForm0(calculus, initial_wave.real());
            board.saveSVG("propagation_time_wave_initial_coarse.svg");
        }

        //! [time_init_proj]
        Eigen::VectorXcd initial_projections = eigen_vectors.transpose() * initial_wave;
        //! [time_init_proj]

        // low pass
        //! [time_low_pass]
        const Calculus::Scalar lambda_cutoff = 4.5;
        const Calculus::Scalar angular_frequency_cutoff = 2*M_PI * cc / lambda_cutoff;
        int cutted = 0;
        for (int kk=0; kk<initial_projections.rows(); kk++)
        {
            const Calculus::Scalar angular_frequency = angular_frequencies(kk);
            if (angular_frequency < angular_frequency_cutoff) continue;
            initial_projections(kk) = 0;
            cutted ++;
        }
        //! [time_low_pass]
        trace.info() << "cutted = " << cutted << "/" << initial_projections.rows() << endl;

        {
            const Eigen::VectorXcd wave = eigen_vectors * initial_projections;
            Board2D board;
            board << domain;
            board << CustomStyle("KForm", new KFormStyle2D(-1, 1));
            board << Calculus::DualForm0(calculus, wave.real());
            board.saveSVG("propagation_time_wave_initial_smoothed.svg");
        }

        const int kk_max = 60;
        trace.progressBar(0,kk_max);
        for (int kk=0; kk<kk_max; kk++)
        {
            //! [time_solve_time]
            const Calculus::Scalar time = kk/20.;
            const Eigen::VectorXcd current_projections = (angular_frequencies * std::complex<double>(0,time)).array().exp() * initial_projections.array();
            const Eigen::VectorXcd current_wave = eigen_vectors * current_projections;
            //! [time_solve_time]

            std::stringstream ss;
            ss << "propagation_time_wave_solution_" << std::setfill('0') << std::setw(3) << kk << ".svg";

            Board2D board;
            board << domain;
            board << CustomStyle("KForm", new KFormStyle2D(-1, 1));
            board << Calculus::DualForm0(calculus, current_wave.real());
            board.saveSVG(ss.str().c_str());

            trace.progressBar(kk+1,kk_max);
        }
        trace.info() << endl;

        trace.endBlock();
    }

    {
        trace.beginBlock("forced oscillations");

        const Z2i::Domain domain(Z2i::Point(0,0), Z2i::Point(50,50));
        const Calculus calculus = CalculusFactory::createFromDigitalSet(generateDiskSet(domain), false);

        const Calculus::DualIdentity0 laplace = calculus.laplace<DUAL>();
        trace.info() << "laplace = " << laplace << endl;

        trace.beginBlock("finding eigen pairs");
        typedef Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> EigenSolverMatrix;
        const EigenSolverMatrix eigen_solver(laplace.myContainer);

        const Eigen::VectorXd laplace_eigen_values = eigen_solver.eigenvalues();
        const Eigen::MatrixXd laplace_eigen_vectors = eigen_solver.eigenvectors();
        trace.info() << "eigen_values_range = " << laplace_eigen_values.minCoeff() << "/" << laplace_eigen_values.maxCoeff() << endl;
        trace.endBlock();

        Calculus::DualForm0 concentration(calculus);
        {
            const Z2i::Point point(25,25);
            const Calculus::Cell cell = calculus.myKSpace.uSpel(point);
            const Calculus::Index index = calculus.getCellIndex(cell);
            concentration.myContainer(index) = 1;
        }

        {
            Board2D board;
            board << domain;
            board << concentration;
            board.saveSVG("propagation_forced_concentration.svg");
        }

        for (int ll=0; ll<6; ll++)
        {
            //! [forced_lambda]
            const Calculus::Scalar lambda = 4*20.75/(1+2*ll);
            //! [forced_lambda]
            trace.info() << "lambda = " << lambda << endl;

            //! [forced_dalembert_eigen]
            const Eigen::VectorXd dalembert_eigen_values = (laplace_eigen_values.array() - (2*M_PI/lambda)*(2*M_PI/lambda)).array().inverse();
            const Eigen::MatrixXd concentration_to_wave = laplace_eigen_vectors * dalembert_eigen_values.asDiagonal() * laplace_eigen_vectors.transpose();
            //! [forced_dalembert_eigen]

            //! [forced_wave]
            Calculus::DualForm0 wave(calculus, concentration_to_wave * concentration.myContainer);
            //! [forced_wave]
            wave.myContainer /= wave.myContainer(calculus.getCellIndex(calculus.myKSpace.uSpel(Z2i::Point(25,25))));

            {
                trace.info() << "saving samples" << endl;
                const Calculus::Properties properties = calculus.getProperties();
                {
                    std::stringstream ss;
                    ss << "propagation_forced_samples_hv_" << ll << ".dat";
                    std::ofstream handle(ss.str().c_str());
                    for (int kk=0; kk<=50; kk++)
                    {
                        const Z2i::Point point_horizontal(kk,25);
                        const Z2i::Point point_vertical(25,kk);
                        const Calculus::Scalar xx = 2 * (kk/50. - .5);
                        handle << xx << " ";
                        handle << sample_dual_0_form<Calculus>(properties, wave, point_horizontal) << " ";
                        handle << sample_dual_0_form<Calculus>(properties, wave, point_vertical) << endl;
                    }
                }

                {
                    std::stringstream ss;
                    ss << "propagation_forced_samples_diag_" << ll << ".dat";
                    std::ofstream handle(ss.str().c_str());
                    for (int kk=0; kk<=50; kk++)
                    {
                        const Z2i::Point point_diag_pos(kk,kk);
                        const Z2i::Point point_diag_neg(kk,50-kk);
                        const Calculus::Scalar xx = 2 * sqrt(2) * (kk/50. - .5);
                        handle << xx << " ";
                        handle << sample_dual_0_form<Calculus>(properties, wave, point_diag_pos) << " ";
                        handle << sample_dual_0_form<Calculus>(properties, wave, point_diag_neg) << endl;
                    }
                }
            }

            {
                std::stringstream ss;
                ss << "propagation_forced_wave_" << ll << ".svg";
                Board2D board;
                board << domain;
                board << CustomStyle("KForm", new KFormStyle2D(-.5, 1));
                board << wave;
                board.saveSVG(ss.str().c_str());
            }
        }

        trace.endBlock();
    }
    trace.endBlock();
}
Example #11
0
int main(int argc, char * argv[])
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;
  if(!readMESH("../shared/octopus-low.mesh",low.V,low.T,low.F))
  {
    cout<<"failed to load mesh"<<endl;
  }
  if(!readMESH("../shared/octopus-high.mesh",high.V,high.T,high.F))
  {
    cout<<"failed to load mesh"<<endl;
  }

  // Precomputation
  {
    Eigen::VectorXi b;
    {
      Eigen::VectorXi J = Eigen::VectorXi::LinSpaced(high.V.rows(),0,high.V.rows()-1);
      Eigen::VectorXd sqrD;
      Eigen::MatrixXd _2;
      cout<<"Finding closest points..."<<endl;
      igl::point_mesh_squared_distance(low.V,high.V,J,sqrD,b,_2);
      assert(sqrD.minCoeff() < 1e-7 && "low.V should exist in high.V");
    }
    // force perfect positioning, rather have popping in low-res than high-res.
    // The correct/elaborate thing to do is express original low.V in terms of
    // linear interpolation (or extrapolation) via elements in (high.V,high.F)
    igl::slice(high.V,b,1,low.V);
    // list of points --> list of singleton lists
    std::vector<std::vector<int> > S;
    igl::matrix_to_list(b,S);
    cout<<"Computing weights for "<<b.size()<<
      " handles at "<<high.V.rows()<<" vertices..."<<endl;
    // Technically k should equal 3 for smooth interpolation in 3d, but 2 is
    // faster and looks OK
    const int k = 2;
    igl::biharmonic_coordinates(high.V,high.T,S,k,W);
    cout<<"Reindexing..."<<endl;
    // Throw away interior tet-vertices, keep weights and indices of boundary
    VectorXi I,J;
    igl::remove_unreferenced(high.V.rows(),high.F,I,J);
    for_each(high.F.data(),high.F.data()+high.F.size(),[&I](int & a){a=I(a);});
    for_each(b.data(),b.data()+b.size(),[&I](int & a){a=I(a);});
    igl::slice(MatrixXd(high.V),J,1,high.V);
    igl::slice(MatrixXd(W),J,1,W);
  }

  // Resize low res (high res will also be resized by affine precision of W)
  low.V.rowwise() -= low.V.colwise().mean();
  low.V /= (low.V.maxCoeff()-low.V.minCoeff());
  low.V.rowwise() += RowVector3d(0,1,0);
  low.U = low.V;
  high.U = high.V;

  arap_data.with_dynamics = true;
  arap_data.max_iter = 10;
  arap_data.energy = ARAP_ENERGY_TYPE_DEFAULT;
  arap_data.h = 0.01;
  arap_data.ym = 0.001;
  if(!arap_precomputation(low.V,low.T,3,VectorXi(),arap_data))
  {
    cerr<<"arap_precomputation failed."<<endl;
    return EXIT_FAILURE;
  }
  // Constant gravitational force
  Eigen::SparseMatrix<double> M;
  igl::massmatrix(low.V,low.T,igl::MASSMATRIX_TYPE_DEFAULT,M);
  const size_t n = low.V.rows();
  arap_data.f_ext =  M * RowVector3d(0,-9.8,0).replicate(n,1);
  // Random initial velocities to wiggle things
  arap_data.vel = MatrixXd::Random(n,3);
  
  igl::viewer::Viewer viewer;
  // Create one huge mesh containing both meshes
  igl::cat(1,low.U,high.U,scene.U);
  igl::cat(1,low.F,MatrixXi(high.F.array()+low.V.rows()),scene.F);
  // Color each mesh
  viewer.data.set_mesh(scene.U,scene.F);
  MatrixXd C(scene.F.rows(),3);
  C<<
    RowVector3d(0.8,0.5,0.2).replicate(low.F.rows(),1),
    RowVector3d(0.3,0.4,1.0).replicate(high.F.rows(),1);
  viewer.data.set_colors(C);

  viewer.callback_key_pressed = 
    [&](igl::viewer::Viewer & viewer,unsigned int key,int mods)->bool
  {
    switch(key)
    {
      default: 
        return false;
      case ' ':
        viewer.core.is_animating = !viewer.core.is_animating;
        return true;
      case 'r':
        low.U = low.V;
        return true;
    }
  };
  viewer.callback_pre_draw = [&](igl::viewer::Viewer & viewer)->bool
  {
    glEnable(GL_CULL_FACE);
    if(viewer.core.is_animating)
    {
      arap_solve(MatrixXd(0,3),arap_data,low.U);
      for(int v = 0;v<low.U.rows();v++)
      {
        // collide with y=0 plane
        const int y = 1;
        if(low.U(v,y) < 0)
        {
          low.U(v,y) = -low.U(v,y);
          // ~ coefficient of restitution
          const double cr = 1.1;
          arap_data.vel(v,y) = - arap_data.vel(v,y) / cr;
        }
      }

      scene.U.block(0,0,low.U.rows(),low.U.cols()) = low.U;
      high.U = W * (low.U.rowwise() + RowVector3d(1,0,0));
      scene.U.block(low.U.rows(),0,high.U.rows(),high.U.cols()) = high.U;

      viewer.data.set_vertices(scene.U);
      viewer.data.compute_normals();
    }
    return false;
  };
  viewer.core.show_lines = false;
  viewer.core.is_animating = true;
  viewer.core.animation_max_fps = 30.;
  viewer.data.set_face_based(true);
  cout<<R"(
[space] to toggle animation
'r'     to reset positions 
      )";
  viewer.core.rotation_type = 
    igl::viewer::ViewerCore::ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP;
  viewer.launch();
}
Example #12
0
Eigen::RowVectorXd scaledValues(Eigen::VectorXd const& x)
{
    return x.unaryExpr(scaledValue(x.minCoeff(),x.maxCoeff())).transpose();
}