<commit_before>//  (C) Copyright Gennadiy Rozental 2004.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.
//
//  File        : $RCSfile$
//
//  Version     : $Revision$
//
//  Description :
// ***************************************************************************

#ifndef BOOST_ALGORITHM_HPP
#define BOOST_ALGORITHM_HPP

#include <utility>

namespace boost {

namespace unit_test {

/// @brief this algorithm search through two collections for first mismatch position that get returned as a pair
/// of iterators, first pointing to the mismatch position in first collection, second iterator in second one

/// @param _F1 - first collection begin iterator
/// @param last1 - first collection end iterator
/// @param first2 - second collection begin iterator
/// @param last2 - second collection end iterator
template <class InputIter1, class InputIter2>
inline std::pair<InputIter1, InputIter2>
mismatch( InputIter1 first1, InputIter1 last1,
          InputIter2 first2, InputIter2 last2 )
{
    while( first1 != last1 && first2 != last2 && *first1 == *first2 ) {
        ++first1;
        ++first2;
    }

    return std::pair<InputIter1, InputIter2>(first1, first2);
}

//____________________________________________________________________________//

/// @brief this algorithm search through two collections for first mismatch position that get returned as a pair
/// of iterators, first pointing to the mismatch position in first collection, second iterator in second one. This algorithms
/// uses supplied predicate for collection elements comparison

/// @param _F1 - first collection begin iterator
/// @param last1 - first collection end iterator
/// @param first2 - second collection begin iterator
/// @param last2 - second collection end iterator
/// @param pred - predicate to be used for search
template <class InputIter1, class InputIter2, class Predicate>
inline std::pair<InputIter1, InputIter2>
mismatch( InputIter1 first1, InputIter1 last1,
          InputIter2 first2, InputIter2 last2,
          Predicate pred )
{
    while( first1 != last1 && first2 != last2 && pred( *first1, *first2 ) ) {
        ++first1;
        ++first2;
    }

    return std::pair<InputIter1, InputIter2>(first1, first2);
}

//____________________________________________________________________________//

/// @brief this algorithm search through first collection for first element that does not belong a second one

/// @param first1 - first collection begin iterator
/// @param last1 - first collection end iterator
/// @param first2 - second collection begin iterator
/// @param last2 - second collection end iterator
template<class ForwardIterator1, class ForwardIterator2>
inline ForwardIterator1
find_first_not_of( ForwardIterator1 first1, ForwardIterator1 last1,
                   ForwardIterator2 first2, ForwardIterator2 last2 )
{
    while( first1 != last1 ) {
        if( std::find( first2, last2, *first1 ) == last2 )
            break;
        ++first1;
    }

    return first1;
}

//____________________________________________________________________________//

/// @brief this algorithm search through first collection for first element that does not satisfy binary
/// predicate in conjunction will any element in second collection

/// @param first1 - first collection begin iterator
/// @param last1 - first collection end iterator
/// @param first2 - second collection begin iterator
/// @param last2 - second collection end iterator
/// @param pred - predicate to be used for search
template<class ForwardIterator1, class ForwardIterator2, class Predicate>
inline ForwardIterator1
find_first_not_of( ForwardIterator1 first1, ForwardIterator1 last1,
                   ForwardIterator2 first2, ForwardIterator2 last2,
                   Predicate pred )
{
    while( first1 != last1 ) {
        if( std::find_if( first2, last2, std::bind1st( pred, *first1 ) ) == last2 )
            break;
        ++first1;
    }

    return first1;
}

//____________________________________________________________________________//

/// @brief this algorithm search through first collection for last element that belongs to a second one

/// @param first1 - first collection begin iterator
/// @param last1 - first collection end iterator
/// @param first2 - second collection begin iterator
/// @param last2 - second collection end iterator
template<class BidirectionalIterator1, class ForwardIterator2>
inline BidirectionalIterator1
find_last_of( BidirectionalIterator1 first1, BidirectionalIterator1 last1,
              ForwardIterator2 first2, ForwardIterator2 last2 )
{
    if( first1 == last1 || first2 == last2 )
        return last1;

    BidirectionalIterator1 it1 = last1;
    while( true ) {
        --it1;
        if( std::find( first2, last2, *it1 ) != last2 || it1 == first1 )
            break;
    }

    return it1 == first1 ? last1 : it1;
}

//____________________________________________________________________________//

/// @brief this algorithm search through first collection for last element that satisfy binary
/// predicate in conjunction will at least one element in second collection

/// @param first1 - first collection begin iterator
/// @param last1 - first collection end iterator
/// @param first2 - second collection begin iterator
/// @param last2 - second collection end iterator
/// @param pred - predicate to be used for search
template<class BidirectionalIterator1, class ForwardIterator2, class Predicate>
inline BidirectionalIterator1
find_last_of( BidirectionalIterator1 first1, BidirectionalIterator1 last1,
              ForwardIterator2 first2, ForwardIterator2 last2,
              Predicate pred )
{
    if( first1 == last1 || first2 == last2 )
        return last1;

    BidirectionalIterator1 it1 = last1;
    while( true ) {
        --it1;
        if( std::find_if( first2, last2, std::bind1st( pred, *it1 ) ) != last2 || it1 == first1 )
            break;
    }

    return it1 == first1 ? last1 : it1;
}

//____________________________________________________________________________//

/// @brief this algorithm search through first collection for last element that does not belong to a second one

/// @param first1 - first collection begin iterator
/// @param last1 - first collection end iterator
/// @param first2 - second collection begin iterator
/// @param last2 - second collection end iterator
template<class BidirectionalIterator1, class ForwardIterator2>
inline BidirectionalIterator1
find_last_not_of( BidirectionalIterator1 first1, BidirectionalIterator1 last1,
                  ForwardIterator2 first2, ForwardIterator2 last2 )
{
    if( first1 == last1 || first2 == last2 )
        return last1;

    BidirectionalIterator1 it1 = last1;
    while( true ) {
        --it1;
        if( std::find( first2, last2, *it1 ) == last2 || it1 == first1 )
            break;
    }

    return it1 == first1 ? last1 : it1;
}

//____________________________________________________________________________//

/// @brief this algorithm search through first collection for last element that does not satisfy binary
/// predicate in conjunction will any element in second collection

/// @param first1 - first collection begin iterator
/// @param last1 - first collection end iterator
/// @param first2 - second collection begin iterator
/// @param last2 - second collection end iterator
/// @param pred - predicate to be used for search
template<class BidirectionalIterator1, class ForwardIterator2, class Predicate>
inline BidirectionalIterator1
find_last_not_of( BidirectionalIterator1 first1, BidirectionalIterator1 last1,
                  ForwardIterator2 first2, ForwardIterator2 last2,
                  Predicate pred )
{
    if( first1 == last1 || first2 == last2 )
        return last1;

    BidirectionalIterator1 it1 = last1;

    while( true ) {
        --it1;
        if( std::find_if( first2, last2, std::bind1st( pred, *it1 ) ) == last2 || it1 == first1 )
            break;
    }

    return it1 == first1 ? last1 : it1;
}

//____________________________________________________________________________//

} // namespace unit_test

} // namespace boost

// ***************************************************************************
//  Revision History :
//
//  $Log$
//  Revision 1.3  2004/05/23 09:50:07  rogeeff
//  more algorithms added
//
//  Revision 1.2  2004/05/21 06:19:35  rogeeff
//  licence update
//
//  Revision 1.1  2004/05/11 11:00:53  rogeeff
//  basic_cstring introduced and used everywhere
//  class properties reworked
//
// ***************************************************************************

#endif // BOOST_ALGORITHM_HPP

<commit_msg>more algorithms added<commit_after>//  (C) Copyright Gennadiy Rozental 2004.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.
//
//  File        : $RCSfile$
//
//  Version     : $Revision$
//
//  Description : addition to STL algorithms
// ***************************************************************************

#ifndef BOOST_ALGORITHM_HPP
#define BOOST_ALGORITHM_HPP

#include <utility>

namespace boost {

namespace unit_test {

/// @brief this algorithm search through two collections for first mismatch position that get returned as a pair
/// of iterators, first pointing to the mismatch position in first collection, second iterator in second one

/// @param _F1 - first collection begin iterator
/// @param last1 - first collection end iterator
/// @param first2 - second collection begin iterator
/// @param last2 - second collection end iterator
template <class InputIter1, class InputIter2>
inline std::pair<InputIter1, InputIter2>
mismatch( InputIter1 first1, InputIter1 last1,
          InputIter2 first2, InputIter2 last2 )
{
    while( first1 != last1 && first2 != last2 && *first1 == *first2 ) {
        ++first1;
        ++first2;
    }

    return std::pair<InputIter1, InputIter2>(first1, first2);
}

//____________________________________________________________________________//

/// @brief this algorithm search through two collections for first mismatch position that get returned as a pair
/// of iterators, first pointing to the mismatch position in first collection, second iterator in second one. This algorithms
/// uses supplied predicate for collection elements comparison

/// @param _F1 - first collection begin iterator
/// @param last1 - first collection end iterator
/// @param first2 - second collection begin iterator
/// @param last2 - second collection end iterator
/// @param pred - predicate to be used for search
template <class InputIter1, class InputIter2, class Predicate>
inline std::pair<InputIter1, InputIter2>
mismatch( InputIter1 first1, InputIter1 last1,
          InputIter2 first2, InputIter2 last2,
          Predicate pred )
{
    while( first1 != last1 && first2 != last2 && pred( *first1, *first2 ) ) {
        ++first1;
        ++first2;
    }

    return std::pair<InputIter1, InputIter2>(first1, first2);
}

//____________________________________________________________________________//

/// @brief this algorithm search through first collection for first element that does not belong a second one

/// @param first1 - first collection begin iterator
/// @param last1 - first collection end iterator
/// @param first2 - second collection begin iterator
/// @param last2 - second collection end iterator
template<class ForwardIterator1, class ForwardIterator2>
inline ForwardIterator1
find_first_not_of( ForwardIterator1 first1, ForwardIterator1 last1,
                   ForwardIterator2 first2, ForwardIterator2 last2 )
{
    while( first1 != last1 ) {
        if( std::find( first2, last2, *first1 ) == last2 )
            break;
        ++first1;
    }

    return first1;
}

//____________________________________________________________________________//

/// @brief this algorithm search through first collection for first element that does not satisfy binary
/// predicate in conjunction will any element in second collection

/// @param first1 - first collection begin iterator
/// @param last1 - first collection end iterator
/// @param first2 - second collection begin iterator
/// @param last2 - second collection end iterator
/// @param pred - predicate to be used for search
template<class ForwardIterator1, class ForwardIterator2, class Predicate>
inline ForwardIterator1
find_first_not_of( ForwardIterator1 first1, ForwardIterator1 last1,
                   ForwardIterator2 first2, ForwardIterator2 last2,
                   Predicate pred )
{
    while( first1 != last1 ) {
        if( std::find_if( first2, last2, std::bind1st( pred, *first1 ) ) == last2 )
            break;
        ++first1;
    }

    return first1;
}

//____________________________________________________________________________//

/// @brief this algorithm search through first collection for last element that belongs to a second one

/// @param first1 - first collection begin iterator
/// @param last1 - first collection end iterator
/// @param first2 - second collection begin iterator
/// @param last2 - second collection end iterator
template<class BidirectionalIterator1, class ForwardIterator2>
inline BidirectionalIterator1
find_last_of( BidirectionalIterator1 first1, BidirectionalIterator1 last1,
              ForwardIterator2 first2, ForwardIterator2 last2 )
{
    if( first1 == last1 || first2 == last2 )
        return last1;

    BidirectionalIterator1 it1 = last1;
    while( true ) {
        --it1;
        if( std::find( first2, last2, *it1 ) != last2 || it1 == first1 )
            break;
    }

    return it1 == first1 ? last1 : it1;
}

//____________________________________________________________________________//

/// @brief this algorithm search through first collection for last element that satisfy binary
/// predicate in conjunction will at least one element in second collection

/// @param first1 - first collection begin iterator
/// @param last1 - first collection end iterator
/// @param first2 - second collection begin iterator
/// @param last2 - second collection end iterator
/// @param pred - predicate to be used for search
template<class BidirectionalIterator1, class ForwardIterator2, class Predicate>
inline BidirectionalIterator1
find_last_of( BidirectionalIterator1 first1, BidirectionalIterator1 last1,
              ForwardIterator2 first2, ForwardIterator2 last2,
              Predicate pred )
{
    if( first1 == last1 || first2 == last2 )
        return last1;

    BidirectionalIterator1 it1 = last1;
    while( true ) {
        --it1;
        if( std::find_if( first2, last2, std::bind1st( pred, *it1 ) ) != last2 || it1 == first1 )
            break;
    }

    return it1 == first1 ? last1 : it1;
}

//____________________________________________________________________________//

/// @brief this algorithm search through first collection for last element that does not belong to a second one

/// @param first1 - first collection begin iterator
/// @param last1 - first collection end iterator
/// @param first2 - second collection begin iterator
/// @param last2 - second collection end iterator
template<class BidirectionalIterator1, class ForwardIterator2>
inline BidirectionalIterator1
find_last_not_of( BidirectionalIterator1 first1, BidirectionalIterator1 last1,
                  ForwardIterator2 first2, ForwardIterator2 last2 )
{
    if( first1 == last1 || first2 == last2 )
        return last1;

    BidirectionalIterator1 it1 = last1;
    while( true ) {
        --it1;
        if( std::find( first2, last2, *it1 ) == last2 || it1 == first1 )
            break;
    }

    return it1 == first1 ? last1 : it1;
}

//____________________________________________________________________________//

/// @brief this algorithm search through first collection for last element that does not satisfy binary
/// predicate in conjunction will any element in second collection

/// @param first1 - first collection begin iterator
/// @param last1 - first collection end iterator
/// @param first2 - second collection begin iterator
/// @param last2 - second collection end iterator
/// @param pred - predicate to be used for search
template<class BidirectionalIterator1, class ForwardIterator2, class Predicate>
inline BidirectionalIterator1
find_last_not_of( BidirectionalIterator1 first1, BidirectionalIterator1 last1,
                  ForwardIterator2 first2, ForwardIterator2 last2,
                  Predicate pred )
{
    if( first1 == last1 || first2 == last2 )
        return last1;

    BidirectionalIterator1 it1 = last1;

    while( true ) {
        --it1;
        if( std::find_if( first2, last2, std::bind1st( pred, *it1 ) ) == last2 || it1 == first1 )
            break;
    }

    return it1 == first1 ? last1 : it1;
}

//____________________________________________________________________________//

} // namespace unit_test

} // namespace boost

