matlibstdtoolbox.cpp

Dream.exe soft source (Visual C++)

					if (left >= right)
					{
						/* The error bound specified is too small! */
						integralError = TRUE;
						return _MAXREAL; /* NaN */
					}

					m1 = m + step;
					x[m] = x[j];
					x[m1] = x[j] + h;
					v[m] = v1;
					v[m1] = v2;
					f1[m] = f1[j];
					f2[m] = fa;
					f3[m] = f2[j];
					f1[m1] = f2[j];
					f2[m1] = fb;
					f3[m1] = f3[j];
					m += 2 * step;
				}
			}
			j += step;
		}
		while (j != jend);
	}
	while (m != mstart);

	if (integralError)
		return _MAXREAL; /* NaN */
	else
		return value;
}

CComplex Quad(MATLIB_CALLBACK_QAUD f, const CReal a, const CReal b,
			  const CReal errorBound)
{
	/* Set globals */
	/* Generate rounding unit */
	CReal value;
	CReal ru = (CReal) 1.0;
	do
	{
		ru = (CReal) 0.5 * ru;
		value = (CReal) 1.0 + ru;
	}
	while (value != (CReal) 1.0);

	ru *= 2;

	CReal integralBound = errorBound;
	_BOOLEAN integralError = FALSE;

	/* Compute */
	return _integral(f, a, b, errorBound, integralBound, integralError, ru);
}

CMatlibVector<CComplex> Fft(const CMatlibVector<CComplex>& cvI,
							const CFftPlans& FftPlans)
{
	int				i;
	CFftPlans*		pCurPlan;
	fftw_complex*	pFftwComplexIn;
	fftw_complex*	pFftwComplexOut;

	const int				n(cvI.GetSize());

	CMatlibVector<CComplex>	cvReturn(n, VTY_TEMP);

	/* If input vector has zero length, return */
	if (n == 0)
		return cvReturn;

	/* Check, if plans are already created, else: create it */
	if (!FftPlans.IsInitialized())
	{
		pCurPlan = new CFftPlans;
		pCurPlan->Init(n);
	}
	else
	{
		/* Ugly, but ok: We transform "const" object in "non constant" object
		   since we KNOW that the original object is not constant since it
		   was already initialized! */
		pCurPlan = (CFftPlans*) &FftPlans;
	}

	pFftwComplexIn = pCurPlan->pFftwComplexIn;
	pFftwComplexOut = pCurPlan->pFftwComplexOut;

	/* fftw (Homepage: http://www.fftw.org/) */
	for (i = 0; i < n; i++)
	{
		pFftwComplexIn[i].re = cvI[i].real();
		pFftwComplexIn[i].im = cvI[i].imag();
	}

	/* Actual fftw call */
	fftw_one(pCurPlan->FFTPlForw, pFftwComplexIn, pFftwComplexOut);

	for (i = 0; i < n; i++)
		cvReturn[i] = CComplex(pFftwComplexOut[i].re, pFftwComplexOut[i].im);

	if (!FftPlans.IsInitialized())
		delete pCurPlan;

	return cvReturn;
}

CMatlibVector<CComplex> Ifft(const CMatlibVector<CComplex>& cvI,
							 const CFftPlans& FftPlans)
{
	int				i;
	CFftPlans*		pCurPlan;
	fftw_complex*	pFftwComplexIn;
	fftw_complex*	pFftwComplexOut;

	const int		n(cvI.GetSize());

	CMatlibVector<CComplex>	cvReturn(n, VTY_TEMP);

	/* If input vector has zero length, return */
	if (n == 0)
		return cvReturn;

	/* Check, if plans are already created, else: create it */
	if (!FftPlans.IsInitialized())
	{
		pCurPlan = new CFftPlans;
		pCurPlan->Init(n);
	}
	else
	{
		/* Ugly, but ok: We transform "const" object in "non constant" object
		   since we KNOW that the original object is not constant since it
		   was already initialized! */
		pCurPlan = (CFftPlans*) &FftPlans;
	}

	pFftwComplexIn = pCurPlan->pFftwComplexIn;
	pFftwComplexOut = pCurPlan->pFftwComplexOut;

	/* fftw (Homepage: http://www.fftw.org/) */
	for (i = 0; i < n; i++)
	{
		pFftwComplexIn[i].re = cvI[i].real();
		pFftwComplexIn[i].im = cvI[i].imag();
	}

	/* Actual fftw call */
	fftw_one(pCurPlan->FFTPlBackw, pFftwComplexIn, pFftwComplexOut);
	
	const CReal scale = (CReal) 1.0 / n;
	for (i = 0; i < n; i++)
	{
		cvReturn[i] = CComplex(pFftwComplexOut[i].re * scale,
			pFftwComplexOut[i].im * scale);
	}

	if (!FftPlans.IsInitialized())
		delete pCurPlan;

	return cvReturn;
}

