Simbody
|
00001 //----------------------------------------------------------------------------- 00002 // File: Quaternion.h 00003 // Class: Quaternion 00004 // Parent: Vec4 00005 // Purpose: Quaternion (Euler parameters) for representing orientation 00006 //----------------------------------------------------------------------------- 00007 #ifndef SimTK_QUATERNION_H 00008 #define SimTK_QUATERNION_H 00009 00010 /* -------------------------------------------------------------------------- * 00011 * SimTK Core: SimTK Simmatrix(tm) * 00012 * -------------------------------------------------------------------------- * 00013 * This is part of the SimTK Core biosimulation toolkit originating from * 00014 * Simbios, the NIH National Center for Physics-Based Simulation of * 00015 * Biological Structures at Stanford, funded under the NIH Roadmap for * 00016 * Medical Research, grant U54 GM072970. See https://simtk.org. * 00017 * * 00018 * Portions copyright (c) 2005-9 Stanford University and the Authors. * 00019 * Authors: Michael Sherman and Paul Mitiguy * 00020 * * 00021 * Permission is hereby granted, free of charge, to any person obtaining a * 00022 * copy of this software and associated documentation files (the "Software"), * 00023 * to deal in the Software without restriction, including without limitation * 00024 * the rights to use, copy, modify, merge, publish, distribute, sublicense, * 00025 * and/or sell copies of the Software, and to permit persons to whom the * 00026 * Software is furnished to do so, subject to the following conditions: * 00027 * * 00028 * The above copyright notice and this permission notice shall be included in * 00029 * all copies or substantial portions of the Software. * 00030 * * 00031 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * 00032 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * 00033 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL * 00034 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, * 00035 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR * 00036 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE * 00037 * USE OR OTHER DEALINGS IN THE SOFTWARE. * 00038 * -------------------------------------------------------------------------- */ 00039 00040 //----------------------------------------------------------------------------- 00041 #include "SimTKcommon/SmallMatrix.h" 00042 #include "SimTKcommon/internal/UnitVec.h" 00043 //----------------------------------------------------------------------------- 00044 #include <iosfwd> // Forward declaration of iostream 00045 //----------------------------------------------------------------------------- 00046 00047 00048 //----------------------------------------------------------------------------- 00049 namespace SimTK { 00050 00051 //----------------------------------------------------------------------------- 00052 // Forward declarations 00053 template <class P> class Rotation_; 00054 template <class P> class Quaternion_; 00055 00056 typedef Quaternion_<Real> Quaternion; 00057 typedef Quaternion_<float> fQuaternion; 00058 typedef Quaternion_<double> dQuaternion; 00059 00060 //----------------------------------------------------------------------------- 00076 //----------------------------------------------------------------------------- 00077 template <class P> 00078 class Quaternion_ : public Vec<4,P> { 00079 typedef P RealP; 00080 typedef Vec<3,P> Vec3P; 00081 typedef Vec<4,P> Vec4P; 00082 public: 00085 Quaternion_() : Vec4P(1,0,0,0) { } 00086 00089 Quaternion_(const Quaternion_& q) : Vec4P(q) {} 00090 00093 Quaternion_& operator=( const Quaternion_& q ) 00094 { Vec4P::operator=(q.asVec4()); return *this; } 00095 00098 Quaternion_( RealP e0, RealP e1, RealP e2, RealP e3 ) : Vec4P(e0,e1,e2,e3) 00099 { normalizeThis(); } 00102 explicit Quaternion_( const Vec4P& q ) : Vec4P(q) 00103 { normalizeThis(); } 00104 00107 SimTK_SimTKCOMMON_EXPORT explicit Quaternion_(const Rotation_<P>&); 00108 00111 void setQuaternionToZeroRotation() {Vec4P::operator=( Vec4P(1,0,0,0) );} 00115 void setQuaternionToNaN() {Vec4P::setToNaN();} 00116 00121 SimTK_SimTKCOMMON_EXPORT void setQuaternionFromAngleAxis(const Vec4P& av); 00125 SimTK_SimTKCOMMON_EXPORT void setQuaternionFromAngleAxis(const RealP& a, const UnitVec<P,1>& v); 00126 00129 SimTK_SimTKCOMMON_EXPORT Vec4P convertQuaternionToAngleAxis() const; 00130 00133 const Vec4P& asVec4() const {return *static_cast<const Vec4P*>(this);} 00134 00145 Quaternion_& normalizeThis() { 00146 const RealP epsilon = std::numeric_limits<RealP>::epsilon(); 00147 const RealP magnitude = Vec4P::norm(); 00148 if( magnitude == 0 ) setQuaternionToZeroRotation(); 00149 else if( magnitude < epsilon ) setQuaternionToNaN(); 00150 else (*this) *= (1/magnitude); 00151 return *this; 00152 } 00153 00160 Quaternion_ normalize() const {return Quaternion_(*this).normalizeThis();} 00161 00167 Quaternion_(const Vec4P& v, bool) : Vec4P(v) {} 00168 00169 //---------------------------------------------------------------------------------------------------- 00170 // The following code is obsolete - it is here temporarily for backward compatibility (Mitiguy 10/13/2007) 00171 //---------------------------------------------------------------------------------------------------- 00172 private: 00173 Vec4P convertToAngleAxis() const { return convertQuaternionToAngleAxis();} 00174 void setToAngleAxis(const Vec4P& av) { setQuaternionFromAngleAxis(av);} 00175 void setToAngleAxis(const RealP& a, const UnitVec<P,1>& v){ setQuaternionFromAngleAxis(a,v);} 00176 void setToNaN() { setQuaternionToNaN(); } 00177 void setToZero() { setQuaternionToZeroRotation();} 00178 00179 }; 00180 00181 00182 //------------------------------------------------------------------------------ 00183 } // End of namespace SimTK 00184 00185 //-------------------------------------------------------------------------- 00186 #endif // SimTK_QUATERNION_H_ 00187 //-------------------------------------------------------------------------- 00188