godot/thirdparty/thekla_atlas/nvmath/Solver.cpp
Hein-Pieter van Braam bf05309af7 Import thekla_atlas
As requested by reduz, an import of thekla_atlas into thirdparty/
2017-12-08 15:47:15 +01:00

745 lines
17 KiB
C++

// 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