momentum_math.h

/*
Copyright 2007-2025. Algoryx Simulation AB.
 
All AGX source code, intellectual property, documentation, sample code,
tutorials, scene files and technical white papers, are copyrighted, proprietary
and confidential material of Algoryx Simulation AB. You may not download, read,
store, distribute, publish, copy or otherwise disseminate, use or expose this
material without having a written signed agreement with Algoryx Simulation AB.
 
Algoryx Simulation AB disclaims all responsibilities for loss or damage caused
from using this software, unless otherwise stated in written agreements with
Algoryx Simulation AB.
*/
 
#ifndef MOMENTUM_MATH_H
#define MOMENTUM_MATH_H
 
#include "momentum_namespace.h"
#include "momentum_export.h"
 
#include "macros.h"
 
#include <agx/Math.h>
 
namespace MOMENTUM_NAMESPACE
{
  // physics related constants
  const double GRAVITY_ACCELERATION = double(9.80665); // in m/s2, SI standard acceleration due to gravity
 
  const double MOMENTUM_EQUIVALENT_EPSILON = (double)1E-9;
 
  const double Infinity   = std::numeric_limits<double>::infinity();
 
  const double REAL_SQRT_EPSILON = std::sqrt(std::numeric_limits< double >::epsilon());
 
 
  const double PI = double(M_PI);
  const double PI_2 = double(M_PI_2);
  const double PI_4 = double(M_PI_4);
 
  template<typename T>
  inline bool _equivalent( T lhs, T rhs, T epsilon = T(MOMENTUM_EQUIVALENT_EPSILON) )
  {
    return (lhs + epsilon >= rhs) && (lhs - epsilon <= rhs);
  }
 
  inline bool equivalent(double lhs, double rhs, double epsilon = (double)MOMENTUM_EQUIVALENT_EPSILON)
  {
    return _equivalent< double >(lhs, rhs, epsilon);
  }
 
 
  template<typename T>
  inline bool _relativelyEquivalent( T lhs, T rhs, T relativeEpsilon = T(MOMENTUM_EQUIVALENT_EPSILON) )
  {
    return _equivalent(lhs, rhs, (std::abs(lhs) + std::abs(rhs) + T(1)) * relativeEpsilon);
  }
 
  inline bool relativelyEquivalent(double lhs, double rhs, double epsilon = (double)MOMENTUM_EQUIVALENT_EPSILON)
  {
    return _relativelyEquivalent< double >(lhs, rhs, epsilon);
  }
 
  inline bool equalsZero( double d, double eps=DBL_EPSILON )
  {
    return ( d < eps && d > -eps );
  }
 
 
  template<typename T1, typename T2>
  inline T1 const lerp(T1 const& a, T1 const& b, T2 s)
  {
    if (s > 1)
      return b;
    if (s <=0)
      return a;
 
    return (T1)(a*(1-s)  + b*s);
  }
 
  template<typename T>
  inline T const logInterpolate(T const& a, T const& b, float s)
  {
    return std::pow(T(10), agx::lerp(std::log10(a), std::log10(b), s));
  }
 
} // namespace momentum
 
#endif  // MOMENTUM_MATH_H