% WAYPT2D - Plan view of idealized track for waypoint_2d behavior.
%
%    waypt2d( wpt, spd, hdg, tim, cap, cir )
%
%    B. A. Moran, 25-May-1998

%    Copyright (C) 1995-1998
%    Massachussetts Institute of Technology
%    Sea Grant AUV Laboratory
%    $Id: waypt2d.m,v 1.1 1998/07/01 13:31:47 bamoran Exp $	

function waypt2d( wpt, spd, hdg, tim, cap, cir )

if nargin < 6, cir = 0; end
if nargin < 5, cap = 0; end

fprintf( 1, ' behavior: waypoint_2d\n\n' )
fprintf( 1, '  b_arg:                       wpt_n(m) %6.1f\n', wpt(1) )
fprintf( 1, '  b_arg:                       wpt_e(m) %6.1f\n', wpt(2) )
fprintf( 1, '  b_arg:                     speed(m/s) %7.2f\n', spd )
fprintf( 1, '  b_arg:                   heading(deg) %6.1f\n', hdg )
fprintf( 1, '  b_arg:                     timeout(s) %4d\n', tim )
fprintf( 1, '  b_arg:                  circle_mode() %4d\n', cir )
fprintf( 1, '  b_arg:                     capture(m) %4.2g\n', cap )
fprintf( 1, '  b_arg:                       depth(m)  \n' )
fprintf( 1, '  b_arg:                    gain(deg/m)  \n' )

if cir == 0, cap = -cap;
else cap = abs(cap); end

por = rb2gr( spd*tim, hdg+180, wpt );
eol = rb2gr( spd*tim - cap, hdg, por );

line( wpt(2), wpt(1), 'Marker', '+', 'LineStyle', 'none' );
line( [por(2) eol(2)], [por(1) eol(1)] );
