Пример #1
0
    /* Solve min||Ax-b|| for a matrix A whose rank is given. */
    VectorXd solve( const VectorXd& b , bool inY=false)
    {
      if( rank==0 ) return VectorXd::Zero(NC);

      /* Approximate solution using no basis transfo (result is meanigless
       * appart from the computation time pov. */
      /*
	VectorXd sol = b.head(rank);
	matrixL().solveInPlace( sol );
	VectorXd res; res.setZero(NC);
	res.head(rank)=sol; return res;
      */

      /* With plain matrices. */
      /*
	VectorXd sol = matrixUr().transpose()*b;
	matrixL().solveInPlace( sol );
	return matrixVr()*sol;
      */

      /* Using the HH representation of V. */
      assert( m_computeThinU || m_computeFullU );
      VectorXd sol; 
      if( inY ) sol.setZero(rank); else sol.setZero(NC); 
      sol.head(rank) = matrixUr().transpose()*b; 
      matrixL().solveInPlace( sol.head(rank) );
      if( ! inY ) sol.applyOnTheLeft(qrv.householderQ());
      return sol;
   }
Пример #2
0
 ConjugateGradientType(Functional f, const GridDescription &gdin, double kT, VectorXd *data, LineMinimizer lm,
                       double stepsize = 0.1)
   : MinimizerInterface(f, gdin, kT, data), step(stepsize), orig_step(step), direction(*data), oldgrad(*data), linmin(lm) {
   direction.setZero();
   oldgrad.setZero();
   oldgradsqr = 0;
 }
Пример #3
0
 void minimize(Functional newf, const GridDescription &gdnew, VectorXd *newx = 0) {
   step = orig_step;
   MinimizerInterface::minimize(newf, gdnew, newx);
   if (newx) {
     direction = *newx;
     oldgrad = *newx;
   }
   direction.setZero();
   oldgrad.setZero();
   oldgradsqr = 0;
 }
Пример #4
0
/** 
 * reconstruct the displacements u in Euler space with RS coordinates y
 * provided.
 * 
 * @param y the RS coordinates.
 * @param u the constructed Euler coordinates represents the displacements.
 * 
 * @return true if construction is success.
 */
