/* add some randomness to texture data */
static void SpeckleTexture(void)
{
int i,j;
int speck;
float t;
for (i=2;i<30;i++)
{
for (j=2;j<30;j++)
{
speck = 1;
while (speck <= 32 && random() % 2)
{
t = (float) MIN_(255,smallTextureArray[i][j]+speck);
smallTextureArray[i][j] = (GLubyte) t;
speck+=speck;
}
speck = 1;
while (speck <= 32 && random() % 2)
{
t = (float) MAX_(0,smallTextureArray[i][j]-speck);
smallTextureArray[i][j] = (GLubyte) t;
speck+=speck;
}
}
}
}
void IOSocket::recv_n()
{
fetch();
if ( m_rBuffer.size() <= 0 )
{
return ;
}
// modified by wenziyong, temperary, not support compress, should improve performance
int copyLen = MIN_(m_rMessages.space(), m_rBuffer.size());
if(copyLen <= 0)
return;
// copy data from m_rBuffer to m_rMessages
if ( copyLen <= m_rBuffer.rd_ptr_tail()-m_rBuffer.rd_ptr() )
{
int ret = m_rMessages.put(m_rBuffer.rd_ptr(),copyLen);
assert(ret == copyLen);
m_rBuffer.rd_ptr_inc(copyLen);
}
else //two parts
{
int len1 = m_rBuffer.rd_ptr_tail()-m_rBuffer.rd_ptr();
int len2 = copyLen-len1;
int ret1 = m_rMessages.put(m_rBuffer.rd_ptr(),len1);
assert(ret1 == len1);
m_rBuffer.rd_ptr_inc(len1);
int ret2 = m_rMessages.put(m_rBuffer.rd_ptr(),len2);
assert(ret2 == len2);
m_rBuffer.rd_ptr_inc(len2);
}
//移动m_rBuffer读指什的位置
/*
NetPacket packet;
unsigned short h = *((unsigned short*)m_rBuffer.rd_ptr());
packet.header(h);
if ( packet.length() > NetPacketMaxSize || packet.length() <= 0 )
{
m_rBuffer.clear(); // 错误包，需要清理缓冲区
return;
}
if ( packet.length() > m_rBuffer.size())
{
return; // 半包返回
}
int len = m_rBuffer.pop(packet.buffer,packet.length());
if ( len < packet.length() || !packet.parse() )
{
return; // 这里如果还取到半包，直接丢弃。
}
m_rMessages.put(packet.ptr_no_head(),packet.length_no_head());
*/
}
static void
AverageLastAndFirstTextures(void)
{
for (int i = 0; i < 32; i++) {
for (int j = 0; j < 32; j++) {
int t = (smallTextureArray[i][j] + bigTextureArray[i][j][0]) / 2;
smallTextureArray[i][j] = (GLubyte)MIN_(255, t);
}
}
}
mrutils::GuiEditBox::GuiEditBox(const char * title, int cols)
: init_(false)
,winRows((gui::init(),7))
,winCols(cols > 0?MIN_(cols,COLS ) : cols < 0 ? COLS + cols : 3*COLS /4)
,win((void*)newwin(winRows,winCols,(LINES-winRows)/2,(COLS-winCols)/2))
,frozen(false)
,title(title)
,termLine( (LINES-winRows)/2 + 4, (COLS-winCols)/2 + 2, 1, winCols - 4 )
{
termLine.setPrompt("");
}
static void MakeSmallTexture(void)
{
static int firstTime = 1;
int i,j;
float r,t;
if (firstTime)
{
firstTime = 0;
for (i=0;i<32;i++)
{
for (j=0;j<32;j++)
{
r = (float) sqrt((i-15.5)*(i-15.5)+(j-15.5)*(j-15.5));
if (r > 15.0f)
{
smallTextureArray[i][j] = 0;
}
else
{
t = 255.0f * (float) cos(r*M_PI/31.0);
smallTextureArray[i][j] = (GLubyte) t;
}
}
}
}
else
{
for (i=0;i<32;i++)
{
for (j=0;j<32;j++)
{
r = (float) sqrt((i-15.5)*(i-15.5)+(j-15.5)*(j-15.5));
if (r > 15.0f)
{
t = 0.0f;
}
else
{
t = 255.0f * (float) cos(r*M_PI/31.0);
}
smallTextureArray[i][j] = (GLubyte) MIN_(255,(t+smallTextureArray[i][j]+smallTextureArray[i][j])/3);
}
}
}
SpeckleTexture();
SmoothTexture();
SmoothTexture();
}
static void flipBytes(unsigned char *buf, int size)
{
unsigned char revBuf[64];
int bytesLeft = size;
int i;
while(bytesLeft) {
int toFlip = MIN_( sizeof(revBuf), bytesLeft );
for(i=0; i<toFlip; ++i)
revBuf[i] = buf[toFlip - (i+1)];
memcpy( buf, revBuf, toFlip );
bytesLeft -= toFlip;
buf += toFlip;
}
memset(revBuf, 0, sizeof(revBuf));
}
char *slice_md5(const char *a_path, unsigned int offset, unsigned int len) {
char *result = NULL;
md5_state_t state;
md5_byte_t digest[16], content[BUFF];
FILE *f = NULL;
int f_res = 0;
int i = 0, j = 0;
/* PRE: assert(valid_path_name(a_path)); */
/* PRE: assert(path_is_file(a_path)); */
/* PRE: assert(offset <= path_size(a_path)); */
/* PRE: assert(len < path_size(a_path) - offset); */
//printf("md5_utils.c: slice_md5: a_path -> %s\n", a_path);
f = fopen(a_path, "r");
assert(f != NULL);
f_res = fseek(f, offset, SEEK_SET);
assert(f_res == 0);
md5_init(&state);
i = len;
do {
j = MIN_(i, BUFF);
//printf("md5_utils.c: slice_md5: i -> %i, j -> %i\n", i, j);
assert(!feof(f));
f_res = fread(&content, sizeof(md5_byte_t), j, f);
assert(f_res == j);
md5_append(&state, (const md5_byte_t *)content, j);
i -= j;
} while (i > 0);
md5_finish(&state, digest);
fclose(f);
result = to_hexa(digest, 16);
//printf("md5_utils.c: slice_md5: result = %s\n", result);
/* POST: */assert(result != NULL);
/* POST: */assert(strlen(result) == 32);
return result;
}
static void parse_membername( struct qqclient* qq, qqpacket* p, qqqun* q )
{
bytebuffer *buf = p->buf;
uint pos;
pos = get_int( buf );
get_int( buf ); //00000000
while( buf->pos < buf->len ){
uint number = get_int( buf );
qunmember* m = qun_member_get( qq, q, number, 0 );
if( !m ){
DBG("m==NULL");
break;
}
uchar name_len = get_byte( buf );
name_len = MIN_( NICKNAME_LEN-1, name_len );
get_data( buf, (uchar*)m->nickname, name_len );
m->nickname[name_len] = 0;
}
}
// compute gradient magnitude and orientation at each location (uses sse)
void CPedestrianDetection::gradMag(float *I, float *M, float *O, int h, int w, int d, bool full) {
int x, y, y1, c, h4, s; float *Gx, *Gy, *M2; __m128 *_Gx, *_Gy, *_M2, _m;
float *acost = acosTable(), acMult=10000.0f;
// allocate memory for storing one column of output (padded so h4%4==0)
h4=(h%4==0) ? h : h-(h%4)+4; s=d*h4*sizeof(float);
M2=(float*) alMalloc(s,16); _M2=(__m128*) M2;
Gx=(float*) alMalloc(s,16); _Gx=(__m128*) Gx;
Gy=(float*) alMalloc(s,16); _Gy=(__m128*) Gy;
// compute gradient magnitude and orientation for each column
for( x=0; x<w; x++ ) {
// compute gradients (Gx, Gy) with maximum squared magnitude (M2)
for(c=0; c<d; c++) {
grad1( I+x*h+c*w*h, Gx+c*h4, Gy+c*h4, h, w, x );
for( y=0; y<h4/4; y++ ) {
y1=h4/4*c+y;
_M2[y1]=ADD(MUL(_Gx[y1],_Gx[y1]),MUL(_Gy[y1],_Gy[y1]));
if( c==0 ) continue; _m = CMPGT( _M2[y1], _M2[y] );
_M2[y] = OR( AND(_m,_M2[y1]), ANDNOT(_m,_M2[y]) );
_Gx[y] = OR( AND(_m,_Gx[y1]), ANDNOT(_m,_Gx[y]) );
_Gy[y] = OR( AND(_m,_Gy[y1]), ANDNOT(_m,_Gy[y]) );
}
}
// compute gradient mangitude (M) and normalize Gx
for( y=0; y<h4/4; y++ ) {
_m = MIN_( RCPSQRT(_M2[y]), SET(1e10f) );
_M2[y] = RCP(_m);
if(O) _Gx[y] = MUL( MUL(_Gx[y],_m), SET(acMult) );
if(O) _Gx[y] = XOR( _Gx[y], AND(_Gy[y], SET(-0.f)) );
};
memcpy( M+x*h, M2, h*sizeof(float) );
// compute and store gradient orientation (O) via table lookup
if( O!=0 ) for( y=0; y<h; y++ ) O[x*h+y] = acost[(int)Gx[y]];
if( O!=0 && full ) {
y1=((~size_t(O+x*h)+1)&15)/4; y=0;
for( ; y<y1; y++ ) O[y+x*h]+=(Gy[y]<0)*PI;
for( ; y<h-4; y+=4 ) STRu( O[y+x*h],
ADD( LDu(O[y+x*h]), AND(CMPLT(LDu(Gy[y]),SET(0.f)),SET(PI)) ) );
for( ; y<h; y++ ) O[y+x*h]+=(Gy[y]<0)*PI;
}
}
alFree(Gx); alFree(Gy); alFree(M2);
}
/* Use the parameters of this keypoint to sample the gradient images
at a set of locations within a circular region around the keypoint.
The (scale,row,col) values are relative to current octave sampling.
The resulting vector is stored in the key.
*/
void MakeKeypointSample(KKeypoint key, Image grad, Image ori, float scale,
float row, float col)
{
int i, intval, changed = FALSE;
float vec[VecLength];
/* Produce sample vector. */
KeySampleVec(vec, key, grad, ori, scale, row, col);
/* Normalize vector. This should provide illumination invariance
for planar lambertian surfaces (except for saturation effects).
Normalization also improves nearest-neighbor metric by
increasing relative distance for vectors with few features.
*/
NormalizeVec(vec, VecLength);
/* Now that normalization has been done, threshold elements of
index vector to decrease emphasis on large gradient magnitudes.
Admittedly, the large magnitude values will have affected the
normalization, and therefore the threshold, so this is of
limited value.
*/
for (i = 0; i < VecLength; i++)
if (vec[i] > MaxIndexVal) {
vec[i] = MaxIndexVal;
changed = TRUE;
}
if (changed)
NormalizeVec(vec, VecLength);
/* Convert float vector to integer. Assume largest value in normalized
vector is likely to be less than 0.5.
*/
key->ivec = (unsigned char*)
MallocPool(VecLength * sizeof(unsigned char), KEY_POOL);
for (i = 0; i < VecLength; i++) {
intval = (int) (512.0 * vec[i]);
assert(intval >= 0);
key->ivec[i] = (unsigned char) MIN_(255, intval);
}
}
static void parse_memberinfo( struct qqclient* qq, qqpacket* p, qqqun* q )
{
bytebuffer *buf = p->buf;
while( buf->pos < buf->len ){
uint number = get_int( buf );
qunmember* m = qun_member_get( qq, q, number, 0 );
if( !m ){
DBG("m==NULL number: %d", number);
break;
}
m->face = get_word( buf );
m->age = get_byte( buf );
m->sex = get_byte( buf );
uchar name_len = get_byte( buf );
name_len = MIN_( NICKNAME_LEN-1, name_len );
get_data( buf, (uchar*)m->nickname, name_len );
m->nickname[name_len] = 0;
//TX技术改革不彻底，还保留使用GB码 2009-1-25 11:02
gb_to_utf8( m->nickname, m->nickname, NICKNAME_LEN-1 );
get_word( buf ); //00 00
m->qqshow = get_byte( buf );
m->flag = get_byte( buf );
}
}
static void parse_quninfo( struct qqclient* qq, qqpacket* p, qqqun* q )
{
uint last_number;
uchar more, status;
bytebuffer *buf = p->buf;
q->ext_number = get_int( buf );
get_word( buf ); //00 00
get_byte( buf ); //00
status = get_byte( buf ); //03 or 02
if( status == 3 ){
q->type = get_byte( buf );
get_int( buf ); //unknown
// get_int( buf ); //(???)unknown in 1205
q->owner = get_int( buf );
q->auth_type = get_byte( buf );
buf->pos += 6;
q->category = get_int( buf );
q->max_member = get_word( buf );
buf->pos += 9;
//name
uchar len = get_byte( buf );
len = MIN_( NICKNAME_LEN-1, len );
get_data( buf, (uchar*)q->name, len );
q->name[len] = 0;
// DBG("qun: %s", q->name );
get_byte( buf );
get_byte( buf ); //separator
//ann
len = get_byte( buf );
get_data( buf, (uchar*)q->ann, len );
q->ann[len] = 0;
//intro
len = get_byte( buf );
get_data( buf, (uchar*)q->intro, len );
q->intro[len] = 0;
//token data
get_token( buf, &q->token_cmd );
}
last_number = get_int( buf ); //member last came in
more = get_byte( buf ); //more member data
while( buf->pos < buf->len ){
uint n = get_int( buf );
uchar org = get_byte( buf );
uchar role = get_byte( buf );
qunmember* m = qun_member_get( qq, q, n, 1 );
if( m==NULL ){
DBG("m==NULL");
break;
}
m->org = org;
m->role = role;
}
if( more ){
prot_qun_get_info( qq, q->number, last_number );
}else{
qun_update_memberinfo( qq, q );
qun_set_members_off( qq, q );
qun_update_online( qq, q );
}
}
//================================================================
int process(jack_nframes_t nframes, void *arg)
{
//fprintf(stderr,".");
//return 0;
//if shutting down fill buffers with 0 and return
if(shutdown_in_progress==1)
{
int i;
for(i=0; i < output_port_count; i++)
{
sample_t *o1;
o1=(sample_t*)jack_port_get_buffer(ioPortArray[i], nframes);
//memset(o1, 0, bytes_per_sample*nframes);
//always 4 bytes, 32 bit float
memset(o1, 0, 4*nframes);
}
return 0;
}
if(process_enabled==1)
{
//if no data for this cycle(all channels)
//is available(!), fill buffers with 0 or re-use old buffers and return
if(jack_ringbuffer_read_space(rb) < port_count * bytes_per_sample*nframes)
{
int i;
for(i=0; i < output_port_count; i++)
{
if(shutdown_in_progress==1 || process_enabled!=1)
{
return 0;
}
if(zero_on_underflow==1)
{
sample_t *o1;
o1=(sample_t*)jack_port_get_buffer(ioPortArray[i], nframes);
//memset(o1, 0, bytes_per_sample*nframes);
//always 4 bytes, 32 bit float
memset(o1, 0, 4*nframes);
}
print_info();
}
multi_channel_drop_counter++;
if(rebuffer_on_underflow==1)
{
pre_buffer_counter=0;
process_enabled=0;
}
//reset avg calculation
time_interval_avg=0;
msg_received_counter=0;
fscs_avg_counter=0;
return 0;
}//end not enough data available in ringbuffer
process_cycle_counter++;
if(process_cycle_counter>receive_max-1 && test_mode==1)
{
last_test_cycle=1;
}
//init to 0. increment before use
fscs_avg_counter++;
frames_since_cycle_start_sum+=frames_since_cycle_start;
frames_since_cycle_start_avg=frames_since_cycle_start_sum/fscs_avg_counter;
//check and reset after use
if(fscs_avg_calc_interval>=fscs_avg_counter)
{
fscs_avg_counter=0;
frames_since_cycle_start_sum=0;
}
//if sender sends more channels than we have output channels, ignore them
int i;
for(i=0; i<port_count; i++)
{
if(shutdown_in_progress==1 || process_enabled!=1)
{
return 0;
}
sample_t *o1;
o1=(sample_t*)jack_port_get_buffer(ioPortArray[i], nframes);
int16_t *o1_16;
if(bytes_per_sample==2)
{
o1_16=malloc(bytes_per_sample*nframes);
}
//32 bit float
if(bytes_per_sample==4)
{
jack_ringbuffer_read(rb, (char*)o1, bytes_per_sample*nframes);
}
//16 bit pcm
else
{
jack_ringbuffer_read(rb, (char*)o1_16, bytes_per_sample*nframes);
int x;
for(x=0;x<nframes;x++)
{
o1[x]=(float)MIN_(MAX_((float)o1_16[x]/32760,-1.0f),1.0f);
}
free(o1_16);
}
/*
fprintf(stderr,"\rreceiving from %s:%s",
sender_host,sender_port
);
*/
print_info();
}//end for i < port_count
//requested via /buffer, for test purposes (make buffer "tight")
if(requested_drop_count>0)
{
uint64_t drop_bytes_count=requested_drop_count
*port_count*period_size*bytes_per_sample;
jack_ringbuffer_read_advance(rb,drop_bytes_count);
requested_drop_count=0;
multi_channel_drop_counter=0;
}
}//end if process_enabled==1
else //if process_enabled==0
{
int i;
for(i=0; i<port_count; i++)
{
if(shutdown_in_progress==1 || process_enabled!=1)
{
return 0;
}
sample_t *o1;
o1=(sample_t*)jack_port_get_buffer(ioPortArray[i], nframes);
//this is per channel, not per cycle. *port_count
if(relaxed_display_counter>=update_display_every_nth_cycle*port_count
|| last_test_cycle==1
)
{
//only for init
if((int)message_number<=0 && starting_transmission==0)
{
if(shutup==0 && quiet==0)
{
fprintf(stderr,"\rwaiting for audio input data...");
}
io_simple("/wait_for_input");
}
else
{
if(shutup==0 && quiet==0)
{
fprintf(stderr,"\r# %" PRId64 " buffering... mc periods to go: %" PRId64 "%s",
message_number,
pre_buffer_size-pre_buffer_counter,
"\033[0J"
);
}
if(io_())
{
lo_message msgio=lo_message_new();
lo_message_add_int64(msgio,message_number);
lo_message_add_int64(msgio,pre_buffer_size-pre_buffer_counter);
lo_send_message(loio, "/buffering", msgio);
lo_message_free(msgio);
}
}
fflush(stderr);
relaxed_display_counter=0;
}
relaxed_display_counter++;
//set output buffer silent
//memset(o1, 0, port_count*bytes_per_sample*nframes);
//always 4 bytes, 32 bit float
memset(o1, 0, port_count*4*nframes);
}//end for i < port_count
}//end process_enabled==0
//tasks independent of process_enabled 0/1
//if sender sends less channels than we have output channels, wee need to fill them with 0
if(input_port_count < output_port_count)
{
int i;
for(i=0;i < (output_port_count-input_port_count);i++)
{
//sample_t *o1;
//o1=(sample_t*)
/////?
jack_port_get_buffer(ioPortArray[input_port_count+i], nframes);
}
}
if(last_test_cycle==1)
{
if(shutup==0)
{
fprintf(stderr,"\ntest finished after %" PRId64 " process cycles\n",process_cycle_counter);
fprintf(stderr,"(waiting and buffering cycles not included)\n");
}
io_simple_long("/test_finished",process_cycle_counter);
shutdown_in_progress=1;
}
//simulate long cycle process duration
//usleep(1000);
frames_since_cycle_start=jack_frames_since_cycle_start(client);
return 0;
} //end process()
void IOSocket::send_n()
{
// modified by wenziyong,对于客户端发送给服务器的包不进行压缩，直接把
int needSendLen = m_sMessages.size();
if(needSendLen > 0)
{
if ( m_sBuffer.space()<needSendLen)
{
flush();
}
// 计算能发多少数据
int willSendLen = MIN_(m_sBuffer.space(), needSendLen);
if(willSendLen > 0 )
{
// 把数据拷贝发送buff
if ( willSendLen <= m_sMessages.rd_ptr_tail()-m_sMessages.rd_ptr() )
{
int ret = m_sBuffer.put(m_sMessages.rd_ptr(),willSendLen);
assert(ret == willSendLen);
m_sMessages.rd_ptr_inc(willSendLen);
}
else //two parts
{
int len1 = m_sMessages.rd_ptr_tail()-m_sMessages.rd_ptr();
int len2 = willSendLen-len1;
int ret1 = m_sBuffer.put(m_sMessages.rd_ptr(),len1);
assert(ret1 == len1);
m_sMessages.rd_ptr_inc(len1);
int ret2 = m_sBuffer.put(m_sMessages.ptr_head(),len2);
assert(ret2 == len2);
m_sMessages.rd_ptr_inc(len2);
}
flush();
}
}
/*
NetPacket packet;
// 将消息队列的数据封包
int len = MIN(packet.capacity(),m_sMessages.size());//maybe compress half msg
if ( m_sMessages.rd_ptr()+len <= m_sMessages.ptr_tail() )
{
len = packet.copy(m_sMessages.rd_ptr(),len);
}
else // split two seg
{
int len0 = m_sMessages.ptr_tail()-m_sMessages.rd_ptr();
int len1 = len-len0;
len = packet.copy(m_sMessages.rd_ptr(),len0);
len +=packet.copy(m_sMessages.ptr_head(),len1);
}
if ( len > 0 && packet.combine() )
{
m_sMessages.rd_ptr(len);
if ( m_sBuffer.space()<packet.length())
{
flush();
}
// 将封好的网络包放入发送缓冲区，如果发送缓冲区放不下，抛异常
int ret = m_sBuffer.put(packet.buffer,packet.length());
if ( ret < packet.length() )
{
//error(2);
return ;
}
flush();
}
*/
}
/* Assign an orientation to this keypoint. This is done by creating a
Gaussian weighted histogram of the gradient directions in the
region. The histogram is smoothed and the largest peak selected.
The results are in the range of -PII to PII.
*/
KKeypoint AssignOriHist(Image grad, Image ori, float octSize, float octScale,
float octRow, float octCol, KKeypoint keys)
{
int i, r, c, row, col, rows, cols, radius, bin, prev, next;
float hist[OriBins], distsq, gval, weight, angle, sigma, interp,
maxval = 0.0;
row = (int) (octRow+0.5);
col = (int) (octCol+0.5);
rows = grad->rows;
cols = grad->cols;
for (i = 0; i < OriBins; i++)
hist[i] = 0.0;
/* Look at pixels within 3 sigma around the point and sum their
Gaussian weighted gradient magnitudes into the histogram.
*/
sigma = OriSigma * octScale;
radius = (int) (sigma * 3.0);
for (r = row - radius; r <= row + radius; r++)
for (c = col - radius; c <= col + radius; c++)
/* Do not use last row or column, which are not valid. */
if (r >= 0 && c >= 0 && r < rows - 2 && c < cols - 2) {
gval = grad->pixels[r][c];
distsq = (r - octRow) * (r - octRow) + (c - octCol) * (c - octCol);
if (gval > 0.0 && distsq < radius * radius + 0.5) {
weight = exp(- distsq / (2.0 * sigma * sigma));
/* Ori is in range of -PII to PII. */
angle = ori->pixels[r][c];
bin = (int) (OriBins * (angle + PII + 0.001) / (2.0 * PII));
assert(bin >= 0 && bin <= OriBins);
bin = MIN_(bin, OriBins - 1);
hist[bin] += weight * gval;
}
}
/* Apply circular smoothing 6 times for accurate Gaussian approximation. */
for (i = 0; i < 6; i++)
SmoothHistogram(hist, OriBins);
/* Find maximum value in histogram. */
for (i = 0; i < OriBins; i++)
if (hist[i] > maxval)
maxval = hist[i];
/* Look for each local peak in histogram. If value is within
OriHistThresh of maximum value, then generate a keypoint.
*/
for (i = 0; i < OriBins; i++) {
prev = (i == 0 ? OriBins - 1 : i - 1);
next = (i == OriBins - 1 ? 0 : i + 1);
if (hist[i] > hist[prev] && hist[i] > hist[next] &&
hist[i] >= OriHistThresh * maxval) {
/* Use parabolic fit to interpolate peak location from 3 samples.
Set angle in range -PII to PII.
*/
interp = InterpPeak(hist[prev], hist[i], hist[next]);
angle = 2.0 * PII * (i + 0.5 + interp) / OriBins - PII;
assert(angle >= -PII && angle <= PII);
/* Create a keypoint with this orientation. */
keys = MakeKeypoint(grad, ori, octSize, octScale, octRow, octCol,
angle, keys);
}
}
return keys;
}
