<commit_before>#include <boost/python/ndarray/ndarray.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>

static boost::mt19937 engine;
static boost::uniform_int<> random_int(2, 5);

template <typename T, int N, int C>
ndarray::Array<T,N,C> makeArray(ndarray::Vector<int,N> const & shape) {
    ndarray::Array<typename boost::remove_const<T>::type,N,N> a = ndarray::allocate(shape);
    ndarray::Array<typename boost::remove_const<T>::type,1,1> flat = ndarray::flatten<1>(a);
    for (int n=0; n < flat.template getSize<0>(); ++n) {
        flat[n] = n;
    }
    return a;
}

template <int N>
ndarray::Vector<int,N> makeShape() {
    ndarray::Vector<int,N> shape;
    for (int n=0; n<N; ++n) {
        shape[n] = random_int(engine);
    }
    return shape;
}

template <typename T, int N, int C>
ndarray::Array<T,N,C> returnArray() {
    ndarray::Array<typename boost::remove_const<T>::type,N,N> a = ndarray::allocate(makeShape<N>());
    ndarray::Array<typename boost::remove_const<T>::type,1,1> flat = ndarray::flatten<1>(a);
    for (int n=0; n < flat.template getSize<0>(); ++n) {
        flat[n] = n;
    }
    return a;
}

template <typename T, int N, int C>
bool acceptArray(ndarray::Array<T,N,C> const & p) {
    ndarray::Array<typename boost::remove_const<T>::type,N,N> a = ndarray::allocate(p.getShape());
    ndarray::Array<typename boost::remove_const<T>::type,1,1> flat = ndarray::flatten<1>(a);
    for (int n=0; n < flat.template getSize<0>(); ++n) {
        flat[n] = n;
    }
    return ndarray::all(equal(a, p));
}

template <typename T, int N, int C>
bool extractArray(boost::python::object const & obj) {
    ndarray::Array<T,N,C> p = boost::python::extract< ndarray::Array<T,N,C> >(obj);
    ndarray::Array<typename boost::remove_const<T>::type,N,N> a = ndarray::allocate(p.getShape());
    ndarray::Array<typename boost::remove_const<T>::type,1,1> flat = ndarray::flatten<1>(a);
    for (int n=0; n < flat.template getSize<0>(); ++n) {
        flat[n] = n;
    }
    return ndarray::all(equal(a, p));
}

template <typename T, int N>
ndarray::Vector<T,N> returnVector() {
    ndarray::Vector<T,N> r;
    for (int n=0; n<N; ++n) {
        r[n] = n;
    }
    return r;
}

template <typename T, int N>
bool acceptVector(ndarray::Vector<T,N> const & p) {
    for (int n=0; n<N; ++n) {
        if (p[n] != T(n)) return false;
    }
    return true;
}

template <typename T, int N>
bool extractVector(boost::python::object const & obj) {
    ndarray::Vector<T,N> p = boost::python::extract< ndarray::Vector<T,N> >(obj);
    for (int n=0; n<N; ++n) {
        if (p[n] != T(n)) return false;
    }
    return true;
}

template <typename T, int N, int C>
ndarray::EigenView<T,N,C> returnEigenView() {
    ndarray::Array<typename boost::remove_const<T>::type,N,N> a = ndarray::allocate(makeShape<N>());
    ndarray::Array<typename boost::remove_const<T>::type,1,1> flat = ndarray::flatten<1>(a);
    for (int n=0; n < flat.template getSize<0>(); ++n) {
        flat[n] = n;
    }
    return ndarray::EigenView<T,N,C>(a);
}

template <typename T, int N, int C>
ndarray::TransposedEigenView<T,N,C> returnTransposedEigenView() {
    ndarray::Array<typename boost::remove_const<T>::type,N,N> a = ndarray::allocate(makeShape<N>());
    ndarray::Array<typename boost::remove_const<T>::type,1,1> flat = ndarray::flatten<1>(a);
    for (int n=0; n < flat.template getSize<0>(); ++n) {
        flat[n] = n;
    }
    return ndarray::TransposedEigenView<T,N,C>(a);
}

BOOST_PYTHON_MODULE(ndarray_mod) {
    boost::python::numpy::initialize();
    boost::python::def("makeArray_d33", makeArray<double,3,3>);
    boost::python::def("returnArray_d11", returnArray<double,1,1>);
    boost::python::def("returnArray_d10", returnArray<double,1,0>);
    boost::python::def("returnArray_d22", returnArray<double,2,2>);
    boost::python::def("returnArray_d21", returnArray<double,2,1>);
    boost::python::def("returnArray_d20", returnArray<double,2,0>);
    boost::python::def("returnArray_d33", returnArray<double,3,3>);
    boost::python::def("returnArray_d32", returnArray<double,3,2>);
    boost::python::def("returnArray_d31", returnArray<double,3,1>);
    boost::python::def("returnArray_d30", returnArray<double,3,0>);
    boost::python::def("returnArray_dc11", returnArray<double const,1,1>);
    boost::python::def("returnArray_dc10", returnArray<double const,1,0>);
    boost::python::def("returnArray_dc22", returnArray<double const,2,2>);
    boost::python::def("returnArray_dc21", returnArray<double const,2,1>);
    boost::python::def("returnArray_dc20", returnArray<double const,2,0>);
    boost::python::def("returnArray_dc33", returnArray<double const,3,3>);
    boost::python::def("returnArray_dc32", returnArray<double const,3,2>);
    boost::python::def("returnArray_dc31", returnArray<double const,3,1>);
    boost::python::def("returnArray_dc30", returnArray<double const,3,0>);
    boost::python::def("acceptArray_d11", acceptArray<double,1,1>);
    boost::python::def("acceptArray_d10", acceptArray<double,1,0>);
    boost::python::def("acceptArray_d22", acceptArray<double,2,2>);
    boost::python::def("acceptArray_d21", acceptArray<double,2,1>);
    boost::python::def("acceptArray_d20", acceptArray<double,2,0>);
    boost::python::def("acceptArray_d33", acceptArray<double,3,3>);
    boost::python::def("acceptArray_d32", acceptArray<double,3,2>);
    boost::python::def("acceptArray_d31", acceptArray<double,3,1>);
    boost::python::def("acceptArray_d30", acceptArray<double,3,0>);
    boost::python::def("acceptArray_dc11", acceptArray<double const,1,1>);
    boost::python::def("acceptArray_dc10", acceptArray<double const,1,0>);
    boost::python::def("acceptArray_dc22", acceptArray<double const,2,2>);
    boost::python::def("acceptArray_dc21", acceptArray<double const,2,1>);
    boost::python::def("acceptArray_dc20", acceptArray<double const,2,0>);
    boost::python::def("acceptArray_dc33", acceptArray<double const,3,3>);
    boost::python::def("acceptArray_dc32", acceptArray<double const,3,2>);
    boost::python::def("acceptArray_dc31", acceptArray<double const,3,1>);
    boost::python::def("acceptArray_dc30", acceptArray<double const,3,0>);
    boost::python::def("extractArray_d11", extractArray<double,1,1>);
    boost::python::def("extractArray_d10", extractArray<double,1,0>);
    boost::python::def("extractArray_d22", extractArray<double,2,2>);
    boost::python::def("extractArray_d21", extractArray<double,2,1>);
    boost::python::def("extractArray_d20", extractArray<double,2,0>);
    boost::python::def("extractArray_d33", extractArray<double,3,3>);
    boost::python::def("extractArray_d32", extractArray<double,3,2>);
    boost::python::def("extractArray_d31", extractArray<double,3,1>);
    boost::python::def("extractArray_d30", extractArray<double,3,0>);
    boost::python::def("extractArray_dc11", extractArray<double const,1,1>);
    boost::python::def("extractArray_dc10", extractArray<double const,1,0>);
    boost::python::def("extractArray_dc22", extractArray<double const,2,2>);
    boost::python::def("extractArray_dc21", extractArray<double const,2,1>);
    boost::python::def("extractArray_dc20", extractArray<double const,2,0>);
    boost::python::def("extractArray_dc33", extractArray<double const,3,3>);
    boost::python::def("extractArray_dc32", extractArray<double const,3,2>);
    boost::python::def("extractArray_dc31", extractArray<double const,3,1>);
    boost::python::def("extractArray_dc30", extractArray<double const,3,0>);
    boost::python::def("returnVector_d3", returnVector<double,3>);
    boost::python::def("returnVector_d2", returnVector<double,2>);
    boost::python::def("acceptVector_d3", acceptVector<double,3>);
    boost::python::def("acceptVector_d2", acceptVector<double,2>);
    boost::python::def("extractVector_d3", extractVector<double,3>);
    boost::python::def("extractVector_d2", extractVector<double,2>);
    boost::python::def("returnEigenView_d22", returnEigenView<double,2,2>);
    boost::python::def("returnEigenView_d21", returnEigenView<double,2,1>);
    boost::python::def("returnEigenView_d20", returnEigenView<double,2,0>);
    boost::python::def("returnEigenView_d11", returnEigenView<double,1,1>);
    boost::python::def("returnEigenView_d10", returnEigenView<double,1,0>);
    boost::python::def("returnEigenView_dc22", returnEigenView<double const,2,2>);
    boost::python::def("returnEigenView_dc21", returnEigenView<double const,2,1>);
    boost::python::def("returnEigenView_dc20", returnEigenView<double const,2,0>);
    boost::python::def("returnEigenView_dc11", returnEigenView<double const,1,1>);
    boost::python::def("returnEigenView_dc10", returnEigenView<double const,1,0>);
    boost::python::def("returnTransposedEigenView_d22", returnTransposedEigenView<double,2,2>);
    boost::python::def("returnTransposedEigenView_d21", returnTransposedEigenView<double,2,1>);
    boost::python::def("returnTransposedEigenView_d20", returnTransposedEigenView<double,2,0>);
    boost::python::def("returnTransposedEigenView_d11", returnTransposedEigenView<double,1,1>);
    boost::python::def("returnTransposedEigenView_d10", returnTransposedEigenView<double,1,0>);
    boost::python::def("returnTransposedEigenView_dc22", returnTransposedEigenView<double const,2,2>);
    boost::python::def("returnTransposedEigenView_dc21", returnTransposedEigenView<double const,2,1>);
    boost::python::def("returnTransposedEigenView_dc20", returnTransposedEigenView<double const,2,0>);
    boost::python::def("returnTransposedEigenView_dc11", returnTransposedEigenView<double const,1,1>);
    boost::python::def("returnTransposedEigenView_dc10", returnTransposedEigenView<double const,1,0>);
}
<commit_msg>ndarray - build system updates to support non-standard (esp boost) installs, workaround for gcc 4.5 ICE, additional svn:ignores<commit_after>#include <boost/python/ndarray/ndarray.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>

static boost::mt19937 engine;
static boost::uniform_int<> random_int(2, 5);

template <typename T, int N, int C>
ndarray::Array<T,N,C> makeArray(ndarray::Vector<int,N> const & shape) {
    ndarray::Array<typename boost::remove_const<T>::type,N,N> a = ndarray::allocate(shape);
    ndarray::Array<typename boost::remove_const<T>::type,1,1> flat = ndarray::flatten<1>(a);
    for (int n=0; n < flat.template getSize<0>(); ++n) {
        flat[n] = n;
    }
    return a;
}

template <int N>
ndarray::Vector<int,N> makeShape() {
    ndarray::Vector<int,N> shape;
    for (int n=0; n<N; ++n) {
        shape[n] = random_int(engine);
    }
    return shape;
}

template <typename T, int N, int C>
ndarray::Array<T,N,C> returnArray() {
    ndarray::Array<typename boost::remove_const<T>::type,N,N> a = ndarray::allocate(makeShape<N>());
    ndarray::Array<typename boost::remove_const<T>::type,1,1> flat = ndarray::flatten<1>(a);
    for (int n=0; n < flat.template getSize<0>(); ++n) {
        flat[n] = n;
    }
    return a;
}