// ***************************************************************************
//  Revision History :
//
//  $Log$
//  Revision 1.4  2004/05/23 09:51:32  rogeeff
//  more algorithms added
//
// ***************************************************************************

#endif // BOOST_ALGORITHM_HPP

<|endoftext|>


<commit_before>#include "fmv.hpp"
#include "imgui/imgui.h"
#include "core/audiobuffer.hpp"
#include "gamedata.hpp"

#ifdef _WIN32
#include <windows.h>
#endif
#ifdef __APPLE__
#  include <OpenGL/gl.h>
#else
#  include <GL/gl.h>
#endif/*__APPLE__*/

class FmvUi
{
private:
    char buf[4096];
    ImGuiTextFilter mFilter;
    int listbox_item_current = 1;
    std::vector<const char*> listbox_items;
public:
    FmvUi()
    {
#ifdef _WIN32
        strcpy(buf, "C:\\Program Files (x86)\\Steam\\SteamApps\\common\\Oddworld Abes Exoddus\\");
#else
        strcpy(buf, "/home/paul/ae_test/");
#endif
    }

    void DrawVideoSelectionUi(std::unique_ptr<Oddlib::Masher>& video, FILE*& fp, SDL_Surface*& videoFrame, const std::string& setName, const std::vector<std::string>& allFmvs)
    {
        std::string name = "Video player (" + setName + ")";
        ImGui::Begin(name.c_str(), nullptr, ImVec2(550, 580), 1.0f, ImGuiWindowFlags_NoCollapse);

        ImGui::InputText("Video path", buf, sizeof(buf));

        mFilter.Draw();

        listbox_items.resize(allFmvs.size());

        int matchingFilter = 0;
        for (size_t i = 0; i < allFmvs.size(); i++)
        {
            if (mFilter.PassFilter(allFmvs[i].c_str()))
            {
                listbox_items[matchingFilter] = allFmvs[i].c_str();
                matchingFilter++;
            }
        }
        ImGui::PushItemWidth(-1);
        ImGui::ListBox("##", &listbox_item_current, listbox_items.data(), matchingFilter, 27);

        if (ImGui::Button("Play", ImVec2(ImGui::GetWindowWidth(), 20)))
        {
            std::string fullPath = std::string(buf) + listbox_items[listbox_item_current];
            std::cout << "Play " << listbox_items[listbox_item_current] << std::endl;
            try
            {
                video = std::make_unique<Oddlib::Masher>(fullPath);
                if (videoFrame)
                {
                    SDL_FreeSurface(videoFrame);
                    videoFrame = nullptr;
                }

                if (video->HasAudio())
                {
                    AudioBuffer::ChangeAudioSpec(video->SingleAudioFrameSizeBytes(), video->AudioSampleRate());
                }

                if (video->HasVideo())
                {
                    videoFrame = SDL_CreateRGBSurface(0, video->Width(), video->Height(), 32, 0, 0, 0, 0);
                    //                    targetFps = video->FrameRate() * 2;
                }

                SDL_ShowCursor(0);
            }
            catch (const Oddlib::Exception& ex)
            {
                // ImGui::Text(ex.what());

                fp = fopen(fullPath.c_str(), "rb");
                AudioBuffer::ChangeAudioSpec(512*4, 37800);

            }
        }

        ImGui::End();
    }
};

struct CDXASector
{
    uint8_t sync[12];
    uint8_t header[4];

    struct CDXASubHeader
    {
        uint8_t file_number;
        uint8_t channel;
        uint8_t submode;
        uint8_t coding_info;
        uint8_t file_number_copy;
        uint8_t channel_number_copy;
        uint8_t submode_copy;
        uint8_t coding_info_copy;
    } subheader;
    uint8_t data[2328];
};

static const uint8_t m_CDXA_STEREO = 3;

struct PsxVideoFrameHeader
{
    unsigned short int mNumMdecCodes;
    unsigned short int m3800Magic;
    unsigned short int mQuantizationLevel;
    unsigned short int mVersion;
};

struct MasherVideoHeaderWrapper
{
    unsigned int mSectorType; // AKIK
    unsigned int mSectorNumber;

    // The 4 "unknown" / 0x80010160 in psx data is replaced by "AKIK" in PC data
    unsigned int mAkikMagic;

    unsigned short int mSectorNumberInFrame;
    unsigned short int mNumSectorsInFrame;
    unsigned int mFrameNum;
    unsigned int mFrameDataLen;
    unsigned short int mWidth;
    unsigned short int mHeight;

    PsxVideoFrameHeader mVideoFrameHeader;
    unsigned int mNulls;

    unsigned char frame[2016 + 240 + 4 + 4 + 4 + 2 + 2 + 4 + 4 + 2 + 2 + 2 + 2 + 2 + 2 + 4];

    // PsxVideoFrameHeader mVideoFrameHeader2;

    /*
    0000  char   {4}     "AKIK"

    // This is standard STR format data here

    0004  uint16 {2}     Sector number within frame (zero-based)
    0006  uint16 {2}     Number of sectors in frame
    0008  uint32 {4}     Frame number within file (one-based)
    000C  uint32 {4}     Frame data length
    0010  uint16 {2}     Video Width
    0012  uint16 {2}     Video Height

    0014  uint16 {2}     Number of MDEC codes divided by two, and rounded up to a multiple of 32
    0016  uint16 {2}     Always 0x3800
    0018  uint16 {2}     Quantization level of the video frame
    001A  uint16 {2}     Version of the video frame (always 2)
    001C  uint32 {4}     Always 0x00000000
    */

    //demultiplexing is joining all of the frame data into one buffer without the headers

};

std::vector<Uint32> pixels;

std::vector<unsigned char> ReadFrame(FILE* fp, bool& end, PSXMDECDecoder& mdec, PSXADPCMDecoder& adpcm, bool firstFrame, int& frameW, int& frameH)
{
    std::vector<unsigned char> r(32768);
    std::vector<unsigned char> outPtr(32678);

    unsigned int numSectorsToRead = 0;
    unsigned int sectorNumber = 0;
    const int kDataSize = 2016;
;

    for (;;)
    {
        MasherVideoHeaderWrapper w;

        if (fread(&w, 1, sizeof(w), fp) != sizeof(w))
        {
            end = true;
            return r;
        }

        if (w.mSectorType != 0x52494f4d)
        {
            // There is probably no way this is correct
            CDXASector* xa = (CDXASector*)&w;

            auto numBytes = adpcm.DecodeFrameToPCM((int8_t *)outPtr.data(), xa->header, true);

            AudioBuffer::SendSamples((char*)outPtr.data(), numBytes);

            // Must be VALE
            continue;
        }
        else
        {
            frameW = w.mWidth;
            frameH = w.mHeight;

            //std::cout << "sector: " << w.mSectorNumber << std::endl;
            //std::cout << "data len: " << w.mFrameDataLen << std::endl;
            //std::cout << "frame number: " << w.mFrameNum << std::endl;
            //std::cout << "num sectors in frame: " << w.mNumSectorsInFrame << std::endl;
            //std::cout << "frame sector number: " << w.mSectorNumberInFrame << std::endl;
            // SetSurfaceSize(w.mWidth, w.mHeight);

            uint32_t bytes_to_copy = w.mFrameDataLen - w.mSectorNumberInFrame *kDataSize;

            if (bytes_to_copy > 0)
            {
                if (bytes_to_copy > kDataSize)
                {
                    bytes_to_copy = kDataSize;
                }

                memcpy(r.data() + w.mSectorNumberInFrame * kDataSize, w.frame, bytes_to_copy);
            }

            if (w.mSectorNumberInFrame == w.mNumSectorsInFrame - 1)
            {
                break;
            }
        }
    }

    if (pixels.empty())
    {
        pixels.resize(frameW * frameH);

    }

    mdec.DecodeFrameToABGR32((uint16_t*)pixels.data(), (uint16_t*)r.data(), frameW, frameH, false);

    SDL_Delay(50);
    return r;
}

