- Aldebaran documentation |
- C++ Libraries |
- index

libalmath 2.8.7.4

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List
- File Members

All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
qigeometry.h
Go to the documentation of this file.
1
8 #pragma once
9 #ifndef LIBALMATH_SCENEGRAPH_QIGEOMETRY_H
10 #define LIBALMATH_SCENEGRAPH_QIGEOMETRY_H
11
12
13 #include <Eigen/Geometry>
14
15 namespace qi
16 {
17 namespace geometry
18 {
19 inline Vector3 makeVector3(double x, double y, double z)
20 {
21 return Vector3{x, y, z};
22 }
23
24 inline std::ostream& operator<<(std::ostream &o, const Vector3 &t)
25 {
26 return o << "Vector3(" << t.x << ", " << t.y << ", " << t.z << ")";
27 }
28
29 inline Quaternion makeQuaternion(double x, double y, double z, double w)
30 {
31 return Quaternion{x, y, z, w};
32 }
33
34 inline Quaternion makeQuaternionFromAngleAxis(double angle, const Vector3 &axis)
35 {
36 Quaternion result;
37 Eigen::Map<Eigen::Quaterniond>(&result.x) =
38 Eigen::AngleAxisd(angle,
39 Eigen::Map<const Eigen::Vector3d>(&axis.x).normalized());
40 return result;
41 }
42
43 inline std::ostream& operator<<(std::ostream &o, const Quaternion &r)
44 {
45 return o << "Quaternion(" << r.x << ", " << r.y << ", " << r.z << ", "
46 << r.w << ")";
47 }
48
49 inline double norm(const Quaternion &r)
50 {
51 return Eigen::Map<const Eigen::Quaterniond>(&r.x).norm();
52 }
53
54 inline bool isNormalized(const Quaternion &r, double epsilon)
55 {
56 return std::abs(norm(r) - 1.0) < epsilon;
57 }
58
59 // Normalize a quaternion in place
60 inline void normalize(Quaternion &r)
61 {
62 Eigen::Map<Eigen::Quaterniond>(&r.x).normalize();
63 }
64
65 // Return a normalized copy of a quaternion
66 inline Quaternion normalized(const Quaternion &r)
67 {
68 Quaternion result = r;
69 normalize(result);
70 return result;
71 }
72
73 inline
74 Transform makeTransform(const Quaternion &rotation, const Vector3 &translation)
75 {
76 return Transform{rotation, translation};
77 }
78
79
80 inline std::ostream& operator<<(std::ostream &o, const Transform &tf)
81 {
82 return o << "Transform(" << tf.rotation << ", " << tf.translation << ")";
83 }
84
85
86 inline Eigen::Affine3d toEigenAffine3d(const Transform &tf)
87 {
88 assert(isNormalized(tf.rotation, 1e-5));
89 const auto& t = tf.translation;
90 const auto& r = tf.rotation;
91 return Eigen::Affine3d(Eigen::Translation3d(t.x, t.y, t.z) *
92 Eigen::Quaterniond(r.w, r.x, r.y, r.z));
93 }
94
95 // assumes lhs and rhs have normalized quaternions
96 inline Transform operator*(const Transform &lhs, const Transform &rhs)
97 {
98 Transform result;
99 assert(isNormalized(lhs.rotation, 1e-5));
100 assert(isNormalized(rhs.rotation, 1e-5));
101 auto lv = Eigen::Map<const Eigen::Vector3d>(&lhs.translation.x);
102 auto rv = Eigen::Map<const Eigen::Vector3d>(&rhs.translation.x);
103 auto lq = Eigen::Map<const Eigen::Quaterniond>(&lhs.rotation.x);
104 auto rq = Eigen::Map<const Eigen::Quaterniond>(&rhs.rotation.x);
105 Eigen::Map<Eigen::Quaterniond>(&result.rotation.x) = lq * rq;
106 Eigen::Map<Eigen::Vector3d>(&result.translation.x) = lv + lq * rv;
107 return result;
108 }
109
110 // assumes lhs and rhs have normalized quaternions
111 inline bool isNear(const Transform &lhs, const Transform &rhs, double epsilon)
112 {
113 auto la = toEigenAffine3d(lhs);
114 auto ra = toEigenAffine3d(rhs);
115 return la.isApprox(ra, epsilon);
116 }
117
118
119 // assumes tf has normalized quaternion
120 inline Transform inverse(const Transform &tf)
121 {
122 Transform result;
123 auto inv = toEigenAffine3d(tf).inverse();
124 Eigen::Map<Eigen::Quaterniond>(&result.rotation.x) = inv.rotation();
125 Eigen::Map<Eigen::Vector3d>(&result.translation.x) = inv.translation();
126 return result;
127 }
128
129 }
130 }
131
132 #endif
qi::geometry::operator<<
std::ostream & operator<<(std::ostream &o, const Vector3 &t)
Definition: qigeometry.h:24
qi::geometry::normalized
Quaternion normalized(const Quaternion &r)
Definition: qigeometry.h:66
qi::geometry::isNormalized
bool isNormalized(const Quaternion &r, double epsilon)
Definition: qigeometry.h:54
qi::geometry::normalize
void normalize(Quaternion &r)
Definition: qigeometry.h:60
qi::geometry::makeQuaternion
Quaternion makeQuaternion(double x, double y, double z, double w)
Definition: qigeometry.h:29
qi::geometry::isNear
bool isNear(const Transform &lhs, const Transform &rhs, double epsilon)
Definition: qigeometry.h:111
qi::geometry::inverse
Transform inverse(const Transform &tf)
Definition: qigeometry.h:120
qi::geometry::makeVector3
Vector3 makeVector3(double x, double y, double z)
Definition: qigeometry.h:19
qi::geometry::operator*
Transform operator*(const Transform &lhs, const Transform &rhs)
Definition: qigeometry.h:96
qi::geometry::makeQuaternionFromAngleAxis
Quaternion makeQuaternionFromAngleAxis(double angle, const Vector3 &axis)
Definition: qigeometry.h:34
qi::geometry::norm
double norm(const Quaternion &r)
Definition: qigeometry.h:49
qi::geometry::toEigenAffine3d
Eigen::Affine3d toEigenAffine3d(const Transform &tf)
Definition: qigeometry.h:86
qi::geometry::makeTransform
Transform makeTransform(const Quaternion &rotation, const Vector3 &translation)
Definition: qigeometry.h:74