text:
#include <fstream>
#include <tuple>
#include <chrono>

#include <ros/ros.h>
#include <rovi_planner/rovi_planner.h>
#include <geometry_msgs/Pose.h>
#include <tf_conversions/tf_eigen.h>
#include <eigen_conversions/eigen_kdl.h>
#include <kdl_conversions/kdl_msg.h>
#include <ur5_controllers/PoseTwist.h>

#include <Eigen/Eigen>

#include <rovi_utils/rovi_utils.h>
#include <rovi_gazebo/rovi_gazebo.h>
#include <ur5_controllers/interface.h>

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

	using namespace rovi_utils;
	using namespace std::chrono_literals;

	ros::init(argc, argv, "traj_test");
	ros::NodeHandle nh;

	// gripper test
	ROS_INFO("Grasping...");
	ur5_controllers::wsg::grasp();
	std::this_thread::sleep_for(1s);

	ROS_INFO("Releasing...");
	ur5_controllers::wsg::release();
	std::this_thread::sleep_for(1s);

	ROS_INFO("Grasping...");
	ur5_controllers::wsg::grasp();
	std::this_thread::sleep_for(0.5s);

	ROS_INFO("Releasing...");
	ur5_controllers::wsg::release();
	std::this_thread::sleep_for(0.5s);

	ROS_INFO("Grasping...");
	ur5_controllers::wsg::grasp();
	std::cin.ignore();

	// joint interpolation test

	// sensor_msgs::JointState state1, state2;
	// // state1.position = { 0.5, 1.0, 0.45, 3.14, 1.12, 1.23 };
	// state1.position = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.5 };
	// state2.position = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 };

	// auto traj_joint_lin = rovi_planner::traj_linear({ state1, state2 }, 1.0, 1.0, 0.001);
	// rovi_utils::export_traj(traj_joint_lin, "traj_jnt_test_lin.csv", 0.01);

	// auto traj_joint_par = rovi_planner::traj_parabolic({ state1, state2 }, 1.0, 1.0, 0.001, 0.001);
	// rovi_utils::export_traj(traj_joint_par, "traj_jnt_test_par.csv", 0.01);

	// create a linear, parabolic and rrt trajectories

	Eigen::Quaternion ori(1.0, 0.0, 0.0, 0.0);

	auto pose_tcp_current = rovi_gazebo::get_current_tcp_pose();
	auto pose_tcp_desired = rovi_utils::make_pose({ 0.1, 0.15, 0.90 }, ori);

	std::vector<geometry_msgs::Pose> waypoints =
	{
		pose_tcp_current,
		pose_tcp_desired
	};

	auto traj_lin = rovi_planner::traj_linear(waypoints, 0.1, 0.1, 0.05);
	auto traj_par = rovi_planner::traj_parabolic(waypoints, 0.1, 0.1, 0.05, 0.5);

	// auto rrt_plan = rovi_planner::traj_moveit(pose_tcp_desired, "RRTstar");
	// auto jnt_states = rovi_utils::joint_states_from_traj(*rrt_plan.trajectory_);
	// auto traj_rrt = rovi_planner::traj_parabolic(jnt_states, 1.0, 1.0, 0.001, 0.001);

	// export to file
	// rovi_utils::export_traj(traj_lin, "traj_lin.csv", 0.01);
	// rovi_utils::export_traj(traj_par, "traj_par.csv", 0.01);
	// rovi_utils::export_traj(traj_rrt, "traj_rrt.csv", 0.01);

	// command trajectory to robot at 100 Hz
	std::cout << "Press [ENTER] to execute trajectory...\n";
	std::cin.ignore();
	exit(0);

	// const auto pub = nh.advertise<ur5_controllers::PoseTwist>("/ur5_cartesian_pose_controller/command", 1);
	// ur5_controllers::PoseTwist msg;
	// ros::Rate lr(100); // Hz

	// for (auto [t, traj] = std::tuple{ 0.0, traj_rrt }; t < traj.Duration() and ros::ok(); t += 0.01)
	// {

	// 	ROS_INFO_STREAM_ONCE("Executing trajectory with duration: " << traj.Duration() << " sec");

	// 	// KDL Frame
	// 	const auto& frame = traj.Pos(t);
	// 	const auto& twist = traj.Vel(t);

	// 	// Convert from KDL to Eigen
	// 	tf::poseKDLToMsg(frame, msg.pose);
	// 	tf::twistKDLToMsg(twist, msg.twist);

	// 	pub.publish(msg);

	// 	lr.sleep();
	// }

	return 0;
}

max_stars_repo_name: martinandrovich/rb-rovi

max_issues_repo_name: martinandrovich/rb-rovi

max_forks_repo_name: martinandrovich/rb-rovi


text:
#include <boost/program_options.hpp>
#include <iostream>
#include <fstream>

#include "TFileWalker/Walker.h"
#include "TFileWalker/OutputHandler.h"
#include "TFileWalker/FileHandler.h"
#include "TFileWalker/RuleParser.h"

int main(int argc, char* argv[]) {
  std::string config; //> Optionally read command-line options from a config file
  std::string output; //> The output file location
  bool overwrite{false}; //> Allow overwriting the output file
  std::vector<std::string> inputFiles; //> The input files
  std::string inTxt; //> A text file containing a list of input files
  std::string rulesTxt; //> A text file containing a list of rules
  std::string inputLoc; //> The path all inputs are relative to
  bool noFileNameInRules; //> Is the input filename specified in the rules

  namespace po = boost::program_options;
  po::options_description desc("Allowed options");
  desc.add_options()
    ("config,c", po::value(&config), "Optionally read options from a config file")
    ("output,o", po::value(&output)->default_value("hist-out.root"), "The output ROOT file location")
    ("overwrite,w", po::bool_switch(&overwrite), "Enable overwriting of the output file")
    ("inTxt,I", po::value(&inTxt), "Text file containing inputs")
    ("rules,R", po::value(&rulesTxt), "Text file containing a list of targets")
    ("in-loc", po::value(&inputLoc)->default_value(""), "The location for input files")
    ("no-file-name-in-rules,F", po::bool_switch(&noFileNameInRules), "Rules do not reference the input file name")
    ("help,h", "Print this message and exit");
  po::options_description hidden("Hidden options");
  hidden.add_options()
    ("inputFiles", po::value(&inputFiles) );
  po::positional_options_description positional;
  positional.add("inputFiles", -1);
  po::variables_map vm;
  po::options_description allOptions;
  allOptions.add(desc).add(hidden);
  po::store(po::command_line_parser(argc, argv).options(allOptions).positional(positional).run(), vm);
  po::notify(vm);
  if (vm.count("config") ) {
    std::ifstream ifs(config);
    po::store(po::parse_config_file(ifs, allOptions), vm);
    po::notify(vm);
  }
  if (vm.count("help") ) {
    std::cout << desc << std::endl;
    return 0;
  }

  // Prepare input
  if (!inTxt.empty() ) {
    std::string line;
    std::ifstream inTxtFin(inTxt);
    if (!inTxtFin.is_open() ) {
      std::cerr << "Failed to open input text file " << inTxt << std::endl;
      return 1;
    }
    while (std::getline(inTxtFin, line) )
      if (!line.empty() )
        inputFiles.push_back(line);
  }

  TFileWalker::FileHandler inputHandler(inputLoc, "READ");

  TFileWalker::Walker walker(
      TFileWalker::RuleParser::parseFile(rulesTxt, !noFileNameInRules),
      TFileWalker::OutputHandler(output, overwrite ? "RECREATE" : "CREATE") );

  for (const std::string& fileName : inputFiles)
    walker.walk(inputHandler, fileName);

  walker.outputHandler().write();
}

ext: cxx

max_stars_repo_name: Jon-Burr/TFileWalker

max_issues_repo_name: Jon-Burr/TFileWalker

max_forks_repo_name: Jon-Burr/TFileWalker


text:
/*
 * This file is part of the DUDS project. It is subject to the BSD-style
 * license terms in the LICENSE file found in the top-level directory of this
 * distribution and at https://github.com/jjackowski/duds/blob/master/LICENSE.
 * No part of DUDS, including this file, may be copied, modified, propagated,
 * or distributed except according to the terms contained in the LICENSE file.
 *
 * Copyright (C) 2017  Jeff Jackowski
 */
#ifndef SPINLOCK_HPP
#define SPINLOCK_HPP

#include <atomic>
#include <thread>
#include <mutex>
#include <boost/noncopyable.hpp>

