<commit_before>/*
 * MSPT.cpp
 *
 *  Created on: 28 Oct 2015
 *      Author: hieu
 */

#include <cassert>
#include <boost/foreach.hpp>
#include "MSPT.h"
#include "../../PhraseBased/PhraseImpl.h"
#include "../../Phrase.h"
#include "../../System.h"
#include "../../Scores.h"
#include "../../InputPathsBase.h"
#include "../../legacy/InputFileStream.h"
#include "util/exception.hh"

#include "../../PhraseBased/InputPath.h"
#include "../../PhraseBased/TargetPhraseImpl.h"
#include "../../PhraseBased/TargetPhrases.h"
#include "../../PhraseBased/SentenceWithCandidates.h"

#include "../../SCFG/PhraseImpl.h"
#include "../../SCFG/TargetPhraseImpl.h"
#include "../../SCFG/InputPath.h"
#include "../../SCFG/Stack.h"
#include "../../SCFG/Stacks.h"
#include "../../SCFG/Manager.h"

using namespace std;

namespace Moses2
{

////////////////////////////////////////////////////////////////////////

MSPT::MSPT(size_t startInd, const std::string &line)
  :PhraseTable(startInd, line)
  ,m_rootPb(NULL)
  ,m_rootSCFG(NULL)
{
  ReadParameters();
}

MSPT::~MSPT()
{
  delete m_rootPb;
  delete m_rootSCFG;
}

// void MSPT::CreatePTForInput(string phraseTableString)
// {
//   FactorCollection &vocab = system.GetVocab();
//   MemPool &systemPool = system.GetSystemPool();
//   MemPool tmpSourcePool;

//   if (system.isPb) {
//     m_rootPb = new PBNODE();
//   } else {
//     m_rootSCFG = new SCFGNODE();
//     //cerr << "m_rootSCFG=" << m_rootSCFG << endl;
//   }

//   vector<string> toks;
//   size_t lineNum = 0;
//   InputFileStream strme(m_path);
//   string line;
//   while (getline(strme, line)) {
//     if (++lineNum % 1000000 == 0) {
//       cerr << lineNum << " ";
//     }
//     toks.clear();
//     TokenizeMultiCharSeparator(toks, line, "|||");
//     UTIL_THROW_IF2(toks.size() < 3, "Wrong format");
//     //cerr << "line=" << line << endl;
//     //cerr << "system.isPb=" << system.isPb << endl;

//     if (system.isPb) {
//       PhraseImpl *source = PhraseImpl::CreateFromString(tmpSourcePool, vocab, system,
//                            toks[0]);
//       //cerr << "created soure" << endl;
//       TargetPhraseImpl *target = TargetPhraseImpl::CreateFromString(systemPool, *this, system,
//                                  toks[1]);
//       //cerr << "created target" << endl;
//       target->GetScores().CreateFromString(toks[2], *this, system, true);
//       //cerr << "created scores:" << *target << endl;

//       if (toks.size() >= 4) {
//         //cerr << "alignstr=" << toks[3] << endl;
//         target->SetAlignmentInfo(toks[3]);
//       }

//       // properties
//       if (toks.size() == 7) {
//         //target->properties = (char*) system.systemPool.Allocate(toks[6].size() + 1);
//         //strcpy(target->properties, toks[6].c_str());
//       }

//       system.featureFunctions.EvaluateInIsolation(systemPool, system, *source,
//           *target);
//       //cerr << "EvaluateInIsolation:" << *target << endl;
//       m_rootPb->AddRule(m_input, *source, target);

//       //cerr << "target=" << target->Debug(system) << endl;
//     } else {
//       SCFG::PhraseImpl *source = SCFG::PhraseImpl::CreateFromString(tmpSourcePool, vocab, system,
//                                  toks[0]);
//       //cerr << "created source:" << *source << endl;
//       SCFG::TargetPhraseImpl *target = SCFG::TargetPhraseImpl::CreateFromString(systemPool, *this,
//                                        system, toks[1]);

//       //cerr << "created target " << *target << " source=" << *source << endl;

//       target->GetScores().CreateFromString(toks[2], *this, system, true);
//       //cerr << "created scores:" << *target << endl;

//       //vector<SCORE> scores = Tokenize<SCORE>(toks[2]);
//       //target->sortScore = (scores.size() >= 3) ? TransformScore(scores[2]) : 0;

//       target->SetAlignmentInfo(toks[3]);

//       // properties
//       if (toks.size() == 7) {
//         //target->properties = (char*) system.systemPool.Allocate(toks[6].size() + 1);
//         //strcpy(target->properties, toks[6].c_str());
//       }

//       system.featureFunctions.EvaluateInIsolation(systemPool, system, *source,
//           *target);
//       //cerr << "EvaluateInIsolation:" << *target << endl;
//       m_rootSCFG->AddRule(m_input, *source, target);
//     }
//   }

//   if (system.isPb) {
//     m_rootPb->SortAndPrune(m_tableLimit, systemPool, system);
//     //cerr << "root=" << &m_rootPb << endl;
//   } else {
//     m_rootSCFG->SortAndPrune(m_tableLimit, systemPool, system);
//     //cerr << "root=" << &m_rootPb << endl;
//   }
//   /*
//   BOOST_FOREACH(const PtMem::Node<Word>::Children::value_type &valPair, m_rootPb.GetChildren()) {
//     const Word &word = valPair.first;
//     cerr << word << " ";
//   }
//   cerr << endl;
//   */

// }

void MSPT::InitializeForInput(const InputType &input)
{
  cerr << "InitializeForInput MSPT" << endl;

  // downcast to SentenceWithCandidates
  const SentenceWithCandidates& inputObj = dynamic_cast<const SentenceWithCandidates&>(input);
  cerr << "Casting done." << endl;
  // cerr << "PhraseTableString member: " << inputObj.getPhraseTableString() << endl;

}

TargetPhrases* MSPT::Lookup(const Manager &mgr, MemPool &pool,
    InputPath &inputPath) const
{
  const SubPhrase<Moses2::Word> &phrase = inputPath.subPhrase;
  TargetPhrases *tps = m_rootPb->Find(m_input, phrase);
  return tps;
}

void MSPT::InitActiveChart(
  MemPool &pool,
  const SCFG::Manager &mgr,
  SCFG::InputPath &path) const
{
  size_t ptInd = GetPtInd();
  ActiveChartEntryMem *chartEntry = new (pool.Allocate<ActiveChartEntryMem>()) ActiveChartEntryMem(pool, *m_rootSCFG);
  path.AddActiveChartEntry(ptInd, chartEntry);
  //cerr << "InitActiveChart=" << path << endl;
}

void MSPT::Lookup(MemPool &pool,
                               const SCFG::Manager &mgr,
                               size_t maxChartSpan,
                               const SCFG::Stacks &stacks,
                               SCFG::InputPath &path) const
{
  if (path.range.GetNumWordsCovered() > maxChartSpan) {
    return;
  }

  size_t endPos = path.range.GetEndPos();

  const SCFG::InputPath *prevPath = static_cast<const SCFG::InputPath*>(path.prefixPath);
  UTIL_THROW_IF2(prevPath == NULL, "prefixPath == NULL");

  // TERMINAL
  const SCFG::Word &lastWord = path.subPhrase.Back();

  const SCFG::InputPath &subPhrasePath = *mgr.GetInputPaths().GetMatrix().GetValue(endPos, 1);

  //cerr << "BEFORE LookupGivenWord=" << *prevPath << endl;
  LookupGivenWord(pool, mgr, *prevPath, lastWord, NULL, subPhrasePath.range, path);
  //cerr << "AFTER LookupGivenWord=" << *prevPath << endl;

  // NON-TERMINAL
  //const SCFG::InputPath *prefixPath = static_cast<const SCFG::InputPath*>(path.prefixPath);
  while (prevPath) {
    const Range &prevRange = prevPath->range;
    //cerr << "prevRange=" << prevRange << endl;

    size_t startPos = prevRange.GetEndPos() + 1;
    size_t ntSize = endPos - startPos + 1;
    const SCFG::InputPath &subPhrasePath = *mgr.GetInputPaths().GetMatrix().GetValue(startPos, ntSize);

    LookupNT(pool, mgr, subPhrasePath.range, *prevPath, stacks, path);

    prevPath = static_cast<const SCFG::InputPath*>(prevPath->prefixPath);
  }
}

void MSPT::LookupGivenNode(
  MemPool &pool,
  const SCFG::Manager &mgr,
  const SCFG::ActiveChartEntry &prevEntry,
  const SCFG::Word &wordSought,
  const Moses2::Hypotheses *hypos,
  const Moses2::Range &subPhraseRange,
  SCFG::InputPath &outPath) const
{
  const ActiveChartEntryMem &prevEntryCast = static_cast<const ActiveChartEntryMem&>(prevEntry);

  const SCFGNODE &prevNode = prevEntryCast.node;
  UTIL_THROW_IF2(&prevNode == NULL, "node == NULL");

  size_t ptInd = GetPtInd();
  const SCFGNODE *nextNode = prevNode.Find(m_input, wordSought);

  /*
  if (outPath.range.GetStartPos() == 1 || outPath.range.GetStartPos() == 2) {
    cerr  << "range=" << outPath.range
          << " prevEntry=" << prevEntry.GetSymbolBind().Debug(mgr.system)
          << " wordSought=" << wordSought.Debug(mgr.system)
          << " nextNode=" << nextNode
          << endl;
  }
  */
  if (nextNode) {
    // new entries
    ActiveChartEntryMem *chartEntry = new (pool.Allocate<ActiveChartEntryMem>()) ActiveChartEntryMem(pool, *nextNode, prevEntry);

    chartEntry->AddSymbolBindElement(subPhraseRange, wordSought, hypos, *this);
    //cerr << "AFTER Add=" << symbolBind << endl;

    outPath.AddActiveChartEntry(ptInd, chartEntry);

    const SCFG::TargetPhrases *tps = nextNode->GetTargetPhrases();
    if (tps) {
      // there are some rules
      /*
      cerr << "outPath=" << outPath.range
      	  << " bind=" << chartEntry->GetSymbolBind().Debug(mgr.system)
      	  << " pt=" << GetPtInd()
        << " tps=" << tps->Debug(mgr.system) << endl;
      */
      outPath.AddTargetPhrasesToPath(pool, mgr.system, *this, *tps, chartEntry->GetSymbolBind());

    }

    //cerr << "AFTER outPath=" << outPath << endl;
  }
}

}

