<commit_before>// $Id$

#include "ConfusionNet.h"
#include <sstream>

#include "FactorCollection.h"
#include "Util.h"
#include "TranslationOptionCollectionConfusionNet.h"
#include "StaticData.h"
#include "Sentence.h"
#include "UserMessage.h"
#include "moses/FF/InputFeature.h"
#include "util/exception.hh"

namespace Moses
{
  struct CNStats {
    size_t created,destr,read,colls,words;

    CNStats() : created(0),destr(0),read(0),colls(0),words(0) {}
    ~CNStats() {
      print(std::cerr);
    }

    void createOne() {
      ++created;
    }
    void destroyOne() {
      ++destr;
    }

    void collect(const ConfusionNet& cn) {
      ++read;
      colls+=cn.GetSize();
      for(size_t i=0; i<cn.GetSize(); ++i)
	words+=cn[i].size();
    }
    void print(std::ostream& out) const {
      if(created>0) {
	out<<"confusion net statistics:\n"
	  " created:\t"<<created<<"\n"
	  " destroyed:\t"<<destr<<"\n"
	  " succ. read:\t"<<read<<"\n"
	  " columns:\t"<<colls<<"\n"
	  " words:\t"<<words<<"\n"
	  " avg. word/column:\t"<<words/(1.0*colls)<<"\n"
	  " avg. cols/sent:\t"<<colls/(1.0*read)<<"\n"
	  "\n\n";
      }
    }
  };

  CNStats stats;

  size_t
  ConfusionNet::
  GetColumnIncrement(size_t i, size_t j) const
  {
    (void) i;
    (void) j;
    return 1;
  }

  ConfusionNet::
  ConfusionNet()
    : InputType()
  {
    stats.createOne();

    const StaticData& staticData = StaticData::Instance();
    if (staticData.IsChart()) {
      m_defaultLabelSet.insert(StaticData::Instance().GetInputDefaultNonTerminal());
    }
    UTIL_THROW_IF2(&InputFeature::Instance() == NULL, "Input feature must be specified");
  }

  ConfusionNet::
  ~ConfusionNet()
  {
    stats.destroyOne();
  }

  ConfusionNet::
  ConfusionNet(Sentence const& s)
  {
    data.resize(s.GetSize());
    for(size_t i=0; i<s.GetSize(); ++i) {
      ScorePair scorePair;
      std::pair<Word, ScorePair > temp = std::make_pair(s.GetWord(i), scorePair);
      data[i].push_back(temp);
    }
  }

  bool
  ConfusionNet::
  ReadF(std::istream& in, const std::vector<FactorType>& factorOrder, int format)
  {
    VERBOSE(2, "read confusion net with format "<<format<<"\n");
    switch(format) {
    case 0:
      return ReadFormat0(in,factorOrder);
    case 1:
      return ReadFormat1(in,factorOrder);
    default:
      std::stringstream strme;
      strme << "ERROR: unknown format '"<<format
	    <<"' in ConfusionNet::Read";
      UserMessage::Add(strme.str());
    }
    return false;
  }

  int
  ConfusionNet::
  Read(std::istream& in,
       const std::vector<FactorType>& factorOrder)
  {
    int rv=ReadF(in,factorOrder,0);
    if(rv) stats.collect(*this);
    return rv;
  }

#if 0
  // Deprecated due to code duplication;
  // use Word::CreateFromString() instead
  void
  ConfusionNet::
  String2Word(const std::string& s,Word& w,
	      const std::vector<FactorType>& factorOrder)
  {
    std::vector<std::string> factorStrVector = Tokenize(s, "|");
    for(size_t i=0; i<factorOrder.size(); ++i)
      w.SetFactor(factorOrder[i],
		  FactorCollection::Instance().AddFactor
		  (Input,factorOrder[i], factorStrVector[i]));
  }
#endif

  bool
  ConfusionNet::
  ReadFormat0(std::istream& in, const std::vector<FactorType>& factorOrder)
  {
    Clear();

    const StaticData   &staticData   = StaticData::Instance();
    const InputFeature &inputFeature = InputFeature::Instance();
    size_t numInputScores   = inputFeature.GetNumInputScores();
    size_t numRealWordCount = inputFeature.GetNumRealWordsInInput();

    size_t totalCount = numInputScores + numRealWordCount;
    bool addRealWordCount = (numRealWordCount > 0);

    std::string line;
    while(getline(in,line)) {
      std::istringstream is(line);
      std::string word;

      Column col;
      while(is>>word) {
	Word w;
	// String2Word(word,w,factorOrder);
	w.CreateFromString(Input,factorOrder,StringPiece(word),false,false);
	std::vector<float> probs(totalCount, 0.0);
	for(size_t i=0; i < numInputScores; i++) {
	  double prob;
	  if (!(is>>prob)) {
	    TRACE_ERR("ERROR: unable to parse CN input - bad link probability, or wrong number of scores\n");
	    return false;
	  }
	  if(prob<0.0) {
	    VERBOSE(1, "WARN: negative prob: "<<prob<<" ->set to 0.0\n");
	    prob=0.0;
	  } else if (prob>1.0) {
	    VERBOSE(1, "WARN: prob > 1.0 : "<<prob<<" -> set to 1.0\n");
	    prob=1.0;
	  }
	  probs[i] = (std::max(static_cast<float>(log(prob)),LOWEST_SCORE));

	}
	//store 'real' word count in last feature if we have one more weight than we do arc scores and not epsilon
	if (addRealWordCount && word!=EPSILON && word!="")
	  probs.back() = -1.0;

	ScorePair scorePair(probs);

	col.push_back(std::make_pair(w,scorePair));
      }
      if(col.size()) {
	data.push_back(col);
	ShrinkToFit(data.back());
      } else break;
    }
    return !data.empty();
  }

  bool
  ConfusionNet::
  ReadFormat1(std::istream& in, const std::vector<FactorType>& factorOrder)
  {
    Clear();
    std::string line;
    if(!getline(in,line)) return 0;
    size_t s;
    if(getline(in,line)) s=atoi(line.c_str());
    else return 0;
    data.resize(s);
    for(size_t i=0; i<data.size(); ++i) {
      if(!getline(in,line)) return 0;
      std::istringstream is(line);
      if(!(is>>s)) return 0;
      std::string word;
      double prob;
      data[i].resize(s);
      for(size_t j=0; j<s; ++j)
	if(is>>word>>prob) {
	  //TODO: we are only reading one prob from this input format, should read many... but this function is unused anyway. -JS
	  data[i][j].second.denseScores = std::vector<float> (1);
	  data[i][j].second.denseScores.push_back((float) log(prob));
	  if(data[i][j].second.denseScores[0]<0) {
	    VERBOSE(1, "WARN: neg costs: "<<data[i][j].second.denseScores[0]<<" -> set to 0\n");
	    data[i][j].second.denseScores[0]=0.0;
	  }
	  // String2Word(word,data[i][j].first,factorOrder);
	  Word& w = data[i][j].first;
	  w.CreateFromString(Input,factorOrder,StringPiece(word),false,false);
	} else return 0;
    }
    return !data.empty();
  }

  void ConfusionNet::Print(std::ostream& out) const
  {
    out<<"conf net: "<<data.size()<<"\n";
    for(size_t i=0; i<data.size(); ++i) {
      out<<i<<" -- ";
      for(size_t j=0; j<data[i].size(); ++j) {
	out<<"("<<data[i][j].first.ToString()<<", ";

	// dense
	std::vector<float>::const_iterator iterDense;
	for(iterDense = data[i][j].second.denseScores.begin();
	    iterDense < data[i][j].second.denseScores.end();
	    ++iterDense) {
	  out<<", "<<*iterDense;
	}

	// sparse
	std::map<StringPiece, float>::const_iterator iterSparse;
	for(iterSparse = data[i][j].second.sparseScores.begin();
	    iterSparse != data[i][j].second.sparseScores.end();
	    ++iterSparse) {
	  out << ", " << iterSparse->first << "=" << iterSparse->second;
	}

	out<<") ";
      }
      out<<"\n";
    }
    out<<"\n\n";
  }

#ifdef _WIN32
#pragma warning(disable:4716)
#endif
  Phrase
  ConfusionNet::
  GetSubString(const WordsRange&) const
  {
    UTIL_THROW2("ERROR: call to ConfusionNet::GetSubString\n");
    //return Phrase(Input);
  }

  std::string
  ConfusionNet::
  GetStringRep(const std::vector<FactorType> /* factorsToPrint */) const  //not well defined yet
  {
    TRACE_ERR("ERROR: call to ConfusionNet::GeStringRep\n");
    return "";
  }
#ifdef _WIN32
#pragma warning(disable:4716)
#endif
  const Word& ConfusionNet::GetWord(size_t) const
  {
    UTIL_THROW2("ERROR: call to ConfusionNet::GetFactorArray\n");
  }
#ifdef _WIN32
#pragma warning(default:4716)
#endif
  std::ostream& operator<<(std::ostream& out,const ConfusionNet& cn)
  {
    cn.Print(out);
    return out;
  }

  TranslationOptionCollection*
  ConfusionNet::
  CreateTranslationOptionCollection() const
  {
    size_t maxNoTransOptPerCoverage
      = StaticData::Instance().GetMaxNoTransOptPerCoverage();
    float translationOptionThreshold
      = StaticData::Instance().GetTranslationOptionThreshold();
    TranslationOptionCollection *rv
      = new TranslationOptionCollectionConfusionNet
      (*this, maxNoTransOptPerCoverage, translationOptionThreshold);
    assert(rv);
    return rv;
  }

}

<commit_msg>Commented out unused variable.<commit_after>// $Id$

#include "ConfusionNet.h"
#include <sstream>

#include "FactorCollection.h"
#include "Util.h"
#include "TranslationOptionCollectionConfusionNet.h"
#include "StaticData.h"
#include "Sentence.h"
#include "UserMessage.h"
#include "moses/FF/InputFeature.h"
#include "util/exception.hh"

namespace Moses
{
  struct CNStats {
    size_t created,destr,read,colls,words;

    CNStats() : created(0),destr(0),read(0),colls(0),words(0) {}
    ~CNStats() {
      print(std::cerr);
    }

    void createOne() {
      ++created;
    }
    void destroyOne() {
      ++destr;
    }

    void collect(const ConfusionNet& cn) {
      ++read;
      colls+=cn.GetSize();
      for(size_t i=0; i<cn.GetSize(); ++i)
	words+=cn[i].size();
    }
    void print(std::ostream& out) const {
      if(created>0) {
	out<<"confusion net statistics:\n"
	  " created:\t"<<created<<"\n"
	  " destroyed:\t"<<destr<<"\n"
	  " succ. read:\t"<<read<<"\n"
	  " columns:\t"<<colls<<"\n"
	  " words:\t"<<words<<"\n"
	  " avg. word/column:\t"<<words/(1.0*colls)<<"\n"
	  " avg. cols/sent:\t"<<colls/(1.0*read)<<"\n"
	  "\n\n";
      }
    }
  };

  CNStats stats;

  size_t
  ConfusionNet::
  GetColumnIncrement(size_t i, size_t j) const
  {
    (void) i;
    (void) j;
    return 1;
  }

  ConfusionNet::
  ConfusionNet()
    : InputType()
  {
    stats.createOne();

    const StaticData& staticData = StaticData::Instance();
    if (staticData.IsChart()) {
      m_defaultLabelSet.insert(StaticData::Instance().GetInputDefaultNonTerminal());
    }
    UTIL_THROW_IF2(&InputFeature::Instance() == NULL, "Input feature must be specified");
  }

