<commit_before>/*
 * superviseddescent: A C++11 implementation of the supervised descent
 *                    optimisation method
 * File: rcr/model.hpp
 *
 * Copyright 2015 Patrik Huber
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#pragma once

#ifndef MODEL_HPP_
#define MODEL_HPP_

#include "helpers.hpp"
#include "adaptive_vlhog.hpp"
#include "landmark.hpp"
#include "superviseddescent/superviseddescent.hpp"
#include "superviseddescent/verbose_solver.hpp"

#include "cereal/cereal.hpp"
#include "cereal/types/string.hpp"
#include "cereal/types/vector.hpp"
#include "cereal/archives/binary.hpp"
#include "superviseddescent/utils/mat_cerealisation.hpp"

#include "opencv2/core/core.hpp"

#include <vector>
#include <string>

namespace rcr {

/**
 * Performs an initial alignment of the model, by putting the mean model into
 * the center of the face box.
 *
 * An optional scaling and translation parameters can be given to generate
 * perturbations of the initialisation.
 *
 * Note 02/04/15: I think with the new perturbation code, we can delete the optional
 * parameters here - make it as simple as possible, don't include what's not needed.
 * Align and perturb should really be separate - separate things.
 *
 * @param[in] mean Mean model points.
 * @param[in] facebox A facebox to align the model to.
 * @param[in] scaling_x Optional scaling in x of the model.
 * @param[in] scaling_y Optional scaling in y of the model.
 * @param[in] translation_x Optional translation in x of the model.
 * @param[in] translation_y Optional translation in y of the model.
 * @return A cv::Mat of the aligned points.
 */
cv::Mat align_mean(cv::Mat mean, cv::Rect facebox, float scaling_x=1.0f, float scaling_y=1.0f, float translation_x=0.0f, float translation_y=0.0f)
{
	using cv::Mat;
	// Initial estimate x_0: Center the mean face at the [-0.5, 0.5] x [-0.5, 0.5] square (assuming the face-box is that square)
	// More precise: Take the mean as it is (assume it is in a space [-0.5, 0.5] x [-0.5, 0.5]), and just place it in the face-box as
	// if the box is [-0.5, 0.5] x [-0.5, 0.5]. (i.e. the mean coordinates get upscaled)
	Mat aligned_mean = mean.clone();
	Mat aligned_mean_x = aligned_mean.colRange(0, aligned_mean.cols / 2);
	Mat aligned_mean_y = aligned_mean.colRange(aligned_mean.cols / 2, aligned_mean.cols);
	aligned_mean_x = (aligned_mean_x*scaling_x + 0.5f + translation_x) * facebox.width + facebox.x;
	aligned_mean_y = (aligned_mean_y*scaling_y + 0.5f + translation_y) * facebox.height + facebox.y;
	return aligned_mean;
}

/**
 * This class handles the adaptive shape update of the RCR
 * during training. The update is normalised by calculating the
 * IED of the given \c params and then returning a normalisation
 * vector 1/IED.
 */
class InterEyeDistanceNormalisation
{
public:
	InterEyeDistanceNormalisation() = default;

	InterEyeDistanceNormalisation(std::vector<std::string> modelLandmarksList, std::vector<std::string> rightEyeIdentifiers, std::vector<std::string> leftEyeIdentifiers) : modelLandmarksList(modelLandmarksList), rightEyeIdentifiers(rightEyeIdentifiers), leftEyeIdentifiers(leftEyeIdentifiers)
	{
	};

	// params: The current landmark estimate
	inline cv::Mat operator()(cv::Mat params) {
		auto lmc = to_landmark_collection(params, modelLandmarksList);
		auto ied = get_ied(lmc, rightEyeIdentifiers, leftEyeIdentifiers);
		return cv::Mat::ones(1, params.cols, params.type()) / ied;
	};

private:
	std::vector<std::string> modelLandmarksList;
	std::vector<std::string> rightEyeIdentifiers;
	std::vector<std::string> leftEyeIdentifiers;

	friend class cereal::access;
	/**
	 * Serialises this class using cereal.
	 *
	 * @param[in] ar The archive to serialise to (or to serialise from).
	 */
	template<class Archive>
	void serialize(Archive& ar)
	{
		ar(modelLandmarksList, rightEyeIdentifiers, leftEyeIdentifiers);
	}
};

/**
 * This class represents a learned RCR landmark detection
 * model. It can detect landmarks and can be stored/loaded.
 */
class detection_model
{
public:
	using model_type = superviseddescent::SupervisedDescentOptimiser<superviseddescent::LinearRegressor<superviseddescent::VerbosePartialPivLUSolver>, InterEyeDistanceNormalisation>;
	detection_model() = default;

	detection_model(model_type optimised_model, cv::Mat mean, std::vector<std::string> landmark_ids, std::vector<rcr::HoGParam> hog_params, std::vector<std::string> right_eye_ids, std::vector<std::string> left_eye_ids) : optimised_model(optimised_model), mean(mean), landmark_ids(landmark_ids), hog_params(hog_params), right_eye_ids(right_eye_ids), left_eye_ids(left_eye_ids)
	{};

	// Run the model from a fbox. i.e. init using the mean, then optimise.
	LandmarkCollection<cv::Vec2f> detect(cv::Mat image, cv::Rect facebox)
	{
		// Obtain the initial estimate using the mean landmarks:
		cv::Mat mean_initialisation = rcr::align_mean(mean, facebox);
		//rcr::draw_landmarks(image, mean_initialisation, { 0, 0, 255 });

		std::vector<cv::Mat> images{ image }; // we can't pass a temporary anymore to HogTransform
		rcr::HogTransform hog(images, hog_params, landmark_ids, right_eye_ids, left_eye_ids);

		cv::Mat landmarks = optimised_model.predict(mean_initialisation, cv::Mat(), hog);

		return to_landmark_collection(landmarks, landmark_ids);
	};

	// Run the model from landmark init, e.g. using the previous frame's lms. I.e. do not init from the mean. Directly optimise.
	LandmarkCollection<cv::Vec2f> detect(cv::Mat image, cv::Mat initialisation)
	{
		//rcr::drawLandmarks(image, initialisation, { 0, 0, 255 });

		std::vector<cv::Mat> images{ image };
		rcr::HogTransform hog(images, hog_params, landmark_ids, right_eye_ids, left_eye_ids);

		cv::Mat landmarks = optimised_model.predict(initialisation, cv::Mat(), hog);

		return to_landmark_collection(landmarks, landmark_ids);
	};

	cv::Mat get_mean()
	{
		return mean;
	};

private:
	model_type optimised_model;
	//std::vector<cv::Mat> regressors; // Note: vector<LinearRegressorModel> or vector<LinearRegressor::model_type> instead?
	cv::Mat mean;						///< The mean of the model, learned and scaled from training data, given a specific face detector
	std::vector<rcr::HoGParam> hog_params;	///< The hog parameters for each regressor level
	std::vector<std::string> landmark_ids;			///< The landmark identifiers the model consists of
	std::vector<std::string> right_eye_ids, left_eye_ids;	///< Holds information about which landmarks are the eyes, to calculate the IED normalisation for the adaptive update

	friend class cereal::access;
	/**
	 * Serialises this class using cereal.
	 *
	 * @param[in] ar The archive to serialise to (or to serialise from).
	 */
	template<class Archive>
	void serialize(Archive& archive)
	{
		archive(optimised_model, mean, landmark_ids, hog_params, right_eye_ids, left_eye_ids);
	};
};

/**
 * Load a trained \c detection_model that was stored
 * as cereal::BinaryInputArchive from the harddisk.
 *
 * @param[in] filename Filename to a model.
 * @return The loaded detection_model.
 */
detection_model load_detection_model(std::string filename)
{
	detection_model rcr_model;

	std::ifstream file(filename, std::ios::binary);
	cereal::BinaryInputArchive input_archive(file);
	input_archive(rcr_model);

	return rcr_model;
};

/**
 * Save a trained \c detection_model to the harddisk
 * as cereal::BinaryInputArchive.
 *
 * @param[in] model The model to be saved.
 * @param[in] filename Filename for the model.
 */
void save_detection_model(detection_model model, std::string filename)
{
	std::ofstream file(filename, std::ios::binary);
	cereal::BinaryOutputArchive output_archive(file);
	output_archive(model);
};

} /* namespace rcr */

#endif /* MODEL_HPP_ */
<commit_msg>Added missing include<commit_after>/*
 * superviseddescent: A C++11 implementation of the supervised descent
 *                    optimisation method
 * File: rcr/model.hpp
 *
 * Copyright 2015 Patrik Huber
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#pragma once

#ifndef MODEL_HPP_
#define MODEL_HPP_

#include "helpers.hpp"
#include "adaptive_vlhog.hpp"
#include "landmark.hpp"
#include "superviseddescent/superviseddescent.hpp"
#include "superviseddescent/verbose_solver.hpp"

#include "cereal/cereal.hpp"
#include "cereal/types/string.hpp"
#include "cereal/types/vector.hpp"
#include "cereal/archives/binary.hpp"
#include "superviseddescent/utils/mat_cerealisation.hpp"

#include "opencv2/core/core.hpp"

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

namespace rcr {

/**
 * Performs an initial alignment of the model, by putting the mean model into
 * the center of the face box.
 *
 * An optional scaling and translation parameters can be given to generate
 * perturbations of the initialisation.
 *
 * Note 02/04/15: I think with the new perturbation code, we can delete the optional
 * parameters here - make it as simple as possible, don't include what's not needed.
 * Align and perturb should really be separate - separate things.
 *
 * @param[in] mean Mean model points.
 * @param[in] facebox A facebox to align the model to.
 * @param[in] scaling_x Optional scaling in x of the model.
 * @param[in] scaling_y Optional scaling in y of the model.
 * @param[in] translation_x Optional translation in x of the model.
 * @param[in] translation_y Optional translation in y of the model.
 * @return A cv::Mat of the aligned points.
 */
cv::Mat align_mean(cv::Mat mean, cv::Rect facebox, float scaling_x=1.0f, float scaling_y=1.0f, float translation_x=0.0f, float translation_y=0.0f)
{
	using cv::Mat;
	// Initial estimate x_0: Center the mean face at the [-0.5, 0.5] x [-0.5, 0.5] square (assuming the face-box is that square)
	// More precise: Take the mean as it is (assume it is in a space [-0.5, 0.5] x [-0.5, 0.5]), and just place it in the face-box as
	// if the box is [-0.5, 0.5] x [-0.5, 0.5]. (i.e. the mean coordinates get upscaled)
	Mat aligned_mean = mean.clone();
	Mat aligned_mean_x = aligned_mean.colRange(0, aligned_mean.cols / 2);
	Mat aligned_mean_y = aligned_mean.colRange(aligned_mean.cols / 2, aligned_mean.cols);
	aligned_mean_x = (aligned_mean_x*scaling_x + 0.5f + translation_x) * facebox.width + facebox.x;
	aligned_mean_y = (aligned_mean_y*scaling_y + 0.5f + translation_y) * facebox.height + facebox.y;
	return aligned_mean;
}

/**
 * This class handles the adaptive shape update of the RCR
 * during training. The update is normalised by calculating the
 * IED of the given \c params and then returning a normalisation
 * vector 1/IED.
 */
class InterEyeDistanceNormalisation
{
public:
	InterEyeDistanceNormalisation() = default;

