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

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-08-03 11:51:25 -07:00
parent 23f5725853 2d11946d98
commit ed7982563b
50 changed files with 1390 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
View File

@@ -372,7 +372,11 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
final Matrix<States, Rows> K = S.transpose().solve(C.times(m_P.transpose())).transpose();
//
// Drop the transposes on symmetric matrices S and P.
//
// K = (S.solve(CP))ᵀ
final Matrix<States, Rows> K = S.solve(C.times(m_P)).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, h.apply(m_xHat, u))));

6
wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
View File

@@ -230,7 +230,11 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
final Matrix<States, Outputs> K = S.transpose().solve(C.times(m_P.transpose())).transpose();
//
// Drop the transposes on symmetric matrices S and P.
//
// K = (S.solve(CP))ᵀ
final Matrix<States, Outputs> K = S.solve(C.times(m_P)).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
m_xHat = m_xHat.plus(K.times(y.minus(C.times(m_xHat).plus(D.times(u)))));

8
wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java
View File

@@ -100,9 +100,11 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
m_K =
new Matrix<>(
S.transpose().getStorage().solve(C.times(P.transpose()).getStorage()).transpose());
//
// Drop the transposes on symmetric matrices S and P.
//
// K = (S.solve(CP))ᵀ
m_K = new Matrix<>(S.getStorage().solve(C.times(P).getStorage()).transpose());
reset();
}

50
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
View File

