<commit_before>// Part of the apollo library -- Copyright (c) Christian Neumüller 2015
// This file is subject to the terms of the BSD 2-Clause License.
// See LICENSE.txt or http://opensource.org/licenses/BSD-2-Clause

#ifndef APOLLO_CONVERTERS_HPP_INCLUDED
#define APOLLO_CONVERTERS_HPP_INCLUDED APOLLO_CONVERTERS_HPP_INCLUDED

#include <apollo/converters_fwd.hpp>
#include <apollo/error.hpp>
#include <apollo/detail/meta_util.hpp>
#include <apollo/detail/ref_binder.hpp>

#include <boost/exception/errinfo_type_info_name.hpp>
#include <boost/exception/info.hpp>
#include <boost/throw_exception.hpp>
#include <boost/type_index.hpp>
#include <apollo/lua_include.hpp>

#include <type_traits>

namespace apollo {

struct raw_function;

template <typename Converter>
using to_type_of = typename detail::remove_qualifiers<Converter>::type::to_type;

namespace detail {
// Lua type constants //

template <typename T, typename Enable=void> // Default to userdata.
struct lua_type_id: std::integral_constant<int, LUA_TUSERDATA> {};

template <typename T> // Any arithmetic type except bool is a number.
struct lua_type_id<T,
        typename std::enable_if<
            std::is_same<T, typename detail::remove_qualifiers<T>::type>::value
            && (
                std::is_arithmetic<T>::value
                || std::is_enum<T>::value)>::type>
        : std::integral_constant<int, LUA_TNUMBER> {};

template <> // boolean
struct lua_type_id<bool>: std::integral_constant<int, LUA_TBOOLEAN> {};

template <> struct lua_type_id<void>: std::integral_constant<int, LUA_TNIL> {};

template <>
struct lua_type_id<void*>: std::integral_constant<int, LUA_TLIGHTUSERDATA> {};

// string
template <>
struct lua_type_id<char*>: std::integral_constant<int, LUA_TSTRING> {};
template <> struct lua_type_id<char const*>: lua_type_id<char*> {};
template <> struct lua_type_id<char>: lua_type_id<char*> {};
template <std::size_t N> struct lua_type_id<char[N]>: lua_type_id<char*> {};
template <> struct lua_type_id<std::string>: lua_type_id<char*> {};

template <> // thread (lua_State*)
struct lua_type_id<lua_State*>: std::integral_constant<int, LUA_TTHREAD> {};

template <typename T>
struct is_plain_function: std::integral_constant<bool,
        std::is_function<typename std::remove_pointer<T>::type>::value>
{ };

// function (plain function (pointer), boost and std function templates)
template <typename T>
struct lua_type_id<T,
        typename std::enable_if<
        detail::is_plain_function<T>::value ||
        std::is_member_function_pointer<T>::value>::type>
    : std::integral_constant<int, LUA_TFUNCTION> {};

template <template <class> class FObj, typename R, typename... Args>
struct lua_type_id<FObj<R(Args...)>>
    : std::integral_constant<int, LUA_TFUNCTION> {};

template <> struct lua_type_id<raw_function>
    : std::integral_constant<int, LUA_TFUNCTION> {};

} // namespace detail

template <typename T, typename Enable=void>
struct convert_cref_by_val: std::integral_constant<bool,
    detail::lua_type_id<T>::value != LUA_TUSERDATA>
{};

// Const references to primitive types are handled as non-references.
template<typename T>
struct converter<T, typename std::enable_if<
        detail::is_const_reference<T>::value &&
        convert_cref_by_val<typename detail::remove_qualifiers<T>::type>::value
    >::type
>: converter<typename detail::remove_qualifiers<T>::type>
{};

template <typename T>
using push_converter_for = converter<
    typename detail::remove_qualifiers<T>::type>;

template <typename T>
using pull_converter_for = converter<typename std::remove_cv<T>::type>;

namespace detail {

inline int push_impl(lua_State*)
{
    return 0;
}

template <typename Head, typename... Tail>
int push_impl(lua_State* L, Head&& head, Tail&&... tail)
{
    int const n_pushed = push_converter_for<Head>().push(
        L, std::forward<Head>(head));
    return n_pushed + push_impl(L, std::forward<Tail>(tail)...);
}

} // namespace detail

template <typename T, typename... MoreTs>
int push(lua_State* L, T&& v, MoreTs&&... more)
{
    return detail::push_impl(L,
        std::forward<T>(v), std::forward<MoreTs>(more)...);
}

template <typename Converter>
to_type_of<Converter> unchecked_to_with(
    Converter&& conv, lua_State* L, int idx, int* next_idx = nullptr)
{
    return conv.idx_to(L, idx, next_idx);
}

template <typename T>
to_type_of<pull_converter_for<T>> unchecked_to(lua_State* L, int idx)
{
    return unchecked_to_with(pull_converter_for<T>(), L, idx);
}

template <typename Converter>
unsigned n_conversion_steps_with(
    Converter&& conv, lua_State* L, int idx, int* next_idx = nullptr)
{
    return conv.idx_n_conversion_steps(L, idx, next_idx);
}

template <typename T>
unsigned n_conversion_steps(lua_State* L, int idx)
{
    return n_conversion_steps_with(pull_converter_for<T>(), L, idx);
}

template <typename Converter>
bool is_convertible_with(Converter const& conv, lua_State* L, int idx)
{
    (void)conv; // Silence MSVC.
    return n_conversion_steps_with(conv, L, idx) != no_conversion;
}

template <typename T>
bool is_convertible(lua_State* L, int idx)
{
    return is_convertible_with(pull_converter_for<T>(), L, idx);
}

template <typename Converter>
to_type_of<Converter> to_with(
    Converter&& conv, lua_State* L, int idx, int* next_idx = nullptr)
{
    try {
        return std::forward<Converter>(conv).idx_safe_to(L, idx, next_idx);
    } catch (to_cpp_conversion_error& e) {
        err_supplement(e, boost::errinfo_type_info_name(
            boost::typeindex::type_id<to_type_of<Converter>>()
            .pretty_name()));
        err_supplement(e, errinfo::msg("conversion from Lua to C++ failed"));
        err_supplement(e, errinfo::stack_index(idx));
        err_supplement(e, errinfo::lua_state(L));
        throw;
    }
}

template <typename T>
to_type_of<pull_converter_for<T>> to(lua_State* L, int idx)
{
    return to_with(pull_converter_for<T>(), L, idx);
}

// Cannot use function here, it would potentially return a reference to local.
#define APOLLO_TO_ARG(L, idx, ...) \
    ::apollo::unwrap_ref(::apollo::to<__VA_ARGS__>(L, idx))

template <typename T>
to_type_of<pull_converter_for<T>> to(lua_State* L, int idx, T&& fallback)
{
    if (!is_convertible<T>(L, idx))
        return std::forward<T>(fallback);
    return unchecked_to<T>(L, idx);
}

} // namepace apollo

#endif // APOLLO_CONVERTERS_HPP_INCLUDED
<commit_msg>Handle convert_cref_by_val in pull_converter_for.<commit_after>// Part of the apollo library -- Copyright (c) Christian Neumüller 2015
// This file is subject to the terms of the BSD 2-Clause License.
// See LICENSE.txt or http://opensource.org/licenses/BSD-2-Clause

#ifndef APOLLO_CONVERTERS_HPP_INCLUDED
#define APOLLO_CONVERTERS_HPP_INCLUDED APOLLO_CONVERTERS_HPP_INCLUDED

#include <apollo/converters_fwd.hpp>
#include <apollo/error.hpp>
#include <apollo/detail/meta_util.hpp>
#include <apollo/detail/ref_binder.hpp>

#include <boost/exception/errinfo_type_info_name.hpp>
#include <boost/exception/info.hpp>
#include <boost/throw_exception.hpp>
#include <boost/type_index.hpp>
#include <apollo/lua_include.hpp>

#include <type_traits>

namespace apollo {

struct raw_function;

template <typename Converter>
using to_type_of = typename detail::remove_qualifiers<Converter>::type::to_type;

namespace detail {
// Lua type constants //

template <typename T, typename Enable=void> // Default to userdata.
struct lua_type_id: std::integral_constant<int, LUA_TUSERDATA> {};

template <typename T> // Any arithmetic type except bool is a number.
struct lua_type_id<T,
        typename std::enable_if<
            std::is_same<T, typename detail::remove_qualifiers<T>::type>::value
            && (
                std::is_arithmetic<T>::value
                || std::is_enum<T>::value)>::type>
        : std::integral_constant<int, LUA_TNUMBER> {};

template <> // boolean
struct lua_type_id<bool>: std::integral_constant<int, LUA_TBOOLEAN> {};

template <> struct lua_type_id<void>: std::integral_constant<int, LUA_TNIL> {};

template <>
struct lua_type_id<void*>: std::integral_constant<int, LUA_TLIGHTUSERDATA> {};

// string
template <>
struct lua_type_id<char*>: std::integral_constant<int, LUA_TSTRING> {};
template <> struct lua_type_id<char const*>: lua_type_id<char*> {};
template <> struct lua_type_id<char>: lua_type_id<char*> {};
template <std::size_t N> struct lua_type_id<char[N]>: lua_type_id<char*> {};
template <> struct lua_type_id<std::string>: lua_type_id<char*> {};

template <> // thread (lua_State*)
struct lua_type_id<lua_State*>: std::integral_constant<int, LUA_TTHREAD> {};

template <typename T>
struct is_plain_function: std::integral_constant<bool,
        std::is_function<typename std::remove_pointer<T>::type>::value>
{ };

// function (plain function (pointer), boost and std function templates)
template <typename T>
struct lua_type_id<T,
        typename std::enable_if<
        detail::is_plain_function<T>::value ||
        std::is_member_function_pointer<T>::value>::type>
    : std::integral_constant<int, LUA_TFUNCTION> {};

template <template <class> class FObj, typename R, typename... Args>
struct lua_type_id<FObj<R(Args...)>>
    : std::integral_constant<int, LUA_TFUNCTION> {};

template <> struct lua_type_id<raw_function>
    : std::integral_constant<int, LUA_TFUNCTION> {};

} // namespace detail

template <typename T, typename Enable=void>
struct convert_cref_by_val: std::integral_constant<bool,
    detail::lua_type_id<T>::value != LUA_TUSERDATA>
{};

template <typename T>
using push_converter_for = converter<
    typename detail::remove_qualifiers<T>::type>;

// pull_converter_for is implemented via helper struct to not overexert MSVC.
namespace detail {
    template <typename T>
    struct pull_converter_for_impl {
    private:
        using transformed_t = typename std::conditional<
            detail::is_const_reference<T>::value
            && convert_cref_by_val<
                typename detail::remove_qualifiers<T>::type>::value,
            typename detail::remove_qualifiers<T>::type,
            typename std::remove_cv<T>::type>::type;
    public:
        using type = converter<transformed_t>;
    };
}

template <typename T>
using pull_converter_for = typename detail::pull_converter_for_impl<T>::type;

namespace detail {

inline int push_impl(lua_State*)
{
    return 0;
}

template <typename Head, typename... Tail>
int push_impl(lua_State* L, Head&& head, Tail&&... tail)
{
    int const n_pushed = push_converter_for<Head>().push(
        L, std::forward<Head>(head));
    return n_pushed + push_impl(L, std::forward<Tail>(tail)...);
}

} // namespace detail

template <typename T, typename... MoreTs>
int push(lua_State* L, T&& v, MoreTs&&... more)
{
    return detail::push_impl(L,
        std::forward<T>(v), std::forward<MoreTs>(more)...);
}

template <typename Converter>
to_type_of<Converter> unchecked_to_with(
    Converter&& conv, lua_State* L, int idx, int* next_idx = nullptr)
{
    return conv.idx_to(L, idx, next_idx);
}

template <typename T>
to_type_of<pull_converter_for<T>> unchecked_to(lua_State* L, int idx)
{
    return unchecked_to_with(pull_converter_for<T>(), L, idx);
}

template <typename Converter>
unsigned n_conversion_steps_with(
    Converter&& conv, lua_State* L, int idx, int* next_idx = nullptr)
{
    return conv.idx_n_conversion_steps(L, idx, next_idx);
}

template <typename T>
unsigned n_conversion_steps(lua_State* L, int idx)
{
    return n_conversion_steps_with(pull_converter_for<T>(), L, idx);
}

template <typename Converter>
bool is_convertible_with(Converter const& conv, lua_State* L, int idx)
{
    (void)conv; // Silence MSVC.
    return n_conversion_steps_with(conv, L, idx) != no_conversion;
}

template <typename T>
bool is_convertible(lua_State* L, int idx)
{
    return is_convertible_with(pull_converter_for<T>(), L, idx);
}

template <typename Converter>
to_type_of<Converter> to_with(
    Converter&& conv, lua_State* L, int idx, int* next_idx = nullptr)
{
    try {
        return std::forward<Converter>(conv).idx_safe_to(L, idx, next_idx);
    } catch (to_cpp_conversion_error& e) {
        err_supplement(e, boost::errinfo_type_info_name(
            boost::typeindex::type_id<to_type_of<Converter>>()
            .pretty_name()));
        err_supplement(e, errinfo::msg("conversion from Lua to C++ failed"));
        err_supplement(e, errinfo::stack_index(idx));
        err_supplement(e, errinfo::lua_state(L));
        throw;
    }
}

template <typename T>
to_type_of<pull_converter_for<T>> to(lua_State* L, int idx)
{
    return to_with(pull_converter_for<T>(), L, idx);
}

// Cannot use function here, it would potentially return a reference to local.
#define APOLLO_TO_ARG(L, idx, ...) \
    ::apollo::unwrap_ref(::apollo::to<__VA_ARGS__>(L, idx))

template <typename T>
to_type_of<pull_converter_for<T>> to(lua_State* L, int idx, T&& fallback)
{
    if (!is_convertible<T>(L, idx))
        return std::forward<T>(fallback);
    return unchecked_to<T>(L, idx);
}

} // namepace apollo

