<commit_before>#include "symbol.h"
#include <math.h>
#include <assert.h>

Symbol Symbol::Addition       ("+", OPERATOR, 2, "+");
Symbol Symbol::Multiplication ("*", OPERATOR, 2, "*");
Symbol Symbol::Division       ("/", OPERATOR, 2, "/");
Symbol Symbol::Substraction   ("-", OPERATOR, 2, "-");
Symbol Symbol::Power          ("^", FUNCTION, 2, "pow");

Symbol Symbol::Modulo         ("%", FUNCTION, 2, "fmod");
Symbol Symbol::Interrogation  ("?", FUNCTION, 3, "interrogation");
Symbol Symbol::Equal          ("==", FUNCTION, 2, "equal");
Symbol Symbol::Equal2         ("=", FUNCTION, 2, "equal");
Symbol Symbol::NotEqual       ("!=", FUNCTION, 2, "notEqual");
Symbol Symbol::Inferior       ("<=", FUNCTION, 2, "inferior");
Symbol Symbol::InferiorStrict ("<", FUNCTION, 2, "inferiorStrict");
Symbol Symbol::Superior       (">=", FUNCTION, 2, "superior");
Symbol Symbol::SuperiorStrict (">", FUNCTION, 2, "superiorStrict");
Symbol Symbol::And            ("&", FUNCTION, 2, "mt_and");
Symbol Symbol::Or             ("|", FUNCTION, 2, "mt_or");
Symbol Symbol::AndNot         ("&!", FUNCTION, 2, "mt_andNot");
Symbol Symbol::Xor            ("", "@", FUNCTION, 2, "mt_xor");
Symbol Symbol::AndUB          ("&u", FUNCTION, 2, "andUB");
Symbol Symbol::OrUB           ("|u", FUNCTION, 2, "orUB");
Symbol Symbol::XorUB          ("u", "@u", FUNCTION, 2, "xorUB");
Symbol Symbol::NegateUB       ("~u", FUNCTION, 1, "negateUB");
Symbol Symbol::PosShiftUB     ("<<", "<<u", FUNCTION, 2, "posshiftUB");
Symbol Symbol::NegShiftUB     (">>", ">>u", FUNCTION, 2, "negshiftUB");
Symbol Symbol::AndSB          ("&s", FUNCTION, 2, "andSB");
Symbol Symbol::OrSB           ("|s", FUNCTION, 2, "orSB");
Symbol Symbol::XorSB          ("s", "@s", FUNCTION, 2, "xorSB");
Symbol Symbol::NegateSB       ("~s", FUNCTION, 1, "negateSB");
Symbol Symbol::PosShiftSB     ("<<s", FUNCTION, 2, "posshiftSB");
Symbol Symbol::NegShiftSB     (">>s", FUNCTION, 2, "negshiftSB");

Symbol Symbol::Pi             ("pi", 3.1415927f, NUMBER, 0, "");
Symbol Symbol::X              ("x", VARIABLE_X, 0, "");
Symbol Symbol::Y              ("y", VARIABLE_Y, 0, "");
Symbol Symbol::Z              ("z", VARIABLE_Z, 0, "");

Symbol Symbol::Cos            ("cos", FUNCTION, 1, "cos");
Symbol Symbol::Sin            ("sin", FUNCTION, 1, "sin");
Symbol Symbol::Tan            ("tan", FUNCTION, 1, "tan");
Symbol Symbol::Log            ("log", FUNCTION, 1, "log");
Symbol Symbol::Exp            ("exp", FUNCTION, 1, "exp");
Symbol Symbol::Abs            ("abs", FUNCTION, 1, "fabs");
Symbol Symbol::Atan           ("atan", FUNCTION, 1, "atan");
Symbol Symbol::Acos           ("acos", FUNCTION, 1, "acos");
Symbol Symbol::Asin           ("asin", FUNCTION, 1, "asin");
Symbol Symbol::Round          ("round", FUNCTION, 1, "round");
Symbol Symbol::Clip           ("clip", FUNCTION, 3, "clamp");
Symbol Symbol::Min            ("min", FUNCTION, 2, "min");
Symbol Symbol::Max            ("max", FUNCTION, 2, "max");
Symbol Symbol::Ceil           ("ceil", FUNCTION, 1, "ceil");
Symbol Symbol::Floor          ("floor", FUNCTION, 1, "floor");
Symbol Symbol::Trunc          ("trunc", FUNCTION, 1, "trunc");

Symbol::Symbol() :
type(UNDEFINED), value(""), value2("")
{
}

Symbol::Symbol(std::string value, Type type, int nParameter, std::string op) :
type(type), value(value), value2(""), nParameter(nParameter), code(op)
{
}

Symbol::Symbol(std::string value, std::string value2, Type type, int nParameter, std::string op) :
type(type), value(value), value2(value2), nParameter(nParameter), code(op)
{
}

Symbol::Symbol(std::string value, float dValue, Type type, int nParameter, std::string op) :
type(type), value(value), value2(""), nParameter(nParameter), code(op)
{
}

Context::Context(const std::deque<Symbol> &expression)
{
   nPos = -1;
   nSymbols = expression.size();
   pSymbols = new Symbol[nSymbols];

   auto it = expression.begin();

   for ( int i = 0; i < nSymbols; i++, it++ )
      pSymbols[i] = *it;
}

Context::~Context()
{
   delete[] pSymbols;
}

std::string Context::rec_infix()
{
    const Symbol &s = pSymbols[--nPos];

    switch ( s.type )
    {
    case Symbol::VARIABLE_X:
    case Symbol::VARIABLE_Y:
    case Symbol::VARIABLE_Z:
    case Symbol::NUMBER: return s.value;
    case Symbol::FUNCTION:
        if (s.nParameter == 1) {
            return s.code + "(" + rec_infix() + ")";
        } else if (s.nParameter == 2) {
            auto op2 = rec_infix();
            return s.code + "(" + rec_infix() + "," + op2 + ")";
        } else {
            auto op3 = rec_infix();
            auto op2 = rec_infix();
            return s.code + "(" + rec_infix() + "," + op2 + "," + op3 + ")";
        }
    case Symbol::OPERATOR:
        {
            auto op2 = rec_infix();
            return "(" + rec_infix() + s.code + op2 + ")";
        }
    default:
        assert(0);
        return "";
    }
}

std::string Context::infix()
{
   nPos = nSymbols;

   return rec_infix();
}
<commit_msg>Automatically "convert" integers to floats in expressions<commit_after>#include "symbol.h"
#include <math.h>
#include <assert.h>

Symbol Symbol::Addition       ("+", OPERATOR, 2, "+");
Symbol Symbol::Multiplication ("*", OPERATOR, 2, "*");
Symbol Symbol::Division       ("/", OPERATOR, 2, "/");
Symbol Symbol::Substraction   ("-", OPERATOR, 2, "-");
Symbol Symbol::Power          ("^", FUNCTION, 2, "pow");

Symbol Symbol::Modulo         ("%", FUNCTION, 2, "fmod");
Symbol Symbol::Interrogation  ("?", FUNCTION, 3, "interrogation");
Symbol Symbol::Equal          ("==", FUNCTION, 2, "equal");
Symbol Symbol::Equal2         ("=", FUNCTION, 2, "equal");
Symbol Symbol::NotEqual       ("!=", FUNCTION, 2, "notEqual");
Symbol Symbol::Inferior       ("<=", FUNCTION, 2, "inferior");
Symbol Symbol::InferiorStrict ("<", FUNCTION, 2, "inferiorStrict");
Symbol Symbol::Superior       (">=", FUNCTION, 2, "superior");
Symbol Symbol::SuperiorStrict (">", FUNCTION, 2, "superiorStrict");
Symbol Symbol::And            ("&", FUNCTION, 2, "mt_and");
Symbol Symbol::Or             ("|", FUNCTION, 2, "mt_or");
Symbol Symbol::AndNot         ("&!", FUNCTION, 2, "mt_andNot");
Symbol Symbol::Xor            ("", "@", FUNCTION, 2, "mt_xor");
Symbol Symbol::AndUB          ("&u", FUNCTION, 2, "andUB");
Symbol Symbol::OrUB           ("|u", FUNCTION, 2, "orUB");
Symbol Symbol::XorUB          ("u", "@u", FUNCTION, 2, "xorUB");
Symbol Symbol::NegateUB       ("~u", FUNCTION, 1, "negateUB");
Symbol Symbol::PosShiftUB     ("<<", "<<u", FUNCTION, 2, "posshiftUB");
Symbol Symbol::NegShiftUB     (">>", ">>u", FUNCTION, 2, "negshiftUB");
Symbol Symbol::AndSB          ("&s", FUNCTION, 2, "andSB");
Symbol Symbol::OrSB           ("|s", FUNCTION, 2, "orSB");
Symbol Symbol::XorSB          ("s", "@s", FUNCTION, 2, "xorSB");
Symbol Symbol::NegateSB       ("~s", FUNCTION, 1, "negateSB");
Symbol Symbol::PosShiftSB     ("<<s", FUNCTION, 2, "posshiftSB");
Symbol Symbol::NegShiftSB     (">>s", FUNCTION, 2, "negshiftSB");

Symbol Symbol::Pi             ("pi", 3.1415927f, NUMBER, 0, "");
Symbol Symbol::X              ("x", VARIABLE_X, 0, "");
Symbol Symbol::Y              ("y", VARIABLE_Y, 0, "");
Symbol Symbol::Z              ("z", VARIABLE_Z, 0, "");

Symbol Symbol::Cos            ("cos", FUNCTION, 1, "cos");
Symbol Symbol::Sin            ("sin", FUNCTION, 1, "sin");
Symbol Symbol::Tan            ("tan", FUNCTION, 1, "tan");
Symbol Symbol::Log            ("log", FUNCTION, 1, "log");
Symbol Symbol::Exp            ("exp", FUNCTION, 1, "exp");
Symbol Symbol::Abs            ("abs", FUNCTION, 1, "fabs");
Symbol Symbol::Atan           ("atan", FUNCTION, 1, "atan");
Symbol Symbol::Acos           ("acos", FUNCTION, 1, "acos");
Symbol Symbol::Asin           ("asin", FUNCTION, 1, "asin");
Symbol Symbol::Round          ("round", FUNCTION, 1, "round");
Symbol Symbol::Clip           ("clip", FUNCTION, 3, "clamp");
Symbol Symbol::Min            ("min", FUNCTION, 2, "min");
Symbol Symbol::Max            ("max", FUNCTION, 2, "max");
Symbol Symbol::Ceil           ("ceil", FUNCTION, 1, "ceil");
Symbol Symbol::Floor          ("floor", FUNCTION, 1, "floor");
Symbol Symbol::Trunc          ("trunc", FUNCTION, 1, "trunc");

Symbol::Symbol() :
type(UNDEFINED), value(""), value2("")
{
}

Symbol::Symbol(std::string value, Type type, int nParameter, std::string op) :
type(type), value(value), value2(""), nParameter(nParameter), code(op)
{
}

Symbol::Symbol(std::string value, std::string value2, Type type, int nParameter, std::string op) :
type(type), value(value), value2(value2), nParameter(nParameter), code(op)
{
}

Symbol::Symbol(std::string value, float dValue, Type type, int nParameter, std::string op) :
type(type), value(value), value2(""), nParameter(nParameter), code(op)
{
}

Context::Context(const std::deque<Symbol> &expression)
{
   nPos = -1;
   nSymbols = expression.size();
   pSymbols = new Symbol[nSymbols];

   auto it = expression.begin();

   for ( int i = 0; i < nSymbols; i++, it++ )
      pSymbols[i] = *it;
}

