<commit_before>#include "LikelihoodProfiler.h"

// from STL
#include <set>

// from ROOT
#include "TCanvas.h"
#include "TGraph.h"
#include "TH2D.h"
#include "TAxis.h"
#include "TStyle.h"
#include "TColor.h"

// from RooFit
#include "RooFitResult.h"

// from DooCore
#include <doocore/io/MsgStream.h>
#include <doocore/io/Progress.h>
#include <doocore/lutils/lutils.h>

// from DooFit
#include "doofit/fitter/AbsFitter.h"
#include "doofit/toy/ToyStudyStd/ToyStudyStd.h"

doofit::plotting::profiles::LikelihoodProfiler::LikelihoodProfiler()
: num_samples_(30)
{}

std::vector<double> doofit::plotting::profiles::LikelihoodProfiler::SetSamplePoint(unsigned int step) {
  using namespace doocore::io;

  std::vector<double> sample_vals;
  int i(scan_vars_.size() - 1);

  //sdebug << "step = " << step << endmsg;

  for (auto var : scan_vars_) {
    int step_this = step/std::pow(num_samples_,i);
    step -= step_this*std::pow(num_samples_,i);
    --i;

    double val(start_vals_[var->GetName()] + 3*var->getError()*(-1.0 + 2.0/static_cast<double>(num_samples_)*step_this));
    var->setVal(val);
    var->setConstant(true);

    sample_vals.push_back(val);
    //sdebug << "  " << var->GetName() << " = " << step_this << " @ " << var->getVal() << endmsg;
  }

  return sample_vals;
}

void doofit::plotting::profiles::LikelihoodProfiler::Scan() {
  using namespace doocore::io;

  //RooRealVar* parameter_scan = scan_vars_.front();

  //num_samples_                   = 12;
  unsigned int num_dimensions    = scan_vars_.size();
  unsigned int num_total_samples = std::pow(num_samples_,num_dimensions);
  unsigned int step              = 0;

  // double value_start = parameter_scan->getVal();
  // double value_scan  = value_start - 5.0*parameter_scan->getError();

  for (auto var : scan_vars_) {
    start_vals_[var->GetName()] = var->getVal();
  }

  Progress p("Sampling likelihood", num_total_samples);
  while (step < num_total_samples) {
    //parameter_scan->setVal(value_scan);
    //parameter_scan->setConstant(true);

    //sinfo << "PROFILE: " << value_scan << endmsg;

    std::vector<double> sample_vals(SetSamplePoint(step));

    fitter_->set_shutup(true);
    fitter_->Fit();

    fit_results_.push_back(fitter_->fit_result());

    //sinfo << fitter_->NegativeLogLikelihood() << endmsg;
    //value_scan += 1*parameter_scan->getError();
    ++step;
    ++p;
  }
  p.Finish();
}

void doofit::plotting::profiles::LikelihoodProfiler::ReadFitResults(doofit::toy::ToyStudyStd& toy_study) {
  using namespace doofit::toy;
  using namespace doocore::io;

  FitResultContainer fit_result_container(toy_study.GetFitResult());
  const RooFitResult* fit_result(std::get<0>(fit_result_container));
  // int i = 0;
  while (fit_result != nullptr) { // && i < 10000) {
    fit_results_.push_back(fit_result);

    fit_result_container = toy_study.GetFitResult();
    fit_result = std::get<0>(fit_result_container);
    // ++i;
  }
}

bool doofit::plotting::profiles::LikelihoodProfiler::FitResultOkay(const RooFitResult& fit_result) const {
  using namespace doocore::io;
  if (fit_result.covQual() < 2) {
    return false;
  } else if (fit_result.statusCodeHistory(0) < 0) {
    return false;
  } else {
    return true;
  }
}

void doofit::plotting::profiles::LikelihoodProfiler::PlotHandler(const std::string& plot_path) {
  using namespace doocore::io;

  std::map<std::string, std::vector<double>> val_scan;
  std::vector<double> val_nll;

  for (auto var : scan_vars_) {
    RooRealVar* var_fixed = dynamic_cast<RooRealVar*>(fit_results_.front()->constPars().find(var->GetName()));
    scan_vars_titles_.push_back(var_fixed->GetTitle());
  }

  // int i = 0;
  double min_nll(0.0);

  Progress p("Processing read in fit results", fit_results_.size());
  for (auto result : fit_results_) {
    if (FitResultOkay(*result)) {
      for (auto var : scan_vars_) {
        RooRealVar* var_fixed = dynamic_cast<RooRealVar*>(result->constPars().find(var->GetName()));

        if (var_fixed == nullptr) {
          serr << "Error in LikelihoodProfiler::PlotHandler(): Cannot find fixed parameter "
               << var->GetName() << " in fit result!" << endmsg;
          throw;
        }

        val_scan[var->GetName()].push_back(var_fixed->getVal());
      }

      if (min_nll == 0.0 || min_nll > result->minNll()) {
        min_nll = result->minNll();
      }
      val_nll.push_back(result->minNll());

      ++p;
    }
  }
  p.Finish();

  for (auto &nll : val_nll) {
    if (nll != 0.0) {
      nll -= min_nll;
    }
  }

  if (val_scan.size() == 1) {
    doocore::lutils::setStyle();
    gStyle->SetPadLeftMargin(0.12);
    gStyle->SetTitleOffset(0.75, "y");
  } else if (val_scan.size() == 2) {
    doocore::lutils::setStyle("2d");
    //gStyle->SetNumberContours(999);
    //gStyle->SetPadRightMargin(0.16);
    gStyle->SetPadRightMargin(0.06);
    gStyle->SetTitleOffset(0.75, "z");
  }

  TCanvas c("c", "c", 800, 600);

  if (val_scan.size() == 1) {
    const std::vector<double>& val_x = val_scan.begin()->second;

    TGraph graph(val_nll.size(), &val_x[0], &val_nll[0]);
    graph.Draw("AP");
    graph.GetXaxis()->SetTitle(scan_vars_titles_.at(0).c_str());
    graph.GetYaxis()->SetTitle("#DeltaLL");
    graph.SetMarkerStyle(1);
    //graph.SetMarkerSize(10);
    graph.SetMarkerColor(kBlue+3);

    //c.SaveAs("profile.pdf");
    doocore::lutils::printPlot(&c, "profile", plot_path);
  } else if (val_scan.size() == 2) {

    const std::vector<double>& val_x = val_scan.begin()->second;
    const std::vector<double>& val_y = val_scan.rbegin()->second;

    auto minmax_x = std::minmax_element(val_x.begin(), val_x.end());
    auto minmax_y = std::minmax_element(val_y.begin(), val_y.end());

    Progress p_distinct("Counting distinct x and y values", val_x.size()+val_y.size());
    std::set<double> distinct_x, distinct_y;
    for (auto x : val_x) {
      distinct_x.insert(x);
      ++p_distinct;
    }
    for (auto y : val_y) {
      distinct_y.insert(y);
      ++p_distinct;
    }
    p_distinct.Finish();

    double min_nll(0.0), max_nll(0.0);
    for (auto nll : val_nll) {
      if (nll != 0 && (min_nll == 0.0 || nll < min_nll)) {
        min_nll = nll;
      }
      if (nll != 0 && (max_nll == 0.0 || nll > max_nll)) {
        max_nll = nll;
      }
    }

    double min_x = *minmax_x.first  - (*minmax_x.second-*minmax_x.first)/(distinct_x.size()-1)*0.5;
    double max_x = *minmax_x.second + (*minmax_x.second-*minmax_x.first)/(distinct_x.size()-1)*0.5;
    double min_y = *minmax_y.first  - (*minmax_y.second-*minmax_y.first)/(distinct_y.size()-1)*0.5;
    double max_y = *minmax_y.second + (*minmax_y.second-*minmax_y.first)/(distinct_y.size()-1)*0.5;

    TH2D histogram("histogram", "histogram", distinct_x.size(), min_x, max_x, distinct_y.size(), min_y, max_y);

    // sdebug << "histogram x: " << *minmax_x.first << " - " <<  *minmax_x.second << endmsg;
    // sdebug << "histogram y: " << *minmax_y.first << " - " <<  *minmax_y.second << endmsg;

    Progress p_hist("Filling 2D profile histogram", val_nll.size());
    for (int i=0; i<val_nll.size(); ++i) {
      // sdebug << val_x.at(i) << ", " << val_y.at(i) << " - " << val_nll.at(i) << endmsg;
      // sdebug << "Bin: " << histogram.FindBin(val_x.at(i), val_y.at(i)) << endmsg;

      int nbin_x, nbin_y, nbin_z;
      histogram.GetBinXYZ(histogram.FindBin(val_x.at(i), val_y.at(i)), nbin_x, nbin_y, nbin_z);

      // sdebug << "Bin center x: " << histogram.GetXaxis()->GetBinCenter(nbin_x) << endmsg;
      // sdebug << "Bin center y: " << histogram.GetYaxis()->GetBinCenter(nbin_y) << endmsg;

      histogram.SetBinContent(histogram.FindBin(val_x.at(i), val_y.at(i)), val_nll.at(i));
      ++p_hist;
    }

    unsigned int num_interpolated_bins(0);
    for (int i=1; i<=histogram.GetNbinsX(); ++i) {
      for (int j=1; j<=histogram.GetNbinsY(); ++j) {
        if (histogram.GetBinContent(i,j) == 0.0) {
          int num_interpolation(0);
          double interpolation(0.0);
          for (int ii=std::max(i-1,1); ii<std::min(i+1,histogram.GetNbinsX()); ++ii) {
            for (int jj=std::max(j-1,1); jj<std::min(j+1,histogram.GetNbinsY()); ++jj) {
              if (histogram.GetBinContent(ii,jj) != 0.0) {
                // sdebug << ii << " - " << jj << endmsg;
                interpolation += histogram.GetBinContent(ii,jj);
                ++num_interpolation;
              }
            }
          }

          interpolation /= num_interpolation;
          // sdebug << "Bin (" << i << "," << j << ") is zero. Will interpolate with " <<  num_interpolation << " bins to " << interpolation << "." << endmsg;
          histogram.SetBinContent(i,j, interpolation);
          ++num_interpolated_bins;
        }
      }
    }
    sinfo << "LikelihoodProfiler::PlotHandler(...): Number of interpolated bins: " << num_interpolated_bins << endmsg;

    p_hist.Finish();

    std::vector<double> stops_cl;
    std::vector<int> colours;

    stops_cl.push_back(0.0);
    colours.push_back(kGray+2);
    stops_cl.push_back(2.30/2.0);
    colours.push_back(kBlue-8);

    if (max_nll > 6.18/2.0) {
      stops_cl.push_back(6.18/2.0);
      colours.push_back(kYellow-8);
    }
    if (max_nll > 11.8/2.0) {
      stops_cl.push_back(11.8/2.0);
      colours.push_back(kRed-8);
    }
    if (max_nll > 18.4/2.0) {
      stops_cl.push_back(18.4/2.0);
      colours.push_back(kCyan-6);
    }
    // if (max_nll > 12.50) {
    //   stops_cl.push_back(12.50);
    //   colours.push_back(kGreen-5);
    // }

    // const Int_t NRGBs = 6;
    // const Int_t NCont = 6;

    // Double_t stops[NRGBs] = { 0.00 , 0.50 , 2.00 , 4.50 , 8.00 , 12.5 };
    // Double_t red[NRGBs]   = { 0.00 , 0.00 , 0.20 , 1.00 , 1.00 , 1.00 };
    // Double_t green[NRGBs] = { 0.00 , 0.00 , 0.20 , 1.00 , 1.00 , 1.00 };
    // Double_t blue[NRGBs]  = { 0.20 , 1.00 , 1.00 , 1.00 , 1.00 , 1.00 };
    // TColor::CreateGradientColorTable(NRGBs, stops, red, green, blue, NCont);
    // gStyle->SetNumberContours(NCont);
    // gStyle->SetPaintTextFormat(".1f");

    gStyle->SetPalette(colours.size(), colours.data());
    histogram.SetContour(stops_cl.size(), stops_cl.data());

    sinfo << "LikelihoodProfiler::PlotHandler(): Drawing histogram." << endmsg;
    histogram.Draw("CONT1");
    histogram.GetZaxis()->SetRangeUser(min_nll, max_nll);
    histogram.SetXTitle(scan_vars_titles_.at(0).c_str());
    histogram.SetYTitle(scan_vars_titles_.at(1).c_str());
    histogram.SetZTitle("#DeltaLL");

    //c.SaveAs("profile.pdf");
    sinfo << "LikelihoodProfiler::PlotHandler(): Saving linear histograms to output files." << endmsg;
    doocore::lutils::printPlot(&c, "profile", plot_path, true);
    // sinfo << "LikelihoodProfiler::PlotHandler(): Saving logarithmic histograms to output files." << endmsg;
    // c.SetLogz(true);
    // doocore::lutils::printPlot(&c, "profile_log", plot_path, true);
    sinfo << "LikelihoodProfiler::PlotHandler(): All done." << endmsg;
  } else {
    serr << "Error in LikelihoodProfiler::PlotHandler(): Cannot (yet) plot 3D likelihood." << endmsg;
    throw;
  }
}

