Quaternion.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_QUATERNION_H
#define SURGSIM_MATH_QUATERNION_H

#include <math.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

namespace SurgSim
{
namespace Math
{

typedef Eigen::Quaternion<float>  Quaternionf;

typedef Eigen::Quaternion<double>  Quaterniond;



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));
}

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);
}

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();
    }
}

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();
    }
}

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;
}

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

namespace Eigen
{

template<typename T>
::std::ostream& operator<<(::std::ostream& os, const Eigen::Quaternion<T>& quaternion)
{
    os << quaternion.coeffs().transpose();
    return os;
}
};

#endif // SURGSIM_MATH_QUATERNION_H