// This code is in the public domain -- castanyo@yahoo.es #include "Solver.h" #include "Sparse.h" #include "nvcore/Array.inl" using namespace nv; namespace { class Preconditioner { public: // Virtual dtor. virtual ~Preconditioner() { } // Apply preconditioning step. virtual void apply(const FullVector & x, FullVector & y) const = 0; }; // Jacobi preconditioner. class JacobiPreconditioner : public Preconditioner { public: JacobiPreconditioner(const SparseMatrix & M, bool symmetric) : m_inverseDiagonal(M.width()) { nvCheck(M.isSquare()); for(uint x = 0; x < M.width(); x++) { float elem = M.getCoefficient(x, x); //nvDebugCheck( elem != 0.0f ); // This can be zero in the presence of zero area triangles. if (symmetric) { m_inverseDiagonal[x] = (elem != 0) ? 1.0f / sqrtf(fabsf(elem)) : 1.0f; } else { m_inverseDiagonal[x] = (elem != 0) ? 1.0f / elem : 1.0f; } } } void apply(const FullVector & x, FullVector & y) const { nvDebugCheck(x.dimension() == m_inverseDiagonal.dimension()); nvDebugCheck(y.dimension() == m_inverseDiagonal.dimension()); // @@ Wrap vector component-wise product into a separate function. const uint D = x.dimension(); for (uint i = 0; i < D; i++) { y[i] = m_inverseDiagonal[i] * x[i]; } } private: FullVector m_inverseDiagonal; }; } // namespace static bool ConjugateGradientSolver(const SparseMatrix & A, const FullVector & b, FullVector & x, float epsilon); static bool ConjugateGradientSolver(const Preconditioner & preconditioner, const SparseMatrix & A, const FullVector & b, FullVector & x, float epsilon); // Solve the symmetric system: At�A�x = At�b bool nv::LeastSquaresSolver(const SparseMatrix & A, const FullVector & b, FullVector & x, float epsilon/*1e-5f*/) { nvDebugCheck(A.width() == x.dimension()); nvDebugCheck(A.height() == b.dimension()); nvDebugCheck(A.height() >= A.width()); // @@ If height == width we could solve it directly... const uint D = A.width(); SparseMatrix At(A.height(), A.width()); transpose(A, At); FullVector Atb(D); //mult(Transposed, A, b, Atb); mult(At, b, Atb); SparseMatrix AtA(D); //mult(Transposed, A, NoTransposed, A, AtA); mult(At, A, AtA); return SymmetricSolver(AtA, Atb, x, epsilon); } // See section 10.4.3 in: Mesh Parameterization: Theory and Practice, Siggraph Course Notes, August 2007 bool nv::LeastSquaresSolver(const SparseMatrix & A, const FullVector & b, FullVector & x, const uint * lockedParameters, uint lockedCount, float epsilon/*= 1e-5f*/) { nvDebugCheck(A.width() == x.dimension()); nvDebugCheck(A.height() == b.dimension()); nvDebugCheck(A.height() >= A.width() - lockedCount); // @@ This is not the most efficient way of building a system with reduced degrees of freedom. It would be faster to do it on the fly. const uint D = A.width() - lockedCount; nvDebugCheck(D > 0); // Compute: b - Al * xl FullVector b_Alxl(b); for (uint y = 0; y < A.height(); y++) { const uint count = A.getRow(y).count(); for (uint e = 0; e < count; e++) { uint column = A.getRow(y)[e].x; bool isFree = true; for (uint i = 0; i < lockedCount; i++) { isFree &= (lockedParameters[i] != column); } if (!isFree) { b_Alxl[y] -= x[column] * A.getRow(y)[e].v; } } } // Remove locked columns from A. SparseMatrix Af(D, A.height()); for (uint y = 0; y < A.height(); y++) { const uint count = A.getRow(y).count(); for (uint e = 0; e < count; e++) { uint column = A.getRow(y)[e].x; uint ix = column; bool isFree = true; for (uint i = 0; i < lockedCount; i++) { isFree &= (lockedParameters[i] != column); if (column > lockedParameters[i]) ix--; // shift columns } if (isFree) { Af.setCoefficient(ix, y, A.getRow(y)[e].v); } } } // Remove elements from x FullVector xf(D); for (uint i = 0, j = 0; i < A.width(); i++) { bool isFree = true; for (uint l = 0; l < lockedCount; l++) { isFree &= (lockedParameters[l] != i); } if (isFree) { xf[j++] = x[i]; } } // Solve reduced system. bool result = LeastSquaresSolver(Af, b_Alxl, xf, epsilon); // Copy results back to x. for (uint i = 0, j = 0; i < A.width(); i++) { bool isFree = true; for (uint l = 0; l < lockedCount; l++) { isFree &= (lockedParameters[l] != i); } if (isFree) { x[i] = xf[j++]; } } return result; } bool nv::SymmetricSolver(const SparseMatrix & A, const FullVector & b, FullVector & x, float epsilon/*1e-5f*/) { nvDebugCheck(A.height() == A.width()); nvDebugCheck(A.height() == b.dimension()); nvDebugCheck(b.dimension() == x.dimension()); JacobiPreconditioner jacobi(A, true); return ConjugateGradientSolver(jacobi, A, b, x, epsilon); //return ConjugateGradientSolver(A, b, x, epsilon); } /** * Compute the solution of the sparse linear system Ab=x using the Conjugate * Gradient method. * * Solving sparse linear systems: * (1) A�x = b * * The conjugate gradient algorithm solves (1) only in the case that A is * symmetric and positive definite. It is based on the idea of minimizing the * function * * (2) f(x) = 1/2�x�A�x - b�x * * This function is minimized when its gradient * * (3) df = A�x - b * * is zero, which is equivalent to (1). The minimization is carried out by * generating a succession of search directions p.k and improved minimizers x.k. * At each stage a quantity alfa.k is found that minimizes f(x.k + alfa.k�p.k), * and x.k+1 is set equal to the new point x.k + alfa.k�p.k. The p.k and x.k are * built up in such a way that x.k+1 is also the minimizer of f over the whole * vector space of directions already taken, {p.1, p.2, . . . , p.k}. After N * iterations you arrive at the minimizer over the entire vector space, i.e., the * solution to (1). * * For a really good explanation of the method see: * * "An Introduction to the Conjugate Gradient Method Without the Agonizing Pain", * Jonhathan Richard Shewchuk. * **/ /*static*/ bool ConjugateGradientSolver(const SparseMatrix & A, const FullVector & b, FullVector & x, float epsilon) { nvDebugCheck( A.isSquare() ); nvDebugCheck( A.width() == b.dimension() ); nvDebugCheck( A.width() == x.dimension() ); int i = 0; const int D = A.width(); const int i_max = 4 * D; // Convergence should be linear, but in some cases, it's not. FullVector r(D); // residual FullVector p(D); // search direction FullVector q(D); // float delta_0; float delta_old; float delta_new; float alpha; float beta; // r = b - A�x; copy(b, r); sgemv(-1, A, x, 1, r); // p = r; copy(r, p); delta_new = dot( r, r ); delta_0 = delta_new; while (i < i_max && delta_new > epsilon*epsilon*delta_0) { i++; // q = A�p mult(A, p, q); // alpha = delta_new / p�q alpha = delta_new / dot( p, q ); // x = alfa�p + x saxpy(alpha, p, x); if ((i & 31) == 0) // recompute r after 32 steps { // r = b - A�x copy(b, r); sgemv(-1, A, x, 1, r); } else { // r = r - alpha�q saxpy(-alpha, q, r); } delta_old = delta_new; delta_new = dot( r, r ); beta = delta_new / delta_old; // p = beta�p + r scal(beta, p); saxpy(1, r, p); } return delta_new <= epsilon*epsilon*delta_0; } // Conjugate gradient with preconditioner. /*static*/ bool ConjugateGradientSolver(const Preconditioner & preconditioner, const SparseMatrix & A, const FullVector & b, FullVector & x, float epsilon) { nvDebugCheck( A.isSquare() ); nvDebugCheck( A.width() == b.dimension() ); nvDebugCheck( A.width() == x.dimension() ); int i = 0; const int D = A.width(); const int i_max = 4 * D; // Convergence should be linear, but in some cases, it's not. FullVector r(D); // residual FullVector p(D); // search direction FullVector q(D); // FullVector s(D); // preconditioned float delta_0; float delta_old; float delta_new; float alpha; float beta; // r = b - A�x copy(b, r); sgemv(-1, A, x, 1, r); // p = M^-1 � r preconditioner.apply(r, p); //copy(r, p); delta_new = dot(r, p); delta_0 = delta_new; while (i < i_max && delta_new > epsilon*epsilon*delta_0) { i++; // q = A�p mult(A, p, q); // alpha = delta_new / p�q alpha = delta_new / dot(p, q); // x = alfa�p + x saxpy(alpha, p, x); if ((i & 31) == 0) // recompute r after 32 steps { // r = b - A�x copy(b, r); sgemv(-1, A, x, 1, r); } else { // r = r - alfa�q saxpy(-alpha, q, r); } // s = M^-1 � r preconditioner.apply(r, s); //copy(r, s); delta_old = delta_new; delta_new = dot( r, s ); beta = delta_new / delta_old; // p = s + beta�p scal(beta, p); saxpy(1, s, p); } return delta_new <= epsilon*epsilon*delta_0; } #if 0 // Nonsymmetric solvers /** Bi-conjugate gradient method. */ MATHLIB_API int BiConjugateGradientSolve( const SparseMatrix &A, const DenseVector &b, DenseVector &x, float epsilon ) { piDebugCheck( A.IsSquare() ); piDebugCheck( A.Width() == b.Dim() ); piDebugCheck( A.Width() == x.Dim() ); int i = 0; const int D = A.Width(); const int i_max = 4 * D; float resid; float rho_1 = 0; float rho_2 = 0; float alpha; float beta; DenseVector r(D); DenseVector rtilde(D); DenseVector p(D); DenseVector ptilde(D); DenseVector q(D); DenseVector qtilde(D); DenseVector tmp(D); // temporal vector. // r = b - A�x; A.Product( x, tmp ); r.Sub( b, tmp ); // rtilde = r rtilde.Set( r ); // p = r; p.Set( r ); // ptilde = rtilde ptilde.Set( rtilde ); float normb = b.Norm(); if( normb == 0.0 ) normb = 1; // test convergence resid = r.Norm() / normb; if( resid < epsilon ) { // method converges? return 0; } while( i < i_max ) { i++; rho_1 = DenseVectorDotProduct( r, rtilde ); if( rho_1 == 0 ) { // method fails. return -i; } if (i == 1) { p.Set( r ); ptilde.Set( rtilde ); } else { beta = rho_1 / rho_2; // p = r + beta * p; p.Mad( r, p, beta ); // ptilde = ztilde + beta * ptilde; ptilde.Mad( rtilde, ptilde, beta ); } // q = A * p; A.Product( p, q ); // qtilde = A^t * ptilde; A.TransProduct( ptilde, qtilde ); alpha = rho_1 / DenseVectorDotProduct( ptilde, q ); // x += alpha * p; x.Mad( x, p, alpha ); // r -= alpha * q; r.Mad( r, q, -alpha ); // rtilde -= alpha * qtilde; rtilde.Mad( rtilde, qtilde, -alpha ); rho_2 = rho_1; // test convergence resid = r.Norm() / normb; if( resid < epsilon ) { // method converges return i; } } return i; } /** Bi-conjugate gradient stabilized method. */ int BiCGSTABSolve( const SparseMatrix &A, const DenseVector &b, DenseVector &x, float epsilon ) { piDebugCheck( A.IsSquare() ); piDebugCheck( A.Width() == b.Dim() ); piDebugCheck( A.Width() == x.Dim() ); int i = 0; const int D = A.Width(); const int i_max = 2 * D; float resid; float rho_1 = 0; float rho_2 = 0; float alpha = 0; float beta = 0; float omega = 0; DenseVector p(D); DenseVector phat(D); DenseVector s(D); DenseVector shat(D); DenseVector t(D); DenseVector v(D); DenseVector r(D); DenseVector rtilde(D); DenseVector tmp(D); // r = b - A�x; A.Product( x, tmp ); r.Sub( b, tmp ); // rtilde = r rtilde.Set( r ); float normb = b.Norm(); if( normb == 0.0 ) normb = 1; // test convergence resid = r.Norm() / normb; if( resid < epsilon ) { // method converges? return 0; } while( i<i_max ) { i++; rho_1 = DenseVectorDotProduct( rtilde, r ); if( rho_1 == 0 ) { // method fails return -i; } if( i == 1 ) { p.Set( r ); } else { beta = (rho_1 / rho_2) * (alpha / omega); // p = r + beta * (p - omega * v); p.Mad( p, v, -omega ); p.Mad( r, p, beta ); } //phat = M.solve(p); phat.Set( p ); //Precond( &phat, p ); //v = A * phat; A.Product( phat, v ); alpha = rho_1 / DenseVectorDotProduct( rtilde, v ); // s = r - alpha * v; s.Mad( r, v, -alpha ); resid = s.Norm() / normb; if( resid < epsilon ) { // x += alpha * phat; x.Mad( x, phat, alpha ); return i; } //shat = M.solve(s); shat.Set( s ); //Precond( &shat, s ); //t = A * shat; A.Product( shat, t ); omega = DenseVectorDotProduct( t, s ) / DenseVectorDotProduct( t, t ); // x += alpha * phat + omega * shat; x.Mad( x, shat, omega ); x.Mad( x, phat, alpha ); //r = s - omega * t; r.Mad( s, t, -omega ); rho_2 = rho_1; resid = r.Norm() / normb; if( resid < epsilon ) { return i; } if( omega == 0 ) { return -i; // ??? } } return i; } /** Bi-conjugate gradient stabilized method. */ int BiCGSTABPrecondSolve( const SparseMatrix &A, const DenseVector &b, DenseVector &x, const IPreconditioner &M, float epsilon ) { piDebugCheck( A.IsSquare() ); piDebugCheck( A.Width() == b.Dim() ); piDebugCheck( A.Width() == x.Dim() ); int i = 0; const int D = A.Width(); const int i_max = D; // const int i_max = 1000; float resid; float rho_1 = 0; float rho_2 = 0; float alpha = 0; float beta = 0; float omega = 0; DenseVector p(D); DenseVector phat(D); DenseVector s(D); DenseVector shat(D); DenseVector t(D); DenseVector v(D); DenseVector r(D); DenseVector rtilde(D); DenseVector tmp(D); // r = b - A�x; A.Product( x, tmp ); r.Sub( b, tmp ); // rtilde = r rtilde.Set( r ); float normb = b.Norm(); if( normb == 0.0 ) normb = 1; // test convergence resid = r.Norm() / normb; if( resid < epsilon ) { // method converges? return 0; } while( i<i_max ) { i++; rho_1 = DenseVectorDotProduct( rtilde, r ); if( rho_1 == 0 ) { // method fails return -i; } if( i == 1 ) { p.Set( r ); } else { beta = (rho_1 / rho_2) * (alpha / omega); // p = r + beta * (p - omega * v); p.Mad( p, v, -omega ); p.Mad( r, p, beta ); } //phat = M.solve(p); //phat.Set( p ); M.Precond( &phat, p ); //v = A * phat; A.Product( phat, v ); alpha = rho_1 / DenseVectorDotProduct( rtilde, v ); // s = r - alpha * v; s.Mad( r, v, -alpha ); resid = s.Norm() / normb; //printf( "--- Iteration %d: residual = %f\n", i, resid ); if( resid < epsilon ) { // x += alpha * phat; x.Mad( x, phat, alpha ); return i; } //shat = M.solve(s); //shat.Set( s ); M.Precond( &shat, s ); //t = A * shat; A.Product( shat, t ); omega = DenseVectorDotProduct( t, s ) / DenseVectorDotProduct( t, t ); // x += alpha * phat + omega * shat; x.Mad( x, shat, omega ); x.Mad( x, phat, alpha ); //r = s - omega * t; r.Mad( s, t, -omega ); rho_2 = rho_1; resid = r.Norm() / normb; if( resid < epsilon ) { return i; } if( omega == 0 ) { return -i; // ??? } } return i; } #endif