#endif // APOLLO_CONVERTERS_HPP_INCLUDED
<|endoftext|>


<commit_before>/**
 * Clever programming language
 * Copyright (c) Clever Team
 *
 * This file is distributed under the MIT license. See LICENSE for details.
 */

#include <iostream>
#include <cstdio>
#include <cstdlib>
#include "core/cthread.h"
#include "core/value.h"
#include "modules/std/core/function.h"
#include "core/modmanager.h"
#include "modules/std/io/io.h"
#include "modules/std/io/serializer.h"

namespace clever { namespace modules { namespace std {

namespace io {

CMutex g_io_mutex;
// flush(void)
// Flushes output buffer (forcefully)
static CLEVER_FUNCTION(flush)
{
	if (!clever_static_check_no_args()) {
		return;
	}
	g_io_mutex.lock();
	::fflush(stdout);
	g_io_mutex.unlock();
}

// print(object a, [ ...])
// Prints the object values without trailing newline
static CLEVER_FUNCTION(print)
{
	g_io_mutex.lock();
	for (size_t i = 0, size = args.size(); i < size; ++i) {
		args[i]->dump();
	}
	g_io_mutex.unlock();
}

// println(object a, [ ...])
// Prints the object values with trailing newline
static CLEVER_FUNCTION(println)
{
	g_io_mutex.lock();
	for (size_t i = 0, size = args.size(); i < size; ++i) {
		args[i]->dump();
		::std::cout << '\n';
	}
	g_io_mutex.unlock();
}

// printf(string format, [...])
// Prints and formats a string to standard output without trailing newline
static CLEVER_FUNCTION(printf)
{
	g_io_mutex.lock();
	const CString* format = args[0]->getStr();
	const char* start = format->c_str();

	for (const char* point = start; point < (start + format->size());) {
		if (*point && (*point == (char)'\\')) {
			unsigned long arg;
			char* skip;

			if ((arg=::std::strtoul(++point, &skip, 10))) {
				if (args.size() > arg) {
					args[arg]->dump();
				}
				point = skip;
			} else {
				::std::cout << *(--point);
				point++;
			}
		} else {
			::std::cout << *point;
			point++;
		}
	}
	g_io_mutex.unlock();
}

// read()
// Reads the next string from the standard stream
static CLEVER_FUNCTION(read)
{
	// TODO(murilov): Trigger error/exception
	if (!clever_static_check_no_args()) {
		return;
	}

	::std::string var;
	::std::cin >> var;

	result->setStr(new StrObject(var));
}

// readi()
// Reads the next integer from the standard stream
static CLEVER_FUNCTION(readi)
{
	// TODO(murilov): Trigger error/exception
	if (!clever_static_check_no_args()) {
		return;
	}

	long var;
	::std::cin >> var;

	result->setInt(var);
}

// readd()
// Reads the next double from the standard stream
static CLEVER_FUNCTION(readd)
{
	// TODO(murilov): Trigger error/exception
	if (!clever_static_check_no_args()) {
		return;
	}

	double var;
	::std::cin >> var;

	result->setDouble(var);
}

} // clever::modules::std::io

/// Initializes Standard module
CLEVER_MODULE_INIT(IOModule)
{
	addType(new Serializer);

	addFunction(new Function("print", &CLEVER_NS_FNAME(io, print)))
		->setVariadic();

	addFunction(new Function("println", &CLEVER_NS_FNAME(io, println)))
		->setVariadic();

	addFunction(new Function("printf", &CLEVER_NS_FNAME(io, printf)))
		->setVariadic();

	addFunction(new Function("flush", &CLEVER_NS_FNAME(io, flush)));

	addFunction(new Function("read", &CLEVER_NS_FNAME(io, read)));

	addFunction(new Function("readi", &CLEVER_NS_FNAME(io, readi)));

	addFunction(new Function("readd", &CLEVER_NS_FNAME(io, readd)));
}

}}} // clever::modules::std
<commit_msg>- Removed uneeded mutex usage<commit_after>/**
 * Clever programming language
 * Copyright (c) Clever Team
 *
 * This file is distributed under the MIT license. See LICENSE for details.
 */

#include <iostream>
#include <cstdio>
#include <cstdlib>
#include "core/cthread.h"
#include "core/value.h"
#include "modules/std/core/function.h"
#include "core/modmanager.h"
#include "modules/std/io/io.h"
#include "modules/std/io/serializer.h"

namespace clever { namespace modules { namespace std {

namespace io {

CMutex g_io_mutex;
// flush(void)
// Flushes output buffer (forcefully)
static CLEVER_FUNCTION(flush)
{
	if (!clever_static_check_no_args()) {
		return;
	}
	g_io_mutex.lock();
	::fflush(stdout);
	g_io_mutex.unlock();
}

// print(object a, [ ...])
// Prints the object values without trailing newline
static CLEVER_FUNCTION(print)
{
	for (size_t i = 0, size = args.size(); i < size; ++i) {
		args[i]->dump();
	}
}

// println(object a, [ ...])
// Prints the object values with trailing newline
static CLEVER_FUNCTION(println)
{
	for (size_t i = 0, size = args.size(); i < size; ++i) {
		args[i]->dump();
		::std::cout << '\n';
	}
}

// printf(string format, [...])
// Prints and formats a string to standard output without trailing newline
static CLEVER_FUNCTION(printf)
{
	const CString* format = args[0]->getStr();
	const char* start = format->c_str();

	for (const char* point = start; point < (start + format->size());) {
		if (*point && (*point == (char)'\\')) {
			unsigned long arg;
			char* skip;

			if ((arg=::std::strtoul(++point, &skip, 10))) {
				if (args.size() > arg) {
					args[arg]->dump();
				}
				point = skip;
			} else {
				::std::cout << *(--point);
				point++;
			}
		} else {
			::std::cout << *point;
			point++;
		}
	}
}

// read()
// Reads the next string from the standard stream
static CLEVER_FUNCTION(read)
{
	// TODO(murilov): Trigger error/exception
	if (!clever_static_check_no_args()) {
		return;
	}

	::std::string var;
	::std::cin >> var;

	result->setStr(new StrObject(var));
}

// readi()
// Reads the next integer from the standard stream
static CLEVER_FUNCTION(readi)
{
	// TODO(murilov): Trigger error/exception
	if (!clever_static_check_no_args()) {
		return;
	}

	long var;
	::std::cin >> var;

	result->setInt(var);
}

// readd()
// Reads the next double from the standard stream
static CLEVER_FUNCTION(readd)
{
	// TODO(murilov): Trigger error/exception
	if (!clever_static_check_no_args()) {
		return;
	}

	double var;
	::std::cin >> var;

	result->setDouble(var);
}

} // clever::modules::std::io

/// Initializes Standard module
CLEVER_MODULE_INIT(IOModule)
{
	addType(new Serializer);

	addFunction(new Function("print",   &CLEVER_NS_FNAME(io, print)))->setVariadic();
	addFunction(new Function("println", &CLEVER_NS_FNAME(io, println)))->setVariadic();
	addFunction(new Function("printf",  &CLEVER_NS_FNAME(io, printf)))->setVariadic();
	addFunction(new Function("flush",   &CLEVER_NS_FNAME(io, flush)));
	addFunction(new Function("read",    &CLEVER_NS_FNAME(io, read)));
	addFunction(new Function("readi",   &CLEVER_NS_FNAME(io, readi)));
	addFunction(new Function("readd",   &CLEVER_NS_FNAME(io, readd)));
}

}}} // clever::modules::std
<|endoftext|>


<commit_before>/************************************************
Post processor using fanuc_post_processor_library.
This file which operate post processor represents
one node of the entire demonstrator
************************************************/

// ROS headers
#include <ros/ros.h>
#include <ros/package.h>
#include <ros/service.h>
#include <tf_conversions/tf_eigen.h>
#include <eigen_conversions/eigen_msg.h>
#include <geometry_msgs/Transform.h>
#include "fanuc_post_processor_library/fanuc_post_processor_library.hpp"

#include <fanuc_grinding_post_processor/PostProcessorService.h> // Description of the Service we will use

boost::shared_ptr<ros::NodeHandle> node;