<commit_msg>some code for parsing input<commit_after>/*
 * MSPT.cpp
 *
 *  Created on: 28 Oct 2015
 *      Author: hieu
 */

#include <cassert>
#include <sstream>
#include <boost/foreach.hpp>
#include "MSPT.h"
#include "../../PhraseBased/PhraseImpl.h"
#include "../../Phrase.h"
#include "../../System.h"
#include "../../Scores.h"
#include "../../InputPathsBase.h"
#include "../../legacy/InputFileStream.h"
#include "util/exception.hh"

#include "../../PhraseBased/InputPath.h"
#include "../../PhraseBased/TargetPhraseImpl.h"
#include "../../PhraseBased/TargetPhrases.h"
#include "../../PhraseBased/SentenceWithCandidates.h"

#include "../../SCFG/PhraseImpl.h"
#include "../../SCFG/TargetPhraseImpl.h"
#include "../../SCFG/InputPath.h"
#include "../../SCFG/Stack.h"
#include "../../SCFG/Stacks.h"
#include "../../SCFG/Manager.h"

using namespace std;

namespace Moses2
{

////////////////////////////////////////////////////////////////////////

MSPT::MSPT(size_t startInd, const std::string &line)
  :PhraseTable(startInd, line)
  ,m_rootPb(NULL)
  ,m_rootSCFG(NULL)
{
  ReadParameters();
}

MSPT::~MSPT()
{
  delete m_rootPb;
  delete m_rootSCFG;
}

// void MSPT::CreatePTForInput(string phraseTableString)
// {
//   FactorCollection &vocab = system.GetVocab();
//   MemPool &systemPool = system.GetSystemPool();
//   MemPool tmpSourcePool;

//   if (system.isPb) {
//     m_rootPb = new PBNODE();
//   } else {
//     m_rootSCFG = new SCFGNODE();
//     //cerr << "m_rootSCFG=" << m_rootSCFG << endl;
//   }

//   vector<string> toks;
//   size_t lineNum = 0;
//   istringstream  strme(phraseTableString);
//   string line;
//   while (getline(strme, line)) {
//     if (++lineNum % 1000000 == 0) {
//       cerr << lineNum << " ";
//     }
//     toks.clear();
//     TokenizeMultiCharSeparator(toks, line, "|||");
//     UTIL_THROW_IF2(toks.size() < 3, "Wrong format");
//     //cerr << "line=" << line << endl;
//     //cerr << "system.isPb=" << system.isPb << endl;

//     if (system.isPb) {
//       PhraseImpl *source = PhraseImpl::CreateFromString(tmpSourcePool, vocab, system,
//                            toks[0]);
//       //cerr << "created soure" << endl;
//       TargetPhraseImpl *target = TargetPhraseImpl::CreateFromString(systemPool, *this, system,
//                                  toks[1]);
//       //cerr << "created target" << endl;
//       target->GetScores().CreateFromString(toks[2], *this, system, true);
//       //cerr << "created scores:" << *target << endl;

//       if (toks.size() >= 4) {
//         //cerr << "alignstr=" << toks[3] << endl;
//         target->SetAlignmentInfo(toks[3]);
//       }

//       // properties
//       if (toks.size() == 7) {
//         //target->properties = (char*) system.systemPool.Allocate(toks[6].size() + 1);
//         //strcpy(target->properties, toks[6].c_str());
//       }

//       system.featureFunctions.EvaluateInIsolation(systemPool, system, *source,
//           *target);
//       //cerr << "EvaluateInIsolation:" << *target << endl;
//       m_rootPb->AddRule(m_input, *source, target);

//       //cerr << "target=" << target->Debug(system) << endl;
//     } else {
//       SCFG::PhraseImpl *source = SCFG::PhraseImpl::CreateFromString(tmpSourcePool, vocab, system,
//                                  toks[0]);
//       //cerr << "created source:" << *source << endl;
//       SCFG::TargetPhraseImpl *target = SCFG::TargetPhraseImpl::CreateFromString(systemPool, *this,
//                                        system, toks[1]);

//       //cerr << "created target " << *target << " source=" << *source << endl;

//       target->GetScores().CreateFromString(toks[2], *this, system, true);
//       //cerr << "created scores:" << *target << endl;

//       //vector<SCORE> scores = Tokenize<SCORE>(toks[2]);
//       //target->sortScore = (scores.size() >= 3) ? TransformScore(scores[2]) : 0;

//       target->SetAlignmentInfo(toks[3]);

//       // properties
//       if (toks.size() == 7) {
//         //target->properties = (char*) system.systemPool.Allocate(toks[6].size() + 1);
//         //strcpy(target->properties, toks[6].c_str());
//       }

//       system.featureFunctions.EvaluateInIsolation(systemPool, system, *source,
//           *target);
//       //cerr << "EvaluateInIsolation:" << *target << endl;
//       m_rootSCFG->AddRule(m_input, *source, target);
//     }
//   }

//   if (system.isPb) {
//     m_rootPb->SortAndPrune(m_tableLimit, systemPool, system);
//     //cerr << "root=" << &m_rootPb << endl;
//   } else {
//     m_rootSCFG->SortAndPrune(m_tableLimit, systemPool, system);
//     //cerr << "root=" << &m_rootPb << endl;
//   }
//   /*
//   BOOST_FOREACH(const PtMem::Node<Word>::Children::value_type &valPair, m_rootPb.GetChildren()) {
//     const Word &word = valPair.first;
//     cerr << word << " ";
//   }
//   cerr << endl;
//   */

// }

void MSPT::InitializeForInput(const InputType &input)
{
  cerr << "InitializeForInput MSPT" << endl;

  // downcast to SentenceWithCandidates
  const SentenceWithCandidates& inputObj = dynamic_cast<const SentenceWithCandidates&>(input);
  cerr << "Casting done." << endl;
  cerr << "PhraseTableString member: " << inputObj.getPhraseTableString() << endl;

}

TargetPhrases* MSPT::Lookup(const Manager &mgr, MemPool &pool,
    InputPath &inputPath) const
{
  const SubPhrase<Moses2::Word> &phrase = inputPath.subPhrase;
  TargetPhrases *tps = m_rootPb->Find(m_input, phrase);
  return tps;
}

void MSPT::InitActiveChart(
  MemPool &pool,
  const SCFG::Manager &mgr,
  SCFG::InputPath &path) const
{
  size_t ptInd = GetPtInd();
  ActiveChartEntryMem *chartEntry = new (pool.Allocate<ActiveChartEntryMem>()) ActiveChartEntryMem(pool, *m_rootSCFG);
  path.AddActiveChartEntry(ptInd, chartEntry);
  //cerr << "InitActiveChart=" << path << endl;
}

void MSPT::Lookup(MemPool &pool,
                               const SCFG::Manager &mgr,
                               size_t maxChartSpan,
                               const SCFG::Stacks &stacks,
                               SCFG::InputPath &path) const
{
  if (path.range.GetNumWordsCovered() > maxChartSpan) {
    return;
  }

  size_t endPos = path.range.GetEndPos();

  const SCFG::InputPath *prevPath = static_cast<const SCFG::InputPath*>(path.prefixPath);
  UTIL_THROW_IF2(prevPath == NULL, "prefixPath == NULL");

  // TERMINAL
  const SCFG::Word &lastWord = path.subPhrase.Back();

  const SCFG::InputPath &subPhrasePath = *mgr.GetInputPaths().GetMatrix().GetValue(endPos, 1);

  //cerr << "BEFORE LookupGivenWord=" << *prevPath << endl;
  LookupGivenWord(pool, mgr, *prevPath, lastWord, NULL, subPhrasePath.range, path);
  //cerr << "AFTER LookupGivenWord=" << *prevPath << endl;

  // NON-TERMINAL
  //const SCFG::InputPath *prefixPath = static_cast<const SCFG::InputPath*>(path.prefixPath);
  while (prevPath) {
    const Range &prevRange = prevPath->range;
    //cerr << "prevRange=" << prevRange << endl;

    size_t startPos = prevRange.GetEndPos() + 1;
    size_t ntSize = endPos - startPos + 1;
    const SCFG::InputPath &subPhrasePath = *mgr.GetInputPaths().GetMatrix().GetValue(startPos, ntSize);

    LookupNT(pool, mgr, subPhrasePath.range, *prevPath, stacks, path);

    prevPath = static_cast<const SCFG::InputPath*>(prevPath->prefixPath);
  }
}

void MSPT::LookupGivenNode(
  MemPool &pool,
  const SCFG::Manager &mgr,
  const SCFG::ActiveChartEntry &prevEntry,
  const SCFG::Word &wordSought,
  const Moses2::Hypotheses *hypos,
  const Moses2::Range &subPhraseRange,
  SCFG::InputPath &outPath) const
{
  const ActiveChartEntryMem &prevEntryCast = static_cast<const ActiveChartEntryMem&>(prevEntry);

  const SCFGNODE &prevNode = prevEntryCast.node;
  UTIL_THROW_IF2(&prevNode == NULL, "node == NULL");

  size_t ptInd = GetPtInd();
  const SCFGNODE *nextNode = prevNode.Find(m_input, wordSought);

  /*
  if (outPath.range.GetStartPos() == 1 || outPath.range.GetStartPos() == 2) {
    cerr  << "range=" << outPath.range
          << " prevEntry=" << prevEntry.GetSymbolBind().Debug(mgr.system)
          << " wordSought=" << wordSought.Debug(mgr.system)
          << " nextNode=" << nextNode
          << endl;
  }
  */
  if (nextNode) {
    // new entries
    ActiveChartEntryMem *chartEntry = new (pool.Allocate<ActiveChartEntryMem>()) ActiveChartEntryMem(pool, *nextNode, prevEntry);

    chartEntry->AddSymbolBindElement(subPhraseRange, wordSought, hypos, *this);
    //cerr << "AFTER Add=" << symbolBind << endl;

    outPath.AddActiveChartEntry(ptInd, chartEntry);

    const SCFG::TargetPhrases *tps = nextNode->GetTargetPhrases();
    if (tps) {
      // there are some rules
      /*
      cerr << "outPath=" << outPath.range
      	  << " bind=" << chartEntry->GetSymbolBind().Debug(mgr.system)
      	  << " pt=" << GetPtInd()
        << " tps=" << tps->Debug(mgr.system) << endl;
      */
      outPath.AddTargetPhrasesToPath(pool, mgr.system, *this, *tps, chartEntry->GetSymbolBind());

    }

    //cerr << "AFTER outPath=" << outPath << endl;
  }
}

}

