RIT VEXU Core API
Loading...
Searching...
No Matches
angle.h
1#pragma once
2
3typedef dim<0, 0, 0, 0, 0, 0, 0, 1> dim_angle;
4
5class Angle;
6template <>
7struct dimension_for_quantity<Angle> {
8 typedef dim_angle type;
9};
10
11struct radian_t {
12 typedef dim_angle dimension;
13 static constexpr double scale_num = static_cast<double>(1);
14 static constexpr double scale_den = static_cast<double>(1);
15 static constexpr bool is_affine = false;
16 static const char* symbol() { return "rad"; }
17 static const char* name() { return "radians"; }
18};
19
20struct degree_t {
21 typedef dim_angle dimension;
22 static constexpr double scale_num = static_cast<double>(17453292519943295.0);
23 static constexpr double scale_den = static_cast<double>(1000000000000000000.0);
24 static constexpr bool is_affine = false;
25 static const char* symbol() { return "deg"; }
26 static const char* name() { return "degrees"; }
27};
28
29struct revolution_t {
30 typedef dim_angle dimension;
31 static constexpr double scale_num = static_cast<double>(6283185307179586.0);
32 static constexpr double scale_den = static_cast<double>(1000000000000000.0);
33 static constexpr bool is_affine = false;
34 static const char* symbol() { return "rev"; }
35 static const char* name() { return "revolutions"; }
36};
37
38class Angle : public quantity_base<Angle, dim_angle> {
39 public:
40 typedef quantity_base<Angle, dim_angle> base_type;
41 typedef dim_angle dimension;
42 static constexpr double pi() { return 3.141592653589793238462643383279502884; }
43 static constexpr double normalization_epsilon() { return 1e-12; }
44
45 private:
46 static constexpr double two_pi() { return 2.0 * pi(); }
47 static constexpr double wrap_positive(double wrapped) { return wrapped < 0.0 ? wrapped + two_pi() : wrapped; }
48 static constexpr double zero_near_endpoints(double normalized) {
49 return gcem::abs(normalized - two_pi()) <= normalization_epsilon() ||
50 gcem::abs(normalized) <= normalization_epsilon()
51 ? 0.0
52 : normalized;
53 }
54 static constexpr double zero_near_origin(double normalized) {
55 return gcem::abs(normalized) <= normalization_epsilon() ? 0.0 : normalized;
56 }
57 static constexpr double normalize_0_to_2pi_value(double radians) {
58 return zero_near_endpoints(wrap_positive(gcem::fmod(radians, two_pi())));
59 }
60 static constexpr double normalize_minus_pi_to_pi_value(double radians) {
61 return zero_near_origin(wrap_positive(gcem::fmod(radians + pi(), two_pi())) - pi());
62 }
63
64 public:
65 constexpr explicit Angle(double radians = 0.0) : base_type(radians) {}
66 template <class OtherQuantity>
67 constexpr Angle(const OtherQuantity& other) : base_type(other.canonical_value()) {}
68 static constexpr Angle from_canonical(double value) { return Angle(value); }
69 template <class UnitTag>
70 static constexpr Angle from(double value) {
71 return Angle(base_type::template to_canonical<UnitTag>(value));
72 }
73 constexpr Angle normalized_0_to_2pi() const { return Angle(normalize_0_to_2pi_value(as<radian_t>())); }
74 constexpr Angle normalized_minus_pi_to_pi() const { return Angle(normalize_minus_pi_to_pi_value(as<radian_t>())); }
75 constexpr Angle normalized_0_to_360() const { return normalized_0_to_2pi(); }
76 constexpr Angle normalized_minus_180_to_180() const { return normalized_minus_pi_to_pi(); }
77};
78
79template <>
80struct is_quantity<Angle> : std::true_type {};
81
82Angle quantity_for_dimension_probe(dim_angle*, int);
83
84template <>
85struct default_unit_for_quantity<Angle> {
86 typedef radian_t type;
87};
88
89namespace literals {
90constexpr Angle operator""_rad(long double value) { return Angle::from<radian_t>(static_cast<double>(value)); }
91constexpr Angle operator""_rad(unsigned long long value) { return Angle::from<radian_t>(static_cast<double>(value)); }
92constexpr Angle operator""_deg(long double value) { return Angle::from<degree_t>(static_cast<double>(value)); }
93constexpr Angle operator""_deg(unsigned long long value) { return Angle::from<degree_t>(static_cast<double>(value)); }
94constexpr Angle operator""_rev(long double value) { return Angle::from<revolution_t>(static_cast<double>(value)); }
95constexpr Angle operator""_rev(unsigned long long value) {
96 return Angle::from<revolution_t>(static_cast<double>(value));
97}
98} // namespace literals
99
100inline constexpr Angle shortest_angular_distance(const Angle& from, const Angle& to) {
101 return (to - from).normalized_minus_pi_to_pi();
102}
103inline constexpr Length arc_length(const Length& radius, const Angle& angle) {
104 return Length::from_canonical(radius.as<meter_t>() * angle.as<radian_t>());
105}
106inline constexpr Length turn_radius(const Curvature& curvature) { return Angle::from<radian_t>(1.0) / curvature; }
107inline constexpr Curvature curvature_from_radius(const Length& radius) { return Angle::from<radian_t>(1.0) / radius; }
108inline constexpr bool is_left_turn(const Curvature& curvature) { return curvature.as<radian_per_meter_t>() > 0.0; }
109inline constexpr bool is_right_turn(const Curvature& curvature) { return curvature.as<radian_per_meter_t>() < 0.0; }
110inline constexpr Number sin(const Angle& angle) { return Number(gcem::sin(angle.as<radian_t>())); }
111inline constexpr Number cos(const Angle& angle) { return Number(gcem::cos(angle.as<radian_t>())); }
112inline constexpr Number tan(const Angle& angle) { return Number(gcem::tan(angle.as<radian_t>())); }
113inline constexpr Angle asin(const Number& value) { return Angle(gcem::asin(value.value())); }
114inline constexpr Angle acos(const Number& value) { return Angle(gcem::acos(value.value())); }
115inline constexpr Angle atan(const Number& value) { return Angle(gcem::atan(value.value())); }
116inline constexpr Angle atan2(const Number& y, const Number& x) { return Angle(gcem::atan2(y.value(), x.value())); }
117inline constexpr Angle atan2(const Length& y, const Length& x) {
118 return Angle(gcem::atan2(y.as<meter_t>(), x.as<meter_t>()));
119}