void readSector(char* buffer, int sector){
int relativeSector;
int head;
int track;
relativeSector=myMod(sector,18)+1;
head=myMod(myDiv(sector,18),2);
track=myDiv(sector,36);
interrupt(0x13,2*256+1,buffer,track*256+relativeSector,head*256);
}
void draw ()
{
int i, j, k;
int indexx, indexy, indexz;
float xyz[4], dir[4], angvel[4], tempVec[4];
static float oldxyz[4] = { 0.0f, 0.0f, 0.0f, 0.0f };
static float oldDir[4] = { 0.0f, 0.0f, 0.0f, 0.0f };
static float oldAngvel[4] = { 0.0f, 0.0f, 0.0f, 0.0f };
float angle, distance;
float rotMat[16];
float newQuat[4] = { 0.0f, 0.0f, 0.0f, 1.0f };
static float quat[4] = { 0.0f, 0.0f, 0.0f, 1.0f };
static int flymode = 1;
static float flymodeChange = 20.0f;
static int seg = 0; /* Section of path */
static float where = 0.0f; /* Position on path */
static float rollVel = 0.0f, rollAcc = 0.0f;
int drawDepth = dDrawdepth + 2;
static float rollChange = 0; /* rsRandf (10.0f) + 2.0f; */
where += (float)dSpeed * 0.05f * elapsedTime;
if (where >= 1.0f) {
where -= 1.0f;
seg++;
}
if (seg >= segments) {
seg = 0;
reconfigure ();
}
/*
* Calculate position
*/
xyz[0] = interpolate (path[seg][0], path[seg][3], path[seg + 1][0], path[seg + 1][3], where);
xyz[1] = interpolate (path[seg][1], path[seg][4], path[seg + 1][1], path[seg + 1][4], where);
xyz[2] = interpolate (path[seg][2], path[seg][5], path[seg + 1][2], path[seg + 1][5], where);
/*
* Do rotation stuff
*/
rsVec_subtract (xyz, oldxyz, (float *)&dir);
rsVec_normalize ((float *)&dir);
rsVec_cross ((float *)&angvel, dir, oldDir); /* Desired axis of rotation */
/* Protect against mild "overflow" */
float dot = MAX(MIN(rsVec_dot (oldDir, dir), -1.0), 1.0);
float maxSpin = 0.25f * (float)dSpeed * elapsedTime;
angle = MAX(MIN(acos(dot), -maxSpin), maxSpin);
rsVec_scale ((float *)&angvel, angle); /* Desired angular velocity */
rsVec_subtract (angvel, oldAngvel, (float *)&tempVec); /* Change in angular velocity */
distance = rsVec_length (tempVec); /* Don't let angular velocity change too much */
float rotationInertia = 0.007f * (float)dSpeed * elapsedTime;
if (distance > rotationInertia * elapsedTime) {
rsVec_scale ((float *)&tempVec, ((rotationInertia * elapsedTime) / distance));
rsVec_add (oldAngvel, tempVec, (float *)&angvel);
}
flymodeChange -= elapsedTime;
if (flymodeChange <= 1.0f) /* prepare to transition */
rsVec_scale ((float *)&angvel, flymodeChange);
if (flymodeChange <= 0.0f) { /* transition from one fly mode to
* the other? */
flymode = rsRandi (4);
flymodeChange = rsRandf ((float)(150 - dSpeed)) + 5.0f;
}
rsVec_copy (angvel, (float *)&tempVec); /* Recompute desired rotation */
angle = rsVec_normalize ((float *)&tempVec);
rsQuat_make ((float *)&newQuat, angle, tempVec[0], tempVec[1], tempVec[2]); /* Use rotation */
if (flymode) /* fly normal (straight) */
rsQuat_preMult ((float *)&quat, newQuat);
else /* don't fly normal (go backwards and stuff) */
rsQuat_postMult ((float *)&quat, newQuat);
/* Roll */
rollChange -= elapsedTime;
if (rollChange <= 0.0f) {
rollAcc = rsRandf (0.02f * (float)dSpeed) - (0.01f * (float)dSpeed);
rollChange = rsRandf (10.0f) + 2.0f;
}
rollVel += rollAcc * elapsedTime;
if (rollVel > (0.04f * (float)dSpeed) && rollAcc > 0.0f)
rollAcc = 0.0f;
if (rollVel < (-0.04f * (float)dSpeed) && rollAcc < 0.0f)
rollAcc = 0.0f;
rsQuat_make ((float *)&newQuat, rollVel * elapsedTime, oldDir[0], oldDir[1], oldDir[2]);
rsQuat_preMult ((float *)&quat, newQuat);
/* quat.normalize(); */
rsQuat_toMat ((float *)&quat, (float *)&rotMat);
/*
* Save old stuff
*/
rsVec_copy (xyz, (float *)&oldxyz);
oldDir[0] = -rotMat[2];
oldDir[1] = -rotMat[6];
oldDir[2] = -rotMat[10];
rsVec_copy (angvel, (float *)&oldAngvel);
/*
* Apply transformations
*/
glMatrixMode (GL_MODELVIEW);
glLoadMatrixf (rotMat);
glTranslatef (-xyz[0], -xyz[1], -xyz[2]);
// Just in case display lists contain no colors
glColor3f(1.0f, 1.0f, 1.0f);
// Environment mapping for crystal, chrome, brass, shiny, and ghostly
if(dTexture == 2 || dTexture == 3 || dTexture == 4 || dTexture == 5 || dTexture == 6){
glTexGeni(GL_S, GL_TEXTURE_GEN_MODE, GL_SPHERE_MAP);
glTexGeni(GL_T, GL_TEXTURE_GEN_MODE, GL_SPHERE_MAP);
glEnable(GL_TEXTURE_GEN_S);
glEnable(GL_TEXTURE_GEN_T);
}
/*
* Render everything
*/
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
for (i = globalxyz[0] - drawDepth; i <= globalxyz[0] + drawDepth; i++) {
for (j = globalxyz[1] - drawDepth; j <= globalxyz[1] + drawDepth; j++) {
for (k = globalxyz[2] - drawDepth; k <= globalxyz[2] + drawDepth; k++) {
float tpos[4]; /* transformed position */
tempVec[0] = (float)i - xyz[0];
tempVec[1] = (float)j - xyz[1];
tempVec[2] = (float)k - xyz[2];
tpos[0] = tempVec[0] * rotMat[0] + tempVec[1] * rotMat[4] + tempVec[2] * rotMat[8]; /* + rotMat[12]; */
tpos[1] = tempVec[0] * rotMat[1] + tempVec[1] * rotMat[5] + tempVec[2] * rotMat[9]; /* + rotMat[13]; */
tpos[2] = tempVec[0] * rotMat[2] + tempVec[1] * rotMat[6] + tempVec[2] * rotMat[10]; /* + rotMat[14]; */
#define radius 0.9f
/* camera_inViewVolume */
/*
* check back plane
*/
if (!(tpos[2] < -(theCamera.farplane + radius))) {
/*
* check bottom plane
*/
if (!(rsVec_dot(tpos, theCamera.cullVec[0]) < -radius)) {
/*
* check top plane
*/
if (!(rsVec_dot(tpos, theCamera.cullVec[1]) < -radius)) {
/*
* check left plane
*/
if (!(rsVec_dot(tpos, theCamera.cullVec[2]) < -radius)) {
/*
* check right plane
*/
if (!(rsVec_dot(tpos, theCamera.cullVec[3]) < -radius)) {
indexx = myMod (i);
indexy = myMod (j);
indexz = myMod (k);
/*
* draw it
*/
glPushMatrix ();
glTranslatef ((float)i, (float)j, (float)k);
glCallList (lattice[indexx][indexy][indexz]);
glPopMatrix ();
}
}
}
}
}
}
}
}
glDisable(GL_TEXTURE_GEN_S);
glDisable(GL_TEXTURE_GEN_T);
glFlush ();
}
// [bm, env, instp, instf] = gammatone_c(x, P_in, fs, cf, align, hrect)
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
double *x=0, *cf, *bm, *env = 0, *instp = 0, *instf = 0 ,*P_in=0, *P_out=0;
int i, j, t, fs, nsamples, hrect, ch, Channels, align=0, intshift=0;
double xx=0,a, tpt, tptbw, gain, envelopecomptime=0, phasealign=0;
double p0r, p1r, p2r, p3r, p4r, p0i, p1i, p2i, p3i, p4i;
double a1, a2, a3, a4, a5, u0r, u0i; /*, u1r, u1i;*/
double qcos, qsin, oldcs, coscf, sincf, oldphase, dp, dps;
/*=========================================
* Check argument numbers
*=========================================
*/
if (nrhs < 4) {
mexPrintf("??? Not enough input arguments.\n");
return;
}
if (nrhs > 6) {
mexPrintf("??? Too many input arguments.\n");
return;
}
/*=========================================
* input arguments
*=========================================
*/
if (nrhs < 6) {
hrect = 0;
}
else {
hrect = (int)mxGetScalar(IN_hrect);
}
if (nrhs < 5) {
align = 0; //default is align=off or zero
}
else {
align = (int)mxGetScalar(IN_align);
}
Channels = mxGetN(IN_cf);//number of channels
x = mxGetPr(IN_x); /* waveform */
i = mxGetN(IN_x);
j = mxGetM(IN_x);
if (i > 1 && j > 1) {
mexPrintf("??? Input x must be a vector.\n");
return;
}
nsamples = myMax(i, j);
fs = (int)mxGetScalar(IN_fs); /* sampling rate */
cf = mxGetPr(IN_cf); /* centre frequency */
P_in= mxGetPr ( IN_P );
/*=========================================
* output arguments
*=========================================
*/
OUT_bm = mxCreateDoubleMatrix( nsamples,Channels ,mxREAL);
bm = mxGetPr(OUT_bm);
if (nlhs > 1) {
OUT_env = mxCreateDoubleMatrix(nsamples,Channels, mxREAL);
env = mxGetPr(OUT_env);
}
if ( nlhs > 2 ) {
OUT_P = mxCreateDoubleMatrix ( 8, Channels, mxREAL );
P_out = mxGetPr ( OUT_P );
}
if (nlhs > 3) {
OUT_instp = mxCreateDoubleMatrix(nsamples,Channels, mxREAL);
instp = mxGetPr(OUT_instp);
}
if (nlhs > 4) {
OUT_instf = mxCreateDoubleMatrix(nsamples,Channels, mxREAL);
instf = mxGetPr(OUT_instf);
}
for (ch = 0; ch < Channels; ch++)
{
/*=========================================
* Initialising variables
*=========================================
*/
oldphase = 0.0;
tpt = (M_PI + M_PI) / fs;
tptbw = tpt * erb(cf[ch]) * BW_CORRECTION;
a = exp(-tptbw);
if (align)
{
//B=1.019*2*M_PI*erb(cf);
envelopecomptime = 3/(BW_CORRECTION*2*M_PI*erb(cf[ch]));
}
else
{
envelopecomptime = 0;
}
intshift=(int)(floor(envelopecomptime*fs));
//
// phasealign=-2*M_PI*cf[ch]*envelopecomptime;
// //phasealign=mod(phasealign,2*M_PI);
// phasealign=fmod(phasealign,2*M_PI);
// //phasealign=myMod(phasealign,2*M_PI);
// phasealign=phasealign/(2*M_PI*cf[ch]);
/* based on integral of impulse response */
gain = (tptbw*tptbw*tptbw*tptbw) / 3;
/* Update filter coefficients */
a1 = 4.0*a; a2 = -6.0*a*a; a3 = 4.0*a*a*a; a4 = -a*a*a*a; a5 = a*a;
p0r = 0.0;
p1r = P_in[0+ch*8]; p2r =P_in[1+ch*8]; p3r = P_in[2+ch*8]; p4r = P_in[3+ch*8];
//P[0]=p1r;P[1]=p2r;P[2]=p3r;P[3]=p4r;P[4]=p1i;P[5]=p2i;P[6]=p3i;P[7]=p4i;
p0i = 0.0;
p1i = P_in[4+ch*8]; p2i = P_in[5+ch*8]; p3i = P_in[6+ch*8]; p4i = P_in[7+ch*8];
/*===========================================================
* exp(a+i*b) = exp(a)*(cos(b)+i*sin(b))
* q = exp(-i*tpt*cf*t) = cos(tpt*cf*t) + i*(-sin(tpt*cf*t))
* qcos = cos(tpt*cf*t)
* qsin = -sin(tpt*cf*t)
*===========================================================
*/
coscf = cos(tpt * cf[ch]);
sincf = sin(tpt * cf[ch]);
qcos = 1; qsin = 0; /* t=0 & q = exp(-i*tpt*t*cf)*/
for (t = 0; t < (nsamples); t++)
{
if (t>(nsamples-1))
{
xx=0;
}
else
{
xx=x[t];
}
/*
x = [x zeros(1,intshift)];
kT=(0:length(x)-1)/fs;
q=exp(1i.*(-wcf.*kT)).*x; % shift down to d.c.
p=filter([1 0],[1 -4*a 6*a^2 -4*a^3 a^4],q); % filter: part 1
u=filter([1 4*a 4*a^2 0],[1 0],p); % filter: part 2
bm=gain*real(exp(1i*wcf*(kT(intshift+1:end)+phasealign)).*u(intshift+1:end)); % shift up in frequency
env = gain*abs(u(intshift+1:end));
instf=real(cf+[diff(unwrap(angle(u(intshift+1:end)))) 0]./tpt);
*/
/* Filter part 1 & shift down to d.c. */
p0r = qcos*xx + a1*p1r + a2*p2r + a3*p3r + a4*p4r;
p0i = qsin*xx + a1*p1i + a2*p2i + a3*p3i + a4*p4i;
/* Clip coefficients to stop them from becoming too close to zero */
if (fabs(p0r) < VERY_SMALL_NUMBER)
p0r = 0.0F;
if (fabs(p0i) < VERY_SMALL_NUMBER)
p0i = 0.0F;
/* Filter part 2 */
u0r = p0r + a1*p1r + a5*p2r;
u0i = p0i + a1*p1i + a5*p2i;
/* Update filter results */
p4r = p3r; p3r = p2r; p2r = p1r; p1r = p0r;
p4i = p3i; p3i = p2i; p2i = p1i; p1i = p0i;
/*==========================================
* Basilar membrane response
* 1/ shift up in frequency first: (u0r+i*u0i) * exp(i*tpt*cf*t) = (u0r+i*u0i) * (qcos + i*(-qsin))
* 2/ take the real part only: bm = real(exp(j*wcf*kT).*u) * gain;
bm = real(exp(j*wcf*kT+j*wcf*phasealign).*u) * gain; =(u0r+i*u0i) * (qcos + i*(-qsin))*(cos(phasealign)+i*sin(phasealign))
=(u0r+i*u0i) *(qcos*C+qsin*S +i(qcos*S-qsin*C))-->Real()=u0r * (qcos * C + qsin* S)+ u0i*(qsin*C-qcos*S)
*==========================================
*/
if (t>(intshift-1))
{
//bm[t + ch*(nsamples)-ch*(intshift)] = (u0r * qcos + u0i * qsin) * gain
bm[t + ch*(nsamples)-(intshift)] =(u0r * (qcos * cos(2*M_PI*cf[ch]*phasealign) + qsin* sin(2*M_PI*cf[ch]*phasealign))+ u0i*(qsin*cos(2*M_PI*cf[ch]*phasealign)-qcos*sin(2*M_PI*cf[ch]*phasealign)) )* gain;
if (1 == hrect && bm[t + ch*(nsamples)-(intshift)] < 0) {
bm[t + ch*(nsamples)-(intshift)] = 0; /* half-wave rectifying */
}
if ( nlhs > 1 ) {
P_out[0+ch*8]=p1r;P_out[1+ch*8]=p2r;P_out[2+ch*8]=p3r;P_out[3+ch*8]=p4r;P_out[4+ch*8]=p1i;P_out[5+ch*8]=p2i;P_out[6+ch*8]=p3i;P_out[7+ch*8]=p4i;
}
/*==========================================
* Instantaneous Hilbert envelope
* env = abs(u) * gain;
*==========================================
*/
if (nlhs > 2) {
env[t + ch*(nsamples)-(intshift)] = sqrt(u0r * u0r + u0i * u0i) * gain;
}
/*==========================================
* Instantaneous phase
* instp = unwrap(angle(u));
*==========================================
*/
if (nlhs > 3) {
instp[t + ch*(nsamples)-(intshift)] = atan2(u0i, u0r);
/* unwrap it */
dp = instp[t + ch*(nsamples)-(intshift)] - oldphase;
if (abs(dp) > M_PI) {
dps = myMod(dp + M_PI, 2 * M_PI) - M_PI;
if (dps == -M_PI && dp > 0) {
dps = M_PI;
}
instp[t + ch*(nsamples)-(intshift)] = instp[t + ch*(nsamples)-(intshift)] + dps - dp;
}
oldphase = instp[t + ch*(nsamples)-(intshift)];
}
/*==========================================
* Instantaneous frequency
* instf = cf + [diff(instp) 0]./tpt;
*==========================================
*/
if (nlhs > 4 && t > (intshift)) {
instf[t - 1 + ch*(nsamples)-(intshift)] = cf[ch] + (instp[t + ch*(nsamples)-(intshift)] - instp[t - 1 + ch*(nsamples)-(intshift)]) / tpt;
}
//
// /*====================================================
// * The basic idea of saving computational load:
// * cos(a+b) = cos(a)*cos(b) - sin(a)*sin(b)
// * sin(a+b) = sin(a)*cos(b) + cos(a)*sin(b)
// * qcos = cos(tpt*cf*t) = cos(tpt*cf + tpt*cf*(t-1))
// * qsin = -sin(tpt*cf*t) = -sin(tpt*cf + tpt*cf*(t-1))
// *====================================================
// */
} //t>(intshift-1)
qcos = coscf * (oldcs = qcos) + sincf * qsin;
qsin = coscf * qsin - sincf * oldcs;
}//samples for loop
if (nlhs > 4) {
//instf[(ch+1)*nsamples - 1] = cf[ch];
instf[nsamples - 1] = cf[ch];
}
}//Channels for loop
return;
} //mexFunction
/*=======================
* Main Function
*=======================
*/
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
double *x, *cf, *bm, *env = 0, *instp = 0, *instf = 0 ,*P_in=0, *P_out=0;
int i, j, t, fs, nsamples, hrect, ch, Channels;
double a, tpt, tptbw, gain;
double p0r, p1r, p2r, p3r, p4r, p0i, p1i, p2i, p3i, p4i;
double a1, a2, a3, a4, a5, u0r, u0i; /*, u1r, u1i;*/
double qcos, qsin, oldcs, coscf, sincf, oldphase, dp, dps;
/*=========================================
* Check argument numbers
*=========================================
*/
if (nrhs < 4) {
mexPrintf("??? Not enough input arguments.\n");
return;
}
if (nrhs > 5) {
mexPrintf("??? Too many input arguments.\n");
return;
}
if (nlhs > 5) {
mexPrintf("??? Too many output arguments.\n");
return;
}
/*=========================================
* input arguments
*=========================================
*/
if (nrhs < 5) {
hrect = 0;
}
else {
hrect = (int)mxGetScalar(IN_hrect);
}
Channels = mxGetN(IN_cf);//number of channles
x = mxGetPr(IN_x); /* waveform */
i = mxGetN(IN_x);
j = mxGetM(IN_x);
if (i > 1 && j > 1) {
mexPrintf("??? Input x must be a vector.\n");
return;
}
nsamples = myMax(i, j);
fs = (int)mxGetScalar(IN_fs); /* sampling rate */
cf = mxGetPr(IN_cf); /* centre frequency */
P_in= mxGetPr ( IN_P );
/*=========================================
* output arguments
*=========================================
*/
OUT_bm = mxCreateDoubleMatrix( nsamples,Channels, mxREAL);
bm = mxGetPr(OUT_bm);
if (nlhs > 1) {
OUT_env = mxCreateDoubleMatrix(nsamples,Channels, mxREAL);
env = mxGetPr(OUT_env);
}
if ( nlhs > 2 ) {
OUT_P = mxCreateDoubleMatrix ( 8, Channels, mxREAL );
P_out = mxGetPr ( OUT_P );
}
if (nlhs > 3) {
OUT_instp = mxCreateDoubleMatrix(nsamples,Channels, mxREAL);
instp = mxGetPr(OUT_instp);
}
if (nlhs > 4) {
OUT_instf = mxCreateDoubleMatrix(nsamples,Channels, mxREAL);
instf = mxGetPr(OUT_instf);
}
for (ch = 0; ch < Channels; ch++)
{
/*=========================================
* Initialising variables
*=========================================
*/
oldphase = 0.0;
tpt = (M_PI + M_PI) / fs;
tptbw = tpt * erb(cf[ch]) * BW_CORRECTION;
a = exp(-tptbw);
/* based on integral of impulse response */
gain = (tptbw*tptbw*tptbw*tptbw) / 3;
/* Update filter coefficients */
a1 = 4.0*a; a2 = -6.0*a*a; a3 = 4.0*a*a*a; a4 = -a*a*a*a; a5 = a*a;
p0r = 0.0;
p1r = P_in[0+ch*8]; p2r =P_in[1+ch*8]; p3r = P_in[2+ch*8]; p4r = P_in[3+ch*8];
//P[0]=p1r;P[1]=p2r;P[2]=p3r;P[3]=p4r;P[4]=p1i;P[5]=p2i;P[6]=p3i;P[7]=p4i;
p0i = 0.0;
p1i = P_in[4+ch*8]; p2i = P_in[5+ch*8]; p3i = P_in[6+ch*8]; p4i = P_in[7+ch*8];
/*===========================================================
* exp(a+i*b) = exp(a)*(cos(b)+i*sin(b))
* q = exp(-i*tpt*cf*t) = cos(tpt*cf*t) + i*(-sin(tpt*cf*t))
* qcos = cos(tpt*cf*t)
* qsin = -sin(tpt*cf*t)
*===========================================================
*/
coscf = cos(tpt * cf[ch]);
sincf = sin(tpt * cf[ch]);
qcos = 1; qsin = 0; /* t=0 & q = exp(-i*tpt*t*cf)*/
for (t = 0; t < nsamples; t++)
{
/* Filter part 1 & shift down to d.c. */
p0r = qcos*x[t] + a1*p1r + a2*p2r + a3*p3r + a4*p4r;
p0i = qsin*x[t] + a1*p1i + a2*p2i + a3*p3i + a4*p4i;
/* Clip coefficients to stop them from becoming too close to zero */
if (fabs(p0r) < VERY_SMALL_NUMBER)
p0r = 0.0F;
if (fabs(p0i) < VERY_SMALL_NUMBER)
p0i = 0.0F;
/* Filter part 2 */
u0r = p0r + a1*p1r + a5*p2r;
u0i = p0i + a1*p1i + a5*p2i;
/* Update filter results */
p4r = p3r; p3r = p2r; p2r = p1r; p1r = p0r;
p4i = p3i; p3i = p2i; p2i = p1i; p1i = p0i;
/*==========================================
* Basilar membrane response
* 1/ shift up in frequency first: (u0r+i*u0i) * exp(i*tpt*cf*t) = (u0r+i*u0i) * (qcos + i*(-qsin))
* 2/ take the real part only: bm = real(exp(j*wcf*kT).*u) * gain;
*==========================================
*/
bm[t + ch*(nsamples)] = (u0r * qcos + u0i * qsin) * gain;
if (1 == hrect && bm[t + ch*(nsamples)] < 0) {
bm[t + ch*(nsamples)] = 0; /* half-wave rectifying */
}
if ( nlhs > 1 ) {
P_out[0+ch*8]=p1r;P_out[1+ch*8]=p2r;P_out[2+ch*8]=p3r;P_out[3+ch*8]=p4r;P_out[4+ch*8]=p1i;P_out[5+ch*8]=p2i;P_out[6+ch*8]=p3i;P_out[7+ch*8]=p4i;
}
/*==========================================
* Instantaneous Hilbert envelope
* env = abs(u) * gain;
*==========================================
*/
if (nlhs > 2) {
env[t + ch*(nsamples)] = sqrt(u0r * u0r + u0i * u0i) * gain;
}
/*==========================================
* Instantaneous phase
* instp = unwrap(angle(u));
*==========================================
*/
if (nlhs > 3) {
instp[t + ch*(nsamples)] = atan2(u0i, u0r);
/* unwrap it */
dp = instp[t + ch*(nsamples)] - oldphase;
if (abs(dp) > M_PI) {
dps = myMod(dp + M_PI, 2 * M_PI) - M_PI;
if (dps == -M_PI && dp > 0) {
dps = M_PI;
}
instp[t + ch*(nsamples)] = instp[t + ch*(nsamples)] + dps - dp;
}
oldphase = instp[t + ch*(nsamples)];
}
/*==========================================
* Instantaneous frequency
* instf = cf + [diff(instp) 0]./tpt;
*==========================================
*/
if (nlhs > 4 && t > 0) {
instf[t - 1 + ch*(nsamples)] = cf[ch] + (instp[t + ch*(nsamples)] - instp[t - 1 + ch*(nsamples)]) / tpt;
}
/*====================================================
* The basic idea of saving computational load:
* cos(a+b) = cos(a)*cos(b) - sin(a)*sin(b)
* sin(a+b) = sin(a)*cos(b) + cos(a)*sin(b)
* qcos = cos(tpt*cf*t) = cos(tpt*cf + tpt*cf*(t-1))
* qsin = -sin(tpt*cf*t) = -sin(tpt*cf + tpt*cf*(t-1))
*====================================================
*/
qcos = coscf * (oldcs = qcos) + sincf * qsin;
qsin = coscf * qsin - sincf * oldcs;
}//samples for loop
if (nlhs > 4) {
instf[nsamples - 1] = cf[ch];
}
}//Channels for loop
return;
} //mexFunction