<|endoftext|>


<commit_before>#include "Stdafx.h"
#include "Karo.h"

namespace Karo {
    namespace Core {
        Karo::Karo() {
            _tiles = gcnew array<Tile^>(20);
            for (int i = 0; i < 20; i++) {
                _tiles[i] = gcnew Tile(i % 5, i / 5);
            }
            _pieces = gcnew array<Piece^>(12);
        }

        Karo::Karo(array<Tile^>^ tiles, array<Piece^>^ pieces) {
            if (tiles == nullptr)
                throw gcnew ArgumentNullException("tiles");
            if (pieces == nullptr)
                throw gcnew ArgumentNullException("pieces");

            if (tiles->Length != 20)
                throw gcnew ArgumentException("The length of tiles must be 20");

            if (pieces->Length != 12)
                throw gcnew ArgumentException("The length of pieces must be 12");

            _tiles = tiles;
            _pieces = pieces;
        }

        Piece^ Karo::GetPiece(int x, int y) {
            for each(auto piece in Pieces)
                if (piece != nullptr && piece->Tile->X == x && piece->Tile->Y == y)
                    return piece;
        }

        int Karo::GetRowLength(int x, int y, int offsetX, int offsetY, Player player) {
            x += offsetX;
            y += offsetY;

            auto piece = GetPiece(x, y);

            if (piece == nullptr || !piece->IsFaceUp || piece->Player != player) return 0;

            return 1 + GetRowLength(x, y, offsetX, offsetY, player);
        }

        bool Karo::IsFinished::get() {
            for each(auto piece1 in Pieces) {
                if (piece1 == nullptr || !piece1->IsFaceUp) {
                    return false;
                }

                int x = piece1->Tile->X;
                int y = piece1->Tile->Y;

                return 1 + GetRowLength(x, y, -1, -1, piece1->Player) +
                    GetRowLength(x, y, 1, 1, piece1->Player) >= 4 ||
                    1 + GetRowLength(x, y, 1, -1, piece1->Player) +
                    GetRowLength(x, y, -1, 1, piece1->Player) >= 4 ||
                    1 + GetRowLength(x, y, -1, 0, piece1->Player) +
                    GetRowLength(x, y, 1, 0, piece1->Player) >= 4 ||
                    1 + GetRowLength(x, y, 0, -1, piece1->Player) +
                    GetRowLength(x, y, 0, 1, piece1->Player) >= 4;
            }

            return false;
        }
        IEnumerable<Piece^>^ Karo::Pieces::get() {
            return _pieces;
        }
        IEnumerable<Tile^>^ Karo::Tiles::get() {
            return _tiles;
        }

        IEnumerable<Tile^>^ Karo::GetCornerTiles() {
            List<Tile ^>^ tiles = gcnew List<Tile^>();

            for each(Tile^ tile in Tiles) {
                Tile^ top = GetTileAt(tile->X, tile->Y - 1);
                Tile^ bottom = GetTileAt(tile->X, tile->Y + 1);
                Tile^ left = GetTileAt(tile->X - 1, tile->Y);
                Tile^ right = GetTileAt(tile->X + 1, tile->Y);

                if (((top == nullptr && left == nullptr) ||
                    (top == nullptr && right == nullptr) ||
                    (bottom == nullptr && left == nullptr) ||
                    (bottom == nullptr && right == nullptr)) &&
                    GetPiece(tile->X, tile->Y) == nullptr)
                    tiles->Add(tile);
            }

            return tiles;
        }
        IEnumerable<Move^>^ Karo::GetAvailableMoves(Player player) {
			List<Move^>^ moves = gcnew List<Move^>(48); // 6 pieces * 8 directions

			if (PieceCount() < 12) // we're still in the first phase
			{
				for each (Tile^ tile in Tiles)
				{
					if (!tile->HasPiece)
					{
						moves->Add(gcnew Move(tile->X, tile->Y, 0, 0, 0, 0));
					}
				}
			}
			else // we're in the tile moving phase
			{
                IEnumerable<Tile^>^cornerTiles;

				for each (Piece^ piece in Pieces)
				{
                    if (piece->Player != player) continue;

					// loop surrounding tiles for positions
                    for (int x = -1; x <= 1; x++)
					{
                        for (int y = -1; y <= 1; y++)
                        {
                            if (x == 0 && y == 0) continue;

                            int newx = piece->Tile->X + x;
                            int newy = piece->Tile->Y + y;

                            // todo: check is filled, jump over
                            Piece^ pieceOnTile = GetPiece(newx, newy);

                            if (pieceOnTile != nullptr) {
                                newx += x;
                                newy += y;

                                pieceOnTile = GetPiece(newx, newy);

                                if (pieceOnTile != nullptr) {
                                    continue;
                                }
                            }

                            Tile^ surroundingTile = GetTileAt(newx, newy);

                            if (surroundingTile == nullptr)
                            {
                                if (cornerTiles == nullptr) {
                                    cornerTiles = GetCornerTiles();
                                }

                                for each(Tile^ cornerTile in cornerTiles)
                                {
                                    Tile^ top = GetTileAt(newx, newy - 1);
                                    Tile^ bottom = GetTileAt(newx, newy + 1);
                                    Tile^ left = GetTileAt(newx - 1, newy);
                                    Tile^ right = GetTileAt(newx + 1, newy);

                                    if (top == nullptr && bottom == nullptr && left == nullptr && right == nullptr) continue;

                                    moves->Add(gcnew Move(newx, newy, piece->Tile->X, piece->Tile->Y, cornerTile->X, cornerTile->Y));
                                }
                            }
                            else
                            {
                                if (pieceOnTile == nullptr) {
                                    moves->Add(gcnew Move(newx, newy, piece->Tile->X, piece->Tile->Y, 0, 0));
                                }
                            }

                        }
					}
				}
			}

			return moves;
        }

		Tile^ Karo::GetTileAt(int x, int y)
		{
			if (Tiles == nullptr)
				return nullptr;

			for each (Tile^ tile in Tiles)
			{
				if (tile != nullptr)
					if (tile->X == x && tile->Y == y)
						return tile;
			}

			return nullptr;
		}

		bool Karo::IsValidMove(Move^ move) {

			//Step0: Initphase //Place piece on current location, just check if there is a piece already
			if (PieceCount() <= 11)
			{
				if (GetPiece(move->NewPieceX, move->NewPieceY) != nullptr)
					return false;
				else
					return true;
			}
			//Move Phase
			else{
				//Step1: Check if new location is different from the current one
				if (move->NewPieceX == move->OldPieceX && move->NewPieceY == move->OldPieceY)
					return false;

				//Step2: Check if there already is a piece on the tile your trying to move to
				for each (Tile^ tile in Tiles){
					if (GetPiece(move->NewPieceX,move->NewPieceY)!= nullptr)
						return false;

					//Step3: Check if there is a piece between your current location, and the new one
					int pieceLocationX, pieceLocationY, distancex, distancey;
					//If the X location won't change, keep the X location. otherwise add distance
					if (move->NewPieceX == move->OldPieceX)
						pieceLocationX = move->NewPieceX;
					else{
						distancex = (move->NewPieceX - move->OldPieceX);
						if (distancex < 0)
							distancex = Math::Abs(-distancex);
						if (move->NewPieceX > move->OldPieceX)
							pieceLocationX = (move->OldPieceX + distancex / 2);
						else
							pieceLocationX = (move->OldPieceX - distancex / 2);

					}
					if (move->NewPieceY == move->OldPieceY)
						pieceLocationY = move->OldPieceY;
					else{
						distancey = (move->NewPieceY - move->OldPieceY);
						if (distancey < 0)
							distancey = Math::Abs(-distancey);
						if (move->NewPieceY > move->OldPieceY)
							pieceLocationY = (move->OldPieceY + distancey / 2);
						else
							pieceLocationY = (move->OldPieceY - distancey / 2);
					}

					//If there is a piece on the calculated X and Y, Then you can jump over it (if enemy)
					if (GetPiece(pieceLocationX, pieceLocationY) != nullptr &&
						GetPiece(pieceLocationX, pieceLocationY)->Player != GetPiece(move->OldPieceX,move->OldPieceY)->Player)
						return true;

					//Check if the new location is into 'move-range'.
					//Distance of X and Y should be max 1. Unless there is a enemy piece next to it (jumping)
					//Keep in mind the possibilty of jumping over other pieces!!
					if (distancex < 0)
						distancex = Math::Abs(-distancex);
					if (distancey < 0)
						distancey = Math::Abs(-distancey);

					if (distancex > 1 || distancey > 1)
						return false;
				}
				return true;
			}
		}

        Karo^ Karo::WithMoveApplied(Move^ move, Player player) {
            if (move == nullptr) {
                throw gcnew ArgumentNullException("move");
            }

            array<Tile^>^ tiles = gcnew array<Tile^> (20);
            array<Piece^>^ pieces = gcnew array<Piece^>(12);
            int tidx = 0, pidx = 0;

            Tile^ movingTile = nullptr, ^targetTile = nullptr;
            Piece^ movingPiece = nullptr;

            for each(Tile^ tile in Tiles) {
                Tile^ newTile = tiles[tidx++] = gcnew Tile(tile->X, tile->Y);

                if (newTile->X == move->OldTileX && newTile->Y == move->OldTileY) {
                    movingTile = newTile;
                }
                if (newTile->X == move->NewPieceX && newTile->Y == move->NewPieceY) {
                    targetTile = newTile;
                }
            }

                for each(Piece^ piece in Pieces) {
                if (piece == nullptr) continue;
                Tile^ tile = nullptr;

                for each(Tile^ t in tiles) {
                    if (t->X == piece->Tile->X && t->Y == piece->Tile->Y) {
                        tile = t;
                        continue;
                    }
                }

                if (tile == nullptr) {
                    throw gcnew Exception("Invalid board state");
                }

                pieces[pidx++] = gcnew Piece(tile, piece->Player, piece->IsFaceUp);

                if (pieces[pidx - 1]->Tile->X == move->OldPieceX && pieces[pidx - 1]->Tile->Y == move->OldPieceY)
                    movingPiece = pieces[pidx - 1];
            }

            int pieceCount = PieceCount();
            if (pieceCount < 12) {
                if (targetTile == nullptr) {
                    throw gcnew Exception("Invalid board state");
                }
                pieces[pieceCount] = gcnew Piece(targetTile, player, false);
                return gcnew Karo(tiles, pieces);;
            }
            if (movingPiece == nullptr) {
                throw gcnew Exception("Invalid board state");
            }

            if (targetTile == nullptr) {
                if (movingTile == nullptr) {
                    throw gcnew Exception("Invalid board state");
                }

                targetTile = movingTile;
                targetTile->X = move->NewPieceX;
                targetTile->Y = move->NewPieceY;
            }

            movingPiece->Tile = targetTile;

            if (Math::Abs(move->NewPieceX - move->OldPieceX) > 1 || Math::Abs(move->NewPieceY - move->OldPieceY) > 1) {
                movingPiece->IsFaceUp = !movingPiece->IsFaceUp;
            }

            return gcnew Karo(tiles, pieces);
        }
		int Karo::PieceCount()
		{
			if (Pieces == nullptr)
				return 0;

			int i = 0;

			for each (Piece^ piece in Pieces)
				if (piece != nullptr)
					i++;

			return i;
		}
    }
}<commit_msg>Bugfix: Cant jump over more then 1 piece<commit_after>#include "Stdafx.h"
#include "Karo.h"

