<commit_before>/////////////////////////////////////////////////////////////////////////////
//                                                                         //
// AliFemtoShareQualityPairCut - a pair cut which checks for some pair     //
// qualities that attempt to identify slit/doubly reconstructed tracks     //
//                                                                         //
/////////////////////////////////////////////////////////////////////////////
/***************************************************************************
 *
 * $Id: AliFemtoShareQualityPairCut.cxx 53713 2011-12-20 12:31:21Z akisiel $
 *
 * Author: Adam Kisiel, Ohio State, kisiel@mps.ohio-state.edu
 ***************************************************************************
 *
 * Description: part of STAR HBT Framework: AliFemtoMaker package
 *   a cut to remove "shared" and "split" pairs
 *
 ***************************************************************************
 *
 *
 **************************************************************************/

#include "AliFemtoShareQualityPairCut.h"
#include <string>
#include <cstdio>

#ifdef __ROOT__
ClassImp(AliFemtoShareQualityPairCut)
#endif

//__________________
AliFemtoShareQualityPairCut::AliFemtoShareQualityPairCut():
  fNPairsPassed(0),
  fNPairsFailed(0),
  fShareQualityMax(1.0),
  fShareFractionMax(1.0),
  fRemoveSameLabel(0)
{
  // Default constructor
  // Nothing to do
}
//__________________
AliFemtoShareQualityPairCut::~AliFemtoShareQualityPairCut(){
  /* no-op */
}
AliFemtoShareQualityPairCut& AliFemtoShareQualityPairCut::operator=(const AliFemtoShareQualityPairCut& cut)
{
  if (this != &cut) {
    AliFemtoPairCut::operator=(cut);
    fNPairsPassed = 0;
    fNPairsFailed = 0;
    fShareQualityMax = cut.fShareQualityMax;
    fShareFractionMax = cut.fShareFractionMax;
    fRemoveSameLabel = cut.fRemoveSameLabel;
  }

  return *this;
}
//__________________
bool AliFemtoShareQualityPairCut::Pass(const AliFemtoPair* pair){
  // Check for pairs that are possibly shared/double reconstruction
  bool temp;

  Int_t nh = 0;
  Int_t an = 0;
  Int_t ns = 0;

  if ((fShareFractionMax >= 1.0) && ( fShareQualityMax >= 1.0)) {
     temp = true;
  }
  else {
     for (unsigned int imap=0; imap<pair->Track1()->Track()->TPCclusters().GetNbits(); imap++) {
        // If both have clusters in the same row
        if (pair->Track1()->Track()->TPCclusters().TestBitNumber(imap) &&
            pair->Track2()->Track()->TPCclusters().TestBitNumber(imap)) {
           // Do they share it ?
           if (pair->Track1()->Track()->TPCsharing().TestBitNumber(imap) &&
               pair->Track2()->Track()->TPCsharing().TestBitNumber(imap))
           {
              //	  cout << "A shared cluster !!!" << endl;
              //	cout << "imap idx1 idx2 "
              //	     << imap << " "
              //	     << tP1idx[imap] << " " << tP2idx[imap] << endl;
              an++;
              nh+=2;
              ns+=2;
           }
           // Different hits on the same padrow
           else {
              an--;
              nh+=2;
           }
        }
        else if (pair->Track1()->Track()->TPCclusters().TestBitNumber(imap) ||
                 pair->Track2()->Track()->TPCclusters().TestBitNumber(imap)) {
           // One track has a hit, the other does not
           an++;
           nh++;
        }
     }
     Float_t hsmval = 0.0;
     Float_t hsfval = 0.0;

     if (nh >0) {
        hsmval = an*1.0/nh;
        hsfval = ns*1.0/nh;
     }
    //  if (hsmval > -0.4) {
    //   cout << "Pair quality: " << hsmval << " " << an << " " << nh << " "
    //        << (pair->Track1()->Track()) << " "
    //        << (pair->Track2()->Track()) << endl;
    //   cout << "Bits: " << pair->Track1()->Track()->TPCclusters().GetNbits() << endl;
    //  }
    //   if (hsfval > 0.0) {
    //     cout << "Pair sharity: " << hsfval << " " << ns << " " << nh << "    " << hsmval << " " << an << " " << nh << endl;
    //   }
     if( (fShareQualityMax < 1) && (fShareFractionMax < 1)  )
        temp = (hsmval < fShareQualityMax) && (hsfval < fShareFractionMax);
     else if (fShareQualityMax < 1)
        temp = (hsmval < fShareQualityMax);
     else if (fShareFractionMax < 1)
        temp = (hsmval < fShareFractionMax);
     else temp = false;
  }

  if (fRemoveSameLabel) {
    if (abs(pair->Track1()->Track()->Label()) == abs(pair->Track2()->Track()->Label())) {
//       cout << "Found a pair with same label " << pair->Track1()->Track()->Label() << endl;
//       cout << "Quality Sharity Passed " << hsmval << " " << hsfval << " " << pair->QInv() << " " << temp << endl;
      temp = kFALSE;
    }
  }

  temp ? fNPairsPassed++ : fNPairsFailed++;
  return temp;
}
//__________________
AliFemtoString AliFemtoShareQualityPairCut::Report(){
  // Prepare the report from the execution
  string stemp = "AliFemtoShareQuality Pair Cut - remove shared and split pairs\n";  char ctemp[100];
  snprintf(ctemp , 100, "Number of pairs which passed:\t%ld  Number which failed:\t%ld\n",fNPairsPassed,fNPairsFailed);
  stemp += ctemp;
  AliFemtoString returnThis = stemp;
  return returnThis;}
//__________________

void AliFemtoShareQualityPairCut::SetShareQualityMax(Double_t aShareQualityMax) {
  fShareQualityMax = aShareQualityMax;
}

Double_t AliFemtoShareQualityPairCut::GetAliFemtoShareQualityMax() const {
  return fShareQualityMax;
}

void AliFemtoShareQualityPairCut::SetShareFractionMax(Double_t aShareFractionMax) {
  fShareFractionMax = aShareFractionMax;
}
Double_t AliFemtoShareQualityPairCut::GetAliFemtoShareFractionMax() const {
  return fShareFractionMax;
}

TList *AliFemtoShareQualityPairCut::ListSettings()
{
  // return a list of settings in a writable form
  TList *tListSetttings = new TList();
  char buf[200];
  snprintf(buf, 200, "AliFemtoShareQualityPairCut.sharequalitymax=%f", fShareQualityMax);
  snprintf(buf, 200, "AliFemtoShareQualityPairCut.sharefractionmax=%f", fShareFractionMax);
  tListSetttings->AddLast(new TObjString(buf));

  return tListSetttings;
}

void     AliFemtoShareQualityPairCut::SetRemoveSameLabel(Bool_t aRemove)
{
  fRemoveSameLabel = aRemove;
}
<commit_msg>PWGCF: AliFemtoShareQualityPairCut - Improved performance of `Pass` method by factoring out repetative calls in loop<commit_after>/////////////////////////////////////////////////////////////////////////////
//                                                                         //
// AliFemtoShareQualityPairCut - a pair cut which checks for some pair     //
// qualities that attempt to identify slit/doubly reconstructed tracks     //
//                                                                         //
/////////////////////////////////////////////////////////////////////////////
/***************************************************************************
 *
 * $Id: AliFemtoShareQualityPairCut.cxx 53713 2011-12-20 12:31:21Z akisiel $
 *
 * Author: Adam Kisiel, Ohio State, kisiel@mps.ohio-state.edu
 ***************************************************************************
 *
 * Description: part of STAR HBT Framework: AliFemtoMaker package
 *   a cut to remove "shared" and "split" pairs
 *
 ***************************************************************************
 *
 *
 **************************************************************************/

#include "AliFemtoShareQualityPairCut.h"
#include <string>
#include <cstdio>

#ifdef __ROOT__
ClassImp(AliFemtoShareQualityPairCut)
#endif

//__________________
AliFemtoShareQualityPairCut::AliFemtoShareQualityPairCut():
  fNPairsPassed(0),
  fNPairsFailed(0),
  fShareQualityMax(1.0),
  fShareFractionMax(1.0),
  fRemoveSameLabel(0)
{
  // Default constructor
  // Nothing to do
}
//__________________
AliFemtoShareQualityPairCut::~AliFemtoShareQualityPairCut(){
  /* no-op */
}
AliFemtoShareQualityPairCut& AliFemtoShareQualityPairCut::operator=(const AliFemtoShareQualityPairCut& cut)
{
  if (this != &cut) {
    AliFemtoPairCut::operator=(cut);
    fNPairsPassed = 0;
    fNPairsFailed = 0;
    fShareQualityMax = cut.fShareQualityMax;
    fShareFractionMax = cut.fShareFractionMax;
    fRemoveSameLabel = cut.fRemoveSameLabel;
  }

  return *this;
}
//__________________
bool AliFemtoShareQualityPairCut::Pass(const AliFemtoPair* pair){
  // Check for pairs that are possibly shared/double reconstruction
  bool temp;

  Int_t nh = 0;
  Int_t an = 0;
  Int_t ns = 0;

  if ((fShareFractionMax >= 1.0) && ( fShareQualityMax >= 1.0)) {
     temp = true;
  }
  else {
    const unsigned int n_bits = pair->Track1()->Track()->TPCclusters().GetNbits();

    const auto tpc_clusters_1 = pair->Track1()->Track()->TPCclusters(),
               tpc_clusters_2 = pair->Track2()->Track()->TPCclusters();

    const auto tpc_sharing_1 = pair->Track1()->Track()->TPCsharing(),
               tpc_sharing_2 = pair->Track2()->Track()->TPCsharing();

     for (unsigned int imap = 0; imap < n_bits; imap++) {
       const bool cluster_bit_1 = tpc_clusters_1.TestBitNumber(imap),
                  cluster_bit_2 = tpc_clusters_1.TestBitNumber(imap);
        // If both have clusters in the same row
        if (cluster_bit_1 && cluster_bit_2) {
           // Do they share it ?
           if (tpc_sharing_1.TestBitNumber(imap) && tpc_sharing_2.TestBitNumber(imap))
           {
              //	  cout << "A shared cluster !!!" << endl;
              //	cout << "imap idx1 idx2 "
              //	     << imap << " "
              //	     << tP1idx[imap] << " " << tP2idx[imap] << endl;
              an++;
              nh+=2;
              ns+=2;
           }
           // Different hits on the same padrow
           else {
              an--;
              nh+=2;
           }
        }
        else if (cluster_bit_1 || cluster_bit_2) {
           // One track has a hit, the other does not
           an++;
           nh++;
        }
     }
     Float_t hsmval = 0.0;
     Float_t hsfval = 0.0;

     if (nh >0) {
        hsmval = an*1.0/nh;
        hsfval = ns*1.0/nh;
     }
    //  if (hsmval > -0.4) {
    //   cout << "Pair quality: " << hsmval << " " << an << " " << nh << " "
    //        << (pair->Track1()->Track()) << " "
    //        << (pair->Track2()->Track()) << endl;
    //   cout << "Bits: " << pair->Track1()->Track()->TPCclusters().GetNbits() << endl;
    //  }
    //   if (hsfval > 0.0) {
    //     cout << "Pair sharity: " << hsfval << " " << ns << " " << nh << "    " << hsmval << " " << an << " " << nh << endl;
    //   }
     if( (fShareQualityMax < 1) && (fShareFractionMax < 1)  )
        temp = (hsmval < fShareQualityMax) && (hsfval < fShareFractionMax);
     else if (fShareQualityMax < 1)
        temp = (hsmval < fShareQualityMax);
     else if (fShareFractionMax < 1)
        temp = (hsmval < fShareFractionMax);
     else temp = false;
  }

  if (fRemoveSameLabel) {
    if (abs(pair->Track1()->Track()->Label()) == abs(pair->Track2()->Track()->Label())) {
//       cout << "Found a pair with same label " << pair->Track1()->Track()->Label() << endl;
//       cout << "Quality Sharity Passed " << hsmval << " " << hsfval << " " << pair->QInv() << " " << temp << endl;
      temp = kFALSE;
    }
  }

  temp ? fNPairsPassed++ : fNPairsFailed++;
  return temp;
}
//__________________
AliFemtoString AliFemtoShareQualityPairCut::Report()
{
  // Prepare the report from the execution
  string stemp = "AliFemtoShareQuality Pair Cut - remove shared and split pairs\n";  char ctemp[100];
  snprintf(ctemp , 100, "Number of pairs which passed:\t%ld  Number which failed:\t%ld\n",fNPairsPassed,fNPairsFailed);
  stemp += ctemp;
  AliFemtoString returnThis = stemp;
  return returnThis;}
//__________________

void AliFemtoShareQualityPairCut::SetShareQualityMax(Double_t aShareQualityMax) {
  fShareQualityMax = aShareQualityMax;
}

Double_t AliFemtoShareQualityPairCut::GetAliFemtoShareQualityMax() const {
  return fShareQualityMax;
}

void AliFemtoShareQualityPairCut::SetShareFractionMax(Double_t aShareFractionMax) {
  fShareFractionMax = aShareFractionMax;
}
Double_t AliFemtoShareQualityPairCut::GetAliFemtoShareFractionMax() const {
  return fShareFractionMax;
}

