<commit_before>// Ouzel by Elviss Strazdins

#ifndef OUZEL_MATH_VECTOR_HPP
#define OUZEL_MATH_VECTOR_HPP

#include <array>
#include <cassert>
#include <cmath>
#include <cstddef>
#include <limits>
#include <type_traits>
#include <utility>

namespace ouzel
{
    template <typename T, std::size_t N> class Vector final
    {
    public:
#ifdef __SSE__
        alignas(N == 4 ? 4 * sizeof(T) : alignof(T))
#endif
        std::array<T, N> v{};

        constexpr Vector() noexcept {}

        template <typename ...A>
        explicit constexpr Vector(const A... args) noexcept:
            v{args...}
        {
        }

        template <auto X = N, std::size_t M, std::enable_if_t<(X != M)>* = nullptr>
        explicit Vector(const Vector<T, M>& vec) noexcept
        {
            for (std::size_t i = 0; i < N && i < M; ++i)
                v[i] = vec.v[i];
        }

        auto& operator[](const std::size_t index) noexcept { return v[index]; }
        constexpr auto operator[](const std::size_t index) const noexcept { return v[index]; }

        template <auto X = N, std::enable_if_t<(X >= 1)>* = nullptr>
        auto& x() noexcept { return v[0]; }

        template <auto X = N, std::enable_if_t<(X >= 1)>* = nullptr>
        constexpr auto x() const noexcept { return v[0]; }

        template <auto X = N, std::enable_if_t<(X >= 2)>* = nullptr>
        auto& y() noexcept { return v[1]; }

        template <auto X = N, std::enable_if_t<(X >= 2)>* = nullptr>
        constexpr auto y() const noexcept { return v[1]; }

        template <auto X = N, std::enable_if_t<(X >= 3)>* = nullptr>
        auto& z() noexcept { return v[2]; }

        template <auto X = N, std::enable_if_t<(X >= 3)>* = nullptr>
        constexpr auto z() const noexcept { return v[2]; }

        template <auto X = N, std::enable_if_t<(X >= 4)>* = nullptr>
        auto& w() noexcept { return v[3]; }

        template <auto X = N, std::enable_if_t<(X >= 4)>* = nullptr>
        constexpr auto w() const noexcept { return v[3]; }

        template <auto X = N, std::enable_if_t<(X == 2)>* = nullptr>
        auto getAngle() const noexcept
        {
            return std::atan2(v[1], v[0]);
        }

        template <auto X = N, std::enable_if_t<(X == 3)>* = nullptr>
        auto getAngle(const Vector& axis) const noexcept
        {
            constexpr T dx = axis.v[0] - v[0] - v[1] * axis.v[2] + v[2] * axis.v[1];
            constexpr T dy = axis.v[1] - v[1] - v[2] * axis.v[0] + v[0] * axis.v[2];
            constexpr T dz = axis.v[2] - v[2] - v[0] * axis.v[1] + v[1] * axis.v[0];

            return std::atan2(std::sqrt(dx * dx + dy * dy + dz * dz), dot(axis));
        }

        template <auto X = N, std::enable_if_t<(X == 4)>* = nullptr>
        auto getAngle(const Vector& axis) const noexcept
        {
            constexpr T dx = v[3] * axis.v[0] - v[0] * axis.v[3] - v[1] * axis.v[2] + v[2] * axis.v[1];
            constexpr T dy = v[3] * axis.v[1] - v[1] * axis.v[3] - v[2] * axis.v[0] + v[0] * axis.v[2];
            constexpr T dz = v[3] * axis.v[2] - v[2] * axis.v[3] - v[0] * axis.v[1] + v[1] * axis.v[0];

            return std::atan2(std::sqrt(dx * dx + dy * dy + dz * dz), dot(axis));
        }

        void clamp(const Vector& min, const Vector& max) noexcept
        {
            for (std::size_t i = 0; i < N; ++i)
                if (v[i] < min.v[i]) v[i] = min.v[i];
                else if (v[i] > max.v[i]) v[i] = max.v[i];
        }

        template <auto X = N, std::enable_if_t<(X == 3)>* = nullptr>
        constexpr auto cross(const Vector& vec) const noexcept
        {
            return Vector{
                (v[1] * vec.v[2]) - (v[2] * vec.v[1]),
                (v[2] * vec.v[0]) - (v[0] * vec.v[2]),
                (v[0] * vec.v[1]) - (v[1] * vec.v[0])
            };
        }

        auto distance(const Vector& vec) const noexcept
        {
            T d = T(0);
            for (std::size_t i = 0; i < N; ++i)
                d += (vec.v[i] - v[i]) * (vec.v[i] - v[i]);
            return std::sqrt(d);
        }

        auto distanceSquared(const Vector& vec) const noexcept
        {
            T d = T(0);
            for (std::size_t i = 0; i < N; ++i)
                d += (vec.v[i] - v[i]) * (vec.v[i] - v[i]);
            return d;
        }

        constexpr auto length() const noexcept
        {
            return std::sqrt(generateLengthSquared(std::make_index_sequence<N>{}));
        }

        constexpr auto lengthSquared() const noexcept
        {
            return generateLengthSquared(std::make_index_sequence<N>{});
        }

        constexpr auto dot(const Vector& vec) const noexcept
        {
            return generateDot(std::make_index_sequence<N>{}, vec);
        }

        void negate() noexcept
        {
            for (auto& c : v)
                c = -c;
        }

        auto isNormalized(const T tolerance = std::numeric_limits<T>::min()) const noexcept
        {
            return std::abs(T(1) - lengthSquared()) < tolerance;
        }

        void normalize() noexcept
        {
            T squared = T(0);
            for (const auto& c : v)
                squared += c * c;

            if (squared == T(1)) // already normalized
                return;

            const auto length = std::sqrt(squared);
            if (length <= std::numeric_limits<T>::min()) // too close to zero
                return;

            const auto multiplier = T(1) / length;
            for (auto& c : v)
                c *= multiplier;
        }

        auto normalized() const noexcept
        {
            T squared = T(0);
            for (const auto& c : v)
                squared += c * c;

            if (squared == T(1)) // already normalized
                return *this;

            const auto length = std::sqrt(squared);
            if (length <= std::numeric_limits<T>::min()) // too close to zero
                return *this;

            const auto multiplier = T(1) / length;
            return *this * multiplier;
        }

        void scale(const T scalar) noexcept
        {
            for (std::size_t i = 0; i < N; ++i)
                v[i] *= scalar;
        }

        void scale(const Vector& scale) noexcept
        {
            for (std::size_t i = 0; i < N; ++i)
                v[i] *= scale.v[i];
        }

        template <auto X = N, std::enable_if_t<(X == 2)>* = nullptr>
        void rotate(const T angle) noexcept
        {
            const auto sinAngle = std::sin(angle);
            const auto cosAngle = std::cos(angle);

            const auto tempX = v[0] * cosAngle - v[1] * sinAngle;
            v[1] = v[1] * cosAngle + v[0] * sinAngle;
            v[0] = tempX;
        }

        template <auto X = N, std::enable_if_t<(X == 2)>* = nullptr>
        void rotate(const Vector<T, 2>& point, T angle) noexcept
        {
            const float sinAngle = std::sin(angle);
            const float cosAngle = std::cos(angle);

            if (point.isZero())
            {
                const auto tempX = v[0] * cosAngle - v[1] * sinAngle;
                v[1] = v[1] * cosAngle + v[0] * sinAngle;
                v[0] = tempX;
            }
            else
            {
                const auto tempX = v[0] - point.v[0];
                const auto tempY = v[1] - point.v[1];

                v[0] = tempX * cosAngle - tempY * sinAngle + point.v[0];
                v[1] = tempY * cosAngle + tempX * sinAngle + point.v[1];
            }
        }

        void smooth(const Vector& target, const T elapsedTime, const T responseTime) noexcept
        {
            if (elapsedTime > T(0))
                *this += (target - *this) * (elapsedTime / (elapsedTime + responseTime));
        }

        auto getMin() const noexcept
        {
            T result = v[0];

            for (std::size_t i = 1; i < N; ++i)
                if (v[i] < result)
                    result = v[i];

            return result;
        }

        auto getMax() const noexcept
        {
            T result = v[0];

            for (std::size_t i = 1; i < N; ++i)
                if (v[i] > result)
                    result = v[i];

            return result;
        }

        constexpr auto operator-() const noexcept
        {
            return generateInverse(std::make_index_sequence<N>{});
        }

        constexpr auto operator+(const Vector& vec) const noexcept
        {
            return generateSum(std::make_index_sequence<N>{}, vec);
        }

        auto& operator+=(const Vector& vec) noexcept
        {
            for (std::size_t i = 0; i < N; ++i)
                v[i] += vec.v[i];
            return *this;
        }

        constexpr auto operator-(const Vector& vec) const noexcept
        {
            return generateDiff(std::make_index_sequence<N>{}, vec);
        }

        auto& operator-=(const Vector& vec) noexcept
        {
            for (std::size_t i = 0; i < N; ++i)
                v[i] -= vec.v[i];
            return *this;
        }

        constexpr auto operator*(const T scalar) const noexcept
        {
            return generateMul(std::make_index_sequence<N>{}, scalar);
        }

        auto& operator*=(const T scalar) noexcept
        {
            for (auto& c : v)
                c *= scalar;
            return *this;
        }

        constexpr auto operator/(const T scalar) const noexcept
        {
            return generateDiv(std::make_index_sequence<N>{}, scalar);
        }

        auto& operator/=(const T scalar) noexcept
        {
            for (auto& c : v)
                c /= scalar;
            return *this;
        }

        auto operator<(const Vector& vec) const noexcept
        {
            for (std::size_t i = 0; i < N; ++i)
                if (v[i] < vec.v[i]) return true;
                else if (vec.v[i] < v[i]) return false;

            return false;
        }

        auto operator==(const Vector& vec) const noexcept
        {
            for (std::size_t i = 0; i < N; ++i)
                if (v[i] != vec.v[i]) return false;
            return true;
        }

        auto operator!=(const Vector& vec) const noexcept
        {
            for (std::size_t i = 0; i < N; ++i)
                if (v[i] != vec.v[i]) return true;
            return false;
        }

        auto isZero() const noexcept
        {
            for (const auto& c : v)
                if (c != T(0)) return false;
            return true;
        }

    private:
        template <std::size_t ...I>
        constexpr auto generateInverse(const std::index_sequence<I...>) const
        {
            return Vector{-v[I]...};
        }

        template <std::size_t ...I>
        constexpr auto generateSum(const std::index_sequence<I...>, const Vector& vec) const
        {
            return Vector{(v[I] + vec.v[I])...};
        }

        template <std::size_t ...I>
        constexpr auto generateDiff(const std::index_sequence<I...>, const Vector& vec) const
        {
            return Vector{(v[I] - vec.v[I])...};
        }

        template <std::size_t ...I>
        constexpr auto generateMul(const std::index_sequence<I...>, T scalar) const
        {
            return Vector{(v[I] * scalar)...};
        }

        template <std::size_t ...I>
        constexpr auto generateDiv(const std::index_sequence<I...>, T scalar) const
        {
            return Vector{(v[I] / scalar)...};
        }

        template<typename ...A>
        static constexpr auto sum(A... args) noexcept
        {
            return (args + ...);
        }

        template <std::size_t ...I>
        constexpr auto generateLengthSquared(const std::index_sequence<I...>) const
        {
            return sum((v[I] * v[I])...);
        }

        template <std::size_t ...I>
        constexpr auto generateDot(const std::index_sequence<I...>, const Vector& vec) const
        {
            return sum((v[I] * vec.v[I])...);
        }
    };

    template <typename T, std::size_t N>
    auto operator*(const T scalar, const Vector<T, N>& vec) noexcept
    {
        return vec * scalar;
    }
}

