multidual.h

Go to the documentation of this file.

#ifndef THEORETICA_MULTIDUAL_H
#define THEORETICA_MULTIDUAL_H
 
 
 
#ifndef THEORETICA_NO_PRINT
#include <sstream>
#include <ostream>
#endif
 
#include "../algebra/vec.h"
#include "../algebra/mat.h"
 
 
namespace theoretica {
 
    template<unsigned int N = 0>
    class multidual {
        
        public:
            
            real a;
 
            vec<real, N> v;
 
            // The template argument of the vector type used
            static constexpr size_t size_argument = N;
 
 
            multidual() : a(0) {}
 
 
            multidual(real r, vec<real, N> u) : a(r), v(u) {}
 
 
            multidual(real r) : a(r), v(vec<real, N>()) {}
 
            ~multidual() = default;
 
 
            inline multidual& operator=(real x) {
                a = x;
                v = vec<real, N>();
                return *this;
            }
 
 
            inline real Re() const {
                return a;
            }
 
 
            inline real& Re() {
                return a;
            }
 
 
            inline vec<real, N> Dual() const {
                return v;
            }
 
 
            inline vec<real, N>& Dual() {
                return v;
            }
 
 
            inline real Dual(unsigned int i) const {
                return v[i];
            }
 
 
            inline real& Dual(unsigned int i) {
                return v[i];
            }
 
 
            inline multidual conjugate() const {
                return multidual(a, -v);
            }
 
 
            inline multidual inverse() const {
 
                if(a == 0) {
                    TH_MATH_ERROR("multidual::inverse", 0, MathError::DivByZero);
                    return multidual(nan(), vec<real, N>(N, nan()));
                }
 
                return multidual(1.0 / a, v * (-1 / (a * a)));
            }
 
 
            inline multidual operator+() const {
                return multidual(a, v);
            }
 
 
            inline multidual operator+(const multidual& other) const {
                return multidual(a + other.a, v + other.v);
            }
 
 
            inline multidual operator+(real r) const {
                return multidual(a + r, v);
            }
 
 
            inline multidual operator-() const {
                return multidual(-a, -v);
            }
 
 
            inline multidual operator-(const multidual& other) const {
                return multidual(a - other.a, v - other.v);
            }
 
 
            inline multidual operator-(real r) const {
                return multidual(a - r, v);
            }
 
 
            inline multidual operator*(const multidual& other) const {
 
                // Equal sizes
                if (v.size() == other.v.size()) {
                    return multidual(a * other.a, other.v * a + v * other.a);
                }
 
                // Trivial scalar cases
                if (v.size() == 0) {
                    return multidual(a * other.a, other.v * a);
                }
 
                if (other.v.size() == 0) {
                    return multidual(a * other.a, v * other.a);
                }
 
                // Treat missing entries as zero
                const unsigned int new_size = th::max(v.size(), other.v.size());
                vec<real, N> res_v;
                res_v.resize(new_size);
 
                for (unsigned int i = 0; i < res_v.size(); ++i) {
                    real lhs = (i < other.v.size()) ? other.v[i] : 0.0;
                    real rhs = (i < v.size()) ? v[i] : 0.0;
                    res_v[i] = lhs * a + rhs * other.a;
                }
 
                return multidual(a * other.a, res_v);
            }
 
 
            inline multidual operator*(real r) const {
                return multidual(a * r, v * r);
            }
 
 
            inline multidual operator/(const multidual& other) const {
 
                if(abs(a) < MACH_EPSILON) {
                    TH_MATH_ERROR("multidual::operator/", 0, MathError::DivByZero);
                    return multidual(nan(), vec<real, N>(N, nan()));
                }
 
                const real res_a = a / other.a;
                const real denom = other.a * other.a;
 
                // Equal sizes
                if (v.size() == other.v.size()) {
                    return multidual(res_a, (v * other.a - other.v * a) / denom);
                }
 
                // Trivial scalar cases
                if (v.size() == 0) {
                    return multidual(res_a, other.v * (-a / denom));
                }
 
                if (other.v.size() == 0) {
                    return multidual(res_a, v / other.a);
                }
 
                // Treat missing entries as zero
                const unsigned int new_size = th::max(v.size(), other.v.size());
                vec<real, N> res_v;
                res_v.resize(new_size);
 
                for (unsigned int i = 0; i < res_v.size(); ++i) {
 
                    real lhs = (i < v.size()) ? v[i] : 0.0;
                    real rhs = (i < other.v.size()) ? other.v[i] : 0.0;
 
                    res_v[i] = (lhs * other.a - rhs * a) / denom;
                }
 
                return multidual(res_a, res_v);
            }
 
            inline multidual operator/(real r) const {
                return multidual(a / r, v / r);
            }
 
 
            inline multidual& operator+=(const multidual& other) {
 
                a += other.a;
                v += other.v;
                return *this;
            }
 
 
            inline multidual& operator+=(real r) {
 
                a += r;
                return *this;
            }
 
            inline multidual& operator-=(const multidual& other) {
 
                a -= other.a;
                v -= other.v;
                return *this;
            }
 
            inline multidual& operator-=(real r) {
 
                a -= r;
                return *this;
            }
 
