CPPLapack
 All Classes Files Functions Variables Friends Pages
dcovector_small-specialized.hpp
Go to the documentation of this file.
1 //=============================================================================
2 /*! calculate vector product only for 2D vector */
3 inline double operator/(const dcovec2& A, const dcovec2& B)
4 {CPPL_VERBOSE_REPORT;
5  return A(0)*B(1) -A(1)*B(0);
6 }
7 
8 //=============================================================================
9 /*! convert 2D vector to theta */
10 inline double v2t(const dcovec2& v)
11 {CPPL_VERBOSE_REPORT;
12  return std::atan2(v(1),v(0));
13 }
14 
15 //=============================================================================
16 /*! rotate 2D vector by theta [rad] */
17 inline dcovec2 rotate(const dcovec2& v, const double& t)
18 {CPPL_VERBOSE_REPORT;
19  dcovec2 w;
20  w(0) =v(0)*std::cos(t) -v(1)*std::sin(t);
21  w(1) =v(0)*std::sin(t) +v(1)*std::cos(t);
22  return w;
23 }
24 
25 ///////////////////////////////////////////////////////////////////////////////
26 ///////////////////////////////////////////////////////////////////////////////
27 ///////////////////////////////////////////////////////////////////////////////
28 ///////////////////////////////////////////////////////////////////////////////
29 ///////////////////////////////////////////////////////////////////////////////
30 ///////////////////////////////////////////////////////////////////////////////
31 ///////////////////////////////////////////////////////////////////////////////
32 ///////////////////////////////////////////////////////////////////////////////
33 ///////////////////////////////////////////////////////////////////////////////
34 
35 //=============================================================================
36 /*! calculate vector product only for 3D vector */
37 inline dcovec3 operator/(const dcovec3& A, const dcovec3& B)
38 {CPPL_VERBOSE_REPORT;
39  dcovec3 C;
40  C(0) =A(1)*B(2) -A(2)*B(1);
41  C(1) =A(2)*B(0) -A(0)*B(2);
42  C(2) =A(0)*B(1) -A(1)*B(0);
43  return C;
44 }
45 
46 //=============================================================================
47 /*! calculate vector product only for 3D vector */
48 inline dcovec3 operator/=(dcovec3& A, const dcovec3& B)
49 {CPPL_VERBOSE_REPORT;
50  A =A/B;
51  return A;
52 }
53 
54 //=============================================================================
55 /*! make quaternion from imag vector and real value */
56 inline dquater vr2q(const dcovec3& v, const double& r)
57 {CPPL_VERBOSE_REPORT;
58  return dquater(v(0),v(1),v(2),r);
59 }
60 
61 //=============================================================================
62 /*! make quaternion from directional vector and rotational angle */
63 inline dquater vt2q(const dcovec3& v, const double& theta)
64 {CPPL_VERBOSE_REPORT;
65  return vr2q( v/(nrm2(v)+DBL_MIN)*std::sin(0.5*theta), std::cos(0.5*theta) );
66 }
67 
68 //=============================================================================
69 /*! rotate 3D vector by quaternion */
70 inline dcovec3 rotate(const dcovec3& v, const dquater& q)
71 {CPPL_VERBOSE_REPORT;
72  return imag( q*vr2q(v,0.)*conj(q) );
73 }
74 
75 ///////////////////////////////////////////////////////////////////////////////
76 ///////////////////////////////////////////////////////////////////////////////
77 ///////////////////////////////////////////////////////////////////////////////
78 ///////////////////////////////////////////////////////////////////////////////
79 ///////////////////////////////////////////////////////////////////////////////
80 ///////////////////////////////////////////////////////////////////////////////
81 ///////////////////////////////////////////////////////////////////////////////
82 ///////////////////////////////////////////////////////////////////////////////
83 ///////////////////////////////////////////////////////////////////////////////
84 
85 //=============================================================================
86 /*! conjuction */
87 inline dquater conj(const dquater& q)
88 {CPPL_VERBOSE_REPORT;
89  return dquater(-q(0),-q(1),-q(2), q(3));
90 }
91 
92 //=============================================================================
93 /*! imag */
94 inline dcovec3 imag(const dquater& q)
95 {CPPL_VERBOSE_REPORT;
96  return dcovec3(q(0),q(1),q(2));
97 }
98 
99 //=============================================================================
100 /*! inverse */
101 inline dquater inv(const dquater& q)
102 {CPPL_VERBOSE_REPORT;
103  return conj(q)/std::pow(nrm2(q),2);
104 }
105 
106 //=============================================================================
107 /*! dquater*dquater operator */
108 inline dquater operator*(const dquater& q1, const dquater& q2)
109 {CPPL_VERBOSE_REPORT;
110  return dquater(q1(3)*q2(0) +q1(0)*q2(3) +q1(1)*q2(2) -q1(2)*q2(1),
111  q1(3)*q2(1) -q1(0)*q2(2) +q1(1)*q2(3) +q1(2)*q2(0),
112  q1(3)*q2(2) +q1(0)*q2(1) -q1(1)*q2(0) +q1(2)*q2(3),
113  q1(3)*q2(3) -q1(0)*q2(0) -q1(1)*q2(1) -q1(2)*q2(2) );
114 }
115 
116 //=============================================================================
117 /*! dquater/dquater operator */
118 inline dquater operator/(const dquater& q1, const dquater& q2)
119 {CPPL_VERBOSE_REPORT;
120  return q1*inv(q2);
121 }
122 
123 //=============================================================================
124 /*! dquater*=dquater operator */
125 inline dquater operator*=(dquater& q1, const dquater& q2)
126 {CPPL_VERBOSE_REPORT;
127  q1 =q1*q2;
128  return q1;
129 }
130 
131 //=============================================================================
132 /*! dquater/=dquater operator */
133 inline dquater operator/=(dquater& q1, const dquater& q2)
134 {CPPL_VERBOSE_REPORT;
135  q1 =q1/q2;
136  return q1;
137 }
138 
139 //=============================================================================
140 /*! return vector from quaternion (|vector|=theta) */
141 inline dcovec3 q2vt(const dquater& q)
142 {CPPL_VERBOSE_REPORT;
143  double sin_theta_half;
144  double theta( 2.*std::acos(q(3)) );
145 
146  if(theta<M_PI){
147  sin_theta_half =std::sin(0.5*theta);
148  }
149  else{
150  theta -=2.*M_PI;
151  sin_theta_half =-std::sin(0.5*theta);
152  }
153 
154  return dcovec3( theta*q(0)/sin_theta_half,
155  theta*q(1)/sin_theta_half,
156  theta*q(2)/sin_theta_half );
157 }
158 
159 //=============================================================================
160 /*! return rotational matrix made of quaternion */
161 inline dgemat3 q2m(const dquater& q)
162 {CPPL_VERBOSE_REPORT;
163  dquater cq( conj(q) );
164  dquater X( dquater(+q(3),+q(2),-q(1),-q(0))*cq );
165  dquater Y( dquater(-q(2),+q(3),+q(0),-q(1))*cq );
166  dquater Z( dquater(+q(1),-q(0),+q(3),-q(2))*cq );
167  dgemat3 mat;
168  mat(0,0)=X(0); mat(0,1)=Y(0); mat(0,2)=Z(0);
169  mat(1,0)=X(1); mat(1,1)=Y(1); mat(1,2)=Z(1);
170  mat(2,0)=X(2); mat(2,1)=Y(2); mat(2,2)=Z(2);
171  return mat;
172 }
dquater inv(const dquater &q)
dquater vt2q(const dcovec3 &v, const double &theta)
dcovec2 rotate(const dcovec2 &v, const double &t)
double nrm2(const _dcovector &vec)
dquater conj(const dquater &q)
dquater vr2q(const dcovec3 &v, const double &r)
dgemat3 q2m(const dquater &q)
dcovec3 operator/=(dcovec3 &A, const dcovec3 &B)
dcovec3 q2vt(const dquater &q)
double v2t(const dcovec2 &v)
double operator/(const dcovec2 &A, const dcovec2 &B)
drovector t(const _dcovector &covec)
dcovec3 imag(const dquater &q)
dquater operator*(const dquater &q1, const dquater &q2)
dquater operator*=(dquater &q1, const dquater &q2)