四元数实现三维绕轴旋转

  前段时间要用到空间旋转,所以看了一下四元数方面的资料。顺便就做了一个三维绕轴旋转的测试小程序。
参考了一下绕任意空间轴旋转三维图形
  那篇博客里面是采用的罗德里格旋转的算法实现的。
  这里采用四元数,另外,采用了动态的显示,以及画出了一个旋转轴,方便观察。
  需要说明的是选择旋转轴经过的点时需在区间内选择。
  下面是主程序的matlab代码:

clear all;
close all;
% 随机生成转轴通过的点(需确保在xyz所包围的立方体内部)
origin=rand(1,3)*5;
% 随机生成转轴方向
direct=rand(1,3);
% direct=[1 0 0];
% 随机生成旋转角度
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);
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

四元数部分的代码未优化。
下面是一个效果图。
四元数实现三维绕轴旋转

上一篇:matlab-极坐标图和对数坐标图的绘制


下一篇:百度地图坐标、Google坐标、腾讯坐标相互转化