// Converts a representative vector per face in the full set of vectors that describe
// an N-RoSy field
void representative_to_nrosy(
const Eigen::MatrixXd& V,
const Eigen::MatrixXi& F,
const Eigen::MatrixXd& R,
const int N,
Eigen::MatrixXd& Y)
{
using namespace Eigen;
using namespace std;
MatrixXd B1, B2, B3;
igl::local_basis(V,F,B1,B2,B3);
Y.resize(F.rows()*N,3);
for (unsigned i=0; i<F.rows(); ++i)
{
double x = R.row(i) * B1.row(i).transpose();
double y = R.row(i) * B2.row(i).transpose();
double angle = atan2(y,x);
for (unsigned j=0; j<N; ++j)
{
double anglej = angle + 2*M_PI*double(j)/double(N);
double xj = cos(anglej);
double yj = sin(anglej);
Y.row(i*N+j) = xj * B1.row(i) + yj * B2.row(i);
}
}
}
void color_intersections(
const Eigen::MatrixXd & V,
const Eigen::MatrixXi & F,
const Eigen::MatrixXd & U,
const Eigen::MatrixXi & G,
Eigen::MatrixXd & C,
Eigen::MatrixXd & D)
{
using namespace igl;
using namespace igl::cgal;
using namespace Eigen;
MatrixXi IF;
const bool first_only = false;
intersect_other(V,F,U,G,first_only,IF);
C.resize(F.rows(),3);
C.col(0).setConstant(0.4);
C.col(1).setConstant(0.8);
C.col(2).setConstant(0.3);
D.resize(G.rows(),3);
D.col(0).setConstant(0.4);
D.col(1).setConstant(0.3);
D.col(2).setConstant(0.8);
for(int f = 0;f<IF.rows();f++)
{
C.row(IF(f,0)) = RowVector3d(1,0.4,0.4);
D.row(IF(f,1)) = RowVector3d(0.8,0.7,0.3);
}
}
std::tuple<Eigen::VectorXi, Eigen::MatrixXd, Eigen::MatrixXd>
lu_row_pivoting(const Eigen::MatrixXd & A)
{
Eigen::MatrixXd Acopy = A;
int n = Acopy.rows();
assert(n == Acopy.cols());
Eigen::VectorXi p = Eigen::VectorXi::LinSpaced(n,1,n);
Eigen::MatrixXd L = Eigen::MatrixXd::Zero(n,n);
Eigen::MatrixXd U = Eigen::MatrixXd::Zero(n,n);
for (int k=0; k<n-1; k++) {
int maxRow, maxCol;
Acopy.block(k,k,n-k,1).array().abs().maxCoeff(&maxRow, &maxCol);
Acopy.row(k).swap(Acopy.row(maxRow+k));
p.row(k).swap(p.row(maxRow+k));
L.row(k).swap(L.row(maxRow+k));
lu_helper(k, n, &Acopy, &L, &U);
}
L(n-1,n-1) = 1;
U(n-1,n-1) = Acopy(n-1,n-1);
return std::make_tuple(p,L,U);
}
void save_bvecs_bvals (const Header& header, const std::string& bvecs_path, const std::string& bvals_path)
{
const auto grad = parse_DW_scheme (header);
// rotate vectors from scanner space to image space
Eigen::MatrixXd G = grad.leftCols<3>() * header.transform().rotation();
// deal with FSL requiring gradient directions to coincide with data strides
// also transpose matrices in preparation for file output
vector<size_t> order;
auto adjusted_transform = File::NIfTI::adjust_transform (header, order);
Eigen::MatrixXd bvecs (3, grad.rows());
Eigen::MatrixXd bvals (1, grad.rows());
for (ssize_t n = 0; n < G.rows(); ++n) {
bvecs(0,n) = header.stride(order[0]) > 0 ? G(n,order[0]) : -G(n,order[0]);
bvecs(1,n) = header.stride(order[1]) > 0 ? G(n,order[1]) : -G(n,order[1]);
bvecs(2,n) = header.stride(order[2]) > 0 ? G(n,order[2]) : -G(n,order[2]);
bvals(0,n) = grad(n,3);
}
// bvecs format actually assumes a LHS coordinate system even if image is
// stored using RHS - x axis is flipped to make linear 3x3 part of
// transform have negative determinant:
if (adjusted_transform.linear().determinant() > 0.0)
bvecs.row(0) = -bvecs.row(0);
save_matrix (bvecs, bvecs_path);
save_matrix (bvals, bvals_path);
}
IGL_INLINE bool igl::decimate(
const Eigen::MatrixXd & V,
const Eigen::MatrixXi & F,
const size_t max_m,
Eigen::MatrixXd & U,
Eigen::MatrixXi & G)
{
int m = F.rows();
const auto & shortest_edge_and_midpoint = [](
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)
{
cost = (V.row(E(e,0))-V.row(E(e,1))).norm();
p = 0.5*(V.row(E(e,0))+V.row(E(e,1)));
};
return decimate(
V,
F,
shortest_edge_and_midpoint,
max_faces_stopping_condition(m,max_m),
U,
G);
}
UnifiedModel::UnifiedModel(const Eigen::MatrixXd& centers, const Eigen::MatrixXd& widths, const Eigen::VectorXd& weights, bool normalized_basis_functions, bool lines_pivot_at_max_activation)
{
int n_basis_functions = centers.rows();
int n_dims = centers.cols();
assert(n_basis_functions==widths.rows());
assert(n_dims ==widths.cols());
assert(n_basis_functions==weights.size());
centers_.resize(n_basis_functions);
covars_.resize(n_basis_functions);
slopes_.resize(n_basis_functions);
offsets_.resize(n_basis_functions);
priors_.resize(n_basis_functions);
for (int i=0; i<n_basis_functions; i++)
{
centers_[i] = centers.row(i);
covars_[i] = widths.row(i).asDiagonal();
covars_[i] = covars_[i].array().square();
offsets_[i] = weights[i];
slopes_[i] = VectorXd::Zero(n_dims);
priors_[i] = 1.0;
}
normalized_basis_functions_ = normalized_basis_functions;
lines_pivot_at_max_activation_ = lines_pivot_at_max_activation;
slopes_as_angles_ = false;
cosine_basis_functions_ = false;
initializeAllValuesVectorSize();
};
void shortest_edge_and_midpoint1(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,
RowVectorXd &p) {
// vectorsum
const int eflip = E(e, 0) > E(e, 1);
const std::vector<int> nV2Fd =
igl::circulation(e, !eflip, F, E, EMAP, EF, EI);
p = 0.5 * (V.row(E(e, 0)) + V.row(E(e, 1)));
Eigen::RowVectorXd pointy(3);
pointy.setZero();
std::set<int> newEdges;
for (int i = 0; i < nV2Fd.size(); i++) {
for (int j = 0; j < 3; j++) {
int curVert = F.row(nV2Fd[i])[j];
if (curVert != E(e, 0) || curVert != E(e, 1)) {
if (newEdges.insert(curVert).second) {
pointy = (V.row(curVert) - p) + pointy;
}
}
}
}
cost = (pointy).norm();
}
void Model::compute_subjective_value(std::vector<std::vector<double>> &U,
const Eigen::MatrixXd &M,
const Parameter ¶meter) {
assert(n_dimensions == 2);
U.resize(n_alternatives);
double a, b, angle;
for (unsigned int i = 0; i < n_alternatives; i++) {
U.at(i).resize(n_dimensions);
a = M.row(i).sum();
b = a;
angle = atan(M.row(i)(1) / M.row(i)(0));
U.at(i).at(0) =
b / pow(pow(tan(angle), parameter.m) + pow(b / a, parameter.m),
1 / parameter.m);
U.at(i).at(1) =
b * pow(1 - pow(U.at(i).at(0) / a, parameter.m), 1 / parameter.m);
}
}
gaussian_process::gaussian_process( const Eigen::MatrixXd& domains
, const Eigen::VectorXd& targets
, const gaussian_process::covariance& covariance
, double self_covariance )
: domains_( domains )
, targets_( targets )
, covariance_( covariance )
, self_covariance_( self_covariance )
, offset_( targets.sum() / targets.rows() )
, K_( domains.rows(), domains.rows() )
{
if( domains.rows() != targets.rows() ) { COMMA_THROW( comma::exception, "expected " << domains.rows() << " row(s) in targets, got " << targets.rows() << " row(s)" ); }
targets_.array() -= offset_; // normalise
//use m_K as Kxx + variance*I, then invert it
//fill Kxx with values from covariance function
//for elements r,c in upper triangle
for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r )
{
K_( r, r ) = self_covariance_;
const Eigen::VectorXd& row = domains.row( r );
for( std::size_t c = r + 1; c < std::size_t( domains.rows() ); ++c )
{
K_( c, r ) = K_( r, c ) = covariance_( row, domains.row( c ) );
}
}
L_.compute( K_ ); // invert Kxx + variance * I to become (by definition) B
alpha_ = L_.solve( targets_ );
}
IGL_INLINE void igl::ViewerData::add_edges(const Eigen::MatrixXd& P1, const Eigen::MatrixXd& P2, const Eigen::MatrixXd& C)
{
Eigen::MatrixXd P1_temp,P2_temp;
// If P1 only has two columns, pad with a column of zeros
if (P1.cols() == 2)
{
P1_temp = Eigen::MatrixXd::Zero(P1.rows(),3);
P1_temp.block(0,0,P1.rows(),2) = P1;
P2_temp = Eigen::MatrixXd::Zero(P2.rows(),3);
P2_temp.block(0,0,P2.rows(),2) = P2;
}
else
{
P1_temp = P1;
P2_temp = P2;
}
int lastid = lines.rows();
lines.conservativeResize(lines.rows() + P1_temp.rows(),9);
for (unsigned i=0; i<P1_temp.rows(); ++i)
lines.row(lastid+i) << P1_temp.row(i), P2_temp.row(i), i<C.rows() ? C.row(i) : C.row(C.rows()-1);
dirty |= DIRTY_OVERLAY_LINES;
}
Eigen::MatrixXd linearCamera( Eigen::MatrixXd &xpt, Eigen::MatrixXd &Xpt)
{
int n = xpt.cols();
if (n < 6)
{
DEBUG_E( ("Camera Matrix can't be estimated in linearCamera(). Minimun required points are 6") );
exit(-1);
}
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(2*n,12);
for (register int k = 0; k < n; ++k)
{
Eigen::RowVectorXd St(4);
St << Xpt(0,k), Xpt(1,k), Xpt(2,k), Xpt(3,k);
A.row(2*k) << Eigen::RowVectorXd::Zero(4), -xpt(2,k)*St, xpt(1,k)*St;
A.row(2*k+1) << xpt(2,k)*St, Eigen::RowVectorXd::Zero(4), -xpt(0,k)*St;
}
// std::cout << "A:\n " << A <<'\n';
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullV | Eigen::ComputeThinU);
Eigen::MatrixXd VV = svd.matrixV();
Eigen::MatrixXd P(3,4);
P << VV(0,11), VV(1,11), VV(2,11), VV(3,11),
VV(4,11), VV(5,11), VV(6,11), VV(7,11),
VV(8,11), VV(9,11), VV(10,11), VV(11,11);
P = P/P(2,3);
return P;
}
Eigen::MatrixXd kalmanFilter::kalmanFunc(Eigen::MatrixXd phi, Eigen::MatrixXd upsilon, Eigen::MatrixXd basis, Eigen::MatrixXd initial, Eigen::MatrixXd initial_cov, int measurements, Eigen::MatrixXd noise){
Eigen::MatrixXd x(measurements,2), xe(measurements,2), ym(measurements,1), covariance(measurements,2);
Eigen::MatrixXd gain;
x.setZero(measurements,2);
x.row(0) = initial;
xe.setZero(measurements,2);
ym.setZero(measurements,1);
ym.row(0) = (basis*x.row(0).transpose()).transpose()+(noise.cwiseSqrt().row(0))*Eigen::MatrixXd::Random(1,1);
covariance.setZero(measurements,2);
covariance.row(0) = initial_cov.diagonal().transpose();
// Main loop
for(int i=0; i<(measurements-1); i++){
// Truth and Measurements
x.row(i+1) = (phi*x.row(i).transpose()+upsilon*(noise.cwiseSqrt().row(1))*Eigen::MatrixXd::Random(1,1)).transpose();
ym.row(i+1) = (basis*x.row(i+1).transpose()).transpose()+(noise.cwiseSqrt().row(0))*Eigen::MatrixXd::Random(1,1);
// Update Equations
gain = initial_cov*basis.transpose()*((basis*initial_cov*basis.transpose())+noise.row(0)).cwiseInverse();
initial_cov = (Eigen::MatrixXd::Identity(2,2)-gain*basis)*initial_cov;
xe.row(i) = xe.row(i)+(gain*(ym.row(i)-basis*xe.row(i).transpose())).transpose();
// Propagation Equations
xe.row(i+1) = (phi*xe.row(i).transpose()).transpose();
initial_cov = phi*initial_cov*phi.transpose()+upsilon*noise.row(1)*upsilon.transpose();
covariance.row(i+1) = initial_cov.diagonal().transpose();
}
Eigen::MatrixXd result(measurements,6);
result << xe, x, (covariance.cwiseSqrt())*3;
return result;
}
// computes the optimal rigid body transformation given a set of points
void PoseTracker::computeOptimalRigidTransformation(Eigen::MatrixXd startP, Eigen::MatrixXd finalP)
{
Eigen::Matrix4d transf;
if (startP.rows()!=finalP.rows())
{
ROS_ERROR("We need that the rows be the same at the beggining");
exit(1);
}
Eigen::RowVector3d centroid_startP = Eigen::RowVector3d::Zero();
Eigen::RowVector3d centroid_finalP = Eigen::RowVector3d::Zero(); //= mean(B);
Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
//calculate the mean
for (int i=0;i<startP.rows();i++)
{
centroid_startP = centroid_startP+startP.row(i);
centroid_finalP = centroid_finalP+finalP.row(i);
}
centroid_startP = centroid_startP/startP.rows();
centroid_finalP = centroid_finalP/startP.rows();
for (int i=0;i<startP.rows();i++)
H = H + (startP.row(i)-centroid_startP).transpose()*(finalP.row(i)-centroid_finalP);
Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::MatrixXd U = svd.matrixU();
Eigen::MatrixXd V = svd.matrixV();
if (V.determinant()<0)
V.col(2)=-V.col(2)*(-1);
Eigen::MatrixXd R = V*U.transpose();
Eigen::Matrix4d C_A = Eigen::Matrix4d::Identity();
Eigen::Matrix4d C_B = Eigen::Matrix4d::Identity();
Eigen::Matrix4d R_new = Eigen::Matrix4d::Identity();
C_A.block<3,1>(0,3) = -centroid_startP.transpose();
R_new.block<3,3>(0,0) = R;
C_B.block<3,1>(0,3) = centroid_finalP.transpose();
transf = C_B * R_new * C_A;
Eigen::Quaterniond mat_rot(transf.block<3,3>(0,0));
Eigen::Vector3d trasl = transf.block<3,1>(0,3).transpose();
transfParameters_ << trasl(0), trasl(1), trasl(2), mat_rot.x(), mat_rot.y(), mat_rot.z(), mat_rot.w();
}
void StompOptimizationTask::computeCosts(const Eigen::MatrixXd& features, Eigen::VectorXd& costs, Eigen::MatrixXd& weighted_feature_values) const
{
weighted_feature_values = Eigen::MatrixXd::Zero(num_time_steps_, num_split_features_);
for (int t=0; t<num_time_steps_; ++t)
{
weighted_feature_values.row(t) = (((features.row(t+stomp::TRAJECTORY_PADDING) - feature_means_.transpose()).array() /
feature_variances_.array().transpose()) * feature_weights_.array().transpose()).matrix();
}
costs = weighted_feature_values.rowwise().sum();
}
void reflect_points3d(const Eigen::MatrixXd& ptsin, const Eigen::Vector3d& center, const Eigen::Vector3d& normal, Eigen::MatrixXd& ptsout)
{
ptsout.resize(ptsin.rows(), ptsin.cols() );
for ( int i = 0; i < (int) ptsin.rows(); ++i)
{
Eigen::Vector3d ptout;
reflect_point3d(ptsin.row(i), center, normal, ptout);
ptsout.row(i) = ptout;
}
}
void cMotion::PostProcessFrames(Eigen::MatrixXd& frames) const
{
int num_frames = static_cast<int>(frames.rows());
double curr_time = gMinTime;
for (int f = 0; f < num_frames; ++f)
{
double duration = frames.row(f)(0, eFrameTime);
frames.row(f)(0, eFrameTime) = curr_time;
curr_time += duration;
}
}
void shortest_edge_and_midpoint8(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,
RowVectorXd &p) {
// random
p = 0.5 * (V.row(E(e, 0)) + V.row(E(e, 1)));
cost = rand();
}
void shortest_edge_and_midpoint3(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,
RowVectorXd &p) {
// manhattan
cost = (V.row(E(e, 0)) - V.row(E(e, 1))).cwiseAbs().sum();
p = 0.5 * (V.row(E(e, 0)) + V.row(E(e, 1)));
}
void shortest_edge_and_midpoint4(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,
RowVectorXd &p) {
// euclidean
cost = (V.row(E(e, 0)) - V.row(E(e, 1))).norm();
p = 0.5 * (V.row(E(e, 0)) + V.row(E(e, 1)));
}
double Statistics::distance(const Eigen::MatrixXd& m1, const Eigen::MatrixXd& m2, int& row1, int& row2, double& p)
{
double dist = m1.row(row1).dot(m2.row(row2));
int i_p = static_cast <int> (p);
if (i_p != p) // if a root of dist should be taken, then dist may not be negative
{
dist = std::abs(dist);
}
return pow(dist, p);
}
Eigen::MatrixXd eddy2scheme (const Eigen::MatrixXd& config, const Eigen::Array<int, Eigen::Dynamic, 1>& indices)
{
if (config.cols() != 4)
throw Exception ("Expected 4 columns in EDDY-format phase-encoding config file");
Eigen::MatrixXd result (indices.size(), 4);
for (ssize_t row = 0; row != indices.size(); ++row) {
if (indices[row] > config.rows())
throw Exception ("Malformed EDDY-style phase-encoding information: Index exceeds number of config entries");
result.row(row) = config.row(indices[row]-1);
}
return result;
}
void compute_C_asymmetric_full(Eigen::MatrixXd &W, Eigen::MatrixXd &C, const Eigen::MatrixXd &D, int n)
{
#pragma omp parallel for schedule(dynamic)
for (int i=0; i<n; ++i)
{
for (int j=0; j<n; ++j)
{
double s=0.0;
s = 0.5*(W.row(j)+W.row(i)).sum();// row access is slower than columns access
C.coeffRef(i,j) += 2.0*(W.coeffRef(i,j)+s)/(D.coeffRef(i)+D.coeffRef(j));
}
}
}
IGL_INLINE void igl::ViewerData::set_colors(const Eigen::MatrixXd &C)
{
using namespace std;
using namespace Eigen;
// Ambient color should be darker color
const auto ambient = [](const MatrixXd & C)->MatrixXd
{
return 0.1*C;
};
// Specular color should be a less saturated and darker color: dampened
// highlights
const auto specular = [](const MatrixXd & C)->MatrixXd
{
const double grey = 0.3;
return grey+0.1*(C.array()-grey);
};
if (C.rows() == 1)
{
for (unsigned i=0;i<V_material_diffuse.rows();++i)
{
V_material_diffuse.row(i) = C.row(0);
}
V_material_ambient = ambient(V_material_diffuse);
V_material_specular = specular(V_material_diffuse);
for (unsigned i=0;i<F_material_diffuse.rows();++i)
{
F_material_diffuse.row(i) = C.row(0);
}
F_material_ambient = ambient(F_material_diffuse);
F_material_specular = specular(F_material_diffuse);
}
else if (C.rows() == V.rows())
{
set_face_based(false);
V_material_diffuse = C;
V_material_ambient = ambient(V_material_diffuse);
V_material_specular = specular(V_material_diffuse);
}
else if (C.rows() == F.rows())
{
set_face_based(true);
F_material_diffuse = C;
F_material_ambient = ambient(F_material_diffuse);
F_material_specular = specular(F_material_diffuse);
}
else
cerr << "ERROR (set_colors): Please provide a single color, or a color per face or per vertex.";
dirty |= DIRTY_DIFFUSE;
}
IGL_INLINE void igl::signed_distance_pseudonormal(
const Eigen::MatrixXd & P,
const Eigen::MatrixXd & V,
const Eigen::MatrixXi & F,
const AABB<Eigen::MatrixXd,3> & tree,
const Eigen::MatrixXd & FN,
const Eigen::MatrixXd & VN,
const Eigen::MatrixXd & EN,
const Eigen::VectorXi & EMAP,
Eigen::VectorXd & S,
Eigen::VectorXi & I,
Eigen::MatrixXd & C,
Eigen::MatrixXd & N)
{
using namespace Eigen;
const size_t np = P.rows();
S.resize(np,1);
I.resize(np,1);
N.resize(np,3);
C.resize(np,3);
# pragma omp parallel for if(np>1000)
for(size_t p = 0;p<np;p++)
{
double s,sqrd;
RowVector3d n,c;
int i = -1;
RowVector3d q = P.row(p);
signed_distance_pseudonormal(tree,V,F,FN,VN,EN,EMAP,q,s,sqrd,i,c,n);
S(p) = s*sqrt(sqrd);
I(p) = i;
N.row(p) = n;
C.row(p) = c;
}
// igl::AABB<MatrixXd,3> tree_P;
// MatrixXi J = VectorXi::LinSpaced(P.rows(),0,P.rows()-1);
// tree_P.init(P,J);
// tree.squared_distance(V,F,tree_P,P,J,S,I,C);
//# pragma omp parallel for if(np>1000)
// for(size_t p = 0;p<np;p++)
// {
// RowVector3d c = C.row(p);
// RowVector3d q = P.row(p);
// const int f = I(p);
// double s;
// RowVector3d n;
// pseudonormal_test(V,F,FN,VN,EN,EMAP,q,f,c,s,n);
// N.row(p) = n;
// S(p) = s*sqrt(S(p));
// }
}
void save(const Eigen::MatrixXd& in, const Eigen::MatrixXd& out, std::ostream& stream)
{
OPENANN_CHECK_EQUALS(in.rows(), out.rows());
int N = in.rows();
int D = in.cols();
int F = out.cols();
stream << N << " " << D << " " << F << std::endl;
for(int n = 0; n < N; n++)
{
stream << in.row(n) << std::endl;
stream << out.row(n) << std::endl;
}
}
void shortest_edge_and_midpoint5(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,
RowVectorXd &p) {
// Perceived edge length
p = 0.5 * (V.row(E(e, 0)) + V.row(E(e, 1)));
Vector3d eye = viewer.core.camera_eye.cast<double>();
eye.normalize();
cost = acos(normals.row(EF(e, 0)).dot(normals.row(EF(e, 1)))) *
abs(eye.dot(normals.row(EF(e, 0)))) *
abs(eye.dot(normals.row(EF(e, 1))));
}
bool Polygon::convertToInequalityConstraints(Eigen::MatrixXd& A, Eigen::VectorXd& b) const
{
Eigen::MatrixXd V(nVertices(), 2);
for (unsigned int i = 0; i < nVertices(); ++i)
V.row(i) = vertices_[i];
// Create k, a list of indices from V forming the convex hull.
// TODO: Assuming counter-clockwise ordered convex polygon.
// MATLAB: k = convhulln(V);
Eigen::MatrixXi k;
k.resizeLike(V);
for (unsigned int i = 0; i < V.rows(); ++i)
k.row(i) << i, (i+1) % V.rows();
Eigen::RowVectorXd c = V.colwise().mean();
V.rowwise() -= c;
A = Eigen::MatrixXd::Constant(k.rows(), V.cols(), NAN);
unsigned int rc = 0;
for (unsigned int ix = 0; ix < k.rows(); ++ix) {
Eigen::MatrixXd F(2, V.cols());
F.row(0) << V.row(k(ix, 0));
F.row(1) << V.row(k(ix, 1));
Eigen::FullPivLU<Eigen::MatrixXd> luDecomp(F);
if (luDecomp.rank() == F.rows()) {
A.row(rc) = F.colPivHouseholderQr().solve(Eigen::VectorXd::Ones(F.rows()));
++rc;
}
}
A = A.topRows(rc);
b = Eigen::VectorXd::Ones(A.rows());
b = b + A * c.transpose();
return true;
}
void Mesh::computeFaceGradients(Eigen::MatrixXd& gradients, const Eigen::VectorXd& u) const
{
for (FaceCIter f = faces.begin(); f != faces.end(); f++) {
Eigen::Vector3d gradient;
gradient.setZero();
Eigen::Vector3d normal = f->normal();
normal.normalize();
HalfEdgeCIter he = f->he;
do {
double ui = u(he->next->next->vertex->index);
Eigen::Vector3d ei = he->next->vertex->position - he->vertex->position;
gradient += ui * normal.cross(ei);
he = he->next;
} while (he != f->he);
gradient /= (2.0 * f->area());
gradient.normalize();
gradients.row(f->index) = -gradient;
}
}
void Mesh::computeIntegratedDivergence(Eigen::VectorXd& integratedDivs,
const Eigen::MatrixXd& gradients) const
{
for (VertexCIter v = vertices.begin(); v != vertices.end(); v++) {
double integratedDiv = 0.0;
Eigen::Vector3d p = v->position;
HalfEdgeCIter he = v->he;
do {
Eigen::Vector3d gradient = gradients.row(he->face->index);
Eigen::Vector3d p1 = he->next->vertex->position;
Eigen::Vector3d p2 = he->next->next->vertex->position;
Eigen::Vector3d e1 = p1 - p;
Eigen::Vector3d e2 = p2 - p;
Eigen::Vector3d ei = p2 - p1;
double theta1 = acos((-e2).dot(-ei) / (e2.norm() * ei.norm()));
double cot1 = 1.0 / tan(theta1);
double theta2 = acos((-e1).dot(ei) / (e1.norm() * ei.norm()));
double cot2 = 1.0 / tan(theta2);
integratedDiv += e1.dot(gradient) * cot1 + e2.dot(gradient) * cot2;
he = he->flip->next;
} while (he != v->he);
integratedDivs[v->index] = 0.5 * integratedDiv;
}
}
void color_selfintersections(
const Eigen::MatrixXd & V,
const Eigen::MatrixXi & F,
Eigen::MatrixXd & C)
{
using namespace igl;
using namespace igl::cgal;
using namespace Eigen;
using namespace std;
MatrixXd SV;
MatrixXi SF,IF;
VectorXi J,IM;
RemeshSelfIntersectionsParam params;
params.detect_only = false;
remesh_self_intersections(V,F,params,SV,SF,IF,J,IM);
writeOBJ("F**K.obj",SV,SF);
C.resize(F.rows(),3);
C.col(0).setConstant(0.4);
C.col(1).setConstant(0.8);
C.col(2).setConstant(0.3);
for(int f = 0;f<IF.rows();f++)
{
C.row(IF(f,0)) = RowVector3d(1,0.4,0.4);
C.row(IF(f,1)) = RowVector3d(1,0.4,0.4);
}
}