TList *AliFemtoShareQualityPairCut::ListSettings()
{
  // return a list of settings in a writable form
  TList *tListSetttings = new TList();
  char buf[200];
  snprintf(buf, 200, "AliFemtoShareQualityPairCut.sharequalitymax=%f", fShareQualityMax);
  snprintf(buf, 200, "AliFemtoShareQualityPairCut.sharefractionmax=%f", fShareFractionMax);
  tListSetttings->AddLast(new TObjString(buf));

  return tListSetttings;
}

void     AliFemtoShareQualityPairCut::SetRemoveSameLabel(Bool_t aRemove)
{
  fRemoveSameLabel = aRemove;
}
<|endoftext|>


<commit_before>//=======================================================================
// Copyright (c) 2014-2016 Baptiste Wicht
// Distributed under the terms of the MIT License.
// (See accompanying file LICENSE or copy at
//  http://opensource.org/licenses/MIT)
//=======================================================================

#include "test.hpp"

TEMPLATE_TEST_CASE_2("alias/1", "[alias]", Z, float, double) {
    etl::fast_matrix<Z, 3, 3> a;
    etl::dyn_matrix<Z> b(3, 3);

    REQUIRE_DIRECT(a.alias(a));
    REQUIRE_DIRECT(b.alias(b));
    REQUIRE_DIRECT(!b.alias(a));
    REQUIRE_DIRECT(!a.alias(b));

    REQUIRE_DIRECT(a.alias(a + a));
    REQUIRE_DIRECT(a.alias(a + 1));
    REQUIRE_DIRECT(!a.alias(b + 1));
    REQUIRE_DIRECT((a + a).alias(a + a));
    REQUIRE_DIRECT(!a.alias(b >> b));

    REQUIRE_DIRECT(a.alias(a(0)));
    REQUIRE_DIRECT(a(0).alias(a(0)));
    REQUIRE_DIRECT(!a(0).alias(a(1)));
    REQUIRE_DIRECT(a.alias(a(0) + a(1)));
}

TEMPLATE_TEST_CASE_2("alias/traits/1", "[alias][traits]", Z, float, double) {
    etl::fast_matrix<Z, 3, 3> a({1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0});

    //Test linear operations
    REQUIRE_DIRECT(etl::decay_traits<decltype(a)>::is_linear);
    REQUIRE_DIRECT(etl::decay_traits<decltype(a* a)>::is_linear);
    REQUIRE_DIRECT(etl::decay_traits<decltype((a >> a) + a - a / a)>::is_linear);
    REQUIRE_DIRECT(etl::decay_traits<decltype(a)>::is_linear);
    REQUIRE_DIRECT(etl::decay_traits<decltype(a(0))>::is_linear);

    //Test non linear operations
    REQUIRE_DIRECT(!etl::decay_traits<decltype(transpose(a))>::is_linear);
    REQUIRE_DIRECT(!etl::decay_traits<decltype(fflip(a))>::is_linear);
    REQUIRE_DIRECT(!etl::decay_traits<decltype(a + fflip(a))>::is_linear);
}

TEMPLATE_TEST_CASE_2("alias/transpose/1", "[alias][transpose]", Z, float, double) {
    etl::fast_matrix<Z, 3, 3> a({1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0});

    a = transpose(a);

    REQUIRE_EQUALS(a(0, 0), 1.0);
    REQUIRE_EQUALS(a(0, 1), 4.0);
    REQUIRE_EQUALS(a(0, 2), 7.0);
    REQUIRE_EQUALS(a(1, 0), 2.0);
    REQUIRE_EQUALS(a(1, 1), 5.0);
    REQUIRE_EQUALS(a(1, 2), 8.0);
    REQUIRE_EQUALS(a(2, 0), 3.0);
    REQUIRE_EQUALS(a(2, 1), 6.0);
    REQUIRE_EQUALS(a(2, 2), 9.0);
}

TEMPLATE_TEST_CASE_2("alias/transpose/2", "[alias][transpose]", Z, float, double) {
    etl::fast_matrix<Z, 3, 3> a({1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0});

    a = (transpose(a) >> 2.0) + (transpose(a) >> 3.0);

    REQUIRE_EQUALS(a(0, 0), 5.0);
    REQUIRE_EQUALS(a(0, 1), 20.0);
    REQUIRE_EQUALS(a(0, 2), 35.0);
    REQUIRE_EQUALS(a(1, 0), 10.0);
    REQUIRE_EQUALS(a(1, 1), 25.0);
    REQUIRE_EQUALS(a(1, 2), 40.0);
    REQUIRE_EQUALS(a(2, 0), 15.0);
    REQUIRE_EQUALS(a(2, 1), 30.0);
    REQUIRE_EQUALS(a(2, 2), 45.0);
}
<commit_msg>Transpose is now a linear operation<commit_after>//=======================================================================
// Copyright (c) 2014-2016 Baptiste Wicht
// Distributed under the terms of the MIT License.
// (See accompanying file LICENSE or copy at
//  http://opensource.org/licenses/MIT)
//=======================================================================

#include "test.hpp"

TEMPLATE_TEST_CASE_2("alias/1", "[alias]", Z, float, double) {
    etl::fast_matrix<Z, 3, 3> a;
    etl::dyn_matrix<Z> b(3, 3);

    REQUIRE_DIRECT(a.alias(a));
    REQUIRE_DIRECT(b.alias(b));
    REQUIRE_DIRECT(!b.alias(a));
    REQUIRE_DIRECT(!a.alias(b));

    REQUIRE_DIRECT(a.alias(a + a));
    REQUIRE_DIRECT(a.alias(a + 1));
    REQUIRE_DIRECT(!a.alias(b + 1));
    REQUIRE_DIRECT((a + a).alias(a + a));
    REQUIRE_DIRECT(!a.alias(b >> b));

    REQUIRE_DIRECT(a.alias(a(0)));
    REQUIRE_DIRECT(a(0).alias(a(0)));
    REQUIRE_DIRECT(!a(0).alias(a(1)));
    REQUIRE_DIRECT(a.alias(a(0) + a(1)));
}

TEMPLATE_TEST_CASE_2("alias/traits/1", "[alias][traits]", Z, float, double) {
    etl::fast_matrix<Z, 3, 3> a({1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0});

    //Test linear operations
    REQUIRE_DIRECT(etl::decay_traits<decltype(a)>::is_linear);
    REQUIRE_DIRECT(etl::decay_traits<decltype(a* a)>::is_linear);
    REQUIRE_DIRECT(etl::decay_traits<decltype((a >> a) + a - a / a)>::is_linear);
    REQUIRE_DIRECT(etl::decay_traits<decltype(a)>::is_linear);
    REQUIRE_DIRECT(etl::decay_traits<decltype(a(0))>::is_linear);
    REQUIRE_DIRECT(etl::decay_traits<decltype(transpose(a))>::is_linear);

    //Test non linear operations
    REQUIRE_DIRECT(!etl::decay_traits<decltype(fflip(a))>::is_linear);
    REQUIRE_DIRECT(!etl::decay_traits<decltype(a + fflip(a))>::is_linear);
}

TEMPLATE_TEST_CASE_2("alias/transpose/1", "[alias][transpose]", Z, float, double) {
    etl::fast_matrix<Z, 3, 3> a({1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0});

    a = transpose(a);

    REQUIRE_EQUALS(a(0, 0), 1.0);
    REQUIRE_EQUALS(a(0, 1), 4.0);
    REQUIRE_EQUALS(a(0, 2), 7.0);
    REQUIRE_EQUALS(a(1, 0), 2.0);
    REQUIRE_EQUALS(a(1, 1), 5.0);
    REQUIRE_EQUALS(a(1, 2), 8.0);
    REQUIRE_EQUALS(a(2, 0), 3.0);
    REQUIRE_EQUALS(a(2, 1), 6.0);
    REQUIRE_EQUALS(a(2, 2), 9.0);
}

TEMPLATE_TEST_CASE_2("alias/transpose/2", "[alias][transpose]", Z, float, double) {
    etl::fast_matrix<Z, 3, 3> a({1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0});

    a = (transpose(a) >> 2.0) + (transpose(a) >> 3.0);

    REQUIRE_EQUALS(a(0, 0), 5.0);
    REQUIRE_EQUALS(a(0, 1), 20.0);
    REQUIRE_EQUALS(a(0, 2), 35.0);
    REQUIRE_EQUALS(a(1, 0), 10.0);
    REQUIRE_EQUALS(a(1, 1), 25.0);
    REQUIRE_EQUALS(a(1, 2), 40.0);
    REQUIRE_EQUALS(a(2, 0), 15.0);
    REQUIRE_EQUALS(a(2, 1), 30.0);
    REQUIRE_EQUALS(a(2, 2), 45.0);
}
<|endoftext|>


<commit_before>// filter.cpp
// Author: Allen Porter <allen@thebends.org>

#include "filter.h"
#include "config.h"
#include <math.h>
#include <stddef.h>

namespace synth {

static const float kE = 2.71828183f;

Filter::Filter() { }

Filter::~Filter() { }

LowPassFilter::LowPassFilter() : cutoff_(NULL), last_cutoff_(0), x1_(0), x2_(0), y1_(0), y2_(0) {
}

LowPassFilter::~LowPassFilter() { }

void LowPassFilter::set_cutoff(Parameter* cutoff) {
  cutoff_ = cutoff;
}

void LowPassFilter::reset(float frequency) {
  // Number of filter passes
  float n = 1;

  // 3dB cutoff frequency
  float f0 = frequency;

  // Sample frequency
  const float fs = Config::kSampleRate;

  // 3dB cutoff correction
  float c = powf(powf(2, 1.0f / n) - 1, -0.25);

  // Polynomial coefficients
  float g = 1;
  float p = sqrtf(2);

  // Corrected cutoff frequency
  float fp = c * (f0 / fs);

  // TODO(allen): We should probably do more filter passes so that we can ensure  // these stability constraints are met for sufficiently high frequencies.
  // Ensure fs is OK for stability constraint
  //assert(fp > 0);
  //assert(fp < 0.125);

  // Warp cutoff freq from analog to digital domain
  float w0 = tanf(M_PI * fp);

  // Calculate the filter co-efficients
  float k1 = p * w0;
  float k2 = g * w0 * w0;

  a0_ = k2 / (1 + k1 + k2);
  a1_ = 2 * a0_;
  a2_ = a0_;
  b1_ = 2 * a0_ * (1 / k2 - 1);
  b2_ = 1 - (a0_ + a1_ + a2_ + b1_);
}

float LowPassFilter::GetValue(float x) {
  if (cutoff_ == NULL) {
    return x;
  }

  // Re-initialize the filter co-efficients if they changed
  float cutoff = cutoff_->GetValue();
  if (cutoff <= 0.0f) {
    return x;
  } else if (cutoff < 0.001f) {
    // Filtering all frequencies
    return 0.0f;
  }
  if (fabs(cutoff - last_cutoff_) > 0.001f) {
    reset(cutoff);
    last_cutoff_ = cutoff;
  }

  float y = a0_ * x + a1_ * x1_ + a2_ *  x2_ + b1_ * y1_ + b2_ * y2_;
  x1_ = x;
  x2_ = x1_;
  y2_ = y1_;
  y1_ = y;
  return y;
}

ResonantFilter::ResonantFilter()
   : cutoff_(NULL), resonance_(0.0f),
     y1_(0.0f), y2_(0.0f), y3_(0.0f), y4_(0.0f),
     oldx_(0.0f), oldy1_(0.0f), oldy2_(0.0f), oldy3_(0.0f) { }

ResonantFilter::~ResonantFilter() { }

void ResonantFilter::set_cutoff(Parameter* cutoff) {
  cutoff_ = cutoff;
}

void ResonantFilter::set_resonance(float resonance) {
  resonance_ = resonance;
}

float ResonantFilter::GetValue(float x) {
  if (cutoff_ == NULL) {
    return x;
  }
  float cutoff = cutoff_->GetValue();
  float f = 2.0f * cutoff / Config::kSampleRate;
  float k = 3.6f * f - 1.6f * f * f - 1;
  float p = (k + 1.0f) * 0.5f;
  float scale = powf(kE, (1.0f - p) * 1.386249);
  float r = resonance_ * scale;

  float out = x - r * y4_;
  y1_ = out * p + oldx_ * p - k * y1_;
  y2_ = y1_ * p + oldy1_ * p - k * y2_;
  y3_ = y2_ * p + oldy2_ * p - k * y3_;
  y4_ = y3_ * p + oldy3_ * p - k * y4_;
  y4_ = y4_ - powf(y4_, 3.0f) / 6.0f;
  oldx_ = out;
  oldy1_ = y1_;
  oldy2_ = y2_;
  oldy3_ = y3_;
  return out;
}

FilterCutoff::FilterCutoff() : cutoff_(-1.0f),
                               modulation_(NULL) { }

FilterCutoff::~FilterCutoff() { }

float FilterCutoff::GetValue() {
  float value = cutoff_ * envelope_.GetValue();
  if (modulation_) {
    value *= modulation_->GetValue();
  }
  return value;
}

}  // namespace synth
<commit_msg>fix to avoid NaN<commit_after>// filter.cpp
// Author: Allen Porter <allen@thebends.org>

#include "filter.h"
#include "config.h"
#include <math.h>
#include <stddef.h>

