QUATERNION Constructor for quaternion objects Q = QUATERNION([s v1 v2 v3]) from 4 elements Q = QUATERNION([s v1 v2 v3]) from 4 elements Q = QUATERNION(v, theta) from vector plus angle Q = QUATERNION(R) from a 3x3 or 4x4 matrix Q = QUATERNION(q) from another quaternion Q = QUATERNION(s) from a scalar Q = QUATERNION(v) from a vector All versions, except the first, are guaranteed to return a unit quaternion. A quaternion is a compact method of representing a 3D rotation that has computational advantages including speed and numerical robustness. A quaternion has 2 parts, a scalar s, and a vector v and is typically written q = s <vx vy vz> A unit quaternion is one for which s^2+vx^2+vy^2+vz^2 = 1. A quaternion can be considered as a rotation about a vector in space where q = cos (theta/2) sin(theta/2) <vx vy vz> where <vx vy vz> is a unit vector. Various functions such as INV, NORM, UNIT and PLOT are overloaded for quaternion objects. Arithmetic operators are also overloaded to allow quaternion multiplication, division, exponentiaton, and quaternion-vector multiplication (rotation). SEE ALSO: QUATERNION/SUBSREF, QUATERNION/PLOT
0001 %QUATERNION Constructor for quaternion objects 0002 % 0003 % Q = QUATERNION([s v1 v2 v3]) from 4 elements 0004 % Q = QUATERNION([s v1 v2 v3]) from 4 elements 0005 % Q = QUATERNION(v, theta) from vector plus angle 0006 % Q = QUATERNION(R) from a 3x3 or 4x4 matrix 0007 % Q = QUATERNION(q) from another quaternion 0008 % Q = QUATERNION(s) from a scalar 0009 % Q = QUATERNION(v) from a vector 0010 % 0011 % All versions, except the first, are guaranteed to return a unit quaternion. 0012 % 0013 % A quaternion is a compact method of representing a 3D rotation that has 0014 % computational advantages including speed and numerical robustness. 0015 % 0016 % A quaternion has 2 parts, a scalar s, and a vector v and is typically written 0017 % 0018 % q = s <vx vy vz> 0019 % 0020 % A unit quaternion is one for which s^2+vx^2+vy^2+vz^2 = 1. 0021 % 0022 % A quaternion can be considered as a rotation about a vector in space where 0023 % q = cos (theta/2) sin(theta/2) <vx vy vz> 0024 % where <vx vy vz> is a unit vector. 0025 % 0026 % Various functions such as INV, NORM, UNIT and PLOT are overloaded for 0027 % quaternion objects. 0028 % 0029 % Arithmetic operators are also overloaded to allow quaternion multiplication, 0030 % division, exponentiaton, and quaternion-vector multiplication (rotation). 0031 % 0032 % SEE ALSO: QUATERNION/SUBSREF, QUATERNION/PLOT 0033 0034 % Copyright (C) 1999-2008, by Peter I. Corke 0035 % 0036 % This file is part of The Robotics Toolbox for Matlab (RTB). 0037 % 0038 % RTB is free software: you can redistribute it and/or modify 0039 % it under the terms of the GNU Lesser General Public License as published by 0040 % the Free Software Foundation, either version 3 of the License, or 0041 % (at your option) any later version. 0042 % 0043 % RTB is distributed in the hope that it will be useful, 0044 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0045 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0046 % GNU Lesser General Public License for more details. 0047 % 0048 % You should have received a copy of the GNU Leser General Public License 0049 % along with RTB. If not, see <http://www.gnu.org/licenses/>. 0050 0051 0052 function q = quaternion(a1, a2) 0053 0054 0055 if nargin == 0, 0056 q.v = []; 0057 q.s = []; 0058 q = class(q, 'quaternion'); 0059 elseif isa(a1, 'quaternion') 0060 % Q = QUATERNION(q) from another quaternion 0061 q = a1; 0062 elseif nargin == 1 0063 if all(size(a1) == [3 3]) 0064 % Q = QUATERNION(R) from a 3x3 or 4x4 matrix 0065 q = quaternion( tr2q(a1) ); 0066 elseif all(size(a1) == [4 4]) 0067 q = quaternion( tr2q(a1(1:3,1:3)) ); 0068 elseif all(size(a1) == [1 4]) | all(size(a1) == [4 1]) 0069 % Q = QUATERNION([s v1 v2 v3]) from 4 elements 0070 a1 = a1(:); 0071 q.s = a1(1); 0072 q.v = a1(2:4)'; 0073 q = class(q, 'quaternion'); 0074 elseif length(a1) == 3, 0075 % Q = QUATERNION(v) from a vector 0076 0077 q.s = 0; 0078 q.v = a1(:)'; 0079 q = class(q, 'quaternion'); 0080 elseif length(a1) == 1, 0081 % Q = QUATERNION(s) from a scalar 0082 q.s = a1(1); 0083 q.v = [0 0 0]; 0084 q = class(q, 'quaternion'); 0085 else 0086 error('unknown dimension of input'); 0087 end 0088 elseif nargin == 2 0089 % Q = QUATERNION(v, theta) from vector plus angle 0090 q = unit( quaternion( [cos(a2/2) sin(a2/2)*unit(a1(:).')]) ); 0091 end 0092 0093 %TR2Q Convert homogeneous transform to a unit-quaternion 0094 % 0095 % Q = tr2q(T) 0096 % 0097 % Return a unit quaternion corresponding to the rotational part of the 0098 % homogeneous transform T. 0099 % 0100 % See also: Q2TR 0101 0102 % Copyright (C) 1993 Peter Corke 0103 function q = tr2q(t) 0104 qs = sqrt(trace(t)+1)/2.0; 0105 kx = t(3,2) - t(2,3); % Oz - Ay 0106 ky = t(1,3) - t(3,1); % Ax - Nz 0107 kz = t(2,1) - t(1,2); % Ny - Ox 0108 0109 if (t(1,1) >= t(2,2)) & (t(1,1) >= t(3,3)) 0110 kx1 = t(1,1) - t(2,2) - t(3,3) + 1; % Nx - Oy - Az + 1 0111 ky1 = t(2,1) + t(1,2); % Ny + Ox 0112 kz1 = t(3,1) + t(1,3); % Nz + Ax 0113 add = (kx >= 0); 0114 elseif (t(2,2) >= t(3,3)) 0115 kx1 = t(2,1) + t(1,2); % Ny + Ox 0116 ky1 = t(2,2) - t(1,1) - t(3,3) + 1; % Oy - Nx - Az + 1 0117 kz1 = t(3,2) + t(2,3); % Oz + Ay 0118 add = (ky >= 0); 0119 else 0120 kx1 = t(3,1) + t(1,3); % Nz + Ax 0121 ky1 = t(3,2) + t(2,3); % Oz + Ay 0122 kz1 = t(3,3) - t(1,1) - t(2,2) + 1; % Az - Nx - Oy + 1 0123 add = (kz >= 0); 0124 end 0125 0126 if add 0127 kx = kx + kx1; 0128 ky = ky + ky1; 0129 kz = kz + kz1; 0130 else 0131 kx = kx - kx1; 0132 ky = ky - ky1; 0133 kz = kz - kz1; 0134 end 0135 nm = norm([kx ky kz]); 0136 if nm == 0, 0137 q = quaternion([1 0 0 0]); 0138 else 0139 s = sqrt(1 - qs^2) / nm; 0140 qv = s*[kx ky kz]; 0141 0142 q = quaternion([qs qv]); 0143 0144 end