	InterEyeDistanceNormalisation(std::vector<std::string> modelLandmarksList, std::vector<std::string> rightEyeIdentifiers, std::vector<std::string> leftEyeIdentifiers) : modelLandmarksList(modelLandmarksList), rightEyeIdentifiers(rightEyeIdentifiers), leftEyeIdentifiers(leftEyeIdentifiers)
	{
	};

	// params: The current landmark estimate
	inline cv::Mat operator()(cv::Mat params) {
		auto lmc = to_landmark_collection(params, modelLandmarksList);
		auto ied = get_ied(lmc, rightEyeIdentifiers, leftEyeIdentifiers);
		return cv::Mat::ones(1, params.cols, params.type()) / ied;
	};

private:
	std::vector<std::string> modelLandmarksList;
	std::vector<std::string> rightEyeIdentifiers;
	std::vector<std::string> leftEyeIdentifiers;

	friend class cereal::access;
	/**
	 * Serialises this class using cereal.
	 *
	 * @param[in] ar The archive to serialise to (or to serialise from).
	 */
	template<class Archive>
	void serialize(Archive& ar)
	{
		ar(modelLandmarksList, rightEyeIdentifiers, leftEyeIdentifiers);
	}
};

/**
 * This class represents a learned RCR landmark detection
 * model. It can detect landmarks and can be stored/loaded.
 */
class detection_model
{
public:
	using model_type = superviseddescent::SupervisedDescentOptimiser<superviseddescent::LinearRegressor<superviseddescent::VerbosePartialPivLUSolver>, InterEyeDistanceNormalisation>;
	detection_model() = default;

	detection_model(model_type optimised_model, cv::Mat mean, std::vector<std::string> landmark_ids, std::vector<rcr::HoGParam> hog_params, std::vector<std::string> right_eye_ids, std::vector<std::string> left_eye_ids) : optimised_model(optimised_model), mean(mean), landmark_ids(landmark_ids), hog_params(hog_params), right_eye_ids(right_eye_ids), left_eye_ids(left_eye_ids)
	{};

	// Run the model from a fbox. i.e. init using the mean, then optimise.
	LandmarkCollection<cv::Vec2f> detect(cv::Mat image, cv::Rect facebox)
	{
		// Obtain the initial estimate using the mean landmarks:
		cv::Mat mean_initialisation = rcr::align_mean(mean, facebox);
		//rcr::draw_landmarks(image, mean_initialisation, { 0, 0, 255 });

		std::vector<cv::Mat> images{ image }; // we can't pass a temporary anymore to HogTransform
		rcr::HogTransform hog(images, hog_params, landmark_ids, right_eye_ids, left_eye_ids);

		cv::Mat landmarks = optimised_model.predict(mean_initialisation, cv::Mat(), hog);

		return to_landmark_collection(landmarks, landmark_ids);
	};

	// Run the model from landmark init, e.g. using the previous frame's lms. I.e. do not init from the mean. Directly optimise.
	LandmarkCollection<cv::Vec2f> detect(cv::Mat image, cv::Mat initialisation)
	{
		//rcr::drawLandmarks(image, initialisation, { 0, 0, 255 });

		std::vector<cv::Mat> images{ image };
		rcr::HogTransform hog(images, hog_params, landmark_ids, right_eye_ids, left_eye_ids);

		cv::Mat landmarks = optimised_model.predict(initialisation, cv::Mat(), hog);

		return to_landmark_collection(landmarks, landmark_ids);
	};

	cv::Mat get_mean()
	{
		return mean;
	};

private:
	model_type optimised_model;
	//std::vector<cv::Mat> regressors; // Note: vector<LinearRegressorModel> or vector<LinearRegressor::model_type> instead?
	cv::Mat mean;						///< The mean of the model, learned and scaled from training data, given a specific face detector
	std::vector<rcr::HoGParam> hog_params;	///< The hog parameters for each regressor level
	std::vector<std::string> landmark_ids;			///< The landmark identifiers the model consists of
	std::vector<std::string> right_eye_ids, left_eye_ids;	///< Holds information about which landmarks are the eyes, to calculate the IED normalisation for the adaptive update

	friend class cereal::access;
	/**
	 * Serialises this class using cereal.
	 *
	 * @param[in] ar The archive to serialise to (or to serialise from).
	 */
	template<class Archive>
	void serialize(Archive& archive)
	{
		archive(optimised_model, mean, landmark_ids, hog_params, right_eye_ids, left_eye_ids);
	};
};

/**
 * Load a trained \c detection_model that was stored
 * as cereal::BinaryInputArchive from the harddisk.
 *
 * @param[in] filename Filename to a model.
 * @return The loaded detection_model.
 */
detection_model load_detection_model(std::string filename)
{
	detection_model rcr_model;

	std::ifstream file(filename, std::ios::binary);
	cereal::BinaryInputArchive input_archive(file);
	input_archive(rcr_model);

	return rcr_model;
};

/**
 * Save a trained \c detection_model to the harddisk
 * as cereal::BinaryInputArchive.
 *
 * @param[in] model The model to be saved.
 * @param[in] filename Filename for the model.
 */
void save_detection_model(detection_model model, std::string filename)
{
	std::ofstream file(filename, std::ios::binary);
	cereal::BinaryOutputArchive output_archive(file);
	output_archive(model);
};

} /* namespace rcr */

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


<commit_before>/**
 *	\file
 */

#pragma once

#include <rleahylib/rleahylib.hpp>
#include <atomic>
#include <type_traits>
#include <utility>

namespace MCPP {

	template <typename T>
	class Singleton {

		private:

			Nullable<T> obj;
			Mutex lock;
			std::atomic<bool> constructed;

		public:

			Singleton () noexcept {

				constructed=false;

			}

			Singleton (const Singleton &) = delete;
			Singleton (Singleton &&) = delete;
			Singleton & operator = (const Singleton &) = delete;
			Singleton & operator = (Singleton &&) = delete;

			T & Get () noexcept(std::is_nothrow_constructible<T>::value) {

				if (!constructed) {

					lock.Execute([&] () {

						if (!constructed) {

							obj.Construct();

							constructed=true;

						}

					});

				}

				return *obj;

			}

			void Destroy () noexcept {

				obj.Destroy();

			}

	};

}
<commit_msg>Singleton Documentation and Bugfix<commit_after>/**
 *	\file
 */

#pragma once

#include <rleahylib/rleahylib.hpp>
#include <atomic>
#include <type_traits>
#include <utility>

namespace MCPP {

	/**
	 *	Contains a singleton.
	 *
	 *	The singleton is not constructed
	 *	until an attempt is made to access it,
	 *	in which case it is thread safely
	 *	constructed and persists until manually
	 *	destroyed.
	 *
	 *	Destruction is not thread safe.
	 *
	 *	After the singleton is manually destroyed,
	 *	an further attempts to access it will
	 *	reconstruct it.
	 */
	template <typename T>
	class Singleton {

		private:

			Nullable<T> obj;
			Mutex lock;
			std::atomic<bool> constructed;

		public:

			/**
			 *	Creates a new singleton container.
			 */
			Singleton () noexcept {

				constructed=false;

			}

			Singleton (const Singleton &) = delete;
			Singleton (Singleton &&) = delete;
			Singleton & operator = (const Singleton &) = delete;
			Singleton & operator = (Singleton &&) = delete;

			/**
			 *	Retrieves the singleton, constructing
			 *	it if necessary.
			 *
			 *	Thread safe.
			 *
			 *	\return
			 *		A reference to the contained
			 *		singleton.
			 */
			T & Get () noexcept(std::is_nothrow_constructible<T>::value) {

				if (!constructed) {

					lock.Execute([&] () {

						if (!constructed) {

							obj.Construct();

							constructed=true;

						}

					});

				}

				return *obj;

			}

			/**
			 *	Destroys the contained singleton.
			 *
			 *	Not thread safe.
			 */
			void Destroy () noexcept {

				obj.Destroy();

				constructed=false;

			}

	};

}
<|endoftext|>


<commit_before>#pragma once

#include <chrono>

namespace stx {

using namespace std;
using namespace std::chrono;
using namespace std::chrono_literals;

class timer {
	high_resolution_clock::time_point m_begin;

	static inline
	high_resolution_clock::time_point now() noexcept { return high_resolution_clock::now(); }

public:
	timer() { reset(); }

	void reset() noexcept { m_begin = now(); }

	double days() const noexcept {
		return duration_cast<duration<double, ratio<86400>>>(now() - m_begin).count();
	}
	double hours() const noexcept {
		return duration_cast<duration<double, ratio<3600>>>(now() - m_begin).count();
	}
	double minutes() const noexcept {
		return duration_cast<duration<double, ratio<60>>>(now() - m_begin).count();
	}
	double seconds() const noexcept {
		return duration_cast<duration<double, ratio<1>>>(now() - m_begin).count();
	}
	double millis() const noexcept {
		return duration_cast<duration<double, milli>>(now() - m_begin).count();
	}
	double micros() const noexcept {
		return duration_cast<duration<double, micro>>(now() - m_begin).count();
	}
	double nanos() const noexcept {
		return duration_cast<duration<double, nano>>(now() - m_begin).count();
	}
};

} // namespace stx
<commit_msg>Added timer::seconds_now() etc.<commit_after>#pragma once

#include <chrono>

namespace stx {

using namespace std;
using namespace std::chrono;
using namespace std::chrono_literals;

class timer {
	high_resolution_clock::time_point m_begin;

	static inline
	high_resolution_clock::time_point now() noexcept { return high_resolution_clock::now(); }

public:
	timer() { reset(); }

	void reset() noexcept { m_begin = now(); }

	double days() const noexcept {
		return duration_cast<duration<double, ratio<86400>>>(now() - m_begin).count();
	}
	double hours() const noexcept {
		return duration_cast<duration<double, ratio<3600>>>(now() - m_begin).count();
	}
	double minutes() const noexcept {
		return duration_cast<duration<double, ratio<60>>>(now() - m_begin).count();
	}
	double seconds() const noexcept {
		return duration_cast<duration<double, ratio<1>>>(now() - m_begin).count();
	}
	double millis() const noexcept {
		return duration_cast<duration<double, milli>>(now() - m_begin).count();
	}
	double micros() const noexcept {
		return duration_cast<duration<double, micro>>(now() - m_begin).count();
	}
	double nanos() const noexcept {
		return duration_cast<duration<double, nano>>(now() - m_begin).count();
	}

	static double days_now() noexcept {
		return duration_cast<duration<double, ratio<86400>>>(now().time_since_epoch()).count();
	}
	static double hours_now() noexcept {
		return duration_cast<duration<double, ratio<3600>>>(now().time_since_epoch()).count();
	}
	static double minutes_now() noexcept {
		return duration_cast<duration<double, ratio<60>>>(now().time_since_epoch()).count();
	}
	static double seconds_now() noexcept {
		return duration_cast<duration<double, ratio<1>>>(now().time_since_epoch()).count();
	}
	static double millis_now() noexcept {
		return duration_cast<duration<double, milli>>(now().time_since_epoch()).count();
	}
	static double micros_now() noexcept {
		return duration_cast<duration<double, micro>>(now().time_since_epoch()).count();
	}
	static double nanos_now() noexcept {
		return duration_cast<duration<double, nano>>(now().time_since_epoch()).count();
	}
};

} // namespace stx
<|endoftext|>


<commit_before>//----------------------------------------------------------------------------
/// \file  math.hpp
//----------------------------------------------------------------------------
/// \brief This file contains some generic math functions.
//----------------------------------------------------------------------------
// Copyright (c) 2010 Serge Aleynikov <saleyn@gmail.com>
// Created: 2010-09-31
//----------------------------------------------------------------------------
/*
***** BEGIN LICENSE BLOCK *****

This file is part of the utxx open-source project.

Copyright (C) 2010 Serge Aleynikov <saleyn@gmail.com>

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

***** END LICENSE BLOCK *****
*/
#pragma once

