matlab練習程序（求向量間的旋轉矩陣與四元數）

問題是這樣，若是咱們知道兩個向量v1和v2，計算從v1轉到v2的旋轉矩陣和四元數，因爲旋轉矩陣和四元數能夠互轉，因此咱們先計算四元數。

咱們能夠認爲v1繞着向量u旋轉θ​角度到v2，u垂直於v1-v2平面。

四元數q能夠表示爲cos(θ/2)​+sin(θ/2)​u，即：q0​=cos(θ/2)​，q1​=sin(θ/2)​u.x，q2=sin(θ/2)​u.y，q3=sin(θ/2)​u.z

因此咱們求出u和θ/2便可，u等於v1與v2的叉積，不要忘了單位化；θ/2用向量夾角公式就能求。

matlab代碼以下：

clear all;
close all;
clc;

v1=[1 2 3];
v2=[4 5 6];

%轉爲單位向量
nv1 = v1/norm(v1);
nv2 = v2/norm(v2);

if norm(nv1+nv2)==0
    q = [0 0 0 0];
else
    u = cross(nv1,nv2);         
    u = u/norm(u);
    
    theta = acos(sum(nv1.*nv2))/2;
    q = [cos(theta) sin(theta)*u];
end

%由四元數構造旋轉矩陣
R=[2*q(1).^2-1+2*q(2)^2  2*(q(2)*q(3)+q(1)*q(4)) 2*(q(2)*q(4)-q(1)*q(3));
    2*(q(2)*q(3)-q(1)*q(4)) 2*q(1)^2-1+2*q(3)^2 2*(q(3)*q(4)+q(1)*q(2));
    2*(q(2)*q(4)+q(1)*q(3)) 2*(q(3)*q(4)-q(1)*q(2)) 2*q(1)^2-1+2*q(4)^2];

s = nv1*R;

%顯示結果
v2
s*norm(v2)

參考：

https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another

https://blog.csdn.net/shenshikexmu/article/details/70991286

http://www.javashuo.com/article/p-njvrtihw-hp.html