template <typename T, int N, int C>
bool acceptArray(ndarray::Array<T,N,C> const & p) {
    ndarray::Array<typename boost::remove_const<T>::type,N,N> a = ndarray::allocate(p.getShape());
    a.deep() = p;
    ndarray::Array<typename boost::remove_const<T>::type,1,1> flat = ndarray::flatten<1>(a);
    for (int n=0; n < flat.template getSize<0>(); ++n) {
        if (flat[n] != n) return false;
    }
    return true;
}

template <typename T, int N, int C>
bool extractArray(boost::python::object const & obj) {
    ndarray::Array<T,N,C> p = boost::python::extract< ndarray::Array<T,N,C> >(obj);
    ndarray::Array<typename boost::remove_const<T>::type,N,N> a = ndarray::allocate(p.getShape());
    a.deep() = p;
    ndarray::Array<typename boost::remove_const<T>::type,1,1> flat = ndarray::flatten<1>(a);
    for (int n=0; n < flat.template getSize<0>(); ++n) {
        if (flat[n] != n) return false;
    }
    return true;
}

template <typename T, int N>
ndarray::Vector<T,N> returnVector() {
    ndarray::Vector<T,N> r;
    for (int n=0; n<N; ++n) {
        r[n] = n;
    }
    return r;
}

template <typename T, int N>
bool acceptVector(ndarray::Vector<T,N> const & p) {
    for (int n=0; n<N; ++n) {
        if (p[n] != T(n)) return false;
    }
    return true;
}

template <typename T, int N>
bool extractVector(boost::python::object const & obj) {
    ndarray::Vector<T,N> p = boost::python::extract< ndarray::Vector<T,N> >(obj);
    for (int n=0; n<N; ++n) {
        if (p[n] != T(n)) return false;
    }
    return true;
}

template <typename T, int N, int C>
ndarray::EigenView<T,N,C> returnEigenView() {
    ndarray::Array<typename boost::remove_const<T>::type,N,N> a = ndarray::allocate(makeShape<N>());
    ndarray::Array<typename boost::remove_const<T>::type,1,1> flat = ndarray::flatten<1>(a);
    for (int n=0; n < flat.template getSize<0>(); ++n) {
        flat[n] = n;
    }
    return ndarray::EigenView<T,N,C>(a);
}

template <typename T, int N, int C>
ndarray::TransposedEigenView<T,N,C> returnTransposedEigenView() {
    ndarray::Array<typename boost::remove_const<T>::type,N,N> a = ndarray::allocate(makeShape<N>());
    ndarray::Array<typename boost::remove_const<T>::type,1,1> flat = ndarray::flatten<1>(a);
    for (int n=0; n < flat.template getSize<0>(); ++n) {
        flat[n] = n;
    }
    return ndarray::TransposedEigenView<T,N,C>(a);
}

BOOST_PYTHON_MODULE(ndarray_mod) {
    boost::python::numpy::initialize();
    boost::python::def("makeArray_d33", makeArray<double,3,3>);
    boost::python::def("returnArray_d11", returnArray<double,1,1>);
    boost::python::def("returnArray_d10", returnArray<double,1,0>);
    boost::python::def("returnArray_d22", returnArray<double,2,2>);
    boost::python::def("returnArray_d21", returnArray<double,2,1>);
    boost::python::def("returnArray_d20", returnArray<double,2,0>);
    boost::python::def("returnArray_d33", returnArray<double,3,3>);
    boost::python::def("returnArray_d32", returnArray<double,3,2>);
    boost::python::def("returnArray_d31", returnArray<double,3,1>);
    boost::python::def("returnArray_d30", returnArray<double,3,0>);
    boost::python::def("returnArray_dc11", returnArray<double const,1,1>);
    boost::python::def("returnArray_dc10", returnArray<double const,1,0>);
    boost::python::def("returnArray_dc22", returnArray<double const,2,2>);
    boost::python::def("returnArray_dc21", returnArray<double const,2,1>);
    boost::python::def("returnArray_dc20", returnArray<double const,2,0>);
    boost::python::def("returnArray_dc33", returnArray<double const,3,3>);
    boost::python::def("returnArray_dc32", returnArray<double const,3,2>);
    boost::python::def("returnArray_dc31", returnArray<double const,3,1>);
    boost::python::def("returnArray_dc30", returnArray<double const,3,0>);
    boost::python::def("acceptArray_d11", acceptArray<double,1,1>);
    boost::python::def("acceptArray_d10", acceptArray<double,1,0>);
    boost::python::def("acceptArray_d22", acceptArray<double,2,2>);
    boost::python::def("acceptArray_d21", acceptArray<double,2,1>);
    boost::python::def("acceptArray_d20", acceptArray<double,2,0>);
    boost::python::def("acceptArray_d33", acceptArray<double,3,3>);
    boost::python::def("acceptArray_d32", acceptArray<double,3,2>);
    boost::python::def("acceptArray_d31", acceptArray<double,3,1>);
    boost::python::def("acceptArray_d30", acceptArray<double,3,0>);
    boost::python::def("acceptArray_dc11", acceptArray<double const,1,1>);
    boost::python::def("acceptArray_dc10", acceptArray<double const,1,0>);
    boost::python::def("acceptArray_dc22", acceptArray<double const,2,2>);
    boost::python::def("acceptArray_dc21", acceptArray<double const,2,1>);
    boost::python::def("acceptArray_dc20", acceptArray<double const,2,0>);
    boost::python::def("acceptArray_dc33", acceptArray<double const,3,3>);
    boost::python::def("acceptArray_dc32", acceptArray<double const,3,2>);
    boost::python::def("acceptArray_dc31", acceptArray<double const,3,1>);
    boost::python::def("acceptArray_dc30", acceptArray<double const,3,0>);
    boost::python::def("extractArray_d11", extractArray<double,1,1>);
    boost::python::def("extractArray_d10", extractArray<double,1,0>);
    boost::python::def("extractArray_d22", extractArray<double,2,2>);
    boost::python::def("extractArray_d21", extractArray<double,2,1>);
    boost::python::def("extractArray_d20", extractArray<double,2,0>);
    boost::python::def("extractArray_d33", extractArray<double,3,3>);
    boost::python::def("extractArray_d32", extractArray<double,3,2>);
    boost::python::def("extractArray_d31", extractArray<double,3,1>);
    boost::python::def("extractArray_d30", extractArray<double,3,0>);
    boost::python::def("extractArray_dc11", extractArray<double const,1,1>);
    boost::python::def("extractArray_dc10", extractArray<double const,1,0>);
    boost::python::def("extractArray_dc22", extractArray<double const,2,2>);
    boost::python::def("extractArray_dc21", extractArray<double const,2,1>);
    boost::python::def("extractArray_dc20", extractArray<double const,2,0>);
    boost::python::def("extractArray_dc33", extractArray<double const,3,3>);
    boost::python::def("extractArray_dc32", extractArray<double const,3,2>);
    boost::python::def("extractArray_dc31", extractArray<double const,3,1>);
    boost::python::def("extractArray_dc30", extractArray<double const,3,0>);
    boost::python::def("returnVector_d3", returnVector<double,3>);
    boost::python::def("returnVector_d2", returnVector<double,2>);
    boost::python::def("acceptVector_d3", acceptVector<double,3>);
    boost::python::def("acceptVector_d2", acceptVector<double,2>);
    boost::python::def("extractVector_d3", extractVector<double,3>);
    boost::python::def("extractVector_d2", extractVector<double,2>);
    boost::python::def("returnEigenView_d22", returnEigenView<double,2,2>);
    boost::python::def("returnEigenView_d21", returnEigenView<double,2,1>);
    boost::python::def("returnEigenView_d20", returnEigenView<double,2,0>);
    boost::python::def("returnEigenView_d11", returnEigenView<double,1,1>);
    boost::python::def("returnEigenView_d10", returnEigenView<double,1,0>);
    boost::python::def("returnEigenView_dc22", returnEigenView<double const,2,2>);
    boost::python::def("returnEigenView_dc21", returnEigenView<double const,2,1>);
    boost::python::def("returnEigenView_dc20", returnEigenView<double const,2,0>);
    boost::python::def("returnEigenView_dc11", returnEigenView<double const,1,1>);
    boost::python::def("returnEigenView_dc10", returnEigenView<double const,1,0>);
    boost::python::def("returnTransposedEigenView_d22", returnTransposedEigenView<double,2,2>);
    boost::python::def("returnTransposedEigenView_d21", returnTransposedEigenView<double,2,1>);
    boost::python::def("returnTransposedEigenView_d20", returnTransposedEigenView<double,2,0>);
    boost::python::def("returnTransposedEigenView_d11", returnTransposedEigenView<double,1,1>);
    boost::python::def("returnTransposedEigenView_d10", returnTransposedEigenView<double,1,0>);
    boost::python::def("returnTransposedEigenView_dc22", returnTransposedEigenView<double const,2,2>);
    boost::python::def("returnTransposedEigenView_dc21", returnTransposedEigenView<double const,2,1>);
    boost::python::def("returnTransposedEigenView_dc20", returnTransposedEigenView<double const,2,0>);
    boost::python::def("returnTransposedEigenView_dc11", returnTransposedEigenView<double const,1,1>);
    boost::python::def("returnTransposedEigenView_dc10", returnTransposedEigenView<double const,1,0>);
}
<|endoftext|>


<commit_before>#include "voxblox/core/block.h"
#include "voxblox/core/voxel.h"