namespace duds { namespace general {

/**
 * A simple spinlock following the lockable and timed lockable concepts so that
 * it can be used with std::lock_guard, std::unique_lock, and std::lock.
 * Spinlock is a simple veneer over std::atomic_flag to keep the code using it
 * a little simpler. As long as the locks are held very briefly, or longer
 * locks are very uncommon, this should have less overhead than std::mutex.
 *
 * The spin lock can optionally call std::this_thread::yield() between attempts
 * to acquire the lock. The functions implementing the C++ concepts for
 * compatablity with the C++ lock objects will yield if the host system reports
 * that it can only run a single thread at a time
 * (std::thread::hardware_concurrency() == 1), and otherwise will not yeild.
 * Functions that always yield and never yield are also providied. In cases
 * where a yield will work better on all hosts, SpinlockYieldingWrapper can
 * be used with C++ lock objects to always yield.
 *
 * A class that uses a spin lock that may have a member function using the lock
 * when its destructor is called on another thread should declare the spin lock
 * as its last non-static member to delay destruction until after a lock can be
 * acquired.
 *
 * @note     Should a spinlock show up in the C++ libraries, this class will
 *           be deprecated.
 *
 * @warning  To promote preformance, there are no run-time checks to ensure
 *           proper usage. One thread could unlock what another thread locked.
 *           To help avoid misuse, std::lock_guard, std::unique_lock, or
 *           something similar should be used instead of directly calling
 *           functions on this class.
 *
 * @author  Jeff Jackowski
 */
class Spinlock : boost::noncopyable {
	/**
	 * Used like a mutually exclusive semaphore, but doesn't involve the
	 * operating system.
	 */
	std::atomic_flag af;
	/**
	 * True when lock() should call yield in its loop; good for uniprocessor,
	 * unicore systems.
	 */
	static bool useYield;
public:
	/**
	 * Makes a Spinlock in the unlocked state.
	 */
	Spinlock() {
		// staring from unknown state
		unlock();
		// state now unlocked
	}
	/**
	 * Locks the spinlock before destruction to delay destruction in case of
	 * a lock.
	 */
	~Spinlock() {
		// avoid destruction while in use
		lock();
	}
	/**
	 * A spiny busy wait that ends with ownership of the lock. This is best
	 * used on multi-processor systems; mult-core counts. On single processor
	 * systems, this will eat processor time until the operating system
	 * preempts the thread.
	 */
	void lockNeverYield() {
		// spiny wait
		while (af.test_and_set(std::memory_order_acquire)) { }
	}
	/**
	 * A yielding wait that ends with ownership of the lock. Every time the
	 * lock in not acquired, yield is called to allow other threads a chance
	 * to run, and maybe unlock the resource. This works well on single
	 * processor systems since if the lock is taken another thread must run to
	 * unlock it. On multi-processor systems, this could make the lock take
	 * longer.
	 */
	void lockAlwaysYield() {
		// not-so-spiny wait
		while (af.test_and_set(std::memory_order_acquire)) {
			std::this_thread::yield();
		}
	}
	/**
	 * A spiny busy or yielding wait that ends with ownership of the lock.
	 * Yield will be used if useYield is true, which is the default for single
	 * processor systems.
	 */
	void lock() {
		if (useYield) {
			lockAlwaysYield();
		} else {
			lockNeverYield();
		}
	}
	/**
	 * A single attempt at gaining ownership of the lock. This may not be very
	 * useful with a spinlock, but it was easy, so I implemented it.
	 * @return  True if ownership of the lock was granted.
	 */
	bool try_lock() {
		// non-spiny wait
		return !af.test_and_set(std::memory_order_acquire);
	}
	/**
	 * A spiny busy wait that ends with ownership of the lock if ownership can
	 * be granted before @a time.
	 * @param time  When to give up attempting to lock.
	 * @return  True if ownership of the lock was granted.
	 */
	template <class Clock, class Duration>
	bool tryLockNeverYeildUntil(const std::chrono::time_point<Clock,Duration> &time) {
		bool res;
		// spiny wait
		while ((res = af.test_and_set(std::memory_order_acquire)) &&
			(time <= std::chrono::time_point<Clock,Duration>::now())
		) { }
		return !res;
	}
	/**
	 * A yielding wait that ends with ownership of the lock if ownership can
	 * be granted before @a time.
	 * @param time  When to give up attempting to lock.
	 * @return  True if ownership of the lock was granted.
	 */
	template <class Clock, class Duration>
	bool tryLockAlwaysYeildUntil(const std::chrono::time_point<Clock,Duration> &time) {
		bool res;
		// not-so-spiny wait
		while ((res = af.test_and_set(std::memory_order_acquire)) &&
			(time <= std::chrono::time_point<Clock,Duration>::clock::now())
		) {
			std::this_thread::yield();
		}
		return !res;
	}
	/**
	 * A spiny busy wait or a yielding wait that ends with ownership of the
	 * lock if ownership can be granted before @a time.
	 * @param time  When to give up attempting to lock.
	 * @return  True if ownership of the lock was granted.
	 */
	template <class Clock, class Duration>
	bool try_lock_until(const std::chrono::time_point<Clock,Duration> &time) {
		if (useYield) {
			return tryLockAlwaysYeildUntil(time);
		} else {
			return tryLockNeverYeildUntil(time);
		}
	}
	/**
	 * A spiny busy wait that ends with ownership of the lock if ownership can
	 * be granted within @a duration.
	 * @param duration  The maximum time span to wait for the lock.
	 * @return  True if ownership of the lock was granted.
	 */
	template <class Rep, class Period>
	bool tryLockNeverYeildFor(const std::chrono::duration<Rep,Period> &duration) {
		return tryLockNeverYeildUntil(std::chrono::steady_clock::now() + duration);
	}
	/**
	 * A yielding wait that ends with ownership of the lock if ownership can
	 * be granted within @a duration.
	 * @param duration  The maximum time span to wait for the lock.
	 * @return  True if ownership of the lock was granted.
	 */
	template <class Rep, class Period>
	bool tryLockAlwaysYeildFor(const std::chrono::duration<Rep,Period> &duration) {
		return tryLockAlwaysYeildUntil(std::chrono::steady_clock::now() + duration);
	}
	/**
	 * A spiny busy wait or yielding wait that ends with ownership of the lock
	 * if ownership can be granted within @a duration.
	 * @param duration  The maximum time span to wait for the lock.
	 * @return  True if ownership of the lock was granted.
	 */
	template <class Rep, class Period>
	bool try_lock_for(const std::chrono::duration<Rep,Period> &duration) {
		if (useYield) {
			return tryLockAlwaysYeildFor(duration);
		} else {
			return tryLockNeverYeildFor(duration);
		}
	}
	/**
	 * Releases ownership of the lock.
	 */
	void unlock() {
		af.clear(std::memory_order_release);
	}
};

/**
 * A convenience typedef for a std::lock_guard using the Spinlock object.
 */
typedef std::lock_guard<duds::general::Spinlock> SpinLockGuard;

/**
 * A convenience typedef for a std::unique_lock using the Spinlock object.
 */
typedef std::unique_lock<duds::general::Spinlock> UniqueSpinLock;

/**
 * A simple wrapper around a Spinlock object that implements the timed lockable
 * concept such that attempts to lock an already locked Spinlock will always
 * yield before trying again. This is useful in cases where a spinlock gives
 * better performance a majority of the time, but seldom occuring longer
 * delays are possible. This class can be used when those longer delays are
 * known to occur to mitigate the performance issues of a spinlock. While
 * functions for yielding exist on Spinlock, this class calls them from the
 * lockable concept functions so that other classes like
 * std::condition_variable_any can yield between lock attempts.
 * @code
 * // something to lock
 * duds::general::Spinlock block;
 * // the wrapper
 * duds::general::SpinlockYieldingWrapper syw(block);
 * // lock using the wrapper; std::unique_lock will now always
 * // yield, even on a host with multiple processors
 * duds::general::UniqueYieldingSpinLock lock(syw);
 * @endcode
 * @author  Jeff Jackowski
 */
class SpinlockYieldingWrapper : boost::noncopyable {
	Spinlock &sl;
public:
	SpinlockYieldingWrapper(Spinlock &l) : sl(l) { }
	void lock() {
		sl.lockAlwaysYield();
	}
	bool try_lock() {
		return sl.try_lock();
	}
	template <class Clock, class Duration>
	bool try_lock_until(const std::chrono::time_point<Clock,Duration> &time) {
		return sl.tryLockAlwaysYeildUntil(time);
	}
	template <class Rep, class Period>
	bool try_lock_for(const std::chrono::duration<Rep,Period> &duration) {
		return sl.tryLockAlwaysYeildFor(duration);
	}
	void unlock() {
		sl.unlock();
	}
};

/**
 * A convenience typedef for a std::lock_guard using the Spinlock yielding
 * wrapper.
 */
typedef std::lock_guard<duds::general::SpinlockYieldingWrapper>
	YieldingSpinLockGuard;

/**
 * A convenience typedef for a std::unique_lock using the Spinlock yielding
 * wrapper.
 */
typedef std::unique_lock<duds::general::SpinlockYieldingWrapper>
	UniqueYieldingSpinLock;

} }

#endif        //  #ifndef SPINLOCK_HPP

ext: hpp

max_stars_repo_name: jjackowski/duds

max_issues_repo_name: jjackowski/duds

max_forks_repo_name: jjackowski/duds


text:
/*
 * Copyright (C) 2012 Open Source Robotics Foundation
 *
 * 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.
 *
*/
#ifdef _WIN32
  // Ensure that Winsock2.h is included before Windows.h, which can get
  // pulled in by anybody (e.g., Boost).
  #include <Winsock2.h>
#endif

#include <boost/bind.hpp>

#include "gazebo/common/MeshManager.hh"
#include "gazebo/transport/transport.hh"

#include "gazebo/rendering/Conversions.hh"
#include "gazebo/rendering/Scene.hh"
#include "gazebo/rendering/DynamicLines.hh"
#include "gazebo/rendering/SonarVisualPrivate.hh"
#include "gazebo/rendering/SonarVisual.hh"

using namespace gazebo;
using namespace rendering;

/////////////////////////////////////////////////
SonarVisual::SonarVisual(const std::string &_name, VisualPtr _vis,
                         const std::string &_topicName)
  : Visual(*new SonarVisualPrivate, _name, _vis)
{
  SonarVisualPrivate *dPtr =
      reinterpret_cast<SonarVisualPrivate *>(this->dataPtr);

  dPtr->type = VT_SENSOR;

  dPtr->receivedMsg = false;

  dPtr->node = transport::NodePtr(new transport::Node());
  dPtr->node->Init(dPtr->scene->Name());

  dPtr->sonarSub = dPtr->node->Subscribe(_topicName,
      &SonarVisual::OnMsg, this, true);

  dPtr->sonarRay = NULL;

  dPtr->connections.push_back(
      event::Events::ConnectPreRender(
        boost::bind(&SonarVisual::Update, this)));
}

/////////////////////////////////////////////////
SonarVisual::~SonarVisual()
{
}

/////////////////////////////////////////////////
void SonarVisual::Load()
{
  Visual::Load();

  SonarVisualPrivate *dPtr =
      reinterpret_cast<SonarVisualPrivate *>(this->dataPtr);

  dPtr->sonarRay = this->CreateDynamicLine(rendering::RENDERING_LINE_LIST);
  dPtr->sonarRay->setMaterial("Gazebo/RedGlow");
  dPtr->sonarRay->AddPoint(0, 0, 0);
  dPtr->sonarRay->AddPoint(0, 0, 0);

  dPtr->coneVis.reset(
      new Visual(this->Name() + "_SONAR_CONE_", shared_from_this(), false));
  dPtr->coneVis->Load();
  dPtr->coneVis->InsertMesh("unit_cone");
  dPtr->coneVis->AttachMesh("unit_cone");
  dPtr->coneVis->SetMaterial("Gazebo/BlueLaser");
  dPtr->coneVis->SetCastShadows(false);

  this->SetVisibilityFlags(GZ_VISIBILITY_GUI);
}

/////////////////////////////////////////////////
void SonarVisual::OnMsg(ConstSonarStampedPtr &_msg)
{
  SonarVisualPrivate *dPtr =
      reinterpret_cast<SonarVisualPrivate *>(this->dataPtr);

  boost::mutex::scoped_lock lock(dPtr->mutex);
  dPtr->sonarMsg = _msg;
  dPtr->receivedMsg = true;
}

/////////////////////////////////////////////////
void SonarVisual::Update()
{
  SonarVisualPrivate *dPtr =
      reinterpret_cast<SonarVisualPrivate *>(this->dataPtr);

  boost::mutex::scoped_lock lock(dPtr->mutex);

  if (!dPtr->sonarMsg || !dPtr->receivedMsg)
    return;

  // Skip the update if the user is moving the sonar.
  if (this->GetScene()->SelectedVisual() &&
      this->GetRootVisual()->Name() ==
      this->GetScene()->SelectedVisual()->Name())
  {
    return;
  }

  double rangeDelta = dPtr->sonarMsg->sonar().range_max()
      - dPtr->sonarMsg->sonar().range_min();
  double radiusScale = dPtr->sonarMsg->sonar().radius()*2.0;

  if (!ignition::math::equal(dPtr->coneVis->Scale().Z(), rangeDelta) ||
      !ignition::math::equal(dPtr->coneVis->Scale().X(), radiusScale))
  {
    dPtr->coneVis->SetScale(
        ignition::math::Vector3d(radiusScale, radiusScale, rangeDelta));
    dPtr->sonarRay->SetPoint(0,
        ignition::math::Vector3d(0, 0, rangeDelta * 0.5));
  }

  ignition::math::Pose3d pose =
    msgs::ConvertIgn(dPtr->sonarMsg->sonar().world_pose());
  this->SetPose(pose);

  if (dPtr->sonarMsg->sonar().has_contact())
  {
    ignition::math::Vector3d pos =
      msgs::ConvertIgn(dPtr->sonarMsg->sonar().contact());
    dPtr->sonarRay->SetPoint(1, pos);
  }
  else
  {
    dPtr->sonarRay->SetPoint(1, ignition::math::Vector3d(0, 0,
          (rangeDelta * 0.5) - dPtr->sonarMsg->sonar().range()));
  }
  dPtr->receivedMsg = false;
}

ext: cc


