133 lines
4.0 KiB
C++
133 lines
4.0 KiB
C++
// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
|
|
// de Barcelona (UAB).
|
|
//
|
|
// This work is licensed under the terms of the MIT license.
|
|
// For a copy, see <https://opensource.org/licenses/MIT>.
|
|
|
|
#pragma once
|
|
|
|
#include "carla/Debug.h"
|
|
#include "carla/geom/Vector3D.h"
|
|
|
|
#include <cmath>
|
|
#include <type_traits>
|
|
#include <utility>
|
|
|
|
namespace carla {
|
|
namespace geom {
|
|
|
|
class Rotation;
|
|
|
|
class Math {
|
|
public:
|
|
|
|
template <typename T>
|
|
static constexpr T Pi() {
|
|
static_assert(std::is_floating_point<T>::value, "type must be floating point");
|
|
return static_cast<T>(3.14159265358979323846264338327950288);
|
|
}
|
|
|
|
template <typename T>
|
|
static constexpr T Pi2() {
|
|
static_assert(std::is_floating_point<T>::value, "type must be floating point");
|
|
return static_cast<T>(static_cast<T>(2) * Pi<T>());
|
|
}
|
|
|
|
template <typename T>
|
|
static constexpr T ToDegrees(T rad) {
|
|
static_assert(std::is_floating_point<T>::value, "type must be floating point");
|
|
return rad * (T(180.0) / Pi<T>());
|
|
}
|
|
|
|
template <typename T>
|
|
static constexpr T ToRadians(T deg) {
|
|
static_assert(std::is_floating_point<T>::value, "type must be floating point");
|
|
return deg * (Pi<T>() / T(180.0));
|
|
}
|
|
|
|
template <typename T>
|
|
static T Clamp(T a, T min = T(0), T max = T(1)) {
|
|
return std::min(std::max(a, min), max);
|
|
}
|
|
|
|
template <typename T>
|
|
static T Square(const T &a) {
|
|
return a * a;
|
|
}
|
|
|
|
static auto Cross(const Vector3D &a, const Vector3D &b) {
|
|
return Vector3D(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
|
|
}
|
|
|
|
static auto Dot(const Vector3D &a, const Vector3D &b) {
|
|
return a.x * b.x + a.y * b.y + a.z * b.z;
|
|
}
|
|
|
|
static auto Dot2D(const Vector3D &a, const Vector3D &b) {
|
|
return a.x * b.x + a.y * b.y;
|
|
}
|
|
|
|
static auto DistanceSquared(const Vector3D &a, const Vector3D &b) {
|
|
return Square(b.x - a.x) + Square(b.y - a.y) + Square(b.z - a.z);
|
|
}
|
|
|
|
static auto DistanceSquared2D(const Vector3D &a, const Vector3D &b) {
|
|
return Square(b.x - a.x) + Square(b.y - a.y);
|
|
}
|
|
|
|
static auto Distance(const Vector3D &a, const Vector3D &b) {
|
|
return std::sqrt(DistanceSquared(a, b));
|
|
}
|
|
|
|
static auto Distance2D(const Vector3D &a, const Vector3D &b) {
|
|
return std::sqrt(DistanceSquared2D(a, b));
|
|
}
|
|
|
|
static float LinearLerp(float a, float b, float f) {
|
|
return a * (1.0f - f) + (b * f);
|
|
}
|
|
|
|
/// Returns the angle between 2 vectors in radians
|
|
static double GetVectorAngle(const Vector3D &a, const Vector3D &b);
|
|
|
|
/// Returns a pair containing:
|
|
/// - @b first: distance from v to p' where p' = p projected on segment
|
|
/// (w - v)
|
|
/// - @b second: Euclidean distance from p to p'
|
|
/// @param p point to calculate distance
|
|
/// @param v first point of the segment
|
|
/// @param w second point of the segment
|
|
static std::pair<float, float> DistanceSegmentToPoint(
|
|
const Vector3D &p,
|
|
const Vector3D &v,
|
|
const Vector3D &w);
|
|
|
|
/// Returns a pair containing:
|
|
/// - @b first: distance across the arc from start_pos to p' where p' = p
|
|
/// projected on Arc
|
|
/// - @b second: Euclidean distance from p to p'
|
|
static std::pair<float, float> DistanceArcToPoint(
|
|
Vector3D p,
|
|
Vector3D start_pos,
|
|
float length,
|
|
float heading, // [radians]
|
|
float curvature);
|
|
|
|
static Vector3D RotatePointOnOrigin2D(Vector3D p, float angle);
|
|
|
|
/// Compute the unit vector pointing towards the X-axis of @a rotation.
|
|
static Vector3D GetForwardVector(const Rotation &rotation);
|
|
|
|
/// Compute the unit vector pointing towards the Y-axis of @a rotation.
|
|
static Vector3D GetRightVector(const Rotation &rotation);
|
|
|
|
/// Compute the unit vector pointing towards the Y-axis of @a rotation.
|
|
static Vector3D GetUpVector(const Rotation &rotation);
|
|
|
|
// Helper function to generate a vector of consecutive integers from a to b
|
|
static std::vector<int> GenerateRange(int a, int b);
|
|
|
|
};
|
|
} // namespace geom
|
|
} // namespace carla
|