inline float32x4_t vdupq_n(const f32 & val) { return vdupq_n_f32(val); }
void meanStdDev(const Size2D &size,
const u16 * srcBase, ptrdiff_t srcStride,
f32 * pMean, f32 * pStdDev)
{
internal::assertSupportedConfiguration();
#ifdef CAROTENE_NEON
size_t blockSize0 = 1 << 10, roiw4 = size.width & ~3;
f64 fsum = 0.0f, fsqsum = 0.0f;
f32 arsum[8];
uint32x4_t v_zero = vdupq_n_u32(0u), v_sum;
float32x4_t v_zero_f = vdupq_n_f32(0.0f), v_sqsum;
for (size_t i = 0; i < size.height; ++i)
{
const u16 * src = internal::getRowPtr(srcBase, srcStride, i);
size_t j = 0u;
while (j < roiw4)
{
size_t blockSize = std::min(roiw4 - j, blockSize0) + j;
v_sum = v_zero;
v_sqsum = v_zero_f;
for ( ; j + 16 < blockSize ; j += 16)
{
internal::prefetch(src + j);
uint16x8_t v_src0 = vld1q_u16(src + j), v_src1 = vld1q_u16(src + j + 8);
// 0
uint32x4_t v_srclo = vmovl_u16(vget_low_u16(v_src0));
uint32x4_t v_srchi = vmovl_u16(vget_high_u16(v_src0));
v_sum = vaddq_u32(v_sum, vaddq_u32(v_srclo, v_srchi));
float32x4_t v_srclo_f = vcvtq_f32_u32(v_srclo);
float32x4_t v_srchi_f = vcvtq_f32_u32(v_srchi);
v_sqsum = vmlaq_f32(v_sqsum, v_srclo_f, v_srclo_f);
v_sqsum = vmlaq_f32(v_sqsum, v_srchi_f, v_srchi_f);
// 1
v_srclo = vmovl_u16(vget_low_u16(v_src1));
v_srchi = vmovl_u16(vget_high_u16(v_src1));
v_sum = vaddq_u32(v_sum, vaddq_u32(v_srclo, v_srchi));
v_srclo_f = vcvtq_f32_u32(v_srclo);
v_srchi_f = vcvtq_f32_u32(v_srchi);
v_sqsum = vmlaq_f32(v_sqsum, v_srclo_f, v_srclo_f);
v_sqsum = vmlaq_f32(v_sqsum, v_srchi_f, v_srchi_f);
}
for ( ; j < blockSize; j += 4)
{
uint32x4_t v_src = vmovl_u16(vld1_u16(src + j));
float32x4_t v_src_f = vcvtq_f32_u32(v_src);
v_sum = vaddq_u32(v_sum, v_src);
v_sqsum = vmlaq_f32(v_sqsum, v_src_f, v_src_f);
}
vst1q_f32(arsum, vcvtq_f32_u32(v_sum));
vst1q_f32(arsum + 4, v_sqsum);
fsum += (f64)arsum[0] + arsum[1] + arsum[2] + arsum[3];
fsqsum += (f64)arsum[4] + arsum[5] + arsum[6] + arsum[7];
}
// collect a few last elements in the current row
for ( ; j < size.width; ++j)
{
f32 srcval = src[j];
fsum += srcval;
fsqsum += srcval * srcval;
}
}
// calc mean and stddev
f64 itotal = 1.0 / size.total();
f64 mean = fsum * itotal;
f64 stddev = sqrt(std::max(fsqsum * itotal - mean * mean, 0.0));
if (pMean)
*pMean = mean;
if (pStdDev)
*pStdDev = stddev;
#else
(void)size;
(void)srcBase;
(void)srcStride;
(void)pMean;
(void)pStdDev;
#endif
}
v4sf set_ps1(float f) {
return vdupq_n_f32(f);
}
namespace Ogre
{
const ArrayQuaternion ArrayQuaternion::ZERO( vdupq_n_f32( 0.0f ), vdupq_n_f32( 0.0f ), vdupq_n_f32( 0.0f ), vdupq_n_f32( 0.0f ) );
const ArrayQuaternion ArrayQuaternion::IDENTITY( vdupq_n_f32( 1.0f ), vdupq_n_f32( 0.0f ), vdupq_n_f32( 0.0f ), vdupq_n_f32( 0.0f ) );
}
static void
thresh_32f( const Mat& _src, Mat& _dst, float thresh, float maxval, int type )
{
int i, j;
Size roi = _src.size();
roi.width *= _src.channels();
const float* src = _src.ptr<float>();
float* dst = _dst.ptr<float>();
size_t src_step = _src.step/sizeof(src[0]);
size_t dst_step = _dst.step/sizeof(dst[0]);
#if CV_SSE2
volatile bool useSIMD = checkHardwareSupport(CV_CPU_SSE);
#endif
if( _src.isContinuous() && _dst.isContinuous() )
{
roi.width *= roi.height;
roi.height = 1;
}
#ifdef HAVE_TEGRA_OPTIMIZATION
if (tegra::thresh_32f(_src, _dst, roi.width, roi.height, thresh, maxval, type))
return;
#endif
#if defined(HAVE_IPP)
CV_IPP_CHECK()
{
IppiSize sz = { roi.width, roi.height };
switch( type )
{
case THRESH_TRUNC:
if (0 <= ippiThreshold_GT_32f_C1R(src, (int)src_step*sizeof(src[0]), dst, (int)dst_step*sizeof(dst[0]), sz, thresh))
{
CV_IMPL_ADD(CV_IMPL_IPP);
return;
}
setIppErrorStatus();
break;
case THRESH_TOZERO:
if (0 <= ippiThreshold_LTVal_32f_C1R(src, (int)src_step*sizeof(src[0]), dst, (int)dst_step*sizeof(dst[0]), sz, thresh+FLT_EPSILON, 0))
{
CV_IMPL_ADD(CV_IMPL_IPP);
return;
}
setIppErrorStatus();
break;
case THRESH_TOZERO_INV:
if (0 <= ippiThreshold_GTVal_32f_C1R(src, (int)src_step*sizeof(src[0]), dst, (int)dst_step*sizeof(dst[0]), sz, thresh, 0))
{
CV_IMPL_ADD(CV_IMPL_IPP);
return;
}
setIppErrorStatus();
break;
}
}
#endif
switch( type )
{
case THRESH_BINARY:
for( i = 0; i < roi.height; i++, src += src_step, dst += dst_step )
{
j = 0;
#if CV_SSE2
if( useSIMD )
{
__m128 thresh4 = _mm_set1_ps(thresh), maxval4 = _mm_set1_ps(maxval);
for( ; j <= roi.width - 8; j += 8 )
{
__m128 v0, v1;
v0 = _mm_loadu_ps( src + j );
v1 = _mm_loadu_ps( src + j + 4 );
v0 = _mm_cmpgt_ps( v0, thresh4 );
v1 = _mm_cmpgt_ps( v1, thresh4 );
v0 = _mm_and_ps( v0, maxval4 );
v1 = _mm_and_ps( v1, maxval4 );
_mm_storeu_ps( dst + j, v0 );
_mm_storeu_ps( dst + j + 4, v1 );
}
}
#elif CV_NEON
float32x4_t v_thresh = vdupq_n_f32(thresh);
uint32x4_t v_maxval = vreinterpretq_u32_f32(vdupq_n_f32(maxval));
for( ; j <= roi.width - 4; j += 4 )
{
float32x4_t v_src = vld1q_f32(src + j);
uint32x4_t v_dst = vandq_u32(vcgtq_f32(v_src, v_thresh), v_maxval);
vst1q_f32(dst + j, vreinterpretq_f32_u32(v_dst));
}
#endif
for( ; j < roi.width; j++ )
dst[j] = src[j] > thresh ? maxval : 0;
}
break;
case THRESH_BINARY_INV:
for( i = 0; i < roi.height; i++, src += src_step, dst += dst_step )
{
j = 0;
#if CV_SSE2
if( useSIMD )
{
__m128 thresh4 = _mm_set1_ps(thresh), maxval4 = _mm_set1_ps(maxval);
for( ; j <= roi.width - 8; j += 8 )
{
__m128 v0, v1;
v0 = _mm_loadu_ps( src + j );
v1 = _mm_loadu_ps( src + j + 4 );
v0 = _mm_cmple_ps( v0, thresh4 );
v1 = _mm_cmple_ps( v1, thresh4 );
v0 = _mm_and_ps( v0, maxval4 );
v1 = _mm_and_ps( v1, maxval4 );
_mm_storeu_ps( dst + j, v0 );
_mm_storeu_ps( dst + j + 4, v1 );
}
}
#elif CV_NEON
float32x4_t v_thresh = vdupq_n_f32(thresh);
uint32x4_t v_maxval = vreinterpretq_u32_f32(vdupq_n_f32(maxval));
for( ; j <= roi.width - 4; j += 4 )
{
float32x4_t v_src = vld1q_f32(src + j);
uint32x4_t v_dst = vandq_u32(vcleq_f32(v_src, v_thresh), v_maxval);
vst1q_f32(dst + j, vreinterpretq_f32_u32(v_dst));
}
#endif
for( ; j < roi.width; j++ )
dst[j] = src[j] <= thresh ? maxval : 0;
}
break;
case THRESH_TRUNC:
for( i = 0; i < roi.height; i++, src += src_step, dst += dst_step )
{
j = 0;
#if CV_SSE2
if( useSIMD )
{
__m128 thresh4 = _mm_set1_ps(thresh);
for( ; j <= roi.width - 8; j += 8 )
{
__m128 v0, v1;
v0 = _mm_loadu_ps( src + j );
v1 = _mm_loadu_ps( src + j + 4 );
v0 = _mm_min_ps( v0, thresh4 );
v1 = _mm_min_ps( v1, thresh4 );
_mm_storeu_ps( dst + j, v0 );
_mm_storeu_ps( dst + j + 4, v1 );
}
}
#elif CV_NEON
float32x4_t v_thresh = vdupq_n_f32(thresh);
for( ; j <= roi.width - 4; j += 4 )
vst1q_f32(dst + j, vminq_f32(vld1q_f32(src + j), v_thresh));
#endif
for( ; j < roi.width; j++ )
dst[j] = std::min(src[j], thresh);
}
break;
case THRESH_TOZERO:
for( i = 0; i < roi.height; i++, src += src_step, dst += dst_step )
{
j = 0;
#if CV_SSE2
if( useSIMD )
{
__m128 thresh4 = _mm_set1_ps(thresh);
for( ; j <= roi.width - 8; j += 8 )
{
__m128 v0, v1;
v0 = _mm_loadu_ps( src + j );
v1 = _mm_loadu_ps( src + j + 4 );
v0 = _mm_and_ps(v0, _mm_cmpgt_ps(v0, thresh4));
v1 = _mm_and_ps(v1, _mm_cmpgt_ps(v1, thresh4));
_mm_storeu_ps( dst + j, v0 );
_mm_storeu_ps( dst + j + 4, v1 );
}
}
#elif CV_NEON
float32x4_t v_thresh = vdupq_n_f32(thresh);
for( ; j <= roi.width - 4; j += 4 )
{
float32x4_t v_src = vld1q_f32(src + j);
uint32x4_t v_dst = vandq_u32(vcgtq_f32(v_src, v_thresh),
vreinterpretq_u32_f32(v_src));
vst1q_f32(dst + j, vreinterpretq_f32_u32(v_dst));
}
#endif
for( ; j < roi.width; j++ )
{
float v = src[j];
dst[j] = v > thresh ? v : 0;
}
}
break;
case THRESH_TOZERO_INV:
for( i = 0; i < roi.height; i++, src += src_step, dst += dst_step )
{
j = 0;
#if CV_SSE2
if( useSIMD )
{
__m128 thresh4 = _mm_set1_ps(thresh);
for( ; j <= roi.width - 8; j += 8 )
{
__m128 v0, v1;
v0 = _mm_loadu_ps( src + j );
v1 = _mm_loadu_ps( src + j + 4 );
v0 = _mm_and_ps(v0, _mm_cmple_ps(v0, thresh4));
v1 = _mm_and_ps(v1, _mm_cmple_ps(v1, thresh4));
_mm_storeu_ps( dst + j, v0 );
_mm_storeu_ps( dst + j + 4, v1 );
}
}
#elif CV_NEON
float32x4_t v_thresh = vdupq_n_f32(thresh);
for( ; j <= roi.width - 4; j += 4 )
{
float32x4_t v_src = vld1q_f32(src + j);
uint32x4_t v_dst = vandq_u32(vcleq_f32(v_src, v_thresh),
vreinterpretq_u32_f32(v_src));
vst1q_f32(dst + j, vreinterpretq_f32_u32(v_dst));
}
#endif
for( ; j < roi.width; j++ )
{
float v = src[j];
dst[j] = v <= thresh ? v : 0;
}
}
break;
default:
return CV_Error( CV_StsBadArg, "" );
}
}
inline uint32x4_t cv_vrndq_u32_f32(float32x4_t v)
{
static float32x4_t v_05 = vdupq_n_f32(0.5f);
return vcvtq_u32_f32(vaddq_f32(v, v_05));
}
/*******************************************************************************
* PROCEDURE: gaussian_smooth
* PURPOSE: Blur an image with a gaussian filter.
* NAME: Mike Heath
* DATE: 2/15/96
*******************************************************************************/
short int* gaussian_smooth(unsigned char *image, int rows, int cols, float sigma)
{
int r, c, rr, cc, /* Counter variables. */
windowsize, /* Dimension of the gaussian kernel. */
center; /* Half of the windowsize. */
float *tempim,*tempim1, /* Buffer for separable filter gaussian smoothing. */
*kernel, /* A one dimensional gaussian kernel. */
dot, /* Dot product summing variable. */
sum; /* Sum of the kernel weights variable. */
/****************************************************************************
* Create a 1-dimensional gaussian smoothing kernel.
****************************************************************************/
if(VERBOSE) printf(" Computing the gaussian smoothing kernel.\n");
make_gaussian_kernel(sigma, &kernel, &windowsize);
center = windowsize / 2;
/****************************************************************************
* Allocate a temporary buffer image and the smoothed image.
****************************************************************************/
if((tempim = (float *) malloc(rows*cols* sizeof(float))) == NULL)
{
fprintf(stderr, "Error allocating the buffer image.\n");
exit(1);
}
short int* smoothedim;
if(((smoothedim) = (short int *) malloc(rows*cols*sizeof(short int))) == NULL)
{
fprintf(stderr, "Error allocating the smoothed image.\n");
exit(1);
}
startTimer(&totalTime);
//Neon impelementation of gaussian smooth starts here
/****************************************************************************
* Blur in the x - direction.
****************************************************************************/
int loop;
int floop;
//Modification of input image for neon implementation
//For Filter 1
float * new_image;
//For Filter 2
float *new_image_col;
//kernel is changed to 17 from 15 for neon (two 0s at the beginning and the end)
float new_kernel[17];
//Generating now kernel filter
for (floop = 0 ; floop < 17 ; floop++)
{
if(floop == 0 || floop == 16 )
new_kernel[floop] = 0 ;
else
new_kernel [floop] = kernel[floop -1];
}
//For filter 1, new cols number for neon
unsigned int new_cols;
new_cols=cols+16;
unsigned int i, k;
unsigned int a;
unsigned int m;
unsigned int n, j;
//Malloc of new image used by neon
new_image = (float*)malloc(new_cols*rows*sizeof(float));
for( i =0; i<rows; i++){
memset(&new_image[i*new_cols],0,8*sizeof(float));
for( k=0; k<cols;k++){
new_image[i*new_cols+8+k] = (float)image[i*cols+k];
}
memset(&new_image[i*new_cols+8+cols],0,8*sizeof(float));
}
// Neon handles four piexel at a time
float32x4_t neon_input;
float32x4_t neon_filter;
float32x4_t temp_sum;
float32x2_t tempUpper;
float32x2_t tempLower;
float32_t zero = 0;
float32_t temp_output;
float Basekernel = 0.0f;
float kernelSum;
//When using the new filter, we always assume the image has more than 9 pixels in a row
//Base sum for the filter
for( a=8; a<=16; a++){
Basekernel += new_kernel[a];
}
//Filter 1, filtering row by row
for(m=0; m<rows; m++){
for( n=0; n<cols; n++){
temp_sum = vdupq_n_f32(0);
if(n==0){
kernelSum = Basekernel;
}
else if(n <=8){
kernelSum += new_kernel[8-n];
}
else if(n>=cols-8){
kernelSum -=new_kernel[cols-n+8];
}
//For each pixel, filtering is performed four times
for( j=0; j<4; j++)
{
int kk=0;
if(j>=2)
{
kk=1;
}
neon_input = vld1q_f32(&new_image[m*new_cols+n+j*4+kk]);
neon_filter = vld1q_f32(&new_kernel[j*4+kk]);
temp_sum = vmlaq_f32(temp_sum,neon_input,neon_filter);
}
unsigned int t;
for( t=0; t<=3; t++){
temp_output += vgetq_lane_f32(temp_sum,t );
}
temp_output += new_image[m*new_cols+n+8] * new_kernel[8];
temp_output /= kernelSum;
tempim[m*cols+n] = temp_output;
temp_output=0;
}
}
for(r=0; r<rows; r++)
{
for(c=0; c<cols; c++)
{
dot = 0.0;
sum = 0.0;
for(cc=(-center); cc<=center; cc++)
{
if(((c+cc) >= 0) && ((c+cc) < cols))
{
dot += (float)image[r*cols+(c+cc)] * kernel[center+cc];
sum += kernel[center+cc];
}
}
tempim1[r*cols+c] = dot/sum;
}
}
/****************************************************************************
* Blur in the y - direction.
****************************************************************************/
unsigned int new_rows;
new_rows=rows+16;
new_image_col = (float*)malloc(new_rows*cols*sizeof(float));
if(VERBOSE) printf(" Bluring the image in the Y-direction.\n");
for( i =0; i<cols; i++){//actually nember of new rows are the number of columns here
memset(&new_image_col[i*new_rows],0,8*sizeof(float));
for( k=0; k<rows;k++){
new_image_col[i*new_rows+8+k] = tempim[k*cols+i];
//new_image_col[i*new_rows+8+k] = imagetest1[k*cols+i];
}
memset(&new_image_col[i*new_rows+8+rows],0,8*sizeof(float));
}
Basekernel = 0.0;
for( a=8; a<=16; a++){
Basekernel += new_kernel[a];
}
for(m=0; m<cols; m++){// it was rows at br
for( n=0; n<rows; n++){
temp_sum = vdupq_n_f32(0);
if(n==0){
kernelSum = Basekernel;
}
else if(n <=8){
kernelSum += new_kernel[8-n];
}
else if(n>=rows-8){
kernelSum -=new_kernel[rows-n+8];
}
for( j=0; j<4; j++)
{
int kk=0;
if(j>=2)
{
kk=1;
}
neon_input = vld1q_f32(&new_image_col[m*new_rows+n+j*4+kk]);
neon_filter = vld1q_f32(&new_kernel[j*4+kk]);
temp_sum = vmlaq_f32(temp_sum,neon_input,neon_filter);
}
unsigned int t;
for( t=0; t<=3; t++){
temp_output += vgetq_lane_f32(temp_sum,t );
}
temp_output += new_image_col[m*new_rows+n+8] * new_kernel[8];
temp_output = (temp_output * BOOSTBLURFACTOR) / kernelSum + 0.5;
smoothedim[n*cols+m] = (short int )temp_output;
temp_output=0;
}
}
stopTimer(&totalTime);
printTimer(&totalTime);
free(tempim);
free(kernel);
return smoothedim;
}
float32x4_t test_vdupq_n_f32(float32_t v1) {
// CHECK-LABEL: test_vdupq_n_f32
return vdupq_n_f32(v1);
// CHECK: dup {{v[0-9]+}}.4s, {{v[0-9]+}}.s[0]
}
static void ne10_fft_split_c2r_1d_float32_neon (ne10_fft_cpx_float32_t *dst,
const ne10_fft_cpx_float32_t *src,
ne10_fft_cpx_float32_t *twiddles,
ne10_int32_t ncfft)
{
ne10_int32_t k;
ne10_int32_t count = ncfft / 2;
ne10_fft_cpx_float32_t fk, fnkc, fek, fok, tmp;
float32x4x2_t q2_fk, q2_fnkc, q2_tw, q2_dst, q2_dst2;
float32x4_t q_fnkc_r, q_fnkc_i;
float32x4_t q_fek_r, q_fek_i, q_fok_r, q_fok_i;
float32x4_t q_tmp0, q_tmp1, q_tmp2, q_tmp3, q_val;
float32x4_t q_dst2_r, q_dst2_i;
float32_t *p_src, *p_src2, *p_dst, *p_dst2, *p_twiddles;
dst[0].r = (src[0].r + src[ncfft].r) * 0.5f;
dst[0].i = (src[0].r - src[ncfft].r) * 0.5f;
if (count >= 4)
{
for (k = 1; k <= count ; k += 4)
{
p_src = (float32_t*) (& (src[k]));
p_src2 = (float32_t*) (& (src[ncfft - k - 3]));
p_twiddles = (float32_t*) (& (twiddles[k - 1]));
p_dst = (float32_t*) (& (dst[k]));
p_dst2 = (float32_t*) (& (dst[ncfft - k - 3]));
q2_fk = vld2q_f32 (p_src);
q2_fnkc = vld2q_f32 (p_src2);
q2_tw = vld2q_f32 (p_twiddles);
q2_fnkc.val[0] = vrev64q_f32 (q2_fnkc.val[0]);
q2_fnkc.val[1] = vrev64q_f32 (q2_fnkc.val[1]);
q_fnkc_r = vcombine_f32 (vget_high_f32 (q2_fnkc.val[0]), vget_low_f32 (q2_fnkc.val[0]));
q_fnkc_i = vcombine_f32 (vget_high_f32 (q2_fnkc.val[1]), vget_low_f32 (q2_fnkc.val[1]));
q_fnkc_i = vnegq_f32 (q_fnkc_i);
q_fek_r = vaddq_f32 (q2_fk.val[0], q_fnkc_r);
q_fek_i = vaddq_f32 (q2_fk.val[1], q_fnkc_i);
q_tmp0 = vsubq_f32 (q2_fk.val[0], q_fnkc_r);
q_tmp1 = vsubq_f32 (q2_fk.val[1], q_fnkc_i);
q_fok_r = vmulq_f32 (q_tmp0, q2_tw.val[0]);
q_fok_i = vmulq_f32 (q_tmp1, q2_tw.val[0]);
q_tmp2 = vmulq_f32 (q_tmp1, q2_tw.val[1]);
q_tmp3 = vmulq_f32 (q_tmp0, q2_tw.val[1]);
q_fok_r = vaddq_f32 (q_fok_r, q_tmp2);
q_fok_i = vsubq_f32 (q_fok_i, q_tmp3);
q_val = vdupq_n_f32 (0.5f);
q_dst2_r = vsubq_f32 (q_fek_r, q_fok_r);
q_dst2_i = vsubq_f32 (q_fok_i, q_fek_i);
q2_dst.val[0] = vaddq_f32 (q_fek_r, q_fok_r);
q2_dst.val[1] = vaddq_f32 (q_fek_i, q_fok_i);
q_dst2_r = vmulq_f32 (q_dst2_r, q_val);
q_dst2_i = vmulq_f32 (q_dst2_i, q_val);
q2_dst.val[0] = vmulq_f32 (q2_dst.val[0], q_val);
q2_dst.val[1] = vmulq_f32 (q2_dst.val[1], q_val);
q_dst2_r = vrev64q_f32 (q_dst2_r);
q_dst2_i = vrev64q_f32 (q_dst2_i);
q2_dst2.val[0] = vcombine_f32 (vget_high_f32 (q_dst2_r), vget_low_f32 (q_dst2_r));
q2_dst2.val[1] = vcombine_f32 (vget_high_f32 (q_dst2_i), vget_low_f32 (q_dst2_i));
vst2q_f32 (p_dst, q2_dst);
vst2q_f32 (p_dst2, q2_dst2);
}
}
else
{
for (k = 1; k <= count ; k++)
{
fk = src[k];
fnkc.r = src[ncfft - k].r;
fnkc.i = -src[ncfft - k].i;
fek.r = fk.r + fnkc.r;
fek.i = fk.i + fnkc.i;
tmp.r = fk.r - fnkc.r;
tmp.i = fk.i - fnkc.i;
fok.r = tmp.r * twiddles[k - 1].r + tmp.i * twiddles[k - 1].i;
fok.i = tmp.i * twiddles[k - 1].r - tmp.r * twiddles[k - 1].i;
dst[k].r = (fek.r + fok.r) * 0.5f;
dst[k].i = (fek.i + fok.i) * 0.5f;
dst[ncfft - k].r = (fek.r - fok.r) * 0.5f;
dst[ncfft - k].i = (fok.i - fek.i) * 0.5f;
}
}
}
#include <stddef.h>
#include <nnpack/arm_neon.h>
#include <nnpack/activations.h>
void nnp_relu__neon(
const float input[restrict static 4],
float output[restrict static 4],
size_t length,
float negative_slope)
{
const float32x4_t vec_negative_slope = vdupq_n_f32(negative_slope);
/* Length is always non-zero and proportional to SIMD width */
do {
vst1q_f32(output,
neon_reluq_f32(vld1q_f32(input), vec_negative_slope));
input += 4;
output += 4;
length -= 4;
} while (length != 0);
}
void nnp_inplace_relu__neon(
float data[restrict static 4],
size_t length,
float negative_slope)
{
const float32x4_t vec_negative_slope = vdupq_n_f32(negative_slope);
static void ne10_fft_split_r2c_1d_float32_neon (ne10_fft_cpx_float32_t *dst,
const ne10_fft_cpx_float32_t *src,
ne10_fft_cpx_float32_t *twiddles,
ne10_int32_t ncfft)
{
ne10_int32_t k;
ne10_int32_t count = ncfft / 2;
ne10_fft_cpx_float32_t fpnk, fpk, f1k, f2k, tw, tdc;
float32x4x2_t q2_fpk, q2_fpnk, q2_tw, q2_dst, q2_dst2;
float32x4_t q_fpnk_r, q_fpnk_i;
float32x4_t q_f1k_r, q_f1k_i, q_f2k_r, q_f2k_i;
float32x4_t q_tw_r, q_tw_i;
float32x4_t q_tmp0, q_tmp1, q_tmp2, q_tmp3, q_val;
float32x4_t q_dst_r, q_dst_i, q_dst2_r, q_dst2_i;
float32_t *p_src, *p_src2, *p_dst, *p_dst2, *p_twiddles;
tdc.r = src[0].r;
tdc.i = src[0].i;
dst[0].r = tdc.r + tdc.i;
dst[ncfft].r = tdc.r - tdc.i;
dst[ncfft].i = dst[0].i = 0;
if (count >= 4)
{
for (k = 1; k <= count ; k += 4)
{
p_src = (float32_t*) (& (src[k]));
p_src2 = (float32_t*) (& (src[ncfft - k - 3]));
p_twiddles = (float32_t*) (& (twiddles[k - 1]));
p_dst = (float32_t*) (& (dst[k]));
p_dst2 = (float32_t*) (& (dst[ncfft - k - 3]));
q2_fpk = vld2q_f32 (p_src);
q2_fpnk = vld2q_f32 (p_src2);
q2_tw = vld2q_f32 (p_twiddles);
q2_fpnk.val[0] = vrev64q_f32 (q2_fpnk.val[0]);
q2_fpnk.val[1] = vrev64q_f32 (q2_fpnk.val[1]);
q_fpnk_r = vcombine_f32 (vget_high_f32 (q2_fpnk.val[0]), vget_low_f32 (q2_fpnk.val[0]));
q_fpnk_i = vcombine_f32 (vget_high_f32 (q2_fpnk.val[1]), vget_low_f32 (q2_fpnk.val[1]));
q_fpnk_i = vnegq_f32 (q_fpnk_i);
q_f1k_r = vaddq_f32 (q2_fpk.val[0], q_fpnk_r);
q_f1k_i = vaddq_f32 (q2_fpk.val[1], q_fpnk_i);
q_f2k_r = vsubq_f32 (q2_fpk.val[0], q_fpnk_r);
q_f2k_i = vsubq_f32 (q2_fpk.val[1], q_fpnk_i);
q_tmp0 = vmulq_f32 (q_f2k_r, q2_tw.val[0]);
q_tmp1 = vmulq_f32 (q_f2k_i, q2_tw.val[1]);
q_tmp2 = vmulq_f32 (q_f2k_r, q2_tw.val[1]);
q_tmp3 = vmulq_f32 (q_f2k_i, q2_tw.val[0]);
q_tw_r = vsubq_f32 (q_tmp0, q_tmp1);
q_tw_i = vaddq_f32 (q_tmp2, q_tmp3);
q_val = vdupq_n_f32 (0.5f);
q_dst2_r = vsubq_f32 (q_f1k_r, q_tw_r);
q_dst2_i = vsubq_f32 (q_tw_i, q_f1k_i);
q_dst_r = vaddq_f32 (q_f1k_r, q_tw_r);
q_dst_i = vaddq_f32 (q_f1k_i, q_tw_i);
q_dst2_r = vmulq_f32 (q_dst2_r, q_val);
q_dst2_i = vmulq_f32 (q_dst2_i, q_val);
q2_dst.val[0] = vmulq_f32 (q_dst_r, q_val);
q2_dst.val[1] = vmulq_f32 (q_dst_i, q_val);
q_dst2_r = vrev64q_f32 (q_dst2_r);
q_dst2_i = vrev64q_f32 (q_dst2_i);
q2_dst2.val[0] = vcombine_f32 (vget_high_f32 (q_dst2_r), vget_low_f32 (q_dst2_r));
q2_dst2.val[1] = vcombine_f32 (vget_high_f32 (q_dst2_i), vget_low_f32 (q_dst2_i));
vst2q_f32 (p_dst, q2_dst);
vst2q_f32 (p_dst2, q2_dst2);
}
}
else
{
for (k = 1; k <= count ; k++)
{
fpk = src[k];
fpnk.r = src[ncfft - k].r;
fpnk.i = - src[ncfft - k].i;
f1k.r = fpk.r + fpnk.r;
f1k.i = fpk.i + fpnk.i;
f2k.r = fpk.r - fpnk.r;
f2k.i = fpk.i - fpnk.i;
tw.r = f2k.r * (twiddles[k - 1]).r - f2k.i * (twiddles[k - 1]).i;
tw.i = f2k.r * (twiddles[k - 1]).i + f2k.i * (twiddles[k - 1]).r;
dst[k].r = (f1k.r + tw.r) * 0.5f;
dst[k].i = (f1k.i + tw.i) * 0.5f;
dst[ncfft - k].r = (f1k.r - tw.r) * 0.5f;
dst[ncfft - k].i = (tw.i - f1k.i) * 0.5f;
}
}
}
static void ne10_fft16_backward_float32_neon (ne10_fft_cpx_float32_t * Fout,
ne10_fft_cpx_float32_t * Fin,
ne10_fft_cpx_float32_t * twiddles)
{
ne10_fft_cpx_float32_t *tw1, *tw2, *tw3;
// the first stage
float32_t *p_src0, *p_src4, *p_src8, *p_src12;
float32x4x2_t q2_in_0123, q2_in_4567, q2_in_89ab, q2_in_cdef;
float32x4_t q_t0_r, q_t0_i, q_t1_r, q_t1_i, q_t2_r, q_t2_i, q_t3_r, q_t3_i;
float32x4_t q_out_r048c, q_out_i048c, q_out_r159d, q_out_i159d;
float32x4_t q_out_r26ae, q_out_i26ae, q_out_r37bf, q_out_i37bf;
p_src0 = (float32_t*) (& (Fin[0]));
p_src4 = (float32_t*) (& (Fin[4]));
p_src8 = (float32_t*) (& (Fin[8]));
p_src12 = (float32_t*) (& (Fin[12]));
q2_in_0123 = vld2q_f32 (p_src0);
q2_in_4567 = vld2q_f32 (p_src4);
q2_in_89ab = vld2q_f32 (p_src8);
q2_in_cdef = vld2q_f32 (p_src12);
q_t2_r = vsubq_f32 (q2_in_0123.val[0], q2_in_89ab.val[0]);
q_t2_i = vsubq_f32 (q2_in_0123.val[1], q2_in_89ab.val[1]);
q_t3_r = vaddq_f32 (q2_in_0123.val[0], q2_in_89ab.val[0]);
q_t3_i = vaddq_f32 (q2_in_0123.val[1], q2_in_89ab.val[1]);
q_t0_r = vaddq_f32 (q2_in_4567.val[0], q2_in_cdef.val[0]);
q_t0_i = vaddq_f32 (q2_in_4567.val[1], q2_in_cdef.val[1]);
q_t1_r = vsubq_f32 (q2_in_4567.val[0], q2_in_cdef.val[0]);
q_t1_i = vsubq_f32 (q2_in_4567.val[1], q2_in_cdef.val[1]);
q_out_r26ae = vsubq_f32 (q_t3_r, q_t0_r);
q_out_i26ae = vsubq_f32 (q_t3_i, q_t0_i);
q_out_r048c = vaddq_f32 (q_t3_r, q_t0_r);
q_out_i048c = vaddq_f32 (q_t3_i, q_t0_i);
q_out_r159d = vsubq_f32 (q_t2_r, q_t1_i);
q_out_i159d = vaddq_f32 (q_t2_i, q_t1_r);
q_out_r37bf = vaddq_f32 (q_t2_r, q_t1_i);
q_out_i37bf = vsubq_f32 (q_t2_i, q_t1_r);
// second stages
float32_t *p_dst0, *p_dst1, *p_dst2, *p_dst3;
float32_t *p_tw1, *p_tw2, *p_tw3;
float32x4_t q_s0_r, q_s0_i, q_s1_r, q_s1_i, q_s2_r, q_s2_i;
float32x4_t q_s3_r, q_s3_i, q_s4_r, q_s4_i, q_s5_r, q_s5_i;
float32x4x2_t q2_tmp_0, q2_tmp_1, q2_tmp_2, q2_tmp_3;
float32x4_t q_in_r0123, q_in_r4567, q_in_r89ab, q_in_rcdef;
float32x4_t q_in_i0123, q_in_i4567, q_in_i89ab, q_in_icdef;
float32x4x2_t q2_tw1, q2_tw2, q2_tw3;
float32x4x2_t q2_out_0123, q2_out_4567, q2_out_89ab, q2_out_cdef;
float32x4_t q_one_by_nfft;
tw1 = twiddles;
tw2 = twiddles + 4;
tw3 = twiddles + 8;
p_dst0 = (float32_t*) (&Fout[0]);
p_dst1 = (float32_t*) (&Fout[4]);
p_dst2 = (float32_t*) (&Fout[8]);
p_dst3 = (float32_t*) (&Fout[12]);
p_tw1 = (float32_t*) tw1;
p_tw2 = (float32_t*) tw2;
p_tw3 = (float32_t*) tw3;
q2_tmp_0 = vzipq_f32 (q_out_r048c, q_out_r159d);
q2_tmp_1 = vzipq_f32 (q_out_i048c, q_out_i159d);
q2_tmp_2 = vzipq_f32 (q_out_r26ae, q_out_r37bf);
q2_tmp_3 = vzipq_f32 (q_out_i26ae, q_out_i37bf);
q_in_r0123 = vcombine_f32 (vget_low_f32 (q2_tmp_0.val[0]), vget_low_f32 (q2_tmp_2.val[0]));
q_in_i0123 = vcombine_f32 (vget_low_f32 (q2_tmp_1.val[0]), vget_low_f32 (q2_tmp_3.val[0]));
q_in_r4567 = vcombine_f32 (vget_high_f32 (q2_tmp_0.val[0]), vget_high_f32 (q2_tmp_2.val[0]));
q_in_i4567 = vcombine_f32 (vget_high_f32 (q2_tmp_1.val[0]), vget_high_f32 (q2_tmp_3.val[0]));
q_in_r89ab = vcombine_f32 (vget_low_f32 (q2_tmp_0.val[1]), vget_low_f32 (q2_tmp_2.val[1]));
q_in_i89ab = vcombine_f32 (vget_low_f32 (q2_tmp_1.val[1]), vget_low_f32 (q2_tmp_3.val[1]));
q_in_rcdef = vcombine_f32 (vget_high_f32 (q2_tmp_0.val[1]), vget_high_f32 (q2_tmp_2.val[1]));
q_in_icdef = vcombine_f32 (vget_high_f32 (q2_tmp_1.val[1]), vget_high_f32 (q2_tmp_3.val[1]));
q2_tw1 = vld2q_f32 (p_tw1);
q2_tw2 = vld2q_f32 (p_tw2);
q2_tw3 = vld2q_f32 (p_tw3);
q_s0_r = vmulq_f32 (q_in_r4567, q2_tw1.val[0]);
q_s0_i = vmulq_f32 (q_in_i4567, q2_tw1.val[0]);
q_s1_r = vmulq_f32 (q_in_r89ab, q2_tw2.val[0]);
q_s1_i = vmulq_f32 (q_in_i89ab, q2_tw2.val[0]);
q_s2_r = vmulq_f32 (q_in_rcdef, q2_tw3.val[0]);
q_s2_i = vmulq_f32 (q_in_icdef, q2_tw3.val[0]);
q_s0_r = vmlaq_f32 (q_s0_r, q_in_i4567, q2_tw1.val[1]);
q_s0_i = vmlsq_f32 (q_s0_i, q_in_r4567, q2_tw1.val[1]);
q_s1_r = vmlaq_f32 (q_s1_r, q_in_i89ab, q2_tw2.val[1]);
q_s1_i = vmlsq_f32 (q_s1_i, q_in_r89ab, q2_tw2.val[1]);
q_s2_r = vmlaq_f32 (q_s2_r, q_in_icdef, q2_tw3.val[1]);
q_s2_i = vmlsq_f32 (q_s2_i, q_in_rcdef, q2_tw3.val[1]);
q_s5_r = vsubq_f32 (q_in_r0123, q_s1_r);
q_s5_i = vsubq_f32 (q_in_i0123, q_s1_i);
q2_out_0123.val[0] = vaddq_f32 (q_in_r0123, q_s1_r);
q2_out_0123.val[1] = vaddq_f32 (q_in_i0123, q_s1_i);
q_s3_r = vaddq_f32 (q_s0_r, q_s2_r);
q_s3_i = vaddq_f32 (q_s0_i, q_s2_i);
q_s4_r = vsubq_f32 (q_s0_r, q_s2_r);
q_s4_i = vsubq_f32 (q_s0_i, q_s2_i);
q_one_by_nfft = vdupq_n_f32 (0.0625f);
q2_out_89ab.val[0] = vsubq_f32 (q2_out_0123.val[0], q_s3_r);
q2_out_89ab.val[1] = vsubq_f32 (q2_out_0123.val[1], q_s3_i);
q2_out_0123.val[0] = vaddq_f32 (q2_out_0123.val[0], q_s3_r);
q2_out_0123.val[1] = vaddq_f32 (q2_out_0123.val[1], q_s3_i);
q2_out_4567.val[0] = vsubq_f32 (q_s5_r, q_s4_i);
q2_out_4567.val[1] = vaddq_f32 (q_s5_i, q_s4_r);
q2_out_cdef.val[0] = vaddq_f32 (q_s5_r, q_s4_i);
q2_out_cdef.val[1] = vsubq_f32 (q_s5_i, q_s4_r);
q2_out_89ab.val[0] = vmulq_f32 (q2_out_89ab.val[0], q_one_by_nfft);
q2_out_89ab.val[1] = vmulq_f32 (q2_out_89ab.val[1], q_one_by_nfft);
q2_out_0123.val[0] = vmulq_f32 (q2_out_0123.val[0], q_one_by_nfft);
q2_out_0123.val[1] = vmulq_f32 (q2_out_0123.val[1], q_one_by_nfft);
q2_out_4567.val[0] = vmulq_f32 (q2_out_4567.val[0], q_one_by_nfft);
q2_out_4567.val[1] = vmulq_f32 (q2_out_4567.val[1], q_one_by_nfft);
q2_out_cdef.val[0] = vmulq_f32 (q2_out_cdef.val[0], q_one_by_nfft);
q2_out_cdef.val[1] = vmulq_f32 (q2_out_cdef.val[1], q_one_by_nfft);
vst2q_f32 (p_dst0, q2_out_0123);
vst2q_f32 (p_dst1, q2_out_4567);
vst2q_f32 (p_dst2, q2_out_89ab);
vst2q_f32 (p_dst3, q2_out_cdef);
}
static void SubbandCoherenceNEON(AecCore* aec,
float efw[2][PART_LEN1],
float xfw[2][PART_LEN1],
float* fft,
float* cohde,
float* cohxd) {
float dfw[2][PART_LEN1];
int i;
if (aec->delayEstCtr == 0)
aec->delayIdx = PartitionDelay(aec);
// Use delayed far.
memcpy(xfw,
aec->xfwBuf + aec->delayIdx * PART_LEN1,
sizeof(xfw[0][0]) * 2 * PART_LEN1);
// Windowed near fft
WindowData(fft, aec->dBuf);
aec_rdft_forward_128(fft);
StoreAsComplex(fft, dfw);
// Windowed error fft
WindowData(fft, aec->eBuf);
aec_rdft_forward_128(fft);
StoreAsComplex(fft, efw);
SmoothedPSD(aec, efw, dfw, xfw);
{
const float32x4_t vec_1eminus10 = vdupq_n_f32(1e-10f);
// Subband coherence
for (i = 0; i + 3 < PART_LEN1; i += 4) {
const float32x4_t vec_sd = vld1q_f32(&aec->sd[i]);
const float32x4_t vec_se = vld1q_f32(&aec->se[i]);
const float32x4_t vec_sx = vld1q_f32(&aec->sx[i]);
const float32x4_t vec_sdse = vmlaq_f32(vec_1eminus10, vec_sd, vec_se);
const float32x4_t vec_sdsx = vmlaq_f32(vec_1eminus10, vec_sd, vec_sx);
float32x4x2_t vec_sde = vld2q_f32(&aec->sde[i][0]);
float32x4x2_t vec_sxd = vld2q_f32(&aec->sxd[i][0]);
float32x4_t vec_cohde = vmulq_f32(vec_sde.val[0], vec_sde.val[0]);
float32x4_t vec_cohxd = vmulq_f32(vec_sxd.val[0], vec_sxd.val[0]);
vec_cohde = vmlaq_f32(vec_cohde, vec_sde.val[1], vec_sde.val[1]);
vec_cohde = vdivq_f32(vec_cohde, vec_sdse);
vec_cohxd = vmlaq_f32(vec_cohxd, vec_sxd.val[1], vec_sxd.val[1]);
vec_cohxd = vdivq_f32(vec_cohxd, vec_sdsx);
vst1q_f32(&cohde[i], vec_cohde);
vst1q_f32(&cohxd[i], vec_cohxd);
}
}
// scalar code for the remaining items.
for (; i < PART_LEN1; i++) {
cohde[i] =
(aec->sde[i][0] * aec->sde[i][0] + aec->sde[i][1] * aec->sde[i][1]) /
(aec->sd[i] * aec->se[i] + 1e-10f);
cohxd[i] =
(aec->sxd[i][0] * aec->sxd[i][0] + aec->sxd[i][1] * aec->sxd[i][1]) /
(aec->sx[i] * aec->sd[i] + 1e-10f);
}
}
int
distance_scan_to_map(
map_t * map,
scan_t * scan,
position_t position)
{
/* Pre-compute sine and cosine of angle for rotation */
double position_theta_radians = radians(position.theta_degrees);
double costheta = cos(position_theta_radians) * map->scale_pixels_per_mm;
double sintheta = sin(position_theta_radians) * map->scale_pixels_per_mm;
/* Pre-compute pixel offset for translation */
double pos_x_pix = position.x_mm * map->scale_pixels_per_mm;
double pos_y_pix = position.y_mm * map->scale_pixels_per_mm;
float32x4_t half_4 = vdupq_n_f32(0.5);
float32x4_t costheta_4 = vdupq_n_f32(costheta);
float32x4_t sintheta_4 = vdupq_n_f32(sintheta);
float32x4_t nsintheta_4 = vdupq_n_f32(-sintheta);
float32x4_t pos_x_4 = vdupq_n_f32(pos_x_pix);
float32x4_t pos_y_4 = vdupq_n_f32(pos_y_pix);
int npoints = 0; /* number of points where scan matches map */
int64_t sum = 0;
/* Stride by 4 over obstacle points in scan */
int i = 0;
for (i=0; i<scan->obst_npoints; i+=4)
{
/* Duplicate current obstacle point X and Y in 128-bit registers */
float32x4_t scan_x_4 = vld1q_f32(&scan->obst_x_mm[i]);
float32x4_t scan_y_4 = vld1q_f32(&scan->obst_y_mm[i]);
/* Compute X coordinate of 4 rotated / translated points at once */
int xarr[4];
neon_coord_4(costheta_4, nsintheta_4, scan_x_4, scan_y_4, pos_x_4, half_4, xarr);
/* Compute Y coordinate of 4 rotated / translated points at once */
int yarr[4];
neon_coord_4(sintheta_4, costheta_4, scan_x_4, scan_y_4, pos_y_4, half_4, yarr);
/* Handle rotated/translated points serially */
int j;
for (j=0; j<4 && (i+j)<scan->obst_npoints; ++j)
{
int x = xarr[j];
int y = yarr[j];
/* Add point if in map bounds */
if (x >= 0 && x < map->size_pixels && y >= 0 && y < map->size_pixels)
{
sum += map->pixels[y * map->size_pixels + x];
npoints++;
}
}
}
return npoints ? (int)(sum * 1024 / npoints) : -1;
}
static float32x4_t vpowq_f32(float32x4_t a, float32x4_t b) {
// a^b = exp2(b * log2(a))
// exp2(x) and log2(x) are calculated using polynomial approximations.
float32x4_t log2_a, b_log2_a, a_exp_b;
// Calculate log2(x), x = a.
{
// To calculate log2(x), we decompose x like this:
// x = y * 2^n
// n is an integer
// y is in the [1.0, 2.0) range
//
// log2(x) = log2(y) + n
// n can be evaluated by playing with float representation.
// log2(y) in a small range can be approximated, this code uses an order
// five polynomial approximation. The coefficients have been
// estimated with the Remez algorithm and the resulting
// polynomial has a maximum relative error of 0.00086%.
// Compute n.
// This is done by masking the exponent, shifting it into the top bit of
// the mantissa, putting eight into the biased exponent (to shift/
// compensate the fact that the exponent has been shifted in the top/
// fractional part and finally getting rid of the implicit leading one
// from the mantissa by substracting it out.
const uint32x4_t vec_float_exponent_mask = vdupq_n_u32(0x7F800000);
const uint32x4_t vec_eight_biased_exponent = vdupq_n_u32(0x43800000);
const uint32x4_t vec_implicit_leading_one = vdupq_n_u32(0x43BF8000);
const uint32x4_t two_n = vandq_u32(vreinterpretq_u32_f32(a),
vec_float_exponent_mask);
const uint32x4_t n_1 = vshrq_n_u32(two_n, kShiftExponentIntoTopMantissa);
const uint32x4_t n_0 = vorrq_u32(n_1, vec_eight_biased_exponent);
const float32x4_t n =
vsubq_f32(vreinterpretq_f32_u32(n_0),
vreinterpretq_f32_u32(vec_implicit_leading_one));
// Compute y.
const uint32x4_t vec_mantissa_mask = vdupq_n_u32(0x007FFFFF);
const uint32x4_t vec_zero_biased_exponent_is_one = vdupq_n_u32(0x3F800000);
const uint32x4_t mantissa = vandq_u32(vreinterpretq_u32_f32(a),
vec_mantissa_mask);
const float32x4_t y =
vreinterpretq_f32_u32(vorrq_u32(mantissa,
vec_zero_biased_exponent_is_one));
// Approximate log2(y) ~= (y - 1) * pol5(y).
// pol5(y) = C5 * y^5 + C4 * y^4 + C3 * y^3 + C2 * y^2 + C1 * y + C0
const float32x4_t C5 = vdupq_n_f32(-3.4436006e-2f);
const float32x4_t C4 = vdupq_n_f32(3.1821337e-1f);
const float32x4_t C3 = vdupq_n_f32(-1.2315303f);
const float32x4_t C2 = vdupq_n_f32(2.5988452f);
const float32x4_t C1 = vdupq_n_f32(-3.3241990f);
const float32x4_t C0 = vdupq_n_f32(3.1157899f);
float32x4_t pol5_y = C5;
pol5_y = vmlaq_f32(C4, y, pol5_y);
pol5_y = vmlaq_f32(C3, y, pol5_y);
pol5_y = vmlaq_f32(C2, y, pol5_y);
pol5_y = vmlaq_f32(C1, y, pol5_y);
pol5_y = vmlaq_f32(C0, y, pol5_y);
const float32x4_t y_minus_one =
vsubq_f32(y, vreinterpretq_f32_u32(vec_zero_biased_exponent_is_one));
const float32x4_t log2_y = vmulq_f32(y_minus_one, pol5_y);
// Combine parts.
log2_a = vaddq_f32(n, log2_y);
}
// b * log2(a)
b_log2_a = vmulq_f32(b, log2_a);
// Calculate exp2(x), x = b * log2(a).
{
// To calculate 2^x, we decompose x like this:
// x = n + y
// n is an integer, the value of x - 0.5 rounded down, therefore
// y is in the [0.5, 1.5) range
//
// 2^x = 2^n * 2^y
// 2^n can be evaluated by playing with float representation.
// 2^y in a small range can be approximated, this code uses an order two
// polynomial approximation. The coefficients have been estimated
// with the Remez algorithm and the resulting polynomial has a
// maximum relative error of 0.17%.
// To avoid over/underflow, we reduce the range of input to ]-127, 129].
const float32x4_t max_input = vdupq_n_f32(129.f);
const float32x4_t min_input = vdupq_n_f32(-126.99999f);
const float32x4_t x_min = vminq_f32(b_log2_a, max_input);
const float32x4_t x_max = vmaxq_f32(x_min, min_input);
// Compute n.
const float32x4_t half = vdupq_n_f32(0.5f);
const float32x4_t x_minus_half = vsubq_f32(x_max, half);
const int32x4_t x_minus_half_floor = vcvtq_s32_f32(x_minus_half);
// Compute 2^n.
const int32x4_t float_exponent_bias = vdupq_n_s32(127);
const int32x4_t two_n_exponent =
vaddq_s32(x_minus_half_floor, float_exponent_bias);
const float32x4_t two_n =
vreinterpretq_f32_s32(vshlq_n_s32(two_n_exponent, kFloatExponentShift));
// Compute y.
const float32x4_t y = vsubq_f32(x_max, vcvtq_f32_s32(x_minus_half_floor));
// Approximate 2^y ~= C2 * y^2 + C1 * y + C0.
const float32x4_t C2 = vdupq_n_f32(3.3718944e-1f);
const float32x4_t C1 = vdupq_n_f32(6.5763628e-1f);
const float32x4_t C0 = vdupq_n_f32(1.0017247f);
float32x4_t exp2_y = C2;
exp2_y = vmlaq_f32(C1, y, exp2_y);
exp2_y = vmlaq_f32(C0, y, exp2_y);
// Combine parts.
a_exp_b = vmulq_f32(exp2_y, two_n);
}
return a_exp_b;
}
int LRN_arm::forward_inplace(Mat& bottom_top_blob) const
{
int w = bottom_top_blob.w;
int h = bottom_top_blob.h;
int channels = bottom_top_blob.c;
int size = w * h;
// squared values with local_size padding
Mat square_blob;
square_blob.create(w, h, channels);
if (square_blob.empty())
return -100;
#pragma omp parallel for
for (int q=0; q<channels; q++)
{
const float* ptr = bottom_top_blob.channel(q);
float* outptr = square_blob.channel(q);
#if __ARM_NEON
int nn = size >> 2;
int remain = size - (nn << 2);
#else
int remain = size;
#endif // __ARM_NEON
#if __ARM_NEON
for (; nn>0; nn--)
{
float32x4_t _p = vld1q_f32(ptr);
float32x4_t _outp = vmulq_f32(_p, _p);
vst1q_f32(outptr, _outp);
ptr += 4;
outptr += 4;
}
#endif // __ARM_NEON
for (; remain>0; remain--)
{
*outptr = *ptr * *ptr;
ptr++;
outptr++;
}
}
if (region_type == NormRegion_ACROSS_CHANNELS)
{
Mat square_sum;
square_sum.create(w, h, channels);
if (square_sum.empty())
return -100;
square_sum.fill(0.f);
const float alpha_div_size = alpha / local_size;
#pragma omp parallel for
for (int q=0; q<channels; q++)
{
// square sum
for (int p=q - local_size / 2; p<=q + local_size / 2; p++)
{
if (p < 0 || p >= channels)
continue;
const float* sptr = square_blob.channel(p);
float* ssptr = square_sum.channel(q);
#if __ARM_NEON
int nn = size >> 2;
int remain = size - (nn << 2);
#else
int remain = size;
#endif // __ARM_NEON
#if __ARM_NEON
for (; nn>0; nn--)
{
float32x4_t _sp = vld1q_f32(sptr);
float32x4_t _ssp = vld1q_f32(ssptr);
_ssp = vaddq_f32(_ssp, _sp);
vst1q_f32(ssptr, _ssp);
sptr += 4;
ssptr += 4;
}
#endif // __ARM_NEON
for (; remain>0; remain--)
{
*ssptr += *sptr;
sptr++;
ssptr++;
}
}
float* ptr = bottom_top_blob.channel(q);
float* ssptr = square_sum.channel(q);
#if __ARM_NEON
int nn = size >> 2;
int remain = size - (nn << 2);
#else
int remain = size;
#endif // __ARM_NEON
#if __ARM_NEON
float32x4_t _bias = vdupq_n_f32(bias);
float32x4_t _ads = vdupq_n_f32(alpha_div_size);
float32x4_t _mb = vdupq_n_f32(-beta);
for (; nn>0; nn--)
{
float32x4_t _p = vld1q_f32(ptr);
float32x4_t _ssp = vld1q_f32(ssptr);
_ssp = vmulq_f32(_ssp, _ads);
_ssp = vaddq_f32(_ssp, _bias);
_ssp = pow_ps(_ssp, _mb);
_p = vmulq_f32(_p, _ssp);
vst1q_f32(ptr, _p);
ssptr += 4;
ptr += 4;
}
#endif // __ARM_NEON
for (; remain>0; remain--)
{
*ptr = *ptr * pow(bias + alpha_div_size * *ssptr, -beta);
ssptr++;
ptr++;
}
}
}
static void OverdriveAndSuppressNEON(AecCore* aec,
float hNl[PART_LEN1],
const float hNlFb,
float efw[2][PART_LEN1]) {
int i;
const float32x4_t vec_hNlFb = vmovq_n_f32(hNlFb);
const float32x4_t vec_one = vdupq_n_f32(1.0f);
const float32x4_t vec_minus_one = vdupq_n_f32(-1.0f);
const float32x4_t vec_overDriveSm = vmovq_n_f32(aec->overDriveSm);
// vectorized code (four at once)
for (i = 0; i + 3 < PART_LEN1; i += 4) {
// Weight subbands
float32x4_t vec_hNl = vld1q_f32(&hNl[i]);
const float32x4_t vec_weightCurve = vld1q_f32(&WebRtcAec_weightCurve[i]);
const uint32x4_t bigger = vcgtq_f32(vec_hNl, vec_hNlFb);
const float32x4_t vec_weightCurve_hNlFb = vmulq_f32(vec_weightCurve,
vec_hNlFb);
const float32x4_t vec_one_weightCurve = vsubq_f32(vec_one, vec_weightCurve);
const float32x4_t vec_one_weightCurve_hNl = vmulq_f32(vec_one_weightCurve,
vec_hNl);
const uint32x4_t vec_if0 = vandq_u32(vmvnq_u32(bigger),
vreinterpretq_u32_f32(vec_hNl));
const float32x4_t vec_one_weightCurve_add =
vaddq_f32(vec_weightCurve_hNlFb, vec_one_weightCurve_hNl);
const uint32x4_t vec_if1 =
vandq_u32(bigger, vreinterpretq_u32_f32(vec_one_weightCurve_add));
vec_hNl = vreinterpretq_f32_u32(vorrq_u32(vec_if0, vec_if1));
{
const float32x4_t vec_overDriveCurve =
vld1q_f32(&WebRtcAec_overDriveCurve[i]);
const float32x4_t vec_overDriveSm_overDriveCurve =
vmulq_f32(vec_overDriveSm, vec_overDriveCurve);
vec_hNl = vpowq_f32(vec_hNl, vec_overDriveSm_overDriveCurve);
vst1q_f32(&hNl[i], vec_hNl);
}
// Suppress error signal
{
float32x4_t vec_efw_re = vld1q_f32(&efw[0][i]);
float32x4_t vec_efw_im = vld1q_f32(&efw[1][i]);
vec_efw_re = vmulq_f32(vec_efw_re, vec_hNl);
vec_efw_im = vmulq_f32(vec_efw_im, vec_hNl);
// Ooura fft returns incorrect sign on imaginary component. It matters
// here because we are making an additive change with comfort noise.
vec_efw_im = vmulq_f32(vec_efw_im, vec_minus_one);
vst1q_f32(&efw[0][i], vec_efw_re);
vst1q_f32(&efw[1][i], vec_efw_im);
}
}
// scalar code for the remaining items.
for (; i < PART_LEN1; i++) {
// Weight subbands
if (hNl[i] > hNlFb) {
hNl[i] = WebRtcAec_weightCurve[i] * hNlFb +
(1 - WebRtcAec_weightCurve[i]) * hNl[i];
}
hNl[i] = powf(hNl[i], aec->overDriveSm * WebRtcAec_overDriveCurve[i]);
// Suppress error signal
efw[0][i] *= hNl[i];
efw[1][i] *= hNl[i];
// Ooura fft returns incorrect sign on imaginary component. It matters
// here because we are making an additive change with comfort noise.
efw[1][i] *= -1;
}
}
static float32x4_t set_vector(float f)
{
return vdupq_n_f32(f);
}
// Updates the following smoothed Power Spectral Densities (PSD):
// - sd : near-end
// - se : residual echo
// - sx : far-end
// - sde : cross-PSD of near-end and residual echo
// - sxd : cross-PSD of near-end and far-end
//
// In addition to updating the PSDs, also the filter diverge state is determined
// upon actions are taken.
static void SmoothedPSD(AecCore* aec,
float efw[2][PART_LEN1],
float dfw[2][PART_LEN1],
float xfw[2][PART_LEN1],
int* extreme_filter_divergence) {
// Power estimate smoothing coefficients.
const float* ptrGCoh = aec->extended_filter_enabled
? WebRtcAec_kExtendedSmoothingCoefficients[aec->mult - 1]
: WebRtcAec_kNormalSmoothingCoefficients[aec->mult - 1];
int i;
float sdSum = 0, seSum = 0;
const float32x4_t vec_15 = vdupq_n_f32(WebRtcAec_kMinFarendPSD);
float32x4_t vec_sdSum = vdupq_n_f32(0.0f);
float32x4_t vec_seSum = vdupq_n_f32(0.0f);
for (i = 0; i + 3 < PART_LEN1; i += 4) {
const float32x4_t vec_dfw0 = vld1q_f32(&dfw[0][i]);
const float32x4_t vec_dfw1 = vld1q_f32(&dfw[1][i]);
const float32x4_t vec_efw0 = vld1q_f32(&efw[0][i]);
const float32x4_t vec_efw1 = vld1q_f32(&efw[1][i]);
const float32x4_t vec_xfw0 = vld1q_f32(&xfw[0][i]);
const float32x4_t vec_xfw1 = vld1q_f32(&xfw[1][i]);
float32x4_t vec_sd = vmulq_n_f32(vld1q_f32(&aec->sd[i]), ptrGCoh[0]);
float32x4_t vec_se = vmulq_n_f32(vld1q_f32(&aec->se[i]), ptrGCoh[0]);
float32x4_t vec_sx = vmulq_n_f32(vld1q_f32(&aec->sx[i]), ptrGCoh[0]);
float32x4_t vec_dfw_sumsq = vmulq_f32(vec_dfw0, vec_dfw0);
float32x4_t vec_efw_sumsq = vmulq_f32(vec_efw0, vec_efw0);
float32x4_t vec_xfw_sumsq = vmulq_f32(vec_xfw0, vec_xfw0);
vec_dfw_sumsq = vmlaq_f32(vec_dfw_sumsq, vec_dfw1, vec_dfw1);
vec_efw_sumsq = vmlaq_f32(vec_efw_sumsq, vec_efw1, vec_efw1);
vec_xfw_sumsq = vmlaq_f32(vec_xfw_sumsq, vec_xfw1, vec_xfw1);
vec_xfw_sumsq = vmaxq_f32(vec_xfw_sumsq, vec_15);
vec_sd = vmlaq_n_f32(vec_sd, vec_dfw_sumsq, ptrGCoh[1]);
vec_se = vmlaq_n_f32(vec_se, vec_efw_sumsq, ptrGCoh[1]);
vec_sx = vmlaq_n_f32(vec_sx, vec_xfw_sumsq, ptrGCoh[1]);
vst1q_f32(&aec->sd[i], vec_sd);
vst1q_f32(&aec->se[i], vec_se);
vst1q_f32(&aec->sx[i], vec_sx);
{
float32x4x2_t vec_sde = vld2q_f32(&aec->sde[i][0]);
float32x4_t vec_dfwefw0011 = vmulq_f32(vec_dfw0, vec_efw0);
float32x4_t vec_dfwefw0110 = vmulq_f32(vec_dfw0, vec_efw1);
vec_sde.val[0] = vmulq_n_f32(vec_sde.val[0], ptrGCoh[0]);
vec_sde.val[1] = vmulq_n_f32(vec_sde.val[1], ptrGCoh[0]);
vec_dfwefw0011 = vmlaq_f32(vec_dfwefw0011, vec_dfw1, vec_efw1);
vec_dfwefw0110 = vmlsq_f32(vec_dfwefw0110, vec_dfw1, vec_efw0);
vec_sde.val[0] = vmlaq_n_f32(vec_sde.val[0], vec_dfwefw0011, ptrGCoh[1]);
vec_sde.val[1] = vmlaq_n_f32(vec_sde.val[1], vec_dfwefw0110, ptrGCoh[1]);
vst2q_f32(&aec->sde[i][0], vec_sde);
}
{
float32x4x2_t vec_sxd = vld2q_f32(&aec->sxd[i][0]);
float32x4_t vec_dfwxfw0011 = vmulq_f32(vec_dfw0, vec_xfw0);
float32x4_t vec_dfwxfw0110 = vmulq_f32(vec_dfw0, vec_xfw1);
vec_sxd.val[0] = vmulq_n_f32(vec_sxd.val[0], ptrGCoh[0]);
vec_sxd.val[1] = vmulq_n_f32(vec_sxd.val[1], ptrGCoh[0]);
vec_dfwxfw0011 = vmlaq_f32(vec_dfwxfw0011, vec_dfw1, vec_xfw1);
vec_dfwxfw0110 = vmlsq_f32(vec_dfwxfw0110, vec_dfw1, vec_xfw0);
vec_sxd.val[0] = vmlaq_n_f32(vec_sxd.val[0], vec_dfwxfw0011, ptrGCoh[1]);
vec_sxd.val[1] = vmlaq_n_f32(vec_sxd.val[1], vec_dfwxfw0110, ptrGCoh[1]);
vst2q_f32(&aec->sxd[i][0], vec_sxd);
}
vec_sdSum = vaddq_f32(vec_sdSum, vec_sd);
vec_seSum = vaddq_f32(vec_seSum, vec_se);
}
{
float32x2_t vec_sdSum_total;
float32x2_t vec_seSum_total;
// A B C D
vec_sdSum_total = vpadd_f32(vget_low_f32(vec_sdSum),
vget_high_f32(vec_sdSum));
vec_seSum_total = vpadd_f32(vget_low_f32(vec_seSum),
vget_high_f32(vec_seSum));
// A+B C+D
vec_sdSum_total = vpadd_f32(vec_sdSum_total, vec_sdSum_total);
vec_seSum_total = vpadd_f32(vec_seSum_total, vec_seSum_total);
// A+B+C+D A+B+C+D
sdSum = vget_lane_f32(vec_sdSum_total, 0);
seSum = vget_lane_f32(vec_seSum_total, 0);
}
// scalar code for the remaining items.
for (; i < PART_LEN1; i++) {
aec->sd[i] = ptrGCoh[0] * aec->sd[i] +
ptrGCoh[1] * (dfw[0][i] * dfw[0][i] + dfw[1][i] * dfw[1][i]);
aec->se[i] = ptrGCoh[0] * aec->se[i] +
ptrGCoh[1] * (efw[0][i] * efw[0][i] + efw[1][i] * efw[1][i]);
// We threshold here to protect against the ill-effects of a zero farend.
// The threshold is not arbitrarily chosen, but balances protection and
// adverse interaction with the algorithm's tuning.
// TODO(bjornv): investigate further why this is so sensitive.
aec->sx[i] =
ptrGCoh[0] * aec->sx[i] +
ptrGCoh[1] * WEBRTC_SPL_MAX(
xfw[0][i] * xfw[0][i] + xfw[1][i] * xfw[1][i],
WebRtcAec_kMinFarendPSD);
aec->sde[i][0] =
ptrGCoh[0] * aec->sde[i][0] +
ptrGCoh[1] * (dfw[0][i] * efw[0][i] + dfw[1][i] * efw[1][i]);
aec->sde[i][1] =
ptrGCoh[0] * aec->sde[i][1] +
ptrGCoh[1] * (dfw[0][i] * efw[1][i] - dfw[1][i] * efw[0][i]);
aec->sxd[i][0] =
ptrGCoh[0] * aec->sxd[i][0] +
ptrGCoh[1] * (dfw[0][i] * xfw[0][i] + dfw[1][i] * xfw[1][i]);
aec->sxd[i][1] =
ptrGCoh[0] * aec->sxd[i][1] +
ptrGCoh[1] * (dfw[0][i] * xfw[1][i] - dfw[1][i] * xfw[0][i]);
sdSum += aec->sd[i];
seSum += aec->se[i];
}
// Divergent filter safeguard update.
aec->divergeState = (aec->divergeState ? 1.05f : 1.0f) * seSum > sdSum;
// Signal extreme filter divergence if the error is significantly larger
// than the nearend (13 dB).
*extreme_filter_divergence = (seSum > (19.95f * sdSum));
}
static void rftbsub_128_neon(float* a) {
const float* c = rdft_w + 32;
int j1, j2;
const float32x4_t mm_half = vdupq_n_f32(0.5f);
a[1] = -a[1];
// Vectorized code (four at once).
// Note: commented number are indexes for the first iteration of the loop.
for (j1 = 1, j2 = 2; j2 + 7 < 64; j1 += 4, j2 += 8) {
// Load 'wk'.
const float32x4_t c_j1 = vld1q_f32(&c[j1]); // 1, 2, 3, 4,
const float32x4_t c_k1 = vld1q_f32(&c[29 - j1]); // 28, 29, 30, 31,
const float32x4_t wkrt = vsubq_f32(mm_half, c_k1); // 28, 29, 30, 31,
const float32x4_t wkr_ = reverse_order_f32x4(wkrt); // 31, 30, 29, 28,
const float32x4_t wki_ = c_j1; // 1, 2, 3, 4,
// Load and shuffle 'a'.
// 2, 4, 6, 8, 3, 5, 7, 9
float32x4x2_t a_j2_p = vld2q_f32(&a[0 + j2]);
// 120, 122, 124, 126, 121, 123, 125, 127,
const float32x4x2_t k2_0_4 = vld2q_f32(&a[122 - j2]);
// 126, 124, 122, 120
const float32x4_t a_k2_p0 = reverse_order_f32x4(k2_0_4.val[0]);
// 127, 125, 123, 121
const float32x4_t a_k2_p1 = reverse_order_f32x4(k2_0_4.val[1]);
// Calculate 'x'.
const float32x4_t xr_ = vsubq_f32(a_j2_p.val[0], a_k2_p0);
// 2-126, 4-124, 6-122, 8-120,
const float32x4_t xi_ = vaddq_f32(a_j2_p.val[1], a_k2_p1);
// 3-127, 5-125, 7-123, 9-121,
// Calculate product into 'y'.
// yr = wkr * xr - wki * xi;
// yi = wkr * xi + wki * xr;
const float32x4_t a_ = vmulq_f32(wkr_, xr_);
const float32x4_t b_ = vmulq_f32(wki_, xi_);
const float32x4_t c_ = vmulq_f32(wkr_, xi_);
const float32x4_t d_ = vmulq_f32(wki_, xr_);
const float32x4_t yr_ = vaddq_f32(a_, b_); // 2-126, 4-124, 6-122, 8-120,
const float32x4_t yi_ = vsubq_f32(c_, d_); // 3-127, 5-125, 7-123, 9-121,
// Update 'a'.
// a[j2 + 0] -= yr;
// a[j2 + 1] -= yi;
// a[k2 + 0] += yr;
// a[k2 + 1] -= yi;
// 126, 124, 122, 120,
const float32x4_t a_k2_p0n = vaddq_f32(a_k2_p0, yr_);
// 127, 125, 123, 121,
const float32x4_t a_k2_p1n = vsubq_f32(yi_, a_k2_p1);
// Shuffle in right order and store.
// 2, 3, 4, 5, 6, 7, 8, 9,
const float32x4_t a_k2_p0nr = vrev64q_f32(a_k2_p0n);
const float32x4_t a_k2_p1nr = vrev64q_f32(a_k2_p1n);
// 124, 125, 126, 127, 120, 121, 122, 123
const float32x4x2_t a_k2_n = vzipq_f32(a_k2_p0nr, a_k2_p1nr);
// 2, 4, 6, 8,
a_j2_p.val[0] = vsubq_f32(a_j2_p.val[0], yr_);
// 3, 5, 7, 9,
a_j2_p.val[1] = vsubq_f32(yi_, a_j2_p.val[1]);
// 2, 3, 4, 5, 6, 7, 8, 9,
vst2q_f32(&a[0 + j2], a_j2_p);
vst1q_f32(&a[122 - j2], a_k2_n.val[1]);
vst1q_f32(&a[126 - j2], a_k2_n.val[0]);
}
// Scalar code for the remaining items.
for (; j2 < 64; j1 += 1, j2 += 2) {
const int k2 = 128 - j2;
const int k1 = 32 - j1;
const float wkr = 0.5f - c[k1];
const float wki = c[j1];
const float xr = a[j2 + 0] - a[k2 + 0];
const float xi = a[j2 + 1] + a[k2 + 1];
const float yr = wkr * xr + wki * xi;
const float yi = wkr * xi - wki * xr;
a[j2 + 0] = a[j2 + 0] - yr;
a[j2 + 1] = yi - a[j2 + 1];
a[k2 + 0] = yr + a[k2 + 0];
a[k2 + 1] = yi - a[k2 + 1];
}
a[65] = -a[65];
}
void sEnv_process(HvBase *_c, SignalEnvelope *o, hv_bInf_t bIn,
void (*sendMessage)(HvBase *, int, const HvMessage *)) {
#if HV_SIMD_AVX
_mm256_stream_ps(o->buffer+o->numSamplesInBuffer, _mm256_mul_ps(bIn,bIn)); // store bIn^2, no need to cache block
o->numSamplesInBuffer += HV_N_SIMD;
if (o->numSamplesInBuffer >= o->windowSize) {
int n4 = o->windowSize & ~HV_N_SIMD_MASK;
__m256 sum = _mm256_setzero_ps();
while (n4) {
__m256 x = _mm256_load_ps(o->buffer + n4 - HV_N_SIMD);
__m256 h = _mm256_load_ps(o->hanningWeights + n4 - HV_N_SIMD);
x = _mm256_mul_ps(x, h);
sum = _mm256_add_ps(sum, x);
n4 -= HV_N_SIMD;
}
sum = _mm256_hadd_ps(sum,sum); // horizontal sum
sum = _mm256_hadd_ps(sum,sum);
sEnv_sendMessage(_c, o, sum[0]+sum[4], sendMessage); // updates numSamplesInBuffer
}
#elif HV_SIMD_SSE
_mm_stream_ps(o->buffer+o->numSamplesInBuffer, _mm_mul_ps(bIn,bIn)); // store bIn^2, no need to cache block
o->numSamplesInBuffer += HV_N_SIMD;
if (o->numSamplesInBuffer >= o->windowSize) {
int n4 = o->windowSize & ~HV_N_SIMD_MASK;
__m128 sum = _mm_setzero_ps();
while (n4) {
__m128 x = _mm_load_ps(o->buffer + n4 - HV_N_SIMD);
__m128 h = _mm_load_ps(o->hanningWeights + n4 - HV_N_SIMD);
x = _mm_mul_ps(x, h);
sum = _mm_add_ps(sum, x);
n4 -= HV_N_SIMD;
}
sum = _mm_hadd_ps(sum,sum); // horizontal sum
sum = _mm_hadd_ps(sum,sum);
sEnv_sendMessage(_c, o, sum[0], sendMessage);
}
#elif HV_SIMD_NEON
vst1q_f32(o->buffer+o->numSamplesInBuffer, vmulq_f32(bIn,bIn)); // store bIn^2, no need to cache block
o->numSamplesInBuffer += HV_N_SIMD;
if (o->numSamplesInBuffer >= o->windowSize) {
int n4 = o->windowSize & ~HV_N_SIMD_MASK;
float32x4_t sum = vdupq_n_f32(0.0f);
while (n4) {
float32x4_t x = vld1q_f32(o->buffer + n4 - HV_N_SIMD);
float32x4_t h = vld1q_f32(o->hanningWeights + n4 - HV_N_SIMD);
x = vmulq_f32(x, h);
sum = vaddq_f32(sum, x);
n4 -= HV_N_SIMD;
}
sEnv_sendMessage(_c, o, sum[0]+sum[1]+sum[2]+sum[3], sendMessage);
}
#else // HV_SIMD_NONE
o->buffer[o->numSamplesInBuffer] = (bIn*bIn);
o->numSamplesInBuffer += HV_N_SIMD;
if (o->numSamplesInBuffer >= o->windowSize) {
float sum = 0.0f;
for (int i = 0; i < o->windowSize; ++i) {
sum += (o->hanningWeights[i] * o->buffer[i]);
}
sEnv_sendMessage(_c, o, sum, sendMessage);
}
#endif
}
void sLine_onMessage(HvBase *_c, SignalLine *o, int letIn,
const HvMessage * const m, void *sendMessage) {
if (msg_isFloat(m,0)) {
if (msg_isFloat(m,1)) {
// new ramp
int n = ctx_millisecondsToSamples(_c, msg_getFloat(m,1));
#if HV_SIMD_AVX
float x = (o->n[1] > 0) ? (o->x[7] + (o->m[7]/8.0f)) : o->t[7]; // current output value
float s = (msg_getFloat(m,0) - x) / ((float) n); // slope per sample
o->n = _mm_set_epi32(n-3, n-2, n-1, n);
o->x = _mm256_set_ps(x+7.0f*s, x+6.0f*s, x+5.0f*s, x+4.0f*s, x+3.0f*s, x+2.0f*s, x+s, x);
o->m = _mm256_set1_ps(8.0f*s);
o->t = _mm256_set1_ps(msg_getFloat(m,0));
#elif HV_SIMD_SSE
float x = (o->n[1] > 0) ? (o->x[3] + (o->m[3]/4.0f)) : o->t[3];
float s = (msg_getFloat(m,0) - x) / ((float) n); // slope per sample
o->n = _mm_set_epi32(n-3, n-2, n-1, n);
o->x = _mm_set_ps(x+3.0f*s, x+2.0f*s, x+s, x);
o->m = _mm_set1_ps(4.0f*s);
o->t = _mm_set1_ps(msg_getFloat(m,0));
#elif HV_SIMD_NEON
float x = (o->n[3] > 0) ? (o->x[3] + (o->m[3]/4.0f)) : o->t[3];
float s = (msg_getFloat(m,0) - x) / ((float) n);
o->n = (int32x4_t) {
n, n-1, n-2, n-3
};
o->x = (float32x4_t) {
x, x+s, x+2.0f*s, x+3.0f*s
};
o->m = vdupq_n_f32(4.0f*s);
o->t = vdupq_n_f32(msg_getFloat(m,0));
#else // HV_SIMD_NONE
o->x = (o->n > 0) ? (o->x + o->m) : o->t; // new current value
o->n = n; // new distance to target
o->m = (msg_getFloat(m,0) - o->x) / ((float) n); // slope per sample
o->t = msg_getFloat(m,0);
#endif
} else {
// Jump to value
#if HV_SIMD_AVX
o->n = _mm_setzero_si128();
o->x = _mm256_set1_ps(msg_getFloat(m,0));
o->m = _mm256_setzero_ps();
o->t = _mm256_set1_ps(msg_getFloat(m,0));
#elif HV_SIMD_SSE
o->n = _mm_setzero_si128();
o->x = _mm_set1_ps(msg_getFloat(m,0));
o->m = _mm_setzero_ps();
o->t = _mm_set1_ps(msg_getFloat(m,0));
#elif HV_SIMD_NEON
o->n = vdupq_n_s32(0);
o->x = vdupq_n_f32(0.0f);
o->m = vdupq_n_f32(0.0f);
o->t = vdupq_n_f32(0.0f);
#else // HV_SIMD_NONE
o->n = 0;
o->x = msg_getFloat(m,0);
o->m = 0.0f;
o->t = msg_getFloat(m,0);
#endif
}
} else if (msg_compareSymbol(m,0,"stop")) {
// Stop line at current position
#if HV_SIMD_AVX
// note o->n[1] is a 64-bit integer; two packed 32-bit ints. We only want to know if the high int is positive,
// which can be done simply by testing the long int for positiveness.
float x = (o->n[1] > 0) ? (o->x[7] + (o->m[7]/8.0f)) : o->t[7];
o->n = _mm_setzero_si128();
o->x = _mm256_set1_ps(x);
o->m = _mm256_setzero_ps();
o->t = _mm256_set1_ps(x);
#elif HV_SIMD_SSE
float x = (o->n[1] > 0) ? (o->x[3] + (o->m[3]/4.0f)) : o->t[3];
o->n = _mm_setzero_si128();
o->x = _mm_set1_ps(x);
o->m = _mm_setzero_ps();
o->t = _mm_set1_ps(x);
#elif HV_SIMD_NEON
float x = (o->n[3] > 0) ? (o->x[3] + (o->m[3]/4.0f)) : o->t[3];
o->n = vdupq_n_s32(0);
o->x = vdupq_n_f32(x);
o->m = vdupq_n_f32(0.0f);
o->t = vdupq_n_f32(x);
#else // HV_SIMD_NONE
o->n = 0;
o->x += o->m;
o->m = 0.0f;
o->t = o->x;
#endif
}
}
void phase(const Size2D &size,
const s16 * src0Base, ptrdiff_t src0Stride,
const s16 * src1Base, ptrdiff_t src1Stride,
u8 * dstBase, ptrdiff_t dstStride)
{
internal::assertSupportedConfiguration();
#ifdef CAROTENE_NEON
FASTATAN2CONST(256.0f / 360.0f)
size_t roiw16 = size.width >= 15 ? size.width - 15 : 0;
size_t roiw8 = size.width >= 7 ? size.width - 7 : 0;
float32x4_t v_05 = vdupq_n_f32(0.5f);
for (size_t i = 0; i < size.height; ++i)
{
const s16 * src0 = internal::getRowPtr(src0Base, src0Stride, i);
const s16 * src1 = internal::getRowPtr(src1Base, src1Stride, i);
u8 * dst = internal::getRowPtr(dstBase, dstStride, i);
size_t j = 0;
for (; j < roiw16; j += 16)
{
internal::prefetch(src0 + j);
internal::prefetch(src1 + j);
int16x8_t v_src00 = vld1q_s16(src0 + j), v_src01 = vld1q_s16(src0 + j + 8);
int16x8_t v_src10 = vld1q_s16(src1 + j), v_src11 = vld1q_s16(src1 + j + 8);
// 0
float32x4_t v_src0_p = vcvtq_f32_s32(vmovl_s16(vget_low_s16(v_src00)));
float32x4_t v_src1_p = vcvtq_f32_s32(vmovl_s16(vget_low_s16(v_src10)));
float32x4_t v_dst32f0;
FASTATAN2VECTOR(v_src1_p, v_src0_p, v_dst32f0)
v_src0_p = vcvtq_f32_s32(vmovl_s16(vget_high_s16(v_src00)));
v_src1_p = vcvtq_f32_s32(vmovl_s16(vget_high_s16(v_src10)));
float32x4_t v_dst32f1;
FASTATAN2VECTOR(v_src1_p, v_src0_p, v_dst32f1)
uint16x8_t v_dst16s0 = vcombine_u16(vmovn_u32(vcvtq_u32_f32(vaddq_f32(v_dst32f0, v_05))),
vmovn_u32(vcvtq_u32_f32(vaddq_f32(v_dst32f1, v_05))));
// 1
v_src0_p = vcvtq_f32_s32(vmovl_s16(vget_low_s16(v_src01)));
v_src1_p = vcvtq_f32_s32(vmovl_s16(vget_low_s16(v_src11)));
FASTATAN2VECTOR(v_src1_p, v_src0_p, v_dst32f0)
v_src0_p = vcvtq_f32_s32(vmovl_s16(vget_high_s16(v_src01)));
v_src1_p = vcvtq_f32_s32(vmovl_s16(vget_high_s16(v_src11)));
FASTATAN2VECTOR(v_src1_p, v_src0_p, v_dst32f1)
uint16x8_t v_dst16s1 = vcombine_u16(vmovn_u32(vcvtq_u32_f32(vaddq_f32(v_dst32f0, v_05))),
vmovn_u32(vcvtq_u32_f32(vaddq_f32(v_dst32f1, v_05))));
vst1q_u8(dst + j, vcombine_u8(vmovn_u16(v_dst16s0),
vmovn_u16(v_dst16s1)));
}
for (; j < roiw8; j += 8)
{
int16x8_t v_src0 = vld1q_s16(src0 + j);
int16x8_t v_src1 = vld1q_s16(src1 + j);
float32x4_t v_src0_p = vcvtq_f32_s32(vmovl_s16(vget_low_s16(v_src0)));
float32x4_t v_src1_p = vcvtq_f32_s32(vmovl_s16(vget_low_s16(v_src1)));
float32x4_t v_dst32f0;
FASTATAN2VECTOR(v_src1_p, v_src0_p, v_dst32f0)
v_src0_p = vcvtq_f32_s32(vmovl_s16(vget_high_s16(v_src0)));
v_src1_p = vcvtq_f32_s32(vmovl_s16(vget_high_s16(v_src1)));
float32x4_t v_dst32f1;
FASTATAN2VECTOR(v_src1_p, v_src0_p, v_dst32f1)
uint16x8_t v_dst = vcombine_u16(vmovn_u32(vcvtq_u32_f32(vaddq_f32(v_dst32f0, v_05))),
vmovn_u32(vcvtq_u32_f32(vaddq_f32(v_dst32f1, v_05))));
vst1_u8(dst + j, vmovn_u16(v_dst));
}
for (; j < size.width; j++)
{
f32 x = src0[j], y = src1[j];
f32 a;
FASTATAN2SCALAR(y, x, a)
dst[j] = (u8)(s32)floor(a + 0.5f);
}
}
#else
(void)size;
(void)src0Base;
(void)src0Stride;
(void)src1Base;
(void)src1Stride;
(void)dstBase;
(void)dstStride;
#endif
}
namespace Ogre
{
const ArrayReal MathlibNEON::HALF = vdupq_n_f32( 0.5f );
const ArrayReal MathlibNEON::ONE = vdupq_n_f32( 1.0f );
const ArrayReal MathlibNEON::THREE = vdupq_n_f32( 3.0f );
const ArrayReal MathlibNEON::NEG_ONE = vdupq_n_f32( -1.0f );
const ArrayReal MathlibNEON::fEpsilon = vdupq_n_f32( 1e-6f );
const ArrayReal MathlibNEON::fSqEpsilon = vdupq_n_f32( 1e-12f );
const ArrayReal MathlibNEON::OneMinusEpsilon= vdupq_n_f32( 1.0f - 1e-6f );
const ArrayReal MathlibNEON::FLOAT_MIN = vdupq_n_f32( std::numeric_limits<Real>::min() );
const ArrayReal MathlibNEON::SIGN_MASK = vdupq_n_f32( -0.0f );
const ArrayReal MathlibNEON::INFINITEA = vdupq_n_f32( std::numeric_limits<Real>::infinity() );
const ArrayReal MathlibNEON::MAX_NEG = vdupq_n_f32( -std::numeric_limits<Real>::max() );
const ArrayReal MathlibNEON::MAX_POS = vdupq_n_f32( std::numeric_limits<Real>::max() );
const ArrayReal MathlibNEON::LAST_AFFINE_COLUMN = (ArrayReal) { 1, 0, 0, 0 };
static const Real _PI = Real( 4.0 * atan( 1.0 ) );
//We can't use Math::fDeg2Rad & Math::fRad2Deg directly because
//it's not guaranteed to have been initialized first
const ArrayReal MathlibNEON::PI = vdupq_n_f32( _PI );
const ArrayReal MathlibNEON::TWO_PI = vdupq_n_f32( 2.0f * _PI );
const ArrayReal MathlibNEON::fDeg2Rad = vdupq_n_f32( _PI / 180.0f );
const ArrayReal MathlibNEON::fRad2Deg = vdupq_n_f32( 180.0f / _PI );
const ArrayReal MathlibNEON::ONE_DIV_2PI= vdupq_n_f32( 1.0f / (2.0f * _PI) );
//-----------------------------------------------------------------------------------
ArrayReal MathlibNEON::Sin4( ArrayReal x )
{
// Map arbitrary angle x to the range [-pi; +pi] without using division.
// Code taken from MSDN's HLSL trick. Architectures with fused mad (i.e. NEON)
// can replace the add, the sub, & the two muls for two mad
ArrayReal integralPart;
x = vaddq_f32( vmulq_f32( x, ONE_DIV_2PI ), HALF );
x = Modf4( x, integralPart );
x = vsubq_f32( vmulq_f32( x, TWO_PI ), PI );
return sin_ps( x );
}
//-----------------------------------------------------------------------------------
ArrayReal MathlibNEON::Cos4( ArrayReal x )
{
// Map arbitrary angle x to the range [-pi; +pi] without using division.
// Code taken from MSDN's HLSL trick. Architectures with fused mad (i.e. NEON)
// can replace the add, the sub, & the two muls for two mad
ArrayReal integralPart;
x = vaddq_f32( vmulq_f32( x, ONE_DIV_2PI ), HALF );
x = Modf4( x, integralPart );
x = vsubq_f32( vmulq_f32( x, TWO_PI ), PI );
return cos_ps( x );
}
//-----------------------------------------------------------------------------------
void MathlibNEON::SinCos4( ArrayReal x, ArrayReal &outSin, ArrayReal &outCos )
{
// TODO: Improve accuracy by mapping to the range [-pi/4, pi/4] and swap
// between cos & sin depending on which quadrant it fell:
// Quadrant | sin | cos
// n = 0 -> sin( x ), cos( x )
// n = 1 -> cos( x ), -sin( x )
// n = 2 -> -sin( x ), -cos( x )
// n = 3 -> -sin( x ), sin( x )
// See ARGUMENT REDUCTION FOR HUGE ARGUMENTS:
// Good to the Last Bit
// K. C. Ng and themembers of the FP group of SunPro
// http://www.derekroconnor.net/Software/Ng--ArgReduction.pdf
// -- Perhaps we can leave this to GSoC students? --
// Map arbitrary angle x to the range [-pi; +pi] without using division.
// Code taken from MSDN's HLSL trick. Architectures with fused mad (i.e. NEON)
// can replace the add, the sub, & the two muls for two mad
ArrayReal integralPart;
x = vaddq_f32( vmulq_f32( x, ONE_DIV_2PI ), HALF );
x = Modf4( x, integralPart );
x = vsubq_f32( vmulq_f32( x, TWO_PI ), PI );
sincos_ps( x, &outSin, &outCos );
}
const ArrayMaskR BooleanMask4::mMasks[NUM_MASKS] =
{
(ArrayMaskR) { 0x00000000, 0x00000000, 0x00000000, 0x00000000 },//MASK_NONE
(ArrayMaskR) { 0xffffffff, 0x00000000, 0x00000000, 0x00000000 },//MASK_X
(ArrayMaskR) { 0x00000000, 0xffffffff, 0x00000000, 0x00000000 },//MASK_Y
(ArrayMaskR) { 0xffffffff, 0xffffffff, 0x00000000, 0x00000000 },//MASK_XY
(ArrayMaskR) { 0x00000000, 0x00000000, 0xffffffff, 0x00000000 },//MASK_Z
(ArrayMaskR) { 0xffffffff, 0x00000000, 0xffffffff, 0x00000000 },//MASK_XZ
(ArrayMaskR) { 0x00000000, 0xffffffff, 0xffffffff, 0x00000000 },//MASK_YZ
(ArrayMaskR) { 0xffffffff, 0xffffffff, 0xffffffff, 0x00000000 },//MASK_XYZ
(ArrayMaskR) { 0x00000000, 0x00000000, 0x00000000, 0xffffffff },//MASK_W
(ArrayMaskR) { 0xffffffff, 0x00000000, 0x00000000, 0xffffffff },//MASK_XW
(ArrayMaskR) { 0x00000000, 0xffffffff, 0x00000000, 0xffffffff },//MASK_YW
(ArrayMaskR) { 0xffffffff, 0xffffffff, 0x00000000, 0xffffffff },//MASK_XYW
(ArrayMaskR) { 0x00000000, 0x00000000, 0xffffffff, 0xffffffff },//MASK_ZW
(ArrayMaskR) { 0xffffffff, 0x00000000, 0xffffffff, 0xffffffff },//MASK_XZW
(ArrayMaskR) { 0x00000000, 0xffffffff, 0xffffffff, 0xffffffff },//MASK_YZW
(ArrayMaskR) { 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff } //MASK_XYZW
};
}
void test_vdupq_nf32 (void)
{
out_float32x4_t = vdupq_n_f32 (0.125);
}