#endif // OUZEL_MATH_VECTOR_HPP
<commit_msg>Remove constexpr from length<commit_after>// Ouzel by Elviss Strazdins

#ifndef OUZEL_MATH_VECTOR_HPP
#define OUZEL_MATH_VECTOR_HPP

#include <array>
#include <cassert>
#include <cmath>
#include <cstddef>
#include <limits>
#include <type_traits>
#include <utility>

namespace ouzel
{
    template <typename T, std::size_t N> class Vector final
    {
    public:
#ifdef __SSE__
        alignas(N == 4 ? 4 * sizeof(T) : alignof(T))
#endif
        std::array<T, N> v{};

        constexpr Vector() noexcept {}

        template <typename ...A>
        explicit constexpr Vector(const A... args) noexcept:
            v{args...}
        {
        }

        template <auto X = N, std::size_t M, std::enable_if_t<(X != M)>* = nullptr>
        explicit Vector(const Vector<T, M>& vec) noexcept
        {
            for (std::size_t i = 0; i < N && i < M; ++i)
                v[i] = vec.v[i];
        }

        auto& operator[](const std::size_t index) noexcept { return v[index]; }
        constexpr auto operator[](const std::size_t index) const noexcept { return v[index]; }

        template <auto X = N, std::enable_if_t<(X >= 1)>* = nullptr>
        auto& x() noexcept { return v[0]; }

        template <auto X = N, std::enable_if_t<(X >= 1)>* = nullptr>
        constexpr auto x() const noexcept { return v[0]; }

        template <auto X = N, std::enable_if_t<(X >= 2)>* = nullptr>
        auto& y() noexcept { return v[1]; }

        template <auto X = N, std::enable_if_t<(X >= 2)>* = nullptr>
        constexpr auto y() const noexcept { return v[1]; }

        template <auto X = N, std::enable_if_t<(X >= 3)>* = nullptr>
        auto& z() noexcept { return v[2]; }

        template <auto X = N, std::enable_if_t<(X >= 3)>* = nullptr>
        constexpr auto z() const noexcept { return v[2]; }

        template <auto X = N, std::enable_if_t<(X >= 4)>* = nullptr>
        auto& w() noexcept { return v[3]; }

        template <auto X = N, std::enable_if_t<(X >= 4)>* = nullptr>
        constexpr auto w() const noexcept { return v[3]; }

        template <auto X = N, std::enable_if_t<(X == 2)>* = nullptr>
        auto getAngle() const noexcept
        {
            return std::atan2(v[1], v[0]);
        }

        template <auto X = N, std::enable_if_t<(X == 3)>* = nullptr>
        auto getAngle(const Vector& axis) const noexcept
        {
            constexpr T dx = axis.v[0] - v[0] - v[1] * axis.v[2] + v[2] * axis.v[1];
            constexpr T dy = axis.v[1] - v[1] - v[2] * axis.v[0] + v[0] * axis.v[2];
            constexpr T dz = axis.v[2] - v[2] - v[0] * axis.v[1] + v[1] * axis.v[0];

            return std::atan2(std::sqrt(dx * dx + dy * dy + dz * dz), dot(axis));
        }

        template <auto X = N, std::enable_if_t<(X == 4)>* = nullptr>
        auto getAngle(const Vector& axis) const noexcept
        {
            constexpr T dx = v[3] * axis.v[0] - v[0] * axis.v[3] - v[1] * axis.v[2] + v[2] * axis.v[1];
            constexpr T dy = v[3] * axis.v[1] - v[1] * axis.v[3] - v[2] * axis.v[0] + v[0] * axis.v[2];
            constexpr T dz = v[3] * axis.v[2] - v[2] * axis.v[3] - v[0] * axis.v[1] + v[1] * axis.v[0];

            return std::atan2(std::sqrt(dx * dx + dy * dy + dz * dz), dot(axis));
        }

        void clamp(const Vector& min, const Vector& max) noexcept
        {
            for (std::size_t i = 0; i < N; ++i)
                if (v[i] < min.v[i]) v[i] = min.v[i];
                else if (v[i] > max.v[i]) v[i] = max.v[i];
        }

        template <auto X = N, std::enable_if_t<(X == 3)>* = nullptr>
        constexpr auto cross(const Vector& vec) const noexcept
        {
            return Vector{
                (v[1] * vec.v[2]) - (v[2] * vec.v[1]),
                (v[2] * vec.v[0]) - (v[0] * vec.v[2]),
                (v[0] * vec.v[1]) - (v[1] * vec.v[0])
            };
        }

        auto distance(const Vector& vec) const noexcept
        {
            T d = T(0);
            for (std::size_t i = 0; i < N; ++i)
                d += (vec.v[i] - v[i]) * (vec.v[i] - v[i]);
            return std::sqrt(d);
        }

        auto distanceSquared(const Vector& vec) const noexcept
        {
            T d = T(0);
            for (std::size_t i = 0; i < N; ++i)
                d += (vec.v[i] - v[i]) * (vec.v[i] - v[i]);
            return d;
        }

        auto length() const noexcept
        {
            return std::sqrt(generateLengthSquared(std::make_index_sequence<N>{}));
        }

        constexpr auto lengthSquared() const noexcept
        {
            return generateLengthSquared(std::make_index_sequence<N>{});
        }

        constexpr auto dot(const Vector& vec) const noexcept
        {
            return generateDot(std::make_index_sequence<N>{}, vec);
        }

        void negate() noexcept
        {
            for (auto& c : v)
                c = -c;
        }

        auto isNormalized(const T tolerance = std::numeric_limits<T>::min()) const noexcept
        {
            return std::abs(T(1) - lengthSquared()) < tolerance;
        }

        void normalize() noexcept
        {
            T squared = T(0);
            for (const auto& c : v)
                squared += c * c;

            if (squared == T(1)) // already normalized
                return;

            const auto length = std::sqrt(squared);
            if (length <= std::numeric_limits<T>::min()) // too close to zero
                return;

            const auto multiplier = T(1) / length;
            for (auto& c : v)
                c *= multiplier;
        }

        auto normalized() const noexcept
        {
            T squared = T(0);
            for (const auto& c : v)
                squared += c * c;

            if (squared == T(1)) // already normalized
                return *this;

            const auto length = std::sqrt(squared);
            if (length <= std::numeric_limits<T>::min()) // too close to zero
                return *this;

            const auto multiplier = T(1) / length;
            return *this * multiplier;
        }

        void scale(const T scalar) noexcept
        {
            for (std::size_t i = 0; i < N; ++i)
                v[i] *= scalar;
        }

        void scale(const Vector& scale) noexcept
        {
            for (std::size_t i = 0; i < N; ++i)
                v[i] *= scale.v[i];
        }

        template <auto X = N, std::enable_if_t<(X == 2)>* = nullptr>
        void rotate(const T angle) noexcept
        {
            const auto sinAngle = std::sin(angle);
            const auto cosAngle = std::cos(angle);

            const auto tempX = v[0] * cosAngle - v[1] * sinAngle;
            v[1] = v[1] * cosAngle + v[0] * sinAngle;
            v[0] = tempX;
        }

        template <auto X = N, std::enable_if_t<(X == 2)>* = nullptr>
        void rotate(const Vector<T, 2>& point, T angle) noexcept
        {
            const float sinAngle = std::sin(angle);
            const float cosAngle = std::cos(angle);

            if (point.isZero())
            {
                const auto tempX = v[0] * cosAngle - v[1] * sinAngle;
                v[1] = v[1] * cosAngle + v[0] * sinAngle;
                v[0] = tempX;
            }
            else
            {
                const auto tempX = v[0] - point.v[0];
                const auto tempY = v[1] - point.v[1];

                v[0] = tempX * cosAngle - tempY * sinAngle + point.v[0];
                v[1] = tempY * cosAngle + tempX * sinAngle + point.v[1];
            }
        }

        void smooth(const Vector& target, const T elapsedTime, const T responseTime) noexcept
        {
            if (elapsedTime > T(0))
                *this += (target - *this) * (elapsedTime / (elapsedTime + responseTime));
        }

        auto getMin() const noexcept
        {
            T result = v[0];

            for (std::size_t i = 1; i < N; ++i)
                if (v[i] < result)
                    result = v[i];

            return result;
        }

        auto getMax() const noexcept
        {
            T result = v[0];

            for (std::size_t i = 1; i < N; ++i)
                if (v[i] > result)
                    result = v[i];

            return result;
        }

        constexpr auto operator-() const noexcept
        {
            return generateInverse(std::make_index_sequence<N>{});
        }

        constexpr auto operator+(const Vector& vec) const noexcept
        {
            return generateSum(std::make_index_sequence<N>{}, vec);
        }

        auto& operator+=(const Vector& vec) noexcept
        {
            for (std::size_t i = 0; i < N; ++i)
                v[i] += vec.v[i];
            return *this;
        }

        constexpr auto operator-(const Vector& vec) const noexcept
        {
            return generateDiff(std::make_index_sequence<N>{}, vec);
        }

        auto& operator-=(const Vector& vec) noexcept
        {
            for (std::size_t i = 0; i < N; ++i)
                v[i] -= vec.v[i];
            return *this;
        }

        constexpr auto operator*(const T scalar) const noexcept
        {
            return generateMul(std::make_index_sequence<N>{}, scalar);
        }

        auto& operator*=(const T scalar) noexcept
        {
            for (auto& c : v)
                c *= scalar;
            return *this;
        }

        constexpr auto operator/(const T scalar) const noexcept
        {
            return generateDiv(std::make_index_sequence<N>{}, scalar);
        }

        auto& operator/=(const T scalar) noexcept
        {
            for (auto& c : v)
                c /= scalar;
            return *this;
        }

        auto operator<(const Vector& vec) const noexcept
        {
            for (std::size_t i = 0; i < N; ++i)
                if (v[i] < vec.v[i]) return true;
                else if (vec.v[i] < v[i]) return false;

            return false;
        }

        auto operator==(const Vector& vec) const noexcept
        {
            for (std::size_t i = 0; i < N; ++i)
                if (v[i] != vec.v[i]) return false;
            return true;
        }

        auto operator!=(const Vector& vec) const noexcept
        {
            for (std::size_t i = 0; i < N; ++i)
                if (v[i] != vec.v[i]) return true;
            return false;
        }

        auto isZero() const noexcept
        {
            for (const auto& c : v)
                if (c != T(0)) return false;
            return true;
        }

    private:
        template <std::size_t ...I>
        constexpr auto generateInverse(const std::index_sequence<I...>) const
        {
            return Vector{-v[I]...};
        }

        template <std::size_t ...I>
        constexpr auto generateSum(const std::index_sequence<I...>, const Vector& vec) const
        {
            return Vector{(v[I] + vec.v[I])...};
        }

        template <std::size_t ...I>
        constexpr auto generateDiff(const std::index_sequence<I...>, const Vector& vec) const
        {
            return Vector{(v[I] - vec.v[I])...};
        }

        template <std::size_t ...I>
        constexpr auto generateMul(const std::index_sequence<I...>, T scalar) const
        {
            return Vector{(v[I] * scalar)...};
        }

        template <std::size_t ...I>
        constexpr auto generateDiv(const std::index_sequence<I...>, T scalar) const
        {
            return Vector{(v[I] / scalar)...};
        }

        template<typename ...A>
        static constexpr auto sum(A... args) noexcept
        {
            return (args + ...);
        }

        template <std::size_t ...I>
        constexpr auto generateLengthSquared(const std::index_sequence<I...>) const
        {
            return sum((v[I] * v[I])...);
        }

        template <std::size_t ...I>
        constexpr auto generateDot(const std::index_sequence<I...>, const Vector& vec) const
        {
            return sum((v[I] * vec.v[I])...);
        }
    };

    template <typename T, std::size_t N>
    auto operator*(const T scalar, const Vector<T, N>& vec) noexcept
    {
        return vec * scalar;
    }
}

