MathUtils.h

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package    ArmarX::Core
* @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
* @date       2015
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
#include <math.h>
 
#include <vector>
 
#include <Eigen/Core>
 
namespace armarx::math
{
    class MathUtils
    {
    public:
        static int
        Sign(double value)
        {
            return value >= 0 ? 1 : -1;
        }
 
        static int
        LimitMinMax(int min, int max, int value)
        {
            return value < min ? min : (value > max ? max : value);
        }
 
        static float
        LimitMinMax(float min, float max, float value)
        {
            return value < min ? min : (value > max ? max : value);
        }
 
        static double
        LimitMinMax(double min, double max, double value)
        {
            return value < min ? min : (value > max ? max : value);
        }
 
        static Eigen::Vector3f
        LimitMinMax(const Eigen::Vector3f& min,
                    const Eigen::Vector3f& max,
                    const Eigen::Vector3f& value)
        {
            return Eigen::Vector3f(LimitMinMax(min(0), max(0), value(0)),
                                   LimitMinMax(min(1), max(1), value(1)),
                                   LimitMinMax(min(2), max(2), value(2)));
        }
 
        static double
        LimitTo(double value, double absThreshold)
        {
            return LimitMinMax(-absThreshold, absThreshold, value);
            //int sign = (value >= 0) ? 1 : -1;
            //return sign * std::min<double>(fabs(value), absThreshold);
        }
 
        static Eigen::Vector3f
        LimitTo(const Eigen::Vector3f& val, float maxNorm)
        {
            float norm = val.norm();
            if (norm > maxNorm)
            {
                return val / norm * maxNorm;
            }
            return val;
        }
 
        static bool
        CheckMinMax(int min, int max, int value)
        {
            return value >= min && value <= max;
        }
 
        static bool
        CheckMinMax(float min, float max, float value)
        {
            return value >= min && value <= max;
        }
 
        static bool
        CheckMinMax(double min, double max, double value)
        {
            return value >= min && value <= max;
        }
 
        static bool
        CheckMinMax(const Eigen::Vector3f& min,
                    const Eigen::Vector3f& max,
                    const Eigen::Vector3f& value)
        {
            return CheckMinMax(min(0), max(0), value(0)) && CheckMinMax(min(1), max(1), value(1)) &&
                   CheckMinMax(min(2), max(2), value(2));
        }
 
        static std::vector<float>
        VectorSubtract(const std::vector<float>& v1, const std::vector<float>& v2)
        {
            std::vector<float> result;
 
            for (size_t i = 0; i < v1.size() && i < v2.size(); i++)
            {
                result.push_back(v1.at(i) - v2.at(i));
            }
 
            return result;
        }
 
        static std::vector<float>
        VectorAbsDiff(const std::vector<float>& v1, const std::vector<float>& v2)
        {
            std::vector<float> result;
 
            for (size_t i = 0; i < v1.size() && i < v2.size(); i++)
            {
                result.push_back(std::fabs(v1.at(i) - v2.at(i)));
            }
 
            return result;
        }
 
        static float
        VectorMax(const std::vector<float>& vec)
        {
            float max = vec.at(0);
 
            for (size_t i = 1; i < vec.size(); i++)
            {
                max = std::max(max, vec.at(i));
            }
 
            return max;
        }
 
        static float
        fmod(float value, float boundLow, float boundHigh)
        {
            value = std::fmod(value - boundLow, boundHigh - boundLow) + boundLow;
            if (value < boundLow)
            {
                value += boundHigh - boundLow;
            }
            return value;
        }
 
        static float
        angleMod2PI(float value)
        {
            return fmod(value, 0, 2 * M_PI);
        }
 
        static float
        angleModPI(float value)
        {
            return angleMod2PI(value + M_PI) - M_PI;
        }
 
        static float
        angleModX(float value, float center)
        {
            return angleMod2PI(value + M_PI - center) - M_PI + center;
        }
 
        static float
        Lerp(float a, float b, float f)
        {
            return a * (1 - f) + b * f;
        }
 
        static float
        LerpClamp(float a, float b, float f)
        {
            return Lerp(a, b, LimitMinMax(0.f, 1.f, f));
        }
 
        static float
        ILerp(float a, float b, float f)
        {
            return (f - a) / (b - a);
        }
 
        static float
        ILerpClamp(float a, float b, float f)
        {
            return LimitMinMax(0.f, 1.f, ILerp(a, b, f));
        }
 
        static float
        AngleLerp(float a, float b, float f)
        {
            b = fmod(b, a - M_PI, a + M_PI);
            return Lerp(a, b, f);
        }
 
        static float
        AngleDelta(float angle1, float angle2)
        {
            return angleModPI(angle2 - angle1);
        }
    };
} // namespace armarx::math