22 #ifndef PLAYRHO_COMMON_MATH_HPP
23 #define PLAYRHO_COMMON_MATH_HPP
100 template <
typename T>
103 return get<2>(value);
109 template <
typename T>
110 PLAYRHO_CONSTEXPR inline std::enable_if_t<std::is_signed<T>::value, std::make_unsigned_t<T>>
113 return static_cast<std::make_unsigned_t<T>
>(arg);
117 template <
typename T, LoValueCheck lo, HiValueCheck hi>
131 template <
typename T,
typename U>
135 return (a1 + (target - s1) * (a2 - a1) / (s2 - s1));
140 template <
typename T>
143 return (a1 + a2) / 2;
148 template <
typename T>
151 static_assert(std::is_integral<T>::value,
"Integral type required.");
169 template <
typename T,
typename = std::enable_if_t<
170 IsIterable<T>::value && IsAddable<decltype(*begin(std::declval<T>()))>::value
174 using value_type = decltype(*begin(std::declval<T>()));
179 assert(zero *
Real{2} == zero);
182 const auto sum = std::accumulate(begin(span), end(span), zero);
183 const auto count = std::max(
size(span), std::size_t{1});
184 return sum /
static_cast<Real>(count);
188 template <
typename T>
189 inline std::enable_if_t<IsArithmetic<T>::value, T>
RoundOff(T value,
unsigned precision = 100000)
191 const auto factor =
static_cast<T
>(precision);
192 return round(value * factor) / factor;
203 template <
typename T, std::
size_t N>
207 for (
auto i = decltype(N){0}; i < N; ++i)
209 result[i] =
abs(v[i]);
224 template <
typename T>
226 std::enable_if_t<std::is_arithmetic<T>::value,
bool>
AlmostZero(T value)
228 return abs(value) < std::numeric_limits<T>::min();
232 template <
typename T>
234 std::enable_if_t<std::is_floating_point<T>::value,
bool>
AlmostEqual(T x, T y,
int ulp = 2)
242 return (
abs(x - y) < (std::numeric_limits<T>::epsilon() *
abs(x + y) * ulp)) ||
AlmostZero(x - y);
248 template <
typename T>
252 return static_cast<T
>(
fmod(dividend, divisor));
258 template <
typename T>
261 const auto quotient = dividend / divisor;
262 const auto integer =
static_cast<T
>(
trunc(quotient));
263 const auto remainder = quotient - integer;
264 return remainder * divisor;
274 #if defined(NORMALIZE_ANGLE_VIA_FMOD)
277 angleInRadians =
ModuloViaFmod(angleInRadians, oneRotationInRadians);
281 angleInRadians =
ModuloViaTrunc(angleInRadians, oneRotationInRadians);
283 if (angleInRadians >
Pi)
286 angleInRadians -=
Pi * 2;
288 else if (angleInRadians < -
Pi)
291 angleInRadians +=
Pi * 2;
293 return angleInRadians *
Radian;
307 template <
typename T>
311 using VT =
typename T::value_type;
312 using OT = decltype(VT{} * VT{});
314 for (
auto&& e: value)
323 template <
typename T>
353 template <
typename T1,
typename T2>
356 static_assert(std::tuple_size<T1>::value == std::tuple_size<T2>::value,
357 "Dot only for same tuple-like sized types");
358 using VT1 =
typename T1::value_type;
359 using VT2 =
typename T2::value_type;
360 using OT = decltype(VT1{} * VT2{});
362 const auto numElements =
size(a);
363 for (
auto i = decltype(numElements){0}; i < numElements; ++i)
365 result += a[i] * b[i];
397 template <
class T1,
class T2, std::enable_if_t<
398 std::tuple_size<T1>::value == 2 && std::tuple_size<T2>::value == 2,
int> = 0>
420 return minuend - subtrahend;
429 template <
class T1,
class T2, std::enable_if_t<
430 std::tuple_size<T1>::value == 3 && std::tuple_size<T2>::value == 3,
int> = 0>
441 return Vector<OT, 3>{
450 template <
typename T,
typename U>
454 const auto inverseCp =
Real{1} / cp;
455 using OutType = decltype((U{} * T{}) / cp);
458 (
get<1>(mat)[1] * b[0] -
get<1>(mat)[0] * b[1]) * inverseCp,
459 (
get<0>(mat)[0] * b[1] -
get<0>(mat)[1] * b[0]) * inverseCp
464 template <
class IN_TYPE>
468 using OutType = decltype(
get<0>(value)[0] / cp);
469 const auto inverseCp =
Real{1} / cp;
483 const auto det = (dp != 0)?
Real{1} / dp: dp;
487 return Vec3{x, y, z};
493 template <
typename T>
497 const auto det = (cp != 0)?
Real{1} / cp: cp;
508 auto det = (a * d) - (b * c);
530 const auto ex_y = det * (a13 * a23 - a12 * a33);
531 const auto ey_z = det * (a13 * a12 - a11 * a23);
532 const auto ex_z = det * (a12 * a23 - a13 * a22);
535 Vec3{det * (a22 * a33 - a23 * a23), ex_y, ex_z},
536 Vec3{ex_y, det * (a11 * a33 - a13 * a13), ey_z},
537 Vec3{ex_z, ey_z, det * (a11 * a22 - a12 * a12)}
550 return T{-
GetY(vector),
GetX(vector)};
562 return T{
GetY(vector), -
GetX(vector)};
569 template <std::
size_t M,
typename T1, std::
size_t N,
typename T2>
596 return Mat22{c1, c2};
628 const auto invLength =
Real{1} / length;
629 vector[0] *= invLength;
630 vector[1] *= invLength;
642 template <
typename T>
645 assert(value < count);
646 return (value + 1) % count;
650 template <
typename T>
653 assert(value < count);
654 return (value? value: count) - 1;
668 return (a1 > a2)? 360_deg - (a1 - a2): a2 - a1;
709 template <
class T, LoValueCheck lo, HiValueCheck hi>
712 return Vector2<T>{u.GetX() * s, u.GetY() * T{s}};
719 return Vector2<T>{u.GetX() * s, u.GetY() * s};
723 template <
class T, LoValueCheck lo, HiValueCheck hi>
726 return Vector2<T>{u.GetX() * s, u.GetY() * T{s}};
733 return Vector2<T>{u.GetX() * s, u.GetY() * s};
739 const auto inverseS =
Real{1} / s;
751 const auto newX = (
GetX(angle) *
GetX(vector)) - (
GetY(angle) *
GetY(vector));
752 const auto newY = (
GetY(angle) *
GetX(vector)) + (
GetX(angle) *
GetY(vector));
766 const auto newX = (
GetX(angle) *
GetX(vector)) + (
GetY(angle) *
GetY(vector));
767 const auto newY = (
GetX(angle) *
GetY(vector)) - (
GetY(angle) *
GetX(vector));
799 const auto d = sweep.pos0.angular - pos0a;
800 sweep.pos0.angular = pos0a;
801 sweep.pos1.angular -= d;
818 return Rotate(v, xfm.q) + xfm.p;
833 const auto v2 = v - T.p;
850 const auto dp = B.
p - A.p;
856 const Length2 localCtr) noexcept
905 const Velocity velB,
const Length2 relB) noexcept;
912 const auto angVelSquared =
Square(velocity.angular);
913 return (angVelSquared <=
Square(angSleepTol)) && (linVelSquared <=
Square(linSleepTol));
921 constexpr
auto TupleSize = std::tuple_size<decltype(axis)>::value;
922 constexpr
auto NumRows = TupleSize;
923 constexpr
auto NumCols = TupleSize;
925 for (
auto row = decltype(NumRows){0}; row < NumRows; ++row)
927 for (
auto col = decltype(NumCols){0}; col < NumCols; ++col)
929 result[row][col] = ((row == col)?
Real{1}:
Real{0}) - axis[row] * axis[col] * 2;
938 #endif // PLAYRHO_COMMON_MATH_HPP