MPUtils.h

Go to the documentation of this file.

#ifndef PMPL_MP_UTILS_H_
#define PMPL_MP_UTILS_H_
 
#include <functional>
#include <iostream>
#include <limits>
#include <map>
#include <memory>
#include <string>
 
#ifndef _PARALLEL
#include <algorithm> // Apparently STAPL missed this in the file below.
#include <containers/sequential/graph/algorithms/connected_components.h>
#endif
 
#include "Vector.h"
using namespace mathtool;
 
#include "IOUtils.h"
 
#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/factorials.hpp>
 
class Cfg;
class Robot;
 
 
/*-------------------------------- Constants ---------------------------------*/
 
#define MAX_INT  std::numeric_limits<int>::max()
#define MAX_DBL  std::numeric_limits<double>::max()
 
template<typename T>
const T
Epsilon(const T& _t1, const T& _t2) {
  static constexpr T tenEpsilon = T(10) * std::numeric_limits<T>::epsilon();
  return std::min(std::abs(_t1), std::abs(_t2)) * tenEpsilon;
}
 
/*------------------------- Random Number Generation -------------------------*/
 
double DRand();
 
long LRand();
 
long MRand();
 
double GRand(bool _reset = false);
 
double GaussianDistribution(double _mean, double _stdev);
 
void SRand(const unsigned long _seed);
 
/*------------------------------ Geometry Utils ------------------------------*/
 
double Normalize(const double& _a);
 
double TriangleHeight(const Point3d& _a, const Point3d& _b, const Point3d& _c);
 
bool PtInTriangle(const Point2d& _a, const Point2d& _b, const Point2d& _c,
    const Point2d & _p);
 
bool PtInTriangle(const Point2d& _a, const Point2d& _b, const Point2d& _c,
    const Point2d& _p, double& _u, double& _v);
 
Point3d GetPtFromBarycentricCoords(const Point3d& _a, const Point3d& _b,
    const Point3d& _c, double _u, double _v);
 
double NormalizeTheta(double _theta);
 
/*---------------------------- Comparators -----------------------------------*/
 
template <typename T, typename U>
struct CompareSecond {
 
  const bool operator()(const std::pair<T, U>& _a, const std::pair<T, U>& _b)
      const noexcept {
    return _a.second < _b.second;
  }
 
};
 
template <typename T, typename U>
struct CompareSecondReverse {
 
  const bool operator()(const std::pair<T, U>& _a, const std::pair<T, U>& _b)
      const noexcept {
    return _a.second > _b.second;
  }
 
};
 
/*------------------------------ Cfg Utilities -------------------------------*/
 
template<class CfgType, class Environment>
bool
IsWithinResolution(const CfgType& _cfg1, const CfgType& _cfg2,
    Environment* _env) {
  CfgType diff = _cfg1 - _cfg2;
  return diff->PositionMagnitude() <= _env->GetPositionRes()
      && diff->OrientationMagnitude() <= _env->GetOrientationRes();
}
 
 
template<class CfgType>
CfgType
ClosestPtOnLineSegment(const CfgType& _ref, const CfgType& _p1,
    const CfgType& _p2) {
  CfgType b = _p2 - _p1;
  CfgType c = _ref - _p1;
 
  double bDotC = 0;
  double bSquared = 0;
 
  for(auto itb = b.GetData().begin(), itc = c.GetData().begin();
      itb != b.GetData().end(); ++itb, ++itc) {
    bDotC += (*itb) * (*itc);
    bSquared += (*itb) * (*itb);
  }
 
  if(bDotC <= 0)
    return _p1;
  else if(bDotC >= bSquared)
    return _p2;
  else
    return b * (bDotC / bSquared) + _p1;
}
 
/*---------------------------- Statistical p-tests --------------------------*/
 
bool
binom_test(double _alpha, size_t _n, size_t _k, double _p);
 
 
/*---------------------------- Distributions --------------------------*/
 
double
binomial(size_t _n, size_t _k, double _p);
 
/*----------------------------- Other Random Stuff ---------------------------*/
 
struct NullOutputIterator : std::iterator<std::output_iterator_tag,
    NullOutputIterator> {
 
  template<typename T>
  void operator=(const T&) { }
 
  NullOutputIterator& operator++() {return *this;}
  NullOutputIterator operator++(int) {return *this;}
  NullOutputIterator& operator*() {return *this;}
 
};
 
 
template <class RandomIterator, class T, class Compare = std::less<T>>
RandomIterator
BinarySearch(RandomIterator _begin, RandomIterator _end, const T& _value,
    Compare _comparator = Compare()) {
  auto start = _begin;
  auto end = _end;
 
  while(start != end) {
    const size_t mid = (size_t) (end - start) / 2;
    RandomIterator middle = start;
    std::advance(middle, mid);
 
    if(_value == *middle)
      return middle;
    if(_comparator(_value, *middle))
      end = middle;
    else
      start = middle;
  }
  return _end;
}
 
std::vector<Cfg> LoadPath(const std::string &_filename, Robot* _robot);
 
 
template <typename T>
T
Identity(const T& _t) noexcept {
  return _t;
}
 
/*----------------------------------------------------------------------------*/
 
 
#endif