namespace voxblox {

// Hidden serialization helpers:
uint8_t serializeDirection(const Eigen::Vector3i& dir) {
  uint8_t data = 0;
  // [0 0] = 0, [1 0] = -1, [0 1] = 1.
  uint8_t dir_x = dir.x() == 0 ? 0 : dir.x() > 0 ? 1 : 2;
  uint8_t dir_y = dir.y() == 0 ? 0 : dir.y() > 0 ? 1 : 2;
  uint8_t dir_z = dir.z() == 0 ? 0 : dir.z() > 0 ? 1 : 2;

  // Leading 2 bits are 0.
  data = dir_x << 4 | dir_y << 2 | dir_z;
  return data;
}

Eigen::Vector3i deserializeDirection(uint8_t data) {
  Eigen::Vector3i dir;
  uint8_t byte_x = data >> 4;
  uint8_t byte_y = (data >> 2) & (2 | 1);
  uint8_t byte_z = data & (2 | 1);

  // [0 0] = 0, [1 0] = -1, [0 1] = 1.
  dir.x() = byte_x == 0 ? 0 : byte_x == 2 ? -1 : 1;
  dir.y() = byte_y == 0 ? 0 : byte_y == 2 ? -1 : 1;
  dir.z() = byte_z == 0 ? 0 : byte_z == 2 ? -1 : 1;

  return dir;
}

// Deserialization functions:
template <>
void Block<TsdfVoxel>::deserializeFromIntegers(
    const std::vector<uint32_t>& data) {
  constexpr size_t kNumDataPacketsPerVoxel = 3u;
  const size_t num_data_packets = data.size();
  CHECK_EQ(num_voxels_ * kNumDataPacketsPerVoxel, num_data_packets);
  for (size_t voxel_idx = 0u, data_idx = 0u;
       voxel_idx < num_voxels_ && data_idx < num_data_packets;
       ++voxel_idx, data_idx += kNumDataPacketsPerVoxel) {
    const uint32_t bytes_1 = data[data_idx];
    const uint32_t bytes_2 = data[data_idx + 1u];
    const uint32_t bytes_3 = data[data_idx + 2u];

    TsdfVoxel& voxel = voxels_[voxel_idx];

    // TODO(mfehr, helenol): find a better way to do this!

    memcpy(&(voxel.distance), &bytes_1, sizeof(bytes_1));
    memcpy(&(voxel.weight), &bytes_2, sizeof(bytes_2));

    voxel.color.r = static_cast<uint8_t>(bytes_3 >> 24);
    voxel.color.g = static_cast<uint8_t>((bytes_3 & 0x00FF0000) >> 16);
    voxel.color.b = static_cast<uint8_t>((bytes_3 & 0x0000FF00) >> 8);
    voxel.color.a = static_cast<uint8_t>(bytes_3 & 0x000000FF);
  }
}

template <>
void Block<OccupancyVoxel>::deserializeFromIntegers(
    const std::vector<uint32_t>& data) {
  constexpr size_t kNumDataPacketsPerVoxel = 2u;
  const size_t num_data_packets = data.size();
  CHECK_EQ(num_voxels_ * kNumDataPacketsPerVoxel, num_data_packets);
  for (size_t voxel_idx = 0u, data_idx = 0u;
       voxel_idx < num_voxels_ && data_idx < num_data_packets;
       ++voxel_idx, data_idx += kNumDataPacketsPerVoxel) {
    const uint32_t bytes_1 = data[data_idx];
    const uint32_t bytes_2 = data[data_idx + 1u];

    OccupancyVoxel& voxel = voxels_[voxel_idx];

    memcpy(&(voxel.probability_log), &bytes_1, sizeof(bytes_1));
    voxel.observed = static_cast<bool>(bytes_2 & 0x000000FF);
  }
}

template <>
void Block<EsdfVoxel>::deserializeFromIntegers(
    const std::vector<uint32_t>& data) {
  constexpr size_t kNumDataPacketsPerVoxel = 2u;
  const size_t num_data_packets = data.size();
  CHECK_EQ(num_voxels_ * kNumDataPacketsPerVoxel, num_data_packets);
  for (size_t voxel_idx = 0u, data_idx = 0u;
       voxel_idx < num_voxels_ && data_idx < num_data_packets;
       ++voxel_idx, data_idx += kNumDataPacketsPerVoxel) {
    const uint32_t bytes_1 = data[data_idx];
    const uint32_t bytes_2 = data[data_idx + 1u];

    EsdfVoxel& voxel = voxels_[voxel_idx];

    memcpy(&(voxel.distance), &bytes_1, sizeof(bytes_1));

    voxel.observed = static_cast<bool>(bytes_2 & 0x000000FF);
    voxel.in_queue = static_cast<bool>(bytes_2 & 0x0000FF00);
    voxel.fixed = static_cast<bool>(bytes_2 & 0x00FF0000);
    voxel.parent =
        deserializeDirection(static_cast<uint8_t>(bytes_2 & 0xFF000000));
  }
}

// Serialization functions:
template <>
void Block<TsdfVoxel>::serializeToIntegers(std::vector<uint32_t>* data) const {
  CHECK_NOTNULL(data);
  constexpr size_t kNumDataPacketsPerVoxel = 3u;
  data->clear();
  data->reserve(num_voxels_ * kNumDataPacketsPerVoxel);
  for (size_t voxel_idx = 0u; voxel_idx < num_voxels_; ++voxel_idx) {
    const TsdfVoxel& voxel = voxels_[voxel_idx];

    // TODO(mfehr, helenol): find a better way to do this!

    const uint32_t* bytes_1_ptr =
        reinterpret_cast<const uint32_t*>(&voxel.distance);
    data->push_back(*bytes_1_ptr);

    const uint32_t* bytes_2_ptr =
        reinterpret_cast<const uint32_t*>(&voxel.weight);
    data->push_back(*bytes_2_ptr);

    data->push_back(static_cast<uint32_t>(voxel.color.a) |
                    (static_cast<uint32_t>(voxel.color.b) << 8) |
                    (static_cast<uint32_t>(voxel.color.g) << 16) |
                    (static_cast<uint32_t>(voxel.color.r) << 24));
  }
  CHECK_EQ(num_voxels_ * kNumDataPacketsPerVoxel, data->size());
}

template <>
void Block<OccupancyVoxel>::serializeToIntegers(
    std::vector<uint32_t>* data) const {
  CHECK_NOTNULL(data);
  constexpr size_t kNumDataPacketsPerVoxel = 2u;
  data->clear();
  data->reserve(num_voxels_ * kNumDataPacketsPerVoxel);
  for (size_t voxel_idx = 0u; voxel_idx < num_voxels_; ++voxel_idx) {
    const OccupancyVoxel& voxel = voxels_[voxel_idx];

    const uint32_t* bytes_1_ptr =
        reinterpret_cast<const uint32_t*>(&voxel.probability_log);
    data->push_back(*bytes_1_ptr);
    data->push_back(static_cast<uint32_t>(voxel.observed));
  }
  CHECK_EQ(num_voxels_ * kNumDataPacketsPerVoxel, data->size());
}

template <>
void Block<EsdfVoxel>::serializeToIntegers(std::vector<uint32_t>* data) const {
  constexpr size_t kNumDataPacketsPerVoxel = 2u;
  data->clear();
  data->reserve(num_voxels_ * kNumDataPacketsPerVoxel);
  for (size_t voxel_idx = 0u; voxel_idx < num_voxels_; ++voxel_idx) {
    const EsdfVoxel& voxel = voxels_[voxel_idx];

    const uint32_t* bytes_1_ptr =
        reinterpret_cast<const uint32_t*>(&voxel.distance);
    data->push_back(*bytes_1_ptr);
    // Repack this as a bunch of bools. Could also pack into a since uint8_t
    // to save space, but this maybe simpler.
    // observed is byte 1, in_queue is byte 2, fixed is byte 3,
    // then direction is packed into byte 4.
    uint8_t byte1 = voxel.observed;
    uint8_t byte2 = voxel.in_queue;
    uint8_t byte3 = voxel.fixed;
    // Packing here is a bit more creative. 2 bits per direction.
    // [0 0] = 0, [1 0] = -1, [0 1] = 1.
    uint8_t byte4 = serializeDirection(voxel.parent);
    data->push_back(static_cast<uint32_t>(byte1) |
                    (static_cast<uint32_t>(byte2) << 8) |
                    (static_cast<uint32_t>(byte3) << 16) |
                    (static_cast<uint32_t>(byte4) << 24));
  }
  CHECK_EQ(num_voxels_ * kNumDataPacketsPerVoxel, data->size());
}

}  // namespace voxblox
<commit_msg>One more CNN<commit_after>#include "voxblox/core/block.h"
#include "voxblox/core/voxel.h"

namespace voxblox {

// Hidden serialization helpers:
uint8_t serializeDirection(const Eigen::Vector3i& dir) {
  uint8_t data = 0;
  // [0 0] = 0, [1 0] = -1, [0 1] = 1.
  uint8_t dir_x = dir.x() == 0 ? 0 : dir.x() > 0 ? 1 : 2;
  uint8_t dir_y = dir.y() == 0 ? 0 : dir.y() > 0 ? 1 : 2;
  uint8_t dir_z = dir.z() == 0 ? 0 : dir.z() > 0 ? 1 : 2;

  // Leading 2 bits are 0.
  data = dir_x << 4 | dir_y << 2 | dir_z;
  return data;
}

Eigen::Vector3i deserializeDirection(uint8_t data) {
  Eigen::Vector3i dir;
  uint8_t byte_x = data >> 4;
  uint8_t byte_y = (data >> 2) & (2 | 1);
  uint8_t byte_z = data & (2 | 1);

  // [0 0] = 0, [1 0] = -1, [0 1] = 1.
  dir.x() = byte_x == 0 ? 0 : byte_x == 2 ? -1 : 1;
  dir.y() = byte_y == 0 ? 0 : byte_y == 2 ? -1 : 1;
  dir.z() = byte_z == 0 ? 0 : byte_z == 2 ? -1 : 1;

  return dir;
}

// Deserialization functions:
template <>
void Block<TsdfVoxel>::deserializeFromIntegers(
    const std::vector<uint32_t>& data) {
  constexpr size_t kNumDataPacketsPerVoxel = 3u;
  const size_t num_data_packets = data.size();
  CHECK_EQ(num_voxels_ * kNumDataPacketsPerVoxel, num_data_packets);
  for (size_t voxel_idx = 0u, data_idx = 0u;
       voxel_idx < num_voxels_ && data_idx < num_data_packets;
       ++voxel_idx, data_idx += kNumDataPacketsPerVoxel) {
    const uint32_t bytes_1 = data[data_idx];
    const uint32_t bytes_2 = data[data_idx + 1u];
    const uint32_t bytes_3 = data[data_idx + 2u];

    TsdfVoxel& voxel = voxels_[voxel_idx];

    // TODO(mfehr, helenol): find a better way to do this!

    memcpy(&(voxel.distance), &bytes_1, sizeof(bytes_1));
    memcpy(&(voxel.weight), &bytes_2, sizeof(bytes_2));

    voxel.color.r = static_cast<uint8_t>(bytes_3 >> 24);
    voxel.color.g = static_cast<uint8_t>((bytes_3 & 0x00FF0000) >> 16);
    voxel.color.b = static_cast<uint8_t>((bytes_3 & 0x0000FF00) >> 8);
    voxel.color.a = static_cast<uint8_t>(bytes_3 & 0x000000FF);
  }
}

template <>
void Block<OccupancyVoxel>::deserializeFromIntegers(
    const std::vector<uint32_t>& data) {
  constexpr size_t kNumDataPacketsPerVoxel = 2u;
  const size_t num_data_packets = data.size();
  CHECK_EQ(num_voxels_ * kNumDataPacketsPerVoxel, num_data_packets);
  for (size_t voxel_idx = 0u, data_idx = 0u;
       voxel_idx < num_voxels_ && data_idx < num_data_packets;
       ++voxel_idx, data_idx += kNumDataPacketsPerVoxel) {
    const uint32_t bytes_1 = data[data_idx];
    const uint32_t bytes_2 = data[data_idx + 1u];

    OccupancyVoxel& voxel = voxels_[voxel_idx];

    memcpy(&(voxel.probability_log), &bytes_1, sizeof(bytes_1));
    voxel.observed = static_cast<bool>(bytes_2 & 0x000000FF);
  }
}

template <>
void Block<EsdfVoxel>::deserializeFromIntegers(
    const std::vector<uint32_t>& data) {
  constexpr size_t kNumDataPacketsPerVoxel = 2u;
  const size_t num_data_packets = data.size();
  CHECK_EQ(num_voxels_ * kNumDataPacketsPerVoxel, num_data_packets);
  for (size_t voxel_idx = 0u, data_idx = 0u;
       voxel_idx < num_voxels_ && data_idx < num_data_packets;
       ++voxel_idx, data_idx += kNumDataPacketsPerVoxel) {
    const uint32_t bytes_1 = data[data_idx];
    const uint32_t bytes_2 = data[data_idx + 1u];

    EsdfVoxel& voxel = voxels_[voxel_idx];

    memcpy(&(voxel.distance), &bytes_1, sizeof(bytes_1));

    voxel.observed = static_cast<bool>(bytes_2 & 0x000000FF);
    voxel.in_queue = static_cast<bool>(bytes_2 & 0x0000FF00);
    voxel.fixed = static_cast<bool>(bytes_2 & 0x00FF0000);
    voxel.parent =
        deserializeDirection(static_cast<uint8_t>(bytes_2 & 0xFF000000));
  }
}

// Serialization functions:
template <>
void Block<TsdfVoxel>::serializeToIntegers(std::vector<uint32_t>* data) const {
  CHECK_NOTNULL(data);
  constexpr size_t kNumDataPacketsPerVoxel = 3u;
  data->clear();
  data->reserve(num_voxels_ * kNumDataPacketsPerVoxel);
  for (size_t voxel_idx = 0u; voxel_idx < num_voxels_; ++voxel_idx) {
    const TsdfVoxel& voxel = voxels_[voxel_idx];

    // TODO(mfehr, helenol): find a better way to do this!
    const uint32_t* bytes_1_ptr =
        reinterpret_cast<const uint32_t*>(&voxel.distance);
    data->push_back(*bytes_1_ptr);

    const uint32_t* bytes_2_ptr =
        reinterpret_cast<const uint32_t*>(&voxel.weight);
    data->push_back(*bytes_2_ptr);

    data->push_back(static_cast<uint32_t>(voxel.color.a) |
                    (static_cast<uint32_t>(voxel.color.b) << 8) |
                    (static_cast<uint32_t>(voxel.color.g) << 16) |
                    (static_cast<uint32_t>(voxel.color.r) << 24));
  }
  CHECK_EQ(num_voxels_ * kNumDataPacketsPerVoxel, data->size());
}

template <>
void Block<OccupancyVoxel>::serializeToIntegers(
    std::vector<uint32_t>* data) const {
  CHECK_NOTNULL(data);
  constexpr size_t kNumDataPacketsPerVoxel = 2u;
  data->clear();
  data->reserve(num_voxels_ * kNumDataPacketsPerVoxel);
  for (size_t voxel_idx = 0u; voxel_idx < num_voxels_; ++voxel_idx) {
    const OccupancyVoxel& voxel = voxels_[voxel_idx];

    const uint32_t* bytes_1_ptr =
        reinterpret_cast<const uint32_t*>(&voxel.probability_log);
    data->push_back(*bytes_1_ptr);
    data->push_back(static_cast<uint32_t>(voxel.observed));
  }
  CHECK_EQ(num_voxels_ * kNumDataPacketsPerVoxel, data->size());
}

template <>
void Block<EsdfVoxel>::serializeToIntegers(std::vector<uint32_t>* data) const {
  CHECK_NOTNULL(data);
  constexpr size_t kNumDataPacketsPerVoxel = 2u;
  data->clear();
  data->reserve(num_voxels_ * kNumDataPacketsPerVoxel);
  for (size_t voxel_idx = 0u; voxel_idx < num_voxels_; ++voxel_idx) {
    const EsdfVoxel& voxel = voxels_[voxel_idx];

    const uint32_t* bytes_1_ptr =
        reinterpret_cast<const uint32_t*>(&voxel.distance);
    data->push_back(*bytes_1_ptr);
    // Repack this as a bunch of bools. Could also pack into a since uint8_t
    // to save space, but this maybe simpler.
    // observed is byte 1, in_queue is byte 2, fixed is byte 3,
    // then direction is packed into byte 4.
    uint8_t byte1 = voxel.observed;
    uint8_t byte2 = voxel.in_queue;
    uint8_t byte3 = voxel.fixed;
    // Packing here is a bit more creative. 2 bits per direction.
    // [0 0] = 0, [1 0] = -1, [0 1] = 1.
    uint8_t byte4 = serializeDirection(voxel.parent);
    data->push_back(static_cast<uint32_t>(byte1) |
                    (static_cast<uint32_t>(byte2) << 8) |
                    (static_cast<uint32_t>(byte3) << 16) |
                    (static_cast<uint32_t>(byte4) << 24));
  }
  CHECK_EQ(num_voxels_ * kNumDataPacketsPerVoxel, data->size());
}

}  // namespace voxblox
<|endoftext|>


