functional_form.h

Go to the documentation of this file.

#pragma once

#include "drake/drakeCore_export.h"

#include <cstddef>
#include <initializer_list>
#include <iosfwd>
#include <memory>
#include <string>
#include <type_traits>
#include <vector>

#include <Eigen/Core>

namespace drake {

class DRAKECORE_EXPORT FunctionalForm {
 public:
  class Variable;
  class Variables;

  FunctionalForm();

  explicit FunctionalForm(double d);

  static FunctionalForm Zero();

  static FunctionalForm Constant();

  static FunctionalForm Linear(Variables v);

  static FunctionalForm Affine(Variables v);

  static FunctionalForm Polynomial(Variables v);

  static FunctionalForm Differentiable(Variables v);

  static FunctionalForm Arbitrary(Variables v);

  static FunctionalForm Undefined(Variables v);

  bool IsZero() const;

  bool IsConstant() const;

  bool IsLinear() const;

  bool IsAffine() const;

  bool IsPolynomial() const;

  bool IsDifferentiable() const;

  bool IsArbitrary() const;

  bool IsUndefined() const;

  bool Is(FunctionalForm const& f) const;

  Variables GetVariables() const;

  friend DRAKECORE_EXPORT std::ostream& operator<<(std::ostream& os,
                                                   FunctionalForm const& f);

  friend DRAKECORE_EXPORT FunctionalForm operator+(FunctionalForm const& lhs,
                                                   FunctionalForm const& rhs);

  friend DRAKECORE_EXPORT FunctionalForm operator+(FunctionalForm const& lhs,
                                                   double rhs);

  friend DRAKECORE_EXPORT FunctionalForm operator+(double lhs,
                                                   FunctionalForm const& rhs);

  friend DRAKECORE_EXPORT FunctionalForm& operator+=(FunctionalForm& lhs,
                                                     FunctionalForm const& rhs);

  friend DRAKECORE_EXPORT FunctionalForm& operator+=(FunctionalForm& lhs,
                                                     double rhs);

  friend DRAKECORE_EXPORT FunctionalForm operator-(FunctionalForm const& lhs,
                                                   FunctionalForm const& rhs);

  friend DRAKECORE_EXPORT FunctionalForm operator-(FunctionalForm const& lhs,
                                                   double rhs);

  friend DRAKECORE_EXPORT FunctionalForm operator-(double lhs,
                                                   FunctionalForm const& rhs);

  friend DRAKECORE_EXPORT FunctionalForm& operator-=(FunctionalForm& lhs,
                                                     FunctionalForm const& rhs);

  friend DRAKECORE_EXPORT FunctionalForm& operator-=(FunctionalForm& lhs,
                                                     double rhs);

  friend DRAKECORE_EXPORT FunctionalForm operator*(FunctionalForm const& lhs,
                                                   FunctionalForm const& rhs);

  friend DRAKECORE_EXPORT FunctionalForm operator*(FunctionalForm const& lhs,
                                                   double rhs);

  friend DRAKECORE_EXPORT FunctionalForm operator*(double lhs,
                                                   FunctionalForm const& rhs);

  friend DRAKECORE_EXPORT FunctionalForm& operator*=(FunctionalForm& lhs,
                                                     FunctionalForm const& rhs);

  friend DRAKECORE_EXPORT FunctionalForm& operator*=(FunctionalForm& lhs,
                                                     double rhs);

  friend DRAKECORE_EXPORT FunctionalForm operator/(FunctionalForm const& lhs,
                                                   FunctionalForm const& rhs);

  friend DRAKECORE_EXPORT FunctionalForm operator/(FunctionalForm const& lhs,
                                                   double rhs);

  friend DRAKECORE_EXPORT FunctionalForm operator/(double lhs,
                                                   FunctionalForm const& rhs);

  friend DRAKECORE_EXPORT FunctionalForm& operator/=(FunctionalForm& lhs,
                                                     FunctionalForm const& rhs);

  friend DRAKECORE_EXPORT FunctionalForm& operator/=(FunctionalForm& lhs,
                                                     double rhs);

  friend DRAKECORE_EXPORT FunctionalForm abs(FunctionalForm const& x);

  friend DRAKECORE_EXPORT FunctionalForm cos(FunctionalForm const& x);

