diff --git a/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h b/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h index d42197345..a5e881487 100644 --- a/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h +++ b/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h @@ -46,13 +46,14 @@ public: InputsAtCompileTime = Functor::InputsAtCompileTime, ValuesAtCompileTime = Functor::ValuesAtCompileTime }; - + typedef typename Functor::InputType InputType; typedef typename Functor::ValueType ValueType; typedef typename Functor::JacobianType JacobianType; - typedef AutoDiffScalar > ActiveScalar; - + typedef Matrix DerivativeType; + typedef AutoDiffScalar ActiveScalar; + typedef Matrix ActiveInput; typedef Matrix ActiveValue; @@ -69,26 +70,20 @@ public: ActiveInput ax = x.template cast(); ActiveValue av(jac.rows()); - + if(InputsAtCompileTime==Dynamic) - { - for (int j=0; jinputs()); for (int j=0; jinputs()); - } - - for (int j=0; jinputs(),i); Functor::operator()(ax, &av); for (int i=0; i +struct ei_make_coherent_impl { + static void run(A& a, B& b) {} +}; + +// resize a to match b is a.size()==0, and conversely. +template +void ei_make_coherent(const A& a, const B&b) +{ + ei_make_coherent_impl::run(a.const_cast_derived(), b.const_cast_derived()); +} + /** \class AutoDiffScalar * \brief A scalar type replacement with automatic differentation capability * @@ -35,7 +47,7 @@ namespace Eigen { * This class represents a scalar value while tracking its respective derivatives. * * It supports the following list of global math function: - * - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos, + * - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos, * - ei_abs, ei_sqrt, ei_pow, ei_exp, ei_log, ei_sin, ei_cos, * - ei_conj, ei_real, ei_imag, ei_abs2. * @@ -49,29 +61,29 @@ class AutoDiffScalar { public: typedef typename ei_traits::Scalar Scalar; - + inline AutoDiffScalar() {} - + inline AutoDiffScalar(const Scalar& value) : m_value(value) { if(m_derivatives.size()>0) m_derivatives.setZero(); } - + inline AutoDiffScalar(const Scalar& value, const DerType& der) : m_value(value), m_derivatives(der) {} - + template inline AutoDiffScalar(const AutoDiffScalar& other) : m_value(other.value()), m_derivatives(other.derivatives()) {} - + inline AutoDiffScalar(const AutoDiffScalar& other) : m_value(other.value()), m_derivatives(other.derivatives()) {} - + template inline AutoDiffScalar& operator=(const AutoDiffScalar& other) { @@ -79,32 +91,33 @@ class AutoDiffScalar m_derivatives = other.derivatives(); return *this; } - + inline AutoDiffScalar& operator=(const AutoDiffScalar& other) { m_value = other.value(); m_derivatives = other.derivatives(); return *this; } - + // inline operator const Scalar& () const { return m_value; } // inline operator Scalar& () { return m_value; } inline const Scalar& value() const { return m_value; } inline Scalar& value() { return m_value; } - + inline const DerType& derivatives() const { return m_derivatives; } inline DerType& derivatives() { return m_derivatives; } - + template inline const AutoDiffScalar,DerType,OtherDerType> > operator+(const AutoDiffScalar& other) const { + ei_make_coherent(m_derivatives, other.derivatives()); return AutoDiffScalar,DerType,OtherDerType> >( m_value + other.value(), m_derivatives + other.derivatives()); } - + template inline AutoDiffScalar& operator+=(const AutoDiffScalar& other) @@ -112,16 +125,17 @@ class AutoDiffScalar (*this) = (*this) + other; return *this; } - + template inline const AutoDiffScalar, DerType,OtherDerType> > operator-(const AutoDiffScalar& other) const { + ei_make_coherent(m_derivatives, other.derivatives()); return AutoDiffScalar, DerType,OtherDerType> >( m_value - other.value(), m_derivatives - other.derivatives()); } - + template inline AutoDiffScalar& operator-=(const AutoDiffScalar& other) @@ -129,7 +143,7 @@ class AutoDiffScalar *this = *this - other; return *this; } - + template inline const AutoDiffScalar, DerType> > operator-() const @@ -138,7 +152,7 @@ class AutoDiffScalar -m_value, -m_derivatives); } - + inline const AutoDiffScalar, DerType> > operator*(const Scalar& other) const { @@ -146,7 +160,7 @@ class AutoDiffScalar m_value * other, (m_derivatives * other)); } - + friend inline const AutoDiffScalar, DerType> > operator*(const Scalar& other, const AutoDiffScalar& a) { @@ -154,7 +168,7 @@ class AutoDiffScalar a.value() * other, a.derivatives() * other); } - + inline const AutoDiffScalar, DerType> > operator/(const Scalar& other) const { @@ -162,7 +176,7 @@ class AutoDiffScalar m_value / other, (m_derivatives * (Scalar(1)/other))); } - + friend inline const AutoDiffScalar, DerType> > operator/(const Scalar& other, const AutoDiffScalar& a) { @@ -170,7 +184,7 @@ class AutoDiffScalar other / a.value(), a.derivatives() * (-Scalar(1)/other)); } - + template inline const AutoDiffScalar, NestByValue, @@ -178,6 +192,7 @@ class AutoDiffScalar NestByValue, OtherDerType> > > > > > operator/(const AutoDiffScalar& other) const { + ei_make_coherent(m_derivatives, other.derivatives()); return AutoDiffScalar, NestByValue, NestByValue, DerType> >, @@ -186,45 +201,91 @@ class AutoDiffScalar ((m_derivatives * other.value()).nestByValue() - (m_value * other.derivatives()).nestByValue()).nestByValue() * (Scalar(1)/(other.value()*other.value()))); } - + template inline const AutoDiffScalar, NestByValue, DerType> >, NestByValue, OtherDerType> > > > operator*(const AutoDiffScalar& other) const { + ei_make_coherent(m_derivatives, other.derivatives()); return AutoDiffScalar, NestByValue, DerType> >, NestByValue, OtherDerType> > > >( m_value * other.value(), (m_derivatives * other.value()).nestByValue() + (m_value * other.derivatives()).nestByValue()); } - + inline AutoDiffScalar& operator*=(const Scalar& other) { *this = *this * other; return *this; } - + template inline AutoDiffScalar& operator*=(const AutoDiffScalar& other) { *this = *this * other; return *this; } - + protected: Scalar m_value; DerType m_derivatives; - + +}; + +template +struct ei_make_coherent_impl, B> { + typedef Matrix A; + static void run(A& a, B& b) { + if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0)) + { + a.resize(b.size()); + a.setZero(); + } + } +}; + +template +struct ei_make_coherent_impl > { + typedef Matrix B; + static void run(A& a, B& b) { + if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0)) + { + b.resize(a.size()); + b.setZero(); + } + } +}; + +template +struct ei_make_coherent_impl, + Matrix > { + typedef Matrix A; + typedef Matrix B; + static void run(A& a, B& b) { + if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0)) + { + a.resize(b.size()); + a.setZero(); + } + else if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0)) + { + b.resize(a.size()); + b.setZero(); + } + } }; } #define EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(FUNC,CODE) \ template \ - inline const AutoDiffScalar::Scalar>, DerType> > \ - FUNC(const AutoDiffScalar& x) { \ + inline const Eigen::AutoDiffScalar::Scalar>, DerType> > \ + FUNC(const Eigen::AutoDiffScalar& x) { \ + using namespace Eigen; \ typedef typename ei_traits::Scalar Scalar; \ typedef AutoDiffScalar, DerType> > ReturnType; \ CODE; \ @@ -234,34 +295,35 @@ namespace std { EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs, return ReturnType(std::abs(x.value()), x.derivatives() * (sign(x.value())));) - + EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sqrt, Scalar sqrtx = std::sqrt(x.value()); return ReturnType(sqrtx,x.derivatives() * (Scalar(0.5) / sqrtx));) - + EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cos, return ReturnType(std::cos(x.value()), x.derivatives() * (-std::sin(x.value())));) - + EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sin, return ReturnType(std::sin(x.value()),x.derivatives() * std::cos(x.value()));) - + EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(exp, Scalar expx = std::exp(x.value()); return ReturnType(expx,x.derivatives() * expx);) EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(ei_log, return ReturnType(std::log(x.value),x.derivatives() * (Scalar(1).x.value()));) - + template - inline const AutoDiffScalar::Scalar>, DerType> > - pow(const AutoDiffScalar& x, typename ei_traits::Scalar y) + inline const Eigen::AutoDiffScalar::Scalar>, DerType> > + pow(const Eigen::AutoDiffScalar& x, typename Eigen::ei_traits::Scalar y) { + using namespace Eigen; typedef typename ei_traits::Scalar Scalar; return AutoDiffScalar, DerType> >( std::pow(x.value(),y), x.derivatives() * (y * std::pow(x.value(),y-1))); } - + } namespace Eigen { diff --git a/unsupported/test/autodiff.cpp b/unsupported/test/autodiff.cpp index b1164897c..a96927b41 100644 --- a/unsupported/test/autodiff.cpp +++ b/unsupported/test/autodiff.cpp @@ -46,12 +46,12 @@ struct TestFunc1 typedef Matrix InputType; typedef Matrix ValueType; typedef Matrix JacobianType; - + int m_inputs, m_values; - + TestFunc1() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} TestFunc1(int inputs, int values) : m_inputs(inputs), m_values(values) {} - + int inputs() const { return m_inputs; } int values() const { return m_values; } @@ -142,7 +142,7 @@ void test_autodiff_scalar() std::cerr << foo >(ax,ay).value() << " <> " << foo >(ax,ay).derivatives().transpose() << "\n\n"; } - + void test_autodiff_jacobian() { for(int i = 0; i < g_repeat; i++) {