SHAPEOP_INLINE bool Solver::initialize(bool dynamic, Scalar masses, Scalar damping, Scalar timestep) {
int n_points = static_cast<int>(points_.cols());
int n_constraints = static_cast<int>(constraints_.size());
assert(n_points != 0);
assert(n_constraints != 0);
std::vector<Triplet> triplets;
int idO = 0;
for (int i = 0; i < n_constraints; ++i) constraints_[i]->addConstraint(triplets, idO);
projections_.setZero(3, idO);
SparseMatrix A = SparseMatrix(idO, n_points);
A.setFromTriplets(triplets.begin(), triplets.end());
At_ = A.transpose();
oldPoints_ = Matrix3X(3, n_points);
//Dynamic
velocities_ = Matrix3X::Zero(3, n_points);
momentum_ = Matrix3X::Zero(3, n_points);
M_ = SparseMatrix(n_points, n_points);
masses_ = masses;
damping_ = damping;
delta_ = timestep;
dynamic_ = dynamic;
if (dynamic_) {
M_.setIdentity();
M_ *= masses_; //TODO: fix this
}
solver_ = std::make_shared<ShapeOp::SimplicialLDLTSolver>();
solver_->initialize(At_ * A + M_);
return true; //TODO: fix this
}
/*
@method CreateSparse
@discussion Create a SparseMatrix, based
on the XML attributes. The object is returned by value.
@throws KII_invalid_argument
@param matElement pointer to Matrix XML element
@result The SparseMatrix object.
*/
SparseMatrix MatrixFactory::CreateSparse(TiXmlElement* matElement)
{
string type;
string init;
int rows, columns;
FLOAT multiplier;
TiXmlHandle matHandle(matElement);
GetAttributes(matElement, type, init, rows, columns, multiplier);
#ifdef MDEBUG
cerr << "Creating SparseMatrix with attributes: " << type << ", " << init
<< ", " << rows << "X" << columns << ", " << multiplier << endl;
#endif
if (type == "diag") {
if (init == "none") { // a string of values is present & passed
TiXmlText* valuesNode = matHandle.FirstChild().Text();
if (valuesNode == NULL)
throw KII_invalid_argument("Contents not specified for Sparese Matrix with init='none'.");
const char* values = valuesNode->Value();
#ifdef MDEBUG
cerr << "\tData present for initialization: " << values << endl;
#endif
return SparseMatrix(rows, columns, multiplier, values);
} else if (init == "const") { // No string of values or XML row data
if (multiplier == 0.0)
return SparseMatrix(rows, columns);
else
throw KII_invalid_argument("A sparse matrix can only be initialized to zero with const XML init");
} else
throw KII_invalid_argument("Invalid init for sparse matrix");
} else if (type == "sparse") {
if (init == "none") // a sequence of row data nodes is present & passed
return SparseMatrix(rows, columns, multiplier, matElement);
else if (init == "const") { // No row data
if (multiplier == 0.0)
return SparseMatrix(rows, columns);
else
throw KII_invalid_argument("A sparse matrix can only be initialized to zero with const XML init");
} else
throw KII_invalid_argument("A sparse matrix can only be initialized to zero with const XML init");
}
// If we get here, then something is really wrong
throw KII_invalid_argument("Invalid type specified for sparse matrix.");
}
void SparseMatrix::consolidate(){
SparseMatrix temp = SparseMatrix(*this);
this->set_cols(temp.ncols);
this->set_rows(temp.nrows);
for(int i = 0; i < nrows; i++){
for(int j = 0; j < ncols; j++){
if(temp(i,j) != 0) this->set_element_at(i,j,temp(i,j));
}
}
}
void SparseMatrix::transpose(){
SparseMatrix temp = SparseMatrix(*this);
this->set_cols(temp.nrows);
this->set_rows(temp.ncols);
for(int i = 0; i < nrows; i++){
for(int j = 0; j < ncols; j++){
this->set_element_at(i,j,temp(j,i));
}
}
}
// Returns the transpose of this matrix
SparseMatrix SparseMatrix::getTranspose() const {
SparseArray m;
SparseColArrayConstIterator j;
SparseArrayConstIterator i;
for(i = m_elements.begin(); i != m_elements.end(); ++i)
for(j = i->second.begin(); j != i->second.end(); ++j)
m[j->first][i->first] = j->second;
// note m and n are switched
return SparseMatrix(m_n, m_m, m);
}
// ECoG positions are reported line by line in the positions Matrix
// mat is supposed to be filled with zeros
// mat is the linear application which maps x (the unknown vector in symmetric system) -> v (potential at the ECoG electrodes)
// difference with Head2EEG is that it interpolates the inner skull layer instead of the scalp layer.
void assemble_Head2ECoG(SparseMatrix& mat, const Geometry& geo, const Matrix& positions, const Interface& i)
{
mat = SparseMatrix(positions.nlin(), (geo.size()-geo.outermost_interface().nb_triangles()));
Vect3 current_position;
Vect3 current_alphas;
Triangle current_triangle;
for ( unsigned it = 0; it < positions.nlin(); ++it) {
for ( unsigned k = 0; k < 3; ++k) {
current_position(k) = positions(it, k);
}
dist_point_interface(current_position, i, current_alphas, current_triangle);
mat(it, current_triangle.s1().index()) = current_alphas(0);
mat(it, current_triangle.s2().index()) = current_alphas(1);
mat(it, current_triangle.s3().index()) = current_alphas(2);
}
}
SparseMatrix SimpleFluid::generateMatrix(){
std::vector<Element> elements;
int cells = std::pow(dim, 2);
//All diagonal entries of the matrix are 4
for (int i = 0; i < cells; ++i){
elements.push_back(Element(i, i, 4));
//Set the cell-cell interaction values for the cells up/down/left/right of our cell
//note that this simulation is currently using wrapping boundaries so we just wrap edges
//in cellNumber
int x, y;
cellPos(i, x, y);
elements.push_back(Element(i, cellNumber(x - 1, y), -1));
elements.push_back(Element(i, cellNumber(x + 1, y), -1));
elements.push_back(Element(i, cellNumber(x, y - 1), -1));
elements.push_back(Element(i, cellNumber(x, y + 1), -1));
}
return SparseMatrix(elements, dim, true);
}
double kantorovich (const T &densityT,
const Functions &densityF,
const Matrix &X,
const Vector &weights,
Vector &g,
SparseMatrix &h)
{
typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Polygon_2<K> Polygon;
typedef K::FT FT;
typedef CGAL::Regular_triangulation_filtered_traits_2<K> RT_Traits;
typedef CGAL::Regular_triangulation_vertex_base_2<RT_Traits> Vbase;
typedef CGAL::Triangulation_vertex_base_with_info_2
<size_t, RT_Traits, Vbase> Vb;
typedef CGAL::Regular_triangulation_face_base_2<RT_Traits> Cb;
typedef CGAL::Triangulation_data_structure_2<Vb,Cb> Tds;
typedef CGAL::Regular_triangulation_2<RT_Traits, Tds> RT;
typedef RT::Vertex_handle Vertex_handle_RT;
typedef RT::Weighted_point Weighted_point;
typedef typename CGAL::Point_2<K> Point;
size_t N = X.rows();
assert(weights.rows() == N);
assert(weights.cols() == 1);
assert(X.cols() == 2);
// insert points with indices in the regular triangulation
std::vector<std::pair<Weighted_point,size_t> > Xw(N);
for (size_t i = 0; i < N; ++i)
{
Xw[i] = std::make_pair(Weighted_point(Point(X(i,0), X(i,1)),
weights(i)), i);
}
RT dt (Xw.begin(), Xw.end());
dt.infinite_vertex()->info() = -1;
// compute the quadratic part
typedef MA::Voronoi_intersection_traits<K> Traits;
typedef typename MA::Tri_intersector<T,RT,Traits> Tri_isector;
typedef typename Tri_isector::Pgon Pgon;
typedef Eigen::Triplet<FT> Triplet;
std::vector<Triplet> htri;
FT total(0), fval(0), total_area(0);
g = Vector::Zero(N);
MA::voronoi_triangulation_intersection_raw
(densityT,dt,
[&] (const Pgon &pgon,
typename T::Face_handle f,
Vertex_handle_RT v)
{
Tri_isector isector;
Polygon p;
std::vector<Vertex_handle_RT> adj;
for (size_t i = 0; i < pgon.size(); ++i)
{
size_t ii = (i==0)?(pgon.size()-1):(i-1);
//size_t ii = (i+1)%pgon.size();
p.push_back(isector.vertex_to_point(pgon[i], pgon[ii]));
adj.push_back((pgon[i].type == Tri_isector::EDGE_DT) ?
pgon[i].edge_dt.second : 0);
}
size_t idv = v->info();
auto fit = densityF.find(f);
assert(fit != densityF.end());
auto fv = fit->second; // function to integrate
// compute hessian
for (size_t i = 0; i < p.size(); ++i)
{
if (adj[i] == 0)
continue;
Vertex_handle_RT w = adj[i];
size_t idw = w->info();
FT r = MA::integrate_1<FT>(p.edge(i), fv);
FT d = 2*sqrt(CGAL::squared_distance(v->point(),
w->point()));
htri.push_back(Triplet(idv, idw, -r/d));
htri.push_back(Triplet(idv, idv, +r/d));
}
// compute value and gradient
FT warea = MA::integrate_1<FT>(p, FT(0), fv);
FT intg = MA::integrate_3<FT>(p, FT(0), [&](Point p)
{
return fv(p) *
CGAL::squared_distance(p,
v->point());
});
fval = fval + warea * weights[idv] - intg;
g[idv] = g[idv] + warea;
total += warea;
total_area += p.area();
});
h = SparseMatrix(N,N);
h.setFromTriplets(htri.begin(), htri.end());
h.makeCompressed();
return fval;
}
void VDBLinearFEMSolverModule<LatticeType>::updateStencil() {
if(!m_obj) {
std::cout << "No object set... shouldn't be updating stencil" << std::endl;
return;
}
const LatticeType& vL = m_obj->getDOFs();
m_vertex_neighbors.resize(3*vL.activeCount());
std::vector<int>::iterator begin=m_vertex_neighbors.begin();
std::vector<int>::iterator end=m_vertex_neighbors.end();
std::fill(begin,end,0);
ActiveLatticeIterator a = m_obj->activeVertexIterator();
int totalReserve = 0;
for(ActiveLatticeIterator active = a; active; ++active) {
for(int32_t m=-1; m < 2; ++m) {
for(int32_t n=-1; n < 2; ++n) {
for(int32_t l=-1; l < 2; ++l) {
//if((m & n & l) == 0)
SIndex3 tmp = active.index() + SIndex3(m,n,l);
//if(!m_obj->validVertexIndex3(tmp)) continue;
if(m_obj->getVertex(tmp.i,tmp.j,tmp.k) != -1) {
m_vertex_neighbors[3*active.value()] += 1;
m_vertex_neighbors[3*active.value()+1] += 1;
m_vertex_neighbors[3*active.value()+2] += 1;
totalReserve+=3;
}
}
}
}
}
int numDOFs = 3*vL.activeCount();
int32_t NI=vL.NI();
int32_t NJ=vL.NJ();
int32_t NK=vL.NK();
std::cout << "Allocating sparse matrix of size: " << numDOFs << std::endl;
M = SparseMatrix(numDOFs,numDOFs);
rhs = Vector::Zero(numDOFs);
//Pre-reserve the places where we will have fillin
M.reserve(m_vertex_neighbors);
typedef Eigen::Triplet<Scalar> Triplet;
typedef std::vector<Triplet> Triplets;
Triplets triplets;
triplets.reserve(totalReserve);
std::vector<bool> used_constraints(numDOFs);
//rms::ScalarGrid::ConstAccessor accessor = GetDistanceGrid()->getConstAccessor();
rms::VectorGridPtr velocityFieldPtr = m_obj->getVectorField();
rms::ScalarGridPtr rigidFieldPtr = m_obj->getRigidField();
rms::ScalarGridPtr heatFieldPtr = m_obj->getHeatField();
rms::RGBAGridPtr materialFieldPtr = m_obj->getMaterialField();
rms::ScalarGrid::ConstAccessor heatAccessor = heatFieldPtr->getConstAccessor();
rms::ScalarGrid::ConstAccessor rigidAccessor = rigidFieldPtr->getConstAccessor();
rms::RGBAGrid::ConstAccessor materialAccessor = materialFieldPtr->getConstAccessor();
rms::VectorGrid::ConstAccessor velocityAccessor = velocityFieldPtr->getConstAccessor();
int count=0;
std::cout << "About to look at heat field" << std::endl;
Scalar scale = heatFieldPtr->transform().voxelVolume();
std::set<int32_t> used_vertex;
StiffnessEntryStorage store(scale,integrator);//TODO: need to fix this
std::cout << "Mincoord: " << m_minCoord << std::endl;
vdb::CoordBBox bbox = m_obj->getDistanceField()->evalActiveVoxelBoundingBox();
std::cout << bbox.min() << " " << bbox.max() << std::endl;
m_minCoord = bbox.min();
for(rms::ScalarGrid::ValueOnCIter it = m_obj->getDistanceField()->cbeginValueOn(); it; ++it) {
if(it.getValue() >= 0) continue;
/*
if(count++ % 1000 == 0) {
std::cout << count << "/" << numDOFs << std::endl;
}
*/
std::cout << "Working in voxel: " << it.getCoord() << std::endl;
Index3 idx = VDBtoLatticeCoord(it.getCoord());
//std::cout << "True index: " << idx << " " << it.getCoord() << std::endl;
/*
if(m_obj->getVertex(idx.i,idx.j,idx.k) == -1) {
std::cout << "bottomleftinner lattice index doesn't exist, clearly lattice just isn't populated"<< std::endl;
}
*/
const vdb::Coord vdbcoord = m_minCoord + vdb::Coord(idx.i,idx.j,idx.k);
vdb::Vec4f material = materialAccessor.getValue(vdbcoord);
vdb::Vec3f velocity = velocityAccessor.getValue(vdbcoord);
float lambda = material(0);
float mu = material(1);
float density = material(3) * 0.125;//Divide because each basis function only takes on 1/8th of a whole cube
for(VDBVoxelNodeIterator<LatticeType> alpha(m_obj->getDOFs(),idx); alpha; ++alpha) {
const Index3 & alpha_index = alpha.index();
const vdb::Coord alpha_vdbcoord = m_minCoord + vdb::Coord(alpha_index.i,alpha_index.j,alpha_index.k);
const int32_t alpha_value = alpha.value();
const int32_t alphaind = mat_ind(alpha_value,0);
//const LinearElasticVertexProperties & alpha_props = m_vertex_properties[alpha_value];
//alpha sets the matrix, betas set the rhs values for alpha
std::cout << alpha_vdbcoord << ": ";
if(rigidAccessor.getValue(alpha_vdbcoord) < 0) {
std::cout << "Rigid!" << std::endl;
if(used_vertex.find(alpha_value) == used_vertex.end()) {
used_vertex.insert(alpha_value);
const int32_t ind = alphaind;
triplets.push_back(Triplet(ind,ind,1));
triplets.push_back(Triplet(ind+1,ind+1,1));
triplets.push_back(Triplet(ind+2,ind+2,1));
std::cout << rhs(alphaind+0) << " ";
std::cout << rhs(alphaind+1) << " ";
std::cout << rhs(alphaind+2) << std::endl;
}
} else {
std::cout << "Elastic!" << std::endl;
rhs(alphaind+0) += velocity(0)*density;
rhs(alphaind+1) += velocity(1)*density;
rhs(alphaind+2) += velocity(2)*density;
//std::cout << velocity << std::endl;
for(VDBVoxelNodeIterator<LatticeType> beta(m_obj->getDOFs(),idx); beta; ++beta) {
const Index3 & beta_index = beta.index();
const int32_t beta_value = beta.value();
/*
if(alpha_value < 0 || beta_value < 0) {
std::cout << "WTF Shouldn't be finding vertices out of nowhere..." << alpha_value << " " << beta_value << ": " << it.getCoord()<< ":A" << alpha_index << ":B" << beta_index << std::endl;
std::cout << idx << " " << it.getCoord() << std::endl;
}*/
//const LinearElasticVertexProperties & beta_props = m_vertex_properties[beta_value];
//const Vector3 beta_externalForce = beta_props.externalForce;
const vdb::Coord beta_vdbcoord = m_minCoord + vdb::Coord(beta_index.i,beta_index.j,beta_index.k);
if(rigidAccessor.getValue(beta_vdbcoord) > 0) {
for(int32_t alphadim = 0; alphadim < 3; ++alphadim) {
const int32_t alpha_i = mat_ind(alpha_value,alphadim);
for(int32_t betadim = 0; betadim < 3; ++betadim) {
const int32_t beta_i = mat_ind(beta_value,betadim);
const Scalar v = store.genericTerm(alpha.pos(), beta.pos(), alphadim, betadim,lambda,mu);
triplets.push_back(Triplet(alpha_i,beta_i,
(
v
)
));
}
}
}
/*
const SIndex3 rel_alpha = alpha_index - idx;
const int32_t ind_alpha = rel_alpha.i * 4 + rel_alpha.j * 2 + rel_alpha.k;
const SIndex3 rel_beta = beta_index - idx;
const int32_t ind_beta = rel_beta.i * 4 + rel_beta.j * 2 + rel_beta.k;
*/
//const Scalar diag = integrator.i(boost::bind(&StiffnessEntryFunctor::diagonalTermi
const Scalar diag = store.diagonalTerm(alpha.pos(), beta.pos(), mu);
for(int32_t dim = 0; dim < 3; ++dim) {
const int32_t ai = mat_ind(alpha_value,dim);
const int32_t bi = mat_ind(beta_value,dim);
triplets.push_back(Triplet(ai,bi,
(
diag
)
));
}
}
}
}
}
//std::cout << rhs.transpose() << std::endl;
std::cout << "Number of matrix etdntries (with duplicates): " << triplets.size() << std::endl;
M.setFromTriplets(triplets.begin(),triplets.end());
M.prune(1,Eigen::NumTraits<Scalar>::dummy_precision());
//std::cout << M << std::endl;
}
void StableFluid3D::initPoisson()
{
clearPoisson();
int n = resX * resY * resZ;
for (int i = 0; i < 2; ++i)
{
r.push_back(Vector(n));
z.push_back(Vector(n));
}
p.Resize(n);
SparseMatrix hTranspose;
SparseMatrix h;
laplacian = SparseMatrix(n, n);
hTranspose = SparseMatrix(n, n);
h = SparseMatrix(n, n);
preconditioner = SparseMatrix(n, n);
pressure.Resize(n);
tempPressure.Resize(n);
vel.Resize(n);
ones.Resize(n);
for (int i = 0; i < n; ++i)
{
ones[i] = 1;
}
double rdxSqr = (resX * resX) / (width * width);
double rdySqr = (resY * resY) / (height * height);
double rdzSqr = (resZ * resZ) / (depth * depth);
// initialize values for the laplacian matrix
for (int i = 0; i < resX; ++i)
{
for (int j = 0; j < resY; ++j)
{
for (int k = 0; k < resZ; ++k)
{
int row = makeIndex(i, j, k);
int x1 = (i == (resX - 1) ? row : row + 1);
int x2 = (i == 0 ? row : row - 1);
int y1 = (j == (resY - 1) ? row : row + resX);
int y2 = (j == 0 ? row : row - resX);
int z1 = (k == (resZ - 1) ? row : row + resY * resX);
int z2 = (k == 0 ? row : row - resY * resX);
laplacian[row][row] += 6 * (rdxSqr + rdySqr + rdzSqr);
laplacian[row][x1] -= rdxSqr;
laplacian[row][x2] -= rdxSqr;
laplacian[row][y1] -= rdySqr;
laplacian[row][y2] -= rdySqr;
laplacian[row][z1] -= rdzSqr;
laplacian[row][z2] -= rdzSqr;
}
}
}
// perform Cholesky preconditioning
// decompose laplacian into H * H^T
SparseVector::iterator iter;
hTranspose = laplacian.Transpose();
SparseMatrix& A = hTranspose;
for (int k = 0; k < n; ++k)
{
SparseVector::iterator aik(A[k]);
aik.seek(k);
if (aik.getIndex() != k)
{
assert(0);
}
double divisor = std::sqrt(*aik);
*aik = divisor;
aik++;
while (aik)
{
*aik /= divisor;
aik++;
}
SparseVector::iterator ajk(A[k]);
ajk.seek(k + 1);
while (ajk)
{
int j = ajk.getIndex();
aik.reset();
aik.seek(j);
SparseVector::iterator aij(A[j]);
aij.seek(j);
while (aij && aik)
{
int i = aij.getIndex();
if (aik.getIndex() == i)
{
*aij -= (*aik) * (*ajk);
aik++;
}
aij++;
if (aij)
{
i = aij.getIndex();
while (aik && aik.getIndex() < i)
{
aik++;
}
}
}
ajk++;
}
}
h = hTranspose.Transpose();
// cerr<<"Sparsity of laplacian is " << laplacian.Sparsity() << endl;
preconditioner = h.FastMultiplyTranspose();
int maxIndex = resX * resY * resZ;
// for (int i = 0; i < maxIndex; ++i)
// preconditioner[i][i] = 1;
// cerr<<"Sparsity of preconditioner is " << preconditioner.Sparsity() << endl;
}
FdmExtOUJumpOp::FdmExtOUJumpOp(
const boost::shared_ptr<FdmMesher>& mesher,
const boost::shared_ptr<ExtOUWithJumpsProcess>& process,
const boost::shared_ptr<YieldTermStructure>& rTS,
const FdmBoundaryConditionSet& bcSet,
Size integroIntegrationOrder)
: mesher_ (mesher),
process_(process),
rTS_ (rTS),
bcSet_ (bcSet),
gaussLaguerreIntegration_(integroIntegrationOrder),
x_ (mesher->locations(0)),
ouOp_ (new FdmExtendedOrnsteinUhlenbackOp(
mesher,
process->getExtendedOrnsteinUhlenbeckProcess(), rTS, bcSet)),
dyMap_ (FirstDerivativeOp(1, mesher)
.mult(-process->beta()*mesher->locations(1)))
{
#if !defined(QL_NO_UBLAS_SUPPORT)
const Real eta = process_->eta();
const Real lambda = process_->jumpIntensity();
const Array yInt = gaussLaguerreIntegration_.x();
const Array weights= gaussLaguerreIntegration_.weights();
integroPart_ = SparseMatrix(mesher_->layout()->size(),
mesher_->layout()->size());
const boost::shared_ptr<FdmLinearOpLayout> layout = mesher_->layout();
const FdmLinearOpIterator endIter = layout->end();
Array yLoc(mesher_->layout()->dim()[1]);
for (FdmLinearOpIterator iter = layout->begin(); iter != endIter;
++iter) {
yLoc[iter.coordinates()[1]] = mesher_->location(iter, 1);
}
for (FdmLinearOpIterator iter = layout->begin(); iter != endIter;
++iter) {
const Size diag = iter.index();
integroPart_(diag, diag) -= lambda;
const Real y = mesher_->location(iter, 1);
const Integer yIndex = iter.coordinates()[1];
for (Size i=0; i < yInt.size(); ++i) {
const Real weight = std::exp(-yInt[i])*weights[i];
const Real ys = y + yInt[i]/eta;
const Integer l = (ys > yLoc.back()) ? yLoc.size()-2
: std::upper_bound(yLoc.begin(),
yLoc.end()-1, ys) - yLoc.begin()-1;
const Real s = (ys-yLoc[l])/(yLoc[l+1]-yLoc[l]);
integroPart_(diag, layout->neighbourhood(iter, 1, l-yIndex))
+= weight*lambda*(1-s);
integroPart_(diag, layout->neighbourhood(iter, 1, l+1-yIndex))
+= weight*lambda*s;
}
}
#endif
}
