1 | |
---|
2 | |
---|
3 | |
---|
4 | |
---|
5 | |
---|
6 | |
---|
7 | |
---|
8 | |
---|
9 | |
---|
10 | |
---|
11 | |
---|
12 | |
---|
13 | #include <stdio.h> |
---|
14 | #include <osg/Quat> |
---|
15 | #include <osg/Matrixf> |
---|
16 | #include <osg/Matrixd> |
---|
17 | #include <osg/Notify> |
---|
18 | |
---|
19 | #include <math.h> |
---|
20 | |
---|
21 | |
---|
22 | |
---|
23 | |
---|
24 | |
---|
25 | using namespace osg; |
---|
26 | |
---|
27 | |
---|
28 | void Quat::set(const Matrixf& matrix) |
---|
29 | { |
---|
30 | *this = matrix.getRotate(); |
---|
31 | } |
---|
32 | |
---|
33 | void Quat::set(const Matrixd& matrix) |
---|
34 | { |
---|
35 | *this = matrix.getRotate(); |
---|
36 | } |
---|
37 | |
---|
38 | void Quat::get(Matrixf& matrix) const |
---|
39 | { |
---|
40 | matrix.makeRotate(*this); |
---|
41 | } |
---|
42 | |
---|
43 | void Quat::get(Matrixd& matrix) const |
---|
44 | { |
---|
45 | matrix.makeRotate(*this); |
---|
46 | } |
---|
47 | |
---|
48 | |
---|
49 | |
---|
50 | |
---|
51 | void Quat::makeRotate( value_type angle, value_type x, value_type y, value_type z ) |
---|
52 | { |
---|
53 | const value_type epsilon = 0.0000001; |
---|
54 | |
---|
55 | value_type length = sqrt( x*x + y*y + z*z ); |
---|
56 | if (length < epsilon) |
---|
57 | { |
---|
58 | |
---|
59 | *this = Quat(); |
---|
60 | return; |
---|
61 | } |
---|
62 | |
---|
63 | value_type inversenorm = 1.0/length; |
---|
64 | value_type coshalfangle = cos( 0.5*angle ); |
---|
65 | value_type sinhalfangle = sin( 0.5*angle ); |
---|
66 | |
---|
67 | _v[0] = x * sinhalfangle * inversenorm; |
---|
68 | _v[1] = y * sinhalfangle * inversenorm; |
---|
69 | _v[2] = z * sinhalfangle * inversenorm; |
---|
70 | _v[3] = coshalfangle; |
---|
71 | } |
---|
72 | |
---|
73 | |
---|
74 | void Quat::makeRotate( value_type angle, const Vec3f& vec ) |
---|
75 | { |
---|
76 | makeRotate( angle, vec[0], vec[1], vec[2] ); |
---|
77 | } |
---|
78 | void Quat::makeRotate( value_type angle, const Vec3d& vec ) |
---|
79 | { |
---|
80 | makeRotate( angle, vec[0], vec[1], vec[2] ); |
---|
81 | } |
---|
82 | |
---|
83 | |
---|
84 | void Quat::makeRotate ( value_type angle1, const Vec3f& axis1, |
---|
85 | value_type angle2, const Vec3f& axis2, |
---|
86 | value_type angle3, const Vec3f& axis3) |
---|
87 | { |
---|
88 | makeRotate(angle1,Vec3d(axis1), |
---|
89 | angle2,Vec3d(axis2), |
---|
90 | angle3,Vec3d(axis3)); |
---|
91 | } |
---|
92 | |
---|
93 | void Quat::makeRotate ( value_type angle1, const Vec3d& axis1, |
---|
94 | value_type angle2, const Vec3d& axis2, |
---|
95 | value_type angle3, const Vec3d& axis3) |
---|
96 | { |
---|
97 | Quat q1; q1.makeRotate(angle1,axis1); |
---|
98 | Quat q2; q2.makeRotate(angle2,axis2); |
---|
99 | Quat q3; q3.makeRotate(angle3,axis3); |
---|
100 | |
---|
101 | *this = q1*q2*q3; |
---|
102 | } |
---|
103 | |
---|
104 | |
---|
105 | void Quat::makeRotate( const Vec3f& from, const Vec3f& to ) |
---|
106 | { |
---|
107 | makeRotate( Vec3d(from), Vec3d(to) ); |
---|
108 | } |
---|
109 | |
---|
110 | |
---|
111 | |
---|
112 | |
---|
113 | |
---|
114 | |
---|
115 | |
---|
116 | |
---|
117 | |
---|
118 | |
---|
119 | |
---|
120 | |
---|
121 | |
---|
122 | |
---|
123 | |
---|
124 | void Quat::makeRotate( const Vec3d& from, const Vec3d& to ) |
---|
125 | { |
---|
126 | |
---|
127 | |
---|
128 | |
---|
129 | |
---|
130 | |
---|
131 | |
---|
132 | |
---|
133 | |
---|
134 | Vec3d sourceVector = from; |
---|
135 | Vec3d targetVector = to; |
---|
136 | |
---|
137 | value_type fromLen2 = from.length2(); |
---|
138 | value_type fromLen; |
---|
139 | |
---|
140 | if ((fromLen2 < 1.0-1e-7) || (fromLen2 > 1.0+1e-7)) { |
---|
141 | fromLen = sqrt(fromLen2); |
---|
142 | sourceVector /= fromLen; |
---|
143 | } else fromLen = 1.0; |
---|
144 | |
---|
145 | value_type toLen2 = to.length2(); |
---|
146 | |
---|
147 | if ((toLen2 < 1.0-1e-7) || (toLen2 > 1.0+1e-7)) { |
---|
148 | value_type toLen; |
---|
149 | |
---|
150 | if ((toLen2 > fromLen2-1e-7) && (toLen2 < fromLen2+1e-7)) { |
---|
151 | toLen = fromLen; |
---|
152 | } |
---|
153 | else toLen = sqrt(toLen2); |
---|
154 | targetVector /= toLen; |
---|
155 | } |
---|
156 | |
---|
157 | |
---|
158 | |
---|
159 | |
---|
160 | double dotProdPlus1 = 1.0 + sourceVector * targetVector; |
---|
161 | |
---|
162 | |
---|
163 | if (dotProdPlus1 < 1e-7) { |
---|
164 | |
---|
165 | |
---|
166 | |
---|
167 | |
---|
168 | |
---|
169 | if (fabs(sourceVector.x()) < 0.6) { |
---|
170 | const double norm = sqrt(1.0 - sourceVector.x() * sourceVector.x()); |
---|
171 | _v[0] = 0.0; |
---|
172 | _v[1] = sourceVector.z() / norm; |
---|
173 | _v[2] = -sourceVector.y() / norm; |
---|
174 | _v[3] = 0.0; |
---|
175 | } else if (fabs(sourceVector.y()) < 0.6) { |
---|
176 | const double norm = sqrt(1.0 - sourceVector.y() * sourceVector.y()); |
---|
177 | _v[0] = -sourceVector.z() / norm; |
---|
178 | _v[1] = 0.0; |
---|
179 | _v[2] = sourceVector.x() / norm; |
---|
180 | _v[3] = 0.0; |
---|
181 | } else { |
---|
182 | const double norm = sqrt(1.0 - sourceVector.z() * sourceVector.z()); |
---|
183 | _v[0] = sourceVector.y() / norm; |
---|
184 | _v[1] = -sourceVector.x() / norm; |
---|
185 | _v[2] = 0.0; |
---|
186 | _v[3] = 0.0; |
---|
187 | } |
---|
188 | } |
---|
189 | |
---|
190 | else { |
---|
191 | |
---|
192 | |
---|
193 | const double s = sqrt(0.5 * dotProdPlus1); |
---|
194 | const Vec3d tmp = sourceVector ^ targetVector / (2.0*s); |
---|
195 | _v[0] = tmp.x(); |
---|
196 | _v[1] = tmp.y(); |
---|
197 | _v[2] = tmp.z(); |
---|
198 | _v[3] = s; |
---|
199 | } |
---|
200 | } |
---|
201 | |
---|
202 | |
---|
203 | |
---|
204 | |
---|
205 | |
---|
206 | |
---|
207 | |
---|
208 | void Quat::makeRotate_original( const Vec3d& from, const Vec3d& to ) |
---|
209 | { |
---|
210 | const value_type epsilon = 0.0000001; |
---|
211 | |
---|
212 | value_type length1 = from.length(); |
---|
213 | value_type length2 = to.length(); |
---|
214 | |
---|
215 | |
---|
216 | value_type cosangle = from*to/(length1*length2); |
---|
217 | |
---|
218 | if ( fabs(cosangle - 1) < epsilon ) |
---|
219 | { |
---|
220 | OSG_INFO<<"*** Quat::makeRotate(from,to) with near co-linear vectors, epsilon= "<<fabs(cosangle-1)<<std::endl; |
---|
221 | |
---|
222 | |
---|
223 | |
---|
224 | |
---|
225 | makeRotate( 0.0, 0.0, 0.0, 1.0 ); |
---|
226 | } |
---|
227 | else |
---|
228 | if ( fabs(cosangle + 1.0) < epsilon ) |
---|
229 | { |
---|
230 | |
---|
231 | |
---|
232 | Vec3d tmp; |
---|
233 | if (fabs(from.x())<fabs(from.y())) |
---|
234 | if (fabs(from.x())<fabs(from.z())) tmp.set(1.0,0.0,0.0); |
---|
235 | else tmp.set(0.0,0.0,1.0); |
---|
236 | else if (fabs(from.y())<fabs(from.z())) tmp.set(0.0,1.0,0.0); |
---|
237 | else tmp.set(0.0,0.0,1.0); |
---|
238 | |
---|
239 | Vec3d fromd(from.x(),from.y(),from.z()); |
---|
240 | |
---|
241 | |
---|
242 | Vec3d axis(fromd^tmp); |
---|
243 | axis.normalize(); |
---|
244 | |
---|
245 | _v[0] = axis[0]; |
---|
246 | _v[1] = axis[1]; |
---|
247 | _v[2] = axis[2]; |
---|
248 | _v[3] = 0; |
---|
249 | |
---|
250 | } |
---|
251 | else |
---|
252 | { |
---|
253 | |
---|
254 | |
---|
255 | Vec3d axis(from^to); |
---|
256 | value_type angle = acos( cosangle ); |
---|
257 | makeRotate( angle, axis ); |
---|
258 | } |
---|
259 | } |
---|
260 | |
---|
261 | void Quat::getRotate( value_type& angle, Vec3f& vec ) const |
---|
262 | { |
---|
263 | value_type x,y,z; |
---|
264 | getRotate(angle,x,y,z); |
---|
265 | vec[0]=x; |
---|
266 | vec[1]=y; |
---|
267 | vec[2]=z; |
---|
268 | } |
---|
269 | void Quat::getRotate( value_type& angle, Vec3d& vec ) const |
---|
270 | { |
---|
271 | value_type x,y,z; |
---|
272 | getRotate(angle,x,y,z); |
---|
273 | vec[0]=x; |
---|
274 | vec[1]=y; |
---|
275 | vec[2]=z; |
---|
276 | } |
---|
277 | |
---|
278 | |
---|
279 | |
---|
280 | |
---|
281 | |
---|
282 | void Quat::getRotate( value_type& angle, value_type& x, value_type& y, value_type& z ) const |
---|
283 | { |
---|
284 | value_type sinhalfangle = sqrt( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] ); |
---|
285 | |
---|
286 | angle = 2.0 * atan2( sinhalfangle, _v[3] ); |
---|
287 | if(sinhalfangle) |
---|
288 | { |
---|
289 | x = _v[0] / sinhalfangle; |
---|
290 | y = _v[1] / sinhalfangle; |
---|
291 | z = _v[2] / sinhalfangle; |
---|
292 | } |
---|
293 | else |
---|
294 | { |
---|
295 | x = 0.0; |
---|
296 | y = 0.0; |
---|
297 | z = 1.0; |
---|
298 | } |
---|
299 | |
---|
300 | } |
---|
301 | |
---|
302 | |
---|
303 | |
---|
304 | |
---|
305 | |
---|
306 | |
---|
307 | |
---|
308 | void Quat::slerp( value_type t, const Quat& from, const Quat& to ) |
---|
309 | { |
---|
310 | const double epsilon = 0.00001; |
---|
311 | double omega, cosomega, sinomega, scale_from, scale_to ; |
---|
312 | |
---|
313 | osg::Quat quatTo(to); |
---|
314 | |
---|
315 | |
---|
316 | cosomega = from.asVec4() * to.asVec4(); |
---|
317 | |
---|
318 | if ( cosomega <0.0 ) |
---|
319 | { |
---|
320 | cosomega = -cosomega; |
---|
321 | quatTo = -to; |
---|
322 | } |
---|
323 | |
---|
324 | if( (1.0 - cosomega) > epsilon ) |
---|
325 | { |
---|
326 | omega= acos(cosomega) ; |
---|
327 | sinomega = sin(omega) ; |
---|
328 | |
---|
329 | scale_from = sin((1.0-t)*omega)/sinomega ; |
---|
330 | scale_to = sin(t*omega)/sinomega ; |
---|
331 | } |
---|
332 | else |
---|
333 | { |
---|
334 | |
---|
335 | |
---|
336 | |
---|
337 | |
---|
338 | |
---|
339 | scale_from = 1.0 - t ; |
---|
340 | scale_to = t ; |
---|
341 | } |
---|
342 | |
---|
343 | *this = (from*scale_from) + (quatTo*scale_to); |
---|
344 | |
---|
345 | |
---|
346 | } |
---|
347 | |
---|
348 | |
---|
349 | #define QX _v[0] |
---|
350 | #define QY _v[1] |
---|
351 | #define QZ _v[2] |
---|
352 | #define QW _v[3] |
---|
353 | |
---|
354 | |
---|
355 | #ifdef OSG_USE_UNIT_TESTS |
---|
356 | void test_Quat_Eueler(value_type heading,value_type pitch,value_type roll) |
---|
357 | { |
---|
358 | osg::Quat q; |
---|
359 | q.makeRotate(heading,pitch,roll); |
---|
360 | |
---|
361 | osg::Matrix q_m; |
---|
362 | q.get(q_m); |
---|
363 | |
---|
364 | osg::Vec3 xAxis(1,0,0); |
---|
365 | osg::Vec3 yAxis(0,1,0); |
---|
366 | osg::Vec3 zAxis(0,0,1); |
---|
367 | |
---|
368 | cout << "heading = "<<heading<<" pitch = "<<pitch<<" roll = "<<roll<<endl; |
---|
369 | |
---|
370 | cout <<"q_m = "<<q_m; |
---|
371 | cout <<"xAxis*q_m = "<<xAxis*q_m << endl; |
---|
372 | cout <<"yAxis*q_m = "<<yAxis*q_m << endl; |
---|
373 | cout <<"zAxis*q_m = "<<zAxis*q_m << endl; |
---|
374 | |
---|
375 | osg::Matrix r_m = osg::Matrix::rotate(roll,0.0,1.0,0.0)* |
---|
376 | osg::Matrix::rotate(pitch,1.0,0.0,0.0)* |
---|
377 | osg::Matrix::rotate(-heading,0.0,0.0,1.0); |
---|
378 | |
---|
379 | cout << "r_m = "<<r_m; |
---|
380 | cout <<"xAxis*r_m = "<<xAxis*r_m << endl; |
---|
381 | cout <<"yAxis*r_m = "<<yAxis*r_m << endl; |
---|
382 | cout <<"zAxis*r_m = "<<zAxis*r_m << endl; |
---|
383 | |
---|
384 | cout << endl<<"*****************************************" << endl<< endl; |
---|
385 | |
---|
386 | } |
---|
387 | |
---|
388 | void test_Quat() |
---|
389 | { |
---|
390 | |
---|
391 | test_Quat_Eueler(osg::DegreesToRadians(20),0,0); |
---|
392 | test_Quat_Eueler(0,osg::DegreesToRadians(20),0); |
---|
393 | test_Quat_Eueler(0,0,osg::DegreesToRadians(20)); |
---|
394 | test_Quat_Eueler(osg::DegreesToRadians(20),osg::DegreesToRadians(20),osg::DegreesToRadians(20)); |
---|
395 | return 0; |
---|
396 | } |
---|
397 | #endif |
---|