  ConfusionNet::
  ~ConfusionNet()
  {
    stats.destroyOne();
  }

  ConfusionNet::
  ConfusionNet(Sentence const& s)
  {
    data.resize(s.GetSize());
    for(size_t i=0; i<s.GetSize(); ++i) {
      ScorePair scorePair;
      std::pair<Word, ScorePair > temp = std::make_pair(s.GetWord(i), scorePair);
      data[i].push_back(temp);
    }
  }

  bool
  ConfusionNet::
  ReadF(std::istream& in, const std::vector<FactorType>& factorOrder, int format)
  {
    VERBOSE(2, "read confusion net with format "<<format<<"\n");
    switch(format) {
    case 0:
      return ReadFormat0(in,factorOrder);
    case 1:
      return ReadFormat1(in,factorOrder);
    default:
      std::stringstream strme;
      strme << "ERROR: unknown format '"<<format
	    <<"' in ConfusionNet::Read";
      UserMessage::Add(strme.str());
    }
    return false;
  }

  int
  ConfusionNet::
  Read(std::istream& in,
       const std::vector<FactorType>& factorOrder)
  {
    int rv=ReadF(in,factorOrder,0);
    if(rv) stats.collect(*this);
    return rv;
  }

#if 0
  // Deprecated due to code duplication;
  // use Word::CreateFromString() instead
  void
  ConfusionNet::
  String2Word(const std::string& s,Word& w,
	      const std::vector<FactorType>& factorOrder)
  {
    std::vector<std::string> factorStrVector = Tokenize(s, "|");
    for(size_t i=0; i<factorOrder.size(); ++i)
      w.SetFactor(factorOrder[i],
		  FactorCollection::Instance().AddFactor
		  (Input,factorOrder[i], factorStrVector[i]));
  }
#endif

  bool
  ConfusionNet::
  ReadFormat0(std::istream& in, const std::vector<FactorType>& factorOrder)
  {
    Clear();

    // const StaticData   &staticData   = StaticData::Instance();
    const InputFeature &inputFeature = InputFeature::Instance();
    size_t numInputScores   = inputFeature.GetNumInputScores();
    size_t numRealWordCount = inputFeature.GetNumRealWordsInInput();

    size_t totalCount = numInputScores + numRealWordCount;
    bool addRealWordCount = (numRealWordCount > 0);

    std::string line;
    while(getline(in,line)) {
      std::istringstream is(line);
      std::string word;

      Column col;
      while(is>>word) {
	Word w;
	// String2Word(word,w,factorOrder);
	w.CreateFromString(Input,factorOrder,StringPiece(word),false,false);
	std::vector<float> probs(totalCount, 0.0);
	for(size_t i=0; i < numInputScores; i++) {
	  double prob;
	  if (!(is>>prob)) {
	    TRACE_ERR("ERROR: unable to parse CN input - bad link probability, or wrong number of scores\n");
	    return false;
	  }
	  if(prob<0.0) {
	    VERBOSE(1, "WARN: negative prob: "<<prob<<" ->set to 0.0\n");
	    prob=0.0;
	  } else if (prob>1.0) {
	    VERBOSE(1, "WARN: prob > 1.0 : "<<prob<<" -> set to 1.0\n");
	    prob=1.0;
	  }
	  probs[i] = (std::max(static_cast<float>(log(prob)),LOWEST_SCORE));

	}
	//store 'real' word count in last feature if we have one more weight than we do arc scores and not epsilon
	if (addRealWordCount && word!=EPSILON && word!="")
	  probs.back() = -1.0;

	ScorePair scorePair(probs);

	col.push_back(std::make_pair(w,scorePair));
      }
      if(col.size()) {
	data.push_back(col);
	ShrinkToFit(data.back());
      } else break;
    }
    return !data.empty();
  }

  bool
  ConfusionNet::
  ReadFormat1(std::istream& in, const std::vector<FactorType>& factorOrder)
  {
    Clear();
    std::string line;
    if(!getline(in,line)) return 0;
    size_t s;
    if(getline(in,line)) s=atoi(line.c_str());
    else return 0;
    data.resize(s);
    for(size_t i=0; i<data.size(); ++i) {
      if(!getline(in,line)) return 0;
      std::istringstream is(line);
      if(!(is>>s)) return 0;
      std::string word;
      double prob;
      data[i].resize(s);
      for(size_t j=0; j<s; ++j)
	if(is>>word>>prob) {
	  //TODO: we are only reading one prob from this input format, should read many... but this function is unused anyway. -JS
	  data[i][j].second.denseScores = std::vector<float> (1);
	  data[i][j].second.denseScores.push_back((float) log(prob));
	  if(data[i][j].second.denseScores[0]<0) {
	    VERBOSE(1, "WARN: neg costs: "<<data[i][j].second.denseScores[0]<<" -> set to 0\n");
	    data[i][j].second.denseScores[0]=0.0;
	  }
	  // String2Word(word,data[i][j].first,factorOrder);
	  Word& w = data[i][j].first;
	  w.CreateFromString(Input,factorOrder,StringPiece(word),false,false);
	} else return 0;
    }
    return !data.empty();
  }

  void ConfusionNet::Print(std::ostream& out) const
  {
    out<<"conf net: "<<data.size()<<"\n";
    for(size_t i=0; i<data.size(); ++i) {
      out<<i<<" -- ";
      for(size_t j=0; j<data[i].size(); ++j) {
	out<<"("<<data[i][j].first.ToString()<<", ";

	// dense
	std::vector<float>::const_iterator iterDense;
	for(iterDense = data[i][j].second.denseScores.begin();
	    iterDense < data[i][j].second.denseScores.end();
	    ++iterDense) {
	  out<<", "<<*iterDense;
	}

	// sparse
	std::map<StringPiece, float>::const_iterator iterSparse;
	for(iterSparse = data[i][j].second.sparseScores.begin();
	    iterSparse != data[i][j].second.sparseScores.end();
	    ++iterSparse) {
	  out << ", " << iterSparse->first << "=" << iterSparse->second;
	}

	out<<") ";
      }
      out<<"\n";
    }
    out<<"\n\n";
  }

#ifdef _WIN32
#pragma warning(disable:4716)
#endif
  Phrase
  ConfusionNet::
  GetSubString(const WordsRange&) const
  {
    UTIL_THROW2("ERROR: call to ConfusionNet::GetSubString\n");
    //return Phrase(Input);
  }

  std::string
  ConfusionNet::
  GetStringRep(const std::vector<FactorType> /* factorsToPrint */) const  //not well defined yet
  {
    TRACE_ERR("ERROR: call to ConfusionNet::GeStringRep\n");
    return "";
  }
#ifdef _WIN32
#pragma warning(disable:4716)
#endif
  const Word& ConfusionNet::GetWord(size_t) const
  {
    UTIL_THROW2("ERROR: call to ConfusionNet::GetFactorArray\n");
  }
#ifdef _WIN32
#pragma warning(default:4716)
#endif
  std::ostream& operator<<(std::ostream& out,const ConfusionNet& cn)
  {
    cn.Print(out);
    return out;
  }

  TranslationOptionCollection*
  ConfusionNet::
  CreateTranslationOptionCollection() const
  {
    size_t maxNoTransOptPerCoverage
      = StaticData::Instance().GetMaxNoTransOptPerCoverage();
    float translationOptionThreshold
      = StaticData::Instance().GetTranslationOptionThreshold();
    TranslationOptionCollection *rv
      = new TranslationOptionCollectionConfusionNet
      (*this, maxNoTransOptPerCoverage, translationOptionThreshold);
    assert(rv);
    return rv;
  }

}

<|endoftext|>


<commit_before>/*
  Copyright (c) 2010 Timothy Lovorn

  Permission is hereby granted, free of charge, to any person obtaining a copy
  of this software and associated documentation files (the "Software"), to deal
  in the Software without restriction, including without limitation the rights
  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  copies of the Software, and to permit persons to whom the Software is
  furnished to do so, subject to the following conditions:

  The above copyright notice and this permission notice shall be included in
  all copies or substantial portions of the Software.

  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  THE SOFTWARE.
*/

#ifndef __SCSS__BZONE_H
#define __SCSS__BZONE_H

#include <cmath>
#include <cfloat>

#include "BaseState.hh"
#include "ZeroTempState.hh"

class BZone {
public:
    template <class SpecializedState>
    static double average(const BaseState& stBase,
        const SpecializedState& stSpec,
        double (*innerFunc)(const SpecializedState&, double, double));

    template <class SpecializedState>
    static double minimum(const BaseState& stBase,
        const SpecializedState& stSpec,
        double (*innerFunc)(const SpecializedState&, double, double));
};

// Would be nice to have a single function handle transforming one BZone point
// into another instead of duplicating the traversal
// OR just specify accumulator function (val = accum(val, thisPointVal))
template <class SpecializedState>
double BZone::minimum(const BaseState& stBase,
        const SpecializedState& stSpec,
        double (*innerFunc)(const SpecializedState&, double, double)) {
    double kx = -M_PI, ky = -M_PI, min = DBL_MAX, val;
    int N = stBase.env.gridLen;
    double step = 2 * M_PI / N;
    while (ky < M_PI) {
        while (kx < M_PI) {
            val = innerFunc(stSpec, kx, ky);
            if (val < min) {
                min = val;
            }
            kx += step;
        }
        ky += step;
        kx = -M_PI;
    }
    return min;
}

template <class SpecializedState>
double BZone::average(const BaseState& stBase,
        const SpecializedState& stSpec,
        double (*innerFunc)(const SpecializedState&, double, double)) {
    double kx = -M_PI, ky = -M_PI, min = DBL_MAX, sum = 0.0;
    int N = stBase.env.gridLen;
    double step = 2 * M_PI / N;
    while (ky < M_PI) {
        while (kx < M_PI) {
            sum += innerFunc(stSpec, kx, ky);
            kx += step;
        }
        ky += step;
        kx = -M_PI;
    }
    return sum / (N * N);
}

#endif
<commit_msg>starting on BZone vector average<commit_after>/*
  Copyright (c) 2010 Timothy Lovorn

  Permission is hereby granted, free of charge, to any person obtaining a copy
  of this software and associated documentation files (the "Software"), to deal
  in the Software without restriction, including without limitation the rights
  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  copies of the Software, and to permit persons to whom the Software is
  furnished to do so, subject to the following conditions:

  The above copyright notice and this permission notice shall be included in
  all copies or substantial portions of the Software.

  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  THE SOFTWARE.
*/

#ifndef __SCSS__BZONE_H
#define __SCSS__BZONE_H

#include <cmath>
#include <cfloat>
#include <vector>

#include "BaseState.hh"
#include "ZeroTempState.hh"

class BZone {
public:
    template <class SpecializedState>
    static double average(const BaseState& stBase,
        const SpecializedState& stSpec,
        double (*innerFunc)(const SpecializedState&, double, double));

    template <class SpecializedState>
    static double minimum(const BaseState& stBase,
        const SpecializedState& stSpec,
        double (*innerFunc)(const SpecializedState&, double, double));

    template <class SpecializedState>
    static std::vector average(const BaseState& stBase,
        const SpecializedState& stSpec,
        std::vector (*innerFunc)(const SpecializedState&, double, double));
};