<commit_before>#include "config.hpp"
#include <cstdlib>
#include <sstream>
#include <fstream>
#include <libevdev/libevdev.h>
#include <linux/input.h>

#include <compositor.h>

#include <config.h>

std::ofstream out;

using std::string;
/* TODO: add checks to see if values are correct */

string wayfire_config_section::get_string(string name, string default_value)
{
    auto it = options.find(name);
    return (it == options.end() ? default_value : it->second);
}

int wayfire_config_section::get_int(string name, int df)
{
    auto it = options.find(name);
    return (it == options.end() ? df : std::atoi(it->second.c_str()));
}

int wayfire_config_section::get_duration(string name, int df)
{
    int result = get_int(name, df * (1000 / refresh_rate));
    return result / (1000 / refresh_rate);
}

double wayfire_config_section::get_double(string name, double df)
{
    auto it = options.find(name);
    return (it == options.end() ? df : std::atof(it->second.c_str()));
}

wayfire_key wayfire_config_section::get_key(string name, wayfire_key df)
{
    auto it = options.find(name);
    if (it == options.end())
        return df;

    if (it->second == "none")
        return {0, 0};

    std::stringstream ss(it->second);
    std::vector<std::string> items;
    std::string t;
    while(ss >> t)
        items.push_back(t);

    wayfire_key ans;

    ans.mod = 0;
    for (size_t i = 0; i < items.size() - 1; i++) {
        if (items[i] == "<alt>")
            ans.mod |= MODIFIER_ALT;
        if (items[i] == "<ctrl>")
            ans.mod |= MODIFIER_CTRL;
        if (items[i] == "<shift>")
            ans.mod |= MODIFIER_SHIFT;
        if (items[i] == "<super>")
            ans.mod |= MODIFIER_SUPER;
    }

    ans.keyval = libevdev_event_code_from_name(EV_KEY, items[items.size() - 1].c_str());
    return ans;
}

wayfire_button wayfire_config_section::get_button(string name, wayfire_button df)
{
    auto it = options.find(name);
    if (it == options.end())
        return df;

    if (it->second == "none")
        return {0, 0};

    std::stringstream ss(it->second);
    std::vector<std::string> items;
    std::string t;
    while(ss >> t)
        items.push_back(t);

    if (items.empty())
        return df;

    wayfire_button ans;
    ans.mod = 0;

    for (size_t i = 0; i < items.size() - 1; i++)
    {
        if (items[i] == "<alt>")
            ans.mod |= MODIFIER_ALT;
        if (items[i] == "<ctrl>")
            ans.mod |= MODIFIER_CTRL;
        if (items[i] == "<shift>")
            ans.mod |= MODIFIER_SHIFT;
        if (items[i] == "<super>")
            ans.mod |= MODIFIER_SUPER;
    }

    auto button = items[items.size() - 1];
    if (button == "left")
        ans.button = BTN_LEFT;
    else if (button == "right")
        ans.button = BTN_RIGHT;
    else if (button == "middle")
        ans.button = BTN_MIDDLE;
    else
        ans.button = 0;

    return ans;
}

wayfire_color wayfire_config_section::get_color(string name, wayfire_color df)
{
    auto it = options.find(name);
    if (it == options.end())
        return df;

    wayfire_color ans = {0, 0, 0, 0};
    std::stringstream ss(it->second);
    ss >> ans.r >> ans.g >> ans.b >> ans.a;
    return ans;
}

namespace
{
    string trim(string x)
    {
        int i = 0, j = x.length() - 1;
        while(i < (int)x.length() && std::iswspace(x[i])) ++i;
        while(j >= 0 && std::iswspace(x[j])) --j;

        if (i <= j)
            return x.substr(i, j - i + 1);
        else
            return "";
    }
}

wayfire_config::wayfire_config(string name, int rr)
{
    std::ifstream file(name);
    string line;

#if WAYFIRE_DEBUG_ENABLED
    out.open("/tmp/.wayfire_config_debug");
    out << "use config: " << name << std::endl;
#endif

    refresh_rate = rr;
    wayfire_config_section *current_section;
    int line_id = -1;

    while(std::getline(file, line))
    {
        ++line_id;

        line = trim(line);
        if (line.size() == 0 || line[0] == '#')
            continue;

#if WAYFIRE_DEBUG_ENABLED
        out << "process line " << line << std::endl;
#endif

        if (line[0] == '[')
        {
            current_section = new wayfire_config_section();
            current_section->refresh_rate = rr;
            current_section->name = line.substr(1, line.size() - 2);
            sections.push_back(current_section);
            continue;
        }

        string name, value;
        int i = 0;
        while (i < (int)line.size() && line[i] != '=') i++;
        name = trim(line.substr(0, i));
        if (i < (int)line.size())
        {
            value = trim(line.substr(i + 1, line.size() - i - 1));
            current_section->options[name] = value;
        }
    }
}

wayfire_config_section* wayfire_config::get_section(string name)
{
    for (auto section : sections)
        if (section->name == name)
            return section;

    auto nsect = new wayfire_config_section();
    nsect->name = name;
    nsect->refresh_rate = refresh_rate;
    sections.push_back(nsect);
    return nsect;
}
<commit_msg>config: implement comments and multi-line options<commit_after>#include "config.hpp"
#include <cstdlib>
#include <sstream>
#include <fstream>
#include <libevdev/libevdev.h>
#include <linux/input.h>

#include <compositor.h>

#include <config.h>

std::ofstream out;

using std::string;
/* TODO: add checks to see if values are correct */

string wayfire_config_section::get_string(string name, string default_value)
{
    auto it = options.find(name);
    return (it == options.end() ? default_value : it->second);
}

int wayfire_config_section::get_int(string name, int df)
{
    auto it = options.find(name);
    return (it == options.end() ? df : std::atoi(it->second.c_str()));
}

int wayfire_config_section::get_duration(string name, int df)
{
    int result = get_int(name, df * (1000 / refresh_rate));
    return result / (1000 / refresh_rate);
}

double wayfire_config_section::get_double(string name, double df)
{
    auto it = options.find(name);
    return (it == options.end() ? df : std::atof(it->second.c_str()));
}

wayfire_key wayfire_config_section::get_key(string name, wayfire_key df)
{
    auto it = options.find(name);
    if (it == options.end())
        return df;

    if (it->second == "none")
        return {0, 0};

    std::stringstream ss(it->second);
    std::vector<std::string> items;
    std::string t;
    while(ss >> t)
        items.push_back(t);

    wayfire_key ans;

    ans.mod = 0;
    for (size_t i = 0; i < items.size() - 1; i++) {
        if (items[i] == "<alt>")
            ans.mod |= MODIFIER_ALT;
        if (items[i] == "<ctrl>")
            ans.mod |= MODIFIER_CTRL;
        if (items[i] == "<shift>")
            ans.mod |= MODIFIER_SHIFT;
        if (items[i] == "<super>")
            ans.mod |= MODIFIER_SUPER;
    }

    ans.keyval = libevdev_event_code_from_name(EV_KEY, items[items.size() - 1].c_str());
    return ans;
}

wayfire_button wayfire_config_section::get_button(string name, wayfire_button df)
{
    auto it = options.find(name);
    if (it == options.end())
        return df;

    if (it->second == "none")
        return {0, 0};

    std::stringstream ss(it->second);
    std::vector<std::string> items;
    std::string t;
    while(ss >> t)
        items.push_back(t);

    if (items.empty())
        return df;

    wayfire_button ans;
    ans.mod = 0;

    for (size_t i = 0; i < items.size() - 1; i++)
    {
        if (items[i] == "<alt>")
            ans.mod |= MODIFIER_ALT;
        if (items[i] == "<ctrl>")
            ans.mod |= MODIFIER_CTRL;
        if (items[i] == "<shift>")
            ans.mod |= MODIFIER_SHIFT;
        if (items[i] == "<super>")
            ans.mod |= MODIFIER_SUPER;
    }

    auto button = items[items.size() - 1];
    if (button == "left")
        ans.button = BTN_LEFT;
    else if (button == "right")
        ans.button = BTN_RIGHT;
    else if (button == "middle")
        ans.button = BTN_MIDDLE;
    else
        ans.button = 0;

    return ans;
}

wayfire_color wayfire_config_section::get_color(string name, wayfire_color df)
{
    auto it = options.find(name);
    if (it == options.end())
        return df;

    wayfire_color ans = {0, 0, 0, 0};
    std::stringstream ss(it->second);
    ss >> ans.r >> ans.g >> ans.b >> ans.a;
    return ans;
}

static string trim(const string& x)
{
    int i = 0, j = x.length() - 1;
    while(i < (int)x.length() && std::iswspace(x[i])) ++i;
    while(j >= 0 && std::iswspace(x[j])) --j;

    if (i <= j)
        return x.substr(i, j - i + 1);
    else
        return "";
}

