dgSPDMatrix.h

/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/

#ifndef __dgSPDMatrix__
#define __dgSPDMatrix__

#include "dgStdafx.h"
#include "dgDebug.h"
#include "dgGeneralMatrix.h"

/*
void _BackAndForwardSustitition (void *rightsideVector,
                                           void *rowPointers,
                                           dgInt32 rowStrideInBytes,
                                           dgInt32 typeSizeInBytes,
                                           dgInt32 size);

bool _SolveByCholeskyDecomposition (void *rightsideVector,
                                                void *rowPointers,
                                                dgInt32 rowStrideInBytes,
                                                dgInt32 typeSizeInBytes,
                                                dgInt32 size);



template<class T>
class dgSPDMatrix: public dgGeneralMatrix<T>
{
    public:
    dgSPDMatrix (dgInt32 size);
    dgSPDMatrix (const dgSPDMatrix<T>& src);
    dgSPDMatrix (dgInt32 size, T *elemBuffer, dgGeneralVector<T>* m_rowBuffer);
    dgSPDMatrix (const dgSPDMatrix<T>& src, T *elemBuffer, dgGeneralVector<T>* m_rowBuffer);

    ~dgSPDMatrix ();

    bool TestPSD () const;
    bool CholeskyDecomposition();
    void DownDateCholeskyDecomposition (dgInt32 column);


    bool Solve (dgGeneralVector<T> &b);
    void BackAndForwardSustitition(dgGeneralVector<T> &b);
};


// ***********************************************************************************************
//
//  LinearSystem
//
// ***********************************************************************************************
template<class T>
dgSPDMatrix<T>::dgSPDMatrix (dgInt32 size)
    :dgGeneralMatrix<T>(size, size)
{
}

template<class T>
dgSPDMatrix<T>::dgSPDMatrix (const dgSPDMatrix<T>& src)
    :dgGeneralMatrix<T>(src)
{
}


template<class T>
dgSPDMatrix<T>::dgSPDMatrix (
    dgInt32 size,
    T *elemBuffer,
    dgGeneralVector<T>* m_rowBuffer)
    :dgGeneralMatrix<T>(size, size, elemBuffer, m_rowBuffer)
{
}

template<class T>
dgSPDMatrix<T>::dgSPDMatrix (
    const dgSPDMatrix<T>& src,
    T *elemBuffer,
    dgGeneralVector<T>* m_rowBuffer)
    :dgGeneralMatrix<T>(src, elemBuffer, m_rowBuffer)
{
}


template<class T>
dgSPDMatrix<T>::~dgSPDMatrix ()
{
}

template<class T>
bool dgSPDMatrix<T>::TestPSD () const
{
    if (!TestSymetry ()) {
        return false;
    }

    dgSPDMatrix<T> tmp (*this);
    return tmp.CholeskyDecomposition();
}


template<class T>
void dgSPDMatrix<T>::BackAndForwardSustitition(dgGeneralVector<T> &b)
{
    _BackAndForwardSustitition (b.m_columns, &m_rows[0].m_columns, sizeof (dgGeneralVector<T>), sizeof (T), m_rowCount);
}


template<class T>
bool dgSPDMatrix<T>::Solve (dgGeneralVector<T> &b)
{
    return _SolveByCholeskyDecomposition (b.m_columns, &m_rows[0].m_columns, sizeof (dgGeneralVector<T>), sizeof (T), m_rowCount);
}



template<class T>
bool dgSPDMatrix<T>::CholeskyDecomposition()
{
    dgInt32 i;
    dgInt32 j;
    dgInt32 k;
    T factor;
    T* rowK;
    T* rowJ;

    #ifdef DG_COUNT_FLOAT_OPS
    dgInt32 memCount;
    dgInt32 floatCount;

    memCount = dgGeneralVector<T>::GetMemWrites();
    floatCount = dgGeneralVector<T>::GetFloatOps();
    #endif

    for (j = 0; j < m_rowCount; j++) {
        rowJ = &m_rows[j].m_columns[0];

        for (k = 0; k < j; k ++ ) {
            rowK = &m_rows[k].m_columns[0];

            factor = rowK[j];
            if (dgAbsf (factor) > 1.0e-6f) {
                for (i = j; i < m_rowCount; i ++) {
                    rowJ[i] -= rowK[i] * factor;
                    #ifdef DG_COUNT_FLOAT_OPS
                    memCount += 1;
                    floatCount += 2;
                    #endif
                }
            }
        }

        factor = rowJ[j];
        if (factor <= T (0.0f)) {
            if (factor <= T(-5.0e-4f)) {
                return false;
            }
            factor = T(1.0e-12f);
        }

        factor = T (dgSqrt (dgFloat32(factor)));
        rowJ[j] = factor;
        factor = T(1.0f / dgFloat32(factor));

        #ifdef DG_COUNT_FLOAT_OPS
        memCount += 1;
        floatCount += 1;
        #endif

        for (k = j + 1; k < m_rowCount; k ++) {
            rowJ[k] *= factor;
            #ifdef DG_COUNT_FLOAT_OPS
            memCount += 1;
            floatCount += 1;
            #endif
        }
    }

    #ifdef DG_COUNT_FLOAT_OPS
    dgGeneralVector<T>::SetMemWrites(memCount);
    dgGeneralVector<T>::SetFloatOps(floatCount);
    #endif

    return true;

}


template<class T>
void dgSPDMatrix<T>::DownDateCholeskyDecomposition (dgInt32 column)
{
    dgInt32 i;
    dgInt32 j;

    T *buffer;
    dgGeneralVector<T>* rowsBuffer;

    rowsBuffer = (dgGeneralVector<dgFloat32>*) dgStackAlloc (m_rowCount * sizeof (dgGeneralVector<T>));
    buffer = (T *) dgStackAlloc ((m_rowCount + 4) * (m_rowCount + 4) * sizeof (T));

    dgGeneralMatrix<T> tmp (m_rowCount, m_rowCount, buffer, rowsBuffer);
    dgGeneralMatrix<T>& me = *this;
    for (i = 0; i < m_rowCount; i ++) {
        tmp[i][i] =  me[i][i];
        for (j = i + 1; j < m_rowCount; j ++) {
            tmp[i][j] = T (0.0f);
            tmp[j][i] =  me[i][j];
        }
    }

    me.MatrixTimeMatrixTranspose (tmp, tmp);
    for (i = 0; i < m_rowCount; i ++) {
        me[i][column] = T (0.0f);
        me[column][i] = T (0.0f);
    }
    me[column][column] = T (1.0f);

    CholeskyDecomposition();
}
*/

#endif