<commit_before>#ifndef OCLUTIL_REDUCE_HPP
#define OCLUTIL_REDUCE_HPP

/**
 * \file   reduce.hpp
 * \author Denis Demidov <ddemidov@ksu.ru>
 * \brief  OpenCL vector reduction.
 */

#ifdef WIN32
#  pragma warning(disable : 4290 4715)
#  define NOMINMAX
#endif

#include <vector>
#include <sstream>
#include <numeric>
#include <limits>
#include <CL/cl.hpp>
#include <oclutil/vector.hpp>

namespace clu {

enum ReductionKind {
    SUM = 0,
    MAX = 1,
    MIN = 2
};

/// Parallel reduction of arbitrary expression.
template <typename real, ReductionKind RDC>
class Reductor {
    public:
	Reductor(const std::vector<cl::CommandQueue> &queue);

	template <class Expr>
	real operator()(const Expr &expr) const;
    private:
	cl::Context context;
	std::vector<cl::CommandQueue> queue;
	std::vector<uint> idx;
	mutable std::vector<real> hbuf;
	std::vector<clu::vector<real>> dbuf;

	template <class Expr>
	struct exdata {
	    static bool       compiled[3];
	    static cl::Kernel kernel[3];
	    static uint       wgsize[3];
	};
};

template <typename real, ReductionKind RDC> template <class Expr>
bool Reductor<real,RDC>::exdata<Expr>::compiled[3] = {false, false, false};

template <typename real, ReductionKind RDC> template <class Expr>
cl::Kernel Reductor<real,RDC>::exdata<Expr>::kernel[3];

template <typename real, ReductionKind RDC> template <class Expr>
uint Reductor<real,RDC>::exdata<Expr>::wgsize[3];

template <typename real, ReductionKind RDC>
Reductor<real,RDC>::Reductor(const std::vector<cl::CommandQueue> &queue)
    : context(queue[0].getInfo<CL_QUEUE_CONTEXT>()), queue(queue)
{
    idx.reserve(queue.size() + 1);
    idx.push_back(0);

    for(auto q = queue.begin(); q != queue.end(); q++) {
	cl::Device d = q->getInfo<CL_QUEUE_DEVICE>();
	uint bufsize = d.getInfo<CL_DEVICE_MAX_COMPUTE_UNITS>() * 2U;
	idx.push_back(idx.back() + bufsize);

	std::vector<cl::CommandQueue> lq(1, *q);
	dbuf.push_back(clu::vector<real>(lq, CL_MEM_READ_WRITE, bufsize));
    }

    hbuf.resize(idx.back());
}

template <typename real, ReductionKind RDC> template <class Expr>
real Reductor<real,RDC>::operator()(const Expr &expr) const {
    if (!exdata<Expr>::compiled[RDC]) {
	std::ostringstream source;

	std::string kernel_name = std::string("reduce_") + expr.kernel_name();

	std::ostringstream increment_line;
	switch (RDC) {
	    case SUM:
		increment_line << "mySum += ";
		expr.kernel_expr(increment_line);
		increment_line << ";\n";
		break;
	    case MAX:
		increment_line << "mySum = max(mySum, ";
		expr.kernel_expr(increment_line);
		increment_line << ");\n";
		break;
	    case MIN:
		increment_line << "mySum = min(mySum, ";
		expr.kernel_expr(increment_line);
		increment_line << ");\n";
		break;
	}

	source << "#if defined(cl_khr_fp64)\n"
		  "#  pragma OPENCL EXTENSION cl_khr_fp64: enable\n"
		  "#elif defined(cl_amd_fp64)\n"
		  "#  pragma OPENCL EXTENSION cl_amd_fp64: enable\n"
		  "#endif\n"
	       << "typedef " << type_name<real>() << " real;\n"
	       << "kernel void " << kernel_name << "(uint n";

	expr.kernel_prm(source);

	source << ",\n\tglobal real *g_odata,\n"
		  "\tlocal  real *sdata\n"
		  "\t)\n"
		  "{\n"
		  "    uint tid        = get_local_id(0);\n"
		  "    uint block_size = get_local_size(0);\n"
		  "    uint p          = get_group_id(0) * block_size * 2 + tid;\n"
		  "    uint gridSize   = get_num_groups(0) * block_size * 2;\n"
		  "    uint i;\n"
		  "\n"
		  "    real mySum = ";
	switch(RDC) {
	    case SUM:
		source << 0;
		break;
	    case MAX:
		source << -std::numeric_limits<real>::max();
		break;
	    case MIN:
		source << std::numeric_limits<real>::max();
		break;
	}
	source << ";\n"
		  "\n"
		  "    while (p < n) {\n"
		  "        i = p;\n"
		  "        " << increment_line.str() <<
		  "        i = p + block_size;\n"
		  "        if (i < n)\n"
		  "            " << increment_line.str() <<
		  "        p += gridSize;\n"
		  "    }\n"
		  "\n"
		  "    sdata[tid] = mySum;\n"
		  "    barrier(CLK_LOCAL_MEM_FENCE);\n"
		  "\n";
	switch (RDC) {
	    case SUM:
		source <<
		  "    if (block_size >= 512) { if (tid < 256) { sdata[tid] = mySum = mySum + sdata[tid + 256]; } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "    if (block_size >= 256) { if (tid < 128) { sdata[tid] = mySum = mySum + sdata[tid + 128]; } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "    if (block_size >= 128) { if (tid <  64) { sdata[tid] = mySum = mySum + sdata[tid +  64]; } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "\n"
		  "    if (tid < 32) {\n"
		  "        local volatile real* smem = sdata;\n"
		  "        if (block_size >=  64) { smem[tid] = mySum = mySum + smem[tid + 32]; }\n"
		  "        if (block_size >=  32) { smem[tid] = mySum = mySum + smem[tid + 16]; }\n"
		  "        if (block_size >=  16) { smem[tid] = mySum = mySum + smem[tid +  8]; }\n"
		  "        if (block_size >=   8) { smem[tid] = mySum = mySum + smem[tid +  4]; }\n"
		  "        if (block_size >=   4) { smem[tid] = mySum = mySum + smem[tid +  2]; }\n"
		  "        if (block_size >=   2) { smem[tid] = mySum = mySum + smem[tid +  1]; }\n"
		  "    }\n"
		  "\n"
		  "    if (tid == 0) g_odata[get_group_id(0)] = sdata[0];\n"
		  "}\n";
		break;
	    case MAX:
		source <<
		  "    if (block_size >= 512) { if (tid < 256) { sdata[tid] = mySum = max(mySum, sdata[tid + 256]); } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "    if (block_size >= 256) { if (tid < 128) { sdata[tid] = mySum = max(mySum, sdata[tid + 128]); } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "    if (block_size >= 128) { if (tid <  64) { sdata[tid] = mySum = max(mySum, sdata[tid +  64]); } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "\n"
		  "    if (tid < 32) {\n"
		  "        local volatile real* smem = sdata;\n"
		  "        if (block_size >=  64) { smem[tid] = mySum = max(mySum, smem[tid + 32]); }\n"
		  "        if (block_size >=  32) { smem[tid] = mySum = max(mySum, smem[tid + 16]); }\n"
		  "        if (block_size >=  16) { smem[tid] = mySum = max(mySum, smem[tid +  8]); }\n"
		  "        if (block_size >=   8) { smem[tid] = mySum = max(mySum, smem[tid +  4]); }\n"
		  "        if (block_size >=   4) { smem[tid] = mySum = max(mySum, smem[tid +  2]); }\n"
		  "        if (block_size >=   2) { smem[tid] = mySum = max(mySum, smem[tid +  1]); }\n"
		  "    }\n"
		  "\n"
		  "    if (tid == 0) g_odata[get_group_id(0)] = sdata[0];\n"
		  "}\n";
		break;
	    case MIN:
		source <<
		  "    if (block_size >= 512) { if (tid < 256) { sdata[tid] = mySum = min(mySum, sdata[tid + 256]); } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "    if (block_size >= 256) { if (tid < 128) { sdata[tid] = mySum = min(mySum, sdata[tid + 128]); } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "    if (block_size >= 128) { if (tid <  64) { sdata[tid] = mySum = min(mySum, sdata[tid +  64]); } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "\n"
		  "    if (tid < 32) {\n"
		  "        local volatile real* smem = sdata;\n"
		  "        if (block_size >=  64) { smem[tid] = mySum = min(mySum, smem[tid + 32]); }\n"
		  "        if (block_size >=  32) { smem[tid] = mySum = min(mySum, smem[tid + 16]); }\n"
		  "        if (block_size >=  16) { smem[tid] = mySum = min(mySum, smem[tid +  8]); }\n"
		  "        if (block_size >=   8) { smem[tid] = mySum = min(mySum, smem[tid +  4]); }\n"
		  "        if (block_size >=   4) { smem[tid] = mySum = min(mySum, smem[tid +  2]); }\n"
		  "        if (block_size >=   2) { smem[tid] = mySum = min(mySum, smem[tid +  1]); }\n"
		  "    }\n"
		  "\n"
		  "    if (tid == 0) g_odata[get_group_id(0)] = sdata[0];\n"
		  "}\n";
		break;
	}

	std::vector<cl::Device> device;
	device.reserve(queue.size());

	for(auto q = queue.begin(); q != queue.end(); q++)
	    device.push_back(q->getInfo<CL_QUEUE_DEVICE>());

	auto program = build_sources(context, source.str());

	exdata<Expr>::kernel[RDC]   = cl::Kernel(program, kernel_name.c_str());
	exdata<Expr>::compiled[RDC] = true;
	exdata<Expr>::wgsize[RDC]   = kernel_workgroup_size(
		exdata<Expr>::kernel[RDC], device);
    }

    for(uint d = 0; d < queue.size(); d++) {
	uint psize = expr.part_size(d);
	uint g_size = (idx[d + 1] - idx[d]) * exdata<Expr>::wgsize[RDC];
	auto lmem = cl::__local(exdata<Expr>::wgsize[RDC] * sizeof(real));

	uint pos = 0;
	exdata<Expr>::kernel[RDC].setArg(pos++, psize);
	expr.kernel_args(exdata<Expr>::kernel[RDC], d, pos);
	exdata<Expr>::kernel[RDC].setArg(pos++, dbuf[d]());
	exdata<Expr>::kernel[RDC].setArg(pos++, lmem);

	queue[d].enqueueNDRangeKernel(exdata<Expr>::kernel[RDC], cl::NullRange,
		g_size, exdata<Expr>::wgsize[RDC]);
    }

    for(uint d = 0; d < queue.size(); d++) {
	copy(dbuf[d], &hbuf[idx[d]]);
    }

    switch(RDC) {
	case SUM:
	    return std::accumulate(
		    hbuf.begin(), hbuf.end(), static_cast<real>(0));
	case MAX:
	    return *std::max_element(hbuf.begin(), hbuf.end());
	case MIN:
	    return *std::min_element(hbuf.begin(), hbuf.end());
    }
}

/// Sum of vector elements.
template <typename real>
real sum(const clu::vector<real> &x) {
    static Reductor<real,SUM> rdc(x.queue);

    return rdc(x);
}

/// Inner product of two vectors.
template <typename real>
real inner_product(const clu::vector<real> &x, const clu::vector<real> &y) {
    static Reductor<real,SUM> rdc(x.queue);

    return rdc(x * y);
}

} // namespace clu

#endif
<commit_msg>Bug fix in reduction for workgroup size >= 1024<commit_after>#ifndef OCLUTIL_REDUCE_HPP
#define OCLUTIL_REDUCE_HPP

/**
 * \file   reduce.hpp
 * \author Denis Demidov <ddemidov@ksu.ru>
 * \brief  OpenCL vector reduction.
 */

#ifdef WIN32
#  pragma warning(disable : 4290 4715)
#  define NOMINMAX
#endif

#include <vector>
#include <sstream>
#include <numeric>
#include <limits>
#include <CL/cl.hpp>
#include <oclutil/vector.hpp>

namespace clu {

enum ReductionKind {
    SUM = 0,
    MAX = 1,
    MIN = 2
};

/// Parallel reduction of arbitrary expression.
template <typename real, ReductionKind RDC>
class Reductor {
    public:
	Reductor(const std::vector<cl::CommandQueue> &queue);

	template <class Expr>
	real operator()(const Expr &expr) const;
    private:
	cl::Context context;
	std::vector<cl::CommandQueue> queue;
	std::vector<uint> idx;
	mutable std::vector<real> hbuf;
	std::vector<clu::vector<real>> dbuf;

	template <class Expr>
	struct exdata {
	    static bool       compiled[3];
	    static cl::Kernel kernel[3];
	    static uint       wgsize[3];
	};
};

template <typename real, ReductionKind RDC> template <class Expr>
bool Reductor<real,RDC>::exdata<Expr>::compiled[3] = {false, false, false};

template <typename real, ReductionKind RDC> template <class Expr>
cl::Kernel Reductor<real,RDC>::exdata<Expr>::kernel[3];

template <typename real, ReductionKind RDC> template <class Expr>
uint Reductor<real,RDC>::exdata<Expr>::wgsize[3];

template <typename real, ReductionKind RDC>
Reductor<real,RDC>::Reductor(const std::vector<cl::CommandQueue> &queue)
    : context(queue[0].getInfo<CL_QUEUE_CONTEXT>()), queue(queue)
{
    idx.reserve(queue.size() + 1);
    idx.push_back(0);

    for(auto q = queue.begin(); q != queue.end(); q++) {
	cl::Device d = q->getInfo<CL_QUEUE_DEVICE>();
	uint bufsize = d.getInfo<CL_DEVICE_MAX_COMPUTE_UNITS>() * 2U;
	idx.push_back(idx.back() + bufsize);

	std::vector<cl::CommandQueue> lq(1, *q);
	dbuf.push_back(clu::vector<real>(lq, CL_MEM_READ_WRITE, bufsize));
    }

    hbuf.resize(idx.back());
}

template <typename real, ReductionKind RDC> template <class Expr>
real Reductor<real,RDC>::operator()(const Expr &expr) const {
    if (!exdata<Expr>::compiled[RDC]) {
	std::ostringstream source;

	std::string kernel_name = std::string("reduce_") + expr.kernel_name();

	std::ostringstream increment_line;
	switch (RDC) {
	    case SUM:
		increment_line << "mySum += ";
		expr.kernel_expr(increment_line);
		increment_line << ";\n";
		break;
	    case MAX:
		increment_line << "mySum = max(mySum, ";
		expr.kernel_expr(increment_line);
		increment_line << ");\n";
		break;
	    case MIN:
		increment_line << "mySum = min(mySum, ";
		expr.kernel_expr(increment_line);
		increment_line << ");\n";
		break;
	}

	source << "#if defined(cl_khr_fp64)\n"
		  "#  pragma OPENCL EXTENSION cl_khr_fp64: enable\n"
		  "#elif defined(cl_amd_fp64)\n"
		  "#  pragma OPENCL EXTENSION cl_amd_fp64: enable\n"
		  "#endif\n"
	       << "typedef " << type_name<real>() << " real;\n"
	       << "kernel void " << kernel_name << "(uint n";

	expr.kernel_prm(source);

	source << ",\n\tglobal real *g_odata,\n"
		  "\tlocal  real *sdata\n"
		  "\t)\n"
		  "{\n"
		  "    uint tid        = get_local_id(0);\n"
		  "    uint block_size = get_local_size(0);\n"
		  "    uint p          = get_group_id(0) * block_size * 2 + tid;\n"
		  "    uint gridSize   = get_num_groups(0) * block_size * 2;\n"
		  "    uint i;\n"
		  "\n"
		  "    real mySum = ";
	switch(RDC) {
	    case SUM:
		source << 0;
		break;
	    case MAX:
		source << -std::numeric_limits<real>::max();
		break;
	    case MIN:
		source << std::numeric_limits<real>::max();
		break;
	}
	source << ";\n"
		  "\n"
		  "    while (p < n) {\n"
		  "        i = p;\n"
		  "        " << increment_line.str() <<
		  "        i = p + block_size;\n"
		  "        if (i < n)\n"
		  "            " << increment_line.str() <<
		  "        p += gridSize;\n"
		  "    }\n"
		  "\n"
		  "    sdata[tid] = mySum;\n"
		  "    barrier(CLK_LOCAL_MEM_FENCE);\n"
		  "\n";
	switch (RDC) {
	    case SUM:
		source <<
		  "    if (block_size >= 1024) { if (tid < 512) { sdata[tid] = mySum = mySum + sdata[tid + 512]; } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "    if (block_size >=  512) { if (tid < 256) { sdata[tid] = mySum = mySum + sdata[tid + 256]; } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "    if (block_size >=  256) { if (tid < 128) { sdata[tid] = mySum = mySum + sdata[tid + 128]; } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "    if (block_size >=  128) { if (tid <  64) { sdata[tid] = mySum = mySum + sdata[tid +  64]; } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "\n"
		  "    if (tid < 32) {\n"
		  "        local volatile real* smem = sdata;\n"
		  "        if (block_size >=  64) { smem[tid] = mySum = mySum + smem[tid + 32]; }\n"
		  "        if (block_size >=  32) { smem[tid] = mySum = mySum + smem[tid + 16]; }\n"
		  "        if (block_size >=  16) { smem[tid] = mySum = mySum + smem[tid +  8]; }\n"
		  "        if (block_size >=   8) { smem[tid] = mySum = mySum + smem[tid +  4]; }\n"
		  "        if (block_size >=   4) { smem[tid] = mySum = mySum + smem[tid +  2]; }\n"
		  "        if (block_size >=   2) { smem[tid] = mySum = mySum + smem[tid +  1]; }\n"
		  "    }\n"
		  "\n"
		  "    if (tid == 0) g_odata[get_group_id(0)] = sdata[0];\n"
		  "}\n";
		break;
	    case MAX:
		source <<
		  "    if (block_size >= 1024) { if (tid < 512) { sdata[tid] = mySum = max(mySum, sdata[tid + 512]); } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "    if (block_size >=  512) { if (tid < 256) { sdata[tid] = mySum = max(mySum, sdata[tid + 256]); } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "    if (block_size >=  256) { if (tid < 128) { sdata[tid] = mySum = max(mySum, sdata[tid + 128]); } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "    if (block_size >=  128) { if (tid <  64) { sdata[tid] = mySum = max(mySum, sdata[tid +  64]); } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "\n"
		  "    if (tid < 32) {\n"
		  "        local volatile real* smem = sdata;\n"
		  "        if (block_size >=  64) { smem[tid] = mySum = max(mySum, smem[tid + 32]); }\n"
		  "        if (block_size >=  32) { smem[tid] = mySum = max(mySum, smem[tid + 16]); }\n"
		  "        if (block_size >=  16) { smem[tid] = mySum = max(mySum, smem[tid +  8]); }\n"
		  "        if (block_size >=   8) { smem[tid] = mySum = max(mySum, smem[tid +  4]); }\n"
		  "        if (block_size >=   4) { smem[tid] = mySum = max(mySum, smem[tid +  2]); }\n"
		  "        if (block_size >=   2) { smem[tid] = mySum = max(mySum, smem[tid +  1]); }\n"
		  "    }\n"
		  "\n"
		  "    if (tid == 0) g_odata[get_group_id(0)] = sdata[0];\n"
		  "}\n";
		break;
	    case MIN:
		source <<
		  "    if (block_size >= 1024) { if (tid < 512) { sdata[tid] = mySum = min(mySum, sdata[tid + 512]); } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "    if (block_size >=  512) { if (tid < 256) { sdata[tid] = mySum = min(mySum, sdata[tid + 256]); } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "    if (block_size >=  256) { if (tid < 128) { sdata[tid] = mySum = min(mySum, sdata[tid + 128]); } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "    if (block_size >=  128) { if (tid <  64) { sdata[tid] = mySum = min(mySum, sdata[tid +  64]); } barrier(CLK_LOCAL_MEM_FENCE); }\n"
		  "\n"
		  "    if (tid < 32) {\n"
		  "        local volatile real* smem = sdata;\n"
		  "        if (block_size >=  64) { smem[tid] = mySum = min(mySum, smem[tid + 32]); }\n"
		  "        if (block_size >=  32) { smem[tid] = mySum = min(mySum, smem[tid + 16]); }\n"
		  "        if (block_size >=  16) { smem[tid] = mySum = min(mySum, smem[tid +  8]); }\n"
		  "        if (block_size >=   8) { smem[tid] = mySum = min(mySum, smem[tid +  4]); }\n"
		  "        if (block_size >=   4) { smem[tid] = mySum = min(mySum, smem[tid +  2]); }\n"
		  "        if (block_size >=   2) { smem[tid] = mySum = min(mySum, smem[tid +  1]); }\n"
		  "    }\n"
		  "\n"
		  "    if (tid == 0) g_odata[get_group_id(0)] = sdata[0];\n"
		  "}\n";
		break;
	}

	std::vector<cl::Device> device;
	device.reserve(queue.size());

	for(auto q = queue.begin(); q != queue.end(); q++)
	    device.push_back(q->getInfo<CL_QUEUE_DEVICE>());

	auto program = build_sources(context, source.str());

	exdata<Expr>::kernel[RDC]   = cl::Kernel(program, kernel_name.c_str());
	exdata<Expr>::compiled[RDC] = true;
	exdata<Expr>::wgsize[RDC]   = kernel_workgroup_size(
		exdata<Expr>::kernel[RDC], device);

	// Strange bug(?) in g++: cannot call getWorkGroupInfo directly on
	// exdata<Expr>::kernel[RDC], but it works like this:
	cl::Kernel &krn = exdata<Expr>::kernel[RDC];

	for(auto d = device.begin(); d != device.end(); d++) {
	    uint smem = d->getInfo<CL_DEVICE_LOCAL_MEM_SIZE>() -
		krn.getWorkGroupInfo<CL_KERNEL_LOCAL_MEM_SIZE>(*d);
	    while(exdata<Expr>::wgsize[RDC] * sizeof(real) > smem)
		exdata<Expr>::wgsize[RDC] /= 2;
	}
    }

    for(uint d = 0; d < queue.size(); d++) {
	uint psize = expr.part_size(d);
	uint g_size = (idx[d + 1] - idx[d]) * exdata<Expr>::wgsize[RDC];
	auto lmem = cl::__local(exdata<Expr>::wgsize[RDC] * sizeof(real));

	uint pos = 0;
	exdata<Expr>::kernel[RDC].setArg(pos++, psize);
	expr.kernel_args(exdata<Expr>::kernel[RDC], d, pos);
	exdata<Expr>::kernel[RDC].setArg(pos++, dbuf[d]());
	exdata<Expr>::kernel[RDC].setArg(pos++, lmem);

	queue[d].enqueueNDRangeKernel(exdata<Expr>::kernel[RDC], cl::NullRange,
		g_size, exdata<Expr>::wgsize[RDC]);
    }

    for(uint d = 0; d < queue.size(); d++) {
	copy(dbuf[d], &hbuf[idx[d]]);
    }

    switch(RDC) {
	case SUM:
	    return std::accumulate(
		    hbuf.begin(), hbuf.end(), static_cast<real>(0));
	case MAX:
	    return *std::max_element(hbuf.begin(), hbuf.end());
	case MIN:
	    return *std::min_element(hbuf.begin(), hbuf.end());
    }
}

/// Sum of vector elements.
template <typename real>
real sum(const clu::vector<real> &x) {
    static Reductor<real,SUM> rdc(x.queue);

    return rdc(x);
}

/// Inner product of two vectors.
template <typename real>
real inner_product(const clu::vector<real> &x, const clu::vector<real> &y) {
    static Reductor<real,SUM> rdc(x.queue);

    return rdc(x * y);
}

} // namespace clu

#endif
<|endoftext|>


<commit_before><commit_msg>Resolves: #i119450# Added EMR_ALPHABLEND case to EnhWMFReader<commit_after><|endoftext|>


<commit_before>#include <cstring>
#include <iomanip>
#include <iostream>
#include <map>
#include <vector>

#include "hash.hpp"
#include "hvectors.hpp"
#include "slice.hpp"

struct Block {
	hash256_t hash = {};
	hash256_t prevBlockHash = {};
	uint32_t bits;

	Block () {}
	Block (const hash256_t& hash, const hash256_t& prevBlockHash, const uint32_t bits) : hash(hash), prevBlockHash(prevBlockHash), bits(bits) {}
};

// find all blocks who are not parents to any other blocks (aka, a chain tip)
auto findChainTips (const HMap<hash256_t, Block>& blocks) {
	std::map<hash256_t, bool> hasChildren;

	for (const auto& blockIter : blocks) {
		const auto& block = blockIter.second;

		// ignore genesis block
		if (blocks.find(block.prevBlockHash) == blocks.end()) continue;

		hasChildren[block.prevBlockHash] = true;
	}

	std::vector<Block> tips;
	for (const auto& blockIter : blocks) {
		const auto& block = blockIter.second;

		// filter to only blocks who have no children
		if (hasChildren.find(block.hash) != hasChildren.end()) continue;

		tips.emplace_back(block);
	}

	return tips;
}

auto determineWork (const HMap<hash256_t, size_t>& workCache, const HMap<hash256_t, Block>& blocks, const Block source) {
	auto visitor = source;
	size_t totalWork = source.bits;

	// naively walk the chain
	while (true) {
		const auto prevBlockIter = blocks.find(visitor.prevBlockHash);
		if (prevBlockIter == blocks.end()) break;

		const auto prevBlockWorkCacheIter = workCache.find(visitor.prevBlockHash);
		if (prevBlockWorkCacheIter != workCache.end()) {
			totalWork += prevBlockWorkCacheIter->second;
			break;
		}

		visitor = prevBlockIter->second;
		totalWork += visitor.bits;
	}

	return totalWork;
}

auto findBestChain (const HMap<hash256_t, Block>& blocks) {
	auto bestBlock = Block();
	size_t mostWork = 0;

	HMap<hash256_t, size_t> workCache;

	for (const auto& blockIter : blocks) {
		const auto& block = blockIter.second;
		const auto work = determineWork(workCache, blocks, block);

		workCache.insort(block.hash, work);

		if (work > mostWork) {
			bestBlock = block;
			mostWork = work;
		}
	}

	auto visitor = bestBlock;
	std::vector<Block> blockchain;
	blockchain.push_back(visitor);

	// walk the chain
	while (true) {
		const auto prevBlockIter = blocks.find(visitor.prevBlockHash);
		if (prevBlockIter == blocks.end()) break;

		visitor = prevBlockIter->second;
		blockchain.push_back(visitor);
	}

	std::reverse(blockchain.begin(), blockchain.end());
	return blockchain;
}

void printerr_hash32 (hash256_t hash) {
	std::cerr << std::hex;
	for (size_t i = 31; i < 32; --i) {
		std::cerr << std::setw(2) << std::setfill('0') << (uint32_t) hash[i];
	}
	std::cerr << std::dec;
}

int main () {
	HMap<hash256_t, Block> blocks;

	// read block headers from stdin until EOF
	{
		while (true) {
			uint8_t rbuf[80];
			const auto read = fread(rbuf, sizeof(rbuf), 1, stdin);

			// EOF?
			if (read == 0) break;

			hash256_t hash, prevBlockHash;
			uint32_t bits;

			hash256(hash.begin(), rbuf, 80);
			memcpy(prevBlockHash.begin(), rbuf + 4, 32);
			memcpy(&bits, rbuf + 72, 4);

			blocks.emplace_back(std::make_pair(hash, Block(hash, prevBlockHash, bits)));
		}

		std::cerr << "Read " << blocks.size() << " headers" << std::endl;
		blocks.sort();
		std::cerr << "Sorted " << blocks.size() << " headers" << std::endl;
	}

	// how many tips exist?
	{
		const auto chainTips = findChainTips(blocks);
		std::cerr << "Found " << chainTips.size() << " chain tips" << std::endl;
	}

	// what is the best?
	const auto bestBlockChain = findBestChain(blocks);

	// print some general information
	{
		const auto genesis = bestBlockChain.front();
		const auto tip = bestBlockChain.back();

		// output
		std::cerr << "Best chain" << std::endl;
		std::cerr << "- Height: " << bestBlockChain.size() - 1 << std::endl;
		std::cerr << "- Genesis: ";
		printerr_hash32(genesis.hash);
		std::cerr << std::endl << "- Tip: ";
		printerr_hash32(tip.hash);
		std::cerr << std::endl;
	}

	// output the best chain [in order]
	{
		HMap<hash256_t, uint32_t> blockChainMap;

		int32_t height = 0;
		for (const auto& block : bestBlockChain) {
			blockChainMap.push_back(std::make_pair(block.hash, height));
			++height;
		}
		blockChainMap.sort();

		StackSlice<36> buffer;
		for (auto&& blockIter : blockChainMap) {
			auto _data = buffer.drop(0);
			_data.writeN(blockIter.first.begin(), 32);
			_data.write<uint32_t>(blockIter.second);
			assert(_data.length() == 0);

			fwrite(buffer.begin(), buffer.length(), 1, stdout);
		}
	}

	return 0;
}
<commit_msg>bestchain: simplify and clarify work calculations<commit_after>#include <cstring>
#include <iomanip>
#include <iostream>
#include <map>
#include <vector>

#include "hash.hpp"
#include "hvectors.hpp"
#include "slice.hpp"

struct Block {
	hash256_t hash = {};
	hash256_t prevBlockHash = {};
	uint32_t bits;

	Block () {}
	Block (const hash256_t& hash, const hash256_t& prevBlockHash, const uint32_t bits) : hash(hash), prevBlockHash(prevBlockHash), bits(bits) {}
};

// find all blocks who are not parents to any other blocks (aka, a chain tip)
auto findChainTips (const HMap<hash256_t, Block>& blocks) {
	std::map<hash256_t, bool> hasChildren;

	for (const auto& blockIter : blocks) {
		const auto& block = blockIter.second;

		// ignore genesis block
		if (blocks.find(block.prevBlockHash) == blocks.end()) continue;

		hasChildren[block.prevBlockHash] = true;
	}

	std::vector<Block> tips;
	for (const auto& blockIter : blocks) {
		const auto& block = blockIter.second;

		// filter to only blocks who have no children
		if (hasChildren.find(block.hash) != hasChildren.end()) continue;

		tips.emplace_back(block);
	}

	return tips;
}

auto determineWork (const HMap<hash256_t, size_t>& workCache, const HMap<hash256_t, Block>& blocks, Block visitor) {
	size_t totalWork = 0;

	// naively walk the chain
	while (true) {
		totalWork += visitor.bits;
		const auto prevBlockIter = blocks.find(visitor.prevBlockHash);

		// is the visitor a genesis block? (no prevBlockIter)
		if (prevBlockIter == blocks.end()) break;

		const auto prevBlockWorkCacheIter = workCache.find(visitor.prevBlockHash);
		if (prevBlockWorkCacheIter != workCache.end()) {
			totalWork += prevBlockWorkCacheIter->second;
			break;
		}

		visitor = prevBlockIter->second;
	}

	return totalWork;
}

auto findBestChain (const HMap<hash256_t, Block>& blocks) {
	auto bestBlock = Block();
	size_t bestChainWork = 0;

	HMap<hash256_t, size_t> workCache;

	for (const auto& blockIter : blocks) {
		const auto& block = blockIter.second;
		const auto chainWork = determineWork(workCache, blocks, block);

		workCache.insort(block.hash, chainWork);
		if (chainWork > bestChainWork) {
			bestBlock = block;
			bestChainWork = chainWork;
		}
	}

	auto visitor = bestBlock;
	std::vector<Block> blockchain;
	blockchain.push_back(visitor);

	// walk the chain
	while (true) {
		const auto prevBlockIter = blocks.find(visitor.prevBlockHash);
		if (prevBlockIter == blocks.end()) break;

		visitor = prevBlockIter->second;
		blockchain.push_back(visitor);
	}

	std::reverse(blockchain.begin(), blockchain.end());
	return blockchain;
}

void printerr_hash32 (hash256_t hash) {
	std::cerr << std::hex;
	for (size_t i = 31; i < 32; --i) {
		std::cerr << std::setw(2) << std::setfill('0') << (uint32_t) hash[i];
	}
	std::cerr << std::dec;
}

int main () {
	HMap<hash256_t, Block> blocks;

	// read block headers from stdin until EOF
	{
		while (true) {
			uint8_t rbuf[80];
			const auto read = fread(rbuf, sizeof(rbuf), 1, stdin);

			// EOF?
			if (read == 0) break;

			hash256_t hash, prevBlockHash;
			uint32_t bits;

			hash256(hash.begin(), rbuf, 80);
			memcpy(prevBlockHash.begin(), rbuf + 4, 32);
			memcpy(&bits, rbuf + 72, 4);

			blocks.emplace_back(std::make_pair(hash, Block(hash, prevBlockHash, bits)));
		}

		std::cerr << "Read " << blocks.size() << " headers" << std::endl;
		blocks.sort();
		std::cerr << "Sorted " << blocks.size() << " headers" << std::endl;
	}

	// how many tips exist?
	{
		const auto chainTips = findChainTips(blocks);
		std::cerr << "Found " << chainTips.size() << " chain tips" << std::endl;
	}

	// what is the best?
	const auto bestBlockChain = findBestChain(blocks);

	// print some general information
	{
		const auto genesis = bestBlockChain.front();
		const auto tip = bestBlockChain.back();

		// output
		std::cerr << "Best chain" << std::endl;
		std::cerr << "- Height: " << bestBlockChain.size() - 1 << std::endl;
		std::cerr << "- Genesis: ";
		printerr_hash32(genesis.hash);
		std::cerr << std::endl << "- Tip: ";
		printerr_hash32(tip.hash);
		std::cerr << std::endl;
	}

	// output the best chain [in order]
	{
		HMap<hash256_t, uint32_t> blockChainMap;

		int32_t height = 0;
		for (const auto& block : bestBlockChain) {
			blockChainMap.push_back(std::make_pair(block.hash, height));
			++height;
		}
		blockChainMap.sort();

		StackSlice<36> buffer;
		for (auto&& blockIter : blockChainMap) {
			auto _data = buffer.drop(0);
			_data.writeN(blockIter.first.begin(), 32);
			_data.write<uint32_t>(blockIter.second);
			assert(_data.length() == 0);

			fwrite(buffer.begin(), buffer.length(), 1, stdout);
		}
	}

	return 0;
}
<|endoftext|>


<commit_before>#include <sys/socket.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <stdlib.h>
#include <unistd.h>
#include <netdb.h>
#include <sys/uio.h>
#include <string.h>
#include <sstream>
#include <istream>
#include "socket.h"
#include "Command.h"
#include "CommandBulider.h"
#include "macro.h"
#include "Message.h"
#include <stdlib.h>

inline slist_t splitString(const std::string &s)
{
    slist_t elems;
    std::istringstream stream(s);
    str_t str;

	while (std::getline(stream, str)) {
        elems.push_back(str);
    }

    return elems;
}

namespace rirc {
	Socket::Socket(str_t ip,uint32_t port,str_t username,command_handler_t commandHandler)
	:self(*this)
	,m_ip(ip)
	,m_username(username)
	,m_port(port)
	,m_commandHandler(commandHandler)
	,m_socketfd(-1)
	{
		if (pthread_mutex_init(&m_lock, NULL) != 0)
	    {
	        __LOGS("\n mutex init failed\n");
	        exit(-1);
	    }
	}

	Socket::~Socket()
	{
		pthread_mutex_destroy(&m_lock);
	}

	void Socket::connect()
	{
		struct sockaddr_in server;
		struct hostent *hserver = NULL;
		int socket = ::socket(AF_INET , SOCK_STREAM , 0);
  		__CHECK(socket > 0);

  		bzero(&server,sizeof(struct sockaddr_in));
  		hserver = gethostbyname(m_ip.data());

  		bcopy(hserver->h_addr, (char *)&server.sin_addr.s_addr,hserver->h_length);
  		server.sin_family = AF_INET;
	    	server.sin_port = htons(m_port);

	    	__IF_DO(::connect(socket , (struct sockaddr *)&server , sizeof(server)) < 0,
					__LOGS("connect error");
	        		return;);
	    	m_socketfd = socket;

	    	Command* cmds[3] = {CommandBulider::bulidPassCommand(m_username),
	    						CommandBulider::bulidNickCommand(m_username),
	    						CommandBulider::bulidUserCommand(m_username.data(),
  											 "tolmoon",
	    										 "tolsun",
	    										 ":Ronnie Reagan")};

	    	for (int i = 0; i < 3; ++i)
	    	{
	    		Command* cmd = cmds[i];
	    		self.sendCommand(*cmd);
	    		delete cmd;
	    		cmds[i] = NULL;
	    	}

   		pthread_attr_t attr;

   		pthread_attr_init(&attr);
   		pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);

   		int res = pthread_create(&m_thread, &attr, Socket::handleSocketStream, (void *)this);
   		pthread_attr_destroy(&attr);
   		__CHECK(res>0);
	}

	void Socket::disconnect()
	{
		Command* cmd = CommandBulider::bulidQuitCommand("");
		self.sendCommand(*cmd);
		delete cmd;
		close(m_socketfd);

		__IF_DO(m_thread>0,pthread_cancel(m_thread););
	}

	void Socket::sendCommand(const Command& cmd) const
	{
		const size_t size = cmd.length();
		uint8_t *buf = new uint8_t[size];
		cmd.serialization(buf,size);
		write(m_socketfd,buf,size);
		delete [] buf;
	}

	void* Socket::handleSocketStream(void* param)
	{
		const Socket& self = *((Socket*)param);
		uint8_t buf[512];
		str_t message;
		ssize_t size = 0;
		do
		{
			message.clear();
			size = 0;
			do{
				bzero(buf,512*sizeof(uint8_t));
				size = ::read(self.m_socketfd,buf,512);
				const str_t msg((char*)buf,size);
				message += msg;
			} while (size >= 512);

			if (message.size() > 0) {
				const slist_t array = ::splitString(message);

	        	for (const str_t & mesg : array)
	        	{
					Message msg = Message::parseMessage(mesg);
					pthread_mutex_lock((pthread_mutex_t*)&self.m_lock);
					self.m_commandHandler(msg,(Socket*)param);
					pthread_mutex_unlock((pthread_mutex_t*)&self.m_lock);
				}
			}
		} while (true);

		return NULL;
	}

}
<commit_msg>first commit<commit_after>#include <sys/socket.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <stdlib.h>
#include <unistd.h>
#include <netdb.h>
#include <sys/uio.h>
#include <string.h>
#include <sstream>
#include <istream>
#include "socket.h"
#include "Command.h"
#include "CommandBulider.h"
#include "macro.h"
#include "Message.h"
#include <stdlib.h>

inline slist_t splitString(const std::string &s)
{
    slist_t elems;
    std::istringstream stream(s);
    str_t str;

	while (std::getline(stream, str)) {
        	elems.push_back(str);
    	}

    return elems;
}

namespace rirc {
	Socket::Socket(str_t ip,uint32_t port,str_t username,command_handler_t commandHandler)
	:self(*this)
	,m_ip(ip)
	,m_username(username)
	,m_port(port)
	,m_commandHandler(commandHandler)
	,m_socketfd(-1)
	{
		if (pthread_mutex_init(&m_lock, NULL) != 0) {
	        	__LOGS("\n mutex init failed\n");
	        	exit(-1);
	    	}
	}

	Socket::~Socket()
	{
		pthread_mutex_destroy(&m_lock);
	}

	void Socket::connect()
	{
		struct sockaddr_in server;
		struct hostent *hserver = NULL;
		int socket = ::socket(AF_INET , SOCK_STREAM , 0);
  		__CHECK(socket > 0);

  		bzero(&server,sizeof(struct sockaddr_in));
  		hserver = gethostbyname(m_ip.data());

  		bcopy(hserver->h_addr, (char *)&server.sin_addr.s_addr,hserver->h_length);
  		server.sin_family = AF_INET;
	    	server.sin_port = htons(m_port);

	    	__IF_DO(::connect(socket , (struct sockaddr *)&server , sizeof(server)) < 0,
					__LOGS("connect error");
	        		return;);
	    	m_socketfd = socket;

	    	Command* cmds[3] = {CommandBulider::bulidPassCommand(m_username),
	    						CommandBulider::bulidNickCommand(m_username),
	    						CommandBulider::bulidUserCommand(m_username.data(),
  											 "tolmoon",
	    										 "tolsun",
	    										 ":Ronnie Reagan")};

	    	for (int i = 0; i < 3; ++i) {
	    		Command* cmd = cmds[i];
	    		self.sendCommand(*cmd);
	    		delete cmd;
	    		cmds[i] = NULL;
	    	}

   		pthread_attr_t attr;

   		pthread_attr_init(&attr);
   		pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);

   		int res = pthread_create(&m_thread, &attr, Socket::handleSocketStream, (void *)this);
   		pthread_attr_destroy(&attr);
   		__CHECK(res>0);
	}

	void Socket::disconnect()
	{
		Command* cmd = CommandBulider::bulidQuitCommand("");
		self.sendCommand(*cmd);
		delete cmd;
		close(m_socketfd);

		__IF_DO(m_thread>0,pthread_cancel(m_thread););
	}

	void Socket::sendCommand(const Command& cmd) const
	{
		const size_t size = cmd.length();
		uint8_t *buf = new uint8_t[size];
		cmd.serialization(buf,size);
		write(m_socketfd,buf,size);
		delete [] buf;
	}

	void* Socket::handleSocketStream(void* param)
	{
		const Socket& self = *((Socket*)param);
		uint8_t buf[512];
		str_t message;
		ssize_t size = 0;
		do {
			message.clear();
			size = 0;
			do{
				bzero(buf,512*sizeof(uint8_t));
				size = ::read(self.m_socketfd,buf,512);
				const str_t msg((char*)buf,size);
				message += msg;
			} while (size >= 512);

			if (message.size() > 0) {
				const slist_t array = ::splitString(message);

	        		for (const str_t & mesg : array) {
					Message msg = Message::parseMessage(mesg);
					pthread_mutex_lock((pthread_mutex_t*)&self.m_lock);
					self.m_commandHandler(msg,(Socket*)param);
					pthread_mutex_unlock((pthread_mutex_t*)&self.m_lock);
				}
			}
		} while (true);

		return NULL;
	}

}
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  $RCSfile: gcach_ftyp.hxx,v $
 *
 *  $Revision: 1.28 $
 *
 *  last change: $Author: rt $ $Date: 2004-07-13 09:30:12 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  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
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (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.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#ifndef _SV_GCACHFTYP_HXX
#define _SV_GCACHFTYP_HXX

#include <glyphcache.hxx>
#include <rtl/textcvt.h>

#include <freetype/freetype.h>
class FreetypeServerFont;
struct FT_GlyphRec_;

// -----------------------------------------------------------------------

// FtFontFile has the responsibility that a font file is only mapped once.
// (#86621#) the old directly ft-managed solution caused it to be mapped
// in up to nTTC*nSizes*nOrientation*nSynthetic times
class FtFontFile
{
public:
    static FtFontFile*      FindFontFile( const ::rtl::OString& rNativeFileName );

    bool                    Map();
    void                    Unmap();

    const unsigned char*    GetBuffer() const { return mpFileMap; }
    int                     GetFileSize() const { return mnFileSize; }
    const ::rtl::OString*   GetFileName() const { return &maNativeFileName; }
    int                     GetLangBoost() const { return mnLangBoost; }

private:
                            FtFontFile( const ::rtl::OString& rNativeFileName );

    const ::rtl::OString    maNativeFileName;
    const unsigned char*    mpFileMap;
    int                     mnFileSize;
    int                     mnRefCount;
    int                     mnLangBoost;
};

// -----------------------------------------------------------------------

// FtFontInfo corresponds to an unscaled font face
class FtFontInfo
{
public:
                           FtFontInfo( const ImplDevFontAttributes&,
                               const ::rtl::OString& rNativeFileName,
                               int nFaceNum, int nFontId, int nSynthetic,
                                const ExtraKernInfo* );
                          ~FtFontInfo();

    const unsigned char*  GetTable( const char*, ULONG* pLength=0 ) const;

    FT_FaceRec_*          GetFaceFT();
    void                  ReleaseFaceFT( FT_FaceRec_* );

    const ::rtl::OString* GetFontFileName() const   { return mpFontFile->GetFileName(); }
    int                   GetFaceNum() const        { return mnFaceNum; }
    int                   GetSynthetic() const      { return mnSynthetic; }
    int                   GetFontId() const         { return mnFontId; }

    bool                  IsSymbolFont() const      { return maDevFontAttributes.IsSymbolFont(); }
    const ImplFontAttributes& GetFontAttributes() const { return maDevFontAttributes; }

    void                  AnnounceFont( ImplDevFontList* );

    int                   GetGlyphIndex( sal_UCS4 cChar ) const;
    void                  CacheGlyphIndex( sal_UCS4 cChar, int nGI ) const;

    bool                  HasExtraKerning() const;
    int                   GetExtraKernPairs( ImplKernPairData** ) const;
    int                   GetExtraGlyphKernValue( int nLeftGlyph, int nRightGlyph ) const;

private:
    FT_FaceRec_*    maFaceFT;
    FtFontFile*     mpFontFile;
    const int       mnFaceNum;
    int             mnRefCount;
    const int       mnSynthetic;

    int             mnFontId;
    ImplDevFontAttributes maDevFontAttributes;

    // cache unicode->glyphid mapping because looking it up is expensive
    // TODO: change to hash_multimap when a use case requires a m:n mapping
    typedef ::std::hash_map<int,int> Int2IntMap;
    mutable Int2IntMap maChar2Glyph;
    mutable Int2IntMap maGlyph2Char;

    const ExtraKernInfo* mpExtraKernInfo;
};

// these two inlines are very important for performance

inline int FtFontInfo::GetGlyphIndex( sal_UCS4 cChar ) const
{
    Int2IntMap::const_iterator it = maChar2Glyph.find( cChar );
    if( it == maChar2Glyph.end() )
        return -1;
    return it->second;
}

inline void FtFontInfo::CacheGlyphIndex( sal_UCS4 cChar, int nIndex ) const
{
    maChar2Glyph[ cChar ] = nIndex;
    maGlyph2Char[ nIndex ] = cChar;
}

// -----------------------------------------------------------------------

class FreetypeManager
{
public:
                        FreetypeManager();
                        ~FreetypeManager();

    long                AddFontDir( const String& rUrlName );
    void                AddFontFile( const rtl::OString& rNormalizedName,
                            int nFaceNum, int nFontId, const ImplDevFontAttributes&,
                            const ExtraKernInfo* );
    void                AnnounceFonts( ImplDevFontList* ) const;
    void                ClearFontList();

    FreetypeServerFont* CreateFont( const ImplFontSelectData& );

private:
    typedef ::std::hash_map<int,FtFontInfo*> FontList;
    FontList            maFontList;

    int                 mnMaxFontId;
    int                 mnNextFontId;
};

// -----------------------------------------------------------------------

class FreetypeServerFont : public ServerFont
{
public:
                                FreetypeServerFont( const ImplFontSelectData&, FtFontInfo* );
    virtual                     ~FreetypeServerFont();

    virtual const ::rtl::OString* GetFontFileName() const { return mpFontInfo->GetFontFileName(); }
    virtual int                 GetFontFaceNum() const { return mpFontInfo->GetFaceNum(); }
    virtual bool                TestFont() const;

    virtual void                FetchFontMetric( ImplFontMetricData&, long& rFactor ) const;

    virtual int                 GetGlyphIndex( sal_UCS4 ) const;
    int                         GetRawGlyphIndex( sal_UCS4 ) const;
    int                         FixupGlyphIndex( int nGlyphIndex, sal_UCS4 ) const;

    virtual bool                GetAntialiasAdvice( void ) const;
    virtual bool                GetGlyphBitmap1( int nGlyphIndex, RawBitmap& ) const;
    virtual bool                GetGlyphBitmap8( int nGlyphIndex, RawBitmap& ) const;
    virtual bool                GetGlyphOutline( int nGlyphIndex, PolyPolygon& ) const;
    virtual int                 GetGlyphKernValue( int nLeftGlyph, int nRightGlyph ) const;
    virtual ULONG               GetKernPairs( ImplKernPairData** ) const;

    const unsigned char*        GetTable( const char* pName, ULONG* pLength )
                                { return mpFontInfo->GetTable( pName, pLength ); }
    int                         GetEmUnits() const;
    const FT_Size_Metrics&      GetMetricsFT() const { return maSizeFT->metrics; }

protected:
    friend class GlyphCache;

    int                         ApplyGlyphTransform( int nGlyphFlags, FT_GlyphRec_* ) const;
    virtual void                InitGlyphData( int nGlyphIndex, GlyphData& ) const;
    virtual ULONG               GetFontCodeRanges( sal_uInt32* pCodes ) const;
    bool                        ApplyGSUB( const ImplFontSelectData& );
    virtual ServerFontLayoutEngine* GetLayoutEngine();

private:
    int                         mnWidth;
    FtFontInfo*                 mpFontInfo;
    FT_Int                      mnLoadFlags;
    double                      mfStretch;
    FT_FaceRec_*                maFaceFT;
    FT_SizeRec_*                maSizeFT;

    typedef ::std::hash_map<int,int> GlyphSubstitution;
    GlyphSubstitution           maGlyphSubstitution;
    rtl_UnicodeToTextConverter  maRecodeConverter;

    ServerFontLayoutEngine*     mpLayoutEngine;
};

// -----------------------------------------------------------------------

class ImplFTSFontData : public ImplFontData
{
private:
    FtFontInfo*             mpFtFontInfo;
    enum { IFTSFONT_MAGIC = 0x1F150A1C };

public:
                            ImplFTSFontData( FtFontInfo*, const ImplDevFontAttributes& );
    virtual                 ~ImplFTSFontData();

    FtFontInfo*             GetFtFontInfo() const { return mpFtFontInfo; }

    virtual ImplFontEntry*  CreateFontInstance( ImplFontSelectData& ) const;
    virtual ImplFontData*   Clone() const   { return new ImplFTSFontData( *this ); }
    virtual int             GetFontId() const { return mpFtFontInfo->GetFontId(); }

    static bool             CheckFontData( const ImplFontData& r ) { return r.CheckMagic( IFTSFONT_MAGIC ); }
};

// -----------------------------------------------------------------------

#endif // _SV_GCACHFTYP_HXX
<commit_msg>INTEGRATION: CWS ooo20040704 (1.27.34); FILE MERGED 2004/08/09 14:56:37 vg 1.27.34.4: RESYNC MERGED 1.27 1.28 Everything below this line will be added to the revision comment. 2004/07/12 14:02:25 haggai 1.27.34.3: #i31318# - Use ft2build.h to include freetype headers 2004/07/02 09:49:06 cmc 1.27.34.2: #i30891# revert header and namespace change 2004/06/28 14:49:08 cmc 1.27.34.1: #i30801# allow using system stl if possible<commit_after>/*************************************************************************
 *
 *  $RCSfile: gcach_ftyp.hxx,v $
 *
 *  $Revision: 1.29 $
 *
 *  last change: $Author: rt $ $Date: 2004-09-08 15:08:12 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  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
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (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.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#ifndef _SV_GCACHFTYP_HXX
#define _SV_GCACHFTYP_HXX

#include <glyphcache.hxx>
#include <rtl/textcvt.h>

#include <ft2build.h>
#include FT_FREETYPE_H
class FreetypeServerFont;
struct FT_GlyphRec_;

// -----------------------------------------------------------------------

// FtFontFile has the responsibility that a font file is only mapped once.
// (#86621#) the old directly ft-managed solution caused it to be mapped
// in up to nTTC*nSizes*nOrientation*nSynthetic times
class FtFontFile
{
public:
    static FtFontFile*      FindFontFile( const ::rtl::OString& rNativeFileName );

    bool                    Map();
    void                    Unmap();

    const unsigned char*    GetBuffer() const { return mpFileMap; }
    int                     GetFileSize() const { return mnFileSize; }
    const ::rtl::OString*   GetFileName() const { return &maNativeFileName; }
    int                     GetLangBoost() const { return mnLangBoost; }

private:
                            FtFontFile( const ::rtl::OString& rNativeFileName );

    const ::rtl::OString    maNativeFileName;
    const unsigned char*    mpFileMap;
    int                     mnFileSize;
    int                     mnRefCount;
    int                     mnLangBoost;
};

// -----------------------------------------------------------------------

// FtFontInfo corresponds to an unscaled font face
class FtFontInfo
{
public:
                           FtFontInfo( const ImplDevFontAttributes&,
                               const ::rtl::OString& rNativeFileName,
                               int nFaceNum, int nFontId, int nSynthetic,
                                const ExtraKernInfo* );
                          ~FtFontInfo();

    const unsigned char*  GetTable( const char*, ULONG* pLength=0 ) const;

    FT_FaceRec_*          GetFaceFT();
    void                  ReleaseFaceFT( FT_FaceRec_* );

    const ::rtl::OString* GetFontFileName() const   { return mpFontFile->GetFileName(); }
    int                   GetFaceNum() const        { return mnFaceNum; }
    int                   GetSynthetic() const      { return mnSynthetic; }
    int                   GetFontId() const         { return mnFontId; }

    bool                  IsSymbolFont() const      { return maDevFontAttributes.IsSymbolFont(); }
    const ImplFontAttributes& GetFontAttributes() const { return maDevFontAttributes; }

    void                  AnnounceFont( ImplDevFontList* );

    int                   GetGlyphIndex( sal_UCS4 cChar ) const;
    void                  CacheGlyphIndex( sal_UCS4 cChar, int nGI ) const;

    bool                  HasExtraKerning() const;
    int                   GetExtraKernPairs( ImplKernPairData** ) const;
    int                   GetExtraGlyphKernValue( int nLeftGlyph, int nRightGlyph ) const;

private:
    FT_FaceRec_*    maFaceFT;
    FtFontFile*     mpFontFile;
    const int       mnFaceNum;
    int             mnRefCount;
    const int       mnSynthetic;

    int             mnFontId;
    ImplDevFontAttributes maDevFontAttributes;

    // cache unicode->glyphid mapping because looking it up is expensive
    // TODO: change to hash_multimap when a use case requires a m:n mapping
    typedef ::std::hash_map<int,int> Int2IntMap;
    mutable Int2IntMap maChar2Glyph;
    mutable Int2IntMap maGlyph2Char;

    const ExtraKernInfo* mpExtraKernInfo;
};

// these two inlines are very important for performance

inline int FtFontInfo::GetGlyphIndex( sal_UCS4 cChar ) const
{
    Int2IntMap::const_iterator it = maChar2Glyph.find( cChar );
    if( it == maChar2Glyph.end() )
        return -1;
    return it->second;
}

inline void FtFontInfo::CacheGlyphIndex( sal_UCS4 cChar, int nIndex ) const
{
    maChar2Glyph[ cChar ] = nIndex;
    maGlyph2Char[ nIndex ] = cChar;
}

// -----------------------------------------------------------------------

class FreetypeManager
{
public:
                        FreetypeManager();
                        ~FreetypeManager();

    long                AddFontDir( const String& rUrlName );
    void                AddFontFile( const rtl::OString& rNormalizedName,
                            int nFaceNum, int nFontId, const ImplDevFontAttributes&,
                            const ExtraKernInfo* );
    void                AnnounceFonts( ImplDevFontList* ) const;
    void                ClearFontList();

    FreetypeServerFont* CreateFont( const ImplFontSelectData& );

private:
    typedef ::std::hash_map<int,FtFontInfo*> FontList;
    FontList            maFontList;

    int                 mnMaxFontId;
    int                 mnNextFontId;
};

// -----------------------------------------------------------------------

class FreetypeServerFont : public ServerFont
{
public:
                                FreetypeServerFont( const ImplFontSelectData&, FtFontInfo* );
    virtual                     ~FreetypeServerFont();

    virtual const ::rtl::OString* GetFontFileName() const { return mpFontInfo->GetFontFileName(); }
    virtual int                 GetFontFaceNum() const { return mpFontInfo->GetFaceNum(); }
    virtual bool                TestFont() const;

    virtual void                FetchFontMetric( ImplFontMetricData&, long& rFactor ) const;

    virtual int                 GetGlyphIndex( sal_UCS4 ) const;
    int                         GetRawGlyphIndex( sal_UCS4 ) const;
    int                         FixupGlyphIndex( int nGlyphIndex, sal_UCS4 ) const;

    virtual bool                GetAntialiasAdvice( void ) const;
    virtual bool                GetGlyphBitmap1( int nGlyphIndex, RawBitmap& ) const;
    virtual bool                GetGlyphBitmap8( int nGlyphIndex, RawBitmap& ) const;
    virtual bool                GetGlyphOutline( int nGlyphIndex, PolyPolygon& ) const;
    virtual int                 GetGlyphKernValue( int nLeftGlyph, int nRightGlyph ) const;
    virtual ULONG               GetKernPairs( ImplKernPairData** ) const;

    const unsigned char*        GetTable( const char* pName, ULONG* pLength )
                                { return mpFontInfo->GetTable( pName, pLength ); }
    int                         GetEmUnits() const;
    const FT_Size_Metrics&      GetMetricsFT() const { return maSizeFT->metrics; }

protected:
    friend class GlyphCache;

    int                         ApplyGlyphTransform( int nGlyphFlags, FT_GlyphRec_* ) const;
    virtual void                InitGlyphData( int nGlyphIndex, GlyphData& ) const;
    virtual ULONG               GetFontCodeRanges( sal_uInt32* pCodes ) const;
    bool                        ApplyGSUB( const ImplFontSelectData& );
    virtual ServerFontLayoutEngine* GetLayoutEngine();

private:
    int                         mnWidth;
    FtFontInfo*                 mpFontInfo;
    FT_Int                      mnLoadFlags;
    double                      mfStretch;
    FT_FaceRec_*                maFaceFT;
    FT_SizeRec_*                maSizeFT;

    typedef ::std::hash_map<int,int> GlyphSubstitution;
    GlyphSubstitution           maGlyphSubstitution;
    rtl_UnicodeToTextConverter  maRecodeConverter;

    ServerFontLayoutEngine*     mpLayoutEngine;
};

// -----------------------------------------------------------------------

class ImplFTSFontData : public ImplFontData
{
private:
    FtFontInfo*             mpFtFontInfo;
    enum { IFTSFONT_MAGIC = 0x1F150A1C };

public:
                            ImplFTSFontData( FtFontInfo*, const ImplDevFontAttributes& );
    virtual                 ~ImplFTSFontData();

    FtFontInfo*             GetFtFontInfo() const { return mpFtFontInfo; }

    virtual ImplFontEntry*  CreateFontInstance( ImplFontSelectData& ) const;
    virtual ImplFontData*   Clone() const   { return new ImplFTSFontData( *this ); }
    virtual int             GetFontId() const { return mpFtFontInfo->GetFontId(); }

    static bool             CheckFontData( const ImplFontData& r ) { return r.CheckMagic( IFTSFONT_MAGIC ); }
};

// -----------------------------------------------------------------------

#endif // _SV_GCACHFTYP_HXX
<|endoftext|>


<commit_before>// Copyright (c) 2014 The Caroline authors. All rights reserved.
// Use of this source file is governed by a MIT license that can be found in the
// LICENSE file.
// Author: Kukleva Anna <Kuklevaanna@gmail.com>

#include "core/camera_calibration.h"

#include <vector>
#include "core/cameras.h"

namespace core {

Cameras CameraCalibration::calibrate(int nx, int ny, float square_size) {
  int n = nx * ny;
  int len;
  int count_of_pairs;
  cv::Matx33d K1_;
  cv::Matx33d K2_;
  cv::Matx33d R_;
  cv::Matx31d T_;
  cv::Matx33d E_;
  cv::Matx33d F_;
  std::vector<double> D1(5, 0);
  std::vector<double> D2(5, 0);

  cv::Size imageSize = {0, 0};
  imageSize = left_images_.at(0).size();
  len = left_images_.size();

  // HARVEST CHESSBOARD 3D OBJECT POINT LIST:
  count_of_pairs = VPoints1.size();

  std::vector<std::vector<cv::Point3f>> VobjectPoints(count_of_pairs);

  for (int i = 0; i < count_of_pairs; i++) {
    VobjectPoints[i] = object_points_;
  }

  first_ = CalebrationOneCamera(kLeft, nx, ny, square_size);
  D1 = D;
  second_ = CalebrationOneCamera(kRight, nx, ny, square_size);
  D2 = D;

  // StereoCalibrate
  cv::stereoCalibrate(VobjectPoints, VPoints1,
      VPoints2, first_, D1, second_, D2,
      imageSize, R_, T_, E_, F_);

  cv::Matx34d P1_(1, 0, 0, 0,
                  0, 1, 0, 0,
                  0, 0, 1, 0);
  cv::Matx34d P2_(R_(0, 0), R_(0, 1), R_(0, 2), T_(0, 0),
                  R_(1, 0), R_(1, 1), R_(1, 2), T_(1, 0),
                  R_(2, 0), R_(2, 1), R_(2, 2), T_(0, 0));

  Cameras cameras_par;
  cameras_par.set_K1(first_);
  cameras_par.set_K2(second_);
  cameras_par.set_P1(P1_);
  cameras_par.set_P2(P2_);

  return cameras_par;
}

void CameraCalibration::addImagePair(const cv::Mat &image1,
                                     const cv::Mat &image2, int nx, int ny) {
  left_images_.push_back(image1);
  right_images_.push_back(image2);

  // harvest points of corners for one image
  std::vector<cv::Point2f> temp1(nx * ny);
  std::vector<cv::Point2f> temp2(nx * ny);

  int len;
  int result1 = 0;
  int result2 = 0;

  // Finds the positions of
  //internal corners of the chessboard
  result1 = cv::findChessboardCorners(
        image1, cv::Size(nx, ny), temp1);
  result2 = cv::findChessboardCorners(
        image2, cv::Size(nx, ny), temp2);

  if (result1 && result2) {
    // Refines the corner locations
    cv::cornerSubPix(image1, temp1, cv::Size(5, 5),
              cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::EPS +
              cv::TermCriteria::COUNT, 30, 0.01));
    cv::cornerSubPix(image2, temp2, cv::Size(11, 11),
              cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::EPS +
              cv::TermCriteria::COUNT, 30, 0.01));
    VPoints1.push_back(temp1);
    VPoints2.push_back(temp2);
  }
}

void CameraCalibration::HarvestChessboardIdealPointList(
    int nx, int ny, float square_size) {
  object_points_.resize(nx * ny);

  for (int i = 0; i < ny; i++) {
    for (int j = 0; j < nx; j++) {
      object_points_[i*nx+j] = cv::Point3f(i*square_size, j*square_size, 0);
    }
  }
}

cv::Matx33d CameraCalibration::CalebrationOneCamera(int CameraOrientation,
                                          int nx, int ny, float square_size) {
  std::vector<std::vector<cv::Point2f>> VPoints;
  std::vector<cv::Mat> images;
  if (CameraOrientation == kLeft) {
    images = left_images_;
    VPoints = VPoints1;
  } else {
    images = right_images_;
    VPoints = VPoints2;
  }

  int result = 0;
  int n = nx * ny;
  int counter;
  int length;
  std::vector<cv::Point2f> temp(n);
  cv::Size imageSize = {0, 0};
  imageSize = images.at(0).size();
  cv::Matx33d K;
  std::vector<cv::Mat> vrvec;
  std::vector<cv::Mat> tvec;

  length = images.size();

  counter = VPoints.size();
  std::vector<std::vector<cv::Point3f>> VobjectPoints(counter);

  for (int i = 0; i < counter; i++) {
    VobjectPoints[i] = object_points_;
  }

  cv::setIdentity(K);

  cv::calibrateCamera(
        VobjectPoints, VPoints, imageSize, K, D, vrvec, tvec, 0,
        cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT,
                         30, 0.01));

  return K;
}

}  //  namespace core
<commit_msg>#58: Style was fixed.<commit_after>// Copyright (c) 2014 The Caroline authors. All rights reserved.
// Use of this source file is governed by a MIT license that can be found in the
// LICENSE file.
// Author: Kukleva Anna <Kuklevaanna@gmail.com>

#include "core/camera_calibration.h"

#include <vector>
#include "core/cameras.h"

namespace core {

Cameras CameraCalibration::calibrate(int nx, int ny, float square_size) {
  int n = nx * ny;
  int len;
  int count_of_pairs;
  cv::Matx33d K1_;
  cv::Matx33d K2_;
  cv::Matx33d R_;
  cv::Matx31d T_;
  cv::Matx33d E_;
  cv::Matx33d F_;
  std::vector<double> D1(5, 0);
  std::vector<double> D2(5, 0);

  cv::Size imageSize = {0, 0};
  imageSize = left_images_.at(0).size();
  len = left_images_.size();

  // HARVEST CHESSBOARD 3D OBJECT POINT LIST:
  count_of_pairs = VPoints1.size();

  std::vector<std::vector<cv::Point3f>> VobjectPoints(count_of_pairs);

  for (int i = 0; i < count_of_pairs; i++) {
    VobjectPoints[i] = object_points_;
  }

  first_ = CalebrationOneCamera(kLeft, nx, ny, square_size);
  D1 = D;
  second_ = CalebrationOneCamera(kRight, nx, ny, square_size);
  D2 = D;

  // StereoCalibrate
  cv::stereoCalibrate(VobjectPoints, VPoints1,
      VPoints2, first_, D1, second_, D2,
      imageSize, R_, T_, E_, F_);

  cv::Matx34d P1_(1, 0, 0, 0,
                  0, 1, 0, 0,
                  0, 0, 1, 0);
  cv::Matx34d P2_(R_(0, 0), R_(0, 1), R_(0, 2), T_(0, 0),
                  R_(1, 0), R_(1, 1), R_(1, 2), T_(1, 0),
                  R_(2, 0), R_(2, 1), R_(2, 2), T_(0, 0));

  Cameras cameras_par;
  cameras_par.set_K1(first_);
  cameras_par.set_K2(second_);
  cameras_par.set_P1(P1_);
  cameras_par.set_P2(P2_);

  return cameras_par;
}

void CameraCalibration::addImagePair(const cv::Mat &image1,
                                     const cv::Mat &image2, int nx, int ny) {
  left_images_.push_back(image1);
  right_images_.push_back(image2);

  // harvest points of corners for one image
  std::vector<cv::Point2f> temp1(nx * ny);
  std::vector<cv::Point2f> temp2(nx * ny);

  int len;
  int result1 = 0;
  int result2 = 0;

  // Finds the positions of
  // internal corners of the chessboard
  result1 = cv::findChessboardCorners(
        image1, cv::Size(nx, ny), temp1);
  result2 = cv::findChessboardCorners(
        image2, cv::Size(nx, ny), temp2);

  if (result1 && result2) {
    // Refines the corner locations
    cv::cornerSubPix(image1, temp1, cv::Size(5, 5),
              cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::EPS +
              cv::TermCriteria::COUNT, 30, 0.01));
    cv::cornerSubPix(image2, temp2, cv::Size(11, 11),
              cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::EPS +
              cv::TermCriteria::COUNT, 30, 0.01));
    VPoints1.push_back(temp1);
    VPoints2.push_back(temp2);
  }
}

void CameraCalibration::HarvestChessboardIdealPointList(
    int nx, int ny, float square_size) {
  object_points_.resize(nx * ny);

  for (int i = 0; i < ny; i++) {
    for (int j = 0; j < nx; j++) {
      object_points_[i*nx+j] = cv::Point3f(i*square_size, j*square_size, 0);
    }
  }
}

cv::Matx33d CameraCalibration::CalebrationOneCamera(int CameraOrientation,
                                          int nx, int ny, float square_size) {
  std::vector<std::vector<cv::Point2f>> VPoints;
  std::vector<cv::Mat> images;
  if (CameraOrientation == kLeft) {
    images = left_images_;
    VPoints = VPoints1;
  } else {
    images = right_images_;
    VPoints = VPoints2;
  }

  int result = 0;
  int n = nx * ny;
  int counter;
  int length;
  std::vector<cv::Point2f> temp(n);
  cv::Size imageSize = {0, 0};
  imageSize = images.at(0).size();
  cv::Matx33d K;
  std::vector<cv::Mat> vrvec;
  std::vector<cv::Mat> tvec;

  length = images.size();

  counter = VPoints.size();
  std::vector<std::vector<cv::Point3f>> VobjectPoints(counter);

  for (int i = 0; i < counter; i++) {
    VobjectPoints[i] = object_points_;
  }

  cv::setIdentity(K);

  cv::calibrateCamera(
        VobjectPoints, VPoints, imageSize, K, D, vrvec, tvec, 0,
        cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT,
                         30, 0.01));

  return K;
}

}  //  namespace core
<|endoftext|>


<commit_before>//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "ClResizeBilinearFloatWorkload.hpp"
#include <backends/cl/ClTensorHandle.hpp>
#include <backends/CpuTensorHandle.hpp>
#include <backends/cl/ClLayerSupport.hpp>
#include <backends/aclCommon/ArmComputeUtils.hpp>
#include <backends/aclCommon/ArmComputeTensorUtils.hpp>

#include "ClWorkloadUtils.hpp"

using namespace armnn::armcomputetensorutils;

namespace armnn
{

ClResizeBilinearFloatWorkload::ClResizeBilinearFloatWorkload(const ResizeBilinearQueueDescriptor& descriptor,
                                                             const WorkloadInfo& info)
    : FloatWorkload<ResizeBilinearQueueDescriptor>(descriptor, info)
{
    m_Data.ValidateInputsOutputs("ClResizeBilinearFloatWorkload", 1, 1);

    arm_compute::ICLTensor& input  = static_cast<IClTensorHandle*>(m_Data.m_Inputs[0])->GetTensor();
    arm_compute::ICLTensor& output = static_cast<IClTensorHandle*>(m_Data.m_Outputs[0])->GetTensor();

    (&input)->info()->set_data_layout(ConvertDataLayout(m_Data.m_Parameters.m_DataLayout));
    (&output)->info()->set_data_layout(ConvertDataLayout(m_Data.m_Parameters.m_DataLayout));

    m_ResizeBilinearLayer.configure(&input, &output, arm_compute::InterpolationPolicy::BILINEAR,
                                    arm_compute::BorderMode::REPLICATE, arm_compute::PixelValue(0.f),
                                    arm_compute::SamplingPolicy::TOP_LEFT);
};

void ClResizeBilinearFloatWorkload::Execute() const
{
    ARMNN_SCOPED_PROFILING_EVENT_CL("ClResizeBilinearFloatWorkload_Execute");
    m_ResizeBilinearLayer.run();
}

} //namespace armnn
<commit_msg>IVGCVSW-1931: Refactor ClResizeBilinearFloatWorkload<commit_after>//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "ClResizeBilinearFloatWorkload.hpp"
#include <backends/cl/ClTensorHandle.hpp>
#include <backends/CpuTensorHandle.hpp>
#include <backends/cl/ClLayerSupport.hpp>
#include <backends/aclCommon/ArmComputeUtils.hpp>
#include <backends/aclCommon/ArmComputeTensorUtils.hpp>

#include "ClWorkloadUtils.hpp"

using namespace armnn::armcomputetensorutils;

namespace armnn
{

ClResizeBilinearFloatWorkload::ClResizeBilinearFloatWorkload(const ResizeBilinearQueueDescriptor& descriptor,
                                                             const WorkloadInfo& info)
    : FloatWorkload<ResizeBilinearQueueDescriptor>(descriptor, info)
{
    m_Data.ValidateInputsOutputs("ClResizeBilinearFloatWorkload", 1, 1);

    arm_compute::ICLTensor& input  = static_cast<IClTensorHandle*>(m_Data.m_Inputs[0])->GetTensor();
    arm_compute::ICLTensor& output = static_cast<IClTensorHandle*>(m_Data.m_Outputs[0])->GetTensor();

    arm_compute::DataLayout aclDataLayout = ConvertDataLayout(m_Data.m_Parameters.m_DataLayout);
    input.info()->set_data_layout(aclDataLayout);
    output.info()->set_data_layout(aclDataLayout);

    m_ResizeBilinearLayer.configure(&input, &output, arm_compute::InterpolationPolicy::BILINEAR,
                                    arm_compute::BorderMode::REPLICATE, arm_compute::PixelValue(0.f),
                                    arm_compute::SamplingPolicy::TOP_LEFT);
};

void ClResizeBilinearFloatWorkload::Execute() const
{
    ARMNN_SCOPED_PROFILING_EVENT_CL("ClResizeBilinearFloatWorkload_Execute");
    m_ResizeBilinearLayer.run();
}

} //namespace armnn
<|endoftext|>


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

  Program:   Visualization Toolkit
  Module:    vtkVectorTopology.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

Copyright (c) 1993-2000 Ken Martin, Will Schroeder, Bill Lorensen
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.

 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.

 * Neither name of Ken Martin, Will Schroeder, or Bill Lorensen nor the names
   of any contributors may be used to endorse or promote products derived
   from this software without specific prior written permission.

 * Modified source versions must be plainly marked as such, and must not be
   misrepresented as being the original software.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS''
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

=========================================================================*/
#include <math.h>
#include "vtkVectorTopology.h"
#include "vtkObjectFactory.h"

//------------------------------------------------------------------------------
vtkVectorTopology* vtkVectorTopology::New()
{
  // First try to create the object from the vtkObjectFactory
  vtkObject* ret = vtkObjectFactory::CreateInstance("vtkVectorTopology");
  if(ret)
    {
    return (vtkVectorTopology*)ret;
    }
  // If the factory was unable to create the object, then create it here.
  return new vtkVectorTopology;
}

// Construct object with distance 0.1.
vtkVectorTopology::vtkVectorTopology()
{
  this->Distance = 0.1;
}

void vtkVectorTopology::Execute()
{
  int cellId, i, j, ptId, npts;
  int negative[3], positive[3], subId=0;
  float x[3], pcoords[3], *v;
  vtkCell *cell;
  vtkVectors *inVectors;
  vtkPoints *newPts;
  vtkCellArray *newVerts;
  vtkDataSet *input=(vtkDataSet *)this->Input;
  vtkPointData *pd=input->GetPointData();
  vtkPolyData *output=(vtkPolyData *)this->Output;
  vtkPointData *outputPD=output->GetPointData();
  float *weights=new float[input->GetMaxCellSize()];
//
// Initialize self; check input; create output objects
//
  vtkDebugMacro(<< "Executing vector topology...");

  // make sure we have vector data
  if ( ! (inVectors = input->GetPointData()->GetVectors()) )
    {
    vtkErrorMacro(<<"No vector data, can't create topology markers...");
    return;
    }

  newPts = vtkPoints::New();
  newPts->Allocate(100);
  newVerts = vtkCellArray::New();
  newVerts->Allocate(newVerts->EstimateSize(1,100));
  outputPD->CopyAllocate(pd);
//
// Find cells whose vector components all pass through zero
//
  pcoords[0] = pcoords[1] = pcoords[2] = 0.5;
  newVerts->InsertNextCell(100); //temporary count
  for (cellId=0; cellId<input->GetNumberOfCells(); cellId++)
    {
    cell = input->GetCell(cellId);
    npts = cell->GetNumberOfPoints();
    for (i=0; i<3; i++)
      {
      negative[i] = positive[i] = 0;
      }
    for (i=0; i < npts; i++)
      {
      ptId = cell->GetPointId(i);
      v = inVectors->GetVector(ptId);
      for (j=0; j<3; j++)
        {
        if ( v[j] < 0.0 )
	  {
	  negative[j] = 1;
	  }
        else if ( v[j] >= 0.0 )
	  {
	  positive[j] = 1;
	  }
        }
      }
    if ( negative[0] && positive[0] && negative[1] && positive[1] &&
    negative[2] && positive[2] )
      { // place point at center of cell
      cell->EvaluateLocation(subId, pcoords, x, weights);
      ptId = newPts->InsertNextPoint(x);
      newVerts->InsertCellPoint(ptId);
      }
    }
  newVerts->UpdateCellCount(newPts->GetNumberOfPoints());

  vtkDebugMacro(<< "Created " << newPts->GetNumberOfPoints() << "points");
  delete [] weights;
//
// Update ourselves
//
  output->SetPoints(newPts);
  output->SetVerts(newVerts);
  output->Squeeze();
}

void vtkVectorTopology::PrintSelf(ostream& os, vtkIndent indent)
{
  vtkDataSetToPolyDataFilter::PrintSelf(os,indent);

  os << indent << "Distance: " << this->Distance << "\n";
}

<commit_msg>Changed output to GetOutput()<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkVectorTopology.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

Copyright (c) 1993-2000 Ken Martin, Will Schroeder, Bill Lorensen
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.

 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.

 * Neither name of Ken Martin, Will Schroeder, or Bill Lorensen nor the names
   of any contributors may be used to endorse or promote products derived
   from this software without specific prior written permission.

 * Modified source versions must be plainly marked as such, and must not be
   misrepresented as being the original software.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS''
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

=========================================================================*/
#include <math.h>
#include "vtkVectorTopology.h"
#include "vtkObjectFactory.h"

//------------------------------------------------------------------------------
vtkVectorTopology* vtkVectorTopology::New()
{
  // First try to create the object from the vtkObjectFactory
  vtkObject* ret = vtkObjectFactory::CreateInstance("vtkVectorTopology");
  if(ret)
    {
    return (vtkVectorTopology*)ret;
    }
  // If the factory was unable to create the object, then create it here.
  return new vtkVectorTopology;
}

// Construct object with distance 0.1.
vtkVectorTopology::vtkVectorTopology()
{
  this->Distance = 0.1;
}

void vtkVectorTopology::Execute()
{
  int cellId, i, j, ptId, npts;
  int negative[3], positive[3], subId=0;
  float x[3], pcoords[3], *v;
  vtkCell *cell;
  vtkVectors *inVectors;
  vtkPoints *newPts;
  vtkCellArray *newVerts;
  vtkDataSet *input=(vtkDataSet *)this->GetInput();
  vtkPointData *pd=input->GetPointData();
  vtkPolyData *output=(vtkPolyData *)this->GetOutput();
  vtkPointData *outputPD=output->GetPointData();
  float *weights=new float[input->GetMaxCellSize()];
//
// Initialize self; check input; create output objects
//
  vtkDebugMacro(<< "Executing vector topology...");

  // make sure we have vector data
  if ( ! (inVectors = input->GetPointData()->GetVectors()) )
    {
    vtkErrorMacro(<<"No vector data, can't create topology markers...");
    return;
    }

  newPts = vtkPoints::New();
  newPts->Allocate(100);
  newVerts = vtkCellArray::New();
  newVerts->Allocate(newVerts->EstimateSize(1,100));
  outputPD->CopyAllocate(pd);
//
// Find cells whose vector components all pass through zero
//
  pcoords[0] = pcoords[1] = pcoords[2] = 0.5;
  newVerts->InsertNextCell(100); //temporary count
  for (cellId=0; cellId<input->GetNumberOfCells(); cellId++)
    {
    cell = input->GetCell(cellId);
    npts = cell->GetNumberOfPoints();
    for (i=0; i<3; i++)
      {
      negative[i] = positive[i] = 0;
      }
    for (i=0; i < npts; i++)
      {
      ptId = cell->GetPointId(i);
      v = inVectors->GetVector(ptId);
      for (j=0; j<3; j++)
        {
        if ( v[j] < 0.0 )
	  {
	  negative[j] = 1;
	  }
        else if ( v[j] >= 0.0 )
	  {
	  positive[j] = 1;
	  }
        }
      }
    if ( negative[0] && positive[0] && negative[1] && positive[1] &&
    negative[2] && positive[2] )
      { // place point at center of cell
      cell->EvaluateLocation(subId, pcoords, x, weights);
      ptId = newPts->InsertNextPoint(x);
      newVerts->InsertCellPoint(ptId);
      }
    }
  newVerts->UpdateCellCount(newPts->GetNumberOfPoints());

  vtkDebugMacro(<< "Created " << newPts->GetNumberOfPoints() << "points");
  delete [] weights;
//
// Update ourselves
//
  output->SetPoints(newPts);
  output->SetVerts(newVerts);
  output->Squeeze();
}

void vtkVectorTopology::PrintSelf(ostream& os, vtkIndent indent)
{
  vtkDataSetToPolyDataFilter::PrintSelf(os,indent);

  os << indent << "Distance: " << this->Distance << "\n";
}

<|endoftext|>


<commit_before>
#include "IPCSession.hpp"
#include "../Client.hpp"
#include <onions-common/Log.hpp>
#include <onions-common/tcp/MemAllocator.hpp>
#include <onions-common/Utils.hpp>
#include <boost/bind.hpp>
#include <algorithm>
#include <fstream>

template <typename Handler>
inline MemAllocator<Handler> makeHandler(HandleAlloc& a, Handler h)
{
  return MemAllocator<Handler>(a, h);
}

IPCSession::IPCSession(boost::asio::io_service& ios) : socket_(ios)
{
}

boost::asio::ip::tcp::socket& IPCSession::getSocket()
{
  return socket_;
}

void IPCSession::start()
{
  asyncReadBuffer();
}

void IPCSession::processRead(const boost::system::error_code& error, size_t n)
{
  if (error || n <= 0)
  {
    Log::get().warn("IPC processRead: " + error.message());
    return;
  }

  std::string domainIn(buffer_.begin(), buffer_.begin() + n);
  Log::get().notice("Read \"" + domainIn + "\" from Tor Browser.");
  std::string onionOut = Client::get().resolve(domainIn);
  Log::get().notice("Writing \"" + onionOut + "\" to Tor Browser... ");

  for (std::size_t j = 0; j < onionOut.size(); j++)
    buffer_[j] = onionOut[j];
  asyncWriteBuffer(onionOut.size());
}

// called by Asio when the buffer has been written to the socket
void IPCSession::processWrite(const boost::system::error_code& error)
{
  if (error)
  {
    Log::get().warn("IPC processWrite: " + error.message());
    return;
  }

  Log::get().notice("Write complete.");
  asyncReadBuffer();
}

// ***************************** PRIVATE METHODS *****************************

void IPCSession::asyncReadBuffer()
{
  socket_.async_read_some(
      boost::asio::buffer(buffer_),
      makeHandler(allocator_,
                  boost::bind(&IPCSession::processRead, shared_from_this(),
                              boost::asio::placeholders::error,
                              boost::asio::placeholders::bytes_transferred)));
}

void IPCSession::asyncWriteBuffer(std::size_t len)
{
  boost::asio::async_write(
      socket_, boost::asio::buffer(buffer_, len),
      makeHandler(allocator_,
                  boost::bind(&IPCSession::processWrite, shared_from_this(),
                              boost::asio::placeholders::error)));
}
<commit_msg>trim IPC read<commit_after>
#include "IPCSession.hpp"
#include "../Client.hpp"
#include <onions-common/Log.hpp>
#include <onions-common/tcp/MemAllocator.hpp>
#include <onions-common/Utils.hpp>
#include <boost/bind.hpp>
#include <algorithm>
#include <fstream>

template <typename Handler>
inline MemAllocator<Handler> makeHandler(HandleAlloc& a, Handler h)
{
  return MemAllocator<Handler>(a, h);
}

IPCSession::IPCSession(boost::asio::io_service& ios) : socket_(ios)
{
}

boost::asio::ip::tcp::socket& IPCSession::getSocket()
{
  return socket_;
}

void IPCSession::start()
{
  asyncReadBuffer();
}

void IPCSession::processRead(const boost::system::error_code& error, size_t n)
{
  if (error || n <= 0)
  {
    Log::get().warn("IPC " + error.message());
    return;
  }

  std::string domainIn(buffer_.begin(), buffer_.begin() + n);
  domainIn = Utils::trimString(domainIn);
  Log::get().notice("Read \"" + domainIn + "\" from Tor Browser.");
  std::string onionOut = Client::get().resolve(domainIn);
  Log::get().notice("Writing \"" + onionOut + "\" to Tor Browser... ");

  for (std::size_t j = 0; j < onionOut.size(); j++)
    buffer_[j] = onionOut[j];
  asyncWriteBuffer(onionOut.size());
}

// called by Asio when the buffer has been written to the socket
void IPCSession::processWrite(const boost::system::error_code& error)
{
  if (error)
  {
    Log::get().warn("IPC processWrite: " + error.message());
    return;
  }

  Log::get().notice("Write complete.");
  asyncReadBuffer();
}

// ***************************** PRIVATE METHODS *****************************

void IPCSession::asyncReadBuffer()
{
  socket_.async_read_some(
      boost::asio::buffer(buffer_),
      makeHandler(allocator_,
                  boost::bind(&IPCSession::processRead, shared_from_this(),
                              boost::asio::placeholders::error,
                              boost::asio::placeholders::bytes_transferred)));
}

void IPCSession::asyncWriteBuffer(std::size_t len)
{
  boost::asio::async_write(
      socket_, boost::asio::buffer(buffer_, len),
      makeHandler(allocator_,
                  boost::bind(&IPCSession::processWrite, shared_from_this(),
                              boost::asio::placeholders::error)));
}
<|endoftext|>


<commit_before>#include <ros/console.h>
#include <string>

#include <teraranger_hub/RangeArray.h>
#include <teraranger_hub/teraranger_evo.h>
#include <teraranger_hub/helper_lib.h>

namespace teraranger_hub
{

TerarangerHubEvo::TerarangerHubEvo()
{
    // Get parameters and namespace
    ros::NodeHandle private_node_handle_("~");
    private_node_handle_.param("portname", portname_,
                               std::string("/dev/ttyACM0"));
    ns_ = ros::this_node::getNamespace();
    ns_ = ros::names::clean(ns_);
    ROS_INFO("node namespace: [%s]", ns_.c_str());

    // Publishers
    range_publisher_ = nh_.advertise<teraranger_hub::RangeArray>("teraranger_evo", 8);

    // Serial Port init
    serial_port_ = new SerialPort();
    if (!serial_port_->connect(portname_)) {
      ROS_ERROR("Could not open : %s ", portname_.c_str());
      ros::shutdown();
      return;
    }
    else {
      serial_data_callback_function_ =
          boost::bind(&TerarangerHubEvo::serialDataCallback, this, _1);
      serial_port_->setSerialCallbackFunction(&serial_data_callback_function_);
    }

    // Output loaded parameters to console for double checking
    ROS_INFO("[%s] is up and running with the following parameters:",
             ros::this_node::getName().c_str());
    ROS_INFO("[%s] portname: %s", ros::this_node::getName().c_str(),
             portname_.c_str());

    //Initialize local parameters and measurement array
    field_of_view = 0.0593;
    max_range = 14.0;
    min_range = 0.2;
    number_of_sensor = 8;
    frame_id = "base_range_";

    // Initialize rangeArray
    for (size_t i=0; i < number_of_sensor; i++)
    {
     sensor_msgs::Range range;
     range.field_of_view = field_of_view;
     range.max_range = max_range;
     range.min_range = min_range;
     range.radiation_type = sensor_msgs::Range::INFRARED;
     range.range = 0.0;
     // set the right range frame depending of the namespace
     if (ns_ == ""){
       range.header.frame_id = frame_id + boost::lexical_cast<std::string>(i);
     }
     else{
       range.header.frame_id = ns_.erase(0,1) + '_'+ frame_id + boost::lexical_cast<std::string>(i);
     }
     measure.ranges.push_back(range);
   }

   // set the right RangeArray frame depending of the namespace
   if (ns_ == ""){
     measure.header.frame_id = "base_hub";
   }
   else{
     measure.header.frame_id = "base_" + ns_.erase(0,1);// Remove first slash
   }

    // This line is needed to start measurements on the hub
    setMode(ENABLE_CMD, 5);
    setMode(BINARY_MODE, 4);
    setMode(TOWER_MODE, 4);
    setMode(RATE_ASAP, 5);

    // Dynamic reconfigure
    dyn_param_server_callback_function_ =
        boost::bind(&TerarangerHubEvo::dynParamCallback, this, _1, _2);
    dyn_param_server_.setCallback(dyn_param_server_callback_function_);
  }

  TerarangerHubEvo::~TerarangerHubEvo() {}

  void TerarangerHubEvo::setMode(const char *c, int length) {
    serial_port_->sendChar(c, length);
  }

  void TerarangerHubEvo::dynParamCallback(
      const teraranger_evo_cfg::TerarangerHubEvoConfig &config, uint32_t level) {

    // Set the mode dynamically
    if (config.Mode == teraranger_evo_cfg::TerarangerHubEvo_Binary) {
      setMode(BINARY_MODE, 4);
    }
    if (config.Mode == teraranger_evo_cfg::TerarangerHubEvo_Text) {
      setMode(TEXT_MODE, 4);
    }

    // Set the rate dynamically
    if (config.Rate == teraranger_evo_cfg::TerarangerHubEvo_ASAP){
      setMode(RATE_ASAP, 5);
    }
    if (config.Rate == teraranger_evo_cfg::TerarangerHubEvo_700){
      setMode(RATE_700, 5);
    }
    if (config.Rate == teraranger_evo_cfg::TerarangerHubEvo_600){
      setMode(RATE_600, 5);
    }
    if (config.Rate == teraranger_evo_cfg::TerarangerHubEvo_500){
      setMode(RATE_500, 5);
    }
    if (config.Rate == teraranger_evo_cfg::TerarangerHubEvo_250){
      setMode(RATE_250, 5);
    }
    if (config.Rate == teraranger_evo_cfg::TerarangerHubEvo_100){
      setMode(RATE_100, 5);
    }
    if (config.Rate == teraranger_evo_cfg::TerarangerHubEvo_50){
      setMode(RATE_50, 5);
    }

  }

  void TerarangerHubEvo::serialDataCallback(uint8_t single_character) {
    static uint8_t input_buffer[BUFFER_SIZE];
    static int buffer_ctr = 0;
    static int seq_ctr = 0;

    ROS_DEBUG("Buffer of size %d : %s | current char : %c", buffer_ctr, input_buffer, (char)single_character);

    if (single_character == 'T' && buffer_ctr == 0) {
      // Waiting for T
      input_buffer[buffer_ctr++] = single_character;
      return;
    }
    else if (single_character == 'H' && buffer_ctr == 1) {
      // Waiting for H after a T
      input_buffer[buffer_ctr++] = single_character;
      return;
    }

    // Gathering after-header range data
    if (buffer_ctr > 1 && buffer_ctr < RANGES_FRAME_LENGTH) {
      input_buffer[buffer_ctr++] = single_character;
      return;
    }
    else if (buffer_ctr == RANGES_FRAME_LENGTH) {
      //Processing full range frame
      int16_t crc = crc8(input_buffer, 19);

      if (crc == input_buffer[RANGE_CRC_POS]) {
          //ROS_DEBUG("Frame of size %d : %s ", buffer_ctr, input_buffer);

          for (size_t i=0; i < measure.ranges.size(); i++) {
            measure.ranges.at(i).header.stamp = ros::Time::now();
            measure.ranges.at(i).header.seq = seq_ctr++;

            // Doesn't go out of range because of fixed buffer size as long as the number of sensor is not above 8
            char c1 = input_buffer[2 * (i + 1)];
            char c2 = input_buffer[2 * (i + 1) + 1];
            ROS_DEBUG("c1 : %x, c2 : %x", (c1 & 0x0FF), (c2 & 0x0FF));
            int16_t current_range = (c1 & 0x0FF) << 8;
            current_range |= (c2 & 0x0FF);

            float float_range = (float)current_range;
            ROS_DEBUG("Value int : %d | float : %f", current_range, float_range);

            if (current_range <= 1 || current_range == 255) {
              float_range = -1.0;
            } else {
              float_range = float_range * 0.001;
            }
            measure.ranges.at(i).range = float_range;
          }
          measure.header.seq = (int) seq_ctr / 8;
          measure.header.stamp = ros::Time::now();
          range_publisher_.publish(measure);
      } else {
        ROS_DEBUG("[%s] crc missmatch", ros::this_node::getName().c_str());
      }
    } else if (buffer_ctr > RANGES_FRAME_LENGTH){
        ROS_DEBUG("[%s] : Buffer overflow, resetting buffer without "
                  "evaluating data",
                  ros::this_node::getName().c_str());
    }
    // resetting buffer and ctr
    buffer_ctr = 0;
    bzero(&input_buffer, BUFFER_SIZE);

    // Appending current char to hook next frame
    input_buffer[buffer_ctr++] = single_character;
}
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "teraranger_hub_evo");
  teraranger_hub::TerarangerHubEvo node;
  ros::spin();

  return 0;
}
<commit_msg>Correct crc8 function naming<commit_after>#include <ros/console.h>
#include <string>

#include <teraranger_hub/RangeArray.h>
#include <teraranger_hub/teraranger_evo.h>
#include <teraranger_hub/helper_lib.h>

namespace teraranger_hub
{

TerarangerHubEvo::TerarangerHubEvo()
{
    // Get parameters and namespace
    ros::NodeHandle private_node_handle_("~");
    private_node_handle_.param("portname", portname_,
                               std::string("/dev/ttyACM0"));
    ns_ = ros::this_node::getNamespace();
    ns_ = ros::names::clean(ns_);
    ROS_INFO("node namespace: [%s]", ns_.c_str());

    // Publishers
    range_publisher_ = nh_.advertise<teraranger_hub::RangeArray>("teraranger_evo", 8);

    // Serial Port init
    serial_port_ = new SerialPort();
    if (!serial_port_->connect(portname_)) {
      ROS_ERROR("Could not open : %s ", portname_.c_str());
      ros::shutdown();
      return;
    }
    else {
      serial_data_callback_function_ =
          boost::bind(&TerarangerHubEvo::serialDataCallback, this, _1);
      serial_port_->setSerialCallbackFunction(&serial_data_callback_function_);
    }

    // Output loaded parameters to console for double checking
    ROS_INFO("[%s] is up and running with the following parameters:",
             ros::this_node::getName().c_str());
    ROS_INFO("[%s] portname: %s", ros::this_node::getName().c_str(),
             portname_.c_str());

    //Initialize local parameters and measurement array
    field_of_view = 0.0593;
    max_range = 14.0;
    min_range = 0.2;
    number_of_sensor = 8;
    frame_id = "base_range_";

    // Initialize rangeArray
    for (size_t i=0; i < number_of_sensor; i++)
    {
     sensor_msgs::Range range;
     range.field_of_view = field_of_view;
     range.max_range = max_range;
     range.min_range = min_range;
     range.radiation_type = sensor_msgs::Range::INFRARED;
     range.range = 0.0;
     // set the right range frame depending of the namespace
     if (ns_ == ""){
       range.header.frame_id = frame_id + boost::lexical_cast<std::string>(i);
     }
     else{
       range.header.frame_id = ns_.erase(0,1) + '_'+ frame_id + boost::lexical_cast<std::string>(i);
     }
     measure.ranges.push_back(range);
   }

   // set the right RangeArray frame depending of the namespace
   if (ns_ == ""){
     measure.header.frame_id = "base_hub";
   }
   else{
     measure.header.frame_id = "base_" + ns_.erase(0,1);// Remove first slash
   }

    // This line is needed to start measurements on the hub
    setMode(ENABLE_CMD, 5);
    setMode(BINARY_MODE, 4);
    setMode(TOWER_MODE, 4);
    setMode(RATE_ASAP, 5);

    // Dynamic reconfigure
    dyn_param_server_callback_function_ =
        boost::bind(&TerarangerHubEvo::dynParamCallback, this, _1, _2);
    dyn_param_server_.setCallback(dyn_param_server_callback_function_);
  }

