void MUMPSLU::allocate_coordinate_format_storage(sparse_matrix const& A)
{
rows.clear();
cols.clear();
coefficients.clear();
rows.reserve(A.nonZeros());
cols.reserve(A.nonZeros());
coefficients.reserve(A.nonZeros());
for (auto k = 0; k < A.outerSize(); ++k)
{
for (sparse_matrix::InnerIterator it(A, k); it; ++it)
{
coefficients.emplace_back(it.value());
rows.emplace_back(it.row() + 1);
cols.emplace_back(it.col() + 1);
}
}
}
void MUMPSLLT::allocate_coordinate_format_storage(sparse_matrix const& A)
{
coefficients.clear();
coefficients.reserve(A.nonZeros());
if (build_sparsity_pattern)
{
rows.reserve(A.nonZeros());
cols.reserve(A.nonZeros());
// Decompress the upper part of the sparse matrix
for (auto k = 0; k < A.outerSize(); ++k)
{
for (sparse_matrix::InnerIterator it(A, k); it; ++it)
{
if (it.col() >= it.row())
{
coefficients.emplace_back(it.value());
rows.emplace_back(it.row() + 1);
cols.emplace_back(it.col() + 1);
}
}
}
rows.shrink_to_fit();
cols.shrink_to_fit();
}
else
{
// Only update the non-zero numerical values
for (auto k = 0; k < A.outerSize(); ++k)
{
for (sparse_matrix::InnerIterator it(A, k); it; ++it)
{
if (it.col() >= it.row())
{
coefficients.emplace_back(it.value());
}
}
}
}
}
void MUMPS::internal_solve(sparse_matrix const& A, vector& x, vector const& b)
{
auto start = std::chrono::high_resolution_clock::now();
x = b;
info.n = A.rows();
info.nz = coefficients.size();
info.a = coefficients.data();
info.irn = rows.data();
info.jcn = cols.data();
if (build_sparsity_pattern)
{
// Analysis phase
info.job = Job::Analysis;
MUMPSAdapter::mumps_c(info);
if (info.info[0] < 0)
{
throw computational_error("Error in analysis phase of MUMPS solver\n");
}
build_sparsity_pattern = false;
}
// Factorization phase
info.job = Job::Factorisation;
MUMPSAdapter::mumps_c(info);
if (info.info[0] < 0)
{
throw computational_error("Error in factorisation phase of MUMPS solver\n");
}
info.rhs = x.data();
info.nrhs = 1;
info.lrhs = info.n;
info.job = Job::BackSubstitution;
MUMPSAdapter::mumps_c(info);
if (info.info[0] < 0)
{
throw computational_error("Error in back substitution phase of MUMPS solver\n");
}
auto end = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> elapsed_seconds = end - start;
std::cout << std::string(6, ' ') << "MUMPS solver took " << elapsed_seconds.count() << "s\n";
}
sparse_bcsr(sparse_matrix<T,dataType> &sp) : sparse_bcsr(sp.get_triplets()) {}
void constraintCalculator::sparseConstraintJacobian(int sr, sparse_matrix<double>& sjc, std::map<std::string, pointmass*> &masses, std::map<std::string, kinematicConstraint*> &kconsts, std::map<std::string, drivingConstraint*> &dconsts /* = 0 */)
{
int sRow = 0;
int iCol = 0;
int jCol = 0;
int ib=0;
int jb=0;
int i = 0;
vector3<double> dij;
vector4<double> ep;
std::map<std::string, kinematicConstraint*>::iterator kit;// = kconsts.begin();
sjc.zeroCount();
sRow = sjc.rows() - masses.size() + 1 + sr;
std::map<std::string, pointmass*>::iterator mit;
for(mit = masses.begin(); mit != masses.end(); mit++){
pointmass* mass = mit->second;
if(!mass->ID()) continue;
euler_parameter<double> ep2 = 2 * mass->Orientation();
sjc.extraction(sRow++, i * 7 + 3, ep2.Pointer(), 4);
i++;
}
for(kit = kconsts.begin(); kit != kconsts.end(); kit++){
kinematicConstraint* kconst = kit->second;
sRow = kconst->sRow + sr;
ib = kconst->i; iCol = kconst->iCol;
jb = kconst->j; jCol = kconst->jCol;
// switch(kconst->type)
// {
// case kinematicConstraint::REVOLUTE:
// if(ib)
// {
// for(unsigned i(0); i < 3; i++) sjc(sRow+i,iCol+i) = -1;
// ep = m->getParameterv(ib);
// sjc.extraction(sRow+0, iCol+3, POINTER(B(ep, -kconst->sp_i)), MAT3X4);
// sjc.extraction(sRow+3, iCol+3, POINTER4(transpose(m->toGlobal(jb, kc->h_j), B(ep, kc->g_i))), VEC4);
// sjc.extraction(sRow+4, iCol+3, POINTER4(transpose(m->toGlobal(jb, kc->h_j), B(ep, kc->f_i))), VEC4);
// }
// if(jb)
// {
// for(unsigned i(0); i < 3; i++) sjc(sRow+i,jCol+i) = 1;
// ep = m->getParameterv(jb);
// sjc.extraction(sRow+0, jCol+3, POINTER(B(ep, kc->sp_j)), MAT3X4);
// sjc.extraction(sRow+3, jCol+3, POINTER4(transpose(m->toGlobal(ib, kc->g_i), B(ep, kc->h_j))), VEC4);
// sjc.extraction(sRow+4, jCol+3, POINTER4(transpose(m->toGlobal(ib, kc->f_i), B(ep, kc->h_j))), VEC4);
// }
// //std::cout << *sjc << std::endl;
// break;
// case kinematicConstraint::SPHERICAL:
// if(ib)
// {
// ep = m->getParameterv(ib);
// for(unsigned i(0); i < 3; i++) sjc(sRow+i,iCol+i) = -1;
// sjc.extraction(sRow+0, iCol+3, POINTER(B(ep, -kc->sp_i)), MAT3X4);
// }
// if(jb)
// {
// ep = m->getParameterv(jb);
// for(unsigned i(0); i < 3; i++) sjc(sRow+i,jCol+i) = 1;
// sjc.extraction(sRow+0, jCol+3, POINTER(B(ep, kc->sp_j)), MAT3X4);
// }
// break;
// case kinematicConstraint::TRANSLATION:
// dij = m->getPositionv(jb) + m->toGlobal(jb, kc->sp_j) - m->getPositionv(ib) - m->toGlobal(ib, kc->sp_i);
// if(ib)
// {
// ep = m->getParameterv(ib);
// sjc.extraction(sRow+0, iCol+3, POINTER4(transpose(m->toGlobal(jb, kc->h_j), B(ep, kc->f_i))), VEC4);
// sjc.extraction(sRow+1, iCol+3, POINTER4(transpose(m->toGlobal(jb, kc->h_j), B(ep, kc->g_i))), VEC4);
// sjc.extraction(sRow+2, iCol+0, POINTER3((-kc->f_i)), POINTER4(transpose(dij + kc->sp_i, B(ep, kc->f_i))), VEC3_4);
// sjc.extraction(sRow+3, iCol+0, POINTER3((-kc->g_i)), POINTER4(transpose(dij + kc->sp_i, B(ep, kc->g_i))), VEC3_4);
// sjc.extraction(sRow+4, iCol+3, POINTER4(transpose(m->toGlobal(jb, kc->g_j), B(ep, kc->g_i))), VEC4);
// }
// if(jb)
// {
// ep = m->getParameterv(jb);
// sjc.extraction(sRow+0, jCol+3, POINTER4(transpose(m->toGlobal(ib, kc->f_i), B(ep, kc->h_j))), VEC4);
// sjc.extraction(sRow+1, jCol+3, POINTER4(transpose(m->toGlobal(ib, kc->g_i), B(ep, kc->h_j))), VEC4);
// sjc.extraction(sRow+2, jCol+0, POINTER3(kc->f_i), POINTER4(transpose(kc->f_i, B(ep, kc->sp_j))), VEC3_4);
// sjc.extraction(sRow+3, jCol+0, POINTER3(kc->g_i), POINTER4(transpose(kc->g_i, B(ep, kc->sp_j))), VEC3_4);
// sjc.extraction(sRow+4, jCol+3, POINTER4(transpose(m->toGlobal(ib, kc->g_i), B(ep, kc->g_j))), VEC4);
// }
// //std::cout << *sjc << std::endl;
// break;
// case kinematicConstraint::CYLINDERICAL:
// //vector3d yy = jb->Getpos() + jb->A()*joint->sp_j;
// dij = m->getPositionv(jb) + m->toGlobal(jb, kc->sp_j) - m->getPositionv(ib) - m->toGlobal(ib, kc->sp_i);
// if(ib)
// {
// ep = m->getParameterv(ib);
// sjc.extraction(sRow+0, iCol+3, POINTER4(transpose(m->toGlobal(jb, kc->h_j), B(ep, kc->g_i))), VEC4);
// sjc.extraction(sRow+1, iCol+3, POINTER4(transpose(m->toGlobal(jb, kc->h_j), B(ep, kc->f_i))), VEC4);
// sjc.extraction(sRow+2, iCol+0, POINTER3((-m->toGlobal(ib, kc->g_i))), POINTER4(transpose(dij + m->toGlobal(ib, kc->sp_i), B(ep, kc->g_i))), VEC3_4);
// sjc.extraction(sRow+3, iCol+0, POINTER3((-m->toGlobal(ib, kc->f_i))), POINTER4(transpose(dij + m->toGlobal(ib, kc->sp_i), B(ep, kc->f_i))), VEC3_4);
//
// }
// if(jb)
// {
// ep = m->getParameterv(jb);
// vector3d gfi = m->toGlobal(ib, kc->f_i);
// vector3d ggi = m->toGlobal(ib, kc->g_i);
// sjc.extraction(sRow+0, jCol+3, POINTER4(transpose(ggi, B(ep, kc->h_j))), VEC4);
// sjc.extraction(sRow+1, jCol+3, POINTER4(transpose(gfi, B(ep, kc->h_j))), VEC4);
// sjc.extraction(sRow+2, jCol+0, POINTER3(ggi), POINTER4(transpose(ggi, B(ep, kc->sp_j))), VEC3_4);
// sjc.extraction(sRow+3, jCol+0, POINTER3(gfi), POINTER4(transpose(gfi, B(ep, kc->sp_j))), VEC3_4);
//
// }
// break;
// }
// }
//
// while(kc != kconsts->end()){
// sRow = kc->sRow + sr;
// ib = kc->i; iCol = kc->iCol;
// jb = kc->j; jCol = kc->jCol;
// switch(kc->type)
// {
// case kinematicConstraint::REVOLUTE:
// if(ib)
// {
// for(unsigned i(0); i < 3; i++) sjc(sRow+i,iCol+i) = -1;
// ep = m->getParameterv(ib);
// sjc.extraction(sRow+0, iCol+3, POINTER(B(ep, -kc->sp_i)), MAT3X4);
// sjc.extraction(sRow+3, iCol+3, POINTER4(transpose(m->toGlobal(jb, kc->h_j), B(ep, kc->g_i))), VEC4);
// sjc.extraction(sRow+4, iCol+3, POINTER4(transpose(m->toGlobal(jb, kc->h_j), B(ep, kc->f_i))), VEC4);
// }
// if(jb)
// {
// for(unsigned i(0); i < 3; i++) sjc(sRow+i,jCol+i) = 1;
// ep = m->getParameterv(jb);
// sjc.extraction(sRow+0, jCol+3, POINTER(B(ep, kc->sp_j)), MAT3X4);
// sjc.extraction(sRow+3, jCol+3, POINTER4(transpose(m->toGlobal(ib, kc->g_i), B(ep, kc->h_j))), VEC4);
// sjc.extraction(sRow+4, jCol+3, POINTER4(transpose(m->toGlobal(ib, kc->f_i), B(ep, kc->h_j))), VEC4);
// }
// //std::cout << *sjc << std::endl;
// break;
// case kinematicConstraint::SPHERICAL:
// if(ib)
// {
// ep = m->getParameterv(ib);
// for(unsigned i(0); i < 3; i++) sjc(sRow+i,iCol+i) = -1;
// sjc.extraction(sRow+0, iCol+3, POINTER(B(ep, -kc->sp_i)), MAT3X4);
// }
// if(jb)
// {
// ep = m->getParameterv(jb);
// for(unsigned i(0); i < 3; i++) sjc(sRow+i,jCol+i) = 1;
// sjc.extraction(sRow+0, jCol+3, POINTER(B(ep, kc->sp_j)), MAT3X4);
// }
// break;
// case kinematicConstraint::TRANSLATION:
// dij = m->getPositionv(jb) + m->toGlobal(jb, kc->sp_j) - m->getPositionv(ib) - m->toGlobal(ib, kc->sp_i);
// if(ib)
// {
// ep = m->getParameterv(ib);
// sjc.extraction(sRow+0, iCol+3, POINTER4(transpose(m->toGlobal(jb, kc->h_j), B(ep, kc->f_i))), VEC4);
// sjc.extraction(sRow+1, iCol+3, POINTER4(transpose(m->toGlobal(jb, kc->h_j), B(ep, kc->g_i))), VEC4);
// sjc.extraction(sRow+2, iCol+0, POINTER3((-kc->f_i)), POINTER4(transpose(dij + kc->sp_i, B(ep, kc->f_i))), VEC3_4);
// sjc.extraction(sRow+3, iCol+0, POINTER3((-kc->g_i)), POINTER4(transpose(dij + kc->sp_i, B(ep, kc->g_i))), VEC3_4);
// sjc.extraction(sRow+4, iCol+3, POINTER4(transpose(m->toGlobal(jb, kc->g_j), B(ep, kc->g_i))), VEC4);
// }
// if(jb)
// {
// ep = m->getParameterv(jb);
// sjc.extraction(sRow+0, jCol+3, POINTER4(transpose(m->toGlobal(ib, kc->f_i), B(ep, kc->h_j))), VEC4);
// sjc.extraction(sRow+1, jCol+3, POINTER4(transpose(m->toGlobal(ib, kc->g_i), B(ep, kc->h_j))), VEC4);
// sjc.extraction(sRow+2, jCol+0, POINTER3(kc->f_i), POINTER4(transpose(kc->f_i, B(ep, kc->sp_j))), VEC3_4);
// sjc.extraction(sRow+3, jCol+0, POINTER3(kc->g_i), POINTER4(transpose(kc->g_i, B(ep, kc->sp_j))), VEC3_4);
// sjc.extraction(sRow+4, jCol+3, POINTER4(transpose(m->toGlobal(ib, kc->g_i), B(ep, kc->g_j))), VEC4);
// }
// //std::cout << *sjc << std::endl;
// break;
// case kinematicConstraint::CYLINDERICAL:
// //vector3d yy = jb->Getpos() + jb->A()*joint->sp_j;
// dij = m->getPositionv(jb) + m->toGlobal(jb, kc->sp_j) - m->getPositionv(ib) - m->toGlobal(ib, kc->sp_i);
// if(ib)
// {
// ep = m->getParameterv(ib);
// sjc.extraction(sRow+0, iCol+3, POINTER4(transpose(m->toGlobal(jb, kc->h_j), B(ep, kc->g_i))), VEC4);
// sjc.extraction(sRow+1, iCol+3, POINTER4(transpose(m->toGlobal(jb, kc->h_j), B(ep, kc->f_i))), VEC4);
// sjc.extraction(sRow+2, iCol+0, POINTER3((-m->toGlobal(ib, kc->g_i))), POINTER4(transpose(dij + m->toGlobal(ib, kc->sp_i), B(ep, kc->g_i))), VEC3_4);
// sjc.extraction(sRow+3, iCol+0, POINTER3((-m->toGlobal(ib, kc->f_i))), POINTER4(transpose(dij + m->toGlobal(ib, kc->sp_i), B(ep, kc->f_i))), VEC3_4);
//
// }
// if(jb)
// {
// ep = m->getParameterv(jb);
// vector3d gfi = m->toGlobal(ib, kc->f_i);
// vector3d ggi = m->toGlobal(ib, kc->g_i);
// sjc.extraction(sRow+0, jCol+3, POINTER4(transpose(ggi, B(ep, kc->h_j))), VEC4);
// sjc.extraction(sRow+1, jCol+3, POINTER4(transpose(gfi, B(ep, kc->h_j))), VEC4);
// sjc.extraction(sRow+2, jCol+0, POINTER3(ggi), POINTER4(transpose(ggi, B(ep, kc->sp_j))), VEC3_4);
// sjc.extraction(sRow+3, jCol+0, POINTER3(gfi), POINTER4(transpose(gfi, B(ep, kc->sp_j))), VEC3_4);
//
// }
// break;
// }
// kc++;
}
//std::cout << sjc;
// for(std::vector<drivingType>::iterator driving = drivings.begin(); driving != drivings.end(); driving++)
// {
// sCol = (driving->targetBody - 1) * 7;
// switch(driving->targetCoord)
// {
// case 'z':
// sjc->extraction(sRow, sCol+3, POINTER(vector4d(0.0,0.0,0.0,1.0)), VEC4);
// sRow+=1;
// break;
// }
//
// }
}
