diff --git a/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp b/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp deleted file mode 100644 index 97968dce908..00000000000 --- a/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp +++ /dev/null @@ -1,19 +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. - -#include "frc/ComputerVisionUtil.h" - -#include "units/math.h" - -namespace frc { - -frc::Pose3d ObjectToRobotPose(const frc::Pose3d& objectInField, - const frc::Transform3d& cameraToObject, - const frc::Transform3d& robotToCamera) { - const auto objectToCamera = cameraToObject.Inverse(); - const auto cameraToRobot = robotToCamera.Inverse(); - return objectInField + objectToCamera + cameraToRobot; -} - -} // namespace frc diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp index 5dcf1a1798a..f0f6786e1a1 100644 --- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp +++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp @@ -6,18 +6,6 @@ namespace frc { -Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) { - return Eigen::Vector3d{pose.Translation().X().value(), - pose.Translation().Y().value(), - pose.Rotation().Radians().value()}; -} - -Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) { - return Eigen::Vector4d{pose.Translation().X().value(), - pose.Translation().Y().value(), pose.Rotation().Cos(), - pose.Rotation().Sin()}; -} - template bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B); template bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, @@ -25,9 +13,4 @@ template bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, template bool IsStabilizable( const Eigen::MatrixXd& A, const Eigen::MatrixXd& B); -Eigen::Vector3d PoseToVector(const Pose2d& pose) { - return Eigen::Vector3d{pose.X().value(), pose.Y().value(), - pose.Rotation().Radians().value()}; -} - } // namespace frc diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp deleted file mode 100644 index 33c390d072d..00000000000 --- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp +++ /dev/null @@ -1,20 +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. - -#include "frc/kinematics/DifferentialDriveWheelSpeeds.h" - -#include "units/math.h" - -using namespace frc; - -void DifferentialDriveWheelSpeeds::Desaturate( - units::meters_per_second_t attainableMaxSpeed) { - auto realMaxSpeed = - units::math::max(units::math::abs(left), units::math::abs(right)); - - if (realMaxSpeed > attainableMaxSpeed) { - left = left / realMaxSpeed * attainableMaxSpeed; - right = right / realMaxSpeed * attainableMaxSpeed; - } -} diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp deleted file mode 100644 index 0f5de422e26..00000000000 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp +++ /dev/null @@ -1,33 +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. - -#include "frc/kinematics/MecanumDriveWheelSpeeds.h" - -#include -#include -#include - -#include "units/math.h" - -using namespace frc; - -void MecanumDriveWheelSpeeds::Desaturate( - units::meters_per_second_t attainableMaxSpeed) { - std::array wheelSpeeds{frontLeft, frontRight, - rearLeft, rearRight}; - units::meters_per_second_t realMaxSpeed = units::math::abs(*std::max_element( - wheelSpeeds.begin(), wheelSpeeds.end(), [](const auto& a, const auto& b) { - return units::math::abs(a) < units::math::abs(b); - })); - - if (realMaxSpeed > attainableMaxSpeed) { - for (int i = 0; i < 4; ++i) { - wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed; - } - frontLeft = wheelSpeeds[0]; - frontRight = wheelSpeeds[1]; - rearLeft = wheelSpeeds[2]; - rearRight = wheelSpeeds[3]; - } -} diff --git a/wpimath/src/main/native/cpp/kinematics/SwerveModulePosition.cpp b/wpimath/src/main/native/cpp/kinematics/SwerveModulePosition.cpp deleted file mode 100644 index 5343de91260..00000000000 --- a/wpimath/src/main/native/cpp/kinematics/SwerveModulePosition.cpp +++ /dev/null @@ -1,15 +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. - -#include "frc/kinematics/SwerveModulePosition.h" - -#include "frc/kinematics/SwerveModuleState.h" -#include "units/math.h" - -using namespace frc; - -bool SwerveModulePosition::operator==(const SwerveModulePosition& other) const { - return units::math::abs(distance - other.distance) < 1E-9_m && - angle == other.angle; -} diff --git a/wpimath/src/main/native/cpp/kinematics/SwerveModuleState.cpp b/wpimath/src/main/native/cpp/kinematics/SwerveModuleState.cpp deleted file mode 100644 index 071d53a35ed..00000000000 --- a/wpimath/src/main/native/cpp/kinematics/SwerveModuleState.cpp +++ /dev/null @@ -1,24 +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. - -#include "frc/kinematics/SwerveModuleState.h" - -#include "units/math.h" - -using namespace frc; - -bool SwerveModuleState::operator==(const SwerveModuleState& other) const { - return units::math::abs(speed - other.speed) < 1E-9_mps && - angle == other.angle; -} - -SwerveModuleState SwerveModuleState::Optimize( - const SwerveModuleState& desiredState, const Rotation2d& currentAngle) { - auto delta = desiredState.angle - currentAngle; - if (units::math::abs(delta.Degrees()) > 90_deg) { - return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}}; - } else { - return {desiredState.speed, desiredState.angle}; - } -} diff --git a/wpimath/src/main/native/include/frc/ComputerVisionUtil.h b/wpimath/src/main/native/include/frc/ComputerVisionUtil.h index 54f9752a96d..072df05003c 100644 --- a/wpimath/src/main/native/include/frc/ComputerVisionUtil.h +++ b/wpimath/src/main/native/include/frc/ComputerVisionUtil.h @@ -30,8 +30,12 @@ namespace frc { * @return The robot's field-relative pose. */ WPILIB_DLLEXPORT -frc::Pose3d ObjectToRobotPose(const frc::Pose3d& objectInField, - const frc::Transform3d& cameraToObject, - const frc::Transform3d& robotToCamera); +constexpr frc::Pose3d ObjectToRobotPose(const frc::Pose3d& objectInField, + const frc::Transform3d& cameraToObject, + const frc::Transform3d& robotToCamera) { + const auto objectToCamera = cameraToObject.Inverse(); + const auto cameraToRobot = robotToCamera.Inverse(); + return objectInField + objectToCamera + cameraToRobot; +} } // namespace frc diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h index 76e11ce64b0..9bb70a0a5f8 100644 --- a/wpimath/src/main/native/include/frc/MathUtil.h +++ b/wpimath/src/main/native/include/frc/MathUtil.h @@ -7,6 +7,7 @@ #include #include +#include #include #include "units/angle.h" @@ -28,10 +29,10 @@ namespace frc { */ template requires std::is_arithmetic_v || units::traits::is_unit_t_v -T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) { +constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) { T magnitude; if constexpr (std::is_arithmetic_v) { - magnitude = std::abs(value); + magnitude = gcem::abs(value); } else { magnitude = units::math::abs(value); } diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h index f851c03e393..7a3b67b89ae 100644 --- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h +++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h @@ -209,7 +209,11 @@ Vectord MakeWhiteNoiseVector(const std::array& stdDevs) { * @return The vector. */ WPILIB_DLLEXPORT -Eigen::Vector3d PoseTo3dVector(const Pose2d& pose); +constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) { + return Eigen::Vector3d{{pose.Translation().X().value(), + pose.Translation().Y().value(), + pose.Rotation().Radians().value()}}; +} /** * Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)]. @@ -219,7 +223,11 @@ Eigen::Vector3d PoseTo3dVector(const Pose2d& pose); * @return The vector. */ WPILIB_DLLEXPORT -Eigen::Vector4d PoseTo4dVector(const Pose2d& pose); +constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) { + return Eigen::Vector4d{{pose.Translation().X().value(), + pose.Translation().Y().value(), pose.Rotation().Cos(), + pose.Rotation().Sin()}}; +} /** * Returns true if (A, B) is a stabilizable pair. @@ -306,7 +314,10 @@ bool IsDetectable(const Matrixd& A, * @return The vector. */ WPILIB_DLLEXPORT -Eigen::Vector3d PoseToVector(const Pose2d& pose); +constexpr Eigen::Vector3d PoseToVector(const Pose2d& pose) { + return Eigen::Vector3d{ + {pose.X().value(), pose.Y().value(), pose.Rotation().Radians().value()}}; +} /** * Clamps input vector between system's minimum and maximum allowable input. @@ -318,12 +329,12 @@ Eigen::Vector3d PoseToVector(const Pose2d& pose); * @return Clamped input vector. */ template -Vectord ClampInputMaxMagnitude(const Vectord& u, - const Vectord& umin, - const Vectord& umax) { +constexpr Vectord ClampInputMaxMagnitude(const Vectord& u, + const Vectord& umin, + const Vectord& umax) { Vectord result; for (int i = 0; i < Inputs; ++i) { - result(i) = std::clamp(u(i), umin(i), umax(i)); + result.coeffRef(i) = std::clamp(u.coeff(i), umin.coeff(i), umax.coeff(i)); } return result; } diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h index 24a7283cc3f..0d21b7a9f6b 100644 --- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h +++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h @@ -5,13 +5,14 @@ #pragma once #include -#include #include #include #include +#include #include #include +#include #include #include @@ -80,7 +81,8 @@ class LinearFilter { * @param ffGains The "feedforward" or FIR gains. * @param fbGains The "feedback" or IIR gains. */ - LinearFilter(std::span ffGains, std::span fbGains) + constexpr LinearFilter(std::span ffGains, + std::span fbGains) : m_inputs(ffGains.size()), m_outputs(fbGains.size()), m_inputGains(ffGains.begin(), ffGains.end()), @@ -92,10 +94,11 @@ class LinearFilter { m_outputs.emplace_front(0.0); } - static int instances = 0; - instances++; - wpi::math::MathSharedStore::ReportUsage( - wpi::math::MathUsageId::kFilter_Linear, instances); + if (!std::is_constant_evaluated()) { + ++instances; + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kFilter_Linear, instances); + } } /** @@ -104,8 +107,8 @@ class LinearFilter { * @param ffGains The "feedforward" or FIR gains. * @param fbGains The "feedback" or IIR gains. */ - LinearFilter(std::initializer_list ffGains, - std::initializer_list fbGains) + constexpr LinearFilter(std::initializer_list ffGains, + std::initializer_list fbGains) : LinearFilter({ffGains.begin(), ffGains.end()}, {fbGains.begin(), fbGains.end()}) {} @@ -124,9 +127,9 @@ class LinearFilter { * @param period The period in seconds between samples taken by the * user. */ - static LinearFilter SinglePoleIIR(double timeConstant, - units::second_t period) { - double gain = std::exp(-period.value() / timeConstant); + static constexpr LinearFilter SinglePoleIIR(double timeConstant, + units::second_t period) { + double gain = gcem::exp(-period.value() / timeConstant); return LinearFilter({1.0 - gain}, {-gain}); } @@ -144,8 +147,9 @@ class LinearFilter { * @param period The period in seconds between samples taken by the * user. */ - static LinearFilter HighPass(double timeConstant, units::second_t period) { - double gain = std::exp(-period.value() / timeConstant); + static constexpr LinearFilter HighPass(double timeConstant, + units::second_t period) { + double gain = gcem::exp(-period.value() / timeConstant); return LinearFilter({gain, -gain}, {-gain}); } @@ -213,7 +217,7 @@ class LinearFilter { Matrixd S; for (int row = 0; row < Samples; ++row) { for (int col = 0; col < Samples; ++col) { - S(row, col) = std::pow(stencil[col], row); + S(row, col) = gcem::pow(stencil[col], row); } } @@ -224,7 +228,7 @@ class LinearFilter { } Vectord a = - S.householderQr().solve(d) / std::pow(period.value(), Derivative); + S.householderQr().solve(d) / gcem::pow(period.value(), Derivative); // Reverse gains list std::vector ffGains; @@ -266,7 +270,7 @@ class LinearFilter { /** * Reset the filter state. */ - void Reset() { + constexpr void Reset() { std::fill(m_inputs.begin(), m_inputs.end(), T{0.0}); std::fill(m_outputs.begin(), m_outputs.end(), T{0.0}); } @@ -321,7 +325,8 @@ class LinearFilter { * @throws std::runtime_error if size of inputBuffer or outputBuffer does not * match the size of ffGains and fbGains provided in the constructor. */ - void Reset(std::span inputBuffer, std::span outputBuffer) { + constexpr void Reset(std::span inputBuffer, + std::span outputBuffer) { // Clear buffers Reset(); @@ -346,7 +351,7 @@ class LinearFilter { * * @return The filtered value at this step */ - T Calculate(T input) { + constexpr T Calculate(T input) { T retVal{0.0}; // Rotate the inputs @@ -376,7 +381,7 @@ class LinearFilter { * * @return The last value. */ - T LastValue() const { return m_lastOutput; } + constexpr T LastValue() const { return m_lastOutput; } private: wpi::circular_buffer m_inputs; @@ -385,6 +390,9 @@ class LinearFilter { std::vector m_outputGains; T m_lastOutput{0.0}; + // Usage reporting instances + inline static int instances = 0; + /** * Factorial of n. * diff --git a/wpimath/src/main/native/include/frc/filter/MedianFilter.h b/wpimath/src/main/native/include/frc/filter/MedianFilter.h index 4c714fb547e..9969122839c 100644 --- a/wpimath/src/main/native/include/frc/filter/MedianFilter.h +++ b/wpimath/src/main/native/include/frc/filter/MedianFilter.h @@ -25,7 +25,8 @@ class MedianFilter { * * @param size The number of samples in the moving window. */ - explicit MedianFilter(size_t size) : m_valueBuffer(size), m_size{size} {} + constexpr explicit MedianFilter(size_t size) + : m_valueBuffer(size), m_size{size} {} /** * Calculates the moving-window median for the next value of the input stream. @@ -33,7 +34,7 @@ class MedianFilter { * @param next The next input value. * @return The median of the moving window, updated to include the next value. */ - T Calculate(T next) { + constexpr T Calculate(T next) { // Insert next value at proper point in sorted array wpi::insert_sorted(m_orderedValues, next); @@ -66,12 +67,12 @@ class MedianFilter { * * @return The last value. */ - T LastValue() const { return m_valueBuffer.front(); } + constexpr T LastValue() const { return m_valueBuffer.front(); } /** * Resets the filter, clearing the window of all elements. */ - void Reset() { + constexpr void Reset() { m_orderedValues.clear(); m_valueBuffer.reset(); } diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h index 675fac20fcf..724845d8c96 100644 --- a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h +++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h @@ -5,9 +5,7 @@ #pragma once #include -#include #include -#include #include #include #include diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h index 4e8302b154a..2c673418a7f 100644 --- a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h @@ -45,7 +45,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { * * @return Twist2d. */ - Twist2d ToTwist2d(units::second_t dt) const { + constexpr Twist2d ToTwist2d(units::second_t dt) const { return Twist2d{vx * dt, vy * dt, omega * dt}; } @@ -68,10 +68,10 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { * * @return Discretized ChassisSpeeds. */ - static ChassisSpeeds Discretize(units::meters_per_second_t vx, - units::meters_per_second_t vy, - units::radians_per_second_t omega, - units::second_t dt) { + static constexpr ChassisSpeeds Discretize(units::meters_per_second_t vx, + units::meters_per_second_t vy, + units::radians_per_second_t omega, + units::second_t dt) { // Construct the desired pose after a timestep, relative to the current // pose. The desired pose has decoupled translation and rotation. Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt}; @@ -101,8 +101,8 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { * * @return Discretized ChassisSpeeds. */ - static ChassisSpeeds Discretize(const ChassisSpeeds& continuousSpeeds, - units::second_t dt) { + static constexpr ChassisSpeeds Discretize( + const ChassisSpeeds& continuousSpeeds, units::second_t dt) { return Discretize(continuousSpeeds.vx, continuousSpeeds.vy, continuousSpeeds.omega, dt); } @@ -123,7 +123,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { * @return ChassisSpeeds object representing the speeds in the robot's frame * of reference. */ - static ChassisSpeeds FromFieldRelativeSpeeds( + static constexpr ChassisSpeeds FromFieldRelativeSpeeds( units::meters_per_second_t vx, units::meters_per_second_t vy, units::radians_per_second_t omega, const Rotation2d& robotAngle) { // CW rotation into chassis frame @@ -148,7 +148,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { * @return ChassisSpeeds object representing the speeds in the robot's frame * of reference. */ - static ChassisSpeeds FromFieldRelativeSpeeds( + static constexpr ChassisSpeeds FromFieldRelativeSpeeds( const ChassisSpeeds& fieldRelativeSpeeds, const Rotation2d& robotAngle) { return FromFieldRelativeSpeeds(fieldRelativeSpeeds.vx, fieldRelativeSpeeds.vy, @@ -171,7 +171,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { * @return ChassisSpeeds object representing the speeds in the field's frame * of reference. */ - static ChassisSpeeds FromRobotRelativeSpeeds( + static constexpr ChassisSpeeds FromRobotRelativeSpeeds( units::meters_per_second_t vx, units::meters_per_second_t vy, units::radians_per_second_t omega, const Rotation2d& robotAngle) { // CCW rotation out of chassis frame @@ -196,7 +196,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { * @return ChassisSpeeds object representing the speeds in the field's frame * of reference. */ - static ChassisSpeeds FromRobotRelativeSpeeds( + static constexpr ChassisSpeeds FromRobotRelativeSpeeds( const ChassisSpeeds& robotRelativeSpeeds, const Rotation2d& robotAngle) { return FromRobotRelativeSpeeds(robotRelativeSpeeds.vx, robotRelativeSpeeds.vy, diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h index 11c04be7a49..e2a45f10115 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h @@ -4,6 +4,8 @@ #pragma once +#include + #include #include "frc/geometry/Twist2d.h" @@ -36,10 +38,12 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics * empirical value may be larger than the physical measured value due to * scrubbing effects. */ - explicit DifferentialDriveKinematics(units::meter_t trackWidth) + constexpr explicit DifferentialDriveKinematics(units::meter_t trackWidth) : trackWidth(trackWidth) { - wpi::math::MathSharedStore::ReportUsage( - wpi::math::MathUsageId::kKinematics_DifferentialDrive, 1); + if (!std::is_constant_evaluated()) { + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kKinematics_DifferentialDrive, 1); + } } /** @@ -83,12 +87,13 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics (rightDistance - leftDistance) / trackWidth * 1_rad}; } - Twist2d ToTwist2d(const DifferentialDriveWheelPositions& start, - const DifferentialDriveWheelPositions& end) const override { + constexpr Twist2d ToTwist2d( + const DifferentialDriveWheelPositions& start, + const DifferentialDriveWheelPositions& end) const override { return ToTwist2d(end.left - start.left, end.right - start.right); } - DifferentialDriveWheelPositions Interpolate( + constexpr DifferentialDriveWheelPositions Interpolate( const DifferentialDriveWheelPositions& start, const DifferentialDriveWheelPositions& end, double t) const override { return start.Interpolate(end, t); diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h index 60575cf8c73..2bc32ae5c42 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h @@ -31,18 +31,10 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions { * @param other The other object. * @return Whether the two objects are equal. */ - bool operator==(const DifferentialDriveWheelPositions& other) const = default; + constexpr bool operator==( + const DifferentialDriveWheelPositions& other) const = default; - /** - * Checks inequality between this DifferentialDriveWheelPositions and another - * object. - * - * @param other The other object. - * @return Whether the two objects are not equal. - */ - bool operator!=(const DifferentialDriveWheelPositions& other) const = default; - - DifferentialDriveWheelPositions Interpolate( + constexpr DifferentialDriveWheelPositions Interpolate( const DifferentialDriveWheelPositions& endValue, double t) const { return {wpi::Lerp(left, endValue.left, t), wpi::Lerp(right, endValue.right, t)}; diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h index 98db2b861aa..3f30d2f7be2 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h @@ -6,6 +6,7 @@ #include +#include "units/math.h" #include "units/velocity.h" namespace frc { @@ -35,7 +36,15 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelSpeeds { * * @param attainableMaxSpeed The absolute max speed that a wheel can reach. */ - void Desaturate(units::meters_per_second_t attainableMaxSpeed); + constexpr void Desaturate(units::meters_per_second_t attainableMaxSpeed) { + auto realMaxSpeed = + units::math::max(units::math::abs(left), units::math::abs(right)); + + if (realMaxSpeed > attainableMaxSpeed) { + left = left / realMaxSpeed * attainableMaxSpeed; + right = right / realMaxSpeed * attainableMaxSpeed; + } + } /** * Adds two DifferentialDriveWheelSpeeds and returns the sum. diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h index 8a9943bcc80..da71d484dbe 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h @@ -40,18 +40,10 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelPositions { * @param other The other object. * @return Whether the two objects are equal. */ - bool operator==(const MecanumDriveWheelPositions& other) const = default; + constexpr bool operator==(const MecanumDriveWheelPositions& other) const = + default; - /** - * Checks inequality between this MecanumDriveWheelPositions and another - * object. - * - * @param other The other object. - * @return Whether the two objects are not equal. - */ - bool operator!=(const MecanumDriveWheelPositions& other) const = default; - - MecanumDriveWheelPositions Interpolate( + constexpr MecanumDriveWheelPositions Interpolate( const MecanumDriveWheelPositions& endValue, double t) const { return {wpi::Lerp(frontLeft, endValue.frontLeft, t), wpi::Lerp(frontRight, endValue.frontRight, t), diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h index 10f971e988a..348ead8763d 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h @@ -4,8 +4,13 @@ #pragma once +#include +#include +#include + #include +#include "units/math.h" #include "units/velocity.h" namespace frc { @@ -45,7 +50,25 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds { * * @param attainableMaxSpeed The absolute max speed that a wheel can reach. */ - void Desaturate(units::meters_per_second_t attainableMaxSpeed); + constexpr void Desaturate(units::meters_per_second_t attainableMaxSpeed) { + std::array wheelSpeeds{frontLeft, frontRight, + rearLeft, rearRight}; + units::meters_per_second_t realMaxSpeed = units::math::abs( + *std::max_element(wheelSpeeds.begin(), wheelSpeeds.end(), + [](const auto& a, const auto& b) { + return units::math::abs(a) < units::math::abs(b); + })); + + if (realMaxSpeed > attainableMaxSpeed) { + for (int i = 0; i < 4; ++i) { + wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed; + } + frontLeft = wheelSpeeds[0]; + frontRight = wheelSpeeds[1]; + rearLeft = wheelSpeeds[2]; + rearRight = wheelSpeeds[3]; + } + } /** * Adds two MecanumDriveWheelSpeeds and returns the sum. diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h index 4cbe97fcb57..9c1cd83d5bd 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h @@ -8,7 +8,6 @@ #include #include "frc/geometry/Rotation2d.h" -#include "units/angle.h" #include "units/length.h" #include "units/math.h" @@ -33,10 +32,13 @@ struct WPILIB_DLLEXPORT SwerveModulePosition { * @param other The other object. * @return Whether the two objects are equal. */ - bool operator==(const SwerveModulePosition& other) const; + constexpr bool operator==(const SwerveModulePosition& other) const { + return units::math::abs(distance - other.distance) < 1E-9_m && + angle == other.angle; + } - SwerveModulePosition Interpolate(const SwerveModulePosition& endValue, - double t) const { + constexpr SwerveModulePosition Interpolate( + const SwerveModulePosition& endValue, double t) const { return {wpi::Lerp(distance, endValue.distance, t), wpi::Lerp(angle, endValue.angle, t)}; } diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h index ccba5db12a8..93c08fc3f04 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h @@ -32,7 +32,10 @@ struct WPILIB_DLLEXPORT SwerveModuleState { * @param other The other object. * @return Whether the two objects are equal. */ - bool operator==(const SwerveModuleState& other) const; + constexpr bool operator==(const SwerveModuleState& other) const { + return units::math::abs(speed - other.speed) < 1E-9_mps && + angle == other.angle; + } /** * Minimize the change in the heading this swerve module state would @@ -42,7 +45,7 @@ struct WPILIB_DLLEXPORT SwerveModuleState { * * @param currentAngle The current module angle. */ - void Optimize(const Rotation2d& currentAngle) { + constexpr void Optimize(const Rotation2d& currentAngle) { auto delta = angle - currentAngle; if (units::math::abs(delta.Degrees()) > 90_deg) { speed *= -1; @@ -60,8 +63,15 @@ struct WPILIB_DLLEXPORT SwerveModuleState { * @param currentAngle The current module angle. */ [[deprecated("Use instance method instead.")]] - static SwerveModuleState Optimize(const SwerveModuleState& desiredState, - const Rotation2d& currentAngle); + constexpr static SwerveModuleState Optimize( + const SwerveModuleState& desiredState, const Rotation2d& currentAngle) { + auto delta = desiredState.angle - currentAngle; + if (units::math::abs(delta.Degrees()) > 90_deg) { + return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}}; + } else { + return {desiredState.speed, desiredState.angle}; + } + } /** * Scales speed by cosine of angle error. This scales down movement @@ -70,7 +80,7 @@ struct WPILIB_DLLEXPORT SwerveModuleState { * * @param currentAngle The current module angle. */ - void CosineScale(const Rotation2d& currentAngle) { + constexpr void CosineScale(const Rotation2d& currentAngle) { speed *= (angle - currentAngle).Cos(); } }; diff --git a/wpimath/src/main/native/include/units/base.h b/wpimath/src/main/native/include/units/base.h index 006c52c09e2..5d375944484 100644 --- a/wpimath/src/main/native/include/units/base.h +++ b/wpimath/src/main/native/include/units/base.h @@ -109,8 +109,8 @@ namespace units namespace units { - template inline constexpr const char* name(const T&); - template inline constexpr const char* abbreviation(const T&); + template constexpr const char* name(const T&); + template constexpr const char* abbreviation(const T&); } //------------------------------ @@ -231,11 +231,11 @@ namespace units * @param abbreviation - abbreviated unit name, e.g. 'm' */ #define UNIT_ADD_NAME(namespaceName, nameSingular, abbrev)\ -template<> inline constexpr const char* name(const namespaceName::nameSingular ## _t&)\ +template<> constexpr const char* name(const namespaceName::nameSingular ## _t&)\ {\ return #nameSingular;\ }\ -template<> inline constexpr const char* abbreviation(const namespaceName::nameSingular ## _t&)\ +template<> constexpr const char* abbreviation(const namespaceName::nameSingular ## _t&)\ {\ return #abbrev;\ } @@ -255,11 +255,11 @@ template<> inline constexpr const char* abbreviation(const namespaceName::nameSi #define UNIT_ADD_LITERALS(namespaceName, nameSingular, abbreviation)\ namespace literals\ {\ - inline constexpr namespaceName::nameSingular ## _t operator""_ ## abbreviation(long double d)\ + constexpr namespaceName::nameSingular ## _t operator""_ ## abbreviation(long double d)\ {\ return namespaceName::nameSingular ## _t(static_cast(d));\ }\ - inline constexpr namespaceName::nameSingular ## _t operator""_ ## abbreviation (unsigned long long d)\ + constexpr namespaceName::nameSingular ## _t operator""_ ## abbreviation (unsigned long long d)\ {\ return namespaceName::nameSingular ## _t(static_cast(d));\ }\ @@ -365,7 +365,7 @@ template<> inline constexpr const char* abbreviation(const namespaceName::nameSi namespace traits\ {\ template struct is_ ## unitCategory ## _unit : std::integral_constant>::value...>::value> {};\ - template inline constexpr bool is_ ## unitCategory ## _unit_v = is_ ## unitCategory ## _unit::value;\ + template constexpr bool is_ ## unitCategory ## _unit_v = is_ ## unitCategory ## _unit::value;\ }\ template \ concept unitCategory ## _unit = traits::is_ ## unitCategory ## _unit_v; @@ -593,7 +593,7 @@ namespace units has_den::value> {}; template - inline constexpr bool is_ratio_v = is_ratio::value; + constexpr bool is_ratio_v = is_ratio::value; } //------------------------------ @@ -619,7 +619,7 @@ namespace units template struct all_true : std::is_same, units::bool_pack> {}; template - inline constexpr bool all_true_t_v = all_true::type::value; + constexpr bool all_true_t_v = all_true::type::value; /** @endcond */ // DOXYGEN IGNORE /** @@ -721,7 +721,7 @@ namespace units template struct is_unit : std::is_base_of::type {}; template - inline constexpr bool is_unit_v = is_unit::value; + constexpr bool is_unit_v = is_unit::value; } /** @} */ // end of TypeTraits @@ -1532,7 +1532,7 @@ namespace units struct is_convertible_unit : std::is_same ::base_unit_type>, base_unit_of::base_unit_type >> {}; template - inline constexpr bool is_convertible_unit_v = is_convertible_unit::value; + constexpr bool is_convertible_unit_v = is_convertible_unit::value; } //------------------------------ @@ -1554,35 +1554,35 @@ namespace units /// convert dispatch for units which are both the same template - static inline constexpr T convert(const T& value, std::true_type, std::false_type, std::false_type) noexcept + static constexpr T convert(const T& value, std::true_type, std::false_type, std::false_type) noexcept { return value; } /// convert dispatch for units which are both the same template - static inline constexpr T convert(const T& value, std::true_type, std::false_type, std::true_type) noexcept + static constexpr T convert(const T& value, std::true_type, std::false_type, std::true_type) noexcept { return value; } /// convert dispatch for units which are both the same template - static inline constexpr T convert(const T& value, std::true_type, std::true_type, std::false_type) noexcept + static constexpr T convert(const T& value, std::true_type, std::true_type, std::false_type) noexcept { return value; } /// convert dispatch for units which are both the same template - static inline constexpr T convert(const T& value, std::true_type, std::true_type, std::true_type) noexcept + static constexpr T convert(const T& value, std::true_type, std::true_type, std::true_type) noexcept { return value; } /// convert dispatch for units of different types w/ no translation and no PI template - static inline constexpr T convert(const T& value, std::false_type, std::false_type, std::false_type) noexcept + static constexpr T convert(const T& value, std::false_type, std::false_type, std::false_type) noexcept { return ((value * Ratio::num) / Ratio::den); } @@ -1590,7 +1590,7 @@ namespace units /// convert dispatch for units of different types w/ no translation, but has PI in numerator // constepxr with PI in numerator template - static inline constexpr + static constexpr std::enable_if_t<(PiRatio::num / PiRatio::den >= 1 && PiRatio::num % PiRatio::den == 0), T> convert(const T& value, std::false_type, std::true_type, std::false_type) noexcept { @@ -1600,7 +1600,7 @@ namespace units /// convert dispatch for units of different types w/ no translation, but has PI in denominator // constexpr with PI in denominator template - static inline constexpr + static constexpr std::enable_if_t<(PiRatio::num / PiRatio::den <= -1 && PiRatio::num % PiRatio::den == 0), T> convert(const T& value, std::false_type, std::true_type, std::false_type) noexcept { @@ -1619,14 +1619,14 @@ namespace units /// convert dispatch for units of different types with a translation, but no PI template - static inline constexpr T convert(const T& value, std::false_type, std::false_type, std::true_type) noexcept + static constexpr T convert(const T& value, std::false_type, std::false_type, std::true_type) noexcept { return ((value * Ratio::num) / Ratio::den) + (static_cast(Translation::num) / Translation::den); } /// convert dispatch for units of different types with a translation AND PI template - static inline constexpr T convert(const T& value, const std::false_type, const std::true_type, const std::true_type) noexcept + static constexpr T convert(const T& value, const std::false_type, const std::true_type, const std::true_type) noexcept { return ((value * std::pow(constants::detail::PI_VAL, PiRatio::num / PiRatio::den) * Ratio::num) / Ratio::den) + (static_cast(Translation::num) / Translation::den); } @@ -1649,7 +1649,7 @@ namespace units * @returns value, converted from units of `UnitFrom` to `UnitTo`. */ template - static inline constexpr T convert(const T& value) noexcept + static constexpr T convert(const T& value) noexcept { static_assert(traits::is_unit::value, "Template parameter `UnitFrom` must be a `unit` type."); static_assert(traits::is_unit::value, "Template parameter `UnitTo` must be a `unit` type."); @@ -1727,7 +1727,7 @@ namespace units template struct has_value_member : traits::detail::has_value_member_impl::type {}; template - inline constexpr bool has_value_member_v = has_value_member::value; + constexpr bool has_value_member_v = has_value_member::value; } /** @endcond */ // END DOXYGEN IGNORE @@ -1867,7 +1867,7 @@ namespace units template struct is_unit_t : std::is_base_of::type {}; template - inline constexpr bool is_unit_t_v = is_unit_t::value; + constexpr bool is_unit_t_v = is_unit_t::value; } /** @@ -1969,7 +1969,7 @@ namespace units * @param[in] value value of the unit_t */ template::value && std::is_arithmetic::value>::type> - inline constexpr unit_t(const Ty value) noexcept : nls(value) + constexpr unit_t(const Ty value) noexcept : nls(value) { } @@ -1980,7 +1980,7 @@ namespace units * @param[in] value value of the unit_t */ template::value && traits::is_ratio::value>> - inline constexpr unit_t(const std::chrono::duration& value) noexcept : + constexpr unit_t(const std::chrono::duration& value) noexcept : nls(units::convert, category::time_unit>, Units>(static_cast(std::chrono::duration_cast(value).count()))) { @@ -1992,7 +1992,7 @@ namespace units * @param[in] rhs unit to copy. */ template class NlsRhs> - inline constexpr unit_t(const unit_t& rhs) noexcept : + constexpr unit_t(const unit_t& rhs) noexcept : nls(units::convert(rhs.m_value), std::true_type() /*store linear value*/) { @@ -2029,7 +2029,7 @@ namespace units * @returns true IFF the value of `this` is less than the value of `rhs` */ template class NlsRhs> - inline constexpr bool operator<(const unit_t& rhs) const noexcept + constexpr bool operator<(const unit_t& rhs) const noexcept { return (nls::m_value < units::convert(rhs.m_value)); } @@ -2041,7 +2041,7 @@ namespace units * @returns true IFF the value of `this` is less than or equal to the value of `rhs` */ template class NlsRhs> - inline constexpr bool operator<=(const unit_t& rhs) const noexcept + constexpr bool operator<=(const unit_t& rhs) const noexcept { return (nls::m_value <= units::convert(rhs.m_value)); } @@ -2053,7 +2053,7 @@ namespace units * @returns true IFF the value of `this` is greater than the value of `rhs` */ template class NlsRhs> - inline constexpr bool operator>(const unit_t& rhs) const noexcept + constexpr bool operator>(const unit_t& rhs) const noexcept { return (nls::m_value > units::convert(rhs.m_value)); } @@ -2065,7 +2065,7 @@ namespace units * @returns true IFF the value of `this` is greater than or equal to the value of `rhs` */ template class NlsRhs> - inline constexpr bool operator>=(const unit_t& rhs) const noexcept + constexpr bool operator>=(const unit_t& rhs) const noexcept { return (nls::m_value >= units::convert(rhs.m_value)); } @@ -2078,7 +2078,7 @@ namespace units * @note This may not be suitable for all applications when the underlying_type of unit_t is a double. */ template class NlsRhs, std::enable_if_t::value || std::is_floating_point::value, int> = 0> - inline constexpr bool operator==(const unit_t& rhs) const noexcept + constexpr bool operator==(const unit_t& rhs) const noexcept { return detail::abs(nls::m_value - units::convert(rhs.m_value)) < std::numeric_limits::epsilon() * detail::abs(nls::m_value + units::convert(rhs.m_value)) || @@ -2086,7 +2086,7 @@ namespace units } template class NlsRhs, std::enable_if_t::value && std::is_integral::value, int> = 0> - inline constexpr bool operator==(const unit_t& rhs) const noexcept + constexpr bool operator==(const unit_t& rhs) const noexcept { return nls::m_value == units::convert(rhs.m_value); } @@ -2099,7 +2099,7 @@ namespace units * @note This may not be suitable for all applications when the underlying_type of unit_t is a double. */ template class NlsRhs> - inline constexpr bool operator!=(const unit_t& rhs) const noexcept + constexpr bool operator!=(const unit_t& rhs) const noexcept { return !(*this == rhs); } @@ -2108,7 +2108,7 @@ namespace units * @brief unit value * @returns value of the unit in it's underlying, non-safe type. */ - inline constexpr underlying_type value() const noexcept + constexpr underlying_type value() const noexcept { return static_cast(*this); } @@ -2118,7 +2118,7 @@ namespace units * @returns value of the unit converted to an arithmetic, non-safe type. */ template::value>> - inline constexpr Ty to() const noexcept + constexpr Ty to() const noexcept { return static_cast(*this); } @@ -2129,7 +2129,7 @@ namespace units * linear scales, this is equivalent to `value`. */ template::value>> - inline constexpr Ty toLinearized() const noexcept + constexpr Ty toLinearized() const noexcept { return static_cast(m_value); } @@ -2144,7 +2144,7 @@ namespace units * *this. */ template - inline constexpr unit_t convert() const noexcept + constexpr unit_t convert() const noexcept { static_assert(traits::is_unit::value, "Template parameter `U` must be a unit type."); return unit_t(*this); @@ -2155,7 +2155,7 @@ namespace units * @details only enabled for scalar unit types. */ template::value && std::is_arithmetic::value, int> = 0> - inline constexpr operator Ty() const noexcept + constexpr operator Ty() const noexcept { // this conversion also resolves any PI exponents, by converting from a non-zero PI ratio to a zero-pi ratio. return static_cast(units::convert, units::category::scalar_unit>>((*this)())); @@ -2166,7 +2166,7 @@ namespace units * @details only enabled for non-dimensionless unit types. */ template::value && std::is_arithmetic::value, int> = 0> - inline constexpr explicit operator Ty() const noexcept + constexpr explicit operator Ty() const noexcept { return static_cast((*this)()); } @@ -2176,7 +2176,7 @@ namespace units * @details only enabled for time unit types. */ template, category::time_unit>>::value, int> = 0> - inline constexpr operator std::chrono::nanoseconds() const noexcept + constexpr operator std::chrono::nanoseconds() const noexcept { return std::chrono::duration_cast(std::chrono::duration(units::convert, category::time_unit>>((*this)()))); } @@ -2184,7 +2184,7 @@ namespace units /** * @brief returns the unit name */ - inline constexpr const char* name() const noexcept + constexpr const char* name() const noexcept { return units::name(*this); } @@ -2192,7 +2192,7 @@ namespace units /** * @brief returns the unit abbreviation */ - inline constexpr const char* abbreviation() const noexcept + constexpr const char* abbreviation() const noexcept { return units::abbreviation(*this); } @@ -2218,7 +2218,7 @@ namespace units * @param[in] value Arithmetic value that represents a quantity in units of `UnitType`. */ template::value>> - inline constexpr UnitType make_unit(const T value) noexcept + constexpr UnitType make_unit(const T value) noexcept { static_assert(traits::is_unit_t::value, "Template parameter `UnitType` must be a unit type (_t)."); @@ -2282,7 +2282,7 @@ namespace units #endif template class NonLinearScale, typename RhsType> - inline unit_t& operator+=(unit_t& lhs, const RhsType& rhs) noexcept + constexpr unit_t& operator+=(unit_t& lhs, const RhsType& rhs) noexcept { static_assert(traits::is_convertible_unit_t, RhsType>::value || (traits::is_dimensionless_unit::value && std::is_arithmetic::value), @@ -2293,7 +2293,7 @@ namespace units } template class NonLinearScale, typename RhsType> - inline unit_t& operator-=(unit_t& lhs, const RhsType& rhs) noexcept + constexpr unit_t& operator-=(unit_t& lhs, const RhsType& rhs) noexcept { static_assert(traits::is_convertible_unit_t, RhsType>::value || (traits::is_dimensionless_unit::value && std::is_arithmetic::value), @@ -2304,7 +2304,7 @@ namespace units } template class NonLinearScale, typename RhsType> - inline unit_t& operator*=(unit_t& lhs, const RhsType& rhs) noexcept + constexpr unit_t& operator*=(unit_t& lhs, const RhsType& rhs) noexcept { static_assert((traits::is_dimensionless_unit::value || std::is_arithmetic::value), "right-hand side parameter must be dimensionless."); @@ -2314,7 +2314,7 @@ namespace units } template class NonLinearScale, typename RhsType> - inline unit_t& operator/=(unit_t& lhs, const RhsType& rhs) noexcept + constexpr unit_t& operator/=(unit_t& lhs, const RhsType& rhs) noexcept { static_assert((traits::is_dimensionless_unit::value || std::is_arithmetic::value), "right-hand side parameter must be dimensionless."); @@ -2329,14 +2329,14 @@ namespace units // unary addition: +T template class NonLinearScale> - constexpr inline unit_t operator+(const unit_t& u) noexcept + constexpr unit_t operator+(const unit_t& u) noexcept { return u; } // prefix increment: ++T template class NonLinearScale> - inline unit_t& operator++(unit_t& u) noexcept + constexpr unit_t& operator++(unit_t& u) noexcept { u = unit_t(u() + 1); return u; @@ -2344,7 +2344,7 @@ namespace units // postfix increment: T++ template class NonLinearScale> - inline unit_t operator++(unit_t& u, int) noexcept + constexpr unit_t operator++(unit_t& u, int) noexcept { auto ret = u; u = unit_t(u() + 1); @@ -2353,14 +2353,14 @@ namespace units // unary addition: -T template class NonLinearScale> - constexpr inline unit_t operator-(const unit_t& u) noexcept + constexpr unit_t operator-(const unit_t& u) noexcept { return unit_t(-u()); } // prefix increment: --T template class NonLinearScale> - inline unit_t& operator--(unit_t& u) noexcept + constexpr unit_t& operator--(unit_t& u) noexcept { u = unit_t(u() - 1); return u; @@ -2368,7 +2368,7 @@ namespace units // postfix increment: T-- template class NonLinearScale> - inline unit_t operator--(unit_t& u, int) noexcept + constexpr unit_t operator--(unit_t& u, int) noexcept { auto ret = u; u = unit_t(u() - 1); @@ -2393,7 +2393,7 @@ namespace units * @sa unit_t::to */ template::value && traits::is_unit_t::value>> - inline constexpr T unit_cast(const Units& value) noexcept + constexpr T unit_cast(const Units& value) noexcept { return static_cast(value); } @@ -2418,7 +2418,7 @@ namespace units template struct has_linear_scale : std::integral_constant::underlying_type>, T>::value...>::value > {}; template - inline constexpr bool has_linear_scale_v = has_linear_scale::value; + constexpr bool has_linear_scale_v = has_linear_scale::value; #else template struct has_linear_scale : std::integral_constant::underlying_type>, T2>::value && std::is_base_of::underlying_type>, T3>::value> {}; template - inline constexpr bool has_linear_scale_v = has_linear_scale::value; + constexpr bool has_linear_scale_v = has_linear_scale::value; #endif /** @@ -2440,7 +2440,7 @@ namespace units template struct has_decibel_scale : std::integral_constant::underlying_type>, T>::value...>::value> {}; template - inline constexpr bool has_decibel_scale_v = has_decibel_scale::value; + constexpr bool has_decibel_scale_v = has_decibel_scale::value; #else template struct has_decibel_scale : std::integral_constant::underlying_type>, T2>::value && std::is_base_of::underlying_type>, T3>::value> {}; template - inline constexpr bool has_decibel_scale_v = has_decibel_scale::value; + constexpr bool has_decibel_scale_v = has_decibel_scale::value; #endif /** @@ -2464,7 +2464,7 @@ namespace units std::is_same::non_linear_scale_type, typename units::traits::unit_t_traits::non_linear_scale_type>::value> {}; template - inline constexpr bool is_same_scale_v = is_same_scale::value; + constexpr bool is_same_scale_v = is_same_scale::value; } //---------------------------------- @@ -2489,17 +2489,17 @@ namespace units template struct linear_scale { - inline constexpr linear_scale() = default; ///< default constructor. - inline constexpr linear_scale(const linear_scale&) = default; + constexpr linear_scale() = default; ///< default constructor. + constexpr linear_scale(const linear_scale&) = default; inline ~linear_scale() = default; inline linear_scale& operator=(const linear_scale&) = default; #if defined(_MSC_VER) && (_MSC_VER > 1800) - inline constexpr linear_scale(linear_scale&&) = default; + constexpr linear_scale(linear_scale&&) = default; inline linear_scale& operator=(linear_scale&&) = default; #endif template - inline constexpr linear_scale(const T& value, Args&&...) noexcept : m_value(value) {} ///< constructor. - inline constexpr T operator()() const noexcept { return m_value; } ///< returns value. + constexpr linear_scale(const T& value, Args&&...) noexcept : m_value(value) {} ///< constructor. + constexpr T operator()() const noexcept { return m_value; } ///< returns value. T m_value; ///< linearized value. }; @@ -2542,7 +2542,7 @@ namespace units /// Addition operator for unit_t types with a linear_scale. template::value, int> = 0> - inline constexpr UnitTypeLhs operator+(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept + constexpr UnitTypeLhs operator+(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept { using UnitsLhs = typename units::traits::unit_t_traits::unit_type; using UnitsRhs = typename units::traits::unit_t_traits::unit_type; @@ -2551,21 +2551,21 @@ namespace units /// Addition operator for scalar unit_t types with a linear_scale. Scalar types can be implicitly converted to built-in types. template::value, int> = 0> - inline constexpr dimensionless::scalar_t operator+(const dimensionless::scalar_t& lhs, T rhs) noexcept + constexpr dimensionless::scalar_t operator+(const dimensionless::scalar_t& lhs, T rhs) noexcept { return dimensionless::scalar_t(lhs() + rhs); } /// Addition operator for scalar unit_t types with a linear_scale. Scalar types can be implicitly converted to built-in types. template::value, int> = 0> - inline constexpr dimensionless::scalar_t operator+(T lhs, const dimensionless::scalar_t& rhs) noexcept + constexpr dimensionless::scalar_t operator+(T lhs, const dimensionless::scalar_t& rhs) noexcept { return dimensionless::scalar_t(lhs + rhs()); } /// Subtraction operator for unit_t types with a linear_scale. template::value, int> = 0> - inline constexpr UnitTypeLhs operator-(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept + constexpr UnitTypeLhs operator-(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept { using UnitsLhs = typename units::traits::unit_t_traits::unit_type; using UnitsRhs = typename units::traits::unit_t_traits::unit_type; @@ -2574,14 +2574,14 @@ namespace units /// Subtraction operator for scalar unit_t types with a linear_scale. Scalar types can be implicitly converted to built-in types. template::value, int> = 0> - inline constexpr dimensionless::scalar_t operator-(const dimensionless::scalar_t& lhs, T rhs) noexcept + constexpr dimensionless::scalar_t operator-(const dimensionless::scalar_t& lhs, T rhs) noexcept { return dimensionless::scalar_t(lhs() - rhs); } /// Subtraction operator for scalar unit_t types with a linear_scale. Scalar types can be implicitly converted to built-in types. template::value, int> = 0> - inline constexpr dimensionless::scalar_t operator-(T lhs, const dimensionless::scalar_t& rhs) noexcept + constexpr dimensionless::scalar_t operator-(T lhs, const dimensionless::scalar_t& rhs) noexcept { return dimensionless::scalar_t(lhs - rhs()); } @@ -2589,7 +2589,7 @@ namespace units /// Multiplication type for convertible unit_t types with a linear scale. @returns the multiplied value, with the same type as left-hand side unit. template::value && traits::has_linear_scale::value, int> = 0> - inline constexpr auto operator*(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t::unit_type>>> + constexpr auto operator*(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t::unit_type>>> { using UnitsLhs = typename units::traits::unit_t_traits::unit_type; using UnitsRhs = typename units::traits::unit_t_traits::unit_type; @@ -2600,7 +2600,7 @@ namespace units /// Multiplication type for non-convertible unit_t types with a linear scale. @returns the multiplied value, whose type is a compound unit of the left and right hand side values. template::value && traits::has_linear_scale::value && !traits::is_dimensionless_unit::value && !traits::is_dimensionless_unit::value, int> = 0> - inline constexpr auto operator*(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t::unit_type, typename units::traits::unit_t_traits::unit_type>> + constexpr auto operator*(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t::unit_type, typename units::traits::unit_t_traits::unit_type>> { using UnitsLhs = typename units::traits::unit_t_traits::unit_type; using UnitsRhs = typename units::traits::unit_t_traits::unit_type; @@ -2611,7 +2611,7 @@ namespace units /// Multiplication by a dimensionless unit for unit_t types with a linear scale. template::value && !traits::is_dimensionless_unit::value && traits::is_dimensionless_unit::value, int> = 0> - inline constexpr UnitTypeLhs operator*(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept + constexpr UnitTypeLhs operator*(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept { // the cast makes sure factors of PI are handled as expected return UnitTypeLhs(lhs() * static_cast(rhs)); @@ -2620,7 +2620,7 @@ namespace units /// Multiplication by a dimensionless unit for unit_t types with a linear scale. template::value && traits::is_dimensionless_unit::value && !traits::is_dimensionless_unit::value, int> = 0> - inline constexpr UnitTypeRhs operator*(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept + constexpr UnitTypeRhs operator*(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept { // the cast makes sure factors of PI are handled as expected return UnitTypeRhs(static_cast(lhs) * rhs()); @@ -2629,7 +2629,7 @@ namespace units /// Multiplication by a scalar for unit_t types with a linear scale. template::value && traits::has_linear_scale::value, int> = 0> - inline constexpr UnitTypeLhs operator*(const UnitTypeLhs& lhs, T rhs) noexcept + constexpr UnitTypeLhs operator*(const UnitTypeLhs& lhs, T rhs) noexcept { return UnitTypeLhs(lhs() * rhs); } @@ -2637,7 +2637,7 @@ namespace units /// Multiplication by a scalar for unit_t types with a linear scale. template::value && traits::has_linear_scale::value, int> = 0> - inline constexpr UnitTypeRhs operator*(T lhs, const UnitTypeRhs& rhs) noexcept + constexpr UnitTypeRhs operator*(T lhs, const UnitTypeRhs& rhs) noexcept { return UnitTypeRhs(lhs * rhs()); } @@ -2645,7 +2645,7 @@ namespace units /// Division for convertible unit_t types with a linear scale. @returns the lhs divided by rhs value, whose type is a scalar template::value && traits::has_linear_scale::value, int> = 0> - inline constexpr dimensionless::scalar_t operator/(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept + constexpr dimensionless::scalar_t operator/(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept { using UnitsLhs = typename units::traits::unit_t_traits::unit_type; using UnitsRhs = typename units::traits::unit_t_traits::unit_type; @@ -2655,7 +2655,7 @@ namespace units /// Division for non-convertible unit_t types with a linear scale. @returns the lhs divided by the rhs, with a compound unit type of lhs/rhs template::value && traits::has_linear_scale::value && !traits::is_dimensionless_unit::value && !traits::is_dimensionless_unit::value, int> = 0> - inline constexpr auto operator/(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t::unit_type, inverse::unit_type>>> + constexpr auto operator/(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t::unit_type, inverse::unit_type>>> { using UnitsLhs = typename units::traits::unit_t_traits::unit_type; using UnitsRhs = typename units::traits::unit_t_traits::unit_type; @@ -2666,7 +2666,7 @@ namespace units /// Division by a dimensionless unit for unit_t types with a linear scale template::value && !traits::is_dimensionless_unit::value && traits::is_dimensionless_unit::value, int> = 0> - inline constexpr UnitTypeLhs operator/(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept + constexpr UnitTypeLhs operator/(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept { return UnitTypeLhs(lhs() / static_cast(rhs)); } @@ -2674,7 +2674,7 @@ namespace units /// Division of a dimensionless unit by a unit_t type with a linear scale template::value && traits::is_dimensionless_unit::value && !traits::is_dimensionless_unit::value, int> = 0> - inline constexpr auto operator/(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t::unit_type>> + constexpr auto operator/(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t::unit_type>> { return unit_t::unit_type>> (static_cast(lhs) / rhs()); @@ -2683,7 +2683,7 @@ namespace units /// Division by a scalar for unit_t types with a linear scale template::value && traits::has_linear_scale::value, int> = 0> - inline constexpr UnitTypeLhs operator/(const UnitTypeLhs& lhs, T rhs) noexcept + constexpr UnitTypeLhs operator/(const UnitTypeLhs& lhs, T rhs) noexcept { return UnitTypeLhs(lhs() / rhs); } @@ -2691,7 +2691,7 @@ namespace units /// Division of a scalar by a unit_t type with a linear scale template::value && traits::has_linear_scale::value, int> = 0> - inline constexpr auto operator/(T lhs, const UnitTypeRhs& rhs) noexcept -> unit_t::unit_type>> + constexpr auto operator/(T lhs, const UnitTypeRhs& rhs) noexcept -> unit_t::unit_type>> { using UnitsRhs = typename units::traits::unit_t_traits::unit_type; return unit_t> @@ -2807,7 +2807,7 @@ namespace units * @returns new unit_t, raised to the given exponent */ template::value, int>> - inline constexpr auto pow(const UnitType& value) noexcept -> unit_t::unit_type>::type, typename units::traits::unit_t_traits::underlying_type, linear_scale> + constexpr auto pow(const UnitType& value) noexcept -> unit_t::unit_type>::type, typename units::traits::unit_t_traits::underlying_type, linear_scale> { return unit_t::unit_type>::type, typename units::traits::unit_t_traits::underlying_type, linear_scale> (gcem::pow(value(), power)); @@ -2822,7 +2822,7 @@ namespace units * @returns new unit_t, raised to the given exponent */ template::value, int>> - inline constexpr auto cpow(const UnitType& value) noexcept -> unit_t::unit_type>::type, typename units::traits::unit_t_traits::underlying_type, linear_scale> + constexpr auto cpow(const UnitType& value) noexcept -> unit_t::unit_type>::type, typename units::traits::unit_t_traits::underlying_type, linear_scale> { static_assert(power >= 0, "cpow cannot accept negative numbers. Try units::math::pow instead."); return unit_t::unit_type>::type, typename units::traits::unit_t_traits::underlying_type, linear_scale> @@ -2843,18 +2843,18 @@ namespace units template struct decibel_scale { - inline constexpr decibel_scale() = default; - inline constexpr decibel_scale(const decibel_scale&) = default; + constexpr decibel_scale() = default; + constexpr decibel_scale(const decibel_scale&) = default; inline ~decibel_scale() = default; inline decibel_scale& operator=(const decibel_scale&) = default; #if defined(_MSC_VER) && (_MSC_VER > 1800) - inline constexpr decibel_scale(decibel_scale&&) = default; + constexpr decibel_scale(decibel_scale&&) = default; inline decibel_scale& operator=(decibel_scale&&) = default; #endif - inline constexpr decibel_scale(const T value) noexcept : m_value(std::pow(10, value / 10)) {} + constexpr decibel_scale(const T value) noexcept : m_value(std::pow(10, value / 10)) {} template - inline constexpr decibel_scale(const T value, std::true_type, Args&&...) noexcept : m_value(value) {} - inline constexpr T operator()() const noexcept { return 10 * std::log10(m_value); } + constexpr decibel_scale(const T value, std::true_type, Args&&...) noexcept : m_value(value) {} + constexpr T operator()() const noexcept { return 10 * std::log10(m_value); } T m_value; ///< linearized value }; @@ -3060,7 +3060,7 @@ namespace units { std::is_base_of, T>::value> {}; template::unit_type> - inline constexpr bool is_unit_value_t_v = is_unit_value_t::value; + constexpr bool is_unit_value_t_v = is_unit_value_t::value; /** * @ingroup TypeTraits @@ -3074,7 +3074,7 @@ namespace units { static_assert(is_base_unit::value, "Template parameter `Category` must be a `base_unit` type."); }; template - inline constexpr bool is_unit_value_t_category_v = is_unit_value_t_category::value; + constexpr bool is_unit_value_t_category_v = is_unit_value_t_category::value; } /** @cond */ // DOXYGEN IGNORE