fid = fopen('Pointer.txt', 'r');

A = [];
b = [];
X = []; Y = []; Z = [];

i = 0;

while 1
    tline = fgetl(fid);
    
    if (tline == -1) break; end
    if (tline(1:2) == '/*') continue; end
    
    [temp, count] = sscanf(tline, '%*s %e %e %e %e %e %e %e %e %e');
    
    if (count ~= 9) continue; end
    if (temp(9) ~= 1) continue; end
    
    b_temp = -temp(1:3);
    A_temp = cat(2, zeros(3), -eye(3) );
    qx = temp(4);
    qy = temp(5);
    qz = temp(6);
    qw = temp(7);
    
    qx2 = qx*qx;
    qy2 = qy*qy;
    qz2 = qz*qz;
    qxy = qx*qy;
    qyz = qy*qz;
    qxz = qx*qz;
    qxw = qx*qw;
    qyw = qy*qw;
    qzw = qz*qw;
    
    A_temp(1,1) = 1 - 2*(qy2 + qz2);
    A_temp(2,2) = 1 - 2*(qx2 + qz2);
    A_temp(3,3) = 1 - 2*(qx2 + qy2);
    A_temp(2,1) = 2*(qxy + qzw);
    A_temp(3,1) = 2*(qxz - qyw);
    A_temp(1,2) = 2*(qxy - qzw);
    A_temp(3,2) = 2*(qyz + qxw);
    A_temp(1,3) = 2*(qxz + qyw);
    A_temp(2,3) = 2*(qyz - qxw);
    
    A(i*3+1:i*3+3, 1:6) = A_temp;
    b(i*3+1:i*3+3)      = b_temp;

    i = i + 1;
    
    X(i) = b_temp(1);
    Y(i) = b_temp(2);
    Z(i) = b_temp(3);
end

fclose(fid);

b = b';

cat(2, A, b);

erg = A\b;

p_pointer = erg(1:3);
p_world   = erg(4:6);

disp('Pointer Coord:'); disp(p_pointer);
disp('World Coord:'); disp(p_world);

disp('Condition of A:'); disp(cond(A) );

plot3(X,Y,Z);

c     = reshape(A*erg-b, 3, []);
norms = [];

for i = 1:size(c,2)
    norms(i) = norm( c(1:3,i) );
end 

means = [];
means(1) = mean( abs(c(1, 1:size(c,2)) ) );
means(2) = mean( abs(c(2, 1:size(c,2)) ) );
means(3) = mean( abs(c(3, 1:size(c,2)) ) );

disp('Norm Residuum 1:'); disp( mean(norms) );
disp('Norm Residuum 2:'); disp( norm(means) );