<commit_msg>LikelihoodProfiler: correct 2D confidence contour levels<commit_after>#include "LikelihoodProfiler.h"

// from STL
#include <set>

// from ROOT
#include "TCanvas.h"
#include "TGraph.h"
#include "TH2D.h"
#include "TAxis.h"
#include "TStyle.h"
#include "TColor.h"

// from RooFit
#include "RooFitResult.h"

// from DooCore
#include <doocore/io/MsgStream.h>
#include <doocore/io/Progress.h>
#include <doocore/lutils/lutils.h>

// from DooFit
#include "doofit/fitter/AbsFitter.h"
#include "doofit/toy/ToyStudyStd/ToyStudyStd.h"

doofit::plotting::profiles::LikelihoodProfiler::LikelihoodProfiler()
: num_samples_(30)
{}

std::vector<double> doofit::plotting::profiles::LikelihoodProfiler::SetSamplePoint(unsigned int step) {
  using namespace doocore::io;

  std::vector<double> sample_vals;
  int i(scan_vars_.size() - 1);

  //sdebug << "step = " << step << endmsg;

  for (auto var : scan_vars_) {
    int step_this = step/std::pow(num_samples_,i);
    step -= step_this*std::pow(num_samples_,i);
    --i;

    double val(start_vals_[var->GetName()] + 3*var->getError()*(-1.0 + 2.0/static_cast<double>(num_samples_)*step_this));
    var->setVal(val);
    var->setConstant(true);

    sample_vals.push_back(val);
    //sdebug << "  " << var->GetName() << " = " << step_this << " @ " << var->getVal() << endmsg;
  }

  return sample_vals;
}

void doofit::plotting::profiles::LikelihoodProfiler::Scan() {
  using namespace doocore::io;

  //RooRealVar* parameter_scan = scan_vars_.front();

  //num_samples_                   = 12;
  unsigned int num_dimensions    = scan_vars_.size();
  unsigned int num_total_samples = std::pow(num_samples_,num_dimensions);
  unsigned int step              = 0;

  // double value_start = parameter_scan->getVal();
  // double value_scan  = value_start - 5.0*parameter_scan->getError();

  for (auto var : scan_vars_) {
    start_vals_[var->GetName()] = var->getVal();
  }

  Progress p("Sampling likelihood", num_total_samples);
  while (step < num_total_samples) {
    //parameter_scan->setVal(value_scan);
    //parameter_scan->setConstant(true);

    //sinfo << "PROFILE: " << value_scan << endmsg;

    std::vector<double> sample_vals(SetSamplePoint(step));

    fitter_->set_shutup(true);
    fitter_->Fit();

    fit_results_.push_back(fitter_->fit_result());

    //sinfo << fitter_->NegativeLogLikelihood() << endmsg;
    //value_scan += 1*parameter_scan->getError();
    ++step;
    ++p;
  }
  p.Finish();
}

void doofit::plotting::profiles::LikelihoodProfiler::ReadFitResults(doofit::toy::ToyStudyStd& toy_study) {
  using namespace doofit::toy;
  using namespace doocore::io;

  FitResultContainer fit_result_container(toy_study.GetFitResult());
  const RooFitResult* fit_result(std::get<0>(fit_result_container));
  // int i = 0;
  while (fit_result != nullptr) { // && i < 10000) {
    fit_results_.push_back(fit_result);

    fit_result_container = toy_study.GetFitResult();
    fit_result = std::get<0>(fit_result_container);
    // ++i;
  }
}

bool doofit::plotting::profiles::LikelihoodProfiler::FitResultOkay(const RooFitResult& fit_result) const {
  using namespace doocore::io;
  if (fit_result.covQual() < 2) {
    return false;
  } else if (fit_result.statusCodeHistory(0) < 0) {
    return false;
  } else {
    return true;
  }
}

void doofit::plotting::profiles::LikelihoodProfiler::PlotHandler(const std::string& plot_path) {
  using namespace doocore::io;

  std::map<std::string, std::vector<double>> val_scan;
  std::vector<double> val_nll;

  for (auto var : scan_vars_) {
    RooRealVar* var_fixed = dynamic_cast<RooRealVar*>(fit_results_.front()->constPars().find(var->GetName()));
    scan_vars_titles_.push_back(var_fixed->GetTitle());
  }

  // int i = 0;
  double min_nll(0.0);

  Progress p("Processing read in fit results", fit_results_.size());
  for (auto result : fit_results_) {
    if (FitResultOkay(*result)) {
      for (auto var : scan_vars_) {
        RooRealVar* var_fixed = dynamic_cast<RooRealVar*>(result->constPars().find(var->GetName()));

        if (var_fixed == nullptr) {
          serr << "Error in LikelihoodProfiler::PlotHandler(): Cannot find fixed parameter "
               << var->GetName() << " in fit result!" << endmsg;
          throw;
        }

        val_scan[var->GetName()].push_back(var_fixed->getVal());
      }

      if (min_nll == 0.0 || min_nll > result->minNll()) {
        min_nll = result->minNll();
      }
      val_nll.push_back(result->minNll());

      ++p;
    }
  }
  p.Finish();

  for (auto &nll : val_nll) {
    if (nll != 0.0) {
      nll -= min_nll;
    }
  }

  if (val_scan.size() == 1) {
    doocore::lutils::setStyle();
    gStyle->SetPadLeftMargin(0.12);
    gStyle->SetTitleOffset(0.75, "y");
  } else if (val_scan.size() == 2) {
    doocore::lutils::setStyle("2d");
    //gStyle->SetNumberContours(999);
    //gStyle->SetPadRightMargin(0.16);
    gStyle->SetPadRightMargin(0.06);
    gStyle->SetTitleOffset(0.75, "z");
  }

  TCanvas c("c", "c", 800, 600);

  if (val_scan.size() == 1) {
    const std::vector<double>& val_x = val_scan.begin()->second;

    TGraph graph(val_nll.size(), &val_x[0], &val_nll[0]);
    graph.Draw("AP");
    graph.GetXaxis()->SetTitle(scan_vars_titles_.at(0).c_str());
    graph.GetYaxis()->SetTitle("#DeltaLL");
    graph.SetMarkerStyle(1);
    //graph.SetMarkerSize(10);
    graph.SetMarkerColor(kBlue+3);

    //c.SaveAs("profile.pdf");
    doocore::lutils::printPlot(&c, "profile", plot_path);
  } else if (val_scan.size() == 2) {

    const std::vector<double>& val_x = val_scan.begin()->second;
    const std::vector<double>& val_y = val_scan.rbegin()->second;

    auto minmax_x = std::minmax_element(val_x.begin(), val_x.end());
    auto minmax_y = std::minmax_element(val_y.begin(), val_y.end());

    Progress p_distinct("Counting distinct x and y values", val_x.size()+val_y.size());
    std::set<double> distinct_x, distinct_y;
    for (auto x : val_x) {
      distinct_x.insert(x);
      ++p_distinct;
    }
    for (auto y : val_y) {
      distinct_y.insert(y);
      ++p_distinct;
    }
    p_distinct.Finish();

    double min_nll(0.0), max_nll(0.0);
    for (auto nll : val_nll) {
      if (nll != 0 && (min_nll == 0.0 || nll < min_nll)) {
        min_nll = nll;
      }
      if (nll != 0 && (max_nll == 0.0 || nll > max_nll)) {
        max_nll = nll;
      }
    }

    double min_x = *minmax_x.first  - (*minmax_x.second-*minmax_x.first)/(distinct_x.size()-1)*0.5;
    double max_x = *minmax_x.second + (*minmax_x.second-*minmax_x.first)/(distinct_x.size()-1)*0.5;
    double min_y = *minmax_y.first  - (*minmax_y.second-*minmax_y.first)/(distinct_y.size()-1)*0.5;
    double max_y = *minmax_y.second + (*minmax_y.second-*minmax_y.first)/(distinct_y.size()-1)*0.5;

    TH2D histogram("histogram", "histogram", distinct_x.size(), min_x, max_x, distinct_y.size(), min_y, max_y);

    // sdebug << "histogram x: " << *minmax_x.first << " - " <<  *minmax_x.second << endmsg;
    // sdebug << "histogram y: " << *minmax_y.first << " - " <<  *minmax_y.second << endmsg;

    Progress p_hist("Filling 2D profile histogram", val_nll.size());
    for (int i=0; i<val_nll.size(); ++i) {
      // sdebug << val_x.at(i) << ", " << val_y.at(i) << " - " << val_nll.at(i) << endmsg;
      // sdebug << "Bin: " << histogram.FindBin(val_x.at(i), val_y.at(i)) << endmsg;

      int nbin_x, nbin_y, nbin_z;
      histogram.GetBinXYZ(histogram.FindBin(val_x.at(i), val_y.at(i)), nbin_x, nbin_y, nbin_z);

      // sdebug << "Bin center x: " << histogram.GetXaxis()->GetBinCenter(nbin_x) << endmsg;
      // sdebug << "Bin center y: " << histogram.GetYaxis()->GetBinCenter(nbin_y) << endmsg;

      histogram.SetBinContent(histogram.FindBin(val_x.at(i), val_y.at(i)), val_nll.at(i));
      ++p_hist;
    }

    unsigned int num_interpolated_bins(0);
    for (int i=1; i<=histogram.GetNbinsX(); ++i) {
      for (int j=1; j<=histogram.GetNbinsY(); ++j) {
        if (histogram.GetBinContent(i,j) == 0.0) {
          int num_interpolation(0);
          double interpolation(0.0);
          for (int ii=std::max(i-1,1); ii<std::min(i+1,histogram.GetNbinsX()); ++ii) {
            for (int jj=std::max(j-1,1); jj<std::min(j+1,histogram.GetNbinsY()); ++jj) {
              if (histogram.GetBinContent(ii,jj) != 0.0) {
                // sdebug << ii << " - " << jj << endmsg;
                interpolation += histogram.GetBinContent(ii,jj);
                ++num_interpolation;
              }
            }
          }

          interpolation /= num_interpolation;
          // sdebug << "Bin (" << i << "," << j << ") is zero. Will interpolate with " <<  num_interpolation << " bins to " << interpolation << "." << endmsg;
          histogram.SetBinContent(i,j, interpolation);
          ++num_interpolated_bins;
        }
      }
    }
    sinfo << "LikelihoodProfiler::PlotHandler(...): Number of interpolated bins: " << num_interpolated_bins << endmsg;

    p_hist.Finish();

    std::vector<double> stops_cl;
    std::vector<int> colours;

    // 2D limits from Numerical Recipies Third Edition Sec 15.6 (p. 815)

    stops_cl.push_back(0.0);
    colours.push_back(kGray+2);
    stops_cl.push_back(2.30/2.0);
    colours.push_back(kBlue-8);

    if (max_nll > 6.18/2.0) {
      stops_cl.push_back(6.18/2.0);
      colours.push_back(kYellow-8);
    }
    if (max_nll > 11.8/2.0) {
      stops_cl.push_back(11.8/2.0);
      colours.push_back(kRed-8);
    }
    if (max_nll > 18.4/2.0) {
      stops_cl.push_back(18.4/2.0);
      colours.push_back(kCyan-6);
    }
    // if (max_nll > 12.50) {
    //   stops_cl.push_back(12.50);
    //   colours.push_back(kGreen-5);
    // }

    // const Int_t NRGBs = 6;
    // const Int_t NCont = 6;

    // Double_t stops[NRGBs] = { 0.00 , 0.50 , 2.00 , 4.50 , 8.00 , 12.5 };
    // Double_t red[NRGBs]   = { 0.00 , 0.00 , 0.20 , 1.00 , 1.00 , 1.00 };
    // Double_t green[NRGBs] = { 0.00 , 0.00 , 0.20 , 1.00 , 1.00 , 1.00 };
    // Double_t blue[NRGBs]  = { 0.20 , 1.00 , 1.00 , 1.00 , 1.00 , 1.00 };
    // TColor::CreateGradientColorTable(NRGBs, stops, red, green, blue, NCont);
    // gStyle->SetNumberContours(NCont);
    // gStyle->SetPaintTextFormat(".1f");

    gStyle->SetPalette(colours.size(), colours.data());
    histogram.SetContour(stops_cl.size(), stops_cl.data());

    sinfo << "LikelihoodProfiler::PlotHandler(): Drawing histogram." << endmsg;
    histogram.Draw("CONT1");
    histogram.GetZaxis()->SetRangeUser(min_nll, max_nll);
    histogram.SetXTitle(scan_vars_titles_.at(0).c_str());
    histogram.SetYTitle(scan_vars_titles_.at(1).c_str());
    histogram.SetZTitle("#DeltaLL");

    //c.SaveAs("profile.pdf");
    sinfo << "LikelihoodProfiler::PlotHandler(): Saving linear histograms to output files." << endmsg;
    doocore::lutils::printPlot(&c, "profile", plot_path, true);
    // sinfo << "LikelihoodProfiler::PlotHandler(): Saving logarithmic histograms to output files." << endmsg;
    // c.SetLogz(true);
    // doocore::lutils::printPlot(&c, "profile_log", plot_path, true);
    sinfo << "LikelihoodProfiler::PlotHandler(): All done." << endmsg;
  } else {
    serr << "Error in LikelihoodProfiler::PlotHandler(): Cannot (yet) plot 3D likelihood." << endmsg;
    throw;
  }
}

