// Copyright (c) CNES 2008 // // This software is part of CelestLab, a CNES toolbox for Scilab // // This software is governed by the CeCILL license under French law and // abiding by the rules of distribution of free software. You can use, // modify and/ or redistribute the software under the terms of the CeCILL // license as circulated by CEA, CNRS and INRIA at the following URL // 'http://www.cecill.info'. function [varargout] = CL_rot_defRot1Ax(u,axis,d,target_ang, numsol) // Determination of a rotation around one axis. // // Calling Sequence // [qa,anga,Inok] = CL_rot_defRot1Ax(u,axis,d,target_ang [,numsol=1] ) // [qa,anga,qb,angb,Inok] = CL_rot_defRot1Ax(u,axis,d,target_ang, numsol=2) // // Description // //

Computes the rotation around one axis so that the angle between the image of a vector // (u) and a vector d is equal to target_ang.

//

That is to say, the rotation R is such that: R(u)=v and angle(v,d)=target_ang. There can be 2 solutions (in usual situations).

//

Each rotation is returned as a quaternion, and the corresponding rotation angle is also returned.

//

//

If the condition "angle(v,d)=target_ang" cannot be achieved, the returned rotations are identical and they are computed // such that "|angle(v,d)-target_ang|" is minimal. The corresponding indices are returned in Inok.

//

When 2 solutions exist:

//

qa is the solution with minimal rotation angle.

//

qb is the solution with maximal rotation angle.

//

The argument axis can either be a vector (3xN) or an axis number (1=x,2=y or 3=z) (1x1). // If it is a number, the vectors u and d must be given in the frame where the axis is one of the basis vectors (coordinates = [1;0;0], [0;1;0], or [0;0;1]).

//

