function [out,out1,out2]=rigid(CAM,varargin);

%RIGID	non-standard element function for the hanling of linearized
%	rigid links
%
%	Syntax : [T,cdof] = rigid(node,elt,mdof)
%                [T,cdof] = rigid(Up)
%
%	RIGID elements are used for a direct elimination of linearized rigid
%       links. A penalization approach is implemented with CELAS elements.
%
%	When using the input structure Up (see UPCOM), Up.Elt is used unless
%       ELT is given, Up.DOF is used unless mdof is given. If coordinate 
%       systems are defined in field Up.bas, PID (position coordinate system) 
%       and DID (displacement coordinate system) are properly handled.
%
%	RIGID elements are ignored during assembly by FE_MK. For the resulting
%	model [m,k,mdof], a basis of the subspace verifying 
%       the constraints is generated using [T,cdof] = rigid(node,elt,mdof) 
%	where you can leave all elements.
%	The constrained model is given by mc=T'*m*T; kc=T'*k*T which are
%	associated to the DOFs cdof. To go back to initial coordinates use
%	the relation  q = T*qc.
%
%	Rigid element property rows follow the format
%         [n1 n2 DofSel 0 0 EltID]
%       where
%	 a DofSel of 123 links only translations, 123456 all DOFs, etc.
%	 A negative DofSel indicates a DOF to DOF connection where the
%	 appropriate DOFs of n2 are eliminated. 
%
%	Warning : when seeking rigid rotations, be sure that the master node
%	  n1 has rotational DOFs. 
%
%	See also help celas, bar1, beam1, ...
%                doc  eltfun, elem0

%	Etienne Balmes   03/22/97, 28/08/02
%       Copyright (c) 2001-2002 by INRIA and SDTools
%       Use under OpenFEM trademark.html license and LGPL.txt library license
%       All Rights Reserved.

if comstr(CAM,'cvs')
 out='$Revision: 1.10 $  $Date: 2006/01/27 14:28:50 $'; return;
end
% standard calls with one input argument
if ischar(CAM)

 [CAM,Cam]=comstr(CAM,1);
 if comstr(Cam,'integinfo')

   out=[];out1=[];out2=[];

 elseif  comstr(Cam,'groupinit');   out = '';
 elseif comstr(Cam,'matcall');   out='';out1='';
 elseif comstr(Cam,'call');   out='';
 elseif comstr(Cam,'node'); out = [1:2];
 elseif comstr(Cam,'prop'); out = [0 0 6];
 elseif comstr(Cam,'dof')
   out =[1+[1:6]/100 2+[1:6]/100]';
 elseif comstr(Cam,'line');  out = [1 2];
 elseif comstr(Cam,'patch'); out = [1 2];
 elseif  comstr(Cam,'face'); out=[];
 elseif comstr(Cam,'sci_face'); out = [1 2 2];
 elseif  comstr(Cam,'edge'); out=[1 2];
 elseif  comstr(Cam,'parent')
   out = 'beam1';
 end
return
end % of standard calls with one input argument

% building the rigid matrix
node=CAM;elt=varargin{1};

cGL=[]; 
if ~isstruct(node) 
 Up=[]; mdof=varargin{2};
else
 Up=node; 
 if ~isfield(Up,'Node'); error('First input must give nodes'); end  
 if isfield(Up,'bas')&~isempty(Up.bas)
  [node,bas]=basis(Up.Node,Up.bas);
  cGL=basis('trans l',bas,Up.Node);
 else; node=Up.Node; end
 if (nargin<2 | isempty(elt))&isfield(Up,'Elt'); elt=Up.Elt; end
 if nargin<3 & isfield(Up,'DOF')&~isempty(Up.DOF); mdof=Up.DOF; 
 else; mdof=feutil('getdof',Up); 
 end
end

[EGroup,nGroup]=getegroup(elt);
NNode(node(:,1))=[1:length(node(:,1))]';
T = speye(length(mdof),length(mdof));
ind = round(rem(mdof,1)*100);ind=find(ind>0&ind<7);
nim = 6;
if isempty(ind) % no mechanical DOF