#endif // OUZEL_MATH_VECTOR_HPP
<|endoftext|>


<commit_before>#include "servercontroller.hpp"
#include "serversocket.hpp"

#include "qhttpserver.h"
#include "qhttprequest.h"
#include "qhttpresponse.h"

#include <QBuffer>
#include <QTcpSocket>

namespace mirrors {

ServerController::ServerController(QObject *parent)
    : QObject(parent),
      sock(new ServerSocket(this)),
      trackerManager(nullptr),
      detectorTimer(new QTimer(this)),
      serverState(Idle),
      currentLevel(0),
      lastLevelChange(0),
      server(nullptr)
{

    connect(this, SIGNAL(markersUpdated(vector<MarkerUpdate>)),
            this, SLOT(broadcastPositions(vector<MarkerUpdate>)));
    connect(sock, SIGNAL(errorOccurred(QString)),
            this, SIGNAL(socketError(QString)));
    connect(this, SIGNAL(markersUpdated(vector<MarkerUpdate>)),
            sock, SLOT(processUpdates()));
    connect(sock, SIGNAL(levelChanged(int)),
            this, SLOT(changeLevel(int)));
    connect(sock, SIGNAL(mirrorRotated(int, float, QTcpSocket*)),
            this, SLOT(setMirrorRotation(int, float, QTcpSocket*)));
    connect(sock, SIGNAL(clientConnected(QTcpSocket*)),
            this, SLOT(handleNewClient(QTcpSocket*)));
    connect(sock, SIGNAL(clientDisconnected(QTcpSocket*)),
            this, SLOT(clientDisconnected()));
    connect(sock, SIGNAL(arViewUpdated(int,cv::Point3f,cv::Point3f)),
            sock, SLOT(broadcastARViewUpdate(int,cv::Point3f,cv::Point3f)));
    connect(server, SIGNAL(newRequest(QHttpRequest*, QHttpResponse*)),
            this, SLOT(sendBoard(QHttpRequest*, QHttpResponse*)));

    // A single-shot Timer with an interval of 0 will
    // directly fire the timeout when control goes back
    // to the event thread. We use this structure to
    // allow signals events to be processed while
    // keeping the detector active as much as possible.
    detectorTimer->setInterval(0);
    detectorTimer->setSingleShot(true);
}

void ServerController::sendBoard(QHttpRequest* req, QHttpResponse* resp) {
    Q_UNUSED(req);

    resp->setHeader("Content-Type", "image/jpeg");
    resp->setHeader("Content-Length", QString::number(boardImageBytes.size()));
    resp->writeHead(200);
    resp->write(boardImageBytes);

    resp->end();
}

void ServerController::handleNewClient(QTcpSocket* newClient) {
    // If this is the first client, initialize the level change time
    if (lastLevelChange == 0) {
        lastLevelChange = time(NULL);
    }

    auto newClientFilter = [&](QTcpSocket* client) {
        return client->peerPort() == newClient->peerPort();
    };

    sock->broadcastLevelUpdate(currentLevel, 0, trackerManager->scaledBoardSize(), newClientFilter);

    for (auto& pair : mirrorRotations) {
        sock->broadcastRotationUpdate(pair.first, pair.second, newClientFilter);
    }

    // Update list of clients
    emit clientsChanged(sock->connections());
}

void ServerController::clientDisconnected() {
    // Update list of clients
    emit clientsChanged(sock->connections());
}

ServerController::~ServerController() {
    if (trackerManager != nullptr) {
        delete trackerManager;
    }
}

void ServerController::changeState(ServerState state) {
    this->serverState = state;
    emit stateChanged(state);
}

void ServerController::fatalError(const QString &message) {
    stopServer();
    emit fatalErrorOccurred(message);
}

void ServerController::startServer(quint16 port, int cameraDevice, cv::Size camSize, BoardDetectionApproach::Type boardDetectionApproach, bool requireEmptyBoard) {
    Q_ASSERT(serverState == Idle);
    Q_ASSERT(trackerManager == nullptr); // Otherwise we get a memory leak.
    changeState(Starting);

    this->requireEmptyBoard = requireEmptyBoard;

    // (Re)Initialize tracking
    trackerManager = new TrackerManager(cameraDevice, camSize, boardDetectionApproach);
    trackerManager->loadPatterns(MARKER_DIRECTORY, MARKER_COUNT);

    sock->setPortNumber(port);
    sock->start();
    detectorTimer->start();

    // Start detecting board
    connect(detectorTimer, SIGNAL(timeout()),
            this,          SLOT(detectBoard()));

    // Start server that broadcasts board image
    server = new QHttpServer;
    server->listen(port + 1);
}

void ServerController::stopServer() {
    Q_ASSERT(serverState == Started || serverState == Starting);
    changeState(Stopping);
    sock->stop();
    server->deleteLater();

    mirrorRotations.clear();
    currentLevel = 0;
    lastLevelChange = 0;
    emit levelChanged(currentLevel);
}

int ServerController::getLevelTime() {
    // lastLevelChange is 0 when no players have joined yet
    if (lastLevelChange == 0) {
        return 0;
    } else {
        return time(NULL) - lastLevelChange;
    }
}

int ServerController::getCurrentLevel() {
    return currentLevel;
}

void ServerController::changeLevel(int nextLevel) {
    if (nextLevel != currentLevel) {
        cv::Size2f boardSize(trackerManager->scaledBoardSize());
        int levelTime = time(NULL) - lastLevelChange;

        sock->broadcastLevelUpdate(nextLevel, levelTime, boardSize);

        currentLevel = nextLevel;
        lastLevelChange = time(NULL);

        emit levelChanged(nextLevel);
    }
}

void ServerController::setMirrorRotation(int id, float rotation, QTcpSocket* source) {
    mirrorRotations[id] = rotation;

    sock->broadcastRotationUpdate(id, rotation, [&](QTcpSocket* client) {
        return client->peerPort() != source->peerPort();
    });
}

void ServerController::detectBoard() {
    Q_ASSERT(trackerManager != nullptr);
    if (state() == Stopping) {
        delete trackerManager;
        trackerManager = nullptr;
        changeState(Idle);

        disconnect(detectorTimer, SIGNAL(timeout()),
                   this,          SLOT(detectBoard()));

        return;
    }
    Q_ASSERT(serverState == Starting);

    QPixmap result;
    bool boardLocated = trackerManager->locateBoard(result, true);

    // Show image to user
    emit imageReady(result);

    if (boardLocated) {
        // Save an image of the board without text overlay
        QPixmap board;
        auto updates = trackerManager->getMarkerUpdates(board, false);

        // If there are markers on the board, abort
        if (updates.size() == 0 || !requireEmptyBoard) {
            boardImageBytes.clear();
            QBuffer buffer(&boardImageBytes);
            buffer.open(QIODevice::WriteOnly);
            board.save(&buffer, "JPG");

            // When the board is found, we stop trying to locate
            // the board and start detecting markers instead.
            disconnect(detectorTimer, SIGNAL(timeout()),
                       this,          SLOT(detectBoard()));
            connect(detectorTimer,    SIGNAL(timeout()),
                    this,             SLOT(detectFrame()));
            changeState(Started);
        }
    }
    detectorTimer->start();
}

void ServerController::setDebugOverlay(bool enable) {
    showDebugOverlay = enable;
}

void ServerController::detectFrame() {
    // detectFrame should never be called when the server is not running.
    Q_ASSERT(serverState != Idle);
    Q_ASSERT(trackerManager != nullptr);
    if (state() == Starting) {
        changeState(Started);
    }

    if (state() == Started) {
        QPixmap result;
        vector<MarkerUpdate> markers = trackerManager->getMarkerUpdates(result, showDebugOverlay);

        emit markersUpdated(markers);
        emit imageReady(result);

        detectorTimer->start();

        emit fpsChanged(trackerManager->getUpdateRate());
    } else {
        changeState(Idle);
        delete trackerManager;
        trackerManager = nullptr;

        emit fpsChanged(-1);

        disconnect(detectorTimer,    SIGNAL(timeout()),
                this,             SLOT(detectFrame()));
    }
}

void ServerController::broadcastPositions(vector<MarkerUpdate> markers) {
    for(MarkerUpdate marker : markers) {
        broadcastPosition(marker);
    }
}

void ServerController::broadcastPosition(const MarkerUpdate& marker) {
    if (marker.type == MarkerUpdateType::REMOVE) {
        sock->broadcastDelete(marker.id);
    } else {
        // Scale marker positions based on their size for Meta 1 tracking
        sock->broadcastPositionUpdate(
                    marker.id,
                    trackerManager->scaledMarkerCoordinate(marker.position),
                    marker.rotation);
    }
}

} // namespace mirrors

<commit_msg>Fix server again<commit_after>#include "servercontroller.hpp"
#include "serversocket.hpp"

#include "qhttpserver.h"
#include "qhttprequest.h"
#include "qhttpresponse.h"

#include <QBuffer>
#include <QTcpSocket>