text:
/**
 *  @copyright Copyright 2018 The J-PET Framework Authors. All rights reserved.
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may find a copy of the License in the LICENCE file.
 *
 *  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.
 *
 *  @file JPetParamBankHandlerTaskTest.cpp
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE JPetParamBankHandlerTask

#include "JPetParamBankHandlerTask/JPetParamBankHandlerTask.h"
#include "JPetCommonTools/JPetCommonTools.h"
#include "JPetOptionsGenerator/JPetOptionsGeneratorTools.h"
#include "JPetParamGetterAscii/JPetParamGetterAscii.h"
#include "JPetParamManager/JPetParamManager.h"
#include "JPetParams/JPetParams.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(FirstSuite)

BOOST_AUTO_TEST_CASE(goodRootFile)
{
  JPetParamBankHandlerTask task;
  auto options = jpet_options_generator_tools::getDefaultOptions();
  options["runId_int"] = -1;
  options["inputFileType_std::string"] = std::string("root");
  options["inputFile_std::string"] = std::string("unitTestData/JPetTaskChainExecutorTest/dabc_17025151847.unk.evt.root");
  std::shared_ptr<JPetParamManager> manager = std::make_shared<JPetParamManager>();
  JPetParams params(options, manager);
  auto& bank = manager->getParamBank();
  BOOST_REQUIRE(bank.isDummy());
  BOOST_REQUIRE(task.init(params));
  auto& bank2 = manager->getParamBank();
  BOOST_REQUIRE(!bank2.isDummy());
}

BOOST_AUTO_TEST_CASE(badRootFile)
{
  gErrorIgnoreLevel = kFatal; /// To turn off ROOT error reporting.
  JPetParamBankHandlerTask task;
  auto options = jpet_options_generator_tools::getDefaultOptions();
  options["runId_int"] = -1;
  options["inputFileType_std::string"] = std::string("root");
  options["inputFile_std::string"] = std::string("unitTestData/auxdata.root");
  std::shared_ptr<JPetParamManager> manager = std::make_shared<JPetParamManager>();
  JPetParams params(options, manager);
  auto& bank = manager->getParamBank();
  BOOST_REQUIRE(bank.isDummy());
  BOOST_REQUIRE(!task.init(params));
  auto& bank2 = manager->getParamBank();
  BOOST_REQUIRE(bank2.isDummy());
  gErrorIgnoreLevel = kPrint; /// Turning back the ROOT error reporting.
}

BOOST_AUTO_TEST_CASE(badHldFile)
{
  JPetParamBankHandlerTask task;
  auto options = jpet_options_generator_tools::getDefaultOptions();
  options["inputFileType_std::string"] = std::string("hld");
  std::shared_ptr<JPetParamManager> manager = std::make_shared<JPetParamManager>();
  JPetParams params(options, manager);
  auto& bank = manager->getParamBank();
  BOOST_REQUIRE(bank.isDummy());
  BOOST_REQUIRE(!task.init(params));
  auto& bank2 = manager->getParamBank();
  BOOST_REQUIRE(bank2.isDummy());
}

BOOST_AUTO_TEST_CASE(goodUnknownFileFromFile)
{
  JPetParamBankHandlerTask task;
  auto options = jpet_options_generator_tools::getDefaultOptions();
  options["inputFile_std::string"] = std::string("unitTestData/JPetTaskChainExecutorTest/dabc_17025151847.unk.evt.root");
  options["inputFileType_std::string"] = std::string("unknown");
  std::shared_ptr<JPetParamManager> manager = std::make_shared<JPetParamManager>();
  JPetParams params(options, manager);
  auto& bank = manager->getParamBank();
  BOOST_REQUIRE(bank.isDummy());
  BOOST_REQUIRE(task.init(params));
  auto& bank2 = manager->getParamBank();
  BOOST_REQUIRE(!bank2.isDummy());
}

BOOST_AUTO_TEST_CASE(badUnknownFileFromFile)
{
  gErrorIgnoreLevel = kFatal; /// To turn off ROOT error reporting.
  JPetParamBankHandlerTask task;
  auto options = jpet_options_generator_tools::getDefaultOptions();
  options["inputFile_std::string"] = std::string("unitTestData/auxdata.root");
  options["inputFileType_std::string"] = std::string("unknown");
  std::shared_ptr<JPetParamManager> manager = std::make_shared<JPetParamManager>();
  JPetParams params(options, manager);
  auto& bank = manager->getParamBank();
  BOOST_REQUIRE(bank.isDummy());
  BOOST_REQUIRE(!task.init(params));
  auto& bank2 = manager->getParamBank();
  BOOST_REQUIRE(bank2.isDummy());
  gErrorIgnoreLevel = kPrint; /// Turning back the ROOT error reporting.
}

BOOST_AUTO_TEST_CASE(goodUnknownFileFromConfig)
{
  gErrorIgnoreLevel = kFatal; /// To turn off ROOT error reporting.
  JPetParamBankHandlerTask task;
  auto options = jpet_options_generator_tools::getDefaultOptions();
  options["runId_int"] = 44;
  options["localDB_std::string"] = std::string("unitTestData/JPetParamBankHandlerTask/large_barrel.json");
  options["inputFile_std::string"] = std::string("unitTestData/auxdata.root");
  options["inputFileType_std::string"] = std::string("unknown");
  std::shared_ptr<JPetParamManager> manager =
      std::make_shared<JPetParamManager>(new JPetParamGetterAscii("unitTestData/JPetParamBankHandlerTask/large_barrel.json"));
  JPetParams params(options, manager);
  auto& bank = manager->getParamBank();
  BOOST_REQUIRE(bank.isDummy());
  BOOST_REQUIRE(task.init(params));
  auto& bank2 = manager->getParamBank();
  BOOST_REQUIRE(!bank2.isDummy());
  gErrorIgnoreLevel = kPrint; /// Turning back the ROOT error reporting.
}

BOOST_AUTO_TEST_CASE(badUnknownFileFromConfig)
{
  gErrorIgnoreLevel = kFatal; /// To turn off ROOT error reporting.
  JPetParamBankHandlerTask task;
  auto options = jpet_options_generator_tools::getDefaultOptions();
  options["runId_int"] = 1;
  options["localDB_std::string"] = std::string("unitTestData/JPetParamBankHandlerTask/large_barrel.json");
  options["inputFile_std::string"] = std::string("unitTestData/auxdata.root");
  options["inputFileType_std::string"] = std::string("unknown");
  std::shared_ptr<JPetParamManager> manager =
      std::make_shared<JPetParamManager>(new JPetParamGetterAscii("unitTestData/JPetParamBankHandlerTask/large_barrel.json"));
  JPetParams params(options, manager);
  auto& bank = manager->getParamBank();
  BOOST_REQUIRE(bank.isDummy());
  BOOST_REQUIRE(task.init(params));
  auto& bank2 = manager->getParamBank();
  BOOST_REQUIRE(!bank2.isDummy());
  gErrorIgnoreLevel = kPrint; /// Turning back the ROOT error reporting.
}

BOOST_AUTO_TEST_CASE(goodHldFile)
{
  JPetParamBankHandlerTask task;
  auto options = jpet_options_generator_tools::getDefaultOptions();
  options["runId_int"] = 44;
  options["localDB_std::string"] = std::string("unitTestData/JPetParamBankHandlerTask/large_barrel.json");
  options["inputFileType_std::string"] = std::string("hld");
  std::shared_ptr<JPetParamManager> manager =
      std::make_shared<JPetParamManager>(new JPetParamGetterAscii("unitTestData/JPetParamBankHandlerTask/large_barrel.json"));
  JPetParams params(options, manager);
  auto& bank = manager->getParamBank();
  BOOST_REQUIRE(bank.isDummy());
  BOOST_REQUIRE(task.init(params));
  auto& bank2 = manager->getParamBank();
  BOOST_REQUIRE(!bank2.isDummy());
}

BOOST_AUTO_TEST_SUITE_END()

ext: cpp

max_stars_repo_name: BlurredChoise/j-pet-framework

max_issues_repo_name: BlurredChoise/j-pet-framework

max_forks_repo_name: BlurredChoise/j-pet-framework


text:
/*
 * Copyright 2015 Aldebaran
 *
 * 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.
 *
*/

/*
* LOCAL includes
*/
#include <naoqi_driver/recorder/globalrecorder.hpp>
#include "../helpers/recorder_helpers.hpp"

/*
* STANDARD includes
*/
#include <ctime>
#include <sstream>

/*
* BOOST includes
*/
#include <boost/filesystem.hpp>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

/*
* ALDEBARAN includes
*/
#include <qi/log.hpp>

qiLogCategory("ros.Recorder");

namespace naoqi
{
namespace recorder
{

  GlobalRecorder::GlobalRecorder(const std::string& prefix_topic):
    _processMutex()
  , _nameBag("")
  , _isStarted(false)
  // , _writer()
  {
    if (!prefix_topic.empty())
    {
      _prefix_topic = "/"+prefix_topic+"/";
    }
    else
    {
      _prefix_topic = "/";
    }
  }

  void GlobalRecorder::startRecord(const std::string& prefix_bag) {
    boost::mutex::scoped_lock startLock( _processMutex );
    if (!_isStarted) {
      try {
        // Get current path
        boost::filesystem::path cur_path( boost::filesystem::current_path() );

        // Get time
        time_t rawtime;
        struct tm * timeinfo;
        char buffer[80];
        std::time(&rawtime);
        timeinfo = std::localtime(&rawtime);
        std::strftime(buffer,80,"%d-%m-%Y_%I:%M:%S",timeinfo);

        if (!prefix_bag.empty()) {
          _nameBag = cur_path.string()+"/"+prefix_bag+"_"+buffer;
        }
        else {
          _nameBag = cur_path.string()+"/"+buffer;
        }
        _nameBag.append(".bag");
        // rosbag2::StorageOptions storage_options({_nameBag, ""});

        // _bag.open(_nameBag, rosbag::bagmode::Write);
        // _writer.open(storage_options, {"", "rmw1_format"});
        _isStarted = true;
        std::cout << YELLOW << "The bag " << BOLDCYAN << _nameBag << RESETCOLOR << YELLOW << " is opened" << RESETCOLOR << std::endl;
      } catch (std::exception e){
        throw std::runtime_error(e.what());
      }
    }
    else {
      qiLogError() << "Cannot start a record. The module is already recording.";
    }
  }

  std::string GlobalRecorder::stopRecord(const std::string& robot_ip) {
    boost::mutex::scoped_lock stopLock( _processMutex );
    if (_isStarted) {
      // _bag.close();
      _isStarted = false;

      std::stringstream message;
      message << _nameBag;
      std::cout << YELLOW << "The bag " << BOLDCYAN << _nameBag << RESETCOLOR << YELLOW << " is closed" << RESETCOLOR << std::endl;

      // Check if we are on a robot
      char* current_path;
      current_path = getenv("HOME");
      std::string cp = current_path;
      if (!(cp.find("nao") == std::string::npos)) {
        std::cout << BOLDRED << "To download this bag on your computer:" << RESETCOLOR << std::endl
                     << GREEN << "\t$ scp nao@" << robot_ip << ":" << _nameBag << " <LOCAL_PATH>" << RESETCOLOR
                        << std::endl;
      }

      _nameBag.clear();
      return message.str();
    }
    else {
      qiLogError() << "Cannot stop recording while it has not been started.";
      return "Cannot stop recording while it has not been started.";
    }
  }

  bool GlobalRecorder::isStarted() {
    return _isStarted;
  }

  void GlobalRecorder::write(const std::string& topic, const std::vector<geometry_msgs::msg::TransformStamped>& msgtf) {
    if (!msgtf.empty())
    {
      std::string ros_topic;
      if (topic[0]!='/')
      {
        ros_topic = _prefix_topic+topic;
      }
      else
      {
        ros_topic = topic;
      }
      tf2_msgs::msg::TFMessage message;
      rclcpp::Time now = helpers::Time::now();
      if (!helpers::recorder::isZero(msgtf[0].header.stamp)) {
        now = msgtf[0].header.stamp;
      }
      for (std::vector<geometry_msgs::msg::TransformStamped>::const_iterator it = msgtf.begin(); it != msgtf.end(); ++it)
      {
        message.transforms.push_back(*it);
      }
      boost::mutex::scoped_lock writeLock( _processMutex );
      if (_isStarted) {
        // _bag.write(ros_topic, now, message);
      }
    }
  }

} // recorder
} // naoqi


text:
//
// RemotePhotoTool - remote camera control software
// Copyright (C) 2008-2018 Michael Fink
//
/// \file SerialPort.hpp serial port classes
//
#pragma once

#include <boost/noncopyable.hpp>
#include <memory>
#include <functional>
#include <ulib/config/BoostAsio.hpp>
#include "SerialPortInfo.hpp"
#include "SerialPortConfig.hpp"
#include <vector>

/// \brief Serial port classes
namespace Serial
{
   /// \brief Serial port class
   /// Implements access to serial port, including synchronous reading and writing, as well as
   /// asynchronous reading. Serial port enumeration is supported via EnumPorts().
   class SerialPort : public boost::noncopyable
   {
   public:
      /// handler function type for asynchronous receive operation
      typedef std::function<void(int errorCode, const std::vector<BYTE>& data, size_t bytesTransferred)> T_fnAsyncReceiveHandler;

      /// ctor opens serial port
      SerialPort(LPCTSTR deviceName, boost::asio::io_service& service);

      /// dtor; closes serial port
      ~SerialPort();

      /// enumerates all available serial ports
      static std::vector<SerialPortInfo> EnumSerialPorts();

      /// returns config object for this port
      SerialPortConfig Config();

      /// returns if socket is open
      bool IsOpen() const;

      /// sends a break sequence
      void SendBreak();

      /// sends data and returns when bytes are written
      void Send(const std::vector<BYTE>& data);

      /// receives available data
      void Receive(std::vector<BYTE>& data, unsigned int numMaxSize = 512);

      /// starts asynchronous receive operation; handler is called when operation finishes
      void AsyncReceive(T_fnAsyncReceiveHandler asyncReceiveHandler, unsigned int numMaxSize = 512);

      /// closes the port
      void Close();

   private:
      friend class SerialPortConfig;

      class Impl;

      /// implementation
      std::shared_ptr<Impl> m_spImpl;
   };

} // namespace Serial

max_stars_repo_name: vividos/RemotePhotoTool

max_issues_repo_name: vividos/RemotePhotoTool

max_forks_repo_name: vividos/RemotePhotoTool


text:
#include <iostream>
#include <boost/scope_exit.hpp>
#include <fts.h>

int main() {
    char const* argv[] = { ".", nullptr };
    auto const ftsp = fts_open(const_cast<char* const*>(argv), FTS_NOCHDIR | FTS_PHYSICAL | FTS_XDEV, nullptr);
    if (!ftsp) return 1;
    BOOST_SCOPE_EXIT_ALL(&) { fts_close(ftsp); };

    auto const chp = fts_children(ftsp, 0);
    if (!chp) return 1;

    FTSENT* p;
    while ((p = fts_read(ftsp))) {
        if (p->fts_info == FTS_F) {
            std::cout << "fts_name = " << p->fts_name << "\n"
                      << "fts_path = " << p->fts_path << "\n"
                      << std::endl;
        }
    }

    return 0;
}


text:
#include <iostream>
#include <typeinfo>

#include <map>

using std::cout;
using std::endl;

#include "viennagrid/forwards.hpp"

