void* walk_thread(void*)//don't know why such prototype { Walk walk; // object to access move function PathPacket pathpackvarlocal; int phi, radius; int x_initial=1, y_initial=0, flag=0; int reflex; int direction; bool flagStop = 0; while(1) { // printf("%s\n","Entering while(1)" ); pthread_mutex_lock(&mutex_pathpacket); // printf("walk_thread locked\n"); if(pathpackvar.updated==1) { pathpackvarlocal=pathpackvar; pathpackvar.updated=0; // flagStop = 1; } pthread_mutex_unlock(&mutex_pathpacket); // printf("walk_thread unlocked\n"); // printf("%d\n",pathpackvarlocal.no_of_points ); for(int i=0;i<pathpackvarlocal.no_of_points;i++) { // printf("%f\n",pathpackvarlocal.finalpath[i].x ); // printf("%f\n",pathpackvarlocal.finalpath[i].y ); } for(int i=0;i<pathpackvarlocal.no_of_points;i++) { // printf("%s\n","Entering for loop" ); /* First packet is arc, second packet is straight line, and so on */ if (pathpackvarlocal.no_of_points==1&&(pathpackvarlocal.BACK_WALK==1 || pathpackvarlocal.NEAR_OBSTACLE)) { radius=forward_move_length(0, pathpackvarlocal.finalpath[i].x,0, pathpackvarlocal.finalpath[i].y); // printf("%f\n",radius ); // fflush(stdout); walk.move(-1*radius,0); // fflush(stdout); usleep(1000000*(radius+2.5)/(0.145*255)); usleep(1000000); continue; } // else if(pathpackvarlocal.no_of_points==1&&pathpackvarlocal.BACK_WALK==0) // { // walk.move(9999,9999); // usleep(1000000); // continue; // } // else // {;} // proceed if(i%2==0) { if(i==0) phi=arc_angle(0,pathpackvarlocal.finalpath[i].x,0,pathpackvarlocal.finalpath[i].y); else phi=arc_angle(pathpackvarlocal.finalpath[i-1].x,pathpackvarlocal.finalpath[i].x,pathpackvarlocal.finalpath[i-1].y,pathpackvarlocal.finalpath[i].y); if(i<2) reflex=vector_cross_reflex(x_initial,pathpackvarlocal.finalpath[i+1].x- pathpackvarlocal.finalpath[i].x ,y_initial,pathpackvarlocal.finalpath[i+1].y- pathpackvarlocal.finalpath[i].y); else if(i!=pathpackvarlocal.no_of_points-1) reflex=vector_cross_reflex(pathpackvarlocal.finalpath[i-1].x - pathpackvarlocal.finalpath[i-2].x , pathpackvarlocal.finalpath[i+1].x - pathpackvarlocal.finalpath[i].x ,pathpackvarlocal.finalpath[i-1].y - pathpackvarlocal.finalpath[i-2].y, pathpackvarlocal.finalpath[i+1].y - pathpackvarlocal.finalpath[i].y ); else reflex=-1; if(i==0) direction=vector_cross_clock(x_initial,pathpackvarlocal.finalpath[i].x+pathpackvarlocal.finalpath[i+1].x,y_initial,pathpackvarlocal.finalpath[i].y+pathpackvarlocal.finalpath[i+1].y); else if(i!=pathpackvarlocal.no_of_points-1) direction=vector_cross_clock(pathpackvarlocal.finalpath[i].x- pathpackvarlocal.finalpath[i-1].x,pathpackvarlocal.finalpath[i].x+pathpackvarlocal.finalpath[i+1].x- pathpackvarlocal.finalpath[i-1].x ,pathpackvarlocal.finalpath[i].y- pathpackvarlocal.finalpath[i-1].y,pathpackvarlocal.finalpath[i].y+pathpackvarlocal.finalpath[i+1].y- pathpackvarlocal.finalpath[i-1].y); else direction=-1; // ostream::flush; if(reflex==-1&&direction==-1) walk.move(0,phi); else if(reflex==1&&direction==-1) walk.move(0,360-phi) ; else if(reflex==-1&&direction==1) walk.move(0,-1*(360-phi)); else walk.move(0,-1*phi); // ostream::flush; // nanosleep(100000); // usleep(1000000); usleep(1.5*1.75 *abs(phi*80/9)*1000); usleep(500000); // ostream::flush; // arc traversal } else { radius=forward_move_length(pathpackvarlocal.finalpath[i-1].x, pathpackvarlocal.finalpath[i].x,pathpackvarlocal.finalpath[i-1].y, pathpackvarlocal.finalpath[i].y); // fflush(stdout); walk.move(radius,0); // fflush(stdout); usleep(1000000*(radius+2.5)/(0.145*255)); usleep(500000); // fflush(stdout); // straight line traversal } if(i%2==1) { pthread_mutex_lock(&mutex_pathpacket); // printf("walk_thread locked\n"); if(pathpackvar.updated==1) { // printf("%s\n","breaking out of for loop" ); pthread_mutex_unlock(&mutex_pathpacket); break; } pthread_mutex_unlock(&mutex_pathpacket); // printf("walk_thread unlocked\n"); } } // printf("%s\n","while(1) ending" ); } }
static void arc_drawing_details (GfigObject *obj, gdouble *minang, GdkPoint *center_pnt, gdouble *arcang, gdouble *radius, gboolean draw_cnts, gboolean do_scale) { DobjPoints *pnt1 = NULL; DobjPoints *pnt2 = NULL; DobjPoints *pnt3 = NULL; DobjPoints dpnts[3]; gdouble ang1, ang2, ang3; gdouble maxang; pnt1 = obj->points; if (!pnt1) return; /* Not fully drawn */ pnt2 = pnt1->next; if (!pnt2) return; /* Not fully drawn */ pnt3 = pnt2->next; if (!pnt3) return; /* Still not fully drawn */ if (do_scale) { /* Adjust pnts for scaling */ /* Warning struct copies here! and casting to double <-> int */ /* Too complex fix me - to much hacking */ gdouble xy[2]; int j; dpnts[0] = *pnt1; dpnts[1] = *pnt2; dpnts[2] = *pnt3; pnt1 = &dpnts[0]; pnt2 = &dpnts[1]; pnt3 = &dpnts[2]; for (j = 0 ; j < 3; j++) { xy[0] = dpnts[j].pnt.x; xy[1] = dpnts[j].pnt.y; if (selvals.scaletoimage) scale_to_original_xy (&xy[0], 1); else scale_to_xy (&xy[0], 1); dpnts[j].pnt.x = xy[0]; dpnts[j].pnt.y = xy[1]; } } arc_details (&pnt1->pnt, &pnt2->pnt, &pnt3->pnt, center_pnt, radius); ang1 = arc_angle (&pnt1->pnt, center_pnt); ang2 = arc_angle (&pnt2->pnt, center_pnt); ang3 = arc_angle (&pnt3->pnt, center_pnt); /* Find min/max angle */ maxang = ang1; if (ang3 > maxang) maxang = ang3; *minang = ang1; if (ang3 < *minang) *minang = ang3; if (ang2 > *minang && ang2 < maxang) *arcang = maxang - *minang; else *arcang = maxang - *minang - 360; }