namespace mirrors {

ServerController::ServerController(QObject *parent)
    : QObject(parent),
      sock(new ServerSocket(this)),
      trackerManager(nullptr),
      detectorTimer(new QTimer(this)),
      serverState(Idle),
      currentLevel(0),
      lastLevelChange(0),
      server(nullptr)
{

    connect(this, SIGNAL(markersUpdated(vector<MarkerUpdate>)),
            this, SLOT(broadcastPositions(vector<MarkerUpdate>)));
    connect(sock, SIGNAL(errorOccurred(QString)),
            this, SIGNAL(socketError(QString)));
    connect(this, SIGNAL(markersUpdated(vector<MarkerUpdate>)),
            sock, SLOT(processUpdates()));
    connect(sock, SIGNAL(levelChanged(int)),
            this, SLOT(changeLevel(int)));
    connect(sock, SIGNAL(mirrorRotated(int, float, QTcpSocket*)),
            this, SLOT(setMirrorRotation(int, float, QTcpSocket*)));
    connect(sock, SIGNAL(clientConnected(QTcpSocket*)),
            this, SLOT(handleNewClient(QTcpSocket*)));
    connect(sock, SIGNAL(clientDisconnected(QTcpSocket*)),
            this, SLOT(clientDisconnected()));
    connect(sock, SIGNAL(arViewUpdated(int,cv::Point3f,cv::Point3f)),
            sock, SLOT(broadcastARViewUpdate(int,cv::Point3f,cv::Point3f)));
    connect(server, SIGNAL(newRequest(QHttpRequest*, QHttpResponse*)),
            this, SLOT(sendBoard(QHttpRequest*, QHttpResponse*)));

    // A single-shot Timer with an interval of 0 will
    // directly fire the timeout when control goes back
    // to the event thread. We use this structure to
    // allow signals events to be processed while
    // keeping the detector active as much as possible.
    detectorTimer->setInterval(0);
    detectorTimer->setSingleShot(true);
}

void ServerController::sendBoard(QHttpRequest* req, QHttpResponse* resp) {
    Q_UNUSED(req);

    resp->setHeader("Content-Type", "image/jpeg");
    resp->setHeader("Content-Length", QString::number(boardImageBytes.size()));
    resp->writeHead(200);
    resp->write(boardImageBytes);

    resp->end();
}

void ServerController::handleNewClient(QTcpSocket* newClient) {
    // If this is the first client, initialize the level change time
    if (lastLevelChange == 0) {
        lastLevelChange = time(NULL);
    }

    auto newClientFilter = [&](QTcpSocket* client) {
        return client->peerPort() == newClient->peerPort();
    };

    sock->broadcastLevelUpdate(currentLevel, 0, trackerManager->scaledBoardSize(), newClientFilter);

    for (auto& pair : mirrorRotations) {
        sock->broadcastRotationUpdate(pair.first, pair.second, newClientFilter);
    }

    // Update list of clients
    emit clientsChanged(sock->connections());
}

void ServerController::clientDisconnected() {
    // Update list of clients
    emit clientsChanged(sock->connections());
}

ServerController::~ServerController() {
    if (trackerManager != nullptr) {
        delete trackerManager;
    }
}

void ServerController::changeState(ServerState state) {
    this->serverState = state;
    emit stateChanged(state);
}

void ServerController::fatalError(const QString &message) {
    stopServer();
    emit fatalErrorOccurred(message);
}

void ServerController::startServer(quint16 port, int cameraDevice, cv::Size camSize, BoardDetectionApproach::Type boardDetectionApproach, bool requireEmptyBoard) {
    Q_ASSERT(serverState == Idle);
    Q_ASSERT(trackerManager == nullptr); // Otherwise we get a memory leak.
    changeState(Starting);

    this->requireEmptyBoard = requireEmptyBoard;

    // (Re)Initialize tracking
    trackerManager = new TrackerManager(cameraDevice, camSize, boardDetectionApproach);
    trackerManager->loadPatterns(MARKER_DIRECTORY, MARKER_COUNT);

    sock->setPortNumber(port);
    sock->start();
    detectorTimer->start();

    // Start detecting board
    connect(detectorTimer, SIGNAL(timeout()),
            this,          SLOT(detectBoard()));

    // Start server that broadcasts board image
    if (server == nullptr) {
        server = new QHttpServer;
        server->listen(port + 1);
    }
}

void ServerController::stopServer() {
    Q_ASSERT(serverState == Started || serverState == Starting);
    changeState(Stopping);
    sock->stop();

    mirrorRotations.clear();
    currentLevel = 0;
    lastLevelChange = 0;
    emit levelChanged(currentLevel);
}

int ServerController::getLevelTime() {
    // lastLevelChange is 0 when no players have joined yet
    if (lastLevelChange == 0) {
        return 0;
    } else {
        return time(NULL) - lastLevelChange;
    }
}

int ServerController::getCurrentLevel() {
    return currentLevel;
}

void ServerController::changeLevel(int nextLevel) {
    if (nextLevel != currentLevel) {
        cv::Size2f boardSize(trackerManager->scaledBoardSize());
        int levelTime = time(NULL) - lastLevelChange;

        sock->broadcastLevelUpdate(nextLevel, levelTime, boardSize);

        currentLevel = nextLevel;
        lastLevelChange = time(NULL);

        emit levelChanged(nextLevel);
    }
}

void ServerController::setMirrorRotation(int id, float rotation, QTcpSocket* source) {
    mirrorRotations[id] = rotation;

    sock->broadcastRotationUpdate(id, rotation, [&](QTcpSocket* client) {
        return client->peerPort() != source->peerPort();
    });
}

void ServerController::detectBoard() {
    Q_ASSERT(trackerManager != nullptr);
    if (state() == Stopping) {
        delete trackerManager;
        trackerManager = nullptr;
        changeState(Idle);

        disconnect(detectorTimer, SIGNAL(timeout()),
                   this,          SLOT(detectBoard()));

        return;
    }
    Q_ASSERT(serverState == Starting);

    QPixmap result;
    bool boardLocated = trackerManager->locateBoard(result, true);

    // Show image to user
    emit imageReady(result);

    if (boardLocated) {
        // Save an image of the board without text overlay
        QPixmap board;
        auto updates = trackerManager->getMarkerUpdates(board, false);

        // If there are markers on the board, abort
        if (updates.size() == 0 || !requireEmptyBoard) {
            boardImageBytes.clear();
            QBuffer buffer(&boardImageBytes);
            buffer.open(QIODevice::WriteOnly);
            board.save(&buffer, "JPG");

            // When the board is found, we stop trying to locate
            // the board and start detecting markers instead.
            disconnect(detectorTimer, SIGNAL(timeout()),
                       this,          SLOT(detectBoard()));
            connect(detectorTimer,    SIGNAL(timeout()),
                    this,             SLOT(detectFrame()));
            changeState(Started);
        }
    }
    detectorTimer->start();
}

void ServerController::setDebugOverlay(bool enable) {
    showDebugOverlay = enable;
}

void ServerController::detectFrame() {
    // detectFrame should never be called when the server is not running.
    Q_ASSERT(serverState != Idle);
    Q_ASSERT(trackerManager != nullptr);
    if (state() == Starting) {
        changeState(Started);
    }

    if (state() == Started) {
        QPixmap result;
        vector<MarkerUpdate> markers = trackerManager->getMarkerUpdates(result, showDebugOverlay);

        emit markersUpdated(markers);
        emit imageReady(result);

        detectorTimer->start();

        emit fpsChanged(trackerManager->getUpdateRate());
    } else {
        changeState(Idle);
        delete trackerManager;
        trackerManager = nullptr;

        emit fpsChanged(-1);

        disconnect(detectorTimer,    SIGNAL(timeout()),
                this,             SLOT(detectFrame()));
    }
}

void ServerController::broadcastPositions(vector<MarkerUpdate> markers) {
    for(MarkerUpdate marker : markers) {
        broadcastPosition(marker);
    }
}

void ServerController::broadcastPosition(const MarkerUpdate& marker) {
    if (marker.type == MarkerUpdateType::REMOVE) {
        sock->broadcastDelete(marker.id);
    } else {
        // Scale marker positions based on their size for Meta 1 tracking
        sock->broadcastPositionUpdate(
                    marker.id,
                    trackerManager->scaledMarkerCoordinate(marker.position),
                    marker.rotation);
    }
}

} // namespace mirrors

<|endoftext|>


<commit_before>#include "Variant.h"
#include "convert.h"
#include <set>
#include <sstream>
#include <getopt.h>

using namespace std;
using namespace vcf;

double convertStrDbl(const string& s) {
    double r;
    convert(s, r);
    return r;
}

void printSummary(char** argv) {
    cerr << "usage: " << argv[0] << " [options] [file]" << endl
         << endl
         << "If overlapping alleles are represented across multiple records, merge" << endl
         << "them into a single record.  Currently only for indels." << endl;
    exit(0);
}

Variant createMultiallelic(vector<Variant>& vars) {

    if (vars.size() == 1) {
        return vars.front();
    }

    int maxpos = vars.front().position + vars.front().ref.size();
    for (vector<Variant>::iterator v = vars.begin(); v != vars.end(); ++v) {
        //cerr << *v << endl;
        if (maxpos < v->position + v->ref.size()) {
            maxpos = v->position + v->ref.size();
        }
    }

    int numalt = vars.size();
    //cerr << "gots overlapping vars " << vars.front().position << "-" << vars.back().position << endl;

    // get REF
    // use start position to extend all other alleles
    int start = vars.front().position;
    string ref = vars.front().ref;

    for (vector<Variant>::iterator v = vars.begin() + 1; v != vars.end(); ++v) {
        int sdiff = (v->position + v->ref.size()) - (start + ref.size());
        int pdiff = (start + ref.size()) - v->position;
        if (sdiff > 0) {
            ref.append(v->ref.substr(pdiff, sdiff));
        }
    }

    //cerr << "ref would be " << ref << " for vars from "
    //     << vars.front().position << " to " << vars.back().position << endl;

    Variant var = vars.front();
    var.alt.clear();
    var.ref = ref;

    for (vector<Variant>::iterator v = vars.begin(); v != vars.end(); ++v) {
        // add alternates and splice them into the reference
        int p5diff = v->position - var.position;
        int p3diff = (var.position + var.ref.size()) - (v->position + v->ref.size());
        string before;
        string after;
        if (p5diff > 0) {
            before = var.ref.substr(0, p5diff);
        }
        if (p3diff > 0 && p3diff < var.ref.size()) {
            after = var.ref.substr(var.ref.size() - p3diff);
        }
        if (p5diff || p3diff) {
            for (vector<string>::iterator a = v->alt.begin(); a != v->alt.end(); ++a) {
                var.alt.push_back(before);
                string& alt = var.alt.back();
                alt.append(*a);
                alt.append(after);
            }
        } else {
            for (vector<string>::iterator a = v->alt.begin(); a != v->alt.end(); ++a) {
                var.alt.push_back(*a);
            }
        }
    }

    stringstream s;
    s << vars.front().position << "-" << vars.back().position;
    var.info["combined"].push_back(s.str());

    return var;

}

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

    VariantCallFile variantFile;

    int c;
    while (true) {
        static struct option long_options[] =
        {
            /* These options set a flag. */
            //{"verbose", no_argument,       &verbose_flag, 1},
            {"help", no_argument, 0, 'h'},
            {0, 0, 0, 0}
        };
        /* getopt_long stores the option index here. */
        int option_index = 0;

        c = getopt_long (argc, argv, "h",
                         long_options, &option_index);

        if (c == -1)
            break;

        switch (c) {

            case 'h':
                printSummary(argv);
                break;

            case '?':
                printSummary(argv);
                exit(1);
                break;

            default:
                abort ();
        }
    }

    if (optind < argc) {
        string filename = argv[optind];
        variantFile.open(filename);
    } else {
        variantFile.open(std::cin);
    }

    if (!variantFile.is_open()) {
        return 1;
    }

    variantFile.addHeaderLine("##INFO=<ID=combined,Number=1,Type=String,Description=\"Range of overlapping variants which were combined into this one using vcfcreatemulti.\">");

    cout << variantFile.header << endl;

    bool first = true;
    bool already = false;
    Variant var(variantFile);
    vector<Variant> vars;
    string lastSeq;

    while (variantFile.getNextVariant(var)) {

        if (lastSeq.empty()) {
            lastSeq = var.sequenceName;
        }

        if (vars.empty()) {
            vars.push_back(var);
            continue;
        } else {
            int maxpos = vars.front().position + vars.front().ref.size();
            for (vector<Variant>::iterator v = vars.begin(); v != vars.end(); ++v) {
                if (maxpos < v->position + v->ref.size()) {
                    maxpos = v->position + v->ref.size();
                }
            }
            if (var.sequenceName != lastSeq) {
                Variant result = createMultiallelic(vars);
                cout << result << endl;
                vars.clear();
                lastSeq = var.sequenceName;
                vars.push_back(var);
            } else if (var.position < maxpos) {
                vars.push_back(var);
            } else {
                Variant result = createMultiallelic(vars);
                cout << result << endl;
                vars.clear();
                vars.push_back(var);
            }
        }

    }

    Variant result = createMultiallelic(vars);
    cout << result << endl;

    return 0;

}

<commit_msg>vcfcreatemulti: Avoid segfault when passed VCF file with no variants<commit_after>#include "Variant.h"
#include "convert.h"
#include <set>
#include <sstream>
#include <getopt.h>

using namespace std;
using namespace vcf;

double convertStrDbl(const string& s) {
    double r;
    convert(s, r);
    return r;
}

void printSummary(char** argv) {
    cerr << "usage: " << argv[0] << " [options] [file]" << endl
         << endl
         << "If overlapping alleles are represented across multiple records, merge" << endl
         << "them into a single record.  Currently only for indels." << endl;
    exit(0);
}