#include <cmath>
#include <boost/assert.hpp>
#include <boost/cstdint.hpp>
#include <utxx/error.hpp>
#include <utxx/bits.hpp>

namespace utxx {
namespace math {

/// Calculate <tt>a</tt> raised to the power of <tt>b</tt>.
template <typename T>
T inline power(T a, size_t b) {
    if (a==0) return 0;
    if (b==0) return 1;
    if (b==1) return a;
    return (b & 1) == 0 ? power(a*a, b >> 1) : a*power(a*a, b >> 1);
}

/// Calculate logarithm of \a n using \a base.
/// If n is 0 or is less than base, the function returns 0.
int inline log(unsigned long n, uint8_t base = 2) {
    // return n <= 1 ? 0 : 1+log(n/base, base);   // This won't inline
    if (n <= 1) return 0;
    int k;
    for (k = 0; n > 1; k++, n /= base);
    return k;
}

/// Calculate logarithm of \a n using \a base.
/// If n is 0 or is less than base, the function returns 0.
int inline log2(unsigned long n) {
    BOOST_ASSERT(!(n & (n-1)));
    return bits::bit_scan_forward(n);
}

/// Calculate logarithm of \a n using \a base, such that
/// the Base to the power of the returned value is greater or equal to \a n.
/// If n is 0 or is less than base, the function returns 0.
template <uint8_t Base>
int inline upper_log(size_t n) {
    int k = log(n, Base);
    return power(k, Base) == n ? k : k+1;
}

int inline upper_log2(size_t n) {
    return (n & (n-1)) == 0 ? log2(n) : log(n,2)+1;
}

/// Returns power of \a base equal or greater than number \a n.
template <typename T>
T inline upper_power(T n, size_t base) {
    int m = log(n, base);
    T   r = power(base, m);
    return r == n ? n : r*base;
}

/// Calculate greatest common denominator of x and y.
/// E.g. gcd(18, 4) = 2
inline long gcd(long x, long y) {
    if (x == 0)
        return y;

    while (y != 0) {
        if (x > y) x = x - y;
        else       y = y - x;
    }

    return x;
}

/// Calculate least common multiple of x and y.
/// E.g. gcd(18, 4) = 36
inline long lcm(long x, long y) { return (x*y)/gcd(x, y); }

/*
/// Check if a number is prime
inline bool is_prime(long x) {
    // 0, 1 are not prime numbers, but 2, 3 are:
    if (x <= 3)
        return x > 1;
    // Even numbers and those divisible by 3 are not primes.
    if ((x & 1) == 0 || (x % 3) == 0)
        return false;
    // Note that all remaining primes are in the form of:
    //  (6*k - 1) or (6*k + 1):  5, 7
    // Iterate through all numbers checking that they are not
    // divisible by the prime numbers in the form stated above:
    for (long i = 5, n = std::sqrt(x); i <= n; i += 6)
        if ((x % i) == 0 || x % (i+2) == 0)
            return false;
    return true;
}
*/

namespace {
    bool div_by(long x, int y) { return x % y == 0; }

    long sqrt_helper(long x)
    {
        if (x < 0) return 0;

        // Use sqrt whenever possible, however the largest int value a double
        // can hold is 1 << 53. So we only use sqrt() for small ints.
        if (x <= 1l << 53)
            return long(std::sqrt(x));

        // Approximate sqrt that is >= the actual square root.
        // Any 64 bit number can be expressed in terms of x and y as follows:

        // sqrt(n) = sqrt(x) * sqrt(2^32) + z, where
        //   z - some value to account for the "y" portion of the equation
        //   (x+1) << 32 is always greater than (x<<32) | y, hence get sqrt(x+1)
        //   and multiply the result by 2^16, getting a slightly greater sqrt:
        auto   ux    = size_t(x);
        size_t shift = 0;

        static const size_t TWO32 = 1ULL << 32;

        for(; ux >= TWO32; ux >>= 32, shift += 32);

        return long(ceil(sqrt((double)(ux + 1)))) << (shift / 2);
    }
}

/// Check if a number is prime
inline bool is_prime(long x)
{
    if (x <= 1)
        return false;

    if (x < 4 || x == 5 || x == 7)
        return true;

    if (div_by(x, 2) || div_by(x, 3) || div_by(x, 5) || div_by(x, 7))
        return false;

    // The remaining primes are in the form 6*k-1 and 6*k+1:
    //   11,13,17,19,23..29,31,..37,41,43...
    // Note that from these we need to eliminate those divisible by 2,3,5.
    // The pattern of divisibility by 3 or 5 repeats from 15 every 30.

    //   03|05|07|09|11|13|15|17|19|21|23|25|27|29|31|33|35|37|39|41|43
    // 0        0     4  6    10 12    16       22 24       30
    // 3        x        x        x        x        x        x        x
    // 5        x              x              x              x

    // NOTE: for end value use +1 to compensate for round-off errors
    for(long d=7, e=sqrt_helper(x)+1; d <= e; d += 30)
        if (div_by(x,d)    || div_by(x,d+4)  || div_by(x,d+6)  || div_by(x,d+10) ||
            div_by(x,d+12) || div_by(x,d+16) || div_by(x,d+22) || div_by(x,d+24))
            return false;
    return true;
}

} // namespace math
} // namespace utxx
<commit_msg>Cleanup<commit_after>//----------------------------------------------------------------------------
/// \file  math.hpp
//----------------------------------------------------------------------------
/// \brief This file contains some generic math functions.
//----------------------------------------------------------------------------
// Copyright (c) 2010 Serge Aleynikov <saleyn@gmail.com>
// Created: 2010-09-31
//----------------------------------------------------------------------------
/*
***** BEGIN LICENSE BLOCK *****

This file is part of the utxx open-source project.

Copyright (C) 2010 Serge Aleynikov <saleyn@gmail.com>

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

***** END LICENSE BLOCK *****
*/
#pragma once

#include <cmath>
#include <boost/assert.hpp>
#include <boost/cstdint.hpp>
#include <utxx/error.hpp>
#include <utxx/bits.hpp>

namespace utxx {
namespace math {

/// Calculate <tt>a</tt> raised to the power of <tt>b</tt>.
template <typename T>
T inline power(T a, size_t b) {
    if (a==0) return 0;
    if (b==0) return 1;
    if (b==1) return a;
    return (b & 1) == 0 ? power(a*a, b >> 1) : a*power(a*a, b >> 1);
}

/// Calculate logarithm of \a n using \a base.
/// If n is 0 or is less than base, the function returns 0.
int inline log(unsigned long n, uint8_t base = 2) {
    // return n <= 1 ? 0 : 1+log(n/base, base);   // This won't inline
    if (n <= 1) return 0;
    int k;
    for (k = 0; n > 1; k++, n /= base);
    return k;
}

/// Calculate logarithm of \a n using \a base.
/// If n is 0 or is less than base, the function returns 0.
int inline log2(unsigned long n) {
    BOOST_ASSERT(!(n & (n-1)));
    return bits::bit_scan_forward(n);
}

/// Calculate logarithm of \a n using \a base, such that
/// the Base to the power of the returned value is greater or equal to \a n.
/// If n is 0 or is less than base, the function returns 0.
template <uint8_t Base>
int inline upper_log(size_t n) {
    int k = log(n, Base);
    return power(k, Base) == n ? k : k+1;
}

int inline upper_log2(size_t n) {
    return (n & (n-1)) == 0 ? log2(n) : log(n,2)+1;
}

/// Returns power of \a base equal or greater than number \a n.
template <typename T>
T inline upper_power(T n, size_t base) {
    int m = log(n, base);
    T   r = power(base, m);
    return r == n ? n : r*base;
}

/// Calculate greatest common denominator of x and y.
/// E.g. gcd(18, 4) = 2
inline long gcd(long x, long y) {
    if (x == 0)
        return y;

    while (y != 0) {
        if (x > y) x = x - y;
        else       y = y - x;
    }

    return x;
}

/// Calculate least common multiple of x and y.
/// E.g. gcd(18, 4) = 36
inline long lcm(long x, long y) { return (x*y)/gcd(x, y); }

/*
/// Check if a number is prime
inline bool is_prime(long x) {
    // 0, 1 are not prime numbers, but 2, 3 are:
    if (x <= 3)
        return x > 1;
    // Even numbers and those divisible by 3 are not primes.
    if ((x & 1) == 0 || (x % 3) == 0)
        return false;
    // Note that all remaining primes are in the form of:
    //  (6*k - 1) or (6*k + 1):  5, 7
    // Iterate through all numbers checking that they are not
    // divisible by the prime numbers in the form stated above:
    for (long i = 5, n = std::sqrt(x); i <= n; i += 6)
        if ((x % i) == 0 || x % (i+2) == 0)
            return false;
    return true;
}
*/

namespace {
    bool div_by(long x, int y) { return x % y == 0; }

    long sqrt_helper(long x)
    {
        if (x < 0) return 0;

        // Use sqrt whenever possible, however the largest int value a double
        // can hold is 1 << 53. So use sqrt() only for smaller ints:
        if (x <= 1l << 53)
            return long(std::sqrt(x));

        // Approximate sqrt that is >= the actual square root.
        // Any 64 bit number can be expressed in terms of x and y as follows:

        // sqrt(n) = sqrt(x) * sqrt(2^32) + z, where
        //   z - some value to account for the "y" portion of the equation
        //   (x+1) << 32 is always greater than (x<<32) | y, hence get sqrt(x+1)
        //   and multiply the result by 2^16 giving a slightly greater sqrt:
        auto   ux    = size_t(x);
        size_t shift = 0;

        for(; ux >= (1ul << 32); ux >>= 32, shift += 32);

        return long(ceil(sqrt((double)(ux + 1)))) << (shift / 2);
    }
}

/// Check if a number is prime
inline bool is_prime(long x)
{
    if (x <= 1)
        return false;

    if (x < 4 || x == 5 || x == 7)
        return true;

    if ((x&1) == 0 || div_by(x, 3) || div_by(x, 5) || div_by(x, 7))
        return false;

    // The remaining primes are in the form 6*k-1 and 6*k+1:
    //   11,13,17,19,23..29,31,..37,41,43...
    // Note that from these we need to eliminate those divisible by 2,3,5.
    // The pattern of divisibility by 3 or 5 repeats from 15 every 30.

    //   03|05|07|09|11|13|15|17|19|21|23|25|27|29|31|33|35|37|39|41|43
    // 0        0     4  6    10 12    16       22 24       30
    // 3        x        x        x        x        x        x        x
    // 5        x              x              x              x

    // NOTE: for end value use +1 to compensate for round-off errors
    for(long d=7, e=sqrt_helper(x)+1; d <= e; d += 30)
        if (div_by(x,d)    || div_by(x,d+4)  || div_by(x,d+6)  || div_by(x,d+10) ||
            div_by(x,d+12) || div_by(x,d+16) || div_by(x,d+22) || div_by(x,d+24))
            return false;
    return true;
}

} // namespace math
} // namespace utxx
<|endoftext|>


<commit_before>//
// Copyright (c) 2009, Markus Rickert
// 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.
//
// 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 HOLDER 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 <array>
#include <iostream>
#include <rl/math/Unit.h>

#include "DeviceException.h"
#include "UniversalRobotsRealtime.h"