#include "viennagrid/storage/view.hpp"
#include "viennagrid/storage/container_collection.hpp"
#include "viennagrid/storage/inserter.hpp"
#include "viennagrid/storage/id_generator.hpp"
#include "viennagrid/storage/hidden_key_map.hpp"
#include "viennagrid/storage/range.hpp"

#include "viennagrid/topology/vertex.hpp"
#include "viennagrid/topology/line.hpp"
#include "viennagrid/topology/simplex.hpp"

#include "viennagrid/element/element_key.hpp"
#include "viennagrid/element/element_orientation.hpp"

#include "viennagrid/config/element_config.hpp"
#include "viennagrid/config/mesh_config.hpp"

#include "viennagrid/point.hpp"

#include "viennagrid/mesh/mesh.hpp"
#include "viennagrid/mesh/element_creation.hpp"

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
namespace ublas = boost::numeric::ublas;

typedef ublas::vector<double> vector_type;

class my_mesh_config
{
  private:
    //typedef viennagrid::storage::pointer_handle_tag handle_tag;
    //typedef viennagrid::storage::iterator_handle_tag handle_tag;
    typedef viennagrid::storage::id_handle_tag          handle_tag;

  public:

    typedef viennagrid::config::result_of::full_mesh_config< viennagrid::tetrahedron_tag, vector_type, handle_tag >::type     type;
};

int main()
{

    //
    // typedefing and setting up the topological mesh
    //

    typedef viennagrid::mesh< my_mesh_config > mesh_type;
    mesh_type mesh;

    //
    // typedefs for the element types
    //

    typedef viennagrid::result_of::element<mesh_type, viennagrid::vertex_tag>::type vertex_type;
    typedef viennagrid::result_of::handle<mesh_type, viennagrid::vertex_tag>::type vertex_handle_type;

    typedef viennagrid::result_of::element<mesh_type, viennagrid::line_tag>::type line_type;
    typedef viennagrid::result_of::handle<mesh_type, viennagrid::line_tag>::type line_handle_type;

    typedef viennagrid::result_of::element<mesh_type, viennagrid::triangle_tag>::type triangle_type;
    typedef viennagrid::result_of::handle<mesh_type, viennagrid::triangle_tag>::type triangle_handle_type;

    typedef viennagrid::result_of::element<mesh_type, viennagrid::tetrahedron_tag>::type tetrahedron_type;
    typedef viennagrid::result_of::handle<mesh_type, viennagrid::tetrahedron_tag>::type tetrahedron_handle_type;

    //
    // typedefs for the view
    //

    // only vertices and tetrahedrons are stored in the view
    typedef viennagrid::result_of::mesh_view<mesh_type, vertex_type, tetrahedron_type>::type geometric_view_type;

    //geometric_view_type geometric_view( viennagrid::make_topologic_view<topologic_view_type>( viennagrid::topology(mesh) ), mesh.get_metainfo_collection() );

    geometric_view_type geometric_view_1 = viennagrid::make_view(mesh);
    geometric_view_type geometric_view_2 = viennagrid::make_view(mesh);

    //
    // Adding a tetrahedron
    //

    std::vector<vertex_handle_type> handles(4);

    // creates four vertices within the mesh, vh is short vor vertex handle
    // make_element is responsible for resizing all meta-info container which are associated with vertex_type
    vertex_handle_type vh0 = viennagrid::make_vertex( geometric_view_1 );
    vertex_handle_type vh1 = viennagrid::make_vertex( geometric_view_1 );
    vertex_handle_type vh2 = viennagrid::make_vertex( geometric_view_1 );
    vertex_handle_type vh3 = viennagrid::make_vertex( geometric_view_1 );

    // create geometric information for the vertices
    vector_type p0(3);
    p0[0] = 0.0; p0[1] = 0.0; p0[2] = 0.0;
    vector_type p1(3);
    p1[0] = 1.0; p1[1] = 0.0; p1[2] = 0.0;
    vector_type p2(3);
    p2[0] = 0.0; p2[1] = 1.0; p2[2] = 0.0;
    vector_type p3(3);
    p3[0] = 0.0; p3[1] = 0.0; p3[2] = 1.0;

    // set the geometric information for the vertices
    // is equivalent to viennagrid::look_up<vector_type>(mesh, vhX)
    viennagrid::point(geometric_view_1, vh0) = p0;
    viennagrid::point(geometric_view_1, vh1) = p1;
    viennagrid::point(geometric_view_1, vh2) = p2;
    viennagrid::point(geometric_view_1, vh3) = p3;

    // pushing a tetrahedron to mesh_view_1
    handles[0] = vh0;
    handles[1] = vh1;
    handles[2] = vh2;
    handles[3] = vh3;

    // creates the tetrahedron within the mesh, all boundary cell generation is done here implicit
    viennagrid::make_element<tetrahedron_type>( geometric_view_1, handles.begin(), handles.end() );

    // creates four vertices within the mesh, vh is short vor vertex handle
    // make_element is responsible for resizing all meta-info container which are associated with vertex_type
    vertex_handle_type vh4 = viennagrid::make_vertex( geometric_view_2 );
    vertex_handle_type vh5 = viennagrid::make_vertex( geometric_view_2 );
    vertex_handle_type vh6 = viennagrid::make_vertex( geometric_view_2 );
    vertex_handle_type vh7 = viennagrid::make_vertex( geometric_view_2 );

    // create geometric information for the vertices
    vector_type p4(3);
    p4[0] = 40.0; p4[1] = 40.0; p4[2] = 40.0;
    vector_type p5(3);
    p5[0] = 41.0; p5[1] = 40.0; p5[2] = 40.0;
    vector_type p6(3);
    p6[0] = 40.0; p6[1] = 41.0; p6[2] = 40.0;
    vector_type p7(3);
    p7[0] = 40.0; p7[1] = 40.0; p7[2] = 41.0;

    // set the geometric information for the vertices
    // is equivalent to viennagrid::look_up<vector_type>(mesh, vhX)
    viennagrid::point(geometric_view_2, vh4) = p4;
    viennagrid::point(geometric_view_2, vh5) = p5;
    viennagrid::point(geometric_view_2, vh6) = p6;
    viennagrid::point(geometric_view_2, vh7) = p7;

    // pushing a tetrahedron to mesh_view_1
    handles[0] = vh4;
    handles[1] = vh5;
    handles[2] = vh6;
    handles[3] = vh7;

    // creates the tetrahedron within the mesh, all boundary cell generation is done here implicit
    viennagrid::make_element<tetrahedron_type>( geometric_view_2, handles.begin(), handles.end() );

    //
    // output
    //

    cout << "All tetrahedrons of the mesh" << endl;
    std::copy( viennagrid::elements<viennagrid::tetrahedron_tag>(mesh).begin(), viennagrid::elements<viennagrid::tetrahedron_tag>(mesh).end(), std::ostream_iterator<tetrahedron_type>(cout, "\n") );
    cout << endl;

    cout << "All tetrahedrons of the geometric_view_1" << endl;
    std::copy( viennagrid::elements<viennagrid::tetrahedron_tag>(geometric_view_1).begin(), viennagrid::elements<viennagrid::tetrahedron_tag>(geometric_view_1).end(), std::ostream_iterator<tetrahedron_type>(cout, "\n") );
    cout << endl;

    cout << "All tetrahedrons of the geometric_view_2" << endl;
    std::copy( viennagrid::elements<viennagrid::tetrahedron_tag>(geometric_view_2).begin(), viennagrid::elements<viennagrid::tetrahedron_tag>(geometric_view_2).end(), std::ostream_iterator<tetrahedron_type>(cout, "\n") );
    cout << endl;

    //
    // geometric iteration
    //

    // iterating over all vertices and piping out the point information
    typedef viennagrid::result_of::const_element_range<mesh_type, viennagrid::vertex_tag>::type mesh_vertex_range;
    typedef viennagrid::result_of::iterator<mesh_vertex_range>::type mesh_vertex_iterator;

    cout << "All vertices of the geometric_view_1 with geometric info" << endl;
    mesh_vertex_range mesh_vtx_range = viennagrid::elements(mesh);
    for (mesh_vertex_iterator it = mesh_vtx_range.begin(); it != mesh_vtx_range.end(); ++it)
        cout << *it << " geometric information: " << viennagrid::point( mesh, *it ) << endl;
    cout << endl;

    typedef viennagrid::result_of::const_element_range<geometric_view_type, viennagrid::vertex_tag>::type view_vertex_range;
    typedef viennagrid::result_of::const_iterator<view_vertex_range>::type view_vertex_iterator;

    // iterating over all vertices and piping out the point information
    cout << "All vertices of the geometric_view_1 with geometric info" << endl;
    view_vertex_range view_vtx_range_1 = viennagrid::elements(geometric_view_1);
    for (view_vertex_iterator it = view_vtx_range_1.begin(); it != view_vtx_range_1.end(); ++it)
        cout << *it << " geometric information: " << viennagrid::point( mesh, *it ) << endl;
    cout << endl;

    // iterating over all vertices and piping out the point information
    cout << "All vertices of the geometric_view_2 with geometric info" << endl;
    view_vertex_range view_vtx_range_2 = viennagrid::elements(geometric_view_2);
    for (view_vertex_iterator it = view_vtx_range_2.begin(); it != view_vtx_range_2.end(); ++it)
        cout << *it << " geometric information: " << viennagrid::point( mesh, *it ) << endl;
    cout << endl;

    return 0;
}

max_stars_repo_name: viennagrid/viennagrid-dev

max_issues_repo_name: viennagrid/viennagrid-dev

max_forks_repo_name: viennagrid/viennagrid-dev


text:
// Copyright 2010 Dean Michael Berris.
// Copyright 2012 Google, Inc.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef NETWORK_PROTOCOL_HTTP_MESSAGE_MODIFIERS_MINOR_VERSION_HPP_20101120
#define NETWORK_PROTOCOL_HTTP_MESSAGE_MODIFIERS_MINOR_VERSION_HPP_20101120

#include <network/protocol/http/support/is_server.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/cstdint.hpp>

namespace network {
namespace http {

template <class Tag> struct basic_request;

template <class Tag>
inline typename enable_if<is_server<Tag>, void>::type minor_version(
    basic_request<Tag>& request,
    boost::uint8_t minor_version_) {
  request.http_version_minor = minor_version_;
}

}  // namespace http
}  // namespace network

#endif /* NETWORK_PROTOCOL_HTTP_MESSAGE_MODIFIERS_MINOR_VERSION_HPP_20101120 */

max_stars_repo_name: project-zerus/cpp-netlib

max_issues_repo_name: project-zerus/cpp-netlib

max_forks_repo_name: project-zerus/cpp-netlib


text:
/*
 * (C) Copyright 2020 UCAR
 *
 * This software is licensed under the terms of the Apache Licence Version 2.0
 * which can be obtained at http://www.apache.org/licenses/LICENSE-2.0.
 */
#include <Eigen/Dense>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <vector>

#include "ioda/Engines/Factory.h"
#include "ioda/Exception.h"
#include "ioda/Group.h"

// These tests really need a better check system.
// Boost unit tests would have been excellent here.

/// Run a series of tests on the input group.
void test_group_backend_engine(ioda::Group g) {
  // Create a group structure with some hierarchy
  auto gObsValue = g.create("ObsValue");
  auto gObsError = g.create("ObsError");
  auto gMetaData = g.create("MetaData");

  auto gMdChild = gMetaData.create("Child 1");

  // Create some variables
  ioda::VariableCreationParameters params;
  params.setFillValue<double>(-999);
  params.chunk = true;
  params.compressWithGZIP();

  auto obsVar = gObsValue.vars.create<double>("myobs", { 2, 2 }, { 2, 2 }, params)
                         .write<double>({ 1.0, 2.0, 3.0, 4.0 });
  auto errVar = gObsError.vars.create<double>("myobs", { 2, 2 }, { 2, 2 }, params)
                         .write<double>({ 0.5, 0.1, 0.05, 0.01 });
  auto latVar = gMetaData.vars.create<double>("latitude", { 2, 2 }, { 2, 2 }, params)
                         .write<double>({ 1.5, 2.5, 3.5, 4.5 });

  // Can we list groups?
  auto g_list = g.list();
  if (g_list.size() != 3) throw ioda::Exception(ioda_Here());
  // Can we list another way?
  auto g_list2 = g.listObjects();
  if (g_list2.empty()) throw ioda::Exception(ioda_Here());

  // Can we list groups and variables recursively?
  auto g_list3 = g.listObjects(ioda::ObjectType::Ignored, true);
  if (g_list3.empty()) throw ioda::Exception(ioda_Here());

  // Can we list variables recursively (templated form)?
  auto g_list4 = g.listObjects<ioda::ObjectType::Variable>(true);
  if (g_list4.empty()) throw ioda::Exception(ioda_Here());
}