<|endoftext|>


<commit_before>#include "LikelihoodProfiler.h"

// from STL
#include <set>

// from ROOT
#include "TCanvas.h"
#include "TGraph.h"
#include "TH2D.h"
#include "TAxis.h"

// from RooFit
#include "RooFitResult.h"

// from DooCore
#include <doocore/io/MsgStream.h>
#include <doocore/io/Progress.h>
#include <doocore/lutils/lutils.h>

// from DooFit
#include "doofit/fitter/AbsFitter.h"
#include "doofit/toy/ToyStudyStd/ToyStudyStd.h"

doofit::plotting::profiles::LikelihoodProfiler::LikelihoodProfiler()
: num_samples_(30)
{}

std::vector<double> doofit::plotting::profiles::LikelihoodProfiler::SetSamplePoint(unsigned int step) {
  using namespace doocore::io;

  std::vector<double> sample_vals;
  int i(scan_vars_.size() - 1);

  //sdebug << "step = " << step << endmsg;

  for (auto var : scan_vars_) {
    int step_this = step/std::pow(num_samples_,i);
    step -= step_this*std::pow(num_samples_,i);
    --i;

    double val(start_vals_[var->GetName()] + 3*var->getError()*(-1.0 + 2.0/static_cast<double>(num_samples_)*step_this));
    var->setVal(val);
    var->setConstant(true);

    sample_vals.push_back(val);
    //sdebug << "  " << var->GetName() << " = " << step_this << " @ " << var->getVal() << endmsg;
  }

  return sample_vals;
}

void doofit::plotting::profiles::LikelihoodProfiler::Scan() {
  using namespace doocore::io;

  //RooRealVar* parameter_scan = scan_vars_.front();

  //num_samples_                   = 12;
  unsigned int num_dimensions    = scan_vars_.size();
  unsigned int num_total_samples = std::pow(num_samples_,num_dimensions);
  unsigned int step              = 0;

  // double value_start = parameter_scan->getVal();
  // double value_scan  = value_start - 5.0*parameter_scan->getError();

  for (auto var : scan_vars_) {
    start_vals_[var->GetName()] = var->getVal();
  }

  Progress p("Sampling likelihood", num_total_samples);
  while (step < num_total_samples) {
    //parameter_scan->setVal(value_scan);
    //parameter_scan->setConstant(true);

    //sinfo << "PROFILE: " << value_scan << endmsg;

    std::vector<double> sample_vals(SetSamplePoint(step));

    fitter_->set_shutup(true);
    fitter_->Fit();

    fit_results_.push_back(fitter_->fit_result());

    //sinfo << fitter_->NegativeLogLikelihood() << endmsg;
    //value_scan += 1*parameter_scan->getError();
    ++step;
    ++p;
  }
  p.Finish();
}

void doofit::plotting::profiles::LikelihoodProfiler::ReadFitResults(doofit::toy::ToyStudyStd& toy_study) {
  using namespace doofit::toy;

  FitResultContainer fit_result_container(toy_study.GetFitResult());
  const RooFitResult* fit_result(std::get<0>(fit_result_container));

  while (fit_result != nullptr) {
    fit_results_.push_back(fit_result);

    fit_result_container = toy_study.GetFitResult();
    fit_result = std::get<0>(fit_result_container);
  }
}

bool doofit::plotting::profiles::LikelihoodProfiler::FitResultOkay(const RooFitResult& fit_result) const {
  using namespace doocore::io;
  if (fit_result.covQual() < 2) {
    return false;
  } else if (fit_result.statusCodeHistory(0) < 0) {
    return false;
  } else {
    return true;
  }
}

void doofit::plotting::profiles::LikelihoodProfiler::PlotHandler(const std::string& plot_path) {
  using namespace doocore::io;

  std::map<std::string, std::vector<double>> val_scan;
  std::vector<double> val_nll;

  doocore::lutils::setStyle("2d");

  for (auto var : scan_vars_) {
    RooRealVar* var_fixed = dynamic_cast<RooRealVar*>(fit_results_.front()->constPars().find(var->GetName()));
    scan_vars_titles_.push_back(var_fixed->GetTitle());
  }

  for (auto result : fit_results_) {
    if (FitResultOkay(*result)) {
      for (auto var : scan_vars_) {
        RooRealVar* var_fixed = dynamic_cast<RooRealVar*>(result->constPars().find(var->GetName()));

        if (var_fixed == nullptr) {
          serr << "Error in LikelihoodProfiler::PlotHandler(): Cannot find fixed parameter "
               << var->GetName() << " in fit result!" << endmsg;
          throw;
        }

        val_scan[var->GetName()].push_back(var_fixed->getVal());
      }

      val_nll.push_back(result->minNll());
    }
  }

  TCanvas c("c", "c", 800, 600);

  if (val_scan.size() == 1) {
    const std::vector<double>& val_x = val_scan.begin()->second;

    TGraph g(val_nll.size(), &val_x[0], &val_nll[0]);
    g.Draw("AP");
    c.SaveAs("profile.pdf");
  } else if (val_scan.size() == 2) {
    const std::vector<double>& val_x = val_scan.begin()->second;
    const std::vector<double>& val_y = val_scan.rbegin()->second;

    auto minmax_x = std::minmax_element(val_x.begin(), val_x.end());
    auto minmax_y = std::minmax_element(val_y.begin(), val_y.end());

    std::set<double> distinct_x, distinct_y;
    for (auto x : val_x) {
      distinct_x.insert(x);
    }
    for (auto y : val_y) {
      distinct_y.insert(y);
    }

    double min_nll(0.0), max_nll(0.0);
    for (auto nll : val_nll) {
      if (nll != 0 && (min_nll == 0.0 || nll < min_nll)) {
        min_nll = nll;
      }
      if (nll != 0 && (max_nll == 0.0 || nll > max_nll)) {
        max_nll = nll;
      }
    }

    double min_x = *minmax_x.first  - (*minmax_x.second-*minmax_x.first)/(distinct_x.size()-1)*0.5;
    double max_x = *minmax_x.second + (*minmax_x.second-*minmax_x.first)/(distinct_x.size()-1)*0.5;
    double min_y = *minmax_y.first  - (*minmax_y.second-*minmax_y.first)/(distinct_y.size()-1)*0.5;
    double max_y = *minmax_y.second + (*minmax_y.second-*minmax_y.first)/(distinct_y.size()-1)*0.5;

    TH2D histogram("histogram", "histogram", distinct_x.size(), min_x, max_x, distinct_y.size(), min_y, max_y);

    // sdebug << "histogram x: " << *minmax_x.first << " - " <<  *minmax_x.second << endmsg;
    // sdebug << "histogram y: " << *minmax_y.first << " - " <<  *minmax_y.second << endmsg;

    for (int i=0; i<val_nll.size(); ++i) {
      // sdebug << val_x.at(i) << ", " << val_y.at(i) << " - " << val_nll.at(i) << endmsg;
      // sdebug << "Bin: " << histogram.FindBin(val_x.at(i), val_y.at(i)) << endmsg;

      int nbin_x, nbin_y, nbin_z;
      histogram.GetBinXYZ(histogram.FindBin(val_x.at(i), val_y.at(i)), nbin_x, nbin_y, nbin_z);

      // sdebug << "Bin center x: " << histogram.GetXaxis()->GetBinCenter(nbin_x) << endmsg;
      // sdebug << "Bin center y: " << histogram.GetYaxis()->GetBinCenter(nbin_y) << endmsg;

      histogram.SetBinContent(histogram.FindBin(val_x.at(i), val_y.at(i)), val_nll.at(i));
    }

    histogram.Draw("COLZ");
    histogram.GetZaxis()->SetRangeUser(min_nll, max_nll);
    histogram.SetXTitle(scan_vars_titles_.at(0).c_str());
    histogram.SetYTitle(scan_vars_titles_.at(1).c_str());

    c.SaveAs("profile.pdf");
  } else {
    serr << "Error in LikelihoodProfiler::PlotHandler(): Cannot (yet) plot 3D likelihood." << endmsg;
    throw;
  }

}

<commit_msg>LikelihoodProfiler: plotting improvements<commit_after>#include "LikelihoodProfiler.h"

// from STL
#include <set>

// from ROOT
#include "TCanvas.h"
#include "TGraph.h"
#include "TH2D.h"
#include "TAxis.h"

// from RooFit
#include "RooFitResult.h"

// from DooCore
#include <doocore/io/MsgStream.h>
#include <doocore/io/Progress.h>
#include <doocore/lutils/lutils.h>

// from DooFit
#include "doofit/fitter/AbsFitter.h"
#include "doofit/toy/ToyStudyStd/ToyStudyStd.h"

doofit::plotting::profiles::LikelihoodProfiler::LikelihoodProfiler()
: num_samples_(30)
{}

std::vector<double> doofit::plotting::profiles::LikelihoodProfiler::SetSamplePoint(unsigned int step) {
  using namespace doocore::io;

  std::vector<double> sample_vals;
  int i(scan_vars_.size() - 1);

  //sdebug << "step = " << step << endmsg;

  for (auto var : scan_vars_) {
    int step_this = step/std::pow(num_samples_,i);
    step -= step_this*std::pow(num_samples_,i);
    --i;

    double val(start_vals_[var->GetName()] + 3*var->getError()*(-1.0 + 2.0/static_cast<double>(num_samples_)*step_this));
    var->setVal(val);
    var->setConstant(true);

    sample_vals.push_back(val);
    //sdebug << "  " << var->GetName() << " = " << step_this << " @ " << var->getVal() << endmsg;
  }

  return sample_vals;
}

void doofit::plotting::profiles::LikelihoodProfiler::Scan() {
  using namespace doocore::io;

  //RooRealVar* parameter_scan = scan_vars_.front();

  //num_samples_                   = 12;
  unsigned int num_dimensions    = scan_vars_.size();
  unsigned int num_total_samples = std::pow(num_samples_,num_dimensions);
  unsigned int step              = 0;

  // double value_start = parameter_scan->getVal();
  // double value_scan  = value_start - 5.0*parameter_scan->getError();

  for (auto var : scan_vars_) {
    start_vals_[var->GetName()] = var->getVal();
  }

  Progress p("Sampling likelihood", num_total_samples);
  while (step < num_total_samples) {
    //parameter_scan->setVal(value_scan);
    //parameter_scan->setConstant(true);

    //sinfo << "PROFILE: " << value_scan << endmsg;

    std::vector<double> sample_vals(SetSamplePoint(step));

    fitter_->set_shutup(true);
    fitter_->Fit();

    fit_results_.push_back(fitter_->fit_result());

    //sinfo << fitter_->NegativeLogLikelihood() << endmsg;
    //value_scan += 1*parameter_scan->getError();
    ++step;
    ++p;
  }
  p.Finish();
}

void doofit::plotting::profiles::LikelihoodProfiler::ReadFitResults(doofit::toy::ToyStudyStd& toy_study) {
  using namespace doofit::toy;

  FitResultContainer fit_result_container(toy_study.GetFitResult());
  const RooFitResult* fit_result(std::get<0>(fit_result_container));

  while (fit_result != nullptr) {
    fit_results_.push_back(fit_result);

    fit_result_container = toy_study.GetFitResult();
    fit_result = std::get<0>(fit_result_container);
  }
}

