diff options
author | over0219 <over0219@umn.edu> | 2020-06-30 05:28:37 +0300 |
---|---|---|
committer | over0219 <over0219@umn.edu> | 2020-06-30 05:28:37 +0300 |
commit | a0310586dfb35cfde3674fb94ac42a9a3ebb6ea1 (patch) | |
tree | abc70b65cb54b950bfeb5bc4cf201fde7613079d | |
parent | fd7bcb209aa7e888a32f980a01d17f79c59b0c51 (diff) |
gauss seidel solver
-rw-r--r-- | extern/softbody/CMakeLists.txt | 2 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_bvh.cpp | 2 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_linsolve.cpp | 147 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_linsolve.h | 36 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_solver.cpp | 2 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_types.h | 8 |
6 files changed, 196 insertions, 1 deletions
diff --git a/extern/softbody/CMakeLists.txt b/extern/softbody/CMakeLists.txt index 3e006f74ae3..b8f036feebb 100644 --- a/extern/softbody/CMakeLists.txt +++ b/extern/softbody/CMakeLists.txt @@ -39,6 +39,8 @@ set(SRC src/admmpd_embeddedmesh.cpp src/admmpd_energy.h src/admmpd_energy.cpp + src/admmpd_linsolve.h + src/admmpd_linsolve.cpp src/admmpd_math.h src/admmpd_math.cpp src/admmpd_solver.h diff --git a/extern/softbody/src/admmpd_bvh.cpp b/extern/softbody/src/admmpd_bvh.cpp index 97c39271199..fffc8c33a74 100644 --- a/extern/softbody/src/admmpd_bvh.cpp +++ b/extern/softbody/src/admmpd_bvh.cpp @@ -23,7 +23,7 @@ void AABBTree<T,DIM>::init(const std::vector<AABB> &leaves) if (np==0) return; std::vector<int> queue(np); - std::iota(queue.begin(), queue.end(), 0); + std::iota(queue.begin(), queue.end(), 0); create_children(root.get(), queue, leaves); } diff --git a/extern/softbody/src/admmpd_linsolve.cpp b/extern/softbody/src/admmpd_linsolve.cpp new file mode 100644 index 00000000000..5b3a62dc7bb --- /dev/null +++ b/extern/softbody/src/admmpd_linsolve.cpp @@ -0,0 +1,147 @@ +// Copyright Matt Overby 2020. +// Distributed under the MIT License. + +#include "admmpd_linsolve.h" +#include <numeric> +#include <iostream> +#include <unordered_set> +#include "BLI_assert.h" + +namespace admmpd { +using namespace Eigen; + +void GaussSeidel::solve( + const Options *options, + SolverData *data) +{ + init_solve(options,data); + std::vector<std::vector<int> > *colors; + //if (data->gsdata.KtK.nonZeros()==0) + colors = &data->gsdata.A_colors; + //else... + + double omega = 1.0; // over relaxation + int n_colors = colors->size(); + + // Outer iteration loop + int iter = 0; + for (; iter < options->max_gs_iters; ++iter) + { + for (int color=0; color<n_colors; ++color) + { + const std::vector<int> &inds = colors->at(color); + int n_inds = inds.size(); + for (int i=0; i<n_inds; ++i) + { + int idx = inds[i]; + + // Special case pins TODO + // We can skip the usual Gauss-Seidel update + // if (is_pinned[idx]) ... + + RowSparseMatrix<double>::InnerIterator rit(data->A,idx); + Vector3d LUx(0,0,0); + Vector3d inv_aii(0,0,0); + for (; rit; ++rit) + { + int r = rit.row(); + int c = rit.col(); + double v = rit.value(); + if (v==0.0) + continue; + + if (r==c) // Diagonal + { + inv_aii.array() = 1.0/v; + continue; + } + Vector3d xj = data->x.row(c); + LUx += v*xj; + } + + // Update x + Vector3d bi = data->b.row(idx); + Vector3d xi = data->x.row(idx); + Vector3d xi_new = (bi-LUx); + + for (int j=0; j<3; ++j) + xi_new[j] *= inv_aii[j]; + data->x.row(idx) = xi*(1.0-omega) + xi_new*omega; + data->gsdata.last_dx.row(idx) = data->x.row(idx)-xi.transpose(); + + // TODO + // We can also apply constraints here, like + // checking against Collision::floor_z + if (data->x(idx,2)<0) + data->x(idx,2)=0; + + } // end loop inds + } // end loop colors + + // TODO check exit condition + + } // end loop GS iters + +} // end solve with constraints + +void GaussSeidel::init_solve( + const Options *options, + SolverData *data) +{ + BLI_assert(options != nullptr); + BLI_assert(data != nullptr); + int nx = data->x.rows(); + BLI_assert(nx>0); + BLI_assert(data->x.cols()==3); + data->b.noalias() = data->M_xbar + data->DtW2*(data->z-data->u); + BLI_assert(data->b.rows()==nx); + BLI_assert(data->b.cols()==data->x.cols()); + + if (data->gsdata.last_dx.rows() != nx) + { + data->gsdata.last_dx.resize(nx,3); + data->gsdata.last_dx.setZero(); + } + + // Do we need to color the default colorings? + if( data->gsdata.A_colors.size() == 0 ) + compute_colors(&data->A,nullptr,data->gsdata.A_colors); + + // TODO: Eventually we'll replace KtK with the full-dof matrix. + // For now use z and test collisions against ground plane. + bool has_constraints = data->K[2].nonZeros()>0; + data->gsdata.KtK = data->K[2].transpose()*data->K[2]; + + if (false)//(has_constraints) + { + (void)(has_constraints); + // TODO color A + KtK + } + +} // end init solve + +void GaussSeidel::compute_colors( + const RowSparseMatrix<double> *A, + const RowSparseMatrix<double> *KtK, // if null, just A + std::vector<std::vector<int> > &colors) +{ + BLI_assert(A != nullptr); + if (KtK != nullptr) + { + throw std::runtime_error("**GaussSeidel::compute_colors TODO: KtK"); + } + + colors.clear(); + int nx = A->rows(); + + { // DEBUGGING + colors.resize(nx,std::vector<int>()); + for (int i=0; i<nx; ++i) + { + colors[i].emplace_back(i); + } + } + +} // end compute colors + +} // namespace admmpd diff --git a/extern/softbody/src/admmpd_linsolve.h b/extern/softbody/src/admmpd_linsolve.h new file mode 100644 index 00000000000..b1dd9ae8f4e --- /dev/null +++ b/extern/softbody/src/admmpd_linsolve.h @@ -0,0 +1,36 @@ +// Copyright Matt Overby 2020. +// Distributed under the MIT License. + +#ifndef ADMMPD_LINSOLVE_H_ +#define ADMMPD_LINSOLVE_H_ + +#include "admmpd_types.h" + +namespace admmpd { + +class GaussSeidel { +public: + // Solves (A + KtK) x = (b + Ktl) + // x and b passed as separate variables + // for debugging/testing purposes. + void solve( + const Options *options, + SolverData *data); + +protected: + // Allocates data, computes colors + void init_solve( + const Options *options, + SolverData *data); + + // Computes colors of A + KtK + void compute_colors( + const RowSparseMatrix<double> *A, + const RowSparseMatrix<double> *KtK, // if null, just A + std::vector<std::vector<int> > &colors); + +}; + +} // namespace admmpd + +#endif // ADMMPD_LINSOLVE_H_ diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp index ef1c1b50ca4..944ebf7826e 100644 --- a/extern/softbody/src/admmpd_solver.cpp +++ b/extern/softbody/src/admmpd_solver.cpp @@ -4,6 +4,7 @@ #include "admmpd_solver.h" #include "admmpd_energy.h" #include "admmpd_collision.h" +#include "admmpd_linsolve.h" #include <Eigen/Geometry> #include <Eigen/Sparse> @@ -80,6 +81,7 @@ int Solver::solve( // Solve Ax=b s.t. Kx=l solve_conjugate_gradients(options,data); + //GaussSeidel().solve(options,data); } // end solver iters diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h index 13edb93c3da..46765150e1c 100644 --- a/extern/softbody/src/admmpd_types.h +++ b/extern/softbody/src/admmpd_types.h @@ -18,6 +18,7 @@ struct Options { double timestep_s; // TODO: Figure out delta time from blender api int max_admm_iters; int max_cg_iters; + int max_gs_iters; double mult_k; // stiffness multiplier for constraints double min_res; // min residual for CG solver double youngs; // Young's modulus // TODO variable per-tet @@ -27,6 +28,7 @@ struct Options { timestep_s(1.0/24.0), max_admm_iters(50), max_cg_iters(10), + max_gs_iters(30), mult_k(1), min_res(1e-6), youngs(1000000), @@ -82,6 +84,12 @@ struct SolverData { Eigen::MatrixXd p; Eigen::MatrixXd Ap; // A * p } cgdata; + struct GSData { // Temporaries used in Gauss-Seidel + RowSparseMatrix<double> KtK; // k * Kt K, different dim than A! + Eigen::MatrixXd last_dx; // last GS iter change in x + std::vector<std::vector<int> > A_colors; // colors of just A matrix + std::vector<std::vector<int> > A_KtK_colors; // colors of just A+KtK + } gsdata; // Set in append_energies: std::vector<Eigen::Vector2i> indices; // per-energy index into D (row, num rows) std::vector<double> rest_volumes; // per-energy rest volume |