CMatlibVector<CComplex> rfft(const CMatlibVector<CReal>& fvI,
							 const CFftPlans& FftPlans)
{
	int			i;
	CFftPlans*	pCurPlan;
	fftw_real*	pFftwRealIn;
	fftw_real*	pFftwRealOut;

	const int	iSizeI = fvI.GetSize();
	const int	iLongLength(iSizeI);
	const int	iShortLength(iLongLength / 2);
	const int	iUpRoundShortLength((iLongLength + 1) / 2);
	
	CMatlibVector<CComplex>	cvReturn(iShortLength
		/* Include Nyquist frequency in case of even N */ + 1, VTY_TEMP);

	/* If input vector has zero length, return */
	if (iLongLength == 0)
		return cvReturn;

	/* Check, if plans are already created, else: create it */
	if (!FftPlans.IsInitialized())
	{
		pCurPlan = new CFftPlans;
		pCurPlan->Init(iLongLength);
	}
	else
	{
		/* Ugly, but ok: We transform "const" object in "non constant" object
		   since we KNOW that the original object is not constant since it
		   was already initialized! */
		pCurPlan = (CFftPlans*) &FftPlans;
	}

	pFftwRealIn = pCurPlan->pFftwRealIn;
	pFftwRealOut = pCurPlan->pFftwRealOut;

	/* fftw (Homepage: http://www.fftw.org/) */
	for (i = 0; i < iSizeI; i++)
		pFftwRealIn[i] = fvI[i];

	/* Actual fftw call */
	rfftw_one(pCurPlan->RFFTPlForw, pFftwRealIn, pFftwRealOut);

	/* Now build complex output vector */
	/* Zero frequency */
	cvReturn[0] = pFftwRealOut[0];
	for (i = 1; i < iUpRoundShortLength; i++)
		cvReturn[i] = CComplex(pFftwRealOut[i], pFftwRealOut[iLongLength - i]);

	/* If N is even, include Nyquist frequency */
	if (iLongLength % 2 == 0)
		cvReturn[iShortLength] = pFftwRealOut[iShortLength];

	if (!FftPlans.IsInitialized())
		delete pCurPlan;

	return cvReturn;
}

CMatlibVector<CReal> rifft(const CMatlibVector<CComplex>& cvI,
						   const CFftPlans& FftPlans)
{
/*
	This function only works with EVEN N!
*/
	int			i;
	CFftPlans*	pCurPlan;
	fftw_real*	pFftwRealIn;
	fftw_real*	pFftwRealOut;

	const int	iShortLength(cvI.GetSize() - 1); /* Nyquist frequency! */
	const int	iLongLength(iShortLength * 2);

	CMatlibVector<CReal> fvReturn(iLongLength, VTY_TEMP);

	/* If input vector is too short, return */
	if (iShortLength <= 0)
		return fvReturn;

	/* Check, if plans are already created, else: create it */
	if (!FftPlans.IsInitialized())
	{
		pCurPlan = new CFftPlans;
		pCurPlan->Init(iLongLength);
	}
	else
	{
		/* Ugly, but ok: We transform "const" object in "non constant" object
		   since we KNOW that the original object is not constant since it
		   was already initialized! */
		pCurPlan = (CFftPlans*) &FftPlans;
	}

	pFftwRealIn = pCurPlan->pFftwRealIn;
	pFftwRealOut = pCurPlan->pFftwRealOut;

	/* Now build half-complex-vector */
	pFftwRealIn[0] = cvI[0].real();
	for (i = 1; i < iShortLength; i++)
	{
		pFftwRealIn[i] = cvI[i].real();
		pFftwRealIn[iLongLength - i] = cvI[i].imag();
	}
	/* Nyquist frequency */
	pFftwRealIn[iShortLength] = cvI[iShortLength].real(); 

	/* Actual fftw call */
	rfftw_one(pCurPlan->RFFTPlBackw, pFftwRealIn, pFftwRealOut);

	/* Scale output vector */
	const CReal scale = (CReal) 1.0 / iLongLength;
	for (i = 0; i < iLongLength; i++) 
		fvReturn[i] = pFftwRealOut[i] * scale;

	if (!FftPlans.IsInitialized())
		delete pCurPlan;

	return fvReturn;
}

