MATLAB 四元数旋转函数

%四元数旋转法，输入theta是旋转角度（右手系），v是旋转轴矢量，u是原始点位置矢量，输出U是旋转后的点位置矢量
function U=QuaternionRotate(theta,v,u)
    v=v./sqrt(sum(v.*v));%归一化
    q=[cos(theta/2),v(1)*sin(theta/2),v(2)*sin(theta/2),v(3)*sin(theta/2)];
    qt=quatconj(q);%取共轭
    w=[0,u];%纯四元数
    W=quatmultiply(quatmultiply(q,w),qt);%调用四元数乘法计算公式
    U=W(2:4);%取后三位
end

以下是测试：

将x轴上的(1,0,0)点，绕xy平面对角线旋转180度，就转到了y轴上的(0,1,0)上去了。

将x轴上的(1,0,0)点，绕xy平面对角线旋转90度，就是上一个测试转了一半，x为0.5，y也为0.5。旋转角度与旋转轴是右手螺旋关系，因此点旋转到xoy平面下方去了，向量长度保持是1，因此z为负二分之一根号二。

以上实验验证，四元数法可以将三维空间内一个点沿着指定旋转轴右手螺旋旋转特定角度。

注意matlab中若要使用自带的四元数运算函数，需要安装Aerospace Toolbox工具箱。如果没有安装，运行代码时会自动安装（登录后一路确定）。

四元数法原理：
https://www.zhihu.com/question/23005815/answer/483589712