RigidTransform.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_RIGIDTRANSFORM_H
#define SURGSIM_MATH_RIGIDTRANSFORM_H
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "SurgSim/Math/Quaternion.h"
namespace SurgSim
{
namespace Math
{
typedef Eigen::Transform<float, 2, Eigen::Isometry> RigidTransform2f;
typedef Eigen::Transform<float, 3, Eigen::Isometry> RigidTransform3f;
typedef Eigen::Transform<double, 2, Eigen::Isometry> RigidTransform2d;
typedef Eigen::Transform<double, 3, Eigen::Isometry> RigidTransform3d;
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;
}
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;
}
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;
}
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;
}
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
Quaternion.h
Definitions of quaternion types.
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
SurgSim::Math::RigidTransform2f
Eigen::Transform< float, 2, Eigen::Isometry > RigidTransform2f
A 2D rigid (isometric) transform, represented as floats.
Definition: RigidTransform.h:34
SurgSim::Math::RigidTransform3f
Eigen::Transform< float, 3, Eigen::Isometry > RigidTransform3f
A 3D rigid (isometric) transform, represented as floats.
Definition: RigidTransform.h:38
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::makeRigidTransform
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
SurgSim::Math::RigidTransform2d
Eigen::Transform< double, 2, Eigen::Isometry > RigidTransform2d
A 2D rigid (isometric) transform, represented as doubles.
Definition: RigidTransform.h:42
SurgSim::Math::RigidTransform3d
Eigen::Transform< double, 3, Eigen::Isometry > RigidTransform3d
A 3D rigid (isometric) transform, represented as doubles.
Definition: RigidTransform.h:46
SurgSim::Math::makeRigidTranslation
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