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