旋转矩阵到四元数源代码新

从旋转矩阵计算得到四元数，代码简单，内附word说明文档。如有不当之处还请多多指正。

function [q0 q1 q2 q3]=RMtoFE（T）
%% Be sure T is a rotation matrix
[m n]=size（T）;
if （m~=3||n~=3）
    error（‘Wrong Rotation matrix‘）;
end
E=1.0e-15;
q0=sqrt（T（11）+T（22）+T（33）+1）/2;
% to avoid  0 divisor
if q0
    q1=（T（32）-T（23））/4/q0;
    q2=（T（13）-T（31））/4/q0;
    q3=（T（21）-T（12））/4/q0;
else
    if （T（12）*T（13））^2+（T（12）*T（23））^2+（T（13）*T（23））^2
        MidValue=sqrt（（T（12）*T（13））^2+（T（12）*T（23））^2+（T（13）*T（23））^2）;
        q1=T（12）*T（13）/MidValue;
        q2=T（12）*T（23）/MidValue;
        q3=T（13）*T（13）/MidValue;
    else
        if T（12）            if T（13）                if T（11）>1-E
                    q1=1;q3=0;q2=0;
                else
                    q1=0;
                    q3=sqrt（（1-T（22））/2）;
                

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       1218  2011-03-09 10:03  旋转矩阵与四元数\RMtoFE.m

     文件      48128  2011-03-09 09:50  旋转矩阵与四元数\四元数.doc

     目录          0  2011-03-09 10:08  旋转矩阵与四元数

----------- ---------  ---------- -----  ----

                49346                    3