108 lines
3.3 KiB
C++
108 lines
3.3 KiB
C++
|
// This file is part of libigl, a simple c++ geometry processing library.
|
||
|
|
//
|
||
|
|
// Copyright (C) 2015 Alec Jacobson <alecjacobson@gmail.com>
|
||
|
|
//
|
||
|
|
// This Source Code Form is subject to the terms of the Mozilla Public License
|
||
|
|
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
|
||
|
|
// obtain one at http://mozilla.org/MPL/2.0/.
|
||
|
|
#include "progressive_hulls_cost_and_placement.h"
|
||
|
|
#include "quadprog.h"
|
||
|
|
#include "../unique.h"
|
||
|
|
#include "../circulation.h"
|
||
|
|
#include <cassert>
|
||
|
|
#include <vector>
|
||
|
|
#include <limits>
|
||
|
|
|
||
|
|
IGL_INLINE void igl::copyleft::progressive_hulls_cost_and_placement(
|
||
|
|
const int e,
|
||
|
|
const Eigen::MatrixXd & V,
|
||
|
|
const Eigen::MatrixXi & F,
|
||
|
|
const Eigen::MatrixXi & E,
|
||
|
|
const Eigen::VectorXi & EMAP,
|
||
|
|
const Eigen::MatrixXi & EF,
|
||
|
|
const Eigen::MatrixXi & EI,
|
||
|
|
double & cost,
|
||
|
|
Eigen::RowVectorXd & p)
|
||
|
|
{
|
||
|
|
using namespace Eigen;
|
||
|
|
using namespace std;
|
||
|
|
// Controls the amount of quadratic energy to add (too small will introduce
|
||
|
|
// instabilities and flaps)
|
||
|
|
const double w = 0.1;
|
||
|
|
|
||
|
|
assert(V.cols() == 3 && "V.cols() should be 3");
|
||
|
|
// Gather list of unique face neighbors
|
||
|
|
vector<int> Nall = circulation(e, true,F,E,EMAP,EF,EI);
|
||
|
|
vector<int> Nother= circulation(e,false,F,E,EMAP,EF,EI);
|
||
|
|
Nall.insert(Nall.end(),Nother.begin(),Nother.end());
|
||
|
|
vector<int> N;
|
||
|
|
igl::unique(Nall,N);
|
||
|
|
// Gather:
|
||
|
|
// A #N by 3 normals scaled by area,
|
||
|
|
// D #N determinants of matrix formed by points as columns
|
||
|
|
// B #N point on plane dot normal
|
||
|
|
MatrixXd A(N.size(),3);
|
||
|
|
VectorXd D(N.size());
|
||
|
|
VectorXd B(N.size());
|
||
|
|
//cout<<"N=[";
|
||
|
|
for(int i = 0;i<N.size();i++)
|
||
|
|
{
|
||
|
|
const int f = N[i];
|
||
|
|
//cout<<(f+1)<<" ";
|
||
|
|
const RowVector3d & v01 = V.row(F(f,1))-V.row(F(f,0));
|
||
|
|
const RowVector3d & v20 = V.row(F(f,2))-V.row(F(f,0));
|
||
|
|
A.row(i) = v01.cross(v20);
|
||
|
|
B(i) = V.row(F(f,0)).dot(A.row(i));
|
||
|
|
D(i) =
|
||
|
|
(Matrix3d()<< V.row(F(f,0)), V.row(F(f,1)), V.row(F(f,2)))
|
||
|
|
.finished().determinant();
|
||
|
|
}
|
||
|
|
//cout<<"];"<<endl;
|
||
|
|
// linear objective
|
||
|
|
Vector3d f = A.colwise().sum().transpose();
|
||
|
|
VectorXd x;
|
||
|
|
//bool success = linprog(f,-A,-B,MatrixXd(0,A.cols()),VectorXd(0,1),x);
|
||
|
|
//VectorXd _;
|
||
|
|
//bool success = mosek_linprog(f,A.sparseView(),B,_,_,_,env,x);
|
||
|
|
//if(success)
|
||
|
|
//{
|
||
|
|
// cost = (1./6.)*(x.dot(f) - D.sum());
|
||
|
|
//}
|
||
|
|
bool success = false;
|
||
|
|
{
|
||
|
|
RowVectorXd mid = 0.5*(V.row(E(e,0))+V.row(E(e,1)));
|
||
|
|
MatrixXd G = w*Matrix3d::Identity(3,3);
|
||
|
|
VectorXd g0 = (1.-w)*f - w*mid.transpose();
|
||
|
|
const int n = A.cols();
|
||
|
|
success = quadprog(
|
||
|
|
G,g0,
|
||
|
|
MatrixXd(n,0),VectorXd(0,1),
|
||
|
|
A.transpose(),-B,x);
|
||
|
|
cost = (1.-w)*(1./6.)*(x.dot(f) - D.sum()) +
|
||
|
|
w*(x.transpose()-mid).squaredNorm() +
|
||
|
|
w*(V.row(E(e,0))-V.row(E(e,1))).norm();
|
||
|
|
}
|
||
|
|
|
||
|
|
// A x >= B
|
||
|
|
// A x - B >=0
|
||
|
|
// This is annoyingly necessary. Seems the solver is letting some garbage
|
||
|
|
// slip by.
|
||
|
|
success = success && ((A*x-B).minCoeff()>-1e-10);
|
||
|
|
if(success)
|
||
|
|
{
|
||
|
|
p = x.transpose();
|
||
|
|
//assert(cost>=0 && "Cost should be positive");
|
||
|
|
}else
|
||
|
|
{
|
||
|
|
cost = std::numeric_limits<double>::infinity();
|
||
|
|
//VectorXi NM;
|
||
|
|
//igl::list_to_matrix(N,NM);
|
||
|
|
//cout<<matlab_format((NM.array()+1).eval(),"N")<<endl;
|
||
|
|
//cout<<matlab_format(f,"f")<<endl;
|
||
|
|
//cout<<matlab_format(A,"A")<<endl;
|
||
|
|
//cout<<matlab_format(B,"B")<<endl;
|
||
|
|
//exit(-1);
|
||
|
|
p = RowVectorXd::Constant(1,3,std::nan("inf-cost"));
|
||
|
|
}
|
||
|
|
}
|