void DiffDriveRobot::spin(){
if (goals_.size()==0) return;
if (stop_mode_){
stop();
return;
}
diff_drive_robot::Goal smoothed_goal, current_goal, next_goal;
current_goal = goals_[goal_idx_];
if ((distance_to_goal(current_goal)<R_MERGE_END) && (goal_idx_ < goals_.size() - 1)){
goal_idx_++;
current_goal = goals_[goal_idx_];
std::cout<<"Moving to goal"<<goal_idx_<<std::endl;
}
std::cout<<"Distance to goal = "<<distance_to_goal(current_goal)<<std::endl;
if (goal_idx_<goals_.size()-1)
next_goal = goals_[goal_idx_+1];
else
next_goal = goals_[goal_idx_];
//smooth transition between consecutive goals.
if ((distance_to_goal(current_goal)<= R_MERGE_START) &&
(distance_to_goal(current_goal)>= R_MERGE_END)){
double d = distance_to_goal(current_goal);
smoothed_goal.x = ((d-R_MERGE_END)/R_MERGE_END)*current_goal.x +
((R_MERGE_START-d)/R_MERGE_END)*next_goal.x ;
smoothed_goal.y = ((d-R_MERGE_END)/R_MERGE_END)*current_goal.y +
((R_MERGE_START-d)/R_MERGE_END)*next_goal.y ;
smoothed_goal.theta = ((d-R_MERGE_END)/R_MERGE_END)*current_goal.theta +
((R_MERGE_START-d)/R_MERGE_END)*next_goal.theta;
}
else smoothed_goal = current_goal; //for all other cases
double dx = smoothed_goal.x-x_;
double dy = smoothed_goal.y-y_;
double eps = atan2(dy, dx);
double rho = hypot(dx, dy);
double gamma = wrap_angle(eps - theta_);
double delta = wrap_angle(smoothed_goal.theta - gamma - theta_);
std::cout<<"rho = "<<rho<<", gamma = "<<gamma*180.0/PI<<", delta = "<<delta*180.0/PI<<std::endl;
double v_desired = K_RHO * rho;
double w_desired = K_GAMMA * gamma + K_DELTA * delta;
std::cout<<"v_desired = "<<v_desired<< ", w_desired = "<<w_desired*180.0/PI;
// check velocity limits
if (fabs(v_desired)>v_max_) v_desired = copysign(v_max_, v_desired);
if (fabs(w_desired)>w_max_) w_desired = copysign(w_max_, w_desired);
// check accelration limits
if (fabs(v_desired-v_)/dt_ > a_max_)
v_desired = v_ + copysign(a_max_*dt_, v_desired-v_);
if (fabs(w_desired-w_)/dt_ > alpha_max_)
w_desired = w_ + copysign(alpha_max_*dt_, w_desired-w_);
w_ = w_desired;
v_ = v_desired *(1.0 -0.25*fabs(w_)/w_max_);
std::cout<<", v = "<<v_<<", w = "<<w_*180.0/PI<<std::endl;
}
void rotate_base_page_3d_scene_heading (float delta_heading)
{
if (get_ui_object_drawable (campaign_page [CAMPAIGN_PAGE_BASE]))
{
page_3d_heading += rad (delta_heading);
page_3d_heading = wrap_angle (page_3d_heading);
}
}
void set_chase_camera_view_target_to_source (entity *source, entity *target)
{
camera
*raw;
float
length;
vec3d
*source_position,
*target_position,
direction;
ASSERT (source);
ASSERT (target);
ASSERT (get_camera_entity ());
raw = get_local_entity_data (get_camera_entity ());
source_position = get_local_entity_vec3d_ptr (source, VEC3D_TYPE_POSITION);
target_position = get_local_entity_vec3d_ptr (target, VEC3D_TYPE_POSITION);
direction.x = source_position->x - target_position->x;
direction.y = source_position->y - target_position->y;
direction.z = source_position->z - target_position->z;
length = (direction.x * direction.x) + (direction.y * direction.y) + (direction.z * direction.z);
if (length > 1.0)
{
length = sqrt (length);
normalise_3d_vector_given_magnitude (&direction, length);
raw->chase_camera_heading = atan2 (direction.x, direction.z);
if (raw->chase_camera_lock_rotate)
{
raw->chase_camera_heading -= get_local_entity_float_value (target, FLOAT_TYPE_HEADING);
raw->chase_camera_heading = wrap_angle (raw->chase_camera_heading);
}
raw->chase_camera_pitch = asin (direction.y);
}
}
void update_static_camera (camera *raw)
{
float
heading,
pitch;
ASSERT (raw);
heading = get_heading_from_attitude_matrix (raw->attitude);
pitch = get_pitch_from_attitude_matrix (raw->attitude);
//
// adjust camera heading
//
if (adjust_view_left_key || joystick_pov_left)
{
heading -= STATIC_CAMERA_ROTATE_RATE * get_delta_time ();
}
else if (adjust_view_right_key || joystick_pov_right)
{
heading += STATIC_CAMERA_ROTATE_RATE * get_delta_time ();
}
heading = wrap_angle (heading);
//
// adjust camera pitch
//
if (adjust_view_up_key || joystick_pov_up)
{
pitch += STATIC_CAMERA_ROTATE_RATE * get_delta_time ();
pitch = min (STATIC_CAMERA_ROTATE_UP_LIMIT, pitch);
}
else if (adjust_view_down_key || joystick_pov_down)
{
pitch -= STATIC_CAMERA_ROTATE_RATE * get_delta_time ();
pitch = max (STATIC_CAMERA_ROTATE_DOWN_LIMIT, pitch);
}
get_3d_transformation_matrix (raw->attitude, heading, pitch, 0.0);
}
float RotationTracker::velocity(double now) const
{
size_t begin;
if (!velocity_calc_begin(begin, now)) {
return 0;
}
double dt = _list.back().when - _list[begin].when;
if (dt <= 0) {
return 0;
}
// Sum intelligently:
double sum = 0;
for (size_t i = begin + 1; i < _list.size(); ++i) {
sum += wrap_angle(_list[i].where - _list[i-1].where);
}
return (float)(sum / dt);
}
static vector_type subtract (const vector_type& a, const vector_type& b) {
return { a(0)-b(0), a(1)-b(1), wrap_angle(a(2)-b(2)) };
}
void update_chase_camera (camera *raw)
{
entity
*en;
float
combined_heading,
z_min,
z_max;
vec3d
rel_camera_position;
//
// pre-amble
//
ASSERT (raw);
ASSERT (raw->external_view_entity);
en = raw->external_view_entity;
//
// adjust camera heading
//
if (adjust_view_left_key || joystick_pov_left)
{
raw->chase_camera_heading += CHASE_CAMERA_ROTATE_RATE * get_delta_time ();
}
else if (adjust_view_right_key || joystick_pov_right)
{
raw->chase_camera_heading -= CHASE_CAMERA_ROTATE_RATE * get_delta_time ();
}
raw->chase_camera_heading = wrap_angle (raw->chase_camera_heading);
//
// adjust camera pitch
//
if (adjust_view_up_key || joystick_pov_up)
{
raw->chase_camera_pitch -= CHASE_CAMERA_ROTATE_RATE * get_delta_time ();
raw->chase_camera_pitch = max (CHASE_CAMERA_ROTATE_DOWN_LIMIT, raw->chase_camera_pitch);
}
else if (adjust_view_down_key || joystick_pov_down)
{
raw->chase_camera_pitch += CHASE_CAMERA_ROTATE_RATE * get_delta_time ();
raw->chase_camera_pitch = min (CHASE_CAMERA_ROTATE_UP_LIMIT, raw->chase_camera_pitch);
}
if (adjust_view_zoom_in_key)
{
raw->chase_camera_zoom -= CHASE_CAMERA_ZOOM_RATE * get_delta_time ();
raw->chase_camera_zoom = max (CHASE_CAMERA_ZOOM_IN_LIMIT, raw->chase_camera_zoom);
}
else if (adjust_view_zoom_out_key)
{
raw->chase_camera_zoom += CHASE_CAMERA_ZOOM_RATE * get_delta_time ();
raw->chase_camera_zoom = min (CHASE_CAMERA_ZOOM_OUT_LIMIT, raw->chase_camera_zoom);
}
//
// get camera attitude
//
if (get_local_entity_int_value (en, INT_TYPE_ALIVE) && raw->chase_camera_lock_rotate)
{
combined_heading = get_local_entity_float_value (en, FLOAT_TYPE_HEADING);
}
else
{
combined_heading = 0.0;
}
combined_heading += raw->chase_camera_heading;
get_3d_transformation_matrix (raw->attitude, combined_heading, raw->chase_camera_pitch, 0.0);
//
// get camera position
//
if (in_flight_zoom_test)
{
z_min = get_local_entity_float_value (en, FLOAT_TYPE_CHASE_VIEW_MIN_DISTANCE_TEST);
z_max = get_local_entity_float_value (en, FLOAT_TYPE_CHASE_VIEW_MAX_DISTANCE_TEST);
}
else
{
z_min = get_local_entity_float_value (en, FLOAT_TYPE_CHASE_VIEW_MIN_DISTANCE);
z_max = get_local_entity_float_value (en, FLOAT_TYPE_CHASE_VIEW_MAX_DISTANCE);
}
ASSERT (z_min < z_max);
rel_camera_position.x = 0.0;
rel_camera_position.y = 0.0;
rel_camera_position.z = -(((z_max - z_min) * raw->chase_camera_zoom * raw->chase_camera_zoom) + z_min);
multiply_matrix3x3_vec3d (&rel_camera_position, raw->attitude, &rel_camera_position);
get_local_entity_target_point (en, &raw->position);
raw->position.x += rel_camera_position.x;
raw->position.y += rel_camera_position.y;
raw->position.z += rel_camera_position.z;
//
// keep point above ground (unless point off map)
//
if (point_inside_map_area (&raw->position))
{
raw->position.y = max (raw->position.y, get_3d_terrain_point_data (raw->position.x, raw->position.z, &raw->terrain_info) + CAMERA_MIN_HEIGHT_ABOVE_GROUND);
}
//
// motion vector
//
get_local_entity_vec3d (en, VEC3D_TYPE_MOTION_VECTOR, &raw->motion_vector);
}
void DiffDriveRobot::set_pose(double x, double y, double theta){
x_ = x;
y_ = y;
theta_ = wrap_angle(theta);
}