void Fmv::RenderVideoUi()
{

    if (!video && !mFp)
    {
        if (mFmvUis.empty())
        {
            auto fmvs = mGameData.Fmvs();
            for (auto fmvSet : fmvs)
            {
                mFmvUis.emplace_back(std::make_unique<FmvUi>());
            }
        }

        if (!mFmvUis.empty())
        {
            int i = 0;
            auto fmvs = mGameData.Fmvs();
            for (auto fmvSet : fmvs)
            {
                mFmvUis[i]->DrawVideoSelectionUi(video, mFp, videoFrame, fmvSet.first, fmvSet.second);
                i++;
            }
        }

    }
    else
    {
        if (mFp)
        {
            bool firstFrame = true;
            bool end = false;
            std::vector<unsigned char> frameData = ReadFrame(mFp, end, mMdec, mAdpcm, firstFrame, frameW, frameH);
            firstFrame = false;
            if (end)
            {
                fclose(mFp);
                mFp = nullptr;
            }
        }
        else if (video)
        {
            std::vector<Uint16> decodedFrame(video->SingleAudioFrameSizeBytes() * 2); // *2 if stereo

            if (!video->Update((Uint32*)videoFrame->pixels, (Uint8*)decodedFrame.data()))
            {
                SDL_ShowCursor(1);
                video = nullptr;
                if (videoFrame)
                {
                    SDL_FreeSurface(videoFrame);
                    videoFrame = nullptr;
                }
                //targetFps = 60;
            }
            else
            {
                AudioBuffer::SendSamples((char*)decodedFrame.data(), decodedFrame.size() * 2);
                while (AudioBuffer::mPlayedSamples < video->FrameNumber() * video->SingleAudioFrameSizeBytes())
                {

                }

            }
        }
    }
}

Fmv::Fmv(GameData& gameData)
    : mGameData(gameData)
{

}

Fmv::~Fmv()
{

}

void Fmv::Play()
{

}

void Fmv::Stop()
{
    SDL_ShowCursor(1);
    video = nullptr;
    if (videoFrame)
    {
        SDL_FreeSurface(videoFrame);
        videoFrame = nullptr;
    }
}

void Fmv::Update()
{

}

void Fmv::Render()
{
    glEnable(GL_TEXTURE_2D);

    RenderVideoUi();

    if (videoFrame || mFp)
    {
        // TODO: Optimize - should use VBO's & update 1 texture rather than creating per frame
        GLuint TextureID = 0;

        glGenTextures(1, &TextureID);
        glBindTexture(GL_TEXTURE_2D, TextureID);

        int Mode = GL_RGB;
        /*
        if (videoFrame->format->BytesPerPixel == 4)
        {
            Mode = GL_RGBA;
        }
        */

        if (videoFrame)
        {
            glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, videoFrame->w, videoFrame->h, 0, GL_RGB, GL_UNSIGNED_BYTE, videoFrame->pixels);
        }
        else if (mFp)
        {
            glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, frameW, frameH, 0, GL_RGBA, GL_UNSIGNED_BYTE, pixels.data());
        }

        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);

        // For Ortho mode, of course
        int X = -1;
        int Y = -1;
        int Width = 2;
        int Height = 2;

        glBegin(GL_QUADS);
        glTexCoord2f(0, 0); glVertex3f(X, Y, 0);
        glTexCoord2f(1, 0); glVertex3f(X + Width, Y, 0);
        glTexCoord2f(1, -1); glVertex3f(X + Width, Y + Height, 0);
        glTexCoord2f(0, -1); glVertex3f(X, Y + Height, 0);
        glEnd();

        glDeleteTextures(1, &TextureID);
    }

}
<commit_msg>audio debug to stdout<commit_after>#include "fmv.hpp"
#include "imgui/imgui.h"
#include "core/audiobuffer.hpp"
#include "gamedata.hpp"

#ifdef _WIN32
#include <windows.h>
#endif
#ifdef __APPLE__
#  include <OpenGL/gl.h>
#else
#  include <GL/gl.h>
#endif/*__APPLE__*/

class FmvUi
{
private:
    char buf[4096];
    ImGuiTextFilter mFilter;
    int listbox_item_current = 1;
    std::vector<const char*> listbox_items;
public:
    FmvUi()
    {
#ifdef _WIN32
        strcpy(buf, "C:\\Program Files (x86)\\Steam\\SteamApps\\common\\Oddworld Abes Oddysee\\");
#else
        strcpy(buf, "/home/paul/ae_test/");
#endif
    }

    void DrawVideoSelectionUi(std::unique_ptr<Oddlib::Masher>& video, FILE*& fp, SDL_Surface*& videoFrame, const std::string& setName, const std::vector<std::string>& allFmvs)
    {
        std::string name = "Video player (" + setName + ")";
        ImGui::Begin(name.c_str(), nullptr, ImVec2(550, 580), 1.0f, ImGuiWindowFlags_NoCollapse);

        ImGui::InputText("Video path", buf, sizeof(buf));

        mFilter.Draw();

        listbox_items.resize(allFmvs.size());

        int matchingFilter = 0;
        for (size_t i = 0; i < allFmvs.size(); i++)
        {
            if (mFilter.PassFilter(allFmvs[i].c_str()))
            {
                listbox_items[matchingFilter] = allFmvs[i].c_str();
                matchingFilter++;
            }
        }
        ImGui::PushItemWidth(-1);
        ImGui::ListBox("##", &listbox_item_current, listbox_items.data(), matchingFilter, 27);

        if (ImGui::Button("Play", ImVec2(ImGui::GetWindowWidth(), 20)))
        {
            std::string fullPath = std::string(buf) + listbox_items[listbox_item_current];
            std::cout << "Play " << listbox_items[listbox_item_current] << std::endl;
            try
            {
                video = std::make_unique<Oddlib::Masher>(fullPath);
                if (videoFrame)
                {
                    SDL_FreeSurface(videoFrame);
                    videoFrame = nullptr;
                }

                if (video->HasAudio())
                {
                    AudioBuffer::ChangeAudioSpec(video->SingleAudioFrameSizeBytes(), video->AudioSampleRate());
                }

                if (video->HasVideo())
                {
                    videoFrame = SDL_CreateRGBSurface(0, video->Width(), video->Height(), 32, 0, 0, 0, 0);
                    //                    targetFps = video->FrameRate() * 2;
                }

                SDL_ShowCursor(0);
            }
            catch (const Oddlib::Exception& ex)
            {
                // ImGui::Text(ex.what());

                fp = fopen(fullPath.c_str(), "rb");
                AudioBuffer::ChangeAudioSpec(512*4, 37800);

            }
        }

        ImGui::End();
    }
};

struct CDXASector
{
    uint8_t sync[12];
    uint8_t header[4];

    struct CDXASubHeader
    {
        uint8_t file_number;
        uint8_t channel;
        uint8_t submode;
        uint8_t coding_info;
        uint8_t file_number_copy;
        uint8_t channel_number_copy;
        uint8_t submode_copy;
        uint8_t coding_info_copy;
    } subheader;
    uint8_t data[2328];
};

static const uint8_t m_CDXA_STEREO = 3;

struct PsxVideoFrameHeader
{
    unsigned short int mNumMdecCodes;
    unsigned short int m3800Magic;
    unsigned short int mQuantizationLevel;
    unsigned short int mVersion;
};

struct MasherVideoHeaderWrapper
{
    unsigned int mSectorType; // AKIK
    unsigned int mSectorNumber;

    // The 4 "unknown" / 0x80010160 in psx data is replaced by "AKIK" in PC data
    unsigned int mAkikMagic;

    unsigned short int mSectorNumberInFrame;
    unsigned short int mNumSectorsInFrame;
    unsigned int mFrameNum;
    unsigned int mFrameDataLen;
    unsigned short int mWidth;
    unsigned short int mHeight;

    PsxVideoFrameHeader mVideoFrameHeader;
    unsigned int mNulls;

    unsigned char frame[2016 + 240 + 4 + 4 + 4 + 2 + 2 + 4 + 4 + 2 + 2 + 2 + 2 + 2 + 2 + 4];

    // PsxVideoFrameHeader mVideoFrameHeader2;

    /*
    0000  char   {4}     "AKIK"

    // This is standard STR format data here

    0004  uint16 {2}     Sector number within frame (zero-based)
    0006  uint16 {2}     Number of sectors in frame
    0008  uint32 {4}     Frame number within file (one-based)
    000C  uint32 {4}     Frame data length
    0010  uint16 {2}     Video Width
    0012  uint16 {2}     Video Height

    0014  uint16 {2}     Number of MDEC codes divided by two, and rounded up to a multiple of 32
    0016  uint16 {2}     Always 0x3800
    0018  uint16 {2}     Quantization level of the video frame
    001A  uint16 {2}     Version of the video frame (always 2)
    001C  uint32 {4}     Always 0x00000000
    */

    //demultiplexing is joining all of the frame data into one buffer without the headers

};

std::vector<Uint32> pixels;

#define CHECK_BIT(var,pos) ((var) & (1<<(pos)))

std::vector<unsigned char> ReadFrame(FILE* fp, bool& end, PSXMDECDecoder& mdec, PSXADPCMDecoder& adpcm, bool firstFrame, int& frameW, int& frameH)
{
    std::vector<unsigned char> r(32768);
    std::vector<unsigned char> outPtr(32678);

    unsigned int numSectorsToRead = 0;
    unsigned int sectorNumber = 0;
    const int kDataSize = 2016;
;

    for (;;)
    {
        MasherVideoHeaderWrapper w;

        if (fread(&w, 1, sizeof(w), fp) != sizeof(w))
        {
            end = true;
            return r;
        }

        if (w.mSectorType != 0x52494f4d)
        {
            // There is probably no way this is correct
            CDXASector* xa = (CDXASector*)&w;

            std::cout <<
                (CHECK_BIT(xa->subheader.coding_info, 0) ? "Mono " : "Stereo ") <<
                (CHECK_BIT(xa->subheader.coding_info, 2) ? "37800Hz " : "18900Hz ") <<
                (CHECK_BIT(xa->subheader.coding_info, 4) ? "4bit " : "8bit ")

                << std::endl;
           // if (xa->subheader.coding_info & 0xA)
            {
                auto numBytes = adpcm.DecodeFrameToPCM((int8_t *)outPtr.data(), xa->header, xa->subheader.coding_info & 0x20);

                AudioBuffer::SendSamples((char*)outPtr.data(), numBytes);
            }

            // Must be VALE
            continue;
        }
        else
        {
            frameW = w.mWidth;
            frameH = w.mHeight;

            //std::cout << "sector: " << w.mSectorNumber << std::endl;
            //std::cout << "data len: " << w.mFrameDataLen << std::endl;
            //std::cout << "frame number: " << w.mFrameNum << std::endl;
            //std::cout << "num sectors in frame: " << w.mNumSectorsInFrame << std::endl;
            //std::cout << "frame sector number: " << w.mSectorNumberInFrame << std::endl;
            // SetSurfaceSize(w.mWidth, w.mHeight);

            uint32_t bytes_to_copy = w.mFrameDataLen - w.mSectorNumberInFrame *kDataSize;

            if (bytes_to_copy > 0)
            {
                if (bytes_to_copy > kDataSize)
                {
                    bytes_to_copy = kDataSize;
                }

                memcpy(r.data() + w.mSectorNumberInFrame * kDataSize, w.frame, bytes_to_copy);
            }

            if (w.mSectorNumberInFrame == w.mNumSectorsInFrame - 1)
            {
                break;
            }
        }
    }

    if (pixels.empty())
    {
        pixels.resize(frameW * frameH);

    }

    mdec.DecodeFrameToABGR32((uint16_t*)pixels.data(), (uint16_t*)r.data(), frameW, frameH, false);

    SDL_Delay(50);
    return r;
}