Variant createMultiallelic(vector<Variant>& vars) {

    if (vars.size() == 1) {
        return vars.front();
    }

    int maxpos = vars.front().position + vars.front().ref.size();
    for (vector<Variant>::iterator v = vars.begin(); v != vars.end(); ++v) {
        //cerr << *v << endl;
        if (maxpos < v->position + v->ref.size()) {
            maxpos = v->position + v->ref.size();
        }
    }

    int numalt = vars.size();
    //cerr << "gots overlapping vars " << vars.front().position << "-" << vars.back().position << endl;

    // get REF
    // use start position to extend all other alleles
    int start = vars.front().position;
    string ref = vars.front().ref;

    for (vector<Variant>::iterator v = vars.begin() + 1; v != vars.end(); ++v) {
        int sdiff = (v->position + v->ref.size()) - (start + ref.size());
        int pdiff = (start + ref.size()) - v->position;
        if (sdiff > 0) {
            ref.append(v->ref.substr(pdiff, sdiff));
        }
    }

    //cerr << "ref would be " << ref << " for vars from "
    //     << vars.front().position << " to " << vars.back().position << endl;

    Variant var = vars.front();
    var.alt.clear();
    var.ref = ref;

    for (vector<Variant>::iterator v = vars.begin(); v != vars.end(); ++v) {
        // add alternates and splice them into the reference
        int p5diff = v->position - var.position;
        int p3diff = (var.position + var.ref.size()) - (v->position + v->ref.size());
        string before;
        string after;
        if (p5diff > 0) {
            before = var.ref.substr(0, p5diff);
        }
        if (p3diff > 0 && p3diff < var.ref.size()) {
            after = var.ref.substr(var.ref.size() - p3diff);
        }
        if (p5diff || p3diff) {
            for (vector<string>::iterator a = v->alt.begin(); a != v->alt.end(); ++a) {
                var.alt.push_back(before);
                string& alt = var.alt.back();
                alt.append(*a);
                alt.append(after);
            }
        } else {
            for (vector<string>::iterator a = v->alt.begin(); a != v->alt.end(); ++a) {
                var.alt.push_back(*a);
            }
        }
    }

    stringstream s;
    s << vars.front().position << "-" << vars.back().position;
    var.info["combined"].push_back(s.str());

    return var;

}

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

    VariantCallFile variantFile;

    int c;
    while (true) {
        static struct option long_options[] =
        {
            /* These options set a flag. */
            //{"verbose", no_argument,       &verbose_flag, 1},
            {"help", no_argument, 0, 'h'},
            {0, 0, 0, 0}
        };
        /* getopt_long stores the option index here. */
        int option_index = 0;

        c = getopt_long (argc, argv, "h",
                         long_options, &option_index);

        if (c == -1)
            break;

        switch (c) {

            case 'h':
                printSummary(argv);
                break;

            case '?':
                printSummary(argv);
                exit(1);
                break;

            default:
                abort ();
        }
    }

    if (optind < argc) {
        string filename = argv[optind];
        variantFile.open(filename);
    } else {
        variantFile.open(std::cin);
    }

    if (!variantFile.is_open()) {
        return 1;
    }

    variantFile.addHeaderLine("##INFO=<ID=combined,Number=1,Type=String,Description=\"Range of overlapping variants which were combined into this one using vcfcreatemulti.\">");

    cout << variantFile.header << endl;

    bool first = true;
    bool already = false;
    Variant var(variantFile);
    vector<Variant> vars;
    string lastSeq;

    while (variantFile.getNextVariant(var)) {

        if (lastSeq.empty()) {
            lastSeq = var.sequenceName;
        }

        if (vars.empty()) {
            vars.push_back(var);
            continue;
        } else {
            int maxpos = vars.front().position + vars.front().ref.size();
            for (vector<Variant>::iterator v = vars.begin(); v != vars.end(); ++v) {
                if (maxpos < v->position + v->ref.size()) {
                    maxpos = v->position + v->ref.size();
                }
            }
            if (var.sequenceName != lastSeq) {
                Variant result = createMultiallelic(vars);
                cout << result << endl;
                vars.clear();
                lastSeq = var.sequenceName;
                vars.push_back(var);
            } else if (var.position < maxpos) {
                vars.push_back(var);
            } else {
                Variant result = createMultiallelic(vars);
                cout << result << endl;
                vars.clear();
                vars.push_back(var);
            }
        }

    }

    if (!vars.empty()) {
        Variant result = createMultiallelic(vars);
        cout << result << endl;
    }

    return 0;

}

<|endoftext|>


<commit_before>// -*- C++ -*-
//===-------------------- support/win32/locale_win32.cpp ------------------===//
//
// Part of the LLVM Project, under the Apache License v2.0 with LLVM Exceptions.
// See https://llvm.org/LICENSE.txt for license information.
// SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception
//
//===----------------------------------------------------------------------===//

#include <locale>
#include <cstdarg> // va_start, va_end
#include <memory>
#include <type_traits>

int __libcpp_vasprintf(char **sptr, const char *__restrict fmt, va_list ap);

using std::__libcpp_locale_guard;

// FIXME: base currently unused. Needs manual work to construct the new locale
locale_t newlocale( int mask, const char * locale, locale_t /*base*/ )
{
    return {_create_locale( LC_ALL, locale ), locale};
}

decltype(MB_CUR_MAX) MB_CUR_MAX_L( locale_t __l )
{
#if defined(_LIBCPP_MSVCRT)
  return ___mb_cur_max_l_func(__l);
#else
  __libcpp_locale_guard __current(__l);
  return MB_CUR_MAX;
#endif
}

lconv *localeconv_l( locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return localeconv();
}
size_t mbrlen_l( const char *__restrict s, size_t n,
                 mbstate_t *__restrict ps, locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return mbrlen( s, n, ps );
}
size_t mbsrtowcs_l( wchar_t *__restrict dst, const char **__restrict src,
                    size_t len, mbstate_t *__restrict ps, locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return mbsrtowcs( dst, src, len, ps );
}
size_t wcrtomb_l( char *__restrict s, wchar_t wc, mbstate_t *__restrict ps,
                  locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return wcrtomb( s, wc, ps );
}
size_t mbrtowc_l( wchar_t *__restrict pwc, const char *__restrict s,
                  size_t n, mbstate_t *__restrict ps, locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return mbrtowc( pwc, s, n, ps );
}
size_t mbsnrtowcs_l( wchar_t *__restrict dst, const char **__restrict src,
                     size_t nms, size_t len, mbstate_t *__restrict ps, locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return mbsnrtowcs( dst, src, nms, len, ps );
}
size_t wcsnrtombs_l( char *__restrict dst, const wchar_t **__restrict src,
                     size_t nwc, size_t len, mbstate_t *__restrict ps, locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return wcsnrtombs( dst, src, nwc, len, ps );
}
wint_t btowc_l( int c, locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return btowc( c );
}
int wctob_l( wint_t c, locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return wctob( c );
}

int snprintf_l(char *ret, size_t n, locale_t loc, const char *format, ...)
{
    __libcpp_locale_guard __current(loc);
    va_list ap;
    va_start( ap, format );
    int result = vsnprintf( ret, n, format, ap );
    va_end(ap);
    return result;
}

int asprintf_l( char **ret, locale_t loc, const char *format, ... )
{
    va_list ap;
    va_start( ap, format );
    int result = vasprintf_l( ret, loc, format, ap );
    va_end(ap);
    return result;
}
int vasprintf_l( char **ret, locale_t loc, const char *format, va_list ap )
{
    __libcpp_locale_guard __current(loc);
    return __libcpp_vasprintf( ret, format, ap );
}

#if !defined(_LIBCPP_MSVCRT)
float strtof_l(const char* nptr, char** endptr, locale_t loc) {
  __libcpp_locale_guard __current(loc);
  return strtof(nptr, endptr);
}

long double strtold_l(const char* nptr, char** endptr, locale_t loc) {
  __libcpp_locale_guard __current(loc);
  return strtold(nptr, endptr);
}
#endif
<commit_msg>[libc++] Speed up certain locale functions on Windows<commit_after>// -*- C++ -*-
//===-------------------- support/win32/locale_win32.cpp ------------------===//
//
// Part of the LLVM Project, under the Apache License v2.0 with LLVM Exceptions.
// See https://llvm.org/LICENSE.txt for license information.
// SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception
//
//===----------------------------------------------------------------------===//

#include <locale>
#include <cstdarg> // va_start, va_end
#include <memory>
#include <type_traits>

int __libcpp_vasprintf(char **sptr, const char *__restrict fmt, va_list ap);

using std::__libcpp_locale_guard;

// FIXME: base currently unused. Needs manual work to construct the new locale
locale_t newlocale( int mask, const char * locale, locale_t /*base*/ )
{
    return {_create_locale( LC_ALL, locale ), locale};
}

decltype(MB_CUR_MAX) MB_CUR_MAX_L( locale_t __l )
{
#if defined(_LIBCPP_MSVCRT)
  return ___mb_cur_max_l_func(__l);
#else
  __libcpp_locale_guard __current(__l);
  return MB_CUR_MAX;
#endif
}

lconv *localeconv_l( locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return localeconv();
}
size_t mbrlen_l( const char *__restrict s, size_t n,
                 mbstate_t *__restrict ps, locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return mbrlen( s, n, ps );
}
size_t mbsrtowcs_l( wchar_t *__restrict dst, const char **__restrict src,
                    size_t len, mbstate_t *__restrict ps, locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return mbsrtowcs( dst, src, len, ps );
}
size_t wcrtomb_l( char *__restrict s, wchar_t wc, mbstate_t *__restrict ps,
                  locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return wcrtomb( s, wc, ps );
}
size_t mbrtowc_l( wchar_t *__restrict pwc, const char *__restrict s,
                  size_t n, mbstate_t *__restrict ps, locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return mbrtowc( pwc, s, n, ps );
}
size_t mbsnrtowcs_l( wchar_t *__restrict dst, const char **__restrict src,
                     size_t nms, size_t len, mbstate_t *__restrict ps, locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return mbsnrtowcs( dst, src, nms, len, ps );
}
size_t wcsnrtombs_l( char *__restrict dst, const wchar_t **__restrict src,
                     size_t nwc, size_t len, mbstate_t *__restrict ps, locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return wcsnrtombs( dst, src, nwc, len, ps );
}
wint_t btowc_l( int c, locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return btowc( c );
}
int wctob_l( wint_t c, locale_t loc )
{
    __libcpp_locale_guard __current(loc);
    return wctob( c );
}

int snprintf_l(char *ret, size_t n, locale_t loc, const char *format, ...)
{
#if !defined(_LIBCPP_MSVCRT)
    __libcpp_locale_guard __current(loc);
#endif
    va_list ap;
    va_start( ap, format );
#if defined(_LIBCPP_MSVCRT)
    int result = _vsnprintf_l( ret, n, format, loc, ap );
#else
    int result = vsnprintf( ret, n, format, ap );
#endif
    va_end(ap);
    return result;
}

int asprintf_l( char **ret, locale_t loc, const char *format, ... )
{
    va_list ap;
    va_start( ap, format );
    int result = vasprintf_l( ret, loc, format, ap );
    va_end(ap);
    return result;
}
int vasprintf_l( char **ret, locale_t loc, const char *format, va_list ap )
{
    __libcpp_locale_guard __current(loc);
    return __libcpp_vasprintf( ret, format, ap );
}

#if !defined(_LIBCPP_MSVCRT)
float strtof_l(const char* nptr, char** endptr, locale_t loc) {
  __libcpp_locale_guard __current(loc);
  return strtof(nptr, endptr);
}

long double strtold_l(const char* nptr, char** endptr, locale_t loc) {
  __libcpp_locale_guard __current(loc);
  return strtold(nptr, endptr);
}
#endif
<|endoftext|>


<commit_before>// This file is a part of the IncludeOS unikernel - www.includeos.org
//
// Copyright 2015 Oslo and Akershus University College of Applied Sciences
// and Alfred Bratterud
//
// 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.

#include <fcntl.h>
#include <unistd.h>
#include <fd_map.hpp>
#include <kernel/os.hpp>

int open(const char*, int, ...)
{
  return -1;
}

int close(int fildes)
{
  try {
    auto& fd = FD_map::_get(fildes);
    // if close success
    if(fd.close() == 0)
      return FD_map::_close(fildes);
  }
  catch(const FD_not_found&) {
    errno = EBADF;
  }
  return -1;
}

int read(int, void*, size_t)
{
  return 0;
}
int write(int file, const void* ptr, size_t len)
{
  if (file < 4) {
    return OS::print((const char*) ptr, len);
  }
  try {
    auto& fd = FD_map::_get(file);
    return fd.write(ptr, len);
  }
  catch(const FD_not_found&) {
    errno = EBADF;
    return -1;
  }
}

