use crate::splines::{
ALONG_PENALTY, BlobWeightRow, FilterBounds, MAX_SUBPIXEL_OFFSET, Point, RidgeRow, SPAN_STEPS,
Sample, Segments, ridge_row_scalar, vertical_row_scalar_tail,
};
use std::arch::x86_64::*;
#[target_feature(enable = "avx2")]
pub(crate) fn sample_span_avx2(
controls: &[Point<f32>; 4],
distances: [f32; 3],
knots: [f32; 4],
out: &mut Vec<Point<f32>>,
) {
let [d0, d1, d2] = distances;
let sums = [_mm256_set1_ps(d0 + d1), _mm256_set1_ps(d1 + d2)];
let distances = distances.map(|d| _mm256_set1_ps(d));
let knots = knots.map(|t| _mm256_set1_ps(t));
let q = controls.map(|p| [_mm256_set1_ps(p.x), _mm256_set1_ps(p.y)]);
let mut points = [Point::default(); SPAN_STEPS.len()];
let lerp = |a: [__m256; 2], b: [__m256; 2], f| {
[
_mm256_add_ps(a[0], _mm256_mul_ps(f, _mm256_sub_ps(b[0], a[0]))),
_mm256_add_ps(a[1], _mm256_mul_ps(f, _mm256_sub_ps(b[1], a[1]))),
]
};
for (points, steps) in points
.as_chunks_mut::<8>()
.0
.iter_mut()
.zip(SPAN_STEPS.as_chunks::<8>().0)
{
let step = unsafe { _mm256_loadu_ps(steps.as_ptr()) };
let tt = _mm256_add_ps(distances[0], _mm256_mul_ps(step, distances[1]));
let dt0 = _mm256_sub_ps(tt, knots[0]);
let dt1 = _mm256_sub_ps(tt, knots[1]);
let dt2 = _mm256_sub_ps(tt, knots[2]);
let f1 = _mm256_div_ps(dt1, distances[1]);
let a0 = lerp(q[0], q[1], _mm256_div_ps(dt0, distances[0]));
let a1 = lerp(q[1], q[2], f1);
let a2 = lerp(q[2], q[3], _mm256_div_ps(dt2, distances[2]));
let b0 = lerp(a0, a1, _mm256_div_ps(dt0, sums[0]));
let b1 = lerp(a1, a2, _mm256_div_ps(dt1, sums[1]));
let [x, y] = lerp(b0, b1, f1);
let mut xs = [0.0; 8];
let mut ys = [0.0; 8];
unsafe {
_mm256_storeu_ps(xs.as_mut_ptr(), x);
_mm256_storeu_ps(ys.as_mut_ptr(), y);
}
for (point, (x, y)) in points.iter_mut().zip(xs.into_iter().zip(ys)) {
*point = Point { x, y };
}
}
out.extend_from_slice(&points[1..]);
}
#[inline]
#[target_feature(enable = "avx2")]
unsafe fn vertical_block<const VECTORS: usize>(
src: &[f32],
w: usize,
bounds: FilterBounds,
kernel: &[f32],
x: usize,
dst: &mut [[f32; 8]; VECTORS],
) {
let mut sums = [_mm256_setzero_ps(); VECTORS];
for (tap, &weight) in kernel.iter().enumerate() {
let start = bounds.index(tap) * w + x;
let weight = _mm256_set1_ps(weight);
let pixels = unsafe { src.get_unchecked(start..start + 8 * VECTORS) };
let (pixels, _) = pixels.as_chunks::<8>();
for (sum, pixels) in sums.iter_mut().zip(pixels) {
let pixels = unsafe { _mm256_loadu_ps(pixels.as_ptr()) };
*sum = _mm256_add_ps(*sum, _mm256_mul_ps(weight, pixels));
}
}
for (block, sum) in dst.iter_mut().zip(sums) {
unsafe { _mm256_storeu_ps(block.as_mut_ptr(), sum) };
}
}
#[target_feature(enable = "avx2")]
pub(crate) fn vertical_row_avx2(
src: &[f32],
w: usize,
bounds: FilterBounds,
kernel: &[f32],
dst: &mut [f32],
) {
if dst.is_empty() {
return;
}
assert!(dst.len() <= w && bounds.index(kernel.len().saturating_sub(1)) < src.len() / w);
let (blocks, tail) = dst.as_chunks_mut::<8>();
let (pairs, blocks) = blocks.as_chunks_mut::<2>();
let mut x = 0;
for pair in pairs {
unsafe { vertical_block(src, w, bounds, kernel, x, pair) };
x += 16;
}
for block in blocks {
unsafe { vertical_block(src, w, bounds, kernel, x, std::array::from_mut(block)) };
x += 8;
}
if !tail.is_empty() {
vertical_row_scalar_tail(src, w, bounds, kernel, x, tail);
}
}
#[target_feature(enable = "avx2")]
pub(crate) fn dot_contrast_avx2(
src: &[f32],
stride: usize,
dst: &mut [f32],
side: usize,
bg: f32,
slope: [f32; 2],
) {
let rows = crate::splines::dots::contrast_patch_rows(src, stride, dst, side);
let radius = (side / 2) as i32;
let dx = _mm256_sub_ps(
_mm256_setr_ps(0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0),
_mm256_set1_ps(radius as f32),
);
let x_term = _mm256_mul_ps(_mm256_set1_ps(slope[0]), dx);
let bg8 = _mm256_set1_ps(bg);
for (j, (src, dst)) in rows.enumerate() {
let y_term = slope[1] * (j as i32 - radius) as f32;
let y8 = _mm256_set1_ps(y_term);
let (src8, src) = src.as_chunks::<8>();
let (dst8, dst) = dst.as_chunks_mut::<8>();
if let ([src], [dst]) = (src8, dst8) {
unsafe {
let pixels = _mm256_loadu_ps(src.as_ptr());
let contrast = _mm256_sub_ps(_mm256_sub_ps(pixels, bg8), x_term);
_mm256_storeu_ps(dst.as_mut_ptr(), _mm256_sub_ps(contrast, y8));
}
}
let (src4, src) = src.as_chunks::<4>();
let (dst4, dst) = dst.as_chunks_mut::<4>();
if let ([src], [dst]) = (src4, dst4) {
unsafe {
let pixels = _mm_loadu_ps(src.as_ptr());
let contrast = _mm_sub_ps(
_mm_sub_ps(pixels, _mm256_castps256_ps128(bg8)),
_mm256_castps256_ps128(x_term),
);
_mm_storeu_ps(
dst.as_mut_ptr(),
_mm_sub_ps(contrast, _mm256_castps256_ps128(y8)),
);
}
}
let x0 = (side - dst.len()) as i32 - radius;
for ((out, &pixel), dx) in dst.iter_mut().zip(src).zip(x0..) {
*out = pixel - bg - slope[0] * dx as f32 - y_term;
}
}
}
#[inline]
#[target_feature(enable = "avx2")]
fn abs(x: __m256) -> __m256 {
_mm256_andnot_ps(_mm256_set1_ps(-0.0), x)
}
#[inline]
#[target_feature(enable = "avx2")]
fn neg(x: __m256) -> __m256 {
_mm256_xor_ps(x, _mm256_set1_ps(-0.0))
}
#[inline]
#[target_feature(enable = "avx2,fma")]
fn pack_bytes(x: __m256i) -> u64 {
let x = _mm_packs_epi32(_mm256_castsi256_si128(x), _mm256_extracti128_si256::<1>(x));
_mm_cvtsi128_si64(_mm_packs_epi16(x, x)) as u64
}
#[target_feature(enable = "avx2,fma")]
pub(crate) fn spline_ridge_row_avx2(s: f32, derivatives: [&[f32]; 5], output: RidgeRow<'_>) {
let n = output.validate(derivatives);
let end = n / 8 * 8;
let [hxx, hyy, hxy, gx, gy] = derivatives;
let s2 = s * s;
let zero = _mm256_setzero_ps();
let eps = _mm256_set1_ps(1e-12);
for i in (0..end).step_by(8) {
unsafe {
let xx = _mm256_loadu_ps(hxx.as_ptr().add(i));
let yy = _mm256_loadu_ps(hyy.as_ptr().add(i));
let xy = _mm256_loadu_ps(hxy.as_ptr().add(i));
let tr = _mm256_mul_ps(_mm256_add_ps(xx, yy), _mm256_set1_ps(0.5));
let half_diff = _mm256_mul_ps(_mm256_sub_ps(xx, yy), _mm256_set1_ps(0.5));
let df = _mm256_sqrt_ps(_mm256_add_ps(
_mm256_mul_ps(half_diff, half_diff),
_mm256_mul_ps(xy, xy),
));
let la = _mm256_add_ps(tr, df);
let lb = _mm256_sub_ps(tr, df);
let a_larger = _mm256_cmp_ps::<_CMP_GT_OQ>(abs(la), abs(lb));
let big = _mm256_blendv_ps(lb, la, a_larger);
let small = _mm256_blendv_ps(la, lb, a_larger);
let abs_big = abs(big);
let strength = _mm256_mul_ps(
_mm256_max_ps(
_mm256_sub_ps(
abs_big,
_mm256_mul_ps(abs(small), _mm256_set1_ps(ALONG_PENALTY)),
),
zero,
),
_mm256_set1_ps(s2),
);
let old_strength = _mm256_loadu_ps(output.strength.as_ptr().add(i));
let mut accept = _mm256_cmp_ps::<_CMP_NLE_UQ>(strength, old_strength);
if _mm256_movemask_ps(accept) == 0 {
continue;
}
let gx = _mm256_loadu_ps(gx.as_ptr().add(i));
let gy = _mm256_loadu_ps(gy.as_ptr().add(i));
let gradient =
_mm256_sqrt_ps(_mm256_add_ps(_mm256_mul_ps(gx, gx), _mm256_mul_ps(gy, gy)));
accept = _mm256_and_ps(
accept,
_mm256_cmp_ps::<_CMP_NGT_UQ>(
_mm256_mul_ps(gradient, _mm256_set1_ps(s)),
_mm256_mul_ps(abs_big, _mm256_set1_ps(s2)),
),
);
if _mm256_movemask_ps(accept) == 0 {
continue;
}
let vy = _mm256_sub_ps(big, xx);
let norm = _mm256_sqrt_ps(_mm256_add_ps(_mm256_mul_ps(xy, xy), _mm256_mul_ps(vy, vy)));
let degenerate = _mm256_cmp_ps::<_CMP_LT_OQ>(norm, eps);
let vx = _mm256_blendv_ps(_mm256_div_ps(xy, norm), _mm256_set1_ps(1.0), degenerate);
let vy = _mm256_blendv_ps(_mm256_div_ps(vy, norm), zero, degenerate);
let t = _mm256_div_ps(
neg(_mm256_add_ps(_mm256_mul_ps(gx, vx), _mm256_mul_ps(gy, vy))),
big,
);
let lower = _mm256_set1_ps(-MAX_SUBPIXEL_OFFSET);
let upper = _mm256_set1_ps(MAX_SUBPIXEL_OFFSET);
let t = _mm256_blendv_ps(t, lower, _mm256_cmp_ps::<_CMP_LT_OQ>(t, lower));
let t = _mm256_blendv_ps(t, upper, _mm256_cmp_ps::<_CMP_GT_OQ>(t, upper));
let t = _mm256_blendv_ps(t, zero, _mm256_cmp_ps::<_CMP_LT_OQ>(abs_big, eps));
let store = |row: &mut [f32], value| {
let ptr = row.as_mut_ptr().add(i);
_mm256_storeu_ps(ptr, _mm256_blendv_ps(_mm256_loadu_ps(ptr), value, accept));
};
store(output.strength, strength);
store(output.nx, vx);
store(output.ny, vy);
store(output.scale, _mm256_set1_ps(s));
store(output.offset, t);
let positive = _mm256_castps_si256(_mm256_cmp_ps::<_CMP_GT_OQ>(big, zero));
let polarity = pack_bytes(_mm256_or_si256(positive, _mm256_set1_epi32(1)));
let mask = pack_bytes(_mm256_castps_si256(accept));
let ptr = output.polarity.as_mut_ptr().add(i).cast::<u64>();
ptr.write_unaligned((ptr.read_unaligned() & !mask) | (polarity & mask));
}
}
ridge_row_scalar(
s,
derivatives.map(|row| &row[end..]),
RidgeRow {
strength: &mut output.strength[end..],
nx: &mut output.nx[end..],
ny: &mut output.ny[end..],
scale: &mut output.scale[end..],
polarity: &mut output.polarity[end..],
offset: &mut output.offset[end..],
},
);
}
#[inline]
#[target_feature(enable = "avx2,fma")]
fn fast_cos(x: __m256) -> __m256 {
use std::f32::consts::{PI, SQRT_2};
let pi2 = _mm256_set1_ps(PI * 2.0);
let periods = _mm256_floor_ps(_mm256_mul_ps(x, _mm256_set1_ps(0.5 / PI)));
let xmod = _mm256_sub_ps(x, _mm256_mul_ps(periods, pi2));
let x_pi = _mm256_min_ps(xmod, _mm256_sub_ps(pi2, xmod));
let above = _mm256_cmp_ps::<_CMP_GE_OQ>(x_pi, _mm256_set1_ps(PI / 2.0));
let xh = _mm256_blendv_ps(x_pi, _mm256_sub_ps(_mm256_set1_ps(PI), x_pi), above);
let xs = _mm256_mul_ps(xh, _mm256_set1_ps(0.25));
let x2 = _mm256_mul_ps(xs, xs);
let x4 = _mm256_mul_ps(x2, x2);
let pre = _mm256_add_ps(
_mm256_mul_ps(x4, _mm256_set1_ps(0.06960438)),
_mm256_add_ps(
_mm256_mul_ps(x2, _mm256_set1_ps(-0.84087373)),
_mm256_set1_ps(1.68179268),
),
);
let s1 = _mm256_sub_ps(_mm256_mul_ps(pre, pre), _mm256_set1_ps(SQRT_2));
let s2 = _mm256_sub_ps(_mm256_mul_ps(s1, s1), _mm256_set1_ps(1.0));
_mm256_blendv_ps(s2, neg(s2), above)
}
#[target_feature(enable = "avx2,fma")]
pub(crate) fn continuous_idct_avx2(dct: &[[f32; 4]], t: f32) -> [f32; 4] {
if dct.len() == 32 {
continuous_idct_impl(&dct[..32], t)
} else {
continuous_idct_impl(dct, t)
}
}
#[inline]
#[target_feature(enable = "avx2,fma")]
fn continuous_idct_impl(dct: &[[f32; 4]], t: f32) -> [f32; 4] {
let mut result = _mm_setzero_ps();
let mut indices = _mm256_setr_ps(0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0);
let cosine = |indices| {
let args = _mm256_mul_ps(
_mm256_mul_ps(indices, _mm256_set1_ps(std::f32::consts::PI / 32.0)),
_mm256_set1_ps(t + 0.5),
);
fast_cos(args)
};
let mut accumulate_pairs = |rows: &[[[f32; 4]; 2]], cos| {
for (i, pair) in rows.iter().enumerate() {
let lo = (i * 2) as i32;
let hi = lo + 1;
let cosines =
_mm256_permutevar8x32_ps(cos, _mm256_setr_epi32(lo, lo, lo, lo, hi, hi, hi, hi));
let values = unsafe { _mm256_loadu_ps(pair.as_ptr().cast::<f32>()) };
let product = _mm256_mul_ps(values, cosines);
result = _mm_add_ps(result, _mm256_castps256_ps128(product));
result = _mm_add_ps(result, _mm256_extractf128_ps::<1>(product));
}
};
let (blocks, tail) = dct.as_chunks::<8>();
for rows in blocks {
accumulate_pairs(rows.as_chunks::<2>().0, cosine(indices));
indices = _mm256_add_ps(indices, _mm256_set1_ps(8.0));
}
if !tail.is_empty() {
let cos = cosine(indices);
let (pairs, remainder) = tail.as_chunks::<2>();
accumulate_pairs(pairs, cos);
if let [row] = remainder {
let cos = _mm256_permutevar8x32_ps(cos, _mm256_set1_epi32((pairs.len() * 2) as i32));
let values = unsafe { _mm_loadu_ps(row.as_ptr()) };
result = _mm_add_ps(result, _mm_mul_ps(values, _mm256_castps256_ps128(cos)));
}
}
let mut out = [0.0; 4];
unsafe { _mm_storeu_ps(out.as_mut_ptr(), result) };
out
}
#[target_feature(enable = "avx2,fma")]
pub(crate) fn spline_distance_avx2(p: Point<f32>, blocks: &[Segments]) -> f32 {
let (px, py) = (_mm256_set1_ps(p.x), _mm256_set1_ps(p.y));
let zero = _mm256_setzero_ps();
let one = _mm256_set1_ps(1.0);
let mut best = _mm256_set1_ps(f32::INFINITY);
for pair in blocks.chunks(2) {
let a = &pair[0];
let b = pair.get(1).unwrap_or(a);
unsafe {
let join = |a: &[f32; 4], b: &[f32; 4]| {
_mm256_insertf128_ps::<1>(
_mm256_castps128_ps256(_mm_loadu_ps(a.as_ptr())),
_mm_loadu_ps(b.as_ptr()),
)
};
let (x, y, dx, dy, len2) = (
join(&a.x, &b.x),
join(&a.y, &b.y),
join(&a.dx, &b.dx),
join(&a.dy, &b.dy),
join(&a.len2, &b.len2),
);
let nonzero = _mm256_cmp_ps::<_CMP_GT_OQ>(len2, _mm256_set1_ps(1e-12));
let numerator = _mm256_add_ps(
_mm256_mul_ps(_mm256_sub_ps(px, x), dx),
_mm256_mul_ps(_mm256_sub_ps(py, y), dy),
);
let t = _mm256_div_ps(numerator, _mm256_blendv_ps(one, len2, nonzero));
let t = _mm256_blendv_ps(t, zero, _mm256_cmp_ps::<_CMP_LT_OQ>(t, zero));
let t = _mm256_blendv_ps(t, one, _mm256_cmp_ps::<_CMP_GT_OQ>(t, one));
let t = _mm256_and_ps(t, nonzero);
let ex = _mm256_sub_ps(px, _mm256_add_ps(x, _mm256_mul_ps(t, dx)));
let ey = _mm256_sub_ps(py, _mm256_add_ps(y, _mm256_mul_ps(t, dy)));
let distance = _mm256_add_ps(_mm256_mul_ps(ex, ex), _mm256_mul_ps(ey, ey));
best = _mm256_min_ps(distance, best);
}
}
let best = _mm_min_ps(
_mm256_castps256_ps128(best),
_mm256_extractf128_ps::<1>(best),
);
let best = _mm_min_ps(best, _mm_movehl_ps(best, best));
_mm_cvtss_f32(_mm_min_ss(best, _mm_shuffle_ps::<0x55>(best, best)))
}
#[inline]
#[target_feature(enable = "avx2")]
fn horner_step(a: __m256, b: __m256, c: __m256) -> __m256 {
_mm256_add_ps(_mm256_mul_ps(a, b), c)
}
#[inline]
#[target_feature(enable = "avx2")]
fn erf(x: __m256, step: impl Fn(__m256, __m256, __m256) -> __m256) -> __m256 {
let a = abs(x);
let d1 = step(
a,
_mm256_set1_ps(7.77394369e-02),
_mm256_set1_ps(2.05260015e-04),
);
let d2 = step(d1, a, _mm256_set1_ps(2.32120216e-01));
let d3 = step(d2, a, _mm256_set1_ps(2.77820801e-01));
let d4 = step(d3, a, _mm256_set1_ps(1.0));
let d5 = _mm256_mul_ps(d4, d4);
let inv = _mm256_div_ps(_mm256_set1_ps(1.0), d5);
let r = _mm256_sub_ps(_mm256_set1_ps(1.0), _mm256_mul_ps(inv, inv));
_mm256_blendv_ps(
r,
neg(r),
_mm256_cmp_ps::<_CMP_LE_OQ>(x, _mm256_setzero_ps()),
)
}
#[inline]
#[target_feature(enable = "avx2")]
fn dx_lanes(x: i32, center: f32) -> __m256 {
debug_assert!(x <= i32::MAX - 7);
let indices = _mm256_cvtepi32_ps(_mm256_add_epi32(
_mm256_set1_epi32(x),
_mm256_setr_epi32(0, 1, 2, 3, 4, 5, 6, 7),
));
_mm256_sub_ps(indices, _mm256_set1_ps(center))
}
#[inline]
#[target_feature(enable = "avx2")]
fn blob_weights(
dx_squared: __m256,
dy_squared: f32,
inv_sigma: __m256,
amp: __m256,
step: impl Fn(__m256, __m256, __m256) -> __m256 + Copy,
) -> __m256 {
let dist = _mm256_sqrt_ps(_mm256_add_ps(dx_squared, _mm256_set1_ps(dy_squared)));
let half = _mm256_mul_ps(dist, _mm256_set1_ps(0.5));
let a = _mm256_mul_ps(_mm256_add_ps(half, _mm256_set1_ps(0.353_553_39)), inv_sigma);
let b = _mm256_mul_ps(_mm256_sub_ps(half, _mm256_set1_ps(0.353_553_39)), inv_sigma);
let e = _mm256_sub_ps(erf(a, step), erf(b, step));
_mm256_mul_ps(_mm256_mul_ps(e, amp), e)
}
#[inline]
#[target_feature(enable = "avx2")]
fn intensity(sample: &Sample, dy: f32, x: usize, amp: f32) -> __m256 {
debug_assert!(x <= i32::MAX as usize - 7);
let dx = dx_lanes(x as i32, sample.position.x);
blob_weights(
_mm256_mul_ps(dx, dx),
dy * dy,
_mm256_set1_ps(sample.inv_sigma),
_mm256_set1_ps(amp),
|a, b, c| horner_step(a, b, c),
)
}
#[inline]
#[target_feature(enable = "avx2,fma")]
fn row_weights(row: BlobWeightRow, x: i32) -> __m256 {
let dx = _mm256_mul_ps(dx_lanes(x, row.center), _mm256_set1_ps(row.step));
blob_weights(
_mm256_mul_ps(dx, dx),
row.dy * row.dy,
_mm256_set1_ps(row.inv_sigma),
_mm256_set1_ps(row.amp),
|a, b, c| _mm256_fmadd_ps(a, b, c),
)
}
#[target_feature(enable = "avx2,fma")]
pub(crate) fn blob_weight_row_avx2(row: BlobWeightRow, x0: i32, weights: &mut [f32]) {
let (blocks, tail) = weights.as_chunks_mut::<8>();
for (i, block) in blocks.iter_mut().enumerate() {
unsafe { _mm256_storeu_ps(block.as_mut_ptr(), row_weights(row, x0 + 8 * i as i32)) };
}
if !tail.is_empty() {
let mut padded = [0.0; 8];
unsafe {
_mm256_storeu_ps(
padded.as_mut_ptr(),
row_weights(row, x0 + 8 * blocks.len() as i32),
)
};
tail.copy_from_slice(&padded[..tail.len()]);
}
}
#[target_feature(enable = "avx2")]
pub(crate) fn spline_render_row_avx2(
sample: &Sample,
dy: f32,
x0: usize,
rows: [&mut [f32]; 3],
amp: f32,
) {
let [rx, ry, rb] = rows;
assert_eq!(rx.len(), ry.len());
assert_eq!(rx.len(), rb.len());
let [(rx, tx), (ry, ty), (rb, tb)] = [rx, ry, rb].map(|row| row.as_chunks_mut::<8>());
let n = rx.len() * 8;
for (i, ((rx, ry), rb)) in rx.iter_mut().zip(ry).zip(rb).enumerate() {
let li = intensity(sample, dy, x0 + i * 8, amp);
for (block, color) in [rx, ry, rb].into_iter().zip(sample.color) {
unsafe {
let ptr = block.as_mut_ptr();
_mm256_storeu_ps(
ptr,
_mm256_add_ps(
_mm256_loadu_ps(ptr),
_mm256_mul_ps(li, _mm256_set1_ps(color)),
),
);
}
}
}
if tx.len() >= 2 {
let mut weights = [0.0; 8];
unsafe { _mm256_storeu_ps(weights.as_mut_ptr(), intensity(sample, dy, x0 + n, amp)) };
for (tail, color) in [tx, ty, tb].into_iter().zip(sample.color) {
for (out, li) in tail.iter_mut().zip(weights) {
*out += color * li;
}
}
} else {
sample.scalar_row(dy, x0 + n, [tx, ty, tb], amp);
}
}
#[target_feature(enable = "avx2")]
pub(crate) fn spline_render_tile_avx2(
sample: &Sample,
tx: usize,
ty: usize,
(x0, x1, y0, y1): (usize, usize, usize, usize),
pixels: &mut [[f32; 64]; 3],
sign: f32,
) {
debug_assert!(x0 >= tx && x0 < x1 && x1 <= tx + 8);
debug_assert!(y0 >= ty && y0 < y1 && y1 <= ty + 8);
let first = (x0 - tx) as i32;
let end = (x1 - tx) as i32;
let columns = _mm256_setr_epi32(0, 1, 2, 3, 4, 5, 6, 7);
let mask = _mm256_castsi256_ps(_mm256_and_si256(
_mm256_cmpgt_epi32(columns, _mm256_set1_epi32(first - 1)),
_mm256_cmpgt_epi32(_mm256_set1_epi32(end), columns),
));
let clipped = first != 0 || end != 8;
let dx = dx_lanes(tx as i32, sample.position.x);
let dx_squared = _mm256_mul_ps(dx, dx);
let inv_sigma = _mm256_set1_ps(sample.inv_sigma);
let amp = _mm256_set1_ps(sample.amp * sign);
let colors = sample.color.map(|color| _mm256_set1_ps(color));
let [rx, ry, rb] = pixels;
let [rx, ry, rb] = [rx, ry, rb].map(|plane| plane.as_chunks_mut::<8>().0);
for y in y0..y1 {
let dy = y as f32 - sample.position.y;
let li = blob_weights(dx_squared, dy * dy, inv_sigma, amp, |a, b, c| {
horner_step(a, b, c)
});
let row = y - ty;
for (pixels, color) in [&mut rx[row], &mut ry[row], &mut rb[row]]
.into_iter()
.zip(colors)
{
unsafe {
let ptr = pixels.as_mut_ptr();
let original = _mm256_loadu_ps(ptr);
let drawn = _mm256_add_ps(original, _mm256_mul_ps(color, li));
let out = if clipped {
_mm256_blendv_ps(original, drawn, mask)
} else {
drawn
};
_mm256_storeu_ps(ptr, out);
}
}
}
}