namespace Karo {
    namespace Core {
        Karo::Karo() {
            _tiles = gcnew array<Tile^>(20);
            for (int i = 0; i < 20; i++) {
                _tiles[i] = gcnew Tile(i % 5, i / 5);
            }
            _pieces = gcnew array<Piece^>(12);
        }

        Karo::Karo(array<Tile^>^ tiles, array<Piece^>^ pieces) {
            if (tiles == nullptr)
                throw gcnew ArgumentNullException("tiles");
            if (pieces == nullptr)
                throw gcnew ArgumentNullException("pieces");

            if (tiles->Length != 20)
                throw gcnew ArgumentException("The length of tiles must be 20");

            if (pieces->Length != 12)
                throw gcnew ArgumentException("The length of pieces must be 12");

            _tiles = tiles;
            _pieces = pieces;
        }

        Piece^ Karo::GetPiece(int x, int y) {
            for each(auto piece in Pieces)
                if (piece != nullptr && piece->Tile->X == x && piece->Tile->Y == y)
                    return piece;
        }

        int Karo::GetRowLength(int x, int y, int offsetX, int offsetY, Player player) {
            x += offsetX;
            y += offsetY;

            auto piece = GetPiece(x, y);

            if (piece == nullptr || !piece->IsFaceUp || piece->Player != player) return 0;

            return 1 + GetRowLength(x, y, offsetX, offsetY, player);
        }

        bool Karo::IsFinished::get() {
            for each(auto piece1 in Pieces) {
                if (piece1 == nullptr || !piece1->IsFaceUp) {
                    return false;
                }

                int x = piece1->Tile->X;
                int y = piece1->Tile->Y;

                return 1 + GetRowLength(x, y, -1, -1, piece1->Player) +
                    GetRowLength(x, y, 1, 1, piece1->Player) >= 4 ||
                    1 + GetRowLength(x, y, 1, -1, piece1->Player) +
                    GetRowLength(x, y, -1, 1, piece1->Player) >= 4 ||
                    1 + GetRowLength(x, y, -1, 0, piece1->Player) +
                    GetRowLength(x, y, 1, 0, piece1->Player) >= 4 ||
                    1 + GetRowLength(x, y, 0, -1, piece1->Player) +
                    GetRowLength(x, y, 0, 1, piece1->Player) >= 4;
            }

            return false;
        }
        IEnumerable<Piece^>^ Karo::Pieces::get() {
            return _pieces;
        }
        IEnumerable<Tile^>^ Karo::Tiles::get() {
            return _tiles;
        }

        IEnumerable<Tile^>^ Karo::GetCornerTiles() {
            List<Tile ^>^ tiles = gcnew List<Tile^>();

            for each(Tile^ tile in Tiles) {
                Tile^ top = GetTileAt(tile->X, tile->Y - 1);
                Tile^ bottom = GetTileAt(tile->X, tile->Y + 1);
                Tile^ left = GetTileAt(tile->X - 1, tile->Y);
                Tile^ right = GetTileAt(tile->X + 1, tile->Y);

                if (((top == nullptr && left == nullptr) ||
                    (top == nullptr && right == nullptr) ||
                    (bottom == nullptr && left == nullptr) ||
                    (bottom == nullptr && right == nullptr)) &&
                    GetPiece(tile->X, tile->Y) == nullptr)
                    tiles->Add(tile);
            }

            return tiles;
        }
        IEnumerable<Move^>^ Karo::GetAvailableMoves(Player player) {
			List<Move^>^ moves = gcnew List<Move^>(48); // 6 pieces * 8 directions

			if (PieceCount() < 12) // we're still in the first phase
			{
				for each (Tile^ tile in Tiles)
				{
					if (!tile->HasPiece)
					{
						moves->Add(gcnew Move(tile->X, tile->Y, 0, 0, 0, 0));
					}
				}
			}
			else // we're in the tile moving phase
			{
                IEnumerable<Tile^>^cornerTiles;

				for each (Piece^ piece in Pieces)
				{
                    if (piece->Player != player) continue;

					// loop surrounding tiles for positions
                    for (int x = -1; x <= 1; x++)
					{
                        for (int y = -1; y <= 1; y++)
                        {
                            if (x == 0 && y == 0) continue;

                            int newx = piece->Tile->X + x;
                            int newy = piece->Tile->Y + y;

                            // todo: check is filled, jump over
                            Piece^ pieceOnTile = GetPiece(newx, newy);

                            if (pieceOnTile != nullptr) {
                                newx += x;
                                newy += y;

                                pieceOnTile = GetPiece(newx, newy);

                                if (pieceOnTile != nullptr) {
                                    continue;
                                }
                            }

                            Tile^ surroundingTile = GetTileAt(newx, newy);

                            if (surroundingTile == nullptr)
                            {
                                if (cornerTiles == nullptr) {
                                    cornerTiles = GetCornerTiles();
                                }

                                for each(Tile^ cornerTile in cornerTiles)
                                {
                                    Tile^ top = GetTileAt(newx, newy - 1);
                                    Tile^ bottom = GetTileAt(newx, newy + 1);
                                    Tile^ left = GetTileAt(newx - 1, newy);
                                    Tile^ right = GetTileAt(newx + 1, newy);

                                    if (top == nullptr && bottom == nullptr && left == nullptr && right == nullptr) continue;

                                    moves->Add(gcnew Move(newx, newy, piece->Tile->X, piece->Tile->Y, cornerTile->X, cornerTile->Y));
                                }
                            }
                            else
                            {
                                if (pieceOnTile == nullptr) {
                                    moves->Add(gcnew Move(newx, newy, piece->Tile->X, piece->Tile->Y, 0, 0));
                                }
                            }

                        }
					}
				}
			}

			return moves;
        }

		Tile^ Karo::GetTileAt(int x, int y)
		{
			if (Tiles == nullptr)
				return nullptr;

			for each (Tile^ tile in Tiles)
			{
				if (tile != nullptr)
					if (tile->X == x && tile->Y == y)
						return tile;
			}

			return nullptr;
		}

		bool Karo::IsValidMove(Move^ move) {

			//Step0: Initphase //Place piece on current location, just check if there is a piece already
			if (PieceCount() <= 11)
			{
				if (GetPiece(move->NewPieceX, move->NewPieceY) != nullptr)
					return false;
				else
					return true;
			}
			//Move Phase
			else{
				//Step1: Check if new location is different from the current one
				if (move->NewPieceX == move->OldPieceX && move->NewPieceY == move->OldPieceY)
					return false;

				//Step2: Check if there already is a piece on the tile your trying to move to
				for each (Tile^ tile in Tiles){
					if (GetPiece(move->NewPieceX,move->NewPieceY)!= nullptr)
						return false;

					//Step3: Check if there is a piece between your current location, and the new one
					int pieceLocationX, pieceLocationY, distancex, distancey;
					//If the X location won't change, keep the X location. otherwise add distance
					if (move->NewPieceX == move->OldPieceX)
						pieceLocationX = move->NewPieceX;
					else{
						distancex = (move->NewPieceX - move->OldPieceX);
						if (distancex < 0)
							distancex = Math::Abs(-distancex);
						if (move->NewPieceX > move->OldPieceX)
							pieceLocationX = (move->OldPieceX + distancex / 2);
						else
							pieceLocationX = (move->OldPieceX - distancex / 2);

					}
					if (move->NewPieceY == move->OldPieceY)
						pieceLocationY = move->OldPieceY;
					else{
						distancey = (move->NewPieceY - move->OldPieceY);
						if (distancey < 0)
							distancey = Math::Abs(-distancey);
						if (move->NewPieceY > move->OldPieceY)
							pieceLocationY = (move->OldPieceY + distancey / 2);
						else
							pieceLocationY = (move->OldPieceY - distancey / 2);
					}

					if (distancex > 2 || distancey > 2)
						return false;
					//If there is a piece on the calculated X and Y, Then you can jump over it (if enemy)
					if (GetPiece(pieceLocationX, pieceLocationY) != nullptr &&
						GetPiece(pieceLocationX, pieceLocationY)->Player != GetPiece(move->OldPieceX,move->OldPieceY)->Player)
						return true;
					//Check if the new location is into 'move-range'.
					//Distance of X and Y should be max 1. Unless there is a enemy piece next to it (jumping)
					//Keep in mind the possibilty of jumping over other pieces!!
					if (distancex > 1 || distancey > 1)
						return false;
				}
				return true;
			}
		}

