Home > @quaternion > quaternion.m

quaternion

PURPOSE ^

QUATERNION Constructor for quaternion objects

SYNOPSIS ^

function q = quaternion(a1, a2)

DESCRIPTION ^

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

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SUBFUNCTIONS ^

SOURCE CODE ^

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

Generated on Sun 15-Feb-2009 18:09:29 by m2html © 2003