| Geant4 Cross Reference |
1 // Copyright (C) 2010, Guy Barrand. All rights 1 // Copyright (C) 2010, Guy Barrand. All rights reserved.
2 // See the file tools.license for terms. 2 // See the file tools.license for terms.
3 3
4 #ifndef tools_qrot 4 #ifndef tools_qrot
5 #define tools_qrot 5 #define tools_qrot
6 6
7 // rotation done with quaternion. 7 // rotation done with quaternion.
8 8
9 namespace tools { 9 namespace tools {
10 10
11 template <class VEC3,class VEC4> 11 template <class VEC3,class VEC4>
12 class qrot { 12 class qrot {
13 protected: 13 protected:
14 //typedef typename VEC3::elem_t T3; 14 //typedef typename VEC3::elem_t T3;
15 typedef typename VEC4::elem_t T; //we assume 15 typedef typename VEC4::elem_t T; //we assume = T3
16 public: 16 public:
17 qrot() 17 qrot()
18 :m_quat(0,0,0,1) //zero rotation around th 18 :m_quat(0,0,0,1) //zero rotation around the positive Z axis.
19 {} 19 {}
20 qrot(const VEC3& a_axis,T a_radians,T(*a_sin 20 qrot(const VEC3& a_axis,T a_radians,T(*a_sin)(T),T(*a_cos)(T)){
21 if(!set_value(a_axis,a_radians,a_sin,a_cos 21 if(!set_value(a_axis,a_radians,a_sin,a_cos)) {} //FIXME : throw
22 } 22 }
23 qrot(const VEC3& a_from,const VEC3& a_to,T(* 23 qrot(const VEC3& a_from,const VEC3& a_to,T(*a_sqrt)(T),T(*a_fabs)(T)){set_value(a_from,a_to,a_sqrt,a_fabs);}
24 virtual ~qrot(){} 24 virtual ~qrot(){}
25 public: 25 public:
26 qrot(const qrot& a_from) 26 qrot(const qrot& a_from)
27 :m_quat(a_from.m_quat) 27 :m_quat(a_from.m_quat)
28 {} 28 {}
29 qrot& operator=(const qrot& a_from){ 29 qrot& operator=(const qrot& a_from){
30 m_quat = a_from.m_quat; 30 m_quat = a_from.m_quat;
31 return *this; 31 return *this;
32 } 32 }
33 protected: 33 protected:
34 qrot(T a_q0,T a_q1,T a_q2,T a_q3) 34 qrot(T a_q0,T a_q1,T a_q2,T a_q3)
35 :m_quat(a_q0,a_q1,a_q2,a_q3) 35 :m_quat(a_q0,a_q1,a_q2,a_q3)
36 { 36 {
37 if(!m_quat.normalize()) {} //FIXME throw 37 if(!m_quat.normalize()) {} //FIXME throw
38 } 38 }
39 39
40 public: 40 public:
41 qrot& operator*=(const qrot& a_q) { 41 qrot& operator*=(const qrot& a_q) {
42 //Multiplies the quaternions. 42 //Multiplies the quaternions.
43 //Note that order is important when combin 43 //Note that order is important when combining quaternions with the
44 //multiplication operator. 44 //multiplication operator.
45 // Formula from <http://www.lboro.ac.uk/de 45 // Formula from <http://www.lboro.ac.uk/departments/ma/gallery/quat/>
46 46
47 T tx = m_quat.v0(); 47 T tx = m_quat.v0();
48 T ty = m_quat.v1(); 48 T ty = m_quat.v1();
49 T tz = m_quat.v2(); 49 T tz = m_quat.v2();
50 T tw = m_quat.v3(); 50 T tw = m_quat.v3();
51 51
52 T qx = a_q.m_quat.v0(); 52 T qx = a_q.m_quat.v0();
53 T qy = a_q.m_quat.v1(); 53 T qy = a_q.m_quat.v1();
54 T qz = a_q.m_quat.v2(); 54 T qz = a_q.m_quat.v2();
55 T qw = a_q.m_quat.v3(); 55 T qw = a_q.m_quat.v3();
56 56
57 m_quat.set_value(qw*tx + qx*tw + qy*tz - q 57 m_quat.set_value(qw*tx + qx*tw + qy*tz - qz*ty,
58 qw*ty - qx*tz + qy*tw + q 58 qw*ty - qx*tz + qy*tw + qz*tx,
59 qw*tz + qx*ty - qy*tx + q 59 qw*tz + qx*ty - qy*tx + qz*tw,
60 qw*tw - qx*tx - qy*ty - q 60 qw*tw - qx*tx - qy*ty - qz*tz);
61 m_quat.normalize(); 61 m_quat.normalize();
62 return *this; 62 return *this;
63 } 63 }
64 64
65 bool operator==(const qrot& a_r) const { 65 bool operator==(const qrot& a_r) const {
66 return m_quat.equal(a_r.m_quat); 66 return m_quat.equal(a_r.m_quat);
67 } 67 }
68 bool operator!=(const qrot& a_r) const { 68 bool operator!=(const qrot& a_r) const {
69 return !operator==(a_r); 69 return !operator==(a_r);
70 } 70 }
71 qrot operator*(const qrot& a_r) const { 71 qrot operator*(const qrot& a_r) const {
72 qrot tmp(*this); 72 qrot tmp(*this);
73 tmp *= a_r; 73 tmp *= a_r;
74 return tmp; 74 return tmp;
75 } 75 }
76 76
77 bool invert(){ 77 bool invert(){
78 T length = m_quat.length(); 78 T length = m_quat.length();
79 if(length==T()) return false; 79 if(length==T()) return false;
80 80
81 // Optimize by doing 1 div and 4 muls inst 81 // Optimize by doing 1 div and 4 muls instead of 4 divs.
82 T inv = one() / length; 82 T inv = one() / length;
83 83
84 m_quat.set_value(-m_quat.v0() * inv, 84 m_quat.set_value(-m_quat.v0() * inv,
85 -m_quat.v1() * inv, 85 -m_quat.v1() * inv,
86 -m_quat.v2() * inv, 86 -m_quat.v2() * inv,
87 m_quat.v3() * inv); 87 m_quat.v3() * inv);
88 88
89 return true; 89 return true;
90 } 90 }
91 91
92 bool inverse(qrot& a_r) const { 92 bool inverse(qrot& a_r) const {
93 //Non-destructively inverses the rotation 93 //Non-destructively inverses the rotation and returns the result.
94 T length = m_quat.length(); 94 T length = m_quat.length();
95 if(length==T()) return false; 95 if(length==T()) return false;
96 96
97 // Optimize by doing 1 div and 4 muls inst 97 // Optimize by doing 1 div and 4 muls instead of 4 divs.
98 T inv = one() / length; 98 T inv = one() / length;
99 99
100 a_r.m_quat.set_value(-m_quat.v0() * inv, 100 a_r.m_quat.set_value(-m_quat.v0() * inv,
101 -m_quat.v1() * inv, 101 -m_quat.v1() * inv,
102 -m_quat.v2() * inv, 102 -m_quat.v2() * inv,
103 m_quat.v3() * inv); 103 m_quat.v3() * inv);
104 104
105 return true; 105 return true;
106 } 106 }
107 107
108 108
109 bool set_value(const VEC3& a_axis,T a_radian 109 bool set_value(const VEC3& a_axis,T a_radians,T(*a_sin)(T),T(*a_cos)(T)) {
110 // Reset rotation with the given axis-of-r 110 // Reset rotation with the given axis-of-rotation and rotation angle.
111 // Make sure axis is not the null vector w 111 // Make sure axis is not the null vector when calling this method.
112 // From <http://www.automation.hut.fi/~jar 112 // From <http://www.automation.hut.fi/~jaro/thesis/hyper/node9.html>.
113 if(a_axis.length()==T()) return false; 113 if(a_axis.length()==T()) return false;
114 m_quat.v3(a_cos(a_radians/2)); 114 m_quat.v3(a_cos(a_radians/2));
115 T sineval = a_sin(a_radians/2); 115 T sineval = a_sin(a_radians/2);
116 VEC3 a = a_axis; 116 VEC3 a = a_axis;
117 a.normalize(); 117 a.normalize();
118 m_quat.v0(a.v0() * sineval); 118 m_quat.v0(a.v0() * sineval);
119 m_quat.v1(a.v1() * sineval); 119 m_quat.v1(a.v1() * sineval);
120 m_quat.v2(a.v2() * sineval); 120 m_quat.v2(a.v2() * sineval);
121 return true; 121 return true;
122 } 122 }
123 123
124 bool set_value(const VEC3& a_from,const VEC3 124 bool set_value(const VEC3& a_from,const VEC3& a_to,T(*a_sqrt)(T),T(*a_fabs)(T)) {
125 // code taken from coin3d/SbRotation. 125 // code taken from coin3d/SbRotation.
126 //NOTE : coin3d/SbMatrix logic is transpos 126 //NOTE : coin3d/SbMatrix logic is transposed relative to us.
127 127
128 VEC3 from(a_from); 128 VEC3 from(a_from);
129 if(from.normalize()==T()) return false; 129 if(from.normalize()==T()) return false;
130 VEC3 to(a_to); 130 VEC3 to(a_to);
131 if(to.normalize()==T()) return false; 131 if(to.normalize()==T()) return false;
132 132
133 T dot = from.dot(to); 133 T dot = from.dot(to);
134 VEC3 crossvec;from.cross(to,crossvec); 134 VEC3 crossvec;from.cross(to,crossvec);
135 T crosslen = crossvec.normalize(); 135 T crosslen = crossvec.normalize();
136 136
137 if(crosslen == T()) { // Parallel vectors 137 if(crosslen == T()) { // Parallel vectors
138 // Check if they are pointing in the sam 138 // Check if they are pointing in the same direction.
139 if (dot > T()) { 139 if (dot > T()) {
140 m_quat.set_value(0,0,0,1); 140 m_quat.set_value(0,0,0,1);
141 } else { 141 } else {
142 // Ok, so they are parallel and pointi 142 // Ok, so they are parallel and pointing in the opposite direction
143 // of each other. 143 // of each other.
144 // Try crossing with x axis. 144 // Try crossing with x axis.
145 VEC3 t;from.cross(VEC3(1,0,0),t); 145 VEC3 t;from.cross(VEC3(1,0,0),t);
146 // If not ok, cross with y axis. 146 // If not ok, cross with y axis.
147 if(t.normalize() == T()) { 147 if(t.normalize() == T()) {
148 from.cross(VEC3(0,1,0),t); 148 from.cross(VEC3(0,1,0),t);
149 t.normalize(); 149 t.normalize();
150 } 150 }
151 m_quat.set_value(t[0],t[1],t[2],0); 151 m_quat.set_value(t[0],t[1],t[2],0);
152 } 152 }
153 } else { // Vectors are not parallel 153 } else { // Vectors are not parallel
154 // The fabs() wrapping is to avoid probl 154 // The fabs() wrapping is to avoid problems when `dot' "overflows"
155 // a tiny wee bit, which can lead to sqr 155 // a tiny wee bit, which can lead to sqrt() returning NaN.
156 crossvec *= (T)a_sqrt(half() * a_fabs(on 156 crossvec *= (T)a_sqrt(half() * a_fabs(one() - dot));
157 // The fabs() wrapping is to avoid probl 157 // The fabs() wrapping is to avoid problems when `dot' "underflows"
158 // a tiny wee bit, which can lead to sqr 158 // a tiny wee bit, which can lead to sqrt() returning NaN.
159 m_quat.set_value(crossvec[0], crossvec[1 159 m_quat.set_value(crossvec[0], crossvec[1], crossvec[2],(T)a_sqrt(half()*a_fabs(one()+dot)));
160 } 160 }
161 161
162 return true; 162 return true;
163 } 163 }
164 164
165 165
166 bool value(VEC3& a_axis,T& a_radians,T(*a_si 166 bool value(VEC3& a_axis,T& a_radians,T(*a_sin)(T),T(*a_acos)(T)) const { //WARNING a_acos and NOT a_cos
167 //WARNING : can fail. 167 //WARNING : can fail.
168 if( (m_quat.v3()<minus_one()) || (m_quat.v 168 if( (m_quat.v3()<minus_one()) || (m_quat.v3()> one()) ) {
169 a_axis.set_value(0,0,1); 169 a_axis.set_value(0,0,1);
170 a_radians = 0; 170 a_radians = 0;
171 return false; 171 return false;
172 } 172 }
173 173
174 a_radians = a_acos(m_quat.v3()) * 2; //in 174 a_radians = a_acos(m_quat.v3()) * 2; //in [0,2*pi]
175 T sineval = a_sin(a_radians/2); 175 T sineval = a_sin(a_radians/2);
176 176
177 if(sineval==T()) { //a_radian = 0 or 2*pi. 177 if(sineval==T()) { //a_radian = 0 or 2*pi.
178 a_axis.set_value(0,0,1); 178 a_axis.set_value(0,0,1);
179 a_radians = 0; 179 a_radians = 0;
180 return false; 180 return false;
181 } 181 }
182 a_axis.set_value(m_quat.v0()/sineval, 182 a_axis.set_value(m_quat.v0()/sineval,
183 m_quat.v1()/sineval, 183 m_quat.v1()/sineval,
184 m_quat.v2()/sineval); 184 m_quat.v2()/sineval);
185 return true; 185 return true;
186 } 186 }
187 187
188 template <class MAT4> 188 template <class MAT4>
189 void set_value(const MAT4& a_m,T(*a_sqrt)(T) 189 void set_value(const MAT4& a_m,T(*a_sqrt)(T)) {
190 // See tests/qrot.icc. 190 // See tests/qrot.icc.
191 // code taken from coin3d/SbRotation. 191 // code taken from coin3d/SbRotation.
192 192
193 //Set the rotation from the components of 193 //Set the rotation from the components of the given matrix.
194 194
195 T scalerow = a_m.v00() + a_m.v11() + a_m.v 195 T scalerow = a_m.v00() + a_m.v11() + a_m.v22();
196 if (scalerow > T()) { 196 if (scalerow > T()) {
197 T _s = a_sqrt(scalerow + a_m.v33()); 197 T _s = a_sqrt(scalerow + a_m.v33());
198 m_quat.v3(_s * half()); 198 m_quat.v3(_s * half());
199 _s = half() / _s; 199 _s = half() / _s;
200 200
201 m_quat.v0((a_m.v21() - a_m.v12()) * _s); 201 m_quat.v0((a_m.v21() - a_m.v12()) * _s);
202 m_quat.v1((a_m.v02() - a_m.v20()) * _s); 202 m_quat.v1((a_m.v02() - a_m.v20()) * _s);
203 m_quat.v2((a_m.v10() - a_m.v01()) * _s); 203 m_quat.v2((a_m.v10() - a_m.v01()) * _s);
204 } else { 204 } else {
205 unsigned int i = 0; 205 unsigned int i = 0;
206 if (a_m.v11() > a_m.v00()) i = 1; 206 if (a_m.v11() > a_m.v00()) i = 1;
207 if (a_m.v22() > a_m.value(i,i)) i = 2; 207 if (a_m.v22() > a_m.value(i,i)) i = 2;
208 208
209 unsigned int j = (i+1)%3; 209 unsigned int j = (i+1)%3;
210 unsigned int k = (j+1)%3; 210 unsigned int k = (j+1)%3;
211 211
212 T _s = a_sqrt((a_m.value(i,i) - (a_m.val 212 T _s = a_sqrt((a_m.value(i,i) - (a_m.value(j,j) + a_m.value(k,k))) + a_m.v33());
213 213
214 m_quat.set_value(i,_s * half()); 214 m_quat.set_value(i,_s * half());
215 _s = half() / _s; 215 _s = half() / _s;
216 216
217 m_quat.v3((a_m.value(k,j) - a_m.value(j, 217 m_quat.v3((a_m.value(k,j) - a_m.value(j,k)) * _s);
218 m_quat.set_value(j,(a_m.value(j,i) + a_m 218 m_quat.set_value(j,(a_m.value(j,i) + a_m.value(i,j)) * _s);
219 m_quat.set_value(k,(a_m.value(k,i) + a_m 219 m_quat.set_value(k,(a_m.value(k,i) + a_m.value(i,k)) * _s);
220 } 220 }
221 221
222 if (a_m.v33()!=one()) { 222 if (a_m.v33()!=one()) {
223 m_quat.multiply(one()/a_sqrt(a_m.v33())) 223 m_quat.multiply(one()/a_sqrt(a_m.v33()));
224 } 224 }
225 } 225 }
226 226
227 template <class MAT4> 227 template <class MAT4>
228 void value(MAT4& a_m) const { 228 void value(MAT4& a_m) const {
229 //Return this rotation in the form of a ma 229 //Return this rotation in the form of a matrix.
230 //NOTE : in coin3d/SbRotation, it looks as 230 //NOTE : in coin3d/SbRotation, it looks as if "w <-> -w", but coin3d/SbMatrix logic is transposed relative to us.
231 231
232 const T x = m_quat.v0(); 232 const T x = m_quat.v0();
233 const T y = m_quat.v1(); 233 const T y = m_quat.v1();
234 const T z = m_quat.v2(); 234 const T z = m_quat.v2();
235 const T w = m_quat.v3(); 235 const T w = m_quat.v3();
236 // q = w + x * i + y * j + z * k 236 // q = w + x * i + y * j + z * k
237 237
238 // first row : 238 // first row :
239 a_m.v00(w*w + x*x - y*y - z*z); 239 a_m.v00(w*w + x*x - y*y - z*z);
240 a_m.v01(2*x*y - 2*w*z); 240 a_m.v01(2*x*y - 2*w*z);
241 a_m.v02(2*x*z + 2*w*y); 241 a_m.v02(2*x*z + 2*w*y);
242 a_m.v03(0); 242 a_m.v03(0);
243 243
244 // second row : 244 // second row :
245 a_m.v10(2*x*y + 2*w*z); 245 a_m.v10(2*x*y + 2*w*z);
246 a_m.v11(w*w - x*x + y*y - z*z); 246 a_m.v11(w*w - x*x + y*y - z*z);
247 a_m.v12(2*y*z - 2*w*x); 247 a_m.v12(2*y*z - 2*w*x);
248 a_m.v13(0); 248 a_m.v13(0);
249 249
250 // third row : 250 // third row :
251 a_m.v20(2*x*z - 2*w*y); 251 a_m.v20(2*x*z - 2*w*y);
252 a_m.v21(2*y*z + 2*w*x); 252 a_m.v21(2*y*z + 2*w*x);
253 a_m.v22(w*w - x*x - y*y + z*z); 253 a_m.v22(w*w - x*x - y*y + z*z);
254 a_m.v23(0); 254 a_m.v23(0);
255 255
256 // fourth row : 256 // fourth row :
257 a_m.v30(0); 257 a_m.v30(0);
258 a_m.v31(0); 258 a_m.v31(0);
259 a_m.v32(0); 259 a_m.v32(0);
260 a_m.v33(w*w + x*x + y*y + z*z); 260 a_m.v33(w*w + x*x + y*y + z*z);
261 } 261 }
262 262
263 template <class MAT3> 263 template <class MAT3>
264 T value_3(MAT3& a_m) const { 264 T value_3(MAT3& a_m) const {
265 //Return this rotation in the form of a 3D 265 //Return this rotation in the form of a 3D matrix.
266 //NOTE : in coin3d/SbRotation, it looks as 266 //NOTE : in coin3d/SbRotation, it looks as if "w <-> -w", but coin3d/SbMatrix logic is transposed relative to us.
267 267
268 const T x = m_quat.v0(); 268 const T x = m_quat.v0();
269 const T y = m_quat.v1(); 269 const T y = m_quat.v1();
270 const T z = m_quat.v2(); 270 const T z = m_quat.v2();
271 const T w = m_quat.v3(); 271 const T w = m_quat.v3();
272 // q = w + x * i + y * j + z * k 272 // q = w + x * i + y * j + z * k
273 273
274 // first row : 274 // first row :
275 a_m.v00(w*w + x*x - y*y - z*z); 275 a_m.v00(w*w + x*x - y*y - z*z);
276 a_m.v01(2*x*y - 2*w*z); 276 a_m.v01(2*x*y - 2*w*z);
277 a_m.v02(2*x*z + 2*w*y); 277 a_m.v02(2*x*z + 2*w*y);
278 278
279 // second row : 279 // second row :
280 a_m.v10(2*x*y + 2*w*z); 280 a_m.v10(2*x*y + 2*w*z);
281 a_m.v11(w*w - x*x + y*y - z*z); 281 a_m.v11(w*w - x*x + y*y - z*z);
282 a_m.v12(2*y*z - 2*w*x); 282 a_m.v12(2*y*z - 2*w*x);
283 283
284 // third row : 284 // third row :
285 a_m.v20(2*x*z - 2*w*y); 285 a_m.v20(2*x*z - 2*w*y);
286 a_m.v21(2*y*z + 2*w*x); 286 a_m.v21(2*y*z + 2*w*x);
287 a_m.v22(w*w - x*x - y*y + z*z); 287 a_m.v22(w*w - x*x - y*y + z*z);
288 288
289 // fourth row : 289 // fourth row :
290 //a_m.v30(0); 290 //a_m.v30(0);
291 //a_m.v31(0); 291 //a_m.v31(0);
292 //a_m.v32(0); 292 //a_m.v32(0);
293 //a_m.v33(w*w + x*x + y*y + z*z); 293 //a_m.v33(w*w + x*x + y*y + z*z);
294 return w*w + x*x + y*y + z*z; //should be 294 return w*w + x*x + y*y + z*z; //should be 1.
295 } 295 }
296 296
297 void mul_vec(const VEC3& a_in,VEC3& a_out) c 297 void mul_vec(const VEC3& a_in,VEC3& a_out) const {
298 const T x = m_quat.v0(); 298 const T x = m_quat.v0();
299 const T y = m_quat.v1(); 299 const T y = m_quat.v1();
300 const T z = m_quat.v2(); 300 const T z = m_quat.v2();
301 const T w = m_quat.v3(); 301 const T w = m_quat.v3();
302 302
303 // first row : 303 // first row :
304 T v0 = (w*w + x*x - y*y - z*z) * a_in. 304 T v0 = (w*w + x*x - y*y - z*z) * a_in.v0()
305 + (2*x*y - 2*w*z) * a_in. 305 + (2*x*y - 2*w*z) * a_in.v1()
306 + (2*x*z + 2*w*y) * a_in. 306 + (2*x*z + 2*w*y) * a_in.v2();
307 307
308 T v1 = (2*x*y + 2*w*z) * a_in. 308 T v1 = (2*x*y + 2*w*z) * a_in.v0()
309 +(w*w - x*x + y*y - z*z) * a_in. 309 +(w*w - x*x + y*y - z*z) * a_in.v1()
310 + (2*y*z - 2*w*x) * a_in. 310 + (2*y*z - 2*w*x) * a_in.v2();
311 311
312 T v2 = (2*x*z - 2*w*y) * a_in. 312 T v2 = (2*x*z - 2*w*y) * a_in.v0()
313 + (2*y*z + 2*w*x) * a_in. 313 + (2*y*z + 2*w*x) * a_in.v1()
314 +(w*w - x*x - y*y + z*z) * a_in. 314 +(w*w - x*x - y*y + z*z) * a_in.v2();
315 315
316 a_out.set_value(v0,v1,v2); 316 a_out.set_value(v0,v1,v2);
317 } 317 }
318 318
319 void mul_vec(VEC3& a_v) const { 319 void mul_vec(VEC3& a_v) const {
320 const T x = m_quat.v0(); 320 const T x = m_quat.v0();
321 const T y = m_quat.v1(); 321 const T y = m_quat.v1();
322 const T z = m_quat.v2(); 322 const T z = m_quat.v2();
323 const T w = m_quat.v3(); 323 const T w = m_quat.v3();
324 324
325 // first row : 325 // first row :
326 T v0 = (w*w + x*x - y*y - z*z) * a_v.v 326 T v0 = (w*w + x*x - y*y - z*z) * a_v.v0()
327 + (2*x*y - 2*w*z) * a_v.v 327 + (2*x*y - 2*w*z) * a_v.v1()
328 + (2*x*z + 2*w*y) * a_v.v 328 + (2*x*z + 2*w*y) * a_v.v2();
329 329
330 T v1 = (2*x*y + 2*w*z) * a_v.v 330 T v1 = (2*x*y + 2*w*z) * a_v.v0()
331 +(w*w - x*x + y*y - z*z) * a_v.v 331 +(w*w - x*x + y*y - z*z) * a_v.v1()
332 + (2*y*z - 2*w*x) * a_v.v 332 + (2*y*z - 2*w*x) * a_v.v2();
333 333
334 T v2 = (2*x*z - 2*w*y) * a_v.v 334 T v2 = (2*x*z - 2*w*y) * a_v.v0()
335 + (2*y*z + 2*w*x) * a_v.v 335 + (2*y*z + 2*w*x) * a_v.v1()
336 +(w*w - x*x - y*y + z*z) * a_v.v 336 +(w*w - x*x - y*y + z*z) * a_v.v2();
337 337
338 a_v.set_value(v0,v1,v2); 338 a_v.set_value(v0,v1,v2);
339 } 339 }
340 340
341 void mul_3(T& a_x,T& a_y,T& a_z) const { 341 void mul_3(T& a_x,T& a_y,T& a_z) const {
342 const T x = m_quat.v0(); 342 const T x = m_quat.v0();
343 const T y = m_quat.v1(); 343 const T y = m_quat.v1();
344 const T z = m_quat.v2(); 344 const T z = m_quat.v2();
345 const T w = m_quat.v3(); 345 const T w = m_quat.v3();
346 346
347 // first row : 347 // first row :
348 T v0 = (w*w + x*x - y*y - z*z) * a_x 348 T v0 = (w*w + x*x - y*y - z*z) * a_x
349 + (2*x*y - 2*w*z) * a_y 349 + (2*x*y - 2*w*z) * a_y
350 + (2*x*z + 2*w*y) * a_z; 350 + (2*x*z + 2*w*y) * a_z;
351 351
352 T v1 = (2*x*y + 2*w*z) * a_x 352 T v1 = (2*x*y + 2*w*z) * a_x
353 +(w*w - x*x + y*y - z*z) * a_y 353 +(w*w - x*x + y*y - z*z) * a_y
354 + (2*y*z - 2*w*x) * a_z; 354 + (2*y*z - 2*w*x) * a_z;
355 355
356 T v2 = (2*x*z - 2*w*y) * a_x 356 T v2 = (2*x*z - 2*w*y) * a_x
357 + (2*y*z + 2*w*x) * a_y 357 + (2*y*z + 2*w*x) * a_y
358 +(w*w - x*x - y*y + z*z) * a_z; 358 +(w*w - x*x - y*y + z*z) * a_z;
359 359
360 a_x = v0; 360 a_x = v0;
361 a_y = v1; 361 a_y = v1;
362 a_z = v2; 362 a_z = v2;
363 } 363 }
364 364
365 public: //for io::streamer 365 public: //for io::streamer
366 const VEC4& quat() const {return m_quat;} 366 const VEC4& quat() const {return m_quat;}
367 VEC4& quat() {return m_quat;} 367 VEC4& quat() {return m_quat;}
368 368
369 protected: 369 protected:
370 static T one() {return T(1);} 370 static T one() {return T(1);}
371 static T minus_one() {return T(-1);} 371 static T minus_one() {return T(-1);}
372 static T half() {return T(0.5);} 372 static T half() {return T(0.5);}
373 373
374 protected: 374 protected:
375 VEC4 m_quat; 375 VEC4 m_quat;
376 376
377 public: 377 public:
378 //NOTE : don't handle a static object becaus 378 //NOTE : don't handle a static object because of mem balance.
379 //static const qrot<double>& identity() { 379 //static const qrot<double>& identity() {
380 // static const qrot<double> s_v(0,0,0,1); 380 // static const qrot<double> s_v(0,0,0,1);
381 // return s_v; 381 // return s_v;
382 //} 382 //}
383 383
384 //private:static void check_instantiation() {q 384 //private:static void check_instantiation() {qrot<float> v;}
385 }; 385 };
386 386
387 } 387 }
388 388
389 #endif 389 #endif