knowrob  2.1.0
A Knowledge Base System for Cognition-enabled Robots
Namespaces | Macros | Functions
algebra.cpp File Reference
#include <Eigen/Geometry>
#include <SWI-cpp.h>
Include dependency graph for algebra.cpp:

Go to the source code of this file.

Namespaces

 knowrob
 
 knowrob::prolog
 

Macros

#define PL_SAFE_ARG_MACROS
 

Functions

void eigen2pl (const Eigen::Quaterniond &q, term_t &out)
 
void eigen2pl (const Eigen::Vector3d &v, term_t &out)
 
void eigen2pl (const Eigen::Matrix4d &m, term_t &out)
 
void pl2eigen (term_t &arg, Eigen::Quaterniond &q)
 
void pl2eigen (term_t &arg, Eigen::Vector3d &v)
 
void pl2eigen (term_t &arg, Eigen::Matrix4d &m)
 
foreign_t quaternion_inverse (term_t t_Quaternion, term_t t_Inverse)
 
foreign_t quaternion_transform (term_t t_Quaternion, term_t t_Vector, term_t t_Transformed)
 
foreign_t quaternion_multiply (term_t t_Quaternion1, term_t t_Quaternion2, term_t t_Multiplied)
 
foreign_t quaternion_diff (term_t t_Quaternion1, term_t t_Quaternion2, term_t t_Diff)
 
foreign_t quaternion_slerp (term_t t_Quaternion1, term_t t_Quaternion2, term_t t_Factor, term_t t_Interpolated)
 
foreign_t quaternion_matrix (term_t t_Quaternion, term_t t_Translation, term_t t_Matrix)
 
foreign_t matrix_quaternion (term_t t_Matrix, term_t t_Quaternion)
 
foreign_t matrix_translation (term_t t_Matrix, term_t t_Translation)
 

Macro Definition Documentation

◆ PL_SAFE_ARG_MACROS

#define PL_SAFE_ARG_MACROS

Definition at line 2 of file algebra.cpp.

Function Documentation

◆ eigen2pl() [1/3]

void eigen2pl ( const Eigen::Matrix4d &  m,
term_t &  out 
)

Definition at line 24 of file algebra.cpp.

24  {
25  PlTail l(out);
26  for (int i = 0; i < 4; ++i) {
27  for (int j = 0; j < 4; ++j) {
28  l.append(m(i, j));
29  }
30  }
31  l.close();
32 }

◆ eigen2pl() [2/3]

void eigen2pl ( const Eigen::Quaterniond &  q,
term_t &  out 
)

Definition at line 7 of file algebra.cpp.

7  {
8  PlTail l(out);
9  l.append(q.x());
10  l.append(q.y());
11  l.append(q.z());
12  l.append(q.w());
13  l.close();
14 }

◆ eigen2pl() [3/3]

void eigen2pl ( const Eigen::Vector3d &  v,
term_t &  out 
)

Definition at line 16 of file algebra.cpp.

16  {
17  PlTail l(out);
18  l.append(v.x());
19  l.append(v.y());
20  l.append(v.z());
21  l.close();
22 }

◆ matrix_quaternion()

foreign_t matrix_quaternion ( term_t  t_Matrix,
term_t  t_Quaternion 
)

Definition at line 133 of file algebra.cpp.

133  {
134  Eigen::Matrix4d m4;
135  pl2eigen(t_Matrix, m4);
136  Eigen::Matrix3d m3 = m4.block(0, 0, 3, 3);
137  Eigen::Quaterniond q(m3);
138  eigen2pl(q, t_Quaternion);
139  return TRUE;
140 }
pl2eigen
void pl2eigen(term_t &arg, Eigen::Quaterniond &q)
Definition: algebra.cpp:34
eigen2pl
void eigen2pl(const Eigen::Quaterniond &q, term_t &out)
Definition: algebra.cpp:7

◆ matrix_translation()

foreign_t matrix_translation ( term_t  t_Matrix,
term_t  t_Translation 
)

Definition at line 143 of file algebra.cpp.

143  {
144  Eigen::Matrix4d m;
145  pl2eigen(t_Matrix, m);
146  Eigen::Vector3d v = m.block<3, 1>(0, 3, 3, 1);
147  eigen2pl(v, t_Translation);
148  return TRUE;
149 }

◆ pl2eigen() [1/3]

void pl2eigen ( term_t &  arg,
Eigen::Matrix4d &  m 
)

