CPPLapack
 All Classes Files Functions Variables Friends
dcovector_small-specialized.hpp
Go to the documentation of this file.
00001 //=============================================================================
00002 /*! calculate vector product only for 2D vector */
00003 inline double operator/(const dcovec2& A, const dcovec2& B)
00004 {VERBOSE_REPORT;
00005   return A(0)*B(1) -A(1)*B(0);
00006 }
00007 
00008 //=============================================================================
00009 /*!  */
00010 inline double v2t(const dcovec2& v)
00011 {VERBOSE_REPORT;
00012   return atan2(v(1),v(0));
00013 }
00014 
00015 //=============================================================================
00016 /*!  */
00017 inline dcovec2 rotate(const dcovec2& v, const double& t)
00018 {VERBOSE_REPORT;
00019   dcovec2 w;
00020   w(0)=v(0)*cos(t)-v(1)*sin(t);
00021   w(1)=v(0)*sin(t)+v(1)*cos(t);
00022   return w;
00023 }
00024 
00025 ///////////////////////////////////////////////////////////////////////////////
00026 ///////////////////////////////////////////////////////////////////////////////
00027 ///////////////////////////////////////////////////////////////////////////////
00028 ///////////////////////////////////////////////////////////////////////////////
00029 ///////////////////////////////////////////////////////////////////////////////
00030 ///////////////////////////////////////////////////////////////////////////////
00031 ///////////////////////////////////////////////////////////////////////////////
00032 ///////////////////////////////////////////////////////////////////////////////
00033 ///////////////////////////////////////////////////////////////////////////////
00034 
00035 //=============================================================================
00036 /*! calculate vector product only for 3D vector */
00037 inline dcovec3 operator/(const dcovec3& A, const dcovec3& B)
00038 {VERBOSE_REPORT;
00039   dcovec3 C;
00040   C(0) =A(1)*B(2) -A(2)*B(1);
00041   C(1) =A(2)*B(0) -A(0)*B(2);
00042   C(2) =A(0)*B(1) -A(1)*B(0);
00043   return C;
00044 }
00045 
00046 //=============================================================================
00047 /*! calculate vector product only for 3D vector */
00048 inline dcovec3 operator/=(dcovec3& A, const dcovec3& B)
00049 {VERBOSE_REPORT;
00050   A =A/B;
00051   return A;
00052 }
00053 
00054 //=============================================================================
00055 /*!  */
00056 inline dquater vr2q(const dcovec3& v, const double& r)
00057 {VERBOSE_REPORT;
00058   return dquater(v(0),v(1),v(2),r);
00059 }
00060 
00061 //=============================================================================
00062 /*!  */
00063 inline dquater vt2q(const dcovec3& v, const double& theta)
00064 {VERBOSE_REPORT;
00065   return vr2q( v/(nrm2(v)+DBL_MIN)*std::sin(0.5*theta), std::cos(0.5*theta) );
00066 }
00067 
00068 //=============================================================================
00069 /*!  */
00070 inline dcovec3 rotate(const dcovec3& v, const dquater& q)
00071 {VERBOSE_REPORT;
00072   return imag( q*vr2q(v,0.)*conj(q) );
00073 }
00074 
00075 ///////////////////////////////////////////////////////////////////////////////
00076 ///////////////////////////////////////////////////////////////////////////////
00077 ///////////////////////////////////////////////////////////////////////////////
00078 ///////////////////////////////////////////////////////////////////////////////
00079 ///////////////////////////////////////////////////////////////////////////////
00080 ///////////////////////////////////////////////////////////////////////////////
00081 ///////////////////////////////////////////////////////////////////////////////
00082 ///////////////////////////////////////////////////////////////////////////////
00083 ///////////////////////////////////////////////////////////////////////////////
00084 
00085 //=============================================================================
00086 /*! conjuction */
00087 inline dquater conj(const dquater& q)
00088 {VERBOSE_REPORT;
00089   return dquater(-q(0),-q(1),-q(2), q(3));
00090 }
00091 
00092 //=============================================================================
00093 /*! imag */
00094 inline dcovec3 imag(const dquater& q)
00095 {VERBOSE_REPORT;
00096   return dcovec3(q(0),q(1),q(2));
00097 }
00098 
00099 //=============================================================================
00100 /*! inverse */
00101 inline dquater inv(const dquater& q)
00102 {VERBOSE_REPORT;
00103   return conj(q)/pow(nrm2(q),2);
00104 }
00105 
00106 //=============================================================================
00107 /*!  */
00108 inline dquater operator*(const dquater& q1, const dquater& q2)
00109 {VERBOSE_REPORT;
00110   return dquater(q1(3)*q2(0) +q1(0)*q2(3) +q1(1)*q2(2) -q1(2)*q2(1),
00111                  q1(3)*q2(1) -q1(0)*q2(2) +q1(1)*q2(3) +q1(2)*q2(0),
00112                  q1(3)*q2(2) +q1(0)*q2(1) -q1(1)*q2(0) +q1(2)*q2(3),
00113                  q1(3)*q2(3) -q1(0)*q2(0) -q1(1)*q2(1) -q1(2)*q2(2) );
00114 }
00115 
00116 //=============================================================================
00117 /*!  */
00118 inline dquater operator/(const dquater& q1, const dquater& q2)
00119 {VERBOSE_REPORT;
00120   return q1*inv(q2);
00121 }
00122 
00123 //=============================================================================
00124 /*!  */
00125 inline dquater operator*=(dquater& q1, const dquater& q2)
00126 {VERBOSE_REPORT;
00127   q1 =q1*q2;
00128   return q1;
00129 }
00130 
00131 //=============================================================================
00132 /*!  */
00133 inline dquater operator/=(dquater& q1, const dquater& q2)
00134 {VERBOSE_REPORT;
00135   q1 =q1/q2;
00136   return q1;
00137 }
00138 
00139 //=============================================================================
00140 /*! return vector from quaternion (|vector|=theta) */
00141 inline dcovec3 q2vt(const dquater& q)
00142 {VERBOSE_REPORT;
00143   double sin_theta_half;
00144   double theta( 2.*std::acos(q(3)) );
00145   
00146   if(theta<M_PI){
00147     sin_theta_half =std::sin(0.5*theta);
00148   }
00149   else{
00150     theta -=2.*M_PI;
00151     sin_theta_half =-std::sin(0.5*theta);
00152   }
00153   
00154   return dcovec3( theta*q(0)/sin_theta_half,
00155                   theta*q(1)/sin_theta_half,
00156                   theta*q(2)/sin_theta_half );
00157 }
00158 
00159 //=============================================================================
00160 /*! return rotational matrix made of quaternion */
00161 inline dgemat3 q2m(const dquater& q)
00162 {VERBOSE_REPORT;
00163   dquater cq( conj(q) );
00164   dquater X( dquater(+q(3),+q(2),-q(1),-q(0))*cq );
00165   dquater Y( dquater(-q(2),+q(3),+q(0),-q(1))*cq );
00166   dquater Z( dquater(+q(1),-q(0),+q(3),-q(2))*cq );
00167   dgemat3 mat;
00168   mat(0,0)=X(0); mat(0,1)=Y(0); mat(0,2)=Z(0);
00169   mat(1,0)=X(1); mat(1,1)=Y(1); mat(1,2)=Z(1);
00170   mat(2,0)=X(2); mat(2,1)=Y(2); mat(2,2)=Z(2);
00171   return mat;
00172 }
 All Classes Files Functions Variables Friends