All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
RigidTransform.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_RIGIDTRANSFORM_H
20 #define SURGSIM_MATH_RIGIDTRANSFORM_H
21 
22 #include <Eigen/Core>
23 #include <Eigen/Geometry>
24 
26 
27 namespace SurgSim
28 {
29 namespace Math
30 {
31 
34 typedef Eigen::Transform<float, 2, Eigen::Isometry> RigidTransform2f;
35 
38 typedef Eigen::Transform<float, 3, Eigen::Isometry> RigidTransform3f;
39 
42 typedef Eigen::Transform<double, 2, Eigen::Isometry> RigidTransform2d;
43 
46 typedef Eigen::Transform<double, 3, Eigen::Isometry> RigidTransform3d;
47 
48 
55 template <typename M, typename V>
56 inline Eigen::Transform<typename M::Scalar, M::RowsAtCompileTime, Eigen::Isometry> makeRigidTransform(
57  const Eigen::MatrixBase<M>& rotation, const Eigen::MatrixBase<V>& translation)
58 {
59  Eigen::Transform<typename M::Scalar, M::RowsAtCompileTime, Eigen::Isometry> rigid;
60  rigid.makeAffine();
61  rigid.linear() = rotation;
62  rigid.translation() = translation;
63  return rigid;
64 }
65 
72 template <typename Q, typename V>
73 inline Eigen::Transform<typename Q::Scalar, 3, Eigen::Isometry> makeRigidTransform(
74  const Eigen::QuaternionBase<Q>& rotation, const Eigen::MatrixBase<V>& translation)
75 {
76  Eigen::Transform<typename Q::Scalar, 3, Eigen::Isometry> rigid;
77  rigid.makeAffine();
78  rigid.linear() = rotation.matrix();
79  rigid.translation() = translation;
80  return rigid;
81 }
82 
93 template <typename T, int VOpt>
94 inline Eigen::Transform<T, 3, Eigen::Isometry> makeRigidTransform(
95  const Eigen::Matrix<T, 3, 1, VOpt>& position,
96  const Eigen::Matrix<T, 3, 1, VOpt>& center,
97  const Eigen::Matrix<T, 3, 1, VOpt>& up)
98 {
99 
100  Eigen::Transform<T, 3, Eigen::Isometry> rigid;
101  rigid.makeAffine();
102 
103  Eigen::Matrix<T, 3, 1, VOpt> forward = (center - position).normalized();
104  Eigen::Matrix<T, 3, 1, VOpt> side = (forward.cross(up)).normalized();
105  Eigen::Matrix<T, 3, 1, VOpt> actualUp = side.cross(forward).normalized();
106 
107  typename Eigen::Transform<T, 3, Eigen::Isometry>::LinearMatrixType rotation;
108  rotation << side[0], actualUp[0], -forward[0],
109  side[1], actualUp[1], -forward[1],
110  side[2], actualUp[2], -forward[2];
111 
112  rigid.linear() = rotation;
113  rigid.translation() = position;
114 
115  return rigid;
116 }
117 
122 template <typename V>
123 inline Eigen::Transform<typename V::Scalar, V::SizeAtCompileTime, Eigen::Isometry> makeRigidTranslation(
124  const Eigen::MatrixBase<V>& translation)
125 {
126  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<V>);
127  Eigen::Transform<typename V::Scalar, V::SizeAtCompileTime, Eigen::Isometry> rigid;
128  rigid.makeAffine();
129  rigid.linear().setIdentity();
130  rigid.translation() = translation;
131  return rigid;
132 }
133 
143 template <typename T, int TOpt>
144 inline Eigen::Transform<T, 3, Eigen::Isometry> interpolate(
145  const Eigen::Transform<T, 3, Eigen::Isometry, TOpt>& t0,
146  const Eigen::Transform<T, 3, Eigen::Isometry, TOpt>& t1,
147  T t)
148 {
149  Eigen::Transform<T, 3, Eigen::Isometry> transform;
150  transform.makeAffine();
151  transform.translation() = t0.translation() * (static_cast<T>(1.0) - t) + t1.translation() * t;
152  {
153  Eigen::Quaternion<T> q0(t0.linear());
154  Eigen::Quaternion<T> q1(t1.linear());
155  q0.normalize();
156  q1.normalize();
157  transform.linear() = interpolate(q0, q1, t).matrix();
158  }
159  return transform;
160 }
161 
162 }; // namespace Math
163 }; // namespace SurgSim
164 
165 #endif // SURGSIM_MATH_RIGIDTRANSFORM_H
Definitions of quaternion types.
Eigen::Transform< float, 2, Eigen::Isometry > RigidTransform2f
A 2D rigid (isometric) transform, represented as floats.
Definition: RigidTransform.h:34
Eigen::Transform< float, 3, Eigen::Isometry > RigidTransform3f
A 3D rigid (isometric) transform, represented as floats.
Definition: RigidTransform.h:38
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::Transform< typename M::Scalar, M::RowsAtCompileTime, Eigen::Isometry > makeRigidTransform(const Eigen::MatrixBase< M > &rotation, const Eigen::MatrixBase< V > &translation)
Create a rigid transform using the specified rotation matrix and translation.
Definition: RigidTransform.h:56
Eigen::Transform< double, 2, Eigen::Isometry > RigidTransform2d
A 2D rigid (isometric) transform, represented as doubles.
Definition: RigidTransform.h:42
Eigen::Transform< double, 3, Eigen::Isometry > RigidTransform3d
A 3D rigid (isometric) transform, represented as doubles.
Definition: RigidTransform.h:46
Eigen::Transform< typename V::Scalar, V::SizeAtCompileTime, Eigen::Isometry > makeRigidTranslation(const Eigen::MatrixBase< V > &translation)
Create a rigid transform using the identity rotation and the specified translation.
Definition: RigidTransform.h:123