void compute_state_imu(){ compute_position_acceleration(); compute_position_velocity(); compute_position(); compute_angle(); //use gyro + mag compute_angular_velocity(); // differentiate above compute_angular_acceleration(); // differentiate above }
FragmentSeparatorCanvas::FragmentSeparatorCanvas(UmlCanvas * canvas, FragmentCanvas * f, double vp) : DiagramCanvas(0, canvas, 0, 0, LIFE_LINE_HEIGHT, 0, -1), fragment(f), vpos(vp) { if (canvas->paste()) // must not be selected else the move done after if wrong canvas->unselect(this); else compute_position(); show(); }
/** * uint8_t request_rssi(unsigned char *p) - attain rssi value from the received packet * and calls the related functions to do the calculation */ float request_rssi(unsigned char *p) { char node=p[1]; switch(node) { case 'B': rssi_store.current_node=0; break; case 'C': rssi_store.current_node=1; break; case 'E': rssi_store.current_node=2; break; default: return 0.0; } int j=8; for(int i=0;i<10;i++) { rssi_store.rssi[rssi_store.current_node][i]=(p[j+1]-'0')*10+(p[j+2]-'0'); if(!(p[j]==' ')) rssi_store.rssi[rssi_store.current_node][i]=(p[j]-'0')*100; j+=4; } // if(!(rssi_store.ready[1][0]||rssi_store.ready[1][1]||rssi_store.ready[1][2])) // { // if(rtc_get_ticks()-time<60) // { // for(int i=0;i<10;i++) // { // rssi_store.pre_rssi[rssi_store.current_node][i]=rssi_store.rssi[rssi_store.current_node][i]; // } // } // time=rtc_get_ticks(); // } // else // { rssi_store.ready[1][rssi_store.current_node]=1; // } if(rssi_store.ready[1][0]&&rssi_store.ready[1][1]&&rssi_store.ready[1][2]) { if(!(rssi_store.ready[0][0]||rssi_store.ready[0][1]||rssi_store.ready[0][2])) { res_store(); } compute_position(); // sprintf(str,"%20f",rssi_store.radius); // strcpy((char*)p, p1); return rssi_store.radius; } return 0.0; }
uint16_t compute_y_position(uint16_t sourcevalue) { if(hasYAnchor) { if(sourcevalue >= slaveYAnchorMin) { if(sourcevalue <= slaveYAnchorMax) { // main (central) area return compute_position(sourcevalue, slaveYAnchorMin, slaveYAnchorMax, usbYAnchorMin, usbYAnchorMax); } else { // max area return compute_position(sourcevalue, slaveYAnchorMax, slaveYMax, usbYAnchorMax, usbYMax); } } else { // min area return compute_position(sourcevalue, slaveYMin, slaveYAnchorMin, usbYMin, usbYAnchorMin); } } else { // no anchor processing return compute_position(sourcevalue, slaveYMin, slaveYMax, usbYMin, usbYMax); } }
int SurfaceEdgeCollapse::compute_newpoint(int v1, int v2, Star &star, pair *link, int num_link, float &x_0, float &y_0, float &z_0) { // This procedure choses the strategy according to which the position of // the new point is to be computed. int okay; switch (new_point) { case 1: okay = compute_midpoint(v1, v2, star, link, num_link, x_0, y_0, z_0); break; case 2: okay = compute_position(v1, v2, star, link, num_link, x_0, y_0, z_0); break; case 3: okay = compute_endpoint(v1, v2, star, link, num_link, x_0, y_0, z_0); break; default: okay = compute_midpoint(v1, v2, star, link, num_link, x_0, y_0, z_0); break; }; return (okay); }
/*Algoritmo Principal*/ int main(int argc, char *argv[]) { char name[20];// = "_kita_1"; char archiveName[20] = "archive"; char fitputName[20] = "fitput"; char varputName[20] = "varput"; char hvName[20]= "hv"; unsigned int i, j, t; unsigned int fun, gen; clock_t startTime, endTime; double duration, clocktime; FILE *fitfile,*varfile; /*Parametros iniciales*/ strcpy(name,argv[1]); fun = atoi(argv[2]); gen = atoi(argv[3]); printf("%s %d %d \n",name,fun,gen); initialize_data(fun,gen); /*Iniciar variables*/ initialize_memory(); /* sprintf(name,"kita_1_"); sprintf(archiveName,strcat(name,"archive.out")); sprintf(fitputName, strcat(name,"fitput.out")); sprintf(varputName, strcat(name,"varput.out")); sprintf(hvName,strcat(name,"hv.out")); */ //fitfile = fopen(strcat(strcat(fitputName,name),".out"),"w"); //varfile = fopen(strcat(strcat(varputName,name),".out"),"w"); //hv = fopen(strcat(strcat(hvName,name),".out"),"w"); /* Iniciar generador de aleatorios*/ initialize_rand(); startTime = clock(); /* Iniciar contador de generaciones*/ t = 0; /* Iniciar valores de la poblacion de manera aleatoria*/ initialize_pop(); /* Calcular velocidad inicial*/ initialize_vel(); /* Evaluar las particulas de la poblacion*/ evaluate(); /* Almacenar el pBest inicial (variables and valor de aptitud) de las particulas*/ store_pbests(); /* Insertar las particulas no domindas de la poblacion en el archivo*/ insert_nondom(); //printf("\n%d",nondomCtr); /*Ciclo Principal*/ while(t <= maxgen) { //clocktime = (clock() - startTime)/(double)CLOCKS_PER_SEC; /*if(verbose > 0 && t%printevery==0 || t == maxgen) { fprintf(stdout,"Generation %d Time: %.2f sec\n",t,clocktime); fflush(stdout); }*/ /*if(t%printevery==0 || t == maxgen) { fprintf(outfile,"Generation %d Time: %.2f sec\n",t,clocktime); }*/ /* Calcular la nueva velocidad de cada particula en la pooblacion*/ //printf("\n 1"); compute_velocity(); /* Calcular la nueva posicion de cada particula en la poblacion*/ //printf("\n 2"); compute_position(); /* Mantener las particulas en la poblacion de la poblacion en el espacio de busqueda*/ //printf("\n 3"); maintain_particles(); /* Pertubar las particulas en la poblacion*/ if(t < maxgen * pMut) mutate(t); /* Evaluar las particulas en la poblacion*/ //printf("\n 4"); evaluate(); /* Insertar nuevas particulas no domindas en el archivo*/ //printf("\n 5"); update_archive(); /* Actualizar el pBest de las particulas en la poblacion*/ //printf("\n 6"); update_pbests(); /* Escribir resultados del mejor hasta ahora*/ //verbose > 0 && t%printevery==0 || t == maxgen /*if(t%printevery==0 || t == maxgen) { //fprintf(outfile, "Size of Pareto Set: %d\n", nondomCtr); fprintf(fitfile, "%d\n",t); fprintf(varfile, "%d\n",t); for(i = 0; i < nondomCtr; i++) { for(j = 0; j < maxfun; j++) fprintf(fitfile, "%f ", archiveFit[i][j]); fprintf(fitfile, "\n"); } fprintf(fitfile, "\n\n"); fflush(fitfile); for(i = 0; i < nondomCtr; i++) { for(j = 0; j < maxfun; j++) fprintf(varfile, "%f ", archiveVar[i][j]); fprintf(varfile, "\n"); } fprintf(varfile, "\n\n"); fflush(varfile); }*/ /* Incrementar contador de generaciones*/ t++; //printf("%d\n",t); } /* Escribir resultados en el archivo */ save_results(strcat(strcat(archiveName,name),".out")); endTime = clock(); duration = ( endTime - startTime ) / (double)CLOCKS_PER_SEC; fprintf(stdout, "%lf sec\n", duration); //fclose(fitfile); //fclose(varfile); free_memory(); return EXIT_SUCCESS; }
void FragmentSeparatorCanvas::update() { hide(); compute_position(); show(); canvas()->update(); }
void lzs_renderer_draw(lzs_renderer_t* self) { assert(self); LOGD("debug"); double t0 = a3d_utime(); // stretch screen to 800x480 glMatrixMode(GL_PROJECTION); glLoadIdentity(); glOrthof(0.0f, SCREEN_W, SCREEN_H, 0.0f, 0.0f, 2.0f); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); // draw camera glEnable(GL_TEXTURE_EXTERNAL_OES); glEnableClientState(GL_TEXTURE_COORD_ARRAY); glBindTexture(GL_TEXTURE_EXTERNAL_OES, self->texid); glVertexPointer(3, GL_FLOAT, 0, VERTEX); glTexCoordPointer(2, GL_FLOAT, 0, COORDS); glDrawArrays(GL_TRIANGLE_FAN, 0, 4); glDisableClientState(GL_TEXTURE_COORD_ARRAY); glDisable(GL_TEXTURE_EXTERNAL_OES); utime_update("setup", &t0); // capture buffers GLint format = TEXGZ_BGRA; GLint type = GL_UNSIGNED_BYTE; glGetIntegerv(GL_IMPLEMENTATION_COLOR_READ_FORMAT_OES, &format); glGetIntegerv(GL_IMPLEMENTATION_COLOR_READ_TYPE_OES, &type); if((format == TEXGZ_BGRA) && (type == GL_UNSIGNED_BYTE)) { LOGD("readpixels format=0x%X, type=0x%X", format, type); // TODO - check for texgz errors // process buffers texgz_tex_t* bc = self->buffer_color; texgz_tex_t* bg = self->buffer_gray; texgz_tex_t* bsx = self->buffer_sx; texgz_tex_t* bsy = self->buffer_sy; glReadPixels(self->sphero_x - RADIUS_BALL, (SCREEN_H - self->sphero_y - 1) - RADIUS_BALL, bc->width, bc->height, bc->format, bc->type, (void*) bc->pixels); #ifdef DEBUG_BUFFERS texgz_tex_export(bc, "/sdcard/laser-shark/color.texgz"); #endif utime_update("readpixels", &t0); texgz_tex_computegray(bc, bg); utime_update("computegray", &t0); texgz_tex_computeedges3x3(bg, bsx, bsy); utime_update("computeedges", &t0); // compute peak { int x; int y; int peak_x = 0; int peak_y = 0; float peak = 0.0f; float* gpixels = (float*) bg->pixels; float* xpixels = (float*) bsx->pixels; float* ypixels = (float*) bsy->pixels; for(x = 0; x < bg->width; ++x) { for(y = 0; y < bg->height; ++y) { int idx = bg->width*y + x; // compute magnitude squared float magsq = xpixels[idx]*xpixels[idx] + ypixels[idx]*ypixels[idx]; gpixels[idx] = magsq; if(magsq > peak) { peak_x = x; peak_y = y; peak = magsq; } } } LOGD("peak=%f, peak_x=%i, peak_y=%i", peak, peak_x, peak_y); utime_update("computepeak", &t0); #ifdef DEBUG_BUFFERS texgz_tex_export(bg, "/sdcard/laser-shark/peak.texgz"); #endif // move sphero center to match peak self->sphero_x += (float) peak_x - (float) bg->width / 2.0f; self->sphero_y -= (float) peak_y - (float) bg->height / 2.0f; } } else { LOGE("unsupported format=0x%X, type=0x%X", format, type); } // compute phone X, Y center compute_position(self, SCREEN_CX, SCREEN_CY, &self->phone_X, &self->phone_Y); // compute sphero X, Y compute_position(self, self->sphero_x, self->sphero_y, &self->sphero_X, &self->sphero_Y); utime_update("computeposition", &t0); // compute goal float dx = self->phone_X - self->sphero_X; float dy = self->phone_Y - self->sphero_Y; float a = fix_angle(atan2f(dx, dy) * 180.0f / M_PI); self->sphero_goal = a - self->sphero_heading_offset; // compute speed float dotp = cosf((a - self->sphero_heading) * M_PI / 180.0f); if(dotp > 0.0f) { // linearly interpolate speed based on the turning angle self->sphero_speed = SPEED_MAX*dotp + SPEED_MIN*(1.0f - dotp); } else { // go slow to turn around self->sphero_speed = SPEED_MIN; } // draw camera cross-hair { float x = SCREEN_CX; float y = SCREEN_CY; float r = RADIUS_CROSS; limit_position(r, &x, &y); lzs_renderer_crosshair(y - r, x - r, y + r, x + r, 1.0f, 0.0f, 0.0f); } // draw sphero search box { float r = RADIUS_BALL; limit_position(r, &self->sphero_x, &self->sphero_y); float x = self->sphero_x; float y = self->sphero_y; lzs_renderer_drawbox(y - r, x - r, y + r, x + r, 0.0f, 1.0f, 0.0f, 0); } lzs_renderer_step(self); // draw string a3d_texstring_printf(self->string_sphero, "sphero: head=%i, x=%0.1f, y=%0.1f, spd=%0.2f, goal=%i", (int) fix_angle(self->sphero_heading + self->sphero_heading_offset), self->sphero_X, self->sphero_Y, self->sphero_speed, (int) fix_angle(self->sphero_goal)); a3d_texstring_printf(self->string_phone, "phone: heading=%i, slope=%i, x=%0.1f, y=%0.1f", (int) fix_angle(self->phone_heading), (int) fix_angle(self->phone_slope), self->phone_X, self->phone_Y); a3d_texstring_draw(self->string_sphero, 400.0f, 16.0f, 800, 480); a3d_texstring_draw(self->string_phone, 400.0f, 16.0f + self->string_sphero->size, 800, 480); a3d_texstring_draw(self->string_fps, (float) SCREEN_W - 16.0f, (float) SCREEN_H - 16.0f, SCREEN_W, SCREEN_H); utime_update("draw", &t0); //texgz_tex_t* screen = texgz_tex_new(SCREEN_W, SCREEN_H, SCREEN_W, SCREEN_H, TEXGZ_UNSIGNED_BYTE, TEXGZ_BGRA, NULL); //glReadPixels(0, 0, screen->width, screen->height, screen->format, screen->type, (void*) screen->pixels); //texgz_tex_export(screen, "/sdcard/laser-shark/screen.texgz"); //texgz_tex_delete(&screen); A3D_GL_GETERROR(); }