bool doofit::plotting::profiles::LikelihoodProfiler::FitResultOkay(const RooFitResult& fit_result) const {
  using namespace doocore::io;
  if (fit_result.covQual() < 2) {
    return false;
  } else if (fit_result.statusCodeHistory(0) < 0) {
    return false;
  } else {
    return true;
  }
}

void doofit::plotting::profiles::LikelihoodProfiler::PlotHandler(const std::string& plot_path) {
  using namespace doocore::io;

  std::map<std::string, std::vector<double>> val_scan;
  std::vector<double> val_nll;

  doocore::lutils::setStyle("2d");

  for (auto var : scan_vars_) {
    RooRealVar* var_fixed = dynamic_cast<RooRealVar*>(fit_results_.front()->constPars().find(var->GetName()));
    scan_vars_titles_.push_back(var_fixed->GetTitle());
  }

  double min_nll(0.0);
  for (auto result : fit_results_) {
    if (FitResultOkay(*result)) {
      for (auto var : scan_vars_) {
        RooRealVar* var_fixed = dynamic_cast<RooRealVar*>(result->constPars().find(var->GetName()));

        if (var_fixed == nullptr) {
          serr << "Error in LikelihoodProfiler::PlotHandler(): Cannot find fixed parameter "
               << var->GetName() << " in fit result!" << endmsg;
          throw;
        }

        val_scan[var->GetName()].push_back(var_fixed->getVal());
      }

      if (min_nll == 0.0 || min_nll > result->minNll()) {
        min_nll = result->minNll();
      }
      val_nll.push_back(result->minNll());
    }
  }

  for (auto &nll : val_nll) {
    if (nll != 0.0) {
      nll -= min_nll;
    }
  }

  TCanvas c("c", "c", 800, 600);

  if (val_scan.size() == 1) {
    const std::vector<double>& val_x = val_scan.begin()->second;

    TGraph graph(val_nll.size(), &val_x[0], &val_nll[0]);
    graph.Draw("AP");
    graph.GetXaxis()->SetTitle(scan_vars_titles_.at(0).c_str());
    graph.GetYaxis()->SetTitle("NLL");
    //c.SaveAs("profile.pdf");
    doocore::lutils::printPlot(&c, "profile", plot_path);
  } else if (val_scan.size() == 2) {
    const std::vector<double>& val_x = val_scan.begin()->second;
    const std::vector<double>& val_y = val_scan.rbegin()->second;

    auto minmax_x = std::minmax_element(val_x.begin(), val_x.end());
    auto minmax_y = std::minmax_element(val_y.begin(), val_y.end());

    std::set<double> distinct_x, distinct_y;
    for (auto x : val_x) {
      distinct_x.insert(x);
    }
    for (auto y : val_y) {
      distinct_y.insert(y);
    }

    double min_nll(0.0), max_nll(0.0);
    for (auto nll : val_nll) {
      if (nll != 0 && (min_nll == 0.0 || nll < min_nll)) {
        min_nll = nll;
      }
      if (nll != 0 && (max_nll == 0.0 || nll > max_nll)) {
        max_nll = nll;
      }
    }

    double min_x = *minmax_x.first  - (*minmax_x.second-*minmax_x.first)/(distinct_x.size()-1)*0.5;
    double max_x = *minmax_x.second + (*minmax_x.second-*minmax_x.first)/(distinct_x.size()-1)*0.5;
    double min_y = *minmax_y.first  - (*minmax_y.second-*minmax_y.first)/(distinct_y.size()-1)*0.5;
    double max_y = *minmax_y.second + (*minmax_y.second-*minmax_y.first)/(distinct_y.size()-1)*0.5;

    TH2D histogram("histogram", "histogram", distinct_x.size(), min_x, max_x, distinct_y.size(), min_y, max_y);

    // sdebug << "histogram x: " << *minmax_x.first << " - " <<  *minmax_x.second << endmsg;
    // sdebug << "histogram y: " << *minmax_y.first << " - " <<  *minmax_y.second << endmsg;

    for (int i=0; i<val_nll.size(); ++i) {
      // sdebug << val_x.at(i) << ", " << val_y.at(i) << " - " << val_nll.at(i) << endmsg;
      // sdebug << "Bin: " << histogram.FindBin(val_x.at(i), val_y.at(i)) << endmsg;

      int nbin_x, nbin_y, nbin_z;
      histogram.GetBinXYZ(histogram.FindBin(val_x.at(i), val_y.at(i)), nbin_x, nbin_y, nbin_z);

      // sdebug << "Bin center x: " << histogram.GetXaxis()->GetBinCenter(nbin_x) << endmsg;
      // sdebug << "Bin center y: " << histogram.GetYaxis()->GetBinCenter(nbin_y) << endmsg;

      histogram.SetBinContent(histogram.FindBin(val_x.at(i), val_y.at(i)), val_nll.at(i));
    }

    histogram.Draw("COLZ");
    histogram.GetZaxis()->SetRangeUser(min_nll, max_nll);
    histogram.SetXTitle(scan_vars_titles_.at(0).c_str());
    histogram.SetYTitle(scan_vars_titles_.at(1).c_str());
    histogram.SetZTitle("#DeltaLL");

    //c.SaveAs("profile.pdf");
    doocore::lutils::printPlot(&c, "profile", plot_path);
  } else {
    serr << "Error in LikelihoodProfiler::PlotHandler(): Cannot (yet) plot 3D likelihood." << endmsg;
    throw;
  }

}

<|endoftext|>


<commit_before>#include "iCubProprioception/BatchCADSuperimposer.h"

#include <exception>
#include <utility>

#include <iCub/ctrl/math.h>
#include <yarp/math/Math.h>
#include <yarp/os/LogStream.h>

using namespace yarp::dev;
using namespace yarp::math;
using namespace yarp::os;
using namespace yarp::sig;
using namespace iCub::ctrl;
using namespace iCub::iKin;

BatchCADSuperimposer::BatchCADSuperimposer(const ConstString& project_name, const ConstString& robot, const ConstString& camera,
                                           const SuperImpose::ObjFileMap& cad_hand, const ConstString& shader_path) :
CADSuperimposer(project_name + "/BatchCADSuperimposer", robot, camera, cad_hand, shader_path)
{
    yInfo() << log_ID_ << "Invoked BatchCADSuperimposer (derived class) ctor...";

    /* Port for the external pose */
    if(!inport_pose_ext_.open("/" + ID_ + "/" + camera_ + "/pose:i"))
    {
        yError() << log_ID_ << "Cannot open input port for external pose!";
        throw std::runtime_error("Cannot open input port for external pose!");
    }

    /* Port for the head encoders */
    if(!inport_head_enc_.open("/" + ID_ + "/" + camera_ + "/head:i"))
    {
        yError() << log_ID_ << "Cannot open input port for head encoders!";
        throw std::runtime_error("Cannot open input port for head encoders!");
    }

    /* Port for the right arm encoders */
    if(!inport_right_arm_enc_.open("/" + ID_ + "/" + camera_ + "/right_arm:i"))
    {
        yError() << log_ID_ << "Cannot open input port for right arm encoders!";
        throw std::runtime_error("Cannot open input port for right arm encoders!");
    }

    /* Port for the torso encoders */
    if(!inport_torso_enc_.open("/" + ID_ + "/" + camera_ + "/torso:i"))
    {
        yError() << log_ID_ << "Cannot open input port for torso encoders!";
        throw std::runtime_error("Cannot open input port for torso encoders!");
    }

    /* Port for the right hand analogs */
    if(!inport_right_hand_analogs_.open("/" + ID_ + "/" + camera_ + "/right_analogs:i"))
    {
        yError() << log_ID_ << "Cannot open input port for right hand analogs!";
        throw std::runtime_error("Cannot open input port for right hand analogs!");
    }

    /* Initialize right arm interface */
    yInfo() << log_ID_ << "Setting right arm interface.";

    right_arm_ = iCubArm("right");

    right_arm_.setAllConstraints(false);
    right_arm_.releaseLink(0);
    right_arm_.releaseLink(1);
    right_arm_.releaseLink(2);

    yInfo() << log_ID_ << "...BatchCADSuperimposer ctor completed!";
}

BatchCADSuperimposer::~BatchCADSuperimposer() noexcept { }

void BatchCADSuperimposer::threadRelease()
{
    if (!inport_pose_ext_.isClosed())           inport_pose_ext_.close();
    if (!inport_head_enc_.isClosed())           inport_head_enc_.close();
    if (!inport_torso_enc_.isClosed())          inport_torso_enc_.close();
    if (!inport_right_arm_enc_.isClosed())      inport_right_arm_enc_.close();
    if (!inport_right_hand_analogs_.isClosed()) inport_right_hand_analogs_.close();

    CADSuperimposer::threadRelease();
}

Vector BatchCADSuperimposer::getEndEffectorPose()
{
    Vector* estimates_mean = inport_pose_ext_.read(synch_);

    if (estimates_mean        != YARP_NULLPTR) ext_pose_copy_ = *estimates_mean;
    if (ext_pose_copy_.size() == 0)            ext_pose_copy_ = *(inport_pose_ext_.read(true));

    return ext_pose_copy_;
}

Vector BatchCADSuperimposer::getHeadEncoders()
{
    return *(inport_head_enc_.read(true));
}

Vector BatchCADSuperimposer::getRightArmEncoders()
{
    return *(inport_right_arm_enc_.read(true));
}

#if ICP_USE_ANALOGS == 1
Vector BatchCADSuperimposer::getRightHandAnalogs()
{
    return *(inport_right_hand_analogs_.read(true));
}
#endif

Vector BatchCADSuperimposer::getTorsoEncoders()
{
    Vector enc_torso = *(inport_torso_enc_.read(true));

    std::swap(enc_torso(0), enc_torso(2));

    return enc_torso;
}

void BatchCADSuperimposer::getExtraObjPoseMap(SuperImpose::ObjPoseMap& hand_pose)
{
    if (view_forearm_)
    {
        SuperImpose::ObjPose pose;

        right_arm_.setAng(CTRL_DEG2RAD * readRootToEE());

        Matrix H_forearm = right_arm_.getH(7, true);

        Vector j_x = H_forearm.getCol(3).subVector(0, 2);
        Vector j_o = dcm2axis(H_forearm);

        pose.assign(j_x.data(), j_x.data()+3);
        pose.insert(pose.end(), j_o.data(), j_o.data()+4);

        hand_pose.emplace("forearm", pose);
    }
}

Vector BatchCADSuperimposer::readRootToEE()
{
    Vector enc_arm = getRightArmEncoders();

    Vector root_ee_enc(10);

    root_ee_enc.setSubvector(0, getTorsoEncoders());

    for (size_t i = 0; i < 7; ++i)
        root_ee_enc(i+3) = enc_arm(i);

    return root_ee_enc;
}

bool BatchCADSuperimposer::sync_input(const bool status)
{
    synch_ = status;

    return true;
}

bool BatchCADSuperimposer::render_extra_mesh(const bool status)
{
    view_forearm_ = status;

    return true;
}
<commit_msg>Clean up code<commit_after>#include "iCubProprioception/BatchCADSuperimposer.h"

#include <exception>
#include <utility>

#include <iCub/ctrl/math.h>
#include <yarp/math/Math.h>
#include <yarp/os/LogStream.h>

using namespace yarp::dev;
using namespace yarp::math;
using namespace yarp::os;
using namespace yarp::sig;
using namespace iCub::ctrl;
using namespace iCub::iKin;

BatchCADSuperimposer::BatchCADSuperimposer(const ConstString& project_name, const ConstString& robot, const ConstString& camera,
                                           const SuperImpose::ObjFileMap& cad_hand, const ConstString& shader_path) :
    CADSuperimposer(project_name + "/BatchCADSuperimposer", robot, camera, cad_hand, shader_path)
{
    yInfo() << log_ID_ << "Invoked BatchCADSuperimposer (derived class) ctor...";

    /* Port for the external pose */
    if(!inport_pose_ext_.open("/" + ID_ + "/" + camera_ + "/pose:i"))
    {
        yError() << log_ID_ << "Cannot open input port for external pose!";
        throw std::runtime_error("Cannot open input port for external pose!");
    }

    /* Port for the head encoders */
    if(!inport_head_enc_.open("/" + ID_ + "/" + camera_ + "/head:i"))
    {
        yError() << log_ID_ << "Cannot open input port for head encoders!";
        throw std::runtime_error("Cannot open input port for head encoders!");
    }

    /* Port for the right arm encoders */
    if(!inport_right_arm_enc_.open("/" + ID_ + "/" + camera_ + "/right_arm:i"))
    {
        yError() << log_ID_ << "Cannot open input port for right arm encoders!";
        throw std::runtime_error("Cannot open input port for right arm encoders!");
    }

    /* Port for the torso encoders */
    if(!inport_torso_enc_.open("/" + ID_ + "/" + camera_ + "/torso:i"))
    {
        yError() << log_ID_ << "Cannot open input port for torso encoders!";
        throw std::runtime_error("Cannot open input port for torso encoders!");
    }

    /* Port for the right hand analogs */
    if(!inport_right_hand_analogs_.open("/" + ID_ + "/" + camera_ + "/right_analogs:i"))
    {
        yError() << log_ID_ << "Cannot open input port for right hand analogs!";
        throw std::runtime_error("Cannot open input port for right hand analogs!");
    }

    /* Initialize right arm interface */
    yInfo() << log_ID_ << "Setting right arm interface.";

    right_arm_ = iCubArm("right");

    right_arm_.setAllConstraints(false);
    right_arm_.releaseLink(0);
    right_arm_.releaseLink(1);
    right_arm_.releaseLink(2);

    yInfo() << log_ID_ << "...BatchCADSuperimposer ctor completed!";
}

