1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
5 //
6 // This Source Code Form is subject to the terms of the Mozilla
7 // Public License v. 2.0. If a copy of the MPL was not distributed
8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9 
10 #ifndef EIGEN_AUTODIFF_VECTOR_H
11 #define EIGEN_AUTODIFF_VECTOR_H
12 
13 namespace Eigen {
14 
15 /* \class AutoDiffScalar
16   * \brief A scalar type replacement with automatic differentation capability
17   *
18   * \param DerType the vector type used to store/represent the derivatives (e.g. Vector3f)
19   *
20   * This class represents a scalar value while tracking its respective derivatives.
21   *
22   * It supports the following list of global math function:
23   *  - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos,
24   *  - internal::abs, internal::sqrt, numext::pow, internal::exp, internal::log, internal::sin, internal::cos,
25   *  - internal::conj, internal::real, internal::imag, numext::abs2.
26   *
27   * AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However,
28   * in that case, the expression template mechanism only occurs at the top Matrix level,
29   * while derivatives are computed right away.
30   *
31   */
32 template<typename ValueType, typename JacobianType>
33 class AutoDiffVector
34 {
35   public:
36     //typedef typename internal::traits<ValueType>::Scalar Scalar;
37     typedef typename internal::traits<ValueType>::Scalar BaseScalar;
38     typedef AutoDiffScalar<Matrix<BaseScalar,JacobianType::RowsAtCompileTime,1> > ActiveScalar;
39     typedef ActiveScalar Scalar;
40     typedef AutoDiffScalar<typename JacobianType::ColXpr> CoeffType;
41     typedef typename JacobianType::Index Index;
42 
AutoDiffVector()43     inline AutoDiffVector() {}
44 
AutoDiffVector(const ValueType & values)45     inline AutoDiffVector(const ValueType& values)
46       : m_values(values)
47     {
48       m_jacobian.setZero();
49     }
50 
51 
52     CoeffType operator[] (Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
53     const CoeffType operator[] (Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
54 
operator()55     CoeffType operator() (Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
operator()56     const CoeffType operator() (Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
57 
coeffRef(Index i)58     CoeffType coeffRef(Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
coeffRef(Index i)59     const CoeffType coeffRef(Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
60 
size()61     Index size() const { return m_values.size(); }
62 
63     // FIXME here we could return an expression of the sum
sum()64     Scalar sum() const { /*std::cerr << "sum \n\n";*/ /*std::cerr << m_jacobian.rowwise().sum() << "\n\n";*/ return Scalar(m_values.sum(), m_jacobian.rowwise().sum()); }
65 
66 
AutoDiffVector(const ValueType & values,const JacobianType & jac)67     inline AutoDiffVector(const ValueType& values, const JacobianType& jac)
68       : m_values(values), m_jacobian(jac)
69     {}
70 
71     template<typename OtherValueType, typename OtherJacobianType>
AutoDiffVector(const AutoDiffVector<OtherValueType,OtherJacobianType> & other)72     inline AutoDiffVector(const AutoDiffVector<OtherValueType, OtherJacobianType>& other)
73       : m_values(other.values()), m_jacobian(other.jacobian())
74     {}
75 
AutoDiffVector(const AutoDiffVector & other)76     inline AutoDiffVector(const AutoDiffVector& other)
77       : m_values(other.values()), m_jacobian(other.jacobian())
78     {}
79 
80     template<typename OtherValueType, typename OtherJacobianType>
81     inline AutoDiffVector& operator=(const AutoDiffVector<OtherValueType, OtherJacobianType>& other)
82     {
83       m_values = other.values();
84       m_jacobian = other.jacobian();
85       return *this;
86     }
87 
88     inline AutoDiffVector& operator=(const AutoDiffVector& other)
89     {
90       m_values = other.values();
91       m_jacobian = other.jacobian();
92       return *this;
93     }
94 
values()95     inline const ValueType& values() const { return m_values; }
values()96     inline ValueType& values() { return m_values; }
97 
jacobian()98     inline const JacobianType& jacobian() const { return m_jacobian; }
jacobian()99     inline JacobianType& jacobian() { return m_jacobian; }
100 
101     template<typename OtherValueType,typename OtherJacobianType>
102     inline const AutoDiffVector<
103       typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type,
104       typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type >
105     operator+(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
106     {
107       return AutoDiffVector<
108       typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type,
109       typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type >(
110         m_values + other.values(),
111         m_jacobian + other.jacobian());
112     }
113 
114     template<typename OtherValueType, typename OtherJacobianType>
115     inline AutoDiffVector&
116     operator+=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
117     {
118       m_values += other.values();
119       m_jacobian += other.jacobian();
120       return *this;
121     }
122 
123     template<typename OtherValueType,typename OtherJacobianType>
124     inline const AutoDiffVector<
125       typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type,
126       typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type >
127     operator-(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
128     {
129       return AutoDiffVector<
130         typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type,
131         typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type >(
132           m_values - other.values(),
133           m_jacobian - other.jacobian());
134     }
135 
136     template<typename OtherValueType, typename OtherJacobianType>
137     inline AutoDiffVector&
138     operator-=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
139     {
140       m_values -= other.values();
141       m_jacobian -= other.jacobian();
142       return *this;
143     }
144 
145     inline const AutoDiffVector<
146       typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, ValueType>::Type,
147       typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, JacobianType>::Type >
148     operator-() const
149     {
150       return AutoDiffVector<
151         typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, ValueType>::Type,
152         typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, JacobianType>::Type >(
153           -m_values,
154           -m_jacobian);
155     }
156 
157     inline const AutoDiffVector<
158       typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
159       typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type>
160     operator*(const BaseScalar& other) const
161     {
162       return AutoDiffVector<
163         typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
164         typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >(
165           m_values * other,
166           m_jacobian * other);
167     }
168 
169     friend inline const AutoDiffVector<
170       typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
171       typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >
172     operator*(const Scalar& other, const AutoDiffVector& v)
173     {
174       return AutoDiffVector<
175         typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
176         typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >(
177           v.values() * other,
178           v.jacobian() * other);
179     }
180 
181 //     template<typename OtherValueType,typename OtherJacobianType>
182 //     inline const AutoDiffVector<
183 //       CwiseBinaryOp<internal::scalar_multiple_op<Scalar>, ValueType, OtherValueType>
184 //       CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
185 //         CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>,
186 //         CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, OtherJacobianType> > >
187 //     operator*(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
188 //     {
189 //       return AutoDiffVector<
190 //         CwiseBinaryOp<internal::scalar_multiple_op<Scalar>, ValueType, OtherValueType>
191 //         CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
192 //           CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>,
193 //           CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, OtherJacobianType> > >(
194 //             m_values.cwise() * other.values(),
195 //             (m_jacobian * other.values()) + (m_values * other.jacobian()));
196 //     }
197 
198     inline AutoDiffVector& operator*=(const Scalar& other)
199     {
200       m_values *= other;
201       m_jacobian *= other;
202       return *this;
203     }
204 
205     template<typename OtherValueType,typename OtherJacobianType>
206     inline AutoDiffVector& operator*=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
207     {
208       *this = *this * other;
209       return *this;
210     }
211 
212   protected:
213     ValueType m_values;
214     JacobianType m_jacobian;
215 
216 };
217 
218 }
219 
220 #endif // EIGEN_AUTODIFF_VECTOR_H
221