diff --git a/upstream_utils/eigen_patches/0001-Disable-warnings.patch b/upstream_utils/eigen_patches/0001-Disable-warnings.patch index 0ff38396e5c..eceda4a347f 100644 --- a/upstream_utils/eigen_patches/0001-Disable-warnings.patch +++ b/upstream_utils/eigen_patches/0001-Disable-warnings.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 18 May 2022 09:14:24 -0700 -Subject: [PATCH 1/2] Disable warnings +Subject: [PATCH 1/3] Disable warnings --- Eigen/src/Core/util/DisableStupidWarnings.h | 6 ++++++ diff --git a/upstream_utils/eigen_patches/0002-Intellisense-fix.patch b/upstream_utils/eigen_patches/0002-Intellisense-fix.patch index 6c369f26fdf..73a956d5ca0 100644 --- a/upstream_utils/eigen_patches/0002-Intellisense-fix.patch +++ b/upstream_utils/eigen_patches/0002-Intellisense-fix.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Fri, 20 Jan 2023 23:41:56 -0800 -Subject: [PATCH 2/2] Intellisense fix +Subject: [PATCH 2/3] Intellisense fix --- Eigen/src/Core/util/ConfigureVectorization.h | 7 +++++++ diff --git a/upstream_utils/eigen_patches/0003-Make-assignment-constexpr.patch b/upstream_utils/eigen_patches/0003-Make-assignment-constexpr.patch new file mode 100644 index 00000000000..a5c7dc83ff7 --- /dev/null +++ b/upstream_utils/eigen_patches/0003-Make-assignment-constexpr.patch @@ -0,0 +1,305 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Sun, 12 Jan 2025 21:04:07 -0800 +Subject: [PATCH 3/3] Make assignment constexpr + +--- + Eigen/src/Core/AssignEvaluator.h | 165 +++++++++++-------- + Eigen/src/Core/EigenBase.h | 2 +- + Eigen/src/Core/functors/AssignmentFunctors.h | 2 +- + 3 files changed, 102 insertions(+), 67 deletions(-) + +diff --git a/Eigen/src/Core/AssignEvaluator.h b/Eigen/src/Core/AssignEvaluator.h +index f7f0b238b8ca70bbc9100262479cc1dbebab9979..9c2436afa7fe98692a036e6ef255ed104a5bf388 100644 +--- a/Eigen/src/Core/AssignEvaluator.h ++++ b/Eigen/src/Core/AssignEvaluator.h +@@ -263,7 +263,7 @@ struct copy_using_evaluator_innervec_CompleteUnrolling { + DstAlignment = Kernel::AssignmentTraits::DstAlignment + }; + +- EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { ++ EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { + kernel.template assignPacketByOuterInner(outer, inner); + enum { NextIndex = Index + unpacket_traits::size }; + copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); +@@ -431,17 +431,25 @@ struct dense_assignment_loop { + template + struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { +- typedef typename Kernel::DstEvaluatorType::XprType DstXprType; +- typedef typename Kernel::PacketType PacketType; +- +- enum { +- size = DstXprType::SizeAtCompileTime, +- packetSize = unpacket_traits::size, +- alignedSize = (int(size) / packetSize) * packetSize +- }; +- +- copy_using_evaluator_linearvec_CompleteUnrolling::run(kernel); +- copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); ++ if (internal::is_constant_evaluated()) { ++ for (Index outer = 0; outer < kernel.outerSize(); ++outer) { ++ for (Index inner = 0; inner < kernel.innerSize(); ++inner) { ++ kernel.assignCoeffByOuterInner(outer, inner); ++ } ++ } ++ } else { ++ typedef typename Kernel::DstEvaluatorType::XprType DstXprType; ++ typedef typename Kernel::PacketType PacketType; ++ ++ enum { ++ size = DstXprType::SizeAtCompileTime, ++ packetSize = unpacket_traits::size, ++ alignedSize = (int(size) / packetSize) * packetSize ++ }; ++ ++ copy_using_evaluator_linearvec_CompleteUnrolling::run(kernel); ++ copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); ++ } + } + }; + +@@ -465,9 +473,17 @@ struct dense_assignment_loop { + + template + struct dense_assignment_loop { +- EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { +- typedef typename Kernel::DstEvaluatorType::XprType DstXprType; +- copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); ++ EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { ++ if (internal::is_constant_evaluated()) { ++ for (Index outer = 0; outer < kernel.outerSize(); ++outer) { ++ for (Index inner = 0; inner < kernel.innerSize(); ++inner) { ++ kernel.assignCoeffByOuterInner(outer, inner); ++ } ++ } ++ } else { ++ typedef typename Kernel::DstEvaluatorType::XprType DstXprType; ++ copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); ++ } + } + }; + +@@ -498,8 +514,16 @@ struct dense_assignment_loop { + template + struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { +- typedef typename Kernel::DstEvaluatorType::XprType DstXprType; +- copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); ++ if (internal::is_constant_evaluated()) { ++ for (Index outer = 0; outer < kernel.outerSize(); ++outer) { ++ for (Index inner = 0; inner < kernel.innerSize(); ++inner) { ++ kernel.assignCoeffByOuterInner(outer, inner); ++ } ++ } ++ } else { ++ typedef typename Kernel::DstEvaluatorType::XprType DstXprType; ++ copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); ++ } + } + }; + +@@ -510,41 +534,49 @@ struct dense_assignment_loop { + template + struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { +- typedef typename Kernel::Scalar Scalar; +- typedef typename Kernel::PacketType PacketType; +- enum { +- packetSize = unpacket_traits::size, +- requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment), +- alignable = +- packet_traits::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar), +- dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment), +- dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment) +- }; +- const Scalar* dst_ptr = kernel.dstDataPtr(); +- if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) { +- // the pointer is not aligned-on scalar, so alignment is not possible +- return dense_assignment_loop::run(kernel); +- } +- const Index packetAlignedMask = packetSize - 1; +- const Index innerSize = kernel.innerSize(); +- const Index outerSize = kernel.outerSize(); +- const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0; +- Index alignedStart = +- ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned(dst_ptr, innerSize); +- +- for (Index outer = 0; outer < outerSize; ++outer) { +- const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask); +- // do the non-vectorizable part of the assignment +- for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner); +- +- // do the vectorizable part of the assignment +- for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize) +- kernel.template assignPacketByOuterInner(outer, inner); +- +- // do the non-vectorizable part of the assignment +- for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner); +- +- alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize); ++ if (internal::is_constant_evaluated()) { ++ for (Index outer = 0; outer < kernel.outerSize(); ++outer) { ++ for (Index inner = 0; inner < kernel.innerSize(); ++inner) { ++ kernel.assignCoeffByOuterInner(outer, inner); ++ } ++ } ++ } else { ++ typedef typename Kernel::Scalar Scalar; ++ typedef typename Kernel::PacketType PacketType; ++ enum { ++ packetSize = unpacket_traits::size, ++ requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment), ++ alignable = ++ packet_traits::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar), ++ dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment), ++ dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment) ++ }; ++ const Scalar* dst_ptr = kernel.dstDataPtr(); ++ if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) { ++ // the pointer is not aligned-on scalar, so alignment is not possible ++ return dense_assignment_loop::run(kernel); ++ } ++ const Index packetAlignedMask = packetSize - 1; ++ const Index innerSize = kernel.innerSize(); ++ const Index outerSize = kernel.outerSize(); ++ const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0; ++ Index alignedStart = ++ ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned(dst_ptr, innerSize); ++ ++ for (Index outer = 0; outer < outerSize; ++outer) { ++ const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask); ++ // do the non-vectorizable part of the assignment ++ for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner); ++ ++ // do the vectorizable part of the assignment ++ for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize) ++ kernel.template assignPacketByOuterInner(outer, inner); ++ ++ // do the non-vectorizable part of the assignment ++ for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner); ++ ++ alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize); ++ } + } + } + }; +@@ -594,9 +626,9 @@ class generic_dense_assignment_kernel { + typedef copy_using_evaluator_traits AssignmentTraits; + typedef typename AssignmentTraits::PacketType PacketType; + +- EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE generic_dense_assignment_kernel(DstEvaluatorType& dst, +- const SrcEvaluatorType& src, +- const Functor& func, DstXprType& dstExpr) ++ EIGEN_DEVICE_FUNC ++ EIGEN_STRONG_INLINE constexpr generic_dense_assignment_kernel(DstEvaluatorType& dst, const SrcEvaluatorType& src, ++ const Functor& func, DstXprType& dstExpr) + : m_dst(dst), m_src(src), m_functor(func), m_dstExpr(dstExpr) { + #ifdef EIGEN_DEBUG_ASSIGN + AssignmentTraits::debug(); +@@ -614,7 +646,7 @@ class generic_dense_assignment_kernel { + EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const EIGEN_NOEXCEPT { return m_src; } + + /// Assign src(row,col) to dst(row,col) through the assignment functor. +- EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index row, Index col) { ++ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(Index row, Index col) { + m_functor.assignCoeff(m_dst.coeffRef(row, col), m_src.coeff(row, col)); + } + +@@ -624,7 +656,7 @@ class generic_dense_assignment_kernel { + } + + /// \sa assignCoeff(Index,Index) +- EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeffByOuterInner(Index outer, Index inner) { ++ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeffByOuterInner(Index outer, Index inner) { + Index row = rowIndexByOuterInner(outer, inner); + Index col = colIndexByOuterInner(outer, inner); + assignCoeff(row, col); +@@ -648,7 +680,7 @@ class generic_dense_assignment_kernel { + assignPacket(row, col); + } + +- EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner) { ++ EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index rowIndexByOuterInner(Index outer, Index inner) { + typedef typename DstEvaluatorType::ExpressionTraits Traits; + return int(Traits::RowsAtCompileTime) == 1 ? 0 + : int(Traits::ColsAtCompileTime) == 1 ? inner +@@ -656,7 +688,7 @@ class generic_dense_assignment_kernel { + : inner; + } + +- EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner) { ++ EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index colIndexByOuterInner(Index outer, Index inner) { + typedef typename DstEvaluatorType::ExpressionTraits Traits; + return int(Traits::ColsAtCompileTime) == 1 ? 0 + : int(Traits::RowsAtCompileTime) == 1 ? inner +@@ -708,8 +740,8 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, co + } + + template +-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, const SrcXprType& src, +- const internal::assign_op& /*func*/) { ++EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void resize_if_allowed(DstXprType& dst, const SrcXprType& src, ++ const internal::assign_op& /*func*/) { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if (((dst.rows() != dstRows) || (dst.cols() != dstCols))) dst.resize(dstRows, dstCols); +@@ -790,7 +822,7 @@ struct Assignment; + // not has to bother about these annoying details. + + template +-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(Dst& dst, const Src& src) { ++EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment(Dst& dst, const Src& src) { + call_assignment(dst, src, internal::assign_op()); + } + template +@@ -807,7 +839,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment( + } + + template +-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment( ++EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment( + Dst& dst, const Src& src, const Func& func, std::enable_if_t::value, void*> = 0) { + call_assignment_no_alias(dst, src, func); + } +@@ -891,9 +923,12 @@ EIGEN_DEVICE_FUNC void check_for_aliasing(const Dst& dst, const Src& src); + // both partial specialization+SFINAE without ambiguous specialization + template + struct Assignment { +- EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, const Functor& func) { ++ EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(DstXprType& dst, const SrcXprType& src, ++ const Functor& func) { + #ifndef EIGEN_NO_DEBUG +- internal::check_for_aliasing(dst, src); ++ if (!internal::is_constant_evaluated()) { ++ internal::check_for_aliasing(dst, src); ++ } + #endif + + call_dense_assignment_loop(dst, src, func); +diff --git a/Eigen/src/Core/EigenBase.h b/Eigen/src/Core/EigenBase.h +index 6d167006a094181fa3693b19f6b9daeb6f2afb79..894bfc13b15eb994abd90f100da15de5bd8b22b7 100644 +--- a/Eigen/src/Core/EigenBase.h ++++ b/Eigen/src/Core/EigenBase.h +@@ -50,7 +50,7 @@ struct EigenBase { + /** \returns a const reference to the derived object */ + EIGEN_DEVICE_FUNC constexpr const Derived& derived() const { return *static_cast(this); } + +- EIGEN_DEVICE_FUNC inline Derived& const_cast_derived() const { ++ EIGEN_DEVICE_FUNC inline constexpr Derived& const_cast_derived() const { + return *static_cast(const_cast(this)); + } + EIGEN_DEVICE_FUNC inline const Derived& const_derived() const { return *static_cast(this); } +diff --git a/Eigen/src/Core/functors/AssignmentFunctors.h b/Eigen/src/Core/functors/AssignmentFunctors.h +index 09d1da8ca2bcb41384520f46e2b793ba8b28a798..3687bb20db4dfe1a2f6cf1342b4fcbd8f91f1f68 100644 +--- a/Eigen/src/Core/functors/AssignmentFunctors.h ++++ b/Eigen/src/Core/functors/AssignmentFunctors.h +@@ -23,7 +23,7 @@ namespace internal { + */ + template + struct assign_op { +- EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; } ++ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; } + + template + EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const { diff --git a/wpimath/src/main/native/include/frc/ct_matrix.h b/wpimath/src/main/native/include/frc/ct_matrix.h index b520cb46fda..ffa684738ac 100644 --- a/wpimath/src/main/native/include/frc/ct_matrix.h +++ b/wpimath/src/main/native/include/frc/ct_matrix.h @@ -163,8 +163,8 @@ class ct_matrix { if (std::is_constant_evaluated()) { ct_matrix result; - for (int row = 0; row < 3; ++row) { - for (int col = 0; col < 3; ++col) { + for (int row = 0; row < rhs.rows(); ++row) { + for (int col = 0; col < rhs.cols(); ++col) { result(row, col) = lhs(row, col) + rhs(row, col); } } @@ -188,8 +188,8 @@ class ct_matrix { if (std::is_constant_evaluated()) { ct_matrix result; - for (int row = 0; row < 3; ++row) { - for (int col = 0; col < 3; ++col) { + for (int row = 0; row < rhs.rows(); ++row) { + for (int col = 0; col < rhs.cols(); ++col) { result(row, col) = lhs(row, col) - rhs(row, col); } } @@ -282,8 +282,8 @@ class ct_matrix { if (std::is_constant_evaluated()) { Scalar sum = 0.0; - for (int row = 0; row < Rows; ++row) { - for (int col = 0; col < Cols; ++col) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { sum += (*this)(row, col) * (*this)(row, col); } } diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h index 0af472b934f..53811240769 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -180,9 +180,9 @@ class WPILIB_DLLEXPORT LinearSystemId { } Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}}; - Matrixd<2, 1> B{0.0, 1.0 / kA.value()}; - Matrixd<1, 2> C{1.0, 0.0}; - Matrixd<1, 1> D{0.0}; + Matrixd<2, 1> B{{0.0}, {1.0 / kA.value()}}; + Matrixd<1, 2> C{{1.0, 0.0}}; + Matrixd<1, 1> D{{0.0}}; return LinearSystem<2, 1, 1>(A, B, C, D); } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h index f7f0b238b8c..9c2436afa7f 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h @@ -263,7 +263,7 @@ struct copy_using_evaluator_innervec_CompleteUnrolling { DstAlignment = Kernel::AssignmentTraits::DstAlignment }; - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { kernel.template assignPacketByOuterInner(outer, inner); enum { NextIndex = Index + unpacket_traits::size }; copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); @@ -431,17 +431,25 @@ struct dense_assignment_loop { template struct dense_assignment_loop { EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { - typedef typename Kernel::DstEvaluatorType::XprType DstXprType; - typedef typename Kernel::PacketType PacketType; - - enum { - size = DstXprType::SizeAtCompileTime, - packetSize = unpacket_traits::size, - alignedSize = (int(size) / packetSize) * packetSize - }; - - copy_using_evaluator_linearvec_CompleteUnrolling::run(kernel); - copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); + if (internal::is_constant_evaluated()) { + for (Index outer = 0; outer < kernel.outerSize(); ++outer) { + for (Index inner = 0; inner < kernel.innerSize(); ++inner) { + kernel.assignCoeffByOuterInner(outer, inner); + } + } + } else { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + typedef typename Kernel::PacketType PacketType; + + enum { + size = DstXprType::SizeAtCompileTime, + packetSize = unpacket_traits::size, + alignedSize = (int(size) / packetSize) * packetSize + }; + + copy_using_evaluator_linearvec_CompleteUnrolling::run(kernel); + copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); + } } }; @@ -465,9 +473,17 @@ struct dense_assignment_loop { template struct dense_assignment_loop { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { - typedef typename Kernel::DstEvaluatorType::XprType DstXprType; - copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { + if (internal::is_constant_evaluated()) { + for (Index outer = 0; outer < kernel.outerSize(); ++outer) { + for (Index inner = 0; inner < kernel.innerSize(); ++inner) { + kernel.assignCoeffByOuterInner(outer, inner); + } + } + } else { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); + } } }; @@ -498,8 +514,16 @@ struct dense_assignment_loop { template struct dense_assignment_loop { EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { - typedef typename Kernel::DstEvaluatorType::XprType DstXprType; - copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); + if (internal::is_constant_evaluated()) { + for (Index outer = 0; outer < kernel.outerSize(); ++outer) { + for (Index inner = 0; inner < kernel.innerSize(); ++inner) { + kernel.assignCoeffByOuterInner(outer, inner); + } + } + } else { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); + } } }; @@ -510,41 +534,49 @@ struct dense_assignment_loop { template struct dense_assignment_loop { EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { - typedef typename Kernel::Scalar Scalar; - typedef typename Kernel::PacketType PacketType; - enum { - packetSize = unpacket_traits::size, - requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment), - alignable = - packet_traits::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar), - dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment), - dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment) - }; - const Scalar* dst_ptr = kernel.dstDataPtr(); - if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) { - // the pointer is not aligned-on scalar, so alignment is not possible - return dense_assignment_loop::run(kernel); - } - const Index packetAlignedMask = packetSize - 1; - const Index innerSize = kernel.innerSize(); - const Index outerSize = kernel.outerSize(); - const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0; - Index alignedStart = - ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned(dst_ptr, innerSize); - - for (Index outer = 0; outer < outerSize; ++outer) { - const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask); - // do the non-vectorizable part of the assignment - for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner); - - // do the vectorizable part of the assignment - for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize) - kernel.template assignPacketByOuterInner(outer, inner); - - // do the non-vectorizable part of the assignment - for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner); - - alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize); + if (internal::is_constant_evaluated()) { + for (Index outer = 0; outer < kernel.outerSize(); ++outer) { + for (Index inner = 0; inner < kernel.innerSize(); ++inner) { + kernel.assignCoeffByOuterInner(outer, inner); + } + } + } else { + typedef typename Kernel::Scalar Scalar; + typedef typename Kernel::PacketType PacketType; + enum { + packetSize = unpacket_traits::size, + requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment), + alignable = + packet_traits::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar), + dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment), + dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment) + }; + const Scalar* dst_ptr = kernel.dstDataPtr(); + if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) { + // the pointer is not aligned-on scalar, so alignment is not possible + return dense_assignment_loop::run(kernel); + } + const Index packetAlignedMask = packetSize - 1; + const Index innerSize = kernel.innerSize(); + const Index outerSize = kernel.outerSize(); + const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0; + Index alignedStart = + ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned(dst_ptr, innerSize); + + for (Index outer = 0; outer < outerSize; ++outer) { + const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask); + // do the non-vectorizable part of the assignment + for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner); + + // do the vectorizable part of the assignment + for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize) + kernel.template assignPacketByOuterInner(outer, inner); + + // do the non-vectorizable part of the assignment + for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner); + + alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize); + } } } }; @@ -594,9 +626,9 @@ class generic_dense_assignment_kernel { typedef copy_using_evaluator_traits AssignmentTraits; typedef typename AssignmentTraits::PacketType PacketType; - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE generic_dense_assignment_kernel(DstEvaluatorType& dst, - const SrcEvaluatorType& src, - const Functor& func, DstXprType& dstExpr) + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE constexpr generic_dense_assignment_kernel(DstEvaluatorType& dst, const SrcEvaluatorType& src, + const Functor& func, DstXprType& dstExpr) : m_dst(dst), m_src(src), m_functor(func), m_dstExpr(dstExpr) { #ifdef EIGEN_DEBUG_ASSIGN AssignmentTraits::debug(); @@ -614,7 +646,7 @@ class generic_dense_assignment_kernel { EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const EIGEN_NOEXCEPT { return m_src; } /// Assign src(row,col) to dst(row,col) through the assignment functor. - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index row, Index col) { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(Index row, Index col) { m_functor.assignCoeff(m_dst.coeffRef(row, col), m_src.coeff(row, col)); } @@ -624,7 +656,7 @@ class generic_dense_assignment_kernel { } /// \sa assignCoeff(Index,Index) - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeffByOuterInner(Index outer, Index inner) { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeffByOuterInner(Index outer, Index inner) { Index row = rowIndexByOuterInner(outer, inner); Index col = colIndexByOuterInner(outer, inner); assignCoeff(row, col); @@ -648,7 +680,7 @@ class generic_dense_assignment_kernel { assignPacket(row, col); } - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner) { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index rowIndexByOuterInner(Index outer, Index inner) { typedef typename DstEvaluatorType::ExpressionTraits Traits; return int(Traits::RowsAtCompileTime) == 1 ? 0 : int(Traits::ColsAtCompileTime) == 1 ? inner @@ -656,7 +688,7 @@ class generic_dense_assignment_kernel { : inner; } - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner) { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index colIndexByOuterInner(Index outer, Index inner) { typedef typename DstEvaluatorType::ExpressionTraits Traits; return int(Traits::ColsAtCompileTime) == 1 ? 0 : int(Traits::RowsAtCompileTime) == 1 ? inner @@ -708,8 +740,8 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, co } template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, const SrcXprType& src, - const internal::assign_op& /*func*/) { +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void resize_if_allowed(DstXprType& dst, const SrcXprType& src, + const internal::assign_op& /*func*/) { Index dstRows = src.rows(); Index dstCols = src.cols(); if (((dst.rows() != dstRows) || (dst.cols() != dstCols))) dst.resize(dstRows, dstCols); @@ -790,7 +822,7 @@ struct Assignment; // not has to bother about these annoying details. template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(Dst& dst, const Src& src) { +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment(Dst& dst, const Src& src) { call_assignment(dst, src, internal::assign_op()); } template @@ -807,7 +839,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment( } template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment( +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment( Dst& dst, const Src& src, const Func& func, std::enable_if_t::value, void*> = 0) { call_assignment_no_alias(dst, src, func); } @@ -891,9 +923,12 @@ EIGEN_DEVICE_FUNC void check_for_aliasing(const Dst& dst, const Src& src); // both partial specialization+SFINAE without ambiguous specialization template struct Assignment { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, const Functor& func) { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(DstXprType& dst, const SrcXprType& src, + const Functor& func) { #ifndef EIGEN_NO_DEBUG - internal::check_for_aliasing(dst, src); + if (!internal::is_constant_evaluated()) { + internal::check_for_aliasing(dst, src); + } #endif call_dense_assignment_loop(dst, src, func); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h index 6d167006a09..894bfc13b15 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h @@ -50,7 +50,7 @@ struct EigenBase { /** \returns a const reference to the derived object */ EIGEN_DEVICE_FUNC constexpr const Derived& derived() const { return *static_cast(this); } - EIGEN_DEVICE_FUNC inline Derived& const_cast_derived() const { + EIGEN_DEVICE_FUNC inline constexpr Derived& const_cast_derived() const { return *static_cast(const_cast(this)); } EIGEN_DEVICE_FUNC inline const Derived& const_derived() const { return *static_cast(this); } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h index 09d1da8ca2b..3687bb20db4 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h @@ -23,7 +23,7 @@ namespace internal { */ template struct assign_op { - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; } template EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const { diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp index 00065df514f..04d9c57ed91 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -79,8 +79,8 @@ TEST(Rotation2dTest, Inequality) { } TEST(Rotation2dTest, ToMatrix) { - Rotation2d before{20_deg}; - Rotation2d after{before.ToMatrix()}; + constexpr Rotation2d before{20_deg}; + constexpr Rotation2d after{before.ToMatrix()}; EXPECT_EQ(before, after); } diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp index 6088bdd7590..7a9c7faeb8f 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp @@ -308,8 +308,8 @@ TEST(Rotation3dTest, Inequality) { } TEST(Rotation3dTest, ToMatrix) { - Rotation3d before{10_deg, 20_deg, 30_deg}; - Rotation3d after{before.ToMatrix()}; + constexpr Rotation3d before{10_deg, 20_deg, 30_deg}; + constexpr Rotation3d after{before.ToMatrix()}; EXPECT_EQ(before, after); } diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp index 39e8a8c2564..d245c6169ee 100644 --- a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp +++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp @@ -12,7 +12,7 @@ #include "units/mass.h" TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) { - auto model = frc::LinearSystemId::DrivetrainVelocitySystem( + constexpr auto model = frc::LinearSystemId::DrivetrainVelocitySystem( frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0); ASSERT_TRUE(model.A().isApprox( @@ -37,8 +37,8 @@ TEST(LinearSystemIDTest, ElevatorSystem) { } TEST(LinearSystemIDTest, FlywheelSystem) { - auto model = frc::LinearSystemId::FlywheelSystem(frc::DCMotor::NEO(2), - 0.00032_kg_sq_m, 1.0); + constexpr auto model = frc::LinearSystemId::FlywheelSystem( + frc::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0); ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-26.87032}, 0.001)); ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1354.166667}, 0.001)); ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 1>{1.0}, 0.001)); @@ -46,8 +46,8 @@ TEST(LinearSystemIDTest, FlywheelSystem) { } TEST(LinearSystemIDTest, DCMotorSystem) { - auto model = frc::LinearSystemId::DCMotorSystem(frc::DCMotor::NEO(2), - 0.00032_kg_sq_m, 1.0); + constexpr auto model = frc::LinearSystemId::DCMotorSystem( + frc::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0); ASSERT_TRUE( model.A().isApprox(frc::Matrixd<2, 2>{{0, 1}, {0, -26.87032}}, 0.001)); ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0, 1354.166667}, 0.001)); @@ -59,10 +59,11 @@ TEST(LinearSystemIDTest, DCMotorSystem) { TEST(LinearSystemIDTest, IdentifyPositionSystem) { // By controls engineering in frc, // x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u - double kv = 1.0; - double ka = 0.5; - auto model = frc::LinearSystemId::IdentifyPositionSystem( - kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq); + constexpr double kv = 1.0; + constexpr double ka = 0.5; + constexpr auto model = + frc::LinearSystemId::IdentifyPositionSystem( + kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq); ASSERT_TRUE(model.A().isApprox( frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001)); @@ -73,10 +74,11 @@ TEST(LinearSystemIDTest, IdentifyVelocitySystem) { // By controls engineering in frc, // V = kv * velocity + ka * acceleration // x-dot = -kv/ka * v + 1/ka \cdot V - double kv = 1.0; - double ka = 0.5; - auto model = frc::LinearSystemId::IdentifyVelocitySystem( - kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq); + constexpr double kv = 1.0; + constexpr double ka = 0.5; + constexpr auto model = + frc::LinearSystemId::IdentifyVelocitySystem( + kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq); ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-kv / ka}, 0.001)); ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1.0 / ka}, 0.001));