solid_angle

 BODYANG  Body (solid) angle computation.
    BA = BODYANG(R1,R2,R3)  Calculates body
    (solid) angle formed by 3 points with
    coordinates R1, R2, R3 (R1=[x1 y1 z1],...)
    as seen from origin of coordinate system.

0001 function  ba = bodyang(R1,R2,R3)
0002 % BODYANG  Body (solid) angle computation.
0003 %    BA = BODYANG(R1,R2,R3)  Calculates body
0004 %    (solid) angle formed by 3 points with
0005 %    coordinates R1, R2, R3 (R1=[x1 y1 z1],...)
0006 %    as seen from origin of coordinate system.
0007 
0008 %  Copyright (c) 1995 by Kirill K. Pankratov.
0009 %    kirill@plume.mit.edu
0010 %    11/16/94,
0011 
0012  % Defaults and parameters ...............................
0013 tol = 4*eps;         % Tolerance for angles
0014 null_value = nan;    % Value for the case of
0015                      % zero-length vectors
0016 
0017  % Handle input ******************************************
0018 if nargin==0, help bodyang, return, end  % Help
0019 if nargin==1  % If input is [R1;R2;R3] or [az1 el1; .. az3,el3]
0020   sz = size(R1);
0021   if all(sz==[2 2]), R1 = [0 0; R1];
0022   elseif all(sz==[2 3]), R1 = R1'; end
0023   if all(size(R1)==[3 2])
0024     [R1,R2,R3] = sph2cart(R1(:,1)',R1(:,2)',1);
0025   end
0026   if all(sz==[3 3]),
0027     R2=R1(2,:)'; R3=R1(3,:)'; R1=R1(1,:)';
0028   elseif any(sz>3)|any(sz<2)
0029     error('  Input matrix must be 3 by 3 or 3 by 2')
0030   end
0031 end
0032 if nargin==2
0033   error('  Must be 1 or 3 input arguments.')
0034 end
0035 if nargin==3
0036   sz = [size(R1); size(R2); size(R3)];
0037   if any(diff(prod(sz')))    % Check sizes
0038     error('  Arguments R1,R2,R3 must have the same size')
0039   end
0040   is3 = all(any(sz'==3));
0041   is2 = all(any(sz'==2));
0042   if is3
0043     nn = find(sz(:,1)==3);
0044     for jj = 1:3
0045       nn = find(sz(jj,:)==3);
0046       if nn(1)>1
0047         eval(['R' num2str(jj) '=R' num2str(jj) ''';']);
0048       end
0049     end
0050   elseif is2
0051   else, error(' Input matrices must be N by 3')
0052   end
0053 end
0054 
0055 
0056  % Auxillary ...............
0057 o3 = ones(3,1);
0058 
0059  % Lengths of all vectors
0060 normal12 = [sqrt(sum(R1.^2)); sqrt(sum(R2.^2)); sqrt(sum(R3.^2))];
0061 numnull = find(~(any(normal12))); % Find vectors with zero length
0062 normal12(:,numnull) = ones(3,length(numnull));
0063 
0064  % Make all vectors to have unit length
0065 a23 = normal12(1,:);
0066 R1 = R1./a23(o3,:);
0067 R2 = R2./normal12(2*o3,:);
0068 R3 = R3./normal12(3*o3,:);
0069 
0070  % Find normals
0071 normal12 = cross(R1,R2);         % Cross product
0072 ln12 = sqrt(sum(normal12.^2));   % Length of normal vectors
0073 normal12 = normal12./ln12(o3,:); % Make normal unit length
0074 normal13 = cross(R1,R3);         %   The same for R1,R3
0075 ln13 = sqrt(sum(normal13.^2));
0076 normal13 = normal13./ln13(o3,:);
0077 normal23 = cross(R2,R3);         %   The same for R2,R3
0078 ln23 = sqrt(sum(normal23.^2));
0079 normal23 = normal23./ln23(o3,:);
0080 
0081 
0082  % Angles between planes
0083 ang1 = acos(abs(sum(normal12.*normal13)));  % (0,1,2),(0,1,3)
0084 ang2 = acos(abs(sum(normal12.*normal23)));  % (0,1,2),(0,2,3)
0085 numnorm = find(ang1+ang2>=pi-tol);
0086 
0087  % Calculate projection of R3 into plane (0,R1,R2)
0088 prjn = sum(R3.*normal12);        % Scalar product with normal
0089 R3prj = R3-normal12.*prjn(o3,:); % Projection into (0,1,2) plane
0090 a12 = sqrt(sum(R3prj.^2));
0091 a12(numnorm) = ones(size(numnorm));
0092 R3prj = R3prj./a12(o3,:);
0093 
0094  % Plane angles in the (0,1,2) plane
0095 a12 = acos(sum(R1.*R2));      % Between (0,1) and (0,2)
0096 a13 = acos(sum(R1.*R3prj));      % (0,1) and (0,3prj)
0097 a23 = acos(sum(R2.*R3prj));      % (0,2) and (0,3prj)
0098 
0099  % Partial body angles .......
0100 ba = a13>pi/2;  % Is span more than pi/2
0101 ba1 = abs(2*ang1.*ba-asin(sin(ang1).*sin(a13)));
0102 ba = a23>pi/2;  % Is span more than pi/2
0103 ba2 = abs(2*ang1.*ba-asin(sin(ang2).*sin(a23)));
0104 
0105 
0106  % Find if projection of R3 is between  R1 and R2 (ba = ba1+ba2)
0107  % or outside (ba = |ba1-ba2|)
0108 ln13 = (a12+a13+a23)>=(2*pi-tol);  % If sum of 3 angles is 2*pi
0109 ln12 = ~ln13&(a13>a12|a23>a12);
0110 ln12 = ln12*2-1;
0111 
0112  % Composite body angle ......
0113 ba = abs(ba1-ba2.*ln12);      % ba1+ba2 or |ba1-ba2|
0114 ba(ln13) = 2*pi-ba(ln13);     % When encircling the North pole
0115 
0116  % Special cases ..............
0117 ba(numnorm) = a12(numnorm);   % When R3 is normal to (0,R1,R2)
0118 ba(numnull) = ba(numnull)*null_value;  % Zero-length vectors