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

[wpimath] Add vector projection and geometry vector conversions (#6343)

Browse Source
This commit is contained in:
Asa Paparo
2024-02-10 13:43:58 -05:00
committed by GitHub
parent 3207795d0d
commit 62cba9a4d3
21 changed files with 234 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
wpimath/src/main/native/cpp/geometry/Pose3d.cpp
View File

@@ -36,15 +36,15 @@ Eigen::Matrix3d RotationVectorToMatrix(const Eigen::Vector3d& rotation) {
} // namespace
Pose3d::Pose3d(Translation3d translation, Rotation3d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
: m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
Pose3d::Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
Rotation3d rotation)
: m_translation(x, y, z), m_rotation(std::move(rotation)) {}
: m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
Pose3d::Pose3d(const Pose2d& pose)
: m_translation(pose.X(), pose.Y(), 0_m),
m_rotation(0_rad, 0_rad, pose.Rotation().Radians()) {}
: m_translation{pose.X(), pose.Y(), 0_m},
m_rotation{0_rad, 0_rad, pose.Rotation().Radians()} {}
Pose3d Pose3d::operator+(const Transform3d& other) const {
return TransformBy(other);

4
wpimath/src/main/native/cpp/geometry/Transform3d.cpp
View File

@@ -19,11 +19,11 @@ Transform3d::Transform3d(Pose3d initial, Pose3d final) {
}
Transform3d::Transform3d(Translation3d translation, Rotation3d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
: m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
Transform3d::Transform3d(units::meter_t x, units::meter_t y, units::meter_t z,
Rotation3d rotation)
: m_translation(x, y, z), m_rotation(std::move(rotation)) {}
: m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
Transform3d Transform3d::Inverse() const {
// We are rotating the difference between the translations

3
wpimath/src/main/native/cpp/geometry/Translation2d.cpp
View File

@@ -11,6 +11,9 @@
using namespace frc;
Translation2d::Translation2d(const Eigen::Vector2d& vector)
: m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}} {}
units::meter_t Translation2d::Distance(const Translation2d& other) const {
return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
}

5
wpimath/src/main/native/cpp/geometry/Translation3d.cpp
View File

@@ -19,6 +19,11 @@ Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
m_z = rectangular.Z();
}
Translation3d::Translation3d(const Eigen::Vector3d& vector)
: m_x{units::meter_t{vector.x()}},
m_y{units::meter_t{vector.y()}},
m_z{units::meter_t{vector.z()}} {}
units::meter_t Translation3d::Distance(const Translation3d& other) const {
return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) +
units::math::pow<2>(other.m_y - m_y) +