Loading [MathJax]/extensions/tex2jax.js
WPILibC++ 2025.3.2
All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages Concepts
MathUtil.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <numbers>
8#include <type_traits>
9
10#include <gcem.hpp>
11#include <wpi/SymbolExports.h>
12
15#include "units/angle.h"
16#include "units/base.h"
17#include "units/length.h"
18#include "units/math.h"
19#include "units/time.h"
20#include "units/velocity.h"
21#include "wpimath/MathShared.h"
22
23namespace frc {
24
25/**
26 * Returns 0.0 if the given value is within the specified range around zero. The
27 * remaining range between the deadband and the maximum magnitude is scaled from
28 * 0.0 to the maximum magnitude.
29 *
30 * @param value Value to clip.
31 * @param deadband Range around zero.
32 * @param maxMagnitude The maximum magnitude of the input (defaults to 1). Can
33 * be infinite.
34 * @return The value after the deadband is applied.
35 */
36template <typename T>
37 requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
38constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
39 T magnitude;
40 if constexpr (std::is_arithmetic_v<T>) {
41 magnitude = gcem::abs(value);
42 } else {
43 magnitude = units::math::abs(value);
44 }
45
46 if (magnitude > deadband) {
47 if (maxMagnitude / deadband > 1.0E12) {
48 // If max magnitude is sufficiently large, the implementation encounters
49 // roundoff error. Implementing the limiting behavior directly avoids
50 // the problem.
51 return value > T{0.0} ? value - deadband : value + deadband;
52 }
53 if (value > T{0.0}) {
54 // Map deadband to 0 and map max to max.
55 //
56 // y - y₁ = m(x - x₁)
57 // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
58 // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
59 //
60 // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max).
61 // x₁ = deadband
62 // y₁ = 0
63 // x₂ = max
64 // y₂ = max
65 //
66 // y = (max - 0)/(max - deadband) (x - deadband) + 0
67 // y = max/(max - deadband) (x - deadband)
68 // y = max (x - deadband)/(max - deadband)
69 return maxMagnitude * (value - deadband) / (maxMagnitude - deadband);
70 } else {
71 // Map -deadband to 0 and map -max to -max.
72 //
73 // y - y₁ = m(x - x₁)
74 // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
75 // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
76 //
77 // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
78 // x₁ = -deadband
79 // y₁ = 0
80 // x₂ = -max
81 // y₂ = -max
82 //
83 // y = (-max - 0)/(-max + deadband) (x + deadband) + 0
84 // y = max/(max - deadband) (x + deadband)
85 // y = max (x + deadband)/(max - deadband)
86 return maxMagnitude * (value + deadband) / (maxMagnitude - deadband);
87 }
88 } else {
89 return T{0.0};
90 }
91}
92
93/**
94 * Returns modulus of input.
95 *
96 * @param input Input value to wrap.
97 * @param minimumInput The minimum value expected from the input.
98 * @param maximumInput The maximum value expected from the input.
99 */
100template <typename T>
101constexpr T InputModulus(T input, T minimumInput, T maximumInput) {
102 T modulus = maximumInput - minimumInput;
103
104 // Wrap input if it's above the maximum input
105 int numMax = (input - minimumInput) / modulus;
106 input -= numMax * modulus;
107
108 // Wrap input if it's below the minimum input
109 int numMin = (input - maximumInput) / modulus;
110 input -= numMin * modulus;
111
112 return input;
113}
114
115/**
116 * Checks if the given value matches an expected value within a certain
117 * tolerance.
118 *
119 * @param expected The expected value
120 * @param actual The actual value
121 * @param tolerance The allowed difference between the actual and the expected
122 * value
123 * @return Whether or not the actual value is within the allowed tolerance
124 */
125template <typename T>
126 requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
127constexpr bool IsNear(T expected, T actual, T tolerance) {
128 if constexpr (std::is_arithmetic_v<T>) {
129 return std::abs(expected - actual) < tolerance;
130 } else {
131 return units::math::abs(expected - actual) < tolerance;
132 }
133}
134
135/**
136 * Checks if the given value matches an expected value within a certain
137 * tolerance. Supports continuous input for cases like absolute encoders.
138 *
139 * Continuous input means that the min and max value are considered to be the
140 * same point, and tolerances can be checked across them. A common example
141 * would be for absolute encoders: calling isNear(2, 359, 5, 0, 360) returns
142 * true because 359 is 1 away from 360 (which is treated as the same as 0) and
143 * 2 is 2 away from 0, adding up to an error of 3 degrees, which is within the
144 * given tolerance of 5.
145 *
146 * @param expected The expected value
147 * @param actual The actual value
148 * @param tolerance The allowed difference between the actual and the expected
149 * value
150 * @param min Smallest value before wrapping around to the largest value
151 * @param max Largest value before wrapping around to the smallest value
152 * @return Whether or not the actual value is within the allowed tolerance
153 */
154template <typename T>
155 requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
156constexpr bool IsNear(T expected, T actual, T tolerance, T min, T max) {
157 T errorBound = (max - min) / 2.0;
158 T error = frc::InputModulus<T>(expected - actual, -errorBound, errorBound);
159
160 if constexpr (std::is_arithmetic_v<T>) {
161 return std::abs(error) < tolerance;
162 } else {
163 return units::math::abs(error) < tolerance;
164 }
165}
166
167/**
168 * Wraps an angle to the range -π to π radians (-180 to 180 degrees).
169 *
170 * @param angle Angle to wrap.
171 */
173constexpr units::radian_t AngleModulus(units::radian_t angle) {
175 units::radian_t{-std::numbers::pi},
176 units::radian_t{std::numbers::pi});
177}
178
179// floorDiv and floorMod algorithms taken from Java
180
181/**
182 * Returns the largest (closest to positive infinity)
183 * {@code int} value that is less than or equal to the algebraic quotient.
184 *
185 * @param x the dividend
186 * @param y the divisor
187 * @return the largest (closest to positive infinity)
188 * {@code int} value that is less than or equal to the algebraic quotient.
189 */
190constexpr std::signed_integral auto FloorDiv(std::signed_integral auto x,
191 std::signed_integral auto y) {
192 auto quot = x / y;
193 auto rem = x % y;
194 // if the signs are different and modulo not zero, round down
195 if ((x < 0) != (y < 0) && rem != 0) {
196 --quot;
197 }
198 return quot;
199}
200
201/**
202 * Returns the floor modulus of the {@code int} arguments.
203 * <p>
204 * The floor modulus is {@code r = x - (floorDiv(x, y) * y)},
205 * has the same sign as the divisor {@code y} or is zero, and
206 * is in the range of {@code -std::abs(y) < r < +std::abs(y)}.
207 *
208 * @param x the dividend
209 * @param y the divisor
210 * @return the floor modulus {@code x - (floorDiv(x, y) * y)}
211 */
212constexpr std::signed_integral auto FloorMod(std::signed_integral auto x,
213 std::signed_integral auto y) {
214 return x - FloorDiv(x, y) * y;
215}
216
217/**
218 * Limits translation velocity.
219 *
220 * @param current Translation at current timestep.
221 * @param next Translation at next timestep.
222 * @param dt Timestep duration.
223 * @param maxVelocity Maximum translation velocity.
224 * @return Returns the next Translation2d limited to maxVelocity
225 */
227 const Translation2d& next,
228 units::second_t dt,
229 units::meters_per_second_t maxVelocity) {
230 if (maxVelocity < 0_mps) {
232 "maxVelocity must be a non-negative number, got {}!", maxVelocity);
233 return next;
234 }
235 Translation2d diff = next - current;
236 units::meter_t dist = diff.Norm();
237 if (dist < 1e-9_m) {
238 return next;
239 }
240 if (dist > maxVelocity * dt) {
241 // Move maximum allowed amount in direction of the difference
242 return current + diff * (maxVelocity * dt / dist);
243 }
244 return next;
245}
246
247/**
248 * Limits translation velocity.
249 *
250 * @param current Translation at current timestep.
251 * @param next Translation at next timestep.
252 * @param dt Timestep duration.
253 * @param maxVelocity Maximum translation velocity.
254 * @return Returns the next Translation3d limited to maxVelocity
255 */
257 const Translation3d& next,
258 units::second_t dt,
259 units::meters_per_second_t maxVelocity) {
260 if (maxVelocity < 0_mps) {
262 "maxVelocity must be a non-negative number, got {}!", maxVelocity);
263 return next;
264 }
265 Translation3d diff = next - current;
266 units::meter_t dist = diff.Norm();
267 if (dist < 1e-9_m) {
268 return next;
269 }
270 if (dist > maxVelocity * dt) {
271 // Move maximum allowed amount in direction of the difference
272 return current + diff * (maxVelocity * dt / dist);
273 }
274 return next;
275}
276
277} // namespace frc
#define WPILIB_DLLEXPORT
Definition SymbolExports.h:36
Represents a translation in 2D space.
Definition Translation2d.h:29
constexpr units::meter_t Norm() const
Returns the norm, or distance from the origin to the translation.
Definition Translation2d.h:106
Represents a translation in 3D space.
Definition Translation3d.h:26
constexpr units::meter_t Norm() const
Returns the norm, or distance from the origin to the translation.
Definition Translation3d.h:130
static void ReportError(const S &format, Args &&... args)
Definition MathShared.h:62
constexpr UnitType abs(const UnitType x) noexcept
Compute absolute value.
Definition math.h:726
@ value
the parser finished reading a JSON value
Definition CAN.h:11
constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude=T{1.0})
Returns 0.0 if the given value is within the specified range around zero.
Definition MathUtil.h:38
constexpr std::signed_integral auto FloorMod(std::signed_integral auto x, std::signed_integral auto y)
Returns the floor modulus of the int arguments.
Definition MathUtil.h:212
WPILIB_DLLEXPORT constexpr units::radian_t AngleModulus(units::radian_t angle)
Wraps an angle to the range -π to π radians (-180 to 180 degrees).
Definition MathUtil.h:173
constexpr bool IsNear(T expected, T actual, T tolerance)
Checks if the given value matches an expected value within a certain tolerance.
Definition MathUtil.h:127
constexpr std::signed_integral auto FloorDiv(std::signed_integral auto x, std::signed_integral auto y)
Returns the largest (closest to positive infinity) int value that is less than or equal to the algebr...
Definition MathUtil.h:190
constexpr Translation2d SlewRateLimit(const Translation2d &current, const Translation2d &next, units::second_t dt, units::meters_per_second_t maxVelocity)
Limits translation velocity.
Definition MathUtil.h:226
constexpr T InputModulus(T input, T minimumInput, T maximumInput)
Returns modulus of input.
Definition MathUtil.h:101
constexpr T abs(const T x) noexcept
Compile-time absolute value function.
Definition abs.hpp:40
constexpr bool is_unit_t_v
Definition base.h:1870