Context::~Context()
{
   delete[] pSymbols;
}

std::string Context::rec_infix()
{
    const Symbol &s = pSymbols[--nPos];

    switch ( s.type )
    {
    case Symbol::VARIABLE_X:
    case Symbol::VARIABLE_Y:
    case Symbol::VARIABLE_Z:
        return s.value;
    case Symbol::NUMBER:
        if (s.value.find('.') == std::string::npos)
            return s.value + ".0f";
        return s.value;
    case Symbol::FUNCTION:
        if (s.nParameter == 1) {
            return s.code + "(" + rec_infix() + ")";
        } else if (s.nParameter == 2) {
            auto op2 = rec_infix();
            return s.code + "(" + rec_infix() + "," + op2 + ")";
        } else {
            auto op3 = rec_infix();
            auto op2 = rec_infix();
            return s.code + "(" + rec_infix() + "," + op2 + "," + op3 + ")";
        }
    case Symbol::OPERATOR:
        {
            auto op2 = rec_infix();
            return "(" + rec_infix() + s.code + op2 + ")";
        }
    default:
        assert(0);
        return "";
    }
}

std::string Context::infix()
{
   nPos = nSymbols;

   return rec_infix();
}
<|endoftext|>


<commit_before>/**
 * @file
 * @brief Implementation of [LCIOWriter] module
 * @copyright Copyright (c) 2017 CERN and the Allpix Squared authors.
 * This software is distributed under the terms of the MIT License, copied verbatim in the file "LICENSE.md".
 * In applying this license, CERN does not waive the privileges and immunities granted to it by virtue of its status as an
 * Intergovernmental Organization or submit itself to any jurisdiction.
 */

#include "LCIOWriterModule.hpp"

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

#include "core/messenger/Messenger.hpp"

#include "core/utils/log.h"

#include <IMPL/LCCollectionVec.h>
#include <IMPL/LCEventImpl.h>
#include <IMPL/LCRunHeaderImpl.h>
#include <IMPL/TrackerDataImpl.h>
#include <IO/LCWriter.h>
#include <IOIMPL/LCFactory.h>
#include <UTIL/CellIDEncoder.h>
#include <lcio.h>

using namespace allpix;
using namespace lcio;

LCIOWriterModule::LCIOWriterModule(Configuration config, Messenger* messenger, GeometryManager* geo)
    : Module(std::move(config)), geo_mgr_(geo) {

    // Bind pixel hits message
    messenger->bindMulti(this, &LCIOWriterModule::pixel_messages_, MsgFlags::REQUIRED);

    // get all detector names and assign id.
    auto detectors = geo_mgr_->getDetectors();
    unsigned int i = 0;
    for(const auto& det : detectors) {
        detectorIDs_[det->getName()] = i;
        LOG(DEBUG) << det->getName() << " has ID " << detectorIDs_[det->getName()];
        i++;
    }

    // Set configuration defaults:
    config_.setDefault("file_name", "output.slcio");
    config_.setDefault("geometry_file", "allpix_squared_gear.xml");
    config_.setDefault("pixel_type", 2);
    config_.setDefault("detector_name", "EUTelescope");
    config_.setDefault("output_collection_name", "zsdata_m26");

    pixelType_ = config_.get<int>("pixel_type");
    DetectorName_ = config_.get<std::string>("detector_name");
    OutputCollectionName_ = config_.get<std::string>("output_collection_name");
}

void LCIOWriterModule::init() {
    // Create the output GEAR file for the detector geometry
    geometry_file_name_ = getOutputPath(config_.get<std::string>("geometry_file"), true);

    // Open LCIO file and write run header
    lcio_file_name_ = getOutputPath(config_.get<std::string>("file_name"));
    lcWriter_ = LCFactory::getInstance()->createLCWriter();
    lcWriter_->open(lcio_file_name_, LCIO::WRITE_NEW);
    auto run = std::make_unique<LCRunHeaderImpl>();
    run->setRunNumber(1);
    run->setDetectorName(DetectorName_);
    lcWriter_->writeRunHeader(run.get());
}

void LCIOWriterModule::run(unsigned int eventNb) {

    auto evt = std::make_unique<LCEventImpl>(); // create the event
    evt->setRunNumber(1);
    evt->setEventNumber(static_cast<int>(eventNb)); // set the event attributes
    evt->parameters().setValue("EventType", 2);

    // Prepare charge vectors
    std::vector<std::vector<float>> charges;
    for(unsigned int i = 0; i < detectorIDs_.size(); i++) {
        std::vector<float> charge;
        charges.push_back(charge);
    }

    // Receive all pixel messages, fill charge vectors
    for(const auto& hit_msg : pixel_messages_) {
        LOG(DEBUG) << hit_msg->getDetector()->getName();
        for(const auto& hitdata : hit_msg->getData()) {
            LOG(DEBUG) << "X: " << hitdata.getPixel().getIndex().x() << ", Y:" << hitdata.getPixel().getIndex().y()
                       << ", Signal: " << hitdata.getSignal();

            unsigned int detectorID = detectorIDs_[hit_msg->getDetector()->getName()];

            switch(pixelType_) {
            case 1: // EUTelSimpleSparsePixel
                charges[detectorID].push_back(static_cast<float>(hitdata.getPixel().getIndex().x())); // x
                charges[detectorID].push_back(static_cast<float>(hitdata.getPixel().getIndex().y())); // y
                charges[detectorID].push_back(static_cast<float>(hitdata.getSignal()));               // signal
                break;
            case 2:  // EUTelGenericSparsePixel
            default: // EUTelGenericSparsePixel is default
                charges[detectorID].push_back(static_cast<float>(hitdata.getPixel().getIndex().x())); // x
                charges[detectorID].push_back(static_cast<float>(hitdata.getPixel().getIndex().y())); // y
                charges[detectorID].push_back(static_cast<float>(hitdata.getSignal()));               // signal
                charges[detectorID].push_back(static_cast<float>(0));                                 // time
                break;
            case 5: // EUTelTimepix3SparsePixel
                charges[detectorID].push_back(static_cast<float>(hitdata.getPixel().getIndex().x())); // x
                charges[detectorID].push_back(static_cast<float>(hitdata.getPixel().getIndex().y())); // y
                charges[detectorID].push_back(static_cast<float>(hitdata.getSignal()));               // signal
                charges[detectorID].push_back(static_cast<float>(0));                                 // time
                charges[detectorID].push_back(static_cast<float>(0));                                 // time
                charges[detectorID].push_back(static_cast<float>(0));                                 // time
                charges[detectorID].push_back(static_cast<float>(0));                                 // time
                break;
            }
        }
    }

    // Prepare hitvector
    LCCollectionVec* hitVec = new LCCollectionVec(LCIO::TRACKERDATA);

    // Fill hitvector with event data
    for(unsigned int detectorID = 0; detectorID < detectorIDs_.size(); detectorID++) {
        auto hit = new TrackerDataImpl();
        CellIDEncoder<TrackerDataImpl> sparseDataEncoder("sensorID:7,sparsePixelType:5", hitVec);
        sparseDataEncoder["sensorID"] = detectorID;
        sparseDataEncoder["sparsePixelType"] = pixelType_;
        sparseDataEncoder.setCellID(hit);
        hit->setChargeValues(charges[detectorID]);
        hitVec->push_back(hit);
    }

    // Add collection to event and write event to LCIO file
    evt->addCollection(hitVec, OutputCollectionName_); // add the collection with a name
    lcWriter_->writeEvent(evt.get());                  // write the event to the file
    write_cnt_++;
}

void LCIOWriterModule::finalize() {
    lcWriter_->close();
    // Print statistics
    LOG(STATUS) << "Wrote " << write_cnt_ << " events to file:" << std::endl << lcio_file_name_;

    // Write geometry:
    std::ofstream geometry_file;
    if(!geometry_file_name_.empty()) {
        geometry_file.open(geometry_file_name_, std::ios_base::out | std::ios_base::trunc);
        if(!geometry_file.good()) {
            throw ModuleError("Cannot write to GEAR geometry file");
        }

        auto detectors = geo_mgr_->getDetectors();
        geometry_file << "<?xml version=\"1.0\" encoding=\"utf-8\"?>" << std::endl
                      << "<!-- ?xml-stylesheet type=\"text/xsl\" href=\"https://cern.ch/allpix-squared/\"? -->" << std::endl
                      << "<gear>" << std::endl;

        geometry_file << "  <global detectorName=\"" << DetectorName_ << "\"/>" << std::endl;
        geometry_file << "  <detectors>" << std::endl;
        geometry_file << "    <detector name=\"SiPlanes\" geartype=\"SiPlanesParameters\">" << std::endl;
        geometry_file << "      <siplanesType type=\"TelescopeWithDUT\">" << std::endl;
        geometry_file << "      <siplanesNumber number=\"" << detectors.size() << "\">" << std::endl;
        geometry_file << "      <layers>" << std::endl;

        for(auto& detector : detectors) {
            // Write header for the layer:
            geometry_file << "<!-- Allpix Squared Detector: " << detector->getName() << " - type: " << detector->getType()
                          << " -->" << std::endl;
            geometry_file << "        <layer>" << std::endl;

            auto position = detector->getPosition();
            auto orientation = detector->getOrientation();

            auto model = detector->getModel();
            auto npixels = model->getNPixels();
            auto pitch = model->getPixelSize();

            auto total_size = model->getSize();
            auto sensitive_size = model->getSensorSize();

            // Write ladder
            geometry_file << "          <ladder ID=\"" << detectorIDs_[detector->getName()] << "\"" << std::endl;
            geometry_file << "            positionX=\"" << position.x() << "\"\tpositionY=\"" << position.y()
                          << "\"\tpositionZ=\"" << position.z() << "\"" << std::endl;
            // FIXME rotation
            geometry_file << "            sizeX=\"" << total_size.x() << "\"\tsizeY=\"" << total_size.y()
                          << "\"\tthickness=\"" << total_size.z() << "\"" << std::endl;
            geometry_file << "            radLength=\"93.65\"" << std::endl;
            geometry_file << "            />" << std::endl;

            // Write sensitive
            geometry_file << "          <sensitive ID=\"" << detectorIDs_[detector->getName()] << "\"" << std::endl;
            geometry_file << "            positionX=\"" << position.x() << "\"\tpositionY=\"" << position.x()
                          << "\"\tpositionZ=\"" << position.x() << "\"" << std::endl;
            geometry_file << "            sizeX=\"" << sensitive_size.x() << "\"\tsizeY=\"" << sensitive_size.y()
                          << "\"\tthickness=\"" << sensitive_size.z() << "\"" << std::endl;
            geometry_file << "            npixelsX=\"" << npixels.x() << "\"\tnpixelsY=\"" << npixels.y() << "\""
                          << std::endl;
            geometry_file << "            pitchX=\"" << pitch.x() << "\"\tpitchY=\"" << pitch.y() << "\"\tresolution=\""
                          << (pitch.x() / std::sqrt(12)) << "\"" << std::endl;

            std::vector<double> components(9);
            orientation.GetComponents(components.begin(), components.end());
            geometry_file << "            rotation1=\"" << components.at(0) << "\"\trotation2=\"" << components.at(1) << "\""
                          << std::endl;
            geometry_file << "            rotation3=\"" << components.at(3) << "\"\trotation4=\"" << components.at(4) << "\""
                          << std::endl;
            geometry_file << "            radLength=\"93.65\"" << std::endl;
            geometry_file << "            />" << std::endl;

            // End the layer:
            geometry_file << "        </layer>" << std::endl;
        }

        // Close XML tree:
        geometry_file << "      </layers>" << std::endl
                      << "    </detector>" << std::endl
                      << "  </detectors>" << std::endl
                      << "</gear>" << std::endl;

        LOG(STATUS) << "Wrote GEAR geometry to file:" << std::endl << geometry_file_name_;
    }
}

