
% CS 5320
% Test file for A2 Matlab functions

my_eps = 0.001;   % my value for close enough to zero

d = cv_dist_pt_pt([1,2,3],[1,2,3]);
if (abs(d)>my_eps)
   disp('cv_dist_pt_pt: same point error');
end
d = cv_dist_pt_pt([1,2,3],[2,2,3]);
if (abs(d-1)>my_eps)
   disp('cv_dist_pt_pt: [1,2,3],[2,2,3] error');
end
d = cv_dist_pt_pt([1,1,1],[0,0,0]);
if (abs(d-sqrt(3))>my_eps)
   disp('cv_dist_pt_pt: [1,1,1],[0,0,0] error');
end

d = zeros(1,4);
d(1,1) = cv_dist_pt_line([0 0 0],[0 0 0; 1 0 0]);
d(1,2) = cv_dist_pt_line([0 0 0],[1 1 0; 1 0 0]);
d(1,3) = cv_dist_pt_line([1 0 0],[0 0 0; 1 1 0]);
d(1,4) = cv_dist_pt_line([2 0 0],[0 0 0; 1 1 0]);
if (abs(d(1,1))>my_eps)
   disp('cv_dist_pt_line: [0,0,0],[0,0,0;1,0,0] error');
end
if (abs(d(1,2)-1)>my_eps)
   disp('cv_dist_pt_line: [0,0,0],[1,1,0;1,0,0] error');
end
if (abs(d(1,3)-0.7071)>my_eps)
   disp('cv_dist_pt_line: [1,0,0],[0,0,0;1,1,0] error');
end
if (abs(d(1,4)-1.4142)>my_eps)
   disp('cv_dist_pt_line: [2,0,0],[0,0,0;1,1,0] error');
end

d(1,1) = cv_dist_line_line([0 0 0; 1 0 0],[0 0 0; 1 0 0]);
d(1,2) = cv_dist_line_line([0 0 0; 1 0 0],[0 0 0; 0 1 0]);
d(1,3) = cv_dist_line_line([0 0 0; 0 1 0],[1 0 0; 0 0 1]);
d(1,4) = cv_dist_line_line([0 0 0; 1 0 0],[0 0 2; 1 0 2]);
if (abs(d(1,1))>my_eps)
   disp('cv_dist_line_line: [0,0,0; 1,0,0],[0,0,0;1,0,0] error');
end
if (abs(d(1,2))>my_eps)
   disp('cv_dist_line_line: [0,0,0; 1,0,0],[0,0,0;0,1,0] error');
end
if (abs(d(1,3)-0.7071)>my_eps)
   disp('cv_dist_line_line:[0 0 0; 0 1 0],[1 0 0; 0 0 1] error');
end
if (abs(d(1,4)-2)>my_eps)
   disp('cv_dist_line_line: [0 0 0; 1 0 0],[0 0 2; 1 0 2] error');
end

d(1,1) = cv_dist_pt_plane([0 0 0],[0,0,0;0 1 0;0 0 1]);
d(1,2) = cv_dist_pt_plane([1 0 0],[0,0,0;0 1 0;0 0 1]);
d(1,3) = cv_dist_pt_plane([0 0 0],[1,0,0;0 1 0;0 0 1]);
d(1,4) = cv_dist_pt_plane([0 0 0],[2,0,0;2 1 0;2 0 1]);
if (abs(d(1,1))>my_eps)
   disp('cv_dist_pt_plane: [0,0,0],[0,0,0;0,1,0;0,0,1] error');
end
if (abs(d(1,2)-1)>my_eps)
   disp('cv_dist_pt_plane: [1,0,0],[0,0,0;0,1,0;0,0,1] error');
end
if (abs(d(1,3)-0.5774)>my_eps)
   disp('cv_dist_pt_plane:[0 0 0],[1 0 0; 0 1,0;0,0,1] error');
end
if (abs(d(1,4)-2)>my_eps)
   disp('cv_dist_pt_plane: [0 0 0],[2,0,0;2,1,0;2,0,1] error');
end

d(1,1) = cv_dist_line_plane([0 0 0; 1 0 0], [0 0 0; 1 0 0; 0 1 0]);
d(1,2) = cv_dist_line_plane([0 0 0; 1 0 0], [1 0 0; 1 1 0; 1 0 1]);
d(1,3) = cv_dist_line_plane([0 0 0; 0 1 0], [1 0 0; 1 1 0; 1 0 1]);
d(1,4) = cv_dist_line_plane([0 0 0; 0 1 0], [1 0 0; 1 1 0; 0 0 1]);
if (abs(d(1,1))>my_eps)
   disp('cv_dist_line_plane: [0,0,0;1,0,0],[0,0,0;1,0,0;0,1,0] error');
