matlab中quat2angle,cord2quat.m

function q = cord2quat(d,dPrime)

% CORD2QUAT 根据旋转前后对应点的坐标(仅存在旋转)，求四元素

% d 是起始坐标系

% dPrime 是旋转后的坐标系

% reference：

% [1] https://blog.csdn.net/u013517182/article/details/53945005

%

% d1 = [1, 0, 0]; %X

% d2 = [0, 1, 0]; %Y

% d3 = [0, 0, 1]; %Z

%

% d1Prime = [0, 1, 0]; %x

% d2Prime = [-1, 0, 0]; %y

% d3Prime = [0, 0, 1]; %z

d1 = d(1,:); %X

d2 = d(2,:); %Y

d3 = d(3,:); %Z

d1Prime = dPrime(1,:); %x

d2Prime = dPrime(2,:); %y

d3Prime = dPrime(3,:); %z

A1 = [ 0 , (d1Prime-d1) ;

(d1-d1Prime)' , crossProductMatrix(d1Prime+d1)' ];

A2 = [ 0 , (d2Prime-d2) ;

(d2-d2Prime)' , crossProductMatrix(d2Prime+d2)' ];

A3 = [ 0 , (d3Prime-d3) ;

(d3-d3Prim