MathUtil: C++20/23 modernisations

I've also created DegreesToRadians/RadiansToDegrees utility functions since there was a lot of repeated code
This commit is contained in:
Joshua Vandaële
2026-06-28 08:33:40 +02:00
parent 774200dcf4
commit 6dcb0817a2
23 changed files with 136 additions and 68 deletions

View File

@@ -8,15 +8,18 @@
#include <cmath>
#include <concepts>
#include <limits>
#include <numbers>
#include <type_traits>
#include <utility>
#include "Common/Assert.h"
#include "Common/CommonTypes.h"
namespace MathUtil
{
constexpr double TAU = 6.2831853071795865;
constexpr double PI = TAU / 2;
template <std::floating_point T>
constexpr T TAU_v = std::numbers::pi_v<T> * 2;
constexpr double TAU = TAU_v<double>;
constexpr double GRAVITY_ACCELERATION = 9.80665;
template <typename T>
@@ -25,12 +28,6 @@ constexpr auto Sign(const T& val) -> decltype((T{} < val) - (val < T{}))
return (T{} < val) - (val < T{});
}
template <typename T, typename F>
constexpr auto Lerp(const T& x, const T& y, const F& a) -> decltype(x + (y - x) * a)
{
return x + (y - x) * a;
}
// Casts the specified value to a Dest. The value will be clamped to fit in the destination type.
// Warning: The result of SaturatingCast(NaN) is undefined.
template <std::integral Dest, typename T>
@@ -56,23 +53,21 @@ constexpr Dest SaturatingCast(T value)
return static_cast<Dest>(value);
}
template <typename T>
template <std::integral T>
constexpr bool IsPow2(T imm)
{
return imm > 0 && (imm & (imm - 1)) == 0;
if constexpr (std::is_signed_v<T>)
{
if (imm <= 0)
return false;
}
return std::has_single_bit(static_cast<std::make_unsigned_t<T>>(imm));
}
constexpr u32 NextPowerOf2(u32 value)
{
--value;
value |= value >> 1;
value |= value >> 2;
value |= value >> 4;
value |= value >> 8;
value |= value >> 16;
++value;
return value;
return std::bit_ceil(value);
}
template <class T>
@@ -173,6 +168,20 @@ private:
// Rounds down. 0 -> undefined
constexpr int IntLog2(u64 val)
{
return 63 - std::countl_zero(val);
DEBUG_ASSERT(val != 0);
return std::bit_width(val) - 1;
}
template <std::floating_point T>
constexpr T DegreesToRadians(T degrees)
{
return degrees * std::numbers::pi_v<T> / T(180);
}
template <std::floating_point T>
constexpr T RadiansToDegrees(T radians)
{
return radians / std::numbers::pi_v<T> * T(180);
}
} // namespace MathUtil

View File