using lines_t = std::vector<string>;
static void prune_comments(lines_t& file)
{
    for (auto& line : file)
    {
        size_t i = line.find_first_of('#');
        if (i != string::npos)
            line = line.substr(0, i);
    }
}

static lines_t filter_empty_lines(const lines_t& file)
{
    lines_t pruned;
    for (const auto& line : file)
    {
        string cleaned_line = trim(line);
        if (cleaned_line.size())
            pruned.push_back(cleaned_line);
    }

    return pruned;
}

static lines_t merge_lines(const lines_t& file)
{
    lines_t merged;
    int i = 0;
    for (; i < (int)file.size(); i++)
    {
        string result = file[i]; ++i;
        while(file[i - 1].back() == '\\' && i < (int)file.size())
        {
            result.pop_back();
            result += file[i++];
        }
        --i;

        merged.push_back(result);
    }

    return merged;
}

wayfire_config::wayfire_config(string name, int rr)
{
    std::ifstream file(name);
    string line;

#if WAYFIRE_DEBUG_ENABLED
    out.open("/tmp/.wayfire_config_debug");
    out << "use config: " << name << std::endl;
#endif

    refresh_rate = rr;
    wayfire_config_section *current_section;

    lines_t lines;
    while(std::getline(file, line))
    {
        lines.push_back(line);
    }

    prune_comments(lines);
    lines = filter_empty_lines(lines);
    lines = merge_lines(lines);

    for (auto line : lines)
    {
        if (line[0] == '[')
        {
            current_section = new wayfire_config_section();
            current_section->refresh_rate = rr;
            current_section->name = line.substr(1, line.size() - 2);
            sections.push_back(current_section);
            continue;
        }

        string name, value;
        size_t i = line.find_first_of('=');
        if (i != string::npos)
        {
            name = trim(line.substr(0, i));
            value = trim(line.substr(i + 1, line.size() - i - 1));
            current_section->options[name] = value;

#if WAYFIRE_DEBUG_ENABLED
            out << current_section->name << ": " << name << " = " << value << std::endl;
#endif
        }
    }
}

wayfire_config_section* wayfire_config::get_section(string name)
{
    for (auto section : sections)
        if (section->name == name)
            return section;

    auto nsect = new wayfire_config_section();
    nsect->name = name;
    nsect->refresh_rate = refresh_rate;
    sections.push_back(nsect);
    return nsect;
}
<|endoftext|>


<commit_before>// @(#)root/mathmore:$Id$
// Authors: L. Moneta, A. Zsenei   08/2005

 /**********************************************************************
  *                                                                    *
  * Copyright (c) 2004 ROOT Foundation,  CERN/PH-SFT                   *
  *                                                                    *
  * This library is free software; you can redistribute it and/or      *
  * modify it under the terms of the GNU General Public License        *
  * as published by the Free Software Foundation; either version 2     *
  * of the License, or (at your option) any later version.             *
  *                                                                    *
  * This library is distributed in the hope that it will be useful,    *
  * but WITHOUT ANY WARRANTY; without even the implied warranty of     *
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU   *
  * General Public License for more details.                           *
  *                                                                    *
  * You should have received a copy of the GNU General Public License  *
  * along with this library (see file COPYING); if not, write          *
  * to the Free Software Foundation, Inc., 59 Temple Place, Suite      *
  * 330, Boston, MA 02111-1307 USA, or contact the author.             *
  *                                                                    *
  **********************************************************************/

// Implementation file for class GSLMultiRootFinder
//
// Created by: moneta  at Sun Nov 14 11:27:11 2004
//
// Last update: Sun Nov 14 11:27:11 2004
//

#include "Math/IFunction.h"
#include "Math/GSLMultiRootFinder.h"
#include "GSLMultiRootSolver.h"
#include "Math/Error.h"

#include "gsl/gsl_multiroots.h"
#include "gsl/gsl_errno.h"
#include <cmath>
#include <iomanip>

#include <algorithm>
#include <functional>
#include <ctype.h>   // need to use c version of tolower defined here

namespace ROOT {
namespace Math {

   // default values

   double gDefaultMaxIter = 100;
   double gDefaultAbsTolerance = 1.E-6;
   double gDefaultRelTolerance = 1.E-10;

// impelmentation of static methods
void GSLMultiRootFinder::SetDefaultTolerance(double abstol, double reltol ) {
   // set default tolerance
      gDefaultAbsTolerance = abstol;
      if (reltol > 0) gDefaultRelTolerance = reltol;
}
void GSLMultiRootFinder::SetDefaultMaxIterations(int maxiter) {
   // set default max iter
   gDefaultMaxIter = maxiter;
}

GSLMultiRootFinder::GSLMultiRootFinder(EType type) :
   fIter(0), fStatus(-1), fPrintLevel(0),
   fType(type), fUseDerivAlgo(false),
   fSolver(0)
{
   // constructor for non derivative type
   fFunctions.reserve(2);
}

GSLMultiRootFinder::GSLMultiRootFinder(EDerivType type) :
   fIter(0), fStatus(-1), fPrintLevel(0),
   fType(type), fUseDerivAlgo(true),
   fSolver(0)
{
   // constructor for non derivative type
   fFunctions.reserve(2);
}

GSLMultiRootFinder::GSLMultiRootFinder(const char * name) :
   fIter(0), fStatus(-1), fPrintLevel(0),
   fType(0), fUseDerivAlgo(false),
   fSolver(0)
{
   // constructor for a string
   fFunctions.reserve(2);
   std::pair<bool,int> type = GetType(name);
   fUseDerivAlgo = type.first;
   fType = type.second;
}

GSLMultiRootFinder::~GSLMultiRootFinder()
{
   // delete function wrapper
   ClearFunctions();
   if (fSolver) delete fSolver;
}

GSLMultiRootFinder::GSLMultiRootFinder(const GSLMultiRootFinder &)
{
}

GSLMultiRootFinder & GSLMultiRootFinder::operator = (const GSLMultiRootFinder &rhs)
{
   // dummy operator=
   if (this == &rhs) return *this;  // time saving self-test

   return *this;
}

int GSLMultiRootFinder::AddFunction(const ROOT::Math::IMultiGenFunction & func) {
   // add a new function in the vector
   ROOT::Math::IMultiGenFunction * f = func.Clone();
   if (!f) return 0;
   fFunctions.push_back(f);
   return fFunctions.size();
}

void GSLMultiRootFinder::ClearFunctions() {
   // clear the function list
   for (unsigned int i = 0; i < fFunctions.size(); ++i) {
      if (fFunctions[i] != 0 ) delete fFunctions[i];
      fFunctions[i] = 0;
   }
   fFunctions.clear();
}

void GSLMultiRootFinder::Clear() {
   // clear the function list and the solver
   ClearFunctions();
   if (fSolver) Clear();
   fSolver = 0;
}

const double * GSLMultiRootFinder::X() const {
   // return x
   return (fSolver != 0) ? fSolver->X() : 0;
}
const double * GSLMultiRootFinder::Dx() const {
   // return x
   return (fSolver != 0) ? fSolver->Dx() : 0;
}
const double * GSLMultiRootFinder::FVal() const {
   // return x
   return (fSolver != 0) ? fSolver->FVal() : 0;
}
const char * GSLMultiRootFinder::Name() const {
   // get GSL name
   return (fSolver != 0) ? fSolver->Name().c_str() : "";
}

// bool GSLMultiRootFinder::AddFunction( const ROOT::Math::IMultiGenFunction & func) {
//    // clone and add function to the list
//    // If using a derivative algorithm the function is checked if it implements
//    // the gradient interface. If this is not the case the type is set to non-derivatibe algo
//    ROOT::Math::IGenMultiFunction * f = func.Clone();
//    if (f != 0) return false;
//    if (fUseDerivAlgo)  {
//       bool gradFunc = (dynamic_cast<ROOT::Math::IMultiGradFunction *> (f) != 0 );
//       if (!gradFunc)  {
//          MATH_ERROR_MSG("GSLMultiRootFinder::AddFunction","Function does not provide gradient interface");
//          MATH_WARN_MSG("GSLMultiRootFinder::AddFunction","clear the function list");
//          ClearFunctions();
//          return false;
//       }
//    }
//    fFunctions.push_back(f);
//    return true;
// }