// Would be nice to have a single function handle transforming one BZone point
// into another instead of duplicating the traversal
// OR just specify accumulator function (val = accum(val, thisPointVal))
template <class SpecializedState>
double BZone::minimum(const BaseState& stBase,
        const SpecializedState& stSpec,
        double (*innerFunc)(const SpecializedState&, double, double)) {
    double kx = -M_PI, ky = -M_PI, min = DBL_MAX, val;
    int N = stBase.env.gridLen;
    double step = 2 * M_PI / N;
    while (ky < M_PI) {
        while (kx < M_PI) {
            val = innerFunc(stSpec, kx, ky);
            if (val < min) {
                min = val;
            }
            kx += step;
        }
        ky += step;
        kx = -M_PI;
    }
    return min;
}

template <class SpecializedState>
double BZone::average(const BaseState& stBase,
        const SpecializedState& stSpec,
        double (*innerFunc)(const SpecializedState&, double, double)) {
    double kx = -M_PI, ky = -M_PI, min = DBL_MAX, sum = 0.0;
    int N = stBase.env.gridLen;
    double step = 2 * M_PI / N;
    while (ky < M_PI) {
        while (kx < M_PI) {
            sum += innerFunc(stSpec, kx, ky);
            kx += step;
        }
        ky += step;
        kx = -M_PI;
    }
    return sum / (N * N);
}

template <class SpecializedState>
std::vector<double> BZone::vectorAverage(const BaseState& stBase,
        const SpecializedState& stSpec,
        std::vector<double> (*innerFunc)(const SpecializedState&,
                                         double, double)) {
    double kx = -M_PI, ky = -M_PI, min = DBL_MAX;
    int N = stBase.env.gridLen;
    double step = 2 * M_PI / N;
    std::vector sums;
    while (ky < M_PI) {
        while (kx < M_PI) {
            std::vector newTerms = innerFunc(stSpec, kx, ky);
            for (it = newTerms.begin() ; it < newTerms.end(); it++ ) {
                // expanded sums if needed; added newTerms
            }
            kx += step;
        }
        ky += step;
        kx = -M_PI;
    }
    return sum / (N * N);
}

#endif
<|endoftext|>


<commit_before>#include <bitbots_ros_control/wolfgang_hardware_interface.h>

namespace bitbots_ros_control {

/**
 * This class provides a combination of multiple hardware interfaces to construct a complete Wolfgang robot.
 * It is similar to a CombinedRobotHw class as specified in ros_control, but it is changed a bit to make sharing of
 * a common bus driver over multiple hardware interfaces possible.
 */
WolfgangHardwareInterface::WolfgangHardwareInterface(ros::NodeHandle &nh) {
  first_ping_error_ = true;
  speak_pub_ = nh.advertise<humanoid_league_msgs::Audio>("/speak", 1);

  // load parameters
  ROS_INFO_STREAM("Loading parameters from namespace " << nh.getNamespace());
  nh.param<bool>("only_imu", only_imu_, false);
  if (only_imu_) ROS_WARN("Starting in only IMU mode");
  nh.param<bool>("only_pressure", only_pressure_, false);
  if (only_pressure_) ROS_WARN("starting in only pressure sensor mode");

  if (only_pressure_ && only_imu_) {
    ROS_ERROR("only_imu AND only_pressure was set to true");
    exit(1);
  }

  // get list of all bus devices
  XmlRpc::XmlRpcValue dxls_xml;
  nh.getParam("device_info", dxls_xml);
  ROS_ASSERT(dxls_xml.getType() == XmlRpc::XmlRpcValue::TypeStruct);

  // Convert dxls to native type: a vector of tuples with name and id for sorting purposes
  std::vector<std::pair<std::string, int>> dxl_devices;
  for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = dxls_xml.begin(); it != dxls_xml.end(); ++it) {
    std::string name = it->first;
    XmlRpc::XmlRpcValue data = it->second;
    int id = data["id"];
    dxl_devices.emplace_back(name, id);
  }

  // sort the devices by id. This way the devices will always be read and written in ID order later, making debug easier.
  std::sort(dxl_devices.begin(), dxl_devices.end(),
            [](std::pair<std::string, int> &a, std::pair<std::string, int> &b) { return a.second < b.second; });

  // create overall servo interface since we need a single interface for the controllers
  servo_interface_ = DynamixelServoHardwareInterface();
  servo_interface_.setParent(this);
  //todo check and remove if possible
  /* duplicate?
  // set the dynamic reconfigure and load standard params for servo interface
 dynamic_reconfigure::Server<bitbots_ros_control::dynamixel_servo_hardware_interface_paramsConfig> server;
 dynamic_reconfigure::Server<bitbots_ros_control::dynamixel_servo_hardware_interface_paramsConfig>::CallbackType
     f;
 f = boost::bind(&bitbots_ros_control::DynamixelServoHardwareInterface::reconfCallback, servo_interface_, _1, _2);
 server.setCallback(f);*/

  // try to ping all devices on the list, add them to the driver and create corresponding hardware interfaces
  // try until interruption to enable the user to turn on the power
  while (ros::ok()) {
    if (create_interfaces(nh, dxl_devices)) {
      break;
    }
    //sleep(3);
  }
}

//todo this could be done parallel with threads for speed up
bool WolfgangHardwareInterface::create_interfaces(ros::NodeHandle &nh,
                                                  std::vector<std::pair<std::string, int>> dxl_devices) {
  interfaces_ = std::vector<std::vector<hardware_interface::RobotHW *>>();
  // init bus drivers
  std::vector<std::string> pinged;
  XmlRpc::XmlRpcValue port_xml;
  nh.getParam("port_info", port_xml);
  for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = port_xml.begin(); it != port_xml.end(); ++it) {
    // read bus driver specifications from config
    XmlRpc::XmlRpcValue port_data = it->second;
    std::string device_file = port_data["device_file"];
    int baudrate = port_data["baudrate"];
    int protocol_version = port_data["protocol_version"];
    auto driver = std::make_shared<DynamixelDriver>();
    if (!driver->init(device_file.c_str(), uint32_t(baudrate))) {
      ROS_ERROR("Error opening serial port %s", device_file.c_str());
      speakError(speak_pub_, "Error opening serial port");
      sleep(1);
      exit(1);
    }
    // some interface seem to produce some gitter directly after connecting. wait or it will interfere with pings
    // sleep(1);
    driver->setPacketHandler(protocol_version);
    std::vector<hardware_interface::RobotHW *> interfaces_on_port;
    // iterate over all devices and ping them to see what is connected to this bus
    std::vector<std::tuple<int, std::string, float, float>> servos_on_port;
    for (std::pair<std::string, int> &device : dxl_devices) {
      std::string name = device.first;
      int id = device.second;
      if (std::find(pinged.begin(), pinged.end(), device.first) != pinged.end()) {
        // we already found this and dont have to search again
      } else {
        ros::NodeHandle dxl_nh(nh, "device_info/" + name);
        int model_number_specified;
        dxl_nh.getParam("model_number", model_number_specified);
        // some devices provide more than one type of interface, e.g. the IMU provides additionally buttons and LEDs
        std::string interface_type;
        dxl_nh.param<std::string>("interface_type", interface_type, "");
        uint16_t model_number_specified_16 = uint16_t(model_number_specified);
        uint16_t *model_number_returned_16 = new uint16_t;
        if (driver->ping(uint8_t(id), model_number_returned_16)) {
          // check if the specified model number matches the actual model number of the device
          if (model_number_specified_16 != *model_number_returned_16) {
            ROS_WARN("Model number of id %d does not match", id);
          }
          // ping was successful, add device correspondingly
          // only add them if the mode is set correspondingly
          // TODO maybe move more of the parameter stuff in the init of the modules instead of doing everything here
          if (model_number_specified == 0xABBA && interface_type == "CORE") {
            // CORE
            int read_rate;
            dxl_nh.param<int>("read_rate", read_rate, 1);
            driver->setTools(model_number_specified_16, id);
            CoreHardwareInterface *interface = new CoreHardwareInterface(driver, id, read_rate);
            // turn on power, just to be sure
            interface->write(ros::Time::now(), ros::Duration(0));
            interfaces_on_port.push_back(interface);
          } else if (model_number_specified == 0 && !only_imu_) {//model number is currently 0 on foot sensors
            // bitfoot
            std::string topic;
            if (!dxl_nh.getParam("topic", topic)) {
              ROS_WARN("Bitfoot topic not specified");
            }
            BitFootHardwareInterface *interface = new BitFootHardwareInterface(driver, id, topic, name);
            interfaces_on_port.push_back(interface);
          } else if (model_number_specified == 0xBAFF && interface_type == "IMU" && !only_pressure_) {
            //IMU
            std::string topic;
            if (!dxl_nh.getParam("topic", topic)) {
              ROS_WARN("IMU topic not specified");
            }
            std::string frame;
            if (!dxl_nh.getParam("frame", frame)) {
              ROS_WARN("IMU frame not specified");
            }
            driver->setTools(model_number_specified_16, id);
            ImuHardwareInterface *interface = new ImuHardwareInterface(driver, id, topic, frame, name);
            /* Hardware interfaces must be registered at the main RobotHW class.
             * Therefore, a pointer to this class is passed down to the RobotHW classes
             * registering further interfaces */
            interface->setParent(this);
            interfaces_on_port.push_back(interface);
          } else if (model_number_specified == 0xBAFF && interface_type == "Button" && !only_pressure_) {
            // Buttons
            std::string topic;
            if (!dxl_nh.getParam("topic", topic)) {
              ROS_WARN("Button topic not specified");
            }
            int read_rate;
            dxl_nh.param<int>("read_rate", read_rate, 50);
            interfaces_on_port.push_back(new ButtonHardwareInterface(driver, id, topic, read_rate));
          } else if ((model_number_specified == 0xBAFF || model_number_specified == 0xABBA) && interface_type == "LED" && !only_pressure_) {
            // LEDs
            int number_of_LEDs, start_number;
            dxl_nh.param<int>("number_of_LEDs", number_of_LEDs, 3);
            dxl_nh.param<int>("start_number", start_number, 0);
            interfaces_on_port.push_back(new LedsHardwareInterface(driver, id, number_of_LEDs, start_number));
          } else if ((model_number_specified == 311 || model_number_specified == 321 || model_number_specified == 1100)
              && !only_pressure_
              && !only_imu_) {
            // Servos
            // We need to add the tool to the driver for later reading and writing
            driver->setTools(model_number_specified_16, id);
            float mounting_offset;
            dxl_nh.param<float>("mounting_offset", mounting_offset, 0.0);
            float joint_offset;
            dxl_nh.param<float>("joint_offset", joint_offset, 0.0);
            servos_on_port.push_back(std::make_tuple(id, name, mounting_offset, joint_offset));
          } else {
            if (!only_pressure_ && !only_imu_) {
              ROS_WARN("Could not identify device for ID %d", id);
            }
          }
          pinged.push_back(name);
        }
      }
    }
    // create a servo bus interface if there were servos found on this bus
    if (servos_on_port.size() > 0) {
      ServoBusInterface *interface = new ServoBusInterface(driver, servos_on_port);
      interfaces_on_port.push_back(interface);
      servo_interface_.addBusInterface(interface);
    }
    // add vector of interfaces on this port to overall collection of interfaces
    interfaces_.push_back(interfaces_on_port);
  }

  if (pinged.size() != dxl_devices.size()) {
    // when we only have 1 or two devices its only the core
    if (pinged.empty() || pinged.size() == 1  || pinged.size() == 2) {
      ROS_ERROR("Could not start ros control. Power is off!");
      speakError(speak_pub_, "Could not start ros control. Power is off!");
    } else {
      if (first_ping_error_) {
        first_ping_error_ = false;
      } else {
        ROS_ERROR("Could not ping all devices!");
        speakError(speak_pub_, "error starting ros control");
        // check which devices were not pinged successful
        for (std::pair<std::string, int> &device : dxl_devices) {
          if (std::find(pinged.begin(), pinged.end(), device.first) != pinged.end()) {
          } else {
            ROS_ERROR("%s with id %d missing", device.first.c_str(), device.second);
          }
        }
      }
    }
    return false;
  } else {
    speakError(speak_pub_, "ross control startup successful");
    return true;
  }
}