@@ -4,8 +4,7 @@
#include "Common/Matrix.h"
#include <cmath>
#include "Common/MathUtil.h"
#include <numbers>
namespace
{
@@ -136,7 +135,7 @@ Vec3 FromQuaternionToEuler(const Quaternion& q)
const float sinp = 2 * (qw * qy - qz * qx);
if (std::abs(sinp) >= 1)
result.y = std::copysign(MathUtil::PI / 2, sinp); // use 90 degrees if out of range
result.y = std::copysign(std::numbers::pi / 2, sinp); // use 90 degrees if out of range
else
result.y = std::asin(sinp);

View File

@@ -37,6 +37,10 @@ union TVec3
constexpr T LengthSquared() const { return Dot(*this); }
T Length() const { return std::sqrt(LengthSquared()); }
TVec3 Normalized() const { return *this / Length(); }
static constexpr TVec3<T> Lerp(const TVec3<T>& a, const TVec3<T>& b, const TVec3<T>& t)
{
return {std::lerp(a.x, b.x, t.x), std::lerp(a.y, b.y, t.y), std::lerp(a.z, b.z, t.z)};
}
constexpr TVec3& operator+=(const TVec3& rhs)
{
@@ -163,6 +167,16 @@ union TVec4
return x == other.x && y == other.y && z == other.z && w == other.w;
}
static constexpr TVec4<T> Lerp(const TVec4<T>& a, const TVec4<T>& b, const TVec4<T>& t)
{
return {
std::lerp(a.x, b.x, t.x),
std::lerp(a.y, b.y, t.y),
std::lerp(a.z, b.z, t.z),
std::lerp(a.w, b.w, t.w),
};
}
constexpr T Dot(const TVec4& other) const
{
return x * other.x + y * other.y + z * other.z + w * other.w;

View File

@@ -112,7 +112,7 @@ public:
// Jordan: I calculate the FOV at 42 degrees horizontally and having a 4:3 aspect ratio.
// This is 31.5 degrees vertically.
static constexpr float CAMERA_AR = 4.f / 3;
static constexpr float CAMERA_FOV_X = 42 * float(MathUtil::TAU) / 360;
static constexpr float CAMERA_FOV_X = MathUtil::DegreesToRadians(42.0f);
static constexpr float CAMERA_FOV_Y = CAMERA_FOV_X / CAMERA_AR;
enum : u8

View File

@@ -5,9 +5,11 @@
#include <algorithm>
#include <cmath>
#include <numbers>
#include <optional>
#include "Common/MathUtil.h"
#include "Common/Matrix.h"
#include "Core/Config/SYSCONFSettings.h"
#include "Core/HW/WiimoteEmu/WiimoteEmu.h"
#include "InputCommon/ControllerEmu/ControlGroup/Cursor.h"
@@ -112,8 +114,8 @@ void EmulateTilt(RotationalState* state, ControllerEmu::Tilt* const tilt_group,
const auto target = tilt_group->GetState();
// 180 degrees is currently the max tilt value.
const ControlState roll = target.x * MathUtil::PI;
const ControlState pitch = target.y * MathUtil::PI;
const ControlState roll = target.x * std::numbers::pi;
const ControlState pitch = target.y * std::numbers::pi;
const auto target_angle = Common::Vec3(pitch, -roll, 0);
@@ -121,7 +123,7 @@ void EmulateTilt(RotationalState* state, ControllerEmu::Tilt* const tilt_group,
for (std::size_t i = 0; i != target_angle.data.size(); ++i)
{
auto& angle = state->angle.data[i];
if (std::abs(angle - target_angle.data[i]) > float(MathUtil::PI))
if (std::abs(angle - target_angle.data[i]) > std::numbers::pi_v<float>)
angle -= std::copysign(MathUtil::TAU, angle);
}
@@ -145,9 +147,9 @@ void EmulateSwing(MotionState* state, ControllerEmu::Force* swing_group, float t
const auto xz_target_dist = Common::Vec2{target_position.x, target_position.z}.Length();
const auto y_target_dist = std::abs(target_position.y);
const auto target_dist = Common::Vec3{xz_target_dist, y_target_dist, xz_target_dist};
const auto speed = MathUtil::Lerp(Common::Vec3{1, 1, 1} * float(swing_group->GetReturnSpeed()),
Common::Vec3{1, 1, 1} * float(swing_group->GetSpeed()),
target_dist / max_distance);
const auto speed = Common::Vec3::Lerp(
Common::Vec3{1, 1, 1} * float(swing_group->GetReturnSpeed()),
Common::Vec3{1, 1, 1} * float(swing_group->GetSpeed()), target_dist / max_distance);
// Convert our m/s "speed" to the jerk required to reach this speed when traveling 1 meter.
const auto max_jerk = speed * speed * speed * 4;
@@ -265,7 +267,7 @@ void EmulatePoint(MotionState* state, ControllerEmu::Cursor* ir_group,
// Higher values will be more responsive but increase rate of M+ "desync".
// I'd rather not expose this value in the UI if not needed.
// At this value, sync is very good and responsiveness still appears instant.
constexpr auto MAX_ACCEL = float(MathUtil::TAU * 8);
constexpr auto MAX_ACCEL = MathUtil::TAU_v<float> * 8;
ApproachAngleWithAccel(state, target_angle, MAX_ACCEL, time_elapsed);
}

View File

@@ -62,7 +62,7 @@ Common::Vec3 MotionPlus::DataFormat::Data::GetAngularVelocity(const CalibrationB
const auto sign_fix = Common::Vec3(-1, +1, -1);
// Adjust deg/s to rad/s.
constexpr auto scalar = float(MathUtil::TAU / 360);
constexpr auto scalar = MathUtil::DegreesToRadians(1.0f);
return gyro.GetNormalizedValue(calibration.value) * sign_fix * Common::Vec3(calibration.degrees) *
scalar;

View File

@@ -131,7 +131,8 @@ public:
static constexpr u16 VALUE_SCALE =
(CALIBRATION_SCALE_OFFSET >> (CALIBRATION_BITS - BITS_OF_PRECISION));
static constexpr float VALUE_SCALE_DEGREES = VALUE_SCALE / float(MathUtil::TAU) * 360;
static constexpr float VALUE_SCALE_DEGREES =
MathUtil::RadiansToDegrees(static_cast<float>(VALUE_SCALE));
static constexpr float SLOW_SCALE = VALUE_SCALE_DEGREES / CALIBRATION_SLOW_SCALE_DEGREES;
static constexpr float FAST_SCALE = VALUE_SCALE_DEGREES / CALIBRATION_FAST_SCALE_DEGREES;

View File

@@ -231,8 +231,8 @@ Wiimote::Wiimote(const unsigned int index) : m_index(index), m_bt_device_index(i
groups.emplace_back(m_swing = new ControllerEmu::Force(_trans("Swing")));
groups.emplace_back(m_imu_ir = new ControllerEmu::IMUCursor("IMUIR", _trans("Point")));
const auto fov_default =
Common::DVec2(CameraLogic::CAMERA_FOV_X, CameraLogic::CAMERA_FOV_Y) / MathUtil::TAU * 360;
const auto fov_default = Common::DVec2(MathUtil::RadiansToDegrees(CameraLogic::CAMERA_FOV_X),
MathUtil::RadiansToDegrees(CameraLogic::CAMERA_FOV_Y));
m_imu_ir->AddSetting(&m_fov_x_setting,
// i18n: FOV stands for "Field of view".
{_trans("Horizontal FOV"),
@@ -509,8 +509,8 @@ void Wiimote::BuildDesiredWiimoteState(DesiredWiimoteState* target_state,
{
target_state->camera_points = CameraLogic::GetCameraPoints(
GetTotalTransformation(),
Common::Vec2(m_fov_x_setting.GetValue(), m_fov_y_setting.GetValue()) / 360 *
float(MathUtil::TAU));
Common::Vec2(MathUtil::DegreesToRadians(m_fov_x_setting.GetValue()),
MathUtil::DegreesToRadians(m_fov_y_setting.GetValue())));
}
else
{

View File

@@ -275,7 +275,7 @@ void DrawVirtualNotches(QPainter& p, ControllerEmu::ReshapableInput& stick, QCol
template <typename F>
void GenerateFibonacciSphere(int point_count, F&& callback)
{
const float golden_angle = MathUtil::PI * (3.f - std::sqrt(5.f));
const float golden_angle = std::numbers::pi * (3.f - std::sqrt(5.f));
for (int i = 0; i != point_count; ++i)
{
@@ -462,7 +462,8 @@ QColor TiltIndicator::GetGateBrushColor() const
void TiltIndicator::Draw()
{
auto adj_coord = Common::DVec2{-m_motion_state.angle.y, m_motion_state.angle.x} / MathUtil::PI;
auto adj_coord =
Common::DVec2{-m_motion_state.angle.y, m_motion_state.angle.x} / std::numbers::pi;
// Angle values after dividing by pi.
constexpr auto norm_180_deg = 1;

View File

@@ -99,7 +99,7 @@ OctagonAnalogStick::OctagonAnalogStick(const char* name_, const char* ui_name_,
ControlState OctagonAnalogStick::GetVirtualNotchSize() const
{
return m_virtual_notch_setting.GetValue() * MathUtil::TAU / 360;
return MathUtil::DegreesToRadians(m_virtual_notch_setting.GetValue());
}
ControlState OctagonAnalogStick::GetGateRadiusAtAngle(double ang) const

View File

@@ -167,12 +167,12 @@ Cursor::StateData Cursor::UpdateState(Cursor::ReshapeData input)
ControlState Cursor::GetTotalYaw() const
{
return m_yaw_setting.GetValue() * MathUtil::TAU / 360;
return MathUtil::DegreesToRadians(m_yaw_setting.GetValue());
}
ControlState Cursor::GetTotalPitch() const
{
return m_pitch_setting.GetValue() * MathUtil::TAU / 360;
return MathUtil::DegreesToRadians(m_pitch_setting.GetValue());
}
ControlState Cursor::GetVerticalOffset() const

View File

@@ -106,7 +106,7 @@ ControlState Force::GetReturnSpeed() const
ControlState Force::GetTwistAngle() const
{
return m_angle_setting.GetValue() * MathUtil::TAU / 360;
return MathUtil::DegreesToRadians(m_angle_setting.GetValue());
}
ControlState Force::GetMaxDistance() const

View File

@@ -49,7 +49,7 @@ IMUCursor::IMUCursor(std::string name_, std::string ui_name_)
ControlState IMUCursor::GetTotalYaw() const
{
return m_yaw_setting.GetValue() * MathUtil::TAU / 360;
return MathUtil::DegreesToRadians(m_yaw_setting.GetValue());
}
ControlState IMUCursor::GetAccelWeight() const

View File

@@ -163,7 +163,7 @@ std::optional<IMUGyroscope::StateData> IMUGyroscope::GetState(bool update)
ControlState IMUGyroscope::GetDeadzone() const
{
return m_deadzone_setting.GetValue() / 360 * MathUtil::TAU;
return MathUtil::DegreesToRadians(m_deadzone_setting.GetValue());
}
bool IMUGyroscope::IsCalibrating() const

View File

@@ -69,7 +69,7 @@ ControlState Tilt::GetDefaultInputRadiusAtAngle(double ang) const
ControlState Tilt::GetMaxRotationalVelocity() const
{
return m_max_rotational_velocity.GetValue() * MathUtil::TAU;
return m_max_rotational_velocity.GetValue() * MathUtil::TAU_v<ControlState>;
}
Control* Tilt::GetModifierInput() const

View File

@@ -5,6 +5,7 @@
#include <algorithm>
#include <cmath>
#include <numbers>
#include <ranges>
#include <fmt/format.h>
@@ -54,7 +55,7 @@ double GetNearestNotch(double angle, double virtual_notch_angle)
constexpr auto rounding = MathUtil::TAU / sides;
const auto closest_notch = std::round(angle / rounding) * rounding;
const auto angle_diff =
std::fmod(angle - closest_notch + MathUtil::PI, MathUtil::TAU) - MathUtil::PI;
std::fmod(angle - closest_notch + std::numbers::pi, MathUtil::TAU) - std::numbers::pi;
return std::abs(angle_diff) < virtual_notch_angle / 2 ? closest_notch : angle;
}
@@ -80,12 +81,12 @@ OctagonStickGate::OctagonStickGate(ControlState radius) : m_radius(radius)
ControlState OctagonStickGate::GetRadiusAtAngle(double angle) const
{
constexpr int sides = 8;
constexpr double sum_int_angles = (sides - 2) * MathUtil::PI;
constexpr double sum_int_angles = (sides - 2) * std::numbers::pi;
constexpr double half_int_angle = sum_int_angles / sides / 2;
angle = std::fmod(angle, MathUtil::TAU / sides);
// Solve ASA triangle using The Law of Sines:
return m_radius / std::sin(MathUtil::PI - angle - half_int_angle) * std::sin(half_int_angle);
return m_radius / std::sin(std::numbers::pi - angle - half_int_angle) * std::sin(half_int_angle);
}
std::optional<u32> OctagonStickGate::GetIdealCalibrationSampleCount() const
@@ -306,7 +307,7 @@ ReshapableInput::ReshapeData ReshapableInput::Reshape(ControlState x, ControlSta
// (which depends on the signs of x and y) does not matter here as dist is zero
// TODO: make the AtAngle functions work with negative angles:
ControlState angle = std::atan2(y, x) + MathUtil::TAU;
ControlState angle = std::atan2(y, x) + MathUtil::TAU_v<ControlState>;
const ControlState input_max_dist = GetInputRadiusAtAngle(angle);
ControlState gate_max_dist = GetGateRadiusAtAngle(angle);

View File

@@ -574,7 +574,7 @@ Device::Device(std::string name, int index, std::string server_address, u16 serv
AddInput(new AccelerometerInput("Accel Backward", m_pad_data.accelerometer_z_g, -accel_scale));
// Convert degrees per second to radians per second
constexpr auto gyro_scale = 360.0 / MathUtil::TAU;
constexpr auto gyro_scale = MathUtil::RadiansToDegrees(1.0);
AddInput(new GyroInput("Gyro Pitch Up", m_pad_data.gyro_pitch_deg_s, gyro_scale));
AddInput(new GyroInput("Gyro Pitch Down", m_pad_data.gyro_pitch_deg_s, -gyro_scale));

View File

@@ -189,7 +189,7 @@ bool ReshapableInputMapper::Update()
float ReshapableInputMapper::GetCurrentAngle() const
{
constexpr auto quarter_circle = float(MathUtil::TAU) * 0.25f;
constexpr auto quarter_circle = MathUtil::TAU_v<float> * 0.25f;
return quarter_circle - (float(m_input_detector.GetResults().size()) * quarter_circle);
}

View File

@@ -257,7 +257,7 @@ Device::Device(hid_device* device) : m_device{device}
// 16.384 (?) LSBs = 1 deg / s
// final output in rads / s
constexpr auto gyro_scale = 16.384 * 360.0 / MathUtil::TAU;
constexpr auto gyro_scale = MathUtil::RadiansToDegrees(16.384);
AddInput(new MotionInput("Gyro Pitch Up", m_latest_input.gyro_pitch, gyro_scale));
AddInput(new MotionInput("Gyro Pitch Down", m_latest_input.gyro_pitch, -gyro_scale));
AddInput(new MotionInput("Gyro Roll Left", m_latest_input.gyro_roll, -gyro_scale));

View File

@@ -3,6 +3,8 @@
#include "InputCommon/ControllerInterface/Wiimote/WiimoteController.h"
#include <numbers>
#include "Common/BitUtils.h"
#include "Common/Logging/Log.h"
#include "Common/MathUtil.h"
@@ -1258,7 +1260,7 @@ void Device::UpdateOrientation()
m_rotation_inputs =
Common::Vec3{WiimoteEmu::GetPitch(m_orientation), WiimoteEmu::GetRoll(m_orientation),
WiimoteEmu::GetYaw(m_orientation)} /
float(MathUtil::PI);
std::numbers::pi_v<float>;
}
void Device::IRState::ProcessData(const DataReportManipulator& manipulator)

View File

@@ -538,7 +538,7 @@ bool evdevDevice::AddNode(std::string devnode, int fd, libevdev* dev)
// evdev resolution is specified in "g"s and deg/s.
// Convert these to m/s/s and rad/s.
constexpr ControlState accel_scale = MathUtil::GRAVITY_ACCELERATION;
constexpr ControlState gyro_scale = MathUtil::TAU / 360;
constexpr ControlState gyro_scale = MathUtil::DegreesToRadians(ControlState{1});
add_motion_inputs(ABS_X, accel_scale);
add_motion_inputs(ABS_RX, gyro_scale);

View File

@@ -40,35 +40,33 @@ struct OutputVertexData
void Lerp(float t, const OutputVertexData* a, const OutputVertexData* b)
{
mvPosition = Common::Vec3::Lerp(a->mvPosition, b->mvPosition, Common::Vec3{1, 1, 1} * t);
#define LINTERP(T, OUT, IN) (OUT) + ((IN - OUT) * T)
#define LINTERP_INT(T, OUT, IN) (OUT) + (((IN - OUT) * T) >> 8)
mvPosition = LINTERP(t, a->mvPosition, b->mvPosition);
// We can't use Common::Vec4::Lerp here. If the game uses an oversized depth range, we hit an
// edge case which needs to be handled with this lerp formula. This is know to affect Samurai
// Warrior 3, which runs under the sw3-dt FifoCI test.
projectedPosition.x = LINTERP(t, a->projectedPosition.x, b->projectedPosition.x);
projectedPosition.y = LINTERP(t, a->projectedPosition.y, b->projectedPosition.y);
projectedPosition.z = LINTERP(t, a->projectedPosition.z, b->projectedPosition.z);
projectedPosition.w = LINTERP(t, a->projectedPosition.w, b->projectedPosition.w);
#undef LINTERP
for (std::size_t i = 0; i < normal.size(); ++i)
{
normal[i] = LINTERP(t, a->normal[i], b->normal[i]);
normal[i] = Common::Vec3::Lerp(a->normal[i], b->normal[i], Common::Vec3{1, 1, 1} * t);
}
const u16 t_int = static_cast<u16>(t * 256);
for (std::size_t i = 0; i < color[0].size(); ++i)
{
color[0][i] = LINTERP_INT(t_int, a->color[0][i], b->color[0][i]);
color[1][i] = LINTERP_INT(t_int, a->color[1][i], b->color[1][i]);
color[0][i] = static_cast<u8>(std::lerp(a->color[0][i], b->color[0][i], t));
color[1][i] = static_cast<u8>(std::lerp(a->color[1][i], b->color[1][i], t));
}
for (std::size_t i = 0; i < texCoords.size(); ++i)
{
texCoords[i] = LINTERP(t, a->texCoords[i], b->texCoords[i]);
texCoords[i] =
Common::Vec3::Lerp(a->texCoords[i], b->texCoords[i], Common::Vec3{1, 1, 1} * t);
}
#undef LINTERP
#undef LINTERP_INT
}
};

View File

@@ -5,6 +5,32 @@
#include "Common/MathUtil.h"
TEST(MathUtil, DegreesToRadians)
{
EXPECT_EQ(0.0, MathUtil::DegreesToRadians(0.0));
EXPECT_EQ(std::numbers::pi_v<double>, MathUtil::DegreesToRadians(180.0));
EXPECT_EQ(std::numbers::pi_v<double> / 2, MathUtil::DegreesToRadians(90.0));
EXPECT_EQ(std::numbers::pi_v<double> / 4, MathUtil::DegreesToRadians(45.0));
EXPECT_EQ(2 * std::numbers::pi_v<double>, MathUtil::DegreesToRadians(360.0));
// Negative angles
EXPECT_EQ(-std::numbers::pi_v<double> / 2, MathUtil::DegreesToRadians(-90.0));
EXPECT_EQ(-std::numbers::pi_v<double>, MathUtil::DegreesToRadians(-180.0));
}
TEST(MathUtil, RadiansToDegrees)
{
EXPECT_EQ(0.0, MathUtil::RadiansToDegrees(0.0));
EXPECT_EQ(180.0, MathUtil::RadiansToDegrees(std::numbers::pi_v<double>));
EXPECT_EQ(90.0, MathUtil::RadiansToDegrees(std::numbers::pi_v<double> / 2));
EXPECT_EQ(45.0, MathUtil::RadiansToDegrees(std::numbers::pi_v<double> / 4));
EXPECT_EQ(360.0, MathUtil::RadiansToDegrees(2 * std::numbers::pi_v<double>));
// Negative angles
EXPECT_EQ(-90.0, MathUtil::RadiansToDegrees(-std::numbers::pi_v<double> / 2));
EXPECT_EQ(-180.0, MathUtil::RadiansToDegrees(-std::numbers::pi_v<double>));
}
TEST(MathUtil, IntLog2)
{
EXPECT_EQ(0, MathUtil::IntLog2(1));
@@ -18,8 +44,23 @@ TEST(MathUtil, IntLog2)
EXPECT_EQ(63, MathUtil::IntLog2(0xFFFFFFFFFFFFFFFFull));
}
TEST(MathUtil, IsPow2)
{
EXPECT_FALSE(MathUtil::IsPow2(std::numeric_limits<int>::min()));
EXPECT_FALSE(MathUtil::IsPow2(-2));
EXPECT_FALSE(MathUtil::IsPow2(-1));
EXPECT_FALSE(MathUtil::IsPow2(0));
EXPECT_TRUE(MathUtil::IsPow2(1));
EXPECT_TRUE(MathUtil::IsPow2(2));
EXPECT_FALSE(MathUtil::IsPow2(3));
EXPECT_TRUE(MathUtil::IsPow2(4));
EXPECT_FALSE(MathUtil::IsPow2(5));
EXPECT_FALSE(MathUtil::IsPow2(std::numeric_limits<int>::max()));
}
TEST(MathUtil, NextPowerOf2)
{
EXPECT_EQ(1U, MathUtil::NextPowerOf2(0));
EXPECT_EQ(4U, MathUtil::NextPowerOf2(3));
EXPECT_EQ(4U, MathUtil::NextPowerOf2(4));
EXPECT_EQ(8U, MathUtil::NextPowerOf2(6));