  TerarangerHubEvo::~TerarangerHubEvo() {}

  void TerarangerHubEvo::setMode(const char *c, int length) {
    serial_port_->sendChar(c, length);
  }

  void TerarangerHubEvo::dynParamCallback(
      const teraranger_evo_cfg::TerarangerHubEvoConfig &config, uint32_t level) {

    // Set the mode dynamically
    if (config.Mode == teraranger_evo_cfg::TerarangerHubEvo_Binary) {
      setMode(BINARY_MODE, 4);
    }
    if (config.Mode == teraranger_evo_cfg::TerarangerHubEvo_Text) {
      setMode(TEXT_MODE, 4);
    }

    // Set the rate dynamically
    if (config.Rate == teraranger_evo_cfg::TerarangerHubEvo_ASAP){
      setMode(RATE_ASAP, 5);
    }
    if (config.Rate == teraranger_evo_cfg::TerarangerHubEvo_700){
      setMode(RATE_700, 5);
    }
    if (config.Rate == teraranger_evo_cfg::TerarangerHubEvo_600){
      setMode(RATE_600, 5);
    }
    if (config.Rate == teraranger_evo_cfg::TerarangerHubEvo_500){
      setMode(RATE_500, 5);
    }
    if (config.Rate == teraranger_evo_cfg::TerarangerHubEvo_250){
      setMode(RATE_250, 5);
    }
    if (config.Rate == teraranger_evo_cfg::TerarangerHubEvo_100){
      setMode(RATE_100, 5);
    }
    if (config.Rate == teraranger_evo_cfg::TerarangerHubEvo_50){
      setMode(RATE_50, 5);
    }

  }