void threaded_init(std::vector<hardware_interface::RobotHW *> &port_interfaces,
                   ros::NodeHandle &root_nh,
                   int &success) {
  success = true;
  for (hardware_interface::RobotHW *interface : port_interfaces) {
    // giving 2 times same node handle to keep interface of base class, dirty
    success &= interface->init(root_nh, root_nh);
  }
}

bool WolfgangHardwareInterface::init(ros::NodeHandle &root_nh) {
  // iterate through all ports
  std::vector<std::thread> threads;
  std::vector<int *> successes;
  int i = 0;
  for (std::vector<hardware_interface::RobotHW *> &port_interfaces : interfaces_) {
    // iterate through all interfaces on this port
    int suc = 0;
    successes.push_back(&suc);
    threads.push_back(std::thread(threaded_init, std::ref(port_interfaces), std::ref(root_nh), std::ref(suc)));
    i++;
  }
  // wait for all reads to finish
  for (std::thread &thread : threads) {
    thread.join();
  }
  // see if all inits were successfull
  bool success = true;
  for (bool s : successes) {
    success &= s;
  }
  // init servo interface last after all servo busses are there
  success &= servo_interface_.init(root_nh, root_nh);
  return success;
}

void threaded_read(std::vector<hardware_interface::RobotHW *> &port_interfaces,
                   const ros::Time &t,
                   const ros::Duration &dt) {
  for (hardware_interface::RobotHW *interface : port_interfaces) {
    // giving 2 times same node handle to keep interface of base class, dirty
    interface->read(t, dt);
  }
}

void WolfgangHardwareInterface::read(const ros::Time &t, const ros::Duration &dt) {
  std::vector<std::thread> threads;
  // start all reads
  for (std::vector<hardware_interface::RobotHW *> &port_interfaces : interfaces_) {
    threads.push_back(std::thread(threaded_read, std::ref(port_interfaces), std::ref(t), std::ref(dt)));
  }
  // wait for all reads to finish
  for (std::thread &thread : threads) {
    thread.join();
  }
  // aggrigate all servo values for controller
  servo_interface_.read(t, dt);
}

void threaded_write(std::vector<hardware_interface::RobotHW *> &port_interfaces,
                    const ros::Time &t,
                    const ros::Duration &dt) {
  for (hardware_interface::RobotHW *interface : port_interfaces) {
    // giving 2 times same node handle to keep interface of base class, dirty
    interface->write(t, dt);
  }
}

void WolfgangHardwareInterface::write(const ros::Time &t, const ros::Duration &dt) {
  // write all controller values to interfaces
  servo_interface_.write(t, dt);

  std::vector<std::thread> threads;
  // start all reads
  for (std::vector<hardware_interface::RobotHW *> &port_interfaces : interfaces_) {
    threads.push_back(std::thread(threaded_write, std::ref(port_interfaces), std::ref(t), std::ref(dt)));
  }

  // wait for all reads to finish
  for (std::thread &thread : threads) {
    thread.join();
  }
}
}
<commit_msg>Update bitbots_ros_control/src/wolfgang_hardware_interface.cpp<commit_after>#include <bitbots_ros_control/wolfgang_hardware_interface.h>

namespace bitbots_ros_control {

/**
 * This class provides a combination of multiple hardware interfaces to construct a complete Wolfgang robot.
 * It is similar to a CombinedRobotHw class as specified in ros_control, but it is changed a bit to make sharing of
 * a common bus driver over multiple hardware interfaces possible.
 */
WolfgangHardwareInterface::WolfgangHardwareInterface(ros::NodeHandle &nh) {
  first_ping_error_ = true;
  speak_pub_ = nh.advertise<humanoid_league_msgs::Audio>("/speak", 1);

  // load parameters
  ROS_INFO_STREAM("Loading parameters from namespace " << nh.getNamespace());
  nh.param<bool>("only_imu", only_imu_, false);
  if (only_imu_) ROS_WARN("Starting in only IMU mode");
  nh.param<bool>("only_pressure", only_pressure_, false);
  if (only_pressure_) ROS_WARN("starting in only pressure sensor mode");

  if (only_pressure_ && only_imu_) {
    ROS_ERROR("only_imu AND only_pressure was set to true");
    exit(1);
  }

  // get list of all bus devices
  XmlRpc::XmlRpcValue dxls_xml;
  nh.getParam("device_info", dxls_xml);
  ROS_ASSERT(dxls_xml.getType() == XmlRpc::XmlRpcValue::TypeStruct);

  // Convert dxls to native type: a vector of tuples with name and id for sorting purposes
  std::vector<std::pair<std::string, int>> dxl_devices;
  for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = dxls_xml.begin(); it != dxls_xml.end(); ++it) {
    std::string name = it->first;
    XmlRpc::XmlRpcValue data = it->second;
    int id = data["id"];
    dxl_devices.emplace_back(name, id);
  }

  // sort the devices by id. This way the devices will always be read and written in ID order later, making debug easier.
  std::sort(dxl_devices.begin(), dxl_devices.end(),
            [](std::pair<std::string, int> &a, std::pair<std::string, int> &b) { return a.second < b.second; });

  // create overall servo interface since we need a single interface for the controllers
  servo_interface_ = DynamixelServoHardwareInterface();
  servo_interface_.setParent(this);
  //todo check and remove if possible
  /* duplicate?
  // set the dynamic reconfigure and load standard params for servo interface
 dynamic_reconfigure::Server<bitbots_ros_control::dynamixel_servo_hardware_interface_paramsConfig> server;
 dynamic_reconfigure::Server<bitbots_ros_control::dynamixel_servo_hardware_interface_paramsConfig>::CallbackType
     f;
 f = boost::bind(&bitbots_ros_control::DynamixelServoHardwareInterface::reconfCallback, servo_interface_, _1, _2);
 server.setCallback(f);*/

  // try to ping all devices on the list, add them to the driver and create corresponding hardware interfaces
  // try until interruption to enable the user to turn on the power
  while (ros::ok()) {
    if (create_interfaces(nh, dxl_devices)) {
      break;
    }
    //sleep(3);
  }
}

//todo this could be done parallel with threads for speed up
bool WolfgangHardwareInterface::create_interfaces(ros::NodeHandle &nh,
                                                  std::vector<std::pair<std::string, int>> dxl_devices) {
  interfaces_ = std::vector<std::vector<hardware_interface::RobotHW *>>();
  // init bus drivers
  std::vector<std::string> pinged;
  XmlRpc::XmlRpcValue port_xml;
  nh.getParam("port_info", port_xml);
  for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = port_xml.begin(); it != port_xml.end(); ++it) {
    // read bus driver specifications from config
    XmlRpc::XmlRpcValue port_data = it->second;
    std::string device_file = port_data["device_file"];
    int baudrate = port_data["baudrate"];
    int protocol_version = port_data["protocol_version"];
    auto driver = std::make_shared<DynamixelDriver>();
    if (!driver->init(device_file.c_str(), uint32_t(baudrate))) {
      ROS_ERROR("Error opening serial port %s", device_file.c_str());
      speakError(speak_pub_, "Error opening serial port");
      sleep(1);
      exit(1);
    }
    // some interface seem to produce some gitter directly after connecting. wait or it will interfere with pings
    // sleep(1);
    driver->setPacketHandler(protocol_version);
    std::vector<hardware_interface::RobotHW *> interfaces_on_port;
    // iterate over all devices and ping them to see what is connected to this bus
    std::vector<std::tuple<int, std::string, float, float>> servos_on_port;
    for (std::pair<std::string, int> &device : dxl_devices) {
      std::string name = device.first;
      int id = device.second;
      if (std::find(pinged.begin(), pinged.end(), device.first) != pinged.end()) {
        // we already found this and dont have to search again
      } else {
        ros::NodeHandle dxl_nh(nh, "device_info/" + name);
        int model_number_specified;
        dxl_nh.getParam("model_number", model_number_specified);
        // some devices provide more than one type of interface, e.g. the IMU provides additionally buttons and LEDs
        std::string interface_type;
        dxl_nh.param<std::string>("interface_type", interface_type, "");
        uint16_t model_number_specified_16 = uint16_t(model_number_specified);
        uint16_t *model_number_returned_16 = new uint16_t;
        if (driver->ping(uint8_t(id), model_number_returned_16)) {
          // check if the specified model number matches the actual model number of the device
          if (model_number_specified_16 != *model_number_returned_16) {
            ROS_WARN("Model number of id %d does not match", id);
          }
          // ping was successful, add device correspondingly
          // only add them if the mode is set correspondingly
          // TODO maybe move more of the parameter stuff in the init of the modules instead of doing everything here
          if (model_number_specified == 0xABBA && interface_type == "CORE") {
            // CORE
            int read_rate;
            dxl_nh.param<int>("read_rate", read_rate, 1);
            driver->setTools(model_number_specified_16, id);
            CoreHardwareInterface *interface = new CoreHardwareInterface(driver, id, read_rate);
            // turn on power, just to be sure
            interface->write(ros::Time::now(), ros::Duration(0));
            interfaces_on_port.push_back(interface);
          } else if (model_number_specified == 0 && !only_imu_) {//model number is currently 0 on foot sensors
            // bitfoot
            std::string topic;
            if (!dxl_nh.getParam("topic", topic)) {
              ROS_WARN("Bitfoot topic not specified");
            }
            BitFootHardwareInterface *interface = new BitFootHardwareInterface(driver, id, topic, name);
            interfaces_on_port.push_back(interface);
          } else if (model_number_specified == 0xBAFF && interface_type == "IMU" && !only_pressure_) {
            //IMU
            std::string topic;
            if (!dxl_nh.getParam("topic", topic)) {
              ROS_WARN("IMU topic not specified");
            }
            std::string frame;
            if (!dxl_nh.getParam("frame", frame)) {
              ROS_WARN("IMU frame not specified");
            }
            driver->setTools(model_number_specified_16, id);
            ImuHardwareInterface *interface = new ImuHardwareInterface(driver, id, topic, frame, name);
            /* Hardware interfaces must be registered at the main RobotHW class.
             * Therefore, a pointer to this class is passed down to the RobotHW classes
             * registering further interfaces */
            interface->setParent(this);
            interfaces_on_port.push_back(interface);
          } else if (model_number_specified == 0xBAFF && interface_type == "Button" && !only_pressure_) {
            // Buttons
            std::string topic;
            if (!dxl_nh.getParam("topic", topic)) {
              ROS_WARN("Button topic not specified");
            }
            int read_rate;
            dxl_nh.param<int>("read_rate", read_rate, 50);
            interfaces_on_port.push_back(new ButtonHardwareInterface(driver, id, topic, read_rate));
          } else if ((model_number_specified == 0xBAFF || model_number_specified == 0xABBA) && interface_type == "LED" && !only_pressure_) {
            // LEDs
            int number_of_LEDs, start_number;
            dxl_nh.param<int>("number_of_LEDs", number_of_LEDs, 3);
            dxl_nh.param<int>("start_number", start_number, 0);
            interfaces_on_port.push_back(new LedsHardwareInterface(driver, id, number_of_LEDs, start_number));
          } else if ((model_number_specified == 311 || model_number_specified == 321 || model_number_specified == 1100)
              && !only_pressure_
              && !only_imu_) {
            // Servos
            // We need to add the tool to the driver for later reading and writing
            driver->setTools(model_number_specified_16, id);
            float mounting_offset;
            dxl_nh.param<float>("mounting_offset", mounting_offset, 0.0);
            float joint_offset;
            dxl_nh.param<float>("joint_offset", joint_offset, 0.0);
            servos_on_port.push_back(std::make_tuple(id, name, mounting_offset, joint_offset));
          } else {
            if (!only_pressure_ && !only_imu_) {
              ROS_WARN("Could not identify device for ID %d", id);
            }
          }
          pinged.push_back(name);
        }
      }
    }
    // create a servo bus interface if there were servos found on this bus
    if (servos_on_port.size() > 0) {
      ServoBusInterface *interface = new ServoBusInterface(driver, servos_on_port);
      interfaces_on_port.push_back(interface);
      servo_interface_.addBusInterface(interface);
    }
    // add vector of interfaces on this port to overall collection of interfaces
    interfaces_.push_back(interfaces_on_port);
  }

  if (pinged.size() != dxl_devices.size()) {
    // when we only have 1 or two devices its only the core
    if (pinged.empty() || pinged.size() == 1  || pinged.size() == 2) {
      ROS_ERROR("Could not start ros control. Power is off!");
      speakError(speak_pub_, "Could not start ros control. Power is off!");
    } else {
      if (first_ping_error_) {
        first_ping_error_ = false;
      } else {
        ROS_ERROR("Could not ping all devices!");
        speakError(speak_pub_, "error starting ros control");
        // check which devices were not pinged successful
        for (std::pair<std::string, int> &device : dxl_devices) {
          if (std::find(pinged.begin(), pinged.end(), device.first) != pinged.end()) {
          } else {
            ROS_ERROR("%s with id %d missing", device.first.c_str(), device.second);
          }
        }
      }
    }
    return false;
  } else {
    speakError(speak_pub_, "ross control startup successful");
    return true;
  }
}

