Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add rotation matrix constructor to Rotation3d (#4413)

Browse Source
This commit is contained in:
Tyler Veness
2022-09-17 00:17:30 -07:00
committed by GitHub
parent 9730032866
commit ab1baf4832
9 changed files with 208 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
View File

@@ -7,6 +7,7 @@
#include <wpi/MathExtras.h>
#include <wpi/numbers>
#include "frc/EigenCore.h"
#include "frc/geometry/Rotation3d.h"
#include "gtest/gtest.h"
@@ -29,6 +30,30 @@ TEST(Rotation3dTest, InitAxisAngleAndRollPitchYaw) {
EXPECT_EQ(rot5, rot6);
}
TEST(Rotation3dTest, InitRotationMatrix) {
// No rotation
const Matrixd<3, 3> R1 = Matrixd<3, 3>::Identity();
const Rotation3d rot1{R1};
EXPECT_EQ(Rotation3d{}, rot1);
// 90 degree CCW rotation around z-axis
Matrixd<3, 3> R2;
R2.block<3, 1>(0, 0) = Vectord<3>{0.0, 1.0, 0.0};
R2.block<3, 1>(0, 1) = Vectord<3>{-1.0, 0.0, 0.0};
R2.block<3, 1>(0, 2) = Vectord<3>{0.0, 0.0, 1.0};
const Rotation3d rot2{R2};
const Rotation3d expected2{0_deg, 0_deg, 90_deg};
EXPECT_EQ(expected2, rot2);
// Matrix that isn't orthogonal
const Matrixd<3, 3> R3{{1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}};
EXPECT_THROW(Rotation3d{R3}, std::domain_error);
// Matrix that's orthogonal but not special orthogonal
const Matrixd<3, 3> R4 = Matrixd<3, 3>::Identity() * 2.0;
EXPECT_THROW(Rotation3d{R4}, std::domain_error);
}
TEST(Rotation3dTest, InitTwoVector) {
const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};