/**
 * This is the service function that is called whenever a request is received
 * @param req[int]
 * @param res[out]
 * @return Always true
 */
bool postProcessor(fanuc_grinding_post_processor::PostProcessorService::Request &req,
                   fanuc_grinding_post_processor::PostProcessorService::Response &res)
{
  // Get parameters from the message and print them
  //ROS_INFO_STREAM(std::endl << req);

  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > robot_poses_eigen;

  for (geometry_msgs::Pose tmp : req.RobotPoses)
  {
    Eigen::Isometry3d pose;
    tf::poseMsgToEigen(tmp, pose);
    pose.translate(Eigen::Vector3d(0, 0, req.TrajectoryZOffset));
    robot_poses_eigen.push_back(pose);
  }

  if (robot_poses_eigen.empty())
  {
    res.ReturnStatus = false;
    res.ReturnMessage = "Trajectory is empty";
    return true;
  }

  if (robot_poses_eigen.size() != req.IsGrindingPose.size())
  {
    res.ReturnStatus = false;
    res.ReturnMessage = "Trajectory is not correct";
    return true;
  }

  FanucPostProcessor fanuc_pp;
  fanuc_pp.setProgramName(req.ProgramName);
  fanuc_pp.setProgramComment(req.Comment);

  unsigned int speed;
  const double grinding_disk_DO(3);

  // First pose is a machining pose, we have to switch on the DO
  fanuc_pp.appendDigitalOutput(grinding_disk_DO, true);
  fanuc_pp.appendWait(1);
  speed = req.MachiningSpeed * 100;
  fanuc_pp.appendPoseCNT(FanucPostProcessor::LINEAR, robot_poses_eigen[0], 1, speed, FanucPostProcessor::CM_MIN, 100);

  for (unsigned i = 1; i < robot_poses_eigen.size(); ++i)
  {
    // if the new point is an extrication point and the old one was a machining point, we have to switch off the DO
    if (req.IsGrindingPose[i] == 0 && req.IsGrindingPose[i - 1] == 1)
    {
      fanuc_pp.appendDigitalOutput(grinding_disk_DO, false);
      fanuc_pp.appendWait(1);
      speed = req.ExtricationSpeed * 100;
    }
    // if the new point is an machining point and the old one was a extrication point, we have to switch on the DO
    else if (req.IsGrindingPose[i] == 1 && req.IsGrindingPose[i - 1] == 0)
    {
      fanuc_pp.appendDigitalOutput(grinding_disk_DO, true);
      fanuc_pp.appendWait(1);
      speed = req.MachiningSpeed * 100;
    }
    fanuc_pp.appendPoseCNT(FanucPostProcessor::LINEAR, robot_poses_eigen[i], i + 1, speed, FanucPostProcessor::CM_MIN, 100);
  }

  std::string program;
  fanuc_pp.generateProgram(program);
  ROS_INFO_STREAM("This is the generated program:\n\n" << program);

  // TODO: Check if writing in a .ls file works
  std::ofstream tp_program_file;
  std::string file_location = req.ProgramLocation + req.ProgramName;

  tp_program_file.open(file_location.c_str(), std::ios::out);
  // We check if file have been opened correctly
  if (tp_program_file.bad())
  {
    res.ReturnStatus = false;
    res.ReturnMessage = "Cannot open/create TP program file";
    return true;
  }

  tp_program_file << program; // We put program into TP program file
  tp_program_file.close();

  if (!req.Upload)
  {
    res.ReturnStatus = true;
    res.ReturnMessage = "Program generated";
    return true;
  }

  // TODO: Try to upload
  ROS_INFO_STREAM("Try to upload program on IP address: " << req.IpAdress);
  if (!fanuc_pp.uploadToFtp(req.IpAdress))
  {
    res.ReturnStatus = false;
    res.ReturnMessage = "Cannot upload the program on IP address: " + req.IpAdress;
    return true;
  }

  res.ReturnStatus = true;
  res.ReturnMessage = "Program generated and uploaded";
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "post_processor");
  node.reset(new ros::NodeHandle);

  // Create service server and wait for incoming requests
  ros::ServiceServer service = node->advertiseService("post_processor_service", postProcessor);
  ros::AsyncSpinner spinner(1);
  spinner.start();

  while (node->ok())
  {
  }

  spinner.stop();
  return 0;
}
<commit_msg>Switch to JOINT positions<commit_after>/************************************************
Post processor using fanuc_post_processor_library.
This file which operate post processor represents
one node of the entire demonstrator
************************************************/

// ROS headers
#include <ros/ros.h>
#include <ros/package.h>
#include <ros/service.h>
#include <tf_conversions/tf_eigen.h>
#include <eigen_conversions/eigen_msg.h>
#include <geometry_msgs/Transform.h>
#include "fanuc_post_processor_library/fanuc_post_processor_library.hpp"

#include <fanuc_grinding_post_processor/PostProcessorService.h> // Description of the Service we will use

boost::shared_ptr<ros::NodeHandle> node;

/**
 * This is the service function that is called whenever a request is received
 * @param req[int]
 * @param res[out]
 * @return Always true
 */
bool postProcessor(fanuc_grinding_post_processor::PostProcessorService::Request &req,
                   fanuc_grinding_post_processor::PostProcessorService::Response &res)
{
  // Get parameters from the message and print them
  //ROS_INFO_STREAM(std::endl << req);

  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > robot_poses_eigen;

  for (geometry_msgs::Pose tmp : req.RobotPoses)
  {
    Eigen::Isometry3d pose;
    tf::poseMsgToEigen(tmp, pose);
    pose.translate(Eigen::Vector3d(0, 0, req.TrajectoryZOffset));
    robot_poses_eigen.push_back(pose);
  }

  if (robot_poses_eigen.empty())
  {
    res.ReturnStatus = false;
    res.ReturnMessage = "Trajectory is empty";
    return true;
  }

  if (robot_poses_eigen.size() != req.IsGrindingPose.size())
  {
    res.ReturnStatus = false;
    res.ReturnMessage = "Trajectory is not correct";
    return true;
  }

  FanucPostProcessor fanuc_pp;
  fanuc_pp.setProgramName(req.ProgramName);
  fanuc_pp.setProgramComment(req.Comment);

  unsigned int speed;
  const double grinding_disk_DO(3);

  // First pose is a machining pose, we have to switch on the DO
  fanuc_pp.appendDigitalOutput(grinding_disk_DO, true);
  fanuc_pp.appendWait(1);
  //speed = req.MachiningSpeed * 100;
  speed = 30;
  fanuc_pp.appendPoseCNT(FanucPostProcessor::JOINT, robot_poses_eigen[0], 1, speed, FanucPostProcessor::PERCENTAGE, 100);

  for (unsigned i = 1; i < robot_poses_eigen.size(); ++i)
  {
    // if the new point is an extrication point and the old one was a machining point, we have to switch off the DO
    if (req.IsGrindingPose[i] == 0 && req.IsGrindingPose[i - 1] == 1)
    {
      fanuc_pp.appendDigitalOutput(grinding_disk_DO, false);
      fanuc_pp.appendWait(1);
      speed = 50;//req.ExtricationSpeed * 100;
    }
    // if the new point is an machining point and the old one was a extrication point, we have to switch on the DO
    else if (req.IsGrindingPose[i] == 1 && req.IsGrindingPose[i - 1] == 0)
    {
      fanuc_pp.appendDigitalOutput(grinding_disk_DO, true);
      fanuc_pp.appendWait(1);
      speed = 30;//req.MachiningSpeed * 100;
    }
    fanuc_pp.appendPoseCNT(FanucPostProcessor::JOINT, robot_poses_eigen[i], i + 1, speed, FanucPostProcessor::PERCENTAGE, 100);
  }

  std::string program;
  fanuc_pp.generateProgram(program);
  ROS_INFO_STREAM("This is the generated program:\n\n" << program);

  // TODO: Check if writing in a .ls file works
  std::ofstream tp_program_file;
  std::string file_location = req.ProgramLocation + req.ProgramName;

  tp_program_file.open(file_location.c_str(), std::ios::out);
  // We check if file have been opened correctly
  if (tp_program_file.bad())
  {
    res.ReturnStatus = false;
    res.ReturnMessage = "Cannot open/create TP program file";
    return true;
  }

  tp_program_file << program; // We put program into TP program file
  tp_program_file.close();

  if (!req.Upload)
  {
    res.ReturnStatus = true;
    res.ReturnMessage = "Program generated";
    return true;
  }

  // TODO: Try to upload
  ROS_INFO_STREAM("Try to upload program on IP address: " << req.IpAdress);
  if (!fanuc_pp.uploadToFtp(req.IpAdress))
  {
    res.ReturnStatus = false;
    res.ReturnMessage = "Cannot upload the program on IP address: " + req.IpAdress;
    return true;
  }

  res.ReturnStatus = true;
  res.ReturnMessage = "Program generated and uploaded";
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "post_processor");
  node.reset(new ros::NodeHandle);

  // Create service server and wait for incoming requests
  ros::ServiceServer service = node->advertiseService("post_processor_service", postProcessor);
  ros::AsyncSpinner spinner(1);
  spinner.start();

  while (node->ok())
  {
  }

  spinner.stop();
  return 0;
}
<|endoftext|>


<commit_before>#ifndef INCLUDED_STATIC_IOTA
#define INCLUDED_STATIC_IOTA
#include<boost/preprocessor/repetition/repeat_from_to.hpp>
#define static_iota_t(z,x,text) x,
#define static_iota(x,y) BOOST_PP_REPEAT_FROM_TO(x,y,static_iota_t,_)
#define IIOTA static_iota
#define INIT_IOTA static_iota
#endif
<commit_msg>Delete static_iota.hpp<commit_after><|endoftext|>


<commit_before>/**
 * stats_client.cc
 *
 * stand-alone client to poll a stats server
 *
 */

#include <iostream>
#include <string>
#include <system_error>
#include <thread>

#include <unistd.h>
#include <sys/socket.h>
#include <sys/un.h>

#include "counter.h"
#include "stats_common.h"
#include "util.h"

using namespace std;
using namespace util;