namespace rl
{
	namespace hal
	{
		UniversalRobotsRealtime::UniversalRobotsRealtime(const ::std::string& address) :
			AxisController(6),
			DigitalInput(),
			DigitalOutput(),
			CartesianForceSensor(6),
			CartesianPositionSensor(6),
			CartesianVelocitySensor(6),
			CyclicDevice(::std::chrono::milliseconds(8)),
			DigitalInputReader(),
			DigitalOutputReader(),
			JointCurrentSensor(6),
			JointPositionSensor(6),
			JointVelocitySensor(6),
			in(),
			socket(Socket::Tcp(Socket::Address::Ipv4(address, 30003)))
		{
		}

		UniversalRobotsRealtime::~UniversalRobotsRealtime()
		{
			if (this->isRunning())
			{
				this->stop();
			}

			if (this->isConnected())
			{
				this->close();
			}
		}

		void
		UniversalRobotsRealtime::close()
		{
			this->socket.close();
			this->setConnected(false);
		}

		void
		UniversalRobotsRealtime::doScript(const ::std::string& script)
		{
			this->socket.send(script.c_str(), script.size());
		}

		::rl::math::ForceVector
		UniversalRobotsRealtime::getCartesianForce() const
		{
			::rl::math::ForceVector f;
			f.force().x() = this->in.tcpForce[0];
			f.force().y() = this->in.tcpForce[1];
			f.force().z() = this->in.tcpForce[2];
			f.moment().x() = this->in.tcpForce[3];
			f.moment().y() = this->in.tcpForce[4];
			f.moment().z() = this->in.tcpForce[5];
			return f;
		}

		::rl::math::Transform
		UniversalRobotsRealtime::getCartesianPosition() const
		{
			::rl::math::Transform x;
			x.setIdentity();

			::rl::math::Vector3 orientation(this->in.toolVectorActual[3], this->in.toolVectorActual[4], this->in.toolVectorActual[5]);
			::rl::math::Real norm = orientation.norm();

			if (::std::abs(norm) <= ::std::numeric_limits< ::rl::math::Real>::epsilon())
			{
				x.linear().setIdentity();
			}
			else
			{
				x.linear() = ::rl::math::AngleAxis(norm, orientation.normalized()).matrix();
			}

			x.translation().x() = this->in.toolVectorActual[0];
			x.translation().y() = this->in.toolVectorActual[1];
			x.translation().z() = this->in.toolVectorActual[2];

			return x;
		}

		::rl::math::MotionVector
		UniversalRobotsRealtime::getCartesianVelocity() const
		{
			::rl::math::MotionVector v;
			v.linear().x() = this->in.tcpSpeedActual[0];
			v.linear().y() = this->in.tcpSpeedActual[1];
			v.linear().z() = this->in.tcpSpeedActual[2];
			v.angular().x() = this->in.tcpSpeedActual[3];
			v.angular().y() = this->in.tcpSpeedActual[4];
			v.angular().z() = this->in.tcpSpeedActual[5];
			return v;
		}

		::boost::dynamic_bitset<>
		UniversalRobotsRealtime::getDigitalInput() const
		{
			return ::boost::dynamic_bitset<>(64, this->in.digitalInputBits);
		}

		bool
		UniversalRobotsRealtime::getDigitalInput(const ::std::size_t& i) const
		{
			return this->in.digitalInputBits & (1ULL << i) ? true : false;
		}

		::std::size_t
		UniversalRobotsRealtime::getDigitalInputCount() const
		{
			return 64;
		}

		::boost::dynamic_bitset<>
		UniversalRobotsRealtime::getDigitalOutput() const
		{
			return ::boost::dynamic_bitset<>(64, this->in.digitalOutputs);
		}

		bool
		UniversalRobotsRealtime::getDigitalOutput(const ::std::size_t& i) const
		{
			return this->in.digitalOutputs & (1ULL << i) ? true : false;
		}

		::std::size_t
		UniversalRobotsRealtime::getDigitalOutputCount() const
		{
			return 64;
		}

		::rl::math::Vector
		UniversalRobotsRealtime::getJointCurrent() const
		{
			::rl::math::Vector i(this->getDof());

			for (::std::size_t j = 0; j < 6; ++j)
			{
				i(j) = this->in.iActual[j];
			}

			return i;
		}

		::rl::math::Vector
		UniversalRobotsRealtime::getJointPosition() const
		{
			::rl::math::Vector q(this->getDof());

			for (::std::size_t i = 0; i < 6; ++i)
			{
				q(i) = this->in.qActual[i];
			}

			return q;
		}

		::rl::math::Vector
		UniversalRobotsRealtime::getJointVelocity() const
		{
			::rl::math::Vector qd(this->getDof());

			for (::std::size_t i = 0; i < 6; ++i)
			{
				qd(i) = this->in.qdActual[i];
			}

			return qd;
		}

		void
		UniversalRobotsRealtime::open()
		{
			this->socket.open();
			this->socket.connect();
			this->socket.setOption(::rl::hal::Socket::OPTION_NODELAY, 1);
			this->setConnected(true);
		}

		void
		UniversalRobotsRealtime::start()
		{
			this->setRunning(true);
		}

		void
		UniversalRobotsRealtime::step()
		{
			::std::array< ::std::uint8_t, 1108> buffer;
			this->socket.recv(buffer.data(), buffer.size());
#if !defined(__APPLE__) && !defined(__QNX__) && !defined(WIN32)
			this->socket.setOption(::rl::hal::Socket::OPTION_QUICKACK, 1);
#endif // __APPLE__ || __QNX__ || WIN32
			this->in.unserialize(buffer.data());

#if 0
			std::cout << "messageSize: " << this->in.messageSize << std::endl;
			std::cout << "time: " << this->in.time << std::endl;
			std::cout << "qTarget[0]: " << this->in.qTarget[0] * rl::math::RAD2DEG << std::endl;
			std::cout << "qTarget[1]: " << this->in.qTarget[1] * rl::math::RAD2DEG << std::endl;
			std::cout << "qTarget[2]: " << this->in.qTarget[2] * rl::math::RAD2DEG << std::endl;
			std::cout << "qTarget[3]: " << this->in.qTarget[3] * rl::math::RAD2DEG << std::endl;
			std::cout << "qTarget[4]: " << this->in.qTarget[4] * rl::math::RAD2DEG << std::endl;
			std::cout << "qTarget[5]: " << this->in.qTarget[5] * rl::math::RAD2DEG << std::endl;
			std::cout << "qdTarget[0]: " << this->in.qdTarget[0] * rl::math::RAD2DEG << std::endl;
			std::cout << "qdTarget[1]: " << this->in.qdTarget[1] * rl::math::RAD2DEG << std::endl;
			std::cout << "qdTarget[2]: " << this->in.qdTarget[2] * rl::math::RAD2DEG << std::endl;
			std::cout << "qdTarget[3]: " << this->in.qdTarget[3] * rl::math::RAD2DEG << std::endl;
			std::cout << "qdTarget[4]: " << this->in.qdTarget[4] * rl::math::RAD2DEG << std::endl;
			std::cout << "qdTarget[5]: " << this->in.qdTarget[5] * rl::math::RAD2DEG << std::endl;
			std::cout << "qddTarget[0]: " << this->in.qddTarget[0] * rl::math::RAD2DEG << std::endl;
			std::cout << "qddTarget[1]: " << this->in.qddTarget[1] * rl::math::RAD2DEG << std::endl;
			std::cout << "qddTarget[2]: " << this->in.qddTarget[2] * rl::math::RAD2DEG << std::endl;
			std::cout << "qddTarget[3]: " << this->in.qddTarget[3] * rl::math::RAD2DEG << std::endl;
			std::cout << "qddTarget[4]: " << this->in.qddTarget[4] * rl::math::RAD2DEG << std::endl;
			std::cout << "qddTarget[5]: " << this->in.qddTarget[5] * rl::math::RAD2DEG << std::endl;
			std::cout << "iTarget[0]: " << this->in.iTarget[0] << std::endl;
			std::cout << "iTarget[1]: " << this->in.iTarget[1] << std::endl;
			std::cout << "iTarget[2]: " << this->in.iTarget[2] << std::endl;
			std::cout << "iTarget[3]: " << this->in.iTarget[3] << std::endl;
			std::cout << "iTarget[4]: " << this->in.iTarget[4] << std::endl;
			std::cout << "iTarget[5]: " << this->in.iTarget[5] << std::endl;
			std::cout << "mTarget[0]: " << this->in.mTarget[0] << std::endl;
			std::cout << "mTarget[1]: " << this->in.mTarget[1] << std::endl;
			std::cout << "mTarget[2]: " << this->in.mTarget[2] << std::endl;
			std::cout << "mTarget[3]: " << this->in.mTarget[3] << std::endl;
			std::cout << "mTarget[4]: " << this->in.mTarget[4] << std::endl;
			std::cout << "mTarget[5]: " << this->in.mTarget[5] << std::endl;
			std::cout << "qActual[0]: " << this->in.qActual[0] * rl::math::RAD2DEG << std::endl;
			std::cout << "qActual[1]: " << this->in.qActual[1] * rl::math::RAD2DEG << std::endl;
			std::cout << "qActual[2]: " << this->in.qActual[2] * rl::math::RAD2DEG << std::endl;
			std::cout << "qActual[3]: " << this->in.qActual[3] * rl::math::RAD2DEG << std::endl;
			std::cout << "qActual[4]: " << this->in.qActual[4] * rl::math::RAD2DEG << std::endl;
			std::cout << "qActual[5]: " << this->in.qActual[5] * rl::math::RAD2DEG << std::endl;
			std::cout << "qdActual[0]: " << this->in.qdActual[0] * rl::math::RAD2DEG << std::endl;
			std::cout << "qdActual[1]: " << this->in.qdActual[1] * rl::math::RAD2DEG << std::endl;
			std::cout << "qdActual[2]: " << this->in.qdActual[2] * rl::math::RAD2DEG << std::endl;
			std::cout << "qdActual[3]: " << this->in.qdActual[3] * rl::math::RAD2DEG << std::endl;
			std::cout << "qdActual[4]: " << this->in.qdActual[4] * rl::math::RAD2DEG << std::endl;
			std::cout << "qdActual[5]: " << this->in.qdActual[5] * rl::math::RAD2DEG << std::endl;
#endif
		}

		void
		UniversalRobotsRealtime::stop()
		{
			this->setRunning(false);
		}