CMatlibVector<CReal> FftFilt(const CMatlibVector<CComplex>& rvH,
							 const CMatlibVector<CReal>& rvI,
							 CMatlibVector<CReal>& rvZ,
							 const CFftPlans& FftPlans)
{
/*
	This function only works with EVEN N!
*/
	CFftPlans*				pCurPlan;
	const int				iL(rvH.GetSize() - 1); /* Nyquist frequency! */
	const int				iL2(2 * iL);
	CMatlibVector<CReal>	rvINew(iL2);
	CMatlibVector<CReal>	rvOutTMP(iL2);

	/* Check, if plans are already created, else: create it */
	if (!FftPlans.IsInitialized())
	{
		pCurPlan = new CFftPlans;
		pCurPlan->Init(iL2);
	}
	else
	{
		/* Ugly, but ok: We transform "const" object in "non constant" object
		   since we KNOW that the original object is not constant since it
		   was already initialized! */
		pCurPlan = (CFftPlans*) &FftPlans;
	}

	/* Update history of input signal */
	rvINew.Merge(rvZ, rvI);

	rvOutTMP = rifft(rfft(rvINew, FftPlans) * rvH, FftPlans);

	/* Save old input signal vector for next block */
	rvZ = rvI;

	/* Cut out correct samples (to get from cyclic convolution to linear
	   convolution) */
	return rvOutTMP(iL + 1, iL2);
}


/* FftPlans implementation -------------------------------------------------- */
CFftPlans::~CFftPlans()
{
	if (bInitialized)
	{
		/* Delete old plans and intermediate buffers */
		rfftw_destroy_plan(RFFTPlForw);
		rfftw_destroy_plan(RFFTPlBackw);
		fftw_destroy_plan(FFTPlForw);
		fftw_destroy_plan(FFTPlBackw);

		delete[] pFftwRealIn;
		delete[] pFftwRealOut;
		delete[] pFftwComplexIn;
		delete[] pFftwComplexOut;
	}
}

void CFftPlans::Init(const int iFSi)
{
	if (bInitialized)
	{
		/* Delete old plans and intermediate buffers */
		rfftw_destroy_plan(RFFTPlForw);
		rfftw_destroy_plan(RFFTPlBackw);
		fftw_destroy_plan(FFTPlForw);
		fftw_destroy_plan(FFTPlBackw);

		delete[] pFftwRealIn;
		delete[] pFftwRealOut;
		delete[] pFftwComplexIn;
		delete[] pFftwComplexOut;
	}

	/* Create new plans and intermediate buffers */
	pFftwRealIn = new fftw_real[iFSi];
	pFftwRealOut = new fftw_real[iFSi];
	pFftwComplexIn = new fftw_complex[iFSi];
	pFftwComplexOut = new fftw_complex[iFSi];

	RFFTPlForw = rfftw_create_plan(iFSi, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE);
	RFFTPlBackw = rfftw_create_plan(iFSi, FFTW_COMPLEX_TO_REAL, FFTW_ESTIMATE);
	FFTPlForw = fftw_create_plan(iFSi, FFTW_FORWARD, FFTW_ESTIMATE);
	FFTPlBackw = fftw_create_plan(iFSi, FFTW_BACKWARD, FFTW_ESTIMATE);

	bInitialized = true;
}