From sources-request at octave dot org Mon Dec 6 13:55:05 2004 Subject: Simple 3D-rotation function From: "Dr.-Ing. Torsten Finke" To: sources at octave dot org Date: Mon, 6 Dec 2004 13:46:04 -0600 --FCuugMFkClbJLl1L Content-Type: text/plain; charset=us-ascii Content-Disposition: inline Hello Octave attached You find a little function to rotate points in 3D-space. I hope it would be useful. Thanks to all who worked on phantastic octave. Best regards T. Finke -- ------------------------------------------------------------------------ Dr.-Ing. Torsten Finke Ingenieurgemeinschaft IgH Heinz-Baecker-Str. 34 D-45356 Essen Tel.: +49 201 / 360-14-17 Fax.: +49 201 / 360-14-14 E-mail: torsten dot finke at igh-essen dot com ------------------------------------------------------------------------ --FCuugMFkClbJLl1L Content-Type: text/plain; charset=us-ascii Content-Disposition: attachment; filename="rotax.m" function y = rotax(p, n, phi, x) % % 3D-routine % rotate vectors x around axis p + t n % % y = xrot(p, n, phi, x) % % (point p, direction n) % by angle (in radians) phi. % % Input: % - p: 3D-vector (point) % - n: direction vector (length != 0) % - phi: rotation angle (scalar) % - x: vectors (one 3D-vector each row) % % Output: % - y: rotated vectors % % % $Revision: 1.5 $ % $Date: 2004/12/06 19:43:36 $ % $RCSfile: rotax.m,v $ % % algorithm by Paul Bourke: % see http://astronomy.swin.edu.au/~pbourke/geometry/rotate/ % Rotation of a point in 3 dimensional space by theta about an % arbitrary axes defined by a line between two % points P1 = (x1,y1,z1) and P2 = (x2,y2,z2) can be achieved by the following steps % % (1) translate space so that the rotation axis % passes through the origin % % (2) rotate space about the x axis so that the % rotation axis lies in the xz plane % % (3) rotate space about the y axis so that the % rotation axis lies along the z axis % % (4) perform the desired rotation % by theta about the z axis % % (5) apply the inverse of step (3) % % (6) apply the inverse of step (2) % % (7) apply the inverse of step (1) % % Note: % % * % If the rotation axis is already aligned with the z axis % then steps 2, 3, 5, and 6 need not be performed. % * % In all that follows a right hand coordinate system is assumed % and rotations are positive when looking down the rotation axis % towards the origin. % * % Symbols representing matrices will be shown in bold text. % * % The inverse of the rotation matrices below are particularly % straightforward since the determinant is unity in each case. % * % All rotation angles are considered positive if anticlockwise % looking down the rotation axis towards the origin. % % Step 1 % % Translate space so that the rotation axis passes through the origin. % This is accomplished by translating space by -P1 (-x1,-y1,-z1). % The translation matrix T and the inverse Ti=T-1 (required for step 7) % are given below % % % / 1 0 0 -x1 \ % | 0 1 0 -y1 | % T = | 0 0 1 -z1 | % \ 0 0 0 1 / % % / 1 0 0 x1 \ % | 0 1 0 y1 | % Ti = | 0 0 1 z1 | % \ 0 0 0 1 / % % % Step 2 % % Rotate space about the x axis so that the rotation axis lies in the % xz plane. Let U = (a,b,c) be the unit vector along the rotation axis. % and define d = sqrt(b^2 + c^2) as the length of the projection onto the % yz plane. If d = 0 then the rotation axis is along the x axis and no % additional rotation is necessary. Otherwise rotate the rotation axis % so that is lies in the xz plane. The rotation angle to achieve this % is the angle between the projection of rotation axis in the yz plane % and the z axis. This can be calculated from the dot product of the z % component of the unit vector U and its yz projection. The sine of the % angle is determine by considering the cross product. % % The rotation matrix Rx and the inverse Rx-1 (required for step 6) % are given below % % / 1 0 0 0 \ % | 0 c/d -b/d 0 | % Rx = | 0 b/d c/d 0 | % \ 0 0 0 1 / % % % / 1 0 0 0 \ % | 0 c/d b/d 0 | % Rx^-1 = | 0 -b/d c/d 0 | % \ 0 0 0 1 / % % Step 3 % % Rotate space about the y axis so that the rotation axis lies along % the positive z axis. Using the appropriate dot and cross product % relationships as before the cosine of the angle is d, the sine of % the angle is a. The rotation matrix about the y axis Ry and the % inverse Ry-1 (required for step 5) are given below. % % / d 0 -a 0 \ % | 0 1 0 0 | % Ry = | a 0 d 0 | % \ 0 0 0 1 / % % % / d 0 a 0 \ % | 0 1 0 0 | % Ry^-1 = | -a 0 d 0 | % \ 0 0 0 1 / % % Step 4 % % Rotation about the z axis by t (theta) is Rz and is simply % % / cos(t) sin(t) 0 0 \ % | -sin(t) cos(t) 0 0 | % Rz = | 0 0 1 0 | % \ 0 0 0 1 / % % The complete transformation to rotate a point (x,y,z) about the % rotation axis to a new point (x`,y`,z`) is as follows, the forward % transforms followed by the reverse transforms. % / x' \ / x \ % | y' | | y | % | z' | = T-1 Rx-1 Ry-1 Rz Ry Rx T | z | % \ 1 / \ 1 / % % so now let's do it in octave % % fortunately the inverse matrixes are identical to the transposed % % please note: % % the algorithm can be improved by doing the final matrix multiplication % in one single step. One has only to do some algebra. But the resulting code % hides the beauty of the algorithm. n = n / norm(n); % direction vector to unit length a = n(1); b = n(2); c = n(3); d = sqrt(b*b + c*c); cs = cos(phi); sn = sin(phi); l = size(x, 1); % we need l(ength) ones o = ones(l, 1); xx = [x, o]'; % pad original vectors with ones and transpose them % so the vectors are in columns now. Ti = T = Rx = Ry = Rz = eye(4); % prepare transformation matrixes T(1:3, 4) = -p'; % shift rotation axis to origin Ti(1:3, 4) = p'; % and back % rotate about x into xz-plane if necessary if d != 0, Rx(2:3, 2:3) = ... [ c/d, -b/d; b/d, c/d ]; end % rotate rotation axis to z-axis Ry (1:2:3, 1:2:3) = ... [ d, -a; a, d ]; % actually rotate vectors Rz (1:2, 1:2) = ... [ cs, -sn; sn, cs ]; yy = Ti * Rx' * Ry' * Rz * Ry * Rx * T * xx; y = yy(1:3, :); % remove ones y = y'; % retranspose --FCuugMFkClbJLl1L--