gusucode.com > vision工具箱matlab源码程序 > vision/+vision/+internal/+pc/rotateAxes.m

    function rotateAxes(ax,dtheta,dphi,axCenter,vertAxis,vertAxisDir,showAxis)
% Rotate Axes around given axis
if nargin < 6
    showAxis = false;
end

currentPose = ax.CameraPosition;
up = ax.CameraUpVector;       

currentTarg = ax.CameraTarget;
dar = ax.DataAspectRatio;

% View Axis: vector from camera position to rotation center
vaxis = axCenter - currentPose;
coordsysval = lower(vertAxis) - 'x' + 1;

% First rotation axis is parallel to the principle up axis
raxis1 = [0 0 0];
if strncmpi(vertAxisDir,'down',1)
    raxis1(coordsysval) = -1;
    dtheta = -dtheta;
    dphi = -dphi;
else
    raxis1(coordsysval) = 1;
end

% Second rotation axis orthogonal to the plane made by raxis1 and vaxis
raxis2 = crossSimple(vaxis,raxis1);    
raxis2 = raxis2/simpleNorm(raxis2); 

upsidedown = (up(coordsysval) < 0);
if upsidedown
    dtheta = -dtheta;
    raxis2 = -raxis2;    
end

if showAxis
    hline1 = findobj(ax,'tag','pcViewerRAxis1'); 
    hline2 = findobj(ax,'tag','pcViewerRAxis2');
    hline3 = findobj(ax,'tag','pcViewerRAxis3');
    XSpan = ax.XLim(2)-ax.XLim(1);
    YSpan = ax.YLim(2)-ax.YLim(1);
    ZSpan = ax.ZLim(2)-ax.ZLim(1);
    
    minSpan = min([XSpan,YSpan,ZSpan]);
    vlength = minSpan*0.2;
    raxis1 = raxis1*vlength;
    raxis2 = raxis2*vlength;
    raxis3 = crossSimple(raxis1,raxis2);
    raxis3 = raxis3/simpleNorm(raxis3)*vlength;
    
    hline1.XData = [axCenter(1),axCenter(1)+raxis1(1)];
    hline1.YData = [axCenter(2),axCenter(2)+raxis1(2)];
    hline1.ZData = [axCenter(3),axCenter(3)+raxis1(3)];
    hline2.XData = [axCenter(1),axCenter(1)+raxis2(1)];
    hline2.YData = [axCenter(2),axCenter(2)+raxis2(2)];
    hline2.ZData = [axCenter(3),axCenter(3)+raxis2(3)];
    hline3.XData = [axCenter(1),axCenter(1)+raxis3(1)];
    hline3.YData = [axCenter(2),axCenter(2)+raxis3(2)];
    hline3.ZData = [axCenter(3),axCenter(3)+raxis3(3)];
    
end

% Check if the camera up vector is parallel with the view direction;
% if yes, use another rotation axis
if any(isnan(raxis2))
    raxis2 = crossSimple(raxis1,up);
end

% Rotate the camera, its upvector, and the camera target around raxis1 and
% raxis2 by dtheta and dphi
[newPos, newUp, newTarg] = localCamrotate(currentPose,currentTarg,axCenter,dar,up,dtheta,dphi,raxis1,raxis2);


if ~all(isnan(newPos))    
    ax.CameraPosition = newPos;
    ax.CameraUpVector= newUp;
end

if ~all(isnan(newTarg))    
    ax.CameraTarget = newTarg;
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Perform camera rotation around 2 axis
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [newPos, newUp, newTarg] = localCamrotate(a,targ,b,dar,up,dt,dp,direction1,direction2)
%   Rotate a and targ around two axis
%   Rotation is first around a line defined by 
%      point b and direction1, then a line defined by point b and
%      direction2

va = (b-a)./dar;
ra = crossSimple(va, up./dar);
ua = crossSimple(ra, va);

dis = simpleNorm(va);
va = va/dis;
ua = ua/simpleNorm(ua);

% same for target
vtarg = (b-targ)./dar;
disTarg = simpleNorm(vtarg);
vtarg = vtarg/disTarg;

haxis = direction1/simpleNorm(direction1);
vaxis = direction2/simpleNorm(direction2);

rotH = localRotMat(haxis,dt);
rotV = localRotMat(vaxis,-dp);

rotHV = rotV*rotH;
newV = -va*rotHV;
newUp = ua*rotHV;
newVTarg = -vtarg*rotHV;

newPos=b+newV*dis.*dar;
newUp=newUp.*dar;
newTarg = b+newVTarg*disTarg.*dar;
end

function rotM = localRotMat(axis,dt)
deg2rad = pi/180;
alph = dt*deg2rad;
cosa = cos(alph);
sina = sin(alph);
vera = 1 - cosa;
x = axis(1);
y = axis(2);
z = axis(3);
rotM = [cosa+x^2*vera x*y*vera-z*sina x*z*vera+y*sina; ...
  x*y*vera+z*sina cosa+y^2*vera y*z*vera-x*sina; ...
  x*z*vera-y*sina y*z*vera+x*sina cosa+z^2*vera]';

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% simple cross product
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function c=crossSimple(a,b)
c(1) = b(3)*a(2) - b(2)*a(3);
c(2) = b(1)*a(3) - b(3)*a(1);
c(3) = b(2)*a(1) - b(1)*a(2);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% simple norm for a 3D vector
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function n = simpleNorm(v)
n = sqrt(v(1)^2+v(2)^2+v(3)^2);
end