BatchCADSuperimposer::~BatchCADSuperimposer() noexcept { }

void BatchCADSuperimposer::threadRelease()
{
    if (!inport_pose_ext_.isClosed())           inport_pose_ext_.close();
    if (!inport_head_enc_.isClosed())           inport_head_enc_.close();
    if (!inport_torso_enc_.isClosed())          inport_torso_enc_.close();
    if (!inport_right_arm_enc_.isClosed())      inport_right_arm_enc_.close();
    if (!inport_right_hand_analogs_.isClosed()) inport_right_hand_analogs_.close();

    CADSuperimposer::threadRelease();
}

Vector BatchCADSuperimposer::getEndEffectorPose()
{
    Vector* estimates_mean = inport_pose_ext_.read(synch_);

    if (estimates_mean        != YARP_NULLPTR) ext_pose_copy_ = *estimates_mean;
    if (ext_pose_copy_.size() == 0)            ext_pose_copy_ = *(inport_pose_ext_.read(true));

    return ext_pose_copy_;
}

Vector BatchCADSuperimposer::getHeadEncoders()
{
    return *(inport_head_enc_.read(true));
}

Vector BatchCADSuperimposer::getRightArmEncoders()
{
    return *(inport_right_arm_enc_.read(true));
}

#if ICP_USE_ANALOGS == 1
Vector BatchCADSuperimposer::getRightHandAnalogs()
{
    return *(inport_right_hand_analogs_.read(true));
}
#endif

Vector BatchCADSuperimposer::getTorsoEncoders()
{
    Vector enc_torso = *(inport_torso_enc_.read(true));

    std::swap(enc_torso(0), enc_torso(2));

    return enc_torso;
}

void BatchCADSuperimposer::getExtraObjPoseMap(SuperImpose::ObjPoseMap& hand_pose)
{
    if (view_forearm_)
    {
        SuperImpose::ObjPose pose;

        right_arm_.setAng(CTRL_DEG2RAD * readRootToEE());

        Matrix H_forearm = right_arm_.getH(7, true);

        Vector j_x = H_forearm.getCol(3).subVector(0, 2);
        Vector j_o = dcm2axis(H_forearm);

        pose.assign(j_x.data(), j_x.data()+3);
        pose.insert(pose.end(), j_o.data(), j_o.data()+4);

        hand_pose.emplace("forearm", pose);
    }
}

Vector BatchCADSuperimposer::readRootToEE()
{
    Vector enc_arm = getRightArmEncoders();

    Vector root_ee_enc(10);

    root_ee_enc.setSubvector(0, getTorsoEncoders());

    for (size_t i = 0; i < 7; ++i)
        root_ee_enc(i+3) = enc_arm(i);

    return root_ee_enc;
}

bool BatchCADSuperimposer::sync_input(const bool status)
{
    synch_ = status;

    return true;
}

bool BatchCADSuperimposer::render_extra_mesh(const bool status)
{
    view_forearm_ = status;

    return true;
}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name PX4 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.
 *
 ****************************************************************************/

/**
 * @file CollisionPrevention.cpp
 * CollisionPrevention controller.
 *
 */

#include <CollisionPrevention/CollisionPrevention.hpp>
using namespace matrix;
using namespace time_literals;

CollisionPrevention::CollisionPrevention(ModuleParams *parent) :
	ModuleParams(parent)
{
}

CollisionPrevention::~CollisionPrevention()
{
	//unadvertise publishers
	if (_mavlink_log_pub != nullptr) {
		orb_unadvertise(_mavlink_log_pub);
	}
}

void CollisionPrevention::_publishConstrainedSetpoint(const Vector2f &original_setpoint,
		const Vector2f &adapted_setpoint)
{
	collision_constraints_s	constraints{};	/**< collision constraints message */

	//fill in values
	constraints.timestamp = hrt_absolute_time();

	constraints.original_setpoint[0] = original_setpoint(0);
	constraints.original_setpoint[1] = original_setpoint(1);
	constraints.adapted_setpoint[0] = adapted_setpoint(0);
	constraints.adapted_setpoint[1] = adapted_setpoint(1);

	// publish constraints
	if (_constraints_pub != nullptr) {
		orb_publish(ORB_ID(collision_constraints), _constraints_pub, &constraints);

	} else {
		_constraints_pub = orb_advertise(ORB_ID(collision_constraints), &constraints);
	}
}

void CollisionPrevention::_publishObstacleDistance(obstacle_distance_s &obstacle)
{
	// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
	if (_obstacle_distance_pub != nullptr) {
		orb_publish(ORB_ID(obstacle_distance_fused), _obstacle_distance_pub, &obstacle);

	} else {
		_obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance_fused), &obstacle);
	}
}

void CollisionPrevention::_updateOffboardObstacleDistance(obstacle_distance_s &obstacle)
{
	_sub_obstacle_distance.update();
	const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();

	// Update with offboard data if the data is not stale
	if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) {
		obstacle = obstacle_distance;
	}
}

void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle)
{
	for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
		distance_sensor_s distance_sensor;
		_sub_distance_sensor[i].copy(&distance_sensor);

		// consider only instaces with updated, valid data and orientations useful for collision prevention
		if ((hrt_elapsed_time(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
		    (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
		    (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {

			if (obstacle.increment > 0) {
				// data from companion
				obstacle.timestamp = math::max(obstacle.timestamp, distance_sensor.timestamp);
				obstacle.max_distance = math::max((int)obstacle.max_distance,
								  (int)distance_sensor.max_distance * 100);
				obstacle.min_distance = math::min((int)obstacle.min_distance,
								  (int)distance_sensor.min_distance * 100);
				// since the data from the companion are already in the distances data structure,
				// keep the increment that is sent
				obstacle.angle_offset = 0.f; //companion not sending this field (needs mavros update)

			} else {
				obstacle.timestamp = distance_sensor.timestamp;
				obstacle.max_distance = distance_sensor.max_distance * 100; // convert to cm
				obstacle.min_distance = distance_sensor.min_distance * 100; // convert to cm
				memset(&obstacle.distances[0], 0xff, sizeof(obstacle.distances));
				obstacle.increment = math::degrees(distance_sensor.h_fov);
				obstacle.angle_offset = 0.f;
			}

			if ((distance_sensor.current_distance > distance_sensor.min_distance) &&
			    (distance_sensor.current_distance < distance_sensor.max_distance)) {

				float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, obstacle.angle_offset);

				matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
				// convert the sensor orientation from body to local frame in the range [0, 360]
				float sensor_yaw_local_deg  = math::degrees(wrap_2pi(Eulerf(attitude).psi() + sensor_yaw_body_rad));

				// calculate the field of view boundary bin indices
				int lower_bound = (int)floor((sensor_yaw_local_deg  - math::degrees(distance_sensor.h_fov / 2.0f)) /
							     obstacle.increment);
				int upper_bound = (int)floor((sensor_yaw_local_deg  + math::degrees(distance_sensor.h_fov / 2.0f)) /
							     obstacle.increment);

				// if increment is lower than 5deg, use an offset
				const int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]);

				if (((lower_bound < 0 || upper_bound < 0) || (lower_bound >= distances_array_size
						|| upper_bound >= distances_array_size)) && obstacle.increment < 5.f) {
					obstacle.angle_offset = sensor_yaw_local_deg ;
					upper_bound  = abs(upper_bound - lower_bound);
					lower_bound  = 0;
				}

				// rotate vehicle attitude into the sensor body frame
				matrix::Quatf attitude_sensor_frame = attitude;
				attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad));
				float attitude_sensor_frame_pitch = cosf(Eulerf(attitude_sensor_frame).theta());

				for (int bin = lower_bound; bin <= upper_bound; ++bin) {
					int wrap_bin = bin;

					if (wrap_bin < 0) {
						// wrap bin index around the array
						wrap_bin = (int)floor(360.f / obstacle.increment) + bin;
					}

					if (wrap_bin >= distances_array_size) {
						// wrap bin index around the array
						wrap_bin = bin - distances_array_size;
					}

					// compensate measurement for vehicle tilt and convert to cm
					obstacle.distances[wrap_bin] = math::min((int)obstacle.distances[wrap_bin],
								       (int)(100 * distance_sensor.current_distance * attitude_sensor_frame_pitch));
				}
			}
		}
	}

	_publishObstacleDistance(obstacle);
}

void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
		const Vector2f &curr_pos, const Vector2f &curr_vel)
{
	obstacle_distance_s obstacle{};
	_updateOffboardObstacleDistance(obstacle);
	_updateDistanceSensor(obstacle);

	float setpoint_length = setpoint.norm();

	if (hrt_elapsed_time(&obstacle.timestamp) < RANGE_STREAM_TIMEOUT_US) {
		if (setpoint_length > 0.001f) {

			Vector2f setpoint_dir = setpoint / setpoint_length;
			float vel_max = setpoint_length;
			int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]);

			for (int i = 0; i < distances_array_size; i++) {

				if (obstacle.distances[i] < obstacle.max_distance &&
				    obstacle.distances[i] > obstacle.min_distance && (float)i * obstacle.increment < 360.f) {
					float distance = obstacle.distances[i] / 100.0f; //convert to meters
					float angle = math::radians((float)i * obstacle.increment);

					if (obstacle.angle_offset > 0.f) {
						angle += math::radians(obstacle.angle_offset);
					}

					Vector2f bin_direction = {cos(angle), sin(angle)};
					float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));

					//calculate max allowed velocity with a P-controller (same gain as in the position controller)
					float delay_distance = curr_vel_parallel * _param_mpc_col_prev_dly.get();
					float vel_max_posctrl = math::max(0.f,
									  _param_mpc_xy_p.get() * (distance - _param_mpc_col_prev_d.get() - delay_distance));
					Vector2f  vel_max_vec = bin_direction * vel_max_posctrl;
					float vel_max_bin = vel_max_vec.dot(setpoint_dir);
					float vel_setpoint_bin = setpoint.dot(bin_direction);

					//limit the velocity
					//do not react to obstacles more than 70 degree off the given stick input
					if (vel_setpoint_bin > 0.94f * setpoint_length && vel_max_bin >= 0) {
						vel_max = math::min(vel_max, vel_max_bin);
					}
				}
			}

			setpoint = setpoint_dir * vel_max;
		}

	} else {
		// if distance data are stale, switch to Loiter
		_publishVehicleCmdDoLoiter();
		mavlink_log_critical(&_mavlink_log_pub, "No range data received, loitering.");
	}
}

void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed,
		const Vector2f &curr_pos, const Vector2f &curr_vel)
{
	//calculate movement constraints based on range data
	Vector2f new_setpoint = original_setpoint;
	_calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);

	//warn user if collision prevention starts to interfere
	bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed
				      || new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed
				      || new_setpoint(1) < original_setpoint(1) - 0.05f * max_speed
				      || new_setpoint(1) > original_setpoint(1) + 0.05f * max_speed);

	if (currently_interfering && (currently_interfering != _interfering)) {
		mavlink_log_critical(&_mavlink_log_pub, "Collision Warning");
	}

	_interfering = currently_interfering;
	_publishConstrainedSetpoint(original_setpoint, new_setpoint);
	original_setpoint = new_setpoint;
}

