root/OpenSceneGraph/trunk/src/osg/Quat.cpp @ 13041

Revision 13041, 11.9 kB (checked in by robert, 2 years ago)

Ran script to remove trailing spaces and tabs

  • Property svn:eol-style set to native
  • Property svn:keywords set to Author Date Id Revision
Line 
1/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
2 *
3 * This library is open source and may be redistributed and/or modified under
4 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
5 * (at your option) any later version.  The full license is in LICENSE file
6 * included with this distribution, and on the openscenegraph.org website.
7 *
8 * This library is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11 * OpenSceneGraph Public License for more details.
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/// Good introductions to Quaternions at:
22/// http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
23/// http://mathworld.wolfram.com/Quaternion.html
24
25using namespace osg;
26
27
28void Quat::set(const Matrixf& matrix)
29{
30    *this = matrix.getRotate();
31}
32
33void Quat::set(const Matrixd& matrix)
34{
35    *this = matrix.getRotate();
36}
37
38void Quat::get(Matrixf& matrix) const
39{
40    matrix.makeRotate(*this);
41}
42
43void Quat::get(Matrixd& matrix) const
44{
45    matrix.makeRotate(*this);
46}
47
48
49/// Set the elements of the Quat to represent a rotation of angle
50/// (radians) around the axis (x,y,z)
51void 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        // ~zero length axis, so reset rotation to zero.
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
74void Quat::makeRotate( value_type angle, const Vec3f& vec )
75{
76    makeRotate( angle, vec[0], vec[1], vec[2] );
77}
78void Quat::makeRotate( value_type angle, const Vec3d& vec )
79{
80    makeRotate( angle, vec[0], vec[1], vec[2] );
81}
82
83
84void 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
93void 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
105void Quat::makeRotate( const Vec3f& from, const Vec3f& to )
106{
107    makeRotate( Vec3d(from), Vec3d(to) );
108}
109
110/** Make a rotation Quat which will rotate vec1 to vec2
111
112This routine uses only fast geometric transforms, without costly acos/sin computations.
113It's exact, fast, and with less degenerate cases than the acos/sin method.
114
115For an explanation of the math used, you may see for example:
116http://logiciels.cnes.fr/MARMOTTES/marmottes-mathematique.pdf
117
118@note This is the rotation with shortest angle, which is the one equivalent to the
119acos/sin transform method. Other rotations exists, for example to additionally keep
120a local horizontal attitude.
121
122@author Nicolas Brodu
123*/
124void Quat::makeRotate( const Vec3d& from, const Vec3d& to )
125{
126
127    // This routine takes any vector as argument but normalized
128    // vectors are necessary, if only for computing the dot product.
129    // Too bad the API is that generic, it leads to performance loss.
130    // Even in the case the 2 vectors are not normalized but same length,
131    // the sqrt could be shared, but we have no way to know beforehand
132    // at this point, while the caller may know.
133    // So, we have to test... in the hope of saving at least a sqrt
134    Vec3d sourceVector = from;
135    Vec3d targetVector = to;
136
137    value_type fromLen2 = from.length2();
138    value_type fromLen;
139    // normalize only when necessary, epsilon test
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    // normalize only when necessary, epsilon test
147    if ((toLen2 < 1.0-1e-7) || (toLen2 > 1.0+1e-7)) {
148        value_type toLen;
149        // re-use fromLen for case of mapping 2 vectors of the same length
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    // Now let's get into the real stuff
159    // Use "dot product plus one" as test as it can be re-used later on
160    double dotProdPlus1 = 1.0 + sourceVector * targetVector;
161
162    // Check for degenerate case of full u-turn. Use epsilon for detection
163    if (dotProdPlus1 < 1e-7) {
164
165        // Get an orthogonal vector of the given vector
166        // in a plane with maximum vector coordinates.
167        // Then use it as quaternion axis with pi angle
168        // Trick is to realize one value at least is >0.6 for a normalized vector.
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        // Find the shortest angle quaternion that transforms normalized vectors
192        // into one other. Formula is still valid when vectors are colinear
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// Make a rotation Quat which will rotate vec1 to vec2
204// Generally take adot product to get the angle between these
205// and then use a cross product to get the rotation axis
206// Watch out for the two special cases of when the vectors
207// are co-incident or opposite in direction.
208void 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    // dot product vec1*vec2
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        // cosangle is close to 1, so the vectors are close to being coincident
223        // Need to generate an angle of zero with any vector we like
224        // We'll choose (1,0,0)
225        makeRotate( 0.0, 0.0, 0.0, 1.0 );
226    }
227    else
228    if ( fabs(cosangle + 1.0) < epsilon )
229    {
230        // vectors are close to being opposite, so will need to find a
231        // vector orthongonal to from to rotate about.
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); // use x axis.
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        // find orthogonal axis.
242        Vec3d axis(fromd^tmp);
243        axis.normalize();
244
245        _v[0] = axis[0]; // sin of half angle of PI is 1.0.
246        _v[1] = axis[1]; // sin of half angle of PI is 1.0.
247        _v[2] = axis[2]; // sin of half angle of PI is 1.0.
248        _v[3] = 0; // cos of half angle of PI is zero.
249
250    }
251    else
252    {
253        // This is the usual situation - take a cross-product of vec1 and vec2
254        // and that is the axis around which to rotate.
255        Vec3d axis(from^to);
256        value_type angle = acos( cosangle );
257        makeRotate( angle, axis );
258    }
259}
260
261void 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}
269void 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// Get the angle of rotation and axis of this Quat object.
280// Won't give very meaningful results if the Quat is not associated
281// with a rotation!
282void 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/// Spherical Linear Interpolation
304/// As t goes from 0 to 1, the Quat object goes from "from" to "to"
305/// Reference: Shoemake at SIGGRAPH 89
306/// See also
307/// http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
308void 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    // this is a dot product
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) ;  // 0 <= omega <= Pi (see man acos)
327        sinomega = sin(omega) ;  // this sinomega should always be +ve so
328        // could try sinomega=sqrt(1-cosomega*cosomega) to avoid a sin()?
329        scale_from = sin((1.0-t)*omega)/sinomega ;
330        scale_to = sin(t*omega)/sinomega ;
331    }
332    else
333    {
334        /* --------------------------------------------------
335           The ends of the vectors are very close
336           we can use simple linear interpolation - no need
337           to worry about the "spherical" interpolation
338           -------------------------------------------------- */
339        scale_from = 1.0 - t ;
340        scale_to = t ;
341    }
342
343    *this = (from*scale_from) + (quatTo*scale_to);
344
345    // so that we get a Vec4
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
356void 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
388void 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
Note: See TracBrowser for help on using the browser.