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] Merge .inc files into headers (#7209)

Browse Source 
Splitting the files didn't help readability or save compilation time and
it confused contributors. Merging them is also in line with how C++
modules will be written.
This commit is contained in:
Tyler Veness
2024-10-14 16:08:10 -07:00
committed by GitHub
parent bedfc09268
commit ee281ea448
55 changed files with 1646 additions and 2531 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
wpimath/src/main/native/include/frc/geometry/Pose2d.h
View File

@@ -6,6 +6,7 @@
#include <initializer_list>
#include <span>
#include <utility>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
@@ -14,6 +15,7 @@
#include "frc/geometry/Transform2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Twist2d.h"
#include "units/length.h"
namespace frc {
@@ -33,7 +35,9 @@ class WPILIB_DLLEXPORT Pose2d {
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
*/
constexpr Pose2d(Translation2d translation, Rotation2d rotation);
constexpr Pose2d(Translation2d translation, Rotation2d rotation)
: m_translation{std::move(translation)},
m_rotation{std::move(rotation)} {}
/**
* Constructs a pose with x and y translations instead of a separate
@@ -43,7 +47,8 @@ class WPILIB_DLLEXPORT Pose2d {
* @param y The y component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
constexpr Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
constexpr Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
/**
* Transforms the pose by the given transformation and returns the new
@@ -59,7 +64,9 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The transformed pose.
*/
constexpr Pose2d operator+(const Transform2d& other) const;
constexpr Pose2d operator+(const Transform2d& other) const {
return TransformBy(other);
}
/**
* Returns the Transform2d that maps the one pose to another.
@@ -109,7 +116,9 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The new scaled Pose2d.
*/
constexpr Pose2d operator*(double scalar) const;
constexpr Pose2d operator*(double scalar) const {
return Pose2d{m_translation * scalar, m_rotation * scalar};
}
/**
* Divides the current pose by a scalar.
@@ -118,7 +127,9 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The new scaled Pose2d.
*/
constexpr Pose2d operator/(double scalar) const;
constexpr Pose2d operator/(double scalar) const {
return *this * (1.0 / scalar);
}
/**
* Rotates the pose around the origin and returns the new pose.
@@ -127,7 +138,9 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The rotated pose.
*/
constexpr Pose2d RotateBy(const Rotation2d& other) const;
constexpr Pose2d RotateBy(const Rotation2d& other) const {
return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
}
/**
* Transforms the pose by the given transformation and returns the new pose.
@@ -137,7 +150,10 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The transformed pose.
*/
constexpr Pose2d TransformBy(const Transform2d& other) const;
constexpr Pose2d TransformBy(const Transform2d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
other.Rotation() + m_rotation};
}
/**
* Returns the current pose relative to the given pose.
@@ -217,4 +233,3 @@ void from_json(const wpi::json& json, Pose2d& pose);
#include "frc/geometry/proto/Pose2dProto.h"
#endif
#include "frc/geometry/struct/Pose2dStruct.h"
#include "frc/geometry/Pose2d.inc"

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

@@ -1,43 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <utility>
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "units/length.h"
namespace frc {
constexpr Pose2d::Pose2d(Translation2d translation, Rotation2d 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)} {}
constexpr Pose2d Pose2d::operator+(const Transform2d& other) const {
return TransformBy(other);
}
constexpr Pose2d Pose2d::operator*(double scalar) const {
return Pose2d{m_translation * scalar, m_rotation * scalar};
}
constexpr Pose2d Pose2d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
constexpr Pose2d Pose2d::RotateBy(const Rotation2d& other) const {
return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
}
constexpr Pose2d Pose2d::TransformBy(const Transform2d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
other.Rotation() + m_rotation};
}
} // namespace frc

55
wpimath/src/main/native/include/frc/geometry/Rotation2d.h
View File

@@ -4,10 +4,15 @@
#pragma once
#include <type_traits>
#include <gcem.hpp>
#include <wpi/StackTrace.h>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
#include "units/angle.h"
#include "wpimath/MathShared.h"
namespace frc {
@@ -32,7 +37,10 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @param value The value of the angle.
*/
constexpr Rotation2d(units::angle_unit auto value); // NOLINT
constexpr Rotation2d(units::angle_unit auto value) // NOLINT
: m_value{value},
m_cos{gcem::cos(value.template convert<units::radian>().value())},
m_sin{gcem::sin(value.template convert<units::radian>().value())} {}
/**
* Constructs a Rotation2d with the given x and y (cosine and sine)
@@ -41,7 +49,22 @@ class WPILIB_DLLEXPORT Rotation2d {
* @param x The x component or cosine of the rotation.
* @param y The y component or sine of the rotation.
*/
constexpr Rotation2d(double x, double y);
constexpr Rotation2d(double x, double y) {
double magnitude = gcem::hypot(x, y);
if (magnitude > 1e-6) {
m_sin = y / magnitude;
m_cos = x / magnitude;
} else {
m_sin = 0.0;
m_cos = 1.0;
if (!std::is_constant_evaluated()) {
wpi::math::MathSharedStore::ReportError(
"x and y components of Rotation2d are zero\n{}",
wpi::GetStackTrace(1));
}
}
m_value = units::radian_t{gcem::atan2(m_sin, m_cos)};
}
/**
* Adds two rotations together, with the result being bounded between -pi and
@@ -54,7 +77,9 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The sum of the two rotations.
*/
constexpr Rotation2d operator+(const Rotation2d& other) const;
constexpr Rotation2d operator+(const Rotation2d& other) const {
return RotateBy(other);
}
/**
* Subtracts the new rotation from the current rotation and returns the new
@@ -67,7 +92,9 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The difference between the two rotations.
*/
constexpr Rotation2d operator-(const Rotation2d& other) const;
constexpr Rotation2d operator-(const Rotation2d& other) const {
return *this + -other;
}
/**
* Takes the inverse of the current rotation. This is simply the negative of
@@ -75,7 +102,7 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The inverse of the current rotation.
*/
constexpr Rotation2d operator-() const;
constexpr Rotation2d operator-() const { return Rotation2d{-m_value}; }
/**
* Multiplies the current rotation by a scalar.
@@ -84,7 +111,9 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The new scaled Rotation2d.
*/
constexpr Rotation2d operator*(double scalar) const;
constexpr Rotation2d operator*(double scalar) const {
return Rotation2d{m_value * scalar};
}
/**
* Divides the current rotation by a scalar.
@@ -93,7 +122,9 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The new scaled Rotation2d.
*/
constexpr Rotation2d operator/(double scalar) const;
constexpr Rotation2d operator/(double scalar) const {
return *this * (1.0 / scalar);
}
/**
* Checks equality between this Rotation2d and another object.
@@ -101,7 +132,9 @@ class WPILIB_DLLEXPORT Rotation2d {
* @param other The other object.
* @return Whether the two objects are equal.
*/
constexpr bool operator==(const Rotation2d& other) const;
constexpr bool operator==(const Rotation2d& other) const {
return gcem::hypot(Cos() - other.Cos(), Sin() - other.Sin()) < 1E-9;
}
/**
* Adds the new rotation to the current rotation using a rotation matrix.
@@ -116,7 +149,10 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The new rotated Rotation2d.
*/
constexpr Rotation2d RotateBy(const Rotation2d& other) const;
constexpr Rotation2d RotateBy(const Rotation2d& other) const {
return {Cos() * other.Cos() - Sin() * other.Sin(),
Cos() * other.Sin() + Sin() * other.Cos()};
}
/**
* Returns the radian value of the rotation.
@@ -173,4 +209,3 @@ void from_json(const wpi::json& json, Rotation2d& rotation);
#include "frc/geometry/proto/Rotation2dProto.h"
#endif
#include "frc/geometry/struct/Rotation2dStruct.h"
#include "frc/geometry/Rotation2d.inc"

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

@@ -1,70 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <string>
#include <type_traits>
#include <gcem.hpp>
#include <wpi/StackTrace.h>
#include "frc/geometry/Rotation2d.h"
#include "units/angle.h"
#include "wpimath/MathShared.h"
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())} {}
constexpr Rotation2d::Rotation2d(double x, double y) {
double magnitude = gcem::hypot(x, y);
if (magnitude > 1e-6) {
m_sin = y / magnitude;
m_cos = x / magnitude;
} else {
m_sin = 0.0;
m_cos = 1.0;
if (!std::is_constant_evaluated()) {
wpi::math::MathSharedStore::ReportError(
"x and y components of Rotation2d are zero\n{}",
wpi::GetStackTrace(1));
}
}
m_value = units::radian_t{gcem::atan2(m_sin, m_cos)};
}
constexpr Rotation2d Rotation2d::operator-() const {
return Rotation2d{-m_value};
}
constexpr Rotation2d Rotation2d::operator*(double scalar) const {
return Rotation2d{m_value * scalar};
}
constexpr Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
return RotateBy(other);
}
constexpr Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
return *this + -other;
}
constexpr Rotation2d Rotation2d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
constexpr bool Rotation2d::operator==(const Rotation2d& other) const {
return gcem::hypot(Cos() - other.Cos(), Sin() - other.Sin()) < 1E-9;
}
constexpr Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
return {Cos() * other.Cos() - Sin() * other.Sin(),
Cos() * other.Sin() + Sin() * other.Cos()};
}
} // namespace frc

19
wpimath/src/main/native/include/frc/geometry/Transform2d.h
View File

@@ -4,8 +4,11 @@
#pragma once
#include <utility>
#include <wpi/SymbolExports.h>
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
namespace frc {
@@ -31,7 +34,9 @@ class WPILIB_DLLEXPORT Transform2d {
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
constexpr Transform2d(Translation2d translation, Rotation2d rotation);
constexpr Transform2d(Translation2d translation, Rotation2d rotation)
: m_translation{std::move(translation)},
m_rotation{std::move(rotation)} {}
/**
* Constructs a transform with x and y translations instead of a separate
@@ -41,8 +46,8 @@ class WPILIB_DLLEXPORT Transform2d {
* @param y The y component of the translational component of the transform.
* @param rotation The rotational component of the transform.
*/
constexpr Transform2d(units::meter_t x, units::meter_t y,
Rotation2d rotation);
constexpr Transform2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
/**
* Constructs the identity transform -- maps an initial pose to itself.
@@ -82,7 +87,12 @@ class WPILIB_DLLEXPORT Transform2d {
*
* @return The inverted transformation.
*/
constexpr Transform2d Inverse() const;
constexpr Transform2d 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 Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
}
/**
* Multiplies the transform by the scalar.
@@ -128,4 +138,3 @@ class WPILIB_DLLEXPORT Transform2d {
#include "frc/geometry/proto/Transform2dProto.h"
#endif
#include "frc/geometry/struct/Transform2dStruct.h"
#include "frc/geometry/Transform2d.inc"

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

@@ -1,30 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <utility>
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Transform2d.h"
#include "frc/geometry/Translation2d.h"
namespace frc {
constexpr Transform2d::Transform2d(Translation2d translation,
Rotation2d 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)} {}
constexpr Transform2d Transform2d::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 Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
}
} // namespace frc

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

@@ -39,7 +39,8 @@ class WPILIB_DLLEXPORT Translation2d {
* @param x The x component of the translation.
* @param y The y component of the translation.
*/
constexpr Translation2d(units::meter_t x, units::meter_t y);
constexpr Translation2d(units::meter_t x, units::meter_t y)
: m_x{x}, m_y{y} {}
/**
* Constructs a Translation2d with the provided distance and angle. This is
@@ -48,7 +49,8 @@ class WPILIB_DLLEXPORT Translation2d {
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
*/
constexpr Translation2d(units::meter_t distance, const Rotation2d& angle);
constexpr Translation2d(units::meter_t distance, const Rotation2d& angle)
: m_x{distance * angle.Cos()}, m_y{distance * angle.Sin()} {}
/**
* Constructs a Translation2d from the provided translation vector's X and Y
@@ -90,7 +92,9 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return A Vector representation of this translation.
*/
constexpr Eigen::Vector2d ToVector() const;
constexpr Eigen::Vector2d ToVector() const {
return Eigen::Vector2d{{m_x.value(), m_y.value()}};
}
/**
* Returns the norm, or distance from the origin to the translation.
@@ -104,7 +108,9 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The angle of the translation
*/
constexpr Rotation2d Angle() const;
constexpr Rotation2d Angle() const {
return Rotation2d{m_x.value(), m_y.value()};
}
/**
* Applies a rotation to the translation in 2D space.
@@ -124,7 +130,10 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The new rotated translation.
*/
constexpr Translation2d RotateBy(const Rotation2d& other) const;
constexpr Translation2d RotateBy(const Rotation2d& other) const {
return {m_x * other.Cos() - m_y * other.Sin(),
m_x * other.Sin() + m_y * other.Cos()};
}
/**
* Rotates this translation around another translation in 2D space.
@@ -139,7 +148,12 @@ class WPILIB_DLLEXPORT Translation2d {
* @return The new rotated translation.
*/
constexpr Translation2d RotateAround(const Translation2d& other,
const Rotation2d& rot) const;
const Rotation2d& rot) const {
return {(m_x - other.X()) * rot.Cos() - (m_y - other.Y()) * rot.Sin() +
other.X(),
(m_x - other.X()) * rot.Sin() + (m_y - other.Y()) * rot.Cos() +
other.Y()};
}
/**
* Returns the sum of two translations in 2D space.
@@ -151,7 +165,9 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The sum of the translations.
*/
constexpr Translation2d operator+(const Translation2d& other) const;
constexpr Translation2d operator+(const Translation2d& other) const {
return {X() + other.X(), Y() + other.Y()};
}
/**
* Returns the difference between two translations.
@@ -163,7 +179,9 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The difference between the two translations.
*/
constexpr Translation2d operator-(const Translation2d& other) const;
constexpr Translation2d operator-(const Translation2d& other) const {
return *this + -other;
}
/**
* Returns the inverse of the current translation. This is equivalent to
@@ -172,7 +190,7 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The inverse of the current translation.
*/
constexpr Translation2d operator-() const;
constexpr Translation2d operator-() const { return {-m_x, -m_y}; }
/**
* Returns the translation multiplied by a scalar.
@@ -183,7 +201,9 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The scaled translation.
*/
constexpr Translation2d operator*(double scalar) const;
constexpr Translation2d operator*(double scalar) const {
return {scalar * m_x, scalar * m_y};
}
/**
* Returns the translation divided by a scalar.
@@ -194,7 +214,9 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The scaled translation.
*/
constexpr Translation2d operator/(double scalar) const;
constexpr Translation2d operator/(double scalar) const {
return operator*(1.0 / scalar);
}
/**
* Checks equality between this Translation2d and another object.
@@ -202,7 +224,10 @@ class WPILIB_DLLEXPORT Translation2d {
* @param other The other object.
* @return Whether the two objects are equal.
*/
constexpr bool operator==(const Translation2d& other) const;
constexpr bool operator==(const Translation2d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m;
}
/**
* Returns the nearest Translation2d from a collection of translations
@@ -236,4 +261,3 @@ void from_json(const wpi::json& json, Translation2d& state);
#include "frc/geometry/proto/Translation2dProto.h"
#endif
#include "frc/geometry/struct/Translation2dStruct.h"
#include "frc/geometry/Translation2d.inc"

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

@@ -1,68 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "frc/geometry/Translation2d.h"
#include "units/length.h"
#include "units/math.h"
namespace frc {
constexpr Translation2d::Translation2d(units::meter_t x, units::meter_t 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()} {}
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()};
}
constexpr Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
return {m_x * other.Cos() - m_y * other.Sin(),
m_x * other.Sin() + m_y * other.Cos()};
}
constexpr Translation2d Translation2d::RotateAround(
const Translation2d& other, const Rotation2d& rot) const {
return {
(m_x - other.X()) * rot.Cos() - (m_y - other.Y()) * rot.Sin() + other.X(),
(m_x - other.X()) * rot.Sin() + (m_y - other.Y()) * rot.Cos() +
other.Y()};
}
constexpr Translation2d Translation2d::operator+(
const Translation2d& other) const {
return {X() + other.X(), Y() + other.Y()};
}
constexpr Translation2d Translation2d::operator-(
const Translation2d& other) const {
return *this + -other;
}
constexpr Translation2d Translation2d::operator-() const {
return {-m_x, -m_y};
}
constexpr Translation2d Translation2d::operator*(double scalar) const {
return {scalar * m_x, scalar * m_y};
}
constexpr Translation2d Translation2d::operator/(double scalar) const {
return operator*(1.0 / scalar);
}
constexpr bool Translation2d::operator==(const Translation2d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m;
}
} // namespace frc

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

@@ -11,6 +11,7 @@
#include "frc/geometry/Rotation3d.h"
#include "frc/geometry/Translation2d.h"
#include "units/length.h"
#include "units/math.h"
namespace frc {
@@ -37,7 +38,8 @@ class WPILIB_DLLEXPORT Translation3d {
* @param y The y component of the translation.
* @param z The z component of the translation.
*/
constexpr Translation3d(units::meter_t x, units::meter_t y, units::meter_t z);
constexpr Translation3d(units::meter_t x, units::meter_t y, units::meter_t z)
: m_x{x}, m_y{y}, m_z{z} {}
/**
* Constructs a Translation3d with the provided distance and angle. This is
@@ -46,7 +48,12 @@ class WPILIB_DLLEXPORT Translation3d {
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
*/
Translation3d(units::meter_t distance, const Rotation3d& angle);
Translation3d(units::meter_t distance, const Rotation3d& angle) {
auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
m_x = rectangular.X();
m_y = rectangular.Y();
m_z = rectangular.Z();
}
/**
* Constructs a Translation3d from the provided translation vector's X, Y, and
@@ -54,7 +61,10 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @param vector The translation vector to represent.
*/
explicit Translation3d(const Eigen::Vector3d& vector);
explicit 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()}} {}
/**
* Calculates the distance between two translations in 3D space.
@@ -66,7 +76,11 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The distance between the two translations.
*/
units::meter_t Distance(const Translation3d& other) const;
units::meter_t 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) +
units::math::pow<2>(other.m_z - m_z));
}
/**
* Returns the X component of the translation.
@@ -94,14 +108,18 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return A Vector representation of this translation.
*/
constexpr Eigen::Vector3d ToVector() const;
constexpr Eigen::Vector3d ToVector() const {
return Eigen::Vector3d{{m_x.value(), m_y.value(), m_z.value()}};
}
/**
* Returns the norm, or distance from the origin to the translation.
*
* @return The norm of the translation.
*/
units::meter_t Norm() const;
units::meter_t Norm() const {
return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
}
/**
* Applies a rotation to the translation in 3D space.
@@ -113,13 +131,20 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The new rotated translation.
*/
Translation3d RotateBy(const Rotation3d& other) const;
Translation3d RotateBy(const Rotation3d& other) const {
Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()};
auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse();
return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()},
units::meter_t{qprime.Z()}};
}
/**
* Returns a Translation2d representing this Translation3d projected into the
* X-Y plane.
*/
constexpr Translation2d ToTranslation2d() const;
constexpr Translation2d ToTranslation2d() const {
return Translation2d{m_x, m_y};
}
/**
* Returns the sum of two translations in 3D space.
@@ -131,7 +156,9 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The sum of the translations.
*/
constexpr Translation3d operator+(const Translation3d& other) const;
constexpr Translation3d operator+(const Translation3d& other) const {
return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
}
/**
* Returns the difference between two translations.
@@ -143,7 +170,9 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The difference between the two translations.
*/
constexpr Translation3d operator-(const Translation3d& other) const;
constexpr Translation3d operator-(const Translation3d& other) const {
return operator+(-other);
}
/**
* Returns the inverse of the current translation. This is equivalent to
@@ -151,7 +180,7 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The inverse of the current translation.
*/
constexpr Translation3d operator-() const;
constexpr Translation3d operator-() const { return {-m_x, -m_y, -m_z}; }
/**
* Returns the translation multiplied by a scalar.
@@ -163,7 +192,9 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The scaled translation.
*/
constexpr Translation3d operator*(double scalar) const;
constexpr Translation3d operator*(double scalar) const {
return {scalar * m_x, scalar * m_y, scalar * m_z};
}
/**
* Returns the translation divided by a scalar.
@@ -175,7 +206,9 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The scaled translation.
*/
constexpr Translation3d operator/(double scalar) const;
constexpr Translation3d operator/(double scalar) const {
return operator*(1.0 / scalar);
}
/**
* Checks equality between this Translation3d and another object.
@@ -183,7 +216,11 @@ class WPILIB_DLLEXPORT Translation3d {
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Translation3d& other) const;
constexpr bool operator==(const Translation3d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m &&
units::math::abs(m_z - other.m_z) < 1E-9_m;
}
private:
units::meter_t m_x = 0_m;
@@ -203,4 +240,3 @@ void from_json(const wpi::json& json, Translation3d& state);
#include "frc/geometry/proto/Translation3dProto.h"
#endif
#include "frc/geometry/struct/Translation3dStruct.h"
#include "frc/geometry/Translation3d.inc"

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

@@ -1,48 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Translation3d.h"
#include "units/length.h"
#include "units/math.h"
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} {}
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()};
}
constexpr Translation3d Translation3d::operator-(
const Translation3d& other) const {
return operator+(-other);
}
constexpr Translation3d Translation3d::operator-() const {
return {-m_x, -m_y, -m_z};
}
constexpr Translation3d Translation3d::operator*(double scalar) const {
return {scalar * m_x, scalar * m_y, scalar * m_z};
}
constexpr Translation3d Translation3d::operator/(double scalar) const {
return operator*(1.0 / scalar);
}
} // namespace frc