11 lines
422 B
Matlab
11 lines
422 B
Matlab
|
function q = qmul(q1, q2)
|
||
|
|
% 四元数相乘
|
||
|
|
%
|
||
|
|
% Inputs: Q1 Q2, 四元数和矩阵一样，不满足交换律
|
||
|
|
% Outputs: Q
|
||
|
|
%
|
||
|
|
q = [ q1(1) * q2(1) - q1(2) * q2(2) - q1(3) * q2(3) - q1(4) * q2(4);
|
||
|
|
q1(1) * q2(2) + q1(2) * q2(1) + q1(3) * q2(4) - q1(4) * q2(3);
|
||
|
|
q1(1) * q2(3) + q1(3) * q2(1) + q1(4) * q2(2) - q1(2) * q2(4);
|
||
|
|
q1(1) * q2(4) + q1(4) * q2(1) + q1(2) * q2(3) - q1(3) * q2(2) ];
|