   const gsl_multiroot_fsolver_type * GetGSLType(GSLMultiRootFinder::EType type) {
     //helper functions to find GSL type
   switch(type)
      {
      case ROOT::Math::GSLMultiRootFinder::kHybridS:
         return gsl_multiroot_fsolver_hybrids;
      case ROOT::Math::GSLMultiRootFinder::kHybrid:
         return gsl_multiroot_fsolver_hybrid;
      case ROOT::Math::GSLMultiRootFinder::kDNewton:
         return gsl_multiroot_fsolver_dnewton;
      case ROOT::Math::GSLMultiRootFinder::kBroyden:
         return gsl_multiroot_fsolver_broyden;
      default:
         return gsl_multiroot_fsolver_hybrids;
      }
   return 0;
}

const gsl_multiroot_fdfsolver_type * GetGSLDerivType(GSLMultiRootFinder::EDerivType type) {
//helper functions to find GSL deriv type
   switch(type)
   {
   case ROOT::Math::GSLMultiRootFinder::kHybridSJ :
      return gsl_multiroot_fdfsolver_hybridsj;
   case ROOT::Math::GSLMultiRootFinder::kHybridJ :
      return gsl_multiroot_fdfsolver_hybridj;
   case ROOT::Math::GSLMultiRootFinder::kNewton :
      return gsl_multiroot_fdfsolver_newton;
   case ROOT::Math::GSLMultiRootFinder::kGNewton :
      return gsl_multiroot_fdfsolver_gnewton;
   default:
      return gsl_multiroot_fdfsolver_hybridsj;
   }
   return 0; // cannot happen
}

std::pair<bool,int> GSLMultiRootFinder::GetType(const char * name) {
   if (name == 0) return std::make_pair<bool,int>(false, -1);
   std::string aname = name;
   std::transform(aname.begin(), aname.end(), aname.begin(), (int(*)(int)) tolower );

   if (aname.find("hybridsj") != std::string::npos) return std::make_pair(true,  kHybridSJ);
   if (aname.find("hybridj") != std::string::npos) return std::make_pair(true,  kHybridJ);
   if (aname.find("hybrids") != std::string::npos) return std::make_pair(false,  kHybridS);
   if (aname.find("hybrid") != std::string::npos) return std::make_pair(false,  kHybrid);
   if (aname.find("gnewton") != std::string::npos) return std::make_pair(true,  kGNewton);
   if (aname.find("dnewton") != std::string::npos) return std::make_pair(false,  kDNewton);
   if (aname.find("newton") != std::string::npos) return std::make_pair(true,  kNewton);
   if (aname.find("broyden") != std::string::npos) return std::make_pair(false,  kBroyden);
   MATH_INFO_MSG("GSLMultiRootFinder::GetType","Unknow algorithm - use default one");
   return std::make_pair(false, -1);
}

bool GSLMultiRootFinder::Solve (const double * x, int maxIter, double absTol, double relTol)
{
   fIter = 0;
   // create the solvers - delete previous existing solver
   if (fSolver) delete fSolver;
   fSolver = 0;

   if (fFunctions.size() == 0) {
      MATH_ERROR_MSG("GSLMultiRootFinder::Solve","Function list is empty");
      fStatus = -1;
      return false;
   }

   if (fUseDerivAlgo) {
      EDerivType type = (EDerivType) fType;
      if (!fSolver) fSolver = new GSLMultiRootDerivSolver( GetGSLDerivType(type), Dim() );
   }
   else {
      EType type = (EType) fType;
      if (!fSolver) fSolver = new GSLMultiRootSolver( GetGSLType(type), Dim() );
   }

   // first set initial values and function
   assert(fSolver != 0);
   bool ret = fSolver->InitSolver( fFunctions, x);
   if (!ret) {
      MATH_ERROR_MSG("GSLMultiRootFinder::Solve","Error initializing the solver");
      fStatus = -2;
      return false;
   }

   if (maxIter == 0) maxIter = gDefaultMaxIter;
   if (absTol <= 0) absTol = gDefaultAbsTolerance;
   if (relTol <= 0) relTol = gDefaultRelTolerance;

   if (fPrintLevel >= 1)
      std::cout << "GSLMultiRootFinder::Solve:" << Name() << " max iterations " <<   maxIter << " and  tolerance " <<  absTol << std::endl;

   // find the roots by iterating
   fStatus = 0;
   int status = 0;
   int iter = 0;
   do {
      iter++;
      status = fSolver->Iterate();

      if (fPrintLevel >= 2) {
         std::cout << "GSLMultiRootFinder::Solve - iteration # " <<   iter << " status = " << status << std::endl;
         PrintState();
      }
      // act in case of error
      if (status == GSL_EBADFUNC) {
         MATH_ERROR_MSG("GSLMultiRootFinder::Solve","The iteration encountered a singolar point due to a bad function value");
         fStatus = status;
         break;
      }
      if (status == GSL_ENOPROG) {
         MATH_ERROR_MSG("GSLMultiRootFinder::Solve","The iteration is not making any progress");
         fStatus = status;
         break;
      }
      if (status != GSL_SUCCESS) {
         MATH_ERROR_MSG("GSLMultiRootFinder::Solve","Uknown iteration error - exit");
         fStatus = status;
         break;
      }

      // test also residual
      status =  fSolver->TestResidual(absTol);

      // should test also the Delta ??
      int status2 =  fSolver->TestDelta(absTol, relTol);
      if (status2 == GSL_SUCCESS) {
         MATH_INFO_MSG("GSLMultiRootFinder::Solve","The iteration converged");
      }
   }
   while (status == GSL_CONTINUE && iter < maxIter);
   if (status == GSL_CONTINUE) {
      MATH_INFO_MSGVAL("GSLMultiRootFinder::Solve","exceeded max iterations, reached tolerance is not sufficient",absTol);
   }
   if (status == GSL_SUCCESS) {
      if (fPrintLevel>=1) {          // print the result
         MATH_INFO_MSG("GSLMultiRootFinder::Solve","The iteration converged");
         std::cout << "GSL Algorithm used is :  " << fSolver->Name() << std::endl;
         std::cout << "Number of iterations  =  " << iter<< std::endl;

         PrintState();
      }
   }
   fIter = iter;
   fStatus = status;
   return  (fStatus == GSL_SUCCESS);

}

void GSLMultiRootFinder::PrintState(std::ostream & os) {
   // print current state
   if (!fSolver) return;
   int wi = int(std::log10(Dim() ) )+1;
   const double * xtmp = fSolver->X();
   const double * ftmp = fSolver->FVal();
   os << "Root values     = ";
   for (unsigned int i = 0; i< Dim(); ++i)
      os << "x[" << std::setw(wi) << i << "] = " << std::setw(12) << xtmp[i] << "   ";
   os << std::endl;
   os << "Function values = ";
   for (unsigned int i = 0; i< Dim(); ++i)
      os << "f[" << std::setw(wi) << i << "] = " << std::setw(12) << ftmp[i] << "   ";
   os << std::endl;
}

} // namespace Math
} // namespace ROOT
<commit_msg>fix a compilation warning and an error on Windows<commit_after>// @(#)root/mathmore:$Id$
// Authors: L. Moneta, A. Zsenei   08/2005

 /**********************************************************************
  *                                                                    *
  * Copyright (c) 2004 ROOT Foundation,  CERN/PH-SFT                   *
  *                                                                    *
  * This library is free software; you can redistribute it and/or      *
  * modify it under the terms of the GNU General Public License        *
  * as published by the Free Software Foundation; either version 2     *
  * of the License, or (at your option) any later version.             *
  *                                                                    *
  * This library is distributed in the hope that it will be useful,    *
  * but WITHOUT ANY WARRANTY; without even the implied warranty of     *
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU   *
  * General Public License for more details.                           *
  *                                                                    *
  * You should have received a copy of the GNU General Public License  *
  * along with this library (see file COPYING); if not, write          *
  * to the Free Software Foundation, Inc., 59 Temple Place, Suite      *
  * 330, Boston, MA 02111-1307 USA, or contact the author.             *
  *                                                                    *
  **********************************************************************/

// Implementation file for class GSLMultiRootFinder
//
// Created by: moneta  at Sun Nov 14 11:27:11 2004
//
// Last update: Sun Nov 14 11:27:11 2004
//

#include "Math/IFunction.h"
#include "Math/GSLMultiRootFinder.h"
#include "GSLMultiRootSolver.h"
#include "Math/Error.h"

#include "gsl/gsl_multiroots.h"
#include "gsl/gsl_errno.h"
#include <cmath>
#include <iomanip>

#include <algorithm>
#include <functional>
#include <ctype.h>   // need to use c version of tolower defined here

namespace ROOT {
namespace Math {

   // default values

   int gDefaultMaxIter = 100;
   double gDefaultAbsTolerance = 1.E-6;
   double gDefaultRelTolerance = 1.E-10;

// impelmentation of static methods
void GSLMultiRootFinder::SetDefaultTolerance(double abstol, double reltol ) {
   // set default tolerance
      gDefaultAbsTolerance = abstol;
      if (reltol > 0) gDefaultRelTolerance = reltol;
}
void GSLMultiRootFinder::SetDefaultMaxIterations(int maxiter) {
   // set default max iter
   gDefaultMaxIter = maxiter;
}

GSLMultiRootFinder::GSLMultiRootFinder(EType type) :
   fIter(0), fStatus(-1), fPrintLevel(0),
   fType(type), fUseDerivAlgo(false),
   fSolver(0)
{
   // constructor for non derivative type
   fFunctions.reserve(2);
}

GSLMultiRootFinder::GSLMultiRootFinder(EDerivType type) :
   fIter(0), fStatus(-1), fPrintLevel(0),
   fType(type), fUseDerivAlgo(true),
   fSolver(0)
{
   // constructor for non derivative type
   fFunctions.reserve(2);
}

GSLMultiRootFinder::GSLMultiRootFinder(const char * name) :
   fIter(0), fStatus(-1), fPrintLevel(0),
   fType(0), fUseDerivAlgo(false),
   fSolver(0)
{
   // constructor for a string
   fFunctions.reserve(2);
   std::pair<bool,int> type = GetType(name);
   fUseDerivAlgo = type.first;
   fType = type.second;
}

GSLMultiRootFinder::~GSLMultiRootFinder()
{
   // delete function wrapper
   ClearFunctions();
   if (fSolver) delete fSolver;
}

GSLMultiRootFinder::GSLMultiRootFinder(const GSLMultiRootFinder &)
{
}

GSLMultiRootFinder & GSLMultiRootFinder::operator = (const GSLMultiRootFinder &rhs)
{
   // dummy operator=
   if (this == &rhs) return *this;  // time saving self-test

   return *this;
}

int GSLMultiRootFinder::AddFunction(const ROOT::Math::IMultiGenFunction & func) {
   // add a new function in the vector
   ROOT::Math::IMultiGenFunction * f = func.Clone();
   if (!f) return 0;
   fFunctions.push_back(f);
   return fFunctions.size();
}

void GSLMultiRootFinder::ClearFunctions() {
   // clear the function list
   for (unsigned int i = 0; i < fFunctions.size(); ++i) {
      if (fFunctions[i] != 0 ) delete fFunctions[i];
      fFunctions[i] = 0;
   }
   fFunctions.clear();
}

void GSLMultiRootFinder::Clear() {
   // clear the function list and the solver
   ClearFunctions();
   if (fSolver) Clear();
   fSolver = 0;
}

const double * GSLMultiRootFinder::X() const {
   // return x
   return (fSolver != 0) ? fSolver->X() : 0;
}
const double * GSLMultiRootFinder::Dx() const {
   // return x
   return (fSolver != 0) ? fSolver->Dx() : 0;
}
const double * GSLMultiRootFinder::FVal() const {
   // return x
   return (fSolver != 0) ? fSolver->FVal() : 0;
}
const char * GSLMultiRootFinder::Name() const {
   // get GSL name
   return (fSolver != 0) ? fSolver->Name().c_str() : "";
}

// bool GSLMultiRootFinder::AddFunction( const ROOT::Math::IMultiGenFunction & func) {
//    // clone and add function to the list
//    // If using a derivative algorithm the function is checked if it implements
//    // the gradient interface. If this is not the case the type is set to non-derivatibe algo
//    ROOT::Math::IGenMultiFunction * f = func.Clone();
//    if (f != 0) return false;
//    if (fUseDerivAlgo)  {
//       bool gradFunc = (dynamic_cast<ROOT::Math::IMultiGradFunction *> (f) != 0 );
//       if (!gradFunc)  {
//          MATH_ERROR_MSG("GSLMultiRootFinder::AddFunction","Function does not provide gradient interface");
//          MATH_WARN_MSG("GSLMultiRootFinder::AddFunction","clear the function list");
//          ClearFunctions();
//          return false;
//       }
//    }
//    fFunctions.push_back(f);
//    return true;
// }