        Karo^ Karo::WithMoveApplied(Move^ move, Player player) {
            if (move == nullptr) {
                throw gcnew ArgumentNullException("move");
            }

            array<Tile^>^ tiles = gcnew array<Tile^> (20);
            array<Piece^>^ pieces = gcnew array<Piece^>(12);
            int tidx = 0, pidx = 0;

            Tile^ movingTile = nullptr, ^targetTile = nullptr;
            Piece^ movingPiece = nullptr;

            for each(Tile^ tile in Tiles) {
                Tile^ newTile = tiles[tidx++] = gcnew Tile(tile->X, tile->Y);

                if (newTile->X == move->OldTileX && newTile->Y == move->OldTileY) {
                    movingTile = newTile;
                }
                if (newTile->X == move->NewPieceX && newTile->Y == move->NewPieceY) {
                    targetTile = newTile;
                }
            }

                for each(Piece^ piece in Pieces) {
                if (piece == nullptr) continue;
                Tile^ tile = nullptr;

                for each(Tile^ t in tiles) {
                    if (t->X == piece->Tile->X && t->Y == piece->Tile->Y) {
                        tile = t;
                        continue;
                    }
                }

                if (tile == nullptr) {
                    throw gcnew Exception("Invalid board state");
                }

                pieces[pidx++] = gcnew Piece(tile, piece->Player, piece->IsFaceUp);

                if (pieces[pidx - 1]->Tile->X == move->OldPieceX && pieces[pidx - 1]->Tile->Y == move->OldPieceY)
                    movingPiece = pieces[pidx - 1];
            }

            int pieceCount = PieceCount();
            if (pieceCount < 12) {
                if (targetTile == nullptr) {
                    throw gcnew Exception("Invalid board state");
                }
                pieces[pieceCount] = gcnew Piece(targetTile, player, false);
                return gcnew Karo(tiles, pieces);;
            }
            if (movingPiece == nullptr) {
                throw gcnew Exception("Invalid board state");
            }

            if (targetTile == nullptr) {
                if (movingTile == nullptr) {
                    throw gcnew Exception("Invalid board state");
                }

                targetTile = movingTile;
                targetTile->X = move->NewPieceX;
                targetTile->Y = move->NewPieceY;
            }

            movingPiece->Tile = targetTile;

            if (Math::Abs(move->NewPieceX - move->OldPieceX) > 1 || Math::Abs(move->NewPieceY - move->OldPieceY) > 1) {
                movingPiece->IsFaceUp = !movingPiece->IsFaceUp;
            }

            return gcnew Karo(tiles, pieces);
        }
		int Karo::PieceCount()
		{
			if (Pieces == nullptr)
				return 0;

			int i = 0;

			for each (Piece^ piece in Pieces)
				if (piece != nullptr)
					i++;

			return i;
		}
    }
}<|endoftext|>


<commit_before>#ifndef LYAPUNOVSOLVER_H
#define LYAPUNOVSOLVER_H

#include <utility>
#include <vector>
#include <algorithm>

#include "LyapunovSolverDecl.hpp"
#include "SlicotWrapper.hpp"

#define TIMER_ON
#include "Timer.hpp"

namespace Lyapunov {

template<class Matrix, class MultiVector, class DenseMatrix>
Solver<Matrix, MultiVector, DenseMatrix>::Solver(Matrix const &A,
                                                 Matrix const &B,
                                                 Matrix const &M)
    :
    A_(A),
    B_(B),
    M_(M),
    max_iter_(1000),
    tol_(1e-4)
{
}

template<class Matrix, class MultiVector, class DenseMatrix>
template<class ParameterList>
int Solver<Matrix, MultiVector, DenseMatrix>::set_parameters(ParameterList params)
{
    max_iter_ = params.get("Maximum iterations", max_iter_);
    tol_ = params.get("Tolerance", tol_);

    return 0;
}

template<class Matrix, class MultiVector, class DenseMatrix>
int Solver<Matrix, MultiVector, DenseMatrix>::solve(MultiVector &V, DenseMatrix &T)
{
    FUNCTION_TIMER("Solver");
    int n = V.length();
    int expand_per_iteration = 3;
    MultiVector AV(V, max_iter_ * expand_per_iteration + 1);
    AV.resize(0);

    V.resize(1);
    V.random();
    V.orthogonalize();

    MultiVector W(V);

    DenseMatrix VAV(max_iter_ * expand_per_iteration + 1, max_iter_ * expand_per_iteration + 1);
    double r0 = B_.norm_frobenius();
    for (int iter = 0; iter < max_iter_; iter++)
    {
        // Perform VAV = V'*A*V by doing VAV = [[VAV; W'*AV], V'*AW]

        // First compute AW
        START_TIMER("A apply");
        MultiVector AW = A_.apply(W);
        END_TIMER("A apply");

        // Now resize VAV. Resizing should not remove what was in VAV
        // previously
        int num_vectors_V = V.num_vectors();
        int num_vectors_AV = AV.num_vectors();
        int s = num_vectors_AV + W.num_vectors();
        VAV.resize(s, s);

        START_TIMER("A DOT");
        // Now compute W'*AV and put it in VAV
        // AV only exists after the first iteration
        if (iter > 0)
        {
            DenseMatrix WAV = W.dot(AV);
            int WAV_m = WAV.M();
            int WAV_n = WAV.N();
            for (int i = 0; i < WAV_m; i++)
                for (int j = 0; j < WAV_n; j++)
                    VAV(i + num_vectors_AV, j) = WAV(i, j);
        }

        // Now compute V'*AW and put it in VAV
        DenseMatrix VAW = V.dot(AW);
        int VAW_m = VAW.M();
        int VAW_n = VAW.N();
        for (int i = 0; i < VAW_m; i++)
            for (int j = 0; j < VAW_n; j++)
                VAV(i, j + num_vectors_AV) = VAW(i, j);
        END_TIMER("A DOT");

        // Now expand AV
        AV.push_back(AW);

        // Compute VBV = V'*B*B'*V. TODO: This can also be done more
        // efficiently I think
        START_TIMER("B DOT");
        MultiVector BV = B_.apply(V);
        DenseMatrix VBV = BV.dot(BV);
        END_TIMER("B DOT");

        dense_solve(VAV, VBV, T);

        int num_eigenvalues = 10;
        DenseMatrix H(num_eigenvalues + 1, num_eigenvalues + 1);
        DenseMatrix eigenvalues(num_eigenvalues + 1, 1);
        MultiVector eigenvectors;
        lanczos(AV, V, T, H, eigenvectors, eigenvalues, num_eigenvalues);

        double res = eigenvalues.norm_inf();

        std::cout << "Iteration " << iter+1
                  << ". Estimate Lanczos, absolute: " << res
                  << ", relative: " << std::abs(res) / r0 / r0 << std::endl;

        if (std::abs(res) / r0 / r0 < tol_ || iter + 1 >= max_iter_ || V.num_vectors() >= n)
            break;

        int expand_vectors = std::min(std::min(expand_per_iteration,
            eigenvalues.M()), n - V.num_vectors());

        // Find the vectors belonging to the largest eigenvalues
        std::vector<int> indices;
        find_largest_eigenvalues(eigenvalues, indices, expand_vectors);

        for (int i = 0; i < expand_vectors; i++)
            V.push_back(eigenvectors.view(indices[i]));
        V.orthogonalize();

        W = V.view(num_vectors_V, num_vectors_V+expand_vectors-1);
    }

    return 0;
}

template<class Matrix, class MultiVector, class DenseMatrix>
int Solver<Matrix, MultiVector, DenseMatrix>::dense_solve(DenseMatrix const &A, DenseMatrix const &B, DenseMatrix &X)
{
    FUNCTION_TIMER("dense_solve");
    X = B.copy();
    DenseMatrix A_copy = A.copy();
    double scale = 1.0;
    int info = 0;
    int n = A.M();
    sb03md('C', 'X', 'N', 'T', n, A_copy, X, &scale, &info);

    X.scale(-1.0);

    return info;
}

template<class Matrix, class MultiVector, class DenseMatrix>
int Solver<Matrix, MultiVector, DenseMatrix>::lanczos(MultiVector const &AV, MultiVector const &V, DenseMatrix const &T, DenseMatrix &H, MultiVector &eigenvectors, DenseMatrix &eigenvalues, int max_iter)
{
    FUNCTION_TIMER("Lyapunov", "lanczos");
    START_TIMER("Lanczos", "Top");
    MultiVector Q(V, max_iter+1);
    Q.resize(1);
    Q.random();
    Q.view(0) /= Q.norm();
    END_TIMER("Lanczos", "Top");

    double alpha = 0.0;
    double beta = 0.0;

    int iter = 0;
    for (int i = 0; i < max_iter; i++)
    {
        Q.resize(iter + 2);

        START_TIMER("Lanczos", "B apply");
        Q.view(iter+1) = B_.apply(Q.view(iter));
        Q.view(iter+1) = B_.apply(Q.view(iter+1));
        END_TIMER("Lanczos", "B apply");

        START_TIMER("Lanczos", "First part");
        DenseMatrix Z = V.dot(Q.view(iter));
        Z = T.apply(Z);
        Q.view(iter+1) += AV.apply(Z);
        END_TIMER("Lanczos", "First part");

        START_TIMER("Lanczos", "Second part");
        Z = AV.dot(Q.view(iter));
        Z = T.apply(Z);
        Q.view(iter+1) += V.apply(Z);
        END_TIMER("Lanczos", "Second part");

        START_TIMER("Lanczos", "alpha");
        alpha = Q.view(iter+1).dot(Q.view(iter))(0, 0);
        H(iter, iter) = alpha;
        END_TIMER("Lanczos", "alpha");

        START_TIMER("Lanczos", "update");
        Q.view(iter+1) -= alpha * Q.view(iter);
        if (iter > 0)
            Q.view(iter+1) -= beta * Q.view(iter-1);
        END_TIMER("Lanczos", "update");

        START_TIMER("Lanczos", "beta");
        beta = Q.view(iter+1).norm();
        END_TIMER("Lanczos", "beta");
        if (beta < 1e-14)
        {
            // Beta is zero, but the offdiagonal elements of H
            // might still be large, in which case we need those.
            // We can't continue because then we would divide by 0.
            iter++;
            break;
        }

        H(iter+1, iter) = beta;
        H(iter, iter+1) = beta;

        Q.view(iter+1) /= beta;

        iter++;
    }

    START_TIMER("Lanczos", "eigv");
    H.resize(iter, iter);
    Q.resize(iter);

    DenseMatrix v;
    H.eigs(v, eigenvalues);

    eigenvectors = Q.apply(v);
    END_TIMER("Lanczos", "eigv");

    return 0;
}

static bool eigenvalue_sorter(std::pair<int, double> const &a, std::pair<int, double> const &b)
{
    return std::abs(a.second) > std::abs(b.second);
}

template<class Matrix, class MultiVector, class DenseMatrix>
int Solver<Matrix, MultiVector, DenseMatrix>::find_largest_eigenvalues(DenseMatrix const &eigenvalues, std::vector<int> &indices, int num_vectors)
{
    std::vector<std::pair<int, double> > index_to_value;
    for (int i = 0; i < eigenvalues.M(); i++)
        index_to_value.push_back(std::pair<int, double>(i, eigenvalues(i)));

    std::sort(index_to_value.begin(), index_to_value.end(), eigenvalue_sorter);

    for (int i = 0; i < num_vectors; i++)
        indices.push_back(index_to_value[i].first);

    return 0;
}

}

