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 cc21417cd7..380820c28c 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" @@ -482,9 +483,9 @@ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( createAngle(params.start_angle()), createAngle(params.final_angle()), initial_velocity, AngularVelocity::fromRadians(robot_constants.robot_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 9cdde04170..0d00657009 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 b3821d363f..1b60f7e8ab 100644 --- a/src/software/ai/hl/stp/play/passing_sim_test.py +++ b/src/software/ai/hl/stp/play/passing_sim_test.py @@ -67,8 +67,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) @@ -80,8 +80,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/attacker/attacker_tactic_keep_away_test.cpp b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_keep_away_test.cpp index 1e10729f3c..c16fb06b19 100644 --- a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_keep_away_test.cpp +++ b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_keep_away_test.cpp @@ -196,7 +196,7 @@ INSTANTIATE_TEST_CASE_P( // the state of the friendly robot RobotStateWithId{ 1, RobotState(Point(0.25, 0), Vector(0, 0), Angle::fromDegrees(180), - Angle::fromDegrees(0))}, + AngularVelocity::fromDegrees(0))}, // the state of the ball BallState(Point(0., 0.), Vector(0, 0)), // the states of the enemy robots @@ -227,8 +227,9 @@ INSTANTIATE_TEST_CASE_P( Pass(Point(FIELD_TOP_LEFT.x() + 0.05, FIELD_TOP_LEFT.y() - 0.05), Point(0, 0), 5), // the state of the friendly robot - RobotStateWithId{1, RobotState(FIELD_TOP_LEFT, Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}, + RobotStateWithId{ + 1, RobotState(FIELD_TOP_LEFT, Vector(0, 0), Angle::fromDegrees(0), + AngularVelocity::fromDegrees(0))}, // the state of the ball BallState(Point(FIELD_TOP_LEFT.x() + 0.05, FIELD_TOP_LEFT.y() - 0.2), Vector(0, 0)), diff --git a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_passing_test.cpp b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_passing_test.cpp index 14a0d6d96b..06312256fb 100644 --- a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_passing_test.cpp +++ b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_passing_test.cpp @@ -5,6 +5,7 @@ #include "software/ai/hl/stp/tactic/attacker/attacker_tactic.h" #include "software/ai/hl/stp/tactic/move/move_tactic.h" #include "software/geom/algorithms/contains.h" +#include "software/geom/angular_velocity.h" #include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" #include "software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h" #include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h" @@ -73,23 +74,23 @@ INSTANTIATE_TEST_CASE_P( // Stationary Ball Tests // Attacker point != Balls location & Balls location != Robots Location std::make_tuple(Pass(Point(0.0, 0.5), Point(0, 0), 5), - RobotStateWithId{ - 1, RobotState(Point(0, 0), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}, + RobotStateWithId{1, RobotState(Point(0, 0), Vector(0, 0), + Angle::fromDegrees(0), + AngularVelocity::fromDegrees(0))}, BallState(Point(0.5, 0.5), Vector(0, 0))), // Attacker point == Balls location & Balls location != Robots Location std::make_tuple(Pass(Point(-0.5, -0.5), Point(0, 0), 5), - RobotStateWithId{ - 1, RobotState(Point(0, 0), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}, + RobotStateWithId{1, RobotState(Point(0, 0), Vector(0, 0), + Angle::fromDegrees(0), + AngularVelocity::fromDegrees(0))}, BallState(Point(-0.5, -0.5), Vector(0, 0))), // Attacker point != Balls location & Balls location == Robots Location std::make_tuple(Pass(Point(0.5, 0.5), Point(0, 1), 5), - RobotStateWithId{ - 1, RobotState(Point(0.5, 0.5), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}, + RobotStateWithId{1, RobotState(Point(0.5, 0.5), Vector(0, 0), + Angle::fromDegrees(0), + AngularVelocity::fromDegrees(0))}, BallState(Point(-0.5, 0.5), Vector(0, 0))), // TODO(#2909) : Enable test once the robot can turn faster and hits the ball with @@ -103,24 +104,24 @@ INSTANTIATE_TEST_CASE_P( // Attacker point far away (not a normal use case, but just to sanity check) std::make_tuple(Pass(Point(0.0, 0.0), Point(0, 0), 5), - RobotStateWithId{ - 1, RobotState(Point(3.5, 2.5), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}, + RobotStateWithId{1, RobotState(Point(3.5, 2.5), Vector(0, 0), + Angle::fromDegrees(0), + AngularVelocity::fromDegrees(0))}, BallState(Point(0.0, 0.0), Vector(0, 0))), // Attacker point != Balls location & Balls location != Robots Location std::make_tuple(Pass(Point(0.0, 0.5), Point(0, 0), 5), - RobotStateWithId{ - 1, RobotState(Point(0, 0), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}, + RobotStateWithId{1, RobotState(Point(0, 0), Vector(0, 0), + Angle::fromDegrees(0), + AngularVelocity::fromDegrees(0))}, BallState(Point(0.5, 0.5), Vector(0, 0))), // Moving Ball Tests // Attacker point == Balls location & Balls location != Robots Location std::make_tuple(Pass(Point(-0.5, -0.5), Point(0, 0), 5), - RobotStateWithId{ - 1, RobotState(Point(0, 0), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}, + RobotStateWithId{1, RobotState(Point(0, 0), Vector(0, 0), + Angle::fromDegrees(0), + AngularVelocity::fromDegrees(0))}, BallState(Point(-0.5, -0.5), Vector(1, 0))), // TODO (#2859): Robot does not kick ball when dribbler is off since it is @@ -135,7 +136,7 @@ INSTANTIATE_TEST_CASE_P( // Attacker point == Balls location & Balls location == Robots Location std::make_tuple(Pass(Point(0.0, 0.0), Point(0, 0), 5), - RobotStateWithId{ - 1, RobotState(Point(0, 0), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}, + RobotStateWithId{1, RobotState(Point(0, 0), Vector(0, 0), + Angle::fromDegrees(0), + AngularVelocity::fromDegrees(0))}, BallState(Point(0.0, 0.0), Vector(1, 0))))); diff --git a/src/software/ai/hl/stp/tactic/move/move_tactic_test.cpp b/src/software/ai/hl/stp/tactic/move/move_tactic_test.cpp index aba0b570a8..98a5844a36 100644 --- a/src/software/ai/hl/stp/tactic/move/move_tactic_test.cpp +++ b/src/software/ai/hl/stp/tactic/move/move_tactic_test.cpp @@ -159,8 +159,8 @@ TEST_F(MoveTacticTest, test_spinning_move_clockwise) BallState ball_state(Point(1, 1), Vector(0, 0)); auto friendly_robots = {RobotStateWithId{ .id = 0, - .robot_state = RobotState(initial_position, Vector(0, 0), Angle::zero(), - AngularVelocity::quarter())}}; + .robot_state = RobotState(initial_position, Vector(0, 0), -Angle::quarter(), + AngularVelocity::zero())}}; auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(4, 0)}); auto tactic = std::make_shared(std::make_shared()); @@ -175,8 +175,8 @@ TEST_F(MoveTacticTest, test_spinning_move_clockwise) [destination, tactic](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { - robotAtAngularVelocity(0, world_ptr, AngularVelocity::fromDegrees(1 * 360), - AngularVelocity::fromDegrees(50), yield); + robotAtAngularVelocity(0, world_ptr, AngularVelocity::fromDegrees(10), + AngularVelocity::fromDegrees(5), yield); robotAtPosition(0, world_ptr, destination, 0.05, yield); robotAtOrientation(0, world_ptr, Angle::zero(), Angle::fromDegrees(5), yield); while (!tactic->done()) @@ -207,7 +207,7 @@ TEST_F(MoveTacticTest, test_spinning_move_counter_clockwise) BallState ball_state(Point(1, 1), Vector(0, 0)); auto friendly_robots = {RobotStateWithId{ .id = 0, - .robot_state = RobotState(initial_position, Vector(0, 0), Angle::quarter(), + .robot_state = RobotState(initial_position, Vector(0, 0), -Angle::quarter(), AngularVelocity::zero())}}; auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(4, 0)}); @@ -223,8 +223,8 @@ TEST_F(MoveTacticTest, test_spinning_move_counter_clockwise) [destination, tactic](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) { - robotAtAngularVelocity(0, world_ptr, AngularVelocity::fromDegrees(-4 * 360), - AngularVelocity::fromDegrees(50), yield); + robotAtAngularVelocity(0, world_ptr, AngularVelocity::fromDegrees(-10), + AngularVelocity::fromDegrees(5), yield); robotAtPosition(0, world_ptr, destination, 0.05, yield); robotAtOrientation(0, world_ptr, Angle::half(), Angle::fromDegrees(5), yield); while (!tactic->done()) diff --git a/src/software/ai/hl/stp/tactic/move_primitive.cpp b/src/software/ai/hl/stp/tactic/move_primitive.cpp index 41028248f7..0905304ea5 100644 --- a/src/software/ai/hl/stp/tactic/move_primitive.cpp +++ b/src/software/ai/hl/stp/tactic/move_primitive.cpp @@ -5,6 +5,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( const Robot &robot, const Point &destination, const Angle &final_angle, @@ -38,9 +39,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/ai/hl/stp/tactic/penalty_kick/penalty_kick_tactic_test.cpp b/src/software/ai/hl/stp/tactic/penalty_kick/penalty_kick_tactic_test.cpp index b0392cf567..7767cd806f 100644 --- a/src/software/ai/hl/stp/tactic/penalty_kick/penalty_kick_tactic_test.cpp +++ b/src/software/ai/hl/stp/tactic/penalty_kick/penalty_kick_tactic_test.cpp @@ -4,6 +4,7 @@ #include +#include "software/geom/angular_velocity.h" #include "software/simulated_tests/non_terminating_validation_functions/ball_in_play_or_scored_validation.h" #include "software/simulated_tests/non_terminating_validation_functions/ball_never_moves_backward_validation.h" #include "software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.h" @@ -21,8 +22,8 @@ class PenaltyKickTacticTest : public SimulatedErForceSimPlayTestFixture, Field field = Field::createField(field_type); BallState ball = BallState(field.friendlyPenaltyMark(), Vector(0, 0)); Point initial_position = field.friendlyPenaltyMark() + Vector(-0.1, 0); - RobotStateWithId shooter = { - 0, RobotState(initial_position, Vector(0, 0), Angle::zero(), Angle::zero())}; + RobotStateWithId shooter = {0, RobotState(initial_position, Vector(0, 0), + Angle::zero(), AngularVelocity::zero())}; }; // TODO (#2232): Improve dribbling control so the ball is not lost during this test @@ -79,22 +80,23 @@ INSTANTIATE_TEST_CASE_P( RobotLocations, PenaltyKickTacticTest, ::testing::Values( // enemy robot stationary at centre of goal - RobotStateWithId{0, RobotState(Field::createSSLDivisionBField().enemyGoalCenter(), - Vector(0, 0), Angle::half(), Angle::zero())}, + RobotStateWithId{ + 0, RobotState(Field::createSSLDivisionBField().enemyGoalCenter(), + Vector(0, 0), Angle::half(), AngularVelocity::zero())}, // enemy robot stationary left of net - RobotStateWithId{0, - RobotState(Field::createSSLDivisionBField().enemyGoalpostNeg(), - Vector(0, 0), Angle::half(), Angle::zero())}, + RobotStateWithId{ + 0, RobotState(Field::createSSLDivisionBField().enemyGoalpostNeg(), + Vector(0, 0), Angle::half(), AngularVelocity::zero())}, // enemy robot stationary right of net - RobotStateWithId{0, - RobotState(Field::createSSLDivisionBField().enemyGoalpostPos(), - Vector(0, 0), Angle::half(), Angle::zero())}, + RobotStateWithId{ + 0, RobotState(Field::createSSLDivisionBField().enemyGoalpostPos(), + Vector(0, 0), Angle::half(), AngularVelocity::zero())}, // enemy robot left of net but moving right - RobotStateWithId{0, - RobotState(Field::createSSLDivisionBField().enemyGoalpostNeg(), - Vector(0, 1.2), Angle::half(), Angle::zero())}, + RobotStateWithId{ + 0, RobotState(Field::createSSLDivisionBField().enemyGoalpostNeg(), + Vector(0, 1.2), Angle::half(), AngularVelocity::zero())}, // enemy robot right of net but moving left - RobotStateWithId{0, - RobotState(Field::createSSLDivisionBField().enemyGoalpostPos(), - Vector(0, -1.2), Angle::half(), Angle::zero())})); + RobotStateWithId{ + 0, RobotState(Field::createSSLDivisionBField().enemyGoalpostPos(), + Vector(0, -1.2), Angle::half(), AngularVelocity::zero())})); diff --git a/src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.cpp b/src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.cpp index 94273fcb90..8488a45aa4 100644 --- a/src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.cpp +++ b/src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.cpp @@ -6,6 +6,7 @@ #include "software/ai/hl/stp/tactic/move/move_tactic.h" #include "software/geom/algorithms/contains.h" +#include "software/geom/angular_velocity.h" #include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" #include "software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h" #include "software/simulated_tests/terminating_validation_functions/friendly_scored_validation.h" @@ -89,27 +90,27 @@ INSTANTIATE_TEST_CASE_P( // Robot slighty off from receive point: test 2 std::make_tuple(Pass(Point(0.0, 0.4), Point(2, 2), 4), - RobotStateWithId{ - 1, RobotState(Point(2.5, 2.0), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}), + RobotStateWithId{1, RobotState(Point(2.5, 2.0), Vector(0, 0), + Angle::fromDegrees(0), + AngularVelocity::fromDegrees(0))}), // Robot facing away from pass std::make_tuple(Pass(Point(0.0, 0.0), Point(-3, 0), 4), RobotStateWithId{1, RobotState(Point(-3, 0), Vector(0, 0), Angle::fromDegrees(180), - Angle::fromDegrees(0))}), + AngularVelocity::fromDegrees(0))}), // Robot facing towards from pass std::make_tuple(Pass(Point(0.0, 0.0), Point(-3, 0), 4), - RobotStateWithId{ - 1, RobotState(Point(-3, 0), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}), + RobotStateWithId{1, RobotState(Point(-3, 0), Vector(0, 0), + Angle::fromDegrees(0), + AngularVelocity::fromDegrees(0))}), // Robot facing towards pass speedy std::make_tuple(Pass(Point(0.0, 0.0), Point(-3, 0), 5), - RobotStateWithId{ - 1, RobotState(Point(-3, 0), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}) + RobotStateWithId{1, RobotState(Point(-3, 0), Vector(0, 0), + Angle::fromDegrees(0), + AngularVelocity::fromDegrees(0))}) )); @@ -187,7 +188,7 @@ INSTANTIATE_TEST_CASE_P( std::make_tuple(Pass(Point(4.0, 1.5), Point(4, -1), 5), RobotStateWithId{1, RobotState(Point(4.0, -1), Vector(0, 0), Angle::fromDegrees(180), - Angle::fromDegrees(0))}), + AngularVelocity::fromDegrees(0))}), // TODO (#2570): re-enable when one-touch works for these tests // std::make_tuple(Pass(Point(4.0, 1.5), Point(3.5, -1), 5), @@ -205,7 +206,7 @@ INSTANTIATE_TEST_CASE_P( std::make_tuple(Pass(Point(4.0, -1.5), Point(4, 1), 5), RobotStateWithId{1, RobotState(Point(4.0, 1), Vector(0, 0), Angle::fromDegrees(180), - Angle::fromDegrees(0))}), + AngularVelocity::fromDegrees(0))}), // TODO (#2570): re-enable when one-touch works for these tests // std::make_tuple(Pass(Point(4.0, -1.5), Point(3.5, 1), 5), @@ -215,9 +216,9 @@ INSTANTIATE_TEST_CASE_P( // Angle::fromDegrees(0))}), std::make_tuple(Pass(Point(4.0, -1.5), Point(3.0, 1), 4.5), - RobotStateWithId{ - 1, RobotState(Point(3.0, 1), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}) + RobotStateWithId{1, RobotState(Point(3.0, 1), Vector(0, 0), + Angle::fromDegrees(0), + AngularVelocity::fromDegrees(0))}) // TODO (#2570): re-enable when one-touch works for these tests // Direct one touch diff --git a/src/software/geom/BUILD b/src/software/geom/BUILD index 7788c52f10..cb823772a8 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 cd0d1c0571..73b5d6ec21 100644 --- a/src/software/geom/angle.h +++ b/src/software/geom/angle.h @@ -1,591 +1,12 @@ #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. - */ -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 + * 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. */ -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 863c3bc207..097a34ca0d 100644 --- a/src/software/python_bindings.cpp +++ b/src/software/python_bindings.cpp @@ -209,6 +209,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()) @@ -323,7 +337,7 @@ PYBIND11_MODULE(python_bindings, m) m.def("contains", py::overload_cast(&contains)); 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 4f156d1039..ee4d55c3a0 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/simulated_tests/simulated_er_force_sim_test_fixture.cpp b/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp index a855576af0..68b9259d92 100644 --- a/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp +++ b/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp @@ -348,18 +348,18 @@ void SimulatedErForceSimTestFixture::runTest( for (size_t i = 0; i < num_robots; i++) { LOG(INFO) << "Robot " << i << std::endl; - LOG(INFO) << "max robot displacement: " << ball_displacement_stats.maximum - << std::endl; - LOG(INFO) << "min robot displacement: " << ball_displacement_stats.minimum - << std::endl; - LOG(INFO) << "avg robot displacement: " << ball_displacement_stats.average - << std::endl; - LOG(INFO) << "max robot velocity difference: " << ball_velocity_stats.maximum - << std::endl; - LOG(INFO) << "min robot velocity difference: " << ball_velocity_stats.minimum - << std::endl; - LOG(INFO) << "avg robot velocity difference: " << ball_velocity_stats.average - << std::endl; + LOG(INFO) << "max robot displacement: " + << robots_displacement_stats[i].maximum << std::endl; + LOG(INFO) << "min robot displacement: " + << robots_displacement_stats[i].minimum << std::endl; + LOG(INFO) << "avg robot displacement: " + << robots_displacement_stats[i].average << std::endl; + LOG(INFO) << "max robot velocity difference: " + << robots_velocity_stats[i].maximum << std::endl; + LOG(INFO) << "min robot velocity difference: " + << robots_velocity_stats[i].minimum << std::endl; + LOG(INFO) << "avg robot velocity difference: " + << robots_velocity_stats[i].average << std::endl; } validation_functions_done = diff --git a/src/software/simulated_tests/terminating_validation_functions/robot_state_validation.cpp b/src/software/simulated_tests/terminating_validation_functions/robot_state_validation.cpp index ad28599b70..60d03d3266 100644 --- a/src/software/simulated_tests/terminating_validation_functions/robot_state_validation.cpp +++ b/src/software/simulated_tests/terminating_validation_functions/robot_state_validation.cpp @@ -84,7 +84,7 @@ void robotAtAngularVelocity(RobotId robot_id, std::shared_ptr world_ptr, << "There is no robot with ID: " + std::to_string(robot_id); Robot robot = robot_optional.value(); - if (robot.angularVelocity().minDiff(angular_velocity) > + if ((robot.angularVelocity() - angular_velocity).abs() > close_to_angular_velocity_threshold) { return robot.angularVelocity(); diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index b58d4989a3..dfe4196fe0 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -370,7 +370,7 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( SSLSimulationProto::RobotControl robot_control; auto sim_state = getSimulatorState(); - std::map> current_velocity_map; + std::map> current_velocity_map; if (side == gameController::Team::BLUE) { const auto& sim_robots = sim_state.blue_robots(); @@ -574,7 +574,7 @@ ErForceSimulator::getRobotIdToLocalVelocityMap( const Vector local_vel = globalToLocalVelocity(Vector(sim_robot.v_x(), sim_robot.v_y()), Angle::fromRadians(sim_robot.angle())); - const AngularVelocity angular_vel = Angle::fromRadians(sim_robot.r_z()); + const AngularVelocity angular_vel = AngularVelocity::fromRadians(sim_robot.r_z()); robot_to_local_velocity[sim_robot.id()] = {local_vel, angular_vel}; } return robot_to_local_velocity; diff --git a/src/software/test_util/equal_within_tolerance.cpp b/src/software/test_util/equal_within_tolerance.cpp index b514b84963..b3029b76a9 100644 --- a/src/software/test_util/equal_within_tolerance.cpp +++ b/src/software/test_util/equal_within_tolerance.cpp @@ -4,9 +4,9 @@ namespace TestUtil { -::testing::AssertionResult equalWithinTolerance(const Duration &duration1, - const Duration &duration2, - const Duration &tolerance) +::testing::AssertionResult equalWithinTolerance(const Duration& duration1, + const Duration& duration2, + const Duration& tolerance) { if (std::abs((duration1 - duration2).toMilliseconds()) < std::abs(tolerance.toMilliseconds())) @@ -20,13 +20,13 @@ ::testing::AssertionResult equalWithinTolerance(const Duration &duration1, } } -::testing::AssertionResult equalWithinTolerance(const Polygon &poly1, - const Polygon &poly2, double tolerance) +::testing::AssertionResult equalWithinTolerance(const Polygon& poly1, + const Polygon& poly2, double tolerance) { auto ppts1 = poly1.getPoints(); auto ppts2 = poly2.getPoints(); if (std::equal(ppts1.begin(), ppts1.end(), ppts2.begin(), - [tolerance](const Point &p1, const Point &p2) + [tolerance](const Point& p1, const Point& p2) { return equalWithinTolerance(p1, p2, tolerance); })) { return ::testing::AssertionSuccess(); @@ -38,8 +38,8 @@ ::testing::AssertionResult equalWithinTolerance(const Polygon &poly1, } } -::testing::AssertionResult equalWithinTolerance(const Stadium &stadium1, - const Stadium &stadium2, double tolerance) +::testing::AssertionResult equalWithinTolerance(const Stadium& stadium1, + const Stadium& stadium2, double tolerance) { if (((equalWithinTolerance(stadium1.segment().getStart(), stadium2.segment().getStart(), tolerance) && @@ -60,7 +60,7 @@ ::testing::AssertionResult equalWithinTolerance(const Stadium &stadium1, } } -::testing::AssertionResult equalWithinTolerance(const Circle &c1, const Circle &c2, +::testing::AssertionResult equalWithinTolerance(const Circle& c1, const Circle& c2, double tolerance) { if (equalWithinTolerance(c1.origin(), c2.origin(), tolerance) && @@ -75,8 +75,8 @@ ::testing::AssertionResult equalWithinTolerance(const Circle &c1, const Circle & } } -::testing::AssertionResult equalWithinTolerance(const Angle &a1, const Angle &a2, - const Angle &tolerance) +::testing::AssertionResult equalWithinTolerance(const Angle& a1, const Angle& a2, + const Angle& tolerance) { // subtract a fixed epsilon for error in: // - angle subtraction (internal to minDiff) @@ -95,8 +95,27 @@ ::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, +::testing::AssertionResult equalWithinTolerance(const Vector& v1, const Vector& v2, double tolerance) { double distance = (v1 - v2).length(); @@ -111,7 +130,7 @@ ::testing::AssertionResult equalWithinTolerance(const Vector &v1, const Vector & } } -::testing::AssertionResult equalWithinTolerance(const Point &pt1, const Point &pt2, +::testing::AssertionResult equalWithinTolerance(const Point& pt1, const Point& pt2, double tolerance) { double dist = distance(pt1, pt2); @@ -143,10 +162,10 @@ ::testing::AssertionResult equalWithinTolerance(double val1, double val2, } } -::testing::AssertionResult equalWithinTolerance(const RobotState &state1, - const RobotState &state2, +::testing::AssertionResult equalWithinTolerance(const RobotState& state1, + const RobotState& state2, const double linear_tolerance, - const Angle &angular_tolerance) + const Angle& angular_tolerance) { auto position_equality_result = equalWithinTolerance(state1.position(), state2.position(), linear_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(); @@ -196,10 +216,10 @@ ::testing::AssertionResult equalWithinTolerance(const RobotState &state1, return assertion_result; } -::testing::AssertionResult equalWithinTolerance(const RobotStateWithId &state1, - const RobotStateWithId &state2, +::testing::AssertionResult equalWithinTolerance(const RobotStateWithId& state1, + const RobotStateWithId& state2, const double linear_tolerance, - const Angle &angular_tolerance) + const Angle& angular_tolerance) { if (state1.id != state2.id) { @@ -217,8 +237,8 @@ ::testing::AssertionResult equalWithinTolerance(const RobotStateWithId &state1, return ::testing::AssertionSuccess(); } -::testing::AssertionResult equalWithinTolerance(const BallState &state1, - const BallState &state2, double tolerance) +::testing::AssertionResult equalWithinTolerance(const BallState& state1, + const BallState& state2, double tolerance) { auto position_equality_result = equalWithinTolerance(state1.position(), state2.position(), tolerance); @@ -247,8 +267,8 @@ ::testing::AssertionResult equalWithinTolerance(const BallState &state1, return assertion_result; } -::testing::AssertionResult equalWithinTolerance(const Eigen::MatrixXd &matrix1, - const Eigen::MatrixXd &matrix2, +::testing::AssertionResult equalWithinTolerance(const Eigen::MatrixXd& matrix1, + const Eigen::MatrixXd& matrix2, double tolerance) { auto distance = matrix1 - matrix2;