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);
}
}
void assert_size(const Eigen::MatrixBase<Derived> &X, int size_expected)
{
common_assert_msg_ex(
X.size() == size_expected,
"matrix size mismatch" << std::endl <<
"actual: " << X.size() << std::endl <<
"expected: " << size_expected,
eigen_utilities::assert_error);
}
IGL_INLINE void igl::diag(
const Eigen::MatrixBase<DerivedV> & V,
Eigen::SparseMatrix<T>& X)
{
assert(V.rows() == 1 || V.cols() == 1);
// clear and resize output
Eigen::DynamicSparseMatrix<T, Eigen::RowMajor> dyn_X(V.size(),V.size());
dyn_X.reserve(V.size());
// loop over non-zeros
for(int i = 0; i<V.size(); i++)
{
dyn_X.coeffRef(i,i) += V[i];
}
X = Eigen::SparseMatrix<T>(dyn_X);
}
void activation(const Eigen::MatrixBase<T>& inputs, Eigen::VectorXd& activation)
{
//activation = inputs;
for (size_t idx = 0; idx < static_cast<size_t>(inputs.size()); idx++) {
activation[idx] = std::max(inputs[idx], 0.);
}
}
void forward_propagation(Eigen::MatrixBase<Derived1> const &input,
Eigen::MatrixBase<Derived2> const &weight,
Eigen::MatrixBase<Derived3> const &bias,
Eigen::MatrixBase<Derived4> &output,
bool no_overlap = true,
UnaryFunc func = UnaryFunc())
{
static_assert(std::is_same<Derived1::Scalar, Derived2::Scalar>::value &&
std::is_same<Derived2::Scalar, Derived3::Scalar>::value &&
std::is_same<Derived4::Scalar, Derived4::Scalar>::value,
"Data type of matrix input, weight, bias and output should be the same");
if(input.rows() != 0 && weight.rows() != 0 &&
bias.rows() != 0){
if(no_overlap){
output.noalias() = weight * input;
}else{
output = weight * input;
}
using Scalar = typename Derived3::Scalar;
using MatType = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
using Mapper = Eigen::Map<const MatType, Eigen::Aligned>;
Mapper Map(&bias(0, 0), bias.size());
output.colwise() += Map;
func(output);
}
}
void activation_and_gradient(const Eigen::MatrixBase<T>& inputs, Eigen::VectorXd& activation, Eigen::VectorXd& gradient)
{
this->activation(inputs, activation);
for (size_t idx = 0; idx < static_cast<size_t>(inputs.size()); idx++)
gradient[idx] = inputs[idx] > 0 ? 1 : 0;
}
IGL_INLINE void igl::triangles_from_strip(
const Eigen::MatrixBase<DerivedS>& S,
Eigen::PlainObjectBase<DerivedF>& F)
{
using namespace std;
F.resize(S.size()-2,3);
for(int s = 0;s < S.size()-2;s++)
{
if(s%2 == 0)
{
F(s,0) = S(s+2);
F(s,1) = S(s+1);
F(s,2) = S(s+0);
}else
{
F(s,0) = S(s+0);
F(s,1) = S(s+1);
F(s,2) = S(s+2);
}
}
}
IGL_INLINE Scalar igl::random_search(
const std::function< Scalar (DerivedX &) > f,
const Eigen::MatrixBase<DerivedLB> & LB,
const Eigen::MatrixBase<DerivedUB> & UB,
const int iters,
DerivedX & X)
{
Scalar min_f = std::numeric_limits<Scalar>::max();
const int dim = LB.size();
assert(UB.size() == dim && "UB should match LB size");
for(int iter = 0;iter<iters;iter++)
{
const DerivedX R = DerivedX::Random(dim).array()*0.5+0.5;
DerivedX Xr = LB.array() + R.array()*(UB-LB).array();
const Scalar fr = f(Xr);
if(fr<min_f)
{
min_f = fr;
X = Xr;
}
}
return min_f;
}
typename Gradient<DerivedX, DerivedDX::ColsAtCompileTime>::type dTransformAdjointTranspose(
const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedX>& X,
const Eigen::MatrixBase<DerivedDT>& dT,
const Eigen::MatrixBase<DerivedDX>& dX) {
assert(dT.cols() == dX.cols());
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 Rtranspose = R.transpose();
auto dRtranspose = transposeGrad(dR, R.rows());
typename Gradient<DerivedX, DerivedDX::ColsAtCompileTime>::type ret(X.size(), nq);
std::array<int, 3> Xomega_rows {0, 1, 2};
std::array<int, 3> Xv_rows {3, 4, 5};
for (int col = 0; col < X.cols(); col++) {
auto Xomega_col = X.template block<3, 1>(0, col);
auto Xv_col = X.template block<3, 1>(3, col);
std::array<int, 1> col_array {col};
auto dXomega_col = getSubMatrixGradient(dX, Xomega_rows, col_array, X.rows());
auto dXv_col = getSubMatrixGradient(dX, Xv_rows, col_array, X.rows());
auto dp_hatXv_col = (dp.colwise().cross(Xv_col) - dXv_col.colwise().cross(p)).eval();
auto Xomega_col_minus_p_cross_Xv_col = (Xomega_col - p.cross(Xv_col)).eval();
auto dXomega_transformed_col = (Rtranspose * (dXomega_col - dp_hatXv_col) + matGradMult(dRtranspose, Xomega_col_minus_p_cross_Xv_col)).eval();
auto dRtransposeXv_col = (Rtranspose * dXv_col + matGradMult(dRtranspose, Xv_col)).eval();
setSubMatrixGradient(ret, dXomega_transformed_col, Xomega_rows, col_array, X.rows());
setSubMatrixGradient(ret, dRtransposeXv_col, Xv_rows, col_array, X.rows());
}
return ret;
}
IGL_INLINE bool igl::median(
const Eigen::MatrixBase<DerivedV> & V, mType & m)
{
using namespace std;
if(V.size() == 0)
{
return false;
}
vector<typename DerivedV::Scalar> vV;
matrix_to_list(V,vV);
// http://stackoverflow.com/a/1719155/148668
size_t n = vV.size()/2;
nth_element(vV.begin(),vV.begin()+n,vV.end());
if(vV.size()%2==0)
{
nth_element(vV.begin(),vV.begin()+n-1,vV.end());
m = 0.5*(vV[n]+vV[n-1]);
}else
{
m = vV[n];
}
return true;
}
void gradient(const Eigen::MatrixBase<T>& inputs, Eigen::VectorXd& derivative)
{
for (size_t idx = 0; idx < static_cast<size_t>(inputs.size()); idx++)
derivative[idx] = inputs[idx] > 0 ? 1 : 0;
}
explicit ForceTpl(const Eigen::MatrixBase<f6> & f)
: data(f)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(f6);
assert( f.size() == 6 );
}
CwiseNullaryOp<circulant_functor<ArgType>, typename circulant_helper<ArgType>::MatrixType>
makeCirculant(const Eigen::MatrixBase<ArgType>& arg)
{
typedef typename circulant_helper<ArgType>::MatrixType MatrixType;
return MatrixType::NullaryExpr(arg.size(), arg.size(), circulant_functor<ArgType>(arg.derived()));
}
IGL_INLINE void igl::point_simplex_squared_distance(
const Eigen::MatrixBase<Derivedp> & p,
const Eigen::MatrixBase<DerivedV> & V,
const Eigen::MatrixBase<DerivedEle> & Ele,
const typename DerivedEle::Index primitive,
Derivedsqr_d & sqr_d,
Eigen::PlainObjectBase<Derivedc> & c)
{
typedef typename Derivedp::Scalar Scalar;
typedef typename Eigen::Matrix<Scalar,1,DIM> Vector;
typedef Vector Point;
const auto & Dot = [](const Point & a, const Point & b)->Scalar
{
return a.dot(b);
};
// Real-time collision detection, Ericson, Chapter 5
const auto & ClosestPtPointTriangle =
[&Dot](Point p, Point a, Point b, Point c)->Point
{
// Check if P in vertex region outside A
Vector ab = b - a;
Vector ac = c - a;
Vector ap = p - a;
Scalar d1 = Dot(ab, ap);
Scalar d2 = Dot(ac, ap);
if (d1 <= 0.0 && d2 <= 0.0) return a; // barycentric coordinates (1,0,0)
// Check if P in vertex region outside B
Vector bp = p - b;
Scalar d3 = Dot(ab, bp);
Scalar d4 = Dot(ac, bp);
if (d3 >= 0.0 && d4 <= d3) return b; // barycentric coordinates (0,1,0)
// Check if P in edge region of AB, if so return projection of P onto AB
Scalar vc = d1*d4 - d3*d2;
if( a != b)
{
if (vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0) {
Scalar v = d1 / (d1 - d3);
return a + v * ab; // barycentric coordinates (1-v,v,0)
}
}
// Check if P in vertex region outside C
Vector cp = p - c;
Scalar d5 = Dot(ab, cp);
Scalar d6 = Dot(ac, cp);
if (d6 >= 0.0 && d5 <= d6) return c; // barycentric coordinates (0,0,1)
// Check if P in edge region of AC, if so return projection of P onto AC
Scalar vb = d5*d2 - d1*d6;
if (vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0) {
Scalar w = d2 / (d2 - d6);
return a + w * ac; // barycentric coordinates (1-w,0,w)
}
// Check if P in edge region of BC, if so return projection of P onto BC
Scalar va = d3*d6 - d5*d4;
if (va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0) {
Scalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
return b + w * (c - b); // barycentric coordinates (0,1-w,w)
}
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
Scalar denom = 1.0 / (va + vb + vc);
Scalar v = vb * denom;
Scalar w = vc * denom;
return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0-v-w
};
assert(p.size() == DIM);
assert(V.cols() == DIM);
assert(Ele.cols() <= DIM+1);
assert(Ele.cols() <= 3 && "Only simplices up to triangles are considered");
c = ClosestPtPointTriangle(
p,
V.row(Ele(primitive,0)),
V.row(Ele(primitive,1%Ele.cols())),
V.row(Ele(primitive,2%Ele.cols())));
sqr_d = (p-c).squaredNorm();
}
IGL_INLINE void igl::ismember(
const Eigen::MatrixBase<DerivedA> & A,
const Eigen::MatrixBase<DerivedB> & B,
Eigen::PlainObjectBase<DerivedIA> & IA,
Eigen::PlainObjectBase<DerivedLOCB> & LOCB)
{
using namespace Eigen;
using namespace std;
IA.resizeLike(A);
IA.setConstant(false);
LOCB.resizeLike(A);
LOCB.setConstant(-1);
// boring base cases
if(A.size() == 0)
{
return;
}
if(B.size() == 0)
{
return;
}
// Get rid of any duplicates
typedef Matrix<typename DerivedA::Scalar,Dynamic,1> VectorA;
typedef Matrix<typename DerivedB::Scalar,Dynamic,1> VectorB;
const VectorA vA(Eigen::Map<const VectorA>(DerivedA(A).data(), A.cols()*A.rows(),1));
const VectorB vB(Eigen::Map<const VectorB>(DerivedB(B).data(), B.cols()*B.rows(),1));
VectorA uA;
VectorB uB;
Eigen::Matrix<typename DerivedA::Index,Dynamic,1> uIA,uIuA,uIB,uIuB;
unique(vA,uA,uIA,uIuA);
unique(vB,uB,uIB,uIuB);
// Sort both
VectorA sA;
VectorB sB;
Eigen::Matrix<typename DerivedA::Index,Dynamic,1> sIA,sIB;
sort(uA,1,true,sA,sIA);
sort(uB,1,true,sB,sIB);
Eigen::Matrix<bool,Eigen::Dynamic,1> uF =
Eigen::Matrix<bool,Eigen::Dynamic,1>::Zero(sA.size(),1);
Eigen::Matrix<typename DerivedLOCB::Scalar, Eigen::Dynamic,1> uLOCB =
Eigen::Matrix<typename DerivedLOCB::Scalar,Eigen::Dynamic,1>::
Constant(sA.size(),1,-1);
{
int bi = 0;
// loop over sA
bool past = false;
for(int a = 0;a<sA.size();a++)
{
while(!past && sA(a)>sB(bi))
{
bi++;
past = bi>=sB.size();
}
if(!past && sA(a)==sB(bi))
{
uF(sIA(a)) = true;
uLOCB(sIA(a)) = uIB(sIB(bi));
}
}
}
Map< Matrix<typename DerivedIA::Scalar,Dynamic,1> >
vIA(IA.data(),IA.cols()*IA.rows(),1);
Map< Matrix<typename DerivedLOCB::Scalar,Dynamic,1> >
vLOCB(LOCB.data(),LOCB.cols()*LOCB.rows(),1);
for(int a = 0;a<A.size();a++)
{
vIA(a) = uF(uIuA(a));
vLOCB(a) = uLOCB(uIuA(a));
}
}
IGL_INLINE void igl::ismember_rows(
const Eigen::MatrixBase<DerivedA> & A,
const Eigen::MatrixBase<DerivedB> & B,
Eigen::PlainObjectBase<DerivedIA> & IA,
Eigen::PlainObjectBase<DerivedLOCB> & LOCB)
{
using namespace Eigen;
using namespace std;
assert(A.cols() == B.cols() && "number of columns must match");
IA.resize(A.rows(),1);
IA.setConstant(false);
LOCB.resize(A.rows(),1);
LOCB.setConstant(-1);
// boring base cases
if(A.size() == 0)
{
return;
}
if(B.size() == 0)
{
return;
}
// Get rid of any duplicates
DerivedA uA;
DerivedB uB;
Eigen::Matrix<typename DerivedA::Index,Dynamic,1> uIA,uIuA,uIB,uIuB;
unique_rows(A,uA,uIA,uIuA);
unique_rows(B,uB,uIB,uIuB);
// Sort both
DerivedA sA;
DerivedB sB;
Eigen::Matrix<typename DerivedA::Index,Dynamic,1> sIA,sIB;
sortrows(uA,true,sA,sIA);
sortrows(uB,true,sB,sIB);
Eigen::Matrix<bool,Eigen::Dynamic,1> uF =
Eigen::Matrix<bool,Eigen::Dynamic,1>::Zero(sA.size(),1);
Eigen::Matrix<typename DerivedLOCB::Scalar, Eigen::Dynamic,1> uLOCB =
Eigen::Matrix<typename DerivedLOCB::Scalar,Eigen::Dynamic,1>::
Constant(sA.size(),1,-1);
const auto & row_greater_than = [&sA,&sB](const int a, const int b)
{
for(int c = 0;c<sA.cols();c++)
{
if(sA(a,c) > sB(b,c)) return true;
if(sA(a,c) < sB(b,c)) return false;
}
return false;
};
{
int bi = 0;
// loop over sA
bool past = false;
for(int a = 0;a<sA.rows();a++)
{
assert(bi < sB.rows());
while(!past && row_greater_than(a,bi))
{
bi++;
past = bi>=sB.rows();
}
if(!past && (sA.row(a).array()==sB.row(bi).array()).all() )
{
uF(sIA(a)) = true;
uLOCB(sIA(a)) = uIB(sIB(bi));
}
}
}
for(int a = 0;a<A.rows();a++)
{
IA(a) = uF(uIuA(a));
LOCB(a) = uLOCB(uIuA(a));
}
}