LCIOWriterModule::~LCIOWriterModule() {
    delete lcWriter_;
}
<commit_msg>LCIOWriter: add explicit conversion to be sure to always write out millimeters<commit_after>/**
 * @file
 * @brief Implementation of [LCIOWriter] module
 * @copyright Copyright (c) 2017 CERN and the Allpix Squared authors.
 * This software is distributed under the terms of the MIT License, copied verbatim in the file "LICENSE.md".
 * In applying this license, CERN does not waive the privileges and immunities granted to it by virtue of its status as an
 * Intergovernmental Organization or submit itself to any jurisdiction.
 */

#include "LCIOWriterModule.hpp"

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

#include "core/messenger/Messenger.hpp"

#include "core/utils/log.h"

#include <IMPL/LCCollectionVec.h>
#include <IMPL/LCEventImpl.h>
#include <IMPL/LCRunHeaderImpl.h>
#include <IMPL/TrackerDataImpl.h>
#include <IO/LCWriter.h>
#include <IOIMPL/LCFactory.h>
#include <UTIL/CellIDEncoder.h>
#include <lcio.h>

using namespace allpix;
using namespace lcio;

LCIOWriterModule::LCIOWriterModule(Configuration config, Messenger* messenger, GeometryManager* geo)
    : Module(std::move(config)), geo_mgr_(geo) {

    // Bind pixel hits message
    messenger->bindMulti(this, &LCIOWriterModule::pixel_messages_, MsgFlags::REQUIRED);

    // get all detector names and assign id.
    auto detectors = geo_mgr_->getDetectors();
    unsigned int i = 0;
    for(const auto& det : detectors) {
        detectorIDs_[det->getName()] = i;
        LOG(DEBUG) << det->getName() << " has ID " << detectorIDs_[det->getName()];
        i++;
    }

    // Set configuration defaults:
    config_.setDefault("file_name", "output.slcio");
    config_.setDefault("geometry_file", "allpix_squared_gear.xml");
    config_.setDefault("pixel_type", 2);
    config_.setDefault("detector_name", "EUTelescope");
    config_.setDefault("output_collection_name", "zsdata_m26");

    pixelType_ = config_.get<int>("pixel_type");
    DetectorName_ = config_.get<std::string>("detector_name");
    OutputCollectionName_ = config_.get<std::string>("output_collection_name");
}

void LCIOWriterModule::init() {
    // Create the output GEAR file for the detector geometry
    geometry_file_name_ = getOutputPath(config_.get<std::string>("geometry_file"), true);

    // Open LCIO file and write run header
    lcio_file_name_ = getOutputPath(config_.get<std::string>("file_name"));
    lcWriter_ = LCFactory::getInstance()->createLCWriter();
    lcWriter_->open(lcio_file_name_, LCIO::WRITE_NEW);
    auto run = std::make_unique<LCRunHeaderImpl>();
    run->setRunNumber(1);
    run->setDetectorName(DetectorName_);
    lcWriter_->writeRunHeader(run.get());
}

void LCIOWriterModule::run(unsigned int eventNb) {

    auto evt = std::make_unique<LCEventImpl>(); // create the event
    evt->setRunNumber(1);
    evt->setEventNumber(static_cast<int>(eventNb)); // set the event attributes
    evt->parameters().setValue("EventType", 2);

    // Prepare charge vectors
    std::vector<std::vector<float>> charges;
    for(unsigned int i = 0; i < detectorIDs_.size(); i++) {
        std::vector<float> charge;
        charges.push_back(charge);
    }

    // Receive all pixel messages, fill charge vectors
    for(const auto& hit_msg : pixel_messages_) {
        LOG(DEBUG) << hit_msg->getDetector()->getName();
        for(const auto& hitdata : hit_msg->getData()) {
            LOG(DEBUG) << "X: " << hitdata.getPixel().getIndex().x() << ", Y:" << hitdata.getPixel().getIndex().y()
                       << ", Signal: " << hitdata.getSignal();

            unsigned int detectorID = detectorIDs_[hit_msg->getDetector()->getName()];

            switch(pixelType_) {
            case 1: // EUTelSimpleSparsePixel
                charges[detectorID].push_back(static_cast<float>(hitdata.getPixel().getIndex().x())); // x
                charges[detectorID].push_back(static_cast<float>(hitdata.getPixel().getIndex().y())); // y
                charges[detectorID].push_back(static_cast<float>(hitdata.getSignal()));               // signal
                break;
            case 2:  // EUTelGenericSparsePixel
            default: // EUTelGenericSparsePixel is default
                charges[detectorID].push_back(static_cast<float>(hitdata.getPixel().getIndex().x())); // x
                charges[detectorID].push_back(static_cast<float>(hitdata.getPixel().getIndex().y())); // y
                charges[detectorID].push_back(static_cast<float>(hitdata.getSignal()));               // signal
                charges[detectorID].push_back(static_cast<float>(0));                                 // time
                break;
            case 5: // EUTelTimepix3SparsePixel
                charges[detectorID].push_back(static_cast<float>(hitdata.getPixel().getIndex().x())); // x
                charges[detectorID].push_back(static_cast<float>(hitdata.getPixel().getIndex().y())); // y
                charges[detectorID].push_back(static_cast<float>(hitdata.getSignal()));               // signal
                charges[detectorID].push_back(static_cast<float>(0));                                 // time
                charges[detectorID].push_back(static_cast<float>(0));                                 // time
                charges[detectorID].push_back(static_cast<float>(0));                                 // time
                charges[detectorID].push_back(static_cast<float>(0));                                 // time
                break;
            }
        }
    }

    // Prepare hitvector
    LCCollectionVec* hitVec = new LCCollectionVec(LCIO::TRACKERDATA);

    // Fill hitvector with event data
    for(unsigned int detectorID = 0; detectorID < detectorIDs_.size(); detectorID++) {
        auto hit = new TrackerDataImpl();
        CellIDEncoder<TrackerDataImpl> sparseDataEncoder("sensorID:7,sparsePixelType:5", hitVec);
        sparseDataEncoder["sensorID"] = detectorID;
        sparseDataEncoder["sparsePixelType"] = pixelType_;
        sparseDataEncoder.setCellID(hit);
        hit->setChargeValues(charges[detectorID]);
        hitVec->push_back(hit);
    }

    // Add collection to event and write event to LCIO file
    evt->addCollection(hitVec, OutputCollectionName_); // add the collection with a name
    lcWriter_->writeEvent(evt.get());                  // write the event to the file
    write_cnt_++;
}

void LCIOWriterModule::finalize() {
    lcWriter_->close();
    // Print statistics
    LOG(STATUS) << "Wrote " << write_cnt_ << " events to file:" << std::endl << lcio_file_name_;

    // Write geometry:
    std::ofstream geometry_file;
    if(!geometry_file_name_.empty()) {
        geometry_file.open(geometry_file_name_, std::ios_base::out | std::ios_base::trunc);
        if(!geometry_file.good()) {
            throw ModuleError("Cannot write to GEAR geometry file");
        }

        auto detectors = geo_mgr_->getDetectors();
        geometry_file << "<?xml version=\"1.0\" encoding=\"utf-8\"?>" << std::endl
                      << "<!-- ?xml-stylesheet type=\"text/xsl\" href=\"https://cern.ch/allpix-squared/\"? -->" << std::endl
                      << "<gear>" << std::endl;

        geometry_file << "  <global detectorName=\"" << DetectorName_ << "\"/>" << std::endl;
        geometry_file << "  <detectors>" << std::endl;
        geometry_file << "    <detector name=\"SiPlanes\" geartype=\"SiPlanesParameters\">" << std::endl;
        geometry_file << "      <siplanesType type=\"TelescopeWithDUT\">" << std::endl;
        geometry_file << "      <siplanesNumber number=\"" << detectors.size() << "\">" << std::endl;
        geometry_file << "      <layers>" << std::endl;

        for(auto& detector : detectors) {
            // Write header for the layer:
            geometry_file << "      <!-- Allpix Squared Detector: " << detector->getName()
                          << " - type: " << detector->getType() << " -->" << std::endl;
            geometry_file << "        <layer>" << std::endl;

            auto position = detector->getPosition();
            auto orientation = detector->getOrientation();

            auto model = detector->getModel();
            auto npixels = model->getNPixels();
            auto pitch = model->getPixelSize();

            auto total_size = model->getSize();
            auto sensitive_size = model->getSensorSize();

            // Write ladder
            geometry_file << "          <ladder ID=\"" << detectorIDs_[detector->getName()] << "\"" << std::endl;
            geometry_file << "            positionX=\"" << Units::convert(position.x(), "mm") << "\"\tpositionY=\""
                          << Units::convert(position.y(), "mm") << "\"\tpositionZ=\"" << Units::convert(position.z(), "mm")
                          << "\"" << std::endl;
            // FIXME rotation
            geometry_file << "            sizeX=\"" << Units::convert(total_size.x(), "mm") << "\"\tsizeY=\""
                          << Units::convert(total_size.y(), "mm") << "\"\tthickness=\""
                          << Units::convert(total_size.z(), "mm") << "\"" << std::endl;
            geometry_file << "            radLength=\"93.65\"" << std::endl;
            geometry_file << "            />" << std::endl;

            // Write sensitive
            geometry_file << "          <sensitive ID=\"" << detectorIDs_[detector->getName()] << "\"" << std::endl;
            geometry_file << "            positionX=\"" << Units::convert(position.x(), "mm") << "\"\tpositionY=\""
                          << Units::convert(position.y(), "mm") << "\"\tpositionZ=\"" << Units::convert(position.z(), "mm")
                          << "\"" << std::endl;
            geometry_file << "            sizeX=\"" << Units::convert(sensitive_size.x(), "mm") << "\"\tsizeY=\""
                          << Units::convert(sensitive_size.y(), "mm") << "\"\tthickness=\""
                          << Units::convert(sensitive_size.z(), "mm") << "\"" << std::endl;
            geometry_file << "            npixelsX=\"" << npixels.x() << "\"\tnpixelsY=\"" << npixels.y() << "\""
                          << std::endl;
            geometry_file << "            pitchX=\"" << Units::convert(pitch.x(), "mm") << "\"\tpitchY=\""
                          << Units::convert(pitch.y(), "mm") << "\"\tresolution=\""
                          << Units::convert(pitch.x() / std::sqrt(12), "mm") << "\"" << std::endl;

            std::vector<double> components(9);
            orientation.GetComponents(components.begin(), components.end());
            geometry_file << "            rotation1=\"" << components.at(0) << "\"\trotation2=\"" << components.at(1) << "\""
                          << std::endl;
            geometry_file << "            rotation3=\"" << components.at(3) << "\"\trotation4=\"" << components.at(4) << "\""
                          << std::endl;
            geometry_file << "            radLength=\"93.65\"" << std::endl;
            geometry_file << "            />" << std::endl;

            // End the layer:
            geometry_file << "        </layer>" << std::endl;
        }

        // Close XML tree:
        geometry_file << "      </layers>" << std::endl
                      << "    </detector>" << std::endl
                      << "  </detectors>" << std::endl
                      << "</gear>" << std::endl;

        LOG(STATUS) << "Wrote GEAR geometry to file:" << std::endl << geometry_file_name_;
    }
}

