Quaternion.h
Go to the documentation of this file.
// This file is a part of the OpenSurgSim project.
// Copyright 2012-2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef SURGSIM_MATH_QUATERNION_H
#define SURGSIM_MATH_QUATERNION_H
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace SurgSim
{
namespace Math
{
typedef Eigen::Quaternion<float> Quaternionf;
typedef Eigen::Quaternion<double> Quaterniond;
template <typename T, int VOpt>
inline Eigen::Quaternion<T> makeRotationQuaternion(const T& angle, const Eigen::Matrix<T, 3, 1, VOpt>& axis)
{
return Eigen::Quaternion<T>(Eigen::AngleAxis<T>(angle, axis));
}
template <typename T, int QOpt>
inline Eigen::Quaternion<T, QOpt> negate(const Eigen::Quaternion<T, QOpt>& q)
{
return Eigen::Quaternion<T, QOpt>(q.coeffs() * -1.0);
}
template <typename T, int QOpt, int VOpt>
inline void computeAngleAndAxis(const Eigen::Quaternion<T, QOpt>& quaternion,
T* angle, Eigen::Matrix<T, 3, 1, VOpt>* axis)
{
using SurgSim::Math::negate;
if (quaternion.w() >= T(0))
{
Eigen::AngleAxis<T> angleAxis(quaternion);
*angle = angleAxis.angle();
*axis = angleAxis.axis();
}
else
{
Eigen::AngleAxis<T> angleAxis(negate(quaternion));
*angle = angleAxis.angle();
*axis = angleAxis.axis();
}
}
template <typename T, int QOpt>
inline T computeAngle(const Eigen::Quaternion<T, QOpt>& quaternion)
{
using SurgSim::Math::negate;
if (quaternion.w() >= T(0))
{
Eigen::AngleAxis<T> angleAxis(quaternion);
return angleAxis.angle();
}
else
{
Eigen::AngleAxis<T> angleAxis(negate(quaternion));
return angleAxis.angle();
}
}
template <typename T, int TOpt, int VOpt>
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)
{
Eigen::Quaternion<T> q1(end.linear());
Eigen::Quaternion<T> q2(start.linear());
double angle;
Eigen::Matrix<T, 3, 1, VOpt> axis;
computeAngleAndAxis((q1 * q2.inverse()).normalized(), &angle, &axis);
*rotationVector = angle * axis;
}
template <typename T, int QOpt>
inline Eigen::Quaternion<T, QOpt> interpolate(const Eigen::Quaternion<T, QOpt>& q0,
const Eigen::Quaternion<T, QOpt>& q1, T t)
{
Eigen::Quaternion<T, QOpt> result;
result = q0.slerp(t, q1);
return result;
}
}; // namespace Math
}; // namespace SurgSim
#endif // SURGSIM_MATH_QUATERNION_H
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
SurgSim::Math::computeAngle
T computeAngle(const Eigen::Matrix< T, 3, 3, MOpt > &matrix)
Get the angle corresponding to a quaternion&#39;s rotation, in radians.
Definition: Matrix.h:142
SurgSim::Math::makeRotationQuaternion
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
SurgSim::Math::interpolate
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
SurgSim::Math::Quaterniond
Eigen::Quaternion< double > Quaterniond
A quaternion of doubles.
Definition: Quaternion.h:38
SurgSim::Math::computeAngleAndAxis
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
SurgSim::Math::negate
Eigen::Quaternion< T, QOpt > negate(const Eigen::Quaternion< T, QOpt > &q)
Quaternion negation (i.e.
Definition: Quaternion.h:60
SurgSim::Math::computeRotationVector
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
SurgSim::Math::Quaternionf
Eigen::Quaternion< float > Quaternionf
A quaternion of floats.
Definition: Quaternion.h:34