// 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;
}
