CellDoubleArray FaceToCellOperator::apply(FaceDoubleArray & u){
CellDoubleArray out = g->makeCellDoubleArray();
/*CellToFaceCoefficients::iterator cIt;
for(cIt = coefficients.begin(); cIt != coefficients.end(); cIt++){
CellToFaceIndex c2f = (*cIt).first;
out(c2f.i,c2f.j) += ((*cIt).second)*u(c2f.faceIndex);
}*/
FaceToCellCoefficients::iterator cIt;
for(cIt = coefficients.begin(); cIt != coefficients.end(); cIt++){
int i = (*cIt).first.i, j = (*cIt).first.j;
FaceCoefficients::iterator fIt;
for(fIt = (*cIt).second.begin(); fIt != (*cIt).second.end(); fIt++){
int faceIndex = (*fIt).first;
double c = (*fIt).second;
out(i,j) += c*u(faceIndex);
}
}
for(int i = g->iMin; i <= g->iMax; i++){
for(int j = g->jMin; j <= g->jMax; j++){
out(i,j) += constantTerm(i,j);
}
}
return out;
}
/* ************************************************************************* */
boost::shared_ptr<GaussianFactor>
LinearizedHessianFactor::linearize(const Values& c) const {
// Construct an error vector in key-order from the Values
Vector dx = Vector::Zero(dim());
size_t index = 0;
for(unsigned int i = 0; i < this->size(); ++i){
Key key = this->keys()[i];
const Value& newPt = c.at(key);
const Value& linPt = lin_points_.at(key);
dx.segment(index, linPt.dim()) = linPt.localCoordinates_(newPt);
index += linPt.dim();
}
// f2 = f1 - 2*dx'*g1 + dx'*G1*dx
//newInfo(this->size(), this->size())(0,0) += -2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm().selfadjointView<Eigen::Upper>() * dx;
double f = constantTerm() - 2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm() * dx;
// g2 = g1 - G1*dx
//newInfo.rangeColumn(0, this->size(), this->size(), 0) -= squaredTerm().selfadjointView<Eigen::Upper>() * dx;
Vector g = linearTerm() - squaredTerm() * dx;
std::vector<Vector> gs;
std::size_t offset = 0;
for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) {
const std::size_t dim = info_.getDim(i);
gs.push_back(g.segment(offset, dim));
offset += dim;
}
// G2 = G1
// Do Nothing
std::vector<Matrix> Gs;
for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) {
Gs.push_back(info_.diagonalBlock(i));
for(DenseIndex j = i + 1; j < info_.nBlocks()-1; ++j) {
Gs.push_back(info_.aboveDiagonalBlock(i, j));
}
}
// Create a Hessian Factor from the modified info matrix
//return boost::shared_ptr<GaussianFactor>(new HessianFactor(js, newInfo));
return boost::shared_ptr<GaussianFactor>(new HessianFactor(keys(), Gs, gs, f));
}
/* ************************************************************************* */
double LinearizedHessianFactor::error(const Values& c) const {
// Construct an error vector in key-order from the Values
Vector dx = Vector::Zero(dim());
size_t index = 0;
for(unsigned int i = 0; i < this->size(); ++i){
Key key = this->keys()[i];
const Value& newPt = c.at(key);
const Value& linPt = lin_points_.at(key);
dx.segment(index, linPt.dim()) = linPt.localCoordinates_(newPt);
index += linPt.dim();
}
// error 0.5*(f - 2*x'*g + x'*G*x)
double f = constantTerm();
double xtg = dx.dot(linearTerm());
double xGx = dx.transpose() * squaredTerm() * dx;
return 0.5 * (f - 2.0 * xtg + xGx);
}
Gradient::Gradient(Grid * g){
//cout << "Trying to create gradient with h = " << g->getH() << endl;
this->g = g;
double h = g->getH();
constantTerm.resize(g->faces.size());
constantTerm = 0;
for(int i = g->iMin; i <= g->iMax; i++){
for(int j = g->jMin; j <= g->jMax; j++){
if(g->cells(i,j)->isUncovered()){
//cout << " is uncovered." << endl;
CellIndex cellIndex(i,j);
TinyVector<Face*,5> faces = g->cells(i,j)->faces;
if(g->isFaceUncovered(i,j,B)){
FaceIndex faceIndex = faces(B)->getIndex();
Coord centroid = faces(B)->getCentroid();
TinyVector<double,2> n = faces(B)->getNormal();
double x, y, r;
x = centroid(0);
y = centroid(1);
r = sqrt(pow2(x) + pow2(y));
TinyVector<double,2> gradU;
double pi = acos(-1);
//gradU(0) = 3*pow(x,2)*pow(y-2,2)+sin(2*(x-pow(y,2)));
//gradU(1) = 2*pow(x,3)*(y-2) - 2*y*sin(2*(x-pow(y,2)));
gradU(0) = 0;//-2*pi*sin(2*pi*r)*x/r;
gradU(1) = 0;//-2*pi*sin(2*pi*r)*y/r;
constantTerm(faceIndex) += gradU(0)*n(0) + gradU(1)*n(1);
//cout << "\tAdding boundary condition to constant term." << endl;
}
if(faces(N) != 0){
FaceIndex faceIndex = faces(N)->getIndex();
if(faces(N)->isRegular()){
coefficients[faceIndex][cellIndex] = -1/h;
}
else if(faces(N)->isIrregular()){
if(coefficients[faceIndex].count(cellIndex) == 0){
double alpha = g->cells(i,j)->getFace(N)->getArea();
if(g->isFaceUncovered(i+1,j,N)){
CellIndex ind1(i,j);
CellIndex ind2(i,j+1);
CellIndex ind3(i+1,j);
CellIndex ind4(i+1,j+1);
interpolateIrregularFace(alpha,ind1,ind2,ind3,ind4,faceIndex);
}
else if(g->isFaceUncovered(i-1,j,N)){
CellIndex ind1(i,j);
CellIndex ind2(i,j+1);
CellIndex ind3(i-1,j);
CellIndex ind4(i-1,j+1);
interpolateIrregularFace(alpha,ind1,ind2,ind3,ind4,faceIndex);
}
}
}
//cout << "\tDid north face" << endl;
}
if(faces(S) != 0){
FaceIndex faceIndex = faces(S)->getIndex();
if(faces(S)->isRegular()){
coefficients[faceIndex][cellIndex] = 1/h;
}
else if(faces(S)->isIrregular()){
if(coefficients[faceIndex].count(cellIndex) == 0){
double alpha = g->cells(i,j)->getFace(S)->getArea();
if(g->isFaceUncovered(i+1,j,S)){
CellIndex ind1(i,j-1);
CellIndex ind2(i,j);
CellIndex ind3(i+1,j-1);
CellIndex ind4(i+1,j);
interpolateIrregularFace(alpha,ind1,ind2,ind3,ind4,faceIndex);
}
else if(g->isFaceUncovered(i-1,j,S)){
CellIndex ind1(i,j-1);
CellIndex ind2(i,j);
CellIndex ind3(i-1,j-1);
CellIndex ind4(i-1,j);
interpolateIrregularFace(alpha,ind1,ind2,ind3,ind4,faceIndex);
}
}
}
//cout << "\tDid south face" << endl;
}
if(faces(E) != 0){
FaceIndex faceIndex = faces(E)->getIndex();
CellIndex cellIndex(i,j);
if(faces(E)->isRegular()){
coefficients[faceIndex][cellIndex] = -1/h;
}
else if(faces(E)->isIrregular()){
if(coefficients[faceIndex].count(cellIndex) == 0){
double alpha = g->cells(i,j)->getFace(E)->getArea();
if(g->isFaceUncovered(i,j+1,E)){
CellIndex ind1(i,j);
CellIndex ind2(i+1,j);
CellIndex ind3(i,j+1);
CellIndex ind4(i+1,j+1);
interpolateIrregularFace(alpha,ind1,ind2,ind3,ind4,faceIndex);
}
else if(g->isFaceUncovered(i,j-1,E)){
CellIndex ind1(i,j);
CellIndex ind2(i+1,j);
CellIndex ind3(i,j-1);
CellIndex ind4(i+1,j-1);
interpolateIrregularFace(alpha,ind1,ind2,ind3,ind4,faceIndex);
}
}
}
//cout << "\tDid east face" << endl;
}
if(faces(W) != 0){
FaceIndex faceIndex = faces(W)->getIndex();
CellIndex cellIndex(i,j);
if(faces(W)->isRegular()){
coefficients[faceIndex][cellIndex] = 1/h;
}
else if(faces(W)->isIrregular()){
if(coefficients[faceIndex].count(cellIndex) == 0){
double alpha = g->cells(i,j)->getFace(W)->getArea();
if(g->isFaceUncovered(i,j+1,W)){
CellIndex ind1(i+1,j);
CellIndex ind2(i,j);
CellIndex ind3(i+1,j+1);
CellIndex ind4(i,j+1);
interpolateIrregularFace(alpha,ind1,ind2,ind3,ind4,faceIndex);
}
else if(g->isFaceUncovered(i,j-1,W)){
CellIndex ind1(i+1,j);
CellIndex ind2(i,j);
CellIndex ind3(i+1,j-1);
CellIndex ind4(i,j-1);
interpolateIrregularFace(alpha,ind1,ind2,ind3,ind4,faceIndex);
}
}
}
//cout << "\tDid west face" << endl;
}
}
}
}
//cout << "Resized" << endl;
/*
for(int i = g->iMin; i <= g->iMax; i++){
for(int j = g->jMin; j <= g->jMax; j++){
cout << "i = " << i << " j = " << j << endl;
if(g->cells(i,j)->isUncovered()){
cout << "Uncovered" << endl;
double c = 1;
CellIndex index(i,j);
TinyVector<Face*,5> faces = g->cells(i,j)->faces;
cout << "Got faces" << endl;
for(int k = 0; k < 5; k++){
cout << "Testing face " << k << endl;
if(faces(k) != 0 && faces(k)->isUncovered()){
cout << "Face " << k << " is uncovered ";
cout << "and has index " << faces(k)->getIndex() << endl;
cout << coefficients(faces(k)->getIndex()).empty() << endl;
coefficients(faces(k)->getIndex())[index] = c;
}
}
}
}
}*/
}
