//==============================================================================
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);
}