00001 /////////////////////////////////////////////////////////////////////////////// 00002 /////////////////////////////////////////////////////////////////////////////// 00003 /////////////////////////////////////////////////////////////////////////////// 00004 /*! 00005 \page pg-example Example Codes 00006 <!---------------------------------------------------------------------------!> 00007 \section st-quat Example of quat 00008 \include quat/main.cpp 00009 <br><br> 00010 00011 <!---------------------------------------------------------------------------!> 00012 \section st-vec Example of vec 00013 \include vec/main.cpp 00014 <br><br> 00015 00016 <!---------------------------------------------------------------------------!> 00017 \section st-mat Example of mat 00018 \include mat/main.cpp 00019 */ 00020 00021 /////////////////////////////////////////////////////////////////////////////// 00022 /////////////////////////////////////////////////////////////////////////////// 00023 /////////////////////////////////////////////////////////////////////////////// 00024 /*! 00025 \page pg-convert How to Convert "quat to vec" and "vec to quat" 00026 <!---------------------------------------------------------------------------!> 00027 <h2>im</h2> 00028 <code>vec im(const quat & q)</code><br> 00029 Pull out the imaginary part of q, and put it into v. 00030 <center><code>v =im(q);</code></center> 00031 \f[ 00032 \overrightarrow{v}= \left\{ q_x, q_y, q_z \right\} 00033 \f] 00034 00035 <!---------------------------------------------------------------------------!> 00036 <h2>vr2q</h2> 00037 <code>quat vr2q(const vec & v, const double & r)</code><br> 00038 Put v into im(q), and put r into q.r .<br> 00039 <center><code>q =vr2q(v,r);</code></center> 00040 \f[ 00041 \mathbf{q}= \left[ v_x, v_y, v_z; r \right] 00042 \f] 00043 00044 <!---------------------------------------------------------------------------!> 00045 <h2>vt2q</h2> 00046 <code>quat vt2q(const vec & u, const double & phi)</code> 00047 The rotation quaternion, which axis direction is v and angular is theta. 00048 <center><code>q =vt2q(u,phi);</code></center> 00049 \f[ 00050 \mathbf{q} 00051 = \left[ 00052 \frac{{u_x}}{|\vec{u}|}\sin \left( \frac{\phi}{2} \right), 00053 \frac{{u_y}}{|\vec{u}|}\sin \left( \frac{\phi}{2} \right), 00054 \frac{{u_z}}{|\vec{u}|}\sin \left( \frac{\phi}{2} \right); 00055 \cos \left( \frac{\phi}{2} \right) \right] 00056 \f] 00057 00058 */ 00059 00060 /////////////////////////////////////////////////////////////////////////////// 00061 /////////////////////////////////////////////////////////////////////////////// 00062 /////////////////////////////////////////////////////////////////////////////// 00063 /*! 00064 <!-- 00065 \page pg-attitude Attitude Quaternion 00066 The following mathod to handle quaternions as attitude expressions is just 00067 <!---------------------------------------------------------------------------!> 00068 <h2>Definition of Attitude Quaternion</h2> 00069 Let \f$A\f$ and \f$B\f$ be the coordinate-systems. 00070 When the following equation is true for any vector \f$\vec{v}\f$, 00071 \f${}^AB\f$ is defined as the attitude of \f$B\f$ coordinate-system on \f$A\f$ coordinate-system. 00072 \f[ 00073 [{}^A\vec{v};0] ={}^AB [{}^B\vec{v};0] \overline{{}^AB} 00074 \f] 00075 00076 Let inertial coordinate-system 00077 --> 00078 */ 00079 00080 /////////////////////////////////////////////////////////////////////////////// 00081 /////////////////////////////////////////////////////////////////////////////// 00082 /////////////////////////////////////////////////////////////////////////////// 00083 /*! 00084 \page pg-rotate How to Rotate "vec" 00085 <!---------------------------------------------------------------------------!> 00086 <h2>rotate</h2> 00087 <center><img src="../image/rotate.png" alt="rotate.png"></center> 00088 To calculate the components of the rotated vector of v around vector u by phi[rad] in right-handed screw rule, 00089 use the following code. 00090 <center><code>v_after =rotate(v_before, vt2q(u, phi));</code></center> 00091 00092 \f[ 00093 \overrightarrow{v}_\mathrm{after} 00094 =\mathbf{R}(\overrightarrow{u}, \phi) \overrightarrow{v}_\mathrm{before} \overline{\mathbf{R}(\overrightarrow{u}, \phi)} 00095 \f] 00096 where \f$\mathbf{R}\f$ is 00097 \f[ 00098 \mathbf{R}(\overrightarrow{u}, \phi) 00099 = \left[ 00100 \frac{{u_x}}{|\vec{u}|}\sin \left( \frac{\phi}{2} \right), 00101 \frac{{u_y}}{|\vec{u}|}\sin \left( \frac{\phi}{2} \right), 00102 \frac{{u_z}}{|\vec{u}|}\sin \left( \frac{\phi}{2} \right); 00103 \cos \left( \frac{\phi}{2} \right) \right] 00104 \f] 00105 00106 00107 <!---------------------------------------------------------------------------!> 00108 <!-- 00109 <h2>orbit</h2> 00110 <center><img src="../image/orbit.png" alt="orbit.png"></center> 00111 To calculate the components of vector v on a different coordinate system, 00112 use the following code. 00113 <center><code>Av =orbit(Bv, AB);</code> or <code>Av =orbit(Bv, conj(BA));</code></center> 00114 00115 \f[ 00116 {}^A\overrightarrow{v} 00117 ={}^A{B} {}^B\overrightarrow{v} \overline{{}^A{B}} 00118 =\overline{{}^B{A}} {}^B\overrightarrow{v} {}^B{A} 00119 \f] 00120 where \f${}^A{B}\f$ and \f${}^B{A}\f$ are attitude quaternions. 00121 --!> 00122 */ 00123 00124 /////////////////////////////////////////////////////////////////////////////// 00125 /////////////////////////////////////////////////////////////////////////////// 00126 /////////////////////////////////////////////////////////////////////////////// 00127 /*! 00128 <!-- 00129 \page pg-nt Short Mathematical Introduction for Quaternion 00130 --!> 00131 */ 00132