void initialize(void)
{
std::size_t cnt = 0;
if ( m_window.size() != m_size )
m_window.resize(m_size);
if ( WINDOW_BARTLETT == m_type )
cnt = m_size / 2;
else if ( WINDOW_TUKEY == m_type )
cnt = m_size - 1;
else
cnt = m_size;
for ( std::size_t idx = 0; idx < cnt; idx++ ) {
switch ( m_type ) {
case WINDOW_HANN:
m_window[ idx ] = 0.5 * ( 1.0 - std::cos(M_PI * 2.0 * idx / ( m_size - 1.0 )) );
break;
case WINDOW_HAMM:
m_window[ idx ] = 0.54 - 0.46 * std::cos(2.0 * M_PI * idx / ( m_size - 1.0 ));
break;
case WINDOW_BLACKMAN:
m_window[ idx ] = 0.42 - 0.5 * std::cos(2.0 * M_PI * idx / ( m_size - 1.0 )) + 0.08 *
std::cos(4.0 * M_PI * idx / ( m_size - 1.0 ));
break;
case WINDOW_BARTLETT:
m_window[ idx ] = ( idx / static_cast<T>( ( m_size / 2.0 ) ) );
m_window[ idx + ( m_size / 2.0 ) ] = ( 1.0 - ( idx / static_cast<T>( ( m_size / 2.0 ) ) ) );
break;
case WINDOW_BLACKMAN_HARRIS:
m_window[ idx ] = 0.35875 - 0.48829 * std::cos(2.0 * M_PI * idx / ( m_size - 1.0 ))
+ 0.14128 * std::cos(4.0 * M_PI * idx / ( m_size - 1.0 )) - 0.01168
* std::cos(6.0 * M_PI * idx / ( m_size - 1.0 ));
break;
case WINDOW_WELCH:
m_window[ idx ] = 4.0 * idx / static_cast< T >(m_size)* ( 1.0 - ( idx / static_cast< T >( m_size ) ) );
break;
case WINDOW_FLATTOP:
m_window[ idx ] = 1.0 - 1.93 * std::cos(2.0 * M_PI * idx / ( m_size - 1.0 ))
+ 1.29 * std::cos(4.0*M_PI*idx / ( m_size - 1.0 )) - 0.388
* std::cos(6.0*M_PI*idx / ( m_size - 1.0 )) + 0.028
* std::cos(8.0*M_PI*idx / ( m_size - 1.0 ));
break;
case WINDOW_COSINE:
m_window[ idx ] = std::cos((M_PI * idx / (m_size - 1.0)) - (M_PI / 2.0));
break;
case WINDOW_NUTALL:
m_window[ idx ] = 0.355768 - 0.487396 * std::cos(2.0 * M_PI * idx / (m_size - 1.0))
+ 0.144232 * std::cos(4.0 * M_PI * idx / (m_size - 1.0)) - 0.012604
* std::cos(6.0 * M_PI * idx / (m_size - 1.0));
break;
case WINDOW_BLACKMAN_NUTALL:
m_window[ idx ] = 0.355768 - 0.487396 * std::cos(2.0 * M_PI * idx / (m_size - 1.0))
+ 0.144232 * std::cos(4.0 * M_PI * idx / (m_size - 1.0)) - 0.012604
* std::cos(6.0 * M_PI * idx / (m_size - 1.0));
break;
case WINDOW_RECTANGULAR:
m_window[ idx ] = 1.0;
break;
// -- https://github.com/JuliaDSP/DSP.jl/blob/master/src/windows.jl
case WINDOW_TUKEY:
if ( 0.0 > m_real || 1.0 < m_real )
throw std::invalid_argument("Alpha value must be >= 0.0 && <= 1.0");
if ( 0.0 == m_real )
m_window[ idx ] = 1.0;
else {
T m = m_real * ( idx - 1.0 ) / 2.0;
if ( idx <= m )
m_window[ idx + 1 ] = 0.5 * ( 1.0 + std::cos(M_PI * ( idx / m - 1.0 )) );
else if ( idx <= m_size - 1.0 - m )
m_window[ idx + 1 ] = 1.0;
else
m_window[ idx ] = 0.5 * ( 1.0 + std::cos(M_PI * ( (idx / (m - 2.0)) / (m_real + 1.0) )) );
}
break;
// -- https://github.com/JuliaDSP/DSP.jl/blob/master/src/windows.jl
case WINDOW_LANCZOS:
m_window[idx] = sinc(2 * idx / ( m_size - 1.0 ) - 1.0);
break;
// -- https://github.com/JuliaDSP/DSP.jl/blob/master/src/windows.jl
/*case WINDOW_GAUSSIAN:
if ( 0.0 > m_real || 0.5 < m_real )
throw std::invalid_argument("Sigma value must be >= 0.0 && <= 0.5");
m_window[ idx ] = std::exp(-0.5*( ( idx - ( m_size - 2.0 ) / 2.0 )
/ ( m_real * ( (m_size - 1.0) / 2.0 ) ) ) ^ 2);
break;*/
case WINDOW_BARTLETT_HANN:
m_window[ idx ] = 0.62 - 0.48 * std::abs((idx / ( m_size - 1.0 )) - 1.0 / 2.0) -
0.38 * std::cos(2.0 * M_PI * idx / (m_size - 1.0));
break;
case WINDOW_HANN_POISSON:
if ( 0.0 <= m_real )
m_real *= -1.0;
m_window[ idx ] = 1.0 / 2.0 * ( 1.0 - std::cos(2.0 * M_PI * idx / (m_size - 1.0)) ) *
std::exp(m_real * std::abs((m_size - 1.0 - 2.0) * idx / (m_size - 1.0)));
break;
default:
throw std::invalid_argument("Unknown or unsupported window type requested");
};
}
return;
}
static REAL hn_lpf(int n,REAL f,REAL fs)
{
REAL t = 1/fs;
REAL omega = 2*PI*f;
return 2*f*t*sinc(n*omega*t);
}
void dxStepBody (dxBody *b, dReal h)
{
// cap the angular velocity
if (b->flags & dxBodyMaxAngularSpeed) {
const dReal max_ang_speed = b->max_angular_speed;
const dReal aspeed = dCalcVectorDot3( b->avel, b->avel );
if (aspeed > max_ang_speed*max_ang_speed) {
const dReal coef = max_ang_speed/dSqrt(aspeed);
dScaleVector3(b->avel, coef);
}
}
// end of angular velocity cap
// handle linear velocity
for (unsigned int j=0; j<3; j++) b->posr.pos[j] += h * b->lvel[j];
if (b->flags & dxBodyFlagFiniteRotation) {
dVector3 irv; // infitesimal rotation vector
dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis) {
// split the angular velocity vector into a component along the finite
// rotation axis, and a component orthogonal to it.
dVector3 frv; // finite rotation vector
dReal k = dCalcVectorDot3 (b->finite_rot_axis,b->avel);
frv[0] = b->finite_rot_axis[0] * k;
frv[1] = b->finite_rot_axis[1] * k;
frv[2] = b->finite_rot_axis[2] * k;
irv[0] = b->avel[0] - frv[0];
irv[1] = b->avel[1] - frv[1];
irv[2] = b->avel[2] - frv[2];
// make a rotation quaternion q that corresponds to frv * h.
// compare this with the full-finite-rotation case below.
h *= REAL(0.5);
dReal theta = k * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = frv[0] * s;
q[2] = frv[1] * s;
q[3] = frv[2] * s;
}
else {
// make a rotation quaternion q that corresponds to w * h
dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
b->avel[2]*b->avel[2]);
h *= REAL(0.5);
dReal theta = wlen * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = b->avel[0] * s;
q[2] = b->avel[1] * s;
q[3] = b->avel[2] * s;
}
// do the finite rotation
dQuaternion q2;
dQMultiply0 (q2,q,b->q);
for (unsigned int j=0; j<4; j++) b->q[j] = q2[j];
// do the infitesimal rotation if required
if (b->flags & dxBodyFlagFiniteRotationAxis) {
dReal dq[4];
dWtoDQ (irv,b->q,dq);
for (unsigned int j=0; j<4; j++) b->q[j] += h * dq[j];
}
}
else {
// the normal way - do an infitesimal rotation
dReal dq[4];
dWtoDQ (b->avel,b->q,dq);
for (unsigned int j=0; j<4; j++) b->q[j] += h * dq[j];
}
// normalize the quaternion and convert it to a rotation matrix
dNormalize4 (b->q);
dQtoR (b->q,b->posr.R);
// notify all attached geoms that this body has moved
dxWorldProcessContext *world_process_context = b->world->UnsafeGetWorldProcessingContext();
for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) {
world_process_context->LockForStepbodySerialization();
dGeomMoved (geom);
world_process_context->UnlockForStepbodySerialization();
}
// notify the user
if (b->moved_callback != NULL) {
b->moved_callback(b);
}
// damping
if (b->flags & dxBodyLinearDamping) {
const dReal lin_threshold = b->dampingp.linear_threshold;
const dReal lin_speed = dCalcVectorDot3( b->lvel, b->lvel );
if ( lin_speed > lin_threshold) {
const dReal k = 1 - b->dampingp.linear_scale;
dScaleVector3(b->lvel, k);
}
}
if (b->flags & dxBodyAngularDamping) {
const dReal ang_threshold = b->dampingp.angular_threshold;
const dReal ang_speed = dCalcVectorDot3( b->avel, b->avel );
if ( ang_speed > ang_threshold) {
const dReal k = 1 - b->dampingp.angular_scale;
dScaleVector3(b->avel, k);
}
}
}
void *impComp(void *Args)
{
struct arg_s *args = (struct arg_s *)Args;
// Temporary variables and constants (high-pass filter)
const double W = 2*M_PI*100/args->fs;
const double R1 = exp(-W);
const double R2 = R1;
const double B1 = 2*R1*cos(W);
const double B2 = -R1 * R1;
const double A1 = -(1+R2);
const double A2 = R2;
double X0, Y0, Y1, Y2;
// Temporary variables and constants (image-method)
double* r = new double[3];
double* s = new double[3];
double* L = new double[3];
double* LPI = new double[args->Tw+1];
double hu[6];
double refl[3];
double dist;
double strength;
int loud_nr;
int mic_nr;
int n1,n2,n3;
int mx,my,mz;
int q, j, k;
int n;
int fdist;
int pos;
uint64_t abs_counter;
for (loud_nr = 0; loud_nr < args->nr_of_louds; loud_nr++ )
{
s[0] = args->ss[loud_nr + 0*args->nr_of_louds] / args->cTs;
s[1] = args->ss[loud_nr + 1*args->nr_of_louds] / args->cTs;
s[2] = args->ss[loud_nr + 2*args->nr_of_louds] / args->cTs;
for (mic_nr = args->tNum; mic_nr < args->nr_of_mics ; mic_nr=mic_nr+args->tTot)
{
r[0] = args->rr[mic_nr + 0*args->nr_of_mics] / args->cTs;
r[1] = args->rr[mic_nr + 1*args->nr_of_mics] / args->cTs;
r[2] = args->rr[mic_nr + 2*args->nr_of_mics] / args->cTs;
n1 = ceil(args->nsamples/(2*args->L[0]))*args->dim_s[0];
n2 = ceil(args->nsamples/(2*args->L[1]))*args->dim_s[1];
n3 = ceil(args->nsamples/(2*args->L[2]))*args->dim_s[2];
// Generate room impulse response
for (mx = -n1 ; mx <= n1 ; mx++)
{
hu[0] = 2*mx*args->L[0];
for (my = -n2 ; my <= n2 ; my++)
{
hu[1] = 2*my*args->L[1];
for (mz = -n3 ; mz <= n3 ; mz++)
{
hu[2] = 2*mz*args->L[2];
for (q = 0 ; q <= 1*args->dim_s[0] ; q++)
{
hu[3] = s[0] - r[0] + 2*q*r[0] + hu[0];
refl[0] = pow(args->beta[0], abs(mx)) * pow(args->beta[1], abs(mx+q));
for (j = 0 ; j <= 1*args->dim_s[1] ; j++)
{
hu[4] = s[1] - r[1] + 2*j*r[1] + hu[1];
refl[1] = pow(args->beta[2], abs(my)) * pow(args->beta[3], abs(my+j));
for (k = 0 ; k <= 1*args->dim_s[2] ; k++)
{
hu[5] = s[2] - r[2] + 2*k*r[2] + hu[2];
refl[2] = pow(args->beta[4],abs(mz)) * pow(args->beta[5], abs(mz+k));
dist = sqrt(pow(hu[3], 2) + pow(hu[4], 2) + pow(hu[5], 2));
fdist = (int) floor(dist);
if (abs(2*mx+q)+abs(2*my+j)+abs(2*mz+k) <= args->order || args->order == -1)
{
if (fdist < args->nsamples)
{
//if ( mx == 0 && my == 0 && mz == 0 && q == 0 && j == 0 && k==0)
//{
// mexPrintf("x: %f | y: %f | z: %f \n",refl[0],refl[1],refl[2]);
//}
strength = sim_microphone(hu[3], hu[4], args->angle, args->mtype)
* refl[0]*refl[1]*refl[2]/(4*M_PI*dist*args->cTs);
//if ( mx == 0 && my == 0 && mz == 0 && q == 0 && j == 0 && k==0)
//{
// mexPrintf("str: %f | dist: %f | dist*cTs: %f \n",strength,dist,dist*cTs);
//}
if (args->lp_filter == 1)
{
for (n = 0 ; n < args->Tw+1 ; n++)
LPI[n] = args->hanning_window[n] * args->Fc * sinc( M_PI*args->Fc*(n-(dist-fdist)-(args->Tw/2)) );
pos = fdist-(args->Tw/2);
for (n = 0; n < args->Tw+1; n++)
{
if (pos+n >=0 && pos+n < args->nsamples)
{
//if ( mx == 0 && my == 0 && mz == 0)
abs_counter = (uint64_t)pos+(uint64_t)n +(uint64_t)args->nsamples*(uint64_t)mic_nr + (uint64_t)args->nsamples*(uint64_t)args->nr_of_mics*(uint64_t)loud_nr;//
args->imp[ abs_counter] += strength * LPI[n];
}
}
}
else
{
//if ( mx == 0 && my == 0 && mz == 0)
abs_counter = (uint64_t)fdist + (uint64_t)args->nsamples*(uint64_t)mic_nr + (uint64_t)args->nsamples*(uint64_t)args->nr_of_mics*(uint64_t)loud_nr;
args->imp[ abs_counter] += strength;
}
}
}
}
}
}
}
}
}
// 'Original' high-pass filter as proposed by Allen and Berkley.
if (args->hp_filter == 1)
{
Y0 = 0.0;
Y1 = 0.0;
Y2 = 0.0;
for (int idx = 0 ; idx < args->nsamples ; idx++)
{
abs_counter = (uint64_t)idx + (uint64_t)args->nsamples*(uint64_t)mic_nr + (uint64_t)args->nsamples*(uint64_t)args->nr_of_mics*(uint64_t)loud_nr;
X0 = args->imp[abs_counter];
Y2 = Y1;
Y1 = Y0;
Y0 = B1*Y1 + B2*Y2 + X0;
args->imp[abs_counter] = Y0 + A1*Y1 + A2*Y2;
}
}
}
}
pthread_exit(NULL);
}
float lanczos(float n, float N, float a)
{
(void)a;
return sinc(2.0f * n / (N - 1.0f) - 1.0f);
}
double Lanczos3_filter( double t ) {
if(t < 0) t = -t;
if(t < 3.0) return(sinc(t) * sinc(t/3.0));
return(0.0);
}
void GWigAy(struct gwig *pWig, double *Xvec, double *pay, double *paypx)
{
int i;
double x, y, z;
double kx, ky, kz, tz, kw;
double cx, sx, chx, shx;
double cy, syky, chy, shy, sz, shyky;
double gamma0, beta0;
double ay, aypx;
x = Xvec[0];
y = Xvec[2];
z = pWig->Zw;
kw = 2e0*PI/(pWig->Lw);
ay = 0e0;
aypx = 0e0;
gamma0 = pWig->E0/XMC2;
beta0 = sqrt(1e0 - 1e0/(gamma0*gamma0));
if (pWig->NHharm && z>=pWig->zStartH && z<=pWig->zEndH) {
if (pWig->PB0H!=0)
pWig->Aw = (q_e/m_e/clight)/(2e0*PI) * (pWig->Lw) * (pWig->PB0H);
else
pWig->Aw = (q_e/m_e/clight)/(2e0*PI) * (pWig->Lw) * (pWig->PB0);
if (!pWig->HSplitPole) {
/* Normal Horizontal Wiggler: note that one potentially could have: kx=0 */
for ( i = 0; i < pWig->NHharm; i++ ){
pWig->HCw[i] = (pWig->HCw_raw[i])*(pWig->Aw)/(gamma0*beta0);
kx = pWig->Hkx[i];
ky = pWig->Hky[i];
kz = pWig->Hkz[i];
tz = pWig->Htz[i];
sx = sin(kx * x);
shy = sinh(ky * y);
sz = sin(kz * z + tz);
ay = ay + (pWig->HCw[i])*(kw/kz)*(kx/ky)*sx*shy*sz;
cx = cos(kx * x);
chy = cosh(ky * y);
aypx = aypx + (pWig->HCw[i])*(kw/kz)*ipow(kx/ky,2) * cx*chy*sz;
}
} else {
/* Split-pole Horizontal Wiggler: note that one potentially could have: ky=0 (caught in main routine) */
for ( i = 0; i < pWig->NHharm; i++ ){
pWig->HCw[i] = (pWig->HCw_raw[i])*(pWig->Aw)/(gamma0*beta0);
kx = pWig->Hkx[i];
ky = pWig->Hky[i];
kz = pWig->Hkz[i];
tz = pWig->Htz[i];
sz = sin(kz * z + tz);
ay = ay - pWig->HCw[i]*(kw/kz)*kx/ky*sin(ky*y)*sinh(kx*x)*sz;
aypx = aypx + pWig->HCw[i]*(kw/kz)*ipow(kx/ky,2)*cos(ky*y)*cosh(kx*x)*sz;
}
}
}
if (pWig->NVharm && z>=pWig->zStartV && z<=pWig->zEndV) {
if (pWig->PB0V!=0)
pWig->Aw = (q_e/m_e/clight)/(2e0*PI) * (pWig->Lw) * (pWig->PB0V);
else
pWig->Aw = (q_e/m_e/clight)/(2e0*PI) * (pWig->Lw) * (pWig->PB0);
if (!pWig->VSplitPole) {
/* Normal Vertical Wiggler, could have ky=0 */
for (i = 0; i < pWig->NVharm; i++ ) {
pWig->VCw[i] = (pWig->VCw_raw[i])*(pWig->Aw)/(gamma0*beta0);
kx = pWig->Vkx[i];
ky = pWig->Vky[i];
kz = pWig->Vkz[i];
tz = pWig->Vtz[i];
chx = cosh(kx * x);
cy = cos(ky * y);
sz = sin(kz * z + tz);
ay = ay + (pWig->VCw[i])*(kw/kz)*chx*cy*sz;
shx = sinh(kx * x);
if (abs(ky/kw) > GWIG_EPS)
syky = sin(ky * y)/ky;
else
syky = y * sinc(ky * y);
aypx = aypx + (pWig->VCw[i])*(kw/kz)* kx*shx*syky*sz;
}
} else {
/* Split-pole Vertical Wiggler, could have kx=0 */
for (i = 0; i < pWig->NVharm; i++ ) {
pWig->VCw[i] = (pWig->VCw_raw[i])*(pWig->Aw)/(gamma0*beta0);
kx = pWig->Vkx[i];
ky = pWig->Vky[i];
kz = pWig->Vkz[i];
tz = pWig->Vtz[i];
sz = sin(kz * z + tz);
ay = ay + (pWig->VCw[i])*(kw/kz)*cosh(ky*y)*cos(kx*x)*sz;
shyky = sinh(ky*y)/ky;
aypx = aypx - (pWig->VCw[i])*(kw/kz)*kx*shyky*sin(kx*x)*sz;
}
}
}
*pay = ay;
*paypx = aypx;
}
static inline void
moveAndRotateBody (dxBody * b, dReal h)
{
int j;
// handle linear velocity
for (j = 0; j < 3; j++)
b->posr.pos[j] += dMUL(h,b->lvel[j]);
if (b->flags & dxBodyFlagFiniteRotation)
{
dVector3 irv; // infitesimal rotation vector
dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis)
{
// split the angular velocity vector into a component along the finite
// rotation axis, and a component orthogonal to it.
dVector3 frv; // finite rotation vector
dReal k = dDOT (b->finite_rot_axis, b->avel);
frv[0] = dMUL(b->finite_rot_axis[0],k);
frv[1] = dMUL(b->finite_rot_axis[1],k);
frv[2] = dMUL(b->finite_rot_axis[2],k);
irv[0] = b->avel[0] - frv[0];
irv[1] = b->avel[1] - frv[1];
irv[2] = b->avel[2] - frv[2];
// make a rotation quaternion q that corresponds to frv * h.
// compare this with the full-finite-rotation case below.
h = dMUL(h,REAL (0.5));
dReal theta = dMUL(k,h);
q[0] = dCos (theta);
dReal s = dMUL(sinc (theta),h);
q[1] = dMUL(frv[0],s);
q[2] = dMUL(frv[1],s);
q[3] = dMUL(frv[2],s);
}
else
{
// make a rotation quaternion q that corresponds to w * h
dReal wlen = dSqrt (dMUL(b->avel[0],b->avel[0]) + dMUL(b->avel[1],b->avel[1]) + dMUL(b->avel[2],b->avel[2]));
h = dMUL(h,REAL (0.5));
dReal theta = dMUL(wlen,h);
q[0] = dCos (theta);
dReal s = dMUL(sinc (theta),h);
q[1] = dMUL(b->avel[0],s);
q[2] = dMUL(b->avel[1],s);
q[3] = dMUL(b->avel[2],s);
}
// do the finite rotation
dQuaternion q2;
dQMultiply0 (q2, q, b->q);
for (j = 0; j < 4; j++)
b->q[j] = q2[j];
// do the infitesimal rotation if required
if (b->flags & dxBodyFlagFiniteRotationAxis)
{
dReal dq[4];
dWtoDQ (irv, b->q, dq);
for (j = 0; j < 4; j++)
b->q[j] += dMUL(h,dq[j]);
}
}
else
{
// the normal way - do an infitesimal rotation
dReal dq[4];
dWtoDQ (b->avel, b->q, dq);
for (j = 0; j < 4; j++)
b->q[j] += dMUL(h,dq[j]);
}
// normalize the quaternion and convert it to a rotation matrix
dNormalize4 (b->q);
dQtoR (b->q, b->posr.R);
// notify all attached geoms that this body has moved
for (dxGeom * geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
dGeomMoved (geom);
}
void GWigAy(struct gwig *pWig, double *Xvec, double *pay, double *paypx)
{
int i;
double x, y, z;
double kx, ky, kz, tz, kw;
double cx, sx, chx, shx;
double cy, syky, chy, shy, sz;
double gamma0, beta0;
double ay, aypx;
x = Xvec[0];
y = Xvec[2];
z = pWig->Zw;
kw = 2e0*PI/(pWig->Lw);
ay = 0e0;
aypx = 0e0;
gamma0 = pWig->E0/XMC2;
beta0 = sqrt(1e0 - 1e0/(gamma0*gamma0));
pWig->Aw = (q_e/m_e/clight)/(2e0*PI) * (pWig->Lw) * (pWig->PB0);
/* Horizontal Wiggler: note that one potentially could have: kx=0 */
for ( i = 0; i < pWig->NHharm; i++ ){
pWig->HCw[i] = (pWig->HCw_raw[i])*(pWig->Aw)/(gamma0*beta0);
kx = pWig->Hkx[i];
ky = pWig->Hky[i];
kz = pWig->Hkz[i];
tz = pWig->Htz[i];
sx = sin(kx * x);
shy = sinh(ky * y);
sz = sin(kz * z + tz);
ay = ay + (pWig->HCw[i])*(kw/kz)*(kx/ky)*sx*shy*sz;
cx = cos(kx * x);
chy = cosh(ky * y);
aypx = aypx + (pWig->HCw[i])*(kw/kz)*pow(kx/ky,2) * cx*chy*sz;
}
/* Vertical Wiggler: note that one potentially could have: ky=0 */
for (i = 0; i < pWig->NVharm; i++) {
pWig->VCw[i] = (pWig->VCw_raw[i])*(pWig->Aw)/(gamma0*beta0);
kx = pWig->Vkx[i];
ky = pWig->Vky[i];
kz = pWig->Vkz[i];
tz = pWig->Vtz[i];
chx = cosh(kx * x);
cy = cos(ky * y);
sz = sin(kz * z + tz);
ay = ay + (pWig->VCw[i])*(kw/kz)*chx*cy*sz;
shx = sinh(kx * x);
if (abs(ky/kw) > GWIG_EPS) {
syky = sin(ky * y)/ky;
} else {
syky = y * sinc(ky * y);
aypx = aypx + (pWig->VCw[i])*(kw/kz)* kx*shx*syky*sz;
}
}
*pay = ay;
*paypx = aypx;
}
status_t Resampler::UpdateCoefficients(int windowType, double alpha)
{
const double bandWidth = (m_StopBandFrequency - m_PassBandFrequency);
DEBUG_LOG("nFp=%g, nFs=%g\n", m_PassBandFrequency, m_StopBandFrequency);
int N = 0;
switch(windowType)
{
case HANNING:
{
N = static_cast<int>(6.2 / 2 / bandWidth + 0.5);
N += (N & 1 ? 0 : 1);
N *= m_L;
DEBUG_LOG("N=%d (Hanning)\n", N);
m_Coefficients.Resize(N);
masp::window::Hanning(m_Coefficients);
}
break;
case HAMMING:
{
N = static_cast<int>(6.6 / 2 / bandWidth + 0.5);
N += (N & 1 ? 0 : 1);
N *= m_L;
DEBUG_LOG("N=%d (Hamming)\n", N);
m_Coefficients.Resize(N);
masp::window::Hamming(m_Coefficients);
}
break;
case BLACKMAN:
{
N = static_cast<int>(11.0 / 2 / bandWidth + 0.5);
N += (N & 1 ? 0 : 1);
N *= m_L;
DEBUG_LOG("N=%d (Blackman)\n", N);
m_Coefficients.Resize(N);
masp::window::Blackman(m_Coefficients);
}
break;
case KAISER:
{
ASSERT( 2.0 <= alpha && alpha <= 10.0 );
const int index = static_cast<int>(alpha + 0.5) - g_KaiserAlphaOffset;
N = static_cast<int>( g_KaiserTransitionBand[index] / bandWidth + 0.5);
N += (N & 1 ? 0 : 1);
N *= m_L;
DEBUG_LOG("N=%d (Kaiser), alpha=%g\n", N, alpha);
m_Coefficients.Resize(N);
masp::window::Kaiser(m_Coefficients, alpha);
}
break;
default:
ASSERT ( 0 );
break;
}
ASSERT( N != 0 );
{
const double cutoff = (m_StopBandFrequency + m_PassBandFrequency) / 2.0 / (m_L > m_M ? m_L : m_M);
DEBUG_LOG("nFp=%g, nFs=%g\n", m_PassBandFrequency, m_StopBandFrequency);
DEBUG_LOG("nFc=%g\n", cutoff);
mcon::Vector<double> sinc(N);
masp::fir::GetCoefficientsLpfSinc(sinc, cutoff);
m_Coefficients *= sinc;
}
return NO_ERROR;
}
qreal UtilityFunction::f(qreal x)
{
return 0.4 + sinc( 4 * x ) + 1.1 * sinc( 4 * x + 2 ) + 0.8 * sinc( 6 * x - 2 ) + 0.7 * sinc( 6 * x - 4 );
//return sin( x );
}
static double
lanczos (double x, int n)
{
return sinc (x) * sinc (x * (1.0 / n));
}
static double lanczos(double n, double N, double alpha)
{
(void)alpha;
return sinc(2.0 * n / (N - 1.0) - 1.0);
}
double U(double x, double y, double z int p) {
return pow(sinc(x)*sinc(y)*sinc(z), p);
}
inline double lpf(int i, double f) { return 2 * f * sinc(i * 2 * M_PI * f); }
static inline double lanzcos(double index)
{
return sinc(index);
}
int main(int argc, char** argv)
{
// nc is the number of bits to store the coefficients
const int nc = 32;
bool polyphase = false;
unsigned int polyM = 160;
unsigned int polyN = 147;
bool debug = false;
double Fs = 48000;
double Fc = 20478;
double atten = 1;
int format = 0;
// in order to keep the errors associated with the linear
// interpolation of the coefficients below the quantization error
// we must satisfy:
// 2^nz >= 2^(nc/2)
//
// for 16 bit coefficients that would be 256
//
// note that increasing nz only increases memory requirements,
// but doesn't increase the amount of computation to do.
//
//
// see:
// Smith, J.O. Digital Audio Resampling Home Page
// https://ccrma.stanford.edu/~jos/resample/, 2011-03-29
//
int nz = 4;
// | 0.1102*(A - 8.7) A > 50
// beta = | 0.5842*(A - 21)^0.4 + 0.07886*(A - 21) 21 <= A <= 50
// | 0 A < 21
// with A is the desired stop-band attenuation in dBFS
//
// for eg:
//
// 30 dB 2.210
// 40 dB 3.384
// 50 dB 4.538
// 60 dB 5.658
// 70 dB 6.764
// 80 dB 7.865
// 90 dB 8.960
// 100 dB 10.056
double beta = 7.865;
// 2*nzc = (A - 8) / (2.285 * dw)
// with dw the transition width = 2*pi*dF/Fs
//
int nzc = 8;
//
// Example:
// 44.1 KHz to 48 KHz resampling
// 100 dB rejection above 28 KHz
// (the spectrum will fold around 24 KHz and we want 100 dB rejection
// at the point where the folding reaches 20 KHz)
// ...___|_____
// | \|
// | ____/|\____
// |/alias| \
// ------/------+------\---------> KHz
// 20 24 28
// Transition band 8 KHz, or dw = 1.0472
//
// beta = 10.056
// nzc = 20
//
int ch;
while ((ch = getopt(argc, argv, ":hds:c:n:f:l:b:p:v:")) != -1) {
switch (ch) {
case 'd':
debug = true;
break;
case 'p':
if (sscanf(optarg, "%u/%u", &polyM, &polyN) != 2) {
usage(argv[0]);
}
polyphase = true;
break;
case 's':
Fs = atof(optarg);
break;
case 'c':
Fc = atof(optarg);
break;
case 'n':
nzc = atoi(optarg);
break;
case 'l':
nz = atoi(optarg);
break;
case 'f':
if (!strcmp(optarg,"fixed")) format = 0;
else if (!strcmp(optarg,"float")) format = 1;
else usage(argv[0]);
break;
case 'b':
beta = atof(optarg);
break;
case 'v':
atten = pow(10, -fabs(atof(optarg))*0.05 );
break;
case 'h':
default:
usage(argv[0]);
break;
}
}
// cut off frequency ratio Fc/Fs
double Fcr = Fc / Fs;
// total number of coefficients (one side)
const int M = (1 << nz);
const int N = M * nzc;
// generate the right half of the filter
if (!debug) {
printf("// cmd-line: ");
for (int i=1 ; i<argc ; i++) {
printf("%s ", argv[i]);
}
printf("\n");
if (!polyphase) {
printf("const int32_t RESAMPLE_FIR_SIZE = %d;\n", N);
printf("const int32_t RESAMPLE_FIR_LERP_INT_BITS = %d;\n", nz);
printf("const int32_t RESAMPLE_FIR_NUM_COEF = %d;\n", nzc);
} else {
printf("const int32_t RESAMPLE_FIR_SIZE = %d;\n", 2*nzc*polyN);
printf("const int32_t RESAMPLE_FIR_NUM_COEF = %d;\n", 2*nzc);
}
if (!format) {
printf("const int32_t RESAMPLE_FIR_COEF_BITS = %d;\n", nc);
}
printf("\n");
printf("static %s resampleFIR[] = {", !format ? "int32_t" : "float");
}
if (!polyphase) {
for (int i=0 ; i<=M ; i++) { // an extra set of coefs for interpolation
for (int j=0 ; j<nzc ; j++) {
int ix = j*M + i;
double x = (2.0 * M_PI * ix * Fcr) / (1 << nz);
double y = kaiser(ix+N, 2*N, beta) * sinc(x) * 2.0 * Fcr;
y *= atten;
if (!debug) {
if (j == 0)
printf("\n ");
}
if (!format) {
int64_t yi = floor(y * ((1ULL<<(nc-1))) + 0.5);
if (yi >= (1LL<<(nc-1))) yi = (1LL<<(nc-1))-1;
printf("0x%08x, ", int32_t(yi));
} else {
printf("%.9g%s ", y, debug ? "," : "f,");
}
}
}
} else {
for (int j=0 ; j<polyN ; j++) {
// calculate the phase
double p = ((polyM*j) % polyN) / double(polyN);
if (!debug) printf("\n ");
else printf("\n");
// generate a FIR per phase
for (int i=-nzc ; i<nzc ; i++) {
double x = 2.0 * M_PI * Fcr * (i + p);
double y = kaiser(i+N, 2*N, beta) * sinc(x) * 2.0 * Fcr;;
y *= atten;
if (!format) {
int64_t yi = floor(y * ((1ULL<<(nc-1))) + 0.5);
if (yi >= (1LL<<(nc-1))) yi = (1LL<<(nc-1))-1;
printf("0x%08x", int32_t(yi));
} else {
printf("%.9g%s", y, debug ? "" : "f");
}
if (debug && (i==nzc-1)) {
} else {
printf(", ");
}
}
}
}
if (!debug) {
printf("\n};");
}
printf("\n");
return 0;
}
static inline double lanzcos(double phase)
{
return sinc(phase);
}
void KspaceCube::evaluate(double &re, double &im, double kx,double ky,double kz, double tau) {
Q_UNUSED(tau);
re=m_length_x*m_length_y*m_length_z*sinc(kx*m_length_x)*sinc(ky*m_length_y)*sinc(kz*m_length_z);
im=0;
}
static inline double window_function(double index)
{
return sinc(M_PI * index);
}
double FIRFilter::_calcH(int n, double omega) {
if (n == 0) {
return omega / PI;
}
return omega / PI * sinc(n*omega);
}
std::complex<double> SBBox::SBBoxImpl::kValue(const Position<double>& k) const
{
return _flux * sinc(k.x*_wo2pi)*sinc(k.y*_ho2pi);
}
int RateChanger<T>::Execute()
{
T *start_of_buffer, *end_of_buffer;
T *read_ptr, *write_ptr;
T input_samp, *input_signal_ptr;
T *output_signal_ptr;
T *output_base;
T sum;
T zero_T;
T *ptr_to_center;
int num_sidelobes;
int max_buf_len;
int is, input_block_size;
int output_block_size;
double samp_intvl;
float *sinc_val;
int idx;
double sinc_offset;
double rt_brack_needed;
double samp_inst;
int num_inputs_avail;
int rt_brack_avail;
int beg_samp_count;
int iflag;
bool input_has_been_read;
//-----------------------------------------
// Get pointers for input and output signals
output_signal_ptr = GET_OUTPUT_PTR(Out_Sig);
output_base = GET_OUTPUT_PTR(Out_Sig);
input_signal_ptr = GET_INPUT_PTR(In_Sig);
input_block_size = In_Sig->GetValidBlockSize();
*DebugFile << "input_block_size = " << input_block_size << endl;
//------------------------------------------------
// copy frequently used items into local storage
//block_size = Max_Block_Size;
samp_intvl = Samp_Intvl;
read_ptr = Read_Ptr;
write_ptr = Write_Ptr;
start_of_buffer = Start_Of_Buffer;
end_of_buffer = End_Of_Buffer;
sinc_val = Sinc_Val;
// case INTERP_MODE_SINC:
num_sidelobes = Num_Sidelobes;
max_buf_len = Max_Buffer_Len;
rt_brack_avail = Rt_Brack_Avail;
num_inputs_avail = input_block_size;
if(PassNumber == 1)
{
//----------------------------------
// preload enough input samples to
// accomodate non-causal tail of sinc window
for(is=Num_Sidelobes; is>=0; is--)
{
*(end_of_buffer-is) = 0.0;
// Buffer_Fullness++;
}
for(is=0; is<Num_Sidelobes; is++)
{
input_samp = *input_signal_ptr++;
*write_ptr++ = input_samp;
//Buffer_Fullness++;
num_inputs_avail--;
}
Out_Samp_Count = 0;
rt_brack_avail = 0;
}
//------------------------------------
// processing for every pass
beg_samp_count = Out_Samp_Count;
while(num_inputs_avail >= 0)
{
samp_inst = Out_Samp_Count*Out_Samp_Intvl/Samp_Intvl;
rt_brack_needed = ceil(samp_inst);
if(rt_brack_needed <= rt_brack_avail)
{
if(samp_inst == rt_brack_needed)
{
// rt_bracket sample is the desired output
// no interpolation needed
ptr_to_center = Read_Ptr_Start + Num_Sidelobes;
if(ptr_to_center > end_of_buffer) ptr_to_center -= Max_Buffer_Len;
sum = *ptr_to_center;
}
else
{
// interploation is needed
// desired output falls between rt_bracket and rt_bracket-1
// generate sinc window
sinc_offset = rt_brack_needed - samp_inst;
for(idx=0; idx < 2*Num_Sidelobes; idx++)
{
sinc_val[idx] = float(sinc( idx + sinc_offset-Num_Sidelobes));
}
sinc_val[0] = float(0.6*sinc_val[0]);
sinc_val[2*Num_Sidelobes-1] = float(0.6*sinc_val[2*Num_Sidelobes-1]);
//---------------------------------------
read_ptr = Read_Ptr_Start;
//-----------------------------------------------
sum = 0.0;
zero_T = 0.0;
for(int sum_idx=0; sum_idx< 2*Num_Sidelobes; sum_idx++)
{
sum += (*read_ptr) * sinc_val[sum_idx];
read_ptr++;
if(read_ptr > end_of_buffer)
{
read_ptr = start_of_buffer;
*DebugFile << "read_ptr wrapped" << endl;
}
}
}
Out_Samp_Count++;
if(Out_Samp_Count == 2016)
{
iflag = 1;
}
*output_signal_ptr++ = sum;
input_has_been_read = false;
} // end of output generation
else
{
// need to read some more inputs
// are any avail?
if(num_inputs_avail > 0)
{
input_samp = *input_signal_ptr++;
*write_ptr++ = input_samp;
if(write_ptr > end_of_buffer)
{
write_ptr = start_of_buffer;
}
rt_brack_avail++;
Read_Ptr_Start++;
if(Read_Ptr_Start > end_of_buffer) Read_Ptr_Start = start_of_buffer;
}
num_inputs_avail--;
input_has_been_read = true;
}
}
output_block_size = Out_Samp_Count - beg_samp_count;
Out_Sig->SetValidBlockSize(output_block_size);
*DebugFile << "delayed output block size = " << output_block_size << endl;
if(input_has_been_read)
{
*DebugFile << "input_has_been_read = true" << endl;
}
else
{
*DebugFile << "input_has_been_read = false" << endl;
}
// break;
Rt_Brack_Avail = rt_brack_avail;
Read_Ptr = read_ptr;
Write_Ptr = write_ptr;
return(_MES_AOK);
}
int main () {
double f = 1.0;
fprintf(stderr, "sinc(%f) = %f\n", f, sinc(f));
return 0;
}
static float do_sinc(int x, int y, int, float* param)
{
return float((sinc((x - param[3])*param[1])*sinc((y - param[4])*param[2]) + param[5])*param[0]);
}
/**
Setup the detector transfer functions. See maos/setup_powfs.c
*/
static void setup_powfs_dtf(POWFS_S *powfs, const PARMS_S* parms){
const int npowfs=parms->maos.npowfs;
for(int ipowfs=0; ipowfs<npowfs; ipowfs++){
const int ncomp=parms->maos.ncomp[ipowfs];
const int ncomp2=ncomp>>1;
const int embfac=parms->maos.embfac[ipowfs];
const int pixpsa=parms->skyc.pixpsa[ipowfs];
const double pixtheta=parms->skyc.pixtheta[ipowfs];
const double blur=parms->skyc.pixblur[ipowfs]*pixtheta;
const double e0=exp(-2*M_PI*M_PI*blur*blur);
const double dxsa=parms->maos.dxsa[ipowfs];
const double pixoffx=parms->skyc.pixoffx[ipowfs];
const double pixoffy=parms->skyc.pixoffy[ipowfs];
const double pxo=-(pixpsa/2-0.5+pixoffx)*pixtheta;
const double pyo=-(pixpsa/2-0.5+pixoffy)*pixtheta;
loc_t *loc_ccd=mksqloc(pixpsa, pixpsa, pixtheta, pixtheta, pxo, pyo);
powfs[ipowfs].dtf=mycalloc(parms->maos.nwvl,DTF_S);
for(int iwvl=0; iwvl<parms->maos.nwvl; iwvl++){
const double wvl=parms->maos.wvl[iwvl];
const double dtheta=wvl/(dxsa*embfac);
const double pdtheta=pixtheta*pixtheta/(dtheta*dtheta);
const double du=1./(dtheta*ncomp);
const double du2=du*du;
const double dupth=du*pixtheta;
cmat *nominal=cnew(ncomp,ncomp);
//cfft2plan(nominal,-1);
//cfft2plan(nominal,1);
cmat* pn=nominal;
const double theta=0;
const double ct=cos(theta);
const double st=sin(theta);
for(int iy=0; iy<ncomp; iy++){
int jy=iy-ncomp2;
for(int ix=0; ix<ncomp; ix++){
int jx=ix-ncomp2;
double ir=ct*jx+st*jy;
double ia=-st*jx+ct*jy;
IND(pn,ix,iy)=sinc(ir*dupth)*sinc(ia*dupth)
*pow(e0,ir*ir*du2)*pow(e0,ia*ia*du2)
*pdtheta;
}
}
if(parms->skyc.fnpsf1[ipowfs]){
warning("powfs %d has additional otf to be multiplied\n", ipowfs);
dcell *psf1c=dcellread("%s", parms->skyc.fnpsf1[ipowfs]);
dmat *psf1=NULL;
if(psf1c->nx == 1){
psf1=dref(psf1c->p[0]);
}else if(psf1c->nx==parms->maos.nwvl){
psf1=dref(psf1c->p[iwvl]);
}else{
error("skyc.fnpsf1 has wrong dimension\n");
}
dcellfree(psf1c);
if(psf1->ny!=2){
error("skyc.fnpsf1 has wrong dimension\n");
}
dmat *psf1x=dnew_ref(psf1->nx, 1, psf1->p);
dmat *psf1y=dnew_ref(psf1->nx, 1, psf1->p+psf1->nx);
dmat *psf2x=dnew(ncomp*ncomp, 1);
for(int iy=0; iy<ncomp; iy++){
int jy=iy-ncomp2;
for(int ix=0; ix<ncomp; ix++){
int jx=ix-ncomp2;
psf2x->p[ix+iy*ncomp]=sqrt(jx*jx+jy*jy)*dtheta;
}
}
info("powfs %d, iwvl=%d, dtheta=%g\n", ipowfs, iwvl, dtheta*206265000);
writebin(psf2x, "powfs%d_psf2x_%d", ipowfs,iwvl);
dmat *psf2=dinterp1(psf1x, psf1y, psf2x, 0);
normalize_sum(psf2->p, psf2->nx*psf2->ny, 1);
psf2->nx=ncomp; psf2->ny=ncomp;
writebin(psf2, "powfs%d_psf2_%d", ipowfs,iwvl);
cmat *otf2=cnew(ncomp, ncomp);
//cfft2plan(otf2, -1);
ccpd(&otf2, psf2);//peak in center
cfftshift(otf2);//peak in corner
cfft2(otf2, -1);
cfftshift(otf2);//peak in center
writebin(otf2, "powfs%d_otf2_%d", ipowfs, iwvl);
writebin(nominal, "powfs%d_dtf%d_nominal_0",ipowfs,iwvl);
for(int i=0; i<ncomp*ncomp; i++){
nominal->p[i]*=otf2->p[i];
}
writebin(nominal, "powfs%d_dtf%d_nominal_1",ipowfs,iwvl);
dfree(psf1x);
dfree(psf1y);
dfree(psf2x);
dfree(psf1);
cfree(otf2);
dfree(psf2);
}
cfftshift(nominal);//peak in corner
cfft2(nominal,-1);
cfftshift(nominal);//peak in center
cfft2i(nominal,1);
warning_once("double check nominal for off centered skyc.fnpsf1\n");
/*This nominal will multiply to OTF with peak in corner. But after
inverse fft, peak will be in center*/
ccp(&powfs[ipowfs].dtf[iwvl].nominal, nominal);
cfree(nominal);
loc_t *loc_psf=mksqloc(ncomp, ncomp, dtheta, dtheta, -ncomp2*dtheta, -ncomp2*dtheta);
powfs[ipowfs].dtf[iwvl].si=mkh(loc_psf,loc_ccd,0,0,1);
locfree(loc_psf);
if(parms->skyc.dbg){
writebin(powfs[ipowfs].dtf[iwvl].nominal,
"%s/powfs%d_dtf%d_nominal",dirsetup,ipowfs,iwvl);
writebin(powfs[ipowfs].dtf[iwvl].si,
"%s/powfs%d_dtf%d_si",dirsetup,ipowfs,iwvl);
}
powfs[ipowfs].dtf[iwvl].U=cnew(ncomp,1);
dcomplex *U=powfs[ipowfs].dtf[iwvl].U->p;
for(int ix=0; ix<ncomp; ix++){
int jx=ix<ncomp2?ix:(ix-ncomp);
U[ix]=COMPLEX(0, -2.*M_PI*jx*du);
}
}/*iwvl */
locfree(loc_ccd);
}/*ipowfs */
}
void dxStepBody (dxBody *b, dReal h)
{
int j;
#ifdef DEBUG_VALID
dIASSERT(dValid(b->avel[0])&&dValid(b->avel[1])&&dValid(b->avel[2]));
#endif
// handle linear velocity
for (j=0; j<3; j++) b->pos[j] += h * b->lvel[j];
if (b->flags & dxBodyFlagFiniteRotation) {
dVector3 irv; // infitesimal rotation vector
dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis) {
// split the angular velocity vector into a component along the finite
// rotation axis, and a component orthogonal to it.
dVector3 frv; // finite rotation vector
dReal k = dDOT (b->finite_rot_axis,b->avel);
frv[0] = b->finite_rot_axis[0] * k;
frv[1] = b->finite_rot_axis[1] * k;
frv[2] = b->finite_rot_axis[2] * k;
irv[0] = b->avel[0] - frv[0];
irv[1] = b->avel[1] - frv[1];
irv[2] = b->avel[2] - frv[2];
// make a rotation quaternion q that corresponds to frv * h.
// compare this with the full-finite-rotation case below.
h *= REAL(0.5);
dReal theta = k * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = frv[0] * s;
q[2] = frv[1] * s;
q[3] = frv[2] * s;
}
else {
// make a rotation quaternion q that corresponds to w * h
dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
b->avel[2]*b->avel[2]);
h *= REAL(0.5);
dReal theta = wlen * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = b->avel[0] * s;
q[2] = b->avel[1] * s;
q[3] = b->avel[2] * s;
}
// do the finite rotation
dQuaternion q2;
dQMultiply0 (q2,q,b->q);
for (j=0; j<4; j++) b->q[j] = q2[j];
// do the infitesimal rotation if required
if (b->flags & dxBodyFlagFiniteRotationAxis) {
dReal dq[4];
dWtoDQ (irv,b->q,dq);
for (j=0; j<4; j++) b->q[j] += h * dq[j];
}
}
else {
// the normal way - do an infitesimal rotation
dReal dq[4];
dWtoDQ (b->avel,b->q,dq);
for (j=0; j<4; j++) b->q[j] += h * dq[j];
}
// normalize the quaternion and convert it to a rotation matrix
dNormalize4 (b->q);
dQtoR (b->q,b->R);
// notify all attached geoms that this body has moved
for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
dGeomMoved (geom);
#ifdef DEBUG_VALID
dIASSERT(dValid(b->avel[0])&&dValid(b->avel[1])&&dValid(b->avel[2]));
#endif
}
const FIRInstance *
ParamFIR::make(int sample_rate) const
{
int i;
/////////////////////////////////////////////////////////////////////////////
// Normalize
double norm_factor = norm? 1.0: 1.0 / sample_rate;
double f1_ = f1 * norm_factor;
double f2_ = f2 * norm_factor;
double df_ = df * norm_factor;
/////////////////////////////////////////////////////////////////////////////
// Trivial cases
if (f1_ < 0.0 || f2_ < 0.0 || df_ <= 0.0 || a < 0.0) return 0;
if (a == 0.0) return new IdentityFIRInstance(sample_rate);
switch (type)
{
case low_pass:
if (f1_ >= 0.5) return new IdentityFIRInstance(sample_rate);
if (f1_ == 0.0) return new GainFIRInstance(sample_rate, db2value(a));
break;
case high_pass:
if (f1_ >= 0.5) return new GainFIRInstance(sample_rate, db2value(a));
if (f1_ == 0.0) return new IdentityFIRInstance(sample_rate);
break;
case band_pass:
if (f1_ >= 0.5) return new GainFIRInstance(sample_rate, db2value(a));
if (f2_ == 0.0) return new GainFIRInstance(sample_rate, db2value(a));
if (f1_ == 0.0 && f2_ >= 0.5) return new IdentityFIRInstance(sample_rate);
break;
case band_stop:
if (f1_ >= 0.5) return new IdentityFIRInstance(sample_rate);
if (f2_ == 0.0) return new IdentityFIRInstance(sample_rate);
if (f1_ == 0.0 && f2_ >= 0.5) return new GainFIRInstance(sample_rate, db2value(a));
break;
default:
return 0;
}
/////////////////////////////////////////////////////////////////////////////
// Build the filter
int n = kaiser_n(a, df_) | 1; // make odd (type 1 filter)
int c = n / 2;
DynamicFIRInstance *fir = new DynamicFIRInstance(sample_rate, n, c);
double *filter = fir->buf;
filter[0] = 1.0;
double alpha = kaiser_alpha(a);
switch (type)
{
case low_pass:
for (i = 0; i < n; i++)
filter[i] = (sample_t) (2 * f1_ * sinc((i - c) * 2 * M_PI * f1_) * kaiser_window(i - c, n, alpha));
return fir;
case high_pass:
for (i = 0; i < n; i++)
filter[i] = (sample_t) (-2 * f1_ * sinc((i - c) * 2 * M_PI * f1_) * kaiser_window(i - c, n, alpha));
filter[c] = (sample_t) ((1 - 2 * f1_) * kaiser_window(0, n, alpha));
return fir;
case band_pass:
for (i = 0; i < n; i++)
filter[i] = (sample_t) ((2 * f2_ * sinc((i - c) * 2 * M_PI * f2_) - 2 * f1_ * sinc((i - c) * 2 * M_PI * f1_)) * kaiser_window(i - c, n, alpha));
return fir;
case band_stop:
for (i = 0; i < n; i++)
filter[i] = (sample_t) ((2 * f1_ * sinc((i - c) * 2 * M_PI * f1_) - 2 * f2_ * sinc((i - c) * 2 * M_PI * f2_)) * kaiser_window(i - c, n, alpha));
filter[c] = (sample_t) ((2 * f1_ + 1 - 2 * f2_) * kaiser_window(0, n, alpha));
return fir;
};
// never be here
assert(false);
delete filter;
return 0;
}
static void gentbl_fsk9600(FILE *f)
{
int i, j, k, l, m;
float s;
float c[44];
float min, max;
fprintf(f, "\n/*\n * fsk9600 specific tables\n */\n");
min = max = 0;
memset(c, 0, sizeof(c));
#if 0
memcpy(c+2, fsk96_tx_coeff_4, sizeof(fsk96_tx_coeff_4));
#else
for (i = 0; i < 29; i++)
c[3+i] = sinc(1.2*((i-14.0)/4.0))*hamming(i/28.0)/3.5;
#endif
fprintf(f, "static unsigned char fsk96_txfilt_4[] = {\n\t");
for (i = 0; i < 4; i++) {
for (j = 0; j < 256; j++) {
for (k = 1, s = 0, l = i; k < 256; k <<= 1) {
if (j & k) {
for (m = 0; m < 4; m++, l++)
s += c[l];
} else {
for (m = 0; m < 4; m++, l++)
s -= c[l];
}
}
s *= 0.75;
if (s > max)
max = s;
if (s < min)
min = s;
fprintf(f, "%4d", (int)(128+127*s));
if (i < 3 || j < 255)
fprintf(f, ",%s", (j & 7) == 7
? "\n\t" : "");
}
}
#ifdef VERBOSE
fprintf(stderr, "fsk9600: txfilt4: min = %f; max = %f\n", min, max);
#endif
fprintf(f, "\n};\n\n");
min = max = 0;
memset(c, 0, sizeof(c));
#if 0
memcpy(c+2, fsk96_tx_coeff_5, sizeof(fsk96_tx_coeff_5));
#else
for (i = 0; i < 36; i++)
c[4+i] = sinc(1.2*((i-17.5)/5.0))*hamming(i/35.0)/4.5;
#endif
fprintf(f, "static unsigned char fsk96_txfilt_5[] = {\n\t");
for (i = 0; i < 5; i++) {
for (j = 0; j < 256; j++) {
for (k = 1, s = 0, l = i; k < 256; k <<= 1) {
if (j & k) {
for (m = 0; m < 5; m++, l++)
s += c[l];
} else {
for (m = 0; m < 5; m++, l++)
s -= c[l];
}
}
s *= 0.75;
if (s > max)
max = s;
if (s < min)
min = s;
fprintf(f, "%4d", (int)(128+127*s));
if (i < 4 || j < 255)
fprintf(f, ",%s", (j & 7) == 7
? "\n\t" : "");
}
}
#ifdef VERBOSE
fprintf(stderr, "fsk9600: txfilt5: min = %f; max = %f\n", min, max);
#endif
fprintf(f, "\n};\n\n");
}
KFR_INTRINSIC friend vec<T, N> get_elements(const expression_lanczos& self, cinput_t cinput,
size_t index, vec_shape<T, N> y)
{
return sinc(get_elements(self.linspace, cinput, index, y));
}
