Loading...
Searching...
No Matches
volk_32f_sincos_32f_x2.h
Go to the documentation of this file.
/* -*- c++ -*- */
/*
* Copyright 2025 Magnus Lundmark <magnuslundmark@gmail.com>
*
* This file is part of VOLK
*
* SPDX-License-Identifier: LGPL-3.0-or-later
*/
#include <inttypes.h>
#include <math.h>
#include <stdio.h>
#ifndef INCLUDED_volk_32f_sincos_32f_x2_a_H
#define INCLUDED_volk_32f_sincos_32f_x2_a_H
#ifdef LV_HAVE_GENERIC
static inline void volk_32f_sincos_32f_x2_generic(float* sinVector,
float* cosVector,
const float* inVector,
unsigned int num_points)
{
for (unsigned int i = 0; i < num_points; i++) {
sinVector[i] = sinf(inVector[i]);
cosVector[i] = cosf(inVector[i]);
}
}
#endif /* LV_HAVE_GENERIC */
#ifdef LV_HAVE_GENERIC
#include <volk/volk_common.h>
static inline void volk_32f_sincos_32f_x2_polynomial(float* sinVector,
float* cosVector,
const float* inVector,
unsigned int num_points)
{
/*
* Cody-Waite argument reduction with shared sin/cos polynomial evaluation
*/
const float two_over_pi = 0x1.45f306p-1f;
const float pi_over_2_hi = 0x1.921fb6p+0f;
const float pi_over_2_lo = -0x1.777a5cp-25f;
for (unsigned int i = 0; i < num_points; i++) {
float x = inVector[i];
float n_f = rintf(x * two_over_pi);
int n = (int)n_f;
float r = fmaf(-n_f, pi_over_2_hi, x);
r = fmaf(-n_f, pi_over_2_lo, r);
float sin_r = volk_sin_poly(r);
float cos_r = volk_cos_poly(r);
// Sin: n&1 swaps, n&2 negates
float sin_result = (n & 1) ? cos_r : sin_r;
sinVector[i] = (n & 2) ? -sin_result : sin_result;
// Cos: n&1 swaps, (n+1)&2 negates
float cos_result = (n & 1) ? sin_r : cos_r;
cosVector[i] = ((n + 1) & 2) ? -cos_result : cos_result;
}
}
#endif /* LV_HAVE_GENERIC */
#ifdef LV_HAVE_AVX512F
#include <immintrin.h>
#include <volk/volk_avx512_intrinsics.h>
static inline void volk_32f_sincos_32f_x2_a_avx512f(float* sinVector,
float* cosVector,
const float* inVector,
unsigned int num_points)
{
float* sinPtr = sinVector;
float* cosPtr = cosVector;
const float* inPtr = inVector;
unsigned int number = 0;
unsigned int sixteenPoints = num_points / 16;
// Constants for Cody-Waite argument reduction
const __m512 two_over_pi = _mm512_set1_ps(0x1.45f306p-1f);
const __m512 pi_over_2_hi = _mm512_set1_ps(0x1.921fb6p+0f);
const __m512 pi_over_2_lo = _mm512_set1_ps(-0x1.777a5cp-25f);
const __m512i ones = _mm512_set1_epi32(1);
const __m512i twos = _mm512_set1_epi32(2);
const __m512i sign_bit = _mm512_set1_epi32(0x80000000);
for (; number < sixteenPoints; number++) {
__m512 x = _mm512_load_ps(inPtr);
// Argument reduction: n = round(x * 2/pi)
__m512 n_f = _mm512_roundscale_ps(_mm512_mul_ps(x, two_over_pi),
_MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
__m512i n = _mm512_cvtps_epi32(n_f);
// r = x - n * (pi/2)
__m512 r = _mm512_fnmadd_ps(n_f, pi_over_2_hi, x);
r = _mm512_fnmadd_ps(n_f, pi_over_2_lo, r);
// Evaluate both sin and cos polynomials
__m512 sin_r = _mm512_sin_poly_avx512(r);
__m512 cos_r = _mm512_cos_poly_avx512(r);
// Quadrant selection
__m512i n_and_1 = _mm512_and_si512(n, ones);
__m512i n_and_2 = _mm512_and_si512(n, twos);
__m512i n_plus_1_and_2 = _mm512_and_si512(_mm512_add_epi32(n, ones), twos);
// For sin: swap when n&1, negate when n&2
__mmask16 sin_swap = _mm512_cmpeq_epi32_mask(n_and_1, ones);
__m512 sin_result = _mm512_mask_blend_ps(sin_swap, sin_r, cos_r);
__mmask16 sin_neg = _mm512_cmpeq_epi32_mask(n_and_2, twos);
sin_result =
_mm512_castsi512_ps(_mm512_mask_xor_epi32(_mm512_castps_si512(sin_result),
sin_neg,
_mm512_castps_si512(sin_result),
sign_bit));
// For cos: swap when n&1, negate when (n+1)&2
__mmask16 cos_swap = sin_swap;
__m512 cos_result = _mm512_mask_blend_ps(cos_swap, cos_r, sin_r);
__mmask16 cos_neg = _mm512_cmpeq_epi32_mask(n_plus_1_and_2, twos);
cos_result =
_mm512_castsi512_ps(_mm512_mask_xor_epi32(_mm512_castps_si512(cos_result),
cos_neg,
_mm512_castps_si512(cos_result),
sign_bit));
_mm512_store_ps(sinPtr, sin_result);
_mm512_store_ps(cosPtr, cos_result);
inPtr += 16;
sinPtr += 16;
cosPtr += 16;
}
number = sixteenPoints * 16;
for (; number < num_points; number++) {
*sinPtr++ = sinf(*inPtr);
*cosPtr++ = cosf(*inPtr++);
}
}
#endif /* LV_HAVE_AVX512F for aligned */
#if LV_HAVE_AVX2 && LV_HAVE_FMA
#include <immintrin.h>
#include <volk/volk_avx2_fma_intrinsics.h>
static inline void volk_32f_sincos_32f_x2_a_avx2_fma(float* sinVector,
float* cosVector,
const float* inVector,
unsigned int num_points)
{
float* sinPtr = sinVector;
float* cosPtr = cosVector;
const float* inPtr = inVector;
unsigned int number = 0;
unsigned int eighthPoints = num_points / 8;
// Constants for Cody-Waite argument reduction
const __m256 two_over_pi = _mm256_set1_ps(0x1.45f306p-1f);
const __m256 pi_over_2_hi = _mm256_set1_ps(0x1.921fb6p+0f);
const __m256 pi_over_2_lo = _mm256_set1_ps(-0x1.777a5cp-25f);
const __m256i ones = _mm256_set1_epi32(1);
const __m256i twos = _mm256_set1_epi32(2);
const __m256 sign_bit = _mm256_set1_ps(-0.0f);
for (; number < eighthPoints; number++) {
__m256 x = _mm256_load_ps(inPtr);
// Argument reduction: n = round(x * 2/pi)
__m256 n_f = _mm256_round_ps(_mm256_mul_ps(x, two_over_pi),
_MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
__m256i n = _mm256_cvtps_epi32(n_f);
// r = x - n * (pi/2)
__m256 r = _mm256_fnmadd_ps(n_f, pi_over_2_hi, x);
r = _mm256_fnmadd_ps(n_f, pi_over_2_lo, r);
// Evaluate both sin and cos polynomials
__m256 sin_r = _mm256_sin_poly_avx2_fma(r);
__m256 cos_r = _mm256_cos_poly_avx2_fma(r);
// Quadrant selection
__m256i n_and_1 = _mm256_and_si256(n, ones);
__m256i n_and_2 = _mm256_and_si256(n, twos);
__m256i n_plus_1_and_2 = _mm256_and_si256(_mm256_add_epi32(n, ones), twos);
// For sin: swap when n&1, negate when n&2
__m256 sin_swap = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_and_1, ones));
__m256 sin_result = _mm256_blendv_ps(sin_r, cos_r, sin_swap);
__m256 sin_neg = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_and_2, twos));
sin_result = _mm256_xor_ps(sin_result, _mm256_and_ps(sin_neg, sign_bit));
// For cos: swap when n&1, negate when (n+1)&2
__m256 cos_result = _mm256_blendv_ps(cos_r, sin_r, sin_swap);
__m256 cos_neg = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_plus_1_and_2, twos));
cos_result = _mm256_xor_ps(cos_result, _mm256_and_ps(cos_neg, sign_bit));
_mm256_store_ps(sinPtr, sin_result);
_mm256_store_ps(cosPtr, cos_result);
inPtr += 8;
sinPtr += 8;
cosPtr += 8;
}
number = eighthPoints * 8;
for (; number < num_points; number++) {
*sinPtr++ = sinf(*inPtr);
*cosPtr++ = cosf(*inPtr++);
}
}
#endif /* LV_HAVE_AVX2 && LV_HAVE_FMA for aligned */
#ifdef LV_HAVE_AVX2
#include <immintrin.h>
#include <volk/volk_avx2_intrinsics.h>
static inline void volk_32f_sincos_32f_x2_a_avx2(float* sinVector,
float* cosVector,
const float* inVector,
unsigned int num_points)
{
float* sinPtr = sinVector;
float* cosPtr = cosVector;
const float* inPtr = inVector;
unsigned int number = 0;
unsigned int eighthPoints = num_points / 8;
// Constants for Cody-Waite argument reduction
const __m256 two_over_pi = _mm256_set1_ps(0x1.45f306p-1f);
const __m256 pi_over_2_hi = _mm256_set1_ps(0x1.921fb6p+0f);
const __m256 pi_over_2_lo = _mm256_set1_ps(-0x1.777a5cp-25f);
const __m256i ones = _mm256_set1_epi32(1);
const __m256i twos = _mm256_set1_epi32(2);
const __m256 sign_bit = _mm256_set1_ps(-0.0f);
for (; number < eighthPoints; number++) {
__m256 x = _mm256_load_ps(inPtr);
// Argument reduction: n = round(x * 2/pi)
__m256 n_f = _mm256_round_ps(_mm256_mul_ps(x, two_over_pi),
_MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
__m256i n = _mm256_cvtps_epi32(n_f);
// r = x - n * (pi/2)
__m256 r = _mm256_sub_ps(x, _mm256_mul_ps(n_f, pi_over_2_hi));
r = _mm256_sub_ps(r, _mm256_mul_ps(n_f, pi_over_2_lo));
// Evaluate both sin and cos polynomials
__m256 sin_r = _mm256_sin_poly_avx2(r);
__m256 cos_r = _mm256_cos_poly_avx2(r);
// Quadrant selection
__m256i n_and_1 = _mm256_and_si256(n, ones);
__m256i n_and_2 = _mm256_and_si256(n, twos);
__m256i n_plus_1_and_2 = _mm256_and_si256(_mm256_add_epi32(n, ones), twos);
// For sin: swap when n&1, negate when n&2
__m256 sin_swap = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_and_1, ones));
__m256 sin_result = _mm256_blendv_ps(sin_r, cos_r, sin_swap);
__m256 sin_neg = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_and_2, twos));
sin_result = _mm256_xor_ps(sin_result, _mm256_and_ps(sin_neg, sign_bit));
// For cos: swap when n&1, negate when (n+1)&2
__m256 cos_result = _mm256_blendv_ps(cos_r, sin_r, sin_swap);
__m256 cos_neg = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_plus_1_and_2, twos));
cos_result = _mm256_xor_ps(cos_result, _mm256_and_ps(cos_neg, sign_bit));
_mm256_store_ps(sinPtr, sin_result);
_mm256_store_ps(cosPtr, cos_result);
inPtr += 8;
sinPtr += 8;
cosPtr += 8;
}
number = eighthPoints * 8;
for (; number < num_points; number++) {
*sinPtr++ = sinf(*inPtr);
*cosPtr++ = cosf(*inPtr++);
}
}
#endif /* LV_HAVE_AVX2 for aligned */
#ifdef LV_HAVE_SSE4_1
#include <smmintrin.h>
#include <volk/volk_sse_intrinsics.h>
static inline void volk_32f_sincos_32f_x2_a_sse4_1(float* sinVector,
float* cosVector,
const float* inVector,
unsigned int num_points)
{
float* sinPtr = sinVector;
float* cosPtr = cosVector;
const float* inPtr = inVector;
unsigned int number = 0;
unsigned int quarterPoints = num_points / 4;
// Constants for Cody-Waite argument reduction
const __m128 two_over_pi = _mm_set1_ps(0x1.45f306p-1f);
const __m128 pi_over_2_hi = _mm_set1_ps(0x1.921fb6p+0f);
const __m128 pi_over_2_lo = _mm_set1_ps(-0x1.777a5cp-25f);
const __m128i ones = _mm_set1_epi32(1);
const __m128i twos = _mm_set1_epi32(2);
const __m128 sign_bit = _mm_set1_ps(-0.0f);
for (; number < quarterPoints; number++) {
__m128 x = _mm_load_ps(inPtr);
// Argument reduction: n = round(x * 2/pi)
__m128 n_f = _mm_round_ps(_mm_mul_ps(x, two_over_pi),
_MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
__m128i n = _mm_cvtps_epi32(n_f);
// r = x - n * (pi/2)
__m128 r = _mm_sub_ps(x, _mm_mul_ps(n_f, pi_over_2_hi));
r = _mm_sub_ps(r, _mm_mul_ps(n_f, pi_over_2_lo));
// Evaluate both sin and cos polynomials
__m128 sin_r = _mm_sin_poly_sse(r);
__m128 cos_r = _mm_cos_poly_sse(r);
// Quadrant selection
__m128i n_and_1 = _mm_and_si128(n, ones);
__m128i n_and_2 = _mm_and_si128(n, twos);
__m128i n_plus_1_and_2 = _mm_and_si128(_mm_add_epi32(n, ones), twos);
// For sin: swap when n&1, negate when n&2
__m128 sin_swap = _mm_castsi128_ps(_mm_cmpeq_epi32(n_and_1, ones));
__m128 sin_result = _mm_blendv_ps(sin_r, cos_r, sin_swap);
__m128 sin_neg = _mm_castsi128_ps(_mm_cmpeq_epi32(n_and_2, twos));
sin_result = _mm_xor_ps(sin_result, _mm_and_ps(sin_neg, sign_bit));
// For cos: swap when n&1, negate when (n+1)&2
__m128 cos_result = _mm_blendv_ps(cos_r, sin_r, sin_swap);
__m128 cos_neg = _mm_castsi128_ps(_mm_cmpeq_epi32(n_plus_1_and_2, twos));
cos_result = _mm_xor_ps(cos_result, _mm_and_ps(cos_neg, sign_bit));
_mm_store_ps(sinPtr, sin_result);
_mm_store_ps(cosPtr, cos_result);
inPtr += 4;
sinPtr += 4;
cosPtr += 4;
}
number = quarterPoints * 4;
for (; number < num_points; number++) {
*sinPtr++ = sinf(*inPtr);
*cosPtr++ = cosf(*inPtr++);
}
}
#endif /* LV_HAVE_SSE4_1 for aligned */
#endif /* INCLUDED_volk_32f_sincos_32f_x2_a_H */
#ifndef INCLUDED_volk_32f_sincos_32f_x2_u_H
#define INCLUDED_volk_32f_sincos_32f_x2_u_H
#ifdef LV_HAVE_AVX512F
#include <immintrin.h>
#include <volk/volk_avx512_intrinsics.h>
static inline void volk_32f_sincos_32f_x2_u_avx512f(float* sinVector,
float* cosVector,
const float* inVector,
unsigned int num_points)
{
float* sinPtr = sinVector;
float* cosPtr = cosVector;
const float* inPtr = inVector;
unsigned int number = 0;
unsigned int sixteenPoints = num_points / 16;
// Constants for Cody-Waite argument reduction
const __m512 two_over_pi = _mm512_set1_ps(0x1.45f306p-1f);
const __m512 pi_over_2_hi = _mm512_set1_ps(0x1.921fb6p+0f);
const __m512 pi_over_2_lo = _mm512_set1_ps(-0x1.777a5cp-25f);
const __m512i ones = _mm512_set1_epi32(1);
const __m512i twos = _mm512_set1_epi32(2);
const __m512i sign_bit = _mm512_set1_epi32(0x80000000);
for (; number < sixteenPoints; number++) {
__m512 x = _mm512_loadu_ps(inPtr);
// Argument reduction: n = round(x * 2/pi)
__m512 n_f = _mm512_roundscale_ps(_mm512_mul_ps(x, two_over_pi),
_MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
__m512i n = _mm512_cvtps_epi32(n_f);
// r = x - n * (pi/2)
__m512 r = _mm512_fnmadd_ps(n_f, pi_over_2_hi, x);
r = _mm512_fnmadd_ps(n_f, pi_over_2_lo, r);
// Evaluate both sin and cos polynomials
__m512 sin_r = _mm512_sin_poly_avx512(r);
__m512 cos_r = _mm512_cos_poly_avx512(r);
// Quadrant selection
__m512i n_and_1 = _mm512_and_si512(n, ones);
__m512i n_and_2 = _mm512_and_si512(n, twos);
__m512i n_plus_1_and_2 = _mm512_and_si512(_mm512_add_epi32(n, ones), twos);
// For sin: swap when n&1, negate when n&2
__mmask16 sin_swap = _mm512_cmpeq_epi32_mask(n_and_1, ones);
__m512 sin_result = _mm512_mask_blend_ps(sin_swap, sin_r, cos_r);
__mmask16 sin_neg = _mm512_cmpeq_epi32_mask(n_and_2, twos);
sin_result =
_mm512_castsi512_ps(_mm512_mask_xor_epi32(_mm512_castps_si512(sin_result),
sin_neg,
_mm512_castps_si512(sin_result),
sign_bit));
// For cos: swap when n&1, negate when (n+1)&2
__mmask16 cos_swap = sin_swap;
__m512 cos_result = _mm512_mask_blend_ps(cos_swap, cos_r, sin_r);
__mmask16 cos_neg = _mm512_cmpeq_epi32_mask(n_plus_1_and_2, twos);
cos_result =
_mm512_castsi512_ps(_mm512_mask_xor_epi32(_mm512_castps_si512(cos_result),
cos_neg,
_mm512_castps_si512(cos_result),
sign_bit));
_mm512_storeu_ps(sinPtr, sin_result);
_mm512_storeu_ps(cosPtr, cos_result);
inPtr += 16;
sinPtr += 16;
cosPtr += 16;
}
number = sixteenPoints * 16;
for (; number < num_points; number++) {
*sinPtr++ = sinf(*inPtr);
*cosPtr++ = cosf(*inPtr++);
}
}
#endif /* LV_HAVE_AVX512F for unaligned */
#if LV_HAVE_AVX2 && LV_HAVE_FMA
#include <immintrin.h>
#include <volk/volk_avx2_fma_intrinsics.h>
static inline void volk_32f_sincos_32f_x2_u_avx2_fma(float* sinVector,
float* cosVector,
const float* inVector,
unsigned int num_points)
{
float* sinPtr = sinVector;
float* cosPtr = cosVector;
const float* inPtr = inVector;
unsigned int number = 0;
unsigned int eighthPoints = num_points / 8;
// Constants for Cody-Waite argument reduction
const __m256 two_over_pi = _mm256_set1_ps(0x1.45f306p-1f);
const __m256 pi_over_2_hi = _mm256_set1_ps(0x1.921fb6p+0f);
const __m256 pi_over_2_lo = _mm256_set1_ps(-0x1.777a5cp-25f);
const __m256i ones = _mm256_set1_epi32(1);
const __m256i twos = _mm256_set1_epi32(2);
const __m256 sign_bit = _mm256_set1_ps(-0.0f);
for (; number < eighthPoints; number++) {
__m256 x = _mm256_loadu_ps(inPtr);
// Argument reduction: n = round(x * 2/pi)
__m256 n_f = _mm256_round_ps(_mm256_mul_ps(x, two_over_pi),
_MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
__m256i n = _mm256_cvtps_epi32(n_f);
// r = x - n * (pi/2)
__m256 r = _mm256_fnmadd_ps(n_f, pi_over_2_hi, x);
r = _mm256_fnmadd_ps(n_f, pi_over_2_lo, r);
// Evaluate both sin and cos polynomials
__m256 sin_r = _mm256_sin_poly_avx2_fma(r);
__m256 cos_r = _mm256_cos_poly_avx2_fma(r);
// Quadrant selection
__m256i n_and_1 = _mm256_and_si256(n, ones);
__m256i n_and_2 = _mm256_and_si256(n, twos);
__m256i n_plus_1_and_2 = _mm256_and_si256(_mm256_add_epi32(n, ones), twos);
// For sin: swap when n&1, negate when n&2
__m256 sin_swap = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_and_1, ones));
__m256 sin_result = _mm256_blendv_ps(sin_r, cos_r, sin_swap);
__m256 sin_neg = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_and_2, twos));
sin_result = _mm256_xor_ps(sin_result, _mm256_and_ps(sin_neg, sign_bit));
// For cos: swap when n&1, negate when (n+1)&2
__m256 cos_result = _mm256_blendv_ps(cos_r, sin_r, sin_swap);
__m256 cos_neg = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_plus_1_and_2, twos));
cos_result = _mm256_xor_ps(cos_result, _mm256_and_ps(cos_neg, sign_bit));
_mm256_storeu_ps(sinPtr, sin_result);
_mm256_storeu_ps(cosPtr, cos_result);
inPtr += 8;
sinPtr += 8;
cosPtr += 8;
}
number = eighthPoints * 8;
for (; number < num_points; number++) {
*sinPtr++ = sinf(*inPtr);
*cosPtr++ = cosf(*inPtr++);
}
}
#endif /* LV_HAVE_AVX2 && LV_HAVE_FMA for unaligned */
#ifdef LV_HAVE_AVX2
#include <immintrin.h>
#include <volk/volk_avx2_intrinsics.h>
static inline void volk_32f_sincos_32f_x2_u_avx2(float* sinVector,
float* cosVector,
const float* inVector,
unsigned int num_points)
{
float* sinPtr = sinVector;
float* cosPtr = cosVector;
const float* inPtr = inVector;
unsigned int number = 0;
unsigned int eighthPoints = num_points / 8;
// Constants for Cody-Waite argument reduction
const __m256 two_over_pi = _mm256_set1_ps(0x1.45f306p-1f);
const __m256 pi_over_2_hi = _mm256_set1_ps(0x1.921fb6p+0f);
const __m256 pi_over_2_lo = _mm256_set1_ps(-0x1.777a5cp-25f);
const __m256i ones = _mm256_set1_epi32(1);
const __m256i twos = _mm256_set1_epi32(2);
const __m256 sign_bit = _mm256_set1_ps(-0.0f);
for (; number < eighthPoints; number++) {
__m256 x = _mm256_loadu_ps(inPtr);
// Argument reduction: n = round(x * 2/pi)
__m256 n_f = _mm256_round_ps(_mm256_mul_ps(x, two_over_pi),
_MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
__m256i n = _mm256_cvtps_epi32(n_f);
// r = x - n * (pi/2)
__m256 r = _mm256_sub_ps(x, _mm256_mul_ps(n_f, pi_over_2_hi));
r = _mm256_sub_ps(r, _mm256_mul_ps(n_f, pi_over_2_lo));
// Evaluate both sin and cos polynomials
__m256 sin_r = _mm256_sin_poly_avx2(r);
__m256 cos_r = _mm256_cos_poly_avx2(r);
// Quadrant selection
__m256i n_and_1 = _mm256_and_si256(n, ones);
__m256i n_and_2 = _mm256_and_si256(n, twos);
__m256i n_plus_1_and_2 = _mm256_and_si256(_mm256_add_epi32(n, ones), twos);
// For sin: swap when n&1, negate when n&2
__m256 sin_swap = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_and_1, ones));
__m256 sin_result = _mm256_blendv_ps(sin_r, cos_r, sin_swap);
__m256 sin_neg = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_and_2, twos));
sin_result = _mm256_xor_ps(sin_result, _mm256_and_ps(sin_neg, sign_bit));
// For cos: swap when n&1, negate when (n+1)&2
__m256 cos_result = _mm256_blendv_ps(cos_r, sin_r, sin_swap);
__m256 cos_neg = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_plus_1_and_2, twos));
cos_result = _mm256_xor_ps(cos_result, _mm256_and_ps(cos_neg, sign_bit));
_mm256_storeu_ps(sinPtr, sin_result);
_mm256_storeu_ps(cosPtr, cos_result);
inPtr += 8;
sinPtr += 8;
cosPtr += 8;
}
number = eighthPoints * 8;
for (; number < num_points; number++) {
*sinPtr++ = sinf(*inPtr);
*cosPtr++ = cosf(*inPtr++);
}
}
#endif /* LV_HAVE_AVX2 for unaligned */
#ifdef LV_HAVE_SSE4_1
#include <smmintrin.h>
#include <volk/volk_sse_intrinsics.h>
static inline void volk_32f_sincos_32f_x2_u_sse4_1(float* sinVector,
float* cosVector,
const float* inVector,
unsigned int num_points)
{
float* sinPtr = sinVector;
float* cosPtr = cosVector;
const float* inPtr = inVector;
unsigned int number = 0;
unsigned int quarterPoints = num_points / 4;
// Constants for Cody-Waite argument reduction
const __m128 two_over_pi = _mm_set1_ps(0x1.45f306p-1f);
const __m128 pi_over_2_hi = _mm_set1_ps(0x1.921fb6p+0f);
const __m128 pi_over_2_lo = _mm_set1_ps(-0x1.777a5cp-25f);
const __m128i ones = _mm_set1_epi32(1);
const __m128i twos = _mm_set1_epi32(2);
const __m128 sign_bit = _mm_set1_ps(-0.0f);
for (; number < quarterPoints; number++) {
__m128 x = _mm_loadu_ps(inPtr);
// Argument reduction: n = round(x * 2/pi)
__m128 n_f = _mm_round_ps(_mm_mul_ps(x, two_over_pi),
_MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
__m128i n = _mm_cvtps_epi32(n_f);
// r = x - n * (pi/2)
__m128 r = _mm_sub_ps(x, _mm_mul_ps(n_f, pi_over_2_hi));
r = _mm_sub_ps(r, _mm_mul_ps(n_f, pi_over_2_lo));
// Evaluate both sin and cos polynomials
__m128 sin_r = _mm_sin_poly_sse(r);
__m128 cos_r = _mm_cos_poly_sse(r);
// Quadrant selection
__m128i n_and_1 = _mm_and_si128(n, ones);
__m128i n_and_2 = _mm_and_si128(n, twos);
__m128i n_plus_1_and_2 = _mm_and_si128(_mm_add_epi32(n, ones), twos);
// For sin: swap when n&1, negate when n&2
__m128 sin_swap = _mm_castsi128_ps(_mm_cmpeq_epi32(n_and_1, ones));
__m128 sin_result = _mm_blendv_ps(sin_r, cos_r, sin_swap);
__m128 sin_neg = _mm_castsi128_ps(_mm_cmpeq_epi32(n_and_2, twos));
sin_result = _mm_xor_ps(sin_result, _mm_and_ps(sin_neg, sign_bit));
// For cos: swap when n&1, negate when (n+1)&2
__m128 cos_result = _mm_blendv_ps(cos_r, sin_r, sin_swap);
__m128 cos_neg = _mm_castsi128_ps(_mm_cmpeq_epi32(n_plus_1_and_2, twos));
cos_result = _mm_xor_ps(cos_result, _mm_and_ps(cos_neg, sign_bit));
_mm_storeu_ps(sinPtr, sin_result);
_mm_storeu_ps(cosPtr, cos_result);
inPtr += 4;
sinPtr += 4;
cosPtr += 4;
}
number = quarterPoints * 4;
for (; number < num_points; number++) {
*sinPtr++ = sinf(*inPtr);
*cosPtr++ = cosf(*inPtr++);
}
}
#endif /* LV_HAVE_SSE4_1 for unaligned */
#ifdef LV_HAVE_NEON
#include <arm_neon.h>
#include <volk/volk_neon_intrinsics.h>
/* NEON polynomial-based sincos with shared argument reduction */
static inline void volk_32f_sincos_32f_x2_neon(float* sinVector,
float* cosVector,
const float* inVector,
unsigned int num_points)
{
// Cody-Waite argument reduction: n = round(x * 2/pi), r = x - n * pi/2
const float32x4_t two_over_pi = vdupq_n_f32(0x1.45f306p-1f);
const float32x4_t pi_over_2_hi = vdupq_n_f32(0x1.921fb6p+0f);
const float32x4_t pi_over_2_lo = vdupq_n_f32(-0x1.777a5cp-25f);
const int32x4_t ones = vdupq_n_s32(1);
const int32x4_t twos = vdupq_n_s32(2);
const float32x4_t sign_bit = vdupq_n_f32(-0.0f);
const float32x4_t half = vdupq_n_f32(0.5f);
const float32x4_t neg_half = vdupq_n_f32(-0.5f);
const float32x4_t fzeroes = vdupq_n_f32(0.0f);
unsigned int number = 0;
const unsigned int quarterPoints = num_points / 4;
for (; number < quarterPoints; number++) {
float32x4_t x = vld1q_f32(inVector);
inVector += 4;
// n = round(x * 2/pi) - emulate round-to-nearest for ARMv7
float32x4_t scaled = vmulq_f32(x, two_over_pi);
uint32x4_t is_neg = vcltq_f32(scaled, fzeroes);
float32x4_t adj = vbslq_f32(is_neg, neg_half, half);
float32x4_t n_f = vcvtq_f32_s32(vcvtq_s32_f32(vaddq_f32(scaled, adj)));
int32x4_t n = vcvtq_s32_f32(n_f);
// r = x - n * (pi/2) using extended precision
float32x4_t r = vmlsq_f32(x, n_f, pi_over_2_hi);
r = vmlsq_f32(r, n_f, pi_over_2_lo);
// Evaluate sin and cos polynomials (shared for both outputs)
float32x4_t sin_r = _vsin_poly_f32(r);
float32x4_t cos_r = _vcos_poly_f32(r);
// Quadrant selection
int32x4_t n_and_1 = vandq_s32(n, ones);
int32x4_t n_and_2 = vandq_s32(n, twos);
int32x4_t n_plus_1_and_2 = vandq_s32(vaddq_s32(n, ones), twos);
uint32x4_t swap_mask = vceqq_s32(n_and_1, ones);
// Sin result: use cos when n&1, negate when n&2
float32x4_t sin_result = vbslq_f32(swap_mask, cos_r, sin_r);
uint32x4_t sin_neg = vceqq_s32(n_and_2, twos);
sin_result = vreinterpretq_f32_u32(
veorq_u32(vreinterpretq_u32_f32(sin_result),
vandq_u32(sin_neg, vreinterpretq_u32_f32(sign_bit))));
// Cos result: use sin when n&1, negate when (n+1)&2
float32x4_t cos_result = vbslq_f32(swap_mask, sin_r, cos_r);
uint32x4_t cos_neg = vceqq_s32(n_plus_1_and_2, twos);
cos_result = vreinterpretq_f32_u32(
veorq_u32(vreinterpretq_u32_f32(cos_result),
vandq_u32(cos_neg, vreinterpretq_u32_f32(sign_bit))));
vst1q_f32(sinVector, sin_result);
vst1q_f32(cosVector, cos_result);
sinVector += 4;
cosVector += 4;
}
for (number = quarterPoints * 4; number < num_points; number++) {
*sinVector++ = sinf(*inVector);
*cosVector++ = cosf(*inVector++);
}
}
#endif /* LV_HAVE_NEON */
#ifdef LV_HAVE_NEONV8
#include <arm_neon.h>
#include <volk/volk_neon_intrinsics.h>
/* NEONv8 polynomial-based sincos with FMA and shared argument reduction */
static inline void volk_32f_sincos_32f_x2_neonv8(float* sinVector,
float* cosVector,
const float* inVector,
unsigned int num_points)
{
// Cody-Waite argument reduction: n = round(x * 2/pi), r = x - n * pi/2
const float32x4_t two_over_pi = vdupq_n_f32(0x1.45f306p-1f);
const float32x4_t pi_over_2_hi = vdupq_n_f32(0x1.921fb6p+0f);
const float32x4_t pi_over_2_lo = vdupq_n_f32(-0x1.777a5cp-25f);
const int32x4_t ones = vdupq_n_s32(1);
const int32x4_t twos = vdupq_n_s32(2);
const float32x4_t sign_bit = vdupq_n_f32(-0.0f);
unsigned int number = 0;
const unsigned int quarterPoints = num_points / 4;
for (; number < quarterPoints; number++) {
float32x4_t x = vld1q_f32(inVector);
inVector += 4;
// n = round(x * 2/pi) using ARMv8 vrndnq_f32
float32x4_t n_f = vrndnq_f32(vmulq_f32(x, two_over_pi));
int32x4_t n = vcvtq_s32_f32(n_f);
// r = x - n * (pi/2) using FMA for extended precision
float32x4_t r = vfmsq_f32(x, n_f, pi_over_2_hi);
r = vfmsq_f32(r, n_f, pi_over_2_lo);
// Evaluate sin and cos polynomials using FMA (shared for both outputs)
float32x4_t sin_r = _vsin_poly_neonv8(r);
float32x4_t cos_r = _vcos_poly_neonv8(r);
// Quadrant selection
int32x4_t n_and_1 = vandq_s32(n, ones);
int32x4_t n_and_2 = vandq_s32(n, twos);
int32x4_t n_plus_1_and_2 = vandq_s32(vaddq_s32(n, ones), twos);
uint32x4_t swap_mask = vceqq_s32(n_and_1, ones);
// Sin result: use cos when n&1, negate when n&2
float32x4_t sin_result = vbslq_f32(swap_mask, cos_r, sin_r);
uint32x4_t sin_neg = vceqq_s32(n_and_2, twos);
sin_result = vreinterpretq_f32_u32(
veorq_u32(vreinterpretq_u32_f32(sin_result),
vandq_u32(sin_neg, vreinterpretq_u32_f32(sign_bit))));
// Cos result: use sin when n&1, negate when (n+1)&2
float32x4_t cos_result = vbslq_f32(swap_mask, sin_r, cos_r);
uint32x4_t cos_neg = vceqq_s32(n_plus_1_and_2, twos);
cos_result = vreinterpretq_f32_u32(
veorq_u32(vreinterpretq_u32_f32(cos_result),
vandq_u32(cos_neg, vreinterpretq_u32_f32(sign_bit))));
vst1q_f32(sinVector, sin_result);
vst1q_f32(cosVector, cos_result);
sinVector += 4;
cosVector += 4;
}
for (number = quarterPoints * 4; number < num_points; number++) {
*sinVector++ = sinf(*inVector);
*cosVector++ = cosf(*inVector++);
}
}
#endif /* LV_HAVE_NEONV8 */
#endif /* INCLUDED_volk_32f_sincos_32f_x2_u_H */