int
main(int argc, char **argv)
{
  if (argc != 3) {
    cerr << "[usage] " << argv[0] << " sockfile counter" << endl;
    return 1;
  }

  const string sockfile(argv[1]);
  const string counter_name(argv[2]);

  int fd = socket(AF_UNIX, SOCK_STREAM, 0);
  if (fd < 0)
    throw system_error(errno, system_category(),
        "creating UNIX domain socket");
  struct sockaddr_un addr;
  memset(&addr, 0, sizeof addr);
  addr.sun_family = AF_UNIX;
  if (sockfile.length() + 1 >= sizeof addr.sun_path)
    throw range_error("UNIX domain socket path too long");
  strcpy(addr.sun_path, sockfile.c_str());
  size_t len = strlen(addr.sun_path) + sizeof(addr.sun_family);
  if (connect(fd, (struct sockaddr *) &addr, len) < 0)
    throw system_error(errno, system_category(),
        "connecting to socket");

  uint8_t buf[1 + counter_name.size()];
  buf[0] = (uint8_t) stats_command::GET_COUNTER_VALUE;
  memcpy(&buf[1], counter_name.data(), counter_name.size());

  packet pkt;
  int r;
  timer loop_timer;
  for (;;) {
    pkt.assign((const char *) &buf[0], sizeof(buf));
    if ((r = pkt.sendpkt(fd))) {
      perror("send - disconnecting");
      return 1;
    }
    if ((r = pkt.recvpkt(fd))) {
      perror("recv - disconnecting");
      return 1;
    }
    const get_counter_value_t *resp = (const get_counter_value_t *) pkt.data();
    cout << resp->timestamp_us_ << " "
         << resp->d_.count_     << " "
         << resp->d_.sum_       << " "
         << resp->d_.max_       << endl;

    const uint64_t last_loop_usec  = loop_timer.lap();
    const uint64_t delay_time_usec = 1000000;
    if (last_loop_usec < delay_time_usec) {
      const uint64_t sleep_ns = (delay_time_usec - last_loop_usec) * 1000;
      struct timespec t;
      t.tv_sec  = sleep_ns / ONE_SECOND_NS;
      t.tv_nsec = sleep_ns % ONE_SECOND_NS;
      nanosleep(&t, nullptr);
    }
  }

  return 0;
}
<commit_msg>stats_client can read multiple counters<commit_after>/**
 * stats_client.cc
 *
 * stand-alone client to poll a stats server
 *
 */

#include <iostream>
#include <string>
#include <system_error>
#include <thread>

#include <unistd.h>
#include <sys/socket.h>
#include <sys/un.h>

#include "counter.h"
#include "stats_common.h"
#include "util.h"

using namespace std;
using namespace util;

int
main(int argc, char **argv)
{
  if (argc != 3) {
    cerr << "[usage] " << argv[0] << " sockfile counterspec" << endl;
    return 1;
  }

  const string sockfile(argv[1]);
  const vector<string> counter_names = split(argv[2], ':');

  int fd = socket(AF_UNIX, SOCK_STREAM, 0);
  if (fd < 0)
    throw system_error(errno, system_category(),
        "creating UNIX domain socket");
  struct sockaddr_un addr;
  memset(&addr, 0, sizeof addr);
  addr.sun_family = AF_UNIX;
  if (sockfile.length() + 1 >= sizeof addr.sun_path)
    throw range_error("UNIX domain socket path too long");
  strcpy(addr.sun_path, sockfile.c_str());
  size_t len = strlen(addr.sun_path) + sizeof(addr.sun_family);
  if (connect(fd, (struct sockaddr *) &addr, len) < 0)
    throw system_error(errno, system_category(),
        "connecting to socket");

  packet pkt;
  int r;
  timer loop_timer;
  for (;;) {
    for (auto &name : counter_names) {
      uint8_t buf[1 + name.size()];
      buf[0] = (uint8_t) stats_command::GET_COUNTER_VALUE;
      memcpy(&buf[1], name.data(), name.size());
      pkt.assign((const char *) &buf[0], sizeof(buf));
      if ((r = pkt.sendpkt(fd))) {
        perror("send - disconnecting");
        return 1;
      }
      if ((r = pkt.recvpkt(fd))) {
        perror("recv - disconnecting");
        return 1;
      }
      const get_counter_value_t *resp = (const get_counter_value_t *) pkt.data();
      cout << name                << " "
           << resp->timestamp_us_ << " "
           << resp->d_.count_     << " "
           << resp->d_.sum_       << " "
           << resp->d_.max_       << endl;
    }

    const uint64_t last_loop_usec  = loop_timer.lap();
    const uint64_t delay_time_usec = 1000000;
    if (last_loop_usec < delay_time_usec) {
      const uint64_t sleep_ns = (delay_time_usec - last_loop_usec) * 1000;
      struct timespec t;
      t.tv_sec  = sleep_ns / ONE_SECOND_NS;
      t.tv_nsec = sleep_ns % ONE_SECOND_NS;
      nanosleep(&t, nullptr);
    }
  }

  return 0;
}
<|endoftext|>


<commit_before>#ifndef AMGCL_ADAPTER_SCALED_PROBLEM_HPP
#define AMGCL_ADAPTER_SCALED_PROBLEM_HPP

/*
The MIT License

Copyright (c) 2012-2019 Denis Demidov <dennis.demidov@gmail.com>

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

/**
\file    amgcl/adapter/scaled_problem.hpp
\author  Denis Demidov <dennis.demidov@gmail.com>
\brief   Scale matrix, rhs, and solution.

Example:
\code
auto A = std::tie(rows, ptr, col, val);
auto scale = amgcl::adapter::scale_diagonal<Backend>(A, bprm);

// Setup solver
Solver solve(scale.matrix(A), prm, bprm);

// option 1: rhs is untouched
solve(*scale.rhs(b), x);

// option 2: rhs is prescaled in-place
scale(b);
solve(b, x);

// postprocess the solution:
scale(x);
\endcode
*/

#include <vector>
#include <amgcl/backend/builtin.hpp>
#include <amgcl/value_type/interface.hpp>

namespace amgcl {
namespace adapter {

template <class Matrix, class Scale>
struct scaled_matrix {
    typedef typename backend::value_type<Matrix>::type value_type;
    typedef typename backend::value_type<Scale>::type  scale_type;

    const Matrix &A;
    const Scale  &s;

    scaled_matrix(const Matrix &A, const Scale &s) : A(A), s(s) {}

    size_t rows()     const { return backend::rows(A);     }
    size_t cols()     const { return backend::cols(A);     }
    size_t nonzeros() const { return backend::nonzeros(A); }

    struct row_iterator : public backend::row_iterator<Matrix>::type {
        typedef typename backend::row_iterator<Matrix>::type Base;

        scale_type  si;
        const Scale &s;

        row_iterator(const Matrix &A, const Scale &s, size_t i)
            : Base(A, i), si(s[i]), s(s) {}

        value_type value() const {
            return si * static_cast<const Base*>(this)->value() * s[this->col()];
        }
    };

    row_iterator row_begin(size_t i) const {
        return row_iterator(A, s, i);
    }
};

template <class Backend, class Scale>
struct scaled_problem {
    typedef typename Backend::params backend_params;

    const std::shared_ptr<Scale> s;
    const backend_params &bprm;

    scaled_problem(std::shared_ptr<Scale> s, const backend_params &bprm = backend_params())
        : s(s), bprm(bprm) {}

    template <class Matrix>
    scaled_matrix<Matrix, Scale> matrix(const Matrix &A) const {
        return scaled_matrix<Matrix, Scale>(A, *s);
    }

    template <class Vector>
    std::shared_ptr<typename Backend::vector> rhs(const Vector &v) const {
        typedef typename backend::value_type<Vector>::type value_type;
        typedef typename math::scalar_of<value_type>::type scalar_type;
        const auto one  = math::identity<scalar_type>();
        const auto zero = math::zero<scalar_type>();

        auto t = Backend::copy_vector(v, bprm);
        (*this)(*t);
        return t;
    }

    template <class Vector>
    void operator()(Vector &x) const {
        typedef typename backend::value_type<Vector>::type value_type;
        typedef typename math::scalar_of<value_type>::type scalar_type;

        const auto one  = math::identity<scalar_type>();
        const auto zero = math::zero<scalar_type>();

        if (backend::is_builtin_vector<Vector>::value) {
            backend::vmul(one, *s, x, zero, x);
        } else {
            backend::vmul(one, *Backend::copy_vector(*s, bprm), x, zero, x);
        }
    }
};

template <class Backend, class Matrix>
scaled_problem<
    Backend,
    std::vector<
        typename math::scalar_of<
            typename backend::value_type<Matrix>::type
            >::type>
        >
scale_diagonal(
        const Matrix &A,
        const typename Backend::params &bprm = typename Backend::params()
        )
{
    typedef typename backend::value_type<Matrix>::type value_type;
    typedef typename math::scalar_of<value_type>::type scalar_type;
    ptrdiff_t n = backend::rows(A);
    auto      s = std::make_shared<std::vector<scalar_type>>(n);

#pragma omp parallel for
    for(ptrdiff_t i = 0; i < n; ++i) {
        for(auto a = backend::row_begin(A, i); a; ++a) {
            if (a.col() == i) {
                (*s)[i] = sqrt(math::norm(a.value()));
                break;
            }
        }
    }

    return scaled_problem<Backend, std::vector<scalar_type>>(s, bprm);
}

} // namespace adapter

} // namespace amgcl

#endif
<commit_msg>Fix a bug in scaling adapter<commit_after>#ifndef AMGCL_ADAPTER_SCALED_PROBLEM_HPP
#define AMGCL_ADAPTER_SCALED_PROBLEM_HPP

/*
The MIT License

Copyright (c) 2012-2019 Denis Demidov <dennis.demidov@gmail.com>

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

/**
\file    amgcl/adapter/scaled_problem.hpp
\author  Denis Demidov <dennis.demidov@gmail.com>
\brief   Scale matrix, rhs, and solution.

Example:
\code
auto A = std::tie(rows, ptr, col, val);
auto scale = amgcl::adapter::scale_diagonal<Backend>(A, bprm);

// Setup solver
Solver solve(scale.matrix(A), prm, bprm);

// option 1: rhs is untouched
solve(*scale.rhs(b), x);

// option 2: rhs is prescaled in-place
scale(b);
solve(b, x);

// postprocess the solution:
scale(x);
\endcode
*/

#include <vector>
#include <amgcl/backend/builtin.hpp>
#include <amgcl/value_type/interface.hpp>

namespace amgcl {
namespace adapter {

template <class Matrix, class Scale>
struct scaled_matrix {
    typedef typename backend::value_type<Matrix>::type value_type;
    typedef typename backend::value_type<Scale>::type  scale_type;

    const Matrix &A;
    const Scale  &s;

    scaled_matrix(const Matrix &A, const Scale &s) : A(A), s(s) {}

    size_t rows()     const { return backend::rows(A);     }
    size_t cols()     const { return backend::cols(A);     }
    size_t nonzeros() const { return backend::nonzeros(A); }

    struct row_iterator : public backend::row_iterator<Matrix>::type {
        typedef typename backend::row_iterator<Matrix>::type Base;

        scale_type  si;
        const Scale &s;

        row_iterator(const Matrix &A, const Scale &s, size_t i)
            : Base(A, i), si(s[i]), s(s) {}

        value_type value() const {
            return si * static_cast<const Base*>(this)->value() * s[this->col()];
        }
    };

    row_iterator row_begin(size_t i) const {
        return row_iterator(A, s, i);
    }
};

template <class Backend, class Scale>
struct scaled_problem {
    typedef typename Backend::params backend_params;

    const std::shared_ptr<Scale> s;
    const backend_params &bprm;

    scaled_problem(std::shared_ptr<Scale> s, const backend_params &bprm = backend_params())
        : s(s), bprm(bprm) {}

    template <class Matrix>
    scaled_matrix<Matrix, Scale> matrix(const Matrix &A) const {
        return scaled_matrix<Matrix, Scale>(A, *s);
    }