void Fmv::RenderVideoUi()
{

    if (!video && !mFp)
    {
        if (mFmvUis.empty())
        {
            auto fmvs = mGameData.Fmvs();
            for (auto fmvSet : fmvs)
            {
                mFmvUis.emplace_back(std::make_unique<FmvUi>());
            }
        }

        if (!mFmvUis.empty())
        {
            int i = 0;
            auto fmvs = mGameData.Fmvs();
            for (auto fmvSet : fmvs)
            {
                mFmvUis[i]->DrawVideoSelectionUi(video, mFp, videoFrame, fmvSet.first, fmvSet.second);
                i++;
            }
        }

    }
    else
    {
        if (mFp)
        {
            bool firstFrame = true;
            bool end = false;
            std::vector<unsigned char> frameData = ReadFrame(mFp, end, mMdec, mAdpcm, firstFrame, frameW, frameH);
            firstFrame = false;
            if (end)
            {
                fclose(mFp);
                mFp = nullptr;
            }
        }
        else if (video)
        {
            std::vector<Uint16> decodedFrame(video->SingleAudioFrameSizeBytes() * 2); // *2 if stereo

            if (!video->Update((Uint32*)videoFrame->pixels, (Uint8*)decodedFrame.data()))
            {
                SDL_ShowCursor(1);
                video = nullptr;
                if (videoFrame)
                {
                    SDL_FreeSurface(videoFrame);
                    videoFrame = nullptr;
                }
                //targetFps = 60;
            }
            else
            {
                AudioBuffer::SendSamples((char*)decodedFrame.data(), decodedFrame.size() * 2);
                while (AudioBuffer::mPlayedSamples < video->FrameNumber() * video->SingleAudioFrameSizeBytes())
                {

                }

            }
        }
    }
}

Fmv::Fmv(GameData& gameData)
    : mGameData(gameData)
{

}

Fmv::~Fmv()
{

}

void Fmv::Play()
{

}

void Fmv::Stop()
{
    SDL_ShowCursor(1);
    video = nullptr;
    if (videoFrame)
    {
        SDL_FreeSurface(videoFrame);
        videoFrame = nullptr;
    }
    if (mFp)
    {
        fclose(mFp);
        mFp = nullptr;
    }
}

void Fmv::Update()
{

}

void Fmv::Render()
{
    glEnable(GL_TEXTURE_2D);

    RenderVideoUi();

    if (videoFrame || mFp)
    {
        // TODO: Optimize - should use VBO's & update 1 texture rather than creating per frame
        GLuint TextureID = 0;

        glGenTextures(1, &TextureID);
        glBindTexture(GL_TEXTURE_2D, TextureID);

        int Mode = GL_RGB;
        /*
        if (videoFrame->format->BytesPerPixel == 4)
        {
            Mode = GL_RGBA;
        }
        */

        if (videoFrame)
        {
            glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, videoFrame->w, videoFrame->h, 0, GL_RGB, GL_UNSIGNED_BYTE, videoFrame->pixels);
        }
        else if (mFp)
        {
            glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, frameW, frameH, 0, GL_RGBA, GL_UNSIGNED_BYTE, pixels.data());
        }

        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);

        // For Ortho mode, of course
        int X = -1;
        int Y = -1;
        int Width = 2;
        int Height = 2;

        glBegin(GL_QUADS);
        glTexCoord2f(0, 0); glVertex3f(X, Y, 0);
        glTexCoord2f(1, 0); glVertex3f(X + Width, Y, 0);
        glTexCoord2f(1, -1); glVertex3f(X + Width, Y + Height, 0);
        glTexCoord2f(0, -1); glVertex3f(X, Y + Height, 0);
        glEnd();

        glDeleteTextures(1, &TextureID);
    }

}
<|endoftext|>


<commit_before>/*
 * eos - A 3D Morphable Model fitting library written in modern C++11/14.
 *
 * File: include/eos/morphablemodel/io/cvssp.hpp
 *
 * Copyright 2014, 2015 Patrik Huber
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#pragma once

#ifndef IO_CVSSP_HPP_
#define IO_CVSSP_HPP_

#include "eos/morphablemodel/MorphableModel.hpp"

#include "opencv2/core/core.hpp"

#include "boost/filesystem/path.hpp"

#include <vector>
#include <iostream>

namespace eos {
	namespace morphablemodel {

// Forward declaration
std::vector<cv::Vec2f> load_isomap(boost::filesystem::path isomap_file);

/**
 * Load a shape or color model from a .scm file containing
 * a Morphable Model in the Surrey format. CVSSP's software
 * internally trains and stores the model in this custom binary
 * format and this class provides means to load them.
 *
 * Note on multi-resolution models: The landmarks to vertex-id mapping is
 * always the same. The lowest resolution model has all the landmarks defined
 * and for the higher resolutions, the mesh is divided from that on.
 * Note: For new landmarks we add, this might not be the case if we add them
 * in the highest resolution model, so take care!
 *
 * - The pcaBasis matrix stored in the file and loaded is the orthogonal PCA basis, i.e. it is not normalised by the eigenvalues.
 *
 * @param[in] model_filename A binary .scm-file containing the model.
 * @param[in] isomap_file An optional path to an isomap containing texture coordinates.
 * @return The Morphable Model loaded from the file.
 * @throws ...
 */
inline MorphableModel load_scm_model(boost::filesystem::path model_filename, boost::filesystem::path isomap_file = boost::filesystem::path())
{
	using cv::Mat;
	if (sizeof(unsigned int) != 4) { // note/todo: maybe use uint32 or similar instead? Yep, but still we could encounter endianness-trouble.
		std::cout << "Warning: We're reading 4 Bytes from the file but sizeof(unsigned int) != 4. Check the code/behaviour." << std::endl;
	}
	if (sizeof(double) != 8) {
		std::cout << "Warning: We're reading 8 Bytes from the file but sizeof(double) != 8. Check the code/behaviour." << std::endl;
	}

	std::ifstream modelFile(model_filename.string(), std::ios::binary);
	if (!modelFile.is_open()) {
		std::string msg("Unable to open model file: " + model_filename.string());
		std::cout << msg << std::endl;
		throw std::runtime_error(msg);
	}

	// Reading the shape model
	// Read (reference?) num triangles and vertices
	unsigned int numVertices = 0;
	unsigned int numTriangles = 0;
	modelFile.read(reinterpret_cast<char*>(&numVertices), 4); // 1 char = 1 byte. uint32=4bytes. float64=8bytes.
	modelFile.read(reinterpret_cast<char*>(&numTriangles), 4);

	// Read triangles
	std::vector<std::array<int, 3>> triangleList;

	triangleList.resize(numTriangles);
	unsigned int v0, v1, v2;
	for (unsigned int i = 0; i < numTriangles; ++i) {
		v0 = v1 = v2 = 0;
		modelFile.read(reinterpret_cast<char*>(&v0), 4);	// would be nice to pass a &vector and do it in one
		modelFile.read(reinterpret_cast<char*>(&v1), 4);	// go, but didn't work. Maybe a cv::Mat would work?
		modelFile.read(reinterpret_cast<char*>(&v2), 4);
		triangleList[i][0] = v0;
		triangleList[i][1] = v1;
		triangleList[i][2] = v2;
	}

	// Read number of rows and columns of the shape projection matrix (pcaBasis)
	unsigned int numShapePcaCoeffs = 0;
	unsigned int numShapeDims = 0;	// dimension of the shape vector (3*numVertices)
	modelFile.read(reinterpret_cast<char*>(&numShapePcaCoeffs), 4);
	modelFile.read(reinterpret_cast<char*>(&numShapeDims), 4);

	if (3 * numVertices != numShapeDims) {
		std::cout << "Warning: Number of shape dimensions is not equal to three times the number of vertices. Something will probably go wrong during the loading." << std::endl;
	}

	// Read shape projection matrix
	Mat unnormalisedPcaBasisShape(numShapeDims, numShapePcaCoeffs, CV_32FC1); // m x n (rows x cols) = numShapeDims x numShapePcaCoeffs
	std::cout << "Loading shape PCA basis matrix with " << unnormalisedPcaBasisShape.rows << " rows and " << unnormalisedPcaBasisShape.cols << " cols." << std::endl;
	for (unsigned int col = 0; col < numShapePcaCoeffs; ++col) {
		for (unsigned int row = 0; row < numShapeDims; ++row) {
			double var = 0.0;
			modelFile.read(reinterpret_cast<char*>(&var), 8);
			unnormalisedPcaBasisShape.at<float>(row, col) = static_cast<float>(var);
		}
	}

	// Read mean shape vector
	unsigned int numMean = 0; // dimension of the mean (3*numVertices)
	modelFile.read(reinterpret_cast<char*>(&numMean), 4);
	if (numMean != numShapeDims) {
		std::cout << "Warning: Number of shape dimensions is not equal to the number of dimensions of the mean. Something will probably go wrong during the loading." << std::endl;
	}
	Mat meanShape(numMean, 1, CV_32FC1);
	unsigned int counter = 0;
	double vd0, vd1, vd2;
	for (unsigned int i = 0; i < numMean / 3; ++i) {
		vd0 = vd1 = vd2 = 0.0;
		modelFile.read(reinterpret_cast<char*>(&vd0), 8);
		modelFile.read(reinterpret_cast<char*>(&vd1), 8);
		modelFile.read(reinterpret_cast<char*>(&vd2), 8);
		meanShape.at<float>(counter, 0) = static_cast<float>(vd0);
		++counter;
		meanShape.at<float>(counter, 0) = static_cast<float>(vd1);
		++counter;
		meanShape.at<float>(counter, 0) = static_cast<float>(vd2);
		++counter;
	}

	// Read shape eigenvalues
	unsigned int numEigenValsShape = 0;
	modelFile.read(reinterpret_cast<char*>(&numEigenValsShape), 4);
	if (numEigenValsShape != numShapePcaCoeffs) {
		std::cout << "Warning: Number of coefficients in the PCA basis matrix is not equal to the number of eigenvalues. Something will probably go wrong during the loading." << std::endl;
	}
	Mat eigenvaluesShape(numEigenValsShape, 1, CV_32FC1);
	for (unsigned int i = 0; i < numEigenValsShape; ++i) {
		double var = 0.0;
		modelFile.read(reinterpret_cast<char*>(&var), 8);
		eigenvaluesShape.at<float>(i, 0) = static_cast<float>(var);
	}

	// We read the unnormalised basis from the file. Now let's normalise it and store the normalised basis separately.
	Mat normalisedPcaBasisShape = normalise_pca_basis(unnormalisedPcaBasisShape, eigenvaluesShape);
	PcaModel shapeModel(meanShape, normalisedPcaBasisShape, eigenvaluesShape, triangleList);

	// Reading the color model
	// Read number of rows and columns of projection matrix
	unsigned int numTexturePcaCoeffs = 0;
	unsigned int numTextureDims = 0;
	modelFile.read(reinterpret_cast<char*>(&numTexturePcaCoeffs), 4);
	modelFile.read(reinterpret_cast<char*>(&numTextureDims), 4);
	// Read color projection matrix
	Mat unnormalisedPcaBasisColor(numTextureDims, numTexturePcaCoeffs, CV_32FC1);
	std::cout << "Loading color PCA basis matrix with " << unnormalisedPcaBasisColor.rows << " rows and " << unnormalisedPcaBasisColor.cols << " cols." << std::endl;
	for (unsigned int col = 0; col < numTexturePcaCoeffs; ++col) {
		for (unsigned int row = 0; row < numTextureDims; ++row) {
			double var = 0.0;
			modelFile.read(reinterpret_cast<char*>(&var), 8);
			unnormalisedPcaBasisColor.at<float>(row, col) = static_cast<float>(var);
		}
	}

	// Read mean color vector
	unsigned int numMeanColor = 0; // dimension of the mean (3*numVertices)
	modelFile.read(reinterpret_cast<char*>(&numMeanColor), 4);
	Mat meanColor(numMeanColor, 1, CV_32FC1);
	counter = 0;
	for (unsigned int i = 0; i < numMeanColor / 3; ++i) {
		vd0 = vd1 = vd2 = 0.0;
		modelFile.read(reinterpret_cast<char*>(&vd0), 8); // order in hdf5: RGB. Order in OCV: BGR. But order in vertex.color: RGB
		modelFile.read(reinterpret_cast<char*>(&vd1), 8);
		modelFile.read(reinterpret_cast<char*>(&vd2), 8);
		meanColor.at<float>(counter, 0) = static_cast<float>(vd0);
		++counter;
		meanColor.at<float>(counter, 0) = static_cast<float>(vd1);
		++counter;
		meanColor.at<float>(counter, 0) = static_cast<float>(vd2);
		++counter;
	}

	// Read color eigenvalues
	unsigned int numEigenValsColor = 0;
	modelFile.read(reinterpret_cast<char*>(&numEigenValsColor), 4);
	Mat eigenvaluesColor(numEigenValsColor, 1, CV_32FC1);
	for (unsigned int i = 0; i < numEigenValsColor; ++i) {
		double var = 0.0;
		modelFile.read(reinterpret_cast<char*>(&var), 8);
		eigenvaluesColor.at<float>(i, 0) = static_cast<float>(var);
	}

	// We read the unnormalised basis from the file. Now let's normalise it and store the normalised basis separately.
	Mat normalisedPcaBasisColor = normalise_pca_basis(unnormalisedPcaBasisColor, eigenvaluesColor);
	PcaModel colorModel(meanColor, normalisedPcaBasisColor, eigenvaluesColor, triangleList);

	modelFile.close();

	// Load the isomap with texture coordinates if a filename has been given:
	std::vector<cv::Vec2f> texCoords;
	if (!isomap_file.empty()) {
		texCoords = load_isomap(isomap_file);
		if (shapeModel.get_data_dimension() / 3.0f != texCoords.size()) {
			std::string errorMessage("Error, wrong number of texture coordinates. Don't have the same number of texcoords than the shape model has vertices.");
			std::cout << errorMessage << std::endl;
			throw std::runtime_error(errorMessage);
		}
	}

	return MorphableModel(shapeModel, colorModel, texCoords);
};

