circlefit3d

 circlefit3d: Compute center and radii of circles in 3d which are defined by three points on the circumference
 usage: [center,rad,v1,v2] = circlefit3d(p1,p2,p3)

 arguments: (input)
  p1, p2, p3 - vectors of points (rowwise, size(p1) = [n 3])
               describing the three corresponding points on the same circle.
               p1, p2 and p3 must have the same length n.

 arguments: (output)
  center - (nx3) matrix of center points for each triple of points in p1,  p2, p3

  rad    - (nx1) vector of circle radii.
           if there have been errors, radii is a negative scalar ( = error code)

  v1, v2 - (nx3) perpendicular vectors inside circle plane

 Example usage:

  (1)
      p1 = rand(10,3);
      p2 = rand(10,3);
      p3 = rand(10,3);
      [center, rad] = circlefit3d(p1,p2,p3);
      % verification, result should be all (nearly) zero
      result(:,1)=sqrt(sum((p1-center).^2,2))-rad;
      result(:,2)=sqrt(sum((p2-center).^2,2))-rad;
      result(:,3)=sqrt(sum((p3-center).^2,2))-rad;
      if sum(sum(abs(result))) < 1e-12,
       disp('All circles have been found correctly.');
      else,
       disp('There had been errors.');
      end


 (2)
       p1=rand(4,3);p2=rand(4,3);p3=rand(4,3);
       [center,rad,v1,v2] = circlefit3d(p1,p2,p3);
       plot3(p1(:,1),p1(:,2),p1(:,3),'bo');hold on;plot3(p2(:,1),p2(:,2),p2(:,3),'bo');plot3(p3(:,1),p3(:,2),p3(:,3),'bo');
       for i=1:361,
           a = i/180*pi;
           x = center(:,1)+sin(a)*rad.*v1(:,1)+cos(a)*rad.*v2(:,1);
           y = center(:,2)+sin(a)*rad.*v1(:,2)+cos(a)*rad.*v2(:,2);
           z = center(:,3)+sin(a)*rad.*v1(:,3)+cos(a)*rad.*v2(:,3);
           plot3(x,y,z,'r.');
       end
       axis equal;grid on;rotate3d on;

 
 Author: Johannes Korsawe
 E-mail: johannes.korsawe@volkswagen.de
 Release: 1.0
 Release date: 26/01/2012