  friend DRAKECORE_EXPORT FunctionalForm exp(FunctionalForm const& x);

  friend DRAKECORE_EXPORT FunctionalForm log(FunctionalForm const& x);

  friend DRAKECORE_EXPORT FunctionalForm sin(FunctionalForm const& x);

  friend DRAKECORE_EXPORT FunctionalForm sqrt(FunctionalForm const& x);

  class DRAKECORE_EXPORT Variables {
   public:
    Variables() = default;

    Variables(std::initializer_list<Variable> init);

    Variables(Variables&&) = default;

    Variables(Variables const&) = default;

    Variables& operator=(Variables&&) = default;

    Variables& operator=(Variables const&) = default;

    static Variables Union(Variables const& l, Variables const& r);

#if defined(DRAKE_DOXYGEN_CXX)

    typedef unspecified_bidirectional_const_iterator<Variable> const_iterator;
#else
    typedef std::vector<Variable>::const_iterator const_iterator;
#endif

    const_iterator begin() const;

    const_iterator end() const;

    bool empty() const;

    std::size_t size() const;

    friend DRAKECORE_EXPORT bool operator==(Variables const& lhs,
                                            Variables const& rhs);

    friend DRAKECORE_EXPORT bool operator!=(Variables const& lhs,
                                            Variables const& rhs);

   private:
    explicit Variables(std::vector<Variable>&& vars);
    std::shared_ptr<std::vector<Variable> const> vars_;
  };

 private:
  enum class Form;
  FunctionalForm(Form f, Variables&& v);

