From b5ef7d9bfb6b212bd2645b13bf84da71b1988b1b Mon Sep 17 00:00:00 2001
From: Anton Firszov <antonfir@gmail.com>
Date: Sun, 7 Oct 2018 01:25:53 +0200
Subject: [PATCH] add bulk Premultiply

---
 .../Common/Extensions/Vector4Extensions.cs    |  1 +
 src/ImageSharp/Common/Helpers/ImageMaths.cs   | 23 ++++++++++++++++++
 .../Helpers/ImageMathsTests.cs                | 19 +++++++++++++++
 .../TestUtilities/TestDataGenerator.cs        | 24 ++++++++++++++++++-
 4 files changed, 66 insertions(+), 1 deletion(-)

diff --git a/src/ImageSharp/Common/Extensions/Vector4Extensions.cs b/src/ImageSharp/Common/Extensions/Vector4Extensions.cs
index b88c229c5d..7fb5fd8ee3 100644
--- a/src/ImageSharp/Common/Extensions/Vector4Extensions.cs
+++ b/src/ImageSharp/Common/Extensions/Vector4Extensions.cs
@@ -4,6 +4,7 @@
 using System;
 using System.Numerics;
 using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
 
 using SixLabors.ImageSharp.PixelFormats;
 
diff --git a/src/ImageSharp/Common/Helpers/ImageMaths.cs b/src/ImageSharp/Common/Helpers/ImageMaths.cs
index b3a1b4ba39..a318d1941c 100644
--- a/src/ImageSharp/Common/Helpers/ImageMaths.cs
+++ b/src/ImageSharp/Common/Helpers/ImageMaths.cs
@@ -2,7 +2,10 @@
 // Licensed under the Apache License, Version 2.0.
 
 using System;
+using System.Numerics;
 using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+
 using SixLabors.ImageSharp.PixelFormats;
 using SixLabors.Primitives;
 
@@ -275,5 +278,25 @@ namespace SixLabors.ImageSharp
 
             return GetBoundingRectangle(topLeft, bottomRight);
         }
+
+        /// <summary>
+        /// Pre-multiply all vectors.
+        /// "x", "y", "z" components of a vector by its "w" component leaving the "w" component intact.
+        /// </summary>
+        /// <seealso cref="Vector4Extensions.Premultiply"/>
+        /// <param name="vectors">The span of vectors</param>
+        public static void Premultiply(Span<Vector4> vectors)
+        {
+            // TODO: This method can be AVX2 optimized using Vector<float>
+            ref Vector4 baseRef = ref MemoryMarshal.GetReference(vectors);
+
+            for (int i = 0; i < vectors.Length; i++)
+            {
+                ref Vector4 v = ref Unsafe.Add(ref baseRef, i);
+                var s = new Vector4(v.W);
+                s.W = 1;
+                v *= s;
+            }
+        }
     }
 }
\ No newline at end of file
diff --git a/tests/ImageSharp.Tests/Helpers/ImageMathsTests.cs b/tests/ImageSharp.Tests/Helpers/ImageMathsTests.cs
index 41e6b65c56..51b407f86a 100644
--- a/tests/ImageSharp.Tests/Helpers/ImageMathsTests.cs
+++ b/tests/ImageSharp.Tests/Helpers/ImageMathsTests.cs
@@ -1,6 +1,10 @@
 // Copyright (c) Six Labors and contributors.
 // Licensed under the Apache License, Version 2.0.
 
+using System;
+using System.Linq;
+using System.Numerics;
+
 using Xunit;
 
 namespace SixLabors.ImageSharp.Tests.Helpers
@@ -35,6 +39,21 @@ namespace SixLabors.ImageSharp.Tests.Helpers
             Assert.Equal(expected, actual);
         }
 
+        [Theory]
+        [InlineData(0)]
+        [InlineData(1)]
+        [InlineData(30)]
+        public void Premultiply_VectorSpan(int length)
+        {
+            var rnd = new Random(42);
+            Vector4[] source = rnd.GenerateRandomVectorArray(length, 0, 1);
+            Vector4[] expected = source.Select(v => v.Premultiply()).ToArray();
+
+            ImageMaths.Premultiply(source);
+
+            Assert.Equal(expected, source, new ApproximateFloatComparer(1e-6f));
+        }
+
         // TODO: We need to test all ImageMaths methods!
     }
 }
\ No newline at end of file
diff --git a/tests/ImageSharp.Tests/TestUtilities/TestDataGenerator.cs b/tests/ImageSharp.Tests/TestUtilities/TestDataGenerator.cs
index 9eb051e7a7..0b1b89cc00 100644
--- a/tests/ImageSharp.Tests/TestUtilities/TestDataGenerator.cs
+++ b/tests/ImageSharp.Tests/TestUtilities/TestDataGenerator.cs
@@ -1,4 +1,5 @@
 using System;
+using System.Numerics;
 
 namespace SixLabors.ImageSharp.Tests
 {
@@ -10,7 +11,23 @@ namespace SixLabors.ImageSharp.Tests
 
             for (int i = 0; i < length; i++)
             {
-                values[i] = (float)rnd.NextDouble() * (maxVal - minVal) + minVal;
+                values[i] = GetRandomFloat(rnd, minVal, maxVal);
+            }
+
+            return values;
+        }
+
+        public static Vector4[] GenerateRandomVectorArray(this Random rnd, int length, float minVal, float maxVal)
+        {
+            var values = new Vector4[length];
+
+            for (int i = 0; i < length; i++)
+            {
+                ref Vector4 v = ref values[i];
+                v.X = GetRandomFloat(rnd, minVal, maxVal);
+                v.Y = GetRandomFloat(rnd, minVal, maxVal);
+                v.Z = GetRandomFloat(rnd, minVal, maxVal);
+                v.W = GetRandomFloat(rnd, minVal, maxVal);
             }
 
             return values;
@@ -28,5 +45,10 @@ namespace SixLabors.ImageSharp.Tests
 
             return values;
         }
+
+        private static float GetRandomFloat(Random rnd, float minVal, float maxVal)
+        {
+            return (float)rnd.NextDouble() * (maxVal - minVal) + minVal;
+        }
     }
 }
\ No newline at end of file