int main(int argc, char** argv) {
  using namespace ioda;
  using namespace std;
  try {
    auto f = Engines::constructFromCmdLine(argc, argv, "test-list_objects.hdf5");

    test_group_backend_engine(f);

  } catch (const std::exception& e) {
    ioda::unwind_exception_stack(e);
    return 1;
  }
  return 0;
}

max_stars_repo_name: NOAA-EMC/ioda

max_issues_repo_name: NOAA-EMC/ioda

max_forks_repo_name: NOAA-EMC/ioda


text:
#ifndef _LEXICAL_HPP_
#define _LEXICAL_HPP_

#include <string>
#include <boost/regex.hpp>

class Lexical
{
public:
	Lexical();
	virtual ~Lexical();
	bool CheckLabel(const std::string& label, const int& line);

private:
	static std::string regex_label_str;
	static boost::regex regex_label;

};

#endif //_LEXICAL_HPP_


text:
//
// Copyright (c) 2021 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/CPPAlliance/http_proto
//

#ifndef BOOST_HTTP_PROTO_CODEC_CODECS_HPP
#define BOOST_HTTP_PROTO_CODEC_CODECS_HPP

#include <boost/http_proto/detail/config.hpp>
#include <boost/http_proto/string_view.hpp>

namespace boost {
namespace http_proto {

#ifndef BOOST_HTTP_PROTO_DOCS
class decoder_type;
class encoder_type;
#endif

class codecs
{
public:
    BOOST_HTTP_PROTO_DECL
    virtual ~codecs() noexcept;

    virtual
    void
    add_decoder(
        string_view name,
        decoder_type&) = 0;

    virtual
    decoder_type*
    find_decoder(
        string_view name) noexcept = 0;

    virtual
    void
    add_encoder(
        string_view name,
        encoder_type&) = 0;

    virtual
    encoder_type*
    find_encoder(
        string_view name) noexcept = 0;
};

} // http_proto
} // boost

#endif


text:
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/module.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>
#include<cctbx/anharmonic.h>

namespace cctbx { namespace boost_python { namespace anharmonic {
  using namespace cctbx::adptbx::anharmonic;

  template <typename FloatType>
  struct anharmonic_adp_wrapper {
    typedef GramCharlier4<FloatType> wt;

    static void wrap() {
      using namespace boost::python;
      typedef return_value_policy<return_by_value> rbv;

      class_<wt>("gram_charlier", no_init)
        .def(init<af::shared<FloatType> const &,
                  af::shared<FloatType> const &>(
              (arg("Cijk"),
               arg("Dijkl"))))
        .def("calculate", &wt::calculate,
          (arg("miller_index")))
        .def("gradient_coefficients", &wt::gradient_coefficients,
          (arg("miller_index")))
        .def("data", &wt::data)
        ;
    }
  };

  void wrap_anharmonic_adp() {
    anharmonic_adp_wrapper<double>::wrap();
  }

  void init_module()
  {
    using namespace boost::python;

    wrap_anharmonic_adp();
  }
}}} // cctbx::boost_python::anharmonic

BOOST_PYTHON_MODULE(cctbx_anharmonic_ext)
{
  cctbx::boost_python::anharmonic::init_module();
}


text:
// Copyright (c) 2018 The Unit-e developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <addrman.h>
#include <esperanza/admincommand.h>
#include <esperanza/adminstate.h>
#include <test/esperanza/finalizationstate_utils.h>
#include <test/test_unite.h>
#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(adminstate_tests, ReducedTestingSetup)

BOOST_AUTO_TEST_CASE(empty_params_mean_no_admin) {
  esperanza::AdminParams emptyParams;
  esperanza::AdminState state(emptyParams);

  const uint160 validatorAddress = RandValidatorAddr();

  BOOST_CHECK(state.IsValidatorAuthorized(validatorAddress));
}

BOOST_AUTO_TEST_CASE(reset_admin) {
  const esperanza::AdminKeySet set0 = MakeKeySet();
  const esperanza::AdminKeySet set1 = MakeKeySet();

  esperanza::AdminParams params;
  params.admin_keys = set0;

  esperanza::AdminState state(params);

  BOOST_CHECK(state.IsAdminAuthorized(set0));
  BOOST_CHECK(!state.IsAdminAuthorized(set1));

  state.ResetAdmin(set1);

  BOOST_CHECK(!state.IsAdminAuthorized(set0));
  BOOST_CHECK(state.IsAdminAuthorized(set1));
}

BOOST_AUTO_TEST_CASE(change_white_list) {
  esperanza::AdminParams params;
  params.admin_keys = MakeKeySet();
  esperanza::AdminState state(params);

  const uint160 validator = RandValidatorAddr();

  BOOST_CHECK(!state.IsValidatorAuthorized(validator));

  state.AddValidator(validator);

  BOOST_CHECK(state.IsValidatorAuthorized(validator));

  state.RemoveValidator(validator);

  BOOST_CHECK(!state.IsValidatorAuthorized(validator));
}

BOOST_AUTO_TEST_CASE(end_permissioning) {
  esperanza::AdminParams params;
  params.admin_keys = MakeKeySet();
  esperanza::AdminState state(params);

  const uint160 validator = RandValidatorAddr();

  BOOST_CHECK(!state.IsValidatorAuthorized(validator));

  state.EndPermissioning();

  BOOST_CHECK(state.IsValidatorAuthorized(validator));
}

BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: frolosofsky/unit-e

max_issues_repo_name: frolosofsky/unit-e

max_forks_repo_name: frolosofsky/unit-e


text: #include <boost/mpl/aux_/preprocessed/msvc60/map.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
/*
 * MPAPOImpl.cpp
 *
 *  Created on: 18 June 2014
 *      Author: cyosp
 */

#include "com/cyosp/mpa/po/MPAPO.hpp"

#include "com/cyosp/mpa/core/MPA.hpp"
#include <boost/log/trivial.hpp>

void mpapo::MPAO::initializeVersion()
{
    this->version = -1;
}

void mpapo::MPAO::updateVersion()
{
    this->version = this->version + 1;
}

bool mpapo::MPAO::isCorrectVersion(int versionToCheck)
{
    return this->version == versionToCheck;
}

void mpapo::MPAO::setUpdated()
{
    this->updated = true;
}

void mpapo::MPAO::resetUpdated()
{
    this->updated = false;
}

void mpapo::MPAO::store()
{
    if( updated )
    {
        updateVersion();
        resetUpdated();
        update();
    }
}

#include <unistd.h>

max_stars_repo_name: cyosp/MPA

max_issues_repo_name: cyosp/MPA

max_forks_repo_name: cyosp/MPA


text:
#include "pch.h"
#include "cool/scenegraph.hpp"
#include "cool/common.hpp"
#include "cool/render.hpp"
#include "cool/logger.hpp"

#include <boost/assert.hpp>
#include <boost/exception/all.hpp>

#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>

#include <pugixml.hpp>

namespace glm {

inline vec3 lerp(const vec3& v1, const vec3& v2, float s) {
	return v1 + (v2 - v1) * s;
}

} // glm

