Revision 033067c96b8ba4e9c800c9a04310cfa05ceafe2f authored by Alec Jacobson on 03 September 2020, 17:46:41 UTC, committed by GitHub on 03 September 2020, 17:46:41 UTC
exposed admm tolerances
quatmultiply.m
function qout = quatmultiply( q, varargin )
% QUATMULTIPLY Calculate the product of two quaternions.
% N = QUATMULTIPLY( Q, R ) calculates the quaternion product, N, for two
% given quaternions, Q and R. Inputs Q and R can be either M-by-4 matrices
% containing M quaternions, or a single 1-by-4 quaternion. N returns an
% M-by-4 matrix of quaternion products. Each element of Q and R must be a
% real number. Additionally, Q and R have their scalar number as the first
% column.
%
% Examples:
%
% Determine the product of two 1-by-4 quaternions:
% q = [1 0 1 0];
% r = [1 0.5 0.5 0.75];
% mult = quatmultiply(q, r)
%
% Determine the product of a 1-by-4 quaternion with itself:
% q = [1 0 1 0];
% mult = quatmultiply(q)
%
% Determine the product of 1-by-4 and 2-by-4 quaternions:
% q = [1 0 1 0];
% r = [1 0.5 0.5 0.75; 2 1 0.1 0.1];
% mult = quatmultiply(q, r)
%
% See also QUATCONJ, QUATDIVIDE, QUATINV, QUATMOD, QUATNORM,
% QUATNORMALIZE, QUATROTATE.
% Copyright 2000-2006 The MathWorks, Inc.
% $Revision: 1.1.6.2 $ $Date: 2006/06/16 20:04:06 $
% Note: Quaternion multiplication is not commutative.
error(nargchk(1, 2, nargin,'struct'));
if any(~isreal(q(:)))
error('aero:quatnorm:isnotreal1','First input elements are not real.');
end
if (size(q,2) ~= 4)
error('aero:quatnorm:wrongdim1','First input dimension is not M-by-4.');
end
if nargin == 1
r = q;
else
r = varargin{1};
if any(~isreal(r(:)))
error('aero:quatnorm:isnotreal2','Second input elements are not real.');
end
if (size(r,2) ~= 4)
error('aero:quatnorm:wrongdim2','Second input dimension is not M-by-4.');
end
if (size(r,1) ~= size(q,1) && ~( size(r,1) == 1 || size(q,1) == 1))
error('aero:quatnorm:wrongdim3',...
'Number of input rows are neither equal nor one.');
end
end
% Calculate vector portion of quaternion product
% vec = s1*v2 + s2*v1 + cross(v1,v2)
vec = [q(:,1).*r(:,2) q(:,1).*r(:,3) q(:,1).*r(:,4)] + ...
[r(:,1).*q(:,2) r(:,1).*q(:,3) r(:,1).*q(:,4)]+...
[ q(:,3).*r(:,4)-q(:,4).*r(:,3) ...
q(:,4).*r(:,2)-q(:,2).*r(:,4) ...
q(:,2).*r(:,3)-q(:,3).*r(:,2)];
% Calculate scalar portion of quaternion product
% scalar = s1*s2 - dot(v1,v2)
scalar = q(:,1).*r(:,1) - q(:,2).*r(:,2) - ...
q(:,3).*r(:,3) - q(:,4).*r(:,4);
qout = [scalar vec];
% enforce unit length
%mag= qout(1)* qout(1) + qout(2)* qout(2) + qout(3)* qout(3) + qout(4)* qout(4);
%
%if( abs(mag-1.0)<0.0001)
% qout=qout;
%else
%
% mag=1.0/sqrt( mag);
% qout(1)=qout(1)* mag;
% qout(2)=qout(2)* mag;
% qout(3)=qout(3)* mag;
% qout(4)=qout(4)* mag;
%end
Computing file changes ...