  Variables vars_;
  Form form_;

#if !defined(DRAKE_DOXYGEN_CXX)
  // Internal class for friendship-access in implementation file.
  class Internal;
  friend class Internal;
#endif  // !defined(DRAKE_DOXYGEN_CXX)
};

#if !defined(DRAKE_DOXYGEN_CXX)
// Delete comparison operators because any function containing a conditional
// cannot be evaluated directly with FunctionalForm as its scalar type.
// Such functions will require a manual overload with FunctionalForm to
// specify their form.
bool operator==(FunctionalForm const&, FunctionalForm const&) = delete;
bool operator!=(FunctionalForm const&, FunctionalForm const&) = delete;
bool operator<(FunctionalForm const&, FunctionalForm const&) = delete;
bool operator<=(FunctionalForm const&, FunctionalForm const&) = delete;
bool operator>(FunctionalForm const&, FunctionalForm const&) = delete;
bool operator>=(FunctionalForm const&, FunctionalForm const&) = delete;
#endif  // !defined(DRAKE_DOXYGEN_CXX)

class
#if !defined(DRAKE_DOXYGEN_CXX)
    DRAKECORE_EXPORT
#endif  // !defined(DRAKE_DOXYGEN_CXX)
        FunctionalForm::Variable {
 public:
  Variable();

  Variable(std::size_t index);

  Variable(std::string name);

  template <int N>
  Variable(char const (&name)[N]) : Variable(std::string(name)) {}

  Variable(Variable const& v);

  Variable(Variable&& v) noexcept;  // NOLINT(whitespace/operators) // false-pos

  ~Variable();

  Variable& operator=(Variable const& v);

  Variable& operator=(Variable&& v) noexcept;

  bool is_nil() const;

  bool is_index() const;

  bool is_named() const;

  std::size_t index() const;

  std::string const& name() const;

  friend DRAKECORE_EXPORT std::ostream& operator<<(std::ostream& os,
                                                   Variable const& v);

  friend DRAKECORE_EXPORT bool operator==(Variable const& lhs,
                                          Variable const& rhs);

  friend DRAKECORE_EXPORT bool operator!=(Variable const& lhs,
                                          Variable const& rhs);

  friend DRAKECORE_EXPORT bool operator<(Variable const& lhs,
                                         Variable const& rhs);

  friend DRAKECORE_EXPORT bool operator<=(Variable const& lhs,
                                          Variable const& rhs);

  friend DRAKECORE_EXPORT bool operator>(Variable const& lhs,
                                         Variable const& rhs);

  friend DRAKECORE_EXPORT bool operator>=(Variable const& lhs,
                                          Variable const& rhs);

 private:
  void Destruct() noexcept;

// Hide this from doxygen because it incorrectly documents these
// members as public.  They are not user-facing anyway.
#if !defined(DRAKE_DOXYGEN_CXX)
  union {
    std::size_t index_;
    std::string name_;
  };
#endif  // !defined(DRAKE_DOXYGEN_CXX)

  enum class Tag;
  Tag tag_;
};

template <typename MatrixL, typename MatrixR>
typename std::enable_if<
    std::is_base_of<Eigen::MatrixBase<MatrixL>, MatrixL>::value &&
        std::is_base_of<Eigen::MatrixBase<MatrixR>, MatrixR>::value &&
        std::is_same<typename MatrixL::Scalar, FunctionalForm>::value &&
        std::is_same<typename MatrixR::Scalar, double>::value,
    typename MatrixL::PlainObject>::type
operator+(MatrixL const& lhs, MatrixR const& rhs) {
  return lhs + rhs.template cast<FunctionalForm>();
}

template <typename MatrixL, typename MatrixR>
typename std::enable_if<
    std::is_base_of<Eigen::MatrixBase<MatrixL>, MatrixL>::value &&
        std::is_base_of<Eigen::MatrixBase<MatrixR>, MatrixR>::value &&
        std::is_same<typename MatrixL::Scalar, double>::value &&
        std::is_same<typename MatrixR::Scalar, FunctionalForm>::value,
    typename MatrixR::PlainObject>::type
operator+(MatrixL const& lhs, MatrixR const& rhs) {
  return lhs.template cast<FunctionalForm>() + rhs;
}

template <typename MatrixL, typename MatrixR>
typename std::enable_if<
    std::is_base_of<Eigen::MatrixBase<MatrixL>, MatrixL>::value &&
        std::is_base_of<Eigen::MatrixBase<MatrixR>, MatrixR>::value &&
        std::is_same<typename MatrixL::Scalar, FunctionalForm>::value &&
        std::is_same<typename MatrixR::Scalar, double>::value,
    MatrixL&>::type
operator+=(MatrixL& lhs, MatrixR const& rhs) {
  return lhs += rhs.template cast<FunctionalForm>();
}

template <typename MatrixL, typename MatrixR>
typename std::enable_if<
    std::is_base_of<Eigen::MatrixBase<MatrixL>, MatrixL>::value &&
        std::is_base_of<Eigen::MatrixBase<MatrixR>, MatrixR>::value &&
        std::is_same<typename MatrixL::Scalar, FunctionalForm>::value &&
        std::is_same<typename MatrixR::Scalar, double>::value,
    typename MatrixL::PlainObject>::type
operator-(MatrixL const& lhs, MatrixR const& rhs) {
  return lhs - rhs.template cast<FunctionalForm>();
}

template <typename MatrixL, typename MatrixR>
typename std::enable_if<
    std::is_base_of<Eigen::MatrixBase<MatrixL>, MatrixL>::value &&
        std::is_base_of<Eigen::MatrixBase<MatrixR>, MatrixR>::value &&
        std::is_same<typename MatrixL::Scalar, double>::value &&
        std::is_same<typename MatrixR::Scalar, FunctionalForm>::value,
    typename MatrixR::PlainObject>::type
operator-(MatrixL const& lhs, MatrixR const& rhs) {
  return lhs.template cast<FunctionalForm>() - rhs;
}

template <typename MatrixL, typename MatrixR>
typename std::enable_if<
    std::is_base_of<Eigen::MatrixBase<MatrixL>, MatrixL>::value &&
        std::is_base_of<Eigen::MatrixBase<MatrixR>, MatrixR>::value &&
        std::is_same<typename MatrixL::Scalar, FunctionalForm>::value &&
        std::is_same<typename MatrixR::Scalar, double>::value,
    MatrixL&>::type
operator-=(MatrixL& lhs, MatrixR const& rhs) {
  return lhs -= rhs.template cast<FunctionalForm>();
}

template <typename MatrixL, typename MatrixR>
typename std::enable_if<
    std::is_base_of<Eigen::MatrixBase<MatrixL>, MatrixL>::value &&
        std::is_base_of<Eigen::MatrixBase<MatrixR>, MatrixR>::value &&
        std::is_same<typename MatrixL::Scalar, FunctionalForm>::value &&
        std::is_same<typename MatrixR::Scalar, double>::value,
    Eigen::Matrix<FunctionalForm, MatrixL::RowsAtCompileTime,
                  MatrixR::ColsAtCompileTime> >::type
operator*(MatrixL const& lhs, MatrixR const& rhs) {
  return lhs * rhs.template cast<FunctionalForm>();
}

template <typename MatrixL, typename MatrixR>
typename std::enable_if<
    std::is_base_of<Eigen::MatrixBase<MatrixL>, MatrixL>::value &&
        std::is_base_of<Eigen::MatrixBase<MatrixR>, MatrixR>::value &&
        std::is_same<typename MatrixL::Scalar, double>::value &&
        std::is_same<typename MatrixR::Scalar, FunctionalForm>::value,
    Eigen::Matrix<FunctionalForm, MatrixL::RowsAtCompileTime,
                  MatrixR::ColsAtCompileTime> >::type
operator*(MatrixL const& lhs, MatrixR const& rhs) {
  return lhs.template cast<FunctionalForm>() * rhs;
}

template <typename MatrixL>
typename std::enable_if<
    std::is_base_of<Eigen::MatrixBase<MatrixL>, MatrixL>::value &&
        std::is_same<typename MatrixL::Scalar, FunctionalForm>::value,
    typename MatrixL::PlainObject>::type
operator*(MatrixL const& lhs, double rhs) {
  return lhs * FunctionalForm(rhs);
}

template <typename MatrixR>
typename std::enable_if<
    std::is_base_of<Eigen::MatrixBase<MatrixR>, MatrixR>::value &&
        std::is_same<typename MatrixR::Scalar, FunctionalForm>::value,
    typename MatrixR::PlainObject>::type
operator*(double lhs, MatrixR const& rhs) {
  return FunctionalForm(lhs) * rhs;
}

template <typename MatrixL>
typename std::enable_if<
    std::is_base_of<Eigen::MatrixBase<MatrixL>, MatrixL>::value &&
        std::is_same<typename MatrixL::Scalar, FunctionalForm>::value,
    MatrixL&>::type
operator*=(MatrixL& lhs, double rhs) {
  return lhs *= FunctionalForm(rhs);
}

template <typename MatrixL>
typename std::enable_if<
    std::is_base_of<Eigen::MatrixBase<MatrixL>, MatrixL>::value &&
        std::is_same<typename MatrixL::Scalar, FunctionalForm>::value,
    typename MatrixL::PlainObject>::type
operator/(MatrixL const& lhs, double rhs) {
  return lhs / FunctionalForm(rhs);
}

template <typename MatrixL>
typename std::enable_if<
    std::is_base_of<Eigen::MatrixBase<MatrixL>, MatrixL>::value &&
        std::is_same<typename MatrixL::Scalar, FunctionalForm>::value,
    MatrixL&>::type
operator/=(MatrixL& lhs, double rhs) {
  return lhs /= FunctionalForm(rhs);
}

}  // namespace drake

#if !defined(DRAKE_DOXYGEN_CXX)
// Define Eigen traits needed for Matrix<FunctionalForm>.
namespace Eigen {

// Eigen scalar type traits for Matrix<FunctionalForm>.
template <>
struct NumTraits<drake::FunctionalForm> {
  enum {
    // Our set of allowed values is discrete, and no epsilon is allowed during
    // equality comparison, so treat this as an unsigned integer type.
    IsInteger = 1,
    IsSigned = 0,
    IsComplex = 0,
    RequireInitialization = 1,
    ReadCost = 1,
    AddCost = 1,
    MulCost = 1
  };

  typedef drake::FunctionalForm Real;
  typedef drake::FunctionalForm Nested;

  static inline Real dummy_precision() { return drake::FunctionalForm(); }
};

namespace internal {

// Eigen component-wise Matrix<FunctionalForm>::isConstant(FunctionalForm).
template <>
struct scalar_fuzzy_impl<drake::FunctionalForm> {
  static inline bool isApprox(drake::FunctionalForm x, drake::FunctionalForm y,
                              drake::FunctionalForm) {
    return x.Is(y);
  }
};

}  // namespace internal
}  // namespace Eigen
#endif  // !defined(DRAKE_DOXYGEN_CXX)