    template <class Vector>
    std::shared_ptr<typename Backend::vector> rhs(const Vector &v) const {
        typedef typename backend::value_type<Vector>::type value_type;
        typedef typename math::scalar_of<value_type>::type scalar_type;
        const auto one  = math::identity<scalar_type>();
        const auto zero = math::zero<scalar_type>();

        auto t = Backend::copy_vector(v, bprm);
        (*this)(*t);
        return t;
    }

    template <class Vector>
    void operator()(Vector &x) const {
        typedef typename backend::value_type<Vector>::type value_type;
        typedef typename math::scalar_of<value_type>::type scalar_type;

        const auto one  = math::identity<scalar_type>();
        const auto zero = math::zero<scalar_type>();

        if (backend::is_builtin_vector<Vector>::value) {
            backend::vmul(one, *s, x, zero, x);
        } else {
            backend::vmul(one, *Backend::copy_vector(*s, bprm), x, zero, x);
        }
    }
};

template <class Backend, class Matrix>
scaled_problem<
    Backend,
    std::vector<
        typename math::scalar_of<
            typename backend::value_type<Matrix>::type
            >::type>
        >
scale_diagonal(
        const Matrix &A,
        const typename Backend::params &bprm = typename Backend::params()
        )
{
    typedef typename backend::value_type<Matrix>::type value_type;
    typedef typename math::scalar_of<value_type>::type scalar_type;
    ptrdiff_t n = backend::rows(A);
    auto      s = std::make_shared<std::vector<scalar_type>>(n);

#pragma omp parallel for
    for(ptrdiff_t i = 0; i < n; ++i) {
        for(auto a = backend::row_begin(A, i); a; ++a) {
            if (a.col() == i) {
                (*s)[i] = math::inverse(sqrt(math::norm(a.value())));
                break;
            }
        }
    }

    return scaled_problem<Backend, std::vector<scalar_type>>(s, bprm);
}

} // namespace adapter

} // namespace amgcl

#endif
<|endoftext|>


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

#pragma once
#ifndef CUDARRAYS_COMPUTE_HPP_
#define CUDARRAYS_COMPUTE_HPP_

#include <array>

#include "common.hpp"
#include "utils.hpp"

namespace cudarrays {

enum compute : unsigned {
    none = partition::NONE,
    x = partition::X,
    y = partition::Y,
    z = partition::Z,
    xy = partition::XY,
    xz = partition::XZ,
    yz = partition::YZ,
    xyz = partition::XYZ
};

template <unsigned Dims>
struct compute_part_helper;

template <>
struct compute_part_helper<1> {
    static std::array<bool, 1>
    make_array(compute c)
    {
        std::array<bool, 1> ret;
        if (c == compute::none)   ret = { false };
        else if (c == compute::y) ret = { true  };
        else abort();
        return ret;
    }
};

template <>
struct compute_part_helper<2> {
    static std::array<bool, 2>
    make_array(compute c)
    {
        std::array<bool, 2> ret;
        if (c == compute::none)    ret = { false, false };
        else if (c == compute::x)  ret = { false, true  };
        else if (c == compute::y)  ret = { true, false  };
        else if (c == compute::xy) ret = { true, true   };
        else abort();
        return ret;
    }
};

template <>
struct compute_part_helper<3> {
    static std::array<bool, 3>
    make_array(compute c)
    {
        std::array<bool, 3> ret;
        if (c == compute::none)     ret = { false, false, false };
        else if (c == compute::x)   ret = { false, false, true  };
        else if (c == compute::y)   ret = { false, true,  false };
        else if (c == compute::z)   ret = { true,  false, false };
        else if (c == compute::xy)  ret = { false, true,  true  };
        else if (c == compute::xz)  ret = { true,  false, true  };
        else if (c == compute::yz)  ret = { true,  true,  false };
        else if (c == compute::xyz) ret = { true,  true,  true  };
        else abort();
        return ret;
    }
};

template <unsigned Dims>
struct compute_conf {
    std::array<bool, Dims> info;
    unsigned procs;

    compute_conf(compute c, unsigned _procs = 0) :
        procs(_procs)
    {
        info = compute_part_helper<Dims>::make_array(c);
    }

    constexpr bool is_dim_part(unsigned dim) const
    {
        return info[dim];
    }

    unsigned get_part_dims() const
    {
        return utils::count(info, true);
    }
};

}

#endif
<commit_msg>Minor fix<commit_after>/*
 * CUDArrays is a library for easy multi-GPU program development.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2013-2015 Barcelona Supercomputing Center and
 *                         University of Illinois
 *
 *  Developed by: Javier Cabezas <javier.cabezas@gmail.com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE. */

#pragma once
#ifndef CUDARRAYS_COMPUTE_HPP_
#define CUDARRAYS_COMPUTE_HPP_

#include <array>

#include "common.hpp"
#include "utils.hpp"

namespace cudarrays {

enum compute : unsigned {
    none = partition::NONE,
    x = partition::X,
    y = partition::Y,
    z = partition::Z,
    xy = partition::XY,
    xz = partition::XZ,
    yz = partition::YZ,
    xyz = partition::XYZ
};

template <unsigned Dims>
struct compute_part_helper;

template <>
struct compute_part_helper<1> {
    static std::array<bool, 1>
    make_array(compute c)
    {
        std::array<bool, 1> ret;
        if (c == compute::none)   ret = { false };
        else if (c == compute::x) ret = { true  };
        else abort();
        return ret;
    }
};

template <>
struct compute_part_helper<2> {
    static std::array<bool, 2>
    make_array(compute c)
    {
        std::array<bool, 2> ret;
        if (c == compute::none)    ret = { false, false };
        else if (c == compute::x)  ret = { false, true  };
        else if (c == compute::y)  ret = { true, false  };
        else if (c == compute::xy) ret = { true, true   };
        else abort();
        return ret;
    }
};

template <>
struct compute_part_helper<3> {
    static std::array<bool, 3>
    make_array(compute c)
    {
        std::array<bool, 3> ret;
        if (c == compute::none)     ret = { false, false, false };
        else if (c == compute::x)   ret = { false, false, true  };
        else if (c == compute::y)   ret = { false, true,  false };
        else if (c == compute::z)   ret = { true,  false, false };
        else if (c == compute::xy)  ret = { false, true,  true  };
        else if (c == compute::xz)  ret = { true,  false, true  };
        else if (c == compute::yz)  ret = { true,  true,  false };
        else if (c == compute::xyz) ret = { true,  true,  true  };
        else abort();
        return ret;
    }
};

template <unsigned Dims>
struct compute_conf {
    std::array<bool, Dims> info;
    unsigned procs;

    compute_conf(compute c, unsigned _procs = 0) :
        procs(_procs)
    {
        info = compute_part_helper<Dims>::make_array(c);
    }

    constexpr bool is_dim_part(unsigned dim) const
    {
        return info[dim];
    }

    unsigned get_part_dims() const
    {
        return utils::count(info, true);
    }
};

}

#endif
<|endoftext|>


<commit_before>#pragma once
/**
	@file
	@brief normal random generator

	Copyright (C) 2007 Cybozu Labs, Inc., all rights reserved.
	@author MITSUNARI Shigeo
*/

namespace cybozu { namespace nlp {

/*
	xor shift
*/
class UniformRandomGenerator {
	double a_;
	double b_;
	unsigned int x_, y_, z_, w_;
public:
	/* generate uniform random value in [a, b) */
	explicit UniformRandomGenerator(double a = 0, double b = 1, int seed = 0)
		: a_(a)
		, b_(b)
	{
		init(seed);
	}
	void init(int seed = 0)
	{
		x_ = 123456789 + seed;
		y_ = 362436069;
		z_ = 521288629;
		w_ = 88675123;
	}
	/* [0, 2^32) random number */
	unsigned int get()
	{
		unsigned int t = x_ ^ (x_ << 11);
		x_ = y_; y_ = z_; z_ = w_;
		return w_ = (w_ ^ (w_ >> 19)) ^ (t ^ (t >> 8));
	}
	/* [a, b) random number */
	double getDouble()
	{
		unsigned int x = get() >> 5;
		unsigned int y = get() >> 6;
		double z = (x * double(1U << 26) + y) * (1.0 / double(1LL << 53));
		return (b_ - a_) * z + a_;
	}
};
/*
	normal random generator
*/
class NormalRandomGenerator {
	UniformRandomGenerator gen_;
	double u_;
	double s_;
public:
	explicit NormalRandomGenerator(double u = 0, double s = 1, int seed = 0)
		: gen_(seed)
		, u_(u)
		, s_(s)
	{
	}
	void init(int seed = 0)
	{
		gen_.init(seed);
	}
	double get()
	{
		double sum = -6;
		for (int i = 0; i < 12; i++) {
			sum += gen_.getDouble();
		}
		return sum * s_ + u_;
	}
};

} } // cybozu::nlp
<commit_msg>same ui as random_generator<commit_after>#pragma once
/**
	@file
	@brief normal random generator

	Copyright (C) 2007 Cybozu Labs, Inc., all rights reserved.
	@author MITSUNARI Shigeo
*/
#include <cybozu/inttype.hpp>

namespace cybozu { namespace nlp {

/*
	xor shift
*/
class UniformRandomGenerator {
	double a_;
	double b_;
	uint32_t x_, y_, z_, w_;
public:
	/* generate uniform random value in [a, b) */
	explicit UniformRandomGenerator(double a = 0, double b = 1, int seed = 0)
		: a_(a)
		, b_(b)
	{
		init(seed);
	}
	void init(int seed = 0)
	{
		x_ = 123456789 + seed;
		y_ = 362436069;
		z_ = 521288629;
		w_ = 88675123;
	}
	/* [0, 2^32) random number */
	uint32_t get()
	{
		unsigned int t = x_ ^ (x_ << 11);
		x_ = y_; y_ = z_; z_ = w_;
		return w_ = (w_ ^ (w_ >> 19)) ^ (t ^ (t >> 8));
	}
	uint32_t operator()() { return get(); }
	uint32_t get32() { return get(); }
	uint64_t get64()
	{
		uint32_t a = get();
		uint32_t b = get();
		return (uint64_t(a) << 32) | b;
	}
	/* [a, b) random number */
	double getDouble()
	{
		unsigned int x = get() >> 5;
		unsigned int y = get() >> 6;
		double z = (x * double(1U << 26) + y) * (1.0 / double(1LL << 53));
		return (b_ - a_) * z + a_;
	}
};
/*
	normal random generator
*/
class NormalRandomGenerator {
	UniformRandomGenerator gen_;
	double u_;
	double s_;
public:
	explicit NormalRandomGenerator(double u = 0, double s = 1, int seed = 0)
		: gen_(seed)
		, u_(u)
		, s_(s)
	{
	}
	void init(int seed = 0)
	{
		gen_.init(seed);
	}
	double get()
	{
		double sum = -6;
		for (int i = 0; i < 12; i++) {
			sum += gen_.getDouble();
		}
		return sum * s_ + u_;
	}
};

} } // cybozu::nlp
<|endoftext|>