  void TerarangerHubEvo::serialDataCallback(uint8_t single_character) {
    static uint8_t input_buffer[BUFFER_SIZE];
    static int buffer_ctr = 0;
    static int seq_ctr = 0;

    ROS_DEBUG("Buffer of size %d : %s | current char : %c", buffer_ctr, input_buffer, (char)single_character);

    if (single_character == 'T' && buffer_ctr == 0) {
      // Waiting for T
      input_buffer[buffer_ctr++] = single_character;
      return;
    }
    else if (single_character == 'H' && buffer_ctr == 1) {
      // Waiting for H after a T
      input_buffer[buffer_ctr++] = single_character;
      return;
    }

    // Gathering after-header range data
    if (buffer_ctr > 1 && buffer_ctr < RANGES_FRAME_LENGTH) {
      input_buffer[buffer_ctr++] = single_character;
      return;
    }
    else if (buffer_ctr == RANGES_FRAME_LENGTH) {
      //Processing full range frame
      int16_t crc = HelperLib::crc8(input_buffer, 19);

      if (crc == input_buffer[RANGE_CRC_POS]) {
          //ROS_DEBUG("Frame of size %d : %s ", buffer_ctr, input_buffer);

          for (size_t i=0; i < measure.ranges.size(); i++) {
            measure.ranges.at(i).header.stamp = ros::Time::now();
            measure.ranges.at(i).header.seq = seq_ctr++;

            // Doesn't go out of range because of fixed buffer size as long as the number of sensor is not above 8
            char c1 = input_buffer[2 * (i + 1)];
            char c2 = input_buffer[2 * (i + 1) + 1];
            ROS_DEBUG("c1 : %x, c2 : %x", (c1 & 0x0FF), (c2 & 0x0FF));
            int16_t current_range = (c1 & 0x0FF) << 8;
            current_range |= (c2 & 0x0FF);

            float float_range = (float)current_range;
            ROS_DEBUG("Value int : %d | float : %f", current_range, float_range);

            if (current_range <= 1 || current_range == 255) {
              float_range = -1.0;
            } else {
              float_range = float_range * 0.001;
            }
            measure.ranges.at(i).range = float_range;
          }
          measure.header.seq = (int) seq_ctr / 8;
          measure.header.stamp = ros::Time::now();
          range_publisher_.publish(measure);
      } else {
        ROS_DEBUG("[%s] crc missmatch", ros::this_node::getName().c_str());
      }
    } else if (buffer_ctr > RANGES_FRAME_LENGTH){
        ROS_DEBUG("[%s] : Buffer overflow, resetting buffer without "
                  "evaluating data",
                  ros::this_node::getName().c_str());
    }
    // resetting buffer and ctr
    buffer_ctr = 0;
    bzero(&input_buffer, BUFFER_SIZE);

    // Appending current char to hook next frame
    input_buffer[buffer_ctr++] = single_character;
}
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "teraranger_hub_evo");
  teraranger_hub::TerarangerHubEvo node;
  ros::spin();

  return 0;
}
<|endoftext|>