            inline multidual& operator*=(const multidual& other) {
 
                const real a_old = a;
 
                // Equal sizes
                if (v.size() == other.v.size()) {
                    a = a_old * other.a;
                    v = other.v * a_old + v * other.a;
                    return *this;
                }
 
                // Trivial scalar cases
                if (v.size() == 0) {
                    a = a_old * other.a;
                    v = other.v * a_old;
                    return *this;
                }
 
                if (other.v.size() == 0) {
                    a = a_old * other.a;
                    v = v * other.a;
                    return *this;
                }
 
                // Treat missing entries as zero
                const unsigned int new_size = th::max(v.size(), other.v.size());
                vec<real, N> new_v;
                new_v.resize(new_size);
 
                for (unsigned int i = 0; i < new_v.size(); ++i) {
 
                    real lhs = (i < other.v.size()) ? other.v[i] : 0.0;
                    real rhs = (i < v.size()) ? v[i] : 0.0;
 
                    new_v[i] = lhs * a_old + rhs * other.a;
                }
 
                a = a_old * other.a;
                v = new_v;
 
                return *this;
            }
 
            inline multidual& operator*=(real r) {
 
                a *= r;
                v *= r;
                return *this;
            }
 
            inline multidual& operator/=(const multidual& other) {
 
                const real a_old = a;
                const real denom = other.a * other.a;
 
                // Equal sizes
                if (v.size() == other.v.size()) {
                    a = a_old / other.a;
                    v = (v * other.a - other.v * a_old) / denom;
                    return *this;
                }
 
                // Trivial scalar cases
                if (v.size() == 0) {
                    a = a_old / other.a;
                    v = other.v * (-a_old / denom);
                    return *this;
                }
 
                if (other.v.size() == 0) {
                    a = a_old / other.a;
                    v = v / other.a;
                    return *this;
                }
 
                // Treat missing entries as zero
                const unsigned int new_size = th::max(v.size(), other.v.size());
                vec<real, N> new_v;
                new_v.resize(new_size);
 
                for (unsigned int i = 0; i < new_v.size(); ++i) {
 
                    real lhs = (i < v.size()) ? v[i] : 0.0;
                    real rhs = (i < other.v.size()) ? other.v[i] : 0.0;
 
                    new_v[i] = (lhs * other.a - rhs * a_old) / denom;
                }
 
                a = a_old / other.a;
                v = new_v;
 
                return *this;
            }
 
            inline multidual& operator/=(real r) {
 
                if(r == 0) {
                    TH_MATH_ERROR("multidual::operator/=", 0, MathError::DivByZero);
                    a = nan();
                    v = vec<real, N>(N, nan());
                    return *this;
                }
 
                a /= r;
                v /= r;
 
                return *this;
            }
 
 
            inline bool operator==(const multidual& other) {
                return (a == other.a) && (v == other.v);
            }
 
 
            template<typename Vector = vec<real, N>, enable_vector<Vector> = true>
            inline static vec<multidual<N>, N> make_argument(const Vector& x) {
 
                vec<multidual<N>, N> arg;
                arg.resize(x.size());
 
                for (unsigned int i = 0; i < x.size(); ++i)
                    arg[i] = multidual<N>(
                        x[i], vec<real, N>::euclidean_base(i, x.size())
                    );
 
                return arg;
            }
 
 
            inline static vec<real, N> extract_real(
                const vec<multidual<N>, N>& v) {
 
                vec<real, N> x;
                x.resize(v.size());
 
                for (unsigned int i = 0; i < N; ++i)
                    x[i] = v[i].Re();
 
                return x;
            }
 
 
            inline static mat<real, N, N> extract_dual(
                const vec<multidual<N>, N>& v) {
 
                mat<real, N, N> J;
                J.resize(v.size(), v.size());
 
                for (unsigned int i = 0; i < N; ++i)
                    for (unsigned int j = 0; j < N; ++j)
                        J(j, i) = v[j].Dual(i);
                
                return J;
            }
 
 
            inline static void extract(
                const vec<multidual<N>, N>& v,
                vec<real, N>& x,
                mat<real, N, N>& J) {
 
                for (unsigned int i = 0; i < N; ++i) {
                    
                    for (unsigned int j = 0; j < N; ++j)
                        J(j, i) = v[j].Dual(i);
                    
                    x[i] = v[i].Re();
                }
            }
 
 
            unsigned int size() const {
                return v.size();
            }
 
 
            inline void resize(unsigned int size) {
                v.resize(size);
            }
 
 
            // Friend operators to enable equations of the form
            // (real) op. (multidual)
            
            inline friend multidual operator+(real a, const multidual& d) {
                return d + a;
            }
 
            inline friend multidual operator-(real a, const multidual& d) {
                return -d + a;
            }
 
            inline friend multidual operator*(real a, const multidual& d) {
                return d * a;
            }
 
            inline friend multidual operator/(real a, const multidual& d) {
                return a * d.inverse();
            }
 
 
#ifndef THEORETICA_NO_PRINT
 
            inline std::string to_string(const std::string& epsilon = "ε") const {
 
                std::stringstream res;
                res << a << " + " << v << epsilon;
 
                return res.str();
            }
 
 
            inline operator std::string() {
                return to_string();
            }
 
 
            inline friend std::ostream& operator<<(std::ostream& out, const multidual& obj) {
                return out << obj.to_string();
            }
 
#endif
        
    };
 
}
 
#endif