   const gsl_multiroot_fsolver_type * GetGSLType(GSLMultiRootFinder::EType type) {
     //helper functions to find GSL type
   switch(type)
      {
      case ROOT::Math::GSLMultiRootFinder::kHybridS:
         return gsl_multiroot_fsolver_hybrids;
      case ROOT::Math::GSLMultiRootFinder::kHybrid:
         return gsl_multiroot_fsolver_hybrid;
      case ROOT::Math::GSLMultiRootFinder::kDNewton:
         return gsl_multiroot_fsolver_dnewton;
      case ROOT::Math::GSLMultiRootFinder::kBroyden:
         return gsl_multiroot_fsolver_broyden;
      default:
         return gsl_multiroot_fsolver_hybrids;
      }
   return 0;
}

const gsl_multiroot_fdfsolver_type * GetGSLDerivType(GSLMultiRootFinder::EDerivType type) {
//helper functions to find GSL deriv type
   switch(type)
   {
   case ROOT::Math::GSLMultiRootFinder::kHybridSJ :
      return gsl_multiroot_fdfsolver_hybridsj;
   case ROOT::Math::GSLMultiRootFinder::kHybridJ :
      return gsl_multiroot_fdfsolver_hybridj;
   case ROOT::Math::GSLMultiRootFinder::kNewton :
      return gsl_multiroot_fdfsolver_newton;
   case ROOT::Math::GSLMultiRootFinder::kGNewton :
      return gsl_multiroot_fdfsolver_gnewton;
   default:
      return gsl_multiroot_fdfsolver_hybridsj;
   }
   return 0; // cannot happen
}

std::pair<bool,int> GSLMultiRootFinder::GetType(const char * name) {
   if (name == 0) return std::make_pair<bool,int>(false, -1);
   std::string aname = name;
   std::transform(aname.begin(), aname.end(), aname.begin(), (int(*)(int)) tolower );

   if (aname.find("hybridsj") != std::string::npos) return std::make_pair(true,  kHybridSJ);
   if (aname.find("hybridj") != std::string::npos) return std::make_pair(true,  kHybridJ);
   if (aname.find("hybrids") != std::string::npos) return std::make_pair(false,  kHybridS);
   if (aname.find("hybrid") != std::string::npos) return std::make_pair(false,  kHybrid);
   if (aname.find("gnewton") != std::string::npos) return std::make_pair(true,  kGNewton);
   if (aname.find("dnewton") != std::string::npos) return std::make_pair(false,  kDNewton);
   if (aname.find("newton") != std::string::npos) return std::make_pair(true,  kNewton);
   if (aname.find("broyden") != std::string::npos) return std::make_pair(false,  kBroyden);
   MATH_INFO_MSG("GSLMultiRootFinder::GetType","Unknow algorithm - use default one");
   return std::make_pair(false, -1);
}

bool GSLMultiRootFinder::Solve (const double * x, int maxIter, double absTol, double relTol)
{
   fIter = 0;
   // create the solvers - delete previous existing solver
   if (fSolver) delete fSolver;
   fSolver = 0;

   if (fFunctions.size() == 0) {
      MATH_ERROR_MSG("GSLMultiRootFinder::Solve","Function list is empty");
      fStatus = -1;
      return false;
   }

   if (fUseDerivAlgo) {
      EDerivType type = (EDerivType) fType;
      if (!fSolver) fSolver = new GSLMultiRootDerivSolver( GetGSLDerivType(type), Dim() );
   }
   else {
      EType type = (EType) fType;
      if (!fSolver) fSolver = new GSLMultiRootSolver( GetGSLType(type), Dim() );
   }

   // first set initial values and function
   assert(fSolver != 0);
   bool ret = fSolver->InitSolver( fFunctions, x);
   if (!ret) {
      MATH_ERROR_MSG("GSLMultiRootFinder::Solve","Error initializing the solver");
      fStatus = -2;
      return false;
   }

   if (maxIter == 0) maxIter = gDefaultMaxIter;
   if (absTol <= 0) absTol = gDefaultAbsTolerance;
   if (relTol <= 0) relTol = gDefaultRelTolerance;

   if (fPrintLevel >= 1)
      std::cout << "GSLMultiRootFinder::Solve:" << Name() << " max iterations " <<   maxIter << " and  tolerance " <<  absTol << std::endl;

   // find the roots by iterating
   fStatus = 0;
   int status = 0;
   int iter = 0;
   do {
      iter++;
      status = fSolver->Iterate();

      if (fPrintLevel >= 2) {
         std::cout << "GSLMultiRootFinder::Solve - iteration # " <<   iter << " status = " << status << std::endl;
         PrintState();
      }
      // act in case of error
      if (status == GSL_EBADFUNC) {
         MATH_ERROR_MSG("GSLMultiRootFinder::Solve","The iteration encountered a singolar point due to a bad function value");
         fStatus = status;
         break;
      }
      if (status == GSL_ENOPROG) {
         MATH_ERROR_MSG("GSLMultiRootFinder::Solve","The iteration is not making any progress");
         fStatus = status;
         break;
      }
      if (status != GSL_SUCCESS) {
         MATH_ERROR_MSG("GSLMultiRootFinder::Solve","Uknown iteration error - exit");
         fStatus = status;
         break;
      }

      // test also residual
      status =  fSolver->TestResidual(absTol);

      // should test also the Delta ??
      int status2 =  fSolver->TestDelta(absTol, relTol);
      if (status2 == GSL_SUCCESS) {
         MATH_INFO_MSG("GSLMultiRootFinder::Solve","The iteration converged");
      }
   }
   while (status == GSL_CONTINUE && iter < maxIter);
   if (status == GSL_CONTINUE) {
      MATH_INFO_MSGVAL("GSLMultiRootFinder::Solve","exceeded max iterations, reached tolerance is not sufficient",absTol);
   }
   if (status == GSL_SUCCESS) {
      if (fPrintLevel>=1) {          // print the result
         MATH_INFO_MSG("GSLMultiRootFinder::Solve","The iteration converged");
         std::cout << "GSL Algorithm used is :  " << fSolver->Name() << std::endl;
         std::cout << "Number of iterations  =  " << iter<< std::endl;

         PrintState();
      }
   }
   fIter = iter;
   fStatus = status;
   return  (fStatus == GSL_SUCCESS);

}

void GSLMultiRootFinder::PrintState(std::ostream & os) {
   // print current state
   if (!fSolver) return;
   double ndigits = std::log10( double( Dim() ) );
   int wi = int(ndigits)+1;
   const double * xtmp = fSolver->X();
   const double * ftmp = fSolver->FVal();
   os << "Root values     = ";
   for (unsigned int i = 0; i< Dim(); ++i)
      os << "x[" << std::setw(wi) << i << "] = " << std::setw(12) << xtmp[i] << "   ";
   os << std::endl;
   os << "Function values = ";
   for (unsigned int i = 0; i< Dim(); ++i)
      os << "f[" << std::setw(wi) << i << "] = " << std::setw(12) << ftmp[i] << "   ";
   os << std::endl;
}

} // namespace Math
} // namespace ROOT
<|endoftext|>


<commit_before>/*M///////////////////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2015, Youssef Kashef
// Copyright (c) 2015, ELM Library Project
// 3-clause BSD License
//
//M*/
#include "elm/layers/medianblur.h"

#include <opencv2/imgproc/imgproc.hpp>

#include "elm/core/exception.h"
#include "elm/core/layerconfig.h"
#include "elm/core/signal.h"
#include "elm/ts/layerattr_.h"

using namespace cv;
using namespace elm;

/** Define parameters, defaults and I/O keys
  */
// paramters
const std::string MedianBlur::PARAM_APERTURE_SIZE = "aperture_size";

/** @todo why does define guard lead to undefined reference error?
 */
//#ifdef __WITH_GTEST
#include <boost/assign/list_of.hpp>
template <>
elm::MapIONames LayerAttr_<MedianBlur>::io_pairs = boost::assign::map_list_of
        ELM_ADD_INPUT_PAIR(detail::BASE_SINGLE_INPUT_FEATURE_LAYER__KEY_INPUT_STIMULUS)
        ELM_ADD_OUTPUT_PAIR(detail::BASE_MATOUTPUT_LAYER__KEY_OUTPUT_RESPONSE)
        ;
//#endif

MedianBlur::MedianBlur()
    : base_FeatureTransformationLayer()
{
    Clear();
}

MedianBlur::MedianBlur(const LayerConfig config)
    : base_FeatureTransformationLayer(config)
{
    Reset(config);
}

void MedianBlur::Clear()
{
    m_ = Mat1f();
}

void MedianBlur::Reset(const LayerConfig &config)
{
    Clear();
    Reconfigure(config);
}

void MedianBlur::Reconfigure(const LayerConfig &config)
{
    PTree p = config.Params();
    ksize_ = p.get<int>(PARAM_APERTURE_SIZE);

    if(ksize_ <= 1 || ksize_ % 2 == 0) {

        std::stringstream s;
        s << "Aperture size for median filer must be > 1 and odd." <<
             " Got " << ksize_;
        ELM_THROW_VALUE_ERROR(s.str());
    }
}

void MedianBlur::Activate(const Signal &signal)
{
    cv::medianBlur(signal.MostRecentMat1f(name_input_), m_, ksize_);
}
<commit_msg>overloaded constructor calls IONames<commit_after>/*M///////////////////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2015, Youssef Kashef
// Copyright (c) 2015, ELM Library Project
// 3-clause BSD License
//
//M*/
#include "elm/layers/medianblur.h"

#include <opencv2/imgproc/imgproc.hpp>

#include "elm/core/exception.h"
#include "elm/core/layerconfig.h"
#include "elm/core/signal.h"
#include "elm/ts/layerattr_.h"

using namespace cv;
using namespace elm;

/** Define parameters, defaults and I/O keys
  */
// paramters
const std::string MedianBlur::PARAM_APERTURE_SIZE = "aperture_size";

/** @todo why does define guard lead to undefined reference error?
 */
//#ifdef __WITH_GTEST
#include <boost/assign/list_of.hpp>
template <>
elm::MapIONames LayerAttr_<MedianBlur>::io_pairs = boost::assign::map_list_of
        ELM_ADD_INPUT_PAIR(detail::BASE_SINGLE_INPUT_FEATURE_LAYER__KEY_INPUT_STIMULUS)
        ELM_ADD_OUTPUT_PAIR(detail::BASE_MATOUTPUT_LAYER__KEY_OUTPUT_RESPONSE)
        ;
//#endif

MedianBlur::MedianBlur()
    : base_FeatureTransformationLayer()
{
    Clear();
}

MedianBlur::MedianBlur(const LayerConfig config)
    : base_FeatureTransformationLayer(config)
{
    Reset(config);
    IONames(config);
}

void MedianBlur::Clear()
{
    m_ = Mat1f();
}

void MedianBlur::Reset(const LayerConfig &config)
{
    Clear();
    Reconfigure(config);
}

void MedianBlur::Reconfigure(const LayerConfig &config)
{
    PTree p = config.Params();
    ksize_ = p.get<int>(PARAM_APERTURE_SIZE);

    if(ksize_ <= 1 || ksize_ % 2 == 0) {

        std::stringstream s;
        s << "Aperture size for median filer must be > 1 and odd." <<
             " Got " << ksize_;
        ELM_THROW_VALUE_ERROR(s.str());
    }
}

