GaussianElimination.hpp

Go to the documentation of this file.

#ifndef SPIDAR_GAUSSIAN_ELIMINATION_HPP
#define SPIDAR_GAUSSIAN_ELIMINATION_HPP

#include <cmath>

namespace Spidar
{
namespace Math
{
class GaussianElimination
{
public:   
    template <class MatrixType, class VectorType>
    static void solve(MatrixType& a, VectorType& b, VectorType& x);

}; // end of class GaussianElimination.

template <class MatrixType, class VectorType>
void GaussianElimination::solve(MatrixType& a, VectorType& b, VectorType& x)
{    
    static_assert(MatrixType::row==MatrixType::column, "row != column");
    static_assert(MatrixType::row==VectorType::size, "matrix size != vector size");
    
    const size_t size = VectorType::size;

    // 前進消去 (forward elimination)
    for (size_t k=0; k<size-1; ++k)
    {   
        for (size_t j=k+1; j<size; ++j)
        {
            a[k][j] /= a[k][k];
        }
        b[k] /= a[k][k];
            
        for (size_t i=k+1; i<size; ++i)
        {
            for (size_t j=k+1; j<size; ++j)
            {
                a[i][j] -= a[i][k] * a[k][j];
            }
            b[i] -= a[i][k] * b[k];
        } 
    } 

    // 後退代入 (backward substitution)
    x[size-1] = b[size-1] / a[size-1][size-1];

    for (size_t k=size-2; k<size; --k)
    {
        x[k] = b[k];
        
        for (size_t j=k+1; j<size; ++j)
        {
            x[k] -= a[k][j] * x[j];
        }
    }
}

} // end of namespace Math.
} // end of namespace Spidar.

#endif // SPIDAR_GAUSSIAN_ELIMINATION_HPP

// end of file.