EigenInterop.h

Go to the documentation of this file.

// Copyright 2014 Sensics, 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 INCLUDED_EigenInterop_h_GUID_A07DFDC3_BA71_4894_6593_732B364A2AE7
#define INCLUDED_EigenInterop_h_GUID_A07DFDC3_BA71_4894_6593_732B364A2AE7

// Internal Includes
#include <osvr/Util/Pose3C.h>

// Library/third-party includes
#include <osvr/Util/EigenCoreGeometry.h>
#include <osvr/Util/EigenExtras.h>

// Standard includes
// - none

namespace osvr {
namespace util {

    inline Eigen::Map<Eigen::Vector3d> vecMap(OSVR_Vec3 &vec) {
        return Eigen::Map<Eigen::Vector3d>(&(vec.data[0]));
    }

    inline Eigen::Map<const Eigen::Vector3d> vecMap(OSVR_Vec3 const &vec) {
        return Eigen::Map<const Eigen::Vector3d>(&(vec.data[0]));
    }

    inline Eigen::Quaterniond fromQuat(OSVR_Quaternion const &q) {
        return Eigen::Quaterniond(osvrQuatGetW(&q), osvrQuatGetX(&q),
                                  osvrQuatGetY(&q), osvrQuatGetZ(&q));
    }

    inline void toQuat(Eigen::Quaterniond const &src, OSVR_Quaternion &q) {
        osvrQuatSetW(&q, src.w());
        osvrQuatSetX(&q, src.x());
        osvrQuatSetY(&q, src.y());
        osvrQuatSetZ(&q, src.z());
    }

    inline Eigen::Isometry3d fromPose(OSVR_Pose3 const &pose) {
        Eigen::Isometry3d newPose;
        newPose.fromPositionOrientationScale(vecMap(pose.translation),
                                             fromQuat(pose.rotation),
                                             Eigen::Vector3d::Constant(1));
        return newPose;
    }

    inline void toPose(Eigen::Isometry3d const &xform, OSVR_Pose3 &pose) {
        vecMap(pose.translation) = xform.translation();
        Eigen::Quaterniond quat(xform.rotation());
        toQuat(quat, pose.rotation);
    }

    inline void toPose(Eigen::Matrix4d const &mat, OSVR_Pose3 &pose) {
        Eigen::Affine3d xform(mat);
        vecMap(pose.translation) = xform.translation();
        Eigen::Quaterniond quat(xform.rotation());
        toQuat(quat, pose.rotation);
    }

    namespace eigen_interop {
        inline Eigen::Map<const Eigen::Vector3d>
        const_map(OSVR_Vec3 const &vec) {
            return vecMap(vec);
        }

        inline Eigen::Map<Eigen::Vector3d> map(OSVR_Vec3 &vec) {
            return vecMap(vec);
        }

        inline Eigen::Map<const Eigen::Vector3d> map(OSVR_Vec3 const &vec) {
            return const_map(vec);
        }
        namespace detail {
            template <char Coefficient> struct QuatAccessor;
            template <> struct QuatAccessor<'W'> {
                static double get(OSVR_Quaternion const &q) {
                    return osvrQuatGetW(&q);
                }

                static void set(OSVR_Quaternion &q, double v) {
                    return osvrQuatSetW(&q, v);
                }
            };
            template <> struct QuatAccessor<'X'> {
                static double get(OSVR_Quaternion const &q) {
                    return osvrQuatGetX(&q);
                }

                static void set(OSVR_Quaternion &q, double v) {
                    return osvrQuatSetX(&q, v);
                }
            };

            template <> struct QuatAccessor<'Y'> {
                static double get(OSVR_Quaternion const &q) {
                    return osvrQuatGetY(&q);
                }

                static void set(OSVR_Quaternion &q, double v) {
                    return osvrQuatSetY(&q, v);
                }
            };

            template <> struct QuatAccessor<'Z'> {
                static double get(OSVR_Quaternion const &q) {
                    return osvrQuatGetZ(&q);
                }

                static void set(OSVR_Quaternion &q, double v) {
                    return osvrQuatSetZ(&q, v);
                }
            };

            template <char Coefficient, typename T = OSVR_Quaternion>
            class QuatCoefficientMap {
              public:
                QuatCoefficientMap(T &quat) : m_quat(&quat) {}
                operator double() const {
                    return QuatAccessor<Coefficient>::get(*m_quat);
                }
                QuatCoefficientMap const &operator=(double v) const {
                    QuatAccessor<Coefficient>::set(*m_quat, v);
                    return *this;
                }

