diff --git a/src/proto/message_translation/er_force_world_test.cpp b/src/proto/message_translation/er_force_world_test.cpp index 00162058e3..fd064bdda1 100644 --- a/src/proto/message_translation/er_force_world_test.cpp +++ b/src/proto/message_translation/er_force_world_test.cpp @@ -2,6 +2,7 @@ #include +#include "software/geom/angular_velocity.h" #include "software/test_util/test_util.h" @@ -47,7 +48,7 @@ TEST(ErForceWorldTest, test_create_robot) const Point expected_pos(sim_robot->p_x(), sim_robot->p_y()); const Vector expected_vel(sim_robot->v_x(), sim_robot->v_y()); RobotState expected_state(expected_pos, expected_vel, Angle::zero(), - Angle::fromRadians(5.0)); + AngularVelocity::fromRadians(5.0)); EXPECT_EQ(0, test_robot.id()); EXPECT_TRUE(TestUtil::equalWithinTolerance(test_robot.currentState(), expected_state, 1e-6, Angle::fromDegrees(0))); diff --git a/src/proto/message_translation/tbots_geometry_test.cpp b/src/proto/message_translation/tbots_geometry_test.cpp index c6b9b3caa2..66ccb1516c 100644 --- a/src/proto/message_translation/tbots_geometry_test.cpp +++ b/src/proto/message_translation/tbots_geometry_test.cpp @@ -16,7 +16,7 @@ TEST(TbotsProtobufTest, point_msg_test) TEST(TbotsProtobufTest, angular_velocity_msg_test) { - auto angular_velocity = Angle::fromRadians(4.20); + auto angular_velocity = AngularVelocity::fromRadians(4.20); auto angular_velocity_msg = createAngularVelocityProto(angular_velocity); EXPECT_EQ(angular_velocity_msg->radians_per_second(), angular_velocity.toRadians()); diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index 1723b0df9b..86be9f8084 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -1,6 +1,7 @@ #include "proto/message_translation/tbots_protobuf.h" #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h" +#include "software/geom/angular_acceleration.h" #include "software/logger/logger.h" @@ -484,9 +485,9 @@ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( initial_velocity, AngularVelocity::fromRadians( robot_constants.robot_trajectory_max_ang_speed_rad_per_s), - AngularVelocity::fromRadians( + AngularAcceleration::fromRadians( robot_constants.robot_max_ang_acceleration_rad_per_s_2), - AngularVelocity::fromRadians( + AngularAcceleration::fromRadians( robot_constants.robot_max_ang_acceleration_rad_per_s_2)); } diff --git a/src/proto/message_translation/tbots_protobuf_test.cpp b/src/proto/message_translation/tbots_protobuf_test.cpp index 0496efc97b..cbad9ab8f2 100644 --- a/src/proto/message_translation/tbots_protobuf_test.cpp +++ b/src/proto/message_translation/tbots_protobuf_test.cpp @@ -121,7 +121,7 @@ TEST(TbotsProtobufTest, robot_state_msg_test) auto position = Point(4.20, 4.20); auto velocity = Vector(4.20, 4.20); auto orientation = Angle::fromRadians(4.20); - auto angular_velocity = Angle::fromRadians(4.20); + auto angular_velocity = AngularVelocity::fromRadians(4.20); Robot robot(0, position, velocity, orientation, angular_velocity, Timestamp::fromSeconds(0)); diff --git a/src/software/ai/hl/stp/play/passing_sim_test.py b/src/software/ai/hl/stp/play/passing_sim_test.py index 5326ae407a..6b66165b64 100644 --- a/src/software/ai/hl/stp/play/passing_sim_test.py +++ b/src/software/ai/hl/stp/play/passing_sim_test.py @@ -68,8 +68,8 @@ def setup_pass_and_robots( index, location, tbots_cpp.Vector(0.0, 0.0), - tbots_cpp.Angle.fromRadians(0), tbots_cpp.Angle(), + tbots_cpp.AngularVelocity(), tbots_cpp.Timestamp(), ) for index, location in enumerate(blue_robot_locations) @@ -81,8 +81,8 @@ def setup_pass_and_robots( index, location, tbots_cpp.Vector(0.0, 0.0), - tbots_cpp.Angle.fromRadians(0), tbots_cpp.Angle(), + tbots_cpp.AngularVelocity(), tbots_cpp.Timestamp(), ) for index, location in enumerate(enemy_robot_positions) diff --git a/src/software/ai/hl/stp/tactic/move_primitive.cpp b/src/software/ai/hl/stp/tactic/move_primitive.cpp index 36597541d1..9c21c6351e 100644 --- a/src/software/ai/hl/stp/tactic/move_primitive.cpp +++ b/src/software/ai/hl/stp/tactic/move_primitive.cpp @@ -7,6 +7,7 @@ #include "proto/primitive/primitive_msg_factory.h" #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h" #include "software/geom/algorithms/end_in_obstacle_sample.h" +#include "software/geom/angular_acceleration.h" MovePrimitive::MovePrimitive( @@ -42,9 +43,9 @@ MovePrimitive::MovePrimitive( robot.orientation(), final_angle, robot.angularVelocity(), AngularVelocity::fromRadians( robot.robotConstants().robot_max_ang_speed_rad_per_s), - AngularVelocity::fromRadians( + AngularAcceleration::fromRadians( robot.robotConstants().robot_max_ang_acceleration_rad_per_s_2), - AngularVelocity::fromRadians( + AngularAcceleration::fromRadians( robot.robotConstants().robot_max_ang_acceleration_rad_per_s_2)); estimated_cost = diff --git a/src/software/geom/BUILD b/src/software/geom/BUILD index 3b25c08bd8..84d2fa1d7b 100644 --- a/src/software/geom/BUILD +++ b/src/software/geom/BUILD @@ -1,9 +1,15 @@ package(default_visibility = ["//visibility:public"]) +cc_library( + name = "generic_angle", + hdrs = ["generic_angle.h"], + deps = [":geom_constants"], +) + cc_library( name = "angle", hdrs = ["angle.h"], - deps = [":geom_constants"], + deps = [":generic_angle"], ) cc_library( @@ -36,13 +42,13 @@ cc_library( cc_library( name = "angular_velocity", hdrs = ["angular_velocity.h"], - deps = [":angle"], + deps = [":generic_angle"], ) cc_library( name = "angular_acceleration", hdrs = ["angular_acceleration.h"], - deps = [":angle"], + deps = [":generic_angle"], ) cc_library( diff --git a/src/software/geom/angle.h b/src/software/geom/angle.h index 09b432de3e..f999181cac 100644 --- a/src/software/geom/angle.h +++ b/src/software/geom/angle.h @@ -1,591 +1,13 @@ #pragma once -#include -#include - -#include "software/geom/geom_constants.h" +#include "software/geom/generic_angle.h" /** * A typesafe representation of an angle. * - * This class helps prevent accidentally combining values in degrees and radians - * without proper conversion. - */ -class Angle final -{ - public: - /** - * The zero angle. - */ - static constexpr Angle zero(); - - /** - * The quarter-turn angle (90°). - */ - static constexpr Angle quarter(); - - /** - * The half-turn angle (180°). - */ - static constexpr Angle half(); - - /** - * The three-quarter turn angle (270°). - */ - static constexpr Angle threeQuarter(); - - /** - * The full-turn angle (360°). - */ - static constexpr Angle full(); - - /** - * Constructs an angle from a value in radians. - * - * @param rad the angle in radians. - * - * @return the constructed angle - */ - static constexpr Angle fromRadians(double rad); - - /** - * Constructs an angle from a value in degrees. - * - * @param deg the angle in degrees - * - * @return the constructed angle - */ - static constexpr Angle fromDegrees(double deg); - - /** - * Computes the arcsine of a value. - * - * @param x the value. - * - * @return the angle. - */ - static Angle asin(double x); - - /** - * Computes the arccosine of a value. - * - * @param x the value. - * - * @return the angle. - */ - static Angle acos(double x); - - /** - * Computes the arctangent of a value. - * - * @param x the value. - * - * @return the angle. - */ - static Angle atan(double x); - - /** - * Constructs the "zero" angle. - */ - explicit constexpr Angle(); - - /** - * Converts this angle to a value in radians. - * - * @return the number of radians in this angle in the range [0, 2PI) - */ - constexpr double toRadians() const; - - /** - * Converts this angle to a value in degrees. - * - * @return the number of degrees in this angle in the range [0,360) - */ - constexpr double toDegrees() const; - - /** - * Computes the modulus of a division between this angle and another - * angle. - * - * @param divisor the divisor. - * - * @return the modulus of this Angle ÷ divisor. - */ - constexpr Angle mod(Angle divisor) const; - - /** - * Computes the remainder of a division between this angle and - * another angle. - * - * @param divisor the divisor. - * - * @return the remainder of this Angle ÷ divisor. - */ - constexpr Angle remainder(const Angle& divisor) const; - - /** - * Returns the absolute value of this angle. - * - * @return the absolute value of this angle. - */ - constexpr Angle abs() const; - - /** - * Checks whether the angle is finite. - * - * @return true if the angle is finite, and false if it is ±∞ or NaN. - */ - bool isFinite() const; - - /** - * Computes the sine of this angle. - * - * @return the sine of this angle. - */ - double sin() const; - - /** - * Computes the cosine of this angle. - * - * @return the cosine of this angle. - */ - double cos() const; - - /** - * Computes the tangent of this angle. - * - * @return the tangent of this angle. - */ - double tan() const; - - /** - * Limits this angle to [−π, π]. - * - * The angle is rotated by a multiple of 2π until it lies within the target - * interval. - * - * @return the clamped angle. - */ - constexpr Angle clamp() const; - - /** - * Returns the smallest possible rotational difference between this angle - * and another angle. - * - * @param other the second angle. - * - * @return the angle between this Angle and other, in the range [0, π]. - */ - constexpr Angle minDiff(const Angle& other) const; - - private: - /** - * The measurement in radians of this Angle. - */ - double rads; - - explicit constexpr Angle(double rads); -}; - -/** - * Negates an angle. - * - * @param angle the angle to negate. - * - * @return the negated angle - */ -constexpr Angle operator-(const Angle& angle) __attribute__((warn_unused_result)); - -/** - * Adds two angles. - * - * @param x the first addend. - * @param y the second addend. - * - * @return the sum of the angles - */ -constexpr Angle operator+(const Angle& x, const Angle& y) - __attribute__((warn_unused_result)); - -/** - * Subtracts two angles. - * - * @param x the minuend. - * - * @param y the subtrahend. - * - * @return the difference between the minuend and subtrahend. + * This class is an instantiation of the GenericAngle template class. Used to + * define more methods such as clamp, minDiff, etc. that only apply to a base + * angle value. */ -constexpr Angle operator-(const Angle& x, const Angle& y) - __attribute__((warn_unused_result)); - -/** - * Multiplies an angle by a scalar factor. - * - * @param angle the angle. - * @param scale the scalar factor. - * - * @return the product of the angle and the scalar factor - */ -constexpr Angle operator*(const Angle& angle, double scale) - __attribute__((warn_unused_result)); - -/** - * Multiplies an angle by a scalar factor. - * - * @param scale the scalar factor. - * @param angle the angle. - * - * @return the product of the angle and the scalar factor - */ -constexpr Angle operator*(double scale, const Angle& angle) - __attribute__((warn_unused_result)); - -/** - * Divides an angle by a scalar divisor. - * - * @param angle the angle. - * @param divisor the scalar divisor. - * - * @return the quotient of this Angle ÷ the divisor. - */ -constexpr Angle operator/(const Angle& angle, double divisor) - __attribute__((warn_unused_result)); - -/** - * Divides two angles. - * - * @param x the divident. - * @param y the divisor. - * - * @return the quotient of the divident ÷ the divisor. - */ -constexpr double operator/(const Angle& x, const Angle& y) - __attribute__((warn_unused_result)); - -/** - * Adds an angle to another angle. - * - * @param x the angle to add to. - * @param y the angle to add. - * - * @return the new angle x - */ -Angle& operator+=(Angle& x, const Angle& y); - -/** - * Subtracts an angle from an angle. - * - * @param x the angle to subtract from. - * @param y the angle to subtract. - * - * @return the new angle x - */ -Angle& operator-=(Angle& x, const Angle& y); - -/** - * Scales an angle by a factor. - * - * @param angle the angle to scale. - * @param scale the scalar factor. - * - * @return the scaled angle. - */ -Angle& operator*=(Angle& angle, double scale); - -/** - * Divides an angle by a scalar divisor. - * - * @param angle the angle to scale. - * - * @param divisor the scalar divisor. - * - * @return the scaled angle. - */ -Angle& operator/=(Angle& angle, double divisor); - -/** - * Compares two angles. - * - * @param x the first angle. - * - * @param y the second angle. - * - * @return true if x is strictly less than y, and false otherwise - */ -constexpr bool operator<(const Angle& x, const Angle& y); - -/** - * Compares two angles. - * - * @param x the first angle. - * @param y the second angle. - * - * @return true if x is strictly greater than y, and false otherwise. - */ -constexpr bool operator>(const Angle& x, const Angle& y); - -/** - * Compares two angles. - * - * @param x the first angle. - * @param y the second angle. - * - * @return true if x is less than or equal to y, and false otherwise. - */ -constexpr bool operator<=(const Angle& x, const Angle& y); - -/** - * Compares two angles. - * - * @param x the first angle. - * @param y the second angle. - * - * @return true if x is greater than or equal to y, and false otherwise. - */ -constexpr bool operator>=(const Angle& x, const Angle& y); - -/** - * Compares two angles for equality - * - * @param x the first angle. - * @param y the second angle. - * - * @return true if x is equal to y, and false otherwise. - */ -bool operator==(const Angle& x, const Angle& y); - -/** - * Compares two angles for inequality. - * - * @param x the first angle. - * @param y the second angle. - * - * @return true if x is not equal to y, and false otherwise - */ -constexpr bool operator!=(const Angle& x, const Angle& y); - -/** - * Prints an Angle to a stream - * - * @param os the stream to print to - * @param a the Point to print - * - * @return the stream with the Angle printed - */ -inline std::ostream& operator<<(std::ostream& os, const Angle& a); - -inline constexpr Angle Angle::zero() -{ - return Angle(); -} - -inline constexpr Angle Angle::quarter() -{ - return Angle(M_PI / 2.0); -} - -inline constexpr Angle Angle::half() -{ - return Angle(M_PI); -} - -inline constexpr Angle Angle::threeQuarter() -{ - return Angle(3.0 / 2.0 * M_PI); -} - -inline constexpr Angle Angle::full() -{ - return Angle(2.0 * M_PI); -} - -inline constexpr Angle Angle::fromRadians(double rad) -{ - return Angle(rad); -} - -inline constexpr Angle Angle::fromDegrees(double deg) -{ - return Angle(deg / 180.0 * M_PI); -} - -inline Angle Angle::asin(double x) -{ - return Angle::fromRadians(std::asin(x)); -} - -inline Angle Angle::acos(double x) -{ - return fromRadians(std::acos(x)); -} - -inline Angle Angle::atan(double x) -{ - return Angle::fromRadians(std::atan(x)); -} - -inline constexpr Angle::Angle() : rads(0.0) {} - -inline constexpr double Angle::toRadians() const -{ - return rads; -} - -inline constexpr double Angle::toDegrees() const -{ - return rads / M_PI * 180.0; -} - -inline constexpr Angle Angle::mod(Angle divisor) const -{ - if (divisor.toRadians() < FIXED_EPSILON) - { - return Angle::fromRadians(toRadians()); - } - else - { - return Angle::fromRadians(toRadians() - static_cast(static_cast( - toRadians() / divisor.toRadians())) * - divisor.toRadians()); - } -} - -inline constexpr Angle Angle::remainder(const Angle& divisor) const -{ - return Angle::fromRadians(toRadians() - - static_cast(static_cast( - (toRadians() / divisor.toRadians()) >= 0 - ? (toRadians() / divisor.toRadians() + 0.5) - : (toRadians() / divisor.toRadians() - 0.5))) * - divisor.toRadians()); -} - -inline constexpr Angle Angle::abs() const -{ - return Angle::fromRadians(toRadians() < 0 ? -toRadians() : toRadians()); -} - -inline bool Angle::isFinite() const -{ - return std::isfinite(toRadians()); -} - -inline double Angle::sin() const -{ - return std::sin(toRadians()); -} - -inline double Angle::cos() const -{ - return std::cos(toRadians()); -} - -inline double Angle::tan() const -{ - return std::tan(toRadians()); -} - -inline constexpr Angle Angle::clamp() const -{ - return remainder(Angle::full()); -} - -inline constexpr Angle Angle::minDiff(const Angle& other) const -{ - return (*this - other).clamp().abs(); -} - -inline constexpr Angle::Angle(double rads) : rads(rads) {} - -inline constexpr Angle operator-(const Angle& angle) -{ - return Angle::fromRadians(-angle.toRadians()); -} - -inline constexpr Angle operator+(const Angle& x, const Angle& y) -{ - return Angle::fromRadians(x.toRadians() + y.toRadians()); -} - -inline constexpr Angle operator-(const Angle& x, const Angle& y) -{ - return Angle::fromRadians(x.toRadians() - y.toRadians()); -} - -inline constexpr Angle operator*(const Angle& angle, double scale) -{ - return Angle::fromRadians(angle.toRadians() * scale); -} - -inline constexpr Angle operator*(double scale, const Angle& angle) -{ - return Angle::fromRadians(scale * angle.toRadians()); -} - -inline constexpr Angle operator/(const Angle& angle, double divisor) -{ - return Angle::fromRadians(angle.toRadians() / divisor); -} - -inline constexpr double operator/(const Angle& x, const Angle& y) -{ - return x.toRadians() / y.toRadians(); -} - -inline Angle& operator+=(Angle& x, const Angle& y) -{ - return x = x + y; -} - -inline Angle& operator-=(Angle& x, const Angle& y) -{ - return x = x - y; -} - -inline Angle& operator*=(Angle& angle, double scale) -{ - return angle = angle * scale; -} - -inline Angle& operator/=(Angle& angle, double divisor) -{ - return angle = angle / divisor; -} - -inline constexpr bool operator<(const Angle& x, const Angle& y) -{ - return x.toRadians() < y.toRadians(); -} - -inline constexpr bool operator>(const Angle& x, const Angle& y) -{ - return x.toRadians() > y.toRadians(); -} - -inline constexpr bool operator<=(const Angle& x, const Angle& y) -{ - return x.toRadians() <= y.toRadians(); -} - -inline constexpr bool operator>=(const Angle& x, const Angle& y) -{ - return x.toRadians() >= y.toRadians(); -} - -inline bool operator==(const Angle& x, const Angle& y) -{ - Angle diff = x.clamp().minDiff(y.clamp()); - return diff.toRadians() <= FIXED_EPSILON; -} - -inline constexpr bool operator!=(const Angle& x, const Angle& y) -{ - return x.toRadians() != y.toRadians(); -} -inline std::ostream& operator<<(std::ostream& os, const Angle& a) -{ - os << a.toRadians() << "R"; - return os; -} +using Angle = GenericAngle; diff --git a/src/software/geom/angular_acceleration.h b/src/software/geom/angular_acceleration.h index 8fcf877050..e44aa5d817 100644 --- a/src/software/geom/angular_acceleration.h +++ b/src/software/geom/angular_acceleration.h @@ -1,10 +1,10 @@ -#include "software/geom/angle.h" +#include "software/geom/generic_angle.h" + /** - * We also use variables of type 'Angle' to represent angular acceleration, since they - * are essentially represented the same. This typedef allows us to refer to Angles as - * AngularAcceleration, which makes the interfaces more intuitive. - * TODO (#3093): Not all methods of Angle class make sense for AngularAcceleration! - * E.g. Angle::clamp does not make sense in the context of AngularAcceleration, as - * 360 deg/s^2 is different from 0 deg/s^2, but 360 deg is the same as 0 deg. + * A typesafe representation of an angular acceleration. + * + * This class is an instantiation of the GenericAngle template class. Used to + * differentiate between the Angle class while keeping most of the common + * functionality that makes sense. */ -using AngularAcceleration = Angle; +using AngularAcceleration = GenericAngle; diff --git a/src/software/geom/angular_velocity.h b/src/software/geom/angular_velocity.h index ed630eed30..ef308baec8 100644 --- a/src/software/geom/angular_velocity.h +++ b/src/software/geom/angular_velocity.h @@ -1,11 +1,10 @@ -#include "software/geom/angle.h" +#include "software/geom/generic_angle.h" /** - * We also use variables of type 'Angle' to represent angular velocities, since they - * are essentially represented the same. This typedef allows us to refer to Angles as - * AngularVelocities, which makes the interfaces more intuitive. - * TODO (#3093): Not all methods of Angle class make sense for AngularVelocity! - * E.g. Angle::clamp does not make sense in the context of AngularVelocity, as 360 deg/s - * is different from 0 deg/s, but 360 deg is the same as 0 deg. + * A typesafe representation of an angular velocity. + * + * This class is an instantiation of the GenericAngle template class. Used to + * differentiate between the Angle class while keeping most of the common + * functionality that makes sense. */ -using AngularVelocity = Angle; +using AngularVelocity = GenericAngle; diff --git a/src/software/geom/generic_angle.h b/src/software/geom/generic_angle.h new file mode 100644 index 0000000000..03b6533834 --- /dev/null +++ b/src/software/geom/generic_angle.h @@ -0,0 +1,659 @@ +#pragma once + +#include +#include +#include + +#include "software/geom/geom_constants.h" + +struct AngleTag; +struct AngularVelocityTag; +struct AngularAccelerationTag; + +template +concept IsBaseAngleTag = std::is_same_v; + +/** + * A typesafe representation of a generic angle. + * + * This template class helps prevent accidentally combining values in degrees + * and radians without proper conversion. + */ +template +class GenericAngle final +{ + public: + /** + * The zero angle. + */ + static constexpr GenericAngle zero(); + + /** + * The quarter-turn angle (90°). + */ + static constexpr GenericAngle quarter(); + + /** + * The half-turn angle (180°). + */ + static constexpr GenericAngle half(); + + /** + * The three-quarter turn angle (270°). + */ + static constexpr GenericAngle threeQuarter(); + + /** + * The full-turn angle (360°). + */ + static constexpr GenericAngle full(); + + /** + * Constructs an angle from a value in radians. + * + * @param rad the angle in radians. + * + * @return the constructed angle + */ + static constexpr GenericAngle fromRadians(double rad); + + /** + * Constructs an angle from a value in degrees. + * + * @param deg the angle in degrees + * + * @return the constructed angle + */ + static constexpr GenericAngle fromDegrees(double deg); + + /** + * Computes the arcsine of a value. + * + * @param x the value. + * + * @return the angle. + */ + static GenericAngle asin(double x); + + /** + * Computes the arccosine of a value. + * + * @param x the value. + * + * @return the angle. + */ + static GenericAngle acos(double x); + + /** + * Computes the arctangent of a value. + * + * @param x the value. + * + * @return the angle. + */ + static GenericAngle atan(double x); + + /** + * Constructs the "zero" angle. + */ + explicit constexpr GenericAngle(); + + /** + * Converts this angle to a value in radians. + * + * @return the number of radians in this angle in the range [0, 2PI) + */ + constexpr double toRadians() const; + + /** + * Converts this angle to a value in degrees. + * + * @return the number of degrees in this angle in the range [0,360) + */ + constexpr double toDegrees() const; + + /** + * Computes the modulus of a division between this angle and another + * angle. + * + * @param divisor the divisor. + * + * @return the modulus of this GenericAngle ÷ divisor. + */ + constexpr GenericAngle mod(GenericAngle divisor) const requires IsBaseAngleTag; + + /** + * Computes the remainder of a division between this angle and + * another angle. + * + * @param divisor the divisor. + * + * @return the remainder of this GenericAngle ÷ divisor. + */ + constexpr GenericAngle remainder(const GenericAngle& divisor) const requires + IsBaseAngleTag; + + /** + * Returns the absolute value of this angle. + * + * @return the absolute value of this angle. + */ + constexpr GenericAngle abs() const; + + /** + * Checks whether the angle is finite. + * + * @return true if the angle is finite, and false if it is ±∞ or NaN. + */ + bool isFinite() const; + + /** + * Computes the sine of this angle. + * + * @return the sine of this angle. + */ + double sin() const; + + /** + * Computes the cosine of this angle. + * + * @return the cosine of this angle. + */ + double cos() const; + + /** + * Computes the tangent of this angle. + * + * @return the tangent of this angle. + */ + double tan() const; + + /** + * Limits this angle to [−π, π]. + * + * The angle is rotated by a multiple of 2π until it lies within the target + * interval. + * + * @return the clamped angle. + */ + constexpr GenericAngle clamp() const requires IsBaseAngleTag; + + /** + * Returns the smallest possible rotational difference between this angle + * and another angle. + * + * @param other the second angle. + * + * @return the angle between this GenericAngle and other, in the range [0, π]. + */ + constexpr GenericAngle minDiff(const GenericAngle& other) const requires + IsBaseAngleTag; + + private: + /** + * The measurement in radians of this GenericAngle. + */ + double rads; + + explicit constexpr GenericAngle(double rads); +}; + +template +concept AngleType = std::is_same_v> || + std::is_same_v> || + std::is_same_v>; + +/** + * Negates an angle. + * + * @param angle the angle to negate. + * + * @return the negated angle + */ +constexpr auto operator-(const AngleType auto& angle) __attribute__((warn_unused_result)); + +/** + * Adds two angles. + * + * @param x the first addend. + * @param y the second addend. + * + * @return the sum of the angles + */ +constexpr auto operator+(const AngleType auto& x, const AngleType auto& y) + __attribute__((warn_unused_result)); + +/** + * Subtracts two angles. + * + * @param x the minuend. + * + * @param y the subtrahend. + * + * @return the difference between the minuend and subtrahend. + */ +constexpr auto operator-(const AngleType auto& x, const AngleType auto& y) + __attribute__((warn_unused_result)); + +/** + * Multiplies an angle by a scalar factor. + * + * @param angle the angle. + * @param scale the scalar factor. + * + * @return the product of the angle and the scalar factor + */ +template +constexpr GenericAngle operator*(const GenericAngle& angle, double scale) + __attribute__((warn_unused_result)); + +/** + * Multiplies an angle by a scalar factor. + * + * @param scale the scalar factor. + * @param angle the angle. + * + * @return the product of the angle and the scalar factor + */ +template +constexpr GenericAngle operator*(double scale, const GenericAngle& angle) + __attribute__((warn_unused_result)); + +/** + * Divides an angle by a scalar divisor. + * + * @param angle the angle. + * @param divisor the scalar divisor. + * + * @return the quotient of this AngleType auto ÷ the divisor. + */ +template +constexpr GenericAngle operator/(const GenericAngle& angle, double divisor) + __attribute__((warn_unused_result)); + +/** + * Divides two angles. + * + * @param x the divident. + * @param y the divisor. + * + * @return the quotient of the divident ÷ the divisor. + */ +constexpr double operator/(const AngleType auto& x, const AngleType auto& y) + __attribute__((warn_unused_result)); + +/** + * Adds an angle to another angle. + * + * @param x the angle to add to. + * @param y the angle to add. + * + * @return the new angle x + */ +auto& operator+=(AngleType auto& x, const AngleType auto& y); + +/** + * Subtracts an angle from an angle. + * + * @param x the angle to subtract from. + * @param y the angle to subtract. + * + * @return the new angle x + */ +auto& operator-=(AngleType auto& x, const AngleType auto& y); + +/** + * Scales an angle by a factor. + * + * @param angle the angle to scale. + * @param scale the scalar factor. + * + * @return the scaled angle. + */ +auto& operator*=(AngleType auto& angle, double scale); + +/** + * Divides an angle by a scalar divisor. + * + * @param angle the angle to scale. + * + * @param divisor the scalar divisor. + * + * @return the scaled angle. + */ +auto& operator/=(AngleType auto& angle, double divisor); + +/** + * Compares two angles. + * + * @param x the first angle. + * + * @param y the second angle. + * + * @return true if x is strictly less than y, and false otherwise + */ +constexpr bool operator<(const AngleType auto& x, const AngleType auto& y); + +/** + * Compares two angles. + * + * @param x the first angle. + * @param y the second angle. + * + * @return true if x is strictly greater than y, and false otherwise. + */ +constexpr bool operator>(const AngleType auto& x, const AngleType auto& y); + +/** + * Compares two angles. + * + * @param x the first angle. + * @param y the second angle. + * + * @return true if x is less than or equal to y, and false otherwise. + */ +constexpr bool operator<=(const AngleType auto& x, const AngleType auto& y); + +/** + * Compares two angles. + * + * @param x the first angle. + * @param y the second angle. + * + * @return true if x is greater than or equal to y, and false otherwise. + */ +constexpr bool operator>=(const AngleType auto& x, const AngleType auto& y); + +/** + * Compares two angles for equality + * + * @param x the first angle. + * @param y the second angle. + * + * @return true if x is equal to y, and false otherwise. + */ +bool operator==(const AngleType auto& x, const AngleType auto& y); + +/** + * Compares two angles for inequality. + * + * @param x the first angle. + * @param y the second angle. + * + * @return true if x is not equal to y, and false otherwise + */ +constexpr bool operator!=(const AngleType auto& x, const AngleType auto& y); + +/** + * Prints an AngleType auto to a stream + * + * @param os the stream to print to + * @param a the Point to print + * + * @return the stream with the AngleType auto printed + */ +inline std::ostream& operator<<(std::ostream& os, const AngleType auto& a); + +template +inline constexpr GenericAngle GenericAngle::zero() +{ + return GenericAngle(); +} + +template +inline constexpr GenericAngle GenericAngle::quarter() +{ + return GenericAngle(M_PI / 2.0); +} + +template +inline constexpr GenericAngle GenericAngle::half() +{ + return GenericAngle(M_PI); +} + +template +inline constexpr GenericAngle GenericAngle::threeQuarter() +{ + return GenericAngle(3.0 / 2.0 * M_PI); +} + +template +inline constexpr GenericAngle GenericAngle::full() +{ + return GenericAngle(2.0 * M_PI); +} + +template +inline constexpr GenericAngle GenericAngle::fromRadians(double rad) +{ + return GenericAngle(rad); +} + +template +inline constexpr GenericAngle GenericAngle::fromDegrees(double deg) +{ + return GenericAngle(deg / 180.0 * M_PI); +} + +template +inline GenericAngle GenericAngle::asin(double x) +{ + return GenericAngle::fromRadians(std::asin(x)); +} + +template +inline GenericAngle GenericAngle::acos(double x) +{ + return fromRadians(std::acos(x)); +} + +template +inline GenericAngle GenericAngle::atan(double x) +{ + return GenericAngle::fromRadians(std::atan(x)); +} + +template +inline constexpr GenericAngle::GenericAngle() : rads(0.0) +{ +} + +template +inline constexpr double GenericAngle::toRadians() const +{ + return rads; +} + +template +inline constexpr double GenericAngle::toDegrees() const +{ + return rads / M_PI * 180.0; +} + +template +inline constexpr GenericAngle GenericAngle::mod( + GenericAngle divisor) const requires IsBaseAngleTag +{ + if (divisor.toRadians() < FIXED_EPSILON) + { + return GenericAngle::fromRadians(toRadians()); + } + else + { + return GenericAngle::fromRadians( + toRadians() - + static_cast(static_cast(toRadians() / divisor.toRadians())) * + divisor.toRadians()); + } +} + +template +inline constexpr GenericAngle GenericAngle::remainder( + const GenericAngle& divisor) const requires IsBaseAngleTag + +{ + return GenericAngle::fromRadians( + toRadians() - static_cast(static_cast( + (toRadians() / divisor.toRadians()) >= 0 + ? (toRadians() / divisor.toRadians() + 0.5) + : (toRadians() / divisor.toRadians() - 0.5))) * + divisor.toRadians()); +} + +template +inline constexpr GenericAngle GenericAngle::abs() const +{ + return GenericAngle::fromRadians(toRadians() < 0 ? -toRadians() : toRadians()); +} + +template +inline bool GenericAngle::isFinite() const +{ + return std::isfinite(toRadians()); +} + +template +inline double GenericAngle::sin() const +{ + return std::sin(toRadians()); +} + +template +inline double GenericAngle::cos() const +{ + return std::cos(toRadians()); +} + +template +inline double GenericAngle::tan() const +{ + return std::tan(toRadians()); +} + +template +inline constexpr GenericAngle GenericAngle::clamp() const requires + IsBaseAngleTag + +{ + return remainder(GenericAngle::full()); +} + +template +inline constexpr GenericAngle GenericAngle::minDiff( + const GenericAngle& other) const requires IsBaseAngleTag + +{ + return (*this - other).clamp().abs(); +} + +template +inline constexpr GenericAngle::GenericAngle(double rads) : rads(rads) +{ +} + +inline constexpr auto operator-(const AngleType auto& angle) +{ + using T = std::remove_cvref_t; + return T::fromRadians(-angle.toRadians()); +} + +inline constexpr auto operator+(const AngleType auto& x, const AngleType auto& y) +{ + using T = std::remove_cvref_t; + return T::fromRadians(x.toRadians() + y.toRadians()); +} + +inline constexpr auto operator-(const AngleType auto& x, const AngleType auto& y) +{ + using T = std::remove_cvref_t; + return T::fromRadians(x.toRadians() - y.toRadians()); +} + +template +inline constexpr GenericAngle operator*(const GenericAngle& angle, double scale) +{ + return GenericAngle::fromRadians(angle.toRadians() * scale); +} + +template +inline constexpr GenericAngle operator*(double scale, const GenericAngle& angle) +{ + return GenericAngle::fromRadians(scale * angle.toRadians()); +} + +template +inline constexpr GenericAngle operator/(const GenericAngle& angle, + double divisor) +{ + return GenericAngle::fromRadians(angle.toRadians() / divisor); +} + +inline constexpr double operator/(const AngleType auto& x, const AngleType auto& y) +{ + return x.toRadians() / y.toRadians(); +} + +inline auto& operator+=(AngleType auto& x, const AngleType auto& y) +{ + return x = x + y; +} + +inline auto& operator-=(AngleType auto& x, const AngleType auto& y) +{ + return x = x - y; +} + +inline auto& operator*=(AngleType auto& angle, double scale) +{ + return angle = angle * scale; +} + +inline auto& operator/=(AngleType auto& angle, double divisor) +{ + return angle = angle / divisor; +} + +inline constexpr bool operator<(const AngleType auto& x, const AngleType auto& y) +{ + return x.toRadians() < y.toRadians(); +} + +inline constexpr bool operator>(const AngleType auto& x, const AngleType auto& y) +{ + return x.toRadians() > y.toRadians(); +} + +inline constexpr bool operator<=(const AngleType auto& x, const AngleType auto& y) +{ + return x.toRadians() <= y.toRadians(); +} + +inline constexpr bool operator>=(const AngleType auto& x, const AngleType auto& y) +{ + return x.toRadians() >= y.toRadians(); +} + +inline bool operator==(const AngleType auto& x, const AngleType auto& y) +{ + if constexpr (std::is_same_v&>) + { + auto diff = x.clamp().minDiff(y.clamp()); + return diff.toRadians() <= FIXED_EPSILON; + } + else + { + return fabs(x.toRadians() - y.toRadians()) <= FIXED_EPSILON; + } +} + +inline constexpr bool operator!=(const AngleType auto& x, const AngleType auto& y) +{ + return !(x == y); +} + +inline std::ostream& operator<<(std::ostream& os, const AngleType auto& a) +{ + os << a.toRadians() << "R"; + return os; +} diff --git a/src/software/python_bindings.cpp b/src/software/python_bindings.cpp index a07a349176..718c9f7a21 100644 --- a/src/software/python_bindings.cpp +++ b/src/software/python_bindings.cpp @@ -217,6 +217,20 @@ PYBIND11_MODULE(python_bindings, m) return stream.str(); }); + py::class_(m, "AngularVelocity") + .def(py::init<>()) + .def_static("fromRadians", &AngularVelocity::fromRadians) + .def_static("fromDegrees", &AngularVelocity::fromDegrees) + .def("toRadians", &AngularVelocity::toRadians) + // Overloaded + .def("__repr__", + [](const AngularVelocity& a) + { + std::stringstream stream; + stream << a; + return stream.str(); + }); + py::class_(m, "ConvexPolygon"); py::class_(m, "Rectangle") .def(py::init()) @@ -340,7 +354,7 @@ PYBIND11_MODULE(python_bindings, m) m.def("intersection", py::overload_cast(&intersection)); py::class_(m, "Robot") - .def(py::init()) + .def(py::init()) .def(py::init()) .def("timestamp", &Robot::timestamp) .def("position", &Robot::position) diff --git a/src/software/sensor_fusion/filter/robot_filter.cpp b/src/software/sensor_fusion/filter/robot_filter.cpp index 9747e6880d..72997ea32d 100644 --- a/src/software/sensor_fusion/filter/robot_filter.cpp +++ b/src/software/sensor_fusion/filter/robot_filter.cpp @@ -84,10 +84,11 @@ std::optional RobotFilter::getFilteredData( current_robot_state.timestamp().toSeconds()); // angular_velocity = orientation difference / time difference - filtered_data.angular_velocity = - (filtered_data.orientation - current_robot_state.orientation()).clamp() / - (filtered_data.timestamp.toSeconds() - - current_robot_state.timestamp().toSeconds()); + filtered_data.angular_velocity = AngularVelocity::fromRadians( + ((filtered_data.orientation - current_robot_state.orientation()).clamp() / + (filtered_data.timestamp.toSeconds() - + current_robot_state.timestamp().toSeconds())) + .toRadians()); // find breakbeam_status bool breakbeam_tripped = breakbeam_tripped_id == getRobotId(); diff --git a/src/software/test_util/equal_within_tolerance.cpp b/src/software/test_util/equal_within_tolerance.cpp index 131438819f..b3029b76a9 100644 --- a/src/software/test_util/equal_within_tolerance.cpp +++ b/src/software/test_util/equal_within_tolerance.cpp @@ -95,6 +95,25 @@ ::testing::AssertionResult equalWithinTolerance(const Angle& a1, const Angle& a2 } } +::testing::AssertionResult equalWithinTolerance(const AngularVelocity& a1, + const AngularVelocity& a2, + const AngularVelocity& tolerance) +{ + // subtract a fixed epsilon for error in: + // - angle subtraction + // - angle absolute value + // - the tolerance abs() + auto difference = (a1 - a2).abs() - AngularVelocity::fromRadians(FIXED_EPSILON * 4); + if (difference < tolerance.abs()) + { + return ::testing::AssertionSuccess(); + } + else + { + return ::testing::AssertionFailure() + << "Angular velocity 1 was " << a1 << ", angular velocity 2 was " << a2; + } +} ::testing::AssertionResult equalWithinTolerance(const Vector& v1, const Vector& v2, double tolerance) @@ -154,8 +173,9 @@ ::testing::AssertionResult equalWithinTolerance(const RobotState& state1, equalWithinTolerance(state1.velocity(), state2.velocity(), linear_tolerance); auto orientation_equality_result = equalWithinTolerance( state1.orientation(), state2.orientation(), angular_tolerance); - auto angular_velocity_equality_result = equalWithinTolerance( - state1.angularVelocity(), state2.angularVelocity(), angular_tolerance); + auto angular_velocity_equality_result = + equalWithinTolerance(state1.angularVelocity(), state2.angularVelocity(), + AngularVelocity::fromRadians(angular_tolerance.toRadians())); auto assertion_result = ::testing::AssertionSuccess();