LCIOWriterModule::~LCIOWriterModule() {
    delete lcWriter_;
}
<|endoftext|>


<commit_before>/*
 *  Copyright (c) 2012 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */

#include "modules/rtp_rtcp/source/producer_fec.h"

#include "modules/rtp_rtcp/source/forward_error_correction.h"
#include "modules/rtp_rtcp/source/rtp_utility.h"

namespace webrtc {

enum { kREDForFECHeaderLength = 1 };
// This controls the maximum amount of excess overhead (actual - target)
// allowed in order to trigger GenerateFEC(), before |params_.max_fec_frames|
// is reached. Overhead here is defined as relative to number of media packets.
enum { kMaxExcessOverhead = 50 };  // Q8.
// This is the minimum number of media packets required (above some protection
// level) in order to trigger GenerateFEC(), before |params_.max_fec_frames| is
// reached.
enum { kMinimumMediaPackets = 4 };
// Threshold on the received FEC protection level, above which we enforce at
// least |kMinimumMediaPackets| packets for the FEC code. Below this
// threshold |kMinimumMediaPackets| is set to default value of 1.
enum { kHighProtectionThreshold = 80 };  // Corresponds to ~30 overhead, range
// is 0 to 255, where 255 corresponds to 100% overhead (relative to number of
// media packets).

struct RtpPacket {
  WebRtc_UWord16 rtpHeaderLength;
  ForwardErrorCorrection::Packet* pkt;
};

RedPacket::RedPacket(int length)
    : data_(new uint8_t[length]),
      length_(length),
      header_length_(0) {
}

RedPacket::~RedPacket() {
  delete [] data_;
}

void RedPacket::CreateHeader(const uint8_t* rtp_header, int header_length,
                             int red_pl_type, int pl_type) {
  assert(header_length + kREDForFECHeaderLength <= length_);
  memcpy(data_, rtp_header, header_length);
  // Replace payload type.
  data_[1] &= 0x80;
  data_[1] += red_pl_type;
  // Add RED header
  // f-bit always 0
  data_[header_length] = pl_type;
  header_length_ = header_length + kREDForFECHeaderLength;
}

void RedPacket::SetSeqNum(int seq_num) {
  assert(seq_num >= 0 && seq_num < (1<<16));
  ModuleRTPUtility::AssignUWord16ToBuffer(&data_[2], seq_num);
}

void RedPacket::AssignPayload(const uint8_t* payload, int length) {
  assert(header_length_ + length <= length_);
  memcpy(data_ + header_length_, payload, length);
}

void RedPacket::ClearMarkerBit() {
  data_[1] &= 0x7F;
}

uint8_t* RedPacket::data() const {
  return data_;
}

int RedPacket::length() const {
  return length_;
}

ProducerFec::ProducerFec(ForwardErrorCorrection* fec)
    : fec_(fec),
      media_packets_fec_(),
      fec_packets_(),
      num_frames_(0),
      incomplete_frame_(false),
      num_first_partition_(0),
      minimum_media_packets_fec_(1),
      params_(),
      new_params_() {
  memset(&params_, 0, sizeof(params_));
  memset(&new_params_, 0, sizeof(new_params_));
}

ProducerFec::~ProducerFec() {
  DeletePackets();
}

void ProducerFec::SetFecParameters(const FecProtectionParams* params,
                                   int num_first_partition) {
  // Number of first partition packets cannot exceed kMaxMediaPackets
  assert(params->fec_rate >= 0 && params->fec_rate < 256);
  if (num_first_partition >
      static_cast<int>(ForwardErrorCorrection::kMaxMediaPackets)) {
      num_first_partition =
          ForwardErrorCorrection::kMaxMediaPackets;
  }
  // Store the new params and apply them for the next set of FEC packets being
  // produced.
  new_params_ = *params;
  num_first_partition_ = num_first_partition;
  if (params->fec_rate > kHighProtectionThreshold) {
    minimum_media_packets_fec_ = kMinimumMediaPackets;
  } else {
    minimum_media_packets_fec_ = 1;
  }
}

RedPacket* ProducerFec::BuildRedPacket(const uint8_t* data_buffer,
                                       int payload_length,
                                       int rtp_header_length,
                                       int red_pl_type) {
  RedPacket* red_packet = new RedPacket(payload_length +
                                        kREDForFECHeaderLength +
                                        rtp_header_length);
  int pl_type = data_buffer[1] & 0x7f;
  red_packet->CreateHeader(data_buffer, rtp_header_length,
                           red_pl_type, pl_type);
  red_packet->AssignPayload(data_buffer + rtp_header_length, payload_length);
  return red_packet;
}

int ProducerFec::AddRtpPacketAndGenerateFec(const uint8_t* data_buffer,
                                            int payload_length,
                                            int rtp_header_length) {
  assert(fec_packets_.empty());
  if (media_packets_fec_.empty()) {
    params_ = new_params_;
  }
  incomplete_frame_ = true;
  const bool marker_bit = (data_buffer[1] & kRtpMarkerBitMask) ? true : false;
  if (media_packets_fec_.size() < ForwardErrorCorrection::kMaxMediaPackets) {
    // Generic FEC can only protect up to kMaxMediaPackets packets.
    ForwardErrorCorrection::Packet* packet = new ForwardErrorCorrection::Packet;
    packet->length = payload_length + rtp_header_length;
    memcpy(packet->data, data_buffer, packet->length);
    media_packets_fec_.push_back(packet);
  }
  if (marker_bit) {
    ++num_frames_;
    incomplete_frame_ = false;
  }
  // Produce FEC over at most |params_.max_fec_frames| frames, or as soon as:
  // (1) the excess overhead (actual overhead - requested/target overhead) is
  // less than |kMaxExcessOverhead|, and
  // (2) at least |minimum_media_packets_fec_| media packets is reached.
  if (!incomplete_frame_ &&
      (num_frames_ == params_.max_fec_frames ||
          (ExcessOverheadBelowMax() && MinimumMediaPacketsReached()))) {
    assert(num_first_partition_ <=
           static_cast<int>(ForwardErrorCorrection::kMaxMediaPackets));
    int ret = fec_->GenerateFEC(media_packets_fec_,
                                params_.fec_rate,
                                num_first_partition_,
                                params_.use_uep_protection,
                                params_.fec_mask_type,
                                &fec_packets_);
    if (fec_packets_.empty()) {
      num_frames_ = 0;
      DeletePackets();
    }
    return ret;
  }
  return 0;
}

// Returns true if the excess overhead (actual - target) for the FEC is below
// the amount |kMaxExcessOverhead|. This effects the lower protection level
// cases and low number of media packets/frame. The target overhead is given by
// |params_.fec_rate|, and is only achievable in the limit of large number of
// media packets.
bool ProducerFec::ExcessOverheadBelowMax() {
  return ((Overhead() - params_.fec_rate) < kMaxExcessOverhead);
}

// Returns true if the media packet list for the FEC is at least
// |minimum_media_packets_fec_|. This condition tries to capture the effect
// that, for the same amount of protection/overhead, longer codes
// (e.g. (2k,2m) vs (k,m)) are generally more effective at recovering losses.
bool ProducerFec::MinimumMediaPacketsReached() {
  float avg_num_packets_frame = static_cast<float>(media_packets_fec_.size() /
                                                   num_frames_);
  if (avg_num_packets_frame < 2.0f) {
  return (static_cast<int>(media_packets_fec_.size()) >=
      minimum_media_packets_fec_);
  } else {
    // For larger rates (more packets/frame), increase the threshold.
    return (static_cast<int>(media_packets_fec_.size()) >=
        minimum_media_packets_fec_ + 1);
  }
}

bool ProducerFec::FecAvailable() const {
  return (fec_packets_.size() > 0);
}

RedPacket* ProducerFec::GetFecPacket(int red_pl_type,
                                     int fec_pl_type,
                                     uint16_t seq_num,
                                     int rtp_header_length) {
  if (fec_packets_.empty())
    return NULL;
  // Build FEC packet. The FEC packets in |fec_packets_| doesn't
  // have RTP headers, so we're reusing the header from the last
  // media packet.
  ForwardErrorCorrection::Packet* packet_to_send = fec_packets_.front();
  ForwardErrorCorrection::Packet* last_media_packet = media_packets_fec_.back();
  RedPacket* return_packet = new RedPacket(packet_to_send->length +
                                           kREDForFECHeaderLength +
                                           rtp_header_length);
  return_packet->CreateHeader(last_media_packet->data,
                              rtp_header_length,
                              red_pl_type,
                              fec_pl_type);
  return_packet->SetSeqNum(seq_num);
  return_packet->ClearMarkerBit();
  return_packet->AssignPayload(packet_to_send->data, packet_to_send->length);
  fec_packets_.pop_front();
  if (fec_packets_.empty()) {
    // Done with all the FEC packets. Reset for next run.
    DeletePackets();
    num_frames_ = 0;
  }
  return return_packet;
}

int ProducerFec::Overhead() const {
  // Overhead is defined as relative to the number of media packets, and not
  // relative to total number of packets. This definition is inhereted from the
  // protection factor produced by video_coding module and how the FEC
  // generation is implemented.
  assert(!media_packets_fec_.empty());
  int num_fec_packets = fec_->GetNumberOfFecPackets(media_packets_fec_.size(),
                                                    params_.fec_rate);
  // Return the overhead in Q8.
  return (num_fec_packets << 8) / media_packets_fec_.size();
}

void ProducerFec::DeletePackets() {
  while (!media_packets_fec_.empty()) {
    delete media_packets_fec_.front();
    media_packets_fec_.pop_front();
  }
  assert(media_packets_fec_.empty());
}

}  // namespace webrtc
<commit_msg>Fix integer divisin truncation error.<commit_after>/*
 *  Copyright (c) 2012 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */

#include "modules/rtp_rtcp/source/producer_fec.h"

#include "modules/rtp_rtcp/source/forward_error_correction.h"
#include "modules/rtp_rtcp/source/rtp_utility.h"

