static int isAbsOutOfRangeV( vptr_half v_src_r, vptr_half v_src_i, vptr_half v_temp, int n )
{
//used for inverse only
vbx_set_vl(n);
vbx(SVH, VABSDIFF, v_temp, 0, v_src_r ); // get abs value of real
vbx(SVH, VSUB, v_temp, 16383, v_temp ); // if (16383 - v_src) < 0, needs scaling
vbx_acc(SVH, VCMV_LTZ, v_temp, 1, v_temp ); // accum # of neg values to see if scaling required
vbx_sync();
if( v_temp[0] ){
return 1;
}
vbx(SVH, VABSDIFF, v_temp, 0, v_src_i ); // get abs value of imag
vbx(SVH, VSUB, v_temp, 16383, v_temp ); // if (16383 - v_src) < 0, needs scaling
vbx_acc(SVH, VCMV_LTZ, v_temp, 1, v_temp ); // accum # of neg values to see if scaling required
vbx_sync();
if( v_temp[0] ){
return 1;
}
return 0;
}
int vector_motest(pixel *input_buffer, luma_type **last_luma, int *motest_x, int *motest_y, int start_x, int start_y, int reset, const int image_width, const int image_height, const int image_pitch)
{
int y, x, starty, startx;
unsigned int sad, sad_min, y_min, x_min;
vbx_uhalf_t *v_search_luma, *v_last_luma;
vbx_uhalf_t *v_row_temp;
vbx_uword_t *v_row;
vbx_uword_t *v_sad;
pixel color;
if(*last_luma == NULL || reset){
init_vector_motest(input_buffer, last_luma, motest_x, motest_y, start_x, start_y, image_pitch);
}
v_search_luma = vbx_sp_malloc( MOTEST_BUFFER_SIZE * sizeof(vbx_uhalf_t) );
v_last_luma = vbx_sp_malloc( MOTEST_BLOCK_SIZE * sizeof(vbx_uhalf_t) );
v_row_temp = vbx_sp_malloc( MOTEST_BUFFER_WIDTH * sizeof(vbx_uhalf_t) );
v_row = vbx_sp_malloc( MOTEST_BUFFER_WIDTH * sizeof(vbx_uword_t) );
v_sad = vbx_sp_malloc( MOTEST_SEARCH_SIZE * sizeof(vbx_uword_t) );
if(v_sad == NULL){
printf("Not enough scratchpad for motest\n");
while(1);
}
startx = *motest_x-(MOTEST_SEARCH_WIDTH/2);
starty = *motest_y-(MOTEST_SEARCH_HEIGHT/2);
if(startx < 0){
startx = 0;
}
if(startx > image_width-MOTEST_BUFFER_WIDTH){
startx = image_width-MOTEST_BUFFER_WIDTH;
}
if(starty < 0){
starty = 0;
}
if(starty > image_height-MOTEST_BUFFER_HEIGHT){
starty = image_height-MOTEST_BUFFER_HEIGHT;
}
vector_rectangle_to_luma(input_buffer, v_search_luma, v_row_temp, v_row, startx, starty, MOTEST_BUFFER_WIDTH, MOTEST_BUFFER_HEIGHT, image_pitch);
vbx_dma_to_vector(v_last_luma, *last_luma, MOTEST_BLOCK_SIZE*sizeof(vbx_uhalf_t));
//Vector compute sad here
vbx_set_2D(MOTEST_BLOCK_HEIGHT, sizeof(vbx_uword_t), MOTEST_BUFFER_WIDTH*sizeof(vbx_uhalf_t), MOTEST_BLOCK_WIDTH*sizeof(vbx_uhalf_t));
for(y = 0; y < MOTEST_SEARCH_HEIGHT; y++){
for(x = 0; x < MOTEST_SEARCH_WIDTH; x++){
vbx_set_vl(MOTEST_BLOCK_WIDTH);
vbx_acc_2D(VVHWU, VABSDIFF, v_row, v_search_luma+(y*MOTEST_BUFFER_WIDTH)+x, v_last_luma);
vbx_set_vl(MOTEST_BLOCK_HEIGHT/2);
vbx_acc(VVWU, VADD, v_sad+(y*MOTEST_SEARCH_WIDTH)+x, v_row, v_row+MOTEST_BLOCK_HEIGHT/2);
}
#if TOUCHSCREEN
#ifdef TOUCH_INTERRUPTS_VBX
if (touchscreen_get_pen(pTouch)) {
vbx_sp_free();
return -1;
}
#endif
#endif
}
vbx_sync();
sad_min = INT_MAX;
y_min = *motest_y;
x_min = *motest_x;
for(y = 0; y < MOTEST_SEARCH_HEIGHT; y++){
for(x = 0; x < MOTEST_SEARCH_WIDTH; x++){
sad = v_sad[y*MOTEST_SEARCH_WIDTH+x];
if(sad < sad_min){
sad_min = sad;
x_min = x+startx;
y_min = y+starty;
} else if(sad == sad_min) {
if( (abs( x - MOTEST_SEARCH_WIDTH/2) + abs( y - MOTEST_SEARCH_HEIGHT/2)) <
(abs((x_min-startx) - MOTEST_SEARCH_WIDTH/2) + abs((y_min-starty) - MOTEST_SEARCH_HEIGHT/2))) {
x_min = x+startx;
y_min = y+starty;
}
}
}
}
color.r = 0;
color.g = 255;
color.b = 0;
color.a = 0;
scalar_draw_line(*motest_x+(MOTEST_BLOCK_WIDTH/2), *motest_y+(MOTEST_BLOCK_HEIGHT/2), x_min+(MOTEST_BLOCK_WIDTH/2), y_min+(MOTEST_BLOCK_HEIGHT/2), color, input_buffer, image_pitch);
*motest_y = y_min;
*motest_x = x_min;
vbx_set_vl(MOTEST_BLOCK_WIDTH);
for(y = 0; y < MOTEST_BLOCK_HEIGHT; y++){
vbx(VVHU, VMOV, v_last_luma+(y*MOTEST_BLOCK_WIDTH), v_search_luma+((y+y_min-starty)*MOTEST_BUFFER_WIDTH)+(x_min-startx), 0);
}
vbx_dma_to_host(*last_luma, v_last_luma, MOTEST_BLOCK_SIZE*sizeof(luma_type));
draw_motest(input_buffer, *motest_x, *motest_y, image_pitch);
//simple hack to draw thicker
draw_motest(input_buffer, *motest_x+1, *motest_y+1, image_pitch);
vbx_sp_free();
return 0;
}
