#!/usr/bin/python

from numpy import *
from pylab import *
from mission.waypoint import waypt_t
from mission.goals import goals_t
from constants import *
from math import cos, sin, atan2, pi

def xytMult(xyt, p):
	b = -xyt[2] + pi/2.0
	c = cos(b)
	s = sin(b)
	r = [	c*p[0] - s*p[1] + xyt[0]*p[2] ,
			s*p[0] + c*p[1] + xyt[1]*p[2] ,
			1*p[2]						  ]
	return r

p = waypt_t(2,1,0)

step = 0.25
xmin, xmax = -5, 5
ymin, ymax = -5, 5
x = arange(xmin, xmax, step)
y = arange(ymin, ymax, step)

p1 = [0,		0, 1]
p2 = [step/3.0, 0, 1]

figure
hold = True
for xi in x:
	for yi in y:
		xyt = [xi, yi, p.bearing(xi, yi)]
		tp1 = xytMult(xyt, p1)
		tp2 = xytMult(xyt, p2)
		plot(	[tp1[0], tp2[0] ],
				[tp1[1], tp2[1] ] )
show()