/**
 * Load a set of 2D texture coordinates pre-generated by the isomap algorithm.
 * After loading, we rescale the coordinates to [0, 1] x [0, 1].
 *
 * @param[in] isomapFile Path to an isomap file containing texture coordinates.
 * @return The 2D texture coordinates for every vertex.
 * @throws ...
 */
inline std::vector<cv::Vec2f> load_isomap(boost::filesystem::path isomapFile)
{
	using std::string;
	std::vector<float> xCoords, yCoords;
	string line;
	std::ifstream myfile(isomapFile.string());
	if (!myfile.is_open()) {
		string logMessage("The isomap file could not be opened. Did you specify a correct filename? " + isomapFile.string());
		throw std::runtime_error(logMessage);
	}
	else {
		while (getline(myfile, line))
		{
			std::istringstream iss(line);
			string x, y;
			iss >> x >> y;
			xCoords.push_back(std::stof(x));
			yCoords.push_back(std::stof(y));
		}
		myfile.close();
	}
	// Process the coordinates: Find the min/max and rescale to [0, 1] x [0, 1]
	auto minMaxX = std::minmax_element(begin(xCoords), end(xCoords)); // minMaxX is a pair, first=min, second=max
	auto minMaxY = std::minmax_element(begin(yCoords), end(yCoords));

	std::vector<cv::Vec2f> texCoords;
	float divisorX = *minMaxX.second - *minMaxX.first;
	float divisorY = *minMaxY.second - *minMaxY.first;
	for (int i = 0; i < xCoords.size(); ++i) {
		texCoords.push_back(cv::Vec2f((xCoords[i] - *minMaxX.first) / divisorX, 1.0f - (yCoords[i] - *minMaxY.first) / divisorY)); // We rescale to [0, 1] and at the same time flip the y-coords (because in the isomap, the coordinates are stored upside-down).
	}

	return texCoords;
};

	} /* namespace morphablemodel */
} /* namespace eos */

#endif /* IO_CVSSP_HPP_ */
<commit_msg>Changed load_scm_model to use Eigen::Matrix<commit_after>/*
 * eos - A 3D Morphable Model fitting library written in modern C++11/14.
 *
 * File: include/eos/morphablemodel/io/cvssp.hpp
 *
 * Copyright 2014, 2015 Patrik Huber
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#pragma once

#ifndef IO_CVSSP_HPP_
#define IO_CVSSP_HPP_

#include "eos/morphablemodel/MorphableModel.hpp"

#include "opencv2/core/core.hpp"

#include "boost/filesystem/path.hpp"

#include <vector>
#include <iostream>

namespace eos {
	namespace morphablemodel {

// Forward declaration
std::vector<std::array<double, 2>> load_isomap(boost::filesystem::path isomap_file);

/**
 * Load a shape or color model from a .scm file containing
 * a Morphable Model in the Surrey format. CVSSP's software
 * internally trains and stores the model in this custom binary
 * format and this class provides means to load them.
 *
 * Note on multi-resolution models: The landmarks to vertex-id mapping is
 * always the same. The lowest resolution model has all the landmarks defined
 * and for the higher resolutions, the mesh is divided from that on.
 * Note: For new landmarks we add, this might not be the case if we add them
 * in the highest resolution model, so take care!
 *
 * - The pcaBasis matrix stored in the file and loaded is the orthogonal PCA basis, i.e. it is not normalised by the eigenvalues.
 *
 * @param[in] model_filename A binary .scm-file containing the model.
 * @param[in] isomap_file An optional path to an isomap containing texture coordinates.
 * @return The Morphable Model loaded from the file.
 * @throws ...
 */
inline MorphableModel load_scm_model(boost::filesystem::path model_filename, boost::filesystem::path isomap_file = boost::filesystem::path())
{
	using cv::Mat;
	if (sizeof(unsigned int) != 4) { // note/todo: maybe use uint32 or similar instead? Yep, but still we could encounter endianness-trouble.
		std::cout << "Warning: We're reading 4 Bytes from the file but sizeof(unsigned int) != 4. Check the code/behaviour." << std::endl;
	}
	if (sizeof(double) != 8) {
		std::cout << "Warning: We're reading 8 Bytes from the file but sizeof(double) != 8. Check the code/behaviour." << std::endl;
	}

	std::ifstream modelFile(model_filename.string(), std::ios::binary);
	if (!modelFile.is_open()) {
		std::string msg("Unable to open model file: " + model_filename.string());
		std::cout << msg << std::endl;
		throw std::runtime_error(msg);
	}

	// Reading the shape model
	// Read (reference?) num triangles and vertices
	unsigned int numVertices = 0;
	unsigned int numTriangles = 0;
	modelFile.read(reinterpret_cast<char*>(&numVertices), 4); // 1 char = 1 byte. uint32=4bytes. float64=8bytes.
	modelFile.read(reinterpret_cast<char*>(&numTriangles), 4);

	// Read triangles
	std::vector<std::array<int, 3>> triangleList;

	triangleList.resize(numTriangles);
	unsigned int v0, v1, v2;
	for (unsigned int i = 0; i < numTriangles; ++i) {
		v0 = v1 = v2 = 0;
		modelFile.read(reinterpret_cast<char*>(&v0), 4);	// would be nice to pass a &vector and do it in one
		modelFile.read(reinterpret_cast<char*>(&v1), 4);	// go, but didn't work. Maybe a cv::Mat would work?
		modelFile.read(reinterpret_cast<char*>(&v2), 4);
		triangleList[i][0] = v0;
		triangleList[i][1] = v1;
		triangleList[i][2] = v2;
	}

	// Read number of rows and columns of the shape projection matrix (pcaBasis)
	unsigned int numShapePcaCoeffs = 0;
	unsigned int numShapeDims = 0;	// dimension of the shape vector (3*numVertices)
	modelFile.read(reinterpret_cast<char*>(&numShapePcaCoeffs), 4);
	modelFile.read(reinterpret_cast<char*>(&numShapeDims), 4);

	if (3 * numVertices != numShapeDims) {
		std::cout << "Warning: Number of shape dimensions is not equal to three times the number of vertices. Something will probably go wrong during the loading." << std::endl;
	}

	// Read shape projection matrix
	Mat unnormalisedPcaBasisShape(numShapeDims, numShapePcaCoeffs, CV_32FC1); // m x n (rows x cols) = numShapeDims x numShapePcaCoeffs
	std::cout << "Loading shape PCA basis matrix with " << unnormalisedPcaBasisShape.rows << " rows and " << unnormalisedPcaBasisShape.cols << " cols." << std::endl;
	for (unsigned int col = 0; col < numShapePcaCoeffs; ++col) {
		for (unsigned int row = 0; row < numShapeDims; ++row) {
			double var = 0.0;
			modelFile.read(reinterpret_cast<char*>(&var), 8);
			unnormalisedPcaBasisShape.at<float>(row, col) = static_cast<float>(var);
		}
	}

	// Read mean shape vector
	unsigned int numMean = 0; // dimension of the mean (3*numVertices)
	modelFile.read(reinterpret_cast<char*>(&numMean), 4);
	if (numMean != numShapeDims) {
		std::cout << "Warning: Number of shape dimensions is not equal to the number of dimensions of the mean. Something will probably go wrong during the loading." << std::endl;
	}
	Mat meanShape(numMean, 1, CV_32FC1);
	unsigned int counter = 0;
	double vd0, vd1, vd2;
	for (unsigned int i = 0; i < numMean / 3; ++i) {
		vd0 = vd1 = vd2 = 0.0;
		modelFile.read(reinterpret_cast<char*>(&vd0), 8);
		modelFile.read(reinterpret_cast<char*>(&vd1), 8);
		modelFile.read(reinterpret_cast<char*>(&vd2), 8);
		meanShape.at<float>(counter, 0) = static_cast<float>(vd0);
		++counter;
		meanShape.at<float>(counter, 0) = static_cast<float>(vd1);
		++counter;
		meanShape.at<float>(counter, 0) = static_cast<float>(vd2);
		++counter;
	}

	// Read shape eigenvalues
	unsigned int numEigenValsShape = 0;
	modelFile.read(reinterpret_cast<char*>(&numEigenValsShape), 4);
	if (numEigenValsShape != numShapePcaCoeffs) {
		std::cout << "Warning: Number of coefficients in the PCA basis matrix is not equal to the number of eigenvalues. Something will probably go wrong during the loading." << std::endl;
	}
	Mat eigenvaluesShape(numEigenValsShape, 1, CV_32FC1);
	for (unsigned int i = 0; i < numEigenValsShape; ++i) {
		double var = 0.0;
		modelFile.read(reinterpret_cast<char*>(&var), 8);
		eigenvaluesShape.at<float>(i, 0) = static_cast<float>(var);
	}

	// We read the unnormalised basis from the file. Now let's normalise it and store the normalised basis separately.
	Mat normalisedPcaBasisShape = normalise_pca_basis(unnormalisedPcaBasisShape, eigenvaluesShape);
	PcaModel shapeModel(meanShape, normalisedPcaBasisShape, eigenvaluesShape, triangleList);

	// Reading the color model
	// Read number of rows and columns of projection matrix
	unsigned int numTexturePcaCoeffs = 0;
	unsigned int numTextureDims = 0;
	modelFile.read(reinterpret_cast<char*>(&numTexturePcaCoeffs), 4);
	modelFile.read(reinterpret_cast<char*>(&numTextureDims), 4);
	// Read color projection matrix
	Mat unnormalisedPcaBasisColor(numTextureDims, numTexturePcaCoeffs, CV_32FC1);
	std::cout << "Loading color PCA basis matrix with " << unnormalisedPcaBasisColor.rows << " rows and " << unnormalisedPcaBasisColor.cols << " cols." << std::endl;
	for (unsigned int col = 0; col < numTexturePcaCoeffs; ++col) {
		for (unsigned int row = 0; row < numTextureDims; ++row) {
			double var = 0.0;
			modelFile.read(reinterpret_cast<char*>(&var), 8);
			unnormalisedPcaBasisColor.at<float>(row, col) = static_cast<float>(var);
		}
	}

	// Read mean color vector
	unsigned int numMeanColor = 0; // dimension of the mean (3*numVertices)
	modelFile.read(reinterpret_cast<char*>(&numMeanColor), 4);
	Mat meanColor(numMeanColor, 1, CV_32FC1);
	counter = 0;
	for (unsigned int i = 0; i < numMeanColor / 3; ++i) {
		vd0 = vd1 = vd2 = 0.0;
		modelFile.read(reinterpret_cast<char*>(&vd0), 8); // order in hdf5: RGB. Order in OCV: BGR. But order in vertex.color: RGB
		modelFile.read(reinterpret_cast<char*>(&vd1), 8);
		modelFile.read(reinterpret_cast<char*>(&vd2), 8);
		meanColor.at<float>(counter, 0) = static_cast<float>(vd0);
		++counter;
		meanColor.at<float>(counter, 0) = static_cast<float>(vd1);
		++counter;
		meanColor.at<float>(counter, 0) = static_cast<float>(vd2);
		++counter;
	}

	// Read color eigenvalues
	unsigned int numEigenValsColor = 0;
	modelFile.read(reinterpret_cast<char*>(&numEigenValsColor), 4);
	Mat eigenvaluesColor(numEigenValsColor, 1, CV_32FC1);
	for (unsigned int i = 0; i < numEigenValsColor; ++i) {
		double var = 0.0;
		modelFile.read(reinterpret_cast<char*>(&var), 8);
		eigenvaluesColor.at<float>(i, 0) = static_cast<float>(var);
	}

	// We read the unnormalised basis from the file. Now let's normalise it and store the normalised basis separately.
	Mat normalisedPcaBasisColor = normalise_pca_basis(unnormalisedPcaBasisColor, eigenvaluesColor);
	PcaModel colorModel(meanColor, normalisedPcaBasisColor, eigenvaluesColor, triangleList);

	modelFile.close();

	// Load the isomap with texture coordinates if a filename has been given:
	std::vector<cv::Vec2f> texCoords;
	if (!isomap_file.empty()) {
		texCoords = load_isomap(isomap_file);
		if (shapeModel.get_data_dimension() / 3.0f != texCoords.size()) {
			std::string errorMessage("Error, wrong number of texture coordinates. Don't have the same number of texcoords than the shape model has vertices.");
			std::cout << errorMessage << std::endl;
			throw std::runtime_error(errorMessage);
		}
	}

	return MorphableModel(shapeModel, colorModel, texCoords);
};

