Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-27 02:01: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) +

4
wpimath/src/main/native/include/frc/geometry/Pose2d.inc
View File

@@ -13,11 +13,11 @@
namespace frc {
constexpr Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
: m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
constexpr Pose2d::Pose2d(units::meter_t x, units::meter_t y,
Rotation2d rotation)
: m_translation(x, y), m_rotation(std::move(rotation)) {}
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
constexpr Pose2d Pose2d::operator+(const Transform2d& other) const {
return TransformBy(other);

8
wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
View File

@@ -12,9 +12,9 @@
namespace frc {
constexpr Rotation2d::Rotation2d(units::angle_unit auto value)
: m_value(value),
m_cos(gcem::cos(value.template convert<units::radian>().value())),
m_sin(gcem::sin(value.template convert<units::radian>().value())) {}
: m_value{value},
m_cos{gcem::cos(value.template convert<units::radian>().value())},
m_sin{gcem::sin(value.template convert<units::radian>().value())} {}
constexpr Rotation2d::Rotation2d(double x, double y) {
double magnitude = gcem::hypot(x, y);
@@ -33,7 +33,7 @@ constexpr Rotation2d Rotation2d::operator-() const {
}
constexpr Rotation2d Rotation2d::operator*(double scalar) const {
return Rotation2d(m_value * scalar);
return Rotation2d{m_value * scalar};
}
constexpr Rotation2d Rotation2d::operator+(const Rotation2d& other) const {

4
wpimath/src/main/native/include/frc/geometry/Transform2d.inc
View File

@@ -14,11 +14,11 @@ namespace frc {
constexpr Transform2d::Transform2d(Translation2d translation,
Rotation2d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
: m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
constexpr Transform2d::Transform2d(units::meter_t x, units::meter_t y,
Rotation2d rotation)
: m_translation(x, y), m_rotation(std::move(rotation)) {}
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
constexpr Transform2d Transform2d::Inverse() const {
// We are rotating the difference between the translations

16
wpimath/src/main/native/include/frc/geometry/Translation2d.h
View File

@@ -7,6 +7,7 @@
#include <initializer_list>
#include <span>
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
@@ -48,6 +49,14 @@ class WPILIB_DLLEXPORT Translation2d {
*/
constexpr Translation2d(units::meter_t distance, const Rotation2d& angle);
/**
* Constructs a Translation2d from the provided translation vector's X and Y
* components. The values are assumed to be in meters.
*
* @param vector The translation vector to represent.
*/
explicit Translation2d(const Eigen::Vector2d& vector);
/**
* Calculates the distance between two translations in 2D space.
*
@@ -73,6 +82,13 @@ class WPILIB_DLLEXPORT Translation2d {
*/
constexpr units::meter_t Y() const { return m_y; }
/**
* Returns a vector representation of this translation.
*
* @return A Vector representation of this translation.
*/
constexpr Eigen::Vector2d ToVector() const;
/**
* Returns the norm, or distance from the origin to the translation.
*

8
wpimath/src/main/native/include/frc/geometry/Translation2d.inc
View File

@@ -10,11 +10,15 @@
namespace frc {
constexpr Translation2d::Translation2d(units::meter_t x, units::meter_t y)
: m_x(x), m_y(y) {}
: m_x{x}, m_y{y} {}
constexpr Translation2d::Translation2d(units::meter_t distance,
const Rotation2d& angle)
: m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {}
: m_x{distance * angle.Cos()}, m_y{distance * angle.Sin()} {}
constexpr Eigen::Vector2d Translation2d::ToVector() const {
return Eigen::Vector2d{{m_x.value(), m_y.value()}};
}
constexpr Rotation2d Translation2d::Angle() const {
return Rotation2d{m_x.value(), m_y.value()};

16
wpimath/src/main/native/include/frc/geometry/Translation3d.h
View File

@@ -4,6 +4,7 @@
#pragma once
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
@@ -47,6 +48,14 @@ class WPILIB_DLLEXPORT Translation3d {
*/
Translation3d(units::meter_t distance, const Rotation3d& angle);
/**
* Constructs a Translation3d from the provided translation vector's X, Y, and
* Z components. The values are assumed to be in meters.
*
* @param vector The translation vector to represent.
*/
explicit Translation3d(const Eigen::Vector3d& vector);
/**
* Calculates the distance between two translations in 3D space.
*
@@ -80,6 +89,13 @@ class WPILIB_DLLEXPORT Translation3d {
*/
constexpr units::meter_t Z() const { return m_z; }
/**
* Returns a vector representation of this translation.
*
* @return A Vector representation of this translation.
*/
constexpr Eigen::Vector3d ToVector() const;
/**
* Returns the norm, or distance from the origin to the translation.
*

6
wpimath/src/main/native/include/frc/geometry/Translation3d.inc
View File

@@ -13,12 +13,16 @@ namespace frc {
constexpr Translation3d::Translation3d(units::meter_t x, units::meter_t y,
units::meter_t z)
: m_x(x), m_y(y), m_z(z) {}
: m_x{x}, m_y{y}, m_z{z} {}
constexpr Translation2d Translation3d::ToTranslation2d() const {
return Translation2d{m_x, m_y};
}
constexpr Eigen::Vector3d Translation3d::ToVector() const {
return Eigen::Vector3d{{m_x.value(), m_y.value(), m_z.value()}};
}
constexpr Translation3d Translation3d::operator+(
const Translation3d& other) const {
return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};