state_vector.h

Go to the documentation of this file.

#pragma once

#include <cstdint>
#include <stdexcept>

#include <Eigen/Dense>

#include "drake/common/drake_assert.h"
#include "drake/systems/framework/vector_interface.h"

namespace drake {
namespace systems {

template <typename T>
class StateVector {
 public:
  virtual ~StateVector() {}

  virtual int size() const = 0;

  virtual const T GetAtIndex(int index) const = 0;

  virtual void SetAtIndex(int index, const T& value) = 0;

  virtual void SetFromVector(const Eigen::Ref<const VectorX<T>>& value) = 0;

  virtual VectorX<T> CopyToVector() const = 0;

  virtual void ScaleAndAddToVector(const T& scale,
                                   Eigen::Ref<VectorX<T>> vec) const {
    if (vec.rows() != size()) {
      throw std::out_of_range("Addends must be the same length.");
    }
    for (int i = 0; i < size(); ++i)
      vec[i] += scale * GetAtIndex(i);
  }

  virtual StateVector& PlusEqScaled(const T& scale, const StateVector<T>& rhs) {
    if (rhs.size() != size()) {
      throw std::out_of_range("Addends must be the same length.");
    }
    for (int i = 0; i < size(); ++i)
      SetAtIndex(i, GetAtIndex(i) + scale * rhs.GetAtIndex(i));
    return *this;
  }

  StateVector& operator+=(const StateVector<T>& rhs) {
    return PlusEqScaled(T(1), rhs);
  }

  StateVector& operator-=(const StateVector<T>& rhs) {
      return PlusEqScaled(T(-1), rhs);
  }

 protected:
  StateVector() {}

 private:
  // StateVector objects are neither copyable nor moveable.
  StateVector(const StateVector& other) = delete;
  StateVector& operator=(const StateVector& other) = delete;
  StateVector(StateVector&& other) = delete;
  StateVector& operator=(StateVector&& other) = delete;
};

}  // namespace systems
}  // namespace drake