namespace synth {

static const float kE = 2.71828183f;

Filter::Filter() { }

Filter::~Filter() { }

LowPassFilter::LowPassFilter() : cutoff_(NULL), last_cutoff_(0), x1_(0), x2_(0), y1_(0), y2_(0) {
}

LowPassFilter::~LowPassFilter() { }

void LowPassFilter::set_cutoff(Parameter* cutoff) {
  cutoff_ = cutoff;
}

void LowPassFilter::reset(float frequency) {
  // Number of filter passes
  float n = 1;

  // 3dB cutoff frequency
  float f0 = frequency;

  // Sample frequency
  const float fs = Config::kSampleRate;

  // 3dB cutoff correction
  float c = powf(powf(2, 1.0f / n) - 1, -0.25);

  // Polynomial coefficients
  float g = 1;
  float p = sqrtf(2);

  // Corrected cutoff frequency
  float fp = c * (f0 / fs);

  // TODO(allen): We should probably do more filter passes so that we can ensure  // these stability constraints are met for sufficiently high frequencies.
  // Ensure fs is OK for stability constraint
  //assert(fp > 0);
  //assert(fp < 0.125);

  // Warp cutoff freq from analog to digital domain
  float w0 = tanf(M_PI * fp);

  // Calculate the filter co-efficients
  float k1 = p * w0;
  float k2 = g * w0 * w0;

  a0_ = k2 / (1 + k1 + k2);
  a1_ = 2 * a0_;
  a2_ = a0_;
  b1_ = 2 * a0_ * (1 / k2 - 1);
  b2_ = 1 - (a0_ + a1_ + a2_ + b1_);
}

float LowPassFilter::GetValue(float x) {
  if (cutoff_ == NULL) {
    return x;
  }

  // Re-initialize the filter co-efficients if they changed
  float cutoff = cutoff_->GetValue();
  if (cutoff <= 0.0f) {
    return x;
  } else if (cutoff < 0.001f) {
    // Filtering all frequencies
    return 0.0f;
  }
  if (fabs(cutoff - last_cutoff_) > 0.001f) {
    reset(cutoff);
    last_cutoff_ = cutoff;
  }

  float y = a0_ * x + a1_ * x1_ + a2_ *  x2_ + b1_ * y1_ + b2_ * y2_;
  x1_ = x;
  x2_ = x1_;
  y2_ = y1_;
  y1_ = y;
  return y;
}

ResonantFilter::ResonantFilter()
   : cutoff_(NULL), resonance_(0.0f),
     y1_(0.0f), y2_(0.0f), y3_(0.0f), y4_(0.0f),
     oldx_(0.0f), oldy1_(0.0f), oldy2_(0.0f), oldy3_(0.0f) { }

ResonantFilter::~ResonantFilter() { }

void ResonantFilter::set_cutoff(Parameter* cutoff) {
  cutoff_ = cutoff;
}

void ResonantFilter::set_resonance(float resonance) {
  resonance_ = resonance;
}

float ResonantFilter::GetValue(float x) {
  if (cutoff_ == NULL) {
    return x;
  }
  float cutoff = cutoff_->GetValue();
  float f = 2.0f * cutoff / Config::kSampleRate;
  float k = 3.6f * f - 1.6f * f * f - 1;
  float p = (k + 1.0f) * 0.5f;
  float scale = powf(kE, (1.0f - p) * 1.386249);
  float r = resonance_ * scale;

  float out = x - r * y4_;
  y1_ = out * p + oldx_ * p - k * y1_;
  y2_ = y1_ * p + oldy1_ * p - k * y2_;
  y3_ = y2_ * p + oldy2_ * p - k * y3_;
  y4_ = y3_ * p + oldy3_ * p - k * y4_;
  y4_ = y4_ - powf(y4_, 3.0f) / 6.0f;
  if (isinf(y4_)) {
    y1_ = 0;
    y2_ = 0;
    y3_ = 0;
    y4_ = 0;
  }
  oldx_ = out;
  oldy1_ = y1_;
  oldy2_ = y2_;
  oldy3_ = y3_;
  return out;
}

FilterCutoff::FilterCutoff() : cutoff_(-1.0f),
                               modulation_(NULL) { }

FilterCutoff::~FilterCutoff() { }

float FilterCutoff::GetValue() {
  float value = cutoff_ * envelope_.GetValue();
  if (modulation_) {
    value *= modulation_->GetValue();
  }
  return value;
}

}  // namespace synth
<|endoftext|>


<commit_before>#ifndef PYTHONIC_NUMPY_SEARCHSORTED_HPP
#define PYTHONIC_NUMPY_SEARCHSORTED_HPP

#include "pythonic/include/numpy/searchsorted.hpp"

#include "pythonic/utils/functor.hpp"
#include "pythonic/utils/numpy_conversion.hpp"
#include "pythonic/utils/int_.hpp"
#include "pythonic/types/ndarray.hpp"
#include "pythonic/types/str.hpp"
#include "pythonic/builtins/None.hpp"
#include "pythonic/builtins/ValueError.hpp"
#include "pythonic/numpy/asarray.hpp"

#include <algorithm>

PYTHONIC_NS_BEGIN

namespace numpy
{

  template <class T, class U>
  typename std::enable_if<!types::is_numexpr_arg<T>::value, long>::type
  searchsorted(U const &a, T const &v, types::str const &side)
  {
    if (side[0] == "l")
      return std::lower_bound(a.begin(), a.end(), v) - a.begin();
    else if (side[0] == "r")
      return std::upper_bound(a.begin(), a.end(), v) - a.begin();
    else
      throw types::ValueError("'" + side +
                              "' is an invalid value for keyword 'side'");
  }

  namespace
  {
    template <class E, class I0, class I1>
    void _search_sorted(E const &a, I0 ibegin, I0 iend, I1 obegin,
                        types::str const &side, utils::int_<1>)
    {
      for (; ibegin != iend; ++ibegin, ++obegin)
        *obegin = searchsorted(a, *ibegin, side);
    }

    template <class E, class I0, class I1, size_t N>
    void _search_sorted(E const &a, I0 ibegin, I0 iend, I1 obegin,
                        types::str const &side, utils::int_<N>)
    {
      for (; ibegin != iend; ++ibegin, ++obegin)
        _search_sorted(a, (*ibegin).begin(), (*ibegin).end(), (*obegin).begin(),
                       side, utils::int_<N - 1>());
    }
  }

  template <class E, class T>
  typename std::enable_if<
      types::is_numexpr_arg<E>::value,
      types::ndarray<long, types::array<long, E::value>>>::type
  searchsorted(T const &a, E const &v, types::str const &side)
  {
    static_assert(T::value == 1,
                  "Not Implemented : searchsorted for dimension != 1");

    types::ndarray<long, types::array<long, E::value>> out(asarray(v)._shape,
                                                           builtins::None);
    _search_sorted(a, v.begin(), v.end(), out.begin(), side,
                   utils::int_<E::value>());
    return out;
  }
}
PYTHONIC_NS_END

