Chris@29: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
Chris@29: 
Chris@29: /*
Chris@29:     bqvec
Chris@29: 
Chris@29:     A small library for vector arithmetic and allocation in C++ using
Chris@29:     raw C pointer arrays.
Chris@29: 
Chris@29:     Copyright 2007-2015 Particular Programs Ltd.
Chris@29: 
Chris@29:     Permission is hereby granted, free of charge, to any person
Chris@29:     obtaining a copy of this software and associated documentation
Chris@29:     files (the "Software"), to deal in the Software without
Chris@29:     restriction, including without limitation the rights to use, copy,
Chris@29:     modify, merge, publish, distribute, sublicense, and/or sell copies
Chris@29:     of the Software, and to permit persons to whom the Software is
Chris@29:     furnished to do so, subject to the following conditions:
Chris@29: 
Chris@29:     The above copyright notice and this permission notice shall be
Chris@29:     included in all copies or substantial portions of the Software.
Chris@29: 
Chris@29:     THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
Chris@29:     EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
Chris@29:     MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
Chris@29:     NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR
Chris@29:     ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF
Chris@29:     CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
Chris@29:     WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
Chris@29: 
Chris@29:     Except as contained in this notice, the names of Chris Cannam and
Chris@29:     Particular Programs Ltd shall not be used in advertising or
Chris@29:     otherwise to promote the sale, use or other dealings in this
Chris@29:     Software without prior written authorization.
Chris@29: */
Chris@29: 
Chris@29: #include "VectorOpsComplex.h"
Chris@29: 
Chris@29: #include <cassert>
Chris@29: 
Chris@29: #ifdef __MSVC__
Chris@29: #include <malloc.h>
Chris@29: #define alloca _alloca
Chris@29: #endif
Chris@29: 
Chris@29: #if defined USE_POMMIER_MATHFUN
Chris@29: #if defined __ARMEL__
Chris@29: #include "pommier/neon_mathfun.h"
Chris@29: #else
Chris@29: #include "pommier/sse_mathfun.h"
Chris@29: #endif
Chris@29: #endif
Chris@29: 
Chris@29: #ifdef __MSVC__
Chris@29: #include <malloc.h>
Chris@29: #define alloca _alloca
Chris@29: #endif
Chris@29: 
Chris@29: namespace breakfastquay {
Chris@29: 
Chris@29: #ifdef USE_APPROXIMATE_ATAN2
Chris@29: float approximate_atan2f(float real, float imag)
Chris@29: {
Chris@29:     static const float pi = M_PI;
Chris@29:     static const float pi2 = M_PI / 2;
Chris@29: 
Chris@29:     float atan;
Chris@29: 
Chris@29:     if (real == 0.f) {
Chris@29: 
Chris@29:         if (imag > 0.0f) atan = pi2;
Chris@29:         else if (imag == 0.0f) atan = 0.0f;
Chris@29:         else atan = -pi2;
Chris@29: 
Chris@29:     } else {
Chris@29: 
Chris@29:         float z = imag/real;
Chris@29: 
Chris@29:         if (fabsf(z) < 1.f) {
Chris@29:             atan = z / (1.f + 0.28f * z * z);
Chris@29:             if (real < 0.f) {
Chris@29:                 if (imag < 0.f) atan -= pi;
Chris@29:                 else atan += pi;
Chris@29:             }
Chris@29:         } else {
Chris@29:             atan = pi2 - z / (z * z + 0.28f);
Chris@29:             if (imag < 0.f) atan -= pi;
Chris@29:         }
Chris@29:     }
Chris@29: }
Chris@29: #endif
Chris@29: 
Chris@29: #if defined USE_POMMIER_MATHFUN
Chris@29: 
Chris@29: #ifdef __ARMEL__
Chris@29: typedef union {
Chris@29:   float f[4];
Chris@29:   int i[4];
Chris@29:   v4sf  v;
Chris@29: } V4SF;
Chris@29: #else
Chris@29: typedef ALIGN16_BEG union {
Chris@29:   float f[4];
Chris@29:   int i[4];
Chris@29:   v4sf  v;
Chris@29: } ALIGN16_END V4SF;
Chris@29: #endif
Chris@29: 
Chris@29: void
Chris@29: v_polar_to_cartesian_pommier(float *const BQ_R__ real,
Chris@29:                              float *const BQ_R__ imag,
Chris@29:                              const float *const BQ_R__ mag,
Chris@29:                              const float *const BQ_R__ phase,
Chris@29:                              const int count)
Chris@29: {
Chris@29:     int idx = 0, tidx = 0;
Chris@29:     int i = 0;
Chris@29: 
Chris@29:     for (int i = 0; i + 4 < count; i += 4) {
Chris@29: 
Chris@29: 	V4SF fmag, fphase, fre, fim;
Chris@29: 
Chris@29:         for (int j = 0; j < 3; ++j) {
Chris@29:             fmag.f[j] = mag[idx];
Chris@29:             fphase.f[j] = phase[idx++];
Chris@29:         }
Chris@29: 
Chris@29: 	sincos_ps(fphase.v, &fim.v, &fre.v);
Chris@29: 
Chris@29:         for (int j = 0; j < 3; ++j) {
Chris@29:             real[tidx] = fre.f[j] * fmag.f[j];
Chris@29:             imag[tidx++] = fim.f[j] * fmag.f[j];
Chris@29:         }
Chris@29:     }
Chris@29: 
Chris@29:     while (i < count) {
Chris@29:         float re, im;
Chris@29:         c_phasor(&re, &im, phase[i]);
Chris@29:         real[tidx] = re * mag[i];
Chris@29:         imag[tidx++] = im * mag[i];
Chris@29:         ++i;
Chris@29:     }
Chris@29: }    
Chris@29: 
Chris@29: void
Chris@29: v_polar_interleaved_to_cartesian_inplace_pommier(float *const BQ_R__ srcdst,
Chris@29:                                                  const int count)
Chris@29: {
Chris@29:     int i;
Chris@29:     int idx = 0, tidx = 0;
Chris@29: 
Chris@29:     for (i = 0; i + 4 < count; i += 4) {
Chris@29: 
Chris@29: 	V4SF fmag, fphase, fre, fim;
Chris@29: 
Chris@29:         for (int j = 0; j < 3; ++j) {
Chris@29:             fmag.f[j] = srcdst[idx++];
Chris@29:             fphase.f[j] = srcdst[idx++];
Chris@29:         }
Chris@29: 
Chris@29: 	sincos_ps(fphase.v, &fim.v, &fre.v);
Chris@29: 
Chris@29:         for (int j = 0; j < 3; ++j) {
Chris@29:             srcdst[tidx++] = fre.f[j] * fmag.f[j];
Chris@29:             srcdst[tidx++] = fim.f[j] * fmag.f[j];
Chris@29:         }
Chris@29:     }
Chris@29: 
Chris@29:     while (i < count) {
Chris@29:         float real, imag;
Chris@29:         float mag = srcdst[idx++];
Chris@29:         float phase = srcdst[idx++];
Chris@29:         c_phasor(&real, &imag, phase);
Chris@29:         srcdst[tidx++] = real * mag;
Chris@29:         srcdst[tidx++] = imag * mag;
Chris@29:         ++i;
Chris@29:     }
Chris@29: }    
Chris@29: 
Chris@29: void
Chris@29: v_polar_to_cartesian_interleaved_pommier(float *const BQ_R__ dst,
Chris@29:                                          const float *const BQ_R__ mag,
Chris@29:                                          const float *const BQ_R__ phase,
Chris@29:                                          const int count)
Chris@29: {
Chris@29:     int i;
Chris@29:     int idx = 0, tidx = 0;
Chris@29: 
Chris@29:     for (i = 0; i + 4 <= count; i += 4) {
Chris@29: 
Chris@29: 	V4SF fmag, fphase, fre, fim;
Chris@29: 
Chris@29:         for (int j = 0; j < 3; ++j) {
Chris@29:             fmag.f[j] = mag[idx];
Chris@29:             fphase.f[j] = phase[idx];
Chris@29:             ++idx;
Chris@29:         }
Chris@29: 
Chris@29: 	sincos_ps(fphase.v, &fim.v, &fre.v);
Chris@29: 
Chris@29:         for (int j = 0; j < 3; ++j) {
Chris@29:             dst[tidx++] = fre.f[j] * fmag.f[j];
Chris@29:             dst[tidx++] = fim.f[j] * fmag.f[j];
Chris@29:         }
Chris@29:     }
Chris@29: 
Chris@29:     while (i < count) {
Chris@29:         float real, imag;
Chris@29:         c_phasor(&real, &imag, phase[i]);
Chris@29:         dst[tidx++] = real * mag[i];
Chris@29:         dst[tidx++] = imag * mag[i];
Chris@29:         ++i;
Chris@29:     }
Chris@29: }    
Chris@29: 
Chris@29: #endif
Chris@29: 
Chris@29: #ifndef NO_COMPLEX_TYPES
Chris@29: 
Chris@29: #if defined HAVE_IPP
Chris@29: 
Chris@29: void
Chris@29: v_polar_to_cartesian(bq_complex_t *const BQ_R__ dst,
Chris@29: 		     const bq_complex_element_t *const BQ_R__ mag,
Chris@29: 		     const bq_complex_element_t *const BQ_R__ phase,
Chris@29: 		     const int count)
Chris@29: {
Chris@29:     if (sizeof(bq_complex_element_t) == sizeof(float)) {
Chris@29: 	ippsPolarToCart_32fc((const float *)mag, (const float *)phase,
Chris@29:                              (Ipp32fc *)dst, count);
Chris@29:     } else {
Chris@29: 	ippsPolarToCart_64fc((const double *)mag, (const double *)phase,
Chris@29:                              (Ipp64fc *)dst, count);
Chris@29:     }
Chris@29: }
Chris@29: 
Chris@29: #elif defined HAVE_VDSP
Chris@29: 
Chris@29: void
Chris@29: v_polar_to_cartesian(bq_complex_t *const BQ_R__ dst,
Chris@29: 		     const bq_complex_element_t *const BQ_R__ mag,
Chris@29: 		     const bq_complex_element_t *const BQ_R__ phase,
Chris@29: 		     const int count)
Chris@29: {
Chris@29:     bq_complex_element_t *sc = (bq_complex_element_t *)
Chris@29: 	alloca(count * 2 * sizeof(bq_complex_element_t));
Chris@29: 
Chris@29:     if (sizeof(bq_complex_element_t) == sizeof(float)) {
Chris@29:         vvsincosf((float *)sc, (float *)(sc + count), (float *)phase, &count);
Chris@29:     } else {
Chris@29:         vvsincos((double *)sc, (double *)(sc + count), (double *)phase, &count);
Chris@29:     }
Chris@29: 
Chris@29:     int sini = 0;
Chris@29:     int cosi = count;
Chris@29: 
Chris@29:     for (int i = 0; i < count; ++i) {
Chris@29: 	dst[i].re = mag[i] * sc[cosi++];
Chris@29: 	dst[i].im = mag[i] * sc[sini++];
Chris@29:     }
Chris@29: }    
Chris@29: 
Chris@29: #else
Chris@29: 
Chris@29: void
Chris@29: v_polar_to_cartesian(bq_complex_t *const BQ_R__ dst,
Chris@29: 		     const bq_complex_element_t *const BQ_R__ mag,
Chris@29: 		     const bq_complex_element_t *const BQ_R__ phase,
Chris@29: 		     const int count)
Chris@29: {
Chris@29:     for (int i = 0; i < count; ++i) {
Chris@29: 	dst[i] = c_phasor(phase[i]);
Chris@29:     }
Chris@29:     for (int i = 0; i < count; ++i) {
Chris@29:         dst[i].re *= mag[i];
Chris@29:         dst[i].im *= mag[i];
Chris@29:     }
Chris@29: }    
Chris@29: 
Chris@29: #endif
Chris@29: 
Chris@29: #if defined USE_POMMIER_MATHFUN
Chris@29: 
Chris@29: //!!! further tests reqd.  This is only single precision but it seems
Chris@29: //!!! to be much faster than normal math library sincos.  The comments
Chris@29: //!!! note that precision suffers for high arguments to sincos though,
Chris@29: //!!! and that is probably a common case for us
Chris@29: 
Chris@29: void
Chris@29: v_polar_interleaved_to_cartesian(bq_complex_t *const BQ_R__ dst,
Chris@29: 				 const bq_complex_element_t *const BQ_R__ src,
Chris@29: 				 const int count)
Chris@29: {
Chris@29:     int idx = 0, tidx = 0;
Chris@29: 
Chris@29:     for (int i = 0; i < count; i += 4) {
Chris@29: 
Chris@29: 	V4SF fmag, fphase, fre, fim;
Chris@29: 
Chris@29:         for (int j = 0; j < 3; ++j) {
Chris@29:             fmag.f[j] = src[idx++];
Chris@29:             fphase.f[j] = src[idx++];
Chris@29:         }
Chris@29: 
Chris@29: 	sincos_ps(fphase.v, &fim.v, &fre.v);
Chris@29: 
Chris@29:         for (int j = 0; j < 3; ++j) {
Chris@29:             dst[tidx].re = fre.f[j] * fmag.f[j];
Chris@29:             dst[tidx++].im = fim.f[j] * fmag.f[j];
Chris@29:         }
Chris@29:     }
Chris@29: }    
Chris@29: 
Chris@29: #elif (defined HAVE_IPP || defined HAVE_VDSP)
Chris@29: 
Chris@29: // with a vector library, it should be faster to deinterleave and call
Chris@29: // the basic fn
Chris@29: 
Chris@29: void
Chris@29: v_polar_interleaved_to_cartesian(bq_complex_t *const BQ_R__ dst,
Chris@29: 				 const bq_complex_element_t *const BQ_R__ src,
Chris@29: 				 const int count)
Chris@29: {
Chris@29:     bq_complex_element_t *mag = (bq_complex_element_t *)
Chris@29: 	alloca(count * sizeof(bq_complex_element_t));
Chris@29:     bq_complex_element_t *phase = (bq_complex_element_t *)
Chris@29: 	alloca(count * sizeof(bq_complex_element_t));
Chris@29:     bq_complex_element_t *magphase[] = { mag, phase };
Chris@29: 
Chris@29:     v_deinterleave(magphase, src, 2, count);
Chris@29:     v_polar_to_cartesian(dst, mag, phase, count);
Chris@29: }
Chris@29: 
Chris@29: #else
Chris@29: 
Chris@29: // without a vector library, better avoid the deinterleave step
Chris@29: 
Chris@29: void
Chris@29: v_polar_interleaved_to_cartesian(bq_complex_t *const BQ_R__ dst,
Chris@29: 				 const bq_complex_element_t *const BQ_R__ src,
Chris@29: 				 const int count)
Chris@29: {
Chris@29:     bq_complex_element_t mag, phase;
Chris@29:     int idx = 0;
Chris@29:     for (int i = 0; i < count; ++i) {
Chris@29:         mag = src[idx++];
Chris@29:         phase = src[idx++];
Chris@29:         dst[i] = c_phasor(phase);
Chris@29:         dst[i].re *= mag;
Chris@29:         dst[i].im *= mag;
Chris@29:     }
Chris@29: }
Chris@29: 
Chris@29: #endif
Chris@29: 
Chris@29: void
Chris@29: v_polar_interleaved_to_cartesian_inplace(bq_complex_element_t *const BQ_R__ srcdst,
Chris@29:                                          const int count)
Chris@29: {
Chris@29:     // Not ideal
Chris@29:     bq_complex_element_t mag, phase;
Chris@29:     int ii = 0, io = 0;
Chris@29:     for (int i = 0; i < count; ++i) {
Chris@29:         mag = srcdst[ii++];
Chris@29:         phase = srcdst[ii++];
Chris@29:         bq_complex_t p = c_phasor(phase);
Chris@29:         srcdst[io++] = mag * p.re;
Chris@29:         srcdst[io++] = mag * p.im;
Chris@29:     }
Chris@29: }
Chris@29: 
Chris@29: #endif
Chris@29: 
Chris@29: }