C++ Vector2d::transpose方法代码示例

//==============================================================================
std::string toString(const Eigen::Vector2d& _v)
{
  return boost::lexical_cast<std::string>(_v.transpose());
}

void Deformable::projectPositions()
{
    if (mNumVertices <= 1) return;
    int i;//,j,k;
    
    // center of mass
    Eigen::Vector2d cm, originalCm;
    cm.setZero(); originalCm.setZero();
    double mass = 0.0;
    
    for (i = 0; i < mNumVertices; i++) {
        double m = mMasses(i,0);
        if (mFixed(i,0)) m *= 1000.0;
        mass += m;
        cm += mNewPos.row(i) * m;
        originalCm += mOriginalPos.row(i) * m;
        
    }
    //std::cout<<std::endl;
    cm /= mass;
    
    originalCm /= mass;
    
    Eigen::Matrix2d Apq;
    Eigen::Matrix2d Aqq;
    Eigen::Vector2d p;
    Eigen::Vector2d q;
    Apq.setZero();
    Aqq.setZero();
    
    for (i = 0; i < mNumVertices; i++) {
        p(0) = mNewPos(i,0) - cm(0);
        p(1) = mNewPos(i,1) - cm(1);
        
        q(0) = mOriginalPos(i,0) - originalCm(0);
        q(1) = mOriginalPos(i,1) - originalCm(1);
        double m = mMasses(i,0);
        Apq += m*p*q.transpose();
        Aqq += m*q*q.transpose();
    }
  
    /*if (!params.allowFlip && Apq.determinant() < 0.0f) {  	// prevent from flipping
        Apq(0,1) = -Apq(0,1);
        Apq(1,1) = -Apq(1,1);
    }*/
    
    //std::cout<<Apq<<std::endl;
    
    Eigen::Matrix2d R;
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Apq, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::MatrixXd U = svd.matrixU();
    Eigen::MatrixXd V = svd.matrixV();
    R = U*V.transpose();
    if (!params.quadraticMatch) {	// --------- linear match
        
        
        Eigen::Matrix2d A = Aqq;
        

        Eigen::Matrix2d A_=A.inverse();
        /*std::cout<<A_.row(0)<<std::endl;
        std::cout<<A_.row(1)<<std::endl;*/
        A = Apq*A_;
        
        if (params.volumeConservation) {
            double det = A.determinant();
            if(det<0) det=-det;
            if (det != 0.0) {
                det = 1.0 / sqrt(fabs(det));
                if (det > 2.0) det = 2.0;
                A *= det;
            }
        }
        float one_beta =1.0 - params.beta;
        Eigen::Matrix2d T = R * one_beta;
        
        
        A=A*params.beta;
        T = T + A;
        
        
        for (i = 0; i < mNumVertices; i++) {
            if (mFixed(i)) continue;
            q(0) = mOriginalPos(i,0) - originalCm(0);
            q(1) = mOriginalPos(i,1) - originalCm(1);
            Eigen::Vector2d Tq = T*q;
            mGoalPos(i,0) = Tq(0)+cm(0);
            mGoalPos(i,1) = Tq(1)+cm(1);
            //mGoalPos.row(i)=(R * (1.0f - params.beta) + A * params.beta)*q+cm;
            mNewPos.row(i) += (mGoalPos.row(i) - mNewPos.row(i)) * params.alpha;
            
            //std::cout<<T.row(0)<<std::endl;
            //std::cout<<T.row(1)<<std::endl;
        }
    }
}

