static inline void _fast_matrix_copy(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& v_to,
const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& v_from) {
int nr = v_from.rows();
int nc = v_from.cols();
v_to.resize(nr, nc);
if (nr > 0 && nc > 0) {
std::memcpy(&v_to(0), &v_from(0), v_from.size() * sizeof(double));
}
}
void MatrixMxN<Scalar>::eigenDecomposition(VectorND<Scalar> &eigen_values_real, VectorND<Scalar> &eigen_values_imag,
MatrixMxN<Scalar> &eigen_vectors_real, MatrixMxN<Scalar> &eigen_vectors_imag)
{
unsigned int rows = this->rows(), cols = this->cols();
if(rows != cols)
{
std::cerr<<"Eigen decomposition is only valid for square matrix!\n";
std::exit(EXIT_FAILURE);
}
#ifdef PHYSIKA_USE_EIGEN_MATRIX
//hack: Eigen::EigenSolver does not support integer types, hence we cast Scalar to long double for decomposition
Eigen::Matrix<long double,Eigen::Dynamic,Eigen::Dynamic> temp_matrix(rows,cols);
for(unsigned int i = 0; i < rows; ++i)
for(unsigned int j = 0; j < cols; ++j)
temp_matrix(i,j) = static_cast<long double>((*ptr_eigen_matrix_MxN_)(i,j));
Eigen::EigenSolver<Eigen::Matrix<long double,Eigen::Dynamic,Eigen::Dynamic> > eigen(temp_matrix);
Eigen::Matrix<std::complex<long double>,Eigen::Dynamic,Eigen::Dynamic> vectors = eigen.eigenvectors();
const Eigen::Matrix<std::complex<long double>,Eigen::Dynamic,1> &values = eigen.eigenvalues();
//resize if have to
if(eigen_vectors_real.rows() != vectors.rows() || eigen_vectors_real.cols() != vectors.cols())
eigen_vectors_real.resize(vectors.rows(),vectors.cols());
if(eigen_vectors_imag.rows() != vectors.rows() || eigen_vectors_imag.cols() != vectors.cols())
eigen_vectors_imag.resize(vectors.rows(),vectors.cols());
if(eigen_values_real.dims() != values.rows())
eigen_values_real.resize(values.rows());
if(eigen_values_imag.dims() != values.rows())
eigen_values_imag.resize(values.rows());
//copy the result
for(unsigned int i = 0; i < vectors.rows(); ++i)
for(unsigned int j = 0; j < vectors.cols(); ++j)
{
eigen_vectors_real(i,j) = static_cast<Scalar>(vectors(i,j).real());
eigen_vectors_imag(i,j) = static_cast<Scalar>(vectors(i,j).imag());
}
for(unsigned int i = 0; i < values.rows(); ++i)
{
eigen_values_real[i] = static_cast<Scalar>(values(i,0).real());
eigen_values_imag[i] = static_cast<Scalar>(values(i,0).imag());
}
#elif defined(PHYSIKA_USE_BUILT_IN_MATRIX)
std::cerr<<"Eigen decomposition not implemeted for built in matrix!\n";
std::exit(EXIT_FAILURE);
#endif
}
void save( Archive & ar,
const Eigen::Matrix<T,RowsAtCompileTime,ColsAtCompileTime,Options,MaxRowsAtCompileTime,MaxColsAtCompileTime> & t,
const unsigned int file_version )
{
int n0 = t.rows();
int n1 = t.cols();
ar << BOOST_SERIALIZATION_NVP( n0 );
ar << BOOST_SERIALIZATION_NVP( n1 );
ar << boost::serialization::make_array( t.data(), n0*n1 );
}
inline
Eigen::Matrix<typename boost::math::tools::promote_args<T1,T2>::type, R, C>
subtract(const Eigen::Matrix<T1,R,C>& m,
const T2& c) {
Eigen::Matrix<typename boost::math::tools::promote_args<T1,T2>::type, R, C>
result(m.rows(),m.cols());
for (int i = 0; i < m.size(); ++i)
result(i) = m(i) - c;
return result;
}
inline
Eigen::Matrix<typename boost::math::tools::promote_args<T1,T2>::type,R,C>
add(const T1& c,
const Eigen::Matrix<T2,R,C>& m) {
Eigen::Matrix<typename boost::math::tools::promote_args<T1,T2>::type,R,C>
result(m.rows(),m.cols());
for (int i = 0; i < result.size(); ++i)
result(i) = c + m(i);
return result;
}
Eigen::Matrix<typename boost::math::tools::promote_args<T1,T2>::type, R, C>
elt_divide(const Eigen::Matrix<T1,R,C>& m1,
const Eigen::Matrix<T2,R,C>& m2) {
stan::math::validate_matching_dims(m1,m2,"elt_multiply");
Eigen::Matrix<typename boost::math::tools::promote_args<T1,T2>::type, R, C>
result(m1.rows(),m2.cols());
for (int i = 0; i < m1.size(); ++i)
result(i) = m1(i) / m2(i);
return result;
}
inline
Eigen::Matrix<fvar<typename stan::return_type<T1,T2>::type>, R2, C2>
multiply(const Eigen::Matrix<fvar<T1>, R2, C2>& m, const T2& c) {
Eigen::Matrix<fvar<typename stan::return_type<T1,T2>::type>,R2,C2> res(m.rows(),m.cols());
for(int i = 0; i < m.rows(); i++) {
for(int j = 0; j < m.cols(); j++)
res(i,j) = to_fvar(c) * m(i,j);
}
return res;
}
inline Eigen::Matrix<T,R,C>
cumulative_sum(const Eigen::Matrix<T,R,C>& m) {
Eigen::Matrix<T,R,C> result(m.rows(),m.cols());
if (m.size() == 0)
return result;
result(0) = m(0);
for (int i = 1; i < result.size(); ++i)
result(i) = m(i) + result(i-1);
return result;
}
std::string matrixString(const Eigen::Matrix<T, R, C>& m)
{
if (m.rows() > 0 && m.cols() > 0)
{
return std::string((char *) m.data(), m.size() * sizeof(T));
} else
{
return "";
}
}
inline
Eigen::Matrix<fvar<typename stan::return_type<T1,T2>::type>,R1,C1>
multiply(const Eigen::Matrix<T1, R1, C1>& m, const fvar<T2>& c) {
Eigen::Matrix<fvar<typename stan::return_type<T1,T2>::type>,R1,C1> res(m.rows(),m.cols());
for(int i = 0; i < m.rows(); i++) {
for(int j = 0; j < m.cols(); j++)
res(i,j) = c * to_fvar(m(i,j));
}
return res;
}
inline bool check_square(const char* function,
const Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic>& y,
const char* name,
T_result* result) {
check_size_match(function,
y.rows(), "Rows of matrix",
y.cols(), "columns of matrix",
result);
return true;
}
inline bool check_cov_matrix(const std::string& function,
const std::string& name,
const Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic>& y) {
check_size_match(function,
"Rows of covariance matrix", y.rows(),
"columns of covariance matrix", y.cols());
check_positive(function, "rows", y.rows());
check_symmetric(function, name, y);
check_pos_definite(function, name, y);
return true;
}
inline
Eigen::Matrix<typename boost::math::tools::promote_args<T1,T2>::type,R,C>
add(const Eigen::Matrix<T1,R,C>& m1,
const Eigen::Matrix<T2,R,C>& m2) {
stan::math::validate_matching_dims(m1,m2,"add");
Eigen::Matrix<typename boost::math::tools::promote_args<T1,T2>::type,R,C>
result(m1.rows(),m1.cols());
for (int i = 0; i < result.size(); ++i)
result(i) = m1(i) + m2(i);
return result;
}
inline
Eigen::Matrix<fvar<T>,R,C>
inverse(const Eigen::Matrix<fvar<T>, R, C>& m) {
using stan::math::multiply;
stan::math::validate_square(m, "inverse");
Eigen::Matrix<T,R,C> m_deriv(m.rows(), m.cols());
Eigen::Matrix<T,R,C> m_inv(m.rows(), m.cols());
for(size_type i = 0; i < m.rows(); i++) {
for(size_type j = 0; j < m.cols(); j++) {
m_inv(i,j) = m(i,j).val_;
m_deriv(i,j) = m(i,j).d_;
}
}
m_inv = m_inv.inverse();
m_deriv = -1 * multiply(multiply(m_inv, m_deriv), m_inv);
return to_fvar(m_inv, m_deriv);
}
IGL_INLINE void igl::average_onto_vertices(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &V,
const Eigen::Matrix<I, Eigen::Dynamic, Eigen::Dynamic> &F,
const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &S,
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &SV)
{
SV = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(V.rows(),S.cols());
Eigen::Matrix<T, Eigen::Dynamic, 1> COUNT = Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(V.rows());
for (int i = 0; i <F.rows(); ++i)
{
for (int j = 0; j<F.cols(); ++j)
{
SV.row(F(i,j)) += S.row(i);
COUNT[F(i,j)] ++;
}
}
for (int i = 0; i <V.rows(); ++i)
SV.row(i) /= COUNT[i];
};
inline typename boost::disable_if<boost::is_same<I, index_uni>, void>::type
assign(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& x,
const cons_index_list<I, nil_index_list>& idxs,
const Eigen::Matrix<U, Eigen::Dynamic, Eigen::Dynamic>& y,
const char* name = "ANON", int depth = 0) {
int x_idx_rows = rvalue_index_size(idxs.head_, x.rows());
math::check_size_match("matrix[multi] assign row sizes",
"lhs", x_idx_rows,
name, y.rows());
math::check_size_match("matrix[multi] assign col sizes",
"lhs", x.cols(),
name, y.cols());
for (int i = 0; i < y.rows(); ++i) {
int m = rvalue_at(i, idxs.head_);
math::check_range("matrix[multi] assign range", name, x.rows(), m);
// recurse to allow double to var assign
for (int j = 0; j < x.cols(); ++j)
x(m - 1, j) = y(i, j);
}
}
Eigen::Matrix<typename boost::math::tools::promote_args<T1,T2>::type, R, C>
elt_divide(const Eigen::Matrix<T1,R,C>& m1,
const Eigen::Matrix<T2,R,C>& m2) {
stan::math::check_matching_dims("elt_divide(%1%)",m1,"m1",
m2,"m2",(double*)0);
Eigen::Matrix<typename boost::math::tools::promote_args<T1,T2>::type, R, C>
result(m1.rows(),m2.cols());
for (int i = 0; i < m1.size(); ++i)
result(i) = m1(i) / m2(i);
return result;
}
void assign(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& x,
const cons_index_list<index_uni,
cons_index_list<index_uni,
nil_index_list> >& idxs,
const U& y,
const char* name = "ANON", int depth = 0) {
int m = idxs.head_.n_;
int n = idxs.tail_.head_.n_;
math::check_range("matrix[uni,uni] assign range", name, x.rows(), m);
math::check_range("matrix[uni,uni] assign range", name, x.cols(), n);
x(m - 1, n - 1) = y;
}
inline bool check_vector(const char* function,
const char* name,
const Eigen::Matrix<T, R, C>& x) {
if (R == 1)
return true;
if (C == 1)
return true;
if (x.rows() == 1 || x.cols() == 1)
return true;
std::ostringstream msg;
msg << ") has " << x.rows() << " rows and "
<< x.cols() << " columns but it should be a vector so it should "
<< "either have 1 row or 1 column";
std::string msg_str(msg.str());
invalid_argument(function,
name,
typename scalar_type<T>::type(),
"(", msg_str.c_str());
return false;
}
inline Eigen::VectorXd
multi_student_t_rng(const double nu,
const Eigen::Matrix<double,Eigen::Dynamic,1>& mu,
const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>& s,
RNG& rng) {
Eigen::VectorXd z(s.cols());
z.setZero();
double w = stan::prob::inv_gamma_rng(nu / 2, nu / 2, rng);
return mu + std::sqrt(w) * stan::prob::multi_normal_rng(z, s, rng);
}
inline void save(
Archive & ar,
const Eigen::Matrix<S, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & g,
const unsigned int version)
{
int rows = g.rows();
int cols = g.cols();
ar & rows;
ar & cols;
ar & boost::serialization::make_array(g.data(), rows * cols);
}
inline
fvar<T>
sum(const Eigen::Matrix<fvar<T>,R,C>& m) {
fvar<T> sum = 0;
if (m.size() == 0)
return 0.0;
for(unsigned i = 0; i < m.rows(); i++) {
for(unsigned j = 0; j < m.cols(); j++)
sum += m(i,j);
}
return sum;
}
static std::string format(Eigen::Matrix<T,S,U> & v, style s = flat)
{
std::stringstream out;
for(int i=0; i<v.rows(); i++)
{
for(int j=0; j<v.cols(); j++)
{
out << v(i,j);
if(j<v.cols()-1)
out << tab;
}
if(i<v.rows()-1)
{
if(s==flat)
out << tab;
else
out << std::endl;
}
}
return out.str();
};
inline
Eigen::Matrix<fvar<T>,R1,C2>
mdivide_right_tri_low(const Eigen::Matrix<double,R1,C1> &A,
const Eigen::Matrix<fvar<T>,R2,C2> &b) {
using stan::math::multiply;
using stan::math::mdivide_right;
stan::math::validate_square(b,"mdivide_right");
stan::math::validate_multiplicable(A,b,"mdivide_right");
Eigen::Matrix<T,R1,C2>
A_mult_inv_b(A.rows(),b.cols());
Eigen::Matrix<T,R2,C2> deriv_b_mult_inv_b(b.rows(),b.cols());
Eigen::Matrix<T,R2,C2> val_b(b.rows(),b.cols());
Eigen::Matrix<T,R2,C2> deriv_b(b.rows(),b.cols());
val_b.setZero();
deriv_b.setZero();
for (int j = 0; j < b.cols(); j++) {
for(int i = j; i < b.rows(); i++) {
val_b(i,j) = b(i,j).val_;
deriv_b(i,j) = b(i,j).d_;
}
}
A_mult_inv_b = mdivide_right(A, val_b);
deriv_b_mult_inv_b = mdivide_right(deriv_b, val_b);
Eigen::Matrix<T,R1,C2>
deriv(A.rows(), b.cols());
deriv = -multiply(A_mult_inv_b, deriv_b_mult_inv_b);
return stan::agrad::to_fvar(A_mult_inv_b, deriv);
}
inline
Eigen::Matrix<fvar<T>,R1,C2>
mdivide_right_tri_low(const Eigen::Matrix<fvar<T>,R1,C1> &A,
const Eigen::Matrix<double,R2,C2> &b) {
using stan::math::multiply;
using stan::math::mdivide_right;
stan::math::validate_square(b,"mdivide_right");
stan::math::validate_multiplicable(A,b,"mdivide_right");
Eigen::Matrix<T,R2,C2> deriv_b_mult_inv_b(b.rows(),b.cols());
Eigen::Matrix<T,R1,C1> val_A(A.rows(),A.cols());
Eigen::Matrix<T,R1,C1> deriv_A(A.rows(),A.cols());
Eigen::Matrix<T,R2,C2> val_b(b.rows(),b.cols());
val_b.setZero();
for (int j = 0; j < A.cols(); j++) {
for(int i = 0; i < A.rows(); i++) {
val_A(i,j) = A(i,j).val_;
deriv_A(i,j) = A(i,j).d_;
}
}
for (size_type j = 0; j < b.cols(); j++) {
for(size_type i = j; i < b.rows(); i++) {
val_b(i,j) = b(i,j);
}
}
return stan::agrad::to_fvar(mdivide_right(val_A, val_b),
mdivide_right(deriv_A, val_b));
}
inline
Eigen::Matrix<fvar<T>, R1, C2>
mdivide_right_tri_low(const Eigen::Matrix<fvar<T>, R1, C1> &A,
const Eigen::Matrix<double, R2, C2> &b) {
check_square("mdivide_right_tri_low", "b", b);
check_multiplicable("mdivide_right_tri_low", "A", A, "b", b);
Eigen::Matrix<T, R2, C2> deriv_b_mult_inv_b(b.rows(), b.cols());
Eigen::Matrix<T, R1, C1> val_A(A.rows(), A.cols());
Eigen::Matrix<T, R1, C1> deriv_A(A.rows(), A.cols());
Eigen::Matrix<T, R2, C2> val_b(b.rows(), b.cols());
val_b.setZero();
for (int j = 0; j < A.cols(); j++) {
for (int i = 0; i < A.rows(); i++) {
val_A(i, j) = A(i, j).val_;
deriv_A(i, j) = A(i, j).d_;
}
}
for (size_type j = 0; j < b.cols(); j++) {
for (size_type i = j; i < b.rows(); i++) {
val_b(i, j) = b(i, j);
}
}
return to_fvar(mdivide_right(val_A, val_b),
mdivide_right(deriv_A, val_b));
}
inline
Eigen::Matrix<fvar<T>, R1, C2>
mdivide_right_tri_low(const Eigen::Matrix<double, R1, C1> &A,
const Eigen::Matrix<fvar<T>, R2, C2> &b) {
check_square("mdivide_right_tri_low", "b", b);
check_multiplicable("mdivide_right_tri_low", "A", A, "b", b);
Eigen::Matrix<T, R1, C2>
A_mult_inv_b(A.rows(), b.cols());
Eigen::Matrix<T, R2, C2> deriv_b_mult_inv_b(b.rows(), b.cols());
Eigen::Matrix<T, R2, C2> val_b(b.rows(), b.cols());
Eigen::Matrix<T, R2, C2> deriv_b(b.rows(), b.cols());
val_b.setZero();
deriv_b.setZero();
for (int j = 0; j < b.cols(); j++) {
for (int i = j; i < b.rows(); i++) {
val_b(i, j) = b(i, j).val_;
deriv_b(i, j) = b(i, j).d_;
}
}
A_mult_inv_b = mdivide_right(A, val_b);
deriv_b_mult_inv_b = mdivide_right(deriv_b, val_b);
Eigen::Matrix<T, R1, C2>
deriv(A.rows(), b.cols());
deriv = -multiply(A_mult_inv_b, deriv_b_mult_inv_b);
return to_fvar(A_mult_inv_b, deriv);
}
inline
Eigen::Matrix<fvar<T>,R1,C2>
mdivide_right(const Eigen::Matrix<double,R1,C1> &A,
const Eigen::Matrix<fvar<T>,R2,C2> &b) {
using stan::math::multiply;
using stan::math::mdivide_right;
stan::math::check_square("mdivide_right(%1%)",b,"b",(double*)0);
stan::math::check_multiplicable("mdivide_right(%1%)",A,"A",
b,"b",(double*)0);
Eigen::Matrix<T,R1,C2>
A_mult_inv_b(A.rows(),b.cols());
Eigen::Matrix<T,R2,C2> deriv_b_mult_inv_b(b.rows(),b.cols());
Eigen::Matrix<T,R2,C2> val_b(b.rows(),b.cols());
Eigen::Matrix<T,R2,C2> deriv_b(b.rows(),b.cols());
for (int j = 0; j < b.cols(); j++) {
for(int i = 0; i < b.rows(); i++) {
val_b(i,j) = b(i,j).val_;
deriv_b(i,j) = b(i,j).d_;
}
}
A_mult_inv_b = mdivide_right(A, val_b);
deriv_b_mult_inv_b = mdivide_right(deriv_b, val_b);
Eigen::Matrix<T,R1,C2>
deriv(A.rows(), b.cols());
deriv = -multiply(A_mult_inv_b, deriv_b_mult_inv_b);
return stan::agrad::to_fvar(A_mult_inv_b, deriv);
}
inline Eigen::VectorXd
multi_normal_cholesky_rng(const Eigen::Matrix<double,Eigen::Dynamic,1>& mu,
const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>& S,
RNG& rng) {
using boost::variate_generator;
using boost::normal_distribution;
static const char* function = "stan::prob::multi_normal_cholesky_rng(%1%)";
using stan::math::check_finite;
check_finite(function, mu, "Location parameter", (double*)0);
variate_generator<RNG&, normal_distribution<> >
std_normal_rng(rng, normal_distribution<>(0,1));
Eigen::VectorXd z(S.cols());
for(int i = 0; i < S.cols(); i++)
z(i) = std_normal_rng();
return mu + S * z;
}
inline bool check_cov_matrix(const char* function,
const Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic>& y,
const char* name,
T_result* result) {
check_size_match(function,
y.rows(), "Rows of covariance matrix",
y.cols(), "columns of covariance matrix",
result);
check_positive(function, y.rows(), "rows", result);
check_symmetric(function, y, name, result);
check_pos_definite(function, y, name, result);
return true;
}