/**
 * Load a set of 2D texture coordinates pre-generated by the isomap algorithm.
 * After loading, we rescale the coordinates to [0, 1] x [0, 1].
 *
 * @param[in] isomapFile Path to an isomap file containing texture coordinates.
 * @return The 2D texture coordinates for every vertex.
 * @throws ...
 */
inline std::vector<cv::Vec2f> load_isomap(boost::filesystem::path isomapFile)
{
	using std::string;
	std::vector<float> xCoords, yCoords;
	string line;
	std::ifstream myfile(isomapFile.string());
	if (!myfile.is_open()) {
		string logMessage("The isomap file could not be opened. Did you specify a correct filename? " + isomapFile.string());
		throw std::runtime_error(logMessage);
	}
	else {
		while (getline(myfile, line))
		{
			std::istringstream iss(line);
			string x, y;
			iss >> x >> y;
			xCoords.push_back(std::stof(x));
			yCoords.push_back(std::stof(y));
		}
		myfile.close();
	}
	// Process the coordinates: Find the min/max and rescale to [0, 1] x [0, 1]
	auto minMaxX = std::minmax_element(begin(xCoords), end(xCoords)); // minMaxX is a pair, first=min, second=max
	auto minMaxY = std::minmax_element(begin(yCoords), end(yCoords));

	std::vector<cv::Vec2f> texCoords;
	float divisorX = *minMaxX.second - *minMaxX.first;
	float divisorY = *minMaxY.second - *minMaxY.first;
	for (int i = 0; i < xCoords.size(); ++i) {
		texCoords.push_back(cv::Vec2f((xCoords[i] - *minMaxX.first) / divisorX, 1.0f - (yCoords[i] - *minMaxY.first) / divisorY)); // We rescale to [0, 1] and at the same time flip the y-coords (because in the isomap, the coordinates are stored upside-down).
	}

	return texCoords;
};

	} /* namespace morphablemodel */
} /* namespace eos */

#endif /* IO_CVSSP_HPP_ */
<|endoftext|>


<commit_before>/*
 * This is part of the FL library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com)
 * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com)
 *
 * Max-Planck Institute for Intelligent Systems, AMD Lab
 * University of Southern California, CLMC Lab
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file linear_algebra.hpp
 * \date January 2015
 * \author Jan Issac (jan.issac@gmail.com)
 * \author Manuel Wuthrich (manuel.wuthrich@gmail.com)
 */

#ifndef FL__UTIL__MATH__LINEAR_ALGEBRA_HPP
#define FL__UTIL__MATH__LINEAR_ALGEBRA_HPP

#include <Eigen/Dense>

#include <cmath>
#include <vector>