// read value of a symbolic link (which we don't have)
ssize_t readlink(const char* path, char*, size_t bufsiz)
{
  printf("readlink(%s, bufsize=%u)\n", path, bufsiz);
  return 0;
}

int fsync(int fildes)
{
  try {
    (void) fildes;
    //auto& fd = FD_map::_get(fildes);
    // files should return 0, and others should not
    return 0;
  }
  catch(const FD_not_found&) {
    errno = EBADF;
    return -1;
  }
}

int fchown(int, uid_t, gid_t)
{
  return -1;
}

#include <kernel/irq_manager.hpp>
#include <kernel/rtc.hpp>
unsigned int sleep(unsigned int seconds)
{
  int64_t now  = RTC::now();
  int64_t done = now + seconds;
  while (true) {
    if (now >= done) break;
    OS::block();
    now = RTC::now();
  }
  return 0;
}
<commit_msg>posix: close() now longer calls close on fd (dtor should clean up)<commit_after>// This file is a part of the IncludeOS unikernel - www.includeos.org
//
// Copyright 2015 Oslo and Akershus University College of Applied Sciences
// and Alfred Bratterud
//
// 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.

#include <fcntl.h>
#include <unistd.h>
#include <fd_map.hpp>
#include <kernel/os.hpp>

int open(const char*, int, ...)
{
  return -1;
}

int close(int fildes)
{
  try
  {
    return FD_map::_close(fildes);
  }
  catch(const FD_not_found&)
  {
    errno = EBADF;
  }
  return -1;
}

int read(int, void*, size_t)
{
  return 0;
}
int write(int file, const void* ptr, size_t len)
{
  if (file < 4) {
    return OS::print((const char*) ptr, len);
  }
  try {
    auto& fd = FD_map::_get(file);
    return fd.write(ptr, len);
  }
  catch(const FD_not_found&) {
    errno = EBADF;
    return -1;
  }
}

// read value of a symbolic link (which we don't have)
ssize_t readlink(const char* path, char*, size_t bufsiz)
{
  printf("readlink(%s, bufsize=%u)\n", path, bufsiz);
  return 0;
}

int fsync(int fildes)
{
  try {
    (void) fildes;
    //auto& fd = FD_map::_get(fildes);
    // files should return 0, and others should not
    return 0;
  }
  catch(const FD_not_found&) {
    errno = EBADF;
    return -1;
  }
}

int fchown(int, uid_t, gid_t)
{
  return -1;
}

#include <kernel/irq_manager.hpp>
#include <kernel/rtc.hpp>
unsigned int sleep(unsigned int seconds)
{
  int64_t now  = RTC::now();
  int64_t done = now + seconds;
  while (true) {
    if (now >= done) break;
    OS::block();
    now = RTC::now();
  }
  return 0;
}
<|endoftext|>


<commit_before>#include <stan/mcmc/mcmc_output.hpp>
#include <gtest/gtest.h>

class stanMcmc : public ::testing::Test {
  // R code:
  // R> set.seed(0)
  // R> y <- rnorm(100)
protected:
  virtual void SetUp() {
    std::vector<double> y1(&r_[0], &r_[50]);
    std::vector<double> y2(&r_[50], &r_[100]);
    mcmc_output.add_chain(y1);
    mcmc_output.add_chain(y2);
  }

  stan::mcmc::mcmc_output mcmc_output;
  static const double r_[100];
};

const double stanMcmc::r_[100] =
    {1.26295428488079,-0.326233360705649,1.3297992629225,1.2724293214294,0.414641434456408,
     -1.53995004190371,-0.928567034713538,-0.29472044679056,-0.00576717274753696,2.40465338885795,
     0.76359346114046,-0.799009248989368,-1.14765700923635,-0.289461573688223,-0.299215117897316,
     -0.411510832795067,0.252223448156132,-0.891921127284569,0.435683299355719,-1.23753842192996,
     -0.224267885278309,0.377395645981701,0.133336360814841,0.804189509744908,-0.0571067743838088,
     0.503607972233726,1.08576936214569,-0.69095383969683,-1.28459935387219,0.046726172188352,
     -0.235706556439501,-0.542888255010254,-0.433310317456782,-0.649471646796233,0.726750747385451,
     1.1519117540872,0.992160365445798,-0.429513109491881,1.23830410085338,-0.279346281854269,
     1.75790308981071,0.560746090888056,-0.452783972553158,-0.832043296117832,-1.16657054708471,
     -1.0655905803883,-1.563782051071,1.15653699715018,0.83204712857239,-0.227328691424755,
     0.266137361672105,-0.376702718583628,2.44136462889459,-0.795339117255372,-0.0548774737115786,
     0.250141322854153,0.618243293566247,-0.172623502645857,-2.22390027400994,-1.26361438497058,
     0.358728895971352,-0.0110454784656636,-0.940649162618608,-0.115825322156954,-0.814968708869917,
     0.242263480859686,-1.4250983947325,0.36594112304922,0.248412648872596,0.0652881816716207,
     0.0191563916602738,0.257338377155533,-0.649010077708898,-0.119168762418038,0.66413569989411,
     1.10096910219409,0.14377148075807,-0.117753598165951,-0.912068366948338,-1.43758624082998,
     -0.797089525071965,1.25408310644997,0.77214218580453,-0.21951562675344,-0.424810283377287,
     -0.418980099421959,0.996986860909106,-0.275778029088027,1.2560188173061,0.646674390495345,
     1.29931230256343,-0.873262111744435,0.00837095999603331,-0.880871723252545,0.59625901661066,
     0.119717641289537,-0.282173877322451,1.45598840106634,0.229019590694692,0.996543928544126};

TEST_F(stanMcmc, effectiveSize)  {
  double ess = mcmc_output.effectiveSize();

  EXPECT_FLOAT_EQ(78.42269, ess);
}

TEST_F(stanMcmc, splitRHat) {
  double splitRHat = mcmc_output.splitRHat();

  EXPECT_FLOAT_EQ(0.9892759, splitRHat);
}

TEST_F(stanMcmc, factory) {
  stan::mcmc::mcmc_output_factory f;
  f.addFile("src/test/mcmc/mcmc_output/blocker1.csv");
  f.addFile("src/test/mcmc/mcmc_output/blocker2.csv");
  stan::mcmc::mcmc_output mu1 = f.create("mu.1");
}

TEST_F(stanMcmc, factoryAvailableVariables) {
  stan::mcmc::mcmc_output_factory f;
  f.addFile("src/test/mcmc/mcmc_output/blocker1.csv");
  f.addFile("src/test/mcmc/mcmc_output/blocker2.csv");
  std::vector<std::string> vars;
  EXPECT_NO_THROW(vars = f.availableVariables());
  EXPECT_FLOAT_EQ(49, vars.size());
  EXPECT_EQ("lp__", vars[0]);
  EXPECT_EQ("mu.1", vars[4]);

  EXPECT_NO_THROW(vars = f.availableVariables(0));
  EXPECT_FLOAT_EQ(49, vars.size());
  EXPECT_EQ("lp__", vars[0]);
  EXPECT_EQ("d", vars[2]);

  EXPECT_NO_THROW(vars = f.availableVariables(1));
  EXPECT_FLOAT_EQ(49, vars.size());
  EXPECT_EQ("lp__", vars[0]);
  EXPECT_EQ("delta.1", vars[26]);

  EXPECT_THROW(vars = f.availableVariables(2), std::runtime_error);
}

<commit_msg>added test for effective size based on R value<commit_after>#include <stan/mcmc/mcmc_output.hpp>
#include <gtest/gtest.h>

class stanMcmc : public ::testing::Test {
  // R code:
  // R> set.seed(0)
  // R> y <- rnorm(100)
protected:
  virtual void SetUp() {
    std::vector<double> y1(&r_[0], &r_[50]);
    std::vector<double> y2(&r_[50], &r_[100]);
    mcmc_output.add_chain(y1);
    mcmc_output.add_chain(y2);

    factory.addFile("src/test/mcmc/mcmc_output/blocker1.csv");
    factory.addFile("src/test/mcmc/mcmc_output/blocker2.csv");
  }

  stan::mcmc::mcmc_output mcmc_output;
  stan::mcmc::mcmc_output_factory factory;
  static const double r_[100];
};

const double stanMcmc::r_[100] =
    {1.26295428488079,-0.326233360705649,1.3297992629225,1.2724293214294,0.414641434456408,
     -1.53995004190371,-0.928567034713538,-0.29472044679056,-0.00576717274753696,2.40465338885795,
     0.76359346114046,-0.799009248989368,-1.14765700923635,-0.289461573688223,-0.299215117897316,
     -0.411510832795067,0.252223448156132,-0.891921127284569,0.435683299355719,-1.23753842192996,
     -0.224267885278309,0.377395645981701,0.133336360814841,0.804189509744908,-0.0571067743838088,
     0.503607972233726,1.08576936214569,-0.69095383969683,-1.28459935387219,0.046726172188352,
     -0.235706556439501,-0.542888255010254,-0.433310317456782,-0.649471646796233,0.726750747385451,
     1.1519117540872,0.992160365445798,-0.429513109491881,1.23830410085338,-0.279346281854269,
     1.75790308981071,0.560746090888056,-0.452783972553158,-0.832043296117832,-1.16657054708471,
     -1.0655905803883,-1.563782051071,1.15653699715018,0.83204712857239,-0.227328691424755,
     0.266137361672105,-0.376702718583628,2.44136462889459,-0.795339117255372,-0.0548774737115786,
     0.250141322854153,0.618243293566247,-0.172623502645857,-2.22390027400994,-1.26361438497058,
     0.358728895971352,-0.0110454784656636,-0.940649162618608,-0.115825322156954,-0.814968708869917,
     0.242263480859686,-1.4250983947325,0.36594112304922,0.248412648872596,0.0652881816716207,
     0.0191563916602738,0.257338377155533,-0.649010077708898,-0.119168762418038,0.66413569989411,
     1.10096910219409,0.14377148075807,-0.117753598165951,-0.912068366948338,-1.43758624082998,
     -0.797089525071965,1.25408310644997,0.77214218580453,-0.21951562675344,-0.424810283377287,
     -0.418980099421959,0.996986860909106,-0.275778029088027,1.2560188173061,0.646674390495345,
     1.29931230256343,-0.873262111744435,0.00837095999603331,-0.880871723252545,0.59625901661066,
     0.119717641289537,-0.282173877322451,1.45598840106634,0.229019590694692,0.996543928544126};

TEST_F(stanMcmc, effectiveSize)  {
  double ess = mcmc_output.effectiveSize();

  EXPECT_FLOAT_EQ(78.42269, ess);
}

TEST_F(stanMcmc, splitRHat) {
  double splitRHat = mcmc_output.splitRHat();

  EXPECT_FLOAT_EQ(0.9892759, splitRHat);
}

TEST_F(stanMcmc, factory) {
  stan::mcmc::mcmc_output mu1;
  EXPECT_NO_THROW(mu1 = factory.create("mu.1"));
}

TEST_F(stanMcmc, factoryAvailableVariables) {
  std::vector<std::string> vars;
  EXPECT_NO_THROW(vars = factory.availableVariables());
  EXPECT_FLOAT_EQ(49, vars.size());
  EXPECT_EQ("lp__", vars[0]);
  EXPECT_EQ("mu.1", vars[4]);

  EXPECT_NO_THROW(vars = factory.availableVariables(0));
  EXPECT_FLOAT_EQ(49, vars.size());
  EXPECT_EQ("lp__", vars[0]);
  EXPECT_EQ("d", vars[2]);

  EXPECT_NO_THROW(vars = factory.availableVariables(1));
  EXPECT_FLOAT_EQ(49, vars.size());
  EXPECT_EQ("lp__", vars[0]);
  EXPECT_EQ("delta.1", vars[26]);

  EXPECT_THROW(vars = factory.availableVariables(2), std::runtime_error);
}

