/usr/include/SurgSim/Math/RigidTransform.h is in libopensurgsim-dev 0.7.0-5.
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 163 164 165 | // 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 2x2 and 3x3 rigid (isometric) transforms.
#ifndef SURGSIM_MATH_RIGIDTRANSFORM_H
#define SURGSIM_MATH_RIGIDTRANSFORM_H
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "SurgSim/Math/Quaternion.h"
namespace SurgSim
{
namespace Math
{
/// A 2D rigid (isometric) transform, represented as floats.
/// This type (and any struct that contain it) can be safely allocated via new.
typedef Eigen::Transform<float, 2, Eigen::Isometry> RigidTransform2f;
/// A 3D rigid (isometric) transform, represented as floats.
/// This type (and any struct that contain it) can be safely allocated via new.
typedef Eigen::Transform<float, 3, Eigen::Isometry> RigidTransform3f;
/// A 2D rigid (isometric) transform, represented as doubles.
/// This type (and any struct that contain it) can be safely allocated via new.
typedef Eigen::Transform<double, 2, Eigen::Isometry> RigidTransform2d;
/// A 3D rigid (isometric) transform, represented as doubles.
/// This type (and any struct that contain it) can be safely allocated via new.
typedef Eigen::Transform<double, 3, Eigen::Isometry> RigidTransform3d;
/// Create a rigid transform using the specified rotation matrix and translation.
/// \tparam M the type used to describe the rotation matrix. Can usually be deduced.
/// \tparam V the type used to describe the translation vector. Can usually be deduced.
/// \param rotation the rotation matrix.
/// \param translation the translation vector.
/// \returns the transform with the specified rotation and translation.
template <typename M, typename V>
inline Eigen::Transform<typename M::Scalar, M::RowsAtCompileTime, Eigen::Isometry> makeRigidTransform(
const Eigen::MatrixBase<M>& rotation, const Eigen::MatrixBase<V>& translation)
{
Eigen::Transform<typename M::Scalar, M::RowsAtCompileTime, Eigen::Isometry> rigid;
rigid.makeAffine();
rigid.linear() = rotation;
rigid.translation() = translation;
return rigid;
}
/// Create a rigid transform using the specified rotation quaternion and translation.
/// \tparam Q the type used to describe the rotation quaternion. Can usually be deduced.
/// \tparam V the type used to describe the translation vector. Can usually be deduced.
/// \param rotation the rotation quaternion.
/// \param translation the translation vector.
/// \returns the transform with the specified rotation and translation.
template <typename Q, typename V>
inline Eigen::Transform<typename Q::Scalar, 3, Eigen::Isometry> makeRigidTransform(
const Eigen::QuaternionBase<Q>& rotation, const Eigen::MatrixBase<V>& translation)
{
Eigen::Transform<typename Q::Scalar, 3, Eigen::Isometry> rigid;
rigid.makeAffine();
rigid.linear() = rotation.matrix();
rigid.translation() = translation;
return rigid;
}
/// Make a rigid transform from a eye point a center view point and an up direction, does not check whether up is
/// colinear with eye-center
/// The original formula can be found at
/// http://www.opengl.org/sdk/docs/man2/xhtml/gluLookAt.xml
/// \tparam typename T T the numeric data type used for arguments and the return value. Can usually be deduced.
/// \tparam int VOpt VOpt the option flags (alignment etc.) used for the axis vector argument. Can be deduced.
/// \param position The position of the object.
/// \param center The point to which the object should point.
/// \param up The up vector to be used for this calculation.
/// \return a RigidTransform that locates the object at position rotated into the direction of center.
template <typename T, int VOpt>
inline Eigen::Transform<T, 3, Eigen::Isometry> makeRigidTransform(
const Eigen::Matrix<T, 3, 1, VOpt>& position,
const Eigen::Matrix<T, 3, 1, VOpt>& center,
const Eigen::Matrix<T, 3, 1, VOpt>& up)
{
Eigen::Transform<T, 3, Eigen::Isometry> rigid;
rigid.makeAffine();
Eigen::Matrix<T, 3, 1, VOpt> forward = (center - position).normalized();
Eigen::Matrix<T, 3, 1, VOpt> side = (forward.cross(up)).normalized();
Eigen::Matrix<T, 3, 1, VOpt> actualUp = side.cross(forward).normalized();
typename Eigen::Transform<T, 3, Eigen::Isometry>::LinearMatrixType rotation;
rotation << side[0], actualUp[0], -forward[0],
side[1], actualUp[1], -forward[1],
side[2], actualUp[2], -forward[2];
rigid.linear() = rotation;
rigid.translation() = position;
return rigid;
}
/// Create a rigid transform using the identity rotation and the specified translation.
/// \tparam V the type used to describe the translation vector. Can usually be deduced.
/// \param translation the translation vector.
/// \returns the transform with the identity rotation and the specified translation.
template <typename V>
inline Eigen::Transform<typename V::Scalar, V::SizeAtCompileTime, Eigen::Isometry> makeRigidTranslation(
const Eigen::MatrixBase<V>& translation)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<V>);
Eigen::Transform<typename V::Scalar, V::SizeAtCompileTime, Eigen::Isometry> rigid;
rigid.makeAffine();
rigid.linear().setIdentity();
rigid.translation() = translation;
return rigid;
}
/// Interpolate (slerp) between 2 rigid transformations
/// \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.
/// \param t0 The start transform (at time 0.0).
/// \param t1 The end transform (at time 1.0).
/// \param t The interpolation time requested. Within [0..1].
/// \returns the transform resulting in the slerp interpolation at time t, between t0 and t1.
/// \note t=0 => returns t0
/// \note t=1 => returns t1
template <typename T, int TOpt>
inline Eigen::Transform<T, 3, Eigen::Isometry> interpolate(
const Eigen::Transform<T, 3, Eigen::Isometry, TOpt>& t0,
const Eigen::Transform<T, 3, Eigen::Isometry, TOpt>& t1,
T t)
{
Eigen::Transform<T, 3, Eigen::Isometry> transform;
transform.makeAffine();
transform.translation() = t0.translation() * (static_cast<T>(1.0) - t) + t1.translation() * t;
{
Eigen::Quaternion<T> q0(t0.linear());
Eigen::Quaternion<T> q1(t1.linear());
q0.normalize();
q1.normalize();
transform.linear() = interpolate(q0, q1, t).matrix();
}
return transform;
}
}; // namespace Math
}; // namespace SurgSim
#endif // SURGSIM_MATH_RIGIDTRANSFORM_H
|