namespace fl
{

/**
 * \ingroup linear_algebra
 *
 * Blockweise matrix inversion using the Sherman-Morrision-Woodbury
 * indentity given that \f$\Sigma^{-1}_{aa}\f$ of
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1}
 * \f$
 *
 * is already available.
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1} =
 * \begin{pmatrix} \Lambda_{aa} & \Lambda_{ab} \\
 *                 \Lambda_{ba} & \Lambda_{bb} \end{pmatrix} = \Lambda
 * \f$
 *
 * \param [in]  A_inv   \f$ \Sigma^{-1}_{aa} \f$
 * \param [in]  B       \f$ \Sigma_{ab} \f$
 * \param [in]  C       \f$ \Sigma_{ba} \f$
 * \param [in]  D       \f$ \Sigma_{bb} \f$
 * \param [out] L_A     \f$ \Lambda_{aa} \f$
 * \param [out] L_B     \f$ \Lambda_{ab} \f$
 * \param [out] L_C     \f$ \Lambda_{ba} \f$
 * \param [out] L_D     \f$ \Lambda_{bb} \f$
 */
template <typename MatrixAInv,
          typename MatrixB,
          typename MatrixC,
          typename MatrixD,
          typename MatrixLA,
          typename MatrixLB,
          typename MatrixLC,
          typename MatrixLD>
inline void smw_inverse(const MatrixAInv& A_inv,
                  const MatrixB& B,
                  const MatrixC& C,
                  const MatrixD& D,
                  MatrixLA& L_A,
                  MatrixLB& L_B,
                  MatrixLC& L_C,
                  MatrixLD& L_D)
{
    auto&& AinvB = (A_inv * B).eval();

    L_D = (D - C * AinvB).inverse();
    L_C = -(L_D * C * A_inv).eval();
    L_A = A_inv - AinvB * L_C;
    L_B = L_C.transpose();
}

//    auto CAinv = C * A_inv;
//    auto AinvB = (A_inv * B).eval();

//    L_D = (D - C * AinvB).inverse();

//    auto L_D_CAinv = L_D * CAinv;

//    L_A = A_inv + AinvB * L_D_CAinv;

//    L_C = -L_D_CAinv;

//    L_B = L_C.transpose();

/**
 * \ingroup linear_algebra
 *
 * Blockweise matrix inversion using the Sherman-Morrision-Woodbury
 * indentity given that \f$\Sigma^{-1}_{aa}\f$ of
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1}
 * \f$
 *
 * is already available.
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1} =
 * \begin{pmatrix} \Lambda_{aa} & \Lambda_{ab} \\
 *                 \Lambda_{ba} & \Lambda_{bb} \end{pmatrix} = \Lambda
 * \f$
 *
 * \param [in]  A_inv   \f$ \Sigma^{-1}_{aa} \f$
 * \param [in]  B       \f$ \Sigma_{ab} \f$
 * \param [in]  C       \f$ \Sigma_{ba} \f$
 * \param [in]  D       \f$ \Sigma_{bb} \f$
 * \param [out] L_A     \f$ \Lambda_{aa} \f$
 * \param [out] L_B     \f$ \Lambda_{ab} \f$
 * \param [out] L_C     \f$ \Lambda_{ba} \f$
 * \param [out] L_D     \f$ \Lambda_{bb} \f$
 * \param [out] L       \f$ \Lambda \f$
 */
template <typename MatrixAInv,
          typename MatrixB,
          typename MatrixC,
          typename MatrixD,
          typename MatrixLA,
          typename MatrixLB,
          typename MatrixLC,
          typename MatrixLD,
          typename ResultMatrix>
inline void smw_inverse(const MatrixAInv& A_inv,
                  const MatrixB& B,
                  const MatrixC& C,
                  const MatrixD& D,
                  MatrixLA& L_A,
                  MatrixLB& L_B,
                  MatrixLC& L_C,
                  MatrixLD& L_D,
                  ResultMatrix& L)
{
     smw_inverse(A_inv, B, C, D, L_A, L_B, L_C, L_D);

    L.resize(L_A.rows() + L_C.rows(), L_A.cols() + L_B.cols());

    L.block(0,          0,          L_A.rows(), L_A.cols()) = L_A;
    L.block(0,          L_A.cols(), L_B.rows(), L_B.cols()) = L_B;
    L.block(L_A.rows(), 0,          L_C.rows(), L_C.cols()) = L_C;
    L.block(L_A.rows(), L_A.cols(), L_D.rows(), L_D.cols()) = L_D;
}

/**
 * \ingroup linear_algebra
 *
 * Blockweise matrix inversion using the Sherman-Morrision-Woodbury
 * indentity given that \f$\Sigma^{-1}_{aa}\f$ of
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1}
 * \f$
 *
 * is already available.
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1} =
 * \begin{pmatrix} \Lambda_{aa} & \Lambda_{ab} \\
 *                 \Lambda_{ba} & \Lambda_{bb} \end{pmatrix} = \Lambda
 * \f$
 *
 * \param [in]  A_inv   \f$ \Sigma^{-1}_{aa} \f$
 * \param [in]  B       \f$ \Sigma_{ab} \f$
 * \param [in]  C       \f$ \Sigma_{ba} \f$
 * \param [in]  D       \f$ \Sigma_{bb} \f$
 * \param [out] L       \f$ \Lambda \f$
 */
template <typename MatrixAInv,
          typename MatrixB,
          typename MatrixC,
          typename MatrixD,
          typename ResultMatrix>
inline void smw_inverse(const MatrixAInv& A_inv,
                  const MatrixB& B,
                  const MatrixC& C,
                  const MatrixD& D,
                  ResultMatrix& L)
{
    MatrixAInv L_A;
    MatrixB L_B;
    MatrixC L_C;
    MatrixD L_D;

    smw_inverse(A_inv, B, C, D, L_A, L_B, L_C, L_D, L);
}

/**
 * \ingroup linear_algebra
 *
 * Normalizes the values of input vector such that their sum is equal to the
 * specified \c sum. For instance, any convex combination requires that the
 * weights of the weighted sum sums up to 1.
 */
template <typename T>
inline std::vector<T> normalize(const std::vector<T>& input, T sum)
{
    T old_sum = 0;
    for(size_t i = 0; i < input.size(); i++)
    {
        old_sum += input[i];
    }
    T factor = sum/old_sum;

    std::vector<T> output(input.size());
    for(size_t i = 0; i < input.size(); i++)
    {
        output[i] = factor*input[i];
    }

    return output;
}

/**
 * \brief Constructs the quaternion matrix for the specified quaternion vetcor
 * \ingroup linear_algebra
 *
 * \param q_xyzw  Quaternion vector
 *
 * \return Matrix representation of the quaternion vector
 */
inline Eigen::Matrix<double, 4, 3> quaternion_matrix(
        const Eigen::Matrix<double, 4, 1>& q_xyzw)
{
    Eigen::Matrix<double, 4, 3> Q;
    Q << q_xyzw(3),  q_xyzw(2), -q_xyzw(1),
        -q_xyzw(2),  q_xyzw(3),  q_xyzw(0),
         q_xyzw(1), -q_xyzw(0),  q_xyzw(3),
        -q_xyzw(0), -q_xyzw(1), -q_xyzw(2);

    return 0.5*Q;
}

/**
 * \ingroup linear_algebra
 *
 */
template <typename RegularMatrix, typename SquareRootMatrix>
void square_root(const RegularMatrix& regular_matrix,
                 SquareRootMatrix& square_root)
{
    square_root = regular_matrix.llt().matrixL();
}

/**
 * \ingroup linear_algebra
 *
 */
template <typename RegularMatrix>
void sqrt_diagonal(const RegularMatrix& regular_matrix,
                        RegularMatrix& square_root)
{
    square_root = regular_matrix;
    for (size_t i = 0; i < square_root.rows(); ++i)
    {
        square_root(i, i) = std::sqrt(square_root(i, i));
    }
}

/**
 * \ingroup linear_algebra
 *
 */
template <typename RegularMatrix, typename SquareRootVector>
void sqrt_diagonal_vector(const RegularMatrix& regular_matrix,
                              SquareRootVector& square_root)
{
    square_root = regular_matrix;
    for (size_t i = 0; i < square_root.rows(); ++i)
    {
        square_root(i, 0) = std::sqrt(square_root(i, 0));
    }
}

/**
 * \ingroup linear_algebra
 *
 */
template <typename SrcDiagonalMatrix, typename DestDiagonalMatrix>
void invert_diagonal_vector(const SrcDiagonalMatrix& diagonal,
                            DestDiagonalMatrix& diagonal_inverse)
{
    diagonal_inverse = diagonal.array().cwiseInverse();

//    diagonal_inverse.resize(diagonal.size());
//    for (int i = 0; i < diagonal.size(); ++i)
//    {
//        diagonal_inverse(i) = 1./diagonal(i);
//    }
}

/**
 * \ingroup linear_algebra
 *
 * Computes the Frobenius norm of a given real matrix.
 *
 * The Frobenius norm is given by
 *
 * \f$
 *   \| A \|_F := \sqrt{\sum\limits_{i=1}^m \sum\limits_{j=1}^n |a_{ij} |^2 }
 * \f$
 */
template <typename Derived>
double frobenius_norm(const Eigen::MatrixBase<Derived>& a)
{
    return std::sqrt(a.array().abs().square().sum());
}

/**
 * \ingroup linear_algebra
 * Checks whether two matrices of the same size are similiar w.r.t. some
 * \f$\epsilon\f$.
 *
 * The similarity check is performed based on the Frobenius Norm
 * (\ref frobenius_norm):
 *
 * For any real matrix set \f$M \subseteq \mathbb{R}^{m\times n} \f$
 *
 * \f$ \text{are_similar}: (M \times M) \mapsto \{0, 1\}\f$
 *
 * \f$ \text{are_similar}(a, b)
 *     := \mathbf{1}_{[0, \epsilon)}\left(\| a - b \|_F\right)\f$
 * with \f$ a, b \in M\f$ and the indicator function
 *
 * \f$ \mathbf{1}_{[0, \epsilon)}(x) :=
 *     \begin{cases} 1 & x \in [0, \epsilon)
 *                  \\ 0, & \text{otherwise} \end{cases}\f$
 */
template <typename DerivedA, typename DerivedB>
bool are_similar(const Eigen::MatrixBase<DerivedA>& a,
                 const Eigen::MatrixBase<DerivedB>& b,
                 const double epsilon = 1.e-6)
{
    return frobenius_norm(a - b) < epsilon;
}

/**
 * \ingroup linear_algebra
 *
 * Robust decomposition of M = L*L^T for positive semidefinite matrices
 */
template <typename Scalar, int Size>
Eigen::Matrix<Scalar, Size, Size>
matrix_sqrt(Eigen::Matrix<Scalar, Size, Size> M)
{
    typedef Eigen::Matrix<Scalar, Size, Size>   Matrix;
    typedef Eigen::Matrix<Scalar, Size, 1>      Vector;

    Eigen::LDLT<Matrix> ldlt;
    ldlt.compute(M);
    Vector D_sqrt = ldlt.vectorD();
    for(int i = 0; i < D_sqrt.rows(); ++i)
    {
        D_sqrt(i) = std::sqrt(std::fabs(D_sqrt(i)));
    }

    return ldlt.transpositionsP().transpose()
                    * (Matrix) ldlt.matrixL()
                    * D_sqrt.asDiagonal();
}

}

#endif
<commit_msg>Addded is_diagonal(x) diagonal matrix check function<commit_after>/*
 * This is part of the FL library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com)
 * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com)
 *
 * Max-Planck Institute for Intelligent Systems, AMD Lab
 * University of Southern California, CLMC Lab
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file linear_algebra.hpp
 * \date January 2015
 * \author Jan Issac (jan.issac@gmail.com)
 * \author Manuel Wuthrich (manuel.wuthrich@gmail.com)
 */

#ifndef FL__UTIL__MATH__LINEAR_ALGEBRA_HPP
#define FL__UTIL__MATH__LINEAR_ALGEBRA_HPP

#include <Eigen/Dense>

#include <cmath>
#include <vector>