void MedianBlur::Activate(const Signal &signal)
{
    cv::medianBlur(signal.MostRecentMat1f(name_input_), m_, ksize_);
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2010 by Cristian Maglie <c.maglie@arduino.cc>
 * Copyright (c) 2014 by Paul Stoffregen <paul@pjrc.com> (Transaction API)
 * Copyright (c) 2014 by Matthijs Kooijman <matthijs@stdin.nl> (SPISettings AVR)
 * Copyright (c) 2014 by Andrew J. Kroll <xxxajk@gmail.com> (atomicity fixes)
 * SPI Master library for arduino.
 *
 * This file is free software; you can redistribute it and/or modify
 * it under the terms of either the GNU General Public License version 2
 * or the GNU Lesser General Public License version 2.1, both as
 * published by the Free Software Foundation.
 */

#include "SPI.h"

SPIClass SPI;

uint8_t SPIClass::initialized = 0;
uint8_t SPIClass::interruptMode = 0;
uint8_t SPIClass::interruptMask = 0;
uint8_t SPIClass::interruptSave = 0;
#ifdef SPI_TRANSACTION_MISMATCH_LED
uint8_t SPIClass::inTransactionFlag = 0;
#endif

void SPIClass::begin()
{
  uint8_t sreg = SREG;
  noInterrupts(); // Protect from a scheduler and prevent transactionBegin
  if (!initialized) {
    // Set SS to high so a connected chip will be "deselected" by default
    digitalWrite(SS, HIGH);

    // When the SS pin is set as OUTPUT, it can be used as
    // a general purpose output port (it doesn't influence
    // SPI operations).
    pinMode(SS, OUTPUT);

    // Warning: if the SS pin ever becomes a LOW INPUT then SPI
    // automatically switches to Slave, so the data direction of
    // the SS pin MUST be kept as OUTPUT.
    SPCR |= _BV(MSTR);
    SPCR |= _BV(SPE);

    // Set direction register for SCK and MOSI pin.
    // MISO pin automatically overrides to INPUT.
    // By doing this AFTER enabling SPI, we avoid accidentally
    // clocking in a single bit since the lines go directly
    // from "input" to SPI control.
    // http://code.google.com/p/arduino/issues/detail?id=888
    pinMode(SCK, OUTPUT);
    pinMode(MOSI, OUTPUT);
  }
  initialized++; // reference count
  SREG = sreg;
}

void SPIClass::end() {
  uint8_t sreg = SREG;
  noInterrupts(); // Protect from a scheduler and prevent transactionBegin
  // Decrease the reference counter
  if (initialized)
    initialized--;
  // If there are no more references disable SPI
  if (!initialized) {
    SPCR &= ~_BV(SPE);
    interruptMode = 0;
    #ifdef SPI_TRANSACTION_MISMATCH_LED
    inTransactionFlag = 0;
    #endif
  }
  SREG = sreg;
}

// mapping of interrupt numbers to bits within SPI_AVR_EIMSK
#if defined(__AVR_ATmega32U4__)
  #define SPI_INT0_MASK  (1<<INT0)
  #define SPI_INT1_MASK  (1<<INT1)
  #define SPI_INT2_MASK  (1<<INT2)
  #define SPI_INT3_MASK  (1<<INT3)
  #define SPI_INT4_MASK  (1<<INT6)
#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
  #define SPI_INT0_MASK  (1<<INT0)
  #define SPI_INT1_MASK  (1<<INT1)
  #define SPI_INT2_MASK  (1<<INT2)
  #define SPI_INT3_MASK  (1<<INT3)
  #define SPI_INT4_MASK  (1<<INT4)
  #define SPI_INT5_MASK  (1<<INT5)
  #define SPI_INT6_MASK  (1<<INT6)
  #define SPI_INT7_MASK  (1<<INT7)
#elif defined(EICRA) && defined(EICRB) && defined(EIMSK)
  #define SPI_INT0_MASK  (1<<INT4)
  #define SPI_INT1_MASK  (1<<INT5)
  #define SPI_INT2_MASK  (1<<INT0)
  #define SPI_INT3_MASK  (1<<INT1)
  #define SPI_INT4_MASK  (1<<INT2)
  #define SPI_INT5_MASK  (1<<INT3)
  #define SPI_INT6_MASK  (1<<INT6)
  #define SPI_INT7_MASK  (1<<INT7)
#else
  #ifdef INT0
  #define SPI_INT0_MASK  (1<<INT0)
  #endif
  #ifdef INT1
  #define SPI_INT1_MASK  (1<<INT1)
  #endif
  #ifdef INT2
  #define SPI_INT2_MASK  (1<<INT2)
  #endif
#endif

void SPIClass::usingInterrupt(uint8_t interruptNumber)
{
  uint8_t mask = 0;
  uint8_t sreg = SREG;
  noInterrupts(); // Protect from a scheduler and prevent transactionBegin
  switch (interruptNumber) {
  #ifdef SPI_INT0_MASK
  case 0: mask = SPI_INT0_MASK; break;
  #endif
  #ifdef SPI_INT1_MASK
  case 1: mask = SPI_INT1_MASK; break;
  #endif
  #ifdef SPI_INT2_MASK
  case 2: mask = SPI_INT2_MASK; break;
  #endif
  #ifdef SPI_INT3_MASK
  case 3: mask = SPI_INT3_MASK; break;
  #endif
  #ifdef SPI_INT4_MASK
  case 4: mask = SPI_INT4_MASK; break;
  #endif
  #ifdef SPI_INT5_MASK
  case 5: mask = SPI_INT5_MASK; break;
  #endif
  #ifdef SPI_INT6_MASK
  case 6: mask = SPI_INT6_MASK; break;
  #endif
  #ifdef SPI_INT7_MASK
  case 7: mask = SPI_INT7_MASK; break;
  #endif
  default:
    interruptMode = 2;
    break;
  }
  interruptMask |= mask;
  if (!interruptMode)
    interruptMode = 1;
  SREG = sreg;
}

void SPIClass::notUsingInterrupt(uint8_t interruptNumber)
{
  // Once in mode 2 we can't go back to 0 without a proper reference count
  if (interruptMode == 2)
    return;
  uint8_t mask = 0;
  uint8_t sreg = SREG;
  noInterrupts(); // Protect from a scheduler and prevent transactionBegin
  switch (interruptNumber) {
  #ifdef SPI_INT0_MASK
  case 0: mask = SPI_INT0_MASK; break;
  #endif
  #ifdef SPI_INT1_MASK
  case 1: mask = SPI_INT1_MASK; break;
  #endif
  #ifdef SPI_INT2_MASK
  case 2: mask = SPI_INT2_MASK; break;
  #endif
  #ifdef SPI_INT3_MASK
  case 3: mask = SPI_INT3_MASK; break;
  #endif
  #ifdef SPI_INT4_MASK
  case 4: mask = SPI_INT4_MASK; break;
  #endif
  #ifdef SPI_INT5_MASK
  case 5: mask = SPI_INT5_MASK; break;
  #endif
  #ifdef SPI_INT6_MASK
  case 6: mask = SPI_INT6_MASK; break;
  #endif
  #ifdef SPI_INT7_MASK
  case 7: mask = SPI_INT7_MASK; break;
  #endif
  default:
    break;
    // this case can't be reached
  }
  interruptMask &= ~mask;
  if (!interruptMask)
    interruptMode = 0;
  SREG = sreg;
}
<commit_msg>Do not influence state of SS if it's already been set to an output previously, e.g. by user sketch<commit_after>/*
 * Copyright (c) 2010 by Cristian Maglie <c.maglie@arduino.cc>
 * Copyright (c) 2014 by Paul Stoffregen <paul@pjrc.com> (Transaction API)
 * Copyright (c) 2014 by Matthijs Kooijman <matthijs@stdin.nl> (SPISettings AVR)
 * Copyright (c) 2014 by Andrew J. Kroll <xxxajk@gmail.com> (atomicity fixes)
 * SPI Master library for arduino.
 *
 * This file is free software; you can redistribute it and/or modify
 * it under the terms of either the GNU General Public License version 2
 * or the GNU Lesser General Public License version 2.1, both as
 * published by the Free Software Foundation.
 */

#include "SPI.h"

SPIClass SPI;

uint8_t SPIClass::initialized = 0;
uint8_t SPIClass::interruptMode = 0;
uint8_t SPIClass::interruptMask = 0;
uint8_t SPIClass::interruptSave = 0;
#ifdef SPI_TRANSACTION_MISMATCH_LED
uint8_t SPIClass::inTransactionFlag = 0;
#endif

void SPIClass::begin()
{
  uint8_t sreg = SREG;
  noInterrupts(); // Protect from a scheduler and prevent transactionBegin
  if (!initialized) {
    // Set SS to high so a connected chip will be "deselected" by default
    uint8_t port = digitalPinToPort(SS);
    uint8_t bit = digitalPinToBitMask(SS);
    volatile uint8_t *reg = portModeRegister(port);

    // if the SS pin is not already configured as an output
    // then set it high (to enable the internal pull-up resistor)
    if(!(*reg & bit)){
      digitalWrite(SS, HIGH);
    }

    // When the SS pin is set as OUTPUT, it can be used as
    // a general purpose output port (it doesn't influence
    // SPI operations).
    pinMode(SS, OUTPUT);

    // Warning: if the SS pin ever becomes a LOW INPUT then SPI
    // automatically switches to Slave, so the data direction of
    // the SS pin MUST be kept as OUTPUT.
    SPCR |= _BV(MSTR);
    SPCR |= _BV(SPE);

    // Set direction register for SCK and MOSI pin.
    // MISO pin automatically overrides to INPUT.
    // By doing this AFTER enabling SPI, we avoid accidentally
    // clocking in a single bit since the lines go directly
    // from "input" to SPI control.
    // http://code.google.com/p/arduino/issues/detail?id=888
    pinMode(SCK, OUTPUT);
    pinMode(MOSI, OUTPUT);
  }
  initialized++; // reference count
  SREG = sreg;
}

void SPIClass::end() {
  uint8_t sreg = SREG;
  noInterrupts(); // Protect from a scheduler and prevent transactionBegin
  // Decrease the reference counter
  if (initialized)
    initialized--;
  // If there are no more references disable SPI
  if (!initialized) {
    SPCR &= ~_BV(SPE);
    interruptMode = 0;
    #ifdef SPI_TRANSACTION_MISMATCH_LED
    inTransactionFlag = 0;
    #endif
  }
  SREG = sreg;
}

// mapping of interrupt numbers to bits within SPI_AVR_EIMSK
#if defined(__AVR_ATmega32U4__)
  #define SPI_INT0_MASK  (1<<INT0)
  #define SPI_INT1_MASK  (1<<INT1)
  #define SPI_INT2_MASK  (1<<INT2)
  #define SPI_INT3_MASK  (1<<INT3)
  #define SPI_INT4_MASK  (1<<INT6)
#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
  #define SPI_INT0_MASK  (1<<INT0)
  #define SPI_INT1_MASK  (1<<INT1)
  #define SPI_INT2_MASK  (1<<INT2)
  #define SPI_INT3_MASK  (1<<INT3)
  #define SPI_INT4_MASK  (1<<INT4)
  #define SPI_INT5_MASK  (1<<INT5)
  #define SPI_INT6_MASK  (1<<INT6)
  #define SPI_INT7_MASK  (1<<INT7)
#elif defined(EICRA) && defined(EICRB) && defined(EIMSK)
  #define SPI_INT0_MASK  (1<<INT4)
  #define SPI_INT1_MASK  (1<<INT5)
  #define SPI_INT2_MASK  (1<<INT0)
  #define SPI_INT3_MASK  (1<<INT1)
  #define SPI_INT4_MASK  (1<<INT2)
  #define SPI_INT5_MASK  (1<<INT3)
  #define SPI_INT6_MASK  (1<<INT6)
  #define SPI_INT7_MASK  (1<<INT7)
#else
  #ifdef INT0
  #define SPI_INT0_MASK  (1<<INT0)
  #endif
  #ifdef INT1
  #define SPI_INT1_MASK  (1<<INT1)
  #endif
  #ifdef INT2
  #define SPI_INT2_MASK  (1<<INT2)
  #endif
#endif

void SPIClass::usingInterrupt(uint8_t interruptNumber)
{
  uint8_t mask = 0;
  uint8_t sreg = SREG;
  noInterrupts(); // Protect from a scheduler and prevent transactionBegin
  switch (interruptNumber) {
  #ifdef SPI_INT0_MASK
  case 0: mask = SPI_INT0_MASK; break;
  #endif
  #ifdef SPI_INT1_MASK
  case 1: mask = SPI_INT1_MASK; break;
  #endif
  #ifdef SPI_INT2_MASK
  case 2: mask = SPI_INT2_MASK; break;
  #endif
  #ifdef SPI_INT3_MASK
  case 3: mask = SPI_INT3_MASK; break;
  #endif
  #ifdef SPI_INT4_MASK
  case 4: mask = SPI_INT4_MASK; break;
  #endif
  #ifdef SPI_INT5_MASK
  case 5: mask = SPI_INT5_MASK; break;
  #endif
  #ifdef SPI_INT6_MASK
  case 6: mask = SPI_INT6_MASK; break;
  #endif
  #ifdef SPI_INT7_MASK
  case 7: mask = SPI_INT7_MASK; break;
  #endif
  default:
    interruptMode = 2;
    break;
  }
  interruptMask |= mask;
  if (!interruptMode)
    interruptMode = 1;
  SREG = sreg;
}

void SPIClass::notUsingInterrupt(uint8_t interruptNumber)
{
  // Once in mode 2 we can't go back to 0 without a proper reference count
  if (interruptMode == 2)
    return;
  uint8_t mask = 0;
  uint8_t sreg = SREG;
  noInterrupts(); // Protect from a scheduler and prevent transactionBegin
  switch (interruptNumber) {
  #ifdef SPI_INT0_MASK
  case 0: mask = SPI_INT0_MASK; break;
  #endif
  #ifdef SPI_INT1_MASK
  case 1: mask = SPI_INT1_MASK; break;
  #endif
  #ifdef SPI_INT2_MASK
  case 2: mask = SPI_INT2_MASK; break;
  #endif
  #ifdef SPI_INT3_MASK
  case 3: mask = SPI_INT3_MASK; break;
  #endif
  #ifdef SPI_INT4_MASK
  case 4: mask = SPI_INT4_MASK; break;
  #endif
  #ifdef SPI_INT5_MASK
  case 5: mask = SPI_INT5_MASK; break;
  #endif
  #ifdef SPI_INT6_MASK
  case 6: mask = SPI_INT6_MASK; break;
  #endif
  #ifdef SPI_INT7_MASK
  case 7: mask = SPI_INT7_MASK; break;
  #endif
  default:
    break;
    // this case can't be reached
  }
  interruptMask &= ~mask;
  if (!interruptMask)
    interruptMode = 0;
  SREG = sreg;
}
<|endoftext|>