Attitudetrinterp.m ( File view )

  • By ZHUZIZZQ 2015-01-29
  • View(s):25
  • Download(s):43
  • Point(s): 0
			%#codegen
function TransMatrix = Attitudetrinterp(StartMat, EndMat, PlanUnit)

    TransMatrix = zeros(3,3,length(PlanUnit));
    q0 = tr2q(StartMat);
    q1 = tr2q(EndMat);

    Transq = interp(q0, q1, PlanUnit);
    
    for k = 1:length(PlanUnit)
        TransMatrix(:,:,k) = q2tr(Transq(:,k));
    end
end


 function q = interp(q1, q2, r)
        %Quaternion.interp Interpolate quaternions
        %
        % QI = Q1.interp(Q2, S) is a unit-quaternion that interpolates a rotation 
        % between Q1 for S=0 and Q2 for S=1.
        %
        % If S is a vector QI is a vector of quaternions, each element
        % corresponding to sequential elements of S.
        %
        % Notes::
        % - This is a spherical linear interpolation (slerp) that can be interpretted 
        %   as interpolation along a great circle arc on a sphere.
        % - The value of S is clipped to the interval 0 to 1.
        %
        % See also ctraj, Quaternion.scale.

        q = zeros(4,length(r)); % preallocate space for Quaternion vector
            cosTheta = q1'*q2;
            % take shortest path along the great circle, patch by Gauthier Gras
            if cosTheta < 0
                q1 = - q1;
                cosTheta = - cosTheta;
            end;
            
            theta = acos(cosTheta);

            % clip values of r
            r(r<0) = 0;
            r(r>1) = 1;
           
            
            for i=1:length(r)
                if theta == 0
                    q(:,i) = q1;
                else
                    q(:,i) = (sin((1-r(i))*theta) * q1 + sin(r(i)*theta) * q2) / sin(theta);
                end
            end
        end

function q = tr2q(t) 
    q = [1;0;0;0];
    
    qs = sqrt(trace(t)+1)/2.0;
    kx = t(3,2) - t(2,3);   % Oz - Ay
    ky = t(1,3) - t(3,1);   % Ax - Nz
    kz = t(2,1) - t(1,2);   % Ny - Ox

    if (t(1,1) >= t(2,2)) && (t(1,1) >= t(3,3)) 
        kx1 = t(1,1) - t(2,2) - t(3,3) + 1; % Nx - Oy - Az + 1
        ky1 = t(2,1) + t(1,2);          % Ny + Ox
        kz1 = t(3,1) + t(1,3);          % Nz + Ax
        add = (kx >= 0);
    elseif (t(2,2) >= t(3,3))
        kx1 = t(2,1) + t(1,2);          % Ny + Ox
        ky1 = t(2,2) - t(1,1) - t(3,3) + 1; % Oy - Nx - Az + 1
        kz1 = t(3,2) + t(2,3);          % Oz + Ay
        add = (ky >= 0);
    else
        kx1 = t(3,1) + t(1,3);          % Nz + Ax
        ky1 = t(3,2) + t(2,3);          % Oz + Ay
        kz1 = t(3,3) - t(1,1) - t(2,2) + 1; % Az - Nx - Oy + 1
        add = (kz >= 0);
    end

    if add
        kx = kx + kx1;
        ky = ky + ky1;
        kz = kz + kz1;
    else
        kx = kx - kx1;
        ky = ky - ky1;
        kz = kz - kz1;
    end
    nm = norm([kx ky kz]);
    if nm == 0
        q = [1 0 0 0]';
    else
        qs = sqrt(1 - qs^2) / nm;
        q = [qs; qs*[kx; ky; kz]];
    end
end


%Q2TR   Convert unit-quaternion to homogeneous transform
%
%   T = q2tr(Q)
%
%   Return the rotational homogeneous transform corresponding to the unit
%   quaternion Q.
%
%   See also: TR2Q

function r = q2tr(q)

    s = q(1);
    x = q(2);
    y = q(3);
    z = q(4);

    r = [   1-2*(y^2+z^2)   2*(x*y-s*z) 2*(x*z+s*y)
        2*(x*y+s*z) 1-2*(x^2+z^2)   2*(y*z-s*x)
        2*(x*z-s*y) 2*(y*z+s*x) 1-2*(x^2+y^2)   ];

end
			
...
Expand> <Close

Want complete source code? Download it here

Point(s): 0

Download
0 lines left, continue to read
Sponsored links

File list

Tips: You can preview the content of files by clicking file names^_^
Name Size Date
Attitudetrinterp.m3.32 kB2014-12-11 10:01
01.97 kB
...
Sponsored links

Attitudetrinterp.m (1.52 kB)

Need 0 point
Your Point(s)
Download Download

Download failed? Click here to download one by one.

Tip: this source code project contains 2 packages, please click the allow button on the browser pop-up dialog,after you click the download button.

▪ Click to download this source code directly

LOGIN

Don't have an account? Register now
Need any help?
Mail to: support@codeforge.com

切换到中文版?

CodeForge Chinese Version
CodeForge English Version

Where are you going?

^_^"Oops ...

Sorry!This guy is mysterious, its blog hasn't been opened, try another, please!
OK

Warm tip!

CodeForge to FavoriteFavorite by Ctrl+D