angle.h

Go to the documentation of this file.

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#pragma once

#include <cmath>
#include <cstdint>
#include <limits>

namespace apollo {
namespace common {
namespace math {

template <typename T>
class Angle {
 public:
  static_assert(std::numeric_limits<T>::is_integer &&
                    std::numeric_limits<T>::is_signed,
                "T must be a signed integer type");

  static Angle from_deg(const double value) {
    return Angle(static_cast<T>(std::lround(value * DEG_TO_RAW)));
  }

  static Angle from_rad(const double value) {
    return Angle(static_cast<T>(std::lround(value * RAD_TO_RAW)));
  }

  explicit Angle(const T value = 0) : value_(value) {}

  static constexpr T RAW_PI = std::numeric_limits<T>::min();

  static constexpr T RAW_PI_2 =
      static_cast<T>(-(std::numeric_limits<T>::min() >> 1));

  static constexpr double DEG_TO_RAW = RAW_PI / -180.0;

  static constexpr double RAD_TO_RAW = RAW_PI * -M_1_PI;

  static constexpr double RAW_TO_DEG = -180.0 / RAW_PI;

  static constexpr double RAW_TO_RAD = -M_PI / RAW_PI;

  T raw() const { return value_; }

  double to_deg() const { return value_ * RAW_TO_DEG; }

  double to_rad() const { return value_ * RAW_TO_RAD; }

  Angle operator+=(Angle other) {
    value_ = static_cast<T>(value_ + other.value_);
    return *this;
  }

  Angle operator-=(Angle other) {
    value_ = static_cast<T>(value_ - other.value_);
    return *this;
  }

  template <typename Scalar>
  Angle operator*=(Scalar s) {
    value_ = static_cast<T>(std::lround(value_ * s));
    return *this;
  }

  template <typename Scalar>
  Angle operator/=(Scalar s) {
    value_ = static_cast<T>(std::lround(value_ / s));
    return *this;
  }

 private:
  T value_;
};

using Angle8 = Angle<int8_t>;
using Angle16 = Angle<int16_t>;
using Angle32 = Angle<int32_t>;
using Angle64 = Angle<int64_t>;

template <typename T>
Angle<T> operator+(Angle<T> lhs, Angle<T> rhs) {
  lhs += rhs;
  return lhs;
}

template <typename T>
Angle<T> operator-(Angle<T> lhs, Angle<T> rhs) {
  lhs -= rhs;
  return lhs;
}

template <typename T, typename Scalar>
Angle<T> operator*(Angle<T> lhs, Scalar rhs) {
  lhs *= rhs;
  return lhs;
}

template <typename T, typename Scalar>
Angle<T> operator*(Scalar lhs, Angle<T> rhs) {
  rhs *= lhs;
  return rhs;
}

template <typename T, typename Scalar>
Angle<T> operator/(Angle<T> lhs, Scalar rhs) {
  lhs /= rhs;
  return lhs;
}

template <typename T>
double operator/(Angle<T> lhs, Angle<T> rhs) {
  return static_cast<double>(lhs.raw()) / rhs.raw();
}

template <typename T>
bool operator==(Angle<T> lhs, Angle<T> rhs) {
  return lhs.raw() == rhs.raw();
}

template <typename T>
bool operator!=(Angle<T> lhs, Angle<T> rhs) {
  return !(lhs == rhs);
}

// Fast trigonometric functions. Single precision is sufficient for Angle16 and
// Angle8.
float sin(Angle16 a);
float cos(Angle16 a);
float tan(Angle16 a);
float sin(Angle8 a);
float cos(Angle8 a);
float tan(Angle8 a);

}  // namespace math
}  // namespace common
}  // namespace apollo