void threaded_init(std::vector<hardware_interface::RobotHW *> &port_interfaces,
                   ros::NodeHandle &root_nh,
                   int &success) {
  success = true;
  for (hardware_interface::RobotHW *interface : port_interfaces) {
    // giving 2 times same node handle to keep interface of base class, dirty
    success &= interface->init(root_nh, root_nh);
  }
}

bool WolfgangHardwareInterface::init(ros::NodeHandle &root_nh) {
  // iterate through all ports
  std::vector<std::thread> threads;
  std::vector<int *> successes;
  int i = 0;
  for (std::vector<hardware_interface::RobotHW *> &port_interfaces : interfaces_) {
    // iterate through all interfaces on this port
    int suc = 0;
    successes.push_back(&suc);
    threads.push_back(std::thread(threaded_init, std::ref(port_interfaces), std::ref(root_nh), std::ref(suc)));
    i++;
  }
  // wait for all inits to finish
  for (std::thread &thread : threads) {
    thread.join();
  }
  // see if all inits were successfull
  bool success = true;
  for (bool s : successes) {
    success &= s;
  }
  // init servo interface last after all servo busses are there
  success &= servo_interface_.init(root_nh, root_nh);
  return success;
}

void threaded_read(std::vector<hardware_interface::RobotHW *> &port_interfaces,
                   const ros::Time &t,
                   const ros::Duration &dt) {
  for (hardware_interface::RobotHW *interface : port_interfaces) {
    // giving 2 times same node handle to keep interface of base class, dirty
    interface->read(t, dt);
  }
}

void WolfgangHardwareInterface::read(const ros::Time &t, const ros::Duration &dt) {
  std::vector<std::thread> threads;
  // start all reads
  for (std::vector<hardware_interface::RobotHW *> &port_interfaces : interfaces_) {
    threads.push_back(std::thread(threaded_read, std::ref(port_interfaces), std::ref(t), std::ref(dt)));
  }
  // wait for all reads to finish
  for (std::thread &thread : threads) {
    thread.join();
  }
  // aggrigate all servo values for controller
  servo_interface_.read(t, dt);
}

void threaded_write(std::vector<hardware_interface::RobotHW *> &port_interfaces,
                    const ros::Time &t,
                    const ros::Duration &dt) {
  for (hardware_interface::RobotHW *interface : port_interfaces) {
    // giving 2 times same node handle to keep interface of base class, dirty
    interface->write(t, dt);
  }
}

void WolfgangHardwareInterface::write(const ros::Time &t, const ros::Duration &dt) {
  // write all controller values to interfaces
  servo_interface_.write(t, dt);

  std::vector<std::thread> threads;
  // start all reads
  for (std::vector<hardware_interface::RobotHW *> &port_interfaces : interfaces_) {
    threads.push_back(std::thread(threaded_write, std::ref(port_interfaces), std::ref(t), std::ref(dt)));
  }

  // wait for all reads to finish
  for (std::thread &thread : threads) {
    thread.join();
  }
}
}
<|endoftext|>


<commit_before>#include <dirent.h>
#include "../InternalTypes.h"

void DirectoryType::__constructor__() {
	auto file_path = Program::argument<TextInstance>();
	auto self = Program::create<DirectoryInstance>();

	self->file_path = file_path->text();
	Program::push(self);
}

void DirectoryType::contents() {
	auto self = Program::argument<DirectoryInstance>();

    auto contents = Program::create<ListInstance>();
    auto directory = opendir(self->file_path.c_str());

    if (directory == nullptr) {
        throw ExceptionInstance(Program::create<TextInstance>("Cannot read directory contents"));
    }

    while(auto entry = readdir(directory)) {
        contents->val.push_back(Program::intern(new DirectoryEntryInstance(std::string(entry->d_name), entry->d_type)));
    }

    Program::push(contents);
}

std::string DirectoryInstance::text() {
	return "Directory(" + this->file_path + ")";
}

bool DirectoryInstance::boolean() {
	return true;
}

<commit_msg>Explictly intern ExceptionInstance thrown by Directory.contents()<commit_after>#include <dirent.h>
#include "../InternalTypes.h"

void DirectoryType::__constructor__() {
	auto file_path = Program::argument<TextInstance>();
	auto self = Program::create<DirectoryInstance>();

	self->file_path = file_path->text();
	Program::push(self);
}

void DirectoryType::contents() {
	auto self = Program::argument<DirectoryInstance>();

    auto contents = Program::create<ListInstance>();
    auto directory = opendir(self->file_path.c_str());

    if (directory == nullptr) {
        throw Program::create<ExceptionInstance>(Program::create<TextInstance>("Cannot read directory contents"));
    }

    while(auto entry = readdir(directory)) {
        contents->val.push_back(Program::intern(new DirectoryEntryInstance(std::string(entry->d_name), entry->d_type)));
    }

    Program::push(contents);
}

std::string DirectoryInstance::text() {
	return "Directory(" + this->file_path + ")";
}

bool DirectoryInstance::boolean() {
	return true;
}

<|endoftext|>


<commit_before>// Copyright eeGeo Ltd (2012-2015), All Rights Reserved

#include "PoiRingController.h"
#include "InteriorsFloorModel.h"
#include "InteriorsModel.h"
#include "InteriorId.h"
#include "TtyHandler.h"
#include "CameraHelpers.h"
#include "RenderCamera.h"
#include "IRayPicker.h"
#include "PoiRingView.h"
#include "RenderContext.h"
#include "MathFunc.h"
#include "MathsHelpers.h"
#include "IMyPinCreationInitiationViewModel.h"
#include "IMyPinCreationModel.h"
#include "EarthConstants.h"
#include "TerrainHeightProvider.h"
#include "TransformHelpers.h"
#include "VectorMath.h"
#include "InteriorController.h"

#include "InteriorHeightHelpers.h"
#include "ScreenProperties.h"

namespace ExampleApp
{
    namespace MyPinCreation
    {
        namespace PoiRing
        {
            namespace SdkModel
            {
                namespace
                {

                    float CalculateAltitudeBasedSphereOuterScale(float altitude)
                    {
                        const float minAltitude = 10.f;
                        const float maxAltitude = 1500.f;
                        const float lowAltitudeScale = 0.05f;
                        const float highAltitudeScale = 1.0f;
                        float t = Eegeo::Math::Clamp01((altitude - minAltitude)/(maxAltitude-minAltitude));
                        return Eegeo::Math::Lerp(lowAltitudeScale, highAltitudeScale, t);
                    }

                    float CalculateAltitudeBasedSphereScale(float altitude, float outerRingRadiusInMeters)
                    {
                        const float minAltitude = 10.f;
                        const float maxAltitude = 18000.f;
                        const float lowAltitudeSphereScale = outerRingRadiusInMeters - 0.5f;
                        const float highAltitudeSphereScale = outerRingRadiusInMeters - 100.f;
                        return lowAltitudeSphereScale + (((altitude - minAltitude)/maxAltitude) * (highAltitudeSphereScale - lowAltitudeSphereScale));
                    }

                    bool RingIsOnScreen(const Eegeo::Camera::RenderCamera& renderCamera, const Eegeo::dv3& position, float radius)
                    {
                        const Eegeo::Geometry::Frustum& frustum = renderCamera.GetFrustum();
                        const Eegeo::dv3 cameraRelativePosition = position - renderCamera.GetEcefLocation();

                        for (int i = 0; i < Eegeo::Geometry::Frustum::PLANES_COUNT; ++i)
                        {
                            const Eegeo::Geometry::Plane& p = frustum.planes[i];
                            double signedDist = p.a * cameraRelativePosition.GetX() + p.b * cameraRelativePosition.GetY() + p.c * cameraRelativePosition.GetZ() + p.d;

                            if (signedDist < -radius)
                            {
                                return false;
                            }
                        }

                        return true;
                    }
                }

                PoiRingController::PoiRingController(MyPinCreation::SdkModel::IMyPinCreationModel& myPinCreationModel,
                                                     PoiRingView& poiRingView,
                                                     Eegeo::Rendering::EnvironmentFlatteningService& environmentFlatteningService,
                                                     Eegeo::Resources::Terrain::Heights::TerrainHeightProvider& terrainHeightProvider,
                                                     Eegeo::Resources::Interiors::InteriorController& interiorController,
                                                     Eegeo::Rendering::ScreenProperties& screenProperties)
                    : m_myPinCreationModel(myPinCreationModel)
                    , m_poiRingView(poiRingView)
                    , m_scaleInterpolationParam(0.f)
                    , m_easeDurationInSeconds(1.2f)
                    , m_environmentFlatteningService(environmentFlatteningService)
                    , m_terrainHeightProvider(terrainHeightProvider)
                    , m_iconPosition(Eegeo::dv3::Zero())
                    , m_iconSize(0.0f)
                    , m_ringRadius(0.0f)
                    , m_interiorController(interiorController)
                    , m_screenProperties(screenProperties)
                {

                }

