Eigen::Matrix<typename DerivedA::Scalar, matGradMultMatNumRows(DerivedA::RowsAtCompileTime, DerivedB::ColsAtCompileTime), DerivedDA::ColsAtCompileTime>
matGradMultMat(
const Eigen::MatrixBase<DerivedA>& A,
const Eigen::MatrixBase<DerivedB>& B,
const Eigen::MatrixBase<DerivedDA>& dA,
const Eigen::MatrixBase<DerivedDB>& dB) {
assert(dA.cols() == dB.cols());
const int nq = dA.cols();
const int nq_at_compile_time = DerivedDA::ColsAtCompileTime;
Eigen::Matrix<typename DerivedA::Scalar,
matGradMultMatNumRows(DerivedA::RowsAtCompileTime, DerivedB::ColsAtCompileTime),
DerivedDA::ColsAtCompileTime> ret(A.rows() * B.cols(), nq);
for (int col = 0; col < B.cols(); col++) {
auto block = ret.template block<DerivedA::RowsAtCompileTime, nq_at_compile_time>(col * A.rows(), 0, A.rows(), nq);
// A * dB part:
block.noalias() = A * dB.template block<DerivedA::ColsAtCompileTime, nq_at_compile_time>(col * A.cols(), 0, A.cols(), nq);
for (int row = 0; row < B.rows(); row++) {
// B * dA part:
block.noalias() += B(row, col) * dA.template block<DerivedA::RowsAtCompileTime, nq_at_compile_time>(row * A.rows(), 0, A.rows(), nq);
}
}
return ret;
// much slower and requires eigen/unsupported:
// return Eigen::kroneckerProduct(Eigen::MatrixXd::Identity(B.cols(), B.cols()), A) * dB + Eigen::kroneckerProduct(B.transpose(), Eigen::MatrixXd::Identity(A.rows(), A.rows())) * dA;
}
IGL_INLINE void igl::cat(
const int dim,
const Eigen::MatrixBase<Derived> & A,
const Eigen::MatrixBase<Derived> & B,
MatC & C)
{
assert(dim == 1 || dim == 2);
// Special case if B or A is empty
if(A.size() == 0)
{
C = B;
return;
}
if(B.size() == 0)
{
C = A;
return;
}
if(dim == 1)
{
assert(A.cols() == B.cols());
C.resize(A.rows()+B.rows(),A.cols());
C << A,B;
}else if(dim == 2)
{
assert(A.rows() == B.rows());
C.resize(A.rows(),A.cols()+B.cols());
C << A,B;
}else
{
fprintf(stderr,"cat.h: Error: Unsupported dimension %d\n",dim);
}
}
inline Eigen::Matrix<typename Eigen::internal::traits<Derived_T>::Scalar, 3, 3> crossMx(
Eigen::MatrixBase<Derived_T> const & v)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived_T>, 3);
assert((v.cols()==3 && v.rows()==1)||(v.rows()==3 && v.cols()==1));
return crossMx(v(0, 0), v(1, 0), v(2, 0));
}
void assert_size(const Eigen::MatrixBase<Derived> &X, int rows_expected, int cols_expected)
{
common_assert_msg_ex(
X.rows() == rows_expected && X.cols() == cols_expected,
"matrix [row, col] mismatch" << std::endl <<
"actual: [" << X.rows() << ", " << X.cols() << "]" << std::endl <<
"expected: [" << rows_expected << ", " << cols_expected << "]",
eigen_utilities::assert_error);
}
bool allGreaterEquals(const Eigen::MatrixBase<DerivedA> & A,
const Eigen::MatrixBase<DerivedB> & B,
double tolerance = math::NUMERICAL_ZERO_DIFFERENCE)
{
assertion(A.rows() == B.rows(), "Matrices with different number of rows can't be compared.");
assertion(A.cols() == B.cols(), "Matrices with different number of cols can't be compared.");
return ((A-B).array() >= tolerance).all();
}
static void run( float a, const Eigen::MatrixBase<Derived2> & A, const Eigen::MatrixBase<Derived1> & x, float b, Eigen::MatrixBase<Derived1> &y) {
EIGEN_STATIC_ASSERT(sizeof(PREC) == sizeof(typename Derived1::Scalar), YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
ASSERTMSG(A.cols() == x.rows() && A.rows() == y.rows() ,"ERROR: Vector/Matrix wrong dimension");
// b_dev = alpha * A_dev * x_old_dev + beta *b_dev
//Derived2 C = A;
#if USE_INTEL_BLAS == 1
CBLAS_ORDER order;
CBLAS_TRANSPOSE trans;
if(Derived1::Flags & Eigen::RowMajorBit) {
order = CblasRowMajor;
} else {
order = CblasColMajor;
}
trans = CblasNoTrans;
mkl_set_dynamic(false);
mkl_set_num_threads(BLAS_NUM_THREADS);
//cout << "Threads:" << mkl_get_max_threads();
cblas_sgemv(order, trans, A.rows(), A.cols(), a, const_cast<double*>(&(A.operator()(0,0))), A.outerStride(), const_cast<double*>(&(x.operator()(0,0))), 1, b, &(y.operator()(0,0)), 1);
//cblas_dgemm(order,trans,trans, A.rows(), A.cols(), A.cols(), 1.0, const_cast<double*>(&(A.operator()(0,0))), A.rows(), const_cast<double*>(&(A.operator()(0,0))), A.rows(), 1.0 , &(C.operator()(0,0)), C.rows() );
#else
#if USE_GOTO_BLAS == 1
/* static DGEMVFunc DGEMV = NULL;
if (DGEMV == NULL) {
HINSTANCE hInstLibrary = LoadLibrary("libopenblasp-r0.1alpha2.2.dll");
DGEMV = (DGEMVFunc)GetProcAddress(hInstLibrary, "DGEMV");
}*/
char trans = 'N';
BLAS_INT idx = 1;
BLAS_INT m = A.rows();
BLAS_INT n = A.cols();
sgemv(&trans, &m, &n, &a, &(A.operator()(0,0)), &m, &(x.operator()(0,0)), &idx, &b, &(y.operator()(0,0)), &idx);
// FreeLibrary(hInstLibrary);
#else
ASSERTMSG(false,"No implementation for BLAS defined!");
#endif
#endif
}
void least_squares_eqcon(Eigen::MatrixBase<data1__> &x, const Eigen::MatrixBase<data2__> &A, const Eigen::MatrixBase<data3__> &b, const Eigen::MatrixBase<data4__> &B, const Eigen::MatrixBase<data5__> &d)
{
typedef Eigen::Matrix<typename Eigen::MatrixBase<data4__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> B_matrixD;
B_matrixD Q, R, temp_B, temp_R;
typedef Eigen::Matrix<typename Eigen::MatrixBase<data2__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> A_matrixD;
A_matrixD A1, A2, temp_A;
typedef Eigen::Matrix<typename Eigen::MatrixBase<data5__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> d_matrixD;
d_matrixD y;
typedef Eigen::Matrix<typename Eigen::MatrixBase<data3__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> b_matrixD;
b_matrixD z, rhs;
typedef Eigen::Matrix<typename Eigen::MatrixBase<data1__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> x_matrixD;
x_matrixD x_temp(A.cols(), b.cols());
typename A_matrixD::Index p(B.rows()), n(A.cols());
#ifdef DEBUG
typename A_matrixD::Index m(b.rows());
#endif
// build Q and R
Eigen::HouseholderQR<B_matrixD> qr(B.transpose());
Q=qr.householderQ();
temp_B=qr.matrixQR();
temp_R=Eigen::TriangularView<B_matrixD, Eigen::Upper>(temp_B);
R=temp_R.topRows(p);
assert((R.rows()==p) && (R.cols()==p));
assert((Q.rows()==n) && (Q.cols()==n));
// build A1 and A2
temp_A=A*Q;
A1=temp_A.leftCols(p);
A2=temp_A.rightCols(n-p);
#ifdef DEBUG
assert((A1.rows()==m) && (A1.cols()==p));
assert((A2.rows()==m) && (A2.cols()==n-p));
#endif
assert(A1.cols()==p);
assert(A2.cols()==n-p);
// solve for y
y=R.transpose().lu().solve(d);
// setup the unconstrained optimization
rhs=b-A1*y;
least_squares_uncon(z, A2, rhs);
// build the solution
x_temp.topRows(p)=y;
x_temp.bottomRows(n-p)=z;
x=Q*x_temp;
}
Eigen::Matrix<typename DerivedDA::Scalar, matGradMultNumRows(DerivedDA::RowsAtCompileTime, Derivedb::RowsAtCompileTime), DerivedDA::ColsAtCompileTime>
matGradMult(const Eigen::MatrixBase<DerivedDA>& dA, const Eigen::MatrixBase<Derivedb>& b) {
const int nq = dA.cols();
assert(b.cols() == 1);
assert(dA.rows() % b.rows() == 0);
const int A_rows = dA.rows() / b.rows();
Eigen::Matrix<typename DerivedDA::Scalar, matGradMultNumRows(DerivedDA::RowsAtCompileTime, Derivedb::RowsAtCompileTime), DerivedDA::ColsAtCompileTime> ret(A_rows, nq);
ret.setZero();
for (int row = 0; row < b.rows(); row++) {
ret += b(row, 0) * dA.block(row * A_rows, 0, A_rows, nq);
}
return ret;
}
bool CameraModel::_project_extrinsic( const Eigen::MatrixBase<Derived1>& mP3D,
Eigen::MatrixBase<Derived2>& mP2D,
Eigen::MatrixBase<Derived3>& mdP2DE
) const {
if( !project( mP3D, mP2D ) ) {
return false;
}
const unsigned int nNum3DDims = 3;
const unsigned int nNumPoints = mP3D.cols();
// Compute finite-diff Jacobians
Derived3 mdP2Dfd( 2, nNumPoints );
Derived3 mP2Dfd( 2, nNumPoints );
typename Eigen::internal::traits<Derived1>::Scalar dEps = 1e-4;
Derived2 mP2DP = mP2D;
Derived2 mP2DM = mP2D;
{
// Compute extrinsic Jacobian
for( size_t kk=0; kk<nNum3DDims; kk++ ) {
Derived1 mP3DP = mP3D;
Derived1 mP3DM = mP3D;
mP3DP.row( kk ).array() += dEps;
mP3DM.row( kk ).array() -= dEps;
project( mP3DP, mP2DP );
project( mP3DM, mP2DM );
Derived2 mdP2Dfd = ( mP2DP - mP2DM ) / (2*dEps);
for( size_t ll=0; ll<nNumPoints; ll++ ) {
mdP2DE.col( nNum3DDims*ll + kk ) = mdP2Dfd.col( ll );
}
}
}
return true;
}
bool checkRange(const Eigen::MatrixBase<DerivedA>& mm,
typename DerivedA::Scalar minVal, typename DerivedA::Scalar maxVal)
{
assert(minVal < maxVal);
// Go through each element in the matrix and ensure they are not
// NaN and fall within the specified min and max values.
for (int ii = 0; ii < mm.rows(); ii++)
{
for (int jj = 0; jj < mm.cols(); jj++)
{
if (std::isnan(mm(ii, jj)))
{
std::stringstream ss;
ss << "NaN detected at index (" << ii << ", " << jj << "), returning false!";
ROS_WARN_STREAM(ss.str());
return false;
}
if (mm(ii, jj) > maxVal || mm(ii, jj) < minVal)
{
std::stringstream ss;
ss << "Value of out range at index (" << ii << ", " << jj << "), returning false.\n"
" - value = " << mm(ii, jj) << "\n"
" - minVal = " << minVal << "\n"
" - maxVal = " << maxVal;
ROS_WARN_STREAM(ss.str());
return false;
}
}
}
return true;
}
Eigen::Matrix<Scalar, HOMOGENEOUS_TRANSFORM_SIZE, DerivedDT::ColsAtCompileTime> dHomogTransInv(
const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedDT>& dT) {
const int nq_at_compile_time = DerivedDT::ColsAtCompileTime;
int nq = dT.cols();
const auto& R = T.linear();
const auto& p = T.translation();
std::array<int, 3> rows {0, 1, 2};
std::array<int, 3> R_cols {0, 1, 2};
std::array<int, 1> p_cols {3};
auto dR = getSubMatrixGradient(dT, rows, R_cols, T.Rows);
auto dp = getSubMatrixGradient(dT, rows, p_cols, T.Rows);
auto dinvT_R = transposeGrad(dR, R.rows());
auto dinvT_p = (-R.transpose() * dp - matGradMult(dinvT_R, p)).eval();
const int numel = HOMOGENEOUS_TRANSFORM_SIZE;
Eigen::Matrix<Scalar, numel, nq_at_compile_time> ret(numel, nq);
setSubMatrixGradient(ret, dinvT_R, rows, R_cols, T.Rows);
setSubMatrixGradient(ret, dinvT_p, rows, p_cols, T.Rows);
// zero out gradient of elements in last row:
const int last_row = 3;
for (int col = 0; col < T.HDim; col++) {
ret.row(last_row + col * T.Rows).setZero();
}
return ret;
}
IGL_INLINE void igl::internal_angles_using_edge_lengths(
const Eigen::MatrixBase<DerivedL>& L,
Eigen::PlainObjectBase<DerivedK> & K)
{
// Note:
// Usage of internal_angles_using_squared_edge_lengths() is preferred to internal_angles_using_squared_edge_lengths()
// This function is deprecated and probably will be removed in future versions
typedef typename DerivedL::Index Index;
assert(L.cols() == 3 && "Edge-lengths should come from triangles");
const Index m = L.rows();
K.resize(m,3);
parallel_for(
m,
[&L,&K](const Index f)
{
for(size_t d = 0;d<3;d++)
{
const auto & s1 = L(f,d);
const auto & s2 = L(f,(d+1)%3);
const auto & s3 = L(f,(d+2)%3);
K(f,d) = acos((s3*s3 + s2*s2 - s1*s1)/(2.*s3*s2));
}
},
1000l);
}
typename Gradient<Eigen::Matrix<typename DerivedR::Scalar, RPY_SIZE, 1>, DerivedDR::ColsAtCompileTime>::type drotmat2rpy(
const Eigen::MatrixBase<DerivedR>& R,
const Eigen::MatrixBase<DerivedDR>& dR)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<DerivedR>, SPACE_DIMENSION, SPACE_DIMENSION);
EIGEN_STATIC_ASSERT(Eigen::MatrixBase<DerivedDR>::RowsAtCompileTime == RotmatSize, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
const int nq = dR.cols();
typedef typename DerivedR::Scalar Scalar;
typedef typename Gradient<Eigen::Matrix<Scalar, RPY_SIZE, 1>, DerivedDR::ColsAtCompileTime>::type ReturnType;
ReturnType drpy(RPY_SIZE, nq);
auto dR11_dq = getSubMatrixGradient(dR, 0, 0, R.rows());
auto dR21_dq = getSubMatrixGradient(dR, 1, 0, R.rows());
auto dR31_dq = getSubMatrixGradient(dR, 2, 0, R.rows());
auto dR32_dq = getSubMatrixGradient(dR, 2, 1, R.rows());
auto dR33_dq = getSubMatrixGradient(dR, 2, 2, R.rows());
Scalar sqterm = R(2,1) * R(2,1) + R(2,2) * R(2,2);
using namespace std;
// droll_dq
drpy.row(0) = (R(2, 2) * dR32_dq - R(2, 1) * dR33_dq) / sqterm;
// dpitch_dq
Scalar sqrt_sqterm = sqrt(sqterm);
drpy.row(1) = (-sqrt_sqterm * dR31_dq + R(2, 0) / sqrt_sqterm * (R(2, 1) * dR32_dq + R(2, 2) * dR33_dq)) / (R(2, 0) * R(2, 0) + R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2));
// dyaw_dq
sqterm = R(0, 0) * R(0, 0) + R(1, 0) * R(1, 0);
drpy.row(2) = (R(0, 0) * dR21_dq - R(1, 0) * dR11_dq) / sqterm;
return drpy;
}
IGL_INLINE void igl::face_areas(
const Eigen::MatrixBase<DerivedL>& L,
const typename DerivedL::Scalar doublearea_nan_replacement,
Eigen::PlainObjectBase<DerivedA>& A)
{
using namespace Eigen;
assert(L.cols() == 6);
const int m = L.rows();
// (unsigned) face Areas (opposite vertices: 1 2 3 4)
Matrix<typename DerivedA::Scalar,Dynamic,1>
A0(m,1), A1(m,1), A2(m,1), A3(m,1);
Matrix<typename DerivedA::Scalar,Dynamic,3>
L0(m,3), L1(m,3), L2(m,3), L3(m,3);
L0<<L.col(1),L.col(2),L.col(3);
L1<<L.col(0),L.col(2),L.col(4);
L2<<L.col(0),L.col(1),L.col(5);
L3<<L.col(3),L.col(4),L.col(5);
doublearea(L0,doublearea_nan_replacement,A0);
doublearea(L1,doublearea_nan_replacement,A1);
doublearea(L2,doublearea_nan_replacement,A2);
doublearea(L3,doublearea_nan_replacement,A3);
A.resize(m,4);
A.col(0) = 0.5*A0;
A.col(1) = 0.5*A1;
A.col(2) = 0.5*A2;
A.col(3) = 0.5*A3;
}
bool CameraModel::_project_intrinsic( const Eigen::MatrixBase<Derived1>& mP3D,
Eigen::MatrixBase<Derived2>& mP2D,
Eigen::MatrixBase<Derived3>& mdP2DI
) const {
if( !project( mP3D, mP2D ) ) {
return false;
}
const unsigned int nNumPoints = mP3D.cols();
const unsigned int nNumCamParams = get_num_parameters();
// Compute finite-diff Jacobians
Derived3 mdP2Dfd( 2, nNumPoints );
Derived3 mP2Dfd( 2, nNumPoints );
double dEps = 1e-4;
Derived2 mP2DP = mP2D;
Derived2 mP2DM = mP2D;
{
// Compute intrinsic Jacobian
// Horrible const_cast ... difficult to avoid
for( size_t kk=0; kk<nNumCamParams; kk++ ) {
std::string sVarName = parameter_index_to_name( kk );
double dVal = get<double>( sVarName );
const_cast<CameraModel*>( this )->set<double>( sVarName, dVal + dEps );
project( mP3D, mP2DP );
const_cast<CameraModel*>( this )->set<double>( sVarName, dVal - dEps );
project( mP3D, mP2DM );
mdP2Dfd = ( mP2DP - mP2DM ) / (2*dEps);
const_cast<CameraModel*>( this )->set<double>( sVarName, dVal );
for( size_t ll=0; ll<nNumPoints; ll++ ) {
mdP2DI.col( nNumCamParams*ll + kk ) = mdP2Dfd.col( ll );
}
}
}
return true;
}
void setSubMatrixGradient(Eigen::MatrixBase<DerivedDM>& dM, const Eigen::MatrixBase<DerivedDMSub>& dM_submatrix, int row, int col, typename DerivedDM::Index M_rows,
typename DerivedDM::Index q_start, typename DerivedDM::Index q_subvector_size) {
if (q_subvector_size == Eigen::Dynamic) {
q_subvector_size = dM.cols() - q_start;
}
dM.template block<1, QSubvectorSize>(row + col * M_rows, q_start, 1, q_subvector_size) = dM_submatrix;
}
IGL_INLINE void igl::hessian(
const Eigen::MatrixBase<DerivedV> & V,
const Eigen::MatrixBase<DerivedF> & F,
Eigen::SparseMatrix<Scalar>& H)
{
typedef typename DerivedV::Scalar denseScalar;
typedef typename Eigen::Matrix<denseScalar, Eigen::Dynamic, 1> VecXd;
typedef typename Eigen::SparseMatrix<Scalar> SparseMat;
typedef typename Eigen::DiagonalMatrix
<Scalar, Eigen::Dynamic, Eigen::Dynamic> DiagMat;
int dim = V.cols();
assert((dim==2 || dim==3) &&
"The dimension of the vertices should be 2 or 3");
//Construct the combined gradient matric
SparseMat G;
igl::grad(Eigen::PlainObjectBase<DerivedV>(V),
Eigen::PlainObjectBase<DerivedF>(F),
G, false);
SparseMat GG(F.rows(), dim*V.rows());
GG.reserve(G.nonZeros());
for(int i=0; i<dim; ++i)
GG.middleCols(i*G.cols(),G.cols()) = G.middleRows(i*F.rows(),F.rows());
SparseMat D;
igl::repdiag(GG,dim,D);
//Compute area matrix
VecXd areas;
igl::doublearea(V, F, areas);
DiagMat A = (0.5*areas).replicate(dim,1).asDiagonal();
//Compute FEM Hessian
H = D.transpose()*A*G;
}
Eigen::Matrix<Scalar, HOMOGENEOUS_TRANSFORM_SIZE, DerivedQdotToV::ColsAtCompileTime> dHomogTrans(
const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedS>& S,
const Eigen::MatrixBase<DerivedQdotToV>& qdot_to_v) {
const int nq_at_compile_time = DerivedQdotToV::ColsAtCompileTime;
int nq = qdot_to_v.cols();
auto qdot_to_twist = (S * qdot_to_v).eval();
const int numel = HOMOGENEOUS_TRANSFORM_SIZE;
Eigen::Matrix<Scalar, numel, nq_at_compile_time> ret(numel, nq);
const auto& Rx = T.linear().col(0);
const auto& Ry = T.linear().col(1);
const auto& Rz = T.linear().col(2);
const auto& qdot_to_omega_x = qdot_to_twist.row(0);
const auto& qdot_to_omega_y = qdot_to_twist.row(1);
const auto& qdot_to_omega_z = qdot_to_twist.row(2);
ret.template middleRows<3>(0) = -Rz * qdot_to_omega_y + Ry * qdot_to_omega_z;
ret.row(3).setZero();
ret.template middleRows<3>(4) = Rz * qdot_to_omega_x - Rx * qdot_to_omega_z;
ret.row(7).setZero();
ret.template middleRows<3>(8) = -Ry * qdot_to_omega_x + Rx * qdot_to_omega_y;
ret.row(11).setZero();
ret.template middleRows<3>(12) = T.linear() * qdot_to_twist.bottomRows(3);
ret.row(15).setZero();
return ret;
}
void SLHA_io::set_block_imag(const std::string& name,
const Eigen::MatrixBase<Derived>& matrix,
const std::string& symbol, double scale)
{
std::ostringstream ss;
ss << "Block " << name;
if (scale != 0.)
ss << " Q= " << FORMAT_SCALE(scale);
ss << '\n';
const int rows = matrix.rows();
const int cols = matrix.cols();
for (int i = 1; i <= rows; ++i) {
if (cols == 1) {
ss << boost::format(vector_formatter) % i % Im(matrix(i-1,0))
% ("Im(" + symbol + "(" + ToString(i) + "))");
} else {
for (int k = 1; k <= cols; ++k) {
ss << boost::format(mixing_matrix_formatter) % i % k % Im(matrix(i-1,k-1))
% ("Im(" + symbol + "(" + ToString(i) + "," + ToString(k) + "))");
}
}
}
set_block(ss);
}
double SLHA_io::read_vector(const std::string& block_name, Eigen::MatrixBase<Derived>& vector) const
{
if (vector.cols() != 1) throw SetupError("Vector has more than 1 column");
auto block = data.find(data.cbegin(), data.cend(), block_name);
const int rows = vector.rows();
double scale = 0.;
while (block != data.cend()) {
for (const auto& line: *block) {
if (!line.is_data_line()) {
// read scale from block definition
if (line.size() > 3 &&
to_lower(line[0]) == "block" && line[2] == "Q=")
scale = convert_to<double>(line[3]);
continue;
}
if (line.size() >= 2) {
const int i = convert_to<int>(line[0]) - 1;
if (0 <= i && i < rows) {
const double value = convert_to<double>(line[1]);
vector(i,0) = value;
}
}
}
++block;
block = data.find(block, data.cend(), block_name);
}
return scale;
}
MatrixSuccessCode LUPartialPivotDecompositionSuccessful(Eigen::MatrixBase<Derived> const& LU)
{
#ifndef BERTINI_DISABLE_ASSERTS
assert(LU.rows()==LU.cols() && "non-square matrix in LUPartialPivotDecompositionSuccessful");
assert(LU.rows()>0 && "empty matrix in LUPartialPivotDecompositionSuccessful");
#endif
// this loop won't test entry (0,0). it's tested separately after.
for (unsigned int ii = LU.rows()-1; ii > 0; ii--)
{
if (IsSmallValue(LU(ii,ii)))
{
return MatrixSuccessCode::SmallValue;
}
if (IsLargeChange(LU(ii-1,ii-1),LU(ii,ii)))
{
return MatrixSuccessCode::LargeChange;
}
}
// this line is the reason for the above assert on non-empty matrix.
if (IsSmallValue(LU(0,0)))
{
return MatrixSuccessCode::SmallValue;
}
return MatrixSuccessCode::Success;
}
IGL_INLINE bool igl::writeDMAT(
const std::string file_name,
const Eigen::MatrixBase<DerivedW> & W,
const bool ascii)
{
FILE * fp = fopen(file_name.c_str(),"wb");
if(fp == NULL)
{
fprintf(stderr,"IOError: writeDMAT() could not open %s...",file_name.c_str());
return false;
}
if(ascii)
{
// first line contains number of rows and number of columns
fprintf(fp,"%d %d\n",(int)W.cols(),(int)W.rows());
// Loop over columns slowly
for(int j = 0;j < W.cols();j++)
{
// loop over rows (down columns) quickly
for(int i = 0;i < W.rows();i++)
{
fprintf(fp,"%0.17lg\n",(double)W(i,j));
}
}
}else
{
// write header for ascii
fprintf(fp,"0 0\n");
// first line contains number of rows and number of columns
fprintf(fp,"%d %d\n",(int)W.cols(),(int)W.rows());
// reader assumes the binary part is double precision
Eigen::MatrixXd Wd = W.template cast<double>();
fwrite(Wd.data(),sizeof(double),Wd.size(),fp);
//// Loop over columns slowly
//for(int j = 0;j < W.cols();j++)
//{
// // loop over rows (down columns) quickly
// for(int i = 0;i < W.rows();i++)
// {
// double d = (double)W(i,j);
// fwrite(&d,sizeof(double),1,fp);
// }
//}
}
fclose(fp);
return true;
}
void Precision(Eigen::MatrixBase<Derived> & v, unsigned prec)
{
using bertini::Precision;
for (int ii=0; ii<v.rows(); ++ii)
for (int jj=0; jj<v.cols(); ++jj)
Precision(v(ii,jj),prec);
}
void ErrorTermFs<C>::setInvR(const Eigen::MatrixBase<DERIVED>& invR)
{
SM_ASSERT_EQ(Exception, invR.rows(), invR.cols(), "The covariance matrix must be square");
SM_ASSERT_EQ(Exception, invR.rows(), (int)dimension(), "The covariance matrix does not match the size of the error");
// http://eigen.tuxfamily.org/dox-devel/classEigen_1_1LDLT.html#details
// LDLT seems to work on positive semidefinite matrices.
sm::eigen::computeMatrixSqrt(invR, _sqrtInvR);
}
typename GetSubMatrixGradientSingleElement<QSubvectorSize, Derived>::type
getSubMatrixGradient(const Eigen::MatrixBase<Derived>& dM, int row, int col, typename Derived::Index M_rows,
typename Derived::Index q_start, typename Derived::Index q_subvector_size) {
if (q_subvector_size == Eigen::Dynamic) {
q_subvector_size = dM.cols() - q_start;
}
return dM.template block<1, QSubvectorSize>(row + col * M_rows, q_start, 1, q_subvector_size);
}
IGL_INLINE void igl::barycentric_coordinates(
const Eigen::MatrixBase<DerivedP> & P,
const Eigen::MatrixBase<DerivedA> & A,
const Eigen::MatrixBase<DerivedB> & B,
const Eigen::MatrixBase<DerivedC> & C,
Eigen::PlainObjectBase<DerivedL> & L)
{
using namespace Eigen;
#ifndef NDEBUG
const int DIM = P.cols();
assert(A.cols() == DIM && "corners must be in same dimension as query");
assert(B.cols() == DIM && "corners must be in same dimension as query");
assert(C.cols() == DIM && "corners must be in same dimension as query");
assert(P.rows() == A.rows() && "Must have same number of queries as corners");
assert(A.rows() == B.rows() && "Corners must be same size");
assert(A.rows() == C.rows() && "Corners must be same size");
#endif
// http://gamedev.stackexchange.com/a/23745
typedef
Eigen::Array<
typename DerivedP::Scalar,
DerivedP::RowsAtCompileTime,
DerivedP::ColsAtCompileTime>
ArrayS;
typedef
Eigen::Array<
typename DerivedP::Scalar,
DerivedP::RowsAtCompileTime,
1>
VectorS;
const ArrayS v0 = B.array() - A.array();
const ArrayS v1 = C.array() - A.array();
const ArrayS v2 = P.array() - A.array();
VectorS d00 = (v0*v0).rowwise().sum();
VectorS d01 = (v0*v1).rowwise().sum();
VectorS d11 = (v1*v1).rowwise().sum();
VectorS d20 = (v2*v0).rowwise().sum();
VectorS d21 = (v2*v1).rowwise().sum();
VectorS denom = d00 * d11 - d01 * d01;
L.resize(P.rows(),3);
L.col(1) = (d11 * d20 - d01 * d21) / denom;
L.col(2) = (d00 * d21 - d01 * d20) / denom;
L.col(0) = 1.0f -(L.col(1) + L.col(2)).array();
}
IGL_INLINE void igl::on_boundary(
const Eigen::MatrixBase<DerivedT>& T,
Eigen::PlainObjectBase<DerivedI>& I,
Eigen::PlainObjectBase<DerivedC>& C)
{
assert(T.cols() == 0 || T.cols() == 4 || T.cols() == 3);
using namespace std;
using namespace Eigen;
// Cop out: use vector of vectors version
vector<vector<typename DerivedT::Scalar> > vT;
matrix_to_list(T,vT);
vector<bool> vI;
vector<vector<bool> > vC;
on_boundary(vT,vI,vC);
list_to_matrix(vI,I);
list_to_matrix(vC,C);
}
typename MatGradMult<DerivedDA, DerivedB>::type
matGradMult(const Eigen::MatrixBase<DerivedDA>& dA, const Eigen::MatrixBase<DerivedB>& B) {
assert(B.rows() == 0 ? dA.rows() == 0 : dA.rows() % B.rows() == 0);
typename DerivedDA::Index A_rows = B.rows() == 0 ? 0 : dA.rows() / B.rows();
const int A_rows_at_compile_time = (DerivedDA::RowsAtCompileTime == Eigen::Dynamic || DerivedB::RowsAtCompileTime == Eigen::Dynamic) ?
Eigen::Dynamic :
static_cast<int>(DerivedDA::RowsAtCompileTime / DerivedB::RowsAtCompileTime);
typename MatGradMult<DerivedDA, DerivedB>::type ret(A_rows * B.cols(), dA.cols());
ret.setZero();
for (int col = 0; col < B.cols(); col++) {
auto block = ret.template block<A_rows_at_compile_time, DerivedDA::ColsAtCompileTime>(col * A_rows, 0, A_rows, dA.cols());
for (int row = 0; row < B.rows(); row++) {
// B * dA part:
block.noalias() += B(row, col) * dA.template block<A_rows_at_compile_time, DerivedDA::ColsAtCompileTime>(row * A_rows, 0, A_rows, dA.cols());
}
}
return ret;
}
IGL_INLINE void igl::barycenter(
const Eigen::MatrixBase<DerivedV> & V,
const Eigen::MatrixBase<DerivedF> & F,
Eigen::PlainObjectBase<DerivedBC> & BC)
{
BC.setZero(F.rows(),V.cols());
// Loop over faces
for(int i = 0;i<F.rows();i++)
{
// loop around face
for(int j = 0;j<F.cols();j++)
{
// Accumulate
BC.row(i) += V.row(F(i,j));
}
// average
BC.row(i) /= double(F.cols());
}
}
IGL_INLINE void igl::face_areas(
const Eigen::MatrixBase<DerivedV>& V,
const Eigen::MatrixBase<DerivedT>& T,
Eigen::PlainObjectBase<DerivedA>& A)
{
assert(T.cols() == 4);
DerivedA L;
edge_lengths(V,T,L);
return face_areas(L,A);
}