Skip to content
This repository has been archived by the owner on Jul 8, 2024. It is now read-only.

Commit

Permalink
Remove Rotation2 normalization and assertions
Browse files Browse the repository at this point in the history
For our use cases, users are only ever passing angles or cos/sin pairs
into Rotation2. This also avoids normalization bugs caused by
uninitialized autodiff variables, though that could have just been
special-cased.
  • Loading branch information
calcmogul committed Jun 26, 2024
1 parent 53cce14 commit c6e46da
Show file tree
Hide file tree
Showing 2 changed files with 6 additions and 18 deletions.
21 changes: 5 additions & 16 deletions include/trajopt/geometry/Rotation2.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,24 +34,13 @@ class Rotation2 {
: m_cos{cos(angle)}, m_sin{sin(angle)} {} // NOLINT

/**
* Constructs a rotation with the given x and y (cosine and sine) components.
* The x and y don't have to be normalized.
* Constructs a rotation with the given cosine and sine components.
*
* @param x The x component or cosine of the rotation.
* @param y The y component or sine of the rotation.
* @param cos The cosine component of the rotation.
* @param sin The sine component of the rotation.
*/
constexpr Rotation2(T x, T y) : m_cos{std::move(x)}, m_sin{std::move(y)} {
auto magnitude = hypot(m_cos, m_sin); // NOLINT
if (magnitude > 1e-6) {
if (abs(m_cos * m_cos + m_sin * m_sin - 1.0) > 1e-9) { // NOLINT
m_cos /= magnitude;
m_sin /= magnitude;
}
} else {
m_cos = 1.0;
m_sin = 0.0;
}
}
constexpr Rotation2(T cos, T sin)
: m_cos{std::move(cos)}, m_sin{std::move(sin)} {}

/**
* Coerces one rotation type into another.
Expand Down
3 changes: 1 addition & 2 deletions test/src/geometry/Translation2dTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -76,8 +76,7 @@ TEST_CASE("Translation2d - Angle", "[Translation2d]") {
const trajopt::Translation2d two{2.0, 5.0};

CHECK(one.Angle().Radians() == std::atan2(3.0, 1.0));
CHECK(two.Angle().Radians() ==
Catch::Approx(std::atan2(5.0, 2.0)).margin(1e-9));
CHECK(two.Angle().Radians() == std::atan2(5.0, 2.0));
}

TEST_CASE("Translation2d - Dot", "[Translation2d]") {
Expand Down

0 comments on commit c6e46da

Please sign in to comment.