bool toEigen(const yarp::sig::Vector & vec_yrp, Eigen::VectorXd & vec_eigen)
{
if( vec_yrp.size() != vec_eigen.size() ) { vec_eigen.resize(vec_yrp.size()); }
if( memcpy(vec_eigen.data(),vec_yrp.data(),sizeof(double)*vec_eigen.size()) != NULL ) {
return true;
} else {
return false;
}
}
void GetBoundingBox(const Eigen::MatrixXd& data, Eigen::VectorXd& minCorner, Eigen::VectorXd& maxCorner)
{
assert(data.cols() > 0);
minCorner.resize(data.rows());
maxCorner.resize(data.rows());
for(unsigned int coordinate = 0; coordinate < data.rows(); ++coordinate)
{
minCorner[coordinate] = std::numeric_limits<double>::max();
maxCorner[coordinate] = std::numeric_limits<double>::min();
for(unsigned int pointId = 0; pointId < data.cols(); ++pointId)
{
if(data(coordinate, pointId) > maxCorner(coordinate))
{
maxCorner(coordinate) = data(coordinate, pointId);
}
if(data(coordinate, pointId) < minCorner(coordinate))
{
minCorner(coordinate) = data(coordinate, pointId);
}
}
}
}
int main(int argc, char** argv) {
ros::init(argc, argv, "joint_to_cart");
ros::NodeHandle nh;
ros::NodeHandle _nh("~");
mRobot = new RTKRobotArm(true);
if(!mRobot->initialize(_nh)) {
ROS_ERROR("Error while loading robot");
return 1;
}
if(!parseParams(_nh)) {
ROS_ERROR("Errors while parsing arguments.");
return 1;
}
numdof = mRobot->numdof;
read_torque.resize(numdof);
read_jpos.resize(numdof);
ee_ft.resize(6);
pub_pose = nh.advertise<geometry_msgs::PoseStamped>(output_cart_pose, 3);
pub_ft = nh.advertise<geometry_msgs::WrenchStamped>(output_cart_ft, 3);
ros::Subscriber sub = nh.subscribe<sensor_msgs::JointState>(input_joint_topic, 10, jointStateCallback,ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_ft = nh.subscribe<geometry_msgs::WrenchStamped>("/right_arm_ft_sensor/wrench", 10, sensorFTCallback, ros::TransportHints().tcpNoDelay());
ROS_INFO("Node started");
ros::spin();
return 0;
}
void LiftingLine::solve_for_best_gamma(double cL)
{
int matsize = this->segments.size() + 1;
Eigen::MatrixXd matrix;
Eigen::VectorXd rhs;
Eigen::VectorXd result;
matrix.resize(matsize, matsize);
matrix.setZero();
rhs.resize(matsize);
rhs.setZero();
result.resize(matsize);
result.setZero();
// adding the main min-function
for (int i = 0; i < (matsize - 1); i++)
{
for (int j = 0; j < (matsize - 1); j++)
{
matrix(i, j) += this->segments[i].b() * this->segments[j].ind_influence(this->segments[i]);
matrix(i, j) += this->segments[j].b() * this->segments[i].ind_influence(this->segments[j]);
}
// adding lagrange multiplicator
matrix(i, matsize - 1) += this->segments[i].lift_factor;
}
for (int i = 0; i < (matsize -1); i++)
{
matrix(matsize - 1, i) += this->segments[i].lift_factor;
}
rhs(matsize - 1) += cL;
result = matrix.fullPivHouseholderQr().solve(rhs);
for (int i = 0; i < matsize - 1; i++)
{
this->segments[i].best_gamma = result[i];
}
}
// Parse right hand side arguments for a matlab mex function.
//
// Inputs:
// nrhs number of right hand side arguments
// prhs pointer to right hand side arguments
// Outputs:
// V n by dim list of mesh vertex positions
// F m by dim list of mesh face indices
// s 1 by dim bone source vertex position
// d 1 by dim bone dest vertex position
// "Throws" matlab errors if dimensions are not sane.
void parse_rhs(
const int nrhs,
const mxArray *prhs[],
Eigen::MatrixXd & V,
Eigen::MatrixXi & F,
Eigen::VectorXd & s,
Eigen::VectorXd & d)
{
using namespace std;
if(nrhs < 4)
{
mexErrMsgTxt("nrhs < 4");
}
const int dim = mxGetN(prhs[0]);
if(dim != 3)
{
mexErrMsgTxt("Mesh vertex list must be #V by 3 list of vertex positions");
}
if(dim != (int)mxGetN(prhs[1]))
{
mexErrMsgTxt("Mesh facet size must equal dimension");
}
if(dim != (int)mxGetN(prhs[2]))
{
mexErrMsgTxt("Source dim must equal vertex dimension");
}
if(dim != (int)mxGetN(prhs[3]))
{
mexErrMsgTxt("Dest dim must equal vertex dimension");
}
// set number of mesh vertices
const int n = mxGetM(prhs[0]);
// set vertex position pointers
double * Vp = mxGetPr(prhs[0]);
// set number of faces
const int m = mxGetM(prhs[1]);
// set face index list pointer
double * Fp = mxGetPr(prhs[1]);
// set source and dest pointers
double * sp = mxGetPr(prhs[2]);
double * dp = mxGetPr(prhs[3]);
// resize output to transpose
V.resize(n,dim);
copy(Vp,Vp+n*dim,V.data());
// resize output to transpose
F.resize(m,dim);
// Q: Is this doing a cast?
// A: Yes.
copy(Fp,Fp+m*dim,F.data());
// http://stackoverflow.com/a/4461466/148668
transform(F.data(),F.data()+m*dim,F.data(),
bind2nd(std::plus<double>(),-1.0));
// resize output to transpose
s.resize(dim);
copy(sp,sp+dim,s.data());
d.resize(dim);
copy(dp,dp+dim,d.data());
}
//TODO: Compute the joint impedance here. Conversion from cart_stiffness to joint_stiffness not implemented yet.
void computeJointImpedance(Eigen::VectorXd& joint_stiff, Eigen::VectorXd& joint_damp) {
if(joint_stiff.size() != numdof) {
joint_stiff.resize(numdof);
}
if(joint_damp.size() != numdof) {
joint_damp.resize(numdof);
}
for(int i=0; i<numdof; ++i) {
joint_stiff[i] = DEFAULT_JSTIFF;
joint_damp[i] = DEFAULT_JDAMP;
}
}
segment_info(unsigned int nc) {
E.resize(6, nc);
E_tilde.resize(6, nc);
G.resize(nc);
M.resize(nc, nc);
EZ.resize(nc);
E.setZero();
E_tilde.setZero();
M.setZero();
G.setZero();
EZ.setZero();
};
segment_info(unsigned int nc):
D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0)
{
E.resize(6, nc);
E_tilde.resize(6, nc);
G.resize(nc);
M.resize(nc, nc);
EZ.resize(nc);
E.setZero();
E_tilde.setZero();
M.setZero();
G.setZero();
EZ.setZero();
};
int main(int argc, char *argv[])
{
using namespace Eigen;
using namespace std;
MatrixXd V;
MatrixXi F;
igl::readOFF(TUTORIAL_SHARED_PATH "/cheburashka.off",V,F);
// Plot the mesh
igl::opengl::glfw::Viewer viewer;
viewer.data().set_mesh(V, F);
viewer.data().show_lines = false;
viewer.callback_key_down = &key_down;
// One fixed point
b.resize(1,1);
// point on belly.
b<<2556;
bc.resize(1,1);
bc<<1;
// Construct Laplacian and mass matrix
SparseMatrix<double> L,M,Minv;
igl::cotmatrix(V,F,L);
igl::massmatrix(V,F,igl::MASSMATRIX_TYPE_VORONOI,M);
//M = (M/M.diagonal().maxCoeff()).eval();
igl::invert_diag(M,Minv);
// Bi-Laplacian
Q = L.transpose() * (Minv * L);
// Zero linear term
B = VectorXd::Zero(V.rows(),1);
// Lower and upper bound
lx = VectorXd::Zero(V.rows(),1);
ux = VectorXd::Ones(V.rows(),1);
// Equality constraint constrain solution to sum to 1
Beq.resize(1,1);
Beq(0) = 0.08;
Aeq = M.diagonal().sparseView().transpose();
// (Empty inequality constraints)
solve(viewer);
cout<<
"Press '.' to increase scale and resolve."<<endl<<
"Press ',' to decrease scale and resolve."<<endl;
viewer.launch();
}
/** Return the vector from a back-substitution step that solves: Ux=b */
void CSparseMatrix::CholeskyDecomp::backsub(
const Eigen::VectorXd& b, Eigen::VectorXd& sol) const
{
ASSERT_(b.size() > 0);
sol.resize(b.size());
this->backsub(&b[0], &sol[0], b.size());
}
void Locator::layoutByWord(const QHash<SymbolPath, SymbolData>& symbolWord,
QVector<QVector2D>& pos2D,
float sparseFactor,
float* radius,
QVector<float>* projRadius)
{
// get doc term matrix and radius of each data
SparseMatrix docTermMat;
Eigen::VectorXd radiusVec;
buildDocTermMat(symbolWord, docTermMat, radiusVec);
// use external radius data
if (projRadius)
{
radiusVec.resize(projRadius->size());
for (int i = 0; i < projRadius->size(); ++i)
radiusVec[i] = (*projRadius)[i];
}
// compute 2D position
m_wordLocator.setDocTermMat(docTermMat, radiusVec);
m_wordLocator.setUseTfIdfMeasure(false);
m_wordLocator.compute(sparseFactor);
//m_wordLocator.saveMatsToFile("H:/Programs/QtCreator/qt-creator_master/src/plugins/MyPlugin/CodeAtlas/codeData.m");
pos2D = m_wordLocator.getOri2DPositions();
if (radius)
*radius = m_wordLocator.getOri2DRadius() + 0.1f;
}
void Locator::buildDocTermMat(const QHash<SymbolPath, SymbolData>& symbolWordList,
SparseMatrix& docTermMat,
Eigen::VectorXd& radiusVec)
{
int nSymbols = symbolWordList.size();
int nWords = SymbolWordAttr::totalWords();
QVector<float> wordCountPerDoc(nSymbols,0.f); // total number of words for each doc
QVector<float> docCountPerWord(nWords ,0.f); // total number of doc for each word
docTermMat = SparseMatrix(nSymbols, nWords);
radiusVec.resize(nSymbols);
QHash<SymbolPath, SymbolData>::ConstIterator pSymbol;
int ithSymbol = 0;
for (pSymbol = symbolWordList.begin();
pSymbol != symbolWordList.end(); ++pSymbol, ++ithSymbol)
{
const SymbolData& item = pSymbol.value();
wordCountPerDoc[ithSymbol] = item.getTotalWordCount();
QMap<int,float>::ConstIterator pWord;
for (pWord = item.m_wordWeightMap.begin();
pWord != item.m_wordWeightMap.end(); ++pWord)
{
int wordId = pWord.key();
float wordCount = pWord.value();
docCountPerWord[wordId] += 1;
docTermMat.insert(ithSymbol, wordId) = wordCount;
}
radiusVec(ithSymbol) = item.getRadius();
}
docTermMat.makeCompressed();
}
//==============================================================================
void Spline::evaluateDerivative(
double _t, int _derivative, Eigen::VectorXd& _tangentVector) const
{
if (mSegments.empty())
throw std::logic_error("Unable to evaluate empty trajectory.");
if (_derivative < 1)
throw std::logic_error("Derivative must be positive.");
const auto targetSegmentInfo = getSegmentForTime(_t);
const auto& targetSegment = mSegments[targetSegmentInfo.first];
const auto evaluationTime = _t - targetSegmentInfo.second;
// Return zero for higher-order derivatives.
if (_derivative < targetSegment.mCoefficients.cols())
{
// TODO: We should transform this into the body frame using the adjoint
// transformation.
_tangentVector = evaluatePolynomial(
targetSegment.mCoefficients, evaluationTime, _derivative);
}
else
{
_tangentVector.resize(mStateSpace->getDimension());
_tangentVector.setZero();
}
}
void Parameterizable::getParameterVectorSelected(Eigen::VectorXd& values, bool normalized) const
{
Eigen::VectorXd all_values;
getParameterVectorAll(all_values);
values.resize(getParameterVectorSelectedSize());
// We cannot do this with Block, because regions might not be contiguous
int ii = 0;
for (int all_ii=0; all_ii<selected_mask_.size(); all_ii++)
if (selected_mask_[all_ii]>0)
values[ii++] = all_values[all_ii];
if (normalized)
{
VectorXd min_vec, max_vec;
getParameterVectorSelectedMinMax(min_vec, max_vec);
VectorXd range = (max_vec.array()-min_vec.array());
for (int ii=0; ii<values.size(); ii++)
{
if (range[ii]>0)
{
values[ii] = (values[ii]-min_vec[ii])/range[ii];
}
else
{
if (abs(max_vec[ii])>0)
values[ii] = values[ii]/abs(2*max_vec[ii]);
}
}
}
}
bool BlockSparseMatrix::ConjugateGradientSolve(const Eigen::VectorXd& rhs, Eigen::VectorXd& sol)
{
//sol = mMatrix.llt().solve(rhs);
return LltSolve(rhs, sol);
MatrixSolver* solver = new PCGSolver();
int vectorLength = static_cast<int>(rhs.size());
double* result = new double[vectorLength];
double* rhsData = new double[vectorLength];
memcpy(rhsData, rhs.data(), vectorLength * sizeof(double));
if (mbCSRDirty)
{
mCSREquivalent.ConvertFromBlockSparseMatrix(*this);
mbCSRDirty = false;
}
solver->SetMatrix(&mCSREquivalent);
solver->SetRHS(rhsData);
solver->Solve(result, true);
sol.resize(vectorLength);
for (int i = 0; i < vectorLength; ++i)
sol[i] = result[i];
delete[] rhsData;
delete[] result;
delete solver;
return true;
}
void DiagonalBlockSparseMatrix::Solve(const Eigen::VectorXd& rhs, Eigen::VectorXd& sol)
{
int numRows = GetNumRows();
int numCols = GetNumCols();
LOG_IF(FATAL, numRows != numCols) << "DiagonalBlockSparseMatrix is not a square matrix and noninvertible.";
int vecLength = rhs.size();
LOG_IF(FATAL, numCols != vecLength) << "DiagonalBlockSparseMatrix and right hand side vector are of different dimensions.";
sol.resize(numCols);
int numUntilLast = 0;
int numDiagElements = static_cast<int>(mDiagElements.size());
for (int i = 0; i < numDiagElements; ++i)
{
int numColsElement = mDiagElements[i].GetNumCols();
Eigen::VectorXd partSol(numColsElement);
Eigen::VectorXd partRhs = rhs.segment(numUntilLast, numColsElement);
#if LLT_SOLVE
mDiagElements[i].LltSolve(partRhs, partSol);
#else
mDiagElements[i].ConjugateGradientSolve(partRhs, partSol);
#endif
sol.segment(numUntilLast, numColsElement) = partSol;
numUntilLast += numColsElement;
}
}
//==============================================================================
TEST(Lemke, Lemke_4D)
{
Eigen::MatrixXd A;
Eigen::VectorXd b;
Eigen::VectorXd* f;
int err;
f = new Eigen::VectorXd(4);
A.resize(4,4);
A <<
3.999,0.9985, 1.001, -2,
0.9985, 3.998, -2,0.9995,
1.001, -2, 4.002, 1.001,
-2,0.9995, 1.001, 4.001;
b.resize(4);
b <<
-0.01008,
-0.009494,
-0.07234,
-0.07177;
err = dart::lcpsolver::Lemke(A,b,f);
EXPECT_EQ(err, 0);
EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
}
MutatorEquation(F f, double mu, double alpha, double target_surplus = 1.0) :
mutation_rate_Q_{ mu },
mutator_gene_transition_rate_P_to_Q_{ alpha }
{
double surplus = 0.0;
fitness_.resize(2 * (GenomeLenght_ + 1), 1);
f_.resize(GenomeLenght_ + 1);
g_.resize(GenomeLenght_ + 1);
for (int i = 0; i < GenomeLenght_ + 1; i++) {
double val_x = 1.0 * GenomeLenght_ + 1 - 2 * i;
val_x /= GenomeLenght_ + 1.0;
double val = f(val_x);
surplus += val;
f_[i] = val;
g_[i] = val;
fitness_(i) = val;
fitness_(i + GenomeLenght_ + 1) = val;
};
//for (int i = 0; i < GenomeLenght_ + 1; i++) {
// f_[i] *= (target_surplus / surplus);
// g_[i] *= (target_surplus / surplus);
//};
//fitness_ *= (target_surplus / surplus);
}
void RobotPlugin::position_subscriber_callback(const gps_agent_pkg::PositionCommand::ConstPtr& msg){
ROS_INFO_STREAM("received position command");
OptionsMap params;
int8_t arm = msg->arm;
params["mode"] = msg->mode;
Eigen::VectorXd data;
data.resize(msg->data.size());
for(int i=0; i<data.size(); i++){
data[i] = msg->data[i];
}
params["data"] = data;
Eigen::MatrixXd pd_gains;
pd_gains.resize(msg->pd_gains.size() / 4, 4);
for(int i=0; i<pd_gains.rows(); i++){
for(int j=0; j<4; j++){
pd_gains(i, j) = msg->pd_gains[i * 4 + j];
}
}
params["pd_gains"] = pd_gains;
if(arm == gps::TRIAL_ARM){
active_arm_controller_->configure_controller(params);
}else if (arm == gps::AUXILIARY_ARM){
passive_arm_controller_->configure_controller(params);
}else{
ROS_ERROR("Unknown position controller arm type");
}
}
//==============================================================================
TEST(Lemke, Lemke_6D)
{
Eigen::MatrixXd A;
Eigen::VectorXd b;
Eigen::VectorXd* f;
int err;
f = new Eigen::VectorXd(6);
A.resize(6,6);
A <<
3.1360, -2.0370, 0.9723, 0.1096, -2.0370, 0.9723,
-2.0370, 3.7820, 0.8302, -0.0257, 2.4730, 0.0105,
0.9723, 0.8302, 5.1250, -2.2390, -1.9120, 3.4080,
0.1096, -0.0257, -2.2390, 3.1010, -0.0257, -2.2390,
-2.0370, 2.4730, -1.9120, -0.0257, 5.4870, -0.0242,
0.9723, 0.0105, 3.4080, -2.2390, -0.0242, 3.3860;
b.resize(6);
b <<
0.1649,
-0.0025,
-0.0904,
-0.0093,
-0.0000,
-0.0889;
err = dart::lcpsolver::Lemke(A,b,f);
EXPECT_EQ(err, 0);
EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
}
ColMat<double, 3> FaceUnwrapper::interpolateFlatFace(const TopoDS_Face& face)
{
if (this->uv_nodes.size() == 0)
throw(std::runtime_error("no uv-coordinates found, interpolating with nurbs is only possible if the Flattener was constructed with a nurbs."));
// extract xyz poles, knots, weights, degree
const Handle(Geom_Surface) &_surface = BRep_Tool::Surface(face);
const Handle(Geom_BSplineSurface) &_bspline = Handle(Geom_BSplineSurface)::DownCast(_surface);
#if OCC_VERSION_HEX < 0x070000
TColStd_Array1OfReal _uknots(1, _bspline->NbUPoles() + _bspline->UDegree() + 1);
TColStd_Array1OfReal _vknots(1, _bspline->NbVPoles() + _bspline->VDegree() + 1);
_bspline->UKnotSequence(_uknots);
_bspline->VKnotSequence(_vknots);
#else
const TColStd_Array1OfReal &_uknots = _bspline->UKnotSequence();
const TColStd_Array1OfReal &_vknots = _bspline->VKnotSequence();
#endif
Eigen::VectorXd weights;
weights.resize(_bspline->NbUPoles() * _bspline->NbVPoles());
long i = 0;
for (long u=1; u <= _bspline->NbUPoles(); u++)
{
for (long v=1; v <= _bspline->NbVPoles(); v++)
{
weights[i] = _bspline->Weight(u, v);
i++;
}
}
Eigen::VectorXd u_knots;
Eigen::VectorXd v_knots;
u_knots.resize(_uknots.Length());
v_knots.resize(_vknots.Length());
for (long u=1; u <= _uknots.Length(); u++)
{
u_knots[u - 1] = _uknots.Value(u);
}
for (long v=1; v <= _vknots.Length(); v++)
{
v_knots[v - 1] = _vknots.Value(v);
}
nu = nurbs::NurbsBase2D(u_knots, v_knots, weights, _bspline->UDegree(), _bspline->VDegree());
A = nu.getInfluenceMatrix(this->uv_nodes);
Eigen::LeastSquaresConjugateGradient<spMat > solver;
solver.compute(A);
ColMat<double, 2> ze_poles;
ColMat<double, 3> flat_poles;
ze_poles.resize(weights.rows(), 2);
flat_poles.resize(weights.rows(), 3);
flat_poles.setZero();
ze_poles = solver.solve(ze_nodes);
flat_poles.col(0) << ze_poles.col(0);
flat_poles.col(1) << ze_poles.col(1);
return flat_poles;
}
/**
* Propagate chain rule to calculate gradients starting from
* the specified variable. Resizes the input vector to be the
* correct size.
*
* The grad() function does not itself recover any memory. use
* <code>agrad::recover_memory()</code> or
* <code>agrad::recover_memory_nested()</code>, defined in ,
* defined in agrad/rev/var_stack.hpp, to recover memory.
*
* @param[in] v Value of function being differentiated
* @param[in] x Variables being differentiated with respect to
* @param[out] g Gradient, d/dx v, evaluated at x.
*/
void grad(var& v,
Eigen::Matrix<var,Eigen::Dynamic,1>& x,
Eigen::VectorXd& g) {
stan::agrad::grad(v.vi_);
g.resize(x.size());
for (int i = 0; i < x.size(); ++i)
g(i) = x(i).vi_->adj_;
}
void ALLModel::calculateWeights(Eigen::MatrixXd& dist, Eigen::VectorXd& w)
{
w.resize(dist.cols());
for (int i = 0; i < dist.cols(); i++)
{
w(i) = exp(-pow(dist(0, i), 2)/(2*pow(kw_, 2)));
}
}
void R<N>::logMap(const StateSpace::State* _in, Eigen::VectorXd& _tangent) const
{
if (static_cast<std::size_t>(_tangent.size()) != getDimension())
_tangent.resize(getDimension());
auto in = static_cast<const State*>(_in);
_tangent = getValue(in);
}
void load( Archive & ar,
Eigen::VectorXd & t,
const unsigned int file_version )
{
int n0;
ar >> BOOST_SERIALIZATION_NVP( n0 );
t.resize( n0 );
ar >> make_array( t.data(), t.size() );
}
void CodeAtlas::FuzzyDependBuilder::buildChildDepend( QMultiHash<QString, ChildPack>& childList ,
Eigen::SparseMatrix<double>& vtxEdgeMat,
Eigen::VectorXd& edgeWeightVec,
QList<FuzzyDependAttr::DependPair>& dependPair)
{
QStringList codeList;
QVector<ChildPack*> childPackPtr;
for (QMultiHash<QString, ChildPack>::Iterator pChild = childList.begin();
pChild != childList.end(); ++pChild)
{
codeList.push_back(pChild.value().m_code);
childPackPtr.push_back(&pChild.value());
}
std::vector<Triplet> tripletArray;
QVector<double> edgeWeightArray;
// add dependency edges between child nodes
int ithSrc = 0;
for (QMultiHash<QString, ChildPack>::Iterator pChild = childList.begin();
pChild != childList.end(); ++pChild, ++ithSrc)
{
// for each child, find number of occurrences of this child's name
ChildPack& srcChild = pChild.value();
const QString& srcName = pChild.key();
QVector<int> occurence;
WordExtractor::findOccurrence(srcName, codeList, occurence);
for (int ithTar = 0; ithTar < childPackPtr.size(); ++ithTar)
{
int nOccur = occurence[ithTar];
if (nOccur == 0 || ithTar == ithSrc)
continue;
ChildPack& tarChild = *childPackPtr[ithTar];
SymbolEdge::Ptr pEdge = SymbolTree::getOrAddEdge(srcChild.m_pNode, tarChild.m_pNode, SymbolEdge::EDGE_FUZZY_DEPEND);
pEdge->clear();
pEdge->strength() = nOccur;
int nEdge = tripletArray.size()/2;
tripletArray.push_back(Triplet(srcChild.m_index, nEdge ,1.0));
tripletArray.push_back(Triplet(tarChild.m_index, nEdge ,-1.0));
edgeWeightArray.push_back(nOccur);
dependPair.push_back(FuzzyDependAttr::DependPair(srcChild.m_pNode, tarChild.m_pNode, nOccur));
}
}
if (int nEdges = tripletArray.size()/2)
{
vtxEdgeMat.resize(childList.size(),nEdges);
vtxEdgeMat.setFromTriplets(tripletArray.begin(), tripletArray.end());
edgeWeightVec.resize(nEdges);
memcpy(edgeWeightVec.data(), edgeWeightArray.data(), sizeof(double)* nEdges);
}
}
ResPreconditionerFixture()
{
_data.resize(8);
_data << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0;
_res.resize(8);
_res << 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 10.0, 20.0;
_compareDataRes.resize(8);
_compareDataRes << 7.07106781186547372897e+00,
1.41421356237309474579e+01,
1.50000000000000000000e+03,
2.00000000000000000000e+03,
2.50000000000000000000e+03,
3.00000000000000000000e+03,
3.13049516849970566046e-01,
3.57770876399966353265e-01;
_compareDataResSum.resize(8);
_compareDataResSum << 7.90585229434499154877e+01,
1.58117045886899830975e+02,
1.67708453051717078779e+04,
2.23611270735622783832e+04,
2.79514088419528488885e+04,
3.35416906103434157558e+04,
3.50007001329973377324e+00,
4.00008001519969536020e+00;
_compareDataResSum2.resize(8);
_compareDataResSum2 << 1.58113093108981217938e+02,
3.16226186217962435876e+02,
4.74339279326943596971e+02,
4.00008000319945455914e+00,
5.00010000399932064141e+00,
6.00012000479918228280e+00,
7.00014000559904481236e+00,
8.00016000639890734192e+00;
_compareDataValue.resize(8);
_compareDataValue << 4.47213595499957927704e-01,
8.94427190999915855407e-01,
3.23498319610315276940e-01,
4.31331092813753647075e-01,
5.39163866017192239255e-01,
6.46996639220630553879e-01,
6.58504607868518165859e-01,
7.52576694706877713514e-01;
_compareDataConstant.resize(8);
_compareDataConstant << 1.00000000000000002082e-03,
2.00000000000000004163e-03,
1.49999999999999977796e+00,
1.99999999999999955591e+00,
2.50000000000000044409e+00,
2.99999999999999955591e+00,
6.99999999999999883585e+05,
7.99999999999999650754e+05;
}
void testEigen(int m, int n, int nnz, std::vector<int>& rows, std::vector<int>& cols,
std::vector<double>& values, double* matB){
double start, stop, time_to_solve, time_to_build;
double tol=1e-9;
Eigen::SparseMatrix<double> A;
std::vector< Eigen::Triplet<double> > trips;
trips.reserve(m * n);
for (int k = 0; k < nnz; k++){
double _val = values[k];
int i = rows[k];
int j = cols[k];
if (fabs(_val) > tol){
trips.push_back(Eigen::Triplet<double>(i-1,j-1,_val));
}
}
//NOTE: setFromTriples() accumulates contributions to the same (i,j)!
A.resize(m, n);
start = second();
A.setFromTriplets(trips.begin(), trips.end());
stop = second();
time_to_build = stop - start;
Eigen::SparseLU< Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int> > solverLU;
Eigen::VectorXd b; b.resize(m);
for (int i = 0; i < m; i++ ) b(i) = matB[i];
printf("\nProcessing in Eigen using LU...\n");
start = second();
solverLU.compute(A);
Eigen::VectorXd X = solverLU.solve(b);
stop = second();
time_to_solve = stop - start;
Eigen::VectorXd ax = A * X;
Eigen::VectorXd bMinusAx = b - ax;
double h_r[m];
for (int i=0; i<m; i++) h_r[i]=bMinusAx(i);
double r_inf = vec_norminf(m, h_r);
printf("(Eigen) |b - A*x| = %E \n", r_inf);
printf("(Eigen) Time to build(sec): %f\n", time_to_build);
printf("(Eigen) Time (sec): %f\n", time_to_solve);
}
void transfer_spheres(const std::vector<Sphere> & spheres,
Eigen::Matrix3Xd & sphereCenter,
Eigen::VectorXd & sphereRadius) {
size_t nSpheres = spheres.size();
sphereCenter.resize(Eigen::NoChange, nSpheres);
sphereRadius.resize(nSpheres);
for (size_t i = 0; i < nSpheres; ++i) {
sphereCenter.col(i) = spheres[i].center;
sphereRadius(i) = spheres[i].radius;
}
}
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();
}
