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

[wpimath] Replace frc/EigenCore.h typedefs with Eigen's where possible (#5597)

Browse Source
This commit is contained in:
Tyler Veness
2023-08-31 11:03:37 -07:00
committed by GitHub
parent 383289bc4b
commit 99f66b1e24
25 changed files with 79 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
View File

@@ -41,23 +41,23 @@ Rotation3d::Rotation3d(units::radian_t roll, units::radian_t pitch,
Rotation3d::Rotation3d(const Eigen::Vector3d& rvec)
: Rotation3d{rvec, units::radian_t{rvec.norm()}} {}
Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) {
Rotation3d::Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle) {
double norm = axis.norm();
if (norm == 0.0) {
return;
}
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
Vectord<3> v = axis / norm * units::math::sin(angle / 2.0);
Eigen::Vector3d v = axis / norm * units::math::sin(angle / 2.0);
m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
}
Rotation3d::Rotation3d(const Matrixd<3, 3>& rotationMatrix) {
Rotation3d::Rotation3d(const Eigen::Matrix3d& rotationMatrix) {
const auto& R = rotationMatrix;
// Require that the rotation matrix is special orthogonal. This is true if the
// matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
if ((R * R.transpose() - Matrixd<3, 3>::Identity()).norm() > 1e-9) {
if ((R * R.transpose() - Eigen::Matrix3d::Identity()).norm() > 1e-9) {
std::string msg =
fmt::format("Rotation matrix isn't orthogonal\n\nR =\n{}\n", R);
@@ -112,7 +112,8 @@ Rotation3d::Rotation3d(const Matrixd<3, 3>& rotationMatrix) {
m_q = Quaternion{w, x, y, z};
}
Rotation3d::Rotation3d(const Vectord<3>& initial, const Vectord<3>& final) {
Rotation3d::Rotation3d(const Eigen::Vector3d& initial,
const Eigen::Vector3d& final) {
double dot = initial.dot(final);
double normProduct = initial.norm() * final.norm();
double dotNorm = dot / normProduct;
@@ -230,7 +231,7 @@ units::radian_t Rotation3d::Z() const {
}
}
Vectord<3> Rotation3d::Axis() const {
Eigen::Vector3d Rotation3d::Axis() const {
double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
if (norm == 0.0) {
return {0.0, 0.0, 0.0};