%truth data 
alat = 51.07536402500000;
alon = -1.142153863528000e+002;
blat = 51.07529610280000;
blon = -1.142154213944000e+002;
clat = 51.07526183060000;
clon = -1.142152586000000e+002;

%use c as the datum
r = 2.09e7;
lat2x = pi*r/180;
lon2y = pi*r/180*cos(clat*pi/180);
cx = 0;
cy = 0;
bx = (blat-clat)*lat2x;
by = (blon-clon)*lon2y;
ax = (alat-clat)*lat2x;
ay = (alon-clon)*lon2y;

%plot centroids
hold off
plot(ay,ax,'r+',by,bx,'r+',cy,cx,'b+');
hold

%plot walls
hl = 6*.5*3.28;
hw = 4*.5*3.28;
nx = bx/sqrt(bx*bx+by*by);
ny = by/sqrt(bx*bx+by*by);
x = [-hl -hl hl hl -hl];
y = [-hw  hw hw -hw -hw];
xr = x*nx - y*ny;
yr = y*nx + x*ny;
plot(ay+yr,ax+xr,'r')
plot(by+yr,bx+xr,'r')
plot(cy+yr,cx+xr,'b')

%put our stuff on there
offx = -17;
offy = 7;
vec = 5;

a1blat = 51.07540218;
a1blon = -114.21537027;
a1wlat = 51.07543225;
a1wlon = -114.21544152;
a1wpsi = 330/180*pi;
a1w2lat = 51.07539476;
a1w2lon = -114.21545984;
a1w2psi = 123/180*pi;
a1bx = (a1blat-clat)*lat2x + offx;
a1by = (a1blon-clon)*lon2y + offy;
a1wx = (a1wlat-clat)*lat2x + offx;
a1wy = (a1wlon-clon)*lon2y + offy;
a1wx = [a1wx,a1wx+cos(a1wpsi)*vec];
a1wy = [a1wy,a1wy+sin(a1wpsi)*vec];
a1w2x = (a1w2lat-clat)*lat2x + offx;
a1w2y = (a1w2lon-clon)*lon2y + offy;
a1w2x = [a1w2x,a1w2x+cos(a1w2psi)*vec];
a1w2y = [a1w2y,a1w2y+sin(a1w2psi)*vec];

plot(a1by,a1bx,'co',a1wy(1),a1wx(1),'c+',a1wy,a1wx,'c',a1w2y(1),a1w2x(1),'cx',a1w2y,a1w2x,'c');

a2blat = 51.07533347;
a2blon = -114.21532187; 
a2wlat = 51.07535038;
a2wlon = -114.21544749;
a2wpsi = 113/180*pi;
a2bx = (a2blat-clat)*lat2x + offx;
a2by = (a2blon-clon)*lon2y + offy;
a2wx = (a2wlat-clat)*lat2x + offx;
a2wy = (a2wlon-clon)*lon2y + offy;
a2wx = [a2wx,a2wx+cos(a2wpsi)*vec];
a2wy = [a2wy,a2wy+sin(a2wpsi)*vec];

plot(a2by,a2bx,'mo',a2wy(1),a2wx(1),'m+',a2wy,a2wx,'m');

hold off