#endif
<commit_msg>Fix performance of numpy.searchsorted (#1794)<commit_after>#ifndef PYTHONIC_NUMPY_SEARCHSORTED_HPP
#define PYTHONIC_NUMPY_SEARCHSORTED_HPP

#include "pythonic/include/numpy/searchsorted.hpp"

#include "pythonic/utils/functor.hpp"
#include "pythonic/utils/numpy_conversion.hpp"
#include "pythonic/utils/int_.hpp"
#include "pythonic/types/ndarray.hpp"
#include "pythonic/types/str.hpp"
#include "pythonic/builtins/None.hpp"
#include "pythonic/builtins/ValueError.hpp"
#include "pythonic/numpy/asarray.hpp"

#include <algorithm>

PYTHONIC_NS_BEGIN

namespace numpy
{
  namespace details
  {
    template <class T, class U>
    long searchsorted(U const &a, T const &v, bool left)
    {
      if (left)
        return std::lower_bound(a.begin(), a.end(), v) - a.begin();
      else
        return std::upper_bound(a.begin(), a.end(), v) - a.begin();
    }

    bool issearchsortedleft(types::str const &side)
    {
      if (side[0] == "l")
        return true;
      else if (side[0] == "r")
        return false;
      else
        throw types::ValueError("'" + side +
                                "' is an invalid value for keyword 'side'");
    }
  }

  template <class T, class U>
  typename std::enable_if<!types::is_numexpr_arg<T>::value, long>::type
  searchsorted(U const &a, T const &v, types::str const &side)
  {
    bool left = details::issearchsortedleft(side);
    return details::searchsorted(a, v, left);
  }

  namespace
  {
    template <class E, class I0, class I1>
    void _search_sorted(E const &a, I0 ibegin, I0 iend, I1 obegin, bool left,
                        utils::int_<1>)
    {
      for (; ibegin != iend; ++ibegin, ++obegin)
        *obegin = details::searchsorted(a, *ibegin, left);
    }

    template <class E, class I0, class I1, size_t N>
    void _search_sorted(E const &a, I0 ibegin, I0 iend, I1 obegin, bool left,
                        utils::int_<N>)
    {
      for (; ibegin != iend; ++ibegin, ++obegin)
        _search_sorted(a, (*ibegin).begin(), (*ibegin).end(), (*obegin).begin(),
                       left, utils::int_<N - 1>());
    }
  }

  template <class E, class T>
  typename std::enable_if<
      types::is_numexpr_arg<E>::value,
      types::ndarray<long, types::array<long, E::value>>>::type
  searchsorted(T const &a, E const &v, types::str const &side)
  {
    static_assert(T::value == 1,
                  "Not Implemented : searchsorted for dimension != 1");
    bool left = details::issearchsortedleft(side);

    types::ndarray<long, types::array<long, E::value>> out(asarray(v)._shape,
                                                           builtins::None);
    _search_sorted(a, v.begin(), v.end(), out.begin(), left,
                   utils::int_<E::value>());
    return out;
  }
}
PYTHONIC_NS_END

#endif
<|endoftext|>


<commit_before>/*
 *
 * Author: Jeffrey Leung
 * Last edited: 2015-09-19
 *
 * This C++ file tests the Labyrinth class implementation.
 *
 */

#include <iostream>

#include "../include/room_properties.hpp"
#include "../include/room.hpp"
#include "../include/coordinate.hpp"
#include "../include/labyrinth.hpp"

int main()
{
  return 0;
}
<commit_msg>Adding Labyrinth testing implementation.<commit_after>/*
 *
 * Author: Jeffrey Leung
 * Last edited: 2015-09-19
 *
 * This C++ file tests the Labyrinth class implementation.
 *
 */

#include <exception>
#include <iostream>
#include <stdexcept>

#include "../include/room_properties.hpp"
#include "../include/room.hpp"
#include "../include/coordinate.hpp"
#include "../include/labyrinth.hpp"

int main()
{
  std::cout << std::endl
            << "TESTING LABYRINTH.CPP IMPLEMENTATION" << std::endl
            << "________________________________________________" << std::endl
            << std::endl;

  std::cout << "Creating a basic, empty Labyrinth with:" << std::endl
            << "  x size = 3" << std::endl
            << "  y size = 5" << std::endl;

  Labyrinth( 3, 5 );
  std::cout << "Completed." << std::endl;
  std::cout << std::endl;

  std::cout << "Type 1 to attempt to create a Labyrinth with x size = 0."
            << std::endl
            << "Type 2 to attempt to create a Labyrinth with y size = 0."
            << std::endl
            << "Type 3 to attempt to create a Labyrinth with x size = "
            << "y size = 0."
            << std::endl
            << "Type 0 to continue."
            << std::endl
            << "> ";

  char c = ' ';
  while ( c < '0' || c > '3' )
  {
    std::cin >> c;
    switch( c )
    {
      case( '1' ):
        try
        {
          Labyrinth( 0, 10 );
        }
        catch( std::exception& e )
        {
          std::cout << e.what() << std::endl;
        }
        break;

      case( '2' ):
        try
        {
          Labyrinth( 10, 0 );
        }
        catch( std::exception& e )
        {
          std::cout << e.what() << std::endl;
        }
        break;

      case( '3' ):
        try
        {
          Labyrinth( 0, 0 );
        }
        catch( std::exception& e )
        {
          std::cout << e.what() << std::endl;
        }
        break;

      case( '0' ):
        break;

      default:
        std::cout << std::endl
                  << "Error: That was not a valid input; please try again: ";
        break;
    }
  }
  std::cout << std::endl;

  std::cout << "________________________________________________" << std::endl;
  std::cout << std::endl;
  std::cout << "All tests completed." << std::endl;
  std::cout << std::endl;
  std::cout << "Press enter to exit.";
  getchar();
  std::cout << std::endl;

  return 0;
}
<|endoftext|>


<commit_before>#include <iostream>
#include <sstream>
#include <string>
#include "pegmatite.hh"
// This is very bad style, but it's okay for a short example...
using namespace std;
using namespace pegmatite;

/**
 * The AST namespace contains the abstract syntax tree for this grammar.
 */
namespace AST
{
/**
 * The base class for expressions in our language.
 */
class Expression : public ASTContainer
{
public:
	/**
	 * Evaluate this expression.  Returns a `double` representing the result of
	 * the evaluation.
	 */
	virtual double eval() const = 0;
	/**
	 * Print the node, at the specified indent depth.
	 */
	virtual void print(int depth = 0) const = 0;
	PEGMATITE_RTTI(Expression, ASTContainer);
};

/**
 * AST node representing a number.
 */
class Number : public Expression
{
public:
	/**
	 * Construct the numerical value from the text in the input range.
	 */
	virtual void construct(const pegmatite::InputRange &r, pegmatite::ASTStack &st)
	{
		stringstream stream;
		for (char c : r)
		{
			stream << c;
		}
		stream >> value;
	}

	virtual double eval() const
	{
		return value;
	}

	virtual void print(int depth) const
	{
		cout << string(depth, '\t') << value << endl;
	}

private:
	double value;
};

/**
 * Superclass for all of the binary expressions.  Contains pointers to the left
 * and right children.  Subclasses encode the operation type.
 */
class BinaryExpression : public Expression
{
protected:
	/**
	 * The pointers to the left and right nodes.  The `ASTPtr` class will
	 * automatically fill these in when this node is constructed, popping the
	 * two top values from the AST stack.
	 */
	ASTPtr<Expression> left, right;
public:

	virtual void print(int depth) const
	{
		left->print(depth);
		right->print(depth);
	}
};

/**
 * Add expression node.
 */
class AddExpression : public BinaryExpression
{
public:
	virtual double eval() const
	{
		return left->eval() + right->eval();
	}

	virtual void print(int depth) const
	{
		cout << string(depth, '\t') << "+" << endl;
		BinaryExpression::print(depth+1);
	}
};

/**
 * Subtract expression node.
 */
class SubtractExpression : public BinaryExpression
{
public:
	virtual double eval() const
	{
		return left->eval() - right->eval();
	}

	virtual void print(int depth) const
	{
		cout << string(depth, '\t') << "-" << endl;
		BinaryExpression::print(depth+1);
	}
};

/**
 * Multiply expression node.
 */
class MultiplyExpression : public BinaryExpression
{
public:
	virtual double eval() const
	{
		return left->eval() * right->eval();
	}

	virtual void print(int depth) const
	{
		cout << string(depth, '\t') << "*" << endl;
		BinaryExpression::print(depth+1);
	}
};

/**
 * Divide expression node.
 */
class DivideExpression : public BinaryExpression
{
public:
	virtual double eval() const
	{
		double ret = 0;
		double rval = right->eval();
		if (rval != 0)
		{
			ret = left->eval() / rval;
		}
		return ret;
	}

	virtual void print(int depth) const
	{
		cout << string(depth, '\t') << "/" << endl;
		BinaryExpression::print(depth+1);
	}
};
}

namespace Parser
{
/**
 * The (singleton) calculator grammar.
 */
struct CalculatorGrammar
{
	/**
	 * Only spaces are recognised as whitespace in this toy example.
	 */
	Rule ws     = " \t\n"_E;
	/**
	 * Digits are things in the range 0-9.
	 */
	Rule digits  = "[0-9]+"_R;
	/**
	 * Numbers are one or more digits, optionally followed by a decimal point,
	 * and one or more digits, optionally followed by an exponent (which may
	 * also be negative.
	 */
	Rule num    = digits >> -('.'_E >> digits >> -("eE"_S >> -("+-"_S) >> digits));
	/**
	 * Values are either numbers or expressions in brackets (highest precedence).
	 */
	Rule val    = num |  '(' >> expr >> ')';
	/**
	 * Multiply operations are values or multiply, or divide operations,
	 * followed by a multiply symbol, followed by a value.  The sides can never
	 * be add or subtract operations, because they have lower precedence and so
	 * can only be parents of multiply or divide operations (or children via
	 * parenthetical expressions), not direct children.
	 */
	Rule mul_op = mul >> '*' >> val;
	/**
	 * Divide operations follow the same syntax as multiply.
	 */
	Rule div_op = mul >> '/' >> val;
	/**
	 * Multiply-precedence operations are either multiply or divide operations,
	 * or simple values (numbers of parenthetical expressions).
	 */
	Rule mul    = mul_op | div_op | val;
	/**
	 * Add operations can have any expression on the left (including other add
	 * expressions), but only higher-precedence operations on the right.
	 */
	Rule add_op = expr >> '+' >> expr;
	/**
	 * Subtract operations follow the same structure as add.
	 */
	Rule sub_op = expr >> '-' >> expr;
	/**
	 * Expressions can be any of the other types.
	 */
	Rule expr   = add_op | sub_op | mul;

	/**
	 * Returns a singleton instance of this grammar.
	 */
	static const CalculatorGrammar& get()
	{
		static CalculatorGrammar g;
		return g;
	}
	private:
	/**
	 * Private constructor.  This class is immutable, and so only the `get()`
	 * method should be used to return the singleton instance.
	 */
	CalculatorGrammar() {};
};

/**
 * CalculatorParser, constructs an AST from an input string.
 */
class CalculatorParser : public ASTParserDelegate
{
	BindAST<AST::Number> num = CalculatorGrammar::get().num;
	BindAST<AST::AddExpression> add = CalculatorGrammar::get().add_op;
	BindAST<AST::SubtractExpression> sub = CalculatorGrammar::get().sub_op;
	BindAST<AST::MultiplyExpression> mul = CalculatorGrammar::get().mul_op;
	BindAST<AST::DivideExpression> div = CalculatorGrammar::get().div_op;
	public:
	const CalculatorGrammar &g = CalculatorGrammar::get();
};

}

int main()
{
	Parser::CalculatorParser p;
	for (;;)
	{
		string s;

		cout << "enter a math expression (+ - * /, floats, parentheses) or enter to exit:\n";
		getline(cin, s);
		if (s.empty()) break;

		//convert the string to input
		StringInput i(std::move(s));

		//parse
		ErrorList el;
		unique_ptr<AST::Expression> root = 0;
		p.parse(i, p.g.expr, p.g.ws, el, root);

		//on success
		if (root)
		{
			double v = root->eval();
			cout << "success\n";
			cout << "result = " << v << endl;
			cout << "parse tree:\n";
			root->print(0);
		}

		//on error
		else
		{
			cout << "errors: \n";
			for (auto &err : el)
			{
				cout << "line " << err.start.line << ", col " << err.finish.col << ": ";
				wcout << "syntax error" << endl;
			}
		}

		//next input
		cout << endl;
	}
	return 0;
}
<commit_msg>Fix calculator grammar error.<commit_after>#include <iostream>
#include <sstream>
#include <string>
#include "pegmatite.hh"
// This is very bad style, but it's okay for a short example...
using namespace std;
using namespace pegmatite;

/**
 * The AST namespace contains the abstract syntax tree for this grammar.
 */
namespace AST
{
/**
 * The base class for expressions in our language.
 */
class Expression : public ASTContainer
{
public:
	/**
	 * Evaluate this expression.  Returns a `double` representing the result of
	 * the evaluation.
	 */
	virtual double eval() const = 0;
	/**
	 * Print the node, at the specified indent depth.
	 */
	virtual void print(int depth = 0) const = 0;
	PEGMATITE_RTTI(Expression, ASTContainer);
};

/**
 * AST node representing a number.
 */
class Number : public Expression
{
public:
	/**
	 * Construct the numerical value from the text in the input range.
	 */
	virtual void construct(const pegmatite::InputRange &r, pegmatite::ASTStack &st)
	{
		stringstream stream;
		for (char c : r)
		{
			stream << c;
		}
		stream >> value;
	}

	virtual double eval() const
	{
		return value;
	}

	virtual void print(int depth) const
	{
		cout << string(depth, '\t') << value << endl;
	}

private:
	double value;
};

/**
 * Superclass for all of the binary expressions.  Contains pointers to the left
 * and right children.  Subclasses encode the operation type.
 */
class BinaryExpression : public Expression
{
protected:
	/**
	 * The pointers to the left and right nodes.  The `ASTPtr` class will
	 * automatically fill these in when this node is constructed, popping the
	 * two top values from the AST stack.
	 */
	ASTPtr<Expression> left, right;
public:

	virtual void print(int depth) const
	{
		left->print(depth);
		right->print(depth);
	}
};

/**
 * Add expression node.
 */
class AddExpression : public BinaryExpression
{
public:
	virtual double eval() const
	{
		return left->eval() + right->eval();
	}

	virtual void print(int depth) const
	{
		cout << string(depth, '\t') << "+" << endl;
		BinaryExpression::print(depth+1);
	}
};

/**
 * Subtract expression node.
 */
class SubtractExpression : public BinaryExpression
{
public:
	virtual double eval() const
	{
		return left->eval() - right->eval();
	}

	virtual void print(int depth) const
	{
		cout << string(depth, '\t') << "-" << endl;
		BinaryExpression::print(depth+1);
	}
};

/**
 * Multiply expression node.
 */
class MultiplyExpression : public BinaryExpression
{
public:
	virtual double eval() const
	{
		return left->eval() * right->eval();
	}

	virtual void print(int depth) const
	{
		cout << string(depth, '\t') << "*" << endl;
		BinaryExpression::print(depth+1);
	}
};

/**
 * Divide expression node.
 */
class DivideExpression : public BinaryExpression
{
public:
	virtual double eval() const
	{
		double ret = 0;
		double rval = right->eval();
		if (rval != 0)
		{
			ret = left->eval() / rval;
		}
		return ret;
	}

	virtual void print(int depth) const
	{
		cout << string(depth, '\t') << "/" << endl;
		BinaryExpression::print(depth+1);
	}
};
}

namespace Parser
{
/**
 * The (singleton) calculator grammar.
 */
struct CalculatorGrammar
{
	/**
	 * Only spaces are recognised as whitespace in this toy example.
	 */
	Rule ws     = " \t\n"_E;
	/**
	 * Digits are things in the range 0-9.
	 */
	Rule digits  = "[0-9]+"_R;
	/**
	 * Numbers are one or more digits, optionally followed by a decimal point,
	 * and one or more digits, optionally followed by an exponent (which may
	 * also be negative.
	 */
	Rule num    = digits >> -('.'_E >> digits >> -("eE"_S >> -("+-"_S) >> digits));
	/**
	 * Values are either numbers or expressions in brackets (highest precedence).
	 */
	Rule val    = num |  '(' >> expr >> ')';
	/**
	 * Multiply operations are values or multiply, or divide operations,
	 * followed by a multiply symbol, followed by a value.  The sides can never
	 * be add or subtract operations, because they have lower precedence and so
	 * can only be parents of multiply or divide operations (or children via
	 * parenthetical expressions), not direct children.
	 */
	Rule mul_op = mul >> '*' >> expr;
	/**
	 * Divide operations follow the same syntax as multiply.
	 */
	Rule div_op = mul >> '/' >> expr;
	/**
	 * Multiply-precedence operations are either multiply or divide operations,
	 * or simple values (numbers of parenthetical expressions).
	 */
	Rule mul    = mul_op | div_op | val;
	/**
	 * Add operations can have any expression on the left (including other add
	 * expressions), but only higher-precedence operations on the right.
	 */
	Rule add_op = expr >> '+' >> expr;
	/**
	 * Subtract operations follow the same structure as add.
	 */
	Rule sub_op = expr >> '-' >> expr;
	/**
	 * Expressions can be any of the other types.
	 */
	Rule expr   = add_op | sub_op | mul;

	/**
	 * Returns a singleton instance of this grammar.
	 */
	static const CalculatorGrammar& get()
	{
		static CalculatorGrammar g;
		return g;
	}
	private:
	/**
	 * Private constructor.  This class is immutable, and so only the `get()`
	 * method should be used to return the singleton instance.
	 */
	CalculatorGrammar() {};
};

/**
 * CalculatorParser, constructs an AST from an input string.
 */
class CalculatorParser : public ASTParserDelegate
{
	BindAST<AST::Number> num = CalculatorGrammar::get().num;
	BindAST<AST::AddExpression> add = CalculatorGrammar::get().add_op;
	BindAST<AST::SubtractExpression> sub = CalculatorGrammar::get().sub_op;
	BindAST<AST::MultiplyExpression> mul = CalculatorGrammar::get().mul_op;
	BindAST<AST::DivideExpression> div = CalculatorGrammar::get().div_op;
	public:
	const CalculatorGrammar &g = CalculatorGrammar::get();
};

}

int main()
{
	Parser::CalculatorParser p;
	for (;;)
	{
		string s;

		cout << "enter a math expression (+ - * /, floats, parentheses) or enter to exit:\n";
		getline(cin, s);
		if (s.empty()) break;

		//convert the string to input
		StringInput i(std::move(s));

		//parse
		ErrorList el;
		unique_ptr<AST::Expression> root = 0;
		p.parse(i, p.g.expr, p.g.ws, el, root);

		//on success
		if (root)
		{
			double v = root->eval();
			cout << "success\n";
			cout << "result = " << v << endl;
			cout << "parse tree:\n";
			root->print(0);
		}

		//on error
		else
		{
			cout << "errors: \n";
			for (auto &err : el)
			{
				cout << "line " << err.start.line << ", col " << err.finish.col << ": ";
				wcout << "syntax error" << endl;
			}
		}

		//next input
		cout << endl;
	}
	return 0;
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2003 The Regents of The University of Michigan
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met: redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer;
 * redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution;
 * neither the name of the copyright holders nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Authors: Steve Reinhardt
 *          Ron Dreslinski
 *          Ali Saidi
 */

/**
 * @file
 * Definitions of page table.
 */
#include <string>
#include <map>
#include <fstream>

#include "arch/faults.hh"
#include "base/bitfield.hh"
#include "base/intmath.hh"
#include "base/trace.hh"
#include "mem/page_table.hh"
#include "sim/process.hh"
#include "sim/sim_object.hh"
#include "sim/system.hh"

using namespace std;
using namespace TheISA;

PageTable::PageTable(Process *_process, Addr _pageSize)
    : pageSize(_pageSize), offsetMask(mask(floorLog2(_pageSize))),
      process(_process)
{
    assert(isPowerOf2(pageSize));
    pTableCache[0].vaddr = 0;
    pTableCache[1].vaddr = 0;
    pTableCache[2].vaddr = 0;
}

PageTable::~PageTable()
{
}

void
PageTable::allocate(Addr vaddr, int64_t size)
{
    // starting address must be page aligned
    assert(pageOffset(vaddr) == 0);

    DPRINTF(MMU, "Allocating Page: %#x-%#x\n", vaddr, vaddr+ size);

    for (; size > 0; size -= pageSize, vaddr += pageSize) {
        PTableItr iter = pTable.find(vaddr);

        if (iter != pTable.end()) {
            // already mapped
            fatal("PageTable::allocate: address 0x%x already mapped",
                    vaddr);
        }

        pTable[vaddr] = TheISA::TlbEntry(process->M5_pid, vaddr,
                process->system->new_page());
        updateCache(vaddr, pTable[vaddr]);
    }
}

bool
PageTable::lookup(Addr vaddr, TheISA::TlbEntry &entry)
{
    Addr page_addr = pageAlign(vaddr);

    if (pTableCache[0].vaddr == page_addr) {
        entry = pTableCache[0].entry;
        return true;
    }
    if (pTableCache[1].vaddr == page_addr) {
        entry = pTableCache[1].entry;
        return true;
    }
    if (pTableCache[2].vaddr == page_addr) {
        entry = pTableCache[2].entry;
        return true;
    }

    PTableItr iter = pTable.find(page_addr);

    if (iter == pTable.end()) {
        return false;
    }

    updateCache(page_addr, iter->second);
    entry = iter->second;
    return true;
}

bool
PageTable::translate(Addr vaddr, Addr &paddr)
{
    TheISA::TlbEntry entry;
    if (!lookup(vaddr, entry)) {
        DPRINTF(MMU, "Couldn't Translate: %#x\n", vaddr);
        return false;
    }
    paddr = pageOffset(vaddr) + entry.pageStart();
    DPRINTF(MMU, "Translating: %#x->%#x\n", vaddr, paddr);
    return true;
}

Fault
PageTable::translate(RequestPtr req)
{
    Addr paddr;
    assert(pageAlign(req->getVaddr() + req->getSize() - 1)
           == pageAlign(req->getVaddr()));
    if (!translate(req->getVaddr(), paddr)) {
        return Fault(new GenericPageTableFault(req->getVaddr()));
    }
    req->setPaddr(paddr);
    if ((paddr & (pageSize - 1)) + req->getSize() > pageSize) {
        panic("Request spans page boundaries!\n");
        return NoFault;
    }
    return NoFault;
}

void
PageTable::serialize(std::ostream &os)
{
    paramOut(os, "ptable.size", pTable.size());

    int count = 0;

    PTableItr iter = pTable.begin();
    PTableItr end = pTable.end();
    while (iter != end) {
        os << "\n[" << csprintf("%s.Entry%d", name(), count) << "]\n";

        paramOut(os, "vaddr", iter->first);
        iter->second.serialize(os);

        ++iter;
        ++count;
    }
    assert(count == pTable.size());
}

void
PageTable::unserialize(Checkpoint *cp, const std::string &section)
{
    int i = 0, count;
    paramIn(cp, section, "ptable.size", count);
    Addr vaddr;
    TheISA::TlbEntry *entry;

    pTable.clear();

    while(i < count) {
        paramIn(cp, csprintf("%s.Entry%d", name(), i), "vaddr", vaddr);
        entry = new TheISA::TlbEntry();
        entry->unserialize(cp, csprintf("%s.Entry%d", name(), i));
        pTable[vaddr] = *entry;
        ++i;
   }
}

<commit_msg>Checkpointing: Name SE page table entries better so that there isn't a problem if multiple workloads are being run at once.<commit_after>/*
 * Copyright (c) 2003 The Regents of The University of Michigan
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met: redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer;
 * redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution;
 * neither the name of the copyright holders nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Authors: Steve Reinhardt
 *          Ron Dreslinski
 *          Ali Saidi
 */

/**
 * @file
 * Definitions of page table.
 */
#include <string>
#include <map>
#include <fstream>

#include "arch/faults.hh"
#include "base/bitfield.hh"
#include "base/intmath.hh"
#include "base/trace.hh"
#include "mem/page_table.hh"
#include "sim/process.hh"
#include "sim/sim_object.hh"
#include "sim/system.hh"

using namespace std;
using namespace TheISA;

PageTable::PageTable(Process *_process, Addr _pageSize)
    : pageSize(_pageSize), offsetMask(mask(floorLog2(_pageSize))),
      process(_process)
{
    assert(isPowerOf2(pageSize));
    pTableCache[0].vaddr = 0;
    pTableCache[1].vaddr = 0;
    pTableCache[2].vaddr = 0;
}

PageTable::~PageTable()
{
}

void
PageTable::allocate(Addr vaddr, int64_t size)
{
    // starting address must be page aligned
    assert(pageOffset(vaddr) == 0);

    DPRINTF(MMU, "Allocating Page: %#x-%#x\n", vaddr, vaddr+ size);

    for (; size > 0; size -= pageSize, vaddr += pageSize) {
        PTableItr iter = pTable.find(vaddr);

        if (iter != pTable.end()) {
            // already mapped
            fatal("PageTable::allocate: address 0x%x already mapped",
                    vaddr);
        }

        pTable[vaddr] = TheISA::TlbEntry(process->M5_pid, vaddr,
                process->system->new_page());
        updateCache(vaddr, pTable[vaddr]);
    }
}

bool
PageTable::lookup(Addr vaddr, TheISA::TlbEntry &entry)
{
    Addr page_addr = pageAlign(vaddr);

    if (pTableCache[0].vaddr == page_addr) {
        entry = pTableCache[0].entry;
        return true;
    }
    if (pTableCache[1].vaddr == page_addr) {
        entry = pTableCache[1].entry;
        return true;
    }
    if (pTableCache[2].vaddr == page_addr) {
        entry = pTableCache[2].entry;
        return true;
    }

    PTableItr iter = pTable.find(page_addr);

    if (iter == pTable.end()) {
        return false;
    }

    updateCache(page_addr, iter->second);
    entry = iter->second;
    return true;
}

bool
PageTable::translate(Addr vaddr, Addr &paddr)
{
    TheISA::TlbEntry entry;
    if (!lookup(vaddr, entry)) {
        DPRINTF(MMU, "Couldn't Translate: %#x\n", vaddr);
        return false;
    }
    paddr = pageOffset(vaddr) + entry.pageStart();
    DPRINTF(MMU, "Translating: %#x->%#x\n", vaddr, paddr);
    return true;
}

Fault
PageTable::translate(RequestPtr req)
{
    Addr paddr;
    assert(pageAlign(req->getVaddr() + req->getSize() - 1)
           == pageAlign(req->getVaddr()));
    if (!translate(req->getVaddr(), paddr)) {
        return Fault(new GenericPageTableFault(req->getVaddr()));
    }
    req->setPaddr(paddr);
    if ((paddr & (pageSize - 1)) + req->getSize() > pageSize) {
        panic("Request spans page boundaries!\n");
        return NoFault;
    }
    return NoFault;
}

void
PageTable::serialize(std::ostream &os)
{
    paramOut(os, "ptable.size", pTable.size());

    int count = 0;

    PTableItr iter = pTable.begin();
    PTableItr end = pTable.end();
    while (iter != end) {
        os << "\n[" << csprintf("%s.Entry%d", process->name(), count) << "]\n";

        paramOut(os, "vaddr", iter->first);
        iter->second.serialize(os);

        ++iter;
        ++count;
    }
    assert(count == pTable.size());
}

void
PageTable::unserialize(Checkpoint *cp, const std::string &section)
{
    int i = 0, count;
    paramIn(cp, section, "ptable.size", count);
    Addr vaddr;
    TheISA::TlbEntry *entry;

    pTable.clear();

    while(i < count) {
        paramIn(cp, csprintf("%s.Entry%d", process->name(), i), "vaddr", vaddr);
        entry = new TheISA::TlbEntry();
        entry->unserialize(cp, csprintf("%s.Entry%d", process->name(), i));
        pTable[vaddr] = *entry;
        ++i;
   }
}

<|endoftext|>


<commit_before>//
// Copyright (c) 2012 Daniel Lundin
//

#include "medida/timer.h"

#include <iostream>
#include <thread>

#include <gtest/gtest.h>

#include "medida/metrics_registry.h"

using namespace medida;

struct TimerTest : public ::testing::Test {
  Timer timer;
};

TEST_F(TimerTest, hasDurationUnit) {
  EXPECT_EQ(std::chrono::milliseconds(1), timer.duration_unit());
}

TEST_F(TimerTest, hasRateUnit) {
  EXPECT_EQ(std::chrono::seconds(1), timer.rate_unit());
}

TEST_F(TimerTest, createFromRegistry) {
  MetricsRegistry registry {};
  auto& timer2 = registry.NewTimer({"a", "b", "c"});
  EXPECT_EQ(0, timer2.count());
}

TEST_F(TimerTest, aBlankTimer) {
  EXPECT_EQ(0, timer.count());
  EXPECT_NEAR(0.0, timer.min(), 0.001);
  EXPECT_NEAR(0.0, timer.max(), 0.001);
  EXPECT_NEAR(0.0, timer.mean(), 0.001);
  EXPECT_NEAR(0.0, timer.std_dev(), 0.001);
  EXPECT_NEAR(0.0, timer.mean_rate(), 0.001);
  EXPECT_NEAR(0.0, timer.one_minute_rate(), 0.001);
  EXPECT_NEAR(0.0, timer.five_minute_rate(), 0.001);
  EXPECT_NEAR(0.0, timer.fifteen_minute_rate(), 0.001);

  auto snapshot = timer.GetSnapshot();
  EXPECT_NEAR(0.0, snapshot.getMedian(), 0.001);
  EXPECT_NEAR(0.0, snapshot.get75thPercentile(), 0.001);
  EXPECT_NEAR(0.0, snapshot.get99thPercentile(), 0.001);
  EXPECT_EQ(0, snapshot.size());
}

TEST_F(TimerTest, timingASeriesOfEvents) {
  timer.Update(std::chrono::milliseconds(10));
  timer.Update(std::chrono::milliseconds(20));
  timer.Update(std::chrono::milliseconds(20));
  timer.Update(std::chrono::milliseconds(30));
  timer.Update(std::chrono::milliseconds(40));

  EXPECT_EQ(5, timer.count());
  EXPECT_NEAR(10.0, timer.min(), 0.001);
  EXPECT_NEAR(40.0, timer.max(), 0.001);
  EXPECT_NEAR(24.0, timer.mean(), 0.001);
  EXPECT_NEAR(11.401, timer.std_dev(), 0.001);

  auto snapshot = timer.GetSnapshot();
  EXPECT_NEAR(20.0, snapshot.getMedian(), 0.001);
  EXPECT_NEAR(35.0, snapshot.get75thPercentile(), 0.001);
  EXPECT_NEAR(40.0, snapshot.get99thPercentile(), 0.001);
  EXPECT_EQ(5, snapshot.size());
}

TEST_F(TimerTest, timingVariantValues) {
  timer.Update(std::chrono::nanoseconds(9223372036854775807));  // INT64_MAX
  timer.Update(std::chrono::nanoseconds(0));
  EXPECT_NEAR(6.521908912666392E12, timer.std_dev(), 0.001);
}

TEST_F(TimerTest, timerTimeScope) {
  {
    auto t = timer.TimeScope();
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }
  {
    auto t = timer.TimeScope();
    std::this_thread::sleep_for(std::chrono::milliseconds(200));
  }
  EXPECT_EQ(2, timer.count());
  EXPECT_NEAR(150.0, timer.mean(), 0.5);
}

void my_func() {
  std::this_thread::sleep_for(std::chrono::milliseconds(100));
}

TEST_F(TimerTest, timerTimeFunction) {
  timer.Time(my_func);
  EXPECT_EQ(1, timer.count());
  EXPECT_NEAR(100.0, timer.mean(), 0.5);
}

TEST_F(TimerTest, timerTimeLambda) {
  timer.Time([]() {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  });
  EXPECT_EQ(1, timer.count());
  EXPECT_NEAR(100.0, timer.mean(), 1.0);
}
<commit_msg>make timer test pass on sliding windows<commit_after>//
// Copyright (c) 2012 Daniel Lundin
//

#include "medida/timer.h"

#include <iostream>
#include <thread>

#include <gtest/gtest.h>

#include "medida/metrics_registry.h"

using namespace medida;

struct TimerTest : public ::testing::Test {
  Timer timer;
};

TEST_F(TimerTest, hasDurationUnit) {
  EXPECT_EQ(std::chrono::milliseconds(1), timer.duration_unit());
}

TEST_F(TimerTest, hasRateUnit) {
  EXPECT_EQ(std::chrono::seconds(1), timer.rate_unit());
}

TEST_F(TimerTest, createFromRegistry) {
  MetricsRegistry registry {};
  auto& timer2 = registry.NewTimer({"a", "b", "c"});
  EXPECT_EQ(0, timer2.count());
}

TEST_F(TimerTest, aBlankTimer) {
  EXPECT_EQ(0, timer.count());
  EXPECT_NEAR(0.0, timer.min(), 0.001);
  EXPECT_NEAR(0.0, timer.max(), 0.001);
  EXPECT_NEAR(0.0, timer.mean(), 0.001);
  EXPECT_NEAR(0.0, timer.std_dev(), 0.001);
  EXPECT_NEAR(0.0, timer.mean_rate(), 0.001);
  EXPECT_NEAR(0.0, timer.one_minute_rate(), 0.001);
  EXPECT_NEAR(0.0, timer.five_minute_rate(), 0.001);
  EXPECT_NEAR(0.0, timer.fifteen_minute_rate(), 0.001);

  auto snapshot = timer.GetSnapshot();
  EXPECT_NEAR(0.0, snapshot.getMedian(), 0.001);
  EXPECT_NEAR(0.0, snapshot.get75thPercentile(), 0.001);
  EXPECT_NEAR(0.0, snapshot.get99thPercentile(), 0.001);
  EXPECT_EQ(0, snapshot.size());
}

TEST_F(TimerTest, timingASeriesOfEvents) {
  // Need to sleep between events so sliding window assigns
  // them to separate timeslices.
  timer.Update(std::chrono::milliseconds(10));
  std::this_thread::sleep_for(std::chrono::milliseconds(500));
  timer.Update(std::chrono::milliseconds(20));
  std::this_thread::sleep_for(std::chrono::milliseconds(500));
  timer.Update(std::chrono::milliseconds(20));
  std::this_thread::sleep_for(std::chrono::milliseconds(500));
  timer.Update(std::chrono::milliseconds(30));
  std::this_thread::sleep_for(std::chrono::milliseconds(500));
  timer.Update(std::chrono::milliseconds(40));

  EXPECT_EQ(5, timer.count());
  EXPECT_NEAR(10.0, timer.min(), 0.001);
  EXPECT_NEAR(40.0, timer.max(), 0.001);
  EXPECT_NEAR(24.0, timer.mean(), 0.001);
  EXPECT_NEAR(11.401, timer.std_dev(), 0.001);

  auto snapshot = timer.GetSnapshot();
  EXPECT_NEAR(20.0, snapshot.getMedian(), 0.001);
  EXPECT_NEAR(35.0, snapshot.get75thPercentile(), 0.001);
  EXPECT_NEAR(40.0, snapshot.get99thPercentile(), 0.001);
  EXPECT_EQ(5, snapshot.size());
}

TEST_F(TimerTest, timingVariantValues) {
  timer.Update(std::chrono::nanoseconds(9223372036854775807));  // INT64_MAX
  timer.Update(std::chrono::nanoseconds(0));
  EXPECT_NEAR(6.521908912666392E12, timer.std_dev(), 0.001);
}

TEST_F(TimerTest, timerTimeScope) {
  {
    auto t = timer.TimeScope();
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }
  {
    auto t = timer.TimeScope();
    std::this_thread::sleep_for(std::chrono::milliseconds(200));
  }
  EXPECT_EQ(2, timer.count());
  EXPECT_NEAR(150.0, timer.mean(), 0.5);
}

void my_func() {
  std::this_thread::sleep_for(std::chrono::milliseconds(100));
}

TEST_F(TimerTest, timerTimeFunction) {
  timer.Time(my_func);
  EXPECT_EQ(1, timer.count());
  EXPECT_NEAR(100.0, timer.mean(), 0.5);
}

TEST_F(TimerTest, timerTimeLambda) {
  timer.Time([]() {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  });
  EXPECT_EQ(1, timer.count());
  EXPECT_NEAR(100.0, timer.mean(), 1.0);
}
<|endoftext|>


<commit_before>#include "mex.h"
#include <Eigen/Dense>
#include <vector>
#include <iostream>
#include "drakeUtil.h"
#include "RigidBodyManipulator.h"

#define INF -2147483648

using namespace Eigen;
using namespace std;

// convert Matlab cell array of strings into a C++ vector of strings
vector<string> get_strings(const mxArray *rhs) {
  int num = mxGetNumberOfElements(rhs);
  vector<string> strings(num);
  for (int i=0; i<num; i++) {
    const mxArray *ptr = mxGetCell(rhs,i);
    int buflen = mxGetN(ptr)*sizeof(mxChar)+1;
    char* str = (char*)mxMalloc(buflen);
    mxGetString(ptr, str, buflen);
    strings[i] = string(str);
    mxFree(str);
  }
  return strings;
}

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
  //DEBUG
  //cout << "constructModelmex: START" << endl;
  //END_DEBUG
  char buf[100];
  mxArray *pm;

  if (nrhs!=1) {
    mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","Usage model_ptr = constructModelmex(obj)");
  }

  RigidBodyManipulator *model=NULL;

//  model->robot_name = get_strings(mxGetProperty(prhs[0],0,"name"));

  const mxArray* featherstone = mxGetProperty(prhs[0],0,"featherstone");
  if (!featherstone) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs", "the featherstone array is invalid");

  const mxArray* pBodies = mxGetProperty(prhs[0],0,"body");
  if (!pBodies) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the body array is invalid");
  int num_bodies = mxGetNumberOfElements(pBodies);

  const mxArray* pFrames = mxGetProperty(prhs[0],0,"frame");
  if (!pFrames) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the frame array is invalid");
  int num_frames = mxGetNumberOfElements(pFrames);

  // set up the model
  pm = mxGetField(featherstone,0,"NB");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.NB.  Are you passing in the correct structure?");
  model = new RigidBodyManipulator((int) mxGetScalar(pm), (int) mxGetScalar(pm), num_bodies, num_frames);

  pm = mxGetField(featherstone,0,"parent");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.parent.");
  double* parent = mxGetPr(pm);

  pm = mxGetField(featherstone,0,"pitch");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.pitch.");
  double* pitch = mxGetPr(pm);

  pm = mxGetField(featherstone,0,"dofnum");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.dofnum.");
  double* dofnum = mxGetPr(pm);

  pm = mxGetField(featherstone,0,"damping");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.damping.");
  memcpy(model->damping,mxGetPr(pm),sizeof(double)*model->NB);

  pm = mxGetField(featherstone,0,"coulomb_friction");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.coulomb_friction.");
  memcpy(model->coulomb_friction,mxGetPr(pm),sizeof(double)*model->NB);

  pm = mxGetField(featherstone,0,"static_friction");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.static_friction.");
  memcpy(model->static_friction,mxGetPr(pm),sizeof(double)*model->NB);

  pm = mxGetField(featherstone,0,"coulomb_window");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.coulomb_window.");
  memcpy(model->coulomb_window,mxGetPr(pm),sizeof(double)*model->NB);

  mxArray* pXtree = mxGetField(featherstone,0,"Xtree");
  if (!pXtree) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.Xtree.");

  mxArray* pI = mxGetField(featherstone,0,"I");
  if (!pI) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.I.");

  for (int i=0; i<model->NB; i++) {
    model->parent[i] = ((int) parent[i]) - 1;  // since it will be used as a C index
    model->pitch[i] = (int) pitch[i];
    model->dofnum[i] = (int) dofnum[i] - 1; // zero-indexed

    mxArray* pXtreei = mxGetCell(pXtree,i);
    if (!pXtreei) mexErrMsgIdAndTxt("Drake:HandCpmex:BadInputs","can't access model.featherstone.Xtree{%d}",i);

    // todo: check that the size is 6x6
    memcpy(model->Xtree[i].data(),mxGetPr(pXtreei),sizeof(double)*6*6);

    mxArray* pIi = mxGetCell(pI,i);
    if (!pIi) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't access model.featherstone.I{%d}",i);

    // todo: check that the size is 6x6
    memcpy(model->I[i].data(),mxGetPr(pIi),sizeof(double)*6*6);
  }

  for (int i=0; i<model->num_bodies; i++) {
    //DEBUG
    //cout << "constructModelmex: body " << i << endl;
    //END_DEBUG
    pm = mxGetProperty(pBodies,i,"linkname");
    mxGetString(pm,buf,100);
    model->bodies[i].linkname.assign(buf,strlen(buf));

    pm = mxGetProperty(pBodies,i,"jointname");
    mxGetString(pm,buf,100);
    model->bodies[i].jointname.assign(buf,strlen(buf));

    pm = mxGetProperty(pBodies,i,"robotnum");
    model->bodies[i].robotnum = (int) mxGetScalar(pm)-1;

    pm = mxGetProperty(pBodies,i,"mass");
    model->bodies[i].mass = (double) mxGetScalar(pm);

    pm = mxGetProperty(pBodies,i,"com");
    if (!mxIsEmpty(pm)) memcpy(model->bodies[i].com.data(),mxGetPr(pm),sizeof(double)*3);

    pm = mxGetProperty(pBodies,i,"dofnum");
    model->bodies[i].dofnum = (int) mxGetScalar(pm) - 1;  //zero-indexed

    pm = mxGetProperty(pBodies,i,"floating");
    model->bodies[i].floating = (int) mxGetScalar(pm);

    pm = mxGetProperty(pBodies,i,"pitch");
    model->bodies[i].pitch = (int) mxGetScalar(pm);

    pm = mxGetProperty(pBodies,i,"parent");
    if (!pm || mxIsEmpty(pm))
    	model->bodies[i].parent = -1;
    else
    	model->bodies[i].parent = mxGetScalar(pm) - 1;

//     if (model->bodies[i].dofnum>=0) {
//       pm = mxGetProperty(pBodies,i,"joint_limit_min");
//       model->joint_limit_min[model->bodies[i].dofnum] = mxGetScalar(pm);
//       pm = mxGetProperty(pBodies,i,"joint_limit_max");
//       model->joint_limit_max[model->bodies[i].dofnum] = mxGetScalar(pm);
//     }

    pm = mxGetProperty(pBodies,i,"Ttree");
    // todo: check that the size is 4x4
    memcpy(model->bodies[i].Ttree.data(),mxGetPr(pm),sizeof(double)*4*4);

    pm = mxGetProperty(pBodies,i,"T_body_to_joint");
    memcpy(model->bodies[i].T_body_to_joint.data(),mxGetPr(pm),sizeof(double)*4*4);

    //DEBUG
    //cout << "constructModelmex: About to parse collision geometry"  << endl;
    //END_DEBUG
    pm = mxGetProperty(pBodies,i,"contact_shapes");
    Matrix4d T;
    if (!mxIsEmpty(pm)){
      for (int j=0; j<mxGetNumberOfElements(pm); j++) {
        //DEBUG
        //cout << "constructModelmex: Body " << i << ", Element " << j << endl;
        //END_DEBUG
        mxArray* pShape = mxGetCell(pm,j);
        auto shape = (DrakeCollision::Shape)mxGetScalar(mxGetProperty(pShape,0,"bullet_shape_id"));
        vector<double> params_vec;
        switch (shape) {
          case DrakeCollision::BOX:
          {
            double* params = mxGetPr(mxGetProperty(pShape,0,"size"));
            params_vec.push_back(params[0]);
            params_vec.push_back(params[1]);
            params_vec.push_back(params[2]);
          }
            break;
          case DrakeCollision::SPHERE:
          {
            params_vec.push_back(*mxGetPr(mxGetProperty(pShape,0,"radius")));
          }
            break;
          case DrakeCollision::CYLINDER:
          {
            params_vec.push_back(*mxGetPr(mxGetProperty(pShape,0,"radius")));
            params_vec.push_back(*mxGetPr(mxGetProperty(pShape,0,"len")));
          }
            break;
          case DrakeCollision::MESH:
          {
            mxArray* pPoints;
            mexCallMATLAB(1,&pPoints,1,&pShape,"getPoints");
            double* params = mxGetPr(pPoints);
            int n_params = (int) mxGetNumberOfElements(pPoints);
            for (int k=0; k<n_params; k++)
              params_vec.push_back(params[k]);
            mxDestroyArray(pPoints);
          }
            break;
          case DrakeCollision::CAPSULE:
          {
            params_vec.push_back(*mxGetPr(mxGetProperty(pShape,0,"radius")));
            params_vec.push_back(*mxGetPr(mxGetProperty(pShape,0,"len")));
          }
            break;
          default:
            // intentionally do nothing..
            break;
        }
        memcpy(T.data(), mxGetPr(mxGetProperty(pShape,0,"T")), sizeof(double)*4*4);
        model->addCollisionElement(i,T,shape,params_vec);
        if (model->bodies[i].parent<0) {
          model->updateCollisionElements(i);  // update static objects only once - right here on load
        }
      }

      // Set collision filtering bitmasks
      pm = mxGetProperty(pBodies,i,"collision_filter");
      const uint16_t* group = (uint16_t*)mxGetPr(mxGetField(pm,0,"belongs_to"));
      const uint16_t* mask = (uint16_t*)mxGetPr(mxGetField(pm,0,"collides_with"));
      //DEBUG
      //cout << "constructModelmex: Group: " << *group << endl;
      //cout << "constructModelmex: Mask " << *mask << endl;
      //END_DEBUG
      model->setCollisionFilter(i,*group,*mask);
    }
  }

  for (int i=0; i<model->num_frames; i++) {
    pm = mxGetProperty(pFrames,i,"name");
    mxGetString(pm,buf,100);
    model->frames[i].name.assign(buf,strlen(buf));

    pm = mxGetProperty(pFrames,i,"body_ind");
    model->frames[i].body_ind = (int) mxGetScalar(pm)-1;

    pm = mxGetProperty(pFrames,i,"T");
    memcpy(model->frames[i].T.data(),mxGetPr(pm),sizeof(double)*4*4);
  }

  memcpy(model->joint_limit_min, mxGetPr(mxGetProperty(prhs[0],0,"joint_limit_min")), sizeof(double)*model->num_dof);
  memcpy(model->joint_limit_max, mxGetPr(mxGetProperty(prhs[0],0,"joint_limit_max")), sizeof(double)*model->num_dof);

  const mxArray* a_grav_array = mxGetProperty(prhs[0],0,"gravity");
  if (a_grav_array && mxGetNumberOfElements(a_grav_array)==3) {
    double* p = mxGetPr(a_grav_array);
    model->a_grav[3] = p[0];
    model->a_grav[4] = p[1];
    model->a_grav[5] = p[2];
  } else {
    mexErrMsgIdAndTxt("Drake:constructModelmex:BadGravity","Couldn't find a 3 element gravity vector in the object.");
  }

  model->compile();

  if (nlhs>0) {  // return a pointer to the model
  	plhs[0] = createDrakeMexPointer((void*)model,"deleteModelmex","RigidBodyManipulator");
  }

}
<commit_msg>Populate cpp RigidBody contact_pts<commit_after>#include "mex.h"
#include <Eigen/Dense>
#include <vector>
#include <iostream>
#include "drakeUtil.h"
#include "RigidBodyManipulator.h"

#define INF -2147483648

using namespace Eigen;
using namespace std;

// convert Matlab cell array of strings into a C++ vector of strings
vector<string> get_strings(const mxArray *rhs) {
  int num = mxGetNumberOfElements(rhs);
  vector<string> strings(num);
  for (int i=0; i<num; i++) {
    const mxArray *ptr = mxGetCell(rhs,i);
    int buflen = mxGetN(ptr)*sizeof(mxChar)+1;
    char* str = (char*)mxMalloc(buflen);
    mxGetString(ptr, str, buflen);
    strings[i] = string(str);
    mxFree(str);
  }
  return strings;
}

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
  //DEBUG
  //cout << "constructModelmex: START" << endl;
  //END_DEBUG
  char buf[100];
  mxArray *pm;

  if (nrhs!=1) {
    mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","Usage model_ptr = constructModelmex(obj)");
  }

  RigidBodyManipulator *model=NULL;

//  model->robot_name = get_strings(mxGetProperty(prhs[0],0,"name"));

  const mxArray* featherstone = mxGetProperty(prhs[0],0,"featherstone");
  if (!featherstone) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs", "the featherstone array is invalid");

  const mxArray* pBodies = mxGetProperty(prhs[0],0,"body");
  if (!pBodies) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the body array is invalid");
  int num_bodies = mxGetNumberOfElements(pBodies);

  const mxArray* pFrames = mxGetProperty(prhs[0],0,"frame");
  if (!pFrames) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the frame array is invalid");
  int num_frames = mxGetNumberOfElements(pFrames);

  // set up the model
  pm = mxGetField(featherstone,0,"NB");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.NB.  Are you passing in the correct structure?");
  model = new RigidBodyManipulator((int) mxGetScalar(pm), (int) mxGetScalar(pm), num_bodies, num_frames);

  pm = mxGetField(featherstone,0,"parent");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.parent.");
  double* parent = mxGetPr(pm);

  pm = mxGetField(featherstone,0,"pitch");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.pitch.");
  double* pitch = mxGetPr(pm);

  pm = mxGetField(featherstone,0,"dofnum");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.dofnum.");
  double* dofnum = mxGetPr(pm);

  pm = mxGetField(featherstone,0,"damping");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.damping.");
  memcpy(model->damping,mxGetPr(pm),sizeof(double)*model->NB);

  pm = mxGetField(featherstone,0,"coulomb_friction");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.coulomb_friction.");
  memcpy(model->coulomb_friction,mxGetPr(pm),sizeof(double)*model->NB);

  pm = mxGetField(featherstone,0,"static_friction");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.static_friction.");
  memcpy(model->static_friction,mxGetPr(pm),sizeof(double)*model->NB);

  pm = mxGetField(featherstone,0,"coulomb_window");
  if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.coulomb_window.");
  memcpy(model->coulomb_window,mxGetPr(pm),sizeof(double)*model->NB);

  mxArray* pXtree = mxGetField(featherstone,0,"Xtree");
  if (!pXtree) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.Xtree.");

  mxArray* pI = mxGetField(featherstone,0,"I");
  if (!pI) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.I.");

  for (int i=0; i<model->NB; i++) {
    model->parent[i] = ((int) parent[i]) - 1;  // since it will be used as a C index
    model->pitch[i] = (int) pitch[i];
    model->dofnum[i] = (int) dofnum[i] - 1; // zero-indexed

    mxArray* pXtreei = mxGetCell(pXtree,i);
    if (!pXtreei) mexErrMsgIdAndTxt("Drake:HandCpmex:BadInputs","can't access model.featherstone.Xtree{%d}",i);

    // todo: check that the size is 6x6
    memcpy(model->Xtree[i].data(),mxGetPr(pXtreei),sizeof(double)*6*6);

    mxArray* pIi = mxGetCell(pI,i);
    if (!pIi) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't access model.featherstone.I{%d}",i);

    // todo: check that the size is 6x6
    memcpy(model->I[i].data(),mxGetPr(pIi),sizeof(double)*6*6);
  }

  for (int i=0; i<model->num_bodies; i++) {
    //DEBUG
    //cout << "constructModelmex: body " << i << endl;
    //END_DEBUG
    pm = mxGetProperty(pBodies,i,"linkname");
    mxGetString(pm,buf,100);
    model->bodies[i].linkname.assign(buf,strlen(buf));

    pm = mxGetProperty(pBodies,i,"jointname");
    mxGetString(pm,buf,100);
    model->bodies[i].jointname.assign(buf,strlen(buf));

    pm = mxGetProperty(pBodies,i,"robotnum");
    model->bodies[i].robotnum = (int) mxGetScalar(pm)-1;

    pm = mxGetProperty(pBodies,i,"mass");
    model->bodies[i].mass = (double) mxGetScalar(pm);

    pm = mxGetProperty(pBodies,i,"com");
    if (!mxIsEmpty(pm)) memcpy(model->bodies[i].com.data(),mxGetPr(pm),sizeof(double)*3);

    pm = mxGetProperty(pBodies,i,"dofnum");
    model->bodies[i].dofnum = (int) mxGetScalar(pm) - 1;  //zero-indexed

    pm = mxGetProperty(pBodies,i,"floating");
    model->bodies[i].floating = (int) mxGetScalar(pm);

    pm = mxGetProperty(pBodies,i,"pitch");
    model->bodies[i].pitch = (int) mxGetScalar(pm);

    pm = mxGetProperty(pBodies,i,"parent");
    if (!pm || mxIsEmpty(pm))
    	model->bodies[i].parent = -1;
    else
    	model->bodies[i].parent = mxGetScalar(pm) - 1;

//     if (model->bodies[i].dofnum>=0) {
//       pm = mxGetProperty(pBodies,i,"joint_limit_min");
//       model->joint_limit_min[model->bodies[i].dofnum] = mxGetScalar(pm);
//       pm = mxGetProperty(pBodies,i,"joint_limit_max");
//       model->joint_limit_max[model->bodies[i].dofnum] = mxGetScalar(pm);
//     }

    pm = mxGetProperty(pBodies,i,"Ttree");
    // todo: check that the size is 4x4
    memcpy(model->bodies[i].Ttree.data(),mxGetPr(pm),sizeof(double)*4*4);

    pm = mxGetProperty(pBodies,i,"T_body_to_joint");
    memcpy(model->bodies[i].T_body_to_joint.data(),mxGetPr(pm),sizeof(double)*4*4);

    //DEBUG
    //cout << "constructModelmex: About to parse collision geometry"  << endl;
    //END_DEBUG
    pm = mxGetProperty(pBodies,i,"contact_shapes");
    Matrix4d T;
    if (!mxIsEmpty(pm)){
      for (int j=0; j<mxGetNumberOfElements(pm); j++) {
        //DEBUG
        //cout << "constructModelmex: Body " << i << ", Element " << j << endl;
        //END_DEBUG
        mxArray* pShape = mxGetCell(pm,j);
        auto shape = (DrakeCollision::Shape)mxGetScalar(mxGetProperty(pShape,0,"bullet_shape_id"));
        vector<double> params_vec;
        switch (shape) {
          case DrakeCollision::BOX:
          {
            double* params = mxGetPr(mxGetProperty(pShape,0,"size"));
            params_vec.push_back(params[0]);
            params_vec.push_back(params[1]);
            params_vec.push_back(params[2]);
          }
            break;
          case DrakeCollision::SPHERE:
          {
            params_vec.push_back(*mxGetPr(mxGetProperty(pShape,0,"radius")));
          }
            break;
          case DrakeCollision::CYLINDER:
          {
            params_vec.push_back(*mxGetPr(mxGetProperty(pShape,0,"radius")));
            params_vec.push_back(*mxGetPr(mxGetProperty(pShape,0,"len")));
          }
            break;
          case DrakeCollision::MESH:
          {
            mxArray* pPoints;
            mexCallMATLAB(1,&pPoints,1,&pShape,"getPoints");
            double* params = mxGetPr(pPoints);
            int n_params = (int) mxGetNumberOfElements(pPoints);
            for (int k=0; k<n_params; k++)
              params_vec.push_back(params[k]);
            mxDestroyArray(pPoints);
          }
            break;
          case DrakeCollision::CAPSULE:
          {
            params_vec.push_back(*mxGetPr(mxGetProperty(pShape,0,"radius")));
            params_vec.push_back(*mxGetPr(mxGetProperty(pShape,0,"len")));
          }
            break;
          default:
            // intentionally do nothing..
            break;
        }
        memcpy(T.data(), mxGetPr(mxGetProperty(pShape,0,"T")), sizeof(double)*4*4);
        model->addCollisionElement(i,T,shape,params_vec);
        if (model->bodies[i].parent<0) {
          model->updateCollisionElements(i);  // update static objects only once - right here on load
        }
      }

      // Set collision filtering bitmasks
      pm = mxGetProperty(pBodies,i,"collision_filter");
      const uint16_t* group = (uint16_t*)mxGetPr(mxGetField(pm,0,"belongs_to"));
      const uint16_t* mask = (uint16_t*)mxGetPr(mxGetField(pm,0,"collides_with"));
      //DEBUG
      //cout << "constructModelmex: Group: " << *group << endl;
      //cout << "constructModelmex: Mask " << *mask << endl;
      //END_DEBUG
      model->setCollisionFilter(i,*group,*mask);
    }
  }

  // THIS IS UGLY: I'm sending the terrain contact points into the
  // contact_pts field of the cpp RigidBody objects
  //DEBUG
  //cout << "constructModelmex: Parsing contact points " << endl;
  //cout << "constructModelmex: Get struct" << endl;
  //END_DEBUG
  mxArray* contact_pts_struct[1];
  if (~mexCallMATLAB(1,contact_pts_struct,1,const_cast<mxArray**>(&prhs[0]),"getTerrainContactPoints")) {
    //DEBUG
    //cout << "constructModelmex: Got struct" << endl;
    //if (contact_pts_struct) {
    //cout << "constructModelmex: Struct pointer: " << contact_pts_struct << endl;
    //} else {
    //cout << "constructModelmex: Struct pointer NULL" << endl;
    //}
    //cout << "constructModelmex: Get numel of struct" << endl;
    //END_DEBUG
    const int n_bodies_w_contact_pts = mxGetNumberOfElements(contact_pts_struct[0]);
    //DEBUG
    //cout << "constructModelmex: Got numel of struct:" << n_bodies_w_contact_pts << endl;
    //END_DEBUG
    mxArray* pPts;
    int body_idx;
    int n_pts;
    for (int j=0; j < n_bodies_w_contact_pts; j++) {
      //DEBUG
      //cout << "constructModelmex: Loop: Iteration " << j << endl;
      //cout << "constructModelmex: Get body_idx" << endl;
      //END_DEBUG
      body_idx = (int) mxGetScalar(mxGetField(contact_pts_struct[0],j,"idx")) - 1;
      //DEBUG
      //cout << "constructModelmex: Got body_idx: " << body_idx << endl;
      //cout << "constructModelmex: Get points" << endl;
      //END_DEBUG
      pPts = mxGetField(contact_pts_struct[0],j,"pts");
      //DEBUG
      //cout << "constructModelmex: Get points" << endl;
      //cout << "constructModelmex: Get number of points" << endl;
      //END_DEBUG
      n_pts = mxGetN(pPts);
      //DEBUG
      //cout << "constructModelmex: Got number of points: " << n_pts << endl;
      //cout << "constructModelmex: Set contact_pts of body" << endl;
      //END_DEBUG
      Map<MatrixXd> pts(mxGetPr(pPts),3,n_pts);
      model->bodies[body_idx].contact_pts.resize(4,n_pts);
      model->bodies[body_idx].contact_pts << pts, MatrixXd::Ones(1,n_pts);
      //DEBUG
      //cout << "constructModelmex: Contact_pts of body: " << endl;
      //cout << model->bodies[body_idx].contact_pts << endl;
      //END_DEBUG
    }
  }

  for (int i=0; i<model->num_frames; i++) {
    pm = mxGetProperty(pFrames,i,"name");
    mxGetString(pm,buf,100);
    model->frames[i].name.assign(buf,strlen(buf));

    pm = mxGetProperty(pFrames,i,"body_ind");
    model->frames[i].body_ind = (int) mxGetScalar(pm)-1;

    pm = mxGetProperty(pFrames,i,"T");
    memcpy(model->frames[i].T.data(),mxGetPr(pm),sizeof(double)*4*4);
  }

  memcpy(model->joint_limit_min, mxGetPr(mxGetProperty(prhs[0],0,"joint_limit_min")), sizeof(double)*model->num_dof);
  memcpy(model->joint_limit_max, mxGetPr(mxGetProperty(prhs[0],0,"joint_limit_max")), sizeof(double)*model->num_dof);

  const mxArray* a_grav_array = mxGetProperty(prhs[0],0,"gravity");
  if (a_grav_array && mxGetNumberOfElements(a_grav_array)==3) {
    double* p = mxGetPr(a_grav_array);
    model->a_grav[3] = p[0];
    model->a_grav[4] = p[1];
    model->a_grav[5] = p[2];
  } else {
    mexErrMsgIdAndTxt("Drake:constructModelmex:BadGravity","Couldn't find a 3 element gravity vector in the object.");
  }

  model->compile();

  if (nlhs>0) {  // return a pointer to the model
  	plhs[0] = createDrakeMexPointer((void*)model,"deleteModelmex","RigidBodyManipulator");
  }

}
<|endoftext|>