#endif
<commit_msg>Don't recompute V'BB'V in every step<commit_after>#ifndef LYAPUNOVSOLVER_H
#define LYAPUNOVSOLVER_H

#include <utility>
#include <vector>
#include <algorithm>

#include "LyapunovSolverDecl.hpp"
#include "SlicotWrapper.hpp"

#define TIMER_ON
#include "Timer.hpp"

namespace Lyapunov {

template<class Matrix, class MultiVector, class DenseMatrix>
Solver<Matrix, MultiVector, DenseMatrix>::Solver(Matrix const &A,
                                                 Matrix const &B,
                                                 Matrix const &M)
    :
    A_(A),
    B_(B),
    M_(M),
    max_iter_(1000),
    tol_(1e-4)
{
}

template<class Matrix, class MultiVector, class DenseMatrix>
template<class ParameterList>
int Solver<Matrix, MultiVector, DenseMatrix>::set_parameters(ParameterList params)
{
    max_iter_ = params.get("Maximum iterations", max_iter_);
    tol_ = params.get("Tolerance", tol_);

    return 0;
}

template<class Matrix, class MultiVector, class DenseMatrix>
int Solver<Matrix, MultiVector, DenseMatrix>::solve(MultiVector &V, DenseMatrix &T)
{
    FUNCTION_TIMER("Solver");
    int n = V.length();
    int expand_per_iteration = 3;

    V.resize(1);
    V.random();
    V.orthogonalize();

    MultiVector W(V);

    MultiVector AV(V, max_iter_ * expand_per_iteration + 1);
    DenseMatrix VAV(max_iter_ * expand_per_iteration + 1, max_iter_ * expand_per_iteration + 1);
    AV.resize(0);

    MultiVector BV(V, max_iter_ * expand_per_iteration + 1);
    DenseMatrix VBV(max_iter_ * expand_per_iteration + 1, max_iter_ * expand_per_iteration + 1);
    BV.resize(0);

    double r0 = B_.norm_frobenius();

    for (int iter = 0; iter < max_iter_; iter++)
    {
        // Perform VAV = V'*A*V by doing VAV = [[VAV; W'*AV], V'*AW]

        // First compute AW
        START_TIMER("Apply A");
        MultiVector AW = A_.apply(W);
        END_TIMER("Apply A");

        START_TIMER("Apply B");
        // TODO: Should be transpose
        MultiVector BW = B_.apply(W);
        END_TIMER("Apply B");

        START_TIMER("Compute VAV");

        // Now resize VAV. Resizing should not remove what was in VAV
        // previously
        int num_vectors_V = V.num_vectors();
        int num_vectors_AV = AV.num_vectors();
        int s = num_vectors_AV + W.num_vectors();
        VAV.resize(s, s);
        VBV.resize(s, s);

        // Now compute W'*AV and put it in VAV
        // We also compute V'*B*B'*V here for maximum efficiency
        // AV only exists after the first iteration
        if (iter > 0)
        {
            DenseMatrix WAV = W.dot(AV);
            DenseMatrix WBV = BW.dot(BV);
            int WAV_m = WAV.M();
            int WAV_n = WAV.N();
            for (int i = 0; i < WAV_m; i++)
                for (int j = 0; j < WAV_n; j++)
                {
                    VAV(i + num_vectors_AV, j) = WAV(i, j);
                    VBV(i + num_vectors_AV, j) = WBV(i, j);
                    VBV(j, i + num_vectors_AV) = WBV(i, j);
                }
        }

        // Now compute V'*AW and put it in VAV
        DenseMatrix VAW = V.dot(AW);
        int VAW_m = VAW.M();
        int VAW_n = VAW.N();
        for (int i = 0; i < VAW_m; i++)
            for (int j = 0; j < VAW_n; j++)
                VAV(i, j + num_vectors_AV) = VAW(i, j);

        // Compute the bottom-right block of VBV
        DenseMatrix WBW = BW.dot(BW);
        int WBW_m = WBW.M();
        int WBW_n = WBW.N();
        for (int i = 0; i < WBW_m; i++)
            for (int j = 0; j < WBW_n; j++)
                VBV(i + num_vectors_AV, j + num_vectors_AV) = WBW(i, j);

        // Now expand AV and BV
        AV.push_back(AW);
        BV.push_back(BW);

        END_TIMER("Compute VAV");

        dense_solve(VAV, VBV, T);

        int num_eigenvalues = 10;
        DenseMatrix H(num_eigenvalues + 1, num_eigenvalues + 1);
        DenseMatrix eigenvalues(num_eigenvalues + 1, 1);
        MultiVector eigenvectors;
        lanczos(AV, V, T, H, eigenvectors, eigenvalues, num_eigenvalues);

        double res = eigenvalues.norm_inf();

        std::cout << "Iteration " << iter+1
                  << ". Estimate Lanczos, absolute: " << res
                  << ", relative: " << std::abs(res) / r0 / r0 << std::endl;

        if (std::abs(res) / r0 / r0 < tol_ || iter + 1 >= max_iter_ || V.num_vectors() >= n)
            break;

        int expand_vectors = std::min(std::min(expand_per_iteration,
            eigenvalues.M()), n - V.num_vectors());

        // Find the vectors belonging to the largest eigenvalues
        std::vector<int> indices;
        find_largest_eigenvalues(eigenvalues, indices, expand_vectors);

        for (int i = 0; i < expand_vectors; i++)
            V.push_back(eigenvectors.view(indices[i]));
        V.orthogonalize();

        W = V.view(num_vectors_V, num_vectors_V+expand_vectors-1);
    }

    return 0;
}

template<class Matrix, class MultiVector, class DenseMatrix>
int Solver<Matrix, MultiVector, DenseMatrix>::dense_solve(DenseMatrix const &A, DenseMatrix const &B, DenseMatrix &X)
{
    FUNCTION_TIMER("dense_solve");
    X = B.copy();
    DenseMatrix A_copy = A.copy();
    double scale = 1.0;
    int info = 0;
    int n = A.M();
    sb03md('C', 'X', 'N', 'T', n, A_copy, X, &scale, &info);

    X.scale(-1.0);

    return info;
}

template<class Matrix, class MultiVector, class DenseMatrix>
int Solver<Matrix, MultiVector, DenseMatrix>::lanczos(MultiVector const &AV, MultiVector const &V, DenseMatrix const &T, DenseMatrix &H, MultiVector &eigenvectors, DenseMatrix &eigenvalues, int max_iter)
{
    FUNCTION_TIMER("Lyapunov", "lanczos");
    START_TIMER("Lanczos", "Top");
    MultiVector Q(V, max_iter+1);
    Q.resize(1);
    Q.random();
    Q.view(0) /= Q.norm();
    END_TIMER("Lanczos", "Top");

    double alpha = 0.0;
    double beta = 0.0;

    int iter = 0;
    for (int i = 0; i < max_iter; i++)
    {
        Q.resize(iter + 2);

        START_TIMER("Lanczos", "B apply");
        Q.view(iter+1) = B_.apply(Q.view(iter));
        Q.view(iter+1) = B_.apply(Q.view(iter+1));
        END_TIMER("Lanczos", "B apply");

        START_TIMER("Lanczos", "First part");
        DenseMatrix Z = V.dot(Q.view(iter));
        Z = T.apply(Z);
        Q.view(iter+1) += AV.apply(Z);
        END_TIMER("Lanczos", "First part");

        START_TIMER("Lanczos", "Second part");
        Z = AV.dot(Q.view(iter));
        Z = T.apply(Z);
        Q.view(iter+1) += V.apply(Z);
        END_TIMER("Lanczos", "Second part");

        START_TIMER("Lanczos", "alpha");
        alpha = Q.view(iter+1).dot(Q.view(iter))(0, 0);
        H(iter, iter) = alpha;
        END_TIMER("Lanczos", "alpha");

        START_TIMER("Lanczos", "update");
        Q.view(iter+1) -= alpha * Q.view(iter);
        if (iter > 0)
            Q.view(iter+1) -= beta * Q.view(iter-1);
        END_TIMER("Lanczos", "update");

        START_TIMER("Lanczos", "beta");
        beta = Q.view(iter+1).norm();
        END_TIMER("Lanczos", "beta");
        if (beta < 1e-14)
        {
            // Beta is zero, but the offdiagonal elements of H
            // might still be large, in which case we need those.
            // We can't continue because then we would divide by 0.
            iter++;
            break;
        }

        H(iter+1, iter) = beta;
        H(iter, iter+1) = beta;

        Q.view(iter+1) /= beta;

        iter++;
    }

    START_TIMER("Lanczos", "eigv");
    H.resize(iter, iter);
    Q.resize(iter);

    DenseMatrix v;
    H.eigs(v, eigenvalues);

    eigenvectors = Q.apply(v);
    END_TIMER("Lanczos", "eigv");

    return 0;
}

static bool eigenvalue_sorter(std::pair<int, double> const &a, std::pair<int, double> const &b)
{
    return std::abs(a.second) > std::abs(b.second);
}

template<class Matrix, class MultiVector, class DenseMatrix>
int Solver<Matrix, MultiVector, DenseMatrix>::find_largest_eigenvalues(DenseMatrix const &eigenvalues, std::vector<int> &indices, int num_vectors)
{
    std::vector<std::pair<int, double> > index_to_value;
    for (int i = 0; i < eigenvalues.M(); i++)
        index_to_value.push_back(std::pair<int, double>(i, eigenvalues(i)));

    std::sort(index_to_value.begin(), index_to_value.end(), eigenvalue_sorter);

    for (int i = 0; i < num_vectors; i++)
        indices.push_back(index_to_value[i].first);

    return 0;
}

}

#endif
<|endoftext|>


<commit_before>//
// Created by David Li on 6/5/17.
//

#include <cmath>
#include "Map.hpp"
#include "Log.hpp"