0001 function [center,rad,v1n,v2nb] = circlefit3d(p1,p2,p3)
0002 % circlefit3d: Compute center and radii of circles in 3d which are defined by three points on the circumference
0003 % usage: [center,rad,v1,v2] = circlefit3d(p1,p2,p3)
0004 %
0005 % arguments: (input)
0006 %  p1, p2, p3 - vectors of points (rowwise, size(p1) = [n 3])
0007 %               describing the three corresponding points on the same circle.
0008 %               p1, p2 and p3 must have the same length n.
0009 %
0010 % arguments: (output)
0011 %  center - (nx3) matrix of center points for each triple of points in p1,  p2, p3
0012 %
0013 %  rad    - (nx1) vector of circle radii.
0014 %           if there have been errors, radii is a negative scalar ( = error code)
0015 %
0016 %  v1, v2 - (nx3) perpendicular vectors inside circle plane
0017 %
0018 % Example usage:
0019 %
0020 %  (1)
0021 %      p1 = rand(10,3);
0022 %      p2 = rand(10,3);
0023 %      p3 = rand(10,3);
0024 %      [center, rad] = circlefit3d(p1,p2,p3);
0025 %      % verification, result should be all (nearly) zero
0026 %      result(:,1)=sqrt(sum((p1-center).^2,2))-rad;
0027 %      result(:,2)=sqrt(sum((p2-center).^2,2))-rad;
0028 %      result(:,3)=sqrt(sum((p3-center).^2,2))-rad;
0029 %      if sum(sum(abs(result))) < 1e-12,
0030 %       disp('All circles have been found correctly.');
0031 %      else,
0032 %       disp('There had been errors.');
0033 %      end
0034 %
0035 %
0036 % (2)
0037 %       p1=rand(4,3);p2=rand(4,3);p3=rand(4,3);
0038 %       [center,rad,v1,v2] = circlefit3d(p1,p2,p3);
0039 %       plot3(p1(:,1),p1(:,2),p1(:,3),'bo');hold on;plot3(p2(:,1),p2(:,2),p2(:,3),'bo');plot3(p3(:,1),p3(:,2),p3(:,3),'bo');
0040 %       for i=1:361,
0041 %           a = i/180*pi;
0042 %           x = center(:,1)+sin(a)*rad.*v1(:,1)+cos(a)*rad.*v2(:,1);
0043 %           y = center(:,2)+sin(a)*rad.*v1(:,2)+cos(a)*rad.*v2(:,2);
0044 %           z = center(:,3)+sin(a)*rad.*v1(:,3)+cos(a)*rad.*v2(:,3);
0045 %           plot3(x,y,z,'r.');
0046 %       end
0047 %       axis equal;grid on;rotate3d on;
0048 %
0049 %
0050 % Author: Johannes Korsawe
0051 % E-mail: johannes.korsawe@volkswagen.de
0052 % Release: 1.0
0053 % Release date: 26/01/2012
0054 
0055 % Default values
0056 center = [];rad = 0;v1n=[];v2nb=[];
0057 
0058 % check inputs
0059 % check number of inputs
0060 if nargin~=3,
0061     fprintf('??? Error using ==> cirlefit3d\nThree input matrices are needed.\n');rad = -1;return;
0062 end
0063 % check size of inputs
0064 if size(p1,2)~=3 || size(p2,2)~=3 || size(p3,2)~=3,
0065     fprintf('??? Error using ==> cirlefit3d\nAll input matrices must have three columns.\n');rad = -2;return;
0066 end
0067 n = size(p1,1);
0068 if size(p2,1)~=n || size(p3,1)~=n,
0069     fprintf('??? Error using ==> cirlefit3d\nAll input matrices must have the same number or rows.\n');rad = -3;return;
0070 end
0071 % more checks are to follow inside calculation
0072 
0073 % Start calculation
0074 % v1, v2 describe the vectors from p1 to p2 and p3, resp.
0075 v1 = p2 - p1;v2 = p3 - p1;
0076 % l1, l2 describe the lengths of those vectors
0077 l1 = sqrt((v1(:,1).*v1(:,1)+v1(:,2).*v1(:,2)+v1(:,3).*v1(:,3)));
0078 l2 = sqrt((v2(:,1).*v2(:,1)+v2(:,2).*v2(:,2)+v2(:,3).*v2(:,3)));
0079 if find(l1==0) | find(l2==0), %#ok<OR2>
0080     fprintf('??? Error using ==> cirlefit3d\nCorresponding input points must not be identical.\n');rad = -4;return;
0081 end
0082 % v1n, v2n describe the normalized vectors v1 and v2
0083 v1n = v1;for i=1:3, v1n(:,i) = v1n(:,i)./l1;end
0084 v2n = v2;for i=1:3, v2n(:,i) = v2n(:,i)./l2;end
0085 % nv describes the normal vector on the plane of the circle
0086 nv = [v1n(:,2).*v2n(:,3) - v1n(:,3).*v2n(:,2) , v1n(:,3).*v2n(:,1) - v1n(:,1).*v2n(:,3) , v1n(:,1).*v2n(:,2) - v1n(:,2).*v2n(:,1)];
0087 if find(sum(abs(nv),2)<1e-5),
0088     fprintf('??? Warning using ==> cirlefit3d\nSome corresponding input points are nearly collinear.\n');
0089 end
0090 % v2nb: orthogonalization of v2n against v1n
0091 dotp = v2n(:,1).*v1n(:,1) + v2n(:,2).*v1n(:,2) + v2n(:,3).*v1n(:,3);
0092 v2nb = v2n;for i=1:3,v2nb(:,i) = v2nb(:,i) - dotp.*v1n(:,i);end
0093 % normalize v2nb
0094 l2nb = sqrt((v2nb(:,1).*v2nb(:,1)+v2nb(:,2).*v2nb(:,2)+v2nb(:,3).*v2nb(:,3)));
0095 for i=1:3, v2nb(:,i) = v2nb(:,i)./l2nb;end
0096 
0097 % remark: the circle plane will now be discretized as follows
0098 %
0099 % origin: p1                    normal vector on plane: nv
0100 % first coordinate vector: v1n  second coordinate vector: v2nb
0101 
0102 % calculate 2d coordinates of points in each plane
0103 % p1_2d = zeros(n,2); % set per construction
0104 % p2_2d = zeros(n,2);p2_2d(:,1) = l1; % set per construction
0105 p3_2d = zeros(n,2); % has to be calculated
0106 for i = 1:3,
0107     p3_2d(:,1) = p3_2d(:,1) + v2(:,i).*v1n(:,i);
0108     p3_2d(:,2) = p3_2d(:,2) + v2(:,i).*v2nb(:,i);
0109 end
0110 
0111 % calculate the fitting circle
0112 % due to the special construction of the 2d system this boils down to solving
0113 % q1 = [0,0], q2 = [a,0], q3 = [b,c] (points on 2d circle)
0114 % crossing perpendicular bisectors, s and t running indices:
0115 % solve [a/2,s] = [b/2 + c*t, c/2 - b*t]
0116 % solution t = (a-b)/(2*c)
0117 
0118 a = l1;b = p3_2d(:,1);c = p3_2d(:,2);
0119 t = 0.5*(a-b)./c;
0120 scale1 = b/2 + c.*t;scale2 = c/2 - b.*t;
0121 
0122 % centers
0123 center = zeros(n,3);
0124 for i=1:3,
0125     center(:,i) = p1(:,i) + scale1.*v1n(:,i) + scale2.*v2nb(:,i);
0126 end
0127 
0128 % radii
0129 rad = sqrt((center(:,1)-p1(:,1)).^2+(center(:,2)-p1(:,2)).^2+(center(:,3)-p1(:,3)).^2);