@@ -109,6 +109,22 @@ public class Translation2d
return Math.hypot(other.m_x - m_x, other.m_y - m_y);
}
/**
* Calculates the square of the distance between two translations in 2D space. This is equivalent
* to squaring the result of {@link #getDistance(Translation2d)}, but avoids computing a square
* root.
*
* <p>The square of the distance between translations is defined as (x₂−x₁)²+(y₂−y₁)².
*
* @param other The translation to compute the squared distance to.
* @return The square of the distance between the two translations, in square meters.
*/
public double getSquaredDistance(Translation2d other) {
double dx = other.m_x - m_x;
double dy = other.m_y - m_y;
return dx * dx + dy * dy;
}
/**
* Returns the X component of the translation.
*
@@ -165,6 +181,16 @@ public class Translation2d
return Math.hypot(m_x, m_y);
}
/**
* Returns the squared norm, or squared distance from the origin to the translation. This is
* equivalent to squaring the result of {@link #getNorm()}, but avoids computing a square root.
*
* @return The squared norm of the translation, in square meters.
*/
public double getSquaredNorm() {
return m_x * m_x + m_y * m_y;
}
/**
* Returns the angle this translation forms with the positive X axis.
*
@@ -214,6 +240,30 @@ public class Translation2d
(m_x - other.getX()) * rot.getSin() + (m_y - other.getY()) * rot.getCos() + other.getY());
}
/**
* Computes the dot product between this translation and another translation in 2D space.
*
* <p>The dot product between two translations is defined as x₁x₂+y₁y₂.
*
* @param other The translation to compute the dot product with.
* @return The dot product between the two translations, in square meters.
*/
public double dot(Translation2d other) {
return m_x * other.m_x + m_y * other.m_y;
}
/**
* Computes the cross product between this translation and another translation in 2D space.
*
* <p>The 2D cross product between two translations is defined as x₁y₂-x₂y₁.
*
* @param other The translation to compute the cross product with.
* @return The cross product between the two translations, in square meters.
*/
public double cross(Translation2d other) {
return m_x * other.m_y - m_y * other.m_x;
}
/**
* Returns the sum of two translations in 2D space.
*

61
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
View File

@@ -125,8 +125,26 @@ public class Translation3d
* @return The distance between the two translations.
*/
public double getDistance(Translation3d other) {
return Math.sqrt(
Math.pow(other.m_x - m_x, 2) + Math.pow(other.m_y - m_y, 2) + Math.pow(other.m_z - m_z, 2));
double dx = other.m_x - m_x;
double dy = other.m_y - m_y;
double dz = other.m_z - m_z;
return Math.sqrt(dx * dx + dy * dy + dz * dz);
}
/**
* Calculates the squared distance between two translations in 3D space. This is equivalent to
* squaring the result of {@link #getDistance(Translation3d)}, but avoids computing a square root.
*
* <p>The squared distance between translations is defined as (x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)².
*
* @param other The translation to compute the squared distance to.
* @return The squared distance between the two translations.
*/
public double getSquaredDistance(Translation3d other) {
double dx = other.m_x - m_x;
double dy = other.m_y - m_y;
double dz = other.m_z - m_z;
return dx * dx + dy * dy + dz * dz;
}
/**
@@ -204,6 +222,16 @@ public class Translation3d
return Math.sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
}
/**
* Returns the squared norm, or squared distance from the origin to the translation. This is
* equivalent to squaring the result of {@link #getNorm()}, but avoids computing a square root.
*
* @return The squared norm of the translation.
*/
public double getSquaredNorm() {
return m_x * m_x + m_y * m_y + m_z * m_z;
}
/**
* Applies a rotation to the translation in 3D space.
*
@@ -230,6 +258,35 @@ public class Translation3d
return this.minus(other).rotateBy(rot).plus(other);
}
/**
* Computes the dot product between this translation and another translation in 3D space.
*
* <p>The dot product between two translations is defined as x₁x₂+y₁y₂+z₁z₂.
*
* @param other The translation to compute the dot product with.
* @return The dot product between the two translations, in square meters.
*/
public double dot(Translation3d other) {
return m_x * other.m_x + m_y * other.m_y + m_z * other.m_z;
}
/**
* Computes the cross product between this translation and another translation in 3D space. The
* resulting translation will be perpendicular to both translations.
*
* <p>The 3D cross product between two translations is defined as &lt;y₁z₂-y₂z₁, z₁x₂-z₂x₁,
* x₁y₂-x₂y₁&gt;.
*
* @param other The translation to compute the cross product with.
* @return The cross product between the two translations.
*/
public Vector<N3> cross(Translation3d other) {
return VecBuilder.fill(
m_y * other.m_z - other.m_y * m_z,
m_z * other.m_x - other.m_z * m_x,
m_x * other.m_y - other.m_x * m_y);
}
/**
* Returns a Translation2d representing this Translation3d projected into the X-Y plane.
*

6
wpimath/src/main/native/include/frc/DARE.h
View File

@@ -128,7 +128,11 @@ Eigen::Matrix<double, States, States> DARE(
//
// V₂ᵀ = W.solve(Gₖᵀ)
// V₂ = W.solve(Gₖᵀ)ᵀ
StateMatrix V_2 = W_solver.solve(G_k.transpose()).transpose();
//
// Since W, Gₖ, and Hₖ are symmetric, drop the transposes on Gₖ and V₂.
//
// V₂ = W.solve(Gₖ)
StateMatrix V_2 = W_solver.solve(G_k);
// Gₖ₊₁ = Gₖ + AₖV₂Aₖᵀ
// Hₖ₊₁ = Hₖ + V₁ᵀHₖAₖ

7
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
View File

@@ -397,8 +397,11 @@ class ExtendedKalmanFilter {
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Matrixd<States, Rows> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
//
// Drop the transposes on symmetric matrices S and P.
//
// K = (S.solve(CP))ᵀ
Matrixd<States, Rows> K = S.ldlt().solve(C * m_P).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + Kₖ₊₁(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, h(m_xHat, u)));

7
wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
View File

@@ -235,8 +235,11 @@ class KalmanFilter {
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Matrixd<States, Outputs> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
//
// Drop the transposes on symmetric matrices S and P.
//
// K = (S.solve(CP))ᵀ
Matrixd<States, Outputs> K = S.ldlt().solve(C * m_P).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
m_xHat += K * (y - (C * m_xHat + D * u));

6
wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h
View File

@@ -114,7 +114,11 @@ class SteadyStateKalmanFilter {
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
m_K = S.transpose().ldlt().solve(C * P.value().transpose()).transpose();
//
// Drop the transposes on symmetric matrices S and P.
//
// K = (S.solve(CP))ᵀ
m_K = S.ldlt().solve(C * P.value()).transpose();
} else if (P.error() == DAREError::QNotSymmetric ||
P.error() == DAREError::QNotPositiveSemidefinite) {
std::string msg =

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

@@ -13,6 +13,7 @@
#include <wpi/json_fwd.h>
#include "frc/geometry/Rotation2d.h"
#include "units/area.h"
#include "units/length.h"
#include "units/math.h"
@@ -75,6 +76,23 @@ class WPILIB_DLLEXPORT Translation2d {
return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
}
/**
* Calculates the square of the distance between two translations in 2D space.
* This is equivalent to squaring the result of Distance(Translation2d), but
* avoids computing a square root.
*
* The square of the distance between translations is defined as
* (x₂−x₁)²+(y₂−y₁)².
*
* @param other The translation to compute the squared distance to.
* @return The square of the distance between the two translations.
*/
constexpr units::square_meter_t SquaredDistance(
const Translation2d& other) const {
return units::math::pow<2>(other.m_x - m_x) +
units::math::pow<2>(other.m_y - m_y);
}
/**
* Returns the X component of the translation.
*
@@ -105,6 +123,17 @@ class WPILIB_DLLEXPORT Translation2d {
*/
constexpr units::meter_t Norm() const { return units::math::hypot(m_x, m_y); }
/**
* Returns the squared norm, or squared distance from the origin to the
* translation. This is equivalent to squaring the result of Norm(), but
* avoids computing a square root.
*
* @return The squared norm of the translation.
*/
constexpr units::square_meter_t SquaredNorm() const {
return units::math::pow<2>(m_x) + units::math::pow<2>(m_y);
}
/**
* Returns the angle this translation forms with the positive X axis.
*
@@ -157,6 +186,32 @@ class WPILIB_DLLEXPORT Translation2d {
other.Y()};
}
/**
* Computes the dot product between this translation and another translation
* in 2D space.
*
* The dot product between two translations is defined as x₁x₂+y₁y₂.
*
* @param other The translation to compute the dot product with.
* @return The dot product between the two translations.
*/
constexpr units::square_meter_t Dot(const Translation2d& other) const {
return m_x * other.X() + m_y * other.Y();
}
/**
* Computes the cross product between this translation and another translation
* in 2D space.
*
* The 2D cross product between two translations is defined as x₁y₂-x₂y₁.
*
* @param other The translation to compute the cross product with.
* @return The cross product between the two translations.
*/
constexpr units::square_meter_t Cross(const Translation2d& other) const {
return m_x * other.Y() - m_y * other.X();
}
/**
* Returns the sum of two translations in 2D space.
*

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

@@ -14,6 +14,7 @@
#include "frc/geometry/Rotation3d.h"
#include "frc/geometry/Translation2d.h"
#include "units/area.h"
#include "units/length.h"
#include "units/math.h"
@@ -96,6 +97,24 @@ class WPILIB_DLLEXPORT Translation3d {
units::math::pow<2>(other.m_z - m_z));
}
/**
* Calculates the squared distance between two translations in 3D space. This
* is equivalent to squaring the result of Distance(Translation3d), but avoids
* computing a square root.
*
* The squared distance between translations is defined as
* (x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)².
*
* @param other The translation to compute the squared distance to.
* @return The squared distance between the two translations.
*/
constexpr units::square_meter_t SquaredDistance(
const Translation3d& other) const {
return 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.
*
@@ -135,6 +154,17 @@ class WPILIB_DLLEXPORT Translation3d {
return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
}
/**
* Returns the squared norm, or squared distance from the origin to the
* translation. This is equivalent to squaring the result of Norm(), but
* avoids computing a square root.
*
* @return The squared norm of the translation.
*/
constexpr units::square_meter_t SquaredNorm() const {
return m_x * m_x + m_y * m_y + m_z * m_z;
}
/**
* Applies a rotation to the translation in 3D space.
*
@@ -164,6 +194,38 @@ class WPILIB_DLLEXPORT Translation3d {
return (*this - other).RotateBy(rot) + other;
}
/**
* Computes the dot product between this translation and another translation
* in 3D space.
*
* The dot product between two translations is defined as x₁x₂+y₁y₂+z₁z₂.
*
* @param other The translation to compute the dot product with.
* @return The dot product between the two translations.
*/
constexpr units::square_meter_t Dot(const Translation3d& other) const {
return m_x * other.X() + m_y * other.Y() + m_z * other.Z();
}
/**
* Computes the cross product between this translation and another
* translation in 3D space. The resulting translation will be perpendicular to
* both translations.
*
* The 3D cross product between two translations is defined as <y₁z₂-y₂z₁,
* z₁x₂-z₂x₁, x₁y₂-x₂y₁>.
*
* @param other The translation to compute the cross product with.
* @return The cross product between the two translations.
*/
constexpr Eigen::Vector<units::square_meter_t, 3> Cross(
const Translation3d& other) const {
return Eigen::Vector<units::square_meter_t, 3>{
{m_y * other.Z() - other.Y() * m_z},
{m_z * other.X() - other.Z() * m_x},
{m_x * other.Y() - other.X() * m_y}};
}
/**
* Returns a Translation2d representing this Translation3d projected into the
* X-Y plane.

13
wpimath/src/main/native/include/units/math.h
View File

@@ -482,11 +482,10 @@ constexpr dimensionless::scalar_t log2(const ScalarUnit x) noexcept {
template <
class UnitType,
std::enable_if_t<units::traits::has_linear_scale<UnitType>::value, int> = 0>
inline constexpr auto sqrt(const UnitType& value) noexcept
-> unit_t<
square_root<typename units::traits::unit_t_traits<UnitType>::unit_type>,
typename units::traits::unit_t_traits<UnitType>::underlying_type,
linear_scale> {
inline constexpr auto sqrt(const UnitType& value) noexcept -> unit_t<
square_root<typename units::traits::unit_t_traits<UnitType>::unit_type>,
typename units::traits::unit_t_traits<UnitType>::underlying_type,
linear_scale> {
return unit_t<
square_root<typename units::traits::unit_t_traits<UnitType>::unit_type>,
typename units::traits::unit_t_traits<UnitType>::underlying_type,
@@ -742,8 +741,8 @@ template <class UnitTypeLhs, class UnitMultiply, class UnitAdd,
class = std::enable_if_t<traits::is_unit_t<UnitTypeLhs>::value &&
traits::is_unit_t<UnitMultiply>::value &&
traits::is_unit_t<UnitAdd>::value>>
auto fma(const UnitTypeLhs x, const UnitMultiply y,
const UnitAdd z) noexcept -> decltype(x * y) {
auto fma(const UnitTypeLhs x, const UnitMultiply y, const UnitAdd z) noexcept
-> decltype(x * y) {
using resultType = decltype(x * y);
static_assert(
traits::is_convertible_unit_t<

7
wpimath/src/main/native/thirdparty/eigen/include/Eigen/Dense vendored Normal file
View File

@@ -0,0 +1,7 @@
#include "Core"
#include "LU"
#include "Cholesky"
#include "QR"
#include "SVD"
#include "Geometry"
#include "Eigenvalues"

30
wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdVector vendored Normal file
View File

@@ -0,0 +1,30 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#ifndef EIGEN_STDVECTOR_MODULE_H
#define EIGEN_STDVECTOR_MODULE_H
#include "Core"
#include <vector>
#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && \
(EIGEN_MAX_STATIC_ALIGN_BYTES <= 16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */
#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...)
#else
// IWYU pragma: begin_exports
#include "src/StlSupport/StdVector.h"
// IWYU pragma: end_exports
#endif
#endif // EIGEN_STDVECTOR_MODULE_H

51
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdVector.h vendored Normal file
View File

@@ -0,0 +1,51 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#ifndef EIGEN_STDVECTOR_H
#define EIGEN_STDVECTOR_H
#ifndef EIGEN_STDVECTOR_MODULE_H
#error "Please include Eigen/StdVector instead of including this file directly."
#endif
#include "details.h"
/**
* This section contains a convenience MACRO which allows an easy specialization of
* std::vector such that for data types with alignment issues the correct allocator
* is used automatically.
*/
#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...) \
namespace std { \
template <> \
class vector<__VA_ARGS__, std::allocator<__VA_ARGS__> > \
: public vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > { \
typedef vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > vector_base; \
\
public: \
typedef __VA_ARGS__ value_type; \
typedef vector_base::allocator_type allocator_type; \
typedef vector_base::size_type size_type; \
typedef vector_base::iterator iterator; \
explicit vector(const allocator_type& a = allocator_type()) : vector_base(a) {} \
template <typename InputIterator> \
vector(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) \
: vector_base(first, last, a) {} \
vector(const vector& c) : vector_base(c) {} \
explicit vector(size_type num, const value_type& val = value_type()) : vector_base(num, val) {} \
vector(iterator start_, iterator end_) : vector_base(start_, end_) {} \
vector& operator=(const vector& x) { \
vector_base::operator=(x); \
return *this; \
} \
}; \
}
#endif // EIGEN_STDVECTOR_H

82
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/details.h vendored Normal file
View File

@@ -0,0 +1,82 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#ifndef EIGEN_STL_DETAILS_H
#define EIGEN_STL_DETAILS_H
#ifndef EIGEN_ALIGNED_ALLOCATOR
#define EIGEN_ALIGNED_ALLOCATOR Eigen::aligned_allocator
#endif
namespace Eigen {
// This one is needed to prevent reimplementing the whole std::vector.
template <class T>
class aligned_allocator_indirection : public EIGEN_ALIGNED_ALLOCATOR<T> {
public:
typedef std::size_t size_type;
typedef std::ptrdiff_t difference_type;
typedef T* pointer;
typedef const T* const_pointer;
typedef T& reference;
typedef const T& const_reference;
typedef T value_type;
template <class U>
struct rebind {
typedef aligned_allocator_indirection<U> other;
};
aligned_allocator_indirection() {}
aligned_allocator_indirection(const aligned_allocator_indirection&) : EIGEN_ALIGNED_ALLOCATOR<T>() {}
aligned_allocator_indirection(const EIGEN_ALIGNED_ALLOCATOR<T>&) {}
template <class U>
aligned_allocator_indirection(const aligned_allocator_indirection<U>&) {}
template <class U>
aligned_allocator_indirection(const EIGEN_ALIGNED_ALLOCATOR<U>&) {}
~aligned_allocator_indirection() {}
};
#if EIGEN_COMP_MSVC
// sometimes, MSVC detects, at compile time, that the argument x
// in std::vector::resize(size_t s,T x) won't be aligned and generate an error
// even if this function is never called. Whence this little wrapper.
#define EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T) \
std::conditional_t<Eigen::internal::is_arithmetic<T>::value, T, Eigen::internal::workaround_msvc_stl_support<T> >
namespace internal {
template <typename T>
struct workaround_msvc_stl_support : public T {
inline workaround_msvc_stl_support() : T() {}
inline workaround_msvc_stl_support(const T& other) : T(other) {}
inline operator T&() { return *static_cast<T*>(this); }
inline operator const T&() const { return *static_cast<const T*>(this); }
template <typename OtherT>
inline T& operator=(const OtherT& other) {
T::operator=(other);
return *this;
}
inline workaround_msvc_stl_support& operator=(const workaround_msvc_stl_support& other) {
T::operator=(other);
return *this;
}
};
} // namespace internal
#else
#define EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T) T
#endif
} // namespace Eigen
#endif // EIGEN_STL_DETAILS_H

27
wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
View File

@@ -86,6 +86,12 @@ class Translation2dTest {
assertEquals(Math.hypot(3.0, 5.0), one.getNorm(), kEpsilon);
}
@Test
void testSquaredNorm() {
var one = new Translation2d(3.0, 5.0);
assertEquals(34.0, one.getSquaredNorm(), kEpsilon);
}
@Test
void testDistance() {
var one = new Translation2d(1, 1);
@@ -93,6 +99,13 @@ class Translation2dTest {
assertEquals(5.0 * Math.sqrt(2.0), one.getDistance(two), kEpsilon);
}
@Test
void testSquaredDistance() {
var one = new Translation2d(1, 1);
var two = new Translation2d(6, 6);
assertEquals(50.0, one.getSquaredDistance(two), kEpsilon);
}
@Test
void testUnaryMinus() {
var original = new Translation2d(-4.5, 7);
@@ -154,4 +167,18 @@ class Translation2dTest {
assertEquals(vec, translation.toVector());
}
@Test
void testDot() {
var one = new Translation2d(2.0, 3.0);
var two = new Translation2d(3.0, 4.0);
assertEquals(18.0, one.dot(two), kEpsilon);
}
@Test
void testCross() {
var one = new Translation2d(2.0, 3.0);
var two = new Translation2d(3.0, 4.0);
assertEquals(-1.0, one.cross(two), kEpsilon);
}
}

32
wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
View File

@@ -149,6 +149,12 @@ class Translation3dTest {
assertEquals(Math.sqrt(83.0), one.getNorm(), kEpsilon);
}
@Test
void testSquaredNorm() {
var one = new Translation3d(3.0, 5.0, 7.0);
assertEquals(83.0, one.getSquaredNorm(), kEpsilon);
}
@Test
void testDistance() {
var one = new Translation3d(1.0, 1.0, 1.0);
@@ -156,6 +162,13 @@ class Translation3dTest {
assertEquals(5.0 * Math.sqrt(3.0), one.getDistance(two), kEpsilon);
}
@Test
void testSquaredDistance() {
var one = new Translation3d(1.0, 1.0, 1.0);
var two = new Translation3d(6.0, 6.0, 6.0);
assertEquals(75.0, one.getSquaredDistance(two), kEpsilon);
}
@Test
void testUnaryMinus() {
var original = new Translation3d(-4.5, 7.0, 9.0);
@@ -225,4 +238,23 @@ class Translation3dTest {
assertEquals(translation1, origin.nearest(List.of(translation1, translation2, translation3)));
assertEquals(translation2, origin.nearest(List.of(translation4, translation2, translation3)));
}
@Test
void testDot() {
var one = new Translation3d(1.0, 2.0, 3.0);
var two = new Translation3d(4.0, 5.0, 6.0);
assertEquals(32.0, one.dot(two));
}
@Test
void testCross() {
var one = new Translation3d(1.0, 2.0, 3.0);
var two = new Translation3d(4.0, 5.0, 6.0);
var cross = one.cross(two);
assertAll(
() -> assertEquals(-3.0, cross.get(0, 0), kEpsilon),
() -> assertEquals(6.0, cross.get(1, 0), kEpsilon),
() -> assertEquals(-3.0, cross.get(2, 0), kEpsilon));
}
}

23
wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
View File

@@ -68,12 +68,23 @@ TEST(Translation2dTest, Norm) {
EXPECT_DOUBLE_EQ(std::hypot(3.0, 5.0), one.Norm().value());
}
TEST(Translation2dTest, SquaredNorm) {
const Translation2d one{3_m, 5_m};
EXPECT_DOUBLE_EQ(34.0, one.SquaredNorm().value());
}
TEST(Translation2dTest, Distance) {
const Translation2d one{1_m, 1_m};
const Translation2d two{6_m, 6_m};
EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), one.Distance(two).value());
}
TEST(Translation2dTest, SquaredDistance) {
const Translation2d one{1_m, 1_m};
const Translation2d two{6_m, 6_m};
EXPECT_DOUBLE_EQ(50.0, one.SquaredDistance(two).value());
}
TEST(Translation2dTest, UnaryMinus) {
const Translation2d original{-4.5_m, 7_m};
const auto inverted = -original;
@@ -162,3 +173,15 @@ TEST(Translation2dTest, Constexpr) {
static_assert(multiplied.X() == 2_m);
static_assert(divided.Y() == 1_m);
}
TEST(Translation2dTest, Dot) {
const Translation2d one{2_m, 3_m};
const Translation2d two{3_m, 4_m};
EXPECT_DOUBLE_EQ(18.0, one.Dot(two).value());
}
TEST(Translation2dTest, Cross) {
const Translation2d one{2_m, 3_m};
const Translation2d two{3_m, 4_m};
EXPECT_DOUBLE_EQ(-1.0, one.Cross(two).value());
}

27
wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
View File

@@ -114,12 +114,23 @@ TEST(Translation3dTest, Norm) {
EXPECT_NEAR(one.Norm().value(), std::hypot(3, 5, 7), kEpsilon);
}
TEST(Translation3dTest, SquaredNorm) {
const Translation3d one{3_m, 5_m, 7_m};
EXPECT_NEAR(one.SquaredNorm().value(), 83.0, kEpsilon);
}
TEST(Translation3dTest, Distance) {
const Translation3d one{1_m, 1_m, 1_m};
const Translation3d two{6_m, 6_m, 6_m};
EXPECT_NEAR(one.Distance(two).value(), 5 * std::sqrt(3), kEpsilon);
}
TEST(Translation3dTest, SquaredDistance) {
const Translation3d one{1_m, 1_m, 1_m};
const Translation3d two{6_m, 6_m, 6_m};
EXPECT_NEAR(one.SquaredDistance(two).value(), 75.0, kEpsilon);
}
TEST(Translation3dTest, UnaryMinus) {
const Translation3d original{-4.5_m, 7_m, 9_m};
const auto inverted = -original;
@@ -214,3 +225,19 @@ TEST(Translation3dTest, Nearest) {
EXPECT_DOUBLE_EQ(nearest3.Y().value(), translation2.Y().value());
EXPECT_DOUBLE_EQ(nearest3.Z().value(), translation2.Z().value());
}
TEST(Translation3dTest, Dot) {
const Translation3d one{1_m, 2_m, 3_m};
const Translation3d two{4_m, 5_m, 6_m};
EXPECT_NEAR(one.Dot(two).value(), 32.0, kEpsilon);
}
TEST(Translation3dTest, Cross) {
const Translation3d one{1_m, 2_m, 3_m};
const Translation3d two{4_m, 5_m, 6_m};
auto cross = one.Cross(two);
EXPECT_NEAR(cross[0].value(), -3.0, kEpsilon);
EXPECT_NEAR(cross[1].value(), 6.0, kEpsilon);
EXPECT_NEAR(cross[2].value(), -3.0, kEpsilon);
}