Solver.h

Go to the documentation of this file.

#pragma once
#ifndef __SOLVER_H
#define __SOLVER_H

#include <osg/Vec2d>
#include <osg/Vec3d>
#include <osg/Quat>

#include <list>

namespace ht
{

    /*************************************************************
    **  Variable - base class to all variables providing
    **    numerical calculation of their partial derivatives
    **    To calculate parital derivative of a function, set
    **    delta to value of step 'h' of numerical approximation
    **    using function SetDelta. GetValueInFunction should now
    **    return value of this variable after adding the delta.
    **    To add delta to the variable itself, call AddDelta.
    *************************************************************/
    class Variable
    {
    public:
        virtual ~Variable() {};

        virtual int GetVariablesCount() = 0;        // Returns number of derivable subvariables
        virtual void SetDelta(int variable, double delta) = 0;      // Variable starts behaving like "var + delta" in functions, but its value itself does not change
        virtual void AddDelta(int variable, double delta) = 0;      // Adds delta to variable
        virtual void ResetDelta() = 0;                              // Resets delta to zero

        // VariableType GetValue();                 // Returns value of the variable
        // VariableType GetValueInFunction();       // Returns value of the variable in function
    };

    class VariableVec3: public Variable
    {
    private:
        osg::Vec3d  _value;
        osg::Vec3d  _delta;
    public:
        VariableVec3(): _value(0.0, 0.0, 0.0), _delta(0.0, 0.0, 0.0) {}
        VariableVec3(const VariableVec3 &v): _value(v._value), _delta(v._delta) {}

        VariableVec3& operator =(const VariableVec3 &v)
        {   _value = v._value;  _delta = v._delta;  return *this;   }

        virtual int GetVariablesCount() override
        {   return 3;   }
        virtual void SetDelta(int variable, double delta) override
        {
            switch (variable)
            {
            case 0:     _delta = osg::Vec3d(delta, 0.0, 0.0);       break;
            case 1:     _delta = osg::Vec3d(0.0, delta, 0.0);       break;
            case 2:     _delta = osg::Vec3d(0.0, 0.0, delta);       break;
            }
        }
        virtual void AddDelta(int variable, double delta) override
        {
            switch (variable)
            {
            case 0:     _value += osg::Vec3d(delta, 0.0, 0.0);      break;
            case 1:     _value += osg::Vec3d(0.0, delta, 0.0);      break;
            case 2:     _value += osg::Vec3d(0.0, 0.0, delta);      break;
            }
        }
        virtual void ResetDelta() override
        {
            _delta = osg::Vec3d(0.0, 0.0, 0.0);
        }

        void SetValue(osg::Vec3d v)     {   _value = v; }
        osg::Vec3d GetValue()           {   return _value;              }
        osg::Vec3d GetValueInFunction() {   return _value + _delta;     }
    };

    class VariableVec2: public Variable
    {
    private:
        osg::Vec2d  _value;
        osg::Vec2d  _delta;
    public:
        VariableVec2(): _value(0.0, 0.0), _delta(0.0, 0.0) {}
        VariableVec2(const VariableVec2 &v): _value(v._value), _delta(v._delta) {}

        VariableVec2& operator =(const VariableVec2 &v)
        {   _value = v._value;  _delta = v._delta;  return *this;   }

        virtual int GetVariablesCount() override
        {   return 2;   }
        virtual void SetDelta(int variable, double delta) override
        {
            switch (variable)
            {
            case 0:     _delta = osg::Vec2d(delta, 0.0);        break;
            case 1:     _delta = osg::Vec2d(0.0, delta);        break;
            }
        }
        virtual void AddDelta(int variable, double delta) override
        {
            switch (variable)
            {
            case 0:     _value += osg::Vec2d(delta, 0.0);       break;
            case 1:     _value += osg::Vec2d(0.0, delta);       break;
            }
        }
        virtual void ResetDelta() override
        {
            _delta = osg::Vec2d(0.0, 0.0);
        }

        void SetValue(osg::Vec2d v)     {   _value = v; }
        osg::Vec2d GetValue()           {   return _value;              }
        osg::Vec2d GetValueInFunction() {   return _value + _delta;     }
    };

