void turn_at_magnetic_angle(double angle) { double turn_angle = 0; mag_info current_mg_i = MMC212xM_GetMagneticFieldInfo(&twid); double current_angle = compute_angle(current_mg_i.y, current_mg_i.x); turn_angle = compute_turn_angle(angle, current_angle); PWM_Set(0, 15); if (turn_angle > 0) { Kierunek(RIGHT_ENGINES, REVERSE_GEAR); Kierunek(LEFT_ENGINES, FORWARD_GEAR); } else { Kierunek(RIGHT_ENGINES, FORWARD_GEAR); Kierunek(LEFT_ENGINES, REVERSE_GEAR); } do { current_mg_i = MMC212xM_GetMagneticFieldInfo(&twid); current_angle = compute_angle(current_mg_i.y, current_mg_i.x); turn_angle = compute_turn_angle(angle, current_angle); waitms(5); } while (ABS(turn_angle) > 1); Kierunek(RIGHT_ENGINES, STOP_GEAR); Kierunek(LEFT_ENGINES, STOP_GEAR); }
/* Compute the angle for each step. This will be needed for the leg rotations! */ void glRouteWithFeet::compute_angles( ) { for (int i=0; i<m_left_steps.size(); i++) compute_angle( m_left_steps[i] ); for (int i=0; i<m_right_steps.size(); i++) compute_angle( m_right_steps[i] ); }
void elastic_poly(int x1, int y1, int x2, int y2, int numsides) { register float angle; register int nx, ny, i; double dx, dy; double init_angle, mag; int ox, oy; dx = x2 - x1; dy = y2 - y1; mag = sqrt(dx * dx + dy * dy); init_angle = compute_angle(dx, dy); ox = x2; oy = y2; /* now append numsides points */ for (i = 1; i < numsides; i++) { angle = (float)(init_angle - M_2PI * (double) i / (double) numsides); if (angle < 0) angle += M_2PI; nx = x1 + round(mag * cos((double) angle)); ny = y1 + round(mag * sin((double) angle)); pw_vector(canvas_win, nx, ny, ox, oy, INV_PAINT, 1, RUBBER_LINE, 0.0, DEFAULT); ox = nx; oy = ny; } pw_vector(canvas_win, ox, oy, x2, y2, INV_PAINT, 1, RUBBER_LINE, 0.0, DEFAULT); }
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 }
int compute_direction(F_pos p1, F_pos p2, F_pos p3) { double diff, dx, dy, alpha, theta; dx = p2.x - p1.x; dy = p1.y - p2.y; /* because origin of the screen is on the * upper left corner */ alpha = compute_angle(dx, dy); dx = p3.x - p2.x; dy = p2.y - p3.y; theta = compute_angle(dx, dy); diff = theta - alpha; if ((0 < diff && diff < M_PI) || diff < -M_PI) { return (1); /* counterclockwise */ } return (0); /* clockwise */ }
void CodDirsCanvas::update_pos(const QPoint & link_start, const QPoint & link_end) { hide(); angle = compute_angle(link_end.x() - link_start.x(), link_end.y() - link_start.y()); QRect r = rect(); r.moveCenter((link_start + link_end) / 2); moveBy(r.x(), r.y()); if (label != 0) update_label_pos(label, TRUE); if (backward_label != 0) update_label_pos(backward_label, FALSE); show(); canvas()->update(); }
void draw_fiend_beam(BITMAP *dest, BITMAP *img, int x1, int y1,int x2, int y2) { int i; V3D_f point[4]; V3D_f temp_point[2]; VERTEX the_point[4]; float angle; angle = compute_angle(x2,y2,x1,y1); // set points of a point... temp_point[0].y = 0; temp_point[0].x = -img->w/2; temp_point[1].y = 0; temp_point[1].x = img->w/2; //rotate the points for(i=0;i<2;i++) { point[i].x = (temp_point[i].x*COS(angle)-temp_point[i].y*SIN(angle)); point[i].y = (temp_point[i].y*COS(angle)+temp_point[i].x*SIN(angle)); } //give the other point thses cords... point[2].x = point[0].x; point[2].y = point[0].y; point[3].x = point[1].x; point[3].y = point[1].y; //transform to world space.. point[0].x += x1; point[0].y += y1; point[1].x += x1; point[1].y += y1; point[2].x += x2; point[2].y += y2; point[3].x += x2; point[3].y += y2; //set the u,v cords (texture cords) for(i=0;i<4;i++) point[i].c = makecol(255,0,0); point[0].u = 0; point[0].v = 0; point[1].u = img->w; point[1].v = 0; point[2].u = 0; point[2].v = distance(point[0].x,point[0].y,point[2].x,point[2].y ); point[3].u = img->w; point[3].v = distance(point[1].x,point[1].y,point[3].x,point[3].y ); for(i=0;i<4;i++) { the_point[i].x = point[i].x; the_point[i].y = point[i].y; the_point[i].u = point[i].u; the_point[i].v = point[i].v; } quad3d_f(dest,POLYTYPE_ATEX_MASK,img, &point[0], &point[1], &point[3],&point[2] ); //fiend_quad3d(dest,img, &the_point[0], &the_point[1], &the_point[3],&the_point[2] ); //for(i=0;i<4;i++) //circlefill(dest,point[i].x,point[i].y,3,makecol(255,0,255)); }