Path: news.mathworks.com!not-for-mail
From: "Bruno Luong" <brunoluong@yahoo.com>
Newsgroups: comp.soft-sys.matlab
Subject: Re: angle from rotation matrix
Date: Wed, 19 Dec 2007 19:39:25 +0000 (UTC)
Organization: The MathWorks, Inc.
Lines: 33
Message-ID: <fkbs1d$25q$1@fred.mathworks.com>
References: <fjvbqk$gbt$1@fred.mathworks.com> <fk0cpd$g0v$1@fred.mathworks.com> <fk0iq9$k0j$1@fred.mathworks.com> <fk59hv$n5r$1@fred.mathworks.com> <2shcm3pkforks50k3jjkjo0bdkioir9jq2@4ax.com> <fk6eis$ra6$1@fred.mathworks.com> <fk6ief$3kq$1@fred.mathworks.com> <fk6q3s$dns$1@fred.mathworks.com> <fk6vf6$9gt$1@fred.mathworks.com> <1dtem3965vls6097t13ven16m2keemmsqb@4ax.com> <fk7vji$9is$1@fred.mathworks.com> <fkbqtl$at5$1@fred.mathworks.com>
Reply-To: "Bruno Luong" <brunoluong@yahoo.com>
NNTP-Posting-Host: webapp-02-blr.mathworks.com
Content-Type: text/plain; charset="ISO-8859-1"
Content-Transfer-Encoding: 8bit
X-Trace: fred.mathworks.com 1198093165 2234 172.30.248.37 (19 Dec 2007 19:39:25 GMT)
X-Complaints-To: news@mathworks.com
NNTP-Posting-Date: Wed, 19 Dec 2007 19:39:25 +0000 (UTC)
X-Newsreader: MATLAB Central Newsreader 1184112
Xref: news.mathworks.com comp.soft-sys.matlab:443066


"Bruno Luong" <brunoluong@yahoo.com> wrote in message 
>     u2=(sum((A+A'-t*I).^2,2)+((4*d-2*t)+1))+uR2;

Opps, "I" is *of course* the Identity matrix. Let's try it
again.

Bruno

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

d=diag(A);
t=sum(d);
c=t-1;
uR=[A(3,2)-A(2,3);...
    A(1,3)-A(3,1); ...
    A(2,1)-A(1,2)];
uR2=uR.^2;
s2=sum(uR2);
a=(3-t)+s2;
if a>0
    theta=atan2(sqrt(s2),c)
    % Mess parenthesis at your own risk...
    u2=(sum((A+A'-t*eye(3)).^2,2)+((4*d-2*t)+1))+uR2;
    absu=sqrt(u2.*(u2>0))/sqrt(a);
    signu=sign(uR);
    signu(signu==0)=1; % we don't want the nil case
    u=absu.*signu;
    u=u/norm(u)
else
    warning('Rotation matrix close to indentity');
    u=[1 0 0]'
    theta=0
end