//
//
// // Parameters // u: Vector to be rotated (3xN or 3x1) // axis: Axis: vector (3xN or 3x1) or number (1x1) // d: Some direction (3xN or 3x1) // target_ang: (optional) Desired angle between d and the image of u by the rotation (1xN or 1x1) // numsol: (optional, integer) Number of solutions returned: 1 or 2. Default is 1 // qa: Quaternion that defines the rotation (first solution) (dim N) // anga: Rotation angle (first solution) (1xN) // qb: Quaternion that defines the rotation (second solution) (dim N) // angb: Rotation angle (second solution) (1xN) // Inok: Indices for which the desired angle between d and the image of u by the rotation cannot be achieved. // // Authors // CNES - DCT/SB // // See also // CL_rot_defQuat // CL_rot_defRotVec // CL_rot_defRot2Ax // // Examples // // Example1: Rotation about x-axis that transforms // // u into a vector perpendicular to d // u = [ 1 ; 2 ; 3 ]; // d = [ 3 ; 1 ; 2 ]; // [q, ang, Inok] = CL_rot_defRot1Ax(u, 1, d, %pi/2); // // Check result : // v = CL_rot_rotVect(q,u); // CL_vectAngle(v,d) // == %pi/2 // // // Example2: Rotation about an axis that transforms u // // into a vector as close as possible to d // u = [ 1 ; 2 ; 3 ]; // d = [ 3 ; 1 ; 2 ]; // axis = [-2;3;4]; // [q, ang, Inok] = CL_rot_defRot1Ax(u, axis, d, 0); // // Check result : // ang = CL_vectAngle(d,CL_rot_rotVect(q,u)) // angmin_approx = min(CL_vectAngle(d,CL_rot_rotVect( .. // CL_rot_axAng2quat(axis,linspace(0,2*%pi,200)),u))) // ---------------------------------------------------------- // Declarations: // ---------------------------------------------------------- // Rotation around one axis of the reference frame (1,2 or 3) // anga: minimum angle // arguments (u,d): same number of columns // n1: 1, 2, or 3 // target_ang: positive function [anga, angb, Inok] = def_rot_1ax(u, n1, d, target_ang) tol = 1.e-8; // tolerance value anga = %nan * ones(u(1,:)); angb = %nan * ones(u(1,:)); n2 = pmodulo(n1,3) + 1; // axis #2 n3 = pmodulo(n2,3) + 1; // axis #3 // note: if atan ill-defined => corresponding theta close to 1 alpha_u = atan(u(n3,:), u(n2,:)); // azimuth of u alpha_d = atan(d(n3,:), d(n2,:)); theta_u = acos(u(n1,:)); // angle between u and axis theta_d = acos(d(n1,:)); // condition for existence of solutions (acos) num = cos(target_ang) - cos(theta_u) .* cos(theta_d); den = sin(theta_u) .* sin(theta_d); expr_valid = abs(num) - abs(den); I = find(expr_valid < 0); if (I <> []) phi = real(acos(num(I) ./ den(I))); anga(I) = CL_rMod(phi + alpha_d(I) - alpha_u(I), -%pi, %pi); angb(I) = CL_rMod(-phi + alpha_d(I) - alpha_u(I), -%pi, %pi); // change order of solutions ("min" first) J = I(find(abs(anga(I)) > abs(angb(I)))); tmp = anga(J); anga(J) = angb(J); angb(J) = tmp; end I = find(expr_valid >= 0); if (I <> []) // solution such that angle(R(u),d) the closest to target_ang // in the (d, axis) plane // ec0 : angle (u,d) if same side // ec1 : angle (u,d) if opposite sides ec0 = abs(theta_d(I)-theta_u(I)); ec1 = min(theta_d(I)+theta_u(I), 2*%pi-(theta_d(I)+theta_u(I))); cond1 = ( abs(target_ang(I)-ec0) <= abs(target_ang(I)-ec1) ); J = I(find(cond1)); // => same side anga(J) = CL_rMod(alpha_d(J)-alpha_u(J), -%pi, %pi); J = I(find(~cond1)); // => opposite sides anga(J) = CL_rMod(alpha_d(J)-alpha_u(J)+%pi, -%pi, %pi); angb(I) = anga(I); // both solutions identical end Inok = find(expr_valid > tol); // also when close to limit endfunction // ---------------------------------------------------------- // main // ---------------------------------------------------------- if (~exists('numsol', 'local')); numsol=1; end if (numsol <> 1 & numsol <> 2) CL__error("Invalid value for numsol"); end if (find(target_ang < 0 | target_ang > %pi) <> []) CL__error("Target angle should be positive and less than pi"); end // Check argument sizes, and resize if necessary: // unit vectors before resizing (for efficiency) // check vectors norms are not 0 [u, nu] = CL_unitVector(u); [d, nd] = CL_unitVector(d); [nraxis, Nax] = size(axis); // number of rows/columns of axis if (nraxis == 1) [u, d, target_ang] = CL__checkInputs(u,3, d,3, target_ang,1); if (Nax > 1 | (axis <> 1 & axis <> 2 & axis <> 3)) CL__error("Invalid axes"); end else naxis = CL_norm(axis); [u, d, axis, target_ang] = CL__checkInputs(u,3, d,3, axis,3, target_ang,1); if (find(naxis == 0) <> []); CL__error("Zero norm axis"); end end // Handle cases where one input is [] if (u == [] | d == [] | axis == [] | target_ang == []) qa = []; anga = []; qb = []; angb = []; Inok = []; return; end if (find (nd .* nu == 0) <> []) CL__error("Vectors should not be zero"); end // ---------------------------------------------------------- Id = eye(3,3); if (size(axis,1) == 1) n1 = axis; [anga, angb, Inok] = def_rot_1ax(u, n1, d, target_ang); qa = CL_rot_axAng2quat(Id(:,n1), anga); qb = CL_rot_axAng2quat(Id(:,n1), angb); else n1 = 1; // frame "x" axis - arbitrary // new frame (quaternion to save memory) q0 = CL_rot_defFrameVec(axis, axis, n1, n1, res="q"); u = CL_rot_rotVect(q0', u); // coordinates in new frame d = CL_rot_rotVect(q0', d); [anga, angb, Inok] = def_rot_1ax(u, n1, d, target_ang); qa = q0 * CL_rot_axAng2quat(Id(:,n1), anga) * q0'; qb = q0 * CL_rot_axAng2quat(Id(:,n1), angb) * q0'; end // Selection of outputs if (numsol == 1) if (argn(1) > 3); CL__error("Too many output arguments"); end varargout = list(qa, anga,Inok); else if (argn(1) > 5); CL__error("Too many output arguments"); end varargout = list(qa, anga, qb, angb, Inok); end endfunction