		void
		UniversalRobotsRealtime::Message::unserialize(::std::uint8_t* ptr)
		{
			this->unserialize(ptr, this->messageSize);

			if (756 != this->messageSize && 764 != this->messageSize && 812 != this->messageSize && 1044 != this->messageSize && 1060 != this->messageSize && 1108 != this->messageSize)
			{
				throw DeviceException("UniversalRobotsRealtime::Message::unserialize() - Incorrect message size");
			}

			this->unserialize(ptr, this->time);
			this->unserialize(ptr, this->qTarget);
			this->unserialize(ptr, this->qdTarget);
			this->unserialize(ptr, this->qddTarget);
			this->unserialize(ptr, this->iTarget);
			this->unserialize(ptr, this->mTarget);
			this->unserialize(ptr, this->qActual);
			this->unserialize(ptr, this->qdActual);
			this->unserialize(ptr, this->iActual);

			if (756 == this->messageSize)
			{
				ptr += 3 * sizeof(double);
				ptr += 15 * sizeof(double);
				this->unserialize(ptr, this->tcpForce);
				this->unserialize(ptr, this->toolVectorActual);
				this->unserialize(ptr, this->tcpSpeedActual);
			}
			else if (764 == this->messageSize || 812 == this->messageSize)
			{
				this->unserialize(ptr, this->toolAccelerometerValues);
				ptr += 15 * sizeof(double);
				this->unserialize(ptr, this->tcpForce);
				this->unserialize(ptr, this->toolVectorActual);
				this->unserialize(ptr, this->tcpSpeedActual);
			}
			else if (1044 == this->messageSize || 1060 == this->messageSize || 1108 == this->messageSize)
			{
				this->unserialize(ptr, this->iControl);
				this->unserialize(ptr, this->toolVectorActual);
				this->unserialize(ptr, this->tcpSpeedActual);
				this->unserialize(ptr, this->tcpForce);
				this->unserialize(ptr, this->toolVectorTarget);
				this->unserialize(ptr, this->tcpSpeedTarget);
			}

			this->unserialize(ptr, this->digitalInputBits);
			this->unserialize(ptr, this->motorTemperatures);
			this->unserialize(ptr, this->controllerTimer);
			this->unserialize(ptr, this->testValue);

			if (764 == this->messageSize || 812 == this->messageSize || 1044 == this->messageSize || 1060 == this->messageSize || 1108 == this->messageSize)
			{
				this->unserialize(ptr, this->robotMode);
			}

			if (812 == this->messageSize || 1044 == this->messageSize || 1060 == this->messageSize || 1108 == this->messageSize)
			{
				this->unserialize(ptr, this->jointModes);
			}

			if (1044 == this->messageSize || 1060 == this->messageSize || 1108 == this->messageSize)
			{
				this->unserialize(ptr, this->safetyMode);
				ptr += 6 * sizeof(double);
				this->unserialize(ptr, this->toolAccelerometerValues);
				ptr += 6 * sizeof(double);
				this->unserialize(ptr, this->speedScaling);
				this->unserialize(ptr, this->linearMomentumNorm);
				ptr += 1 * sizeof(double);
				ptr += 1 * sizeof(double);
				this->unserialize(ptr, this->vMain);
				this->unserialize(ptr, this->vRobot);
				this->unserialize(ptr, this->iRobot);
				this->unserialize(ptr, this->vActual);
			}

			if (1060 == this->messageSize || 1108 == this->messageSize)
			{
				this->unserialize(ptr, this->digitalOutputs);
				this->unserialize(ptr, this->programState);
			}

			if (1108 == this->messageSize)
			{
				this->unserialize(ptr, this->elbowPosition);
				this->unserialize(ptr, this->elbowVelocity);
			}
		}

		template<>
		void
		UniversalRobotsRealtime::Message::unserialize(::std::uint8_t*& ptr, ::std::int64_t& t)
		{
			::std::memcpy(&t, ptr, sizeof(t));
			ptr += sizeof(t);
		}
	}
}
<commit_msg>Cleanup<commit_after>//
// Copyright (c) 2009, Markus Rickert
// 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.
//
// 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 HOLDER 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 <array>
#include <iostream>
#include <rl/math/Unit.h>

#include "DeviceException.h"
#include "UniversalRobotsRealtime.h"

namespace rl
{
	namespace hal
	{
		UniversalRobotsRealtime::UniversalRobotsRealtime(const ::std::string& address) :
			AxisController(6),
			DigitalInput(),
			DigitalOutput(),
			CartesianForceSensor(6),
			CartesianPositionSensor(6),
			CartesianVelocitySensor(6),
			CyclicDevice(::std::chrono::milliseconds(8)),
			DigitalInputReader(),
			DigitalOutputReader(),
			JointCurrentSensor(6),
			JointPositionSensor(6),
			JointVelocitySensor(6),
			in(),
			socket(Socket::Tcp(Socket::Address::Ipv4(address, 30003)))
		{
		}

		UniversalRobotsRealtime::~UniversalRobotsRealtime()
		{
			if (this->isRunning())
			{
				this->stop();
			}

			if (this->isConnected())
			{
				this->close();
			}
		}

		void
		UniversalRobotsRealtime::close()
		{
			this->socket.close();
			this->setConnected(false);
		}

		void
		UniversalRobotsRealtime::doScript(const ::std::string& script)
		{
			this->socket.send(script.c_str(), script.size());
		}

		::rl::math::ForceVector
		UniversalRobotsRealtime::getCartesianForce() const
		{
			::rl::math::ForceVector f;
			f.force().x() = this->in.tcpForce[0];
			f.force().y() = this->in.tcpForce[1];
			f.force().z() = this->in.tcpForce[2];
			f.moment().x() = this->in.tcpForce[3];
			f.moment().y() = this->in.tcpForce[4];
			f.moment().z() = this->in.tcpForce[5];
			return f;
		}

		::rl::math::Transform
		UniversalRobotsRealtime::getCartesianPosition() const
		{
			::rl::math::Transform x;
			x.setIdentity();

			::rl::math::Vector3 orientation(this->in.toolVectorActual[3], this->in.toolVectorActual[4], this->in.toolVectorActual[5]);
			::rl::math::Real norm = orientation.norm();

			if (::std::abs(norm) <= ::std::numeric_limits< ::rl::math::Real>::epsilon())
			{
				x.linear().setIdentity();
			}
			else
			{
				x.linear() = ::rl::math::AngleAxis(norm, orientation.normalized()).matrix();
			}

			x.translation().x() = this->in.toolVectorActual[0];
			x.translation().y() = this->in.toolVectorActual[1];
			x.translation().z() = this->in.toolVectorActual[2];

			return x;
		}

		::rl::math::MotionVector
		UniversalRobotsRealtime::getCartesianVelocity() const
		{
			::rl::math::MotionVector v;
			v.linear().x() = this->in.tcpSpeedActual[0];
			v.linear().y() = this->in.tcpSpeedActual[1];
			v.linear().z() = this->in.tcpSpeedActual[2];
			v.angular().x() = this->in.tcpSpeedActual[3];
			v.angular().y() = this->in.tcpSpeedActual[4];
			v.angular().z() = this->in.tcpSpeedActual[5];
			return v;
		}

		::boost::dynamic_bitset<>
		UniversalRobotsRealtime::getDigitalInput() const
		{
			return ::boost::dynamic_bitset<>(64, this->in.digitalInputBits);
		}

		bool
		UniversalRobotsRealtime::getDigitalInput(const ::std::size_t& i) const
		{
			return this->in.digitalInputBits & (1ULL << i) ? true : false;
		}

		::std::size_t
		UniversalRobotsRealtime::getDigitalInputCount() const
		{
			return 64;
		}

		::boost::dynamic_bitset<>
		UniversalRobotsRealtime::getDigitalOutput() const
		{
			return ::boost::dynamic_bitset<>(64, this->in.digitalOutputs);
		}

		bool
		UniversalRobotsRealtime::getDigitalOutput(const ::std::size_t& i) const
		{
			return this->in.digitalOutputs & (1ULL << i) ? true : false;
		}

		::std::size_t
		UniversalRobotsRealtime::getDigitalOutputCount() const
		{
			return 64;
		}

		::rl::math::Vector
		UniversalRobotsRealtime::getJointCurrent() const
		{
			::rl::math::Vector i(this->getDof());

			for (::std::size_t j = 0; j < 6; ++j)
			{
				i(j) = this->in.iActual[j];
			}

			return i;
		}

		::rl::math::Vector
		UniversalRobotsRealtime::getJointPosition() const
		{
			::rl::math::Vector q(this->getDof());

			for (::std::size_t i = 0; i < 6; ++i)
			{
				q(i) = this->in.qActual[i];
			}

			return q;
		}

		::rl::math::Vector
		UniversalRobotsRealtime::getJointVelocity() const
		{
			::rl::math::Vector qd(this->getDof());

			for (::std::size_t i = 0; i < 6; ++i)
			{
				qd(i) = this->in.qdActual[i];
			}

			return qd;
		}

		void
		UniversalRobotsRealtime::open()
		{
			this->socket.open();
			this->socket.connect();
			this->socket.setOption(::rl::hal::Socket::OPTION_NODELAY, 1);
			this->setConnected(true);
		}

		void
		UniversalRobotsRealtime::start()
		{
			this->setRunning(true);
		}

		void
		UniversalRobotsRealtime::step()
		{
			::std::array< ::std::uint8_t, 1108> buffer;
			this->socket.recv(buffer.data(), buffer.size());
#if !defined(__APPLE__) && !defined(__QNX__) && !defined(WIN32)
			this->socket.setOption(::rl::hal::Socket::OPTION_QUICKACK, 1);
#endif // __APPLE__ || __QNX__ || WIN32
			this->in.unserialize(buffer.data());
		}

		void
		UniversalRobotsRealtime::stop()
		{
			this->setRunning(false);
		}

		void
		UniversalRobotsRealtime::Message::unserialize(::std::uint8_t* ptr)
		{
			this->unserialize(ptr, this->messageSize);

			if (756 != this->messageSize && 764 != this->messageSize && 812 != this->messageSize && 1044 != this->messageSize && 1060 != this->messageSize && 1108 != this->messageSize)
			{
				throw DeviceException("UniversalRobotsRealtime::Message::unserialize() - Incorrect message size");
			}

			this->unserialize(ptr, this->time);
			this->unserialize(ptr, this->qTarget);
			this->unserialize(ptr, this->qdTarget);
			this->unserialize(ptr, this->qddTarget);
			this->unserialize(ptr, this->iTarget);
			this->unserialize(ptr, this->mTarget);
			this->unserialize(ptr, this->qActual);
			this->unserialize(ptr, this->qdActual);
			this->unserialize(ptr, this->iActual);

			if (756 == this->messageSize)
			{
				ptr += 3 * sizeof(double);
				ptr += 15 * sizeof(double);
				this->unserialize(ptr, this->tcpForce);
				this->unserialize(ptr, this->toolVectorActual);
				this->unserialize(ptr, this->tcpSpeedActual);
			}
			else if (764 == this->messageSize || 812 == this->messageSize)
			{
				this->unserialize(ptr, this->toolAccelerometerValues);
				ptr += 15 * sizeof(double);
				this->unserialize(ptr, this->tcpForce);
				this->unserialize(ptr, this->toolVectorActual);
				this->unserialize(ptr, this->tcpSpeedActual);
			}
			else if (1044 == this->messageSize || 1060 == this->messageSize || 1108 == this->messageSize)
			{
				this->unserialize(ptr, this->iControl);
				this->unserialize(ptr, this->toolVectorActual);
				this->unserialize(ptr, this->tcpSpeedActual);
				this->unserialize(ptr, this->tcpForce);
				this->unserialize(ptr, this->toolVectorTarget);
				this->unserialize(ptr, this->tcpSpeedTarget);
			}

			this->unserialize(ptr, this->digitalInputBits);
			this->unserialize(ptr, this->motorTemperatures);
			this->unserialize(ptr, this->controllerTimer);
			this->unserialize(ptr, this->testValue);

			if (764 == this->messageSize || 812 == this->messageSize || 1044 == this->messageSize || 1060 == this->messageSize || 1108 == this->messageSize)
			{
				this->unserialize(ptr, this->robotMode);
			}

			if (812 == this->messageSize || 1044 == this->messageSize || 1060 == this->messageSize || 1108 == this->messageSize)
			{
				this->unserialize(ptr, this->jointModes);
			}

			if (1044 == this->messageSize || 1060 == this->messageSize || 1108 == this->messageSize)
			{
				this->unserialize(ptr, this->safetyMode);
				ptr += 6 * sizeof(double);
				this->unserialize(ptr, this->toolAccelerometerValues);
				ptr += 6 * sizeof(double);
				this->unserialize(ptr, this->speedScaling);
				this->unserialize(ptr, this->linearMomentumNorm);
				ptr += 1 * sizeof(double);
				ptr += 1 * sizeof(double);
				this->unserialize(ptr, this->vMain);
				this->unserialize(ptr, this->vRobot);
				this->unserialize(ptr, this->iRobot);
				this->unserialize(ptr, this->vActual);
			}

			if (1060 == this->messageSize || 1108 == this->messageSize)
			{
				this->unserialize(ptr, this->digitalOutputs);
				this->unserialize(ptr, this->programState);
			}

			if (1108 == this->messageSize)
			{
				this->unserialize(ptr, this->elbowPosition);
				this->unserialize(ptr, this->elbowVelocity);
			}
		}