namespace webrtc {

enum { kREDForFECHeaderLength = 1 };
// This controls the maximum amount of excess overhead (actual - target)
// allowed in order to trigger GenerateFEC(), before |params_.max_fec_frames|
// is reached. Overhead here is defined as relative to number of media packets.
enum { kMaxExcessOverhead = 50 };  // Q8.
// This is the minimum number of media packets required (above some protection
// level) in order to trigger GenerateFEC(), before |params_.max_fec_frames| is
// reached.
enum { kMinimumMediaPackets = 4 };
// Threshold on the received FEC protection level, above which we enforce at
// least |kMinimumMediaPackets| packets for the FEC code. Below this
// threshold |kMinimumMediaPackets| is set to default value of 1.
enum { kHighProtectionThreshold = 80 };  // Corresponds to ~30 overhead, range
// is 0 to 255, where 255 corresponds to 100% overhead (relative to number of
// media packets).

struct RtpPacket {
  WebRtc_UWord16 rtpHeaderLength;
  ForwardErrorCorrection::Packet* pkt;
};

RedPacket::RedPacket(int length)
    : data_(new uint8_t[length]),
      length_(length),
      header_length_(0) {
}

RedPacket::~RedPacket() {
  delete [] data_;
}

void RedPacket::CreateHeader(const uint8_t* rtp_header, int header_length,
                             int red_pl_type, int pl_type) {
  assert(header_length + kREDForFECHeaderLength <= length_);
  memcpy(data_, rtp_header, header_length);
  // Replace payload type.
  data_[1] &= 0x80;
  data_[1] += red_pl_type;
  // Add RED header
  // f-bit always 0
  data_[header_length] = pl_type;
  header_length_ = header_length + kREDForFECHeaderLength;
}

void RedPacket::SetSeqNum(int seq_num) {
  assert(seq_num >= 0 && seq_num < (1<<16));
  ModuleRTPUtility::AssignUWord16ToBuffer(&data_[2], seq_num);
}

void RedPacket::AssignPayload(const uint8_t* payload, int length) {
  assert(header_length_ + length <= length_);
  memcpy(data_ + header_length_, payload, length);
}

void RedPacket::ClearMarkerBit() {
  data_[1] &= 0x7F;
}

uint8_t* RedPacket::data() const {
  return data_;
}

int RedPacket::length() const {
  return length_;
}

ProducerFec::ProducerFec(ForwardErrorCorrection* fec)
    : fec_(fec),
      media_packets_fec_(),
      fec_packets_(),
      num_frames_(0),
      incomplete_frame_(false),
      num_first_partition_(0),
      minimum_media_packets_fec_(1),
      params_(),
      new_params_() {
  memset(&params_, 0, sizeof(params_));
  memset(&new_params_, 0, sizeof(new_params_));
}

ProducerFec::~ProducerFec() {
  DeletePackets();
}

void ProducerFec::SetFecParameters(const FecProtectionParams* params,
                                   int num_first_partition) {
  // Number of first partition packets cannot exceed kMaxMediaPackets
  assert(params->fec_rate >= 0 && params->fec_rate < 256);
  if (num_first_partition >
      static_cast<int>(ForwardErrorCorrection::kMaxMediaPackets)) {
      num_first_partition =
          ForwardErrorCorrection::kMaxMediaPackets;
  }
  // Store the new params and apply them for the next set of FEC packets being
  // produced.
  new_params_ = *params;
  num_first_partition_ = num_first_partition;
  if (params->fec_rate > kHighProtectionThreshold) {
    minimum_media_packets_fec_ = kMinimumMediaPackets;
  } else {
    minimum_media_packets_fec_ = 1;
  }
}

RedPacket* ProducerFec::BuildRedPacket(const uint8_t* data_buffer,
                                       int payload_length,
                                       int rtp_header_length,
                                       int red_pl_type) {
  RedPacket* red_packet = new RedPacket(payload_length +
                                        kREDForFECHeaderLength +
                                        rtp_header_length);
  int pl_type = data_buffer[1] & 0x7f;
  red_packet->CreateHeader(data_buffer, rtp_header_length,
                           red_pl_type, pl_type);
  red_packet->AssignPayload(data_buffer + rtp_header_length, payload_length);
  return red_packet;
}

int ProducerFec::AddRtpPacketAndGenerateFec(const uint8_t* data_buffer,
                                            int payload_length,
                                            int rtp_header_length) {
  assert(fec_packets_.empty());
  if (media_packets_fec_.empty()) {
    params_ = new_params_;
  }
  incomplete_frame_ = true;
  const bool marker_bit = (data_buffer[1] & kRtpMarkerBitMask) ? true : false;
  if (media_packets_fec_.size() < ForwardErrorCorrection::kMaxMediaPackets) {
    // Generic FEC can only protect up to kMaxMediaPackets packets.
    ForwardErrorCorrection::Packet* packet = new ForwardErrorCorrection::Packet;
    packet->length = payload_length + rtp_header_length;
    memcpy(packet->data, data_buffer, packet->length);
    media_packets_fec_.push_back(packet);
  }
  if (marker_bit) {
    ++num_frames_;
    incomplete_frame_ = false;
  }
  // Produce FEC over at most |params_.max_fec_frames| frames, or as soon as:
  // (1) the excess overhead (actual overhead - requested/target overhead) is
  // less than |kMaxExcessOverhead|, and
  // (2) at least |minimum_media_packets_fec_| media packets is reached.
  if (!incomplete_frame_ &&
      (num_frames_ == params_.max_fec_frames ||
          (ExcessOverheadBelowMax() && MinimumMediaPacketsReached()))) {
    assert(num_first_partition_ <=
           static_cast<int>(ForwardErrorCorrection::kMaxMediaPackets));
    int ret = fec_->GenerateFEC(media_packets_fec_,
                                params_.fec_rate,
                                num_first_partition_,
                                params_.use_uep_protection,
                                params_.fec_mask_type,
                                &fec_packets_);
    if (fec_packets_.empty()) {
      num_frames_ = 0;
      DeletePackets();
    }
    return ret;
  }
  return 0;
}

// Returns true if the excess overhead (actual - target) for the FEC is below
// the amount |kMaxExcessOverhead|. This effects the lower protection level
// cases and low number of media packets/frame. The target overhead is given by
// |params_.fec_rate|, and is only achievable in the limit of large number of
// media packets.
bool ProducerFec::ExcessOverheadBelowMax() {
  return ((Overhead() - params_.fec_rate) < kMaxExcessOverhead);
}

// Returns true if the media packet list for the FEC is at least
// |minimum_media_packets_fec_|. This condition tries to capture the effect
// that, for the same amount of protection/overhead, longer codes
// (e.g. (2k,2m) vs (k,m)) are generally more effective at recovering losses.
bool ProducerFec::MinimumMediaPacketsReached() {
  float avg_num_packets_frame = static_cast<float>(media_packets_fec_.size()) /
                                num_frames_;
  if (avg_num_packets_frame < 2.0f) {
  return (static_cast<int>(media_packets_fec_.size()) >=
      minimum_media_packets_fec_);
  } else {
    // For larger rates (more packets/frame), increase the threshold.
    return (static_cast<int>(media_packets_fec_.size()) >=
        minimum_media_packets_fec_ + 1);
  }
}

bool ProducerFec::FecAvailable() const {
  return (fec_packets_.size() > 0);
}

RedPacket* ProducerFec::GetFecPacket(int red_pl_type,
                                     int fec_pl_type,
                                     uint16_t seq_num,
                                     int rtp_header_length) {
  if (fec_packets_.empty())
    return NULL;
  // Build FEC packet. The FEC packets in |fec_packets_| doesn't
  // have RTP headers, so we're reusing the header from the last
  // media packet.
  ForwardErrorCorrection::Packet* packet_to_send = fec_packets_.front();
  ForwardErrorCorrection::Packet* last_media_packet = media_packets_fec_.back();
  RedPacket* return_packet = new RedPacket(packet_to_send->length +
                                           kREDForFECHeaderLength +
                                           rtp_header_length);
  return_packet->CreateHeader(last_media_packet->data,
                              rtp_header_length,
                              red_pl_type,
                              fec_pl_type);
  return_packet->SetSeqNum(seq_num);
  return_packet->ClearMarkerBit();
  return_packet->AssignPayload(packet_to_send->data, packet_to_send->length);
  fec_packets_.pop_front();
  if (fec_packets_.empty()) {
    // Done with all the FEC packets. Reset for next run.
    DeletePackets();
    num_frames_ = 0;
  }
  return return_packet;
}

int ProducerFec::Overhead() const {
  // Overhead is defined as relative to the number of media packets, and not
  // relative to total number of packets. This definition is inhereted from the
  // protection factor produced by video_coding module and how the FEC
  // generation is implemented.
  assert(!media_packets_fec_.empty());
  int num_fec_packets = fec_->GetNumberOfFecPackets(media_packets_fec_.size(),
                                                    params_.fec_rate);
  // Return the overhead in Q8.
  return (num_fec_packets << 8) / media_packets_fec_.size();
}

void ProducerFec::DeletePackets() {
  while (!media_packets_fec_.empty()) {
    delete media_packets_fec_.front();
    media_packets_fec_.pop_front();
  }
  assert(media_packets_fec_.empty());
}

}  // namespace webrtc
<|endoftext|>


<commit_before>/**
   Copyright (c) 2015 Beckhoff Automation GmbH & Co. KG

   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.
 */

#include "AmsConnection.h"
#include "Log.h"

AmsResponse::AmsResponse()
    : frame(4096),
    invokeId(0)
{}

void AmsResponse::Notify()
{
    invokeId = 0;
    cv.notify_all();
}

bool AmsResponse::Wait(uint32_t timeout_ms)
{
    std::unique_lock<std::mutex> lock(mutex);
    return cv.wait_for(lock, std::chrono::milliseconds(timeout_ms), [&]() {
        return !invokeId;
    });
}

std::shared_ptr<NotificationDispatcher> AmsConnection::DispatcherListAdd(const VirtualConnection& connection)
{
    const auto dispatcher = DispatcherListGet(connection);
    if (dispatcher) {
        return dispatcher;
    }
    std::lock_guard<std::recursive_mutex> lock(dispatcherListMutex);
    return dispatcherList.emplace(connection,
                                  std::make_shared<NotificationDispatcher>(*this, connection)).first->second;
}

std::shared_ptr<NotificationDispatcher> AmsConnection::DispatcherListGet(const VirtualConnection& connection)
{
    std::lock_guard<std::recursive_mutex> lock(dispatcherListMutex);

    const auto it = dispatcherList.find(connection);
    if (it != dispatcherList.end()) {
        return it->second;
    }
    return std::shared_ptr<NotificationDispatcher>();
}

AmsConnection::AmsConnection(Router& __router, IpV4 __destIp)
    : router(__router),
    socket(__destIp, ADS_TCP_SERVER_PORT),
    invokeId(0),
    destIp(__destIp),
    ownIp(socket.Connect())
{
    receiver = std::thread(&AmsConnection::TryRecv, this);
}

AmsConnection::~AmsConnection()
{
    socket.Shutdown();
    receiver.join();
}

NotifyMapping AmsConnection::CreateNotifyMapping(uint32_t hNotify, Notification& notification)
{
    const auto dispatcher = DispatcherListAdd(notification.connection);
    notification.hNotify(hNotify);
    dispatcher->Emplace(hNotify, notification);
    return NotifyMapping {hNotify, dispatcher};
}

long AmsConnection::DeleteNotification(const AmsAddr& amsAddr, uint32_t hNotify, uint32_t tmms, uint16_t port)
{
    AmsRequest request {
        amsAddr,
        port, AoEHeader::DEL_DEVICE_NOTIFICATION,
        0, nullptr, nullptr,
        sizeof(hNotify)
    };
    request.frame.prepend(qToLittleEndian(hNotify));

    return AdsRequest<AoEResponseHeader>(request, tmms);
}

AmsResponse* AmsConnection::Write(Frame& request, const AmsAddr destAddr, const AmsAddr srcAddr, uint16_t cmdId)
{
    AoEHeader aoeHeader { destAddr.netId, destAddr.port, srcAddr.netId, srcAddr.port, cmdId,
                          static_cast<uint32_t>(request.size()), GetInvokeId() };
    request.prepend<AoEHeader>(aoeHeader);

    AmsTcpHeader header { static_cast<uint32_t>(request.size()) };
    request.prepend<AmsTcpHeader>(header);

    auto response = Reserve(aoeHeader.invokeId(), srcAddr.port);

    if (!response) {
        return nullptr;
    }

    if (request.size() != socket.write(request)) {
        Release(response);
        return nullptr;
    }
    return response;
}

uint32_t AmsConnection::GetInvokeId()
{
    uint32_t result;
    do {
        result = invokeId.fetch_add(1);
    } while (!result);
    return result;
}

