MathHelper.hpp

Go to the documentation of this file.

#pragma once

#include <Eigen/Geometry>
#include <cmath>
#include "common_type/Pose.hpp"
#include "common_type/Quaternion.hpp"
#include "common_type/VoteSpecifier.hpp"
#include "common_type/Point.hpp"

namespace ISM {
  class MathHelper {
  public:

    static bool getNextCombination(const std::vector<unsigned int>::iterator first, std::vector<unsigned int>::iterator k, const std::vector<unsigned int>::iterator last)
    {
      if ((first == last) || (first == k) || (last == k))
        return false;

      std::vector<unsigned int>::iterator it1 = first;
      std::vector<unsigned int>::iterator it2 = last;
      it1++;
      if (last == it1)
        return false;
      it1 = last;
      it1--;
      it1 = k;
      it2--;
      while (first != it1)
        {
          if (*--it1 < *it2)
            {
              std::vector<unsigned int>::iterator j = k;
              while (!(*it1 < *j)) ++j;
              std::iter_swap(it1,j);
              it1++;
              j++;
              it2 = k;
              std::rotate(it1,j,last);
              while (last != j)
                {
                  j++;
                  it2++;
                }
              std::rotate(k,it2,last);
              return true;
            }
        }
      std::rotate(first,k,last);
      return false;
    }

  };

}