Simbody 3.7
Loading...
Searching...
No Matches
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//-----------------------------------------------------------------------------
36namespace SimTK {
37
38//-----------------------------------------------------------------------------
39// Forward declarations
40template <class P> class Rotation_;
41template <class P> class Quaternion_;
42
46
47//-----------------------------------------------------------------------------
63//-----------------------------------------------------------------------------
64template <class P>
65class Quaternion_ : public Vec<4,P> {
66 typedef P RealP;
67 typedef Vec<3,P> Vec3P;
68 typedef Vec<4,P> Vec4P;
69public:
72 Quaternion_() : Vec4P(1,0,0,0) { }
73
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
103
113
117
120 const Vec4P& asVec4() const {return *static_cast<const Vec4P*>(this);}
121
133 const RealP epsilon = std::numeric_limits<RealP>::epsilon();
134 const RealP magnitude = Vec4P::norm();
136 else if( magnitude < epsilon ) setQuaternionToNaN();
137 else (*this) *= (1/magnitude);
138 return *this;
139 }
140
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//----------------------------------------------------------------------------------------------------
159private:
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
#define SimTK_SimTKCOMMON_EXPORT
Definition SimTKcommon/include/SimTKcommon/internal/common.h:224
This file is the user-includeable header to be included in user programs to provide fixed-length Vec ...
Declares and defines the UnitVec and UnitRow classes.
A Quaternion is a Vec4 with the following behavior:
Definition Quaternion.h:65
void setQuaternionFromAngleAxis(const Vec4P &av)
Set this quaternion from an angle-axis rotation packed into a 4-vector as [a vx vy vz].
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_ normalize() const
Return a normalized copy of this quaternion; but do you really need to do this? Quaternions should be...
Definition Quaternion.h:147
Quaternion_(const Quaternion_ &q)
Zero-cost copy constructor just copies the source without conversion to canonical form or normalizati...
Definition Quaternion.h:76
Quaternion_(const Vec4P &v, bool)
Use this constructor only if you are sure v is normalized to 1.0.
Definition Quaternion.h:154
Quaternion_ & normalizeThis()
Normalize an already constructed quaternion in place; but do you really need to do this?...
Definition Quaternion.h:132
Quaternion_ & operator=(const Quaternion_ &q)
Zero-cost copy assignment just copies the source without conversion to canonical form or normalizatio...
Definition Quaternion.h:80
void setQuaternionFromAngleAxis(const RealP &a, const UnitVec< P, 1 > &v)
Set this quaternion from an angle-axis rotation provided as an angle a and a separate unit vector [vx...
Quaternion_()
Default constructor produces the ZeroRotation quaternion [1 0 0 0] (not NaN - even in debug mode).
Definition Quaternion.h:72
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_(const Vec4P &q)
Construct a quaternion from a 4-vector and normalize the result, which costs about 40 flops.
Definition Quaternion.h:89
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
void setQuaternionToNaN()
Set quaternion to all-NaN.
Definition Quaternion.h:102
Quaternion_(const Rotation_< P > &)
Constructs a canonical quaternion from a rotation matrix (cost is about 60 flops).
The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 ...
Definition Rotation.h:111
This class is a Vec3 plus an ironclad guarantee either that:
Definition UnitVec.h:56
This is a fixed-length column vector designed for no-overhead inline computation.
Definition Vec.h:184
void setToNaN()
Set every scalar in this Vec to NaN; this is the default initial value in Debug builds,...
Definition Vec.h:812
Vec & operator=(const Vec &src)
Copy assignment operator copies the logically-included elements from the source Vec; gaps due to stri...
Definition Vec.h:445
CNT< ScalarNormSq >::TSqrt norm() const
Definition Vec.h:610
void set(int i, const E &value)
Variant of operator[] that's scripting friendly to set ith entry.
Definition Vec.h:995
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition Assembler.h:37
Quaternion_< Real > Quaternion
Definition Quaternion.h:43
Quaternion_< double > dQuaternion
Definition Quaternion.h:45
Quaternion_< float > fQuaternion
Definition Quaternion.h:44