//---------------------------------------------------------------------------
void CheckMax3(Uint8 *dst, Uint8 *buff1, Uint8 *buff2, int w, int h)
{
Uint8 val = 0;
for(int i=0; i<h; i++)
{
for(int j=0; j<w; j++)
{
val = myMax(*buff1, *buff2);
buff1++;
buff2++;
val = myMax(*dst, val);
*dst++ = val;
}
}
}
int main(int argc,char **argv)
{
int a=5,b=5;
int c=a+b;
int d;
bool even=0;
#pragma SPIN_TARGET
while(d<c)
{
#pragma SPIN_TARGET
d++;
}
for (int i=0;i<100;i++)
{
a=1;
}
#pragma SPIN_TARGET
c=myMax(a,b);
#pragma SPIN_TARGET
if ((d-1)==0)
{
even=true;
}
#pragma assert(c==10);
return 0;
}
void cvMyOverLapping( IplImage* src1, IplImage* src2, IplImage* src3, IplImage* src4, IplImage* Index, IplImage* dst )
{
uchar* idx = ( uchar* )Index->imageData;
int widthStep1 = Index->widthStep;
int widthStep = src1->widthStep/4;
int baseIndex1 = -1*widthStep1;
int baseIndex = -1*widthStep;
int width = Index->width;
int height = Index->height;
float* src1Data = ( float* )src1->imageData;
float* src2Data = ( float* )src2->imageData;
float* src3Data = ( float* )src3->imageData;
float* src4Data = ( float* )src4->imageData;
float* dstData = ( float* )dst->imageData;
double val1, val2, val3;
int i, j;
int k =0;
uchar a = (uchar) 255;
for( i =0; i<height; i++)
{
baseIndex1 +=widthStep1;
baseIndex +=widthStep;
for( j = 0 ; j<width; j++ )
{ k++;
if(idx[ baseIndex1+j ] == a )
{
val1 = src1Data[ baseIndex+j ] * src1Data[ baseIndex+j ];
val2 = src2Data[ baseIndex+j ] * src2Data[ baseIndex+j ];
val1 = myMax( val1, val2 );
val3 = src3Data[ baseIndex+j ] * src3Data[ baseIndex+j ];
val2 = src4Data[ baseIndex+j ] * src4Data[ baseIndex+j ];
val2 = myMax( val3, val2 );
val3 = sqrt(val1+val2) -1;
//printf("%lf\n",val3);
dstData[ baseIndex+j ] =val3;
}
}
//printf("\n");
}
//printf("%d", k);
}
int isBalancedNew(struct TreeNode *root)
{
if(root == NULL) return 0;
int left = isBalancedNew(root->left);
int right = isBalancedNew(root->right);
if(left < 0 || right < 0 || abs(left-right) > 1) return -1;
return myMax(left,right)+1;
}
//---------------------------------------------------------------------------
void CheckMaxAll(Uint8 *dst, Uint8 *buff, int w, int h)
{
Uint8 val = 0;
for(int i=0; i<h; i++)
{
for(int j=0; j<w; j++)
{
val = myMax(*dst, *buff);
*dst++ = val;
buff++;
}
}
}
void
BlockAnalyzer::resizeEvent( QResizeEvent *e )
{
DEBUG_BLOCK
Analyzer::Base2D::resizeEvent( e );
const uint oldRows = m_rows;
//all is explained in analyze()..
//+1 to counter -1 in maxSizes, trust me we need this!
m_columns = myMax( uint(double(width()+1) / (WIDTH+1)), MAX_COLUMNS );
m_rows = uint(double(height()+1) / (HEIGHT+1));
//this is the y-offset for drawing from the top of the widget
m_y = (height() - (m_rows * (HEIGHT+1)) + 2) / 2;
m_scope.resize( m_columns );
if( m_rows != oldRows ) {
m_barPixmap = QPixmap( WIDTH, m_rows*(HEIGHT+1) );
for ( uint i = 0; i < FADE_SIZE; ++i )
m_fade_bars[i] = QPixmap( WIDTH, m_rows*(HEIGHT+1) );
m_yscale.resize( m_rows + 1 );
const float PRE = 1, PRO = 1; //PRE and PRO allow us to restrict the range somewhat
for( uint z = 0; z < m_rows; ++z )
m_yscale[z] = 1 - (log10( PRE+z ) / log10( PRE+m_rows+PRO ));
m_yscale[m_rows] = 0;
determineStep();
paletteChange( palette() );
}
else if( width() > e->oldSize().width() || height() > e->oldSize().height() )
drawBackground();
analyze( m_scope );
}
// [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
// parse texture model and align the vertices
void TextureModel()
{
GLMgroup* group = OBJ->groups;
float maxx, maxy, maxz;
float minx, miny, minz;
float dx, dy, dz;
maxx = minx = OBJ->vertices[3];
maxy = miny = OBJ->vertices[4];
maxz = minz = OBJ->vertices[5];
for(unsigned int i=2; i<=OBJ->numvertices; i++){
GLfloat vx, vy, vz;
vx = OBJ->vertices[i*3+0];
vy = OBJ->vertices[i*3+1];
vz = OBJ->vertices[i*3+2];
if(vx > maxx) maxx = vx; if(vx < minx) minx = vx;
if(vy > maxy) maxy = vy; if(vy < miny) miny = vy;
if(vz > maxz) maxz = vz; if(vz < minz) minz = vz;
}
//printf("max\n%f %f, %f %f, %f %f\n", maxx, minx, maxy, miny, maxz, minz);
dx = maxx - minx;
dy = maxy - miny;
dz = maxz - minz;
//printf("dx,dy,dz = %f %f %f\n", dx, dy, dz);
GLfloat normalizationScale = myMax(myMax(dx, dy), dz)/2;
OBJ->position[0] = (maxx + minx) / 2;
OBJ->position[1] = (maxy + miny) / 2;
OBJ->position[2] = (maxz + minz) / 2;
int gCount = 0;
while(group){
for(unsigned int i=0; i<group->numtriangles; i++){
// triangle index
int triangleID = group->triangles[i];
// the index of each vertex
int indv1 = OBJ->triangles[triangleID].vindices[0];
int indv2 = OBJ->triangles[triangleID].vindices[1];
int indv3 = OBJ->triangles[triangleID].vindices[2];
// vertices
GLfloat vx, vy, vz;
vx = OBJ->vertices[indv1*3];
vy = OBJ->vertices[indv1*3+1];
vz = OBJ->vertices[indv1*3+2];
//printf("%f %f %f\n", vx, vy, vz);
vertices[gCount][i*9+0] = vx;
vertices[gCount][i*9+1] = vy;
vertices[gCount][i*9+2] = vz;
vx = OBJ->vertices[indv2*3];
vy = OBJ->vertices[indv2*3+1];
vz = OBJ->vertices[indv2*3+2];
//printf("%f %f %f\n", vx, vy, vz);
vertices[gCount][i*9+3] = vx;
vertices[gCount][i*9+4] = vy;
vertices[gCount][i*9+5] = vz;
vx = OBJ->vertices[indv3*3];
vy = OBJ->vertices[indv3*3+1];
vz = OBJ->vertices[indv3*3+2];
//printf("%f %f %f\n", vx, vy, vz);
vertices[gCount][i*9+6] = vx;
vertices[gCount][i*9+7] = vy;
vertices[gCount][i*9+8] = vz;
//printf("\n");
int indt1 = OBJ->triangles[triangleID].tindices[0];
int indt2 = OBJ->triangles[triangleID].tindices[1];
int indt3 = OBJ->triangles[triangleID].tindices[2];
// TODO: texture coordinates should be aligned by yourself
//OBJ->texcoords[indt1*2];
//OBJ->texcoords[indt1*2+1];
//OBJ->texcoords[indt2*2];
//OBJ->texcoords[indt2*2+1];
//OBJ->texcoords[indt3*2];
//OBJ->texcoords[indt3*2+1];
GLfloat tx, ty;
tx = OBJ->texcoords[indt1*2];
ty = OBJ->texcoords[indt1*2+1];
vtextures[gCount][i*6+0] = tx;
vtextures[gCount][i*6+1] = ty;
tx = OBJ->texcoords[indt2*2];
ty = OBJ->texcoords[indt2*2+1];
vtextures[gCount][i*6+2] = tx;
vtextures[gCount][i*6+3] = ty;
tx = OBJ->texcoords[indt3*2];
ty = OBJ->texcoords[indt3*2+1];
vtextures[gCount][i*6+4] = tx;
vtextures[gCount][i*6+5] = ty;
}
group = group->next;
gCount++;
}
// custom normalization matrix for each loaded model
normalization_matrix.identity();
normalization_matrix = normalization_matrix * myTranslateMatrix(-OBJ->position[0], -OBJ->position[1], -OBJ->position[2]);
normalization_matrix = normalization_matrix * myScaleMatrix(1/normalizationScale, 1/normalizationScale, 1/normalizationScale);
}
void sendFile(int sockfd, char* fileName, struct sockaddr* cliAddr, socklen_t cliAddrLen, int cliWindow, int servWindow)
{
struct sockaddr_in *destAddr;
//bzero(&destAddr, sizeof(struct sockaddr));
destAddr= (struct sockaddr_in *)cliAddr;
printf("New Client address %s:%u\n",inet_ntoa(destAddr->sin_addr), ntohs(destAddr->sin_port));
printf("Sockfd = %d\n", sockfd);
fd_set rset;
int maxfdp1;
FD_ZERO(&rset);
fd_set rset_probe;
int maxfdp1_probe;
FD_ZERO(&rset_probe);
//Signals Initialization
sigset_t mySignalSet;
if(sigemptyset(&mySignalSet) < 0)
{
perror("Sigemptyset error:");
exit(1);
}
if(sigaddset(&mySignalSet, SIGALRM), 0)
{
perror("Sigaddset error");
exit(1);
}
signal(SIGALRM, sig_alarm);
//End signal initialization
//Congestion Control
struct congestion myCongestion;
myCongestion.ssthresh = myMin(cliWindow, servWindow);
myCongestion.maxWindowSize = myMin(cliWindow, servWindow);
myCongestion.cliWindowSize = cliWindow;
myCongestion.currWindowSize = 1;
myCongestion.state = MultiplicativeIncrease;
int congestionFlag = MultiplicativeIncrease;
int firstPacket = 1;
//End Congestion control
struct itimerval value, ovalue, pvalue, zeroValue;
int which = ITIMER_REAL;
int tempTime, tempSec, tempMSec;
struct iovec iovsend[2], iovrecv[2];
struct iovec *iovsendDup;
struct myList* packet = NULL;
if (rttinit == 0) {
my_rtt_init(&rttinfo); /* first time we're called */
rttinit = 1;
rtt_d_flag = 1;
}
mybool flag = 1;
mybool probe = 2;
//int fin;
FILE *fin;
fin = fopen(fileName,"r");
if(fin == NULL) {
perror("Error opening a file:");
exit(1);
}
//printf("Sizeof myheader = %d\n", sizeof(struct header));
//printf("Sizeof file buffer = %d\n", FILEBUFSIZE);
int currentTime=0;
int countDup=0; //For counting the duplicates Acks
int sequenceNumber=0; //The sequence number used while sending
int tempClientWindow = cliWindow;
int tempCounter=0;
//int sequenceACK=1; //Gives me the sequence number of the packet recently ACKed
int firstSequence = 1;
int firstSequenceSent=0; //Gives me the sequence number of first packet sent
int lastSequenceSent=0; //Gives me the sequence number of
//int lastACKRecv =0;
int receivedACK=0; //Gives me the sequence number of the packet recently AC
int expectedACK =0;
int isLast = 0;
int recvCliWindow=0;
int sendAgain = 0;
int readBytes =0;
char buf[FILEBUFSIZE];
memset(buf, 0, FILEBUFSIZE);
char recvBuf[RECVBUFSIZE];
memset(recvBuf, 0, RECVBUFSIZE);
//printf("Hi I'm here \n");
int packetSend =0;
//int tempWindow = cliWindow;
int tempWindow = 1;
int retransmitCounter = 0;
struct timeval time_count;
recvCliWindow = cliWindow;
tempCounter =0;
start_sending:
//tempCounter =0;
//packetSend++;
//tempWindow = cliWindow;
printf("Recieved Window = %d\n", recvCliWindow);
if(sigprocmask(SIG_BLOCK, &mySignalSet, NULL) < 0)
{
perror("sigprocmask block error:");
exit(1);
}
// Persistant timer code for probe packet
FD_ZERO(&rset_probe);
retransmitCounter = 0;
probe_check:
if(recvCliWindow == 0)
{
printf("---------Sending Probe Packet-------------\n");
retransmitCounter++;
if(retransmitCounter > 12)
{
printf("Client window size = 0 after trying for 12 times\n");
printf("Now will terminate the server\n");
exit(1);
}
time_count.tv_sec = 3;
time_count.tv_usec = 0;
memset((void *)&sendhdr, 0, sizeof(struct header));
memset(buf, 0, FILEBUFSIZE);
sendhdr.seq = 0;
sendhdr.isLast = probe;
iovsend[0].iov_base = (void*)&sendhdr;
iovsend[0].iov_len = sizeof(struct header);
iovsend[1].iov_base = (void*)&buf;
iovsend[1].iov_len = sizeof(buf);
sleep(5);
if(writev(sockfd, iovsend, 2) < 0)
{
perror("Writev failed:");
exit(1);
}
FD_SET(sockfd, &rset_probe);
maxfdp1_probe=sockfd+1;
if(select(maxfdp1_probe, &rset_probe, NULL, NULL, &time_count) < 0)
{
if(errno == EINTR)
goto check_window;
else
{
perror("Select Error :");
exit(1);
}
}
if(FD_ISSET(sockfd, &rset_probe))
{
//printf("Hi I'm here");
memset((void *)&recvhdr, 0, sizeof(struct header));
memset(recvBuf, 0, RECVBUFSIZE);
iovrecv[0].iov_base = (void*)&recvhdr;
iovrecv[0].iov_len = sizeof(recvhdr);
iovrecv[1].iov_base = recvBuf;
iovrecv[1].iov_len = sizeof(recvBuf);
if(readv(sockfd, iovrecv, 2) < 0)
{
perror("Error while reading the ACK:");
exit(1);
}
if(recvhdr.isLast == 3)
{
recvCliWindow = recvhdr.availWindow;
printf("Received Reply for Probe Packet : Window Size = %d\n", recvCliWindow);
}
}
else
{
goto check_window;
}
check_window:
if(recvCliWindow <= 0)
goto probe_check;
}
//Persistant timer code end
printf("Will Send packets :\n");
if(firstPacket != 1)
{
printf("Update congestion values\n");
myCongestion.maxWindowSize = recvCliWindow;
congestionValues(&myCongestion, myCongestion.state);
tempWindow = myCongestion.currWindowSize;
}
printf("Current Window Size = %d, ssthreshold = %d, state = %d\n", myCongestion.currWindowSize, myCongestion.ssthresh, myCongestion.state);
firstPacket = 0;
//currWindowSize stores the info about the packets which is to be sent
//printf("Hi before already packets\n");
if((getLength(head) >= myCongestion.currWindowSize) && recvCliWindow > 0)
{
tempWindow = myCongestion.currWindowSize;
printf("_____Packets already in Server Window_______\n");
packet = head;
my_rtt_newpack(&rttinfo);
while(tempWindow > 0)
{
printf("Sending packet with sequence number = %d\n", ((struct header *)packet->iv[0].iov_base)->seq);
iovsendDup = packet->iv;
if(writev(sockfd, iovsendDup, 2) < 0)
{
perror("Writev failed:");
exit(1);
}
lastSequenceSent = ((struct header *)packet->iv[0].iov_base)->seq;
packet = packet->next;
tempWindow--;
}
//goto wait_timer;
}
//printf("Hi after already packets\n");
if(getLength(head) > 0 && (getLength(head) < myCongestion.currWindowSize) && recvCliWindow > 0)
{
tempWindow = myCongestion.currWindowSize;
packet = head;
my_rtt_newpack(&rttinfo);
while(tempWindow > 0 && packet != NULL)
{
printf("Sending packet with sequence number = %d\n", ((struct header *)packet->iv[0].iov_base)->seq);
iovsendDup = packet->iv;
if(writev(sockfd, iovsendDup, 2) < 0)
{
perror("Writev failed:");
exit(1);
}
lastSequenceSent = ((struct header *)packet->iv[0].iov_base)->seq;
packet = packet->next;
tempWindow--;
}
}
//printf("Hi before adding to linked list\n");
printf("TempWindow = %d\n", tempWindow);
//while(tempCounter < packetSend && tempCounter < 6)
while(tempWindow > 0 && recvCliWindow > 0)
{
memset((void *)&sendhdr, 0, sizeof(struct header));
memset(buf, 0, FILEBUFSIZE);
if((readBytes = fread(buf, FILEBUFSIZE, sizeof(char), fin)) < 0)
{
perror("fread Error:");
exit(1);
}
#ifdef DEBUG1
printf("Read buffer : %s\n", buf);
#endif
sendhdr.seq = ++sequenceNumber;
if(feof(fin))
{
printf("Last packet \n");
sendhdr.isLast = flag;
isLast = 1;
}
else
{
//sendhdr.isLast = 0;
}
printf("\nSequence number : %d\n", sendhdr.seq);
iovsend[0].iov_base = (void*)&sendhdr;
iovsend[0].iov_len = sizeof(struct header);
iovsend[1].iov_base = (void*)&buf;
iovsend[1].iov_len = sizeof(buf);
head = addToList(head, iovsend);
#ifdef DEBUG1
printf("Sequence check = %d\n", ((struct header *)head->iv[0].iov_base)->seq);
printf("Data check = %s\n", (char*)head->iv[1].iov_base);
#endif
//printList(head);
my_rtt_newpack(&rttinfo);
sendhdr.ts = my_rtt_ts(&rttinfo);
if(writev(sockfd, iovsend, 2) < 0)
{
perror("Writev failed:");
exit(1);
}
//sendhdr.ts = my_rtt_ts(&rttinfo);
printf("Sending packet with sequence number = %d\n", sendhdr.seq);
lastSequenceSent = sendhdr.seq;
//tempCounter++;
tempWindow--;
if(sendhdr.isLast == myTRUE)
tempWindow=0;
}
common_all:
printf("Window contains %d to %d sequence Numbers\n", ((struct header *)head->iv[0].iov_base)->seq , lastSequenceSent);
sendAgain = 0;
if(sigprocmask(SIG_UNBLOCK, &mySignalSet, NULL) < 0)
{
perror("sigprocmask unblock error:");
exit(1);
}
wait_timer:
expectedACK = ((struct header *)head->iv[0].iov_base)->seq;
if(sendAgain == 1)
{
if((packet = getNode(head, expectedACK)) != NULL)
{
printf("Retransmission for packet with sequence number = %d after timeout\n", expectedACK);
iovsendDup = packet->iv;
if(writev(sockfd, iovsendDup, 2) < 0)
{
perror("Writev failed:");
exit(1);
}
//setitimer(ITIMER_REAL, &zeroValue,0);
//my_rtt_newpack(&rttinfo);
}
}
// Timeout for next ACK
// if(Current Time - Previous time) < RTO, goto select and wait for the remaining time
countDup =0;
//currentTime = my_rtt_ts(&rttinfo);
//tempTime = (int32_t)(rttinfo.rtt_rto - (currentTime - ((struct header *)head->iv[0].iov_base)->ts));
tempTime = my_rtt_start(&rttinfo);
//tempTime = ((struct header *)head->iv[0].iov_base)->ts;
if (tempTime >= 1000) {
tempSec = tempTime / 1000;
}
tempMSec = (tempTime % 1000)*1000;
value.it_interval.tv_sec = 0;
value.it_interval.tv_usec = 0;
value.it_value.tv_sec = tempSec;
value.it_value.tv_usec = tempMSec;
zeroValue.it_interval.tv_sec = 0;
zeroValue.it_interval.tv_usec = 0;
zeroValue.it_value.tv_sec = 0;
zeroValue.it_value.tv_usec = 0;
printf("\nTIMER Value: %d %d\n", (int)value.it_value.tv_sec,(int)value.it_value.tv_usec);
setitimer(ITIMER_REAL, &zeroValue,0);
setitimer(ITIMER_REAL, &value,0);
if (sigsetjmp(jmpbuf, 1) != 0)
{
if (my_rtt_timeout(&rttinfo) < 0) {
printf("Network conditions pretty bad.\n");
rttinit = 0; /* reinit in case we're called again */
errno = ETIMEDOUT;
exit(1);
//return(-1);
}
#ifdef DEBUG
err_msg("Timeout, retransmitting\n");
#endif
sendAgain =1;
myCongestion.ssthresh = myMax(myCongestion.ssthresh/2, 2);
myCongestion.currWindowSize = 1;
myCongestion.state = MultiplicativeIncrease;
//congestionValues(&myCongestion, MultiplicativeIncrease);
printf("Current Window Size = %d, ssthreshold = %d, state = %d\n", myCongestion.currWindowSize, myCongestion.ssthresh, myCongestion.state);
//goto start_sending;
goto wait_timer;
}
FD_ZERO(&rset);
wait_ACK:
FD_SET(sockfd, &rset);
maxfdp1=sockfd+1;
if(select(maxfdp1, &rset, NULL, NULL, NULL) < 0)
{
if(errno == EINTR)
goto wait_ACK;
else
{
perror("Select Error :");
exit(1);
}
}
if(FD_ISSET(sockfd, &rset))
{
memset((void *)&recvhdr, 0, sizeof(struct header));
iovrecv[0].iov_base = (void*)&recvhdr;
iovrecv[0].iov_len = sizeof(recvhdr);
iovrecv[1].iov_base = recvBuf;
iovrecv[1].iov_len = sizeof(recvBuf);
if(readv(sockfd, iovrecv, 2) < 0)
{
perror("Error while reading the ACK:");
exit(1);
}
//expectedACK = ((struct header *)head->iv[0].iov_base)->seq;
//printf("Expected ACK = %d\n", expectedACK);
printf("Window Size Available = %d\n", recvhdr.availWindow);
if(recvhdr.isACK ==1)
{
printf("Window Size Available = %d\n", recvhdr.availWindow);
recvCliWindow = recvhdr.availWindow;
receivedACK = recvhdr.seq;
printf("Received ACK for packet : %d\n", recvhdr.seq);
packet = getNode(head, recvhdr.seq);
if(packet != NULL)
{
if(recvhdr.ts == ((struct header*)(packet->iv[0].iov_base))->ts)
{
my_rtt_stop(&rttinfo, my_rtt_ts(&rttinfo) - ((struct header*)(head->iv[0].iov_base))->ts);
//my_rtt_stop(&rttinfo, my_rtt_ts(&rttinfo) - recvhdr.ts);
printf("RTO updated to: %d millisec\n", rttinfo.rtt_rto);
}
my_rtt_newpack(&rttinfo);
}
printf("ReceivedACK =%d ExpectedACK = %d last sequence = %d\n",receivedACK, expectedACK, lastSequenceSent);
if(receivedACK == expectedACK)
{
expectedACK++;
}
//printf("Length of the linked list = %d\n",getLength(head));
//head = deleteFromList(head, receivedACK);
head = deleteNodes(head, receivedACK);
//printf("Length of the linked list = %d\n",getLength(head));
if(isLast == 1)
{
if(receivedACK == lastSequenceSent)
{
printf("File transfer completed successfully \n");
goto close_file;
}
}
if(receivedACK == lastSequenceSent)
{
printf("Received the required sequence number before timeout\n");
setitimer(ITIMER_REAL, &zeroValue,0);
goto start_sending;
}
if(receivedACK < expectedACK)
{
if((packet = getNode(head, receivedACK)) == NULL)
{
countDup++;
}
if(countDup == 2)
{
if((packet = getNode(head, receivedACK+1)) != NULL)
{
//Fast Retransmit the packet with sequence number (receivedACK+1)
printf("Fast retransmission for packet with sequence number = %d\n", receivedACK+1);
iovsendDup = packet->iv;
if(writev(sockfd, iovsendDup, 2) < 0)
{
perror("Writev failed:");
exit(1);
}
setitimer(ITIMER_REAL, &zeroValue,0);
my_rtt_newpack(&rttinfo);
printf("Fast recovery Happened here\n");
myCongestion.ssthresh = myMax(myCongestion.ssthresh/2, 2);
myCongestion.currWindowSize = myCongestion.ssthresh;
myCongestion.state = AdditiveIncrease;
//congestionValues(&myCongestion, AdditiveIncrease);
printf("Current Window Size = %d, ssthreshold = %d, state = %d\n", myCongestion.currWindowSize, myCongestion.ssthresh, myCongestion.state);
goto wait_timer;
}
}
}
if(receivedACK > expectedACK)
{
expectedACK = receivedACK+1;
}
}
}
else
{
/*printf("Timeout occured\n");
int willRetry =0;
if(lastACKRecv == 0)
{
//Have to retransmit packet 1
willRetry = my_rtt_timeout(&rttinfo);
if(willRetry < 0)
{
printf("Network conditions pretty bad.\n");
rttinit = 0;
errno = ETIMEDOUT;
exit(1);
//return(-1);
}
packet = getNode(head, firstSequence);
if(packet != NULL)
{
((struct header*)(packet->iv[0].iov_base))->ts = my_rtt_ts(&rttinfo);
printf("Retransmission of packet with seqNo = : %d\n", firstSequence);
//writev(sockfd, packet->iv, 2);
}
}*/
goto wait_ACK;
}
goto wait_ACK;
close_file:
fclose(fin);
}
void groupRectangles(std::vector<MyRect>& rectList, int groupThreshold, float eps)
{
if( groupThreshold <= 0 || rectList.empty() )
return;
std::vector<int> labels;
int nclasses = partition(rectList, labels, eps);
std::vector<MyRect> rrects(nclasses);
std::vector<int> rweights(nclasses);
int i, j, nlabels = (int)labels.size();
for( i = 0; i < nlabels; i++ )
{
int cls = labels[i];
rrects[cls].x += rectList[i].x;
rrects[cls].y += rectList[i].y;
rrects[cls].width += rectList[i].width;
rrects[cls].height += rectList[i].height;
rweights[cls]++;
}
for( i = 0; i < nclasses; i++ )
{
MyRect r = rrects[i];
float s = 1.f/rweights[i];
rrects[i].x = myRound(r.x*s);
rrects[i].y = myRound(r.y*s);
rrects[i].width = myRound(r.width*s);
rrects[i].height = myRound(r.height*s);
}
rectList.clear();
for( i = 0; i < nclasses; i++ )
{
MyRect r1 = rrects[i];
int n1 = rweights[i];
if( n1 <= groupThreshold )
continue;
/* filter out small face rectangles inside large rectangles */
for( j = 0; j < nclasses; j++ )
{
int n2 = rweights[j];
/*********************************
* if it is the same rectangle,
* or the number of rectangles in class j is < group threshold,
* do nothing
********************************/
if( j == i || n2 <= groupThreshold )
continue;
MyRect r2 = rrects[j];
int dx = myRound( r2.width * eps );
int dy = myRound( r2.height * eps );
if( i != j &&
r1.x >= r2.x - dx &&
r1.y >= r2.y - dy &&
r1.x + r1.width <= r2.x + r2.width + dx &&
r1.y + r1.height <= r2.y + r2.height + dy &&
(n2 > myMax(3, n1) || n1 < 3) )
break;
}
if( j == nclasses )
{
rectList.push_back(r1); // insert back r1
}
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
//L2L2OpticalFlow(u1,u2,tol,lambda,ux,uy,ut)
{
double *u1 = mxGetPr(prhs[0]);
double *u2 = mxGetPr(prhs[1]);
float tol = (float)mxGetScalar(prhs[2]);
float lambda = (float)mxGetScalar(prhs[3]);
int maxIterations = (int)mxGetScalar(prhs[4]);
const size_t *sizeImage = mxGetDimensions(prhs[0]);
double *inputV = 0;
double *inputY = 0;
int typeNorm = 1;
if (nrhs > 5)
{
typeNorm = (int)mxGetScalar(prhs[5]);
}
if (nrhs > 6)
{
inputV = mxGetPr(prhs[6]);
}
if (nrhs > 7)
{
inputY = mxGetPr(prhs[7]);
}
int nPx = (int)(sizeImage[0] * sizeImage[1]);
const size_t sizeY[2] = {5*nPx,1};
// Output v1
plhs[0] = mxCreateNumericArray(2, sizeImage, mxDOUBLE_CLASS, mxREAL);
double *Outv1 = mxGetPr(plhs[0]);
// Output v2
plhs[1] = mxCreateNumericArray(2, sizeImage, mxDOUBLE_CLASS, mxREAL);
double *Outv2 = mxGetPr(plhs[1]);
// Output Y
plhs[2] = mxCreateNumericArray(2, sizeY, mxDOUBLE_CLASS, mxREAL);
double *YOut = mxGetPr(plhs[2]);
float* v1 = new float[nPx];
float* v2 = new float[nPx];
float* u = new float[nPx];
float* ut = new float[nPx];
float* sigut = new float[nPx];
float* y11 = new float[nPx];
float* y12 = new float[nPx];
float* y21 = new float[nPx];
float* y22 = new float[nPx];
float* y5 = new float[nPx];
float* Kty1 = new float[nPx];
float* Kty2 = new float[nPx];
float* Kx11 = new float[nPx];
float* Kx12 = new float[nPx];
float* Kx21 = new float[nPx];
float* Kx22 = new float[nPx];
float* Kx5 = new float[nPx];
float tau = 1.0f / sqrt(8.0f);
float sigma = tau;
float dTau = 1.0f / tau;
float dSigma = 1.0f / sigma;
//residuals
float p = 0;
float d = 0;
float err = 1.0;
float ssl = 1.0f - sigma / (sigma + lambda);
#pragma omp parallel for
for (int j = 0; j < sizeImage[1]; ++j)
{
for (int i = 0; i < sizeImage[0]; ++i)
{
int tmpIndex = index2DtoLinear(sizeImage, i, j);
//Index for gradients
u[tmpIndex] = (float)u1[tmpIndex];
ut[tmpIndex] = (float)(u2[tmpIndex] - u1[tmpIndex]);
sigut[tmpIndex] = (float)(sigma*ut[tmpIndex]);
if (nrhs > 6)
{
v1[tmpIndex] = (float)inputV[tmpIndex];
v2[tmpIndex] = (float)inputV[nPx + tmpIndex];
}
else
{
v1[tmpIndex] = 0;
v2[tmpIndex] = 0;
}
Kty1[tmpIndex] = 0;
Kty2[tmpIndex] = 0;
if (nrhs > 7)
{
y11[tmpIndex] = (float)inputY[tmpIndex];
y12[tmpIndex] = (float)inputY[nPx + tmpIndex];
y21[tmpIndex] = (float)inputY[2 * nPx + tmpIndex];
y22[tmpIndex] = (float)inputY[3 * nPx + tmpIndex];
y5[tmpIndex] = (float)inputY[4 * nPx + tmpIndex];
}
else
{
y11[tmpIndex] = 0;
y12[tmpIndex] = 0;
y21[tmpIndex] = 0;
y22[tmpIndex] = 0;
y5[tmpIndex] = 0;
}
Kx11[tmpIndex] = 0;
Kx12[tmpIndex] = 0;
Kx21[tmpIndex] = 0;
Kx22[tmpIndex] = 0;
Kx5[tmpIndex] = 0;
}
}
int iterations = 0;
while (err > tol && iterations <= maxIterations)
{
++iterations;
if (iterations % 50 == 0)
{
p = 0;
d = 0;
}
//primal step
#pragma omp parallel for reduction(+:p)
for (int j = 0; j < sizeImage[1]; ++j)
{
for (int i = 0; i < sizeImage[0]; ++i)
{
int tmpIndex = index2DtoLinear(sizeImage, i, j);
float Kty1Old = Kty1[tmpIndex];
float Kty2Old = Kty2[tmpIndex];
//transpose equals -div
Kty1[tmpIndex] = -(dxm(y11, sizeImage, i, j) + dym(y12, sizeImage, i, j) + dycT(y5, u, sizeImage, i, j));
Kty2[tmpIndex] = -(dxm(y21, sizeImage, i, j) + dym(y22, sizeImage, i, j) + dxcT(y5, u, sizeImage, i, j));
float v1Old = v1[tmpIndex];
float v2Old = v2[tmpIndex];
v1[tmpIndex] = v1Old - tau*Kty1[tmpIndex];
v2[tmpIndex] = v2Old - tau*Kty2[tmpIndex];
if (iterations % 50 == 0)
{
//residuals
p += myAbs((v1Old - v1[tmpIndex]) * dTau - Kty1Old + Kty1[tmpIndex])
+ myAbs((v2Old - v2[tmpIndex]) * dTau - Kty2Old + Kty2[tmpIndex]);
}
}
}
//dual step
#pragma omp parallel for reduction(+:d)
for (int j = 0; j < sizeImage[1]; ++j)
{
for (int i = 0; i < sizeImage[0]; ++i)
{
int tmpIndex = index2DtoLinear(sizeImage, i, j);
float Kx11Old = Kx11[tmpIndex];
float Kx12Old = Kx12[tmpIndex];
float Kx21Old = Kx21[tmpIndex];
float Kx22Old = Kx22[tmpIndex];
float Kx5Old = Kx5[tmpIndex];
Kx11[tmpIndex] = dxp(v1, sizeImage, i, j);
Kx12[tmpIndex] = dyp(v1, sizeImage, i, j);
Kx21[tmpIndex] = dxp(v2, sizeImage, i, j);
Kx22[tmpIndex] = dyp(v2, sizeImage, i, j);
Kx5[tmpIndex] = dyc(v1, u, sizeImage, i, j) + dxc(v2, u, sizeImage, i, j);
float y11Old = y11[tmpIndex];
float y12Old = y12[tmpIndex];
float y21Old = y21[tmpIndex];
float y22Old = y22[tmpIndex];
float y5Old = y5[tmpIndex];
y11[tmpIndex] = ssl * (y11[tmpIndex] + sigma*(2 * Kx11[tmpIndex] - Kx11Old));
y12[tmpIndex] = ssl * (y12[tmpIndex] + sigma*(2 * Kx12[tmpIndex] - Kx12Old));
y21[tmpIndex] = ssl * (y21[tmpIndex] + sigma*(2 * Kx21[tmpIndex] - Kx21Old));
y22[tmpIndex] = ssl * (y22[tmpIndex] + sigma*(2 * Kx22[tmpIndex] - Kx22Old));
y5[tmpIndex] = myMax(-1.0f, myMin(1.0f, y5[tmpIndex] + sigma*(2 * Kx5[tmpIndex] - Kx5Old) + sigut[tmpIndex]));
if (iterations % 50 == 0)
{
d += myAbs((y11Old - y11[tmpIndex]) * dSigma - Kx11Old + Kx11[tmpIndex]) +
myAbs((y12Old - y12[tmpIndex]) * dSigma - Kx12Old + Kx12[tmpIndex]) +
myAbs((y21Old - y21[tmpIndex]) * dSigma - Kx21Old + Kx21[tmpIndex]) +
myAbs((y22Old - y22[tmpIndex]) * dSigma - Kx22Old + Kx22[tmpIndex]) +
myAbs((y5Old - y5[tmpIndex]) * dSigma - Kx5Old + Kx5[tmpIndex]);
}
}
}
if (iterations % 50 == 0)
{
err = (d*d + p*p) / nPx;
}
if (iterations % 1000 == 0)
{
mexPrintf("Iteration %d,Residual %e\n", iterations, err);
mexEvalString("drawnow;");
}
}
//write output
#pragma omp parallel for
for (int j = 0; j < sizeImage[1]; ++j)
{
for (int i = 0; i < sizeImage[0]; ++i)
{
int tmpIndex = index2DtoLinear(sizeImage, i, j);
YOut[tmpIndex] = (double)y11[tmpIndex];
YOut[tmpIndex + nPx] = (double)y12[tmpIndex];
YOut[tmpIndex + 2 * nPx] = (double)y21[tmpIndex];
YOut[tmpIndex + 3 * nPx] = (double)y22[tmpIndex];
YOut[tmpIndex + 4 * nPx] = (double)y5[tmpIndex];
Outv1[tmpIndex] = (double) v1[tmpIndex];
Outv2[tmpIndex] = (double) v2[tmpIndex];
}
}
}
void *calcForce(void *arg) {
int myID = *((int*)arg);
double fac, fx, fy, fz;
double dx, dy, dz, sq, dist;
int t = 0, i, j,a,b;
while ( t<TimeSteps){
/* Loop over points calculating force between each pair.*/
for ( i=myID; i<BodyNum; i+=thread_num ){
a=myMin(i+(BodyNum+1)/2,BodyNum); //计算任务Wia
b=myMax(0,i-(BodyNum+1)/2); //计算任务Wbi
for ( j=i+1; j<a; j++ ) {/*Calculate force between particle i and j according to Newton's Law*/
if ( i==j ) continue;
dx = *(pBody+4*i+1) - *(pBody+4*j+1);
dy = *(pBody+4*i+2) - *(pBody+4*j+2);
dz = *(pBody+4*i+3) - *(pBody+4*j+3);
sq = dx*dx + dy*dy + dz*dz;
dist = sqrt(sq);
fac = (*(pBody+4*i)) * (*(pBody+4*j)) / ( dist * sq );
fx = fac * dx;
fy = fac * dy;
fz = fac * dz;
/*Add in force and opposite force to particle i and j */
pthread_mutex_lock(&mutex1);
*(pForce+3*i) = *(pForce+3*i) - fx;
*(pForce+3*i+1) = *(pForce+3*i+1) - fy;
*(pForce+3*i+2) = *(pForce+3*i+2) - fz;
*(pForce+3*j) = *(pForce+3*j) + fx;
*(pForce+3*j+1) = *(pForce+3*j+1) + fy;
*(pForce+3*j+2) = *(pForce+3*j+2) + fz;
pthread_mutex_unlock(&mutex1);
}
for ( j=b; j<i; j++ ){/*Calculate force between particle i and j according to Newton's Law*/
if ( i==j ) continue;
dx = *(pBody+4*i+1) - *(pBody+4*j+1);
dy = *(pBody+4*i+2) - *(pBody+4*j+2);
dz = *(pBody+4*i+3) - *(pBody+4*j+3);
sq = dx*dx + dy*dy + dz*dz;
dist = sqrt(sq);
fac = (*(pBody+4*i)) * (*(pBody+4*j)) / ( dist * sq );
fx = fac * dx;
fy = fac * dy;
fz = fac * dz;
/*Add in force and opposite force to particle i and j */
pthread_mutex_lock(&mutex1);
*(pForce+3*i) = *(pForce+3*i) - fx;
*(pForce+3*i+1) = *(pForce+3*i+1) - fy;
*(pForce+3*i+2) = *(pForce+3*i+2) - fz;
*(pForce+3*j) = *(pForce+3*j) + fx;
*(pForce+3*j+1) = *(pForce+3*j+1) + fy;
*(pForce+3*j+2) = *(pForce+3*j+2) + fz;
pthread_mutex_unlock(&mutex1);
}
}
//synchronization: to ensure that the computation of force on each body has been completed before updating its state
pthread_mutex_lock (&mutex2);
progress++;
if ( progress!=thread_num){
pthread_cond_wait( &cond, &mutex2);
}else {
pthread_cond_broadcast(&cond);
progress = 0;
}
pthread_mutex_unlock (&mutex2);
for ( i=myID; i<BodyNum; i+=thread_num ){
*(pBody+4*i+1) = *(pBody+4*i+1) + (*(pForce+3*i)) / (*(pBody+4*i));
*(pForce+3*i) = 0;
*(pBody+4*i+2) = *(pBody+4*i+2) + (*(pForce+3*i+1)) / (*(pBody+4*i));
*(pForce+3*i+1) = 0;
*(pBody+4*i+3) = *(pBody+4*i+3) + (*(pForce+3*i+2)) / (*(pBody+4*i));
*(pForce+3*i+2) = 0;
}
//synchronization: to ensure that each thread has completed the update of body states before the next step simulation
pthread_mutex_lock (&mutex2);
progress++;
if ( progress!=thread_num){
pthread_cond_wait( &cond, &mutex2);
}else {
pthread_cond_broadcast(&cond);
progress = 0;
}
pthread_mutex_unlock (&mutex2);
t++;
}
return((void*)NULL);
}
int depthTree(struct TreeNode *root)
{
if(root == NULL) return 0;
return myMax(depthTree(root->left), depthTree(root->right)) + 1;
}
/*=======================
* 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
