miscellaneous-algorithms.hpp

Go to the documentation of this file.

 
#ifndef STATEOBSERVATIONTOOLSMISCELANEOUSALGORITHMS
#define STATEOBSERVATIONTOOLSMISCELANEOUSALGORITHMS
#include <boost/utility.hpp>
#include <cmath>
 
#include <state-observation/api.h>
#include <state-observation/tools/definitions.hpp>
 
namespace stateObservation
{
namespace tools
{

template<class T>
inline T square(const T & x)
{
  return T(x * x);
}

template<class T>
inline T derivate(const T & o1, const T & o2, double dt)
{
  T o(o2 - o1);
  return o * (1 / dt);
}

template<typename T>
inline int signum(T x)
{
  return (T(0) < x) - (x < T(0));
}
 
template<typename T>
inline std::string toString(T val)
{
  std::stringstream ss("");
  ss << val;
  return ss.str();
}

inline bool checkIfNormalized(const Vector3 & v)
{
  if(fabs(v.squaredNorm() - 1) > cst::epsilon1)
  {
    return false;
  }
  else
  {
    return true;
  }
}

inline bool checkIfNormalized(const Vector3 & v, double & outputSquaredNorm)
{
  outputSquaredNorm = v.squaredNorm();
  if(fabs(outputSquaredNorm - 1) > cst::epsilon1)
  {
    return false;
  }
  else
  {
    return true;
  }
}

template<typename T>
inline T clampScalar(const T & x, const T & max, const T & min)
{
  if(x > max)
  {
    return max;
  }
  else
  {
    if(x < min)
    {
      return min;
    }
    else
    {
      return x;
    }
  }
}

template<typename T>
inline T clampScalar(const T & x, const T & limit)
{
  return clampScalar(x, limit, -limit);
}

inline Vector3 normalizedLazy(const Vector3 & v)
{
  double squaredNorm;
  if(checkIfNormalized(v, squaredNorm))
  {
    return v;
  }
  else
  {
    return v / sqrt(squaredNorm);
  }
}

inline double STATE_OBSERVATION_DLLAPI finiteTimeAccControl(double x, double xd, double kp = -1, double kv = -1)
{
  double sax = sqrt(fabs(x));
  double xdr = kp * signum(x) * sax;
  double y = xd - xdr;
  double ydr = -kv * signum(y) * sqrt(fabs(y));
  return ydr - kp * xd / (2 * sax);
}

inline Vector STATE_OBSERVATION_DLLAPI finiteTimeAccControl(const Vector & x,
                                                            const Vector & xd,
                                                            double kp = -1,
                                                            double kv = -1)
{
  Vector xdd(x.size());
  for(Index i = 1; i < x.size(); ++i)
  {
    xdd(i) = finiteTimeAccControl(x(i), xd(i), kp, kv);
  }
  return xdd;
}
 
namespace Detail
{
double constexpr sqrtNewtonRaphson(double x, double curr, double prev)
{
  return curr == prev ? curr : sqrtNewtonRaphson(x, 0.5 * (curr + x / curr), curr);
}
} // namespace Detail

double constexpr sqrt(double x)
{
  return x >= 0 && x < std::numeric_limits<double>::infinity() ? Detail::sqrtNewtonRaphson(x, x, 0)
                                                               : std::numeric_limits<double>::quiet_NaN();
}
 
} // namespace tools
 
} // namespace stateObservation
 
#endif // STATEOBSERVATIONTOOLSMISCELANEOUSALGORITHMS