// 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 [M, omega] = CL_rot_quat2matrix(q, qdot) // Quaternion to transformation matrix // // Calling Sequence // [M, omega] = CL_rot_quat2matrix(q, [, qdot]) // // Description // <itemizedlist><listitem> // <p>Computes the transformation matrix that corresponds to a given quaternion.</p> // <p>Also computes the angular velocity vector from the quaternion derivative (if it is present and not empty).</p> // <p></p></listitem> // <listitem> // <p><b>Notes:</b></p> // <p> - See <link linkend="CL_rot_angles2matrix">CL_rot_angles2matrix</link> for // conventions regarding the definition of the transformation matrix. </p> // <p> - See <link linkend="Data types">Data types</link> or <link linkend="CL_rot_defQuat">CL_rot_defQuat</link> for // more details on quaternions. </p> // </listitem> // </itemizedlist> // // Parameters // q: Quaternion (dim N) // qdot: Quaternion time derivative. Can be empty (dim N) // M: Transformation matrix (3x3xN) // omega: Angular velocity vector. Empty if not computed (3xN) // // Bibliography // 1) Mecanique Spatiale - CNES Cepadues 1995, Tome I, 7.2.2.3 Les quaternions, eq.15 // 2) James R. Wertz, Spacecraft attitude determination and control (volume 73 of Astrophyisics and Space Science Library), D. Reidel Publishing Company, 1980, appendix D-E // // Authors // CNES - DCT/SB // // See also // CL_rot_defQuat // CL_rot_matrix2quat // // Examples // ang = CL_deg2rad(10) // M1 = CL_rot_angles2matrix(3,ang) // q1 = CL_rot_matrix2quat(M1) // M2 = CL_rot_quat2matrix(q1) // same as M1 // // Declarations: // Code: // qdot initialized to [], but would not be used if (~exists("qdot", "local")); qdot=[]; end // cder: %t if derivative is computed cder = %t; // number of arguments (left, right) [lhs, rhs] = argn(); if (lhs <= 1); cder = %f; end if (rhs <= 1 | size(qdot) == 0); cder = %f; end if (typeof(q) <> "CLquat") CL__error("Wrong type of input argument. Quaternion expected"); end // default outputs M = []; omega = []; n = size(q); // return [] if arguments are empty if (n == 0) return; end // q1, q2, q3, q4: not the usual CelestLab convention! q4 = q.r; q1 = q.i(1,:); q2 = q.i(2,:); q3 = q.i(3,:); M = hypermat([3, 3, n], ... [ q1.^2 - q2.^2 - q3.^2 + q4.^2; 2*(q1.*q2 - q3.*q4); ... 2*(q1.*q3 + q2.*q4); 2*(q1.*q2 + q3.*q4); ... (-q1.^2 + q2.^2 - q3.^2 + q4.^2); ... 2*(q2.*q3 - q1.*q4); 2*(q1.*q3 - q2.*q4); ... 2*(q2.*q3 + q1.*q4); ... (-q1.^2 - q2.^2 + q3.^2 + q4.^2)]); if (n == 1) M = M(:,:,1); // avoids hypermat if only one quaternion end; if (cder) if (typeof(qdot) <> "CLquat") CL__error("Wrong type for argument ''qdot''. Quaternion expected"); end if (size(qdot) <> n) CL__error("Wrong size for argument ''qdot''"); end q_omega = 2 * qdot * q'; omega = q_omega.i; end endfunction