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

[wpimath] Make geometry classes constexpr (#7222)

Browse Source
This commit is contained in:
Tyler Veness
2024-10-18 16:08:41 -07:00
committed by GitHub
parent 2054d0f57e
commit 95b9bd880b
30 changed files with 1324 additions and 1114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
wpimath/src/main/native/include/frc/geometry/Transform3d.h
View File

@@ -4,13 +4,15 @@
#pragma once
#include <utility>
#include <wpi/SymbolExports.h>
#include "frc/geometry/Translation3d.h"
namespace frc {
class WPILIB_DLLEXPORT Pose3d;
class Pose3d;
/**
* Represents a transformation for a Pose3d in the pose's frame.
@@ -23,7 +25,7 @@ class WPILIB_DLLEXPORT Transform3d {
* @param initial The initial pose for the transformation.
* @param final The final pose for the transformation.
*/
Transform3d(Pose3d initial, Pose3d final);
constexpr Transform3d(const Pose3d& initial, const Pose3d& final);
/**
* Constructs a transform with the given translation and rotation components.
@@ -31,7 +33,9 @@ class WPILIB_DLLEXPORT Transform3d {
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
Transform3d(Translation3d translation, Rotation3d rotation);
constexpr Transform3d(Translation3d translation, Rotation3d rotation)
: m_translation{std::move(translation)},
m_rotation{std::move(rotation)} {}
/**
* Constructs a transform with x, y, and z translations instead of a separate
@@ -42,8 +46,9 @@ class WPILIB_DLLEXPORT Transform3d {
* @param z The z component of the translational component of the transform.
* @param rotation The rotational component of the transform.
*/
Transform3d(units::meter_t x, units::meter_t y, units::meter_t z,
Rotation3d rotation);
constexpr 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)} {}
/**
* Constructs the identity transform -- maps an initial pose to itself.
@@ -55,42 +60,47 @@ class WPILIB_DLLEXPORT Transform3d {
*
* @return Reference to the translational component of the transform.
*/
const Translation3d& Translation() const { return m_translation; }
constexpr const Translation3d& Translation() const { return m_translation; }
/**
* Returns the X component of the transformation's translation.
*
* @return The x component of the transformation's translation.
*/
units::meter_t X() const { return m_translation.X(); }
constexpr units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the transformation's translation.
*
* @return The y component of the transformation's translation.
*/
units::meter_t Y() const { return m_translation.Y(); }
constexpr units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the Z component of the transformation's translation.
*
* @return The z component of the transformation's translation.
*/
units::meter_t Z() const { return m_translation.Z(); }
constexpr units::meter_t Z() const { return m_translation.Z(); }
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
const Rotation3d& Rotation() const { return m_rotation; }
constexpr const Rotation3d& Rotation() const { return m_rotation; }
/**
* Invert the transformation. This is useful for undoing a transformation.
*
* @return The inverted transformation.
*/
Transform3d Inverse() const;
constexpr Transform3d Inverse() const {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
return Transform3d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
}
/**
* Multiplies the transform by the scalar.
@@ -98,7 +108,7 @@ class WPILIB_DLLEXPORT Transform3d {
* @param scalar The scalar.
* @return The scaled Transform3d.
*/
Transform3d operator*(double scalar) const {
constexpr Transform3d operator*(double scalar) const {
return Transform3d(m_translation * scalar, m_rotation * scalar);
}
@@ -108,7 +118,9 @@ class WPILIB_DLLEXPORT Transform3d {
* @param scalar The scalar.
* @return The scaled Transform3d.
*/
Transform3d operator/(double scalar) const { return *this * (1.0 / scalar); }
constexpr Transform3d operator/(double scalar) const {
return *this * (1.0 / scalar);
}
/**
* Composes two transformations. The second transform is applied relative to
@@ -117,17 +129,38 @@ class WPILIB_DLLEXPORT Transform3d {
* @param other The transform to compose with this one.
* @return The composition of the two transformations.
*/
Transform3d operator+(const Transform3d& other) const;
constexpr Transform3d operator+(const Transform3d& other) const;
/**
* Checks equality between this Transform3d and another object.
*/
bool operator==(const Transform3d&) const = default;
constexpr bool operator==(const Transform3d&) const = default;
private:
Translation3d m_translation;
Rotation3d m_rotation;
};
} // namespace frc
#include "frc/geometry/Pose3d.h"
namespace frc {
constexpr Transform3d::Transform3d(const Pose3d& initial, const Pose3d& final) {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
m_translation = (final.Translation() - initial.Translation())
.RotateBy(-initial.Rotation());
m_rotation = final.Rotation() - initial.Rotation();
}
constexpr Transform3d Transform3d::operator+(const Transform3d& other) const {
return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
}
} // namespace frc
#ifndef NO_PROTOBUF