diff --git a/README.md b/README.md index d6623e4..1050e4e 100644 --- a/README.md +++ b/README.md @@ -15,6 +15,9 @@ shader semantics. - scalar-to-vector splats - swizzles for common lanes - conversions to and from `System.Numerics` +- `BatchMath` SoA kernels over `Span` / `ReadOnlySpan` using + `System.Numerics.Vector` when available, including radial falloff + acceleration and semi-implicit Euler integration - `math` intrinsics: `radians`, `degrees`, `abs`, `floor`, `ceil`, `frac`, `min`, `max`, `clamp`, `saturate`, `lerp`, `step`, `smoothstep`, `dot`, `cross`, `length`, `distance`, `normalize`, `reflect`, `csum`, exponential diff --git a/src/CultMath/BatchMath.cs b/src/CultMath/BatchMath.cs new file mode 100644 index 0000000..df62e07 --- /dev/null +++ b/src/CultMath/BatchMath.cs @@ -0,0 +1,147 @@ +using System.Numerics; + +namespace CultMath; + +public static class BatchMath +{ + private const float MinimumDistanceSquared = 1.0e-8f; + + public static int LaneCount => Vector.Count; + + public static bool IsHardwareAccelerated => Vector.IsHardwareAccelerated; + + public static void Clear(Span values) => values.Clear(); + + public static void AddRadialFalloffAcceleration2D( + ReadOnlySpan positionX, + ReadOnlySpan positionY, + float centerX, + float centerY, + float strength, + float radius, + Span accelerationX, + Span accelerationY) + { + ValidateEqualLengths(positionX.Length, positionY.Length, accelerationX.Length, accelerationY.Length); + + if (radius <= 0.0f || strength == 0.0f || positionX.Length == 0) + { + return; + } + + var radiusVector = new Vector(radius); + var centerXVector = new Vector(centerX); + var centerYVector = new Vector(centerY); + var strengthVector = new Vector(strength); + var one = Vector.One; + var zero = Vector.Zero; + var minimumDistanceSquared = new Vector(MinimumDistanceSquared); + + var i = 0; + var lanes = Vector.Count; + for (; i <= positionX.Length - lanes; i += lanes) + { + var px = new Vector(positionX.Slice(i, lanes)); + var py = new Vector(positionY.Slice(i, lanes)); + var dx = centerXVector - px; + var dy = centerYVector - py; + var distanceSquared = Vector.Max(dx * dx + dy * dy, minimumDistanceSquared); + var distance = Vector.SquareRoot(distanceSquared); + var inRange = Vector.LessThanOrEqual(distance, radiusVector); + var falloff = Vector.Max(zero, one - distance / radiusVector); + var magnitude = strengthVector * falloff; + var ax = Vector.ConditionalSelect(inRange, dx / distance * magnitude, zero); + var ay = Vector.ConditionalSelect(inRange, dy / distance * magnitude, zero); + + (new Vector(accelerationX.Slice(i, lanes)) + ax).CopyTo(accelerationX.Slice(i, lanes)); + (new Vector(accelerationY.Slice(i, lanes)) + ay).CopyTo(accelerationY.Slice(i, lanes)); + } + + for (; i < positionX.Length; i++) + { + var dx = centerX - positionX[i]; + var dy = centerY - positionY[i]; + var distanceSquared = MathF.Max(dx * dx + dy * dy, MinimumDistanceSquared); + var distance = MathF.Sqrt(distanceSquared); + if (distance > radius) + { + continue; + } + + var falloff = MathF.Max(0.0f, 1.0f - distance / radius); + var magnitude = strength * falloff; + accelerationX[i] += dx / distance * magnitude; + accelerationY[i] += dy / distance * magnitude; + } + } + + public static void IntegrateSemiImplicitEuler2D( + float deltaTime, + ReadOnlySpan dynamicMask, + Span positionX, + Span positionY, + Span velocityX, + Span velocityY, + ReadOnlySpan accelerationX, + ReadOnlySpan accelerationY) + { + if (deltaTime <= 0.0f) + { + throw new ArgumentOutOfRangeException(nameof(deltaTime), "Delta time must be positive."); + } + + ValidateEqualLengths( + dynamicMask.Length, + positionX.Length, + positionY.Length, + velocityX.Length, + velocityY.Length, + accelerationX.Length, + accelerationY.Length); + + var delta = new Vector(deltaTime); + var i = 0; + var lanes = Vector.Count; + for (; i <= positionX.Length - lanes; i += lanes) + { + var mask = new Vector(dynamicMask.Slice(i, lanes)); + var vx = new Vector(velocityX.Slice(i, lanes)); + var vy = new Vector(velocityY.Slice(i, lanes)); + var px = new Vector(positionX.Slice(i, lanes)); + var py = new Vector(positionY.Slice(i, lanes)); + var ax = new Vector(accelerationX.Slice(i, lanes)); + var ay = new Vector(accelerationY.Slice(i, lanes)); + + vx += ax * delta * mask; + vy += ay * delta * mask; + px += vx * delta * mask; + py += vy * delta * mask; + + vx.CopyTo(velocityX.Slice(i, lanes)); + vy.CopyTo(velocityY.Slice(i, lanes)); + px.CopyTo(positionX.Slice(i, lanes)); + py.CopyTo(positionY.Slice(i, lanes)); + } + + for (; i < positionX.Length; i++) + { + var mask = dynamicMask[i]; + velocityX[i] += accelerationX[i] * deltaTime * mask; + velocityY[i] += accelerationY[i] * deltaTime * mask; + positionX[i] += velocityX[i] * deltaTime * mask; + positionY[i] += velocityY[i] * deltaTime * mask; + } + } + + private static void ValidateEqualLengths(params int[] lengths) + { + var length = lengths[0]; + for (var i = 1; i < lengths.Length; i++) + { + if (lengths[i] != length) + { + throw new ArgumentException("CultMath batch spans must have equal length."); + } + } + } +} diff --git a/tests/CultMath.Tests/MathTests.cs b/tests/CultMath.Tests/MathTests.cs index 18fa203..44dd129 100644 --- a/tests/CultMath.Tests/MathTests.cs +++ b/tests/CultMath.Tests/MathTests.cs @@ -116,4 +116,41 @@ public sealed class MathTests Assert.True(colors[1].r > colors[1].g); Assert.True(colors[2].r > 120 && colors[2].g > 120 && colors[2].b > 120); } + + [Fact] + public void BatchRadialFalloffAccelerationMatchesScalarContract() + { + var xs = new[] { 0.0f, 5.0f, 10.0f }; + var ys = new[] { 0.0f, 0.0f, 0.0f }; + var ax = new float[xs.Length]; + var ay = new float[xs.Length]; + + BatchMath.AddRadialFalloffAcceleration2D(xs, ys, 10.0f, 0.0f, 10.0f, 20.0f, ax, ay); + + Assert.True(BatchMath.LaneCount >= 4); + Assert.Equal(5.0f, ax[0], precision: 5); + Assert.Equal(7.5f, ax[1], precision: 5); + Assert.Equal(0.0f, ax[2], precision: 5); + Assert.Equal(0.0f, ay[0], precision: 5); + } + + [Fact] + public void BatchEulerIntegrationHonorsDynamicMask() + { + var dynamicMask = new[] { 1.0f, 0.0f }; + var px = new[] { 0.0f, 0.0f }; + var py = new[] { 0.0f, 0.0f }; + var vx = new[] { 1.0f, 1.0f }; + var vy = new[] { 0.0f, 0.0f }; + var ax = new[] { 2.0f, 2.0f }; + var ay = new[] { 0.0f, 4.0f }; + + BatchMath.IntegrateSemiImplicitEuler2D(0.5f, dynamicMask, px, py, vx, vy, ax, ay); + + Assert.Equal(2.0f, vx[0], precision: 5); + Assert.Equal(1.0f, px[0], precision: 5); + Assert.Equal(1.0f, vx[1], precision: 5); + Assert.Equal(0.0f, px[1], precision: 5); + Assert.Equal(0.0f, py[1], precision: 5); + } }