Simbody  3.5
Quaternion.h
Go to the documentation of this file.
1 #ifndef SimTK_SimTKCOMMON_QUATERNION_H
2 #define SimTK_SimTKCOMMON_QUATERNION_H
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm): SimTKcommon *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2005-12 Stanford University and the Authors. *
13  * Authors: Michael Sherman, Paul Mitiguy *
14  * Contributors: *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
27 //-----------------------------------------------------------------------------
30 //-----------------------------------------------------------------------------
31 #include <iosfwd> // Forward declaration of iostream
32 //-----------------------------------------------------------------------------
33 
34 
35 //-----------------------------------------------------------------------------
36 namespace SimTK {
37 
38 //-----------------------------------------------------------------------------
39 // Forward declarations
40 template <class P> class Rotation_;
41 template <class P> class Quaternion_;
42 
46 
47 //-----------------------------------------------------------------------------
63 //-----------------------------------------------------------------------------
64 template <class P>
65 class Quaternion_ : public Vec<4,P> {
66  typedef P RealP;
67  typedef Vec<3,P> Vec3P;
68  typedef Vec<4,P> Vec4P;
69 public:
72  Quaternion_() : Vec4P(1,0,0,0) { }
73 
76  Quaternion_(const Quaternion_& q) : Vec4P(q) {}
77 
81  { Vec4P::operator=(q.asVec4()); return *this; }
82 
85  Quaternion_( RealP e0, RealP e1, RealP e2, RealP e3 ) : Vec4P(e0,e1,e2,e3)
86  { normalizeThis(); }
89  explicit Quaternion_( const Vec4P& q ) : Vec4P(q)
90  { normalizeThis(); }
91 
95 
98  void setQuaternionToZeroRotation() {Vec4P::operator=( Vec4P(1,0,0,0) );}
103 
113 
117 
120  const Vec4P& asVec4() const {return *static_cast<const Vec4P*>(this);}
121 
132  Quaternion_& normalizeThis() {
133  const RealP epsilon = std::numeric_limits<RealP>::epsilon();
134  const RealP magnitude = Vec4P::norm();
135  if( magnitude == 0 ) setQuaternionToZeroRotation();
136  else if( magnitude < epsilon ) setQuaternionToNaN();
137  else (*this) *= (1/magnitude);
138  return *this;
139  }
140 
147  Quaternion_ normalize() const {return Quaternion_(*this).normalizeThis();}
148 
154  Quaternion_(const Vec4P& v, bool) : Vec4P(v) {}
155 
156 //----------------------------------------------------------------------------------------------------
157 // The following code is obsolete - it is here temporarily for backward compatibility (Mitiguy 10/13/2007)
158 //----------------------------------------------------------------------------------------------------
159 private:
160  Vec4P convertToAngleAxis() const { return convertQuaternionToAngleAxis();}
161  void setToAngleAxis(const Vec4P& av) { setQuaternionFromAngleAxis(av);}
162  void setToAngleAxis(const RealP& a, const UnitVec<P,1>& v){ setQuaternionFromAngleAxis(a,v);}
163  void setToNaN() { setQuaternionToNaN(); }
164  void setToZero() { setQuaternionToZeroRotation();}
165 
166 };
167 
168 
169 //------------------------------------------------------------------------------
170 } // End of namespace SimTK
171 
172 //--------------------------------------------------------------------------
173 #endif // SimTK_SimTKCOMMON_QUATERNION_H_
174 //--------------------------------------------------------------------------
175 
void setQuaternionFromAngleAxis(const Vec4P &av)
Set this quaternion from an angle-axis rotation packed into a 4-vector as [a vx vy vz]...
#define SimTK_SimTKCOMMON_EXPORT
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:218
const Vec4P & asVec4() const
Zero-cost cast of a Quaternion_ to its underlying Vec4; this is not converted to axis-angle form...
Definition: Quaternion.h:120
Quaternion_< Real > Quaternion
Definition: Quaternion.h:41
This class is a Vec3 plus an ironclad guarantee either that:
Definition: UnitVec.h:40
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
Quaternion_(RealP e0, RealP e1, RealP e2, RealP e3)
Construct a quaternion from four scalars and normalize the result, which costs about 40 flops...
Definition: Quaternion.h:85
The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 ...
Definition: Quaternion.h:40
Quaternion_(const Vec4P &v, bool)
Use this constructor only if you are *sure* v is normalized to 1.0.
Definition: Quaternion.h:154
Quaternion_(const Quaternion_ &q)
Zero-cost copy constructor just copies the source without conversion to canonical form or normalizati...
Definition: Quaternion.h:76
Quaternion_()
Default constructor produces the ZeroRotation quaternion [1 0 0 0] (not NaN - even in debug mode)...
Definition: Quaternion.h:72
void setQuaternionToZeroRotation()
The ZeroRotation quaternion is [1 0 0 0].
Definition: Quaternion.h:98
Vec4P convertQuaternionToAngleAxis() const
Returns [ a vx vy vz ] with (a,v) in canonical form, i.e., -180 < a <= 180 and |v|=1.
Quaternion_< double > dQuaternion
Definition: Quaternion.h:45
Declares and defines the UnitVec and UnitRow classes.
Quaternion_ & normalizeThis()
Normalize an already constructed quaternion in place; but do you really need to do this...
Definition: Quaternion.h:132
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:605
void setQuaternionToNaN()
Set quaternion to all-NaN.
Definition: Quaternion.h:102
void setToNaN()
Set every scalar in this Vec to NaN; this is the default initial value in Debug builds, but not in Release.
Definition: Vec.h:810
Vec & operator=(const Vec &src)
Copy assignment operator copies the logically-included elements from the source Vec; gaps due to stri...
Definition: Vec.h:443
Quaternion_< float > fQuaternion
Definition: Quaternion.h:44
Quaternion_(const Vec4P &q)
Construct a quaternion from a 4-vector and normalize the result, which costs about 40 flops...
Definition: Quaternion.h:89
Quaternion_ normalize() const
Return a normalized copy of this quaternion; but do you really need to do this? Quaternions should be...
Definition: Quaternion.h:147
This file is the user-includeable header to be included in user programs to provide fixed-length Vec ...
CNT< ScalarNormSq >::TSqrt norm() const
Definition: Vec.h:608
A Quaternion is a Vec4 with the following behavior:
Definition: Quaternion.h:41
Quaternion_ & operator=(const Quaternion_ &q)
Zero-cost copy assignment just copies the source without conversion to canonical form or normalizatio...
Definition: Quaternion.h:80