<commit_before>//=============================================================================================================
/**
* @file     main.cpp
* @author   Lorenz Esch <lorenz.esch@tu-ilmenau.de>;
*           Matti Hamalainen <msh@nmr.mgh.harvard.edu>
* @version  1.0
* @date     September, 2018
*
* @section  LICENSE
*
* Copyright (C) 2018, Lorenz Esch and Matti Hamalainen. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification, are permitted provided that
* the following conditions are met:
*     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
*       following disclaimer.
*     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
*       the following disclaimer in the documentation and/or other materials provided with the distribution.
*     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
*       to endorse or promote products derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
* PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*
* @brief    Read data and perform tf analysis
*
*/

//*************************************************************************************************************
//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include <fiff/fiff.h>

#include <utils/spectrogram.h>

#include <disp/plots/tfplot.h>

//*************************************************************************************************************
//=============================================================================================================
// QT INCLUDES
//=============================================================================================================

#include <QApplication>
#include <QCommandLineParser>

//*************************************************************************************************************
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace FIFFLIB;
using namespace DISPLIB;
using namespace UTILSLIB;

//*************************************************************************************************************
//=============================================================================================================
// MAIN
//=============================================================================================================

//=============================================================================================================
/**
* The function main marks the entry point of the program.
* By default, main has the storage class extern.
*
* @param [in] argc (argument count) is an integer that indicates how many arguments were entered on the command line when the program was started.
* @param [in] argv (argument vector) is an array of pointers to arrays of character objects. The array objects are null-terminated strings, representing the arguments that were entered on the command line when the program was started.
* @return the value that was set to exit() (which is 0 if exit() is called via quit()).
*/
int main(int argc, char *argv[])
{
    QApplication a(argc, argv);

    // Command Line Parser
    QCommandLineParser parser;
    parser.setApplicationDescription("Read Evoked Example");
    parser.addHelpOption();

    //QCommandLineOption evokedFileOption("ave", "Path to the evoked/average <file>.", "file", QCoreApplication::applicationDirPath() + "/MNE-sample-data/MEG/sample/sample_audvis-ave.fif");
    QCommandLineOption evokedFileOption("ave", "Path to the evoked/average <file>.", "file", "/cluster/fusion/data/MEG_EEG/john/170322/assr_40_223_ave.fif");
    QCommandLineOption evokedIdxOption("aveIdx", "The average <index> to choose from the average file.", "index", "0");

    parser.addOption(evokedFileOption);
    parser.addOption(evokedIdxOption);

    parser.process(a);

    //generate FiffEvoked object
    QFile t_sampleFile(parser.value(evokedFileOption));
    FiffEvoked p_FiffEvoked(t_sampleFile,QVariant(parser.value(evokedIdxOption)));

    //tf plot
    VectorXd dataCol = p_FiffEvoked.data.row(0).transpose();
    MatrixXd dataSpectrum = Spectrogram::makeSpectrogram(dataCol, p_FiffEvoked.info.sfreq*0.1);

    TFplot tfplot(dataSpectrum, p_FiffEvoked.info.sfreq, 0, 100, ColorMaps::Jet);
    tfplot.show();

    return a.exec();
}

//*************************************************************************************************************
//=============================================================================================================
// STATIC DEFINITIONS
//=============================================================================================================
<commit_msg>[SC-169] Change tf plot example<commit_after>//=============================================================================================================
/**
* @file     main.cpp
* @author   Lorenz Esch <lorenz.esch@tu-ilmenau.de>;
*           Matti Hamalainen <msh@nmr.mgh.harvard.edu>
* @version  1.0
* @date     September, 2018
*
* @section  LICENSE
*
* Copyright (C) 2018, Lorenz Esch and Matti Hamalainen. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification, are permitted provided that
* the following conditions are met:
*     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
*       following disclaimer.
*     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
*       the following disclaimer in the documentation and/or other materials provided with the distribution.
*     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
*       to endorse or promote products derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
* PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*
* @brief    Read data and perform tf analysis
*
*/

//*************************************************************************************************************
//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include <fiff/fiff.h>

#include <utils/spectrogram.h>

#include <disp/plots/tfplot.h>

//*************************************************************************************************************
//=============================================================================================================
// QT INCLUDES
//=============================================================================================================

#include <QApplication>
#include <QCommandLineParser>

//*************************************************************************************************************
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace FIFFLIB;
using namespace DISPLIB;
using namespace UTILSLIB;

//*************************************************************************************************************
//=============================================================================================================
// MAIN
//=============================================================================================================

//=============================================================================================================
/**
* The function main marks the entry point of the program.
* By default, main has the storage class extern.
*
* @param [in] argc (argument count) is an integer that indicates how many arguments were entered on the command line when the program was started.
* @param [in] argv (argument vector) is an array of pointers to arrays of character objects. The array objects are null-terminated strings, representing the arguments that were entered on the command line when the program was started.
* @return the value that was set to exit() (which is 0 if exit() is called via quit()).
*/
int main(int argc, char *argv[])
{
    QApplication a(argc, argv);

    // Command Line Parser
    QCommandLineParser parser;
    parser.setApplicationDescription("TF Plot Example");
    parser.addHelpOption();

    QCommandLineOption evokedFileOption("ave", "Path to the evoked/average <file>.", "file", QCoreApplication::applicationDirPath() + "/MNE-sample-data/MEG/sample/sample_audvis-ave.fif");
    QCommandLineOption evokedIdxOption("aveIdx", "The average <index> to choose from the average file.", "index", "2");

    parser.addOption(evokedFileOption);
    parser.addOption(evokedIdxOption);

    parser.process(a);

    //generate FiffEvoked object
    QFile t_sampleFile(parser.value(evokedFileOption));
    FiffEvoked p_FiffEvoked(t_sampleFile,QVariant(parser.value(evokedIdxOption)));

    //tf plot
    VectorXd dataCol = p_FiffEvoked.data.row(0).transpose();
    MatrixXd dataSpectrum = Spectrogram::makeSpectrogram(dataCol, p_FiffEvoked.info.sfreq*0.15);

    TFplot tfplot(dataSpectrum, p_FiffEvoked.info.sfreq, 0, 50, ColorMaps::Jet);
    tfplot.show();

    return a.exec();
}

//*************************************************************************************************************
//=============================================================================================================
// STATIC DEFINITIONS
//=============================================================================================================
<|endoftext|>


<commit_before>/*
 * eos - A 3D Morphable Model fitting library written in modern C++11/14.
 *
 * File: include/eos/core/read_obj.hpp
 *
 * Copyright 2017, 2018 Patrik Huber
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#pragma once

#ifndef EOS_READ_OBJ_HPP
#define EOS_READ_OBJ_HPP

#include "eos/core/Mesh.hpp"
#include "eos/cpp17/optional.hpp"

#include "Eigen/Core"

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

namespace eos {
namespace core {

namespace detail {

// From: https://stackoverflow.com/a/1493195/1345959
template <class ContainerType>
void tokenize(const std::string& str, ContainerType& tokens, const std::string& delimiters = " ",
              bool trim_empty = false)
{
    std::string::size_type pos, last_pos = 0;
    const auto length = str.length();

    using value_type = typename ContainerType::value_type;
    using size_type = typename ContainerType::size_type;

    while (last_pos < length + 1)
    {
        pos = str.find_first_of(delimiters, last_pos);
        if (pos == std::string::npos)
        {
            pos = length;
        }

        if (pos != last_pos || !trim_empty)
            tokens.push_back(value_type(str.data() + last_pos, (size_type)pos - last_pos));

        last_pos = pos + 1;
    }
}

/**
 * @brief Parse a line starting with 'v' from an obj file.
 *
 * The 'v' will have already been stripped from the line.
 *
 * These can contain vertex colours too actually. Can potentially have 3, 4, 6 or 7 elements...? (xyz, xyzw, xyzrgb, xyzwrgb)
 * assimp only deals with 3, 4 or 6 elements: https://github.com/assimp/assimp/blob/master/code/ObjFileParser.cpp#L138
 * For now, let's ignore homogeneous coordinates, and deal with the case when we have either 3 or 6 elements.
 * If it's homogeneous coords, we can also just divide by 'w', like assimp.
 *
 * Another obj parser we can check: https://github.com/qnzhou/PyMesh/blob/master/src/IO/OBJParser.cpp (and same file with .h)
 *
 * Todo: Consider using std::string_view for these instead of const string&.
 * And should change to glm::vec3, and just divide by 'w'. As soon as we change the Mesh to vec3.
 */
inline std::pair<Eigen::Vector4f, cpp17::optional<Eigen::Vector3f>> parse_vertex(const std::string& line)
{
    std::vector<std::string> tokens;
    tokenize(line, tokens, " ");
    assert(tokens.size() == 3 || tokens.size() == 6); // Maybe we should throw instead?
    const Eigen::Vector4f vertex(std::stof(tokens[0]), std::stof(tokens[1]), std::stof(tokens[2]), 1.0);
    cpp17::optional<Eigen::Vector3f> vertex_color;
    if (tokens.size() == 6)
    {
        vertex_color = Eigen::Vector3f(std::stof(tokens[3]), std::stof(tokens[4]), std::stof(tokens[5]));
    }
    return {vertex, vertex_color};
};

inline Eigen::Vector2f parse_texcoords(const std::string& line)
{
    std::vector<std::string> tokens;
    tokenize(line, tokens, " ");
    assert(tokens.size() == 2);
    const Eigen::Vector2f texcoords(std::stof(tokens[0]), std::stof(tokens[1]));
    return texcoords;
};

inline void parse_vertex_normal(const std::string& line)
{
    throw std::runtime_error("Parsing \"vt\" is not yet implemented.");
};

// Note: Indices in obj's start at 1!
// Oh no, these contain 3 entries for triangles, but can be quads (=4 entries), and potentially more.
// Their triplets of: vertex, texture and normal indices. Some of them can be missing. E.g.:
//  f 1 2 3
//  f 3/1 4/2 5/3
//  f 6/4/1 3/5/3 7/6/5
//  f 7//1 8//2 9//3
inline auto parse_face(const std::string& line)
{
    using std::string;
    using std::vector;

    // Obj indices are 1-based.
    vector<int> vertex_indices;  // size() = 3 or 4
    vector<int> texture_indices; // size() = 3 or 4
    vector<int> normal_indices;  // size() = 3 or 4

    vector<string> tokens;
    tokenize(line, tokens, " ");
    assert(tokens.size() == 3 || tokens.size() == 4); // For now we need this to be 3 (triangles) or 4 (quads).
    // Now for each of these tokens, we want to split on "/":
    for (const auto& token : tokens)
    {
        vector<string> subtokens;
        tokenize(token, subtokens, "/"); // do we want trim_empty true or false?
        assert(subtokens.size() > 0 && subtokens.size() <= 3); // <= 3 correct or not?
        // Ok, let's make our life easy, for now only support the 1/2/3 syntax of the FaceWarehouse scans. In
        // fact the normal_indices are 0... we should check for that - zero-index = ignore, but note that, it's
        // probably a non-standard obj format extension.
        assert(subtokens.size() == 3);                         // FaceWarehouse
        vertex_indices.push_back(std::stoi(subtokens[0]) - 1); // obj indices are 1-based, so we subtract one.
        texture_indices.push_back(std::stoi(subtokens[1]) - 1);
        // subtokens[2] is zero, hence, no normal_indices.
    }

    return std::make_tuple(vertex_indices, texture_indices, normal_indices);
};

} /* namespace detail */

