Singleton.hpp

//
// Created by Hamza El-Kebir on 6/21/21.
//
 
#ifndef LODESTAR_SINGLETON_HPP
#define LODESTAR_SINGLETON_HPP
 
#include <type_traits>
#include <Eigen/Dense>
#include "Lodestar/aux/CompileTimeQualifiers.hpp"
 
#include "SetExpression.hpp"
#include "SetUnion.hpp"
#include "EmptySet.hpp"
#include "SetComplement.hpp"
 
namespace ls {
    namespace primitives {
        namespace sets {
            template<int TDimension = -1, typename TScalarType = double>
            class Singleton : public SetExpression<Singleton<TDimension, TScalarType>> {
            public:
                template<typename, typename>
                friend
                class SetUnion;
 
                using Base = SetExpression<Singleton<TDimension, TScalarType>>; 
                using type = Singleton<TDimension, TScalarType>; 
 
                typedef Eigen::Matrix<TScalarType, LS_STATIC_UNLESS_DYNAMIC(TDimension), 1> TDValue; 
 
                Singleton() : value_{TDValue::Zero()}
                {
                    this->sEnum_ = SetEnum::Singleton;
                }
 
                template<typename TDerived>
                Singleton(const Eigen::EigenBase<TDerived> &value) : value_(value)
                {
                    this->sEnum_ = SetEnum::Singleton;
                }
 
                template<int T_TDimension = TDimension>
                int
                dimension(typename std::enable_if<(T_TDimension < 0)>::type * = nullptr) const
                {
                    return T_TDimension;
                }
 
                template<int T_TDimension = TDimension>
                int
                dimension(typename std::enable_if<(T_TDimension >= 0)>::type * = nullptr) const
                {
                    return value_.rows();
                }
 
                template<int T_TDimension = TDimension>
                void
                setDimension(size_t dim, typename std::enable_if<(T_TDimension < 0)>::type * = nullptr)
                {
                    static_assert(TDimension > -1, "Cannot set dimension of statically sized singleton.");
                }
 
                template<int T_TDimension = TDimension>
                void
                setDimension(size_t dim, typename std::enable_if<(T_TDimension >= 0)>::type * = nullptr)
                {
                    value_.conservativeResize(dim);
                }
 
                template<typename TExpression>
                typename std::enable_if<std::is_same<TExpression, type>::value, bool>::type
                contains(const SetExpression<TExpression> &expr, double tol = 1e-6) const
                {
                    return (value_ - static_cast<const TExpression *>(&expr)->value_).isMuchSmallerThan(tol);
                }
 
                template<typename TExpression>
                typename std::enable_if<!std::is_same<TExpression, type>::value, bool>::type
                contains(const SetExpression<TExpression> &expr, double tol = 1e-6) const
                {
                    return false;
                }
 
                template<typename TExpression>
                typename std::enable_if<std::is_same<TExpression, type>::value, bool>::type
                operator==(const SetExpression<TExpression> &expr) const
                {
                    return (value_ - static_cast<const TExpression *>(&expr)->value_).isMuchSmallerThan(1e-6);
                }
 
                // TODO: Add dynamic case.
 
                template<typename TExpression>
                typename std::enable_if<!std::is_same<TExpression, type>::value, bool>::type
                operator==(const SetExpression<TExpression> &expr) const
                {
                    return false;
                }
 
                template<typename TExpression>
                typename std::enable_if<std::is_same<TExpression, type>::value, bool>::type
                operator!=(const SetExpression<TExpression> &expr) const
                {
                    return !((value_ - static_cast<const TExpression *>(&expr)->value_).isMuchSmallerThan(1e-6));
                }
 
                template<typename TExpression>
                typename std::enable_if<!std::is_same<TExpression, type>::value, bool>::type
                operator!=(const SetExpression<TExpression> &expr) const
                {
                    return true;
                }
 
                template<typename TExpression>
                SetUnion<type, TExpression> unionize(const SetExpression<TExpression> &expr)
                {
                    return SetUnion<type, TExpression>(*this, *static_cast<const TExpression *>(&expr));
                }
 
                template<typename TExpression>
                bool isSubset(const SetExpression<TExpression> &expr)
                {
                    return static_cast<const TExpression *>(&expr)->contains(*this);
                }
 
                template<typename TExpression>
                bool isSuperset(const SetExpression<TExpression> &expr)
                {
                    return contains(expr);
                }
 
                bool isEmpty() const
                {
                    return false;
                }
 
                template<typename TExpression>
                SetComplement<type, TExpression> relComplement(const SetExpression<TExpression> &expr)
                {
                    return SetComplement<type, TExpression>(*this, *static_cast<const TExpression *>(&expr),
                                                            isSubset(expr));
                }
 
                template<typename TDerived>
                double sdf(Eigen::MatrixBase<TDerived> &p) const
                {
                    p.resize(dimension());
                    return (value_ - p).norm();
                }
 
            protected:
                TDValue value_; 
            };
        }
    }
}
 
#endif //LODESTAR_SINGLETON_HPP