/usr/include/SurgSim/Math/Quaternion.h is in libopensurgsim-dev 0.7.0-6ubuntu1.
This file is owned by root:root, with mode 0o644.
The actual contents of the file can be viewed below.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 | // 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.
/// \file
/// Definitions of quaternion types.
#ifndef SURGSIM_MATH_QUATERNION_H
#define SURGSIM_MATH_QUATERNION_H
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace SurgSim
{
namespace Math
{
/// A quaternion of floats.
/// This type (and any structs that contain it) can be safely allocated via new.
typedef Eigen::Quaternion<float> Quaternionf;
/// A quaternion of doubles.
/// This type (and any structs that contain it) can be safely allocated via new.
typedef Eigen::Quaternion<double> Quaterniond;
/// Create a quaternion rotation corresponding to the specified angle (in radians) and axis.
/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
/// \tparam VOpt the option flags (alignment etc.) used for the axis vector argument. Can be deduced.
/// \param angle the angle of the rotation, in radians.
/// \param axis the axis of the rotation.
/// \returns the rotation quaternion.
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));
}
/// Quaternion negation (i.e. unary operator -)
/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
/// \tparam QOpt the option flags (alignment etc.) used for the quaternion arguments. Can be deduced.
/// \param q The quaternion to negate
/// \returns the negation of q (i.e. -q)
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);
}
/// Get the angle (in radians) and axis corresponding to a quaternion's rotation.
/// \note Unit quaternions cover the unit sphere twice (q=-q). To make sure that the same rotation (q or -q)
/// \note returns the same Axis/Angle, we need a pi range for the angle.
/// \note We choose to enforce half the angle in the quadrant [0 +pi/2], which leads to an output angle in [0 +pi].
/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
/// \tparam QOpt the option flags (alignment etc.) used for the quaternion argument. Can be deduced.
/// \tparam VOpt the option flags (alignment etc.) used for the axis vector argument. Can be deduced.
/// \param quaternion the rotation quaternion to inspect.
/// \param [out] angle the angle of the rotation in [0 +pi], in radians.
/// \param [out] axis the axis of the rotation.
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();
}
}
/// Get the angle corresponding to a quaternion's rotation, in radians.
/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
/// \tparam QOpt the option flags (alignment etc.) used for the quaternion argument. Can be deduced.
/// \param quaternion the rotation quaternion to inspect.
/// \returns the angle of the rotation within [0 +pi], in radians.
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();
}
}
/// Get the vector corresponding to the rotation between transforms.
/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
/// \tparam TOpt the option flags (alignment etc.) used for the transform arguments. Can be deduced.
/// \tparam VOpt the option flags (alignment etc.) used for the vector argument. Can be deduced.
/// \param end the transform after the rotation.
/// \param start the transform before the rotation.
/// \param[out] rotationVector a vector describing the rotation.
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;
}
/// Interpolate (slerp) between 2 quaternions
/// \tparam T the numeric data type used for arguments and the return value. Can usually be deduced.
/// \tparam QOpt the option flags (alignment etc.) used for the quaternion arguments. Can be deduced.
/// \param q0 The start quaternion (at time 0.0).
/// \param q1 The end quaternion (at time 1.0).
/// \param t The interpolation time requested. Within [0..1].
/// \returns the quaternion resulting in the slerp interpolation at time t, between q0 and q1.
/// \note t=0 => returns either q0 or -q0
/// \note t=1 => returns either q1 or -q1
/// \note 'Interpolate' has been created because slerp might not be enough in certain cases.
/// This gives room for correction and special future treatment
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
|