    class VariableFloat: public Variable
    {
    private:
        double  _value;
        double  _delta;
    public:
        VariableFloat(): _value(0.0), _delta(0.0) {}
        VariableFloat(const VariableFloat &f): _value(f._value), _delta(f._delta) {}

        VariableFloat& operator =(const VariableFloat &f)
        {   _value = f._value;  _delta = f._delta;  return *this;   }

        virtual int GetVariablesCount() override
        {   return 1;   }
        virtual void SetDelta(int variable, double delta) override
        {
            switch (variable)
            {
            case 0:     _delta = delta;     break;
            }
        }
        virtual void AddDelta(int variable, double delta) override
        {
            switch (variable)
            {
            case 0:     _value += delta;        break;
            }
        }
        virtual void ResetDelta() override
        {
            _delta = 0.0;
        }

        void SetValue(double v)     {   _value = v; }
        double GetValue()           {   return _value;              }
        double GetValueInFunction() {   return _value + _delta;     }
    };

    class VariableQuat: public Variable
    {
    private:
        osg::Quat   _value;
        osg::Quat   _delta;
    public:
        VariableQuat(): _value(0.0, osg::Vec3d(1.0, 0.0, 0.0)), _delta(0.0, osg::Vec3d(1.0, 0.0, 0.0)) {}
        VariableQuat(const VariableQuat &v): _value(v._value), _delta(v._delta) {}

        VariableQuat& operator =(const VariableQuat &v)
        {   _value = v._value;  _delta = v._delta;  return *this;   }

        virtual int GetVariablesCount() override
        {   return 3;   }
        virtual void SetDelta(int variable, double delta) override
        {
            switch (variable)
            {
            case 0:     _delta = osg::Quat(delta, osg::Vec3d(1.0, 0.0, 0.0));       break;
            case 1:     _delta = osg::Quat(delta, osg::Vec3d(0.0, 1.0, 0.0));       break;
            case 2:     _delta = osg::Quat(delta, osg::Vec3d(0.0, 0.0, 1.0));       break;
            }
        }
        virtual void AddDelta(int variable, double delta) override
        {
            switch (variable)
            {
            case 0:     _value = _value * osg::Quat(delta, osg::Vec3d(1.0, 0.0, 0.0));  _value /= _value.length();      break;
            case 1:     _value = _value * osg::Quat(delta, osg::Vec3d(0.0, 1.0, 0.0));  _value /= _value.length();      break;
            case 2:     _value = _value * osg::Quat(delta, osg::Vec3d(0.0, 0.0, 1.0));  _value /= _value.length();      break;
            }
        }
        virtual void ResetDelta() override
        {
            _delta = osg::Quat(0.0, osg::Vec3d(1.0, 0.0, 0.0));
        }

        void SetValue(osg::Quat v)      {   _value = v; }
        osg::Quat GetValue()            {   return _value;              }
        osg::Quat GetValueInFunction()  {   return _value * _delta;     }
    };

    /*********************************************
    **  Function - base class for any function.
    **    The most important method is Evaluate,
    **    which returns value of the Function
    **    (including valud of delta).
    *********************************************/
    class Function
    {
    protected:
        // Returns distance of point v from line (l1,l2)
        double _dist_line(osg::Vec3d v, osg::Vec3d l1, osg::Vec3d l2)
        {
            // According to WolframAlpha, distance of point 'v' from line 'l1 l2' is
            // d = |(v-l1)cross(v-l2)| / |l1-l2|
            double denom = (l1-l2).length();
            double numer = ((v-l1) ^ (v-l2)).length();
            if (denom < 1e-9)       return (v-l2).length();
            if (numer < 1e-9)       return 0.00;
            return numer / denom;
        }
        double _dist_line2(osg::Vec3d v, osg::Vec3d l1, osg::Vec3d l2)
        {
            return _dist_line(v, l1, l2) * _dist_line(v, l1, l2);
        }
    public:
        virtual double Evaluate() = 0;
    };

    typedef std::list<Function *> ListOfFunctions;
    typedef std::list<Variable *> ListOfVariables;

    // Finds a minimum of the sum of functions in list of functions 'fcns'
    //   changing value of variables in list 'vars'
    void solve(ListOfFunctions &fcns, ListOfVariables &vars, double start_step = 0.01, double next_step_scale = 0.8, double min_step_size = 1e-6);

}

#endif  // __SOLVER_H