		template<>
		void
		UniversalRobotsRealtime::Message::unserialize(::std::uint8_t*& ptr, ::std::int64_t& t)
		{
			::std::memcpy(&t, ptr, sizeof(t));
			ptr += sizeof(t);
		}
	}
}
<|endoftext|>


<commit_before>/*
 * Copyright(c) Sophist Solutions, Inc. 1990-2021.  All rights reserved
 */
#include "../StroikaPreComp.h"

#include <charconv>
#include <cstdarg>
#include <cstdlib>
#include <iomanip>
#include <limits>
#include <sstream>
#if __cpp_lib_format
#include <format>
#endif

#include "../Characters/StringBuilder.h"
#include "../Characters/ToString.h"
#include "../Containers/Common.h"
#include "../Debug/Assertions.h"
#include "../Debug/Trace.h"
#include "../Debug/Valgrind.h"
#include "../Math/Common.h"
#include "../Memory/SmallStackBuffer.h"

#include "FloatConversion.h"

using namespace Stroika::Foundation;
using namespace Stroika::Foundation::Characters;
using namespace Stroika::Foundation::Memory;

/*
 ********************************************************************************
 ******************** FloatConversion::ToStringOptions **************************
 ********************************************************************************
 */
FloatConversion::ToStringOptions::ToStringOptions (UseCurrentLocale)
    : fUseLocale_{locale{}}
{
}

String FloatConversion::ToStringOptions::ToString () const
{
    StringBuilder sb;
    sb += L"{";
    if (fPrecision_) {
        sb += L"Precision:" + Characters::ToString (*fPrecision_) + L",";
    }
    if (fFmtFlags_) {
        sb += L"Fmt-Flags:" + Characters::ToString ((int)*fFmtFlags_, ios_base::hex) + L",";
    }
    if (fUseLocale_) {
        sb += L"Use-Locale" + String::FromNarrowSDKString (fUseLocale_->name ()) + L",";
    }
    if (fTrimTrailingZeros_) {
        sb += L"Trim-Trailing-Zeros: " + Characters::ToString (*fTrimTrailingZeros_) + L",";
    }
    if (fFloatFormat_) {
        sb += L"Scientific-Notation: " + Characters::ToString ((int)*fFloatFormat_) + L",";
    }
    sb += L"}";
    return sb.str ();
}

namespace {
    inline char* mkFmtWithPrecisionArg_ (char* formatBufferStart, [[maybe_unused]] char* formatBufferEnd, char _Spec)
    {
        char* fmtPtr = formatBufferStart;
        *fmtPtr++    = '%';
        // include precision arg
        *fmtPtr++ = '.';
        *fmtPtr++ = '*';
        if (_Spec != '\0') {
            *fmtPtr++ = _Spec; // e.g. 'L' qualifier
        }
        *fmtPtr++ = 'g'; // format specifier
        *fmtPtr   = '\0';
        Require (fmtPtr < formatBufferEnd);
        return formatBufferStart;
    }

    template <typename FLOAT_TYPE>
    inline String Float2String_GenericCase_ (FLOAT_TYPE f, const FloatConversion::ToStringOptions& options)
    {
        Require (not isnan (f));
        Require (not isinf (f));

        // expensive to construct, and slightly cheaper to just use thread_local version of
        // the same stringstream each time (only one per thread can be in use)
        static thread_local stringstream s;
        static const ios_base::fmtflags  kDefaultIOSFmtFlags_ = s.flags (); // Just copy out of the first constructed stringstream

        s.str (string{});
        s.clear ();

        static const locale kCLocale_ = locale::classic ();
        s.imbue (options.GetUseLocale ().value_or (kCLocale_));

        //  must set explictly (even if defaulted)  because of the thread_local thing
        s.flags (options.GetIOSFmtFlags ().value_or (kDefaultIOSFmtFlags_));

        // todo must set default precision because of the thread_local thing
        unsigned int usePrecision = options.GetPrecision ().value_or (FloatConversion::ToStringOptions::kDefaultPrecision.fPrecision);
        s.precision (usePrecision);

        {
            optional<ios_base::fmtflags> useFloatField;
            switch (options.GetFloatFormat ().value_or (FloatConversion::FloatFormatType::eDEFAULT)) {
                case FloatConversion::FloatFormatType::eScientific:
                    useFloatField = ios_base::scientific;
                    break;
                case FloatConversion::FloatFormatType::eDefaultFloat:
                    break;
                case FloatConversion::FloatFormatType::eFixedPoint:
                    useFloatField = ios_base::fixed;
                    break;
                case FloatConversion::FloatFormatType::eAutomatic: {
                    bool useScientificNotation = abs (f) >= pow (10, usePrecision / 2) or (f != 0 and abs (f) < pow (10, -static_cast<int> (usePrecision) / 2)); // scientific preserves more precision - but non-scientific looks better
                    if (useScientificNotation) {
                        useFloatField = ios_base::scientific;
                    }
                } break;
                default:
                    RequireNotReached ();
                    break;
            }
            if (useFloatField) {
                s.setf (*useFloatField, ios_base::floatfield);
            }
            else {
                s.unsetf (ios_base::floatfield); // see std::defaultfloat - not same as ios_base::fixed
            }
        }

        s << f;

        String tmp = options.GetUseLocale () ? String::FromNarrowString (s.str (), *options.GetUseLocale ()) : String::FromASCII (s.str ());
        if (options.GetTrimTrailingZeros ().value_or (FloatConversion::ToStringOptions::kDefaultTrimTrailingZeros)) {
            Characters::FloatConversion::Private_::TrimTrailingZeros_ (&tmp);
        }
        return tmp;
    }
    template <typename FLOAT_TYPE>
    inline String Float2String_ (FLOAT_TYPE f, const FloatConversion::ToStringOptions& options)
    {
        switch (fpclassify (f)) {
            case FP_INFINITE: {
                Assert (isinf (f));
                Assert (!isnan (f));
                static const String kNEG_INF_STR_{L"-INF"sv};
                static const String kINF_STR_{L"INF"sv};
                return f > 0 ? kINF_STR_ : kNEG_INF_STR_;
            }
            case FP_NAN: {
                Assert (isnan (f));
                Assert (!isinf (f));
                static const String kNAN_STR_{L"NAN"sv};
                return kNAN_STR_;
            }
            default: {
                if constexpr (qCompilerAndStdLib_valgrind_fpclassify_check_Buggy && qDebug) {
                    if (Debug::IsRunningUnderValgrind ()) {
                        if (isinf (f)) {
                            DbgTrace ("fpclassify (%f) = %d", (double)f, fpclassify (f));
                            static const String kNEG_INF_STR_{L"-INF"sv};
                            static const String kINF_STR_{L"INF"sv};
                            return f > 0 ? kINF_STR_ : kNEG_INF_STR_;
                        }
                    }
                }
            }
        }
        Assert (!isnan (f));
        Assert (!isinf (f));
        return Float2String_GenericCase_<FLOAT_TYPE> (f, options);
    }
}

/*
 ********************************************************************************
 *********** FloatConversion::Private_::Legacy_Float2String_ ********************
 ********************************************************************************
 */
String Characters::FloatConversion::Private_::Legacy_Float2String_ (float f, const FloatConversion::ToStringOptions& options)
{
    return Float2String_<float> (f, options);
}
String Characters::FloatConversion::Private_::Legacy_Float2String_ (double f, const FloatConversion::ToStringOptions& options)
{
    return Float2String_<double> (f, options);
}
String Characters::FloatConversion::Private_::Legacy_Float2String_ (long double f, const FloatConversion::ToStringOptions& options)
{
    return Float2String_<long double> (f, options);
}

/*
 ********************************************************************************
 ********************************* String2Float *********************************
 ********************************************************************************
 */
namespace {
    template <typename RETURN_TYPE, typename FUNCTION>
    inline RETURN_TYPE String2Float_LegacyStr2DHelper_ (const String& s, FUNCTION F)
    {
        wchar_t*       e   = nullptr;
        const wchar_t* cst = s.c_str ();
        if (::iswspace (*cst)) {
            return Math::nan<RETURN_TYPE> (); // this was a bug in the 2.1b14 and earlier code according to docs - we should REJECT strings with leading space in this case
        }
        RETURN_TYPE d = F (cst, &e);
        // if trailing crap - return nan
        if (*e != '\0') {
            return Math::nan<RETURN_TYPE> ();
        }
        if (d == 0) {
            if (cst == e) {
                return Math::nan<RETURN_TYPE> ();
            }
        }
        return d;
    }
    template <typename RETURN_TYPE, typename FUNCTION>
    inline RETURN_TYPE String2Float_LegacyStr2DHelper_ (const String& s, String* remainder, FUNCTION F)
    {
        RequireNotNull (remainder);
        wchar_t*       e   = nullptr;
        const wchar_t* cst = s.c_str ();
        RETURN_TYPE    d   = F (cst, &e);
        *remainder         = e;
        return d;
    }

    template <typename RETURN_TYPE>
    RETURN_TYPE String2Float_LegacyStr2D_ (const String& s);
    template <>
    inline float String2Float_LegacyStr2D_ (const String& s)
    {
        return String2Float_LegacyStr2DHelper_<float> (s, wcstof);
    }
    template <>
    inline double String2Float_LegacyStr2D_ (const String& s)
    {
        return String2Float_LegacyStr2DHelper_<double> (s, wcstod);
    }
    template <>
    inline long double String2Float_LegacyStr2D_ (const String& s)
    {
        return String2Float_LegacyStr2DHelper_<long double> (s, wcstold);
    }

}

namespace Stroika::Foundation::Characters::FloatConversion::Private_ {
    template <>
    float ToFloat_Legacy_ (const String& s)
    {
        return String2Float_LegacyStr2D_<float> (s);
    }
    template <>
    double ToFloat_Legacy_ (const String& s)
    {
        return String2Float_LegacyStr2D_<double> (s);
    }
    template <>
    long double ToFloat_Legacy_ (const String& s)
    {
        return String2Float_LegacyStr2D_<long double> (s);
    }
}

namespace Stroika::Foundation::Characters::FloatConversion::Private_ {
    template <>
    float ToFloat_Legacy_ (const String& s, String* remainder)
    {
        return String2Float_LegacyStr2DHelper_<float> (s, remainder, wcstof);
    }
    template <>
    double ToFloat_Legacy_ (const String& s, String* remainder)
    {
        return String2Float_LegacyStr2DHelper_<double> (s, remainder, wcstod);
    }
    template <>
    long double ToFloat_Legacy_ (const String& s, String* remainder)
    {
        return String2Float_LegacyStr2DHelper_<long double> (s, remainder, wcstold);
    }
}
<commit_msg>cosmetic<commit_after>/*
 * Copyright(c) Sophist Solutions, Inc. 1990-2021.  All rights reserved
 */