namespace scenegraph {

void decompose_matrix(const glm::mat4& m, glm::vec3& scaling, glm::quat& rotation, glm::vec3& translation) {
	// extract translation
	translation = glm::vec3(m[3]);

	glm::vec3 col1(m[0]);
	glm::vec3 col2(m[1]);
	glm::vec3 col3(m[2]);

	// extract scaling
	scaling.x = glm::length(col1);
	scaling.y = glm::length(col2);
	scaling.z = glm::length(col3);

	// handle negative scaling
	if (glm::determinant(m) < 0) {
		scaling = -scaling;
	}

	// remove scaling
	if (scaling.x != 0) { col1 /= scaling.x; }
	if (scaling.y != 0) { col2 /= scaling.y; }
	if (scaling.z != 0) { col3 /= scaling.z; }

	// extract rotation
	rotation = glm::quat_cast(glm::mat3(col1, col2, col3));
}

glm::mat4 compose_matrix(const glm::vec3& scaling, const glm::quat& rotation, const glm::vec3& translation) {
	glm::mat4 m;
	m = glm::scale(m, scaling);
	m *= glm::mat4_cast(rotation);
	m = glm::translate(m, translation);
	return m;
}

// bounding_box

bounding_box::bounding_box()
	: _min(std::numeric_limits<float>::max())
	, _max(std::numeric_limits<float>::min())
{
}

void bounding_box::add_internal_point(const glm::vec3& p) {
	_min.x = std::min(_min.x, p.x);
	_min.y = std::min(_min.y, p.y);
	_min.z = std::min(_min.z, p.z);

	_max.x = std::max(_max.x, p.x);
	_max.y = std::max(_max.y, p.y);
	_max.z = std::max(_max.z, p.z);
}

// bounding_sphere

bounding_sphere bounding_sphere::from_bounding_box(const bounding_box& bbox) {
	const glm::vec3& min = bbox.get_min_point();
	const glm::vec3& max = bbox.get_max_point();

	glm::vec3 center = (max + min) * 0.5f;
	float radius = glm::length(max - center);

	return bounding_sphere(center, radius);
}

// bone

void bone::add_child(bone* child) {
	BOOST_ASSERT(!!child);
	_children.push_back(child);
}

glm::mat4 bone::compute_absolute_transform() const {
	if (_parent) {
		return _parent->compute_absolute_transform() * _transform;
	}

	return _transform;
}

// mesh

mesh::mesh()
	: _type(mesh_type::regular)
	, _bone()
	, _material()
{
	init();
}

mesh::mesh(mesh_type type)
	: _type(type)
	, _bone()
	, _material()
{
	init();
}

void mesh::init() {
	io::input_stream_ptr vert_src;
	io::input_stream_ptr frag_src;

	// TODO: smarter resource management needed
	if (_type == mesh_type::regular) {
		vert_src = io::stream::from_asset("regular_mesh.vsh");
		frag_src = io::stream::from_asset("regular_mesh.fsh");
	} else if (_type == mesh_type::skinned) {
		vert_src = io::stream::from_asset("skinned_mesh.vsh");
		frag_src = io::stream::from_asset("skinned_mesh.fsh");
	} else {
		BOOST_ASSERT_MSG(false, "unknown mesh type");
	}

	_shader.create(vert_src.get(), frag_src.get());
}

void mesh::set_vertex_data(const render::vertex_element_description* decl, const void* data, size_t size) {
	_vertices.create(render::buffer_type::vertex, decl, size, data);
	compute_bounding_sphere(data, size, decl->stride);
}

void mesh::compute_bounding_sphere(const void* data, size_t size, size_t stride) {
	bounding_box bbox;

	for (size_t i = 0; i < size; i += stride) {
		const glm::vec3* pos = reinterpret_cast<const glm::vec3*>(reinterpret_cast<const uint8_t*>(data) + i);
		bbox.add_internal_point(*pos);
	}

	_bsphere = bounding_sphere::from_bounding_box(bbox);
}

void mesh::set_index_data(const void* data, size_t size) {
	_indices.create(render::buffer_type::index16, nullptr, size, data);
}

void mesh::draw() {
	pre_draw();

	render::renderer& r = render::renderer::get();

	r.set_geometry(&_vertices, &_indices);
	r.set_texture(_material->get_texture());

	for (MeshParts::const_iterator it = _mesh_parts.begin(); it != _mesh_parts.end(); ++it) {
		r.draw_elements(render::primitives::triangles, (*it).first, (*it).second);
	}

	r.set_geometry(0, 0);
	r.set_texture(0);

	post_draw();
}

void mesh::pre_draw() {
	_shader.bind();
}

void mesh::post_draw() {
	_shader.unbind();
}

// skinned mesh

void skinned_mesh::pre_draw() {
	const glm::mat4 absolute_transform = get_bone()->compute_absolute_transform();
	const size_t count = get_influent_bone_count();

	_matrices.clear();
	_matrices.reserve(count);

	for (size_t i = 0; i < count; ++i) {
		_matrices.push_back(absolute_transform * get_influent_bone_offset(i) * get_influent_bone(i)->compute_absolute_transform());
	}

	// shader binds here
	mesh::pre_draw();

	_shader.set_uniform("u_matrix_palette[0]", glm::value_ptr(_matrices.front()), count * 16);
}

// animation_channel

namespace {

template <typename T, typename Collection>
T find_frame_in_collection(const Collection& coll, float time) {
	BOOST_ASSERT(!coll.empty());

	if (coll.size() == 1 || time >= coll.back().first) {
		return coll.back().second;
	}

	if (time <= coll.front().first) {
		return coll.front().second;
	}

	// assume that collection is sorted
	typename Collection::const_iterator it = std::lower_bound(std::begin(coll), std::end(coll), time,
		[](const typename Collection::value_type& val, float time) {
			return val.first < time;
		});

	if (it == coll.end()) {
		return coll.back().second;
	}

	if (it == coll.begin() || almost_equal_floats(time, (*it).first)) {
		return (*it).second;
	}

	// move to the previous frame
	typename Collection::const_iterator it2 = it--;

	// linear interpolate between two frames
	float s = (time - (*it).first) / ((*it2).first - (*it).first);
	return glm::lerp((*it).second, (*it2).second, s);
}

} // namespace

void animation_channel::get_srt(float time, glm::vec3& scaling, glm::quat& rotation, glm::vec3& translation) const {
	scaling = find_frame_in_collection<glm::vec3>(_scaling_keys, time);
	rotation = find_frame_in_collection<glm::quat>(_rotation_keys, time);
	translation = find_frame_in_collection<glm::vec3>(_translation_keys, time);
}

// animation

float animation::get_periodic_position(float time) const {
	return std::fmod(time, _period);
}

animation_channel* animation::get_channel(size_t index) const {
	BOOST_ASSERT(index < _channels.size());

	if (index < _channels.size()) {
		return boost::get_pointer(_channels[index]);
	}

	return nullptr;
}

// model

void model::load(io::input_stream* stream) {
	io::input_stream_ptr guard(stream);

	std::vector<uint8_t> buffer;
	io::stream::read_all(stream, buffer);

	guard.reset(); // don't need it any longer

	if (buffer.empty()) {
		LOGE("model xml file is empty");
		return;
	}

	pugi::xml_document doc;
	pugi::xml_parse_result result = doc.load_buffer_inplace(&buffer.front(), buffer.size());
	if (!result) {
		LOGE("failed to parse model xml: ") << result.description();
		return;
	}

	MeshRefs mesh_refs;

	load_materials(doc.child("model").child("materials"));
	load_bones(doc.child("model"), mesh_refs);
	load_meshes(doc.child("model").child("meshes"), mesh_refs);
	load_animations(doc.child("model").child("animations"));
}

void model::load_materials(pugi::xml_node elem) {
	size_t materials_count = elem.attribute("count").as_uint();

	_materials.clear();
	_materials.reserve(materials_count);

	size_t material_num = 0;
	for (pugi::xml_node material_xml = elem.child("material"); material_xml; material_xml = material_xml.next_sibling("material"), ++material_num) {
		load_material(material_xml);
	}
	BOOST_ASSERT(material_num == materials_count);

	std::sort(_materials.begin(), _materials.end(),
		[](const Materials::value_type& m1, const Materials::value_type& m2) {
			return m1.first < m2.first;
		});
}

void model::load_material(pugi::xml_node elem) {
	material_ptr m = new material();

	m->set_duffuse_color(load_color_value(elem.child("diffuse")));
	m->set_specular_color(load_color_value(elem.child("specular")));
	m->set_emissive_color(load_color_value(elem.child("emissive")));
	m->set_specular_power(elem.child("specular").attribute("power").as_float());

	// TODO: manage resources cleverer
	render::texture2d_ptr tex = new render::texture2d();
	const char* tex_name = elem.child("texture").attribute("name").as_string();
	tex->create(imaging::image::from_stream(io::stream::from_asset(tex_name)), render::texture_address::repeat);
	m->set_texture(tex.get());

	std::string name = elem.attribute("id").as_string();
	_materials.emplace_back(name, m);
}

void model::load_bones(pugi::xml_node elem, MeshRefs& mesh_refs) {
	_bones.clear();

	for (pugi::xml_node bone_xml = elem.child("node"); bone_xml; bone_xml = bone_xml.next_sibling("node")) {
		load_bone(bone_xml, nullptr, mesh_refs);
	}

	// in sorted collection we will be able to find duplicated values
	std::sort(mesh_refs.begin(), mesh_refs.end(),
		[](const MeshRefs::value_type& r1, const MeshRefs::value_type& r2) {
			return r1.first < r2.first;
		});

	std::sort(_bones.begin(), _bones.end(),
		[](const Bones::value_type& b1, const Bones::value_type& b2) {
			return b1.first < b2.first;
		});

	// right after sorting array we can assign indices to the bones
	for (size_t i = 0; i < _bones.size(); ++i) {
		_bones[i].second->set_index(i);
	}
}

void model::load_bone(pugi::xml_node elem, bone* parent, MeshRefs& mesh_refs) {
	bone_ptr b = new bone(parent);
	if (parent) {
		parent->add_child(b.get());
	}

	b->set_transform(load_matrix(elem.child("matrix")));
	_bones.emplace_back(elem.attribute("name").as_string(), b);

	// collect reference to mesh if there is
	pugi::xml_node mesh = elem.child("mesh");
	if (mesh) {
		mesh_refs.emplace_back(mesh.attribute("ref").as_string(), b.get());
	}

	// load children recursively
	for (pugi::xml_node child_xml = elem.child("node"); child_xml; child_xml = child_xml.next_sibling("node")) {
		load_bone(child_xml, b.get(), mesh_refs);
	}
}

void model::load_meshes(pugi::xml_node elem, const MeshRefs& mesh_refs) {
	size_t mesh_count = elem.attribute("count").as_uint();

	_meshes.clear();
	_meshes.reserve(mesh_count);

	size_t mesh_num = 0;
	for (pugi::xml_node mesh_xml = elem.child("mesh"); mesh_xml; mesh_xml = mesh_xml.next_sibling("mesh"), ++mesh_num) {
		load_mesh(mesh_xml);
	}
	BOOST_ASSERT(mesh_num == mesh_count);

	std::sort(_meshes.begin(), _meshes.end(),
		[](const Meshes::value_type& m1, const Meshes::value_type& m2) {
			return m1.first < m2.first;
		});

	// link meshes with corresponding bones
	std::string last_mesh;
	for (MeshRefs::const_iterator it = mesh_refs.begin(); it != mesh_refs.end(); ++it) {
		const std::string& mesh_name = (*it).first;
		bone* mesh_bone = (*it).second;
		if (mesh_name == last_mesh) {
			BOOST_ASSERT_MSG(false, "there is duplicating mesh, cloning needed");
		} else {
			mesh* m = find_mesh(mesh_name.c_str());
			BOOST_ASSERT(!!m);
			if (m) {
				m->set_bone(mesh_bone);
			}
		}

		last_mesh = (*it).first;
	}
}

void model::load_mesh(pugi::xml_node elem) {
	mesh_ptr m;

	pugi::xml_node vertices_xml = elem.child("vertices");
	pugi::xml_node bones_xml = elem.child("bones");

	uint32_t vertices_count = vertices_xml.attribute("count").as_uint();

	if (bones_xml) {
		typedef std::vector<std::pair<uint32_t, float>> Weights;
		typedef std::vector<Weights> VertexWeights;

		skinned_mesh_ptr sm = new skinned_mesh();
		VertexWeights all_vertex_weights;

		size_t bones_count = bones_xml.attribute("count").as_uint();

		sm->reserve_bone_influences(bones_count);
		all_vertex_weights.reserve(bones_count);

		size_t bone_num = 0;
		for (pugi::xml_node bone_xml = bones_xml.child("bone"); bone_xml; bone_xml = bone_xml.next_sibling("bone"), ++bone_num) {
			sm->add_bone_influence(find_bone(bone_xml.attribute("node").as_string()), load_matrix(bone_xml.child("matrix")));

			all_vertex_weights.emplace_back();
			Weights& vertex_weights = all_vertex_weights.back();

			vertex_weights.reserve(bone_xml.attribute("weights").as_uint());
			for (pugi::xml_node weight = bone_xml.child("weight"); weight; weight = weight.next_sibling("weight")) {
				vertex_weights.emplace_back(weight.attribute("vertex").as_uint(), weight.attribute("weight").as_float());
			}
		}
		BOOST_ASSERT(bone_num == bones_count);

		VertexWeights vertex_weights(vertices_count);
		for (uint32_t i = 0; i < all_vertex_weights.size(); ++i) {
			const Weights& weights = all_vertex_weights[i];
			for (Weights::const_iterator it = weights.begin(); it != weights.end(); ++it) {
				vertex_weights[(*it).first].emplace_back(i, (*it).second);
			}
		}

		// clean up
		VertexWeights().swap(all_vertex_weights);

		std::vector<render::vertex_position_normal_beta5_texture> vertices;
		vertices.reserve(vertices_count);

		uint32_t vertex_num = 0;
		for (pugi::xml_node vertex_xml = vertices_xml.child("vertex"); vertex_xml; vertex_xml = vertex_xml.next_sibling("vertex"), ++vertex_num) {
			vertices.emplace_back();
			render::vertex_position_normal_beta5_texture& v = vertices.back();
			v.x = vertex_xml.attribute("x").as_float();
			v.y = vertex_xml.attribute("y").as_float();
			v.z = vertex_xml.attribute("z").as_float();
			v.nx = vertex_xml.attribute("nx").as_float();
			v.ny = vertex_xml.attribute("ny").as_float();
			v.nz = vertex_xml.attribute("nz").as_float();
			v.tu = vertex_xml.attribute("tu").as_float();
			v.tv = vertex_xml.attribute("tv").as_float();
			v.beta[0] = 0.0f;
			v.beta[1] = 0.0f;
			v.beta[2] = 0.0f;
			v.beta[3] = 0.0f;
			v.beta[4] = 0.0f;

			for (uint32_t i = 0; i < std::min(4u, vertex_weights[vertex_num].size()); ++i) {
				v.indices[i] = static_cast<uint8_t>(vertex_weights[vertex_num][i].first);
				v.weights[i] = vertex_weights[vertex_num][i].second;
			}
		}
		BOOST_ASSERT(vertex_num == vertices_count);

		m = sm;
		m->set_vertex_data(render::vertex_position_normal_beta5_texture::decl(), &vertices.front(), vertices.size());
	} else {
		std::vector<render::vertex_position_normal_texture> vertices;
		vertices.reserve(vertices_count);

		uint32_t vertex_num = 0;
		for (pugi::xml_node vertex_xml = vertices_xml.child("vertex"); vertex_xml; vertex_xml = vertex_xml.next_sibling("vertex"), ++vertex_num) {
			vertices.emplace_back();
			render::vertex_position_normal_texture& v = vertices.back();
			v.x = vertex_xml.attribute("x").as_float();
			v.y = vertex_xml.attribute("y").as_float();
			v.z = vertex_xml.attribute("z").as_float();
			v.nx = vertex_xml.attribute("nx").as_float();
			v.ny = vertex_xml.attribute("ny").as_float();
			v.nz = vertex_xml.attribute("nz").as_float();
			v.tu = vertex_xml.attribute("tu").as_float();
			v.tv = vertex_xml.attribute("tv").as_float();
		}
		BOOST_ASSERT(vertex_num == vertices_count);

		m = new mesh();
		m->set_vertex_data(render::vertex_position_normal_texture::decl(), &vertices.front(), vertices.size());
	}

	pugi::xml_node faces_xml = elem.child("faces");

	size_t faces_count = faces_xml.attribute("count").as_uint();

	std::vector<uint16_t> indices;
	indices.reserve(faces_count);

	size_t face_num = 0;
	for (pugi::xml_node face_xml = faces_xml.child("face"); face_xml; face_xml = face_xml.next_sibling("face"), ++face_num) {
		indices.push_back(static_cast<uint16_t>(face_xml.attribute("a").as_uint()));
		indices.push_back(static_cast<uint16_t>(face_xml.attribute("b").as_uint()));
		indices.push_back(static_cast<uint16_t>(face_xml.attribute("c").as_uint()));
	}
	BOOST_ASSERT(face_num == faces_count);

	m->set_index_data(&indices.front(), indices.size());

	// TODO: divide mesh into several parts according to hardware capabilities
	m->add_mesh_part(indices.size(), 0);
	m->set_material(find_material(elem.attribute("material").as_string()));

	std::string name = elem.attribute("id").as_string();
	_meshes.emplace_back(name, m);
}

void model::load_animations(pugi::xml_node elem) {
	size_t animations_count = elem.attribute("count").as_uint();

	_animations.clear();
	_animations.reserve(animations_count);

	size_t animation_num = 0;
	for (pugi::xml_node animation_xml = elem.child("animation"); animation_xml; animation_xml = animation_xml.next_sibling("animation"), ++animation_num) {
		load_animation(animation_xml);
	}
	BOOST_ASSERT(animation_num == animations_count);

	std::sort(_animations.begin(), _animations.end(),
		[](const Animations::value_type& a1, const Animations::value_type& a2) {
			return a1.first < a2.first;
		});
}

void model::load_animation(pugi::xml_node elem) {
	animation_ptr anim = new animation();

	anim->set_period(elem.attribute("duration").as_float(4.0f));
	anim->set_fps(elem.attribute("ticksPerSec").as_float(24.0f));

	for (pugi::xml_node channel_xml = elem.child("channel"); channel_xml; channel_xml = channel_xml.next_sibling("channel")) {
		bone* animation_bone = find_bone(channel_xml.attribute("node").as_string());
		if (!animation_bone) {
			LOGW("skipping animation for unknown node: ") << channel_xml.attribute("node").as_string();
			continue;
		}

		animation_channel_ptr channel = load_animation_channel(channel_xml);
		channel->set_bone(animation_bone);
		anim->add_channel(channel.get());
	}

	std::string name = elem.attribute("name").as_string();
	_animations.emplace_back(name, anim);
}

animation_channel_ptr model::load_animation_channel(pugi::xml_node elem) {
	animation_channel_ptr anim = new animation_channel();

	pugi::xml_node transforms_xml = elem.child("transforms");
	if (transforms_xml) {
		size_t transforms_count = transforms_xml.attribute("count").as_uint();

		anim->reserve_rotations(transforms_count);
		anim->reserve_scalings(transforms_count);
		anim->reserve_translations(transforms_count);

		glm::vec3 last_scaling;
		glm::quat last_rotation;
		glm::vec3 last_translation;
		bool first = true;

		size_t transforms_num = 0;
		for (pugi::xml_node transform_xml = transforms_xml.child("transform"); transform_xml; transform_xml = transform_xml.next_sibling("transform"), ++transforms_num) {
			glm::vec3 scaling;
			glm::quat rotation;
			glm::vec3 translation;

			decompose_matrix(load_matrix(transform_xml), scaling, rotation, translation);

			float time = transform_xml.attribute("time").as_float();

			if (first || last_scaling != scaling) {
				anim->add_scaling(time, scaling);
			}

			if (first || last_rotation != rotation) {
				anim->add_rotation(time, rotation);
			}

			if (first || last_translation != translation) {
				anim->add_translation(time, translation);
			}

			last_scaling = scaling;
			last_rotation = rotation;
			last_translation = translation;
			first = false;
		}
		BOOST_ASSERT(transforms_num == transforms_count);
	} else {
		pugi::xml_node rotations_xml = elem.child("rotations");
		size_t rotations_count = rotations_xml.attribute("count").as_uint();

		anim->reserve_rotations(rotations_count);

		size_t rotation_num = 0;
		for (pugi::xml_node rotation_xml = rotations_xml.child("rotation"); rotation_xml; rotation_xml = rotation_xml.next_sibling("rotation"), ++rotation_num) {
			anim->add_rotation(rotation_xml.attribute("time").as_float(), load_quat(rotation_xml));
		}
		BOOST_ASSERT(rotation_num == rotations_count);

		pugi::xml_node scalings_xml = elem.child("scalings");
		size_t scalings_count = scalings_xml.attribute("count").as_uint();

		anim->reserve_scalings(scalings_count);

		size_t scaling_num = 0;
		for (pugi::xml_node scaling_xml = scalings_xml.child("scaling"); scaling_xml; scaling_xml = scaling_xml.next_sibling("scaling"), ++scaling_num) {
			anim->add_scaling(scaling_xml.attribute("time").as_float(), load_vec3(scaling_xml));
		}
		BOOST_ASSERT(scaling_num == scalings_count);

		pugi::xml_node positions_xml = elem.child("positions");
		size_t positions_count = positions_xml.attribute("count").as_uint();

		anim->reserve_translations(positions_count);

		size_t position_num = 0;
		for (pugi::xml_node position_xml = positions_xml.child("position"); position_xml; position_xml = position_xml.next_sibling("position"), ++position_num) {
			anim->add_translation(position_xml.attribute("time").as_float(), load_vec3(position_xml));
		}
		BOOST_ASSERT(position_num == positions_count);
	}

	return anim;
}

render::color_value model::load_color_value(pugi::xml_node elem) {
	render::color_value color(
		elem.attribute("b").as_float(),
		elem.attribute("g").as_float(),
		elem.attribute("r").as_float(),
		elem.attribute("a").as_float(1.0f)
	);

	return color;
}

glm::mat4 model::load_matrix(pugi::xml_node elem) {
	glm::mat4 mat(
		elem.attribute("_11").as_float(), elem.attribute("_12").as_float(), elem.attribute("_13").as_float(), elem.attribute("_14").as_float(),
		elem.attribute("_21").as_float(), elem.attribute("_22").as_float(), elem.attribute("_23").as_float(), elem.attribute("_24").as_float(),
		elem.attribute("_31").as_float(), elem.attribute("_32").as_float(), elem.attribute("_33").as_float(), elem.attribute("_34").as_float(),
		elem.attribute("_41").as_float(), elem.attribute("_42").as_float(), elem.attribute("_43").as_float(), elem.attribute("_44").as_float()
	);

	return mat;
}

glm::vec3 model::load_vec3(pugi::xml_node elem) {
	glm::vec3 vec(
		elem.attribute("x").as_float(),
		elem.attribute("y").as_float(),
		elem.attribute("z").as_float()
	);

	return vec;
}

glm::quat model::load_quat(pugi::xml_node elem) {
	glm::quat quat(
		elem.attribute("x").as_float(),
		elem.attribute("y").as_float(),
		elem.attribute("z").as_float(),
		elem.attribute("w").as_float()
	);

	return quat;
}

void model::draw() {
	for (Meshes::const_iterator it = _meshes.begin(); it != _meshes.end(); ++it) {
		mesh* m = boost::get_pointer((*it).second);
		m->draw();
	}
}

namespace {

template <typename T, typename Collection>
T* find_in_sorted_collection(const Collection& coll, const char* name) {
	typename Collection::const_iterator it = std::lower_bound(std::begin(coll), std::end(coll), name,
		[](const typename Collection::value_type& val, const char* name) {
			return val.first < name;
		});

	if ((*it).first == name) {
		return boost::get_pointer((*it).second);
	}

	return nullptr;
}

} // namespace

mesh* model::find_mesh(const char* name) const {
	return find_in_sorted_collection<mesh>(_meshes, name);
}

material* model::find_material(const char* name) const {
	return find_in_sorted_collection<material>(_materials, name);
}

bone* model::find_bone(const char* name) const {
	return find_in_sorted_collection<bone>(_bones, name);
}

animation* model::find_animation(const char* name) const {
	return find_in_sorted_collection<animation>(_animations, name);
}

// animation info

animation_player::animation_info::animation_info(animation* anim)
	: _anim(anim)
	, _paused(false)
	, _time(0.0f)
	, _speed(1.0f)
	, _blend(1.0f)
{
}

void animation_player::animation_info::advance_time(float dt) {
	_time += dt * _speed;
}

// animation player

animation_player::animation_player(model* m)
	: _model(m)
{
}

void animation_player::play(const char* name) {
	Animations::iterator it = _animations.find(name);
	if (it != _animations.end()) {
		animation_info& info = (*it).second;
		// unpause and return if already playing
		if (info.is_paused()) {
			info.set_paused(false);
			return;
		}
	}

	animation* anim = _model->find_animation(name);
	BOOST_ASSERT(!!anim);

	if (anim) {
		_animations.emplace(name, animation_info(anim));
	}
}

void animation_player::pause(const char* name) {
	Animations::iterator it = _animations.find(name);
	BOOST_ASSERT_MSG(it != _animations.end(), "animation not playing");
	if (it != _animations.end()) {
		animation_info& info = (*it).second;
		// unpause and return if already playing
		if (!info.is_paused()) {
			info.set_paused(true);
			return;
		}
	}
}

void animation_player::stop(const char* name) {
	_animations.erase(name);
}

void animation_player::update(float dt) {
	for (auto& kvp: _animations) {
		animation_info& info = kvp.second;
		if (!info.is_paused()) {
			info.advance_time(dt);
		}
	}
}

} // namespace scenegraph

