int minimal_rpos_diff( carmen_point_t pos1, carmen_point_t pos2, double pos_diff_min_dist, double pos_diff_min_rot ) { carmen_vec2_t v1, v2; v1.x = pos1.x; v1.y = pos1.y; v2.x = pos2.x; v2.y = pos2.y; if ( carmen_vec_distance(v1,v2) > pos_diff_min_dist ) return(TRUE); if ( carmen_orientation_diff(pos1.theta,pos2.theta) > pos_diff_min_rot ) return(TRUE); return(FALSE); }
carmen_move_t carmen_move_between_points( carmen_point_t start, carmen_point_t end ) { carmen_move_t move; /* compute forward and sideward sensing_MOVEMENT */ move.forward = + (end.y - start.y) * sin(start.theta) + (end.x - start.x) * cos(start.theta); move.sideward = - (end.y - start.y) * cos(start.theta) + (end.x - start.x) * sin(start.theta); move.rotation = carmen_orientation_diff( end.theta, start.theta ); return( move ); }