libalmath  2.5.7.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
almatheigen.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2015 Aldebaran Robotics. All rights reserved.
3  * Use of this source code is governed by a BSD-style license that can be
4  * found in the COPYING file.
5  */
6 
7 #pragma once
8 #ifndef _LIBALMATH_ALMATH_SCENEGRAPH_ALMATHEIGEN_H_
9 #define _LIBALMATH_ALMATH_SCENEGRAPH_ALMATHEIGEN_H_
10 
14 #include <Eigen/Dense>
15 
16 namespace AL {
17 namespace Math {
18 
19 typedef Eigen::Matrix<float, 3, 4> Matrix34f;
20 typedef Eigen::Matrix<float, 3, 4, Eigen::RowMajor> Matrix34frm;
21 typedef Eigen::Matrix<float, 6, 1> Vector6f;
22 
24  return Matrix34f(Eigen::Map<const Matrix34frm>(&tr.r1_c1));
25 }
26 
27 inline Eigen::Matrix3f toEigenMatrix3(const AL::Math::Transform &tr) {
28  return Eigen::Matrix3f(
29  Eigen::Map<const Matrix34frm>(&tr.r1_c1).block<3, 3>(0, 0));
30 }
31 
32 inline Eigen::Vector3f toEigenVector3(const AL::Math::Position3D &v) {
33  return Eigen::Vector3f(Eigen::Map<const Eigen::Vector3f>(&v.x));
34 }
35 
36 inline Eigen::AffineCompact3f toEigenAffineCompact3(
37  const AL::Math::Transform &tr) {
38  return Eigen::AffineCompact3f(Eigen::Map<const Matrix34frm>(&tr.r1_c1));
39 }
40 
41 template <typename Derived0>
42 void toALMathTransform(const Eigen::MatrixBase<Derived0> &in, Transform &out) {
43  EIGEN_STATIC_ASSERT(
44  (Eigen::internal::is_same<float, typename Derived0::Scalar>::value),
45  YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
46  // will fail if in has the wrong size
47  Eigen::Map<Matrix34frm>(&out.r1_c1) = in;
48 }
49 
50 template <typename Derived0>
51 Transform toALMathTransform(const Eigen::MatrixBase<Derived0> &m) {
52  Transform tr;
53  toALMathTransform(m, tr);
54  return tr;
55 }
56 
57 template <int _Mode, int _Options>
58 void toALMathTransform(const Eigen::Transform<float, 3, _Mode, _Options> &in,
59  Transform &out) {
60  Eigen::Map<Matrix34frm>(&out.r1_c1) = in.affine();
61 }
62 
63 template <int _Mode, int _Options>
65  const Eigen::Transform<float, 3, _Mode, _Options> &tr) {
67  toALMathTransform(tr, out);
68  return out;
69 }
70 
71 template <typename Derived0>
72 Position3D toALMathPosition3D(const Eigen::MatrixBase<Derived0> &in) {
73  EIGEN_STATIC_ASSERT(
74  (Eigen::internal::is_same<float, typename Derived0::Scalar>::value),
75  YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
76  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived0, 3, 1)
77  return Position3D(in[0], in[1], in[2]);
78 }
79 
80 template <typename Derived0>
81 void toALMathVelocity6D(const Eigen::MatrixBase<Derived0> &in,
82  Velocity6D &out) {
83  EIGEN_STATIC_ASSERT(
84  (Eigen::internal::is_same<float, typename Derived0::Scalar>::value),
85  YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
86  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived0, 6, 1)
87  assert((in.rows() == 6) && (in.cols() == 1));
88  Eigen::Map<Vector6f>(&out.xd) = in;
89 }
90 
91 template <typename Derived0>
92 Velocity6D toALMathVelocity6D(const Eigen::MatrixBase<Derived0> &in) {
93  EIGEN_STATIC_ASSERT(
94  (Eigen::internal::is_same<float, typename Derived0::Scalar>::value),
95  YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
96  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived0, 6, 1)
97  return Velocity6D(in[0], in[1], in[2], in[3], in[4], in[5]);
98 }
99 }
100 }
101 #endif
Create and play with a Velocity6D.
Definition: alvelocity6d.h:24
Eigen::Matrix< float, 3, 4, Eigen::RowMajor > Matrix34frm
Definition: almatheigen.h:20
void toALMathTransform(const Eigen::MatrixBase< Derived0 > &in, Transform &out)
Definition: almatheigen.h:42
Eigen::AffineCompact3f toEigenAffineCompact3(const AL::Math::Transform &tr)
Definition: almatheigen.h:36
Eigen::Vector3f toEigenVector3(const AL::Math::Position3D &v)
Definition: almatheigen.h:32
Eigen::Matrix3f toEigenMatrix3(const AL::Math::Transform &tr)
Definition: almatheigen.h:27
void toALMathVelocity6D(const Eigen::MatrixBase< Derived0 > &in, Velocity6D &out)
Definition: almatheigen.h:81
Create and play with a Position3D.
Definition: alposition3d.h:23
A homogenous transformation matrix.
Definition: altransform.h:22
Eigen::Matrix< float, 3, 4 > Matrix34f
Definition: almatheigen.h:19
Position3D toALMathPosition3D(const Eigen::MatrixBase< Derived0 > &in)
Definition: almatheigen.h:72
Matrix34f toEigenMatrix34(const AL::Math::Transform &tr)
Definition: almatheigen.h:23
Eigen::Matrix< float, 6, 1 > Vector6f
Definition: almatheigen.h:21