namespace hlt {
    auto intersect_segment_circle(
        const Location& start, const Location& end,
        const Location& center, double r,
        double fudge) -> bool {
        // Derived with SymPy
        // Parameterize the segment as start + t * (end - start),
        // and substitute into the equation of a circle
        // Solve for t
        const auto dx = end.pos_x - start.pos_x;
        const auto dy = end.pos_y - start.pos_y;

        const auto a = std::pow(dx, 2) + std::pow(dy, 2);
        const auto b = -2 * (std::pow(start.pos_x, 2) - start.pos_x*end.pos_x -
            start.pos_x*center.pos_x + end.pos_x*center.pos_x +
            std::pow(start.pos_y, 2) - start.pos_y*end.pos_y -
            start.pos_y*center.pos_y + end.pos_y*center.pos_y);
        const auto c = std::pow(start.pos_x - center.pos_x, 2) +
            std::pow(start.pos_y - center.pos_y, 2);

        if (a == 0.0) {
            // Start and end are the same point
            return start.distance(center) <= r + fudge;
        }

        // Time along segment when closest to the circle (vertex of the quadratic)
        const auto t = std::min(-b / (2 * a), 1.0);
        if (t < 0) {
            return false;
        }

        const auto closest_x = start.pos_x + dx * t;
        const auto closest_y = start.pos_y + dy * t;
        const auto closest_location = Location{closest_x, closest_y};
        const auto closest_distance = closest_location.distance(center);

        return closest_distance <= r + fudge;
    }

    Map::Map() {
        map_width = 0;
        map_height = 0;
        ships = { {} };
        planets = {};
    }

    Map::Map(const Map& other_map) {
        map_width = other_map.map_width;
        map_height = other_map.map_height;
        ships = other_map.ships;
        planets = other_map.planets;
    }

    Map::Map(unsigned short width, unsigned short height) : Map() {
        map_width = width;
        map_height = height;
    }

    auto Map::within_bounds(const Location& location) const -> bool {
        return location.pos_x >= 0 && location.pos_y >= 0 &&
            location.pos_x < map_width && location.pos_y < map_height;
    }

    auto Map::is_valid(EntityId entity_id) -> bool {
        switch (entity_id.type) {
            case EntityType::InvalidEntity:
                return false;
            case EntityType::PlanetEntity:
                return entity_id.entity_index() < planets.size() && planets[entity_id.entity_index()].is_alive();
            case EntityType::ShipEntity:
                return ships.at(entity_id.player_id()).count(entity_id.entity_index()) > 0;
        }
    }

    auto Map::get_ship(PlayerId player, EntityIndex entity) -> Ship& {
        return ships.at(player).at(entity);
    }

    auto Map::get_ship(PlayerId player, EntityIndex entity) const -> const Ship& {
        return ships.at(player).at(entity);
    }

    auto Map::get_ship(EntityId entity_id) -> Ship& {
        assert(entity_id.is_valid());
        assert(entity_id.type == EntityType::ShipEntity);
        return get_ship(entity_id.player_id(), entity_id.entity_index());
    }

    auto Map::get_planet(EntityId entity_id) -> Planet& {
        assert(entity_id.is_valid());
        assert(entity_id.type == EntityType::PlanetEntity);
        assert(entity_id.entity_index() < planets.size());
        return planets[entity_id.entity_index()];
    }

    auto Map::get_entity(EntityId entity_id) -> Entity& {
        switch (entity_id.type) {
            case EntityType::InvalidEntity:
                throw std::string("Can't get entity from invalid ID");
            case EntityType::PlanetEntity:
                return get_planet(entity_id);
            case EntityType::ShipEntity:
                return get_ship(entity_id);
        }
    }

    auto Map::get_distance(Location l1, Location l2) const -> double {
        auto dx = l1.pos_x - l2.pos_x;
        auto dy = l1.pos_y - l2.pos_y;
        return sqrt((dx * dx) + (dy * dy));
    }

    auto Map::location_with_delta(const Location &location,
                                  double dx,
                                  double dy) -> possibly<Location> {
        const auto pos_x = location.pos_x + dx;
        if (pos_x < 0 || pos_x >= map_width) {
            return { Location{}, false };
        }

        const auto pos_y = location.pos_y + dy;
        if (pos_y < 0 || pos_y >= map_height) {
            return { Location{}, false };
        }

        return {
            Location{pos_x, pos_y},
            true
        };
    }

    auto Map::test(const Location &location, double radius) -> std::vector<EntityId> {
        std::vector<EntityId> result;

        for (hlt::EntityIndex planet_idx = 0; planet_idx < planets.size(); planet_idx++) {
            const auto& planet = planets[planet_idx];
            if (!planet.is_alive()) continue;

            if (location.distance2(planet.location) <= std::pow(radius + planet.radius, 2)) {
                result.push_back(EntityId::for_planet(planet_idx));
            }
        }

        for (hlt::PlayerId player_id = 0; player_id < MAX_PLAYERS; player_id++) {
            const auto& player_ships = ships[player_id];

            for (const auto& ship_pair : player_ships) {
                const auto& ship = ship_pair.second;
                if (!ship.is_alive()) continue;

                if (location.distance2(ship.location) <= std::pow(radius + ship.radius, 2)) {
                    result.push_back(EntityId::for_ship(player_id, ship_pair.first));
                }
            }
        }

        return result;
    }

    auto Map::pathable(const Location& start, const Location& target, double fudge) const -> bool {
        if (!within_bounds(target)) {
            return false;
        }

        std::stringstream logmsg;
        logmsg << "Checking pathability from " << start << " to " << target;
        Log::log(logmsg.str());

        for (const auto& planet_pair : planets) {
            const auto intersects = intersect_segment_circle(
                start, target,
                planet_pair.second.location,
                planet_pair.second.radius, fudge);
            std::stringstream msg;
            msg << "\tChecking against " << planet_pair.second.location << " r=" << planet_pair.second.radius << ' ' << intersects;
            Log::log(msg.str());
            if (intersects) {
                return false;
            }
        }

        return true;
    }

    auto Map::forecast_collision(const Location& start, const Location& target) -> bool {
        if (!within_bounds(target)) {
            return false;
        }

        for (const auto& player_ships : ships) {
            for (const auto& ship_pair : player_ships) {
                const auto& ship = ship_pair.second;
                if (intersect_segment_circle(
                    start, target,
                    ship.location, ship.radius, 0.6)) {
                    return false;
                }
            }
        }

        return true;
    }

    auto Map::adjust_for_collision(
        const Location& start, double angle, unsigned short thrust,
        int tries) -> std::pair<double, unsigned short> {
        while (tries > 0) {
            const auto target = Location{
                start.pos_x + thrust * std::cos(angle),
                start.pos_y + thrust * std::sin(angle),
            };
            if (!pathable(start, target, 0.6) || forecast_collision(start, target)) {
                std::stringstream log_msg;
                log_msg << "Forecasted collision for " << start
                        << " at angle " << angle << " at thrust " << thrust;
                Log::log(log_msg.str());
                angle += M_PI / 12;
            }
            else {
                std::stringstream log_msg;
                log_msg << "No forecasted collision for " << start
                        << " at angle " << angle << " at thrust " << thrust;
                Log::log(log_msg.str());
                break;
            }

            tries--;
        }

        return std::make_pair(angle, thrust);
    }
}
<commit_msg>Fix collision forecasting in C++ starter kit<commit_after>//
// Created by David Li on 6/5/17.
//

#include <cmath>
#include "Map.hpp"
#include "Log.hpp"

namespace hlt {
    auto intersect_segment_circle(
        const Location& start, const Location& end,
        const Location& center, double r,
        double fudge) -> bool {
        // Derived with SymPy
        // Parameterize the segment as start + t * (end - start),
        // and substitute into the equation of a circle
        // Solve for t
        const auto dx = end.pos_x - start.pos_x;
        const auto dy = end.pos_y - start.pos_y;

        const auto a = std::pow(dx, 2) + std::pow(dy, 2);
        const auto b = -2 * (std::pow(start.pos_x, 2) - start.pos_x*end.pos_x -
            start.pos_x*center.pos_x + end.pos_x*center.pos_x +
            std::pow(start.pos_y, 2) - start.pos_y*end.pos_y -
            start.pos_y*center.pos_y + end.pos_y*center.pos_y);
        const auto c = std::pow(start.pos_x - center.pos_x, 2) +
            std::pow(start.pos_y - center.pos_y, 2);

        if (a == 0.0) {
            // Start and end are the same point
            return start.distance(center) <= r + fudge;
        }

        // Time along segment when closest to the circle (vertex of the quadratic)
        const auto t = std::min(-b / (2 * a), 1.0);
        if (t < 0) {
            return false;
        }

        const auto closest_x = start.pos_x + dx * t;
        const auto closest_y = start.pos_y + dy * t;
        const auto closest_location = Location{closest_x, closest_y};
        const auto closest_distance = closest_location.distance(center);

        return closest_distance <= r + fudge;
    }

    Map::Map() {
        map_width = 0;
        map_height = 0;
        ships = { {} };
        planets = {};
    }

    Map::Map(const Map& other_map) {
        map_width = other_map.map_width;
        map_height = other_map.map_height;
        ships = other_map.ships;
        planets = other_map.planets;
    }

    Map::Map(unsigned short width, unsigned short height) : Map() {
        map_width = width;
        map_height = height;
    }

    auto Map::within_bounds(const Location& location) const -> bool {
        return location.pos_x >= 0 && location.pos_y >= 0 &&
            location.pos_x < map_width && location.pos_y < map_height;
    }

    auto Map::is_valid(EntityId entity_id) -> bool {
        switch (entity_id.type) {
            case EntityType::InvalidEntity:
                return false;
            case EntityType::PlanetEntity:
                return entity_id.entity_index() < planets.size() && planets[entity_id.entity_index()].is_alive();
            case EntityType::ShipEntity:
                return ships.at(entity_id.player_id()).count(entity_id.entity_index()) > 0;
        }
    }

    auto Map::get_ship(PlayerId player, EntityIndex entity) -> Ship& {
        return ships.at(player).at(entity);
    }

    auto Map::get_ship(PlayerId player, EntityIndex entity) const -> const Ship& {
        return ships.at(player).at(entity);
    }

    auto Map::get_ship(EntityId entity_id) -> Ship& {
        assert(entity_id.is_valid());
        assert(entity_id.type == EntityType::ShipEntity);
        return get_ship(entity_id.player_id(), entity_id.entity_index());
    }