              private:
                T *m_quat;
            };
            template <typename T = OSVR_Quaternion const> class BaseQuatMap {
              public:
                BaseQuatMap(T &quat) : m_quat(&quat) {}
#if 0
                BaseQuatMap(BaseQuatMap const&) = delete;
                BaseQuatMap(BaseQuatMap && other) : m_quat(other.m_quat) {}
#endif

                Eigen::Quaterniond quat() const { return fromQuat(*m_quat); }

                operator Eigen::Quaterniond() const { return quat(); }

                double w() const { return osvrQuatGetW(m_quat); }
                double x() const { return osvrQuatGetX(m_quat); }
                double y() const { return osvrQuatGetY(m_quat); }
                double z() const { return osvrQuatGetZ(m_quat); }

              protected:
                T *m_quat;
            };

            typedef BaseQuatMap<> ConstQuatMap;
            class QuatMap : public BaseQuatMap<OSVR_Quaternion> {
              public:
                QuatMap(OSVR_Quaternion &quat) : BaseQuatMap(quat) {}
#if 0
                QuatMap(QuatMap const&) = delete;
                QuatMap(QuatMap && other) : BaseQuatMap(std::move(other)) {}
#endif
                template <typename T>
                QuatMap const &operator=(BaseQuatMap<T> const &other) const {
                    *m_quat = *other.m_quat;
                    return *this;
                }
                template <typename Derived>
                QuatMap const &
                operator=(Eigen::QuaternionBase<Derived> const &other) const {
                    osvrQuatSetW(m_quat, other.w());
                    osvrQuatSetX(m_quat, other.x());
                    osvrQuatSetY(m_quat, other.y());
                    osvrQuatSetZ(m_quat, other.z());
                    return *this;
                }

                QuatCoefficientMap<'W'> w() const {
                    return QuatCoefficientMap<'W'>(*m_quat);
                }
                QuatCoefficientMap<'X'> x() const {
                    return QuatCoefficientMap<'X'>(*m_quat);
                }
                QuatCoefficientMap<'Y'> y() const {
                    return QuatCoefficientMap<'Y'>(*m_quat);
                }
                QuatCoefficientMap<'Z'> z() const {
                    return QuatCoefficientMap<'Z'>(*m_quat);
                }
            };
        } // namespace detail

        inline detail::ConstQuatMap const_map(OSVR_Quaternion const &quat) {
            return detail::ConstQuatMap(quat);
        }

        inline detail::ConstQuatMap map(OSVR_Quaternion const &quat) {
            return const_map(quat);
        }

        inline detail::QuatMap map(OSVR_Quaternion &quat) {
            return detail::QuatMap(quat);
        }

        namespace detail {
            template <typename T = OSVR_Pose3 const> class BasePoseMap {
              public:
                BasePoseMap(T &pose) : m_pose(&pose) {}
#if 0
                BasePoseMap(BasePoseMap const&) = delete;
                BasePoseMap(BasePoseMap && other) : m_pose(other.m_pose) {}
#endif
                typedef Eigen::Isometry3d TransformType;
                typedef typename TransformType::MatrixType MatrixType;

                TransformType transform() const { return fromPose(*m_pose); }
                operator TransformType() const { return transform(); }

                MatrixType matrix() const { return fromPose(*m_pose).matrix(); }

                Eigen::Map<const Eigen::Vector3d> translation() const {
                    return const_map(m_pose->translation);
                }
                ConstQuatMap rotation() const {
                    return const_map(m_pose->rotation);
                }

              protected:
                T *m_pose;
            };

            typedef BasePoseMap<> ConstPoseMap;

            class PoseMap : public BasePoseMap<OSVR_Pose3> {
              public:
                PoseMap(OSVR_Pose3 &pose) : BasePoseMap(pose) {}
#if 0
                PoseMap(PoseMap const&) = delete;
                PoseMap(PoseMap && other) : BasePoseMap(std::move(other)) {}
#endif
                template <typename T>
                PoseMap const &operator=(BasePoseMap<T> const &other) const {
                    *m_pose = *other.m_pose;
                    return *this;
                }

                PoseMap const &operator=(Eigen::Isometry3d const &other) const {
                    toPose(other, *m_pose);
                    return *this;
                }

                PoseMap const &operator=(Eigen::Matrix4d const &other) const {
                    toPose(other, *m_pose);
                    return *this;
                }

                Eigen::Map<Eigen::Vector3d> translation() const {
                    return map(m_pose->translation);
                }

                QuatMap rotation() const { return map(m_pose->rotation); }
            };
        } // namespace detail

        inline detail::ConstPoseMap const_map(OSVR_Pose3 const &pose) {
            return detail::ConstPoseMap(pose);
        }

        inline detail::ConstPoseMap map(OSVR_Pose3 const &pose) {
            return const_map(pose);
        }

        inline detail::PoseMap map(OSVR_Pose3 &pose) {
            return detail::PoseMap(pose);
        }
    } // namespace eigen_interop
} // namespace util
} // namespace osvr
#endif // INCLUDED_EigenInterop_h_GUID_A07DFDC3_BA71_4894_6593_732B364A2AE7