/**
 * @brief Reads the given Wavefront .obj file into a \c Mesh.
 *
 * X.
 * https://en.wikipedia.org/wiki/Wavefront_.obj_file as of 22 August 2017.
 *
 * @param[in] filename Input filename (ending in ".obj").
 * @return X.
 */
inline Mesh read_obj(std::string filename)
{
    std::ifstream file(filename);
    if (!file)
    {
        throw std::runtime_error(std::string("Could not open obj file: " + filename));
    }

    // We'll need these helper functions for the parsing:
    const auto starts_with = [](const std::string& input, const std::string& match) {
        return input.size() >= match.size() && std::equal(match.begin(), match.end(), input.begin());
    };

    /*    auto trim_left = [](const std::string& input, std::string pattern = " \t") {
            auto first = input.find_first_not_of(pattern);
            if (first == std::string::npos)
            {
                return input;
            }
            return input.substr(first, input.size());
        }; */

    Mesh mesh;

    std::string line;
    while (getline(file, line))
    {
        if (starts_with(line, "#"))
        {
            continue;
        }

        if (starts_with(line, "v "))
        { // matching with a space so that it doesn't match 'vt'
            auto vertex_data =
                detail::parse_vertex(line.substr(2)); // pass the string without the first two characters
            mesh.vertices.push_back(
                Eigen::Vector3f(vertex_data.first[0], vertex_data.first[1], vertex_data.first[2]));
            if (vertex_data.second)
            { // there are vertex colours:
                mesh.colors.push_back(vertex_data.second.value());
            }
        }
        if (starts_with(line, "vt "))
        {
            const auto texcoords = detail::parse_texcoords(line.substr(3));
            mesh.texcoords.push_back(texcoords);
        }
        if (starts_with(line, "vn "))
        {
            // detail::parse_vertex_normal(line.substr(3));
            // Not handled yet, our Mesh class doesn't contain normals right now anyway.
        }
        // There's other things like "vp ", which we don't handle
        if (starts_with(line, "f "))
        {
            auto face_data = detail::parse_face(line.substr(2));
            if (std::get<0>(face_data).size() == 3) // 3 triangle indices, nothing to do:
            {
                mesh.tvi.push_back(
                    {std::get<0>(face_data)[0], std::get<0>(face_data)[1], std::get<0>(face_data)[2]});
            }
            // If their sizes are 4, we convert the quad to two triangles:
            // Actually I think MeshLab does the same, it shows the FaceWarehouse number of "Faces" as twice the "f" entries in the obj.
            else if (std::get<0>(face_data).size() == 4)
            {
                // Just create two faces with (quad[0], quad[1], quad[2]) and (quad[0], quad[2], quad[3]).
                mesh.tvi.push_back(
                    {std::get<0>(face_data)[0], std::get<0>(face_data)[1], std::get<0>(face_data)[2]});
                mesh.tvi.push_back(
                    {std::get<0>(face_data)[0], std::get<0>(face_data)[2], std::get<0>(face_data)[3]});
            }
            // We don't handle normal_indices for now.
        }
        // There can be other stuff in obj's like materials, named objects, etc., which are not handled here.
    }
    return mesh;
}

} /* namespace core */
} /* namespace eos */

#endif /* EOS_READ_OBJ_HPP */
<commit_msg>Added doxygen comment for tokenize()<commit_after>/*
 * eos - A 3D Morphable Model fitting library written in modern C++11/14.
 *
 * File: include/eos/core/read_obj.hpp
 *
 * Copyright 2017, 2018 Patrik Huber
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#pragma once

#ifndef EOS_READ_OBJ_HPP
#define EOS_READ_OBJ_HPP

#include "eos/core/Mesh.hpp"
#include "eos/cpp17/optional.hpp"

#include "Eigen/Core"

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

namespace eos {
namespace core {

namespace detail {

/**
 * @brief Splits a string into a list of tokens.
 *
 * Taken from: https://stackoverflow.com/a/1493195/1345959
 */
template <class ContainerType>
void tokenize(const std::string& str, ContainerType& tokens, const std::string& delimiters = " ",
              bool trim_empty = false)
{
    std::string::size_type pos, last_pos = 0;
    const auto length = str.length();

    using value_type = typename ContainerType::value_type;
    using size_type = typename ContainerType::size_type;

    while (last_pos < length + 1)
    {
        pos = str.find_first_of(delimiters, last_pos);
        if (pos == std::string::npos)
        {
            pos = length;
        }

        if (pos != last_pos || !trim_empty)
            tokens.push_back(value_type(str.data() + last_pos, (size_type)pos - last_pos));

        last_pos = pos + 1;
    }
};

/**
 * @brief Parse a line starting with 'v' from an obj file.
 *
 * The 'v' will have already been stripped from the line.
 *
 * These can contain vertex colours too actually. Can potentially have 3, 4, 6 or 7 elements...? (xyz, xyzw, xyzrgb, xyzwrgb)
 * assimp only deals with 3, 4 or 6 elements: https://github.com/assimp/assimp/blob/master/code/ObjFileParser.cpp#L138
 * For now, let's ignore homogeneous coordinates, and deal with the case when we have either 3 or 6 elements.
 * If it's homogeneous coords, we can also just divide by 'w', like assimp.
 *
 * Another obj parser we can check: https://github.com/qnzhou/PyMesh/blob/master/src/IO/OBJParser.cpp (and same file with .h)
 *
 * Todo: Consider using std::string_view for these instead of const string&.
 * And should change to glm::vec3, and just divide by 'w'. As soon as we change the Mesh to vec3.
 */
inline std::pair<Eigen::Vector4f, cpp17::optional<Eigen::Vector3f>> parse_vertex(const std::string& line)
{
    std::vector<std::string> tokens;
    tokenize(line, tokens, " ");
    assert(tokens.size() == 3 || tokens.size() == 6); // Maybe we should throw instead?
    const Eigen::Vector4f vertex(std::stof(tokens[0]), std::stof(tokens[1]), std::stof(tokens[2]), 1.0);
    cpp17::optional<Eigen::Vector3f> vertex_color;
    if (tokens.size() == 6)
    {
        vertex_color = Eigen::Vector3f(std::stof(tokens[3]), std::stof(tokens[4]), std::stof(tokens[5]));
    }
    return {vertex, vertex_color};
};

inline Eigen::Vector2f parse_texcoords(const std::string& line)
{
    std::vector<std::string> tokens;
    tokenize(line, tokens, " ");
    assert(tokens.size() == 2);
    const Eigen::Vector2f texcoords(std::stof(tokens[0]), std::stof(tokens[1]));
    return texcoords;
};

inline void parse_vertex_normal(const std::string& line)
{
    throw std::runtime_error("Parsing \"vt\" is not yet implemented.");
};

// Note: Indices in obj's start at 1!
// Oh no, these contain 3 entries for triangles, but can be quads (=4 entries), and potentially more.
// Their triplets of: vertex, texture and normal indices. Some of them can be missing. E.g.:
//  f 1 2 3
//  f 3/1 4/2 5/3
//  f 6/4/1 3/5/3 7/6/5
//  f 7//1 8//2 9//3
inline auto parse_face(const std::string& line)
{
    using std::string;
    using std::vector;

    // Obj indices are 1-based.
    vector<int> vertex_indices;  // size() = 3 or 4
    vector<int> texture_indices; // size() = 3 or 4
    vector<int> normal_indices;  // size() = 3 or 4

    vector<string> tokens;
    tokenize(line, tokens, " ");
    assert(tokens.size() == 3 || tokens.size() == 4); // For now we need this to be 3 (triangles) or 4 (quads).
    // Now for each of these tokens, we want to split on "/":
    for (const auto& token : tokens)
    {
        vector<string> subtokens;
        tokenize(token, subtokens, "/"); // do we want trim_empty true or false?
        assert(subtokens.size() > 0 && subtokens.size() <= 3); // <= 3 correct or not?
        // Ok, let's make our life easy, for now only support the 1/2/3 syntax of the FaceWarehouse scans. In
        // fact the normal_indices are 0... we should check for that - zero-index = ignore, but note that, it's
        // probably a non-standard obj format extension.
        assert(subtokens.size() == 3);                         // FaceWarehouse
        vertex_indices.push_back(std::stoi(subtokens[0]) - 1); // obj indices are 1-based, so we subtract one.
        texture_indices.push_back(std::stoi(subtokens[1]) - 1);
        // subtokens[2] is zero, hence, no normal_indices.
    }

    return std::make_tuple(vertex_indices, texture_indices, normal_indices);
};

} /* namespace detail */

/**
 * @brief Reads the given Wavefront .obj file into a \c Mesh.
 *
 * X.
 * https://en.wikipedia.org/wiki/Wavefront_.obj_file as of 22 August 2017.
 *
 * @param[in] filename Input filename (ending in ".obj").
 * @return X.
 */
inline Mesh read_obj(std::string filename)
{
    std::ifstream file(filename);
    if (!file)
    {
        throw std::runtime_error(std::string("Could not open obj file: " + filename));
    }

    // We'll need these helper functions for the parsing:
    const auto starts_with = [](const std::string& input, const std::string& match) {
        return input.size() >= match.size() && std::equal(match.begin(), match.end(), input.begin());
    };

    /*    auto trim_left = [](const std::string& input, std::string pattern = " \t") {
            auto first = input.find_first_not_of(pattern);
            if (first == std::string::npos)
            {
                return input;
            }
            return input.substr(first, input.size());
        }; */

    Mesh mesh;

    std::string line;
    while (getline(file, line))
    {
        if (starts_with(line, "#"))
        {
            continue;
        }

        if (starts_with(line, "v "))
        { // matching with a space so that it doesn't match 'vt'
            auto vertex_data =
                detail::parse_vertex(line.substr(2)); // pass the string without the first two characters
            mesh.vertices.push_back(
                Eigen::Vector3f(vertex_data.first[0], vertex_data.first[1], vertex_data.first[2]));
            if (vertex_data.second)
            { // there are vertex colours:
                mesh.colors.push_back(vertex_data.second.value());
            }
        }
        if (starts_with(line, "vt "))
        {
            const auto texcoords = detail::parse_texcoords(line.substr(3));
            mesh.texcoords.push_back(texcoords);
        }
        if (starts_with(line, "vn "))
        {
            // detail::parse_vertex_normal(line.substr(3));
            // Not handled yet, our Mesh class doesn't contain normals right now anyway.
        }
        // There's other things like "vp ", which we don't handle
        if (starts_with(line, "f "))
        {
            auto face_data = detail::parse_face(line.substr(2));
            if (std::get<0>(face_data).size() == 3) // 3 triangle indices, nothing to do:
            {
                mesh.tvi.push_back(
                    {std::get<0>(face_data)[0], std::get<0>(face_data)[1], std::get<0>(face_data)[2]});
            }
            // If their sizes are 4, we convert the quad to two triangles:
            // Actually I think MeshLab does the same, it shows the FaceWarehouse number of "Faces" as twice the "f" entries in the obj.
            else if (std::get<0>(face_data).size() == 4)
            {
                // Just create two faces with (quad[0], quad[1], quad[2]) and (quad[0], quad[2], quad[3]).
                mesh.tvi.push_back(
                    {std::get<0>(face_data)[0], std::get<0>(face_data)[1], std::get<0>(face_data)[2]});
                mesh.tvi.push_back(
                    {std::get<0>(face_data)[0], std::get<0>(face_data)[2], std::get<0>(face_data)[3]});
            }
            // We don't handle normal_indices for now.
        }
        // There can be other stuff in obj's like materials, named objects, etc., which are not handled here.
    }
    return mesh;
}

} /* namespace core */
} /* namespace eos */