#include "../StroikaPreComp.h"

#include <charconv>
#include <cstdarg>
#include <cstdlib>
#include <iomanip>
#include <limits>
#include <sstream>

#include "../Characters/StringBuilder.h"
#include "../Characters/ToString.h"
#include "../Containers/Common.h"
#include "../Debug/Assertions.h"
#include "../Debug/Trace.h"
#include "../Debug/Valgrind.h"
#include "../Math/Common.h"
#include "../Memory/SmallStackBuffer.h"

#include "FloatConversion.h"

using namespace Stroika::Foundation;
using namespace Stroika::Foundation::Characters;
using namespace Stroika::Foundation::Memory;

/*
 ********************************************************************************
 ******************** FloatConversion::ToStringOptions **************************
 ********************************************************************************
 */
FloatConversion::ToStringOptions::ToStringOptions (UseCurrentLocale)
    : fUseLocale_{locale{}}
{
}

String FloatConversion::ToStringOptions::ToString () const
{
    StringBuilder sb;
    sb += L"{";
    if (fPrecision_) {
        sb += L"Precision:" + Characters::ToString (*fPrecision_) + L",";
    }
    if (fFmtFlags_) {
        sb += L"Fmt-Flags:" + Characters::ToString ((int)*fFmtFlags_, ios_base::hex) + L",";
    }
    if (fUseLocale_) {
        sb += L"Use-Locale" + String::FromNarrowSDKString (fUseLocale_->name ()) + L",";
    }
    if (fTrimTrailingZeros_) {
        sb += L"Trim-Trailing-Zeros: " + Characters::ToString (*fTrimTrailingZeros_) + L",";
    }
    if (fFloatFormat_) {
        sb += L"Scientific-Notation: " + Characters::ToString ((int)*fFloatFormat_) + L",";
    }
    sb += L"}";
    return sb.str ();
}

namespace {
    inline char* mkFmtWithPrecisionArg_ (char* formatBufferStart, [[maybe_unused]] char* formatBufferEnd, char _Spec)
    {
        char* fmtPtr = formatBufferStart;
        *fmtPtr++    = '%';
        // include precision arg
        *fmtPtr++ = '.';
        *fmtPtr++ = '*';
        if (_Spec != '\0') {
            *fmtPtr++ = _Spec; // e.g. 'L' qualifier
        }
        *fmtPtr++ = 'g'; // format specifier
        *fmtPtr   = '\0';
        Require (fmtPtr < formatBufferEnd);
        return formatBufferStart;
    }

    template <typename FLOAT_TYPE>
    inline String Float2String_GenericCase_ (FLOAT_TYPE f, const FloatConversion::ToStringOptions& options)
    {
        Require (not isnan (f));
        Require (not isinf (f));

        // expensive to construct, and slightly cheaper to just use thread_local version of
        // the same stringstream each time (only one per thread can be in use)
        static thread_local stringstream s;
        static const ios_base::fmtflags  kDefaultIOSFmtFlags_ = s.flags (); // Just copy out of the first constructed stringstream

        s.str (string{});
        s.clear ();

        static const locale kCLocale_ = locale::classic ();
        s.imbue (options.GetUseLocale ().value_or (kCLocale_));

        //  must set explictly (even if defaulted)  because of the thread_local thing
        s.flags (options.GetIOSFmtFlags ().value_or (kDefaultIOSFmtFlags_));

        // todo must set default precision because of the thread_local thing
        unsigned int usePrecision = options.GetPrecision ().value_or (FloatConversion::ToStringOptions::kDefaultPrecision.fPrecision);
        s.precision (usePrecision);

        {
            optional<ios_base::fmtflags> useFloatField;
            switch (options.GetFloatFormat ().value_or (FloatConversion::FloatFormatType::eDEFAULT)) {
                case FloatConversion::FloatFormatType::eScientific:
                    useFloatField = ios_base::scientific;
                    break;
                case FloatConversion::FloatFormatType::eDefaultFloat:
                    break;
                case FloatConversion::FloatFormatType::eFixedPoint:
                    useFloatField = ios_base::fixed;
                    break;
                case FloatConversion::FloatFormatType::eAutomatic: {
                    bool useScientificNotation = abs (f) >= pow (10, usePrecision / 2) or (f != 0 and abs (f) < pow (10, -static_cast<int> (usePrecision) / 2)); // scientific preserves more precision - but non-scientific looks better
                    if (useScientificNotation) {
                        useFloatField = ios_base::scientific;
                    }
                } break;
                default:
                    RequireNotReached ();
                    break;
            }
            if (useFloatField) {
                s.setf (*useFloatField, ios_base::floatfield);
            }
            else {
                s.unsetf (ios_base::floatfield); // see std::defaultfloat - not same as ios_base::fixed
            }
        }

        s << f;

        String tmp = options.GetUseLocale () ? String::FromNarrowString (s.str (), *options.GetUseLocale ()) : String::FromASCII (s.str ());
        if (options.GetTrimTrailingZeros ().value_or (FloatConversion::ToStringOptions::kDefaultTrimTrailingZeros)) {
            Characters::FloatConversion::Private_::TrimTrailingZeros_ (&tmp);
        }
        return tmp;
    }
    template <typename FLOAT_TYPE>
    inline String Float2String_ (FLOAT_TYPE f, const FloatConversion::ToStringOptions& options)
    {
        switch (fpclassify (f)) {
            case FP_INFINITE: {
                Assert (isinf (f));
                Assert (!isnan (f));
                static const String kNEG_INF_STR_{L"-INF"sv};
                static const String kINF_STR_{L"INF"sv};
                return f > 0 ? kINF_STR_ : kNEG_INF_STR_;
            }
            case FP_NAN: {
                Assert (isnan (f));
                Assert (!isinf (f));
                static const String kNAN_STR_{L"NAN"sv};
                return kNAN_STR_;
            }
            default: {
                if constexpr (qCompilerAndStdLib_valgrind_fpclassify_check_Buggy && qDebug) {
                    if (Debug::IsRunningUnderValgrind ()) {
                        if (isinf (f)) {
                            DbgTrace ("fpclassify (%f) = %d", (double)f, fpclassify (f));
                            static const String kNEG_INF_STR_{L"-INF"sv};
                            static const String kINF_STR_{L"INF"sv};
                            return f > 0 ? kINF_STR_ : kNEG_INF_STR_;
                        }
                    }
                }
            }
        }
        Assert (!isnan (f));
        Assert (!isinf (f));
        return Float2String_GenericCase_<FLOAT_TYPE> (f, options);
    }
}

/*
 ********************************************************************************
 *********** FloatConversion::Private_::Legacy_Float2String_ ********************
 ********************************************************************************
 */
String Characters::FloatConversion::Private_::Legacy_Float2String_ (float f, const FloatConversion::ToStringOptions& options)
{
    return Float2String_<float> (f, options);
}
String Characters::FloatConversion::Private_::Legacy_Float2String_ (double f, const FloatConversion::ToStringOptions& options)
{
    return Float2String_<double> (f, options);
}
String Characters::FloatConversion::Private_::Legacy_Float2String_ (long double f, const FloatConversion::ToStringOptions& options)
{
    return Float2String_<long double> (f, options);
}

/*
 ********************************************************************************
 ********************************* String2Float *********************************
 ********************************************************************************
 */
namespace {
    template <typename RETURN_TYPE, typename FUNCTION>
    inline RETURN_TYPE String2Float_LegacyStr2DHelper_ (const String& s, FUNCTION F)
    {
        wchar_t*       e   = nullptr;
        const wchar_t* cst = s.c_str ();
        if (::iswspace (*cst)) {
            return Math::nan<RETURN_TYPE> (); // this was a bug in the 2.1b14 and earlier code according to docs - we should REJECT strings with leading space in this case
        }
        RETURN_TYPE d = F (cst, &e);
        // if trailing crap - return nan
        if (*e != '\0') {
            return Math::nan<RETURN_TYPE> ();
        }
        if (d == 0) {
            if (cst == e) {
                return Math::nan<RETURN_TYPE> ();
            }
        }
        return d;
    }
    template <typename RETURN_TYPE, typename FUNCTION>
    inline RETURN_TYPE String2Float_LegacyStr2DHelper_ (const String& s, String* remainder, FUNCTION F)
    {
        RequireNotNull (remainder);
        wchar_t*       e   = nullptr;
        const wchar_t* cst = s.c_str ();
        RETURN_TYPE    d   = F (cst, &e);
        *remainder         = e;
        return d;
    }

    template <typename RETURN_TYPE>
    RETURN_TYPE String2Float_LegacyStr2D_ (const String& s);
    template <>
    inline float String2Float_LegacyStr2D_ (const String& s)
    {
        return String2Float_LegacyStr2DHelper_<float> (s, wcstof);
    }
    template <>
    inline double String2Float_LegacyStr2D_ (const String& s)
    {
        return String2Float_LegacyStr2DHelper_<double> (s, wcstod);
    }
    template <>
    inline long double String2Float_LegacyStr2D_ (const String& s)
    {
        return String2Float_LegacyStr2DHelper_<long double> (s, wcstold);
    }

}

namespace Stroika::Foundation::Characters::FloatConversion::Private_ {
    template <>
    float ToFloat_Legacy_ (const String& s)
    {
        return String2Float_LegacyStr2D_<float> (s);
    }
    template <>
    double ToFloat_Legacy_ (const String& s)
    {
        return String2Float_LegacyStr2D_<double> (s);
    }
    template <>
    long double ToFloat_Legacy_ (const String& s)
    {
        return String2Float_LegacyStr2D_<long double> (s);
    }
}

namespace Stroika::Foundation::Characters::FloatConversion::Private_ {
    template <>
    float ToFloat_Legacy_ (const String& s, String* remainder)
    {
        return String2Float_LegacyStr2DHelper_<float> (s, remainder, wcstof);
    }
    template <>
    double ToFloat_Legacy_ (const String& s, String* remainder)
    {
        return String2Float_LegacyStr2DHelper_<double> (s, remainder, wcstod);
    }
    template <>
    long double ToFloat_Legacy_ (const String& s, String* remainder)
    {
        return String2Float_LegacyStr2DHelper_<long double> (s, remainder, wcstold);
    }
}
<|endoftext|>


<commit_before>/*
 * LuaTangibleObject.cpp
 *
 *  Created on: 24/01/2012
 *      Author: xyborn
 */

#include "LuaTangibleObject.h"
#include "server/zone/objects/tangible/TangibleObject.h"
#include "server/zone/objects/creature/CreatureFlag.h"
#include "server/zone/templates/params/PaletteColorCustomizationVariable.h"
#include "server/zone/templates/customization/AssetCustomizationManagerTemplate.h"
#include "server/zone/templates/appearance/PaletteTemplate.h"

const char LuaTangibleObject::className[] = "LuaTangibleObject";