void CollisionPrevention::_publishVehicleCmdDoLoiter()
{
	vehicle_command_s command{};
	command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
	command.param1 = (float)1; // base mode
	command.param3 = (float)0; // sub mode
	command.target_system = 1;
	command.target_component = 1;
	command.source_system = 1;
	command.source_component = 1;
	command.confirmation = false;
	command.from_external = false;
	command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
	command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;

	// publish the vehicle command
	if (_pub_vehicle_command == nullptr) {
		_pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command,
				       vehicle_command_s::ORB_QUEUE_LENGTH);

	} else {
		orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command);
	}
}
<commit_msg>change size of reaction angle<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name PX4 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.
 *
 ****************************************************************************/

/**
 * @file CollisionPrevention.cpp
 * CollisionPrevention controller.
 *
 */

#include <CollisionPrevention/CollisionPrevention.hpp>
using namespace matrix;
using namespace time_literals;

CollisionPrevention::CollisionPrevention(ModuleParams *parent) :
	ModuleParams(parent)
{
}

CollisionPrevention::~CollisionPrevention()
{
	//unadvertise publishers
	if (_mavlink_log_pub != nullptr) {
		orb_unadvertise(_mavlink_log_pub);
	}
}

void CollisionPrevention::_publishConstrainedSetpoint(const Vector2f &original_setpoint,
		const Vector2f &adapted_setpoint)
{
	collision_constraints_s	constraints{};	/**< collision constraints message */

	//fill in values
	constraints.timestamp = hrt_absolute_time();

	constraints.original_setpoint[0] = original_setpoint(0);
	constraints.original_setpoint[1] = original_setpoint(1);
	constraints.adapted_setpoint[0] = adapted_setpoint(0);
	constraints.adapted_setpoint[1] = adapted_setpoint(1);

	// publish constraints
	if (_constraints_pub != nullptr) {
		orb_publish(ORB_ID(collision_constraints), _constraints_pub, &constraints);

	} else {
		_constraints_pub = orb_advertise(ORB_ID(collision_constraints), &constraints);
	}
}

void CollisionPrevention::_publishObstacleDistance(obstacle_distance_s &obstacle)
{
	// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
	if (_obstacle_distance_pub != nullptr) {
		orb_publish(ORB_ID(obstacle_distance_fused), _obstacle_distance_pub, &obstacle);

	} else {
		_obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance_fused), &obstacle);
	}
}

void CollisionPrevention::_updateOffboardObstacleDistance(obstacle_distance_s &obstacle)
{
	_sub_obstacle_distance.update();
	const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();

	// Update with offboard data if the data is not stale
	if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) {
		obstacle = obstacle_distance;
	}
}

void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle)
{
	for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
		distance_sensor_s distance_sensor;
		_sub_distance_sensor[i].copy(&distance_sensor);

		// consider only instaces with updated, valid data and orientations useful for collision prevention
		if ((hrt_elapsed_time(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
		    (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
		    (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {

			if (obstacle.increment > 0) {
				// data from companion
				obstacle.timestamp = math::max(obstacle.timestamp, distance_sensor.timestamp);
				obstacle.max_distance = math::max((int)obstacle.max_distance,
								  (int)distance_sensor.max_distance * 100);
				obstacle.min_distance = math::min((int)obstacle.min_distance,
								  (int)distance_sensor.min_distance * 100);
				// since the data from the companion are already in the distances data structure,
				// keep the increment that is sent
				obstacle.angle_offset = 0.f; //companion not sending this field (needs mavros update)

			} else {
				obstacle.timestamp = distance_sensor.timestamp;
				obstacle.max_distance = distance_sensor.max_distance * 100; // convert to cm
				obstacle.min_distance = distance_sensor.min_distance * 100; // convert to cm
				memset(&obstacle.distances[0], 0xff, sizeof(obstacle.distances));
				obstacle.increment = math::degrees(distance_sensor.h_fov);
				obstacle.angle_offset = 0.f;
			}

			if ((distance_sensor.current_distance > distance_sensor.min_distance) &&
			    (distance_sensor.current_distance < distance_sensor.max_distance)) {

				float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, obstacle.angle_offset);

				matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
				// convert the sensor orientation from body to local frame in the range [0, 360]
				float sensor_yaw_local_deg  = math::degrees(wrap_2pi(Eulerf(attitude).psi() + sensor_yaw_body_rad));

				// calculate the field of view boundary bin indices
				int lower_bound = (int)floor((sensor_yaw_local_deg  - math::degrees(distance_sensor.h_fov / 2.0f)) /
							     obstacle.increment);
				int upper_bound = (int)floor((sensor_yaw_local_deg  + math::degrees(distance_sensor.h_fov / 2.0f)) /
							     obstacle.increment);

				// if increment is lower than 5deg, use an offset
				const int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]);

				if (((lower_bound < 0 || upper_bound < 0) || (lower_bound >= distances_array_size
						|| upper_bound >= distances_array_size)) && obstacle.increment < 5.f) {
					obstacle.angle_offset = sensor_yaw_local_deg ;
					upper_bound  = abs(upper_bound - lower_bound);
					lower_bound  = 0;
				}

				// rotate vehicle attitude into the sensor body frame
				matrix::Quatf attitude_sensor_frame = attitude;
				attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad));
				float attitude_sensor_frame_pitch = cosf(Eulerf(attitude_sensor_frame).theta());

				for (int bin = lower_bound; bin <= upper_bound; ++bin) {
					int wrap_bin = bin;

					if (wrap_bin < 0) {
						// wrap bin index around the array
						wrap_bin = (int)floor(360.f / obstacle.increment) + bin;
					}

					if (wrap_bin >= distances_array_size) {
						// wrap bin index around the array
						wrap_bin = bin - distances_array_size;
					}

					// compensate measurement for vehicle tilt and convert to cm
					obstacle.distances[wrap_bin] = math::min((int)obstacle.distances[wrap_bin],
								       (int)(100 * distance_sensor.current_distance * attitude_sensor_frame_pitch));
				}
			}
		}
	}

	_publishObstacleDistance(obstacle);
}

void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
		const Vector2f &curr_pos, const Vector2f &curr_vel)
{
	obstacle_distance_s obstacle{};
	_updateOffboardObstacleDistance(obstacle);
	_updateDistanceSensor(obstacle);

	float setpoint_length = setpoint.norm();

	if (hrt_elapsed_time(&obstacle.timestamp) < RANGE_STREAM_TIMEOUT_US) {
		if (setpoint_length > 0.001f) {

			Vector2f setpoint_dir = setpoint / setpoint_length;
			float vel_max = setpoint_length;
			int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]);

			for (int i = 0; i < distances_array_size; i++) {

				if (obstacle.distances[i] < obstacle.max_distance &&
				    obstacle.distances[i] > obstacle.min_distance && (float)i * obstacle.increment < 360.f) {
					float distance = obstacle.distances[i] / 100.0f; //convert to meters
					float angle = math::radians((float)i * obstacle.increment);

					if (obstacle.angle_offset > 0.f) {
						angle += math::radians(obstacle.angle_offset);
					}

					Vector2f bin_direction = {cos(angle), sin(angle)};
					float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));

					//calculate max allowed velocity with a P-controller (same gain as in the position controller)
					float delay_distance = curr_vel_parallel * _param_mpc_col_prev_dly.get();
					float vel_max_posctrl = math::max(0.f,
									  _param_mpc_xy_p.get() * (distance - _param_mpc_col_prev_d.get() - delay_distance));
					Vector2f  vel_max_vec = bin_direction * vel_max_posctrl;
					float vel_max_bin = vel_max_vec.dot(setpoint_dir);
					float vel_setpoint_bin = setpoint.dot(bin_direction);

					//limit the velocity
					//do not react to obstacles more than 45 degree off the given stick input cos(45deg) = 0.71
					if (vel_setpoint_bin > 0.71f * setpoint_length && vel_max_bin >= 0) {
						vel_max = math::min(vel_max, vel_max_bin);
					}
				}
			}

			setpoint = setpoint_dir * vel_max;
		}

	} else {
		// if distance data are stale, switch to Loiter
		_publishVehicleCmdDoLoiter();
		mavlink_log_critical(&_mavlink_log_pub, "No range data received, loitering.");
	}
}

void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed,
		const Vector2f &curr_pos, const Vector2f &curr_vel)
{
	//calculate movement constraints based on range data
	Vector2f new_setpoint = original_setpoint;
	_calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);

	//warn user if collision prevention starts to interfere
	bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed
				      || new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed
				      || new_setpoint(1) < original_setpoint(1) - 0.05f * max_speed
				      || new_setpoint(1) > original_setpoint(1) + 0.05f * max_speed);

	if (currently_interfering && (currently_interfering != _interfering)) {
		mavlink_log_critical(&_mavlink_log_pub, "Collision Warning");
	}

	_interfering = currently_interfering;
	_publishConstrainedSetpoint(original_setpoint, new_setpoint);
	original_setpoint = new_setpoint;
}

void CollisionPrevention::_publishVehicleCmdDoLoiter()
{
	vehicle_command_s command{};
	command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
	command.param1 = (float)1; // base mode
	command.param3 = (float)0; // sub mode
	command.target_system = 1;
	command.target_component = 1;
	command.source_system = 1;
	command.source_component = 1;
	command.confirmation = false;
	command.from_external = false;
	command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
	command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;

	// publish the vehicle command
	if (_pub_vehicle_command == nullptr) {
		_pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command,
				       vehicle_command_s::ORB_QUEUE_LENGTH);

	} else {
		orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command);
	}
}
<|endoftext|>


<commit_before>/*------------------------------------------------------------------------
* (The MIT License)
*
* Copyright (c) 2008-2011 Rhomobile, Inc.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*
* http://rhomobile.com
*------------------------------------------------------------------------*/

#include "RhoLogSink.h"

#include "common/RhoFile.h"
#include "common/StringConverter.h"
#include "common/RhodesApp.h"
#include "net/RawSocket.h"
#include "net/URI.h"

#if defined( OS_SYMBIAN )
#include <e32debug.h>
#endif