AmsResponse* AmsConnection::GetPending(uint32_t id, uint16_t port)
{
    const uint32_t currentId = queue[port - Router::PORT_BASE].invokeId;
    if (currentId == id) {
        return &queue[port - Router::PORT_BASE];
    }
    LOG_WARN("InvokeId missmatch: waiting for 0x" << std::hex << currentId << " received 0x" << id);
    return nullptr;
}

AmsResponse* AmsConnection::Reserve(uint32_t id, uint16_t port)
{
    uint32_t isFree = 0;
    if (!queue[port - Router::PORT_BASE].invokeId.compare_exchange_weak(isFree, id)) {
        LOG_WARN("Port: " << port << " already in use as " << isFree);
        return nullptr;
    }
    return &queue[port - Router::PORT_BASE];
}

void AmsConnection::Release(AmsResponse* response)
{
    response->frame.reset();
    response->invokeId = 0;
}

void AmsConnection::Receive(void* buffer, size_t bytesToRead) const
{
    auto pos = reinterpret_cast<uint8_t*>(buffer);
    while (bytesToRead) {
        const size_t bytesRead = socket.read(pos, bytesToRead, nullptr);
        bytesToRead -= bytesRead;
        pos += bytesRead;
    }
}

void AmsConnection::ReceiveJunk(size_t bytesToRead) const
{
    uint8_t buffer[1024];
    while (bytesToRead > sizeof(buffer)) {
        Receive(buffer, sizeof(buffer));
        bytesToRead -= sizeof(buffer);
    }
    Receive(buffer, bytesToRead);
}

Frame& AmsConnection::ReceiveFrame(Frame& frame, size_t bytesLeft) const
{
    if (bytesLeft > frame.capacity()) {
        LOG_WARN("Frame to long: " << std::dec << bytesLeft << '<' << frame.capacity());
        ReceiveJunk(bytesLeft);
        return frame.clear();
    }
    Receive(frame.rawData(), bytesLeft);
    return frame.limit(bytesLeft);
}

bool AmsConnection::ReceiveNotification(const AoEHeader& header)
{
    const auto dispatcher = DispatcherListGet(VirtualConnection { header.targetPort(), header.sourceAms() });
    if (!dispatcher) {
        ReceiveJunk(header.length());
        LOG_WARN("No dispatcher found for notification");
        return false;
    }

    auto& ring = dispatcher->ring;
    auto bytesLeft = header.length();
    if (bytesLeft > ring.BytesFree()) {
        ReceiveJunk(bytesLeft);
        LOG_WARN("port " << std::dec << header.targetPort() << " receive buffer was full");
        return false;
    }

    auto chunk = ring.WriteChunk();
    while (bytesLeft > chunk) {
        Receive(ring.write, chunk);
        ring.Write(chunk);
        bytesLeft -= chunk;
        chunk = ring.WriteChunk();
    }
    Receive(ring.write, bytesLeft);
    ring.Write(bytesLeft);
    dispatcher->Notify();
    return true;
}

void AmsConnection::TryRecv()
{
    try {
        Recv();
    }catch (std::runtime_error& e) {
        LOG_INFO(e.what());
    }
}

void AmsConnection::Recv()
{
    AmsTcpHeader amsTcpHeader;
    AoEHeader aoeHeader;
    for ( ; ; ) {
        Receive(amsTcpHeader);
        if (amsTcpHeader.length() < sizeof(aoeHeader)) {
            LOG_WARN("Frame to short to be AoE");
            ReceiveJunk(amsTcpHeader.length());
            continue;
        }

        Receive(aoeHeader);
        if (aoeHeader.cmdId() == AoEHeader::DEVICE_NOTIFICATION) {
            ReceiveNotification(aoeHeader);
            continue;
        }

        auto response = GetPending(aoeHeader.invokeId(), aoeHeader.targetPort());
        if (!response) {
            LOG_WARN("No response pending");
            ReceiveJunk(aoeHeader.length());
            continue;
        }

        ReceiveFrame(response->frame, aoeHeader.length());

        switch (aoeHeader.cmdId()) {
        case AoEHeader::READ_DEVICE_INFO:
        case AoEHeader::READ:
        case AoEHeader::WRITE:
        case AoEHeader::READ_STATE:
        case AoEHeader::WRITE_CONTROL:
        case AoEHeader::ADD_DEVICE_NOTIFICATION:
        case AoEHeader::DEL_DEVICE_NOTIFICATION:
        case AoEHeader::READ_WRITE:
            break;

        default:
            LOG_WARN("Unkown AMS command id");
            response->frame.clear();
        }

        response->errorCode = aoeHeader.errorCode();
        response->Notify();
    }
}
<commit_msg>Fixes #17 array access out of bounds<commit_after>/**
   Copyright (c) 2015 Beckhoff Automation GmbH & Co. KG

   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.
 */

#include "AmsConnection.h"
#include "Log.h"

AmsResponse::AmsResponse()
    : frame(4096),
    invokeId(0)
{}

void AmsResponse::Notify()
{
    invokeId = 0;
    cv.notify_all();
}

bool AmsResponse::Wait(uint32_t timeout_ms)
{
    std::unique_lock<std::mutex> lock(mutex);
    return cv.wait_for(lock, std::chrono::milliseconds(timeout_ms), [&]() {
        return !invokeId;
    });
}

std::shared_ptr<NotificationDispatcher> AmsConnection::DispatcherListAdd(const VirtualConnection& connection)
{
    const auto dispatcher = DispatcherListGet(connection);
    if (dispatcher) {
        return dispatcher;
    }
    std::lock_guard<std::recursive_mutex> lock(dispatcherListMutex);
    return dispatcherList.emplace(connection,
                                  std::make_shared<NotificationDispatcher>(*this, connection)).first->second;
}

std::shared_ptr<NotificationDispatcher> AmsConnection::DispatcherListGet(const VirtualConnection& connection)
{
    std::lock_guard<std::recursive_mutex> lock(dispatcherListMutex);

    const auto it = dispatcherList.find(connection);
    if (it != dispatcherList.end()) {
        return it->second;
    }
    return std::shared_ptr<NotificationDispatcher>();
}

AmsConnection::AmsConnection(Router& __router, IpV4 __destIp)
    : router(__router),
    socket(__destIp, ADS_TCP_SERVER_PORT),
    invokeId(0),
    destIp(__destIp),
    ownIp(socket.Connect())
{
    receiver = std::thread(&AmsConnection::TryRecv, this);
}

AmsConnection::~AmsConnection()
{
    socket.Shutdown();
    receiver.join();
}

NotifyMapping AmsConnection::CreateNotifyMapping(uint32_t hNotify, Notification& notification)
{
    const auto dispatcher = DispatcherListAdd(notification.connection);
    notification.hNotify(hNotify);
    dispatcher->Emplace(hNotify, notification);
    return NotifyMapping {hNotify, dispatcher};
}

long AmsConnection::DeleteNotification(const AmsAddr& amsAddr, uint32_t hNotify, uint32_t tmms, uint16_t port)
{
    AmsRequest request {
        amsAddr,
        port, AoEHeader::DEL_DEVICE_NOTIFICATION,
        0, nullptr, nullptr,
        sizeof(hNotify)
    };
    request.frame.prepend(qToLittleEndian(hNotify));

    return AdsRequest<AoEResponseHeader>(request, tmms);
}

AmsResponse* AmsConnection::Write(Frame& request, const AmsAddr destAddr, const AmsAddr srcAddr, uint16_t cmdId)
{
    AoEHeader aoeHeader { destAddr.netId, destAddr.port, srcAddr.netId, srcAddr.port, cmdId,
                          static_cast<uint32_t>(request.size()), GetInvokeId() };
    request.prepend<AoEHeader>(aoeHeader);

    AmsTcpHeader header { static_cast<uint32_t>(request.size()) };
    request.prepend<AmsTcpHeader>(header);

    auto response = Reserve(aoeHeader.invokeId(), srcAddr.port);

    if (!response) {
        return nullptr;
    }

    if (request.size() != socket.write(request)) {
        Release(response);
        return nullptr;
    }
    return response;
}

uint32_t AmsConnection::GetInvokeId()
{
    uint32_t result;
    do {
        result = invokeId.fetch_add(1);
    } while (!result);
    return result;
}

AmsResponse* AmsConnection::GetPending(uint32_t id, uint16_t port)
{
    const uint16_t portIndex = port - Router::PORT_BASE;
    if (portIndex >= Router::NUM_PORTS_MAX) {
        LOG_WARN("Port 0x" << std::hex << port << " is out of range");
        return nullptr;
    }

    const uint32_t currentId = queue[portIndex].invokeId;
    if (currentId == id) {
        return &queue[portIndex];
    }
    LOG_WARN("InvokeId mismatch: waiting for 0x" << std::hex << currentId << " received 0x" << id);
    return nullptr;
}

AmsResponse* AmsConnection::Reserve(uint32_t id, uint16_t port)
{
    uint32_t isFree = 0;
    if (!queue[port - Router::PORT_BASE].invokeId.compare_exchange_weak(isFree, id)) {
        LOG_WARN("Port: " << port << " already in use as " << isFree);
        return nullptr;
    }
    return &queue[port - Router::PORT_BASE];
}

void AmsConnection::Release(AmsResponse* response)
{
    response->frame.reset();
    response->invokeId = 0;
}

void AmsConnection::Receive(void* buffer, size_t bytesToRead) const
{
    auto pos = reinterpret_cast<uint8_t*>(buffer);
    while (bytesToRead) {
        const size_t bytesRead = socket.read(pos, bytesToRead, nullptr);
        bytesToRead -= bytesRead;
        pos += bytesRead;
    }
}

void AmsConnection::ReceiveJunk(size_t bytesToRead) const
{
    uint8_t buffer[1024];
    while (bytesToRead > sizeof(buffer)) {
        Receive(buffer, sizeof(buffer));
        bytesToRead -= sizeof(buffer);
    }
    Receive(buffer, bytesToRead);
}

Frame& AmsConnection::ReceiveFrame(Frame& frame, size_t bytesLeft) const
{
    if (bytesLeft > frame.capacity()) {
        LOG_WARN("Frame to long: " << std::dec << bytesLeft << '<' << frame.capacity());
        ReceiveJunk(bytesLeft);
        return frame.clear();
    }
    Receive(frame.rawData(), bytesLeft);
    return frame.limit(bytesLeft);
}

bool AmsConnection::ReceiveNotification(const AoEHeader& header)
{
    const auto dispatcher = DispatcherListGet(VirtualConnection { header.targetPort(), header.sourceAms() });
    if (!dispatcher) {
        ReceiveJunk(header.length());
        LOG_WARN("No dispatcher found for notification");
        return false;
    }

    auto& ring = dispatcher->ring;
    auto bytesLeft = header.length();
    if (bytesLeft > ring.BytesFree()) {
        ReceiveJunk(bytesLeft);
        LOG_WARN("port " << std::dec << header.targetPort() << " receive buffer was full");
        return false;
    }

    auto chunk = ring.WriteChunk();
    while (bytesLeft > chunk) {
        Receive(ring.write, chunk);
        ring.Write(chunk);
        bytesLeft -= chunk;
        chunk = ring.WriteChunk();
    }
    Receive(ring.write, bytesLeft);
    ring.Write(bytesLeft);
    dispatcher->Notify();
    return true;
}

void AmsConnection::TryRecv()
{
    try {
        Recv();
    }catch (std::runtime_error& e) {
        LOG_INFO(e.what());
    }
}

