math.h

Go to the documentation of this file.

#pragma once

#define _USE_MATH_DEFINES
#include <cmath>
#include <stdexcept>
#include <tf/tf.h>

namespace math {

double angleWrap(double x);

double clamp(double x, double min, double max);

template <class T> tf::Transform getTransformFromVector(const T &input) {
  tf::Transform transform;
  if (input.size() != 6) {
    throw std::runtime_error("The input does not have 6 elements x,y,z, r,p,y");
  } else {
    transform.setOrigin(tf::Vector3(input[0], input[1], input[2]));
    transform.setRotation(
        tf::createQuaternionFromRPY(input[3], input[4], input[5]));
  }
  return transform;
}

template <class T>
std::vector<tf::Transform> getTransformsFromVector(const T &input) {
  if (input.size() % 6 != 0) {
    throw std::runtime_error(
        "The input does not have a multiple of 6 elements x,y,z, r,p,y");
  }

  std::vector<tf::Transform> transforms(input.size() / 6);
  for (int i = 0, j = 0; i < input.size(); i += 6, j++) {
    transforms.at(j).setOrigin(
        tf::Vector3(input[i + 0], input[i + 1], input[i + 2]));
    transforms.at(j).setRotation(
        tf::createQuaternionFromRPY(input[i + 3], input[i + 4], input[i + 5]));
  }
  return transforms;
}
}