GteGaussianElimination.h

Go to the documentation of this file.

// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2017
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.0 (2016/06/19)

#pragma once

#include <LowLevel/GteLexicoArray2.h>
#include <LowLevel/GteLogger.h>
#include <LowLevel/GteWrapper.h>
#include <cmath>
#include <cstring>
#include <vector>

// The input matrix M must be NxN.  The storage convention for element lookup
// is determined by GTE_USE_ROW_MAJOR or GTE_USE_COL_MAJOR, whichever is
// active.  If you want the inverse of M, pass a nonnull pointer inverseM;
// this matrix must also be NxN and use the same storage convention as M.  If
// you do not want the inverse of M, pass a nullptr for inverseM.  If you want
// to solve M*X = B for X, where X and B are Nx1, pass nonnull pointers for B
// and X.  If you want to solve M*Y = C for Y, where X and C are NxK, pass
// nonnull pointers for C and Y and pass K to numCols.  In all cases, pass
// N to numRows.

namespace gte
{

template <typename Real>
class GaussianElimination
{
public:
    bool operator()(int numRows,
        Real const* M, Real* inverseM, Real& determinant,
        Real const* B, Real* X,
        Real const* C, int numCols, Real* Y) const;

private:
    // Support for copying source to target or to set target to zero.  If
    // source is nullptr, then target is set to zero; otherwise source is
    // copied to target.  This function hides the type traits used to
    // determine whether Real is native floating-point or otherwise (such
    // as BSNumber or BSRational).
    void Set(int numElements, Real const* source, Real* target) const;
};


template <typename Real>
bool GaussianElimination<Real>::operator()(int numRows, Real const* M,
    Real* inverseM, Real& determinant, Real const* B, Real* X, Real const* C,
    int numCols, Real* Y) const
{
    if (numRows <= 0 || !M
        || ((B != nullptr) != (X != nullptr))
        || ((C != nullptr) != (Y != nullptr))
        || (C != nullptr && numCols < 1))
    {
        LogError("Invalid input.");
        return false;
    }

    int numElements = numRows * numRows;
    bool wantInverse = (inverseM != nullptr);
    std::vector<Real> localInverseM;
    if (!wantInverse)
    {
        localInverseM.resize(numElements);
        inverseM = localInverseM.data();
    }
    Set(numElements, M, inverseM);

    if (B)
    {
        Set(numRows, B, X);
    }

    if (C)
    {
        Set(numRows * numCols, C, Y);
    }

#if defined(GTE_USE_ROW_MAJOR)
    LexicoArray2<true, Real> matInvM(numRows, numRows, inverseM);
    LexicoArray2<true, Real> matY(numRows, numCols, Y);
#else
    LexicoArray2<false, Real> matInvM(numRows, numRows, inverseM);
    LexicoArray2<false, Real> matY(numRows, numCols, Y);
#endif

    std::vector<int> colIndex(numRows), rowIndex(numRows), pivoted(numRows);
    std::fill(pivoted.begin(), pivoted.end(), 0);

    Real const zero = (Real)0;
    Real const one = (Real)1;
    bool odd = false;
    determinant = one;

    // Elimination by full pivoting.
    int i1, i2, row = 0, col = 0;
    for (int i0 = 0; i0 < numRows; ++i0)
    {
        // Search matrix (excluding pivoted rows) for maximum absolute entry.
        Real maxValue = zero;
        for (i1 = 0; i1 < numRows; ++i1)
        {
            if (!pivoted[i1])
            {
                for (i2 = 0; i2 < numRows; ++i2)
                {
                    if (!pivoted[i2])
                    {
                        Real absValue = std::abs(matInvM(i1, i2));
                        if (absValue > maxValue)
                        {
                            maxValue = absValue;
                            row = i1;
                            col = i2;
                        }
                    }
                }
            }
        }

        if (maxValue == zero)
        {
            // The matrix is not invertible.
            if (wantInverse)
            {
                Set(numElements, nullptr, inverseM);
            }
            determinant = zero;

            if (B)
            {
                Set(numRows, nullptr, X);
            }

            if (C)
            {
                Set(numRows * numCols, nullptr, Y);
            }
            return false;
        }

        pivoted[col] = true;

        // Swap rows so that the pivot entry is in row 'col'.
        if (row != col)
        {
            odd = !odd;
            for (int i = 0; i < numRows; ++i)
            {
                std::swap(matInvM(row, i), matInvM(col, i));
            }

            if (B)
            {
                std::swap(X[row], X[col]);
            }

            if (C)
            {
                for (int i = 0; i < numCols; ++i)
                {
                    std::swap(matY(row, i), matY(col, i));
                }
            }
        }

        // Keep track of the permutations of the rows.
        rowIndex[i0] = row;
        colIndex[i0] = col;

        // Scale the row so that the pivot entry is 1.
        Real diagonal = matInvM(col, col);
        determinant *= diagonal;
        Real inv = one / diagonal;
        matInvM(col, col) = one;
        for (i2 = 0; i2 < numRows; ++i2)
        {
            matInvM(col, i2) *= inv;
        }

        if (B)
        {
            X[col] *= inv;
        }

        if (C)
        {
            for (i2 = 0; i2 < numCols; ++i2)
            {
                matY(col, i2) *= inv;
            }
        }

        // Zero out the pivot column locations in the other rows.
        for (i1 = 0; i1 < numRows; ++i1)
        {
            if (i1 != col)
            {
                Real save = matInvM(i1, col);
                matInvM(i1, col) = zero;
                for (i2 = 0; i2 < numRows; ++i2)
                {
                    matInvM(i1, i2) -= matInvM(col, i2) * save;
                }

                if (B)
                {
                    X[i1] -= X[col] * save;
                }

                if (C)
                {
                    for (i2 = 0; i2 < numCols; ++i2)
                    {
                        matY(i1, i2) -= matY(col, i2) * save;
                    }
                }
            }
        }
    }

    if (wantInverse)
    {
        // Reorder rows to undo any permutations in Gaussian elimination.
        for (i1 = numRows - 1; i1 >= 0; --i1)
        {
            if (rowIndex[i1] != colIndex[i1])
            {
                for (i2 = 0; i2 < numRows; ++i2)
                {
                    std::swap(matInvM(i2, rowIndex[i1]),
                        matInvM(i2, colIndex[i1]));
                }
            }
        }
    }

    if (odd)
    {
        determinant = -determinant;
    }

    return true;
}

template <typename Real>
void GaussianElimination<Real>::Set(int numElements, Real const* source,
    Real* target) const
{
    if (std::is_floating_point<Real>() == std::true_type())
    {
        // Fast set/copy for native floating-point.
        size_t numBytes = numElements * sizeof(Real);
        if (source)
        {
            Memcpy(target, source, numBytes);
        }
        else
        {
            memset(target, 0, numBytes);
        }
    }
    else
    {
        // The inputs are not std containers, so ensure assignment works
        // correctly.
        if (source)
        {
            for (int i = 0; i < numElements; ++i)
            {
                target[i] = source[i];
            }
        }
        else
        {
            Real const zero = (Real)0;
            for (int i = 0; i < numElements; ++i)
            {
                target[i] = zero;
            }
        }
    }
}


}