void LipmVarHeightPlanner::backwardPass(const Traj<3,3> &com)
{
Eigen::Matrix<double,6,6> Qxx;
Eigen::Matrix<double,3,3> Quu;
Eigen::Matrix<double,3,3> invQuu;
Eigen::Matrix<double,3,6> Qux;
Eigen::Matrix<double,6,3> Qxu;
Eigen::Matrix<double,6,1> Qx;
Eigen::Matrix<double,3,1> Qu;
Eigen::Matrix<double,6,6> A;
Eigen::Matrix<double,6,3> B;
Eigen::Matrix<double,6,6> Vxx = _Vxx;
Eigen::Matrix<double,6,1> Vx = Eigen::Matrix<double,6,1>::Zero();
Eigen::Matrix<double,6,1> cur_x;
Eigen::Matrix<double,3,1> cur_u;
Eigen::Matrix<double,6,1> x_h;
Eigen::Matrix<double,3,1> u_h;
//NEW_GSL_MATRIX(tmpXX0, tmpXX0_d, tmpXX0_v, 6, 6);
//NEW_GSL_MATRIX(tmpUX0, tmpUX0_d, tmpUX0_v, 3, 6);
const double *x0, *u0;
for (int t = (int)_K.size()-1; t >= 0; t--) {
//printf("======================================\n");
// get x u
x0 = _traj0[t].x;
u0 = _traj0[t].u;
//printf("%g %g %g %g %g %g\n", x0[0], x0[1], x0[2], x0[3], x0[4], x0[5]);
//printf("%g %g %g\n", u0[0], u0[1], u0[2]);
for (int i = 0; i < 6; i++) {
cur_x(i) = x0[i];
x_h(i) = x0[i] - _zmp_d[t].x[i];
}
for (int i = 0; i < 3; i++) {
cur_u(i) = u0[i];
u_h(i) = u0[i] - _zmp_d[t].u[i];
}
// get A B
getAB(x0, u0, _z0[t], A, B);
//printGSLMatrix("A", A);
//printGSLMatrix("B", B);
// Qx = Q*x_hat + A'*Vx
Qx = _Q*x_h + A.transpose()*Vx;
// Qu = R*u_hat + B'*Vx
Qu = _R*u_h + B.transpose()*Vx;
// Qxx = Q + A'*Vxx*A
Qxx = _Q + A.transpose()*Vxx*A;
// Qxu = A'*Vxx*B
Qxu = A.transpose()*Vxx*B;
// Quu = R + B'*Vxx*B
Quu = _R + B.transpose()*Vxx*B;
// Qux = Qxu'
Qux = B.transpose()*Vxx*A;
// K = -inv(Quu)*Qux
_K[t] = -(Quu.llt().solve(Qux));
// du = -inv(Quu)*Qu
_du[t] = -(Quu.llt().solve(Qu));
// Vx = Qx + K'*Qu
Vx = Qx + _K[t].transpose()*Qu;
// Vxx = Qxx + Qxu*K
Vxx = Qxx + Qxu*_K[t];
//getchar();
//assert(!hasInfNan(_K[t]));
//assert(!hasInfNan(_du[t]));
}
}
double LipmVarHeightPlanner::forwardPass(const double *x0, Traj<3,3> &traj1) const
{
Eigen::Matrix<double,6,1> z;
Eigen::Matrix<double,6,1> z1;
Eigen::Matrix<double,3,1> u;
traj1 = _zmp_d;
//traj1 = Traj<3,3>();
//for (size_t i = 0; i < _zmp_d.size(); i++)
// traj1.append(_zmp_d[i].time, _zmp_d[i].type, NULL, NULL, NULL, NULL);
dvec_copy(traj1[0].x, x0, 6);
double cost = 0;
Eigen::Matrix<double,6,1> x_h;
Eigen::Matrix<double,3,1> u_h;
for (size_t t = 0; t < _zmp_d.size(); t++) {
// x - xref
for (int i = 0; i < 6; i++) {
z(i) = traj1[t].x[i] - _traj0[t].x[i];
x_h(i) = traj1[t].x[i] - _zmp_d[t].x[i];
}
// u = alpha*du + uref
u = _alpha*_du[t];
for (int i = 0; i < 3; i++)
u(i) += _traj0[t].u[i];
// u += K*z
u += _K[t]*z;
for (int i = 0; i < 3; i++)
traj1[t].u[i] = u(i);
for (int i = 0; i < 3; i++)
u_h(i) = traj1[t].u[i] - _zmp_d[t].u[i];
// compute cost
cost += 0.5 * x_h.transpose() * _Q * x_h;
cost += 0.5 * u_h.transpose() * _R * u_h;
// integrate
if (t < _zmp_d.size()-1)
integrate(traj1[t].x, traj1[t].u, _z0[t], traj1[t].acc, traj1[1+t].x);
}
/*
FILE *out = fopen("tmp/lipmz_traj0", "w");
for (size_t t = 0; t < _traj0.size(); t++) {
fprintf(out, "%g %g %g %g %g %g\n", _traj0[t].x[0], _traj0[t].x[1], _traj0[t].x[2], _traj0[t].u[0], _traj0[t].u[1], _traj0[t].u[2]);
}
fclose(out);
out = fopen("tmp/lipmz_traj1", "w");
for (size_t t = 0; t < traj1.size(); t++) {
fprintf(out, "%g %g %g %g %g %g\n", traj1[t].x[0], traj1[t].x[1], traj1[t].x[2], traj1[t].u[0], traj1[t].u[1], traj1[t].u[2]);
}
fclose(out);
*/
return cost;
}
int main(int argc, char * argv[])
{
typedef MPI::HGeometryForest<DIM,DOW> forest_t;
typedef MPI::BirdView<forest_t> ir_mesh_t;
typedef FEMSpace<double,DIM,DOW> fe_space_t;
typedef MPI::DOF::GlobalIndex<forest_t, fe_space_t> global_index_t;
MPI_Init(&argc, &argv);
forest_t forest(MPI_COMM_WORLD);
ir_mesh_t ir_mesh;
MPI::load_mesh(argv[1], forest, ir_mesh); /// 从一个目录中读入网格数据
int round = 0;
if (argc >= 3) round = atoi(argv[2]);
ir_mesh.globalRefine(round);
ir_mesh.semiregularize();
ir_mesh.regularize(false);
TemplateGeometry<DIM> tri;
tri.readData("triangle.tmp_geo");
CoordTransform<DIM,DIM> tri_ct;
tri_ct.readData("triangle.crd_trs");
TemplateDOF<DIM> tri_td(tri);
tri_td.readData("triangle.1.tmp_dof");
BasisFunctionAdmin<double,DIM,DIM> tri_bf(tri_td);
tri_bf.readData("triangle.1.bas_fun");
std::vector<TemplateElement<double,DIM,DIM> > tmp_ele(1);
tmp_ele[0].reinit(tri, tri_td, tri_ct, tri_bf);
RegularMesh<DIM,DOW>& mesh = ir_mesh.regularMesh();
fe_space_t fem_space(mesh, tmp_ele);
u_int n_ele = mesh.n_geometry(DIM);
fem_space.element().resize(n_ele);
for (int i = 0;i < n_ele;i ++) {
fem_space.element(i).reinit(fem_space, i, 0);
}
fem_space.buildElement();
fem_space.buildDof();
fem_space.buildDofBoundaryMark();
std::cout << "Building global indices ... " << std::flush;
global_index_t global_index(forest, fem_space);
global_index.build();
std::cout << "OK!" << std::endl;
Epetra_MpiComm comm(forest.communicator());
Epetra_Map map(global_index.n_global_dof(), global_index.n_primary_dof(), 0, comm);
global_index.build_epetra_map(map);
/// 构造 Epetra 的分布式稀疏矩阵模板
std::cout << "Build sparsity pattern ... " << std::flush;
Epetra_FECrsGraph G(Copy, map, 10);
fe_space_t::ElementIterator
the_ele = fem_space.beginElement(),
end_ele = fem_space.endElement();
for (;the_ele != end_ele;++ the_ele) {
const std::vector<int>& ele_dof = the_ele->dof();
u_int n_ele_dof = ele_dof.size();
/**
* 建立从局部自由度数组到全局自由度数组的映射表，这是实现分布式并行
* 状态下的数据结构的关键一步。
*/
std::vector<int> indices(n_ele_dof);
for (u_int i = 0;i < n_ele_dof;++ i) {
indices[i] = global_index(ele_dof[i]);
}
G.InsertGlobalIndices(n_ele_dof, &indices[0], n_ele_dof, &indices[0]);
}
G.GlobalAssemble();
std::cout << "OK!" << std::endl;
/// 准备构造 Epetra 的分布式稀疏矩阵和计算分布式右端项
std::cout << "Build sparse matrix ... " << std::flush;
Epetra_FECrsMatrix A(Copy, G);
Epetra_FEVector b(map);
the_ele = fem_space.beginElement();
for (;the_ele != end_ele;++ the_ele) {
double vol = the_ele->templateElement().volume();
const QuadratureInfo<DIM>& qi = the_ele->findQuadratureInfo(5);
std::vector<Point<DIM> > q_pnt = the_ele->local_to_global(qi.quadraturePoint());
int n_q_pnt = qi.n_quadraturePoint();
std::vector<double> jac = the_ele->local_to_global_jacobian(qi.quadraturePoint());
std::vector<std::vector<double> > bas_val = the_ele->basis_function_value(q_pnt);
std::vector<std::vector<std::vector<double> > > bas_grad = the_ele->basis_function_gradient(q_pnt);
const std::vector<int>& ele_dof = the_ele->dof();
u_int n_ele_dof = ele_dof.size();
FullMatrix<double> ele_mat(n_ele_dof, n_ele_dof);
Vector<double> ele_rhs(n_ele_dof);
for (u_int l = 0;l < n_q_pnt;++ l) {
double JxW = vol*jac[l]*qi.weight(l);
double f_val = _f_(q_pnt[l]);
for (u_int i = 0;i < n_ele_dof;++ i) {
for (u_int j = 0;j < n_ele_dof;++ j) {
ele_mat(i, j) += JxW*(innerProduct(bas_grad[i][l], bas_grad[j][l]));
}
ele_rhs(i) += JxW*f_val*bas_val[i][l];
}
}
/**
* 此处将单元矩阵和单元载荷先计算好，然后向全局的矩阵和载荷向量上
* 集中，可以提高效率。
*/
std::vector<int> indices(n_ele_dof);
for (u_int i = 0;i < n_ele_dof;++ i) {
indices[i] = global_index(ele_dof[i]);
}
A.SumIntoGlobalValues(n_ele_dof, &indices[0], n_ele_dof, &indices[0], &ele_mat(0,0));
b.SumIntoGlobalValues(n_ele_dof, &indices[0], &ele_rhs(0));
}
A.GlobalAssemble();
b.GlobalAssemble();
std::cout << "OK!" << std::endl;
/// 准备解向量。
Epetra_FEVector x(map);
/// 加上狄氏边值条件
u_int n_bnd_dof = 0; /// 首先清点边界上自由度的个数
for (u_int i = 0;i < fem_space.n_dof();++ i) {
if (fem_space.dofBoundaryMark(i) > 0) {
/// 如果不是在主几何体上就不做
if (! global_index.is_dof_on_primary_geometry(i)) continue;
n_bnd_dof += 1;
}
}
/// 准备空间存储边界上全局标号、自变量和右端项
std::vector<int> bnd_idx(n_bnd_dof);
std::vector<double> x_entry(n_bnd_dof), rhs_entry(n_bnd_dof);
/// 对自由度做循环
for (u_int i = 0, j = 0;i < fem_space.n_dof();++ i) {
if (fem_space.dofBoundaryMark(i) > 0) { /// 边界上的自由度?
/// 如果不是在主几何体上就不做
if (! global_index.is_dof_on_primary_geometry(i)) continue;
const int& idx = global_index(i); /// 行的全局标号
bnd_idx[j] = idx;
/// 修改矩阵
int lrid = A.LRID(idx);
int row_nnz, *row_idx;
double *row_entry, row_diag;
A.ExtractMyRowView(lrid, row_nnz, row_entry, row_idx); /// 取出矩阵的行
for (int k = 0;k < row_nnz;++ k) { /// 对矩阵的行进行修改
if (A.LCID(row_idx[k]) != lrid) { /// 如果不是对角元
row_entry[k] = 0.0; /// 则将矩阵元素清零
} else { /// 而对角元保持不变
row_diag = row_entry[k]; /// 并记录下对角元
}
}
/// 计算并记下自变量和右端项，假设自由度值为插值量
double u_b_val = _u_b_(fem_space.dofInfo(i).interp_point);
x_entry[j] = u_b_val;
rhs_entry[j] = row_diag*u_b_val;
j += 1;
}
}
std::cout << "# DOF on the boundary: " << n_bnd_dof << std::endl;
/// 修改解变量和右端项
x.ReplaceGlobalValues(n_bnd_dof, &bnd_idx[0], &x_entry[0]);
b.ReplaceGlobalValues(n_bnd_dof, &bnd_idx[0], &rhs_entry[0]);
/// 调用 AztecOO 的求解器。
std::cout << "Solving the linear system ..." << std::flush;
Epetra_LinearProblem problem(&A, &x, &b);
AztecOO solver(problem);
ML_Epetra::MultiLevelPreconditioner precond(A, true);
solver.SetPrecOperator(&precond);
solver.SetAztecOption(AZ_solver, AZ_gmres);
solver.SetAztecOption(AZ_output, 100);
solver.Iterate(5000, 1.0e-12);
std::cout << "OK!" << std::endl;
Epetra_Map fe_map(-1, global_index.n_local_dof(), &global_index(0), 0, comm);
FEMFunction<double,DIM> u_h(fem_space);
Epetra_Import importer(fe_map, map);
Epetra_Vector X(View, fe_map, &u_h(0));
X.Import(x, importer, Add);
char filename[1024];
sprintf(filename, "u_h%d.dx", forest.rank());
u_h.writeOpenDXData(filename);
MPI_Finalize();
return 0;
}
int main(int argc, char * argv[])
{
typedef MPI::HGeometryForest<DIM,DOW> forest_t;
typedef MPI::BirdView<forest_t> ir_mesh_t;
typedef FEMSpace<double,DIM,DOW> fe_space_t;
typedef MPI::DOF::GlobalIndex<forest_t, fe_space_t> global_index_t;
MPI_Init(&argc, &argv);
forest_t forest(MPI_COMM_WORLD);
ir_mesh_t ir_mesh;
MPI::load_mesh(argv[1], forest, ir_mesh); /// 从一个目录中读入网格数据
int round = 0;
if (argc >= 3) round = atoi(argv[2]);
ir_mesh.globalRefine(round);
ir_mesh.semiregularize();
ir_mesh.regularize(false);
TemplateGeometry<DIM> tri;
tri.readData("triangle.tmp_geo");
CoordTransform<DIM,DIM> tri_ct;
tri_ct.readData("triangle.crd_trs");
TemplateDOF<DIM> tri_td(tri);
tri_td.readData("triangle.1.tmp_dof");
BasisFunctionAdmin<double,DIM,DIM> tri_bf(tri_td);
tri_bf.readData("triangle.1.bas_fun");
std::vector<TemplateElement<double,DIM,DIM> > tmp_ele(1);
tmp_ele[0].reinit(tri, tri_td, tri_ct, tri_bf);
RegularMesh<DIM,DOW>& mesh = ir_mesh.regularMesh();
fe_space_t fem_space(mesh, tmp_ele);
u_int n_ele = mesh.n_geometry(DIM);
fem_space.element().resize(n_ele);
for (int i = 0;i < n_ele;i ++) {
fem_space.element(i).reinit(fem_space, i, 0);
}
fem_space.buildElement();
fem_space.buildDof();
fem_space.buildDofBoundaryMark();
std::cout << "Building global indices ... " << std::flush;
global_index_t global_index(forest, fem_space);
global_index.build();
std::cout << "OK!" << std::endl;
Epetra_MpiComm comm(forest.communicator());
Epetra_Map map(global_index.n_global_dof(), global_index.n_primary_dof(), 0, comm);
global_index.build_epetra_map(map);
/// 构造 Epetra 的分布式稀疏矩阵模板
std::cout << "Build sparsity pattern ... " << std::flush;
Epetra_FECrsGraph G(Copy, map, 10);
fe_space_t::ElementIterator
the_ele = fem_space.beginElement(),
end_ele = fem_space.endElement();
for (;the_ele != end_ele;++ the_ele) {
const std::vector<int>& ele_dof = the_ele->dof();
u_int n_ele_dof = ele_dof.size();
/**
* 建立从局部自由度数组到全局自由度数组的映射表，这是实现分布式并行
* 状态下的数据结构的关键一步。
*/
std::vector<int> indices(n_ele_dof);
for (u_int i = 0;i < n_ele_dof;++ i) {
indices[i] = global_index(ele_dof[i]);
}
G.InsertGlobalIndices(n_ele_dof, &indices[0], n_ele_dof, &indices[0]);
}
G.GlobalAssemble();
std::cout << "OK!" << std::endl;
/// 准备构造 Epetra 的分布式稀疏矩阵和计算分布式右端项
std::cout << "Build sparse matrix ... " << std::flush;
Epetra_FECrsMatrix A(Copy, G);
Epetra_FEVector b(map);
the_ele = fem_space.beginElement();
for (;the_ele != end_ele;++ the_ele) {
double vol = the_ele->templateElement().volume();
const QuadratureInfo<DIM>& qi = the_ele->findQuadratureInfo(5);
std::vector<Point<DIM> > q_pnt = the_ele->local_to_global(qi.quadraturePoint());
int n_q_pnt = qi.n_quadraturePoint();
std::vector<double> jac = the_ele->local_to_global_jacobian(qi.quadraturePoint());
std::vector<std::vector<double> > bas_val = the_ele->basis_function_value(q_pnt);
std::vector<std::vector<std::vector<double> > > bas_grad = the_ele->basis_function_gradient(q_pnt);
const std::vector<int>& ele_dof = the_ele->dof();
u_int n_ele_dof = ele_dof.size();
FullMatrix<double> ele_mat(n_ele_dof, n_ele_dof);
Vector<double> ele_rhs(n_ele_dof);
for (u_int l = 0;l < n_q_pnt;++ l) {
double JxW = vol*jac[l]*qi.weight(l);
double f_val = _f_(q_pnt[l]);
for (u_int i = 0;i < n_ele_dof;++ i) {
for (u_int j = 0;j < n_ele_dof;++ j) {
ele_mat(i, j) += JxW*(bas_val[i][l]*bas_val[j][l] +
innerProduct(bas_grad[i][l], bas_grad[j][l]));
}
ele_rhs(i) += JxW*f_val*bas_val[i][l];
}
}
/**
* 此处将单元矩阵和单元载荷先计算好，然后向全局的矩阵和载荷向量上
* 集中，可以提高效率。
*/
std::vector<int> indices(n_ele_dof);
for (u_int i = 0;i < n_ele_dof;++ i) {
indices[i] = global_index(ele_dof[i]);
}
A.SumIntoGlobalValues(n_ele_dof, &indices[0], n_ele_dof, &indices[0], &ele_mat(0,0));
b.SumIntoGlobalValues(n_ele_dof, &indices[0], &ele_rhs(0));
}
A.GlobalAssemble();
b.GlobalAssemble();
std::cout << "OK!" << std::endl;
/// 准备解向量。
Epetra_Vector x(map);
/// 调用 AztecOO 的求解器。
std::cout << "Solving the linear system ..." << std::flush;
Epetra_LinearProblem problem(&A, &x, &b);
AztecOO solver(problem);
ML_Epetra::MultiLevelPreconditioner precond(A, true);
solver.SetPrecOperator(&precond);
solver.SetAztecOption(AZ_solver, AZ_cg);
solver.SetAztecOption(AZ_output, 100);
solver.Iterate(5000, 1.0e-12);
std::cout << "OK!" << std::endl;
Epetra_Map fe_map(-1, global_index.n_local_dof(), &global_index(0), 0, comm);
FEMFunction<double,DIM> u_h(fem_space);
Epetra_Import importer(fe_map, map);
Epetra_Vector X(View, fe_map, &u_h(0));
X.Import(x, importer, Add);
char filename[1024];
sprintf(filename, "u_h%d.dx", forest.rank());
u_h.writeOpenDXData(filename);
MPI_Finalize();
return 0;
}
int main(int argc, char * argv[])
{
typedef MPI::HGeometryForest<DIM,DOW> forest_t;
typedef MPI::BirdView<forest_t> ir_mesh_t;
typedef FEMSpace<double,DIM,DOW> fe_space_t;
typedef MPI::DOF::GlobalIndex<forest_t, fe_space_t> global_index_t;
PetscInitialize(&argc, &argv, (char *)NULL, help);
forest_t forest(PETSC_COMM_WORLD);
forest.readMesh(argv[1]);
ir_mesh_t ir_mesh(forest);
int round = 0;
if (argc >= 3) round = atoi(argv[2]);
ir_mesh.globalRefine(round);
ir_mesh.semiregularize();
ir_mesh.regularize(false);
setenv("AFEPACK_TEMPLATE_PATH", "/usr/local/AFEPack/template/triangle", 1);
TemplateGeometry<DIM> tri;
tri.readData("triangle.tmp_geo");
CoordTransform<DIM,DIM> tri_ct;
tri_ct.readData("triangle.crd_trs");
TemplateDOF<DIM> tri_td(tri);
tri_td.readData("triangle.1.tmp_dof");
BasisFunctionAdmin<double,DIM,DIM> tri_bf(tri_td);
tri_bf.readData("triangle.1.bas_fun");
std::vector<TemplateElement<double,DIM,DIM> > tmp_ele(1);
tmp_ele[0].reinit(tri, tri_td, tri_ct, tri_bf);
RegularMesh<DIM,DOW>& mesh = ir_mesh.regularMesh();
fe_space_t fem_space(mesh, tmp_ele);
u_int n_ele = mesh.n_geometry(DIM);
fem_space.element().resize(n_ele);
for (int i = 0;i < n_ele;i ++) {
fem_space.element(i).reinit(fem_space, i, 0);
}
fem_space.buildElement();
fem_space.buildDof();
fem_space.buildDofBoundaryMark();
std::cout << "Building global indices ... " << std::flush;
global_index_t global_index(forest, fem_space);
global_index.build();
std::cout << "OK!" << std::endl;
std::cout << "Building the linear system ... " << std::flush;
Mat A;
Vec x, b;
MatCreateMPIAIJ(PETSC_COMM_WORLD,
global_index.n_primary_dof(), global_index.n_primary_dof(),
PETSC_DECIDE, PETSC_DECIDE,
0, PETSC_NULL, 0, PETSC_NULL, &A);
VecCreateMPI(PETSC_COMM_WORLD, global_index.n_primary_dof(), PETSC_DECIDE, &b);
fe_space_t::ElementIterator
the_ele = fem_space.beginElement(),
end_ele = fem_space.endElement();
for (;the_ele != end_ele;++ the_ele) {
double vol = the_ele->templateElement().volume();
const QuadratureInfo<DIM>& qi = the_ele->findQuadratureInfo(5);
std::vector<Point<DIM> > q_pnt = the_ele->local_to_global(qi.quadraturePoint());
int n_q_pnt = qi.n_quadraturePoint();
std::vector<double> jac = the_ele->local_to_global_jacobian(qi.quadraturePoint());
std::vector<std::vector<double> > bas_val = the_ele->basis_function_value(q_pnt);
std::vector<std::vector<std::vector<double> > > bas_grad = the_ele->basis_function_gradient(q_pnt);
const std::vector<int>& ele_dof = the_ele->dof();
u_int n_ele_dof = ele_dof.size();
FullMatrix<double> ele_mat(n_ele_dof, n_ele_dof);
Vector<double> ele_rhs(n_ele_dof);
for (u_int l = 0;l < n_q_pnt;++ l) {
double JxW = vol*jac[l]*qi.weight(l);
double f_val = _f_(q_pnt[l]);
for (u_int i = 0;i < n_ele_dof;++ i) {
for (u_int j = 0;j < n_ele_dof;++ j) {
ele_mat(i, j) += JxW*(bas_val[i][l]*bas_val[j][l] +
innerProduct(bas_grad[i][l], bas_grad[j][l]));
}
ele_rhs(i) += JxW*f_val*bas_val[i][l];
}
}
/**
* 此处将单元矩阵和单元载荷先计算好，然后向全局的矩阵和载荷向量上
* 集中，可以提高效率。
*/
std::vector<int> indices(n_ele_dof);
for (u_int i = 0;i < n_ele_dof;++ i) {
indices[i] = global_index(ele_dof[i]);
}
MatSetValues(A, n_ele_dof, &indices[0], n_ele_dof, &indices[0], &ele_mat(0,0), ADD_VALUES);
VecSetValues(b, n_ele_dof, &indices[0], &ele_rhs(0), ADD_VALUES);
}
MatAssemblyBegin(A, MAT_FINAL_ASSEMBLY);
MatAssemblyEnd(A, MAT_FINAL_ASSEMBLY);
VecAssemblyBegin(b);
VecAssemblyEnd(b);
std::cout << "OK!" << std::endl;
/// 加上狄氏边值条件
std::cout << "Applying the Dirichlet boundary condition ... " << std::flush;
u_int n_bnd_dof = 0; /// 首先清点边界上自由度的个数
for (u_int i = 0;i < fem_space.n_dof();++ i) {
if (fem_space.dofBoundaryMark(i) > 0) {
/// 如果不是在主几何体上就不做
if (! global_index.is_dof_on_primary_geometry(i)) continue;
n_bnd_dof += 1;
}
}
/// 准备空间存储边界上全局标号、自变量和右端项
std::vector<int> bnd_idx(n_bnd_dof);
std::vector<double> rhs_entry(n_bnd_dof);
/// 对自由度做循环
for (u_int i = 0, j = 0;i < fem_space.n_dof();++ i) {
if (fem_space.dofBoundaryMark(i) > 0) { /// 边界上的自由度?
/// 如果不是在主几何体上就不做
if (! global_index.is_dof_on_primary_geometry(i)) continue;
bnd_idx[j] = global_index(i); /// 行的全局标号
/// 计算并记下自变量和右端项，假设自由度值为插值量
double u_b_val = _u_b_(fem_space.dofInfo(i).interp_point);
rhs_entry[j] = u_b_val;
j += 1;
}
}
/// 将矩阵修改为对角元 1.0，其它元素为零的状态
/// MatSetOption(A, MAT_KEEP_ZEROED_ROWS);
MatZeroRows(A, n_bnd_dof, &bnd_idx[0], 1.0);
/// 修改右端项为相应点的边值
Vec rhs_bnd;
VecCreateSeqWithArray(PETSC_COMM_SELF, n_bnd_dof, &rhs_entry[0], &rhs_bnd);
IS is_bnd;
ISCreateGeneralWithArray(PETSC_COMM_WORLD, n_bnd_dof, &bnd_idx[0], &is_bnd);
VecScatter bnd_scatter;
VecScatterCreate(rhs_bnd, PETSC_NULL, b, is_bnd, &bnd_scatter);
VecScatterBegin(bnd_scatter, rhs_bnd, b, INSERT_VALUES, SCATTER_FORWARD);
VecScatterEnd(bnd_scatter, rhs_bnd, b, INSERT_VALUES, SCATTER_FORWARD);
VecDestroy(rhs_bnd);
ISDestroy(is_bnd);
VecScatterDestroy(bnd_scatter);
std::cout << "OK!" << std::endl;
VecDuplicate(b, &x);
KSP solver;
KSPCreate(PETSC_COMM_WORLD, &solver);
KSPSetOperators(solver, A, A, SAME_NONZERO_PATTERN);
KSPSetType(solver, KSPGMRES);
KSPSetFromOptions(solver);
KSPSolve(solver, b, x);
if (forest.rank() == 0) {
KSPConvergedReason reason;
KSPGetConvergedReason(solver,&reason);
if (reason == KSP_DIVERGED_INDEFINITE_PC) {
printf("\nDivergence because of indefinite preconditioner;\n");
printf("Run the executable again but with -pc_ilu_shift option.\n");
} else if (reason<0) {
printf("\nOther kind of divergence: this should not happen.\n");
} else {
PetscInt its;
KSPGetIterationNumber(solver,&its);
printf("\nConvergence in %d iterations.\n",(int)its);
}
printf("\n");
}
MatDestroy(A);
VecDestroy(b);
KSPDestroy(solver);
FEMFunction<double,DIM> u_h(fem_space);
Vec X;
VecCreateSeqWithArray(PETSC_COMM_SELF, global_index.n_local_dof(), &u_h(0), &X);
std::vector<int> primary_idx(global_index.n_primary_dof());
global_index.build_primary_index(&primary_idx[0]);
IS is;
ISCreateGeneralWithArray(PETSC_COMM_WORLD, global_index.n_local_dof(),
&global_index(0), &is);
VecScatter scatter;
VecScatterCreate(x, is, X, PETSC_NULL, &scatter);
VecScatterBegin(scatter, x, X, INSERT_VALUES, SCATTER_FORWARD);
VecScatterEnd(scatter, x, X, INSERT_VALUES, SCATTER_FORWARD);
VecDestroy(x);
VecDestroy(X);
VecScatterDestroy(scatter);
ISDestroy(is);
char filename[1024];
sprintf(filename, "u_h%d.dx", forest.rank());
u_h.writeOpenDXData(filename);
PetscFinalize();
return 0;
}