void Deformable::projectPositionsCluster(std::vector<int> cluster, int cluster_indx)
{
    int numVertices =  cluster.size();
    if (numVertices <= 1) return;
    int i;//,j,k;
    double beta_cluster =params.lbeta.at(cluster_indx);
    
    // center of mass
    Eigen::Vector2d cm, originalCm;
    cm.setZero(); originalCm.setZero();
    double mass = 0.0;
    int indx;
    for (i = 0; i < numVertices; i++) {
        indx= cluster.at(i);
        double m = mMasses(indx,0);
        if (mFixed(indx,0)) m *= 100.0;
        mass += m;
        cm += mNewPos.row(indx) * m;
        originalCm += mOriginalPos.row(indx) * m;
        //std::cout<<"before: "<<mNewPos.row(indx)<<std::endl;
    }
    
    cm /= mass;
    originalCm /= mass;
    
    Eigen::Matrix2d Apq;
    Eigen::Matrix2d Aqq;
    Eigen::Vector2d p;
    Eigen::Vector2d q;
    Apq.setZero();
    Aqq.setZero();
    
    for (i = 0; i < numVertices; i++) {
        indx= cluster.at(i);
        p(0) = mNewPos(indx,0) - cm(0);
        p(1) = mNewPos(indx,1) - cm(1);
        
        q(0) = mOriginalPos(indx,0) - originalCm(0);
        q(1) = mOriginalPos(indx,1) - originalCm(1);
        double m = mMasses(indx,0);
        Apq += m*p*q.transpose();
        Aqq += m*q*q.transpose();
    }
    

    
    if (!params.allowFlip && Apq.determinant() < 0.0f)
    {  	// prevent from flipping
        Apq(0,1) = -Apq(0,1);
        Apq(1,1) = -Apq(1,1);
    }
    
    Eigen::Matrix2d R;
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Apq, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::MatrixXd U = svd.matrixU();
    Eigen::MatrixXd V = svd.matrixV();
    R = U*V.transpose();
    
    if (!params.quadraticMatch) {	// --------- linear match
        Eigen::Matrix2d A = Aqq;
        A = Apq*A.inverse();
        
        if (params.volumeConservation) {
            
            double det = A.determinant();
            if (det != 0.0) {
                det = 1.0 / sqrt(fabs(det));
                if (det > 2.0) det = 2.0;
                
                A = A*det;
            }
        }
        
        Eigen::Matrix2d T = R * (1.0 - beta_cluster) + A * beta_cluster;
        std::cout<<"cluster's beta "<<beta_cluster<<std::endl;
        
        for (i = 0; i < numVertices; i++) {
            int indx= cluster.at(i);
            indxCount.at(indx) +=1;
            
            if (mFixed(indx)) continue;
            q(0) = mOriginalPos(indx,0) - originalCm(0);
            q(1) = mOriginalPos(indx,1) - originalCm(1);
            Eigen::Vector2d Tq = T*q;
            
            mGoalPos(indx,0) = Tq(0)+cm(0);
            mGoalPos(indx,1) = Tq(1)+cm(1);
            mNewPos.row(indx) += (mGoalPos.row(indx) - mNewPos.row(indx)) * params.alpha;
            
            mGoalPos_sum.row(indx) += mNewPos.row(indx);
        }
    }
}

/// Find the plan parameters by computing the Lagrangian coefficients by solving for the 
/// quadratic program
Eigen::Vector3d lagrangian () {

	bool dbg = 0;

	// Create the cost function
	int N = data.size();
	double H_ [N * N], f_ [N];
	for(int i = 0; i < N; i++) {
		for(int j = 0; j < N; j++) 
			H_[i * N + j] = data[i].second * data[j].second * data[i].first.dot(data[j].first);
		f_[i] = -1.0;
		H_[i * (N+1)] += 1e-8;
	}
	QuadProgPP::Matrix <double> H (&(H_[0]), N, N);
	QuadProgPP::Vector <double> f (&(f_[0]), N);
	if(dbg) cout << "H: " << H << endl;
	if(dbg) cout << "f: " << f << endl;

	// Create the inequality constraints (alpha_i >= 0)
	double I_ [N * N], i0_ [N];
	for(int i = 0; i < N; i++) {
		I_[i * (N+1)] = 1.0;
		i0_[i] = 0.0;
	}
	QuadProgPP::Matrix <double> I (&(I_[0]), N, N);
	QuadProgPP::Vector <double> i0 (&(i0_[0]), N);
	if(dbg) cout << "I: " << I << endl;
	if(dbg) cout << "i0: " << i0 << endl;

	// Create the equality constraint ((sum_i alpha_i * y_i) = 0)
	double E_ [N], e0_ [1];
	for(int i = 0; i < N; i++) E_[i] = data[i].second;
	e0_[0] = 0.0;
	QuadProgPP::Matrix <double> E (&(E_[0]), N, 1);
	QuadProgPP::Vector <double> e0 (&(e0_[0]), 1);
	if(dbg) cout << "E: " << E << endl;
	if(dbg) cout << "e0: " << e0 << endl;

	// Solve the problem
	QuadProgPP::Vector <double> x;
	solve_quadprog(H, f, E, e0, I, i0, x);
	if(dbg) cout << "x: " << x << endl;

	// Compute the line direction 
	Eigen::Vector2d w (0, 0);
	Eigen::VectorXd x_ (N), y(N);
	for(int i = 0; i < N; i++) {
		w += x[i] * data[i].second * data[i].first;
		x_(i) = x[i];
		y(i) = data[i].second;
	}
	if(dbg) cout << "w: " << w.transpose() << endl;

	// Compute the line intersection
	int minPos;
	for(int i = 0; i < N; i++) {
		if((x[i] > 0.1) && (data[i].second > 0)) {
			minPos = i;
			break;
		}
	}
	Eigen::MatrixXd X (N,2), G(N,N);
	for(int i = 0; i < N; i++) X.row(i) = data[i].first;
	G = X * X.transpose();
	double b = 1 - (G.row(minPos) * (x_.cwiseProduct(y)));
	if(dbg) printf("b: %lf\n", b);

	return Eigen::Vector3d(w(0), w(1), b);
}