Definition at line 58 of file algebra.cpp.

58  {
59  PlTail list(arg);
60  PlTerm value;
61  for (int i = 0; i < 4; ++i) {
62  for (int j = 0; j < 4; ++j) {
63  list.next(value);
64  m(i, j) = value;
65  }
66  }
67 }

◆ pl2eigen() [2/3]

void pl2eigen ( term_t &  arg,
Eigen::Quaterniond &  q 
)

Definition at line 34 of file algebra.cpp.

34  {
35  PlTail list(arg);
36  PlTerm value;
37  list.next(value);
38  q.x() = value;
39  list.next(value);
40  q.y() = value;
41  list.next(value);
42  q.z() = value;
43  list.next(value);
44  q.w() = value;
45 }

◆ pl2eigen() [3/3]

void pl2eigen ( term_t &  arg,
Eigen::Vector3d &  v 
)

Definition at line 47 of file algebra.cpp.

47  {
48  PlTail list(arg);
49  PlTerm value;
50  list.next(value);
51  v.x() = value;
52  list.next(value);
53  v.y() = value;
54  list.next(value);
55  v.z() = value;
56 }

◆ quaternion_diff()

foreign_t quaternion_diff ( term_t  t_Quaternion1,
term_t  t_Quaternion2,
term_t  t_Diff 
)

Definition at line 97 of file algebra.cpp.

97  {
98  Eigen::Quaterniond q1, q2;
99  pl2eigen(t_Quaternion1, q1);
100  pl2eigen(t_Quaternion2, q2);
101  eigen2pl(q1.inverse() * q2, t_Diff);
102  return TRUE;
103 }

◆ quaternion_inverse()

foreign_t quaternion_inverse ( term_t  t_Quaternion,
term_t  t_Inverse 
)

Definition at line 70 of file algebra.cpp.

70  {
71  Eigen::Quaterniond q;
72  pl2eigen(t_Quaternion, q);
73  eigen2pl(q.inverse(), t_Inverse);
74  return TRUE;
75 }

◆ quaternion_matrix()

foreign_t quaternion_matrix ( term_t  t_Quaternion,
term_t  t_Translation,
term_t  t_Matrix 
)

Definition at line 118 of file algebra.cpp.

118  {
119  Eigen::Quaterniond q;
120  Eigen::Vector3d v;
121  pl2eigen(t_Quaternion, q);
122  pl2eigen(t_Translation, v);
123  Eigen::Matrix4d m = Eigen::Matrix4d::Identity();
124  // 3x3 block starting at (0,0)
125  m.block(0, 0, 3, 3) = q.toRotationMatrix();
126  // 3x1 block starting at (0,3)
127  m.block(0, 3, 3, 1) = v;
128  eigen2pl(m, t_Matrix);
129  return TRUE;
130 }

◆ quaternion_multiply()

foreign_t quaternion_multiply ( term_t  t_Quaternion1,
term_t  t_Quaternion2,
term_t  t_Multiplied 
)

Definition at line 88 of file algebra.cpp.

88  {
89  Eigen::Quaterniond q1, q2;
90  pl2eigen(t_Quaternion1, q1);
91  pl2eigen(t_Quaternion2, q2);
92  eigen2pl(q1 * q2, t_Multiplied);
93  return TRUE;
94 }

◆ quaternion_slerp()

foreign_t quaternion_slerp ( term_t  t_Quaternion1,
term_t  t_Quaternion2,
term_t  t_Factor,
term_t  t_Interpolated 
)

Definition at line 106 of file algebra.cpp.

106  {
107  Eigen::Quaterniond q1, q2;
108  pl2eigen(t_Quaternion1, q1);
109  pl2eigen(t_Quaternion2, q2);
110  double factor = 0.0;
111  if (!PL_get_float(t_Factor, &factor)) return FALSE;
112  Eigen::Quaterniond q = q1.slerp(factor, q2);
113  eigen2pl(q, t_Interpolated);
114  return TRUE;
115 }

◆ quaternion_transform()

foreign_t quaternion_transform ( term_t  t_Quaternion,
term_t  t_Vector,
term_t  t_Transformed 
)

Definition at line 78 of file algebra.cpp.

78  {
79  Eigen::Quaterniond q;
80  Eigen::Vector3d v;
81  pl2eigen(t_Quaternion, q);
82  pl2eigen(t_Vector, v);
83  eigen2pl(q * v, t_Transformed);
84  return TRUE;
85 }