// do_circle - initiate moving in a circle void Copter::do_circle(const AP_Mission::Mission_Command& cmd) { Location_Class circle_center(cmd.content.location); // default lat/lon to current position if not provided // To-Do: use stopping point or position_controller's target instead of current location to avoid jerk? if (circle_center.lat == 0 && circle_center.lng == 0) { circle_center.lat = current_loc.lat; circle_center.lng = current_loc.lng; } // default target altitude to current altitude if not provided if (circle_center.alt == 0) { int32_t curr_alt; if (current_loc.get_alt_cm(circle_center.get_alt_frame(),curr_alt)) { // circle altitude uses frame from command circle_center.set_alt_cm(curr_alt,circle_center.get_alt_frame()); } else { // default to current altitude above origin circle_center.set_alt_cm(current_loc.alt, current_loc.get_alt_frame()); Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); } } // calculate radius uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1 // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge auto_circle_movetoedge_start(circle_center, circle_radius_m); }
//====================================================================== // Function: edge_tangent // Description: return curvature at a point on the edge // Author: sjowen // Date: 2/01 //====================================================================== CubitStatus CubitFacetEdge::edge_curvature( const CubitVector &/*point_on_edge*/, CubitVector &curvature, CubitFacetEdge *closest_edge ) { CubitVector vec_ba, vec_ca, center_point; //if point(0) is middle point if( closest_edge->other_point( point(0) ) ) { center_point = point(0)->coordinates(); vec_ba = closest_edge->point(0)->coordinates() - center_point; vec_ca = point(1)->coordinates() - center_point; } //if point(1) is middle point else if( closest_edge->other_point( point(1) ) ) { center_point = point(1)->coordinates(); vec_ba = point(0)->coordinates() - center_point; vec_ca = closest_edge->point(1)->coordinates() - center_point; } else assert(0); // Squares of lengths of the edges incident to `a'. double ba_length = vec_ba.length_squared(); double ca_length = vec_ca.length_squared(); // Cross product of these edges. // (Take your chances with floating-point roundoff.) CubitVector cross_bc = vec_ba * vec_ca; // Calculate the denominator of the formulae. double temp_dbl = cross_bc % cross_bc; CubitVector circle_center(0.0,0.0,0.0); if(fabs(temp_dbl) > CUBIT_DBL_MIN){ double denominator = 0.5 / (temp_dbl); assert(denominator != 0.0); // Calculate offset (from `a') of circumcenter. circle_center = (ba_length * vec_ca - ca_length * vec_ba) * cross_bc; circle_center *= denominator; //store radius double radius = circle_center.length(); circle_center.normalize(); circle_center /= radius; } curvature = circle_center; return CUBIT_SUCCESS; }
bool App::update() { game_time.update(); canvas.clear(clan::Colorf(0.0f,0.0f,0.0f, 0.0f)); rock.set_color(clan::Colorf(1.0f, 1.0f, 1.0f, 0.8f)); rock.draw(canvas, 0.0f, 0.0f); // Rotate tux rotation += game_time.get_time_elapsed() * 100.0f; clan::Angle angle; angle.set_degrees(rotation); tux.set_angle(angle); // Caculate tux position clan::Pointf circle_center( (float) (canvas.get_width()/2), (float) (canvas.get_height()/2) ); const float radius = 210.0f; int tux_circle = 12; tux_position.x = -(radius - tux_radius - tux_circle) * cos( angle.to_radians() / 2.0f ); tux_position.y = (radius - tux_radius - tux_circle) * sin( angle.to_radians()/ 2.0f ); tux_position += circle_center; tux_position.x -= tux.get_width()/2; tux_position.y -= tux.get_height()/2; // Give tux circle blue outer outline, because it looks nice canvas.fill_circle(tux_position.x + tux_radius, tux_position.y + tux_radius, tux_radius+tux_circle, clan::Colorf(0.0f, 0.0f, 1.0f, 1.0f)); // Make see through border canvas.set_blend_state(blend_state_off); canvas.fill_circle(tux_position.x + tux_radius, tux_position.y + tux_radius, tux_radius+tux_circle-2, clan::Colorf(0.0f, 0.0f, 0.0f, 0.0f)); canvas.reset_blend_state(); // Give tux circle blue outline, to mask the alpha channel canvas.fill_circle(tux_position.x + tux_radius, tux_position.y + tux_radius, tux_radius+2, clan::Colorf(0.0f, 0.0f, 1.0f, 1.0f)); // Draw tux tux.draw(canvas, tux_position.x, tux_position.y); // Draw text font_large.draw_text(canvas, 10-2, 50-2, "ClanLib Layered Window", clan::Colorf(0.1f, 0.1f, 0.1f, 1.0f)); font_large.draw_text(canvas, 10, 50, "ClanLib Layered Window", clan::Colorf::green); font_small.draw_text(canvas, 60-2, 80-2, "Click mouse on the penguin to exit", clan::Colorf(0.1f, 0.1f, 0.1f, 1.0f)); font_small.draw_text(canvas, 60, 80, "Click mouse on the penguin to exit", clan::Colorf::green); font_small.draw_text(canvas, 110-2, 110-2, "Drag rock to move window", clan::Colorf(0.1f, 0.1f, 0.1f, 1.0f)); font_small.draw_text(canvas, 110, 110, "Drag rock to move window", clan::Colorf::green); window.flip(1); return !quit; }