                void PoiRingController::Update(float dt, const Eegeo::Camera::RenderCamera& renderCamera, const Eegeo::dv3& cameraEcefInterestPoint)
                {
                    const bool showingInterior = m_interiorController.InteriorIsVisible();
                    float interiorDownScale = showingInterior ? 0.5f : 1.0f;
                    const float altitude = (float)(renderCamera.GetAltitude() - (m_myPinCreationModel.GetPosition().Length() - Eegeo::Space::EarthConstants::Radius));
                    const float outerRingRadiusInMeters = 120.f * interiorDownScale;
                    const float altitudeScale = CalculateAltitudeBasedSphereOuterScale(altitude);
                    const float transitionScale = CalculateTransitionScale(dt);
                    m_ringRadius = outerRingRadiusInMeters * altitudeScale * transitionScale;

                    const bool ringIsOnScreen = RingIsOnScreen(renderCamera, m_myPinCreationModel.GetPosition(), m_ringRadius);

                    m_poiRingView.SetShouldRenderRing(m_scaleInterpolationParam > 0.f && ringIsOnScreen);

                    if (m_myPinCreationModel.GetCreationStage() == Inactive && !ringIsOnScreen)
                    {
                        m_scaleInterpolationParam = 0.f;
                    }

                    if (m_scaleInterpolationParam < 0.01f && m_myPinCreationModel.GetCreationStage() != Details)
                    {
                        m_myPinCreationModel.SetPosition(cameraEcefInterestPoint);
                    }

                    if(m_myPinCreationModel.NeedsTerrainHeight())
                    {
                        float terrainHeight;
                        if(m_terrainHeightProvider.TryGetHeight(m_myPinCreationModel.GetPosition(), 11, terrainHeight))
                        {
                            m_myPinCreationModel.SetTerrainHeight(terrainHeight);
                        }
                    }

                    if(m_myPinCreationModel.GetCreationStage() == Ring)
                    {
                        m_myPinCreationModel.SetInterior(showingInterior);
                        if(showingInterior)
                        {
                            const Eegeo::Resources::Interiors::InteriorsModel *pModel = NULL;
                            bool success = m_interiorController.TryGetCurrentModel(pModel);
                            if(success)
                            {
                                const Eegeo::Resources::Interiors::InteriorId& buildingId = pModel->GetId();
                                m_myPinCreationModel.SetBuildingId(buildingId);
                            }

                            m_myPinCreationModel.SetFloor(m_interiorController.GetCurrentFloorIndex());
                            float floorHeightAboveSeaLevel = Helpers::InteriorHeightHelpers::GetFloorHeightAboveSeaLevel(*pModel, m_interiorController.GetCurrentFloorIndex());
                            const float floorHeightAboveTerrain = floorHeightAboveSeaLevel - m_myPinCreationModel.GetTerrainHeight();
                            m_myPinCreationModel.SetHeightAboveTerrain(floorHeightAboveTerrain);
                        }
                    }

                    Eegeo::m44 sphereTransformMatrix;
                    sphereTransformMatrix.Scale(m_ringRadius);

                    Eegeo::dv3 scaledPoint = Eegeo::Rendering::EnvironmentFlatteningService::GetScaledPointEcef(m_myPinCreationModel.GetPosition(), m_environmentFlatteningService.GetCurrentScale());

                    Eegeo::dv3 cameraRelativePosition = scaledPoint - renderCamera.GetEcefLocation();
                    sphereTransformMatrix.SetRow(3, Eegeo::v4(cameraRelativePosition.ToSingle(), 1.f));

                    m_poiRingView.SetRingTransforms(sphereTransformMatrix);

                    float altitudeBasedScale = CalculateAltitudeBasedSphereScale(altitude, m_ringRadius);
                    m_poiRingView.SetInnerSphereScale(altitudeBasedScale);

                    Eegeo::dv3 unflattenedIconPosition = m_myPinCreationModel.GetPosition();
                    Eegeo::dv3 iconPosition = Eegeo::Rendering::EnvironmentFlatteningService::GetScaledPointEcef(
                                                  unflattenedIconPosition,
                                                  m_environmentFlatteningService.GetCurrentScale());

                    const float assetSize = 75.f;
                    const float iconScale = Eegeo::Helpers::TransformHelpers::ComputeModelScaleForConstantScreenSizeWithVerticalFoV(renderCamera, iconPosition) / (m_screenProperties.GetScreenHeight()* 0.5f)*m_screenProperties.GetPixelScale() * assetSize;

                    m_iconSize = Eegeo::Max(iconScale * transitionScale, 0.0f);
                    m_poiRingView.AddIconSprite(renderCamera, iconPosition, m_iconSize);

                    m_iconPosition = iconPosition + (Eegeo::dv3)renderCamera.GetModelMatrix().GetRow(1) * m_iconSize * 0.5f;
                }

                float PoiRingController::CalculateTransitionScale(float dt)
                {
                    float delta = m_myPinCreationModel.GetCreationStage() == Ring ? dt : -dt;
                    delta /= m_easeDurationInSeconds;
                    m_scaleInterpolationParam = Eegeo::Clamp(m_scaleInterpolationParam + delta, 0.f, 1.f);
                    return Eegeo::Helpers::MathsHelpers::PennerElasticEaseInOut(0.f, 1.f, m_scaleInterpolationParam);
                }

                void PoiRingController::GetIconPositionAndSize(Eegeo::dv3& out_positionEcef, float& out_sizeMeters) const
                {
                    out_positionEcef = m_iconPosition;
                    out_sizeMeters = m_iconSize;
                }

                void PoiRingController::GetSpherePositionAndRadius(Eegeo::dv3& out_sphereCenterEcef, float& out_sphereRadius) const
                {
                    out_sphereCenterEcef = m_myPinCreationModel.GetPosition();
                    out_sphereRadius = m_ringRadius;
                }
            }
        }
    }
}
<commit_msg>Fix for MPLY-6048.  Pin creation should no longer associated pins being in the last interior entered while in exterior mode. Buddy: Ian H<commit_after>// Copyright eeGeo Ltd (2012-2015), All Rights Reserved

#include "PoiRingController.h"
#include "InteriorsFloorModel.h"
#include "InteriorsModel.h"
#include "InteriorId.h"
#include "TtyHandler.h"
#include "CameraHelpers.h"
#include "RenderCamera.h"
#include "IRayPicker.h"
#include "PoiRingView.h"
#include "RenderContext.h"
#include "MathFunc.h"
#include "MathsHelpers.h"
#include "IMyPinCreationInitiationViewModel.h"
#include "IMyPinCreationModel.h"
#include "EarthConstants.h"
#include "TerrainHeightProvider.h"
#include "TransformHelpers.h"
#include "VectorMath.h"
#include "InteriorController.h"

#include "InteriorHeightHelpers.h"
#include "ScreenProperties.h"

namespace ExampleApp
{
    namespace MyPinCreation
    {
        namespace PoiRing
        {
            namespace SdkModel
            {
                namespace
                {

                    float CalculateAltitudeBasedSphereOuterScale(float altitude)
                    {
                        const float minAltitude = 10.f;
                        const float maxAltitude = 1500.f;
                        const float lowAltitudeScale = 0.05f;
                        const float highAltitudeScale = 1.0f;
                        float t = Eegeo::Math::Clamp01((altitude - minAltitude)/(maxAltitude-minAltitude));
                        return Eegeo::Math::Lerp(lowAltitudeScale, highAltitudeScale, t);
                    }

                    float CalculateAltitudeBasedSphereScale(float altitude, float outerRingRadiusInMeters)
                    {
                        const float minAltitude = 10.f;
                        const float maxAltitude = 18000.f;
                        const float lowAltitudeSphereScale = outerRingRadiusInMeters - 0.5f;
                        const float highAltitudeSphereScale = outerRingRadiusInMeters - 100.f;
                        return lowAltitudeSphereScale + (((altitude - minAltitude)/maxAltitude) * (highAltitudeSphereScale - lowAltitudeSphereScale));
                    }

                    bool RingIsOnScreen(const Eegeo::Camera::RenderCamera& renderCamera, const Eegeo::dv3& position, float radius)
                    {
                        const Eegeo::Geometry::Frustum& frustum = renderCamera.GetFrustum();
                        const Eegeo::dv3 cameraRelativePosition = position - renderCamera.GetEcefLocation();

                        for (int i = 0; i < Eegeo::Geometry::Frustum::PLANES_COUNT; ++i)
                        {
                            const Eegeo::Geometry::Plane& p = frustum.planes[i];
                            double signedDist = p.a * cameraRelativePosition.GetX() + p.b * cameraRelativePosition.GetY() + p.c * cameraRelativePosition.GetZ() + p.d;

                            if (signedDist < -radius)
                            {
                                return false;
                            }
                        }

                        return true;
                    }
                }

                PoiRingController::PoiRingController(MyPinCreation::SdkModel::IMyPinCreationModel& myPinCreationModel,
                                                     PoiRingView& poiRingView,
                                                     Eegeo::Rendering::EnvironmentFlatteningService& environmentFlatteningService,
                                                     Eegeo::Resources::Terrain::Heights::TerrainHeightProvider& terrainHeightProvider,
                                                     Eegeo::Resources::Interiors::InteriorController& interiorController,
                                                     Eegeo::Rendering::ScreenProperties& screenProperties)
                    : m_myPinCreationModel(myPinCreationModel)
                    , m_poiRingView(poiRingView)
                    , m_scaleInterpolationParam(0.f)
                    , m_easeDurationInSeconds(1.2f)
                    , m_environmentFlatteningService(environmentFlatteningService)
                    , m_terrainHeightProvider(terrainHeightProvider)
                    , m_iconPosition(Eegeo::dv3::Zero())
                    , m_iconSize(0.0f)
                    , m_ringRadius(0.0f)
                    , m_interiorController(interiorController)
                    , m_screenProperties(screenProperties)
                {

                }