end
if (abs(d(1,2))>my_eps)
   disp('cv_dist_line_plane: [1,0,0],[0,0,0;0,1,0;0,0,1] error');
end
if (abs(d(1,3)-1)>my_eps)
   disp('cv_dist_line_plane: [0,0,0;0,1,0],[1,0,0;1,1,0;1,0,1] error');
end
if (abs(d(1,4)-0.7071)>my_eps)
   disp('cv_dist_line_plane: [0 0 0;0,1,0],[1,0,0;1,1,0;0,0,1] error');
end

p = cv_int_line_line([0 0 0; 1 0 0], [0 0 0; 0 1 0],0.01);
if (cv_dist_pt_pt(p,[0,0,0])>my_eps)
  disp('cv_int_line_line: error in [0 0 0; 1 0 0], [0 0 0; 0 1 0]');
end
p = cv_int_line_line([0 0 0; 1 0 0], [0 0 1; 0 1 1],0.01);
if ~isinf(p)
disp('cv_int_line_line: error in [0 0 0; 1 0 0], [0 0 0; 0 1 0]');
end
p = cv_int_line_line([0 0 0; 1 1 0], [1 0 0; 0 1 0],0.01);
if (cv_dist_pt_pt(p,[0.5,0.5,0])>my_eps)
  disp('cv_int_line_line: error in [0 0 0; 1 0 0], [0 0 0; 0 1 0]');
end
l1 = cv_int_line_line([0 0 0; 1 0 0], [0 0 0; 1 0 0],0.01);
if (cv_dist_pt_pt(l1(1,:),[0,0,0])>my_eps)
  disp('cv_int_line_line: error in [0 0 0; 1 0 0], [0 0 0; 1 0 0]');
end
if (cv_dist_pt_pt(l1(2,:),[1,0,0])>my_eps)
  disp('cv_int_line_line: error in [0 0 0; 1 0 0], [0 0 0; 1 0 0]');
end

l1 =  cv_int_line_plane([0 1 0; 1 0 0], [0 0 0; 1 0 0; 1 1 0],0.01);
if (cv_dist_pt_pt(l1(1,:),[0,1,0])>my_eps)
  disp('cv_int_line_plane: error in[0 1 0;1 0 0],[0 0 0;1 0 0;1 1 0]');
end
if (cv_dist_pt_pt(l1(2,:),[1,0,0])>my_eps)
  disp('cv_int_line_plane: error in[0 1 0;1 0 0],[0 0 0;1 0 0;1 1 0]');
end

p = cv_int_line_plane([0 0 1; 1 0 0], [0 0 0; 1 0 0; 1 1 0],0.01);
if (cv_dist_pt_pt(p,[1,0,0])>my_eps)
  disp('cv_int_line_plane: error in[0 0 1;1 0 0],[0 0 0;1 0 0;1 1 0]');
end

p =  cv_int_line_plane([0 0 1; 1 0 1], [0 0 0; 1 0 0; 1 1 0],0.01);
if (~isinf(p))
  disp('cv_int_line_plane: error in[0 0 1;1 0 1],[0 0 0;1 0 0;1 1 0]');
end

p = cv_int_line_plane([0 0 1; 1 0 0.9], [0 0 0; 1 0 0; 1 1 0],0.01);
if (cv_dist_pt_pt(p,[10,0,0])>my_eps)
  disp('cv_int_line_plane: error in[0 0 1;1 0 0.9],[0 0 0;1 0 0;1 1 0]');
end
p = cv_int_line_plane([0 0 1; 1 1 0.9], [0 0 0; 1 0 0; 1 1 0],0.01);
if (cv_dist_pt_pt(p,[10,10,0])>my_eps)
  disp('cv_int_line_plane: error in[0 0 1;1 1 0.9],[0 0 0;1 0 0;1 1 0]');
end

p1 = [0 0 0; 1 0 0; 1 1 0];
pl = cv_int_plane_plane(p1,p1,0.01);
if (cv_dist_line_plane(pl(1:2,:),p1)>my_eps)
  disp('cv_int_plane_plane: error in p1,p1');
end

p3 = [0 0 0; 0 1 0; 0 0 1];
l1 = cv_int_plane_plane(p1,p3,0.01);
if (cv_dist_line_line(l1,[0,0,0;0,1,0])>my_eps)
  disp('cv_int_plane_plane: error in p1,p3');
end

p2 = [0,0,1; 1,0,1;1 1 1];
p = cv_int_plane_plane(p1,p2,0.01);
if (~isinf(p))
  disp('cv_int_plane_plane: error in p1,p2');
end