else % some translation DOFs

nind=sparse(fix(mdof(ind))*nim+round(rem(mdof(ind),1)*100)-1,1, ...
              ind,ceil(max(mdof(ind))*nim+nim),1);

cEGI=[];for jGroup = 1:nGroup %loop on element groups
   [ElemF,i1]= feutil('getelemf',elt(EGroup(jGroup),:),jGroup);
   if comstr(ElemF,'rigid') & (length(i1)==0 | i1(1)>=0 )
    cEGI=[cEGI EGroup(jGroup)+1:EGroup(jGroup+1)-1];
   end
end

% find repeated slave nodes
i2=elt(cEGI,2);i2=i2(:);
i2=find(sparse(i2+1,1,ones(size(i2,1),size(i2,2)))>1)-1;i2=i2(find(i2));
if ~isempty(i2)
  st=sprintf('%i ',i2);
  if length(i2)==1; warning(['Slave node' st ' is repeated']);
  else  warning(['Slave nodes' st ' are repeated']); end
end

% build the constraint matrix 

for j1=cEGI
      i2 = elt(j1,3);i2=abs(sprintf('%i',elt(j1,3)))-48;
      %i2=[rem(i2,10) rem(fix(i2/10),10) rem(fix(i2/100),10) ...
      %   rem(fix(i2/1e3),10) rem(fix(i2/1e4),10) rem(fix(i2/1e5),10)];
      %i2 = find(sparse(abs(i2)+1,1,i2))-1; 

      if elt(j1,3)>0
       i3=elt(j1,1)*nim+[0:5];   if max(i3)>length(nind); nind(max(i3))=0;end
       i3=full(nind(i3));i3i=find(i3);
       x = node(NNode(elt(j1,[1 2])),5:7);x=x(2,:)-x(1,:);
       x = [0 x(3) -x(2) ;-x(3) 0 x(1);x(2) -x(1) 0];
       r = sparse([eye(3,3) x;zeros(3,3) eye(3,3)]);

       i4=elt(j1,2)*nim+i2-1;   if max(i4)>length(nind); nind(max(i4))=0;end
       i4=full(nind(i4));i4i=find(i4);
       if length(i2)>length(i3i)
           warning(sprintf('Missing DOFs in Master node %i',elt(j1,1)));
       elseif ~any(any(T(:,i3(i3i))))
           warning(sprintf('Master node is fixed %i %i %i',elt(j1,[1 2 3])));
       end
       T(:,i4(i4i))=T(:,i3(i3i))*r(i2(i4i),i3i)';
       if any(any(T(i4(i4i,:))))
           warning(sprintf('Slave node problem %i %i %i',elt(j1,[1 2 3])));
       end
      else %negative value for direct DOF match
         i3=elt(j1,1)*nim+i2-1;   if max(i3)>length(nind); nind(max(i3))=0;end
         i3=full(nind(i3));i3i=find(i3);
         i4=elt(j1,2)*nim+i2-1;   if max(i4)>length(nind); nind(max(i4))=0;end
         i4=full(nind(i4));i4i=find(i4);
         if ~any(any(T(:,i3(i3i))))
           warning(sprintf('Master node is fixed %i %i %i',elt(j1,[1 2 3])));
         end
         T(:,i4(i4i))=T(:,i3(i3i));
         if any(any(T(i4(i4i),:)))
           warning(sprintf('Slave node problem %i %i %i',elt(j1,[1 2 3])));
         end
      end
end % of loop on rigid elements
end % there are mechanical DOFs

T=T';i1=find(any(T));T=T(:,i1);cdof=mdof(i1);
if ~isempty(cGL);  T=cGL(i1,i1)'*T*cGL(i1,i1); end


if isstruct(Up)&nargout<2
  T=struct('Stack',[],'T',T,'DOF',cdof);
end
out=T;out1=cdof;
