367 lines
11 KiB
C++
367 lines
11 KiB
C++
#ifndef DEF_AUTOMATIC_DIFFERENTIATION
|
|
#define DEF_AUTOMATIC_DIFFERENTIATION
|
|
|
|
#include <cmath>
|
|
#include <ostream>
|
|
|
|
#include <Eigen/Dense>
|
|
|
|
/// Implementation of dual numbers for automatic differentiation.
|
|
/// This implementation uses vectors for b so that function gradients can be computed in one function call.
|
|
/// Set the index of every variable with the ::d(int i) function and call the function to be computed : f(x+DualVector::d(0), y+DualVector::d(1), z+DualVector::d(2), ...)
|
|
/// reference : http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.89.7749&rep=rep1&type=pdf
|
|
template<typename Scalar, int N>
|
|
class DualVector
|
|
{
|
|
public:
|
|
using VectorT = Eigen::Matrix<Scalar, N, 1>;
|
|
|
|
DualVector(const Scalar & _a, const VectorT & _b = VectorT::Zero())
|
|
: a(_a),
|
|
b(_b)
|
|
{}
|
|
|
|
DualVector(const Scalar & _a, const Scalar & _b)
|
|
: a(_a)
|
|
{
|
|
b.fill(_b);
|
|
}
|
|
|
|
static DualVector d(int i = 0)
|
|
{
|
|
assert(i >= 0);
|
|
assert(i < N);
|
|
VectorT _d = VectorT::Zero();
|
|
_d[i] = Scalar(1.);
|
|
return DualVector(Scalar(0.), _d);
|
|
}
|
|
|
|
DualVector & operator+=(const DualVector & x)
|
|
{
|
|
a += x.a;
|
|
b += x.b;
|
|
return *this;
|
|
}
|
|
|
|
DualVector & operator-=(const DualVector & x)
|
|
{
|
|
a -= x.a;
|
|
b -= x.b;
|
|
return *this;
|
|
}
|
|
|
|
DualVector & operator*=(const DualVector & x)
|
|
{
|
|
b = a*x.b + b*x.a;
|
|
a *= x.a;
|
|
return *this;
|
|
}
|
|
|
|
DualVector & operator/=(const DualVector & x)
|
|
{
|
|
b = (x.a*b - a*x.b)/(x.a*x.a);
|
|
a /= x.a;
|
|
return *this;
|
|
}
|
|
|
|
DualVector operator+(const DualVector & x) const
|
|
{
|
|
DualVector res(*this);
|
|
return (res += x);
|
|
}
|
|
|
|
DualVector operator+(void) const // +x
|
|
{
|
|
return (*this);
|
|
}
|
|
|
|
DualVector operator-(const DualVector & x) const {
|
|
DualVector res(*this);
|
|
return (res -= x);
|
|
}
|
|
|
|
DualVector operator-(void) const // -x
|
|
{
|
|
return DualVector(-a, -b);
|
|
}
|
|
|
|
DualVector operator*(const DualVector & x) const
|
|
{
|
|
DualVector res(*this);
|
|
return (res *= x);
|
|
}
|
|
|
|
DualVector operator/(const DualVector & x) const
|
|
{
|
|
DualVector res(*this);
|
|
return (res /= x);
|
|
}
|
|
|
|
Scalar a; /// Real part
|
|
VectorT b; /// Infinitesimal parts
|
|
};
|
|
|
|
// Basic mathematical functions for Scalar numbers
|
|
|
|
// Trigonometric functions
|
|
template<typename Scalar> Scalar sec(const Scalar & x) {
|
|
return Scalar(1.)/cos(x);
|
|
}
|
|
|
|
template<typename Scalar> Scalar cot(const Scalar & x) {
|
|
return cos(x)/sin(x);
|
|
}
|
|
|
|
template<typename Scalar> Scalar csc(const Scalar & x) {
|
|
return Scalar(1.)/sin(x);
|
|
}
|
|
|
|
// Inverse trigonometric functions
|
|
template<typename Scalar> Scalar asec(const Scalar & x) {
|
|
return acos(Scalar(1.)/x);
|
|
}
|
|
|
|
template<typename Scalar> Scalar acot(const Scalar & x) {
|
|
return atan(Scalar(1.)/x);
|
|
}
|
|
|
|
template<typename Scalar> Scalar acsc(const Scalar & x) {
|
|
return asin(Scalar(1.)/x);
|
|
}
|
|
|
|
// Hyperbolic trigonometric functions
|
|
template<typename Scalar> Scalar sech(const Scalar & x) {
|
|
return Scalar(1.)/cosh(x);
|
|
}
|
|
|
|
template<typename Scalar> Scalar coth(const Scalar & x) {
|
|
return cosh(x)/sinh(x);
|
|
}
|
|
|
|
template<typename Scalar> Scalar csch(const Scalar & x) {
|
|
return Scalar(1.)/sinh(x);
|
|
}
|
|
|
|
// Inverse hyperbolic trigonometric functions
|
|
template<typename Scalar> Scalar asech(const Scalar & x) {
|
|
return log((Scalar(1.) + sqrt(Scalar(1.) - x*x))/x);
|
|
}
|
|
|
|
template<typename Scalar> Scalar acoth(const Scalar & x) {
|
|
return Scalar(0.5)*log((x + Scalar(1.))/(x - Scalar(1.)));
|
|
}
|
|
|
|
template<typename Scalar> Scalar acsch(const Scalar & x) {
|
|
return (x >= Scalar(0.)) ? log((Scalar(1.) + sqrt(Scalar(1.) + x*x))/x) : log((Scalar(1.) - sqrt(Scalar(1.) + x*x))/x);
|
|
}
|
|
|
|
template<typename Scalar> Scalar exp10(const Scalar & x) {
|
|
return exp(x*log(Scalar(10.)));
|
|
}
|
|
|
|
template<typename Scalar> Scalar sign(const Scalar & x) {
|
|
return (x >= Scalar(0.)) ? ((x > Scalar(0.)) ? Scalar(1.) : Scalar(0.)) : Scalar(-1.);
|
|
}
|
|
|
|
template<typename Scalar> Scalar heaviside(const Scalar & x) {
|
|
return Scalar(x >= Scalar(0.));
|
|
}
|
|
|
|
// Basic mathematical functions for DualVector numbers
|
|
// f(a + b*d) = f(a) + b*f'(a)*d
|
|
|
|
// Trigonometric functions
|
|
template<typename Scalar, int N> DualVector<Scalar, N> cos(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(cos(x.a), -x.b*sin(x.a));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> sin(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(sin(x.a), x.b*cos(x.a));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> tan(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(tan(x.a), x.b*sec(x.a)*sec(x.a));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> sec(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(sec(x.a), x.b*sec(x.a)*tan(x.a));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> cot(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(cot(x.a), x.b*(-csc(x.a)*csc(x.a)));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> csc(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(csc(x.a), x.b*(-cot(x.a)*csc(x.a)));
|
|
}
|
|
|
|
// Inverse trigonometric functions
|
|
template<typename Scalar, int N> DualVector<Scalar, N> acos(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(acos(x.a), x.b*(-Scalar(1.)/sqrt(Scalar(1.)-x.a*x.a)));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> asin(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(asin(x.a), x.b*(Scalar(1.)/sqrt(Scalar(1.)-x.a*x.a)));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> atan(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(atan(x.a), x.b*(Scalar(1.)/(x.a*x.a + Scalar(1.))));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> asec(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(asec(x.a), x.b*(Scalar(1.)/(sqrt(Scalar(1.)-Scalar(1.)/(x.a*x.a))*(x.a*x.a))));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> acot(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(acot(x.a), x.b*(-Scalar(1.)/((x.a*x.a)+Scalar(1.))));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> acsc(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(acsc(x.a), x.b*(-Scalar(1.)/(sqrt(Scalar(1.)-Scalar(1.)/(x.a*x.a))*(x.a*x.a))));
|
|
}
|
|
|
|
// Hyperbolic trigonometric functions
|
|
template<typename Scalar, int N> DualVector<Scalar, N> cosh(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(cosh(x.a), x.b*sinh(x.a));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> sinh(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(sinh(x.a), x.b*cosh(x.a));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> tanh(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(tanh(x.a), x.b*sech(x.a)*sech(x.a));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> sech(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(sech(x.a), x.b*(-sech(x.a)*tanh(x.a)));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> coth(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(coth(x.a), x.b*(-csch(x.a)*csch(x.a)));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> csch(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(csch(x.a), x.b*(-coth(x.a)*csch(x.a)));
|
|
}
|
|
|
|
// Inverse hyperbolic trigonometric functions
|
|
template<typename Scalar, int N> DualVector<Scalar, N> acosh(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(acosh(x.a), x.b*(Scalar(1.)/sqrt((x.a*x.a)-Scalar(1.))));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> asinh(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(asinh(x.a), x.b*(Scalar(1.)/sqrt((x.a*x.a)+Scalar(1.))));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> atanh(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(atanh(x.a), x.b*(Scalar(1.)/(Scalar(1.)-(x.a*x.a))));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> asech(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(asech(x.a), x.b*(Scalar(-1.)/(sqrt(Scalar(1.)/(x.a*x.a)-Scalar(1.))*(x.a*x.a))));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> acoth(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(acoth(x.a), x.b*(-Scalar(1.)/((x.a*x.a)-Scalar(1.))));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> acsch(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(acsch(x.a), x.b*(-Scalar(1.)/(sqrt(Scalar(1.)/(x.a*x.a)+Scalar(1.))*(x.a*x.a))));
|
|
}
|
|
|
|
// Exponential functions
|
|
template<typename Scalar, int N> DualVector<Scalar, N> exp(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(exp(x.a), x.b*exp(x.a));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> log(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(log(x.a), x.b/x.a);
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> exp10(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(exp10(x.a), x.b*(log(Scalar(10.))*exp10(x.a)));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> log10(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(log10(x.a), x.b/(log(Scalar(10.))*x.a));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> exp2(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(exp2(x.a), x.b*(log(Scalar(2.))*exp2(x.a)));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> log2(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(log2(x.a), x.b/(log(Scalar(2.))*x.a));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> pow(const DualVector<Scalar, N> & x, const DualVector<Scalar, N> & n) {
|
|
return exp(n*log(x));
|
|
}
|
|
|
|
// Other functions
|
|
template<typename Scalar, int N> DualVector<Scalar, N> sqrt(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(sqrt(x.a), x.b/(Scalar(2.)*sqrt(x.a)));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> sign(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(sign(x.a), DualVector<Scalar, N>::VectorT::Zero());
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> abs(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(fabs(x.a), x.b*sign(x.a));
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> heaviside(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(heaviside(x.a), DualVector<Scalar, N>::VectorT::Zero());
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> floor(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(floor(x.a), DualVector<Scalar, N>::VectorT::Zero());
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> ceil(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(ceil(x.a), DualVector<Scalar, N>::VectorT::Zero());
|
|
}
|
|
|
|
template<typename Scalar, int N> DualVector<Scalar, N> round(const DualVector<Scalar, N> & x) {
|
|
return DualVector<Scalar, N>(round(x.a), DualVector<Scalar, N>::VectorT::Zero());
|
|
}
|
|
|
|
template<typename Scalar, int N> std::ostream & operator<<(std::ostream & s, const DualVector<Scalar, N> & x)
|
|
{
|
|
return (s << x.a);
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|