function [y q] = fval2(v,p)

m = rotation_matrix(v);
%q(1) = -v(4)*m(1,1)/m(3,1)-p(1);
%q(2) = -v(4)*m(2,1)/m(3,1)-p(2);
%q(3) = -v(4)*m(1,2)/m(3,2)-p(3);
%q(4) = -v(4)*m(2,2)/m(3,2)-p(4);
% x-vanishing point
if isinf(p(1))
    q(1:2) = [m(1,1)-1 m(2,1)];
else
    q(1) = v(4)*m(1,1)-p(1,1)*m(3,1);
    q(2) = v(4)*m(2,1)-p(1,2)*m(3,1);
end
% y-vanishing point
if isinf(p(4))
    q(3:4) = [m(1,2) m(2,2)-1];
else
    q(3) = v(4)*m(1,2)-p(2,1)*m(3,2);
    q(4) = v(4)*m(2,2)-p(2,2)*m(3,2);
end
y = dot(q,q);

Published with MATLAB® 7.10