Mitigate catastrophic cancellation in cross products and other code

doc/snippets/MagnumMath.cpp

@@ -39,6 +39,7 @@
 #include "Magnum/Math/Range.h"
 #include "Magnum/Math/Algorithms/GramSchmidt.h"
 #include "Magnum/Math/StrictWeakOrdering.h"
+#include "Magnum/Math/Swizzle.h"
 
 using namespace Magnum;
 using namespace Magnum::Math::Literals;

doc/snippets/MagnumTrade.cpp

@@ -32,8 +32,9 @@
 #include "Magnum/ImageView.h"
 #include "Magnum/Mesh.h"
 #include "Magnum/PixelFormat.h"
-#include "Magnum/MeshTools/Interleave.h"
 #include "Magnum/Animation/Player.h"
+#include "Magnum/Math/Swizzle.h"
+#include "Magnum/MeshTools/Interleave.h"
 #include "Magnum/MeshTools/Transform.h"
 #include "Magnum/Trade/AbstractImporter.h"
 #include "Magnum/Trade/AnimationData.h"

src/Magnum/Math/Test/CMakeLists.txt

@@ -68,6 +68,7 @@ corrade_add_test(MathInterpolationBenchmark InterpolationBenchmark.cpp LIBRARIES
 corrade_add_test(MathConfigurationValueTest ConfigurationValueTest.cpp LIBRARIES MagnumMathTestLib)
 corrade_add_test(MathStrictWeakOrderingTest StrictWeakOrderingTest.cpp LIBRARIES MagnumMathTestLib)
 
+corrade_add_test(MathVectorBenchmark VectorBenchmark.cpp LIBRARIES MagnumMathTestLib)
 corrade_add_test(MathMatrixBenchmark MatrixBenchmark.cpp LIBRARIES MagnumMathTestLib)
 
 set_property(TARGET

src/Magnum/Math/Test/ColorTest.cpp

@@ -35,6 +35,7 @@
 #include "Magnum/Math/Color.h"
 #include "Magnum/Math/Half.h"
 #include "Magnum/Math/StrictWeakOrdering.h"
+#include "Magnum/Math/Swizzle.h"
 
 struct Vec3 {
     float x, y, z;

src/Magnum/Math/Test/Vector2Test.cpp

@@ -29,6 +29,7 @@
 
 #include "Magnum/Math/Vector3.h" /* Vector3 used in Vector2Test::cross() */
 #include "Magnum/Math/StrictWeakOrdering.h"
+#include "Magnum/Math/Swizzle.h"
 
 struct Vec2 {
     float x, y;
@@ -65,6 +66,7 @@ struct Vector2Test: Corrade::TestSuite::Tester {
 
     void access();
     void cross();
+    void crossCatastrophicCancellation();
     void axes();
     void scales();
     void perpendicular();
@@ -78,6 +80,7 @@ struct Vector2Test: Corrade::TestSuite::Tester {
 
 typedef Math::Vector3<Int> Vector3i;
 typedef Math::Vector2<Float> Vector2;
+typedef Math::Vector2<Double> Vector2d;
 typedef Math::Vector2<Int> Vector2i;
 
 Vector2Test::Vector2Test() {
@@ -91,6 +94,7 @@ Vector2Test::Vector2Test() {
 
               &Vector2Test::access,
               &Vector2Test::cross,
+              &Vector2Test::crossCatastrophicCancellation,
               &Vector2Test::axes,
               &Vector2Test::scales,
               &Vector2Test::perpendicular,
@@ -207,6 +211,18 @@ void Vector2Test::cross() {
     CORRADE_COMPARE(Math::cross<Int>({a, 0}, {b, 0}), Vector3i(0, 0, Math::cross(a, b)));
 }
 
+void Vector2Test::crossCatastrophicCancellation() {
+    /* Repro case and algorithm from
+       https://pharr.org/matt/blog/2019/11/03/difference-of-floats.html */
+
+    Vector2 a{33962.035f, 41563.4f};
+    Vector2 b{-24871.969f, -30438.8f};
+
+    Float expected = -75.1656f;
+    CORRADE_COMPARE(Float(Math::cross(Vector2d{a}, Vector2d{b})), expected);
+    CORRADE_COMPARE(Math::cross(a, b), expected);
+}
+
 void Vector2Test::axes() {
     constexpr Vector2 x = Vector2::xAxis(5.0f);
     constexpr Vector2 y = Vector2::yAxis(6.0f);

src/Magnum/Math/Test/Vector3Test.cpp

@@ -29,6 +29,7 @@
 
 #include "Magnum/Math/Vector3.h"
 #include "Magnum/Math/StrictWeakOrdering.h"
+#include "Magnum/Math/Swizzle.h"
 
 struct Vec3 {
     float x, y, z;
@@ -66,6 +67,7 @@ struct Vector3Test: Corrade::TestSuite::Tester {
 
     void access();
     void cross();
+    void crossCatastrophicCancellation();
     void axes();
     void scales();
     void twoComponent();
@@ -77,6 +79,7 @@ struct Vector3Test: Corrade::TestSuite::Tester {
 };
 
 typedef Math::Vector3<Float> Vector3;
+typedef Math::Vector3<Double> Vector3d;
 typedef Math::Vector3<Int> Vector3i;
 typedef Math::Vector2<Float> Vector2;
 
@@ -92,6 +95,7 @@ Vector3Test::Vector3Test() {
 
               &Vector3Test::access,
               &Vector3Test::cross,
+              &Vector3Test::crossCatastrophicCancellation,
               &Vector3Test::axes,
               &Vector3Test::scales,
               &Vector3Test::twoComponent,
@@ -227,6 +231,17 @@ void Vector3Test::cross() {
     CORRADE_COMPARE(Math::cross(a, b), Vector3i(-10, -3, 7));
 }
 
+void Vector3Test::crossCatastrophicCancellation() {
+    /* Repro case and algorithm from
+       https://pharr.org/matt/blog/2019/11/03/difference-of-floats.html */
+    Vector3 a{33962.035f, 41563.4f, 7706.415f};
+    Vector3 b{-24871.969f, -30438.8f, -5643.727f};
+
+    Vector3 expected{1556.0276f, -1257.5151f, -75.1656f};
+    CORRADE_COMPARE(Vector3{Math::cross(Vector3d{a}, Vector3d{b})}, expected);
+    CORRADE_COMPARE(Math::cross(a, b), expected);
+}
+
 void Vector3Test::axes() {
     constexpr Vector3 x = Vector3::xAxis(5.0f);
     constexpr Vector3 y = Vector3::yAxis(6.0f);

src/Magnum/Math/Test/Vector4Test.cpp

@@ -29,6 +29,7 @@
 
 #include "Magnum/Math/Vector4.h"
 #include "Magnum/Math/StrictWeakOrdering.h"
+#include "Magnum/Math/Swizzle.h"
 
 struct Vec4 {
     float x, y, z, w;

src/Magnum/Math/Test/VectorBenchmark.cpp

@@ -0,0 +1,233 @@
+/*
+    This file is part of Magnum.
+
+    Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019
+              Vladimír Vondruš <[email protected]>
+
+    Permission is hereby granted, free of charge, to any person obtaining a
+    copy of this software and associated documentation files (the "Software"),
+    to deal in the Software without restriction, including without limitation
+    the rights to use, copy, modify, merge, publish, distribute, sublicense,
+    and/or sell copies of the Software, and to permit persons to whom the
+    Software is furnished to do so, subject to the following conditions:
+
+    The above copyright notice and this permission notice shall be included
+    in all copies or substantial portions of the Software.
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+    THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+    DEALINGS IN THE SOFTWARE.
+*/
+
+#include <Corrade/TestSuite/Tester.h>
+
+#include "Magnum/Math/Vector3.h"
+
+#ifdef CORRADE_TARGET_SSE2
+#include <xmmintrin.h>
+#endif
+
+namespace Magnum { namespace Math { namespace Test { namespace {
+
+struct VectorBenchmark: Corrade::TestSuite::Tester {
+    explicit VectorBenchmark();
+
+    void dot();
+
+    template<class T> void cross2Baseline();
+    void cross2();
+
+    template<class T> void cross3Baseline();
+    void cross3();
+    #ifdef CORRADE_TARGET_SSE2
+    void cross3SseNaive();
+    void cross3SseOneShuffleLess();
+    #endif
+};
+
+VectorBenchmark::VectorBenchmark() {
+    addBenchmarks({
+        &VectorBenchmark::dot,
+
+        &VectorBenchmark::cross2Baseline<Float>,
+        &VectorBenchmark::cross2Baseline<Double>,
+        &VectorBenchmark::cross2,
+
+        &VectorBenchmark::cross3Baseline<Float>,
+        &VectorBenchmark::cross3Baseline<Double>,
+        &VectorBenchmark::cross3,
+        #ifdef CORRADE_TARGET_SSE2
+        &VectorBenchmark::cross3SseNaive,
+        &VectorBenchmark::cross3SseOneShuffleLess,
+        #endif
+    }, 500);
+}
+
+typedef Math::Constants<Float> Constants;
+typedef Math::Vector2<Float> Vector2;
+typedef Math::Vector3<Float> Vector3;
+
+enum: std::size_t { Repeats = 100000 };
+
+using namespace Literals;
+
+void VectorBenchmark::dot() {
+    Vector3 a{1.3f, -1.1f, 1.0f};
+    Vector3 b{4.5f, 3.2f, 7.3f};
+    CORRADE_COMPARE(Math::dot(a, b), 9.63f);
+
+    CORRADE_BENCHMARK(Repeats) {
+        a.x() = Math::dot(a, b);
+    }
+
+    CORRADE_COMPARE(a, (Vector3{Constants::inf(), -1.1f, 1.0f}));
+}
+
+template<class T> inline T cross2Baseline(const Vector2& v1, const Vector2& v2) {
+    T v1x = T(v1.x()),
+      v1y = T(v1.y());
+    T v2x = T(v2.x()),
+      v2y = T(v2.y());
+    return T(v1x*v2y - v1y*v2x);
+}
+
+template<class T> void VectorBenchmark::cross2Baseline() {
+    setTestCaseTemplateName(TypeTraits<T>::name());
+
+    Vector2 a{1.3f, -1.1f};
+    Vector2 b{4.5f, 3.2f};
+    CORRADE_COMPARE(Test::cross2Baseline<T>(a, b), 9.11f);
+
+    CORRADE_BENCHMARK(Repeats) {
+        a.x() = Test::cross2Baseline<T>(a, b);
+    }
+
+    CORRADE_COMPARE(a, (Vector2{Constants::inf(), -1.1f}));
+}
+
+void VectorBenchmark::cross2() {
+    Vector2 a{1.3f, -1.1f};
+    Vector2 b{4.5f, 3.2f};
+    CORRADE_COMPARE(Math::cross(a, b), 9.11f);
+
+    CORRADE_BENCHMARK(Repeats) {
+        a.x() = Math::cross(a, b);
+    }
+
+    CORRADE_COMPARE(a, (Vector2{Constants::inf(), -1.1f}));
+}
+
+template<class T> inline Vector3 cross3Baseline(const Vector3& v1, const Vector3& v2) {
+    T v1x = T(v1.x()),
+      v1y = T(v1.y()),
+      v1z = T(v1.z());
+    T v2x = T(v2.x()),
+      v2y = T(v2.y()),
+      v2z = T(v2.z());
+    return Vector3(v1y*v2z - v1z*v2y,
+                   v1z*v2x - v1x*v2z,
+                   v1x*v2y - v1y*v2x);
+}
+
+template<class T> void VectorBenchmark::cross3Baseline() {
+    setTestCaseTemplateName(TypeTraits<T>::name());
+
+    Vector3 a{1.3f, -1.1f, 1.0f};
+    Vector3 b{4.5f, 3.2f, 7.3f};
+    CORRADE_COMPARE(Test::cross3Baseline<T>(a, b),
+        (Vector3{-11.23f, -4.99f, 9.11f}));
+
+    CORRADE_BENCHMARK(Repeats) {
+        a = Test::cross3Baseline<T>(a, b);
+    }
+
+    CORRADE_VERIFY(a != a);
+}
+
+void VectorBenchmark::cross3() {
+    Vector3 a{1.3f, -1.1f, 1.0f};
+    Vector3 b{4.5f, 3.2f, 7.3f};
+    CORRADE_COMPARE(Math::cross(a, b),
+        (Vector3{-11.23f, -4.99f, 9.11f}));
+
+    CORRADE_BENCHMARK(Repeats) {
+        a = Math::cross(a, b);
+    }
+
+    CORRADE_VERIFY(a != a);
+}
+
+#ifdef CORRADE_TARGET_SSE2
+inline Vector3 crossSseNaive(const Vector3& a, const Vector3& b) {
+    union {
+        __m128 v;
+        Float s[4];
+    };
+
+    const __m128 aa = _mm_set_ps(0.0f, a[2], a[1], a[0]);
+    const __m128 bb = _mm_set_ps(0.0f, b[2], b[1], b[0]);
+
+    v = _mm_sub_ps(
+        _mm_mul_ps(_mm_shuffle_ps(aa, aa, _MM_SHUFFLE(3, 0, 2, 1)),
+                   _mm_shuffle_ps(bb, bb, _MM_SHUFFLE(3, 1, 0, 2))),
+        _mm_mul_ps(_mm_shuffle_ps(aa, aa, _MM_SHUFFLE(3, 1, 0, 2)),
+                   _mm_shuffle_ps(bb, bb, _MM_SHUFFLE(3, 0, 2, 1))));
+    return {s[0], s[1], s[2]};
+}
+
+/* https://twitter.com/sjb3d/status/563640846671953920. Originally the
+   Math::cross() was doing this, implemented as
+    gather<'y', 'z', 'x'>(a*gather<'y', 'z', 'x'>(b) -
+                          b*gather<'y', 'z', 'x'>(a))
+   but while slightly faster in Release (on GCC at least) than the
+   straightforward version, it was insanely slow in Debug. */
+inline Vector3 crossSseOneShuffleLess(const Vector3& a, const Vector3& b) {
+    union {
+        __m128 v;
+        Float s[4];
+    };
+
+    const __m128 aa = _mm_set_ps(0.0f, a[2], a[1], a[0]);
+    const __m128 bb = _mm_set_ps(0.0f, b[2], b[1], b[0]);
+    const __m128 cc = _mm_sub_ps(
+        _mm_mul_ps(aa, _mm_shuffle_ps(bb, bb, _MM_SHUFFLE(3, 0, 2, 1))),
+        _mm_mul_ps(bb, _mm_shuffle_ps(aa, aa, _MM_SHUFFLE(3, 0, 2, 1))));
+
+    v = _mm_shuffle_ps(cc, cc, _MM_SHUFFLE(3, 0, 2, 1));
+    return {s[0], s[1], s[2]};
+}
+
+void VectorBenchmark::cross3SseNaive() {
+    Vector3 a{1.3f, -1.1f, 1.0f};
+    Vector3 b{4.5f, 3.2f, 7.3f};
+    CORRADE_COMPARE(Test::crossSseNaive(a, b),
+        (Vector3{-11.23f, -4.99f, 9.11f}));
+
+    CORRADE_BENCHMARK(Repeats) {
+        a = Test::crossSseNaive(a, b);
+    }
+
+    CORRADE_VERIFY(a != a);
+}
+
+void VectorBenchmark::cross3SseOneShuffleLess() {
+    Vector3 a{1.3f, -1.1f, 1.0f};
+    Vector3 b{4.5f, 3.2f, 7.3f};
+    CORRADE_COMPARE(Test::crossSseOneShuffleLess(a, b),
+        (Vector3{-11.23f, -4.99f, 9.11f}));
+
+    CORRADE_BENCHMARK(Repeats) {
+        a = Test::crossSseOneShuffleLess(a, b);
+    }
+
+    CORRADE_VERIFY(a != a);
+}
+#endif
+
+}}}}
+
+CORRADE_TEST_MAIN(Magnum::Math::Test::VectorBenchmark)

src/Magnum/Math/Vector.h

@@ -102,7 +102,10 @@ the same general direction, `1` when two *normalized* vectors are parallel,
 @see @ref Vector::dot() const, @ref Vector::operator-(), @ref Vector2::perpendicular()
 */
 template<std::size_t size, class T> inline T dot(const Vector<size, T>& a, const Vector<size, T>& b) {
-    return (a*b).sum();
+    T out{};
+    for(std::size_t i = 0; i != size; ++i)
+        out += a._data[i]*b._data[i];
+    return out;
 }
 
 /** @relatesalso Vector
@@ -686,6 +689,8 @@ template<std::size_t size, class T> class Vector {
         template<std::size_t size_, class T_> friend BoolVector<size_> equal(const Vector<size_, T_>&, const Vector<size_, T_>&);
         template<std::size_t size_, class T_> friend BoolVector<size_> notEqual(const Vector<size_, T_>&, const Vector<size_, T_>&);
 
+        template<std::size_t size_, class U> friend U dot(const Vector<size_, U>&, const Vector<size_, U>&);
+
         /* Implementation for Vector<size, T>::Vector(const Vector<size, U>&) */
         template<class U, std::size_t ...sequence> constexpr explicit Vector(Implementation::Sequence<sequence...>, const Vector<size, U>& vector) noexcept: _data{T(vector._data[sequence])...} {}