function [] = ellipse(xpos, ypos, a, b, theta, color)
% matlab script to draw an ellipse with majaxis=1, minaxis=b, at angle
% theta, centered at (xpos, ypos)

if (nargin < 6) 
  color = 'y';
end

npts = 100;

st = sin(theta);
ct = cos(theta);

if ((a==0)&(b==0)) 
  disp('draw_ellipse: axes are zero');
  return
end

vec1 = zeros(2,npts+1);
vec2 = zeros(2,npts+1);

lastx = -a * ct;
lasty = -a * st;

vec1(1,1) = xpos + lastx;
vec1(2,1) = ypos + lasty;
vec2(1,1) = xpos - lastx;
vec2(2,1) = ypos - lasty;

for i=1:npts
  x = -a + (2 * a * i/npts);
  y = b * sqrt(1.0 - (x*x)/(a*a));
  % rotate this point x,y by theta 
  dx = x * ct - y * st;
  dy = y * ct + x * st;
  vec1(1,i+1) = xpos + dx;
  vec1(2,i+1) = ypos + dy;
  vec2(1,i+1) = xpos - dx;
  vec2(2,i+1) = ypos - dy;
end
plot(vec1(1,:), vec1(2,:), color);
hold on
plot(vec2(1,:), vec2(2,:), color)
axis('equal')