max_stars_repo_name: mzhirnov/cool

max_issues_repo_name: mzhirnov/cool

max_forks_repo_name: mzhirnov/cool


text:
/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  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 the Rice University 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.
*********************************************************************/

/* Author: Mark Moll, Ioan Sucan */

#include "ompl/base/spaces/SO3StateSpace.h"
#include <algorithm>
#include <limits>
#include <cmath>
#include "ompl/tools/config/MagicConstants.h"
#include <boost/math/constants/constants.hpp>
#include <boost/assert.hpp>

static const double MAX_QUATERNION_NORM_ERROR = 1e-9;

/// @cond IGNORE
namespace ompl
{
    namespace base
    {
        static inline void computeAxisAngle(SO3StateSpace::StateType &q, double ax, double ay, double az, double angle)
        {
            double norm = std::sqrt(ax * ax + ay * ay + az * az);
            if (norm < MAX_QUATERNION_NORM_ERROR)
                q.setIdentity();
            else
            {
                double half_angle = angle / 2.0;
                double s = sin(half_angle) / norm;
                q.x = s * ax;
                q.y = s * ay;
                q.z = s * az;
                q.w = cos(half_angle);
            }
        }

        /* Standard quaternion multiplication: q = q0 * q1 */
        static inline void quaternionProduct(SO3StateSpace::StateType &q, const SO3StateSpace::StateType &q0, const SO3StateSpace::StateType &q1)
        {
            q.x = q0.w * q1.x + q0.x * q1.w + q0.y * q1.z - q0.z * q1.y;
            q.y = q0.w * q1.y + q0.y * q1.w + q0.z * q1.x - q0.x * q1.z;
            q.z = q0.w * q1.z + q0.z * q1.w + q0.x * q1.y - q0.y * q1.x;
            q.w = q0.w * q1.w - q0.x * q1.x - q0.y * q1.y - q0.z * q1.z;
        }

        inline double quaternionNormSquared(const SO3StateSpace::StateType &q)
        {
            return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
        }
    }
}
/// @endcond

void ompl::base::SO3StateSpace::StateType::setAxisAngle(double ax, double ay, double az, double angle)
{
    computeAxisAngle(*this, ax, ay, az, angle);
}

void ompl::base::SO3StateSpace::StateType::setIdentity()
{
    x = y = z = 0.0;
    w = 1.0;
}

void ompl::base::SO3StateSampler::sampleUniform(State *state)
{
    rng_.quaternion(&state->as<SO3StateSpace::StateType>()->x);
}

void ompl::base::SO3StateSampler::sampleUniformNear(State *state, const State *near, const double distance)
{
    if (distance >= .25 * boost::math::constants::pi<double>())
    {
        sampleUniform(state);
        return;
    }
    double d = rng_.uniform01();
    SO3StateSpace::StateType q,
        *qs = static_cast<SO3StateSpace::StateType*>(state);
    const SO3StateSpace::StateType *qnear = static_cast<const SO3StateSpace::StateType*>(near);
    computeAxisAngle(q, rng_.gaussian01(), rng_.gaussian01(), rng_.gaussian01(),
        2. * pow(d, boost::math::constants::third<double>()) * distance);
    quaternionProduct(*qs, *qnear, q);
}

void ompl::base::SO3StateSampler::sampleGaussian(State *state, const State * mean, const double stdDev)
{
    // The standard deviation of the individual components of the tangent
    // perturbation needs to be scaled so that the expected quaternion distance
    // between the sampled state and the mean state is stdDev. The factor 2 is
    // due to the way we define distance (see also Matt Mason's lecture notes
    // on quaternions at
    // http://www.cs.cmu.edu/afs/cs/academic/class/16741-s07/www/lecture7.pdf).
    // The 1/sqrt(3) factor is necessary because the distribution in the tangent
    // space is a 3-dimensional Gaussian, so that the *length* of a tangent
    // vector needs to be scaled by 1/sqrt(3).
    double rotDev = (2. * stdDev) / boost::math::constants::root_three<double>();

    // CDF of N(0, 1.17) at -pi/4 is approx. .25, so there's .25 probability
    // weight in each tail. Since the maximum distance in SO(3) is pi/2, we're
    // essentially as likely to sample a state within distance [0, pi/4] as
    // within distance [pi/4, pi/2]. With most weight in the tails (that wrap
    // around in case of quaternions) we might as well sample uniformly.
    if (rotDev > 1.17)
    {
        sampleUniform(state);
        return;
    }

    double x = rng_.gaussian(0, rotDev), y = rng_.gaussian(0, rotDev), z = rng_.gaussian(0, rotDev),
        theta = std::sqrt(x*x + y*y + z*z);
    if (theta < std::numeric_limits<double>::epsilon())
        space_->copyState(state, mean);
    else
    {
        SO3StateSpace::StateType q,
            *qs = static_cast<SO3StateSpace::StateType*>(state);
        const SO3StateSpace::StateType *qmu = static_cast<const SO3StateSpace::StateType*>(mean);
        double half_theta = theta / 2.0;
        double s = sin(half_theta) / theta;
        q.w = cos(half_theta);
        q.x = s * x;
        q.y = s * y;
        q.z = s * z;
        quaternionProduct(*qs, *qmu, q);
    }
}