void AmsConnection::Recv()
{
    AmsTcpHeader amsTcpHeader;
    AoEHeader aoeHeader;
    for ( ; ; ) {
        Receive(amsTcpHeader);
        if (amsTcpHeader.length() < sizeof(aoeHeader)) {
            LOG_WARN("Frame to short to be AoE");
            ReceiveJunk(amsTcpHeader.length());
            continue;
        }

        Receive(aoeHeader);
        if (aoeHeader.cmdId() == AoEHeader::DEVICE_NOTIFICATION) {
            ReceiveNotification(aoeHeader);
            continue;
        }

        auto response = GetPending(aoeHeader.invokeId(), aoeHeader.targetPort());
        if (!response) {
            LOG_WARN("No response pending");
            ReceiveJunk(aoeHeader.length());
            continue;
        }

        ReceiveFrame(response->frame, aoeHeader.length());

        switch (aoeHeader.cmdId()) {
        case AoEHeader::READ_DEVICE_INFO:
        case AoEHeader::READ:
        case AoEHeader::WRITE:
        case AoEHeader::READ_STATE:
        case AoEHeader::WRITE_CONTROL:
        case AoEHeader::ADD_DEVICE_NOTIFICATION:
        case AoEHeader::DEL_DEVICE_NOTIFICATION:
        case AoEHeader::READ_WRITE:
            break;

        default:
            LOG_WARN("Unkown AMS command id");
            response->frame.clear();
        }

        response->errorCode = aoeHeader.errorCode();
        response->Notify();
    }
}
<|endoftext|>


<commit_before>#include "spheremesh.h"
#include <fstream>
#include <sstream>
#include "Eigen/Dense"

using Eigen::Matrix4f;
using Eigen::Vector4f;

class Face;
class Vertex {
    public:
        Vector3d point;
        vector<Face*> faces;
        Vertex(Vector3d p, vector<Face*> faceList) : point(p), faces(faceList) {}
        Vertex(Vector3d p) : point(p) {}
};
class Face {
    public:
        Vector3d normal;
        Vertex* a;
        Vertex* b;
        Vertex* c;
        Face(Vector3d norm, Vertex* ap, Vertex* bp, Vertex *cp) : normal(norm), a(ap), b(bp), c(cp) {}
        Face() {}
        float getArea() {
            Vector3d one = Vector3d::sub(b->point, a->point);
            Vector3d two = Vector3d::sub(c->point, a->point);
            Vector3d cross = Vector3d::cross(one, two);
            return cross.magnitude() * 0.5;
        }
};
class SQEM {
    public:
        Matrix4f A;
        Vector4f b;
        float c;
        SQEM() {
            A = Matrix4f::Zero();
            b = Vector4f::Zero();
        }
        SQEM(Vertex* vertex) {
            A = Matrix4f::Zero();
            b = Vector4f::Zero();
            vector<Face*> faces = vertex->faces;
            for (int i = 0; i < faces.size(); i++) {
                Face* face = faces[i];
                float coeff = face->getArea() * 1/3;
                SQEM faceSqem(face);
                A += coeff * faceSqem.A;
                b += coeff * faceSqem.b;
                c += coeff * faceSqem.c;
            }
        }
        SQEM(Face* face) {
            Vector3d normal = face->normal;
            A << 2 * normal.x * normal.x, 2 * normal.x * normal.y, 2 * normal.x * normal.z, 2 * normal.x,
                 2 * normal.y * normal.x, 2 * normal.y * normal.y, 2 * normal.y * normal.z, 2 * normal.y,
                 2 * normal.z * normal.x, 2 * normal.z * normal.y, 2 * normal.z * normal.z, 2 * normal.z,
                 2 * normal.x,            2 * normal.y,            2 * normal.z,            2 * 1;
            float coeff = Vector3d::dot_num(normal, face->a->point);
            b << 2 * coeff * normal.x, 2 * coeff * normal.y, 2 * coeff * normal.z, 2 * coeff * 1;
            c = coeff * coeff;
        }
        static SQEM addSQEMs(SQEM one, SQEM two) {
            SQEM n;
            n.A = one.A + two.A;
            n.b = one.b + two.b;
            n.c = one.c + two.c;
            return n;
        }
        float computeError(Sphere sphere) {
            Vector4f s;
            s << sphere.center.x, sphere.center.y, sphere.center.z, sphere.radius;
            Vector4f lr = A * s;
            float left = 0.5 * s.dot(lr);
            float middle = b.dot(s);

            return left + middle + c;
        }
        Sphere getMinSphere() {
            Sphere sphere;
            return sphere;
        }
};
class SphereVertex {
    public:
        SQEM sqem;
        vector<SphereVertex*> neighbors;
        SphereVertex() {}
};

void split(string s, char delim, vector<string> *elems) {
    stringstream stream;
    stream.str(s);
    string str;
    while (getline(stream, str, delim)) {
        elems->push_back(str);
    }
}

void readObjFile(string filename, vector<Vertex*> *vertices, vector<Face*> *faces) {
    ifstream file(filename);
    string str;
    while (getline(file, str)) {
        vector<string> elems;
        split(str, ' ', &elems);
        if (elems.size() >= 4) {
            string command = elems[0];
            if (command == "v") {
                Vector3d point(stof(elems[1]), stof(elems[2]), stof(elems[3]));
                Vertex* vertex = new Vertex(point);
                vertices->push_back(vertex);
            } else if (command == "f") {
                if (elems.size() - 1 == 3) {
                    vector<Vertex*> faceVertices;
                    Face* face = new Face();
                    for (int i = 1; i <= 3; i++) {
                        vector<string> params;
                        split(elems[i], '/', &params);
                        int index = stoi(params[0]) - 1;
                        faceVertices.push_back(vertices->at(index));
                        vertices->at(index)->faces.push_back(face);
                    }
                    Vector3d vec1 = Vector3d::sub(faceVertices[1]->point, faceVertices[0]->point);
                    Vector3d vec2 = Vector3d::sub(faceVertices[2]->point, faceVertices[0]->point);
                    Vector3d normal = Vector3d::cross(vec1, vec2);
                    face->a = faceVertices[0];
                    face->b = faceVertices[1];
                    face->c = faceVertices[2];
                    face->normal = normal;
                    faces->push_back(face);
                } else {
                    printf("This sphere mesh implementation only supports triangular faces!");
                }
            }
        }
    }
}
void calcSqem(vector<Vertex*> *vertices, vector<Face*> *faces, vector<SphereVertex*> *spheres) {

}
//Read from file and store spheres and edges in passed arguments
void calcSphereMesh(string filename, int sphereCount, vector<Sphere> *outputSpheres, vector<SphereEdge> *outputEdges) {
    printf("outputing to vectors!\n");
    vector<Vertex*> vertices;
    vector<Face*> faces;
    readObjFile(filename, &vertices, &faces);
    vector<SphereVertex*> spheres;
    calcSqem(&vertices, &faces, &spheres);
}

//Read from file and store spheres and edges in output file
void calcSphereMesh(string filename, int sphereCount, string outputFile) {
    printf("writing to file!\n");
}<commit_msg>More SQEM<commit_after>#include "spheremesh.h"
#include <fstream>
#include <sstream>
#include "Eigen/Dense"

#define epsilon 0.0001

using Eigen::Matrix4f;
using Eigen::Vector4f;

class Face;
class Vertex {
    public:
        Vector3d point;
        vector<Face*> faces;
        Vertex(Vector3d p, vector<Face*> faceList) : point(p), faces(faceList) {}
        Vertex(Vector3d p) : point(p) {}
};
class Face {
    public:
        Vector3d normal;
        Vertex* a;
        Vertex* b;
        Vertex* c;
        Face(Vector3d norm, Vertex* ap, Vertex* bp, Vertex *cp) : normal(norm), a(ap), b(bp), c(cp) {}
        Face() {}
        float getArea() {
            Vector3d one = Vector3d::sub(b->point, a->point);
            Vector3d two = Vector3d::sub(c->point, a->point);
            Vector3d cross = Vector3d::cross(one, two);
            return cross.magnitude() * 0.5;
        }
};
class SQEM {
    public:
        Matrix4f A;
        Vector4f b;
        float c;
        SQEM() {
            A = Matrix4f::Zero();
            b = Vector4f::Zero();
        }
        SQEM(Vertex* vertex) {
            A = Matrix4f::Zero();
            b = Vector4f::Zero();
            vector<Face*> faces = vertex->faces;
            for (int i = 0; i < faces.size(); i++) {
                Face* face = faces[i];
                float coeff = face->getArea() * 1/3;
                SQEM faceSqem(face);
                A += coeff * faceSqem.A;
                b += coeff * faceSqem.b;
                c += coeff * faceSqem.c;
            }
        }
        SQEM(Face* face) {
            Vector3d normal = face->normal;
            A << 2 * normal.x * normal.x, 2 * normal.x * normal.y, 2 * normal.x * normal.z, 2 * normal.x,
                 2 * normal.y * normal.x, 2 * normal.y * normal.y, 2 * normal.y * normal.z, 2 * normal.y,
                 2 * normal.z * normal.x, 2 * normal.z * normal.y, 2 * normal.z * normal.z, 2 * normal.z,
                 2 * normal.x,            2 * normal.y,            2 * normal.z,            2 * 1;
            float coeff = Vector3d::dot_num(normal, face->a->point);
            b << 2 * coeff * normal.x, 2 * coeff * normal.y, 2 * coeff * normal.z, 2 * coeff * 1;
            c = coeff * coeff;
        }
        static SQEM addSQEMs(SQEM one, SQEM two) {
            SQEM n;
            n.A = one.A + two.A;
            n.b = one.b + two.b;
            n.c = one.c + two.c;
            return n;
        }
        float computeError(Sphere sphere) {
            Vector4f s;
            s << sphere.center.x, sphere.center.y, sphere.center.z, sphere.radius;
            Vector4f lr = A * s;
            float left = 0.5 * s.dot(lr);
            float middle = b.dot(s);

            return left + middle + c;
        }
        Sphere getMinSphere() {
            float determinant = A.determinant();
            if (determinant > epsilon) {
                Matrix4f inverse = A.inverse();
                Vector4f minimum = inverse * b;
                float radius = minimum(3);
                if (radius >= 0) {
                    Vector3d center(minimum(0), minimum(1), minimum(2));
                    Sphere sphere(center, radius);
                    return sphere;
                }
            }
        }
        Sphere getMinSphere(Vector3d u, Vector3d v) {
            float determinant = A.determinant();
            if (determinant > epsilon) {
                Matrix4f inverse = A.inverse();
                Vector4f minimum = inverse * b;
                float radius = minimum(3);
                if (radius >= 0) {
                    Vector3d center(minimum(0), minimum(1), minimum(2));
                    Sphere sphere(center, radius);
                    return sphere;
                }
            }

            Sphere sphere;
            return sphere;
        }
};
class SphereVertex {
    public:
        SQEM sqem;
        vector<SphereVertex*> neighbors;
        Vector3d u;
        Vector3d v;
        SphereVertex() {}
};

void split(string s, char delim, vector<string> *elems) {
    stringstream stream;
    stream.str(s);
    string str;
    while (getline(stream, str, delim)) {
        elems->push_back(str);
    }
}

