Math.hpp

Go to the documentation of this file.

/*
 * Original work Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
 * Modified work Copyright (c) 2017 Louis Langholtz https://github.com/louis-langholtz/PlayRho
 *
 * This software is provided 'as-is', without any express or implied
 * warranty. In no event will the authors be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 *    claim that you wrote the original software. If you use this software
 *    in a product, an acknowledgment in the product documentation would be
 *    appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 *    misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
#ifndef PLAYRHO_COMMON_MATH_HPP
#define PLAYRHO_COMMON_MATH_HPP
 
#include <PlayRho/Common/Settings.hpp>
#include <PlayRho/Common/BoundedValue.hpp>
#include <PlayRho/Common/Span.hpp>
#include <PlayRho/Common/UnitVec.hpp>
#include <PlayRho/Common/Vector2.hpp>
#include <PlayRho/Common/Vector3.hpp>
#include <PlayRho/Common/Position.hpp>
#include <PlayRho/Common/Velocity.hpp>
#include <PlayRho/Common/Acceleration.hpp>
#include <PlayRho/Common/Transformation.hpp>
#include <PlayRho/Common/Sweep.hpp>
#include <PlayRho/Common/Matrix.hpp>
#include <PlayRho/Common/FixedMath.hpp>
 
#include <cmath>
#include <vector>
#include <numeric>
 
namespace playrho {
 
// Import common standard mathematical functions into the playrho namespace...
using std::signbit;
using std::nextafter;
using std::trunc;
using std::fmod;
using std::isfinite;
using std::round;
using std::isnormal;
using std::isnan;
using std::hypot;
using std::cos;
using std::sin;
using std::atan2;
using std::sqrt;
using std::pow;
using std::abs;
 
// Other templates.
 
template <typename T>
PLAYRHO_CONSTEXPR inline auto& GetX(T& value)
{
    return get<0>(value);
}
 
template <typename T>
PLAYRHO_CONSTEXPR inline auto& GetY(T& value)
{
    return get<1>(value);
}
 
template <typename T>
PLAYRHO_CONSTEXPR inline auto& GetZ(T& value)
{
    return get<2>(value);
}
 
template <typename T>
PLAYRHO_CONSTEXPR inline auto GetX(const T& value)
{
    return get<0>(value);
}
 
template <typename T>
PLAYRHO_CONSTEXPR inline auto GetY(const T& value)
{
    return get<1>(value);
}
 
template <typename T>
PLAYRHO_CONSTEXPR inline auto GetZ(const T& value)
{
    return get<2>(value);
}
 
template <typename T>
PLAYRHO_CONSTEXPR inline std::enable_if_t<std::is_signed<T>::value, std::make_unsigned_t<T>>
MakeUnsigned(const T& arg) noexcept
{
    return static_cast<std::make_unsigned_t<T>>(arg);
}
 
template <typename T, LoValueCheck lo, HiValueCheck hi>
PLAYRHO_CONSTEXPR inline auto StripUnit(const BoundedValue<T, lo, hi>& v)
{
    return StripUnit(v.get());
}
 
 
template <typename T, typename U>
PLAYRHO_CONSTEXPR inline U Secant(T target, U a1, T s1, U a2, T s2) noexcept
{
    static_assert(IsArithmetic<T>::value && IsArithmetic<U>::value, "Arithmetic types required.");
    return (a1 + (target - s1) * (a2 - a1) / (s2 - s1));
}
 
template <typename T>
PLAYRHO_CONSTEXPR inline T Bisect(T a1, T a2) noexcept
{
    return (a1 + a2) / 2;
}
 
template <typename T>
PLAYRHO_CONSTEXPR inline bool IsOdd(T val) noexcept
{
    static_assert(std::is_integral<T>::value, "Integral type required.");
    return val % 2;
}
 
template<class TYPE>
PLAYRHO_CONSTEXPR inline auto Square(TYPE t) noexcept { return t * t; }
 
template<typename T>
inline auto Atan2(T y, T x)
{
    return Angle{static_cast<Real>(atan2(StripUnit(y), StripUnit(x))) * Radian};
}
 
template <typename T, typename = std::enable_if_t<
    IsIterable<T>::value && IsAddable<decltype(*begin(std::declval<T>()))>::value
    > >
inline auto Average(const T& span)
{
    using value_type = decltype(*begin(std::declval<T>()));
 
    // Relies on C++11 zero initialization to zero initialize value_type.
    // See: http://en.cppreference.com/w/cpp/language/zero_initialization
    PLAYRHO_CONSTEXPR const auto zero = value_type{};
    assert(zero * Real{2} == zero);
    
    // For C++17, switch from using std::accumulate to using std::reduce.
    const auto sum = std::accumulate(begin(span), end(span), zero);
    const auto count = std::max(size(span), std::size_t{1});
    return sum / static_cast<Real>(count);
}
 
template <typename T>
inline std::enable_if_t<IsArithmetic<T>::value, T> RoundOff(T value, unsigned precision = 100000)
{
    const auto factor = static_cast<T>(precision);
    return round(value * factor) / factor;
}
 
inline Vec2 RoundOff(Vec2 value, std::uint32_t precision = 100000)
{
    return Vec2{RoundOff(value[0], precision), RoundOff(value[1], precision)};
}
 
template <typename T, std::size_t N>
PLAYRHO_CONSTEXPR inline Vector<T, N> abs(const Vector<T, N>& v) noexcept
{
    auto result = Vector<T, N>{};
    for (auto i = decltype(N){0}; i < N; ++i)
    {
        result[i] = abs(v[i]);
    }
    return result;
}
 
inline d2::UnitVec abs(const d2::UnitVec& v) noexcept
{
    return v.Absolute();
}
 
template <typename T>
PLAYRHO_CONSTEXPR inline
std::enable_if_t<std::is_arithmetic<T>::value, bool> AlmostZero(T value)
{
    return abs(value) < std::numeric_limits<T>::min();
}
 
template <typename T>
PLAYRHO_CONSTEXPR inline
std::enable_if_t<std::is_floating_point<T>::value, bool> AlmostEqual(T x, T y, int ulp = 2)
{
    // From http://en.cppreference.com/w/cpp/types/numeric_limits/epsilon :
    //   "the machine epsilon has to be scaled to the magnitude of the values used
    //    and multiplied by the desired precision in ULPs (units in the last place)
    //    unless the result is subnormal".
    // Where "subnormal" means almost zero.
    //
    return (abs(x - y) < (std::numeric_limits<T>::epsilon() * abs(x + y) * ulp)) || AlmostZero(x - y);
}
 
template <typename T>
inline auto ModuloViaFmod(T dividend, T divisor) noexcept
{
    // Note: modulo via std::fmod appears slower than via std::trunc.
    return static_cast<T>(fmod(dividend, divisor));
}
 
template <typename T>
inline auto ModuloViaTrunc(T dividend, T divisor) noexcept
{
    const auto quotient = dividend / divisor;
    const auto integer = static_cast<T>(trunc(quotient));
    const auto remainder = quotient - integer;
    return remainder * divisor;
}
 
inline Angle GetNormalized(Angle value) noexcept
{
    PLAYRHO_CONSTEXPR const auto oneRotationInRadians = Real{2 * Pi};
    auto angleInRadians = Real{value / Radian};
#if defined(NORMALIZE_ANGLE_VIA_FMOD)
    // Note: std::fmod appears slower than std::trunc.
    //   See Benchmark ModuloViaFmod for data.
    angleInRadians = ModuloViaFmod(angleInRadians, oneRotationInRadians);
#else
    // Note: std::trunc appears more than twice as fast as std::fmod.
    //   See Benchmark ModuloViaTrunc for data.
    angleInRadians = ModuloViaTrunc(angleInRadians, oneRotationInRadians);
#endif
    if (angleInRadians > Pi)
    {
        // 190_deg becomes 190_deg - 360_deg = -170_deg
        angleInRadians -= Pi * 2;
    }
    else if (angleInRadians < -Pi)
    {
        // -200_deg becomes -200_deg + 360_deg = 100_deg
        angleInRadians += Pi * 2;
    }
    return angleInRadians * Radian;
}
 
template <class T>
inline Angle GetAngle(const Vector2<T> value)
{
    return Atan2(GetY(value), GetX(value));
}
 
template <typename T>
PLAYRHO_CONSTEXPR inline
auto GetMagnitudeSquared(T value) noexcept
{
    using VT = typename T::value_type;
    using OT = decltype(VT{} * VT{});
    auto result = OT{};
    for (auto&& e: value)
    {
        result += Square(e);
    }
    return result;
}
 
template <typename T>
inline auto GetMagnitude(T value)
{
    return sqrt(GetMagnitudeSquared(value));
}
 
template <typename T1, typename T2>
PLAYRHO_CONSTEXPR inline auto Dot(const T1 a, const T2 b) noexcept
{
    static_assert(std::tuple_size<T1>::value == std::tuple_size<T2>::value,
                  "Dot only for same tuple-like sized types");
    using VT1 = typename T1::value_type;
    using VT2 = typename T2::value_type;
    using OT = decltype(VT1{} * VT2{});
    auto result = OT{};
    const auto numElements = size(a);
    for (auto i = decltype(numElements){0}; i < numElements; ++i)
    {
        result += a[i] * b[i];
    }
    return result;
}
 
template <class T1, class T2, std::enable_if_t<
    std::tuple_size<T1>::value == 2 && std::tuple_size<T2>::value == 2, int> = 0>
PLAYRHO_CONSTEXPR inline auto Cross(T1 a, T2 b) noexcept
{
    assert(isfinite(StripUnit(get<0>(a))));
    assert(isfinite(StripUnit(get<1>(a))));
    assert(isfinite(StripUnit(get<0>(b))));
    assert(isfinite(StripUnit(get<1>(b))));
 
    // Both vectors of same direction...
    // If a = Vec2{1, 2} and b = Vec2{1, 2} then: a x b = 1 * 2 - 2 * 1 = 0.
    // If a = Vec2{1, 2} and b = Vec2{2, 4} then: a x b = 1 * 4 - 2 * 2 = 0.
    //
    // Vectors at +/- 90 degrees of each other...
    // If a = Vec2{1, 2} and b = Vec2{-2, 1} then: a x b = 1 * 1 - 2 * (-2) = 1 + 4 = 5.
    // If a = Vec2{1, 2} and b = Vec2{2, -1} then: a x b = 1 * (-1) - 2 * 2 = -1 - 4 = -5.
    //
    // Vectors between 0 and 180 degrees of each other excluding 90 degrees...
    // If a = Vec2{1, 2} and b = Vec2{-1, 2} then: a x b = 1 * 2 - 2 * (-1) = 2 + 2 = 4.
    const auto minuend = get<0>(a) * get<1>(b);
    const auto subtrahend = get<1>(a) * get<0>(b);
    assert(isfinite(StripUnit(minuend)));
    assert(isfinite(StripUnit(subtrahend)));
    return minuend - subtrahend;
}
 
template <class T1, class T2, std::enable_if_t<
    std::tuple_size<T1>::value == 3 && std::tuple_size<T2>::value == 3, int> = 0>
PLAYRHO_CONSTEXPR inline auto Cross(T1 a, T2 b) noexcept
{
    assert(isfinite(get<0>(a)));
    assert(isfinite(get<1>(a)));
    assert(isfinite(get<2>(a)));
    assert(isfinite(get<0>(b)));
    assert(isfinite(get<1>(b)));
    assert(isfinite(get<2>(b)));
 
    using OT = decltype(get<0>(a) * get<0>(b));
    return Vector<OT, 3>{
        GetY(a) * GetZ(b) - GetZ(a) * GetY(b),
        GetZ(a) * GetX(b) - GetX(a) * GetZ(b),
        GetX(a) * GetY(b) - GetY(a) * GetX(b)
    };
}
 
template <typename T, typename U>
PLAYRHO_CONSTEXPR inline auto Solve(const Matrix22<U> mat, const Vector2<T> b) noexcept
{
    const auto cp = Cross(get<0>(mat), get<1>(mat));
    const auto inverseCp = Real{1} / cp;
    using OutType = decltype((U{} * T{}) / cp);
    return (!AlmostZero(StripUnit(cp)))?
        Vector2<OutType>{
            (get<1>(mat)[1] * b[0] - get<1>(mat)[0] * b[1]) * inverseCp,
            (get<0>(mat)[0] * b[1] - get<0>(mat)[1] * b[0]) * inverseCp
        }: Vector2<OutType>{};
}
 
template <class IN_TYPE>
PLAYRHO_CONSTEXPR inline auto Invert(const Matrix22<IN_TYPE> value) noexcept
{
    const auto cp = Cross(get<0>(value), get<1>(value));
    using OutType = decltype(get<0>(value)[0] / cp);
    const auto inverseCp = Real{1} / cp;
    return (!AlmostZero(StripUnit(cp)))?
        Matrix22<OutType>{
            Vector2<OutType>{ get<1>(get<1>(value)) * inverseCp, -get<1>(get<0>(value)) * inverseCp},
            Vector2<OutType>{-get<0>(get<1>(value)) * inverseCp,  get<0>(get<0>(value)) * inverseCp}
        }:
        Matrix22<OutType>{};
}
 
PLAYRHO_CONSTEXPR inline Vec3 Solve33(const Mat33& mat, const Vec3 b) noexcept
{
    const auto dp = Dot(GetX(mat), Cross(GetY(mat), GetZ(mat)));
    const auto det = (dp != 0)? Real{1} / dp: dp;
    const auto x = det * Dot(b, Cross(GetY(mat), GetZ(mat)));
    const auto y = det * Dot(GetX(mat), Cross(b, GetZ(mat)));
    const auto z = det * Dot(GetX(mat), Cross(GetY(mat), b));
    return Vec3{x, y, z};
}
    
template <typename T>
PLAYRHO_CONSTEXPR inline T Solve22(const Mat33& mat, const T b) noexcept
{
    const auto cp = GetX(GetX(mat)) * GetY(GetY(mat)) - GetX(GetY(mat)) * GetY(GetX(mat));
    const auto det = (cp != 0)? Real{1} / cp: cp;
    const auto x = det * (GetY(GetY(mat)) * GetX(b) - GetX(GetY(mat)) * GetY(b));
    const auto y = det * (GetX(GetX(mat)) * GetY(b) - GetY(GetX(mat)) * GetX(b));
    return T{x, y};
}
 
PLAYRHO_CONSTEXPR inline Mat33 GetInverse22(const Mat33& value) noexcept
{
    const auto a = GetX(GetX(value)), b = GetX(GetY(value)), c = GetY(GetX(value)), d = GetY(GetY(value));
    auto det = (a * d) - (b * c);
    if (det != Real{0})
    {
        det = Real{1} / det;
    }
    return Mat33{Vec3{det * d, -det * c, Real{0}}, Vec3{-det * b, det * a, 0}, Vec3{0, 0, 0}};
}
    
PLAYRHO_CONSTEXPR inline Mat33 GetSymInverse33(const Mat33& value) noexcept
{
    auto det = Dot(GetX(value), Cross(GetY(value), GetZ(value)));
    if (det != Real{0})
    {
        det = Real{1} / det;
    }
    
    const auto a11 = GetX(GetX(value)), a12 = GetX(GetY(value)), a13 = GetX(GetZ(value));
    const auto a22 = GetY(GetY(value)), a23 = GetY(GetZ(value));
    const auto a33 = GetZ(GetZ(value));
    
    const auto ex_y = det * (a13 * a23 - a12 * a33);
    const auto ey_z = det * (a13 * a12 - a11 * a23);
    const auto ex_z = det * (a12 * a23 - a13 * a22);
    
    return Mat33{
        Vec3{det * (a22 * a33 - a23 * a23), ex_y, ex_z},
        Vec3{ex_y, det * (a11 * a33 - a13 * a13), ey_z},
        Vec3{ex_z, ey_z, det * (a11 * a22 - a12 * a12)}
    };
}
 
template <class T>
PLAYRHO_CONSTEXPR inline auto GetRevPerpendicular(const T vector) noexcept
{
    // See http://mathworld.wolfram.com/PerpendicularVector.html
    return T{-GetY(vector), GetX(vector)};
}
    
template <class T>
PLAYRHO_CONSTEXPR inline auto GetFwdPerpendicular(const T vector) noexcept
{
    // See http://mathworld.wolfram.com/PerpendicularVector.html
    return T{GetY(vector), -GetX(vector)};
}
 
template <std::size_t M, typename T1, std::size_t N, typename T2>
PLAYRHO_CONSTEXPR inline auto Transform(const Vector<T1, M> v, const Matrix<T2, M, N>& m) noexcept
{
    return m * v;
}
 
PLAYRHO_CONSTEXPR inline Vec2 Transform(const Vec2 v, const Mat33& A) noexcept
{
    return Vec2{
        get<0>(get<0>(A)) * v[0] + get<0>(get<1>(A)) * v[1],
        get<1>(get<0>(A)) * v[0] + get<1>(get<1>(A)) * v[1]
    };
}
 
PLAYRHO_CONSTEXPR inline Vec2 InverseTransform(const Vec2 v, const Mat22& A) noexcept
{
    return Vec2{Dot(v, GetX(A)), Dot(v, GetY(A))};
}
 
PLAYRHO_CONSTEXPR inline Mat22 MulT(const Mat22& A, const Mat22& B) noexcept
{
    const auto c1 = Vec2{Dot(GetX(A), GetX(B)), Dot(GetY(A), GetX(B))};
    const auto c2 = Vec2{Dot(GetX(A), GetY(B)), Dot(GetY(A), GetY(B))};
    return Mat22{c1, c2};
}
 
inline Mat22 abs(const Mat22& A)
{
    return Mat22{abs(GetX(A)), abs(GetY(A))};
}
 
inline std::uint64_t NextPowerOfTwo(std::uint64_t x)
{
    x |= (x >>  1u);
    x |= (x >>  2u);
    x |= (x >>  4u);
    x |= (x >>  8u);
    x |= (x >> 16u);
    x |= (x >> 32u);
    return x + 1;
}
 
inline Real Normalize(Vec2& vector)
{
    const auto length = GetMagnitude(vector);
    if (!AlmostZero(length))
    {
        const auto invLength = Real{1} / length;
        vector[0] *= invLength;
        vector[1] *= invLength;
        return length;
    }
    return 0;
}
 
Length2 ComputeCentroid(const Span<const Length2>& vertices);
 
template <typename T>
PLAYRHO_CONSTEXPR inline T GetModuloNext(T value, T count) noexcept
{
    assert(value < count);
    return (value + 1) % count;
}
 
template <typename T>
PLAYRHO_CONSTEXPR inline T GetModuloPrev(T value, T count) noexcept
{
    assert(value < count);
    return (value? value: count) - 1;
}
 
Angle GetDelta(Angle a1, Angle a2) noexcept;
 
PLAYRHO_CONSTEXPR inline Angle GetRevRotationalAngle(Angle a1, Angle a2) noexcept
{
    return (a1 > a2)? 360_deg - (a1 - a2): a2 - a1;
}
    
std::vector<Length2> GetCircleVertices(Length radius, unsigned slices,
                                        Angle start = 0_deg, Real turns = Real{1});
 
NonNegative<Area> GetAreaOfCircle(Length radius);
 
NonNegative<Area> GetAreaOfPolygon(Span<const Length2> vertices);
 
SecondMomentOfArea GetPolarMoment(Span<const Length2> vertices);
 
 
namespace d2 {
 
PLAYRHO_CONSTEXPR inline Vec2 GetVec2(const UnitVec value)
{
    return Vec2{get<0>(value), get<1>(value)};
}
 
inline Angle GetAngle(const UnitVec value)
{
    return Atan2(GetY(value), GetX(value));
}
 
template <class T, LoValueCheck lo, HiValueCheck hi>
PLAYRHO_CONSTEXPR inline Vector2<T> operator* (BoundedValue<T, lo, hi> s, UnitVec u) noexcept
{
    return Vector2<T>{u.GetX() * s, u.GetY() * T{s}};
}
 
template <class T>
PLAYRHO_CONSTEXPR inline Vector2<T> operator* (const T s, const UnitVec u) noexcept
{
    return Vector2<T>{u.GetX() * s, u.GetY() * s};
}
 
template <class T, LoValueCheck lo, HiValueCheck hi>
PLAYRHO_CONSTEXPR inline Vector2<T> operator* (UnitVec u, BoundedValue<T, lo, hi> s) noexcept
{
    return Vector2<T>{u.GetX() * s, u.GetY() * T{s}};
}
 
template <class T>
PLAYRHO_CONSTEXPR inline Vector2<T> operator* (const UnitVec u, const T s) noexcept
{
    return Vector2<T>{u.GetX() * s, u.GetY() * s};
}
 
PLAYRHO_CONSTEXPR inline Vec2 operator/ (const UnitVec u, const UnitVec::value_type s) noexcept
{
    const auto inverseS = Real{1} / s;
    return Vec2{GetX(u) * inverseS, GetY(u) * inverseS};
}
 
template <class T>
PLAYRHO_CONSTEXPR inline auto Rotate(const Vector2<T> vector, const UnitVec& angle) noexcept
{
    const auto newX = (GetX(angle) * GetX(vector)) - (GetY(angle) * GetY(vector));
    const auto newY = (GetY(angle) * GetX(vector)) + (GetX(angle) * GetY(vector));
    return Vector2<T>{newX, newY};
}
 
template <class T>
PLAYRHO_CONSTEXPR inline auto InverseRotate(const Vector2<T> vector, const UnitVec& angle) noexcept
{
    const auto newX = (GetX(angle) * GetX(vector)) + (GetY(angle) * GetY(vector));
    const auto newY = (GetX(angle) * GetY(vector)) - (GetY(angle) * GetX(vector));
    return Vector2<T>{newX, newY};
}
 
template <class T>
inline UnitVec GetUnitVector(Vector2<T> value, UnitVec fallback = UnitVec::GetDefaultFallback())
{
    return std::get<0>(UnitVec::Get(StripUnit(GetX(value)), StripUnit(GetY(value)), fallback));
}
 
inline Position GetNormalized(const Position& val) noexcept
{
    return Position{val.linear, playrho::GetNormalized(val.angular)};
}
 
inline Sweep GetNormalized(Sweep sweep) noexcept
{
    const auto pos0a = playrho::GetNormalized(sweep.pos0.angular);
    const auto d = sweep.pos0.angular - pos0a;
    sweep.pos0.angular = pos0a;
    sweep.pos1.angular -= d;
    return sweep;
}
 
PLAYRHO_CONSTEXPR inline Length2 Transform(const Length2 v, const Transformation xfm) noexcept
{
    return Rotate(v, xfm.q) + xfm.p;
}
 
PLAYRHO_CONSTEXPR inline Length2 InverseTransform(const Length2 v, const Transformation T) noexcept
{
    const auto v2 = v - T.p;
    return InverseRotate(v2, T.q);
}
 
PLAYRHO_CONSTEXPR inline Transformation Mul(const Transformation& A, const Transformation& B) noexcept
{
    return Transformation{A.p + Rotate(B.p, A.q), A.q.Rotate(B.q)};
}
 
PLAYRHO_CONSTEXPR inline Transformation MulT(const Transformation& A, const Transformation& B) noexcept
{
    const auto dp = B.p - A.p;
    return Transformation{InverseRotate(dp, A.q), B.q.Rotate(A.q.FlipY())};
}
 
PLAYRHO_CONSTEXPR inline Transformation GetTransformation(const Length2 ctr, const UnitVec rot,
                                                            const Length2 localCtr) noexcept
{
    assert(IsValid(rot));
    return Transformation{ctr - (Rotate(localCtr, rot)), rot};
}
 
inline Transformation GetTransformation(const Position pos, const Length2 local_ctr) noexcept
{
    assert(IsValid(pos));
    assert(IsValid(local_ctr));
    return GetTransformation(pos.linear, UnitVec::Get(pos.angular), local_ctr);
}
 
inline Transformation GetTransformation(const Sweep& sweep, const Real beta) noexcept
{
    assert(beta >= 0);
    assert(beta <= 1);
    return GetTransformation(GetPosition(sweep.pos0, sweep.pos1, beta), sweep.GetLocalCenter());
}
 
inline Transformation GetTransform0(const Sweep& sweep) noexcept
{
    return GetTransformation(sweep.pos0, sweep.GetLocalCenter());
}
 
inline Transformation GetTransform1(const Sweep& sweep) noexcept
{
    return GetTransformation(sweep.pos1, sweep.GetLocalCenter());
}
 
LinearVelocity2 GetContactRelVelocity(const Velocity velA, const Length2 relA,
                                      const Velocity velB, const Length2 relB) noexcept;
 
inline bool IsUnderActive(Velocity velocity,
                          LinearVelocity linSleepTol, AngularVelocity angSleepTol) noexcept
{
    const auto linVelSquared = GetMagnitudeSquared(velocity.linear);
    const auto angVelSquared = Square(velocity.angular);
    return (angVelSquared <= Square(angSleepTol)) && (linVelSquared <= Square(linSleepTol));
}
 
PLAYRHO_CONSTEXPR inline auto GetReflectionMatrix(UnitVec axis)
{
    constexpr auto TupleSize = std::tuple_size<decltype(axis)>::value;
    constexpr auto NumRows = TupleSize;
    constexpr auto NumCols = TupleSize;
    auto result = Matrix<Real, NumRows, NumCols>{};
    for (auto row = decltype(NumRows){0}; row < NumRows; ++row)
    {
        for (auto col = decltype(NumCols){0}; col < NumCols; ++col)
        {
            result[row][col] = ((row == col)? Real{1}: Real{0}) - axis[row] * axis[col] * 2;
        }
    }
    return result;
}
 
} // namespace d2
} // namespace playrho
 
#endif // PLAYRHO_COMMON_MATH_HPP