bool RS2Euler::reconstruct(const VectorXd &y, VectorXd &u){

  assert (tetmesh != NULL);
  const int node_numx3 = tetmesh->nodes().size()*3;
  bool succ = true;

  // assemble the right_side
  VectorXd b;
  assemble_b(y,b);
  assert_eq(VG_t.cols(),b.size());
  assert_eq(VG_t.rows(),node_numx3);

  VectorXd right_side(node_numx3 + numFixedDofs());
  right_side.segment(0, node_numx3) = VG_t*b;
  right_side.segment(node_numx3,numFixedDofs()).setZero();
  right_side.segment(node_numx3,barycenter_uc.size()) = barycenter_uc;

  // solve A*u = right_side
  assert_eq (right_side.size(), A_solver.rows());
  u = A_solver.solve(right_side);

  // get the result 
  if(A_solver.info()!=Eigen::Success) {
	
  	succ = false;
  	u.resize(node_numx3);
  	u.setZero();
  	ERROR_LOG("failed to solve for A X = P.");
  }else{
	assert_gt(u.size(), node_numx3);
	const VectorXd x = u.head(node_numx3);
	u = x;
  }
  return succ;
}
Пример #5
0
void ImplicitEuler::ImplicitCalculateForces( MatrixXd& TVk, SparseMatrix<double>& forceGradient, VectorXd& x_k, VectorXd& f){
	// //gravity
	f.setZero();
	for(unsigned int i=0; i<M.tets.size(); i++){
		double vertex_mass = M.tets[i].undeformedVol/4;//assume const density 1
		Vector4i indices = M.tets[i].verticesIndex;
		f(3*indices(0)+1) += vertex_mass*gravity;
		f(3*indices(1)+1) += vertex_mass*gravity; 
		f(3*indices(2)+1) += vertex_mass*gravity;
		f(3*indices(3)+1) += vertex_mass*gravity;
	}

	//elastic
	for(unsigned int i=0; i<M.tets.size(); i++){
		Vector4i indices = M.tets[i].verticesIndex;
		MatrixXd F_tet = M.tets[i].computeElasticForces(TVk, simTime%2);
		f.segment<3>(3*indices(0)) += F_tet.col(0);
		f.segment<3>(3*indices(1)) += F_tet.col(1);
		f.segment<3>(3*indices(2)) += F_tet.col(2);
		f.segment<3>(3*indices(3)) += F_tet.col(3);
	}
	// cout<<f<<endl<<endl;
	//damping
	f += rayleighCoeff*forceGradient*(x_k - x_old)/h;
	// cout<<f<<endl<<endl;
	return;
}
Пример #6
0
void BuildCausalVector(VectorXd& vec2Build , VectorXd& index){
    vec2Build.setZero();
    for(int i = 0; i <index.size(); i++){
        if(index[i] >0){
            vec2Build[index[i]-1] = 1;
        }
    }
}
Пример #7
0
VectorXd vec(MatrixXd& M){
	VectorXd V;
	V.setZero(M.rows()*M.cols());
	for(int i=0;i<M.cols();i++){
		V.segment(i*M.rows(), M.rows()) = M.col(i);
	}
	return V;
}
Пример #8
0
VectorXd Tetra10::Give_weights()
{
    VectorXd w;
    w.setZero(ngauss);

    w(0) = 0.0416666666666667;
    w(1) = 0.0416666666666667;
    w(2) = 0.0416666666666667;
    w(3) = 0.0416666666666667;
    return w;
}
Пример #9
0
VectorXd vech(MatrixXd& M){
	int n = M.rows();
	VectorXd V;
	V.setZero(n*(n-1)/2+n);
	V.segment(0,n) = M.diagonal();
	int k = n;
	for(int i=0;i<n-1;i++){
		V.segment(k,n-i-1) = M.block(i,i+1,1,n-i-1).transpose();
		k += n-i-1;
	}
	return V;
}
Пример #10
0
void Simulation::calculateElasticForces(VectorXd &f, MatrixXd& TV){
	f.setZero();
	//elastic
	for(unsigned int i=0; i< M.tets.size(); i++){
		Vector4i indices = M.tets[i].verticesIndex;
		MatrixXd F_tet = M.tets[i].computeElasticForces(TV, 1);
		f.segment<3>(3*indices(0)) += F_tet.col(0);
		f.segment<3>(3*indices(1)) += F_tet.col(1);
		f.segment<3>(3*indices(2)) += F_tet.col(2);
		f.segment<3>(3*indices(3)) += F_tet.col(3);
	}
	return;
}
Пример #11
0
VectorXd Triangle<ConcreteShape>::QuadratureIntegrationWeight(const int order) {

  VectorXd wn;
  if(order == 3) {
    int num_pts = 12;
    wn.setZero(num_pts);
    quadrature_weights_p3_triangle(wn.data());
  } else {
    std::cerr << "ERROR: Order NOT implemented!\n";
    MPI::COMM_WORLD.Abort(-1);
  }
  return wn;
}
Пример #12
0
const VectorXd &AniEditDM::getUforConstraint(const int frame)const{
  
  static VectorXd tempt_u0;
  if (interpolator != NULL && totalFrameNum() > 0){
	assert_in(frame,0, totalFrameNum()-1);
	return interpolator->getUforConstraint(frame);
  }else{
	if (tempt_u0.size() != fullDim()){
	  tempt_u0.resize(fullDim());
	  tempt_u0.setZero();
	}
	return tempt_u0;
  }
}
Пример #13
0
VectorXd multivariateNormal_density(const MatrixXd &x,
                                    const MatrixXd &Mu, const MatrixXd &Sigma) {
    if ( x.rows() != Mu.rows()) {
        std::cerr << "FROM multivariateNormal_density." << std::endl;
        std::cerr << "ERROR: X_ROWS != MU_ROWS." << std::endl;
    }
    VectorXd res;
    int T = x.rows();
    res.setZero(T);
    for(int i = 0; i<x.rows(); i++) {
        res(i) = double_multivariateNormal_density(x.row(i),
                 Mu.row(i),
                 Sigma);
    }
    return res;
}
Пример #14
0
const VectorXd &AniEditDM::getInputU(const int frame, const bool translate)const{
  
  static VectorXd tempt_u0;
  assert_in (frame, 0, totalFrameNum() );
  if (interpolator != NULL && totalFrameNum() > 0){
	if (!translate){
	  return interpolator->getInputU(frame);
	}else{
	  tempt_u0 = interpolator->getInputU(frame);
	  this->translate(tempt_u0, frame);
	  return tempt_u0;
	}
  }else{
	if (tempt_u0.size() != fullDim()){
	  tempt_u0.resize(fullDim());
	  tempt_u0.setZero();
	}
	return tempt_u0;
  }
}
Пример #15
0
void ImplicitEuler::ImplicitTVtoX(VectorXd& x_tv, MatrixXd& TVk){
	x_tv.setZero();
	for(unsigned int i = 0; i < M.tets.size(); i++){
		Vector4i indices = M.tets[i].verticesIndex;

		x_tv(3*indices(0)) = TVk.row(indices(0))[0];
		x_tv(3*indices(0)+1) = TVk.row(indices(0))[1];
		x_tv(3*indices(0)+2) = TVk.row(indices(0))[2];

		x_tv(3*indices(1)) = TVk.row(indices(1))[0];
		x_tv(3*indices(1)+1) = TVk.row(indices(1))[1];
		x_tv(3*indices(1)+2) = TVk.row(indices(1))[2];

		x_tv(3*indices(2)) = TVk.row(indices(2))[0];
		x_tv(3*indices(2)+1) = TVk.row(indices(2))[1];
		x_tv(3*indices(2)+2) = TVk.row(indices(2))[2];

		x_tv(3*indices(3)) = TVk.row(indices(3))[0];
		x_tv(3*indices(3)+1) = TVk.row(indices(3))[1];
		x_tv(3*indices(3)+2) = TVk.row(indices(3))[2];
	}
}
Пример #16
0
void read_SparseMatrix(SparseMatrix<double,0,int>& M, string path,string name){

	FILE * pFile;
	pFile = fopen((path + name+"i.bin").c_str(), "rb");
	if (pFile==NULL) {fputs ("File error",stderr); exit (1);}

	int n,m,ni;
	if(1 != fread(&n,sizeof(int),1,pFile)){
		fputs ("Read error\n",stderr); exit (1);
	}
	if(1 != fread(&m,sizeof(int),1,pFile)){
		fputs ("Read error\n",stderr); exit (1);
	}

	if(1 != fread(&ni,sizeof(int),1,pFile)){
		fputs ("Read error\n",stderr); exit (1);
	}
	M.resize(n,m);
	MatrixXi ij;
	ij.setZero(ni,2);
	if(ni*2 != fread(ij.data(),sizeof(int),ni*2,pFile)){
		fputs ("Read error\n",stderr); exit (1);
	}
	fclose(pFile);
	VectorXd v;
	v.setZero(ni);
	pFile = fopen((path + name+"v.bin").c_str(), "rb");
	if (pFile==NULL) {fputs ("File error",stderr); exit (1);}
	if(ni != fread(v.data(),sizeof(double),ni,pFile)){
		fputs ("Read error\n",stderr); exit (1);
	}
	fclose(pFile);
	vector<T> coef;
	for(int i=0;i<ni;i++){
		coef.push_back(T(ij(i,0),ij(i,1),v(i)));
	}
	M.setFromTriplets(coef.begin(), coef.end());
}
Пример #17
0
int Simulation::initializeSimulation(double deltaT, int iterations, char method, MatrixXi& TT, MatrixXd& TV, MatrixXd& B, vector<int>& moveVertices, vector<int> fixVertices, double youngs, double poissons){
	iters = iterations;

	if (method =='e'){
		integrator = new Verlet();
		cout<<"Initialized Verlet"<<endl;	
	}else if(method == 'i'){
		integrator = new ImplicitEuler();
		cout<<"Initialized Implicit Euler"<<endl;
	}
	else if(method == 'n'){
		integrator = new ImplicitNewmark();
		cout<<"Initialized Implicit Newmark"<<endl;
	}
	else{
		cout<<"Method not supported yet"<<endl;
		exit(0);
	}
	VectorXd force;
	force.resize(3*TV.rows());
	force.setZero();

	setInitPosition(force, fixVertices, moveVertices);

	if(moveVertices.size()>0 or fixVertices.size()>0){
		//cout << "DOING STUFFS" << endl;
		MatrixXd newTV;
		newTV.resize(TV.rows(), TV.cols());
		newTV.setZero();
		MatrixXi newTT;
		newTT.resize(TT.rows(), TT.cols());
		newTT.setZero();
		//cout << "MoveVertsSize :: " << moveVertices.size() << endl;

		//TODO: Make this shit more efficient
		//Hash maps or something
		vector<int> vertexNewIndices;
		for(int i=0; i<TV.rows(); i++){
			bool flag =false;
			for(unsigned int j=0; j<fixVertices.size(); j++){
				if(i==fixVertices[j]){
					flag = true;
				}
			}
			for(unsigned int j=0; j<moveVertices.size(); j++){
				if(i==moveVertices[j]){
					flag = true;
				}
			}
			// if vertex not fixed or moved, re-index to front
			//[v, v, v, v...., f, f, f...., m, m, m...,m]
			if(!flag){
				vertexNewIndices.push_back(i);
			}
		}
		//re-index fixed verts
		for(unsigned int j=0; j<fixVertices.size(); j++){
			vertexNewIndices.push_back(fixVertices[j]);
		}
		//re-index move verts
		for(unsigned int j=0; j<moveVertices.size(); j++){
			vertexNewIndices.push_back(moveVertices[j]);
		}

		//these are the new indices for the fixed verts
		vector<int> newfixIndices;
		for(unsigned int i= vertexNewIndices.size() - (moveVertices.size() + fixVertices.size()); i<(vertexNewIndices.size()-moveVertices.size()); i++){
			newfixIndices.push_back(i);
		}

		//new indices for the moving verts
		vector<int> newMoveIndices;
		for(unsigned int i= vertexNewIndices.size() - moveVertices.size(); i<vertexNewIndices.size(); i++){
			newMoveIndices.push_back(i);
		}

		//cout << "NewMoveIndicesSize :: " << newMoveIndices.size() << endl;

		VectorXd new_force;
		new_force.resize(3*TV.rows());
		reIndexTVandTT(vertexNewIndices, fixVertices.size(), moveVertices.size(), TV, TT, force, newTV, newTT, new_force);

		igl::barycenter(newTV, newTT, B);
		//Initialize Solid Mesh
		M.initializeMesh(newTT, newTV, youngs, poissons);
		if(moveVertices.size() != 0){
			// cout<<"Move vertices "<<moveVertices.size()<<endl;
			// cout<<"fix verts "<<fixVertices.size()<<endl;
			binarySearchYoungs(newMoveIndices, newTV, newTT, fixVertices.size(), B);
			// syntheticTests(newMoveIndices, newTV, newTT, fixVertices.size(), B);
		
		}
		
		integrator->initializeIntegrator(deltaT, M, newTV, newTT);
		this->external_force = new_force;
		integrator->fixVertices(newfixIndices);
		int ignorePastIndex = newTV.rows() - newfixIndices.size();
		staticSolveNewtonsForces(newTV, newTT, B, new_force, ignorePastIndex);


	}else{
		//cout << "Doing Other Stuffs" << endl;
		igl::barycenter(TV, TT, B);
		M.initializeMesh(TT, TV, youngs, poissons);
		integrator->initializeIntegrator(deltaT, M, TV, TT);
		this->external_force = force;
		integrator->fixVertices(fixVertices);
	}

	return 1;
}
Пример #18
0
int main(int argc, char *argv[])
{
    FEM *clip_;
    
    clip_ = new GMSH("ClipCpp.inp");
    clip_->assembleK();
    clip_->sortK();
    clip_->sortT();
    clip_->computeInverseForceCondensedK();
    clip_->computeDisplacementCondensedK();

    DeformableObject *defClip_;

    defClip_ = new DeformableObject(*clip_);
    VectorXd eF (defClip_->getSizeF()); // applied forces
    VectorXd uD (defClip_->getSizeD()); // applied displacements
    eF.setZero();
    uD.setZero();
    std::cout << "size of eF = " << eF.size() << std::endl;
    std::cout << "size of uD = " << uD.size() << std::endl;
    defClip_->setDisplayMode(DeformableObject::REGULAR_RENDERING);
    //defClip_->setDisplayMode(DeformableObject::VON_MISES_STRESS);
    defClip_->setNormalsMode(DeformableObject::NORMAL_PER_TRIANGLE);
    //defClip_->setNormalsMode(DeformableObject::NORMAL_PER_VERTEX);

    double t_ = 0.00;

    glfwInit();
    glfwOpenWindow(DEFAULT_W,DEFAULT_H,0,0,0,0,24,0, GLFW_WINDOW);
    GLfloat light0pos[] = { 0.0, 4.0, 6.0, 1.0 };
    GLfloat light1pos[] = { 6.0, 4.0, 0.0, 1.0 };
    GLfloat white[] = { 0.6, 0.6, 0.6, 1.0 };
    glfwSetKeyCallback(keyboard);

    glClearColor(0, 0, 0, 1.0);
    glEnable(GL_DEPTH_TEST);

    glEnable(GL_CULL_FACE);
    glCullFace(GL_BACK);

    glEnable(GL_LIGHTING);
    glEnable(GL_LIGHT0);
    glEnable(GL_LIGHT1);
    glLightfv(GL_LIGHT0, GL_DIFFUSE, white);
    glLightfv(GL_LIGHT0, GL_SPECULAR, white);
    glLightfv(GL_LIGHT1, GL_DIFFUSE, white);
    glLightfv(GL_LIGHT1, GL_SPECULAR, white);
    glLightfv(GL_LIGHT0, GL_POSITION, light0pos);
    glLightfv(GL_LIGHT1, GL_POSITION, light1pos);


    std::cout << std::endl;
    CvVideoWriter *videoWriter;
    IplImage *image;
    while (!quit_flag){
        std::cout << "\r t = " << t_ << std::flush;
        if (movie_flag && t_ == 0.){
            videoWriter = cvCreateVideoWriter(
                "clip.avi",
                CV_FOURCC('D','I','V','X'),
                20,
                cvSize(DEFAULT_W, DEFAULT_H));
            image = cvCreateImage(
                cvSize(DEFAULT_W, DEFAULT_H),
                IPL_DEPTH_8U, 3);
        }

        eF(0)=sin(M_PI*t_)/50000;
        eF(6)=-sin(M_PI*t_)/50000;
    
        // the three calls for the deformation
        defClip_->setForce(eF);
        defClip_->setDisplacement(uD);
        struct timeval t1, t2;
        gettimeofday(&t1, NULL);
        defClip_->solveU();
        gettimeofday(&t2, NULL);
        std::cout << "solveU() : " << (t2.tv_sec - t1.tv_sec)*1000 + (t2.tv_usec - t1.tv_usec)/1000.0 << "[ms]" << std::endl;
        defClip_->updateNodes();

        if (start_flag || movie_flag) t_ += 0.05;

        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        gluPerspective(45, 
                       (double)DEFAULT_W / (double)DEFAULT_H, 
                       0.1, 100);
        double radius = 0.2, eye[3];
        eye[0] = radius*cos(UDangle)*cos(LRangle);
        eye[1] = radius*cos(UDangle)*sin(LRangle);
        eye[2] = radius*sin(UDangle);
        gluLookAt(eye[0], eye[1], eye[2],
                  0,0,0,
                  0,0,1);

        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        glMatrixMode(GL_MODELVIEW);

        defClip_->draw();

        if (movie_flag){
            unsigned char rgb[DEFAULT_W*DEFAULT_H*3];
            glReadBuffer(GL_BACK);
            glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
            for (int i=0; i<DEFAULT_H; i++){
                glReadPixels(0,(DEFAULT_H-1-i),DEFAULT_W,
                             1,GL_RGB,GL_UNSIGNED_BYTE,
                             rgb + i*3*DEFAULT_W);
            }
            char *bgr = image->imageData;
            for (int i=0; i<DEFAULT_W*DEFAULT_H; i++){
                bgr[i*3  ] = rgb[i*3+2]; 
                bgr[i*3+1] = rgb[i*3+1]; 
                bgr[i*3+2] = rgb[i*3  ]; 
            }
            cvWriteFrame(videoWriter, image);
            if (t_ > 10){
                cvReleaseVideoWriter(&videoWriter);
                cvReleaseImage(&image);
                movie_flag = false;
            }
        }

        glfwSwapBuffers();
        usleep(10000);
    }
    std::cout << std::endl;

    return 0;
}
Пример #19
0
void Mesh::elasticEnergy(const VectorXd &q,
                         const VectorXd &g,
                         double &energyB,
                         double &energyS,
                         VectorXd &gradq,
                         Eigen::SparseMatrix<double> &hessq,
                         Eigen::SparseMatrix<double> &gradggradq,
                         int derivativesRequested) const
{
    assert(q.size() == numdofs());
    assert(g.size() == numedges());
    energyB = energyS = 0;

    if(derivativesRequested & ElasticEnergy::DR_DQ)
    {
        gradq.resize(numdofs());
        gradq.setZero();
        if(derivativesRequested & ElasticEnergy::DR_HQ)
            hessq.resize(numdofs(), numdofs());
    }

    if(derivativesRequested & ElasticEnergy::DR_DGDQ)
    {
        gradggradq.resize(numedges(), numdofs());
    }


    vector<Tr> Hqcoeffs;
    vector<Tr> Hgcoeffs;
    vector<Tr> dgdqcoeffs;

    // bending energy
    for(OMMesh::VertexIter vi = mesh_->vertices_begin(); vi != mesh_->vertices_end(); ++vi)
    {
        if(mesh_->is_boundary(vi.handle()))
            continue;

        vector<int> spokeidx;
        vector<int> rightoppidx;
        vector<int> nbidx;
        for(OMMesh::VertexOHalfedgeIter voh = mesh_->voh_iter(vi.handle()); voh; ++voh)
        {
            OMMesh::HalfedgeHandle heh = voh.handle();
            int eidx = mesh_->edge_handle(heh).idx();
            spokeidx.push_back(eidx);

            OMMesh::VertexOHalfedgeIter nextoh = voh;
            ++nextoh;
            if(!nextoh)
                nextoh = mesh_->voh_iter(vi.handle());

            OMMesh::VertexHandle nextvert = mesh_->to_vertex_handle(nextoh.handle());

            OMMesh::HalfedgeHandle opp = mesh_->next_halfedge_handle(heh);;
            if(mesh_->to_vertex_handle(opp) != nextvert)
            {
                opp = mesh_->prev_halfedge_handle(mesh_->opposite_halfedge_handle(heh));
                assert(mesh_->from_vertex_handle(opp) == nextvert);
            }

            int oidx = mesh_->edge_handle(opp).idx();
            rightoppidx.push_back(oidx);

            OMMesh::VertexHandle vh = mesh_->to_vertex_handle(heh);
            nbidx.push_back(vh.idx());
        }

        int centidx = vi.handle().idx();

        energyB += ElasticEnergy::bendingEnergy(q, g, centidx, nbidx, spokeidx, rightoppidx, gradq, Hqcoeffs, dgdqcoeffs, params_, derivativesRequested);
    }

    // Stretching energy
    for(OMMesh::FaceIter it = mesh_->faces_begin(); it != mesh_->faces_end(); ++it)
    {
        int qidx[3];
        int gidx[3];

        int idx=0;
        for(OMMesh::FaceHalfedgeIter fhi = mesh_->fh_iter(it.handle()); fhi; ++fhi)
        {
            assert(idx < 3);
            OMMesh::HalfedgeHandle heh = fhi.handle();
            OMMesh::EdgeHandle eh = mesh_->edge_handle(heh);
            OMMesh::VertexHandle from = mesh_->from_vertex_handle(heh);
            gidx[idx] = eh.idx();
            qidx[(idx+1)%3] = from.idx();
            idx++;
        }
        assert(idx == 3);

        energyS += ElasticEnergy::stretchingEnergy(q, g, qidx, gidx, gradq, Hqcoeffs, dgdqcoeffs, params_, derivativesRequested);
    }

    if(derivativesRequested & ElasticEnergy::DR_HQ)
        hessq.setFromTriplets(Hqcoeffs.begin(), Hqcoeffs.end());
    if(derivativesRequested & ElasticEnergy::DR_DGDQ)
        gradggradq.setFromTriplets(dgdqcoeffs.begin(), dgdqcoeffs.end());
}
double RationalSuperShape2D :: XiSquare7D(
        const vector < Vector2d, aligned_allocator< Vector2d> > Data,
        MatrixXd &alpha,
        VectorXd &beta,
        int functionused,
        bool update) {
    VectorXd dj; dj = VectorXd::Zero(7);
    Matrix3d Tr,Rot, dTrdx0, dTrdy0;
    //functions pointer
    double (RationalSuperShape2D ::*pt2ConstMember)(const Vector2d P, vector<double> &Dffinal) = NULL;
    switch (functionused){
    case 1 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction1; }break;
    case 2 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction2; }break;
    case 3 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction3; }break;
    default : pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction1;
    }
    vector<double> Df;
    double x, y, tht, dthtdx, dthtdy,dthtdx0, dthtdy0, drdth,
            ChiSquare(1e15),
            f(0),
            x0(Get_xoffset()),y0(Get_yoffset()),tht0(Get_thtoffset()),
            dxdx0,dxdy0, dydx0,dydy0;
    //clean memory
    if(update) {
        alpha.setZero();
        beta.setZero();
    }
    //evaluate ChiSquare, components for the beta and matrix alpha
    ChiSquare=0;
    //First define inverse translation T-1
    Tr.setZero();
    Rot.setZero();
    Tr << 1 , 0 , -x0 ,
            0 , 1 , -y0 ,
            0 , 0 , 1;
    //derivatives of translation matrix are constant and can be computed once for all
    dTrdx0 << 0 , 0 , -1 , 0 , 0 , 0 , 0 , 0 , 1;
    dTrdy0 << 0 , 0 , 0 , 0 , 0 , -1 , 0 , 0 , 1;
    //Then define inverse rotation, i.e. transposed rotation
    Rot << cos(tht0) , sin(tht0) , 0 ,
            -sin(tht0) , cos(tht0) , 0 ,
            0 , 0 , 1;
    for(size_t i=0; i<Data.size(); i++){
        double DfDr;
        //global inverse transform is T * R
        Vector2d dum(Data[i]);
        Vector3d dum2(dum[0],dum[1],1);
        //apply inverse transform
        Vector3d dum3 ( Rot * (Tr * dum2));
        Vector2d P(dum3[0],dum3[1]); //2D point in canonical ref
        //get partial derivatives for canonical point
        Vector3d dPdx0 ( Rot * (dTrdx0 * dum2) );
        Vector3d dPdy0 ( Rot * (dTrdy0 * dum2) );
        //simplify notations
        dxdx0 = dPdx0[0];
        dxdy0 = dPdy0[0];
        dydx0 = dPdx0[1];
        dydy0 = dPdy0[1];
        x = P[0]; y = P[1];
        // avoid division by 0 ==> numerical stability
        if (P.norm()<EPSILON) continue; // avoids division by zero
        tht = atan2(y,x); if( tht < 0 ) tht+=2.*M_PI;

        //theta = Arctan(Y/X)
        dthtdx = -sin(tht) ;//-y / (x*x+y*y);
        dthtdy = cos(tht); //x / (x*x+y*y);
        //partial derivatives of theta regarding x offset, y offset, and angular offset
        dthtdx0 = dthtdx * dxdx0 + dthtdy * dydx0;
        dthtdy0 = dthtdx * dxdy0 + dthtdy * dydy0;
        //avoid non differentiable cases
        drdth = DrDtheta(tht);
        f = (*this.*pt2ConstMember)(P, Df); // call to the implicit function
        //
        //compute elements beta[i][0] and alpha[i][j]
        //
        //==> requires partial derivatives!!
        DfDr = Df[2]; //Df/Dr stored at index 2 in array Df during the call to ImplicitFunction1-2-3
        // F1 = R-PL ==> DfDr = 1. ;
        // F2 = 1-PL/R ==> DfDr = PL/R\B2 ;
        // F3 = log ( R\B2/PSL) ==> DfDr = 2/R
        dj.setZero();
        //df/da = df/dr * dr/da
        dj[0] = DfDr * DrDa(tht) ;
        //df/db = df/dr * dr/db
        dj[1] = DfDr * DrDb(tht) ;
        //df/dn1 = df/dr * dr/dn1
        dj[2] = DfDr * DrDn1(tht);
        //df/dn2 = df/dr * dr/dn2
        dj[3] = DfDr * DrDn2(tht);
        //df/dn3 = df/dr * dr/dn3
        dj[4] = DfDr * DrDn3(tht);
        //df/dx0 = df/dr * dr/dtht *dtht/dx0
        dj[5]= DfDr * drdth*dthtdx0;
        //df/dy0 = df/dr * dr/dtht *dtht/dy0
        dj[6]= DfDr * drdth*dthtdy0;
        //squared summation
        ChiSquare += f*f;
        if(update){
            for(int k=0; k<7; k++)
            {
                beta[k] -= f*dj[k];
            }
            //compute approximation of Hessian matrix
            for(int k=0; k<7; k++)
                for(int j=0; j<7; j++)
                    alpha(k,j) += dj[k]*dj[j];
        }
    }//for all vertices
    return ChiSquare;
}
double RationalSuperShape2D :: XiSquare5D(
        const vector < Vector2d, aligned_allocator< Vector2d> > Data,
        MatrixXd &alpha,
        VectorXd &beta,
        int functionused,
        bool update) {
    //five dimensional optimization: a, b, n1, n2, n3 are optimized
    VectorXd dj; dj = VectorXd::Zero(5);
    Matrix3d Tr,Rot;
    //functions pointer
    double (RationalSuperShape2D ::*pt2ConstMember)(const Vector2d P, vector<double> &Dffinal) = NULL;
    switch (functionused){
    case 1 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction1; }break;
    case 2 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction2; }break;
    case 3 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction3; }break;
    default : pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction1;
    }
    vector<double> Df;
    double x, y, tht, ChiSquare(1e15), f(0),
            x0(Get_xoffset()),y0(Get_yoffset()),tht0(Get_thtoffset());
    //clean memory
    if ( update ) {
        alpha.setZero();
        beta.setZero();
    }
    //evaluate ChiSquare, components for the beta and matrix alpha
    ChiSquare=0;
    //First define inverse translation T-1
    Tr.setZero();
    Rot.setZero();
    Tr << 1 , 0 , -x0 ,
            0 , 1 , -y0 ,
            0 , 0 , 1;
    //Then define inverse rotation, i.e. transposed rotation
    Rot << cos(tht0) , sin(tht0) , 0 ,
            -sin(tht0) , cos(tht0) , 0 ,
            0 , 0 , 1;
    //summation of squared potentials
    for(size_t i=0; i<Data.size(); i++ ){
        double DfDr;
        //global inverse transform is T * R
        Vector2d dum(Data[i]);
        Vector3d dum2(dum[0],dum[1],1);
        //apply inverse transform
        Vector3d dum3 ( Rot * (Tr * dum2));
        Vector2d P( dum3[0], dum3[1]); //2D point in canonical referential
        //get partial derivatives for canonical point
        x = P[0]; y = P[1];
        // avoid division by 0 ==> numerical stability
        if (P.norm()<EPSILON) continue; // avoids division by zero
        tht = atan2(y,x); if( tht<0) tht+=2.*M_PI;
        //theta = Arctan(Y/X)

        //avoid non differentiable cases
        f = (*this.*pt2ConstMember)(P, Df); // call to the implicit function
        //
        //compute elements beta[i][0] and alpha[i][j]
        //
        //==> requires partial derivatives!!
        DfDr = Df[2]; //Df/Dr stored at index 2 in array Df during the call to ImplicitFunction1-2-3
        // F1 = R-PL ==> DfDr = 1. ;
        // F2 = 1-PL/R ==> DfDr = PL/R\B2 ;
        // F3 = log ( R\B2/PSL) ==> DfDr = 2/R
        dj.setZero();
        //df/da = df/dr * dr/da
        dj[0] = DfDr * DrDa(tht) ;
        //df/db = df/dr * dr/db
        dj[1] = DfDr * DrDb(tht) ;
        //df/dn1 = df/dr * dr/dn1
        dj[2] = DfDr * DrDn1(tht);
        //df/dn2 = df/dr * dr/dn2
        dj[3] = DfDr * DrDn2(tht);
        //df/dn3 = df/dr * dr/dn3
        dj[4] = DfDr * DrDn3(tht);
        ChiSquare += f*f;
        if(update){
            //gradient beta
            for(int k=0;k<5; k++)
            {
                beta[k] -= f * dj[k];
            }
            //compute approximation of Hessian matrix
            for(int k=0; k<5; k++)
                for(int j=0; j<5; j++)
                    alpha(k,j) += dj[k]*dj[j];
        }
    }//for all vertices
    return ChiSquare;
}
Пример #22
0
// barebones version of the lasso for fixed lambda
// Eigen library is used for linear algebra
// x .............. predictor matrix
// y .............. response
// lambda ......... penalty parameter
// useSubset ...... logical indicating whether lasso should be computed on a
//                  subset
// subset ......... indices of subset on which lasso should be computed
// normalize ...... logical indicating whether predictors should be normalized
// useIntercept ... logical indicating whether intercept should be included
// eps ............ small numerical value (effective zero)
// useGram ........ logical indicating whether Gram matrix should be computed
//                  in advance
// useCrit ........ logical indicating whether to compute objective function
void fastLasso(const MatrixXd& x, const VectorXd& y, const double& lambda,
		const bool& useSubset, const VectorXi& subset, const bool& normalize, 
    const bool& useIntercept, const double& eps, const bool& useGram, 
    const bool& useCrit,
    // intercept, coefficients, residuals and objective function are returned 
    // through the following parameters
    double& intercept, VectorXd& beta, VectorXd& residuals, double& crit) {

	// data initializations
	int n, p = x.cols();
	MatrixXd xs;
	VectorXd ys;
	if(useSubset) {
		n = subset.size();
		xs.resize(n, p);
		ys.resize(n);
		int s;
		for(int i = 0; i < n; i++) {
			s = subset(i);
			xs.row(i) = x.row(s);
			ys(i) = y(s);
		}
	} else {
		n = x.rows();
		xs = x;	// does this copy memory?
		ys = y;	// does this copy memory?
	}
	double rescaledLambda = n * lambda / 2;

	// center data and store means
	RowVectorXd meanX;
	double meanY;
	if(useIntercept) {
		meanX = xs.colwise().mean();	// columnwise means of predictors
		xs.rowwise() -= meanX;			// sweep out columnwise means
		meanY = ys.mean();				// mean of response
		for(int i = 0; i < n; i++) {
			ys(i) -= meanY;				// sweep out mean
		}
	} else {
		meanY = 0;		// just to avoid warning, this is never used
//		intercept = 0;	// zero intercept
	}

	// some initializations
	VectorXi inactive(p);	// inactive predictors
	int m = 0;				// number of inactive predictors
	VectorXi ignores;		// indicates variables to be ignored
	int s = 0;				// number of ignored variables

	// normalize predictors and store norms
	RowVectorXd normX;
  if(normalize) {
    normX = xs.colwise().norm();	// columnwise norms
	  double epsNorm = eps * sqrt(n);	// R package 'lars' uses n, not n-1
	  for(int j = 0; j < p; j++) {
		  if(normX(j) < epsNorm) {
			  // variance is too small: ignore variable
			  ignores.append(j, s);
			  s++;
			  // set norm to tolerance to avoid numerical problems
			  normX(j) = epsNorm;
		  } else {
			  inactive(m) = j;		// add variable to inactive set
			  m++;					// increase number of inactive variables
		  }
		  xs.col(j) /= normX(j);		// sweep out norm
	  }
	  // resize inactive set and update number of variables if necessary
	  if(m < p) {
		  inactive.conservativeResize(m);
		  p = m;
	  }
  } else {
    for(int j = 0; j < p; j++) inactive(j) = j;  // add variable to inactive set
    m = p;
  }

	// compute Gram matrix if requested (saves time if number of variables is
	// not too large)
	MatrixXd Gram;
	if(useGram) {
		Gram.noalias() = xs.transpose() * xs;
	}

	// further initializations for iterative steps
  RowVectorXd corY;
  corY.noalias() = ys.transpose() * xs; // current correlations
  beta.resize(p+s);                     // final coefficients

  // compute lasso solution
  if(p == 1) {

    // special case of only one variable (with sufficiently large norm)
    int j = inactive(0);          
    // set maximum step size in the direction of that variable
    double maxStep = corY(j);
    if(maxStep < 0) maxStep = -maxStep; // absolute value
    // compute coefficients for least squares solution
    VectorXd betaLS = xs.col(j).householderQr().solve(ys);
    // compute lasso coefficients
    beta.setZero();
    if(rescaledLambda < maxStep) {
      // interpolate towards least squares solution
      beta(j) = (maxStep - rescaledLambda) * betaLS(0) / maxStep;
    }

  } else {

    // further initializations for iterative steps
    VectorXi active;  // active predictors
  	int k = 0;        // number of active predictors
    // previous and current regression coefficients
  	VectorXd previousBeta = VectorXd::Zero(p+s), currentBeta = VectorXd::Zero(p+s);
  	// previous and current penalty parameter
    double previousLambda = 0, currentLambda = 0;
  	// indicates variables to be dropped
    VectorXi drops;
  	// keep track of sign of correlations for the active variables 
    // (double precision is necessary for solve())
    VectorXd signs;
  	// Cholesky L of Gram matrix of active variables
    MatrixXd L;
  	int rank = 0;		// rank of Cholesky L
    // maximum number of variables to be sequenced
  	int maxActive = findMaxActive(n, p, useIntercept);

  	// modified LARS algorithm for lasso solution
  	while(k < maxActive) {

  		// extract current correlations of inactive variables
  		VectorXd corInactiveY(m);
  		for(int j = 0; j < m; j++) {
  			corInactiveY(j) = corY(inactive(j));
  		}
  		// compute absolute values of correlations and find maximum
  		VectorXd absCorInactiveY = corInactiveY.cwiseAbs();
  		double maxCor = absCorInactiveY.maxCoeff();
  		// update current lambda
  		if(k == 0) {	// no active variables
  			previousLambda = maxCor;
  		} else {
  			previousLambda = currentLambda;
  		}
  		currentLambda = maxCor;
  		if(currentLambda <= rescaledLambda) break;

  		if(drops.size() == 0) {
  			// new active variables
  			VectorXi newActive = findNewActive(absCorInactiveY, maxCor - eps);
  			// do calculations for new active variables
  			for(int j = 0; j < newActive.size(); j++) {
  				// update Cholesky L of Gram matrix of active variables
  				// TODO: put this into void function
  				int newJ = inactive(newActive(j));
  				VectorXd xNewJ;
  				double newX;
  				if(useGram) {
  					newX = Gram(newJ, newJ);
  				} else {
  					xNewJ = xs.col(newJ);
  					newX = xNewJ.squaredNorm();
  				}
  				double normNewX = sqrt(newX);
  				if(k == 0) {	// no active variables, L is empty
  					L.resize(1,1);
  					L(0, 0) = normNewX;
  					rank = 1;
  				} else {
  					VectorXd oldX(k);
  					if(useGram) {
  						for(int j = 0; j < k; j++) {
  							oldX(j) = Gram(active(j), newJ);
  						}
  					} else {
  						for(int j = 0; j < k; j++) {
  							oldX(j) = xNewJ.dot(xs.col(active(j)));
  						}
  					}
  					VectorXd l = L.triangularView<Lower>().solve(oldX);
  					double lkk = newX - l.squaredNorm();
  					// check if L is machine singular
  					if(lkk > eps) {
  						// no singularity: update Cholesky L
  						lkk = sqrt(lkk);
  						rank++;
  						// add new row and column to Cholesky L
  						// this is a little trick: sometimes we need
  						// lower triangular matrix, sometimes upper
  						// hence we define quadratic matrix and use
  						// triangularView() to interpret matrix the
  						// correct way
  						L.conservativeResize(k+1, k+1);
  						for(int j = 0; j < k; j++) {
  							L(k, j) = l(j);
  							L(j, k) = l(j);
  						}
  						L(k,k) = lkk;
  					}
  				}
  				// add new variable to active set or drop it for good
  				// in case of singularity
  				if(rank == k) {
  					// singularity: drop variable for good
  					ignores.append(newJ, s);
  					s++;	// increase number of ignored variables
  					p--;	// decrease number of variables
  					if(p < maxActive) {
  						// adjust maximum number of active variables
  						maxActive = p;
  					}
  				} else {
  					// no singularity: add variable to active set
  					active.append(newJ, k);
  					// keep track of sign of correlation for new active variable
  					signs.append(sign(corY(newJ)), k);
  					k++;	// increase number of active variables
  				}
  			}
  			// remove new active or ignored variables from inactive variables
  			// and corresponding vector of current correlations
  			inactive.remove(newActive);
  			corInactiveY.remove(newActive);
  			m = inactive.size();	// update number of inactive variables
  		}
  		// prepare for computation of step size
  		// here double precision of signs is necessary
  		VectorXd b = L.triangularView<Lower>().solve(signs);
  		VectorXd G = L.triangularView<Upper>().solve(b);
  		// correlations of active variables with equiangular vector
  		double corActiveU = 1/sqrt(G.dot(signs));
  		// coefficients of active variables in linear combination forming the
  		// equiangular vector
  		VectorXd w = G * corActiveU;	// note that this has the right signs
  		// equiangular vector
  		VectorXd u;
  		if(!useGram) {
  			// we only need equiangular vector if we don't use the precomputed
  			// Gram matrix, otherwise we can compute the correlations directly
  			// from the Gram matrix
  			u = VectorXd::Zero(n);
  			for(int i = 0; i < n; i++) {
  				for(int j = 0; j < k; j++) {
  					u(i) += xs(i, active(j)) * w(j);
  				}
  			}
  		}
  		// compute step size in equiangular direction
  		double step;
  		if(k < maxActive) {
  			// correlations of inactive variables with equiangular vector
  			VectorXd corInactiveU(m);
  			if(useGram) {
  				for(int j = 0; j < m; j++) {
  					corInactiveU(j) = 0;
  					for(int i = 0; i < k; i++) {
  						corInactiveU(j) += w(i) * Gram(active(i), inactive(j));
  					}
  				}
  			} else {
  				for(int j = 0; j < m; j++) {
  					corInactiveU(j) = u.dot(xs.col(inactive(j)));
  				}
  			}
  			// compute step size in the direction of the equiangular vector
  			step = findStep(maxCor, corInactiveY, corActiveU, corInactiveU, eps);
  		} else {
  			// last step: take maximum possible step
  			step = maxCor/corActiveU;
  		}
  		// adjust step size if any sign changes and drop corresponding variables
  		drops = findDrops(currentBeta, active, w, eps, step);
  		// update current regression coefficients
  		previousBeta = currentBeta;
  		for(int j = 0; j < k; j++) {
  			currentBeta(active(j)) += step * w(j);
  		}
  		// update current correlations
  		if(useGram) {
  			// we also need to do this for active variables, since they may be
  			// dropped at a later stage
  			// TODO: computing a vector step * w in advance may save some computation time
  			for(int j = 0; j < Gram.cols(); j++) {
  				for(int i = 0; i < k; i++) {
  					corY(j) -= step * w(i) * Gram(active(i), j);
  				}
  			}
  		} else {
  			ys -= step * u;	// take step in equiangular direction
  			corY.noalias() = ys.transpose() * xs;	// update correlations
  		}
  		// drop variables if necessary
  		if(drops.size() > 0) {
  			// downdate Cholesky L
  			// TODO: put this into void function
  			for(int j = drops.size()-1; j >= 0; j--) {
  				// variables need to be dropped in descending order
  				int drop = drops(j);	// index with respect to active set
  				// modify upper triangular part as in R package 'lars'
  				// simply deleting columns is not enough, other computations
  				// necessary but complicated due to Fortran code
  				L.removeCol(drop);
  				VectorXd z = VectorXd::Constant(k, 1, 1);
  				k--;	// decrease number of active variables
  				for(int i = drop; i < k; i++) {
  					double a = L(i,i), b = L(i+1,i);
  					if(b != 0.0) {
  						// compute the rotation
  						double tau, s, c;
  						if(std::abs(b) > std::abs(a)) {
  							tau = -a/b;
  							s = 1.0/sqrt(1.0+tau*tau);
  							c = s * tau;
  						} else {
  							tau = -b/a;
  							c = 1.0/sqrt(1.0+tau*tau);
  							s = c * tau;
  						}
  						// update 'L' and 'z';
  						L(i,i) = c*a - s*b;
  						for(int j = i+1; j < k; j++) {
  							a = L(i,j);
  							b = L(i+1,j);
  							L(i,j) = c*a - s*b;
  							L(i+1,j) = s*a + c*b;
  						}
  						a = z(i);
  						b = z(i+1);
  						z(i) = c*a - s*b;
  						z(i+1) = s*a + c*b;
  					}
  				}
  				// TODO: removing all rows together may save some computation time
  				L.conservativeResize(k, NoChange);
  				rank--;
  			}
  			// mirror lower triangular part
  			for(int j = 0; j < k; j++) {
  				for(int i = j+1; i < k; i++) {
  					L(i,j) = L(j,i);
  				}
  			}
  			// add dropped variables to inactive set and make sure
  			// coefficients are 0
  			inactive.conservativeResize(m + drops.size());
  			for(int j = 0; j < drops.size(); j++) {
  				int newInactive = active(drops(j));
  				inactive(m + j) = newInactive;
  				currentBeta(newInactive) = 0;	// make sure coefficient is 0
  			}
  			m = inactive.size();	// update number of inactive variables
  			// drop variables from active set and sign vector
  			// number of active variables is already updated above
  			active.remove(drops);
  			signs.remove(drops);
  		}
  	}

  	// interpolate coefficients for given lambda
    if(k == 0) {
      // lambda larger than largest lambda from steps of LARS algorithm
  		beta.setZero();
    } else {
    	// penalty parameter within two steps
      if(k == maxActive) {
          // current coefficients are the least squares solution (in the 
          // high-dimensional case, as far along the solution path as possible)
          // current and previous values of the penalty parameter need to be 
          // reset for interpolation
          previousLambda = currentLambda;
          currentLambda = 0;
      }
      // interpolate coefficients
    	beta = ((rescaledLambda - currentLambda) * previousBeta +
  				(previousLambda - rescaledLambda) * currentBeta) /
  				(previousLambda - currentLambda);
    }
  }

	// transform coefficients back
  VectorXd normedBeta;
	if(normalize) {
    if(useCrit) normedBeta = beta;
    for(int j = 0; j < p; j++) beta(j) /= normX(j);
	}
	if(useIntercept) intercept = meanY - beta.dot(meanX);

  // compute residuals for all observations
  n = x.rows();
  residuals = y - x * beta;
  if(useIntercept) {
    for(int i = 0; i < n; i++) residuals(i) -= intercept;
  }

  // compute value of objective function on the subset
  if(useCrit && useSubset) {
    if(normalize) crit = objective(normedBeta, residuals, subset, lambda);
    else crit = objective(beta, residuals, subset, lambda);
  }
}
Пример #23
0
int main(int argc, char *argv[])
{
    FEM *stopper_;
    struct timeval t1, t2;
    
    stopper_ = new GMSH("09_part_g_free.inp");
    gettimeofday(&t1, NULL);
    stopper_->assembleK();
    gettimeofday(&t2, NULL);
    printf("assembleK() : %6.3f[ms]\n", (t2.tv_sec - t1.tv_sec)*1000 + (t2.tv_usec - t1.tv_usec)/1000.0);
    gettimeofday(&t1, NULL);
    stopper_->sortK();
    gettimeofday(&t2, NULL);
    printf("sortK() : %6.3f[ms]\n", (t2.tv_sec - t1.tv_sec)*1000 + (t2.tv_usec - t1.tv_usec)/1000.0);
    std::cout << "after sortK():" << memused() << "[MB]" << std::endl;
    stopper_->sortT();
    std::cout << "after sortT():" << memused() << "[MB]" << std::endl; 
    gettimeofday(&t1, NULL);
    stopper_->computeInverseForceCondensedK();
    std::cout << "after computing cK:"  << memused() << "[MB]" << std::endl; 
    gettimeofday(&t2, NULL);
    printf("computeInverseForceCondensedK() : %6.3f[ms]\n", (t2.tv_sec - t1.tv_sec)*1000 + (t2.tv_usec - t1.tv_usec)/1000.0);
    stopper_->computeDisplacementCondensedK();
    std::cout << "before creating a DeformableObject:"  << memused() << "[MB]" << std::endl; 

    DeformableObject *defstopper_;

    defstopper_ = new DeformableObject(*stopper_);
    VectorXd eF (defstopper_->getSizeF()); // applied forces
    VectorXd uD (defstopper_->getSizeD()); // applied displacements
    eF.setZero();
    uD.setZero();
    std::cout << "size of eF = " << eF.size() << std::endl;
    std::cout << "size of uD = " << uD.size() << std::endl;
    defstopper_->setDisplayMode(DeformableObject::VON_MISES_STRESS);
    defstopper_->setNormalsMode(DeformableObject::NORMAL_PER_TRIANGLE);
    //defstopper_->setNormalsMode(DeformableObject::NORMAL_PER_VERTEX);

    double t_ = 0.00;

    glfwInit();
    glfwOpenWindow(DEFAULT_W,DEFAULT_H,0,0,0,0,24,0, GLFW_WINDOW);
    GLfloat light0pos[] = { 0.0, 4.0, 6.0, 1.0 };
    GLfloat light1pos[] = { 6.0, 4.0, 0.0, 1.0 };
    GLfloat white[] = { 0.6, 0.6, 0.6, 1.0 };
    glfwSetKeyCallback(keyboard);

    glClearColor(0, 0, 0, 1.0);
    glEnable(GL_DEPTH_TEST);

    glEnable(GL_CULL_FACE);
    glCullFace(GL_BACK);

    glEnable(GL_LIGHTING);
    glEnable(GL_LIGHT0);
    glEnable(GL_LIGHT1);
    glLightfv(GL_LIGHT0, GL_DIFFUSE, white);
    glLightfv(GL_LIGHT0, GL_SPECULAR, white);
    glLightfv(GL_LIGHT1, GL_DIFFUSE, white);
    glLightfv(GL_LIGHT1, GL_SPECULAR, white);
    glLightfv(GL_LIGHT0, GL_POSITION, light0pos);
    glLightfv(GL_LIGHT1, GL_POSITION, light1pos);


    std::cout << std::endl;
    CvVideoWriter *videoWriter;
    IplImage *image;
    double LRangle=M_PI/4;
    double UDangle=M_PI/4;
    int prevState, prevX, prevY;
    prevState = glfwGetMouseButton(GLFW_MOUSE_BUTTON_LEFT);
    glfwGetMousePos(&prevX, &prevY);
    while (!quit_flag){
        std::cout << "\r t = " << t_ << std::flush;
        if (movie_flag && t_ == 0.){
            videoWriter = cvCreateVideoWriter(
                "04_stopper_rotation.avi",
                CV_FOURCC('D','I','V','X'),
                20,
                cvSize(DEFAULT_W, DEFAULT_H));
            image = cvCreateImage(
                cvSize(DEFAULT_W, DEFAULT_H),
                IPL_DEPTH_8U, 3);
        }
        if (edge_flag){
            defstopper_->setDisplayMode(DeformableObject::WIRE_FRAME);
        }else{
            defstopper_->setDisplayMode(DeformableObject::REGULAR_RENDERING);
        }

        eF(1500)=sin(M_PI*t_)*0.1;
    
        // the three calls for the deformation
        defstopper_->setForce(eF);
        defstopper_->setDisplacement(uD);
        struct timeval t1, t2;
        gettimeofday(&t1, NULL);
        defstopper_->solveU();
        gettimeofday(&t2, NULL);
        //printf("\n solveU() : %6.3f[ms]", (t2.tv_sec - t1.tv_sec)*1000 + (t2.tv_usec - t1.tv_usec)/1000.0);
        defstopper_->updateNodes();

        if (start_flag || movie_flag) t_ += 0.05;

        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        gluPerspective(45, 
                       (double)DEFAULT_W / (double)DEFAULT_H, 
                       1, 1000);

        int x,y,state;
        glfwGetMousePos(&x, &y);
        state = glfwGetMouseButton(GLFW_MOUSE_BUTTON_LEFT);
        if (state == GLFW_PRESS && prevState == GLFW_PRESS){
            LRangle -= (x - prevX)*0.01;
            UDangle += (y - prevY)*0.01;
            if (UDangle < -M_PI/2) UDangle = -M_PI/2;
            if (UDangle >  M_PI/2) UDangle =  M_PI/2;
        }
        prevState = state;
        prevX = x;
        prevY = y;
        
        double radius = 100, eye[3];
        double center[3] = {-100.5, 0, 0};
        eye[1] = radius*cos(UDangle)*cos(LRangle)+center[1];
        eye[2] = radius*cos(UDangle)*sin(LRangle)+center[2];
        eye[0] = radius*sin(UDangle)+center[0];
        gluLookAt(eye[0], eye[1], eye[2],
                  center[0], center[1], center[2],
                  1,0,0);

        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        glMatrixMode(GL_MODELVIEW);

        defstopper_->draw();

        if (movie_flag){
            unsigned char rgb[DEFAULT_W*DEFAULT_H*3];
            glReadBuffer(GL_BACK);
            glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
            for (int i=0; i<DEFAULT_H; i++){
                glReadPixels(0,(DEFAULT_H-1-i),DEFAULT_W,
                             1,GL_RGB,GL_UNSIGNED_BYTE,
                             rgb + i*3*DEFAULT_W);
            }
            char *bgr = image->imageData;
            for (int i=0; i<DEFAULT_W*DEFAULT_H; i++){
                bgr[i*3  ] = rgb[i*3+2]; 
                bgr[i*3+1] = rgb[i*3+1]; 
                bgr[i*3+2] = rgb[i*3  ]; 
            }
            cvWriteFrame(videoWriter, image);
            if (t_ > 10){
                cvReleaseVideoWriter(&videoWriter);
                cvReleaseImage(&image);
                movie_flag = false;
            }
        }

        glfwSwapBuffers();
        usleep(10000);
    }
    std::cout << std::endl;

    return 0;
}
Пример #24
0
slep_result_t slep_mc_plain_lr(
		CDotFeatures* features,
		CMulticlassLabels* labels,
		float64_t z,
		const slep_options& options)
{
	int i,j;
	// obtain problem parameters
	int n_feats   = features->get_dim_feature_space();
	int n_vecs    = features->get_num_vectors();
	int n_classes = labels->get_num_classes();

	// labels vector containing values in range (0 .. n_classes)
	SGVector<float64_t> labels_vector = labels->get_labels();

	// initialize matrices and vectors to be used
	// weight vector
	MatrixXd w  = MatrixXd::Zero(n_feats, n_classes);
	// intercepts (biases)
	VectorXd c  = VectorXd::Zero(n_classes);

	if (options.last_result)
	{
		SGMatrix<float64_t> last_w = options.last_result->w;
		SGVector<float64_t> last_c = options.last_result->c;
		for (i=0; i<n_classes; i++)
		{
			c[i] = last_c[i];
			for (j=0; j<n_feats; j++)
				w(j,i) = last_w(j,i);
		}
	}
	// iterative process matrices and vectors
	MatrixXd wp = w, wwp = MatrixXd::Zero(n_feats, n_classes);
	VectorXd cp = c, ccp = VectorXd::Zero(n_classes);
	// search point weight vector
	MatrixXd search_w = MatrixXd::Zero(n_feats, n_classes);
	// search point intercepts
	VectorXd search_c = VectorXd::Zero(n_classes);
	// dot products
	MatrixXd Aw  = MatrixXd::Zero(n_vecs, n_classes);
	for (j=0; j<n_classes; j++)
		features->dense_dot_range(Aw.col(j).data(), 0, n_vecs, NULL, w.col(j).data(), n_feats, 0.0);
	MatrixXd As  = MatrixXd::Zero(n_vecs, n_classes);
	MatrixXd Awp = MatrixXd::Zero(n_vecs, n_classes);
	// gradients
	MatrixXd g   = MatrixXd::Zero(n_feats, n_classes);
	VectorXd gc  = VectorXd::Zero(n_classes);
	// projection
	MatrixXd v   = MatrixXd::Zero(n_feats, n_classes);

	// Lipschitz continuous gradient parameter for line search
	double L = 1.0/(n_vecs*n_classes);
	// coefficients for search point computation
	double alphap = 0, alpha = 1;

	// lambda regularization parameter
	double lambda = z;
	// objective values
	double objective = 0.0;
	double objective_p = 0.0;

	int iter = 0;
	bool done = false;
	CTime time;
	//internal::set_is_malloc_allowed(false);
	while ((!done) && (iter<options.max_iter) && (!CSignal::cancel_computations()))
	{
		double beta = (alphap-1)/alpha;
		// compute search points
		search_w = w + beta*wwp;
		search_c = c + beta*ccp;

		// update dot products with search point
		As = Aw + beta*(Aw-Awp);

		// compute objective and gradient at search point
		double fun_s = 0;
		g.setZero();
		gc.setZero();
		// for each vector
		for (i=0; i<n_vecs; i++)
		{
			// class of current vector
			int vec_class = labels_vector[i];
			// for each class
			for (j=0; j<n_classes; j++)
			{
				// compute logistic loss
				double aa = ((vec_class == j) ? -1.0 : 1.0)*(As(i,j) + search_c(j));
				double bb = aa > 0.0 ? aa : 0.0;
				// avoid underflow via log-sum-exp trick
				fun_s += CMath::log(CMath::exp(-bb) + CMath::exp(aa-bb)) + bb;
				double prob = 1.0/(1+CMath::exp(aa));
				double b = ((vec_class == j) ? -1.0 : 1.0)*(1-prob);///(n_vecs*n_classes);
				// update gradient of intercepts
				gc[j] += b;
				// update gradient of weight vectors
				features->add_to_dense_vec(b, i, g.col(j).data(), n_feats);
			}
		}
		//fun_s /= (n_vecs*n_classes);

		wp = w;
		Awp = Aw;
		cp = c;

		int inner_iter = 0;
		double fun_x = 0;

		// line search process
		while (inner_iter<5000)
		{
			// compute line search point
			v = search_w - g/L;
			c = search_c - gc/L;

			// compute projection of gradient
			eppMatrix(w.data(),v.data(),n_feats,n_classes,lambda/L,options.q);

			v = w - search_w;

			// update dot products
			for (j=0; j<n_classes; j++)
				features->dense_dot_range(Aw.col(j).data(), 0, n_vecs, NULL, w.col(j).data(), n_feats, 0.0);

			// compute objective at search point
			fun_x = 0;
			for (i=0; i<n_vecs; i++)
			{
				int vec_class = labels_vector[i];
				for (j=0; j<n_classes; j++)
				{
					double aa = ((vec_class == j) ? -1.0 : 1.0)*(Aw(i,j) + c(j));
					double bb = aa > 0.0 ? aa : 0.0;
					fun_x += CMath::log(CMath::exp(-bb) + CMath::exp(aa-bb)) + bb;
				}
			}
			//fun_x /= (n_vecs*n_classes);

			// check for termination of line search
			double r_sum = (v.squaredNorm() + (c-search_c).squaredNorm())/2;
			double l_sum = fun_x - fun_s - v.cwiseProduct(g).sum() - (c-search_c).dot(gc);

			// stop if projected gradient is less than 1e-20
			if (r_sum <= 1e-20)
			{
				SG_SINFO("Gradient step makes little improvement (%f)\n",r_sum)
				done = true;
				break;
			}

			if (l_sum <= r_sum*L)
				break;
			else
				L = CMath::max(2*L, l_sum/r_sum);

			inner_iter++;
		}

		// update alpha coefficients
		alphap = alpha;
		alpha = (1+CMath::sqrt(4*alpha*alpha+1))/2;

		// update wwp and ccp
		wwp = w - wp;
		ccp = c - cp;

		// update objectives
		objective_p = objective;
		objective = fun_x;

		// regularize objective with tree norm
		double L1q_norm = 0.0;
		for (int m=0; m<n_classes; m++)
			L1q_norm += w.col(m).norm();
		objective += lambda*L1q_norm;

		//cout << "Objective = " << objective << endl;

		// check for termination of whole process
		if ((CMath::abs(objective - objective_p) < options.tolerance*CMath::abs(objective_p)) && (iter>2))
		{
			SG_SINFO("Objective changes less than tolerance\n")
			done = true;
		}

		iter++;
	}
	SG_SINFO("%d iterations passed, objective = %f\n",iter,objective)
	//internal::set_is_malloc_allowed(true);

	// output computed weight vectors and intercepts
	SGMatrix<float64_t> r_w(n_feats,n_classes);
	for (j=0; j<n_classes; j++)
	{
		for (i=0; i<n_feats; i++)
			r_w(i,j) = w(i,j);
	}
	//r_w.display_matrix();
	SGVector<float64_t> r_c(n_classes);
	for (j=0; j<n_classes; j++)
		r_c[j] = c[j];
	return slep_result_t(r_w, r_c);
};