namespace fl
{

/**
 * \ingroup linear_algebra
 *
 * Blockweise matrix inversion using the Sherman-Morrision-Woodbury
 * indentity given that \f$\Sigma^{-1}_{aa}\f$ of
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1}
 * \f$
 *
 * is already available.
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1} =
 * \begin{pmatrix} \Lambda_{aa} & \Lambda_{ab} \\
 *                 \Lambda_{ba} & \Lambda_{bb} \end{pmatrix} = \Lambda
 * \f$
 *
 * \param [in]  A_inv   \f$ \Sigma^{-1}_{aa} \f$
 * \param [in]  B       \f$ \Sigma_{ab} \f$
 * \param [in]  C       \f$ \Sigma_{ba} \f$
 * \param [in]  D       \f$ \Sigma_{bb} \f$
 * \param [out] L_A     \f$ \Lambda_{aa} \f$
 * \param [out] L_B     \f$ \Lambda_{ab} \f$
 * \param [out] L_C     \f$ \Lambda_{ba} \f$
 * \param [out] L_D     \f$ \Lambda_{bb} \f$
 */
template <typename MatrixAInv,
          typename MatrixB,
          typename MatrixC,
          typename MatrixD,
          typename MatrixLA,
          typename MatrixLB,
          typename MatrixLC,
          typename MatrixLD>
inline void smw_inverse(const MatrixAInv& A_inv,
                  const MatrixB& B,
                  const MatrixC& C,
                  const MatrixD& D,
                  MatrixLA& L_A,
                  MatrixLB& L_B,
                  MatrixLC& L_C,
                  MatrixLD& L_D)
{
    auto&& AinvB = (A_inv * B).eval();

    L_D = (D - C * AinvB).inverse();
    L_C = -(L_D * C * A_inv).eval();
    L_A = A_inv - AinvB * L_C;
    L_B = L_C.transpose();
}

//    auto CAinv = C * A_inv;
//    auto AinvB = (A_inv * B).eval();

//    L_D = (D - C * AinvB).inverse();

//    auto L_D_CAinv = L_D * CAinv;

//    L_A = A_inv + AinvB * L_D_CAinv;

//    L_C = -L_D_CAinv;

//    L_B = L_C.transpose();

/**
 * \ingroup linear_algebra
 *
 * Blockweise matrix inversion using the Sherman-Morrision-Woodbury
 * indentity given that \f$\Sigma^{-1}_{aa}\f$ of
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1}
 * \f$
 *
 * is already available.
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1} =
 * \begin{pmatrix} \Lambda_{aa} & \Lambda_{ab} \\
 *                 \Lambda_{ba} & \Lambda_{bb} \end{pmatrix} = \Lambda
 * \f$
 *
 * \param [in]  A_inv   \f$ \Sigma^{-1}_{aa} \f$
 * \param [in]  B       \f$ \Sigma_{ab} \f$
 * \param [in]  C       \f$ \Sigma_{ba} \f$
 * \param [in]  D       \f$ \Sigma_{bb} \f$
 * \param [out] L_A     \f$ \Lambda_{aa} \f$
 * \param [out] L_B     \f$ \Lambda_{ab} \f$
 * \param [out] L_C     \f$ \Lambda_{ba} \f$
 * \param [out] L_D     \f$ \Lambda_{bb} \f$
 * \param [out] L       \f$ \Lambda \f$
 */
template <typename MatrixAInv,
          typename MatrixB,
          typename MatrixC,
          typename MatrixD,
          typename MatrixLA,
          typename MatrixLB,
          typename MatrixLC,
          typename MatrixLD,
          typename ResultMatrix>
inline void smw_inverse(const MatrixAInv& A_inv,
                  const MatrixB& B,
                  const MatrixC& C,
                  const MatrixD& D,
                  MatrixLA& L_A,
                  MatrixLB& L_B,
                  MatrixLC& L_C,
                  MatrixLD& L_D,
                  ResultMatrix& L)
{
     smw_inverse(A_inv, B, C, D, L_A, L_B, L_C, L_D);

    L.resize(L_A.rows() + L_C.rows(), L_A.cols() + L_B.cols());

    L.block(0,          0,          L_A.rows(), L_A.cols()) = L_A;
    L.block(0,          L_A.cols(), L_B.rows(), L_B.cols()) = L_B;
    L.block(L_A.rows(), 0,          L_C.rows(), L_C.cols()) = L_C;
    L.block(L_A.rows(), L_A.cols(), L_D.rows(), L_D.cols()) = L_D;
}

/**
 * \ingroup linear_algebra
 *
 * Blockweise matrix inversion using the Sherman-Morrision-Woodbury
 * indentity given that \f$\Sigma^{-1}_{aa}\f$ of
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1}
 * \f$
 *
 * is already available.
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1} =
 * \begin{pmatrix} \Lambda_{aa} & \Lambda_{ab} \\
 *                 \Lambda_{ba} & \Lambda_{bb} \end{pmatrix} = \Lambda
 * \f$
 *
 * \param [in]  A_inv   \f$ \Sigma^{-1}_{aa} \f$
 * \param [in]  B       \f$ \Sigma_{ab} \f$
 * \param [in]  C       \f$ \Sigma_{ba} \f$
 * \param [in]  D       \f$ \Sigma_{bb} \f$
 * \param [out] L       \f$ \Lambda \f$
 */
template <typename MatrixAInv,
          typename MatrixB,
          typename MatrixC,
          typename MatrixD,
          typename ResultMatrix>
inline void smw_inverse(const MatrixAInv& A_inv,
                  const MatrixB& B,
                  const MatrixC& C,
                  const MatrixD& D,
                  ResultMatrix& L)
{
    MatrixAInv L_A;
    MatrixB L_B;
    MatrixC L_C;
    MatrixD L_D;

    smw_inverse(A_inv, B, C, D, L_A, L_B, L_C, L_D, L);
}

/**
 * \ingroup linear_algebra
 *
 * Normalizes the values of input vector such that their sum is equal to the
 * specified \c sum. For instance, any convex combination requires that the
 * weights of the weighted sum sums up to 1.
 */
template <typename T>
inline std::vector<T> normalize(const std::vector<T>& input, T sum)
{
    T old_sum = 0;
    for(size_t i = 0; i < input.size(); i++)
    {
        old_sum += input[i];
    }
    T factor = sum/old_sum;

    std::vector<T> output(input.size());
    for(size_t i = 0; i < input.size(); i++)
    {
        output[i] = factor*input[i];
    }

    return output;
}

/**
 * \brief Constructs the quaternion matrix for the specified quaternion vetcor
 * \ingroup linear_algebra
 *
 * \param q_xyzw  Quaternion vector
 *
 * \return Matrix representation of the quaternion vector
 */
inline Eigen::Matrix<double, 4, 3> quaternion_matrix(
        const Eigen::Matrix<double, 4, 1>& q_xyzw)
{
    Eigen::Matrix<double, 4, 3> Q;
    Q << q_xyzw(3),  q_xyzw(2), -q_xyzw(1),
        -q_xyzw(2),  q_xyzw(3),  q_xyzw(0),
         q_xyzw(1), -q_xyzw(0),  q_xyzw(3),
        -q_xyzw(0), -q_xyzw(1), -q_xyzw(2);

    return 0.5*Q;
}

/**
 * \ingroup linear_algebra
 *
 */
template <typename RegularMatrix, typename SquareRootMatrix>
void square_root(const RegularMatrix& regular_matrix,
                 SquareRootMatrix& square_root)
{
    square_root = regular_matrix.llt().matrixL();
}

/**
 * \ingroup linear_algebra
 *
 */
template <typename RegularMatrix>
void sqrt_diagonal(const RegularMatrix& regular_matrix,
                        RegularMatrix& square_root)
{
    square_root = regular_matrix;
    for (size_t i = 0; i < square_root.rows(); ++i)
    {
        square_root(i, i) = std::sqrt(square_root(i, i));
    }
}

/**
 * \ingroup linear_algebra
 *
 */
template <typename RegularMatrix, typename SquareRootVector>
void sqrt_diagonal_vector(const RegularMatrix& regular_matrix,
                              SquareRootVector& square_root)
{
    square_root = regular_matrix;
    for (size_t i = 0; i < square_root.rows(); ++i)
    {
        square_root(i, 0) = std::sqrt(square_root(i, 0));
    }
}

/**
 * \ingroup linear_algebra
 *
 */
template <typename SrcDiagonalMatrix, typename DestDiagonalMatrix>
void invert_diagonal_vector(const SrcDiagonalMatrix& diagonal,
                            DestDiagonalMatrix& diagonal_inverse)
{
    diagonal_inverse = diagonal.array().cwiseInverse();

//    diagonal_inverse.resize(diagonal.size());
//    for (int i = 0; i < diagonal.size(); ++i)
//    {
//        diagonal_inverse(i) = 1./diagonal(i);
//    }
}

/**
 * \ingroup linear_algebra
 *
 * Computes the Frobenius norm of a given real matrix.
 *
 * The Frobenius norm is given by
 *
 * \f$
 *   \| A \|_F := \sqrt{\sum\limits_{i=1}^m \sum\limits_{j=1}^n |a_{ij} |^2 }
 * \f$
 */
template <typename Derived>
double frobenius_norm(const Eigen::MatrixBase<Derived>& a)
{
    return std::sqrt(a.array().abs().square().sum());
}

/**
 * \ingroup linear_algebra
 * Checks whether two matrices of the same size are similiar w.r.t. some
 * \f$\epsilon\f$.
 *
 * The similarity check is performed based on the Frobenius Norm
 * (\ref frobenius_norm):
 *
 * For any real matrix set \f$M \subseteq \mathbb{R}^{m\times n} \f$
 *
 * \f$ \text{are_similar}: (M \times M) \mapsto \{0, 1\}\f$
 *
 * \f$ \text{are_similar}(a, b)
 *     := \mathbf{1}_{[0, \epsilon)}\left(\| a - b \|_F\right)\f$
 * with \f$ a, b \in M\f$ and the indicator function
 *
 * \f$ \mathbf{1}_{[0, \epsilon)}(x) :=
 *     \begin{cases} 1 & x \in [0, \epsilon)
 *                  \\ 0, & \text{otherwise} \end{cases}\f$
 */
template <typename DerivedA, typename DerivedB>
bool are_similar(const Eigen::MatrixBase<DerivedA>& a,
                 const Eigen::MatrixBase<DerivedB>& b,
                 const double epsilon = 1.e-6)
{
    return frobenius_norm(a - b) < epsilon;
}

/**
 * \ingroup linear_algebra
 *
 * Robust decomposition of M = L*L^T for positive semidefinite matrices
 */
template <typename Scalar, int Size>
Eigen::Matrix<Scalar, Size, Size>
matrix_sqrt(Eigen::Matrix<Scalar, Size, Size> M)
{
    typedef Eigen::Matrix<Scalar, Size, Size>   Matrix;
    typedef Eigen::Matrix<Scalar, Size, 1>      Vector;

    Eigen::LDLT<Matrix> ldlt;
    ldlt.compute(M);
    Vector D_sqrt = ldlt.vectorD();
    for(int i = 0; i < D_sqrt.rows(); ++i)
    {
        D_sqrt(i) = std::sqrt(std::fabs(D_sqrt(i)));
    }

    return ldlt.transpositionsP().transpose()
                    * (Matrix) ldlt.matrixL()
                    * D_sqrt.asDiagonal();
}

/**
 * Checks wheather the specified dense matrix has a diagonal form
 *
 * \return true if diagonal, false otherwise
 */
template <typename Derived>
bool is_diagonal(const Eigen::MatrixBase<Derived>& m)
{
    Derived z = m;
    z -= z.diagonal().asDiagonal();

    return z.isZero();
}

}

#endif
<|endoftext|>


<commit_before>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2011 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_EXPRESSION_EVALUATOR_HPP
#define MAPNIK_EXPRESSION_EVALUATOR_HPP

// boost
#include <boost/regex.hpp>
//#include <boost/regex/config.hpp>
#if defined(BOOST_REGEX_HAS_ICU)
#include <boost/regex/icu.hpp>
#endif

namespace mapnik
{
template <typename T0, typename T1>
struct evaluate : boost::static_visitor<T1>
{
    typedef T0 feature_type;
    typedef T1 value_type;

    explicit evaluate(feature_type const& f)
        : feature_(f) {}

    value_type operator() (value_type x) const { return x; }
    value_type operator() (attribute const& attr) const
    {
        return attr.value<value_type,feature_type>(feature_);
    }

    value_type operator() (binary_node<tags::logical_and> const & x) const
    {
        return (boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.left).to_bool())
            && (boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.right).to_bool());
    }

    value_type operator() (binary_node<tags::logical_or> const & x) const
    {
        return (boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.left).to_bool())
            || (boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.right).to_bool());
    }

    template <typename Tag>
    value_type operator() (binary_node<Tag> const& x) const
    {
        typename make_op<Tag>::type operation;
        return operation(boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.left),
                         boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.right));
    }

    template <typename Tag>
    value_type operator() (unary_node<Tag> const& x) const
    {
        return ! (boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.expr).to_bool());
    }

    value_type operator() (regex_match_node const& x) const
    {
        value_type v = boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.expr);
#if defined(BOOST_REGEX_HAS_ICU)
        return boost::u32regex_match(v.to_unicode(),x.pattern);
#else
        return boost::regex_match(v.to_string(),x.pattern);
#endif

    }

    value_type operator() (regex_replace_node const& x) const
    {
        value_type v = boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.expr);
#if defined(BOOST_REGEX_HAS_ICU)
        return boost::u32regex_replace(v.to_unicode(),x.pattern,x.format);
#else
        std::string repl = boost::regex_replace(v.to_string(),x.pattern,x.format);
        mapnik::transcoder tr_("utf8");
        return tr_.transcode(repl.c_str());
#endif
    }

    feature_type const& feature_;
};

}

#endif // MAPNIK_EXPRESSION_EVALUATOR_HPP
<commit_msg>formatting<commit_after>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2011 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_EXPRESSION_EVALUATOR_HPP
#define MAPNIK_EXPRESSION_EVALUATOR_HPP

// boost
#include <boost/regex.hpp>
//#include <boost/regex/config.hpp>
#if defined(BOOST_REGEX_HAS_ICU)
#include <boost/regex/icu.hpp>
#endif

namespace mapnik
{
template <typename T0, typename T1>
struct evaluate : boost::static_visitor<T1>
{
    typedef T0 feature_type;
    typedef T1 value_type;

    explicit evaluate(feature_type const& f)
        : feature_(f) {}

    value_type operator() (value_type x) const
    {
        return x;
    }

    value_type operator() (attribute const& attr) const
    {
        return attr.value<value_type,feature_type>(feature_);
    }

    value_type operator() (binary_node<tags::logical_and> const & x) const
    {
        return (boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.left).to_bool())
            && (boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.right).to_bool());
    }

    value_type operator() (binary_node<tags::logical_or> const & x) const
    {
        return (boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.left).to_bool())
            || (boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.right).to_bool());
    }

    template <typename Tag>
    value_type operator() (binary_node<Tag> const& x) const
    {
        typename make_op<Tag>::type operation;
        return operation(boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.left),
                         boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.right));
    }

    template <typename Tag>
    value_type operator() (unary_node<Tag> const& x) const
    {
        return ! (boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.expr).to_bool());
    }

    value_type operator() (regex_match_node const& x) const
    {
        value_type v = boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.expr);
#if defined(BOOST_REGEX_HAS_ICU)
        return boost::u32regex_match(v.to_unicode(),x.pattern);
#else
        return boost::regex_match(v.to_string(),x.pattern);
#endif

    }

    value_type operator() (regex_replace_node const& x) const
    {
        value_type v = boost::apply_visitor(evaluate<feature_type,value_type>(feature_),x.expr);
#if defined(BOOST_REGEX_HAS_ICU)
        return boost::u32regex_replace(v.to_unicode(),x.pattern,x.format);
#else
        std::string repl = boost::regex_replace(v.to_string(),x.pattern,x.format);
        mapnik::transcoder tr_("utf8");
        return tr_.transcode(repl.c_str());
#endif
    }

    feature_type const& feature_;
};

}

#endif // MAPNIK_EXPRESSION_EVALUATOR_HPP
<|endoftext|>