<commit_before>{{#global}}
#include <SPI.h>
#include <Ethernet2.h>
{{/global}}

struct State {
};

{{ GENERATED_CODE }}

void evaluate(Context ctx) {
    if (!isInputDirty<input_SND>(ctx))
        return;

    auto client = EthernetClient(getValue<input_SOCK>(ctx));
    auto line = getValue<input_LINE>(ctx);

    size_t lastWriteSize;

    for (auto it = line->iterate(); it; ++it) {
        lastWriteSize = client.write((char)*it);
        if (lastWriteSize == 0) {
            emitValue<output_ERR>(ctx, 1);
            return;
        }
    }

    client.flush();

    emitValue<output_DONE>(ctx, 1);
}
<commit_msg>fix(stdlib): fix wrong input name in implementation of xod-dev/w5500/send<commit_after>{{#global}}
#include <SPI.h>
#include <Ethernet2.h>
{{/global}}

struct State {
};

{{ GENERATED_CODE }}

void evaluate(Context ctx) {
    if (!isInputDirty<input_SND>(ctx))
        return;

    auto client = EthernetClient(getValue<input_SOCK>(ctx));
    auto msg = getValue<input_MSG>(ctx);

    size_t lastWriteSize;

    for (auto it = msg->iterate(); it; ++it) {
        lastWriteSize = client.write((char)*it);
        if (lastWriteSize == 0) {
            emitValue<output_ERR>(ctx, 1);
            return;
        }
    }

    client.flush();

    emitValue<output_DONE>(ctx, 1);
}
<|endoftext|>


<commit_before>#ifndef CglConicCutGenerator_H
#define CglConicCutGenerator_H

// CGL headers
#include <CglTreeInfo.hpp>
// OsiConic headers
#include <OsiConicSolverInterface.hpp>

// this is an abstract base class for generating conic cuts.
// specific conic cut generators inherit this class.
//
//

// what is common to all conic cut generators?
// for now we assume there is not much in common.
// I will come back here when I implemented all specific cuts
// since I will know then what is common.
// following maybe?
// --cut generating cone
// ----size of cone
// ----members of the cut generating cone
// --disjunction?

class CglConicCutGenerator {
public:
  // default constructor
  CglConicCutGenerator();
  // copy constructor
  CglConicCutGenerator(CglConicCutGenerator const & other);
  // copy assignment operator
  CglConicCutGenerator & operator=(CglConicCutGenerator const & rhs);
  virtual ~CglConicCutGenerator();
  // clone method
  virtual CglConicCutGenerator * clone() const = 0;
  // Create C++ lines so set generator, I do not see why we have this.
  virtual std::string generateCpp( FILE * ) {return "";}

  /// This can be used to refresh any information
  virtual void refreshSolver(OsiConicSolverInterface * si) {}
  // generate cuts
  virtual void generateCuts(const OsiConicSolverInterface & si,
			    OsiConicCuts & cs,
			    const CglTreeInfo info = CglTreeInfo()) = 0;
  // generate linear/ordinary cuts.
  virtual void generateCuts(const OsiConicSolverInterface & si,
			    OsiCuts & cs,
			    const CglTreeInfo info = CglTreeInfo());
  int aggressiveness() const;
  void setAggressiveness(int value);
  // set whether can do global cuts
  void setGlobalCuts(bool ability);
  bool canDoGlobalCuts() const;
  /**
     Returns true if may generate cuts in tree (rather than root node).
     Used so know if matrix will change in tree.  Really
     meant so column cut generators can still be active
     without worrying code.
     Default is true
  */
  virtual bool mayGenerateRowCutsInTree() const;
  /// Return true if needs optimal basis to do cuts
  virtual bool needsOptimalBasis() const;
  /// Return maximum length of cut in tree
  virtual int maximumLengthOfCutInTree() const
  { return COIN_INT_MAX;}
private:
  /**
     Aggressiveness - 0 = neutral, 100 is normal root node.
     Really just a hint to cut generator
  */
  int aggressive_;
  /// True if can do global cuts i.e. no general integers
  bool canDoGlobalCuts_;
};

// we define a class to represent a disjunction. This is for conic Cgl
// use only. Not intented for public use.
class Disjunction {
  int size_;
  double * c1_;
  double c10_;
  double * c2_;
  double c20_;
public:
  // defualt constructor
  Disjunction(int size,
	      double const * c1, double c10,
	      double const * c2, double c20): size_(size) {
    c1_ = new double[size];
    std::copy (c1, c1+size, c1_);
    c2_ = new double[size];
    std::copy (c2, c2+size, c2_);
    c10_ = c10;
    c20_ = c20;
  }
  // copy constructor
  Disjunction(Disjunction const & other) {
    // save size_
    size_ = other.size();
    // copy c1_
    if (c1_)
      delete[] c1_;
    c1_ = new double[size_];
    double const * other_c1 = other.get_c1();
    std::copy(other_c1, other_c1+size_, c1_);
    // copy c2_
    if (c2_)
      delete[] c2_;
    c2_ = new double[size_];
    double const * other_c2 = other.get_c2();
    std::copy(other_c2, other_c2+size_, c2_);
    // save c10 and c20
    c10_ = other.get_c10();
    c20_ = other.get_c20();
  }
  // copy assignment operator
  Disjunction & operator=(Disjunction const & rhs) {
    // save size_
    size_ = rhs.size();
    // copy c1_
    if (c1_)
      delete[] c1_;
    c1_ = new double[size_];
    double const * rhs_c1 = rhs.get_c1();
    std::copy(rhs_c1, rhs_c1+size_, c1_);
    // copy c2_
    if (c2_)
      delete[] c2_;
    c2_ = new double[size_];
    double const * rhs_c2 = rhs.get_c2();
    std::copy(rhs_c2, rhs_c2+size_, c2_);
    // save c10 and c20
    c10_ = rhs.get_c10();
    c20_ = rhs.get_c20();
    return *this;
  }
  ~Disjunction() { delete[] c1_; delete[] c2_; }
  double const * get_c1() const { return c1_; }
  double get_c10() const { return c10_; }
  double const * get_c2() const { return c2_; }
  double get_c20() const { return c20_; }
  int size() const {return size_;}
};

#endif
<commit_msg>generateCuts functions should not be pure. We do not know what kind of cuts will be generated. An implementation may choose to generate either nonlinear (conic) or linear cuts.<commit_after>#ifndef CglConicCutGenerator_H
#define CglConicCutGenerator_H

// CGL headers
#include <CglTreeInfo.hpp>
// OsiConic headers
#include <OsiConicSolverInterface.hpp>

// this is an abstract base class for generating conic cuts.
// specific conic cut generators inherit this class.
//
//

// what is common to all conic cut generators?
// for now we assume there is not much in common.
// I will come back here when I implemented all specific cuts
// since I will know then what is common.
// following maybe?
// --cut generating cone
// ----size of cone
// ----members of the cut generating cone
// --disjunction?

class CglConicCutGenerator {
public:
  // default constructor
  CglConicCutGenerator();
  // copy constructor
  CglConicCutGenerator(CglConicCutGenerator const & other);
  // copy assignment operator
  CglConicCutGenerator & operator=(CglConicCutGenerator const & rhs);
  virtual ~CglConicCutGenerator();
  // clone method
  virtual CglConicCutGenerator * clone() const = 0;
  // Create C++ lines so set generator, I do not see why we have this.
  virtual std::string generateCpp( FILE * ) {return "";}

  /// This can be used to refresh any information
  virtual void refreshSolver(OsiConicSolverInterface * si) {}
  // generate cuts
  virtual void generateCuts(const OsiConicSolverInterface & si,
			    OsiConicCuts & cs,
			    const CglTreeInfo info = CglTreeInfo());
  // generate linear/ordinary cuts.
  virtual void generateCuts(const OsiConicSolverInterface & si,
			    OsiCuts & cs,
			    const CglTreeInfo info = CglTreeInfo());
  int aggressiveness() const;
  void setAggressiveness(int value);
  // set whether can do global cuts
  void setGlobalCuts(bool ability);
  bool canDoGlobalCuts() const;
  /**
     Returns true if may generate cuts in tree (rather than root node).
     Used so know if matrix will change in tree.  Really
     meant so column cut generators can still be active
     without worrying code.
     Default is true
  */
  virtual bool mayGenerateRowCutsInTree() const;
  /// Return true if needs optimal basis to do cuts
  virtual bool needsOptimalBasis() const;
  /// Return maximum length of cut in tree
  virtual int maximumLengthOfCutInTree() const
  { return COIN_INT_MAX;}
private:
  /**
     Aggressiveness - 0 = neutral, 100 is normal root node.
     Really just a hint to cut generator
  */
  int aggressive_;
  /// True if can do global cuts i.e. no general integers
  bool canDoGlobalCuts_;
};

// we define a class to represent a disjunction. This is for conic Cgl
// use only. Not intented for public use.
class Disjunction {
  int size_;
  double * c1_;
  double c10_;
  double * c2_;
  double c20_;
public:
  // defualt constructor
  Disjunction(int size,
	      double const * c1, double c10,
	      double const * c2, double c20): size_(size) {
    c1_ = new double[size];
    std::copy (c1, c1+size, c1_);
    c2_ = new double[size];
    std::copy (c2, c2+size, c2_);
    c10_ = c10;
    c20_ = c20;
  }
  // copy constructor
  Disjunction(Disjunction const & other) {
    // save size_
    size_ = other.size();
    // copy c1_
    if (c1_)
      delete[] c1_;
    c1_ = new double[size_];
    double const * other_c1 = other.get_c1();
    std::copy(other_c1, other_c1+size_, c1_);
    // copy c2_
    if (c2_)
      delete[] c2_;
    c2_ = new double[size_];
    double const * other_c2 = other.get_c2();
    std::copy(other_c2, other_c2+size_, c2_);
    // save c10 and c20
    c10_ = other.get_c10();
    c20_ = other.get_c20();
  }
  // copy assignment operator
  Disjunction & operator=(Disjunction const & rhs) {
    // save size_
    size_ = rhs.size();
    // copy c1_
    if (c1_)
      delete[] c1_;
    c1_ = new double[size_];
    double const * rhs_c1 = rhs.get_c1();
    std::copy(rhs_c1, rhs_c1+size_, c1_);
    // copy c2_
    if (c2_)
      delete[] c2_;
    c2_ = new double[size_];
    double const * rhs_c2 = rhs.get_c2();
    std::copy(rhs_c2, rhs_c2+size_, c2_);
    // save c10 and c20
    c10_ = rhs.get_c10();
    c20_ = rhs.get_c20();
    return *this;
  }
  ~Disjunction() { delete[] c1_; delete[] c2_; }
  double const * get_c1() const { return c1_; }
  double get_c10() const { return c10_; }
  double const * get_c2() const { return c2_; }
  double get_c20() const { return c20_; }
  int size() const {return size_;}
};

#endif
<|endoftext|>