                void PoiRingController::Update(float dt, const Eegeo::Camera::RenderCamera& renderCamera, const Eegeo::dv3& cameraEcefInterestPoint)
                {
                    const bool showingInterior = m_interiorController.InteriorIsVisible();
                    float interiorDownScale = showingInterior ? 0.5f : 1.0f;
                    const float altitude = (float)(renderCamera.GetAltitude() - (m_myPinCreationModel.GetPosition().Length() - Eegeo::Space::EarthConstants::Radius));
                    const float outerRingRadiusInMeters = 120.f * interiorDownScale;
                    const float altitudeScale = CalculateAltitudeBasedSphereOuterScale(altitude);
                    const float transitionScale = CalculateTransitionScale(dt);
                    m_ringRadius = outerRingRadiusInMeters * altitudeScale * transitionScale;

                    const bool ringIsOnScreen = RingIsOnScreen(renderCamera, m_myPinCreationModel.GetPosition(), m_ringRadius);

                    m_poiRingView.SetShouldRenderRing(m_scaleInterpolationParam > 0.f && ringIsOnScreen);

                    if (m_myPinCreationModel.GetCreationStage() == Inactive && !ringIsOnScreen)
                    {
                        m_scaleInterpolationParam = 0.f;
                    }

                    if (m_scaleInterpolationParam < 0.01f && m_myPinCreationModel.GetCreationStage() != Details)
                    {
                        m_myPinCreationModel.SetPosition(cameraEcefInterestPoint);
                    }

                    if(m_myPinCreationModel.NeedsTerrainHeight())
                    {
                        float terrainHeight;
                        if(m_terrainHeightProvider.TryGetHeight(m_myPinCreationModel.GetPosition(), 11, terrainHeight))
                        {
                            m_myPinCreationModel.SetTerrainHeight(terrainHeight);
                        }
                    }

                    if(m_myPinCreationModel.GetCreationStage() == Ring)
                    {
                        m_myPinCreationModel.SetInterior(showingInterior);
                        if(showingInterior)
                        {
                            const Eegeo::Resources::Interiors::InteriorsModel *pModel = NULL;
                            bool success = m_interiorController.TryGetCurrentModel(pModel);
                            if(success)
                            {
                                const Eegeo::Resources::Interiors::InteriorId& buildingId = pModel->GetId();
                                m_myPinCreationModel.SetBuildingId(buildingId);
                            }

                            m_myPinCreationModel.SetFloor(m_interiorController.GetCurrentFloorIndex());
                            float floorHeightAboveSeaLevel = Helpers::InteriorHeightHelpers::GetFloorHeightAboveSeaLevel(*pModel, m_interiorController.GetCurrentFloorIndex());
                            const float floorHeightAboveTerrain = floorHeightAboveSeaLevel - m_myPinCreationModel.GetTerrainHeight();
                            m_myPinCreationModel.SetHeightAboveTerrain(floorHeightAboveTerrain);
                        }
                        else
                        {
                            m_myPinCreationModel.SetBuildingId(Eegeo::Resources::Interiors::InteriorId::NullId());
                            m_myPinCreationModel.SetFloor(0);
                        }
                    }

                    Eegeo::m44 sphereTransformMatrix;
                    sphereTransformMatrix.Scale(m_ringRadius);

                    Eegeo::dv3 scaledPoint = Eegeo::Rendering::EnvironmentFlatteningService::GetScaledPointEcef(m_myPinCreationModel.GetPosition(), m_environmentFlatteningService.GetCurrentScale());

                    Eegeo::dv3 cameraRelativePosition = scaledPoint - renderCamera.GetEcefLocation();
                    sphereTransformMatrix.SetRow(3, Eegeo::v4(cameraRelativePosition.ToSingle(), 1.f));

                    m_poiRingView.SetRingTransforms(sphereTransformMatrix);

                    float altitudeBasedScale = CalculateAltitudeBasedSphereScale(altitude, m_ringRadius);
                    m_poiRingView.SetInnerSphereScale(altitudeBasedScale);

                    Eegeo::dv3 unflattenedIconPosition = m_myPinCreationModel.GetPosition();
                    Eegeo::dv3 iconPosition = Eegeo::Rendering::EnvironmentFlatteningService::GetScaledPointEcef(
                                                  unflattenedIconPosition,
                                                  m_environmentFlatteningService.GetCurrentScale());

                    const float assetSize = 75.f;
                    const float iconScale = Eegeo::Helpers::TransformHelpers::ComputeModelScaleForConstantScreenSizeWithVerticalFoV(renderCamera, iconPosition) / (m_screenProperties.GetScreenHeight()* 0.5f)*m_screenProperties.GetPixelScale() * assetSize;

                    m_iconSize = Eegeo::Max(iconScale * transitionScale, 0.0f);
                    m_poiRingView.AddIconSprite(renderCamera, iconPosition, m_iconSize);

                    m_iconPosition = iconPosition + (Eegeo::dv3)renderCamera.GetModelMatrix().GetRow(1) * m_iconSize * 0.5f;
                }

                float PoiRingController::CalculateTransitionScale(float dt)
                {
                    float delta = m_myPinCreationModel.GetCreationStage() == Ring ? dt : -dt;
                    delta /= m_easeDurationInSeconds;
                    m_scaleInterpolationParam = Eegeo::Clamp(m_scaleInterpolationParam + delta, 0.f, 1.f);
                    return Eegeo::Helpers::MathsHelpers::PennerElasticEaseInOut(0.f, 1.f, m_scaleInterpolationParam);
                }

                void PoiRingController::GetIconPositionAndSize(Eegeo::dv3& out_positionEcef, float& out_sizeMeters) const
                {
                    out_positionEcef = m_iconPosition;
                    out_sizeMeters = m_iconSize;
                }

                void PoiRingController::GetSpherePositionAndRadius(Eegeo::dv3& out_sphereCenterEcef, float& out_sphereRadius) const
                {
                    out_sphereCenterEcef = m_myPinCreationModel.GetPosition();
                    out_sphereRadius = m_ringRadius;
                }
            }
        }
    }
}
<|endoftext|>


<commit_before>#include "Pipe.h"

const int Pipe::pipe_size_middle = (PIPE_SIZE / 2);
const int Pipe::pipe_size_middle_start = Pipe::pipe_size_middle - 1;

Pipe::Pipe(SDL_Surface *sprite_param, SDL_Surface *alt_sprite_param, bool top_param, bool right_param, bool down_param, bool left_param)
{
    init(sprite_param, alt_sprite_param, top_param, right_param, down_param, left_param);
}

Pipe::Pipe(SDL_Surface *sprite_param, SDL_Surface *alt_sprite_param)
{
    bool t = rand() & 0x1;
    bool r = rand() & 0x1;
    bool d = rand() & 0x1;
    bool l = rand() & 0x1;
    init(sprite_param, alt_sprite_param, t, r, d, l);
}

void Pipe::init(SDL_Surface *sprite_param, SDL_Surface *alt_sprite_param, bool top_param, bool right_param, bool down_param, bool left_param)
{
    sprite = sprite_param;
    alt_sprite = alt_sprite_param;

    top = top_param;
    right = right_param;
    down = down_param;
    left = left_param;

    sprite_position.w = sprite_position.h = PIPE_SIZE;

    flow = false;
    flowed_pixels = time = 0;

    // Sets up the sprites position based on the pipe openings
    if(top == true && right == true && down == true && left == true) {
        sprite_position.x = 0;
        sprite_position.y = 0;
    } else if(top == true && right == true && down == true) {
        sprite_position.x = PIPE_SIZE * 4;
        sprite_position.y = 0;
    } else if(top == true && right == true && left == true) {
        sprite_position.x = PIPE_SIZE * 3;
        sprite_position.y = 0;
    } else if(top == true && down == true && left == true) {
        sprite_position.x = PIPE_SIZE * 2;
        sprite_position.y = 0;
    } else if(right == true && down == true && left == true) {
        sprite_position.x = PIPE_SIZE;
        sprite_position.y = 0;
    } else if(top == true && right == true) {
        sprite_position.x = PIPE_SIZE;
        sprite_position.y = PIPE_SIZE;
    } else if(top == true && down == true) {
        sprite_position.x = PIPE_SIZE * 3;
        sprite_position.y = PIPE_SIZE;
    } else if(top == true && left == true) {
        sprite_position.x = PIPE_SIZE * 2;
        sprite_position.y = PIPE_SIZE;
    } else if(right == true && down == true) {
        sprite_position.x = PIPE_SIZE * 5;
        sprite_position.y = 0;
    } else if(right == true && left == true) {
        sprite_position.x = PIPE_SIZE * 4;
        sprite_position.y = PIPE_SIZE;
    } else if(down == true && left == true) {
        sprite_position.x = 0;
        sprite_position.y = PIPE_SIZE;
    } else if(top == true) {
        sprite_position.x = PIPE_SIZE * 2;
        sprite_position.y = PIPE_SIZE * 2;
    } else if(right == true) {
        sprite_position.x = PIPE_SIZE * 5;
        sprite_position.y = PIPE_SIZE;
    } else if(down == true) {
        sprite_position.x = PIPE_SIZE;
        sprite_position.y = PIPE_SIZE * 2;
    } else if(left == true) {
        sprite_position.x = 0;
        sprite_position.y = PIPE_SIZE * 2;
    } else {
        sprite_position.x = PIPE_SIZE * 5;
        sprite_position.y = PIPE_SIZE * 2;
    }
}

void Pipe::Draw(SDL_Surface* surface, SDL_Rect* position) {
    // draws the pipe
    SDL_BlitSurface(alt_sprite, &sprite_position, surface, position);

    unsigned int rgb = SDL_MapRGB(surface->format, 255, 0, 0);

    // first half flow
    SDL_Rect rect = FirstFlowRect(position, flow_start_position);
    SDL_FillRect(surface, &rect, rgb);

    // second half flow
    if(flowed_pixels >= PIPE_SIZE / 2) {
        rect = LastFlowRect(position, flow_turn_position);
        SDL_FillRect(surface, &rect, rgb);
    }
}

void Pipe::Update() {
  if(flowed_pixels < PIPE_SIZE && flow == true) {
    int current_time = SDL_GetTicks();

    if(current_time > time + FLOW_SPEED) {
      flowed_pixels += 2;
      time = current_time;
    }
  }
}

bool Pipe::isBlocked (void)
{
    // if it has flow then its blocked
    return flow;
}

bool Pipe::hasFlowEntry(int entry) {
    return (entry == FLOW_TOP && top) ||
           (entry == FLOW_RIGHT && right) ||
           (entry == FLOW_DOWN && down) ||
           (entry == FLOW_LEFT && left);
}

void Pipe::StartFlow(int start_position) {
    if(flow == false) {
        flow = true;
        flow_start_position = start_position;
        flow_turn_position = 0;
        time = SDL_GetTicks();
    }
}

SDL_Rect Pipe::FirstFlowRect(SDL_Rect* position, unsigned int flow_start) {
    SDL_Rect rect;

    // makes it go only halfway
    int max_flowed_pixels = flowed_pixels <= Pipe::pipe_size_middle ? flowed_pixels : Pipe::pipe_size_middle;

    if(flow_start == FLOW_TOP) {
        rect.x = position->x + Pipe::pipe_size_middle_start;
        rect.y = position->y;
        rect.w = FLOW_LENGTH;
        rect.h = max_flowed_pixels;
    } else if(flow_start == FLOW_RIGHT) {
        rect.x = (position->x + PIPE_SIZE) - max_flowed_pixels;
        rect.y = position->y + Pipe::pipe_size_middle_start;
        rect.w = PIPE_SIZE - (PIPE_SIZE - max_flowed_pixels);
        rect.h = FLOW_LENGTH;
    } else if(flow_start == FLOW_DOWN) {
        rect.x = position->x + Pipe::pipe_size_middle_start;
        rect.y = (position->y + PIPE_SIZE) - max_flowed_pixels;
        rect.w = FLOW_LENGTH;
        rect.h = PIPE_SIZE - (PIPE_SIZE - max_flowed_pixels);
    } else if(flow_start == FLOW_LEFT) {
        rect.x = position->x;
        rect.y = position->y + Pipe::pipe_size_middle_start;
        rect.w = max_flowed_pixels;
        rect.h = FLOW_LENGTH;
    }

    return rect;
}

SDL_Rect Pipe::LastFlowRect(SDL_Rect* position, unsigned int flow_end) {
    SDL_Rect rect;

    // makes it go only halfway
    int max_flowed_pixels = flowed_pixels - Pipe::pipe_size_middle;

    if(flow_end == FLOW_TOP) {
        rect.x = position->x + Pipe::pipe_size_middle_start;
        rect.y = (position->y + pipe_size_middle) - max_flowed_pixels;
        rect.w = FLOW_LENGTH;
        rect.h = PIPE_SIZE - (PIPE_SIZE - max_flowed_pixels);
    } else if(flow_end == FLOW_RIGHT) {
        rect.x = position->x + Pipe::pipe_size_middle;
        rect.y = position->y + Pipe::pipe_size_middle_start;
        rect.w = max_flowed_pixels;
        rect.h = FLOW_LENGTH;
    } else if(flow_end == FLOW_DOWN) {
        rect.x = position->x + Pipe::pipe_size_middle_start;
        rect.y = position->y + pipe_size_middle;
        rect.w = FLOW_LENGTH;
        rect.h = max_flowed_pixels;
    } else if(flow_end == FLOW_LEFT) {
        rect.x = (position->x + pipe_size_middle) - max_flowed_pixels;
        rect.y = position->y + Pipe::pipe_size_middle_start;
        rect.w = pipe_size_middle - (pipe_size_middle - max_flowed_pixels);
        rect.h = FLOW_LENGTH;
    }

    return rect;
}