void readObjFile(string filename, vector<Vertex*> *vertices, vector<Face*> *faces) {
    ifstream file(filename);
    string str;
    while (getline(file, str)) {
        vector<string> elems;
        split(str, ' ', &elems);
        if (elems.size() >= 4) {
            string command = elems[0];
            if (command == "v") {
                Vector3d point(stof(elems[1]), stof(elems[2]), stof(elems[3]));
                Vertex* vertex = new Vertex(point);
                vertices->push_back(vertex);
            } else if (command == "f") {
                if (elems.size() - 1 == 3) {
                    vector<Vertex*> faceVertices;
                    Face* face = new Face();
                    for (int i = 1; i <= 3; i++) {
                        vector<string> params;
                        split(elems[i], '/', &params);
                        int index = stoi(params[0]) - 1;
                        faceVertices.push_back(vertices->at(index));
                        vertices->at(index)->faces.push_back(face);
                    }
                    Vector3d vec1 = Vector3d::sub(faceVertices[1]->point, faceVertices[0]->point);
                    Vector3d vec2 = Vector3d::sub(faceVertices[2]->point, faceVertices[0]->point);
                    Vector3d normal = Vector3d::cross(vec1, vec2);
                    face->a = faceVertices[0];
                    face->b = faceVertices[1];
                    face->c = faceVertices[2];
                    face->normal = normal;
                    faces->push_back(face);
                } else {
                    printf("This sphere mesh implementation only supports triangular faces!");
                }
            }
        }
    }
}
void calcSqem(vector<Vertex*> *vertices, vector<Face*> *faces, vector<SphereVertex*> *spheres) {

}
//Read from file and store spheres and edges in passed arguments
void calcSphereMesh(string filename, int sphereCount, vector<Sphere> *outputSpheres, vector<SphereEdge> *outputEdges) {
    printf("outputing to vectors!\n");
    vector<Vertex*> vertices;
    vector<Face*> faces;
    readObjFile(filename, &vertices, &faces);
    vector<SphereVertex*> spheres;
    calcSqem(&vertices, &faces, &spheres);
}

//Read from file and store spheres and edges in output file
void calcSphereMesh(string filename, int sphereCount, string outputFile) {
    printf("writing to file!\n");
}<|endoftext|>


<commit_before>/** @file
    @brief Implementation

    @date 2015

    @author
    Sensics, Inc.
    <http://sensics.com/osvr>
*/

// Copyright 2015 Sensics, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// 	http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// Internal Includes
#include <osvr/Common/DecomposeOriginalSource.h>
#include <osvr/Common/PathNode.h>
#include <osvr/Common/ApplyPathNodeVisitor.h>
#include <osvr/Common/RoutingExceptions.h>

// Library/third-party includes
#include <boost/variant/static_visitor.hpp>
#include <boost/variant/apply_visitor.hpp>
#include <boost/variant/get.hpp>
#include <boost/optional.hpp>
#include <boost/lexical_cast.hpp>

// Standard includes
#include <string>

namespace osvr {
namespace common {

    OriginalSource::OriginalSource()
        : m_device(nullptr), m_interface(nullptr), m_sensor(nullptr) {}

    bool OriginalSource::isResolved() const {
        return m_device != nullptr && m_interface != nullptr;
    }

    void OriginalSource::setDevice(common::PathNode &device) {
        BOOST_ASSERT_MSG(m_device == nullptr,
                         "Device should only be set once.");
        m_device = &device;
    }

    void OriginalSource::setInterface(common::PathNode &iface) {
        BOOST_ASSERT_MSG(m_interface == nullptr,
                         "Interface should only be set once.");
        m_interface = &iface;
    }

    void OriginalSource::setSensor(common::PathNode &sensor) {
        BOOST_ASSERT_MSG(m_sensor == nullptr,
                         "Sensor should only be set once.");
        m_sensor = &sensor;
    }

    void OriginalSource::setTransform(std::string const &transform) {
        BOOST_ASSERT_MSG(m_transform.empty(),
                         "Transform should only be set once.");
        m_transform = transform;
    }

    common::PathNode *OriginalSource::getDevice() const { return m_device; }

    common::elements::DeviceElement const &
    OriginalSource::getDeviceElement() const {
        BOOST_ASSERT_MSG(isResolved(),
                         "Only makes sense when called on a resolved source.");
        return boost::get<common::elements::DeviceElement>(m_device->value());
    }

    common::PathNode *OriginalSource::getInterface() const {
        return m_interface;
    }

    std::string OriginalSource::getInterfaceName() const {
        BOOST_ASSERT_MSG(isResolved(),
                         "Only makes sense when called on a resolved source.");
        std::string ret;
        if (nullptr == m_interface) {
            return ret;
        }
        ret = m_interface->getName();
        return ret;
    }

    common::PathNode *OriginalSource::getSensor() const { return m_sensor; }

    boost::optional<int> OriginalSource::getSensorNumber() const {
        BOOST_ASSERT_MSG(isResolved(),
                         "Only makes sense when called on a resolved source.");
        boost::optional<int> ret;
        if (nullptr == m_sensor) {
            return ret;
        }
        try {
            ret = boost::lexical_cast<int>(m_sensor->getName());
        } catch (boost::bad_lexical_cast &) {
            // Just means we can't convert to int, so returning an empty
            // optional is fine.
        }
        return ret;
    }

    std::string OriginalSource::getTransform() const { return m_transform; }

    namespace {
        class DecomposeOriginalSource : public boost::static_visitor<> {
          public:
            DecomposeOriginalSource(PathNode &node, OriginalSource &source)
                : boost::static_visitor<>(), m_source(source) {
                m_applyVisitor(node);
            }

            template <typename T> void operator()(PathNode &, T &) {
                throw std::runtime_error("Element type encountered that is not "
                                         "recognized in decomposition!");
            }

            void operator()(PathNode &node, elements::SensorElement &) {
                m_source.setSensor(node);
                if (nullptr == node.getParent()) {
                    throw exceptions::invariants::SensorMissingParent(
                        getFullPath(node));
                }
                m_applyVisitor(*node.getParent());
                if (nullptr == m_source.getInterface()) {
                    // Hmm, finished traversing upward and didn't get an
                    // interface.
                    throw exceptions::invariants::SensorMissingInterfaceParent(
                        getFullPath(node));
                }
            }

            void operator()(PathNode &node, elements::InterfaceElement &) {
                m_source.setInterface(node);
                if (nullptr == node.getParent()) {
                    throw exceptions::invariants::InterfaceMissingParent(
                        getFullPath(node));
                }
                m_applyVisitor(*node.getParent());
            }

            void operator()(PathNode &node, elements::DeviceElement &) {
                m_source.setDevice(node);
                /// stop recursing through parents - the device is as far as we
                /// need.
            }

          private:
            void m_applyVisitor(PathNode &node) {
                applyPathNodeVisitor(*this, node);
            }
            OriginalSource &m_source;
        };
    } // namespace
    void OriginalSource::decompose(PathNode &node) {
        DecomposeOriginalSource decomp{node, *this};
    }
} // namespace common
} // namespace osvr
<commit_msg>Try silencing another warning with noncopyable<commit_after>/** @file
    @brief Implementation

    @date 2015

    @author
    Sensics, Inc.
    <http://sensics.com/osvr>
*/

// Copyright 2015 Sensics, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// 	http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// Internal Includes
#include <osvr/Common/DecomposeOriginalSource.h>
#include <osvr/Common/PathNode.h>
#include <osvr/Common/ApplyPathNodeVisitor.h>
#include <osvr/Common/RoutingExceptions.h>

// Library/third-party includes
#include <boost/variant/static_visitor.hpp>
#include <boost/variant/apply_visitor.hpp>
#include <boost/variant/get.hpp>
#include <boost/optional.hpp>
#include <boost/lexical_cast.hpp>

// Standard includes
#include <string>

namespace osvr {
namespace common {

    OriginalSource::OriginalSource()
        : m_device(nullptr), m_interface(nullptr), m_sensor(nullptr) {}

    bool OriginalSource::isResolved() const {
        return m_device != nullptr && m_interface != nullptr;
    }

    void OriginalSource::setDevice(common::PathNode &device) {
        BOOST_ASSERT_MSG(m_device == nullptr,
                         "Device should only be set once.");
        m_device = &device;
    }

    void OriginalSource::setInterface(common::PathNode &iface) {
        BOOST_ASSERT_MSG(m_interface == nullptr,
                         "Interface should only be set once.");
        m_interface = &iface;
    }

    void OriginalSource::setSensor(common::PathNode &sensor) {
        BOOST_ASSERT_MSG(m_sensor == nullptr,
                         "Sensor should only be set once.");
        m_sensor = &sensor;
    }

    void OriginalSource::setTransform(std::string const &transform) {
        BOOST_ASSERT_MSG(m_transform.empty(),
                         "Transform should only be set once.");
        m_transform = transform;
    }

    common::PathNode *OriginalSource::getDevice() const { return m_device; }

    common::elements::DeviceElement const &
    OriginalSource::getDeviceElement() const {
        BOOST_ASSERT_MSG(isResolved(),
                         "Only makes sense when called on a resolved source.");
        return boost::get<common::elements::DeviceElement>(m_device->value());
    }

    common::PathNode *OriginalSource::getInterface() const {
        return m_interface;
    }

    std::string OriginalSource::getInterfaceName() const {
        BOOST_ASSERT_MSG(isResolved(),
                         "Only makes sense when called on a resolved source.");
        std::string ret;
        if (nullptr == m_interface) {
            return ret;
        }
        ret = m_interface->getName();
        return ret;
    }

    common::PathNode *OriginalSource::getSensor() const { return m_sensor; }

    boost::optional<int> OriginalSource::getSensorNumber() const {
        BOOST_ASSERT_MSG(isResolved(),
                         "Only makes sense when called on a resolved source.");
        boost::optional<int> ret;
        if (nullptr == m_sensor) {
            return ret;
        }
        try {
            ret = boost::lexical_cast<int>(m_sensor->getName());
        } catch (boost::bad_lexical_cast &) {
            // Just means we can't convert to int, so returning an empty
            // optional is fine.
        }
        return ret;
    }

    std::string OriginalSource::getTransform() const { return m_transform; }

    namespace {
        class DecomposeOriginalSource : public boost::static_visitor<>,
                                        boost::noncopyable {
          public:
            DecomposeOriginalSource(PathNode &node, OriginalSource &source)
                : boost::static_visitor<>(), m_source(source) {
                m_applyVisitor(node);
            }

            template <typename T> void operator()(PathNode &, T &) {
                throw std::runtime_error("Element type encountered that is not "
                                         "recognized in decomposition!");
            }

            void operator()(PathNode &node, elements::SensorElement &) {
                m_source.setSensor(node);
                if (nullptr == node.getParent()) {
                    throw exceptions::invariants::SensorMissingParent(
                        getFullPath(node));
                }
                m_applyVisitor(*node.getParent());
                if (nullptr == m_source.getInterface()) {
                    // Hmm, finished traversing upward and didn't get an
                    // interface.
                    throw exceptions::invariants::SensorMissingInterfaceParent(
                        getFullPath(node));
                }
            }

            void operator()(PathNode &node, elements::InterfaceElement &) {
                m_source.setInterface(node);
                if (nullptr == node.getParent()) {
                    throw exceptions::invariants::InterfaceMissingParent(
                        getFullPath(node));
                }
                m_applyVisitor(*node.getParent());
            }

            void operator()(PathNode &node, elements::DeviceElement &) {
                m_source.setDevice(node);
                /// stop recursing through parents - the device is as far as we
                /// need.
            }

          private:
            void m_applyVisitor(PathNode &node) {
                applyPathNodeVisitor(*this, node);
            }
            OriginalSource &m_source;
        };
    } // namespace
    void OriginalSource::decompose(PathNode &node) {
        DecomposeOriginalSource decomp{node, *this};
    }
} // namespace common
} // namespace osvr
<|endoftext|>