#endif /* EOS_READ_OBJ_HPP */
<|endoftext|>


<commit_before>#include "Variant.h"
#include "split.h"
#include <string>
#include <iostream>

using namespace std;
using namespace vcf;

void annotateWithBlankGenotypes(Variant& var, string& annotationTag) {

    var.addFormatField(annotationTag);

    map<string, map<string, vector<string> > >::iterator s     = var.samples.begin();
    map<string, map<string, vector<string> > >::iterator sEnd  = var.samples.end();

    for (; s != sEnd; ++s) {
        map<string, vector<string> >& sample = s->second;
        sample[annotationTag].clear(); // means "no genotype" genotype
        sample[annotationTag].push_back("./."); // means "no genotype" genotype
    }
}

void annotateWithGenotypes(Variant& varA, Variant& varB, string& annotationTag) {

    varA.addFormatField(annotationTag);

    map<string, map<string, vector<string> > >::iterator s     = varA.samples.begin();
    map<string, map<string, vector<string> > >::iterator sEnd  = varA.samples.end();

    for (; s != sEnd; ++s) {
        map<string, vector<string> >& sample = s->second;
        const string& name = s->first;
        map<string, map<string, vector<string> > >::iterator o = varB.samples.find(name);
        sample[annotationTag].clear();
        if (o == varB.samples.end()) {
            sample[annotationTag].push_back("./."); // means "no genotype"
        } else {
            map<string, vector<string> >& other = o->second;
            string& otherGenotype = other["GT"].front();
            sample[annotationTag].push_back(otherGenotype);
        }
    }

}

int main(int argc, char** argv) {

    if (argc != 4) {
        cerr << "usage: " << argv[0] << " <annotation-tag> <vcf file> <vcf file>" << endl
             << "annotates genotypes in the first file with genotypes in the second" << endl
             << "adding the genotype as another flag to each sample filed in the first file." << endl
             << "annotation-tag is the name of the sample flag which is added to store the annotation." << endl
             << "also adds a 'has_variant' flag for sites where the second file has a variant." << endl;
        return 1;
    }

    string annotag = argv[1];
    string filenameA = argv[2];
    string filenameB = argv[3];

    if (filenameA == filenameB) {
        cerr << "it won't help to annotate samples with their own genotypes!" << endl;
        return 1;
    }

    VariantCallFile variantFileA;
    if (filenameA == "-") {
        variantFileA.open(std::cin);
    } else {
        variantFileA.open(filenameA);
    }

    VariantCallFile variantFileB;
    if (filenameB == "-") {
        variantFileB.open(std::cin);
    } else {
        variantFileB.open(filenameB);
    }

    if (!variantFileA.is_open() || !variantFileB.is_open()) {
        return 1;
    }

    Variant varA(variantFileA);
    Variant varB(variantFileB);

    // while the first file doesn't match the second positionally,
    // step forward, annotating each genotype record with an empty genotype
    // when the two match, iterate through the genotypes from the first file
    // and get the genotypes reported in the second file

    variantFileA.getNextVariant(varA);
    variantFileB.getNextVariant(varB);

    string line = "##INFO=<ID=" + annotag + ".has_variant,Number=0,Type=Flag,Description=\"True if "
        + annotag + " has a called alternate among samples under comparison.\">";
    variantFileA.addHeaderLine(line);

    cout << variantFileA.header << endl;

    if (variantFileA.done()) {
        cout << "is done" << endl;
    }

    if (variantFileB.done()) {
        cout << "is done" << endl;
    }

    do {

        while (!variantFileA.done()
                && varA.sequenceName < varB.sequenceName
                || (varA.sequenceName == varB.sequenceName && varA.position < varB.position)
                && !variantFileA.done()) {
            annotateWithBlankGenotypes(varA, annotag);
            cout << varA << endl;
            variantFileA.getNextVariant(varA);
        }

        while (!variantFileB.done() && varB.sequenceName < varA.sequenceName
                || (varB.sequenceName == varA.sequenceName && varB.position < varA.position)
                && !variantFileB.done()) {
            variantFileB.getNextVariant(varB);
        }

        while (!variantFileA.done() && varA.sequenceName == varB.sequenceName && varA.position == varB.position) {
            annotateWithGenotypes(varA, varB, annotag);
            // XXX assume that if the other file has a corresponding record, some kind of variation was detected at the same site
            varA.infoFlags[annotag + ".has_variant"] = true;
            cout << varA << endl;
            variantFileA.getNextVariant(varA);
        }

    } while (!variantFileA.done() && !variantFileB.done());

    return 0;

}

<commit_msg>fix annotation to examine genotypes of other VCF<commit_after>#include "Variant.h"
#include "split.h"
#include <string>
#include <iostream>

using namespace std;
using namespace vcf;

void annotateWithBlankGenotypes(Variant& var, string& annotationTag) {

    var.addFormatField(annotationTag);

    map<string, map<string, vector<string> > >::iterator s     = var.samples.begin();
    map<string, map<string, vector<string> > >::iterator sEnd  = var.samples.end();

    for (; s != sEnd; ++s) {
        map<string, vector<string> >& sample = s->second;
        sample[annotationTag].clear(); // means "no genotype" genotype
        sample[annotationTag].push_back("./."); // means "no genotype" genotype
    }
}

void annotateWithGenotypes(Variant& varA, Variant& varB, string& annotationTag) {

    varA.addFormatField(annotationTag);

    map<string, map<string, vector<string> > >::iterator s     = varA.samples.begin();
    map<string, map<string, vector<string> > >::iterator sEnd  = varA.samples.end();

    map<string, int> varAAlleleInts;
    int i = 1;
    for (vector<string>::iterator a = varA.alt.begin(); a != varA.alt.end(); ++a, ++i) {
        varAAlleleInts[*a] = i;
    }

    map<int, int> varBconvertToVarA; // maps alleles in the second file to allele numbers for the first
    varBconvertToVarA[0] = 0; // reference == reference!
    i = 1;
    for (vector<string>::iterator a = varB.alt.begin(); a != varB.alt.end(); ++a, ++i) {
        map<string, int>::iterator ita = varAAlleleInts.find(*a);
        if (ita != varAAlleleInts.end()) {
            varBconvertToVarA[i] = ita->second;
        }
    }

    for (; s != sEnd; ++s) {
        map<string, vector<string> >& sample = s->second;
        const string& name = s->first;
        map<string, map<string, vector<string> > >::iterator o = varB.samples.find(name);
        sample[annotationTag].clear();
        if (o == varB.samples.end()) {
            sample[annotationTag].push_back("./."); // means "no genotype"
        } else {
            map<string, vector<string> >& other = o->second;
            string& otherGenotype = other["GT"].front();
            // XXX this must compare the genotypes in the two files
            map<int, int> gtB = decomposeGenotype(otherGenotype);
            map<int, int> gtnew;
            for (map<int, int>::iterator g = gtB.begin(); g != gtB.end(); ++g) {
                map<int, int>::iterator f = varBconvertToVarA.find(g->first);
                if (f != varBconvertToVarA.end()) {
                    gtnew[f->second] += g->second;
                } else {
                    gtnew[-1] += g->second;
                }
            }
            sample[annotationTag].push_back(genotypeToString(gtnew));
        }
    }

}

int main(int argc, char** argv) {

    if (argc != 4) {
        cerr << "usage: " << argv[0] << " <annotation-tag> <vcf file> <vcf file>" << endl
             << "annotates genotypes in the first file with genotypes in the second" << endl
             << "adding the genotype as another flag to each sample filed in the first file." << endl
             << "annotation-tag is the name of the sample flag which is added to store the annotation." << endl
             << "also adds a 'has_variant' flag for sites where the second file has a variant." << endl;
        return 1;
    }

    string annotag = argv[1];
    string filenameA = argv[2];
    string filenameB = argv[3];

    if (filenameA == filenameB) {
        cerr << "it won't help to annotate samples with their own genotypes!" << endl;
        return 1;
    }

    VariantCallFile variantFileA;
    if (filenameA == "-") {
        variantFileA.open(std::cin);
    } else {
        variantFileA.open(filenameA);
    }

    VariantCallFile variantFileB;
    if (filenameB == "-") {
        variantFileB.open(std::cin);
    } else {
        variantFileB.open(filenameB);
    }

    if (!variantFileA.is_open() || !variantFileB.is_open()) {
        return 1;
    }

    Variant varA(variantFileA);
    Variant varB(variantFileB);

    // while the first file doesn't match the second positionally,
    // step forward, annotating each genotype record with an empty genotype
    // when the two match, iterate through the genotypes from the first file
    // and get the genotypes reported in the second file

    variantFileA.getNextVariant(varA);
    variantFileB.getNextVariant(varB);

    string line = "##INFO=<ID=" + annotag + ".has_variant,Number=0,Type=Flag,Description=\"True if "
        + annotag + " has a called alternate among samples under comparison.\">";
    variantFileA.addHeaderLine(line);

    cout << variantFileA.header << endl;

    if (variantFileA.done()) {
        cout << "is done" << endl;
    }

    if (variantFileB.done()) {
        cout << "is done" << endl;
    }

    do {

        while (!variantFileA.done()
                && varA.sequenceName < varB.sequenceName
                || (varA.sequenceName == varB.sequenceName && varA.position < varB.position)
                && !variantFileA.done()) {
            annotateWithBlankGenotypes(varA, annotag);
            cout << varA << endl;
            variantFileA.getNextVariant(varA);
        }

        while (!variantFileB.done() && varB.sequenceName < varA.sequenceName
                || (varB.sequenceName == varA.sequenceName && varB.position < varA.position)
                && !variantFileB.done()) {
            variantFileB.getNextVariant(varB);
        }

        while (!variantFileA.done() && varA.sequenceName == varB.sequenceName && varA.position == varB.position) {
            annotateWithGenotypes(varA, varB, annotag);
            // XXX assume that if the other file has a corresponding record, some kind of variation was detected at the same site
            varA.infoFlags[annotag + ".has_variant"] = true;
            cout << varA << endl;
            variantFileA.getNextVariant(varA);
        }

    } while (!variantFileA.done() && !variantFileB.done());

    return 0;

}

<|endoftext|>