Use doubles to calculate the listener matrix translation

To help stablize sources near the listener, when away from origin. Also clean
up some related methods and move them to more appropriate places.

2 files changed, 87 insertions, 81 deletions

diff --git a/alc/alu.cpp b/alc/alu.cpp
index 9f0c7ce5..7e7dabf9 100644
--- a/alc/alu.cpp
+++ b/alc/alu.cpp
@@ -362,33 +362,6 @@ auto GetAmbi2DLayout(AmbiLayout layouttype) noexcept -> const std::array<uint8_t
 }
 
 
-inline alu::Vector aluCrossproduct(const alu::Vector &in1, const alu::Vector &in2)
-{
-    return alu::Vector{
-        in1[1]*in2[2] - in1[2]*in2[1],
-        in1[2]*in2[0] - in1[0]*in2[2],
-        in1[0]*in2[1] - in1[1]*in2[0],
-        0.0f
-    };
-}
-
-inline float aluDotproduct(const alu::Vector &vec1, const alu::Vector &vec2)
-{
-    return vec1[0]*vec2[0] + vec1[1]*vec2[1] + vec1[2]*vec2[2];
-}
-
-
-alu::Vector operator*(const alu::Matrix &mtx, const alu::Vector &vec) noexcept
-{
-    return alu::Vector{
-        vec[0]*mtx[0][0] + vec[1]*mtx[1][0] + vec[2]*mtx[2][0] + vec[3]*mtx[3][0],
-        vec[0]*mtx[0][1] + vec[1]*mtx[1][1] + vec[2]*mtx[2][1] + vec[3]*mtx[3][1],
-        vec[0]*mtx[0][2] + vec[1]*mtx[1][2] + vec[2]*mtx[2][2] + vec[3]*mtx[3][2],
-        vec[0]*mtx[0][3] + vec[1]*mtx[1][3] + vec[2]*mtx[2][3] + vec[3]*mtx[3][3]
-    };
-}
-
-
 bool CalcContextParams(ALCcontext *Context)
 {
     ALcontextProps *props{Context->mUpdate.exchange(nullptr, std::memory_order_acq_rel)};
@@ -418,19 +391,22 @@ bool CalcListenerParams(ALCcontext *Context)
     alu::Vector V{props->OrientUp[0], props->OrientUp[1], props->OrientUp[2], 0.0f};
     V.normalize();
     /* Build and normalize right-vector */
-    alu::Vector U{aluCrossproduct(N, V)};
+    alu::Vector U{N.cross_product(V)};
     U.normalize();
 
-    Listener.Params.Matrix = alu::Matrix{
-        U[0], V[0], -N[0], 0.0f,
-        U[1], V[1], -N[1], 0.0f,
-        U[2], V[2], -N[2], 0.0f,
-        0.0f, 0.0f,  0.0f, 1.0f
-    };
+    const alu::MatrixR<double> rot{
+        U[0], V[0], -N[0], 0.0,
+        U[1], V[1], -N[1], 0.0,
+        U[2], V[2], -N[2], 0.0,
+         0.0,  0.0,   0.0, 1.0};
+    const alu::VectorR<double> pos{props->Position[0],props->Position[1],props->Position[2],1.0};
+    const alu::Vector P{alu::cast_to<float>(rot * pos)};
 
-    const alu::Vector P{Listener.Params.Matrix *
-        alu::Vector{props->Position[0], props->Position[1], props->Position[2], 1.0f}};
-    Listener.Params.Matrix.setRow(3, -P[0], -P[1], -P[2], 1.0f);
+    Listener.Params.Matrix = alu::Matrix{
+         U[0],  V[0], -N[0], 0.0f,
+         U[1],  V[1], -N[1], 0.0f,
+         U[2],  V[2], -N[2], 0.0f,
+        -P[0], -P[1], -P[2], 1.0f};
 
     const alu::Vector vel{props->Velocity[0], props->Velocity[1], props->Velocity[2], 0.0f};
     Listener.Params.Velocity = Listener.Params.Matrix * vel;
@@ -905,7 +881,7 @@ void CalcPanningAndFilters(Voice *voice, const float xpos, const float ypos, con
                 V = Listener.Params.Matrix * V;
             }
             /* Build and normalize right-vector */
-            alu::Vector U{aluCrossproduct(N, V)};
+            alu::Vector U{N.cross_product(V)};
             U.normalize();
 
             /* Build a rotation matrix. Manually fill the zeroth- and first-
@@ -1427,8 +1403,7 @@ void CalcAttnSourceParams(Voice *voice, const VoiceProps *props, const ALCcontex
     /* Calculate directional soundcones */
     if(directional && props->InnerAngle < 360.0f)
     {
-        const float Angle{Rad2Deg(std::acos(-aluDotproduct(Direction, ToSource)) *
-            ConeScale * 2.0f)};
+        const float Angle{Rad2Deg(std::acos(Direction.dot_product(ToSource)) * ConeScale * -2.0f)};
 
         float ConeGain, ConeHF;
         if(!(Angle > props->InnerAngle))
@@ -1521,8 +1496,8 @@ void CalcAttnSourceParams(Voice *voice, const VoiceProps *props, const ALCcontex
     if(DopplerFactor > 0.0f)
     {
         const alu::Vector &lvelocity = Listener.Params.Velocity;
-        float vss{aluDotproduct(Velocity, ToSource) * -DopplerFactor};
-        float vls{aluDotproduct(lvelocity, ToSource) * -DopplerFactor};
+        float vss{Velocity.dot_product(ToSource) * -DopplerFactor};
+        float vls{lvelocity.dot_product(ToSource) * -DopplerFactor};
 
         const float SpeedOfSound{Listener.Params.SpeedOfSound};
         if(!(vls < SpeedOfSound))
diff --git a/common/vecmat.h b/common/vecmat.h
index cdf47125..5a61ad14 100644
--- a/common/vecmat.h
+++ b/common/vecmat.h
@@ -6,22 +6,26 @@
 #include <cstddef>
 #include <limits>
 
+#include "alspan.h"
+
 
 namespace alu {
 
-class Vector {
-    alignas(16) std::array<float,4> mVals;
+template<typename T, std::enable_if_t<std::is_floating_point<T>::value, bool> = true>
+class VectorR {
+    alignas(16) std::array<T,4> mVals;
 
 public:
-    Vector() noexcept = default;
-    constexpr Vector(float a, float b, float c, float d) noexcept
-      : mVals{{a, b, c, d}}
-    { }
+    constexpr VectorR() noexcept = default;
+    constexpr VectorR(const VectorR&) noexcept = default;
+    constexpr VectorR(T a, T b, T c, T d) noexcept : mVals{{a, b, c, d}} { }
+
+    constexpr VectorR& operator=(const VectorR&) noexcept = default;
 
-    float& operator[](size_t idx) noexcept { return mVals[idx]; }
-    constexpr const float& operator[](size_t idx) const noexcept { return mVals[idx]; }
+    T& operator[](size_t idx) noexcept { return mVals[idx]; }
+    constexpr const T& operator[](size_t idx) const noexcept { return mVals[idx]; }
 
-    Vector& operator+=(const Vector &rhs) noexcept
+    VectorR& operator+=(const VectorR &rhs) noexcept
     {
         mVals[0] += rhs.mVals[0];
         mVals[1] += rhs.mVals[1];
@@ -30,55 +34,82 @@ public:
         return *this;
     }
 
-    float normalize()
+    T normalize()
     {
-        const float length{std::sqrt(mVals[0]*mVals[0] + mVals[1]*mVals[1] + mVals[2]*mVals[2])};
-        if(length > std::numeric_limits<float>::epsilon())
+        const T length{std::sqrt(mVals[0]*mVals[0] + mVals[1]*mVals[1] + mVals[2]*mVals[2])};
+        if(length > std::numeric_limits<T>::epsilon())
         {
-            float inv_length = 1.0f/length;
+            T inv_length{T{1}/length};
             mVals[0] *= inv_length;
             mVals[1] *= inv_length;
             mVals[2] *= inv_length;
             return length;
         }
-        mVals[0] = mVals[1] = mVals[2] = 0.0f;
-        return 0.0f;
+        mVals[0] = mVals[1] = mVals[2] = T{0};
+        return T{0};
+    }
+
+    constexpr VectorR cross_product(const alu::VectorR<T> &rhs) const
+    {
+        return VectorR{
+            (*this)[1]*rhs[2] - (*this)[2]*rhs[1],
+            (*this)[2]*rhs[0] - (*this)[0]*rhs[2],
+            (*this)[0]*rhs[1] - (*this)[1]*rhs[0],
+            T{0}};
     }
+
+    constexpr T dot_product(const alu::VectorR<T> &rhs) const
+    { return (*this)[0]*rhs[0] + (*this)[1]*rhs[1] + (*this)[2]*rhs[2]; }
 };
+using Vector = VectorR<float>;
 
-class Matrix {
-    alignas(16) std::array<std::array<float,4>,4> mVals;
+template<typename T, std::enable_if_t<std::is_floating_point<T>::value, bool> = true>
+class MatrixR {
+    alignas(16) std::array<T,16> mVals;
 
 public:
-    Matrix() noexcept = default;
-    constexpr Matrix(float aa, float ab, float ac, float ad,
-                     float ba, float bb, float bc, float bd,
-                     float ca, float cb, float cc, float cd,
-                     float da, float db, float dc, float dd) noexcept
-      : mVals{{{{aa, ab, ac, ad}}, {{ba, bb, bc, bd}}, {{ca, cb, cc, cd}}, {{da, db, dc, dd}}}}
+    constexpr MatrixR() noexcept = default;
+    constexpr MatrixR(const MatrixR&) noexcept = default;
+    constexpr MatrixR(T aa, T ab, T ac, T ad,
+                      T ba, T bb, T bc, T bd,
+                      T ca, T cb, T cc, T cd,
+                      T da, T db, T dc, T dd) noexcept
+      : mVals{{aa,ab,ac,ad, ba,bb,bc,bd, ca,cb,cc,cd, da,db,dc,dd}}
     { }
 
-    std::array<float,4>& operator[](size_t idx) noexcept { return mVals[idx]; }
-    constexpr const std::array<float,4>& operator[](size_t idx) const noexcept { return mVals[idx]; }
+    constexpr MatrixR& operator=(const MatrixR&) noexcept = default;
 
-    void setRow(size_t idx, float a, float b, float c, float d) noexcept
-    {
-        mVals[idx][0] = a;
-        mVals[idx][1] = b;
-        mVals[idx][2] = c;
-        mVals[idx][3] = d;
-    }
+    auto operator[](size_t idx) noexcept { return al::span<T,4>{&mVals[idx*4], 4}; }
+    constexpr auto operator[](size_t idx) const noexcept
+    { return al::span<const T,4>{&mVals[idx*4], 4}; }
 
-    static constexpr Matrix Identity() noexcept
+    static constexpr MatrixR Identity() noexcept
     {
-        return Matrix{
-            1.0f, 0.0f, 0.0f, 0.0f,
-            0.0f, 1.0f, 0.0f, 0.0f,
-            0.0f, 0.0f, 1.0f, 0.0f,
-            0.0f, 0.0f, 0.0f, 1.0f
-        };
+        return MatrixR{
+            T{1}, T{0}, T{0}, T{0},
+            T{0}, T{1}, T{0}, T{0},
+            T{0}, T{0}, T{1}, T{0},
+            T{0}, T{0}, T{0}, T{1}};
     }
 };
+using Matrix = MatrixR<float>;
+
+template<typename T>
+inline VectorR<T> operator*(const MatrixR<T> &mtx, const VectorR<T> &vec) noexcept
+{
+    return VectorR<T>{
+        vec[0]*mtx[0][0] + vec[1]*mtx[1][0] + vec[2]*mtx[2][0] + vec[3]*mtx[3][0],
+        vec[0]*mtx[0][1] + vec[1]*mtx[1][1] + vec[2]*mtx[2][1] + vec[3]*mtx[3][1],
+        vec[0]*mtx[0][2] + vec[1]*mtx[1][2] + vec[2]*mtx[2][2] + vec[3]*mtx[3][2],
+        vec[0]*mtx[0][3] + vec[1]*mtx[1][3] + vec[2]*mtx[2][3] + vec[3]*mtx[3][3]};
+}
+
+template<typename U, typename T>
+inline VectorR<U> cast_to(const VectorR<T> &vec) noexcept
+{
+    return VectorR<U>{static_cast<U>(vec[0]), static_cast<U>(vec[1]),
+        static_cast<U>(vec[2]), static_cast<U>(vec[3])};
+}
 
 } // namespace alu