All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Quaternion.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2012-2013, SimQuest Solutions Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
18 
19 #ifndef SURGSIM_MATH_QUATERNION_H
20 #define SURGSIM_MATH_QUATERNION_H
21 
22 #include <math.h>
23 
24 #include <Eigen/Core>
25 #include <Eigen/Geometry>
26 
27 namespace SurgSim
28 {
29 namespace Math
30 {
31 
34 typedef Eigen::Quaternion<float> Quaternionf;
35 
38 typedef Eigen::Quaternion<double> Quaterniond;
39 
40 
41 
48 template <typename T, int VOpt>
49 inline Eigen::Quaternion<T> makeRotationQuaternion(const T& angle, const Eigen::Matrix<T, 3, 1, VOpt>& axis)
50 {
51  return Eigen::Quaternion<T>(Eigen::AngleAxis<T>(angle, axis));
52 }
53 
59 template <typename T, int QOpt>
60 inline Eigen::Quaternion<T, QOpt> negate(const Eigen::Quaternion<T, QOpt>& q)
61 {
62  return Eigen::Quaternion<T, QOpt>(q.coeffs() * -1.0);
63 }
64 
75 template <typename T, int QOpt, int VOpt>
76 inline void computeAngleAndAxis(const Eigen::Quaternion<T, QOpt>& quaternion,
77  T* angle, Eigen::Matrix<T, 3, 1, VOpt>* axis)
78 {
80 
81  if (quaternion.w() >= T(0))
82  {
83  Eigen::AngleAxis<T> angleAxis(quaternion);
84  *angle = angleAxis.angle();
85  *axis = angleAxis.axis();
86  }
87  else
88  {
89  Eigen::AngleAxis<T> angleAxis(negate(quaternion));
90  *angle = angleAxis.angle();
91  *axis = angleAxis.axis();
92  }
93 }
94 
100 template <typename T, int QOpt>
101 inline T computeAngle(const Eigen::Quaternion<T, QOpt>& quaternion)
102 {
103  using SurgSim::Math::negate;
104 
105  if (quaternion.w() >= T(0))
106  {
107  Eigen::AngleAxis<T> angleAxis(quaternion);
108  return angleAxis.angle();
109  }
110  else
111  {
112  Eigen::AngleAxis<T> angleAxis(negate(quaternion));
113  return angleAxis.angle();
114  }
115 }
116 
124 template <typename T, int TOpt, int VOpt>
125 void computeRotationVector(const Eigen::Transform<T, 3, Eigen::Isometry, TOpt>& end,
126  const Eigen::Transform<T, 3, Eigen::Isometry, TOpt>& start,
127  Eigen::Matrix<T, 3, 1, VOpt>* rotationVector)
128 {
129  Eigen::Quaternion<T> q1(end.linear());
130  Eigen::Quaternion<T> q2(start.linear());
131  double angle;
132  Eigen::Matrix<T, 3, 1, VOpt> axis;
133  computeAngleAndAxis((q1 * q2.inverse()).normalized(), &angle, &axis);
134  *rotationVector = angle * axis;
135 }
136 
148 template <typename T, int QOpt>
149 inline Eigen::Quaternion<T, QOpt> interpolate(const Eigen::Quaternion<T, QOpt>& q0,
150  const Eigen::Quaternion<T, QOpt>& q1, T t)
151 {
152  Eigen::Quaternion<T, QOpt> result;
153 
154  result = q0.slerp(t, q1);
155 
156  return result;
157 }
158 
159 }; // namespace Math
160 }; // namespace SurgSim
161 
162 #endif // SURGSIM_MATH_QUATERNION_H
T computeAngle(const Eigen::Matrix< T, 3, 3, MOpt > &matrix)
Get the angle corresponding to a quaternion's rotation, in radians.
Definition: Matrix.h:142
Eigen::Quaternion< T > makeRotationQuaternion(const T &angle, const Eigen::Matrix< T, 3, 1, VOpt > &axis)
Create a quaternion rotation corresponding to the specified angle (in radians) and axis...
Definition: Quaternion.h:49
Eigen::Quaternion< T, QOpt > interpolate(const Eigen::Quaternion< T, QOpt > &q0, const Eigen::Quaternion< T, QOpt > &q1, T t)
Interpolate (slerp) between 2 quaternions.
Definition: Quaternion.h:149
Eigen::Quaternion< double > Quaterniond
A quaternion of doubles.
Definition: Quaternion.h:38
void computeAngleAndAxis(const Eigen::Matrix< T, 3, 3, MOpt > &matrix, T *angle, Eigen::Matrix< T, 3, 1, VOpt > *axis)
Get the angle (in radians) and axis corresponding to a rotation matrix.
Definition: Matrix.h:127
Eigen::Quaternion< T, QOpt > negate(const Eigen::Quaternion< T, QOpt > &q)
Quaternion negation (i.e.
Definition: Quaternion.h:60
void computeRotationVector(const Eigen::Transform< T, 3, Eigen::Isometry, TOpt > &end, const Eigen::Transform< T, 3, Eigen::Isometry, TOpt > &start, Eigen::Matrix< T, 3, 1, VOpt > *rotationVector)
Get the vector corresponding to the rotation between transforms.
Definition: Quaternion.h:125
Eigen::Quaternion< float > Quaternionf
A quaternion of floats.
Definition: Quaternion.h:34