bool Pipe::isFlowFinished() {
    return flowed_pixels == PIPE_SIZE;
}

int Pipe::getFlowStartPosition() {
    return flow_start_position;
}

int Pipe::getFlowTurnPosition() {
    return flow_turn_position;
}

void Pipe::setFlowTurnPosition(int direction) {
    flow_turn_position = direction;
}
<commit_msg>Debug flow path algorithm only allowing the creation of pipes with 2 connections (1 in 1 out).<commit_after>#include "Pipe.h"

const int Pipe::pipe_size_middle = (PIPE_SIZE / 2);
const int Pipe::pipe_size_middle_start = Pipe::pipe_size_middle - 1;

Pipe::Pipe(SDL_Surface *sprite_param, SDL_Surface *alt_sprite_param, bool top_param, bool right_param, bool down_param, bool left_param)
{
    init(sprite_param, alt_sprite_param, top_param, right_param, down_param, left_param);
}

Pipe::Pipe(SDL_Surface *sprite_param, SDL_Surface *alt_sprite_param)
{
    int sum;

    bool t = rand() & 0x1;
    bool r = rand() & 0x1;
    bool d = rand() & 0x1;
    bool l = rand() & 0x1;

    do {
        t = rand() & 0x1;
        r = rand() & 0x1;
        d = rand() & 0x1;
        l = rand() & 0x1;

        sum = t + r + d + l;
    } while (sum != 2);

    init(sprite_param, alt_sprite_param, t, r, d, l);
}

void Pipe::init(SDL_Surface *sprite_param, SDL_Surface *alt_sprite_param, bool top_param, bool right_param, bool down_param, bool left_param)
{
    sprite = sprite_param;
    alt_sprite = alt_sprite_param;

    top = top_param;
    right = right_param;
    down = down_param;
    left = left_param;

    sprite_position.w = sprite_position.h = PIPE_SIZE;

    flow = false;
    flowed_pixels = time = 0;

    // Sets up the sprites position based on the pipe openings
    if(top == true && right == true && down == true && left == true) {
        sprite_position.x = 0;
        sprite_position.y = 0;
    } else if(top == true && right == true && down == true) {
        sprite_position.x = PIPE_SIZE * 4;
        sprite_position.y = 0;
    } else if(top == true && right == true && left == true) {
        sprite_position.x = PIPE_SIZE * 3;
        sprite_position.y = 0;
    } else if(top == true && down == true && left == true) {
        sprite_position.x = PIPE_SIZE * 2;
        sprite_position.y = 0;
    } else if(right == true && down == true && left == true) {
        sprite_position.x = PIPE_SIZE;
        sprite_position.y = 0;
    } else if(top == true && right == true) {
        sprite_position.x = PIPE_SIZE;
        sprite_position.y = PIPE_SIZE;
    } else if(top == true && down == true) {
        sprite_position.x = PIPE_SIZE * 3;
        sprite_position.y = PIPE_SIZE;
    } else if(top == true && left == true) {
        sprite_position.x = PIPE_SIZE * 2;
        sprite_position.y = PIPE_SIZE;
    } else if(right == true && down == true) {
        sprite_position.x = PIPE_SIZE * 5;
        sprite_position.y = 0;
    } else if(right == true && left == true) {
        sprite_position.x = PIPE_SIZE * 4;
        sprite_position.y = PIPE_SIZE;
    } else if(down == true && left == true) {
        sprite_position.x = 0;
        sprite_position.y = PIPE_SIZE;
    } else if(top == true) {
        sprite_position.x = PIPE_SIZE * 2;
        sprite_position.y = PIPE_SIZE * 2;
    } else if(right == true) {
        sprite_position.x = PIPE_SIZE * 5;
        sprite_position.y = PIPE_SIZE;
    } else if(down == true) {
        sprite_position.x = PIPE_SIZE;
        sprite_position.y = PIPE_SIZE * 2;
    } else if(left == true) {
        sprite_position.x = 0;
        sprite_position.y = PIPE_SIZE * 2;
    } else {
        sprite_position.x = PIPE_SIZE * 5;
        sprite_position.y = PIPE_SIZE * 2;
    }
}

void Pipe::Draw(SDL_Surface* surface, SDL_Rect* position) {
    // draws the pipe
    SDL_BlitSurface(alt_sprite, &sprite_position, surface, position);

    unsigned int rgb = SDL_MapRGB(surface->format, 255, 0, 0);

    // first half flow
    SDL_Rect rect = FirstFlowRect(position, flow_start_position);
    SDL_FillRect(surface, &rect, rgb);

    // second half flow
    if(flowed_pixels >= PIPE_SIZE / 2) {
        rect = LastFlowRect(position, flow_turn_position);
        SDL_FillRect(surface, &rect, rgb);
    }
}

void Pipe::Update() {
  if(flowed_pixels < PIPE_SIZE && flow == true) {
    int current_time = SDL_GetTicks();

    if(current_time > time + FLOW_SPEED) {
      flowed_pixels += 2;
      time = current_time;
    }
  }
}

bool Pipe::isBlocked (void)
{
    // if it has flow then its blocked
    return flow;
}

bool Pipe::hasFlowEntry(int entry) {
    return (entry == FLOW_TOP && top) ||
           (entry == FLOW_RIGHT && right) ||
           (entry == FLOW_DOWN && down) ||
           (entry == FLOW_LEFT && left);
}

void Pipe::StartFlow(int start_position) {
    if(flow == false) {
        flow = true;
        flow_start_position = start_position;
        flow_turn_position = 0;
        time = SDL_GetTicks();
    }
}

SDL_Rect Pipe::FirstFlowRect(SDL_Rect* position, unsigned int flow_start) {
    SDL_Rect rect;

    // makes it go only halfway
    int max_flowed_pixels = flowed_pixels <= Pipe::pipe_size_middle ? flowed_pixels : Pipe::pipe_size_middle;

    if(flow_start == FLOW_TOP) {
        rect.x = position->x + Pipe::pipe_size_middle_start;
        rect.y = position->y;
        rect.w = FLOW_LENGTH;
        rect.h = max_flowed_pixels;
    } else if(flow_start == FLOW_RIGHT) {
        rect.x = (position->x + PIPE_SIZE) - max_flowed_pixels;
        rect.y = position->y + Pipe::pipe_size_middle_start;
        rect.w = PIPE_SIZE - (PIPE_SIZE - max_flowed_pixels);
        rect.h = FLOW_LENGTH;
    } else if(flow_start == FLOW_DOWN) {
        rect.x = position->x + Pipe::pipe_size_middle_start;
        rect.y = (position->y + PIPE_SIZE) - max_flowed_pixels;
        rect.w = FLOW_LENGTH;
        rect.h = PIPE_SIZE - (PIPE_SIZE - max_flowed_pixels);
    } else if(flow_start == FLOW_LEFT) {
        rect.x = position->x;
        rect.y = position->y + Pipe::pipe_size_middle_start;
        rect.w = max_flowed_pixels;
        rect.h = FLOW_LENGTH;
    }

    return rect;
}

SDL_Rect Pipe::LastFlowRect(SDL_Rect* position, unsigned int flow_end) {
    SDL_Rect rect;

    // makes it go only halfway
    int max_flowed_pixels = flowed_pixels - Pipe::pipe_size_middle;

    if(flow_end == FLOW_TOP) {
        rect.x = position->x + Pipe::pipe_size_middle_start;
        rect.y = (position->y + pipe_size_middle) - max_flowed_pixels;
        rect.w = FLOW_LENGTH;
        rect.h = PIPE_SIZE - (PIPE_SIZE - max_flowed_pixels);
    } else if(flow_end == FLOW_RIGHT) {
        rect.x = position->x + Pipe::pipe_size_middle;
        rect.y = position->y + Pipe::pipe_size_middle_start;
        rect.w = max_flowed_pixels;
        rect.h = FLOW_LENGTH;
    } else if(flow_end == FLOW_DOWN) {
        rect.x = position->x + Pipe::pipe_size_middle_start;
        rect.y = position->y + pipe_size_middle;
        rect.w = FLOW_LENGTH;
        rect.h = max_flowed_pixels;
    } else if(flow_end == FLOW_LEFT) {
        rect.x = (position->x + pipe_size_middle) - max_flowed_pixels;
        rect.y = position->y + Pipe::pipe_size_middle_start;
        rect.w = pipe_size_middle - (pipe_size_middle - max_flowed_pixels);
        rect.h = FLOW_LENGTH;
    }

    return rect;
}

bool Pipe::isFlowFinished() {
    return flowed_pixels == PIPE_SIZE;
}

int Pipe::getFlowStartPosition() {
    return flow_start_position;
}

int Pipe::getFlowTurnPosition() {
    return flow_turn_position;
}

void Pipe::setFlowTurnPosition(int direction) {
    flow_turn_position = direction;
}
<|endoftext|>


<commit_before>/*=========================================================================

  Program:   Visualization Library
  Module:    Plane.cc
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

Description:
---------------------------------------------------------------------------
This file is part of the Visualization Library. No part of this file
or its contents may be copied, reproduced or altered in any way
without the express written consent of the authors.

Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen 1993, 1994

=========================================================================*/
#include "Plane.hh"
#include "vlMath.hh"

vlPlane::vlPlane()
{
  this->Normal[0] = 0.0;
  this->Normal[1] = 0.0;
  this->Normal[2] = 1.0;

  this->Origin[0] = 0.0;
  this->Origin[1] = 0.0;
  this->Origin[2] = 0.0;
}

void vlPlane::ProjectPoint(float x[3], float origin[3], float normal[3], float xproj[3])
{
  int i;
  vlMath math;
  float t, xo[3];

  for (i=0; i<3; i++) xo[i] = x[i] - origin[i];
  t = math.Dot(normal,xo);
  for (i=0; i<3; i++) xproj[i] = x[i] - t * normal[i];
}

float vlPlane::Evaluate(float x, float y, float z)
{
  return ( this->Normal[0]*(x-this->Origin[0]) +
           this->Normal[1]*(y-this->Origin[1]) +
           this->Normal[2]*(z-this->Origin[2]) );
}
<commit_msg>*** empty log message ***<commit_after>/*=========================================================================

  Program:   Visualization Library
  Module:    Plane.cc
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

Description:
---------------------------------------------------------------------------
This file is part of the Visualization Library. No part of this file
or its contents may be copied, reproduced or altered in any way
without the express written consent of the authors.

Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen 1993, 1994

=========================================================================*/
#include "Plane.hh"
#include "vlMath.hh"

vlPlane::vlPlane()
{
  this->Normal[0] = 0.0;
  this->Normal[1] = 0.0;
  this->Normal[2] = 1.0;

  this->Origin[0] = 0.0;
  this->Origin[1] = 0.0;
  this->Origin[2] = 0.0;
}

// NOTE : normal assumed to have magnitude 1
void vlPlane::ProjectPoint(float x[3], float origin[3], float normal[3], float xproj[3])
{
  int i;
  vlMath math;
  float t, xo[3];

  for (i=0; i<3; i++) xo[i] = x[i] - origin[i];
  t = math.Dot(normal,xo);
  for (i=0; i<3; i++) xproj[i] = x[i] - t * normal[i];
}

float vlPlane::Evaluate(float x, float y, float z)
{
  return ( this->Normal[0]*(x-this->Origin[0]) +
           this->Normal[1]*(y-this->Origin[1]) +
           this->Normal[2]*(z-this->Origin[2]) );
}
<|endoftext|>