四元数实现三维绕轴旋转

  前段时间要用到空间旋转，所以看了一下四元数方面的资料。顺便就做了一个三维绕轴旋转的测试小程序。
参考了一下绕任意空间轴旋转三维图形
  那篇博客里面是采用的罗德里格旋转的算法实现的。
  这里采用四元数，另外，采用了动态的显示，以及画出了一个旋转轴，方便观察。
四元数的资料可以参考下面的文章
四元数
  写的很不错，另外，秦永元的《惯性导航》里面也写的很清楚。
  需要说明的是选择旋转轴经过的点时需在区间内选择。
  下面是主程序的matlab代码：

clear all;
close all;
% 随机生成转轴通过的点(需确保在xyz所包围的立方体内部)
origin=rand(1,3)*5;
% origin=[5 5 0];
% 随机生成转轴方向
direct=rand(1,3);
% direct=[0 0 1];
% 随机生成旋转角度
theta=rand*5;
[x,y,z]=peaks;

% 图像显示的立方体边界
xmin=-10;xmax=20;ymin=-10;ymax=20;zmin=-10;zmax=20;
xyz=[xmin xmax;ymin ymax;zmin zmax];
directZero=find(~abs(direct));   %找出等于零的元素索引
directNum=length(directZero);
dotCros=NaN(6,3);

%     运用直线的两点式方程确定旋转轴与显示六面体的六个面的六个交点
if(directNum==3)
    error('Not valid vector.');   
else
    for k=1:3
        tCrossMin=(xyz(k,1)-origin(k))/direct(k);
        dotCros((k-1)*2+1,:)=[tCrossMin*direct(1)+origin(1) tCrossMin*direct(2)+origin(2) tCrossMin*direct(3)+origin(3)];

        tCrossMax=(xyz(k,2)-origin(k))/direct(k);
        dotCros((k-1)*2+2,:)=[tCrossMax*direct(1)+origin(1) tCrossMax*direct(2)+origin(2) tCrossMax*direct(3)+origin(3)];
    end
end
% 确定六个点里面的直线与六面体的两个交点,即旋转轴的端点
lineDot=zeros(2,3);
% 数值的精度
dataRoundNum=3;
xyz=round(xyz,dataRoundNum);
dotCros=round(dotCros,3);
s=1;
for k=1:6
    if(xyz(1,1)<=dotCros(k,1) && dotCros(k,1)<=xyz(1,2) && xyz(2,1)<=dotCros(k,2)...
            && dotCros(k,2)<=xyz(2,2) && xyz(3,1)<=dotCros(k,3) && dotCros(k,3)<=xyz(3,2))
        lineDot(s,:)=dotCros(k,:);
        s=s+1;
        if(s>2)
            break;
        end
    end
end

set(gcf,'name','旋转显示','Numbertitle','off');
plot3([lineDot(1,1) lineDot(2,1)],[lineDot(1,2) lineDot(2,2)],[lineDot(1,3) lineDot(2,3)],'r');
ax1=gca;ax1.BoxStyle='full';box on;
axis([xmin xmax ymin ymax zmin zmax]);
title('四元数绕轴旋转')
hold on;
fig=mesh(x,y,z);
hold off;

P=[x(:),y(:),z(:)];
for k=1:500
%     Pr=rot3d(P,origin,direct,(theta+k*0.5));
    Pr=rot3dQuad(P,origin,direct,theta+k*0.1);  %四元数旋转函数
    xr=reshape(Pr(:,1),size(x));
    yr=reshape(Pr(:,2),size(x));
    zr=reshape(Pr(:,3),size(x));
    hold on;
    fig.delete;
    fig=mesh(xr,yr,zr);
    hold off;
    pause(0.1);
end

这里也可以调用上面提到的博客里面的旋转函数rot3d。下面是基于四元数旋转的matlab函数。

function Pr=rot3dQuad(P,origin,direct,theta)
% 将坐标点P绕着，过origin点，方向为dirct的直线，旋转theta角
% P：需要旋转的做标集合，n×3矩阵
% origin：转轴通过的点，1×3向量
% direct：转轴方向向量，1×3向量
% theta：旋转角度，单位弧度
Pr=zeros(size(P));
direct=direct(:)/norm(direct);
q1=quaternion(cos(theta/2),sin(theta/2)*direct(1),sin(theta/2)*direct(2),sin(theta/2)*direct(3));
originQuad=quaternion(0,origin(1),origin(2),origin(3));
for k=1:size(P,1)
    q2=quaternion(0,P(k,1),P(k,2),P(k,3));
    q=q1*(q2-originQuad)*q1'+originQuad;
    [~,b,c,d]=parts(q);
    Pr(k,:)=[b c d];
end

四元数部分的代码未优化。
下面是一个效果图。

附注1：
昨天发现在某些情况下旋转轴计算错误的情况，原因是在下面的判断中

    if(xyz(1,1)<=dotCros(k,1) && dotCros(k,1)<=xyz(1,2) && xyz(2,1)<=dotCros(k,2)...
            && dotCros(k,2)<=xyz(2,2) && xyz(3,1)<=dotCros(k,3) && dotCros(k,3)<=xyz(3,2))

由于计算精度的问题，会出现交点某一轴的坐标和其上下界相差极小的情况，比如，上界是10.0，而交点由于计算误差为

10.000000000000002

所以上面的程序进行了修正，比之前增加了下面几行：

% 数值的精度
dataRoundNum=3;
xyz=round(xyz,dataRoundNum);
dotCros=round(dotCros,3);

表示精确到小数点后三位。