namespace rho {

CLogFileSink::CLogFileSink(const LogSettings& oSettings)
    : m_pFile(0), m_pPosFile(0), m_oLogConf(oSettings), m_nCirclePos(-1), m_nFileLogSize(0)
{

}

CLogFileSink::~CLogFileSink()
{
	if(m_pFile)
		delete m_pFile;
}

void CLogFileSink::writeLogMessage( String& strMsg ){
    unsigned int len = strMsg.length();

    if ( !m_pFile )
        m_pFile = new common::CRhoFile();

    if ( !m_pFile->isOpened() ){
        m_pFile->open( getLogConf().getLogFilePath().c_str(), common::CRhoFile::OpenForAppend );
        m_nFileLogSize = m_pFile->size();
        loadLogPosition();
    }

    if ( getLogConf().getMaxLogFileSize() > 0 )
    {
        if ( ( m_nCirclePos >= 0 && m_nCirclePos + len > getLogConf().getMaxLogFileSize() ) ||
             ( m_nCirclePos < 0 && m_nFileLogSize + len > getLogConf().getMaxLogFileSize() ) )
        {
            m_pFile->movePosToStart();
            m_nFileLogSize = 0;
            m_nCirclePos = 0;
        }
    }

    int nWritten = m_pFile->write( strMsg.c_str(), len );
    m_pFile->flush();

    if ( m_nCirclePos >= 0 )
        m_nCirclePos += nWritten;
    else
        m_nFileLogSize += nWritten;

    saveLogPosition();
}

int CLogFileSink::getCurPos()
{
    return m_nCirclePos >= 0 ? m_nCirclePos : m_nFileLogSize;
}

void CLogFileSink::clear(){
    if ( m_pFile )    {
        delete m_pFile;
        m_pFile = NULL;
    }

    common::CRhoFile().deleteFile(getLogConf().getLogFilePath().c_str());
    String strPosPath = getLogConf().getLogFilePath() + "_pos";
    common::CRhoFile().deleteFile(strPosPath.c_str());
}

void CLogFileSink::loadLogPosition(){
    if ( !m_pPosFile )
        m_pPosFile = new common::CRhoFile();

    if ( !m_pPosFile->isOpened() ){
        String strPosPath = getLogConf().getLogFilePath() + "_pos";
        m_pPosFile->open( strPosPath.c_str(), common::CRhoFile::OpenForReadWrite );
    }

    if ( !m_pPosFile->isOpened() )
        return;

    String strPos;
    m_pPosFile->movePosToStart();
    m_pPosFile->readString(strPos);
    if ( strPos.length() == 0 )
        return;

    m_nCirclePos = atoi(strPos.c_str());
    if ( m_nCirclePos < 0 || m_nCirclePos > (int)m_nFileLogSize )
    	m_nCirclePos = -1;

    if ( m_nCirclePos >= 0 )
        m_pFile->setPosTo( m_nCirclePos );
}

void CLogFileSink::saveLogPosition(){
    if ( m_nCirclePos < 0 )
        return;

    if ( m_nCirclePos > (int)getLogConf().getMaxLogFileSize() )
    	return;

    String strPos = common::convertToStringA(m_nCirclePos);
    for( int i = strPos.length(); i < 10; i++ )
    	strPos += ' ';

    m_pPosFile->movePosToStart();
    m_pPosFile->write( strPos.c_str(), strPos.length() );
    m_pPosFile->flush();
}

#ifndef min
#define min(a,b)            (((a) < (b)) ? (a) : (b))
#endif

void CLogOutputSink::writeLogMessage( String& strMsg )
{
    if ( strMsg.length() > 1 && strMsg[strMsg.length()-2] == '\r' )
        strMsg.erase(strMsg.length()-2,1);

    const char* szMsg = strMsg.c_str();

#if defined( OS_WINDOWS_DESKTOP )
		::OutputDebugStringA(szMsg);
#elif defined( OS_PLATFORM_MOTCE )
		::OutputDebugStringW(common::convertToStringW(strMsg).c_str());
#elif defined(OS_WP8)
		::OutputDebugStringA(szMsg);
#elif defined( OS_SYMBIAN )
        TPtrC8 des((const TUint8*)szMsg);
      	RDebug::RawPrint(des);
        return;
#endif

#if !defined( OS_PLATFORM_MOTCE )
    for( int n = 0; n < (int)strMsg.length(); n+= 100 )
        fwrite(szMsg+n, 1, min(100,strMsg.length()-n) , stdout );

    fflush(stdout);
#endif
}

CLogSocketSink::CLogSocketSink(const LogSettings& oSettings)
{
	m_URL = oSettings.getLogURL();

    CThreadQueue::setLogCategory(LogCategory("NO_LOGGING"));
    setPollInterval(QUEUE_POLL_INTERVAL_INFINITE);
    start(epLow);
}

CLogSocketSink::~CLogSocketSink()
{
	//wait till all commands will be sent to server
    CRhoThread::stop(2000);
}

void CLogSocketSink::setUrl(String url)
{
	m_URL = url;
}

void CLogSocketSink::writeLogMessage( String& strMsg )
{
	addQueueCommand(new LogCommand(m_URL.c_str(), strMsg.c_str()));
}

void CLogSocketSink::processCommand(IQueueCommand* pCmd)
{
    LogCommand *cmd = (LogCommand *)pCmd;
    if (!cmd || (common::CRhodesApp::getInstance()==0))
        return;
#if defined(APP_BUILD_CAPABILITY_SHARED_RUNTIME)

	getNet().doRequest( "GET", cmd->m_url+"?LogSeverity="+cmd->m_body[0]+"&LogComment="+net::URI::urlEncode(cmd->m_body), String(), 0, 0 );
#else
	getNet().doRequest( "POST", cmd->m_url, cmd->m_body, 0, 0 );
#endif
}

}
<commit_msg>comment added<commit_after>/*------------------------------------------------------------------------
* (The MIT License)
*
* Copyright (c) 2008-2011 Rhomobile, Inc.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*
* http://rhomobile.com
*------------------------------------------------------------------------*/

#include "RhoLogSink.h"

#include "common/RhoFile.h"
#include "common/StringConverter.h"
#include "common/RhodesApp.h"
#include "net/RawSocket.h"
#include "net/URI.h"

#if defined( OS_SYMBIAN )
#include <e32debug.h>
#endif

namespace rho {

CLogFileSink::CLogFileSink(const LogSettings& oSettings)
    : m_pFile(0), m_pPosFile(0), m_oLogConf(oSettings), m_nCirclePos(-1), m_nFileLogSize(0)
{

}

CLogFileSink::~CLogFileSink()
{
	if(m_pFile)
		delete m_pFile;
}

void CLogFileSink::writeLogMessage( String& strMsg ){
    unsigned int len = strMsg.length();

    if ( !m_pFile )
        m_pFile = new common::CRhoFile();

    if ( !m_pFile->isOpened() ){
        m_pFile->open( getLogConf().getLogFilePath().c_str(), common::CRhoFile::OpenForAppend );
        m_nFileLogSize = m_pFile->size();
        loadLogPosition();
    }

    if ( getLogConf().getMaxLogFileSize() > 0 )
    {
        if ( ( m_nCirclePos >= 0 && m_nCirclePos + len > getLogConf().getMaxLogFileSize() ) ||
             ( m_nCirclePos < 0 && m_nFileLogSize + len > getLogConf().getMaxLogFileSize() ) )
        {
            m_pFile->movePosToStart();
            m_nFileLogSize = 0;
            m_nCirclePos = 0;
        }
    }

    int nWritten = m_pFile->write( strMsg.c_str(), len );
    m_pFile->flush();

    if ( m_nCirclePos >= 0 )
        m_nCirclePos += nWritten;
    else
        m_nFileLogSize += nWritten;

    saveLogPosition();
}

int CLogFileSink::getCurPos()
{
    return m_nCirclePos >= 0 ? m_nCirclePos : m_nFileLogSize;
}

void CLogFileSink::clear(){
    if ( m_pFile )    {
        delete m_pFile;
        m_pFile = NULL;
    }

    common::CRhoFile().deleteFile(getLogConf().getLogFilePath().c_str());
    String strPosPath = getLogConf().getLogFilePath() + "_pos";
    common::CRhoFile().deleteFile(strPosPath.c_str());
}

void CLogFileSink::loadLogPosition(){
    if ( !m_pPosFile )
        m_pPosFile = new common::CRhoFile();

    if ( !m_pPosFile->isOpened() ){
        String strPosPath = getLogConf().getLogFilePath() + "_pos";
        m_pPosFile->open( strPosPath.c_str(), common::CRhoFile::OpenForReadWrite );
    }

    if ( !m_pPosFile->isOpened() )
        return;

    String strPos;
    m_pPosFile->movePosToStart();
    m_pPosFile->readString(strPos);
    if ( strPos.length() == 0 )
        return;

    m_nCirclePos = atoi(strPos.c_str());
    if ( m_nCirclePos < 0 || m_nCirclePos > (int)m_nFileLogSize )
    	m_nCirclePos = -1;

    if ( m_nCirclePos >= 0 )
        m_pFile->setPosTo( m_nCirclePos );
}

void CLogFileSink::saveLogPosition(){
    if ( m_nCirclePos < 0 )
        return;

    if ( m_nCirclePos > (int)getLogConf().getMaxLogFileSize() )
    	return;

    String strPos = common::convertToStringA(m_nCirclePos);
    for( int i = strPos.length(); i < 10; i++ )
    	strPos += ' ';

    m_pPosFile->movePosToStart();
    m_pPosFile->write( strPos.c_str(), strPos.length() );
    m_pPosFile->flush();
}

#ifndef min
#define min(a,b)            (((a) < (b)) ? (a) : (b))
#endif

void CLogOutputSink::writeLogMessage( String& strMsg )
{
    if ( strMsg.length() > 1 && strMsg[strMsg.length()-2] == '\r' )
        strMsg.erase(strMsg.length()-2,1);

    const char* szMsg = strMsg.c_str();

#if defined( OS_WINDOWS_DESKTOP )
		::OutputDebugStringA(szMsg);
#elif defined( OS_PLATFORM_MOTCE )
		::OutputDebugStringW(common::convertToStringW(strMsg).c_str());
#elif defined(OS_WP8)
		::OutputDebugStringA(szMsg);
#elif defined( OS_SYMBIAN )
        TPtrC8 des((const TUint8*)szMsg);
      	RDebug::RawPrint(des);
        return;
#endif

#if !defined( OS_PLATFORM_MOTCE )
    for( int n = 0; n < (int)strMsg.length(); n+= 100 )
        fwrite(szMsg+n, 1, min(100,strMsg.length()-n) , stdout );

    fflush(stdout);
#endif
}

CLogSocketSink::CLogSocketSink(const LogSettings& oSettings)
{
	m_URL = oSettings.getLogURL();

    CThreadQueue::setLogCategory(LogCategory("NO_LOGGING"));
    setPollInterval(QUEUE_POLL_INTERVAL_INFINITE);
    start(epLow);
}

CLogSocketSink::~CLogSocketSink()
{
	//wait till all commands will be sent to server
    CRhoThread::stop(2000);
}

void CLogSocketSink::setUrl(String url)
{
	m_URL = url;
}

void CLogSocketSink::writeLogMessage( String& strMsg )
{
	addQueueCommand(new LogCommand(m_URL.c_str(), strMsg.c_str()));
}

void CLogSocketSink::processCommand(IQueueCommand* pCmd)
{
    LogCommand *cmd = (LogCommand *)pCmd;

    /*
      Checking CRhodesApp instance is needed because net request requires it to be valid.
      If socket sink is initialized from ext manager, log messages could occur before app instance is set so we will crash.
    */
    if (!cmd || (common::CRhodesApp::getInstance()==0))
        return;
#if defined(APP_BUILD_CAPABILITY_SHARED_RUNTIME)

	getNet().doRequest( "GET", cmd->m_url+"?LogSeverity="+cmd->m_body[0]+"&LogComment="+net::URI::urlEncode(cmd->m_body), String(), 0, 0 );
#else
	getNet().doRequest( "POST", cmd->m_url, cmd->m_body, 0, 0 );
#endif
}

}
<|endoftext|>


<commit_before>// Copyright mogemimi. Distributed under the MIT license.

#include "pomdog/gpu/direct3d/hlsl_compiling.h"
#include "pomdog/basic/conditional_compilation.h"
#include "pomdog/content/utility/binary_reader.h"
#include "pomdog/filesystem/file_system.h"
#include "pomdog/gpu/backends/shader_bytecode.h"
#include "pomdog/gpu/backends/shader_compile_options.h"
#include "pomdog/platform/win32/prerequisites_win32.h"
#include "pomdog/utility/assert.h"
#include "pomdog/utility/errors.h"
#include "pomdog/utility/path_helper.h"

POMDOG_SUPPRESS_WARNINGS_GENERATED_BY_STD_HEADERS_BEGIN
#include <fstream>
#include <string>
#include <utility>
#include <vector>
POMDOG_SUPPRESS_WARNINGS_GENERATED_BY_STD_HEADERS_END

namespace pomdog::gpu::detail::direct3d {
namespace {

[[nodiscard]] std::string
ToString(const ShaderProfile& profile) noexcept
{
    std::string output;

    switch (profile.PipelineStage) {
    case ShaderPipelineStage::VertexShader:
        output += "vs_";
        break;
    case ShaderPipelineStage::PixelShader:
        output += "ps_";
        break;
    //case ShaderPipelineStage::GeometryShader:
    //    output += "gs_";
    //    break;
    //case ShaderPipelineStage::DomainShader:
    //    output += "ds_";
    //    break;
    //case ShaderPipelineStage::ComputeShader:
    //    output += "cs_";
    //    break;
    //case ShaderPipelineStage::HullShader:
    //    output += "hs_";
    //    break;
    }

    output += std::to_string(profile.ShaderModel.Major);
    output += '_';
    output += std::to_string(profile.ShaderModel.Minor);
    return output;
}

class HLSLCodeInclude final : public ID3DInclude {
private:
    std::string currentDirectory;
    std::vector<std::uint8_t> outputSource;
    std::unique_ptr<Error> lastError;

public:
    explicit HLSLCodeInclude(const std::string& curentDirectoryIn)
        : currentDirectory(curentDirectoryIn)
    {
    }

    HRESULT __stdcall Open(D3D_INCLUDE_TYPE includeType, LPCSTR pFileName, [[maybe_unused]] LPCVOID pParentData, LPCVOID* ppData, UINT* pBytes)
    {
        //if (D3D10_INCLUDE_FORCE_DWORD == includeType) {
        //    return E_FAIL;
        //}

        std::string includePath;
        if (D3D_INCLUDE_LOCAL == includeType) {
            includePath = PathHelper::Join(currentDirectory, pFileName);
        }
        else {
            includePath = pFileName;
        }

        if (D3D_INCLUDE_LOCAL != includeType && D3D_INCLUDE_SYSTEM != includeType) {
            return E_FAIL;
        }

#if 0
#if defined(DEBUG) && !defined(NDEBUG)
        if (D3D_INCLUDE_LOCAL == includeType) {
            Log::Internal("ShaderInclude: Local");
        }
        else if (D3D_INCLUDE_SYSTEM == includeType) {
            Log::Internal("ShaderInclude: System");
        }
        Log::Internal(StringHelper::Format("include shader file : %s", includePath.c_str()));
#endif
#endif

        std::ifstream stream{includePath, std::ifstream::binary};

        if (!stream) {
            lastError = errors::New("Could not find a shader source file: " + includePath);
            return E_FAIL;
        }

        auto [size, err] = FileSystem::GetFileSize(includePath);
        if (err != nullptr) {
            lastError = errors::New("failed to get file size: " + includePath);
            return E_FAIL;
        }

        if (size <= 0) {
            lastError = errors::New("The file is too small: " + includePath);
            return E_FAIL;
        }

        outputSource = BinaryReader::ReadArray<std::uint8_t>(stream, size);

        *ppData = outputSource.data();
        *pBytes = static_cast<UINT>(outputSource.size());

        return S_OK;
    }

