Eigen::MatrixXd TimeIntegrator::Newmark(Eigen::MatrixXd &K, Eigen::MatrixXd &M, double &del, double &gam)
{
Eigen::MatrixXd U,V,A;
U.setZero(K.rows(),_nstep+1);
V.setZero(K.rows(),_nstep+1); A.setZero(K.rows(),_nstep+1);
Eigen::VectorXd F = Eigen::VectorXd::Zero(K.rows());
// Loop over time steps
for (int istep=0; istep<_nstep; ++istep)
{
for (int i=0; i<_napp.rows(); ++i)
{
F(_napp(i)) = F1(istep);
}
U.col(istep+1) = (1.0/(_dt*_dt*gam)*M+K).llt().solve(F+1.0/(_dt*_dt*gam)*M*U.col(istep)+1/(_dt*gam)*M*V.col(istep)+(1.0/(2*gam)-1.0)*M*A.col(istep));
A.col(istep+1) = 1.0/(_dt*_dt*gam)*(U.col(istep+1)-U.col(istep))-1.0/(_dt*gam)*V.col(istep)+(1.0-1.0/(2.0*gam))*A.col(istep);
V.col(istep+1) = V.col(istep)+_dt*(del*A.col(istep+1)+(1-del)*A.col(istep));
}
return U;
}
void relabelFaces(Eigen::MatrixXi& aggregated,
const Eigen::MatrixXd& vertices,
const Eigen::MatrixXi& faces,
Eigen::Vector3i& vertexOffset,
int& offset) {
for (int i = 0; i < faces.rows(); i++) {
aggregated.row(offset++) = vertexOffset + Eigen::Vector3i(faces.row(i));
}
int numVerts = vertices.rows();
vertexOffset += Eigen::Vector3i(numVerts, numVerts, numVerts);
}
void ATIForceTorqueSensorTWE::setNullForceTorque(Eigen::MatrixXd ft_null)
{
if( (ft_null.rows() != 6) || (ft_null.cols() != 1) ){
ROS_ERROR("Invalid ft null size");
return;
}
ft_scale_param_mutex_.lock();
ft_null_ = ft_null;
ft_scale_param_mutex_.unlock();
}
void lpengine::SwapCols(Eigen::MatrixXd& matn, int index_matn,
Eigen::MatrixXd& matb, int index_matb) {
Eigen::VectorXd tmp(matb.rows());
tmp = matb.col(index_matb);
matb.col(index_matb) = matn.col(index_matn);
matn.col(index_matn) = tmp;
int tmp_index;
tmp_index = p_.basic_set_[index_matb];
p_.basic_set_[index_matb] = p_.nonbasic_set_[index_matn];
p_.nonbasic_set_[index_matn] = tmp_index;
}
IGL_INLINE void igl::ViewerData::add_points(const Eigen::MatrixXd& P, const Eigen::MatrixXd& C)
{
Eigen::MatrixXd P_temp;
// If P only has two columns, pad with a column of zeros
if (P.cols() == 2)
{
P_temp = Eigen::MatrixXd::Zero(P.rows(),3);
P_temp.block(0,0,P.rows(),2) = P;
}
else
P_temp = P;
int lastid = points.rows();
points.conservativeResize(points.rows() + P_temp.rows(),6);
for (unsigned i=0; i<P_temp.rows(); ++i)
points.row(lastid+i) << P_temp.row(i), i<C.rows() ? C.row(i) : C.row(C.rows()-1);
dirty |= DIRTY_OVERLAY_POINTS;
}
bool FastMarchingData::BuildConnectivity(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F)
{
if (V.rows() == 0 || F.rows() == 0) return false;
//V_border = igl::is_border_vertex(V, F);
vertex_vertex_adjacency(F, VV);
vertex_triangle_adjacency(F, VF);
triangle_triangle_adjacency(F, TT, TTi);
// igl::edge_topology(V, F, E, F2E, E2F);
return true;
}
void MacauOnePrior<FType>::sample_latents(
Eigen::MatrixXd &U,
const Eigen::SparseMatrix<double> &Ymat,
double mean_value,
const Eigen::MatrixXd &V,
double alpha,
const int num_latent)
{
const int N = U.cols();
const int D = U.rows();
#pragma omp parallel for schedule(dynamic, 4)
for (int i = 0; i < N; i++) {
const int nnz = Ymat.outerIndexPtr()[i + 1] - Ymat.outerIndexPtr()[i];
VectorXd Yhat(nnz);
// precalculating Yhat and Qi
int idx = 0;
VectorXd Qi = lambda;
for (SparseMatrix<double>::InnerIterator it(Ymat, i); it; ++it, idx++) {
Qi.noalias() += alpha * V.col(it.row()).cwiseAbs2();
Yhat(idx) = mean_value + U.col(i).dot( V.col(it.row()) );
}
VectorXd rnorms(num_latent);
bmrandn_single(rnorms);
for (int d = 0; d < D; d++) {
// computing Lid
const double uid = U(d, i);
double Lid = lambda(d) * (mu(d) + Uhat(d, i));
idx = 0;
for ( SparseMatrix<double>::InnerIterator it(Ymat, i); it; ++it, idx++) {
const double vjd = V(d, it.row());
// L_id += alpha * (Y_ij - k_ijd) * v_jd
Lid += alpha * (it.value() - (Yhat(idx) - uid*vjd)) * vjd;
}
// Now use Lid and Qid to update uid
double uid_old = U(d, i);
double uid_var = 1.0 / Qi(d);
// sampling new u_id ~ Norm(Lid / Qid, 1/Qid)
U(d, i) = Lid * uid_var + sqrt(uid_var) * rnorms(d);
// updating Yhat
double uid_delta = U(d, i) - uid_old;
idx = 0;
for (SparseMatrix<double>::InnerIterator it(Ymat, i); it; ++it, idx++) {
Yhat(idx) += uid_delta * V(d, it.row());
}
}
}
}
void load( Archive & ar,
Eigen::MatrixXd & t,
const unsigned int file_version )
{
int n0;
ar >> BOOST_SERIALIZATION_NVP( n0 );
int n1;
ar >> BOOST_SERIALIZATION_NVP( n1 );
t.resize( n0, n1 );
ar >> make_array( t.data(), t.rows()*t.cols() );
}
void compare_force_constant_matrix(
Eigen::MatrixXd & low_carb_result,
const std::string & filename_reach_results,
double precision,
bool is_hex,
bool toggle_row_and_col) {
if (!exists(boost::filesystem::path(filename_reach_results))) {
throw std::runtime_error("file does not exist: " + filename_reach_results);
}
std::ifstream file_reach_results;
file_reach_results.open(filename_reach_results);
size_t rows=0;
size_t cols=0;
std::string row;
while (std::getline(file_reach_results,row)) {
cols=0;
size_t begin=0;
size_t delimiter_pos;
std::string entry;
do {
delimiter_pos = row.find(',',begin);
entry = row.substr(begin, delimiter_pos-begin);
double low_carb_result_entry;
if(toggle_row_and_col) {
low_carb_result_entry = low_carb_result(cols,rows);
} else {
low_carb_result_entry = low_carb_result(rows, cols);
}
if (is_hex)
{
BOOST_REQUIRE_CLOSE_FRACTION(cast_hex_to_double(entry), low_carb_result_entry, precision);
}
else
BOOST_REQUIRE_CLOSE_FRACTION(std::atof(entry.c_str()), low_carb_result_entry, precision);
begin=delimiter_pos+1;
cols++;
} while (delimiter_pos != std::string::npos);
rows++;
}
BOOST_CHECK_EQUAL(rows, cols);
BOOST_CHECK_EQUAL(rows, low_carb_result.rows());
BOOST_CHECK_EQUAL(cols, low_carb_result.cols());
//std::cout << "average difference :" << diff_sum/(cols*rows) << std::endl;
}
void Trajectory::set_misc(const Eigen::MatrixXd& misc)
{
if (misc.rows()==ts_.size())
{
// misc is of size n_time_steps X n_dims_misc
misc_ = misc;
}
else if (misc.rows()==1)
{
// misc is of size 1 X n_dim_misc
// then replicate it so that it becomes of size n_time_steps X n_dims_misc
misc_ = misc.replicate(ts_.size(),1);
}
else
{
cerr << __FILE__ << ":" << __LINE__ << ":";
cerr << "misc must have 1 or " << ts_.size() << " rows, but it has " << misc.rows() << endl;
}
}
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;
}
}
/**
* @brief fprintf
* @param mat
*/
void fprintfMat(Eigen::MatrixXd &mat, std::string name)
{
FILE *file = fopen(name.c_str(), "w");
for(int i = 0; i < mat.rows(); i++){
for(int j = 0; j < mat.cols(); j++){
fprintf(file, "%f ", mat(i, j));
}
fprintf(file, "\n");
}
fclose(file);
}
Eigen::MatrixXd normProbMatrix(Eigen::MatrixXd P)
{
// each column is a probability simplex
Eigen::MatrixXd P_norm(P.rows(), P.cols());
for (int col = 0; col < P.cols(); col++)
{
Eigen::VectorXd P_vec = P.col(col);
P_norm.col(col) = normProbVector(P_vec);
}
return P_norm;
}
void eigen2mat(Eigen::MatrixXd &grayscale, cv::Mat &dst)
{
int width = grayscale.cols();
int height = grayscale.rows();
dst = cv::Mat(height, width, CV_8UC1);
uint8_t* image_ptr = (uint8_t*)dst.data;
for (int i = 0; i < height; i++)
for (int j = 0; j < width; j++)
image_ptr[i*dst.cols + j] = grayscale(i, j);
}
void KMeans::updateCenters(const Eigen::MatrixXd& X)
{
const int N = X.rows();
for(int n = 0; n < N; ++n)
{
const int cluster = clusterIndices[n];
v(cluster)++;
const double eta = 1.0 / (double) v(cluster);
C.row(cluster) += eta * (X.row(n) - C.row(cluster));
}
}
void prettyPrint (Eigen::MatrixXd const & mm, std::ostream & os,
std::string const & title, std::string const & prefix,
bool vecmode, bool nonl)
{
char const * nlornot ("\n");
if (nonl) {
nlornot = "";
}
if ( ! title.empty()) {
os << title << nlornot;
}
if ((mm.rows() <= 0) || (mm.cols() <= 0)) {
os << prefix << " (empty)" << nlornot;
}
else {
if (vecmode) {
if ( ! prefix.empty()) {
os << prefix;
}
for (int ir (0); ir < mm.rows(); ++ir) {
prettyPrint (mm.coeff (ir, 0), os);
}
os << nlornot;
}
else {
for (int ir (0); ir < mm.rows(); ++ir) {
if ( ! prefix.empty()) {
os << prefix;
}
for (int ic (0); ic < mm.cols(); ++ic) {
prettyPrint (mm.coeff (ir, ic), os);
}
os << nlornot;
}
}
}
}
TEST(slice_into, dense_identity)
{
Eigen::MatrixXd A = Eigen::MatrixXd::Random(10,9);
Eigen::VectorXi I = Eigen::VectorXi::LinSpaced(A.rows(),0,A.rows()-1);
Eigen::VectorXi J = Eigen::VectorXi::LinSpaced(A.cols(),0,A.cols()-1);
{
Eigen::MatrixXd B(I.maxCoeff()+1,J.maxCoeff()+1);
igl::slice_into(A,I,J,B);
test_common::assert_eq(A,B);
}
{
Eigen::MatrixXd B(I.maxCoeff()+1,A.cols());
igl::slice_into(A,I,1,B);
test_common::assert_eq(A,B);
}
{
Eigen::MatrixXd B(A.rows(),J.maxCoeff()+1);
igl::slice_into(A,J,2,B);
test_common::assert_eq(A,B);
}
}
int main(int argc, char *argv[])
{
using namespace Eigen;
using namespace std;
igl::readOBJ(TUTORIAL_SHARED_PATH "/bump-domain.obj",V,F);
U=V;
// Find boundary vertices outside annulus
typedef Matrix<bool,Dynamic,1> VectorXb;
VectorXb is_outer = (V.rowwise().norm().array()-1.0)>-1e-15;
VectorXb is_inner = (V.rowwise().norm().array()-0.15)<1e-15;
VectorXb in_b = is_outer.array() || is_inner.array();
igl::colon<int>(0,V.rows()-1,b);
b.conservativeResize(stable_partition( b.data(), b.data()+b.size(),
[&in_b](int i)->bool{return in_b(i);})-b.data());
bc.resize(b.size(),1);
for(int bi = 0;bi<b.size();bi++)
{
bc(bi) = (is_outer(b(bi))?0.0:1.0);
}
// Pseudo-color based on selection
MatrixXd C(F.rows(),3);
RowVector3d purple(80.0/255.0,64.0/255.0,255.0/255.0);
RowVector3d gold(255.0/255.0,228.0/255.0,58.0/255.0);
for(int f = 0;f<F.rows();f++)
{
if( in_b(F(f,0)) && in_b(F(f,1)) && in_b(F(f,2)))
{
C.row(f) = purple;
}else
{
C.row(f) = gold;
}
}
// Plot the mesh with pseudocolors
igl::viewer::Viewer viewer;
viewer.data.set_mesh(U, F);
viewer.core.show_lines = false;
viewer.data.set_colors(C);
viewer.core.trackball_angle = Eigen::Quaternionf(0.81,-0.58,-0.03,-0.03);
viewer.core.trackball_angle.normalize();
viewer.callback_pre_draw = &pre_draw;
viewer.callback_key_down = &key_down;
viewer.core.is_animating = true;
viewer.core.animation_max_fps = 30.;
cout<<
"Press [space] to toggle animation."<<endl<<
"Press '.' to increase k."<<endl<<
"Press ',' to decrease k."<<endl;
viewer.launch();
}
void BranchList::calculateOrientations()
{
for (int i = 0; i < mBranches.size(); i++)
{
Eigen::MatrixXd positions = mBranches[i]->getPositions();
Eigen::MatrixXd diff = positions.rightCols(positions.cols() - 1) - positions.leftCols(positions.cols() - 1);
Eigen::MatrixXd orientations(positions.rows(),positions.cols());
orientations.leftCols(orientations.cols() - 1) = diff / diff.norm();
orientations.rightCols(1) = orientations.col(orientations.cols() - 1);
mBranches[i]->setOrientations(orientations);
}
}
double Fnorm(Eigen::MatrixXd &d)
{
double norm = 0;
for(int i = 0; i < d.rows(); i++)
{
for(int j = 0; j < d.cols(); j++)
{
norm += d(i, j) * d(i, j);
}
}
return sqrt(norm);
}
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;
}
IGL_INLINE void igl::ViewerData::set_uv(const Eigen::MatrixXd& UV)
{
using namespace std;
if (UV.rows() == V.rows())
{
set_face_based(false);
V_uv = UV;
}
else
cerr << "ERROR (set_UV): Please provide uv per vertex.";
dirty |= DIRTY_UV;
}
void
write_metric(stan::interface_callbacks::writer::base_writer& writer) {
writer("Elements of inverse mass matrix:");
std::stringstream mInv_ss;
for (int i = 0; i < mInv.rows(); ++i) {
mInv_ss.str("");
mInv_ss << mInv(i, 0);
for (int j = 1; j < mInv.cols(); ++j)
mInv_ss << ", " << mInv(i, j);
writer(mInv_ss.str());
}
}
//------------------------------------------------------------------------------
void LocalAssembler::assemble_interior_facet(Eigen::MatrixXd& A,
UFC& ufc,
const std::vector<double>& vertex_coordinates,
const ufc::cell& ufc_cell,
const Cell& cell,
const Facet& facet,
const std::size_t local_facet,
const MeshFunction<std::size_t>* domains)
{
// Skip if there are no interior facet integrals
if (!ufc.form.has_interior_facet_integrals())
return;
// Extract default interior facet integral
ufc::interior_facet_integral* integral
= ufc.default_interior_facet_integral.get();
// Get integral for sub domain (if any)
if (domains && !domains->empty())
integral = ufc.get_interior_facet_integral((*domains)[facet]);
// Skip integral if zero
if (!integral)
return;
// Update to current pair of cells and facets
ufc.update(cell, vertex_coordinates, ufc_cell,
cell, vertex_coordinates, ufc_cell,
integral->enabled_coefficients());
// Tabulate interior facet tensor on macro element
integral->tabulate_tensor(ufc.macro_A.data(), ufc.macro_w(),
vertex_coordinates.data(),
vertex_coordinates.data(),
local_facet, local_facet,
ufc_cell.orientation,
ufc_cell.orientation);
// Stuff upper left quadrant (corresponding to this cell) into A
const std::size_t M = A.rows();
const std::size_t N = A.cols();
if (N == 1)
{
for (std::size_t i = 0; i < M; i++)
A(i, 0) = ufc.macro_A[i];
}
else
{
for (std::size_t i = 0; i < M; i++)
for (std::size_t j = 0; j < N; j++)
A(i, j) += ufc.macro_A[2*N*i + j];
}
}
/**
* @function MatrixMultiply
*/
Eigen::MatrixXd MatrixMultiply( Eigen::MatrixXd _A,
Eigen::MatrixXd _B ) {
Eigen::MatrixXd C( _A.rows(), _B.cols() );
if( _A.cols() != _B.rows() ) {
printf("--(!) Incompatible dimensions! \n");
}
else {
for( size_t i = 0; i < _A.rows(); ++i ) {
for( size_t j =0; j < _B.cols(); ++j ) {
C(i,j) = 0;
for( size_t k=0; k < _A.cols(); ++k ) {
C(i,j) += _A(i,k)*_B(k,j);
}
}
}
}
return C;
}
void printCovariance(ofstream &printName, const Eigen::MatrixXd &covariance)
{
for(int i=0; i<covariance.rows(); i++)
{
for(int j=0; j<covariance.cols(); j++)
{
printName<<covariance(i,j)<<" ";
}
}
printName<<endl;
}
// Returns the full transformation matrix for the kinematic chain of the arm
static Eigen::Matrix4d arm_tr_mat(const Eigen::Vector4d& joints)
{
Eigen::MatrixXd dh = _dh_mat(joints);
Eigen::Matrix4d mat = Eigen::Matrix4d::Identity(4, 4);
for (int i = 0; i < dh.rows(); i++) {
mat = mat * _trmat_dh(dh.row(i));
}
return mat;
}
// build transformation
void NuTo::ZeroMeanUnitVarianceTransformation::Build(const Eigen::MatrixXd& rCoordinates)
{
// check input
if (rCoordinates.cols() < 2)
{
throw Exception("[NuTo::ZeroMeanUnitVarianceTransformation::Build] number of points must be greater "
"than one - check the number of columns of your matrix.");
}
if (rCoordinates.rows() <= this->mCoordinate)
{
throw Exception("[NuTo::ZeroMeanUnitVarianceTransformation::Build] coordinate to be transformed is "
"out of range - check the number of rows of your Matrix.");
}
// calculate mean
this->mMean = 0.0;
const double* dataPtr = &rCoordinates.data()[mCoordinate];
for (int count = 0; count < rCoordinates.cols(); count++)
{
this->mMean += *dataPtr;
dataPtr += rCoordinates.rows();
}
this->mMean /= rCoordinates.cols();
// calculate variance
double variance = 0.0;
dataPtr = &rCoordinates.data()[mCoordinate];
for (int count = 0; count < rCoordinates.cols(); count++)
{
double delta = *dataPtr - this->mMean;
variance += delta * delta;
dataPtr += rCoordinates.rows();
}
variance /= rCoordinates.cols() - 1;
this->mStandardDeviation = sqrt(variance);
if (this->mStandardDeviation < 1e-12)
{
throw Exception("[NuTo::ZeroMeanUnitVarianceTransformation::Build] the standard deviation is almost zero");
}
}
Eigen::VectorXd kalman::update(Eigen::MatrixXd finalP)
{
Eigen::MatrixXd kalman_W = Eigen::MatrixXd::Identity(3*finalP.rows(),3*finalP.rows())*0.1;
Eigen::MatrixXd kalman_S(3*finalP.rows(),3*finalP.rows());
Eigen::VectorXd kalman_y(3*finalP.rows());
Eigen::MatrixXd kalman_K(7,3*finalP.rows());
int k=0;
for (int i=0;i<finalP.rows();i++)
for(int j=0;j<3;j++)
kalman_y[k++]=finalP(i,j);
kalman_y = kalman_y-kalman_h;
kalman_S = (kalman_H*kalman_P)*kalman_H.transpose() + kalman_W;
kalman_K=(kalman_P*kalman_H.transpose())*kalman_S.inverse();
//std::cout << "The inverse of S is:" <<std::endl<<kalman_S.inverse() << endl;
kalman_x += kalman_K * kalman_y;
for (int i=3;i<7;i++)
{
kalman_x(i)=kalman_x(i)/(sqrt(kalman_x(3)*kalman_x(3)+kalman_x(4)*kalman_x(4)+kalman_x(5)*kalman_x(5)+kalman_x(6)*kalman_x(6)));
}
kalman_P -= kalman_K*kalman_H*kalman_P;
ROS_INFO("kalman_x %G",kalman_x(0));
return kalman_x;
}
// Helpers that draws the most common meshes
IGL_INLINE void igl::ViewerData::set_mesh(const Eigen::MatrixXd& _V, const Eigen::MatrixXi& _F)
{
using namespace std;
Eigen::MatrixXd V_temp;
// If V only has two columns, pad with a column of zeros
if (_V.cols() == 2)
{
V_temp = Eigen::MatrixXd::Zero(_V.rows(),3);
V_temp.block(0,0,_V.rows(),2) = _V;
}
else
V_temp = _V;
if (V.rows() == 0 && F.rows() == 0)
{
clear();
V = V_temp;
F = _F;
compute_normals();
uniform_colors(Eigen::Vector3d(51.0/255.0,43.0/255.0,33.3/255.0),
Eigen::Vector3d(255.0/255.0,228.0/255.0,58.0/255.0),
Eigen::Vector3d(255.0/255.0,235.0/255.0,80.0/255.0));
grid_texture();
}
else
{
if (_V.rows() == V.rows() && _F.rows() == F.rows())
{
V = V_temp;
F = _F;
}
else
cerr << "ERROR (set_mesh): The new mesh has a different number of vertices/faces. Please clear the mesh before plotting.";
}
dirty |= DIRTY_FACE | DIRTY_POSITION;
}