TEST_F(stanMcmc, mu1) {
  stan::mcmc::mcmc_output mu1 = factory.create("mu.1");
  EXPECT_FLOAT_EQ(13.599755, mu1.effectiveSize());
}
<|endoftext|>


<commit_before>/* @doc Fundamental object used to compute :
   - Center Of Mass,
   - Zero Momentum Point.

   OS: Updated the names of the contributors, the documentation
   and added a sample file for WalkPlugin
   OS (21/12/2006): removed any reference to non-homogeneous matrix
   library.

   Copyright (c) 2005-2006,
   @author :
   Adrien Escande,
   Abderrahmane Kheddar,
   Olivier Stasse,
   Ramzi Sellouati

   JRL-Japan, CNRS/AIST

   All rights reserved.

   Please refers to file License.txt for details on the license.

*/

#include "Body.h"

using namespace dynamicsJRLJapan;

/**************************************************/
/* Implementation of constructors and destructors */
/**************************************************/

Body::Body(void)
{
  m_mass	= 0;
  attCoefMass = 0;
  posCoM[0] = 0;
  posCoM[1] = 0;
  posCoM[2] = 0;
  MAL_S3x3_MATRIX_SET_IDENTITY(inertie);
  label=-1;
  labelMother=-1;
  m_Explored =0 ;
  m_Initialized = false;
}

Body::Body(double lmass)
{
  this->m_mass		= lmass;
  m_Explored =0 ;
  m_Initialized = false;
}

Body::Body(double lmass,
	   MAL_S3_VECTOR(positionCoM,double))
{
  this->m_mass		= lmass;
  m_Explored =0 ;
  m_Initialized = false;
  this->posCoM	= positionCoM;
}
Body::Body(double lmass,
	   MAL_S3_VECTOR(positionCoM,double),
	   MAL_S3x3_MATRIX(matriceInertie,double))
{
  this->posCoM	= positionCoM;
  this->m_mass		= lmass;
  m_Explored =0 ;
  m_Initialized = false;
  this->inertie	= matriceInertie;
}

Body::~Body(void)
{
}

/***********************************************/
/* Implementation of setters and getters       */
/***********************************************/

int Body::getLabel() const
{
  return label;
}

void Body::setLabel(int i)
{
  label = i;
}

double Body::getMass(void) const
{
  return m_mass;
}

string Body::getName() const
{
  return Name;
}

void Body::setName(char *aname)
{
  Name = aname;
}

Body & Body::operator=( Body const & r)
{
  label = r.getLabel();
  m_mass = r.getMass();
  posCoM =r.localCenterOfMass();
  inertie = r.getInertie();
  Name= r.getName();
  labelMother=r.getLabelMother();
  m_Explored = r.getExplored();
  return *this;
}

void Body::setInertie(double mi[9])
{
  for(unsigned int i=0;i<3;i++)
    for(unsigned int j=0;j<3;j++)
      inertie(i,j) = mi[i*3+j];
}

void Body::setMass(double lmass)
{
  m_mass =lmass;
}

void Body::setLabelMother(int al)
{
  labelMother = al;
}

int Body::getLabelMother() const
{
  return labelMother;
}

int Body::getExplored() const
{
  return m_Explored;
}

void Body::setExplored(int anEx)
{
  m_Explored = anEx;

}

bool Body::getInitialized() const
{
  return m_Initialized;
}

void Body::setInitialized(bool anInitialized)
{
  m_Initialized = anInitialized;
}

/***********************************************/
/* Implementation of the methods for display   */
/***********************************************/

void Body::Display()
{
  cout << "Name  :" << Name << endl;
  cout << "Mass :" << m_mass << endl;
  cout << "Center of Mass    : " << posCoM[0] << " "
       << posCoM[1] << " " <<posCoM[2]<<endl;
  cout << "Inertia Matrix : " << endl;
  for(int i=0;i<3;i++)
    {
      for(int j=0;j<3;j++)
	cout << inertie(i,j) << " ";
      cout << endl;
    }
  cout << "Mother: " << labelMother<< endl;
}

/***********************************************/
/* Implementation of the generic JRL interface */
/***********************************************/

const MAL_S3_VECTOR(,double) & Body::localCenterOfMass() const
{
  return posCoM;
}

void Body::localCenterOfMass(const MAL_S3_VECTOR(,double) &inlocalCenterOfMass)
{
  posCoM = inlocalCenterOfMass;
}

/*! Returns inertia matrix in the local reference frame */
const MAL_S3x3_MATRIX(,double) & Body::inertiaMatrix() const
{
  return inertie;
}

void Body::inertiaMatrix(const MAL_S3x3_MATRIX(,double) &inInertiaMatrix)
{
  inertie = inInertiaMatrix;
}

const CjrlJoint* Body::joint() const
{
  return m_AttachedJoint;
}

JointPrivate* Body::getJointPrivate()
{
  return m_AttachedJoint;
}

void Body::joint(JointPrivate * ajoint)
{
  m_AttachedJoint = ajoint;
  if (m_AttachedJoint!=0)
    m_AttachedJoint->setLinkedBody(*this);
}

<commit_msg>Initialize properly the attached joint.<commit_after>/* @doc Fundamental object used to compute :
   - Center Of Mass,
   - Zero Momentum Point.

   OS: Updated the names of the contributors, the documentation
   and added a sample file for WalkPlugin
   OS (21/12/2006): removed any reference to non-homogeneous matrix
   library.

   Copyright (c) 2005-2006,
   @author :
   Adrien Escande,
   Abderrahmane Kheddar,
   Olivier Stasse,
   Ramzi Sellouati

   JRL-Japan, CNRS/AIST

   All rights reserved.

   Please refers to file License.txt for details on the license.

*/

#include "Body.h"

using namespace dynamicsJRLJapan;

/**************************************************/
/* Implementation of constructors and destructors */
/**************************************************/

Body::Body(void)
{
  m_AttachedJoint =0;
  m_mass	= 0;
  attCoefMass = 0;
  posCoM[0] = 0;
  posCoM[1] = 0;
  posCoM[2] = 0;
  MAL_S3x3_MATRIX_SET_IDENTITY(inertie);
  label=-1;
  labelMother=-1;
  m_Explored =0 ;
  m_Initialized = false;
}

Body::Body(double lmass)
{
  m_AttachedJoint =0;
  this->m_mass		= lmass;
  m_Explored =0 ;
  m_Initialized = false;
}

Body::Body(double lmass,
	   MAL_S3_VECTOR(positionCoM,double))
{
  m_AttachedJoint =0;
  this->m_mass		= lmass;
  m_Explored =0 ;
  m_Initialized = false;
  this->posCoM	= positionCoM;
}
Body::Body(double lmass,
	   MAL_S3_VECTOR(positionCoM,double),
	   MAL_S3x3_MATRIX(matriceInertie,double))
{
  m_AttachedJoint =0;
  this->posCoM	= positionCoM;
  this->m_mass		= lmass;
  m_Explored =0 ;
  m_Initialized = false;
  this->inertie	= matriceInertie;
}

Body::~Body(void)
{
}

/***********************************************/
/* Implementation of setters and getters       */
/***********************************************/

int Body::getLabel() const
{
  return label;
}

void Body::setLabel(int i)
{
  label = i;
}

double Body::getMass(void) const
{
  return m_mass;
}

string Body::getName() const
{
  return Name;
}

void Body::setName(char *aname)
{
  Name = aname;
}

Body & Body::operator=( Body const & r)
{
  label = r.getLabel();
  m_mass = r.getMass();
  posCoM =r.localCenterOfMass();
  inertie = r.getInertie();
  Name= r.getName();
  labelMother=r.getLabelMother();
  m_Explored = r.getExplored();
  return *this;
}

void Body::setInertie(double mi[9])
{
  for(unsigned int i=0;i<3;i++)
    for(unsigned int j=0;j<3;j++)
      inertie(i,j) = mi[i*3+j];
}

void Body::setMass(double lmass)
{
  m_mass =lmass;
}

void Body::setLabelMother(int al)
{
  labelMother = al;
}

int Body::getLabelMother() const
{
  return labelMother;
}

int Body::getExplored() const
{
  return m_Explored;
}

void Body::setExplored(int anEx)
{
  m_Explored = anEx;

}

bool Body::getInitialized() const
{
  return m_Initialized;
}

void Body::setInitialized(bool anInitialized)
{
  m_Initialized = anInitialized;
}

/***********************************************/
/* Implementation of the methods for display   */
/***********************************************/

void Body::Display()
{
  cout << "Name  :" << Name << endl;
  cout << "Mass :" << m_mass << endl;
  cout << "Center of Mass    : " << posCoM[0] << " "
       << posCoM[1] << " " <<posCoM[2]<<endl;
  cout << "Inertia Matrix : " << endl;
  for(int i=0;i<3;i++)
    {
      for(int j=0;j<3;j++)
	cout << inertie(i,j) << " ";
      cout << endl;
    }
  cout << "Mother: " << labelMother<< endl;
}

/***********************************************/
/* Implementation of the generic JRL interface */
/***********************************************/

const MAL_S3_VECTOR(,double) & Body::localCenterOfMass() const
{
  return posCoM;
}

void Body::localCenterOfMass(const MAL_S3_VECTOR(,double) &inlocalCenterOfMass)
{
  posCoM = inlocalCenterOfMass;
}

/*! Returns inertia matrix in the local reference frame */
const MAL_S3x3_MATRIX(,double) & Body::inertiaMatrix() const
{
  return inertie;
}

void Body::inertiaMatrix(const MAL_S3x3_MATRIX(,double) &inInertiaMatrix)
{
  inertie = inInertiaMatrix;
}

const CjrlJoint* Body::joint() const
{
  return m_AttachedJoint;
}

JointPrivate* Body::getJointPrivate()
{
  return m_AttachedJoint;
}

void Body::joint(JointPrivate * ajoint)
{
  m_AttachedJoint = ajoint;
  if (m_AttachedJoint!=0)
    m_AttachedJoint->setLinkedBody(*this);
}

<|endoftext|>


<commit_before>#include <stdexcept>
#include <vector>

#include "mettle.hpp"
using namespace mettle;

struct some_type {
  some_type(const some_type &) = delete;
  some_type & operator =(const some_type &) = delete;
};