    HRESULT __stdcall Close([[maybe_unused]] LPCVOID pData)
    {
        outputSource.clear();
        return S_OK;
    }

    std::unique_ptr<Error>
    MoveLastError() noexcept
    {
        return std::move(lastError);
    }
};

[[nodiscard]] std::unique_ptr<Error>
CompileFromShaderFile(
    const ShaderBytecode& shaderBytecode,
    const std::string& entrypoint,
    const std::string& shaderProfile,
    const std::string& currentDirectory,
    const D3D_SHADER_MACRO* preprocessorMacros,
    ID3DBlob** ppBlobOut)
{
    POMDOG_ASSERT(shaderBytecode.Code != nullptr);
    POMDOG_ASSERT(shaderBytecode.ByteLength > 0);

    DWORD shaderFlags = D3DCOMPILE_ENABLE_STRICTNESS;
#if defined(DEBUG) && !defined(NDEBUG)
    shaderFlags |= D3DCOMPILE_DEBUG;
#endif

    HLSLCodeInclude shaderInclude(currentDirectory);
    Microsoft::WRL::ComPtr<ID3DBlob> errorBlob;

    const auto hr = ::D3DCompile(
        shaderBytecode.Code,
        shaderBytecode.ByteLength,
        nullptr,
        preprocessorMacros,
        &shaderInclude,
        entrypoint.c_str(),
        shaderProfile.c_str(),
        shaderFlags,
        0,
        ppBlobOut,
        &errorBlob);

    if (auto err = shaderInclude.MoveLastError(); err != nullptr) {
        return errors::Wrap(std::move(err), "failed to compile shader");
    }

    if (FAILED(hr)) {
        if (errorBlob != nullptr) {
            std::string str(reinterpret_cast<LPCSTR>(errorBlob->GetBufferPointer()), errorBlob->GetBufferSize());
            return errors::New("failed to compile shader. error: " + str);
        }
        return errors::New("D3DCompile() failed. HRESULT = " + std::to_string(hr));
    }

    if (errorBlob != nullptr) {
        std::string str(reinterpret_cast<LPCSTR>(errorBlob->GetBufferPointer()), errorBlob->GetBufferSize());
        return errors::New("warning: " + str);
    }

    return nullptr;
}

} // namespace

std::tuple<Microsoft::WRL::ComPtr<ID3DBlob>, std::unique_ptr<Error>>
CompileHLSL(
    const ShaderBytecode& shaderBytecode,
    const ShaderCompileOptions& compileOptions) noexcept
{
    const auto target = ToString(compileOptions.Profile);
    POMDOG_ASSERT(!target.empty());

    std::vector<D3D_SHADER_MACRO> defines;
    defines.reserve(compileOptions.PreprocessorMacros.size());

    for (auto& macro : compileOptions.PreprocessorMacros) {
        if (macro.Name.empty()) {
            return std::make_tuple(nullptr, errors::New("macro.Name is empty"));
        }

        D3D_SHADER_MACRO shaderMacro;
        shaderMacro.Name = macro.Name.c_str();
        shaderMacro.Definition = macro.Definition.empty() ? nullptr : macro.Definition.c_str();
        defines.push_back(std::move(shaderMacro));
    }

    if (!defines.empty()) {
        D3D_SHADER_MACRO shaderMacro;
        shaderMacro.Name = nullptr;
        shaderMacro.Definition = nullptr;
        defines.push_back(std::move(shaderMacro));
    }

    Microsoft::WRL::ComPtr<ID3DBlob> codeBlob;
    if (auto err = CompileFromShaderFile(shaderBytecode,
            compileOptions.EntryPoint,
            target,
            compileOptions.CurrentDirectory,
            (defines.empty() ? nullptr : defines.data()),
            &codeBlob);
        err != nullptr) {
        return std::make_tuple(nullptr, errors::Wrap(std::move(err), "CompileFromShaderFile() failed"));
    }

    return std::make_tuple(std::move(codeBlob), nullptr);
}

} // namespace pomdog::gpu::detail::direct3d
<commit_msg>Add missing using directive<commit_after>// Copyright mogemimi. Distributed under the MIT license.

#include "pomdog/gpu/direct3d/hlsl_compiling.h"
#include "pomdog/basic/conditional_compilation.h"
#include "pomdog/content/utility/binary_reader.h"
#include "pomdog/filesystem/file_system.h"
#include "pomdog/gpu/backends/shader_bytecode.h"
#include "pomdog/gpu/backends/shader_compile_options.h"
#include "pomdog/platform/win32/prerequisites_win32.h"
#include "pomdog/utility/assert.h"
#include "pomdog/utility/errors.h"
#include "pomdog/utility/path_helper.h"

POMDOG_SUPPRESS_WARNINGS_GENERATED_BY_STD_HEADERS_BEGIN
#include <fstream>
#include <string>
#include <utility>
#include <vector>
POMDOG_SUPPRESS_WARNINGS_GENERATED_BY_STD_HEADERS_END

using pomdog::detail::BinaryReader;

namespace pomdog::gpu::detail::direct3d {
namespace {

[[nodiscard]] std::string
ToString(const ShaderProfile& profile) noexcept
{
    std::string output;

    switch (profile.PipelineStage) {
    case ShaderPipelineStage::VertexShader:
        output += "vs_";
        break;
    case ShaderPipelineStage::PixelShader:
        output += "ps_";
        break;
    //case ShaderPipelineStage::GeometryShader:
    //    output += "gs_";
    //    break;
    //case ShaderPipelineStage::DomainShader:
    //    output += "ds_";
    //    break;
    //case ShaderPipelineStage::ComputeShader:
    //    output += "cs_";
    //    break;
    //case ShaderPipelineStage::HullShader:
    //    output += "hs_";
    //    break;
    }

    output += std::to_string(profile.ShaderModel.Major);
    output += '_';
    output += std::to_string(profile.ShaderModel.Minor);
    return output;
}

class HLSLCodeInclude final : public ID3DInclude {
private:
    std::string currentDirectory;
    std::vector<std::uint8_t> outputSource;
    std::unique_ptr<Error> lastError;

public:
    explicit HLSLCodeInclude(const std::string& curentDirectoryIn)
        : currentDirectory(curentDirectoryIn)
    {
    }

    HRESULT __stdcall Open(D3D_INCLUDE_TYPE includeType, LPCSTR pFileName, [[maybe_unused]] LPCVOID pParentData, LPCVOID* ppData, UINT* pBytes)
    {
        //if (D3D10_INCLUDE_FORCE_DWORD == includeType) {
        //    return E_FAIL;
        //}

        std::string includePath;
        if (D3D_INCLUDE_LOCAL == includeType) {
            includePath = PathHelper::Join(currentDirectory, pFileName);
        }
        else {
            includePath = pFileName;
        }

        if (D3D_INCLUDE_LOCAL != includeType && D3D_INCLUDE_SYSTEM != includeType) {
            return E_FAIL;
        }

#if 0
#if defined(DEBUG) && !defined(NDEBUG)
        if (D3D_INCLUDE_LOCAL == includeType) {
            Log::Internal("ShaderInclude: Local");
        }
        else if (D3D_INCLUDE_SYSTEM == includeType) {
            Log::Internal("ShaderInclude: System");
        }
        Log::Internal(StringHelper::Format("include shader file : %s", includePath.c_str()));
#endif
#endif

        std::ifstream stream{includePath, std::ifstream::binary};

        if (!stream) {
            lastError = errors::New("Could not find a shader source file: " + includePath);
            return E_FAIL;
        }

        auto [size, err] = FileSystem::GetFileSize(includePath);
        if (err != nullptr) {
            lastError = errors::New("failed to get file size: " + includePath);
            return E_FAIL;
        }

        if (size <= 0) {
            lastError = errors::New("The file is too small: " + includePath);
            return E_FAIL;
        }

        outputSource = BinaryReader::ReadArray<std::uint8_t>(stream, size);

        *ppData = outputSource.data();
        *pBytes = static_cast<UINT>(outputSource.size());

        return S_OK;
    }

    HRESULT __stdcall Close([[maybe_unused]] LPCVOID pData)
    {
        outputSource.clear();
        return S_OK;
    }

    std::unique_ptr<Error>
    MoveLastError() noexcept
    {
        return std::move(lastError);
    }
};

[[nodiscard]] std::unique_ptr<Error>
CompileFromShaderFile(
    const ShaderBytecode& shaderBytecode,
    const std::string& entrypoint,
    const std::string& shaderProfile,
    const std::string& currentDirectory,
    const D3D_SHADER_MACRO* preprocessorMacros,
    ID3DBlob** ppBlobOut)
{
    POMDOG_ASSERT(shaderBytecode.Code != nullptr);
    POMDOG_ASSERT(shaderBytecode.ByteLength > 0);

    DWORD shaderFlags = D3DCOMPILE_ENABLE_STRICTNESS;
#if defined(DEBUG) && !defined(NDEBUG)
    shaderFlags |= D3DCOMPILE_DEBUG;
#endif

    HLSLCodeInclude shaderInclude(currentDirectory);
    Microsoft::WRL::ComPtr<ID3DBlob> errorBlob;

    const auto hr = ::D3DCompile(
        shaderBytecode.Code,
        shaderBytecode.ByteLength,
        nullptr,
        preprocessorMacros,
        &shaderInclude,
        entrypoint.c_str(),
        shaderProfile.c_str(),
        shaderFlags,
        0,
        ppBlobOut,
        &errorBlob);

    if (auto err = shaderInclude.MoveLastError(); err != nullptr) {
        return errors::Wrap(std::move(err), "failed to compile shader");
    }

    if (FAILED(hr)) {
        if (errorBlob != nullptr) {
            std::string str(reinterpret_cast<LPCSTR>(errorBlob->GetBufferPointer()), errorBlob->GetBufferSize());
            return errors::New("failed to compile shader. error: " + str);
        }
        return errors::New("D3DCompile() failed. HRESULT = " + std::to_string(hr));
    }

    if (errorBlob != nullptr) {
        std::string str(reinterpret_cast<LPCSTR>(errorBlob->GetBufferPointer()), errorBlob->GetBufferSize());
        return errors::New("warning: " + str);
    }

    return nullptr;
}

} // namespace

std::tuple<Microsoft::WRL::ComPtr<ID3DBlob>, std::unique_ptr<Error>>
CompileHLSL(
    const ShaderBytecode& shaderBytecode,
    const ShaderCompileOptions& compileOptions) noexcept
{
    const auto target = ToString(compileOptions.Profile);
    POMDOG_ASSERT(!target.empty());

    std::vector<D3D_SHADER_MACRO> defines;
    defines.reserve(compileOptions.PreprocessorMacros.size());

    for (auto& macro : compileOptions.PreprocessorMacros) {
        if (macro.Name.empty()) {
            return std::make_tuple(nullptr, errors::New("macro.Name is empty"));
        }

        D3D_SHADER_MACRO shaderMacro;
        shaderMacro.Name = macro.Name.c_str();
        shaderMacro.Definition = macro.Definition.empty() ? nullptr : macro.Definition.c_str();
        defines.push_back(std::move(shaderMacro));
    }

    if (!defines.empty()) {
        D3D_SHADER_MACRO shaderMacro;
        shaderMacro.Name = nullptr;
        shaderMacro.Definition = nullptr;
        defines.push_back(std::move(shaderMacro));
    }

    Microsoft::WRL::ComPtr<ID3DBlob> codeBlob;
    if (auto err = CompileFromShaderFile(shaderBytecode,
            compileOptions.EntryPoint,
            target,
            compileOptions.CurrentDirectory,
            (defines.empty() ? nullptr : defines.data()),
            &codeBlob);
        err != nullptr) {
        return std::make_tuple(nullptr, errors::Wrap(std::move(err), "CompileFromShaderFile() failed"));
    }

    return std::make_tuple(std::move(codeBlob), nullptr);
}

} // namespace pomdog::gpu::detail::direct3d
<|endoftext|>