root/library/utia_legacy/ticket_12/ldform.m @ 777

function [LD,D]=ldform(AD,D0)
% transform general decomposition A 'D0 A into L'DL decomposition with 
% lower triangular(trapeziod) L  with unites on diagonal by Householder transformation.
% Both input and output can be given either in a "packed form" 
% as one matrix AD resp. LD  or as a pair of matrices (A,D0) resp. (L,D). 
% The new decomposition fullfills: A'*D0*A = L'*D*L
%
% [L, D] = ldform(A,D0)
% [L, D] = ldform(AD)     ( [A,D0]=ld2ld(AD) )
% [LD]   = ldform(A,D0)
% [LD]   = ldform(AD)     ( [A,D0]=ld2ld(AD) )
%
% AD   = an arbitrary dimension matrix with squares of row weights on its
%        diagonal.
% A    = an arbitrary dimension matrix
% D0   = diagonal matrix containing squares of weights of corresponding
%        rows of L0
% L   = lower triangular (trapeziod) matrix normalized to unit diagonal
% D   = diagonal matrix
% LD  = lower triangular matrix with unit diagonal replaced by diagonal 
%       D 
%
%       -----            --
%      |     |   -->    |   \
%      |     |          |    \
%      ------           ------
%
% Design : J. Bohm
% Project: post ProDaCTools

% Calls   : ld2ld

if(nargin<1) error('At least one input must be given'); end;

[m,n]=size(AD);

if(nargin<2)
    if(m~=n)
        error('packed input has sense only for square matrices');
    end;
    [A,D0]=ld2ld(AD);
else
    A=AD;
end;

if((nargout==1)&(m~=n))
        error('packed output has sense only for square matrices');
end;

if(size(D0,1)~=m) error('matrix D0 must have the same row count as A'); end;
    
v=zeros(1,m);

dd=sqrt(D0);
L=dd*A;
D=D0;
mn=min(m,n);

for i=n:-1:n-mn+1,
    v=zeros(1,m);   
    sum=L(1:m-n+i,i)'*L(1:m-n+i,i);
    if(L(m-n+i,i)==0)
        beta=sqrt(sum);
    else
        beta= L(m-n+i,i)+sign(L(m-n+i,i))*sqrt(sum);
    end;

    if abs(beta)<eps,D(m-n+i,m-n+i)=0;L(m-n+i,i)=1;continue; end
    
    v(1:m-n+i-1)=L(1:m-n+i-1,i)/beta;
    v(m-n+i)=1;
    w=zeros(1,n);
    sum=v(1:m-n+i)*v(1:m-n+i)';
    
    pom=-2/sum;
    for j=i:-1:1
        w(j)=v(1:m-n+i)*L(1:m-n+i,j)*pom;
    end
   

    L(1:m-n+i,1:i-1)=L(1:m-n+i,1:i-1)+v(1:m-n+i)'*w(1:i-1);
    L(1:m-n+i-1,i)=0;
    L(m-n+i,i)=L(m-n+i,i)+v(m-n+i)*w(i);
    
    D(m-n+i,m-n+i)=L(m-n+i,i)^2;
    L(m-n+i,:)= L(m-n+i,:)/L(m-n+i,i);
end

for(i=1:m-n)
    D(i,i)=0;
end;

if(nargout==1)
    LD=ld2ld(L,D);
else
    LD=L;
end;