suite<> matcher_tests("test matchers", [](auto &_) {

  _.test("anything()", []() {
    expect(true, anything());
    expect(false, anything());
    expect(123, anything());
    expect(some_type{}, anything());

    expect(anything().desc(), equal_to("anything"));
  });

  _.test("equal_to()", []() {
    expect(true, equal_to(true));
    expect(123, equal_to(123));

    expect(equal_to(123).desc(), equal_to("123"));
  });

  _.test("not_equal_to()", []() {
    expect(true, not_equal_to(false));
    expect(123, not_equal_to(1234));

    expect(not_equal_to(123).desc(), equal_to("not 123"));
  });

  _.test("greater()", []() {
    expect(123, greater(0));

    expect(greater(123).desc(), equal_to("> 123"));
  });

  _.test("greater_equal()", []() {
    expect(123, greater_equal(0));

    expect(greater_equal(123).desc(), equal_to(">= 123"));
  });

  _.test("less()", []() {
    expect(123, less(1000));

    expect(less(123).desc(), equal_to("< 123"));
  });

  _.test("less_equal()", []() {
    expect(123, less_equal(1000));

    expect(less_equal(123).desc(), equal_to("<= 123"));
  });

  _.test("is_not()", []() {
    expect(123, is_not(equal_to(100)));
    expect(123, is_not(100));

    expect(is_not(123).desc(), equal_to("not 123"));
  });

  _.test("any_of()", []() {
    expect(123, any_of(equal_to(1), equal_to(2), equal_to(123)));
    expect(123, any_of(1, 2, 123));
    expect(123, is_not(any_of(1, 2, 3)));
    expect(123, is_not(any_of()));

    expect(any_of(1, 2, 3).desc(), equal_to("any of(1, 2, 3)"));
  });

  _.test("all_of()", []() {
    expect(123, all_of(123));
    expect(123, all_of(not_equal_to(1), not_equal_to(2), greater(3)));
    expect(123, all_of());

    expect(all_of(1, 2, 3).desc(), equal_to("all of(1, 2, 3)"));
  });

  _.test("member()", []() {
    expect(std::vector<int>{}, is_not(member(0)));
    expect(std::vector<int>{1, 2, 3}, member(1));
    expect(std::vector<int>{1, 2, 3}, is_not(member(4)));

    expect(member(123).desc(), equal_to("member 123"));
  });

  _.test("each()", []() {
    expect(std::vector<int>{}, each( is_not(anything()) ));
    expect(std::vector<int>{1, 2, 3}, each( greater(0)) );
    expect(std::vector<int>{1, 2, 3}, is_not( each(less(2)) ));

    expect(each(123).desc(), equal_to("each 123"));
  });

  _.test("array()", []() {
    expect(std::vector<int>{}, array());
    expect(std::vector<int>{1, 2, 3}, array(1, 2, 3));

    expect(array(1, 2, 3).desc(), equal_to("[1, 2, 3]"));
  });

  _.test("thrown()", []() {
    auto thrower = []() { throw std::runtime_error("message"); };
    expect(thrower, thrown<std::runtime_error>());
    expect(thrower, thrown<std::exception>());
    expect(thrower, is_not(thrown<std::logic_error>()));
    expect(thrower, thrown());
    expect(thrower, thrown<std::runtime_error>("message"));
    expect(thrower, thrown<std::runtime_error>(is_not("wrong")));
    expect(thrower, is_not(thrown<std::logic_error>("message")));
    expect(thrower, is_not(thrown<std::logic_error>( is_not("wrong") )));

    auto int_thrower = []() { throw 123; };
    expect(int_thrower, thrown<int>());
    expect(int_thrower, is_not(thrown<std::exception>()));
    expect(int_thrower, thrown());
    expect(int_thrower, thrown_raw<int>(123));
    expect(int_thrower, is_not(thrown_raw<int>(0)));
    expect(int_thrower, is_not(thrown<std::exception>("message")));
    expect(int_thrower, is_not(thrown<std::exception>( is_not("wrong") )));

    auto noop = []() {};
    expect(noop, is_not(thrown<std::exception>()));
    expect(noop, is_not(thrown()));
    expect(noop, is_not(thrown<std::exception>("message")));
    expect(noop, is_not(thrown<std::exception>( is_not("wrong") )));
  });

});
<commit_msg>Test string output of thrown() matcher<commit_after>#include <stdexcept>
#include <vector>

#include "mettle.hpp"
using namespace mettle;

struct some_type {
  some_type(const some_type &) = delete;
  some_type & operator =(const some_type &) = delete;
};

suite<> matcher_tests("test matchers", [](auto &_) {

  _.test("anything()", []() {
    expect(true, anything());
    expect(false, anything());
    expect(123, anything());
    expect(some_type{}, anything());

    expect(anything().desc(), equal_to("anything"));
  });

  _.test("equal_to()", []() {
    expect(true, equal_to(true));
    expect(123, equal_to(123));

    expect(equal_to(123).desc(), equal_to("123"));
  });

  _.test("not_equal_to()", []() {
    expect(true, not_equal_to(false));
    expect(123, not_equal_to(1234));

    expect(not_equal_to(123).desc(), equal_to("not 123"));
  });

  _.test("greater()", []() {
    expect(123, greater(0));

    expect(greater(123).desc(), equal_to("> 123"));
  });

  _.test("greater_equal()", []() {
    expect(123, greater_equal(0));

    expect(greater_equal(123).desc(), equal_to(">= 123"));
  });

  _.test("less()", []() {
    expect(123, less(1000));

    expect(less(123).desc(), equal_to("< 123"));
  });

  _.test("less_equal()", []() {
    expect(123, less_equal(1000));

    expect(less_equal(123).desc(), equal_to("<= 123"));
  });

  _.test("is_not()", []() {
    expect(123, is_not(equal_to(100)));
    expect(123, is_not(100));

    expect(is_not(123).desc(), equal_to("not 123"));
  });

  _.test("any_of()", []() {
    expect(123, any_of(equal_to(1), equal_to(2), equal_to(123)));
    expect(123, any_of(1, 2, 123));
    expect(123, is_not(any_of(1, 2, 3)));
    expect(123, is_not(any_of()));

    expect(any_of(1, 2, 3).desc(), equal_to("any of(1, 2, 3)"));
  });

  _.test("all_of()", []() {
    expect(123, all_of(123));
    expect(123, all_of(not_equal_to(1), not_equal_to(2), greater(3)));
    expect(123, all_of());

    expect(all_of(1, 2, 3).desc(), equal_to("all of(1, 2, 3)"));
  });

  _.test("member()", []() {
    expect(std::vector<int>{}, is_not(member(0)));
    expect(std::vector<int>{1, 2, 3}, member(1));
    expect(std::vector<int>{1, 2, 3}, is_not(member(4)));

    expect(member(123).desc(), equal_to("member 123"));
  });

  _.test("each()", []() {
    expect(std::vector<int>{}, each( is_not(anything()) ));
    expect(std::vector<int>{1, 2, 3}, each( greater(0)) );
    expect(std::vector<int>{1, 2, 3}, is_not( each(less(2)) ));

    expect(each(123).desc(), equal_to("each 123"));
  });

  _.test("array()", []() {
    expect(std::vector<int>{}, array());
    expect(std::vector<int>{1, 2, 3}, array(1, 2, 3));

    expect(array(1, 2, 3).desc(), equal_to("[1, 2, 3]"));
  });

  _.test("thrown()", []() {
    auto thrower = []() { throw std::runtime_error("message"); };
    expect(thrower, thrown<std::runtime_error>());
    expect(thrower, thrown<std::exception>());
    expect(thrower, is_not(thrown<std::logic_error>()));
    expect(thrower, thrown());
    expect(thrower, thrown<std::runtime_error>("message"));
    expect(thrower, thrown<std::runtime_error>(is_not("wrong")));
    expect(thrower, is_not(thrown<std::logic_error>("message")));
    expect(thrower, is_not(thrown<std::logic_error>( is_not("wrong") )));

    auto int_thrower = []() { throw 123; };
    expect(int_thrower, thrown<int>());
    expect(int_thrower, is_not(thrown<std::exception>()));
    expect(int_thrower, thrown());
    expect(int_thrower, thrown_raw<int>(123));
    expect(int_thrower, is_not(thrown_raw<int>(0)));
    expect(int_thrower, is_not(thrown<std::exception>("message")));
    expect(int_thrower, is_not(thrown<std::exception>( is_not("wrong") )));

    auto noop = []() {};
    expect(noop, is_not(thrown<std::exception>()));
    expect(noop, is_not(thrown()));
    expect(noop, is_not(thrown<std::exception>("message")));
    expect(noop, is_not(thrown<std::exception>( is_not("wrong") )));

    expect(thrown().desc(), equal_to("threw"));
    expect(thrown<std::exception>("message").desc(), equal_to("threw message"));
  });

});
<|endoftext|>


<commit_before>#include "boost_test.h"
#include "ComplexFixedPoint.h"
#include <complex>

using namespace std;

BOOST_AUTO_TEST_CASE( CFxpAccessors )
{
	CFxp a(1, 2, 8);

	BOOST_CHECK_EQUAL(a.width(), 8);

}

BOOST_AUTO_TEST_CASE( CFxpEquality )
{
	CFxp a(1, 2, 8);
	CFxp b(1, 2, 8);
	CFxp c(1, 2, 10);
	CFxp d(13, 2, 8);
	CFxp e(1, 13, 8);
	CFxp f(24, 38, 21);

	/* same object as lhs and rhs */
	BOOST_CHECK_EQUAL(a == a, true);
	BOOST_CHECK_EQUAL(a != a, false);

	/* different equal objects */
	BOOST_CHECK_EQUAL(a == b, true);
	BOOST_CHECK_EQUAL(a != b, false);

	/* same value, different widths */
	BOOST_CHECK_EQUAL(a == c, false);
	BOOST_CHECK_EQUAL(a != c, true);

	/* different values, same widths */
	BOOST_CHECK_EQUAL(a == d, false);
	BOOST_CHECK_EQUAL(a != d, true);
	BOOST_CHECK_EQUAL(a == e, false);
	BOOST_CHECK_EQUAL(a != e, true);

	/* everything different */
	BOOST_CHECK_EQUAL(a == f, false);
	BOOST_CHECK_EQUAL(a != f, true);
}

BOOST_AUTO_TEST_CASE( CFxpScalarMultiplication )
{
	CFxp a(1, 2, 8);
	Fxp b(2, 5);

	/* multiplication by scalar is commutative */
	BOOST_CHECK_EQUAL(a * b, b * a);

	/* width after multiplication by scalar */
	BOOST_CHECK_EQUAL((a * b).width(), 13);

	/* result check */
	BOOST_CHECK_EQUAL(a * b, CFxp(2, 4, 13));
}
<commit_msg>Added CFxp unit tests for assignment operator<commit_after>#include "boost_test.h"
#include "ComplexFixedPoint.h"
#include <complex>

using namespace std;

BOOST_AUTO_TEST_CASE( CFxpAccessors )
{
	CFxp a(1, 2, 8);

	BOOST_CHECK_EQUAL(a.width(), 8);

}

BOOST_AUTO_TEST_CASE( CFxpAssignment )
{
	CFxp a(1, 2, 8);
	CFxp b(1, 2, 10);
	CFxp c;
	CFxp d(1, 2, 8, true);
	CFxp e(1, 2, 8, false);

	BOOST_CHECK_THROW(a = b, SizeMismatchException);
	BOOST_CHECK_NO_THROW(c = a);
	BOOST_CHECK_EQUAL(c, a);
	BOOST_CHECK_NO_THROW(d = b);
	BOOST_CHECK_EQUAL(d, b);
	BOOST_CHECK_THROW(e = b, SizeMismatchException);
}

BOOST_AUTO_TEST_CASE( CFxpEquality )
{
	CFxp a(1, 2, 8);
	CFxp b(1, 2, 8);
	CFxp c(1, 2, 10);
	CFxp d(13, 2, 8);
	CFxp e(1, 13, 8);
	CFxp f(24, 38, 21);

	/* same object as lhs and rhs */
	BOOST_CHECK_EQUAL(a == a, true);
	BOOST_CHECK_EQUAL(a != a, false);

	/* different equal objects */
	BOOST_CHECK_EQUAL(a == b, true);
	BOOST_CHECK_EQUAL(a != b, false);

	/* same value, different widths */
	BOOST_CHECK_EQUAL(a == c, false);
	BOOST_CHECK_EQUAL(a != c, true);

	/* different values, same widths */
	BOOST_CHECK_EQUAL(a == d, false);
	BOOST_CHECK_EQUAL(a != d, true);
	BOOST_CHECK_EQUAL(a == e, false);
	BOOST_CHECK_EQUAL(a != e, true);

	/* everything different */
	BOOST_CHECK_EQUAL(a == f, false);
	BOOST_CHECK_EQUAL(a != f, true);
}

BOOST_AUTO_TEST_CASE( CFxpScalarMultiplication )
{
	CFxp a(1, 2, 8);
	Fxp b(2, 5);

	/* multiplication by scalar is commutative */
	BOOST_CHECK_EQUAL(a * b, b * a);

	/* width after multiplication by scalar */
	BOOST_CHECK_EQUAL((a * b).width(), 13);

	/* result check */
	BOOST_CHECK_EQUAL(a * b, CFxp(2, 4, 13));
}
<|endoftext|>