    auto Map::get_planet(EntityId entity_id) -> Planet& {
        assert(entity_id.is_valid());
        assert(entity_id.type == EntityType::PlanetEntity);
        assert(entity_id.entity_index() < planets.size());
        return planets[entity_id.entity_index()];
    }

    auto Map::get_entity(EntityId entity_id) -> Entity& {
        switch (entity_id.type) {
            case EntityType::InvalidEntity:
                throw std::string("Can't get entity from invalid ID");
            case EntityType::PlanetEntity:
                return get_planet(entity_id);
            case EntityType::ShipEntity:
                return get_ship(entity_id);
        }
    }

    auto Map::get_distance(Location l1, Location l2) const -> double {
        auto dx = l1.pos_x - l2.pos_x;
        auto dy = l1.pos_y - l2.pos_y;
        return sqrt((dx * dx) + (dy * dy));
    }

    auto Map::location_with_delta(const Location &location,
                                  double dx,
                                  double dy) -> possibly<Location> {
        const auto pos_x = location.pos_x + dx;
        if (pos_x < 0 || pos_x >= map_width) {
            return { Location{}, false };
        }

        const auto pos_y = location.pos_y + dy;
        if (pos_y < 0 || pos_y >= map_height) {
            return { Location{}, false };
        }

        return {
            Location{pos_x, pos_y},
            true
        };
    }

    auto Map::test(const Location &location, double radius) -> std::vector<EntityId> {
        std::vector<EntityId> result;

        for (hlt::EntityIndex planet_idx = 0; planet_idx < planets.size(); planet_idx++) {
            const auto& planet = planets[planet_idx];
            if (!planet.is_alive()) continue;

            if (location.distance2(planet.location) <= std::pow(radius + planet.radius, 2)) {
                result.push_back(EntityId::for_planet(planet_idx));
            }
        }

        for (hlt::PlayerId player_id = 0; player_id < MAX_PLAYERS; player_id++) {
            const auto& player_ships = ships[player_id];

            for (const auto& ship_pair : player_ships) {
                const auto& ship = ship_pair.second;
                if (!ship.is_alive()) continue;

                if (location.distance2(ship.location) <= std::pow(radius + ship.radius, 2)) {
                    result.push_back(EntityId::for_ship(player_id, ship_pair.first));
                }
            }
        }

        return result;
    }

    auto Map::pathable(const Location& start, const Location& target, double fudge) const -> bool {
        if (!within_bounds(target)) {
            return false;
        }

        for (const auto& planet_pair : planets) {
            if (intersect_segment_circle(
                start, target,
                planet_pair.second.location,
                planet_pair.second.radius, fudge)) {
                return false;
            }
        }

        return true;
    }

    auto Map::forecast_collision(const Location& start, const Location& target) -> bool {
        if (!within_bounds(target) || !pathable(start, target, 0.6)) {
            return true;
        }

        for (const auto& player_ships : ships) {
            for (const auto& ship_pair : player_ships) {
                const auto& ship = ship_pair.second;

                if (ship.location == start) continue;

                if (intersect_segment_circle(
                    start, target,
                    ship.location, ship.radius, 0.6)) {
                    return true;
                }
            }
        }

        return false;
    }

    auto Map::adjust_for_collision(
        const Location& start, double angle, unsigned short thrust,
        int tries) -> std::pair<double, unsigned short> {
        while (tries > 0) {
            const auto target = Location{
                start.pos_x + thrust * std::cos(angle),
                start.pos_y + thrust * std::sin(angle),
            };
            if (forecast_collision(start, target)) {
                std::stringstream log_msg;
                log_msg << "Forecasted collision for " << start
                        << " at angle " << angle << " at thrust " << thrust;
                Log::log(log_msg.str());
                angle += M_PI / 12;
            }
            else {
                std::stringstream log_msg;
                log_msg << "No forecasted collision for " << start
                        << " at angle " << angle << " at thrust " << thrust;
                Log::log(log_msg.str());
                break;
            }

            tries--;
        }

        return std::make_pair(angle, thrust);
    }
}
<|endoftext|>


<commit_before>#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <algorithm>
#include "MemoryFunction.h"

#ifdef WIN32

#include <windows.h>

#elif defined(__APPLE__)

#include "TargetConditionals.h"
#include <mach/mach_init.h>
#include <mach/vm_map.h>

#if defined(TARGET_IPHONE_SIMULATOR) || defined(TARGET_OS_MAC)
extern "C" void __clear_cache(void* begin, void* end);
#endif

#elif defined(__ANDROID__)

#include <sys/mman.h>

#endif

CMemoryFunction::CMemoryFunction()
: m_code(nullptr)
, m_size(0)
{

}

CMemoryFunction::CMemoryFunction(const void* code, size_t size)
: m_code(nullptr)
{
#ifdef WIN32
	m_size = size;
	m_code = malloc(size);
	memcpy(m_code, code, size);

	DWORD oldProtect = 0;
	BOOL result = VirtualProtect(m_code, size, PAGE_EXECUTE_READWRITE, &oldProtect);
	assert(result == TRUE);
#elif defined(__APPLE__)
	vm_size_t page_size = 0;
	host_page_size(mach_task_self(), &page_size);
	unsigned int allocSize = ((size + page_size - 1) / page_size) * page_size;
	vm_allocate(mach_task_self(), reinterpret_cast<vm_address_t*>(&m_code), allocSize, TRUE);
	memcpy(m_code, code, size);
	__clear_cache(m_code, reinterpret_cast<uint8*>(m_code) + size);
	kern_return_t result = vm_protect(mach_task_self(), reinterpret_cast<vm_address_t>(m_code), size, 0, VM_PROT_READ | VM_PROT_EXECUTE);
	assert(result == 0);
	m_size = allocSize;
#elif defined(__ANDROID__)
	m_size = size;
	m_code = mmap(nullptr, size, PROT_WRITE | PROT_EXEC, MAP_PRIVATE | MAP_ANONYMOUS, -1, 0);
	assert(m_code != MAP_FAILED);
	memcpy(m_code, code, size);
	__clear_cache(m_code, reinterpret_cast<uint8*>(m_code) + size);
#endif
}

CMemoryFunction::~CMemoryFunction()
{
	Reset();
}

void CMemoryFunction::Reset()
{
	if(m_code != nullptr)
	{
#ifdef WIN32
		free(m_code);
#elif defined(__APPLE__)
		vm_deallocate(mach_task_self(), reinterpret_cast<vm_address_t>(m_code), m_size);
#elif defined(__ANDROID__)
		munmap(m_code, m_size);
#endif
	}
	m_code = nullptr;
	m_size = 0;
}

bool CMemoryFunction::IsEmpty() const
{
	return m_code == nullptr;
}

CMemoryFunction& CMemoryFunction::operator =(CMemoryFunction&& rhs)
{
	Reset();
	std::swap(m_code, rhs.m_code);
	std::swap(m_size, rhs.m_size);
	return (*this);
}

void CMemoryFunction::operator()(void* context)
{
	typedef void (*FctType)(void*);
	auto fct = reinterpret_cast<FctType>(m_code);
	fct(context);
}

void* CMemoryFunction::GetCode() const
{
	return m_code;
}

size_t CMemoryFunction::GetSize() const
{
	return m_size;
}
<commit_msg>Use proper cache clear function on MacOSX/iOS.<commit_after>#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <algorithm>
#include "MemoryFunction.h"

#ifdef WIN32

#include <windows.h>

#elif defined(__APPLE__)

#include "TargetConditionals.h"
#include <mach/mach_init.h>
#include <mach/vm_map.h>
#include <libkern/OSCacheControl.h>

#elif defined(__ANDROID__)

#include <sys/mman.h>

#endif

CMemoryFunction::CMemoryFunction()
: m_code(nullptr)
, m_size(0)
{

}

CMemoryFunction::CMemoryFunction(const void* code, size_t size)
: m_code(nullptr)
{
#ifdef WIN32
	m_size = size;
	m_code = malloc(size);
	memcpy(m_code, code, size);

	DWORD oldProtect = 0;
	BOOL result = VirtualProtect(m_code, size, PAGE_EXECUTE_READWRITE, &oldProtect);
	assert(result == TRUE);
#elif defined(__APPLE__)
	vm_size_t page_size = 0;
	host_page_size(mach_task_self(), &page_size);
	unsigned int allocSize = ((size + page_size - 1) / page_size) * page_size;
	vm_allocate(mach_task_self(), reinterpret_cast<vm_address_t*>(&m_code), allocSize, TRUE);
	memcpy(m_code, code, size);
	sys_icache_invalidate(m_code, size);
	kern_return_t result = vm_protect(mach_task_self(), reinterpret_cast<vm_address_t>(m_code), size, 0, VM_PROT_READ | VM_PROT_EXECUTE);
	assert(result == 0);
	m_size = allocSize;
#elif defined(__ANDROID__)
	m_size = size;
	m_code = mmap(nullptr, size, PROT_WRITE | PROT_EXEC, MAP_PRIVATE | MAP_ANONYMOUS, -1, 0);
	assert(m_code != MAP_FAILED);
	memcpy(m_code, code, size);
	__clear_cache(m_code, reinterpret_cast<uint8*>(m_code) + size);
#endif
}

CMemoryFunction::~CMemoryFunction()
{
	Reset();
}

void CMemoryFunction::Reset()
{
	if(m_code != nullptr)
	{
#ifdef WIN32
		free(m_code);
#elif defined(__APPLE__)
		vm_deallocate(mach_task_self(), reinterpret_cast<vm_address_t>(m_code), m_size);
#elif defined(__ANDROID__)
		munmap(m_code, m_size);
#endif
	}
	m_code = nullptr;
	m_size = 0;
}

bool CMemoryFunction::IsEmpty() const
{
	return m_code == nullptr;
}

CMemoryFunction& CMemoryFunction::operator =(CMemoryFunction&& rhs)
{
	Reset();
	std::swap(m_code, rhs.m_code);
	std::swap(m_size, rhs.m_size);
	return (*this);
}

void CMemoryFunction::operator()(void* context)
{
	typedef void (*FctType)(void*);
	auto fct = reinterpret_cast<FctType>(m_code);
	fct(context);
}

void* CMemoryFunction::GetCode() const
{
	return m_code;
}

size_t CMemoryFunction::GetSize() const
{
	return m_size;
}
<|endoftext|>