Luna<LuaTangibleObject>::RegType LuaTangibleObject::Register[] = {
		{ "_setObject", &LuaTangibleObject::_setObject },
		{ "_getObject", &LuaSceneObject::_getObject },
		{ "setOptionsBitmask", &LuaTangibleObject::setOptionsBitmask },
		{ "setPvpStatusBitmask", &LuaTangibleObject::setPvpStatusBitmask },
		{ "setPvpStatusBit", &LuaTangibleObject::setPvpStatusBit },
		{ "getPvpStatusBitmask", &LuaTangibleObject::getPvpStatusBitmask },
		{ "isChangingFactionStatus", &LuaTangibleObject::isChangingFactionStatus },
		{ "setCustomizationVariable", &LuaTangibleObject::setCustomizationVariable },
		{ "getPaletteColorCount", &LuaTangibleObject::getPaletteColorCount },
		{ "setConditionDamage", &LuaTangibleObject::setConditionDamage },
		{ "setFaction", &LuaTangibleObject::setFaction },
		{ "getFaction", &LuaTangibleObject::getFaction },
		{ "isImperial", &LuaTangibleObject::isImperial },
		{ "isRebel", &LuaTangibleObject::isRebel },
		{ "hasActiveArea", &LuaTangibleObject::hasActiveArea},
		{ 0, 0 }
};

LuaTangibleObject::LuaTangibleObject(lua_State *L) : LuaSceneObject(L) {
#ifdef DYNAMIC_CAST_LUAOBJECTS
	realObject = dynamic_cast<TangibleObject*>(_getRealSceneObject());

	assert(!_getRealSceneObject() || realObject != NULL);
#else
	realObject = static_cast<TangibleObject*>(lua_touserdata(L, 1));
#endif
}

LuaTangibleObject::~LuaTangibleObject(){
}

int LuaTangibleObject::_setObject(lua_State* L) {
	LuaSceneObject::_setObject(L);

#ifdef DYNAMIC_CAST_LUAOBJECTS
	realObject = dynamic_cast<TangibleObject*>(_getRealSceneObject());

	assert(!_getRealSceneObject() || realObject != NULL);
#else
	realObject = static_cast<TangibleObject*>(lua_touserdata(L, -1));
#endif

	return 0;
}

int LuaTangibleObject::setCustomizationVariable(lua_State* L) {
	String type = lua_tostring(L, -2);
	int value = lua_tonumber(L, -1);

	realObject->setCustomizationVariable(type, value, true);

	return 0;
}

int LuaTangibleObject::getPaletteColorCount(lua_State* L) {
	String variableName = lua_tostring(L, -1);

	String appearanceFilename = realObject->getObjectTemplate()->getAppearanceFilename();
	VectorMap<String, Reference<CustomizationVariable*> > variables;
	AssetCustomizationManagerTemplate::instance()->getCustomizationVariables(appearanceFilename.hashCode(), variables, false);

	int colors = 0;

	for (int i = 0; i< variables.size(); ++i) {
		String varkey = variables.elementAt(i).getKey();

		if (varkey.contains(variableName)) {
			CustomizationVariable* customizationVariable = variables.get(varkey).get();

			if (customizationVariable == NULL)
				continue;

			PaletteColorCustomizationVariable* palette = dynamic_cast<PaletteColorCustomizationVariable*>(customizationVariable);

			if (palette != NULL) {
				String paletteFileName = palette->getPaletteFileName();
				PaletteTemplate* paletteTemplate = TemplateManager::instance()->getPaletteTemplate(paletteFileName);

				if (paletteTemplate == NULL)
					continue;

				colors = paletteTemplate->getColorCount();
				break;
			}
		}
	}

	lua_pushinteger(L, colors);

	return 1;
}

int LuaTangibleObject::setOptionsBitmask(lua_State* L) {
	uint32 bitmask = lua_tointeger(L, -1);

	realObject->setOptionsBitmask(bitmask, true);

	return 0;
}

int LuaTangibleObject::setPvpStatusBitmask(lua_State* L) {
	uint32 bitmask = lua_tointeger(L, -1);

	realObject->setPvpStatusBitmask(bitmask, true);

	return 0;
}

int LuaTangibleObject::setPvpStatusBit(lua_State* L) {
	uint32 bit = lua_tointeger(L, -1);

	realObject->setPvpStatusBit(bit, true);

	return 0;
}

int LuaTangibleObject::getPvpStatusBitmask(lua_State* L) {
	uint32 bitmask = realObject->getPvpStatusBitmask();

	lua_pushinteger(L, bitmask);

	return 1;
}

int LuaTangibleObject::isChangingFactionStatus(lua_State* L) {
	lua_pushboolean(L, realObject->getPvpStatusBitmask() & CreatureFlag::CHANGEFACTIONSTATUS);

	return 1;
}

int LuaTangibleObject::setConditionDamage(lua_State* L) {
	float damage = lua_tonumber(L, -1);

	realObject->setConditionDamage(damage, true);

	return 0;
}

int LuaTangibleObject::setFaction(lua_State* L){
	uint32 faction = lua_tointeger(L, -1);

	realObject->setFaction(faction);

	return 0;
}

int LuaTangibleObject::getFaction(lua_State* L){
	uint32 faction = realObject->getFaction();

	lua_pushinteger(L, faction);

	return 1;

}

int LuaTangibleObject::isImperial(lua_State* L){
	lua_pushboolean(L, realObject->isImperial());

	return 1;
}

int LuaTangibleObject::isRebel(lua_State* L){
	lua_pushboolean(L, realObject->isRebel());

	return 1;
}

int LuaTangibleObject::hasActiveArea(lua_State* L) {
	uint64 objectid = lua_tointeger(L, -1);

	bool res = realObject->hasActiveArea(objectid);

	lua_pushboolean(L, res);

	return 1;
}
<commit_msg>[Fixed] stability issue<commit_after>/*
 * LuaTangibleObject.cpp
 *
 *  Created on: 24/01/2012
 *      Author: xyborn
 */

#include "LuaTangibleObject.h"
#include "server/zone/objects/tangible/TangibleObject.h"
#include "server/zone/objects/creature/CreatureFlag.h"
#include "server/zone/templates/params/PaletteColorCustomizationVariable.h"
#include "server/zone/templates/customization/AssetCustomizationManagerTemplate.h"
#include "server/zone/templates/appearance/PaletteTemplate.h"

const char LuaTangibleObject::className[] = "LuaTangibleObject";

Luna<LuaTangibleObject>::RegType LuaTangibleObject::Register[] = {
		{ "_setObject", &LuaTangibleObject::_setObject },
		{ "_getObject", &LuaSceneObject::_getObject },
		{ "setOptionsBitmask", &LuaTangibleObject::setOptionsBitmask },
		{ "setPvpStatusBitmask", &LuaTangibleObject::setPvpStatusBitmask },
		{ "setPvpStatusBit", &LuaTangibleObject::setPvpStatusBit },
		{ "getPvpStatusBitmask", &LuaTangibleObject::getPvpStatusBitmask },
		{ "isChangingFactionStatus", &LuaTangibleObject::isChangingFactionStatus },
		{ "setCustomizationVariable", &LuaTangibleObject::setCustomizationVariable },
		{ "getPaletteColorCount", &LuaTangibleObject::getPaletteColorCount },
		{ "setConditionDamage", &LuaTangibleObject::setConditionDamage },
		{ "setFaction", &LuaTangibleObject::setFaction },
		{ "getFaction", &LuaTangibleObject::getFaction },
		{ "isImperial", &LuaTangibleObject::isImperial },
		{ "isRebel", &LuaTangibleObject::isRebel },
		{ "hasActiveArea", &LuaTangibleObject::hasActiveArea},
		{ 0, 0 }
};

LuaTangibleObject::LuaTangibleObject(lua_State *L) : LuaSceneObject(L) {
#ifdef DYNAMIC_CAST_LUAOBJECTS
	realObject = dynamic_cast<TangibleObject*>(_getRealSceneObject());

	assert(!_getRealSceneObject() || realObject != NULL);
#else
	realObject = static_cast<TangibleObject*>(lua_touserdata(L, 1));
#endif
}

LuaTangibleObject::~LuaTangibleObject(){
}

int LuaTangibleObject::_setObject(lua_State* L) {
	LuaSceneObject::_setObject(L);

#ifdef DYNAMIC_CAST_LUAOBJECTS
	realObject = dynamic_cast<TangibleObject*>(_getRealSceneObject());

	assert(!_getRealSceneObject() || realObject != NULL);
#else
	realObject = static_cast<TangibleObject*>(lua_touserdata(L, -1));
#endif

	return 0;
}

int LuaTangibleObject::setCustomizationVariable(lua_State* L) {
	String type = lua_tostring(L, -2);
	int value = lua_tonumber(L, -1);

	Locker locker(realObject);

	realObject->setCustomizationVariable(type, value, true);

	return 0;
}

int LuaTangibleObject::getPaletteColorCount(lua_State* L) {
	String variableName = lua_tostring(L, -1);

	String appearanceFilename = realObject->getObjectTemplate()->getAppearanceFilename();
	VectorMap<String, Reference<CustomizationVariable*> > variables;
	AssetCustomizationManagerTemplate::instance()->getCustomizationVariables(appearanceFilename.hashCode(), variables, false);

	int colors = 0;

	for (int i = 0; i< variables.size(); ++i) {
		String varkey = variables.elementAt(i).getKey();

		if (varkey.contains(variableName)) {
			CustomizationVariable* customizationVariable = variables.get(varkey).get();

			if (customizationVariable == NULL)
				continue;

			PaletteColorCustomizationVariable* palette = dynamic_cast<PaletteColorCustomizationVariable*>(customizationVariable);

			if (palette != NULL) {
				String paletteFileName = palette->getPaletteFileName();
				PaletteTemplate* paletteTemplate = TemplateManager::instance()->getPaletteTemplate(paletteFileName);

				if (paletteTemplate == NULL)
					continue;

				colors = paletteTemplate->getColorCount();
				break;
			}
		}
	}

	lua_pushinteger(L, colors);

	return 1;
}

int LuaTangibleObject::setOptionsBitmask(lua_State* L) {
	uint32 bitmask = lua_tointeger(L, -1);

	realObject->setOptionsBitmask(bitmask, true);

	return 0;
}

int LuaTangibleObject::setPvpStatusBitmask(lua_State* L) {
	uint32 bitmask = lua_tointeger(L, -1);

	realObject->setPvpStatusBitmask(bitmask, true);

	return 0;
}

int LuaTangibleObject::setPvpStatusBit(lua_State* L) {
	uint32 bit = lua_tointeger(L, -1);

	realObject->setPvpStatusBit(bit, true);

	return 0;
}

int LuaTangibleObject::getPvpStatusBitmask(lua_State* L) {
	uint32 bitmask = realObject->getPvpStatusBitmask();

	lua_pushinteger(L, bitmask);

	return 1;
}

int LuaTangibleObject::isChangingFactionStatus(lua_State* L) {
	lua_pushboolean(L, realObject->getPvpStatusBitmask() & CreatureFlag::CHANGEFACTIONSTATUS);

	return 1;
}

int LuaTangibleObject::setConditionDamage(lua_State* L) {
	float damage = lua_tonumber(L, -1);

	realObject->setConditionDamage(damage, true);

	return 0;
}

int LuaTangibleObject::setFaction(lua_State* L){
	uint32 faction = lua_tointeger(L, -1);

	realObject->setFaction(faction);

	return 0;
}

int LuaTangibleObject::getFaction(lua_State* L){
	uint32 faction = realObject->getFaction();

	lua_pushinteger(L, faction);

	return 1;

}

int LuaTangibleObject::isImperial(lua_State* L){
	lua_pushboolean(L, realObject->isImperial());

	return 1;
}

int LuaTangibleObject::isRebel(lua_State* L){
	lua_pushboolean(L, realObject->isRebel());

	return 1;
}

int LuaTangibleObject::hasActiveArea(lua_State* L) {
	uint64 objectid = lua_tointeger(L, -1);

	bool res = realObject->hasActiveArea(objectid);

	lua_pushboolean(L, res);

	return 1;
}
<|endoftext|>