unsigned int ompl::base::SO3StateSpace::getDimension() const
{
    return 3;
}

double ompl::base::SO3StateSpace::getMaximumExtent() const
{
    return .5 * boost::math::constants::pi<double>();
}

double ompl::base::SO3StateSpace::getMeasure() const
{
    // half of the surface area of a unit 3-sphere
    return boost::math::constants::pi<double>() * boost::math::constants::pi<double>();
}

double ompl::base::SO3StateSpace::norm(const StateType *state) const
{
    double nrmSqr = quaternionNormSquared(*state);
    return (fabs(nrmSqr - 1.0) > std::numeric_limits<double>::epsilon()) ? std::sqrt(nrmSqr) : 1.0;
}

void ompl::base::SO3StateSpace::enforceBounds(State *state) const
{
    // see http://stackoverflow.com/questions/11667783/quaternion-and-normalization/12934750#12934750
    StateType *qstate = static_cast<StateType*>(state);
    double nrmsq = quaternionNormSquared(*qstate);
    double error = std::abs(1.0 - nrmsq);
    const double epsilon = 2.107342e-08;
    if (error < epsilon)
    {
        double scale = 2.0 / (1.0 + nrmsq);
        qstate->x *= scale;
        qstate->y *= scale;
        qstate->z *= scale;
        qstate->w *= scale;
    }
    else
    {
        if (nrmsq < 1e-6)
            qstate->setIdentity();
        else
        {
            double scale = 1.0 / std::sqrt(nrmsq);
            qstate->x *= scale;
            qstate->y *= scale;
            qstate->z *= scale;
            qstate->w *= scale;
        }
    }
}

bool ompl::base::SO3StateSpace::satisfiesBounds(const State *state) const
{
    return fabs(norm(static_cast<const StateType*>(state)) - 1.0) < MAX_QUATERNION_NORM_ERROR;
}

void ompl::base::SO3StateSpace::copyState(State *destination, const State *source) const
{
    const StateType *qsource = static_cast<const StateType*>(source);
    StateType *qdestination = static_cast<StateType*>(destination);
    qdestination->x = qsource->x;
    qdestination->y = qsource->y;
    qdestination->z = qsource->z;
    qdestination->w = qsource->w;
}

unsigned int ompl::base::SO3StateSpace::getSerializationLength() const
{
    return sizeof(double) * 4;
}

void ompl::base::SO3StateSpace::serialize(void *serialization, const State *state) const
{
    memcpy(serialization, &state->as<StateType>()->x, sizeof(double) * 4);
}

void ompl::base::SO3StateSpace::deserialize(State *state, const void *serialization) const
{
    memcpy(&state->as<StateType>()->x, serialization, sizeof(double) * 4);
}

/// @cond IGNORE

/*
Based on code from :

Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
*/
namespace ompl
{
    namespace base
    {
        static inline double arcLength(const State *state1, const State *state2)
        {
            const SO3StateSpace::StateType *qs1 = static_cast<const SO3StateSpace::StateType*>(state1);
            const SO3StateSpace::StateType *qs2 = static_cast<const SO3StateSpace::StateType*>(state2);
            double dq = fabs(qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w);
            if (dq > 1.0 - MAX_QUATERNION_NORM_ERROR)
                return 0.0;
            else
                return acos(dq);
        }
    }
}
/// @endcond

double ompl::base::SO3StateSpace::distance(const State *state1, const State *state2) const
{
    BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2),
        "The states passed to SO3StateSpace::distance are not within bounds. Call "
        "SO3StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, "
        "ompl::control::StatePropagator, or ompl::base::StateValidityChecker");
    return arcLength(state1, state2);
}
double ompl::base::SO3StateSpace::distance2(const State *state1, const State *state2)
{
    BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2),
        "The states passed to SO3StateSpace::distance are not within bounds. Call "
        "SO3StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, "
        "ompl::control::StatePropagator, or ompl::base::StateValidityChecker");
    return arcLength(state1, state2);
}
double ompl::base::SO3StateSpace::distance3(const State *state1, const State *state2)
{
    BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2),
        "The states passed to SO3StateSpace::distance are not within bounds. Call "
        "SO3StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, "
        "ompl::control::StatePropagator, or ompl::base::StateValidityChecker");
    return arcLength(state1, state2);
}

bool ompl::base::SO3StateSpace::equalStates(const State *state1, const State *state2) const
{
    return arcLength(state1, state2) < std::numeric_limits<double>::epsilon();
}

/*
Based on code from :

Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
*/
void ompl::base::SO3StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
{
    assert(fabs(norm(static_cast<const StateType*>(from)) - 1.0) < MAX_QUATERNION_NORM_ERROR);
    assert(fabs(norm(static_cast<const StateType*>(to)) - 1.0) < MAX_QUATERNION_NORM_ERROR);

    double theta = arcLength(from, to);
    if (theta > std::numeric_limits<double>::epsilon())
    {
        double d = 1.0 / sin(theta);
        double s0 = sin((1.0 - t) * theta);
        double s1 = sin(t * theta);

        const StateType *qs1 = static_cast<const StateType*>(from);
        const StateType *qs2 = static_cast<const StateType*>(to);
        StateType       *qr  = static_cast<StateType*>(state);
        double dq = qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w;
        if (dq < 0)  // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
            s1 = -s1;

        qr->x = (qs1->x * s0 + qs2->x * s1) * d;
        qr->y = (qs1->y * s0 + qs2->y * s1) * d;
        qr->z = (qs1->z * s0 + qs2->z * s1) * d;
        qr->w = (qs1->w * s0 + qs2->w * s1) * d;
    }
    else
    {
        if (state != from)
            copyState(state, from);
    }
}

ompl::base::StateSamplerPtr ompl::base::SO3StateSpace::allocDefaultStateSampler() const
{
    return StateSamplerPtr(new SO3StateSampler(this));
}

ompl::base::State* ompl::base::SO3StateSpace::allocState() const
{
    return new StateType();
}

void ompl::base::SO3StateSpace::freeState(State *state) const
{
    delete static_cast<StateType*>(state);
}

void ompl::base::SO3StateSpace::registerProjections()
{
    class SO3DefaultProjection : public ProjectionEvaluator
    {
    public:

        SO3DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
        {
        }

        virtual unsigned int getDimension() const
        {
            return 3;
        }

        virtual void defaultCellSizes()
        {
            cellSizes_.resize(3);
            cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
            cellSizes_[1] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
            cellSizes_[2] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
            bounds_.resize(3);
            bounds_.setLow(-1.0);
            bounds_.setHigh(1.0);
        }

        virtual void project(const State *state, EuclideanProjection &projection) const
        {
            projection(0) = state->as<SO3StateSpace::StateType>()->x;
            projection(1) = state->as<SO3StateSpace::StateType>()->y;
            projection(2) = state->as<SO3StateSpace::StateType>()->z;
        }
    };

    registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SO3DefaultProjection(this))));
}

double* ompl::base::SO3StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
{
    return index < 4 ? &(state->as<StateType>()->x) + index : nullptr;
}

void ompl::base::SO3StateSpace::printState(const State *state, std::ostream &out) const
{
    out << "SO3State [";
    if (state)
    {
        const StateType *qstate = static_cast<const StateType*>(state);
        out << qstate->x << " " << qstate->y << " " << qstate->z << " " << qstate->w;
    }
    else
        out << "nullptr";
    out << ']' << std::endl;
}

void ompl::base::SO3StateSpace::printSettings(std::ostream &out) const
{
    out << "SO(3) state space '" << getName() << "' (represented using quaternions)" << std::endl;
}


text:
// Copyright (c) 2010, Adam Petersen <adam@adampetersen.se>. All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
//
//   1. Redistributions of source code must retain the above copyright notice, this list of
//      conditions and the following disclaimer.
//
//   2. Redistributions in binary form must reproduce the above copyright notice, this list
//      of conditions and the following disclaimer in the documentation and/or other materials
//      provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY Adam Petersen ``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 Adam Petersen OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
// ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "tinch_pp/erl_any.h"
#include "tinch_pp/erlang_types.h"
#include "tinch_pp/exceptions.h"
#include "term_conversions.h"
#include "ext_term_grammar.h"
#include "matchable_seq.h"
#include <boost/bind.hpp>
#include <boost/optional.hpp>
#include <boost/assign/list_of.hpp>
#include <cassert>

using namespace tinch_pp;
using namespace tinch_pp::erl;
using namespace boost;
using namespace boost::assign;

namespace {

bool match_int(msg_seq_iter& f, const msg_seq_iter& l, const any& instance)
{
   return int_(instance).match(f, l);
}

bool match_atom(msg_seq_iter& f, const msg_seq_iter& l, const any& instance)
{
   return atom(instance).match(f, l);
}

bool match_string(msg_seq_iter& f, const msg_seq_iter& l, const any& instance)
{
   return e_string(instance).match(f, l);
}

bool match_float(msg_seq_iter& f, const msg_seq_iter& l, const any& instance)
{
   return float_(instance).match(f, l);
}

bool match_reference(msg_seq_iter& f, const msg_seq_iter& l, const any& instance)
{
  return erl::ref(instance).match(f, l);
}

bool match_pid(msg_seq_iter& f, const msg_seq_iter& l, const any& instance)
{
   return pid(instance).match(f, l);
}

bool match_binary(msg_seq_iter& f, const msg_seq_iter& l, const any& instance)
{
   return binary(instance).match(f, l);
}

bool match_tuple(msg_seq_iter& f, const msg_seq_iter& l, const any& instance)
{
   msg_seq_iter start = f;

   size_t parsed_length = 0;
   bool match = binary_to_term<small_tuple_head_ext>(f, l, parsed_length);
   assert(match); // already checked in the previous dispatch-mechanism

   instance.save_matched_bytes(msg_seq(start, f));

   for(size_t i = 0; match && (i < parsed_length); ++i)
      match &= instance.match(f, l);

   return match;
}

bool match_list(msg_seq_iter& f, const msg_seq_iter& l, const any& instance)
{
   msg_seq_iter start = f;

   size_t parsed_length = 0;
   bool match = binary_to_term<list_head_ext>(f, l, parsed_length);
   assert(match); // already checked in the previous dispatch-mechanism

   instance.save_matched_bytes(msg_seq(start, f));

   for(size_t i = 0; match && (i < parsed_length); ++i)
      match &= instance.match(f, l);

   return match;
}

optional<int> extract_type_tag(msg_seq_iter& f, const msg_seq_iter& l)
{
   optional<int> tag;

   if(f != l)
      tag = static_cast<int>(*f);

   return tag;
}

}

const any::dynamic_element_matcher_type any::dynamic_element_matcher =
   map_list_of
      //    Type                    Match function
      (type_tag::small_integer,     bind(match_int,        ::_1, ::_2, ::_3))
      (type_tag::integer,           bind(match_int,        ::_1, ::_2, ::_3))
      (type_tag::atom_ext,          bind(match_atom,       ::_1, ::_2, ::_3))
      (type_tag::small_tuple,       bind(match_tuple,      ::_1, ::_2, ::_3))
      (type_tag::list,              bind(match_list,       ::_1, ::_2, ::_3))
      (type_tag::string_ext,        bind(match_string,     ::_1, ::_2, ::_3))
      (type_tag::pid,               bind(match_pid,        ::_1, ::_2, ::_3))
      (type_tag::new_reference_ext, bind(match_reference,  ::_1, ::_2, ::_3))
      (type_tag::float_ext,         bind(match_float,      ::_1, ::_2, ::_3))
      (type_tag::binary_ext,        bind(match_binary,     ::_1, ::_2, ::_3))
      (type_tag::bit_binary_ext,    bind(match_binary,     ::_1, ::_2, ::_3));

bool any::match_dynamically(msg_seq_iter& f, const msg_seq_iter& l, const any& instance)
{
   bool matched = false;

   if(const optional<term_id_type> tag = extract_type_tag(f, l)) {
      dynamic_element_matcher_type::const_iterator m = dynamic_element_matcher.find(*tag);

      // TODO: Once we support all types, this should be considered an erronoues term (raise exception).
      if(m != dynamic_element_matcher.end()) {
         const term_matcher_type matcher = m->second;
         matched = matcher(f, l, instance);
      }
   }

   return matched;
}

any::any()
   : to_assign(&placeholder)
{}

any::any(matchable_ptr* a_to_assign)
   : to_assign(a_to_assign)
{}

bool any::match(msg_seq_iter& f, const msg_seq_iter& l) const
{
   const bool res = match_dynamically(f, l, *this);
   to_assign->reset(new matchable_seq(matched_raw_bytes));

   return res;
}

bool any::save_matched_bytes(const msg_seq& raw_msg_part) const
{
   matched_raw_bytes.insert(matched_raw_bytes.end(), raw_msg_part.begin(), raw_msg_part.end());

   return true;
}