Changeset 13041 for OpenSceneGraph/trunk/src/osg/Quat.cpp
- Timestamp:
- 03/21/12 18:36:20 (14 months ago)
- Files:
-
- 1 modified
-
OpenSceneGraph/trunk/src/osg/Quat.cpp (modified) (18 diffs)
Legend:
- Unmodified
- Added
- Removed
-
OpenSceneGraph/trunk/src/osg/Quat.cpp
r11472 r13041 1 /* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield 1 /* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield 2 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 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 5 * (at your option) any later version. The full license is in LICENSE file 6 6 * included with this distribution, and on the openscenegraph.org website. 7 * 7 * 8 8 * This library is distributed in the hope that it will be useful, 9 9 * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 11 * OpenSceneGraph Public License for more details. 12 12 */ … … 82 82 83 83 84 void Quat::makeRotate ( value_type angle1, const Vec3f& axis1, 84 void Quat::makeRotate ( value_type angle1, const Vec3f& axis1, 85 85 value_type angle2, const Vec3f& axis2, 86 86 value_type angle3, const Vec3f& axis3) … … 89 89 angle2,Vec3d(axis2), 90 90 angle3,Vec3d(axis3)); 91 } 92 93 void Quat::makeRotate ( value_type angle1, const Vec3d& axis1, 91 } 92 93 void Quat::makeRotate ( value_type angle1, const Vec3d& axis1, 94 94 value_type angle2, const Vec3d& axis2, 95 95 value_type angle3, const Vec3d& axis3) … … 100 100 101 101 *this = q1*q2*q3; 102 } 102 } 103 103 104 104 … … 110 110 /** Make a rotation Quat which will rotate vec1 to vec2 111 111 112 This routine uses only fast geometric transforms, without costly acos/sin computations. 112 This routine uses only fast geometric transforms, without costly acos/sin computations. 113 113 It's exact, fast, and with less degenerate cases than the acos/sin method. 114 114 115 For an explanation of the math used, you may see for example: 115 For an explanation of the math used, you may see for example: 116 116 http://logiciels.cnes.fr/MARMOTTES/marmottes-mathematique.pdf 117 117 118 @note This is the rotation with shortest angle, which is the one equivalent to the 119 acos/sin transform method. Other rotations exists, for example to additionally keep 118 @note This is the rotation with shortest angle, which is the one equivalent to the 119 acos/sin transform method. Other rotations exists, for example to additionally keep 120 120 a local horizontal attitude. 121 121 … … 125 125 { 126 126 127 // This routine takes any vector as argument but normalized 127 // This routine takes any vector as argument but normalized 128 128 // vectors are necessary, if only for computing the dot product. 129 129 // Too bad the API is that generic, it leads to performance loss. … … 134 134 Vec3d sourceVector = from; 135 135 Vec3d targetVector = to; 136 136 137 137 value_type fromLen2 = from.length2(); 138 138 value_type fromLen; … … 142 142 sourceVector /= fromLen; 143 143 } else fromLen = 1.0; 144 144 145 145 value_type toLen2 = to.length2(); 146 146 // normalize only when necessary, epsilon test … … 150 150 if ((toLen2 > fromLen2-1e-7) && (toLen2 < fromLen2+1e-7)) { 151 151 toLen = fromLen; 152 } 152 } 153 153 else toLen = sqrt(toLen2); 154 154 targetVector /= toLen; 155 155 } 156 156 157 157 158 158 // Now let's get into the real stuff 159 159 // Use "dot product plus one" as test as it can be re-used later on 160 160 double dotProdPlus1 = 1.0 + sourceVector * targetVector; 161 161 162 162 // Check for degenerate case of full u-turn. Use epsilon for detection 163 163 if (dotProdPlus1 < 1e-7) { 164 164 165 165 // Get an orthogonal vector of the given vector 166 166 // in a plane with maximum vector coordinates. … … 169 169 if (fabs(sourceVector.x()) < 0.6) { 170 170 const double norm = sqrt(1.0 - sourceVector.x() * sourceVector.x()); 171 _v[0] = 0.0; 171 _v[0] = 0.0; 172 172 _v[1] = sourceVector.z() / norm; 173 173 _v[2] = -sourceVector.y() / norm; … … 187 187 } 188 188 } 189 189 190 190 else { 191 191 // Find the shortest angle quaternion that transforms normalized vectors … … 212 212 value_type length1 = from.length(); 213 213 value_type length2 = to.length(); 214 214 215 215 // dot product vec1*vec2 216 216 value_type cosangle = from*to/(length1*length2); … … 219 219 { 220 220 OSG_INFO<<"*** Quat::makeRotate(from,to) with near co-linear vectors, epsilon= "<<fabs(cosangle-1)<<std::endl; 221 221 222 222 // cosangle is close to 1, so the vectors are close to being coincident 223 223 // Need to generate an angle of zero with any vector we like … … 236 236 else if (fabs(from.y())<fabs(from.z())) tmp.set(0.0,1.0,0.0); 237 237 else tmp.set(0.0,0.0,1.0); 238 238 239 239 Vec3d fromd(from.x(),from.y(),from.z()); 240 240 241 241 // find orthogonal axis. 242 242 Vec3d axis(fromd^tmp); 243 243 axis.normalize(); 244 244 245 245 _v[0] = axis[0]; // sin of half angle of PI is 1.0. 246 246 _v[1] = axis[1]; // sin of half angle of PI is 1.0. … … 310 310 const double epsilon = 0.00001; 311 311 double omega, cosomega, sinomega, scale_from, scale_to ; 312 312 313 313 osg::Quat quatTo(to); 314 314 // this is a dot product 315 315 316 316 cosomega = from.asVec4() * to.asVec4(); 317 317 318 318 if ( cosomega <0.0 ) 319 { 320 cosomega = -cosomega; 319 { 320 cosomega = -cosomega; 321 321 quatTo = -to; 322 322 } … … 342 342 343 343 *this = (from*scale_from) + (quatTo*scale_to); 344 344 345 345 // so that we get a Vec4 346 346 } … … 358 358 osg::Quat q; 359 359 q.makeRotate(heading,pitch,roll); 360 360 361 361 osg::Matrix q_m; 362 362 q.get(q_m); 363 363 364 364 osg::Vec3 xAxis(1,0,0); 365 365 osg::Vec3 yAxis(0,1,0); 366 366 osg::Vec3 zAxis(0,0,1); 367 367 368 368 cout << "heading = "<<heading<<" pitch = "<<pitch<<" roll = "<<roll<<endl; 369 369 … … 372 372 cout <<"yAxis*q_m = "<<yAxis*q_m << endl; 373 373 cout <<"zAxis*q_m = "<<zAxis*q_m << endl; 374 374 375 375 osg::Matrix r_m = osg::Matrix::rotate(roll,0.0,1.0,0.0)* 376 376 osg::Matrix::rotate(pitch,1.0,0.0,0.0)* 377 377 osg::Matrix::rotate(-heading,0.0,0.0,1.0); 378 378 379 379 cout << "r_m = "<<r_m; 380 380 cout <<"xAxis*r_m = "<<xAxis*r_m << endl; 381 381 cout <<"yAxis*r_m = "<<yAxis*r_m << endl; 382 382 cout <<"zAxis*r_m = "<<zAxis*r_m << endl; 383 383 384 384 cout << endl<<"*****************************************" << endl<< endl; 385 385 386 386 } 387 387
