Eigen3d.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  * General Robotix Inc.
9  */
10 
11 #ifndef HRPUTIL_EIGEN3D_H_INCLUDED
12 #define HRPUTIL_EIGEN3D_H_INCLUDED
13 
14 #include "config.h"
15 #include "EigenTypes.h"
16 
17 namespace hrp
18 {
19  typedef Eigen::Vector3d Vector3Ref;
20  inline Vector3Ref getVector3Ref(const double* data) {
21  return Vector3Ref(data[0], data[1], data[2]);
22  }
23 
24  HRP_UTIL_EXPORT void calcRodrigues(Matrix33& out_R, const Vector3& axis, double q);
25  HRP_UTIL_EXPORT void calcRotFromRpy(Matrix33& out_R, double r, double p, double y);
26 
27  inline Matrix33 rodrigues(const Vector3& axis, double q){
28  Matrix33 R;
29  calcRodrigues(R, axis, q);
30  return R;
31  }
32 
33  inline Matrix33 rotFromRpy(const Vector3& rpy){
34  Matrix33 R;
35  calcRotFromRpy(R, rpy[0], rpy[1], rpy[2]);
36  return R;
37  }
38 
39  inline Matrix33 rotFromRpy(double r, double p, double y){
40  Matrix33 R;
41  calcRotFromRpy(R, r, p, y);
42  return R;
43  }
44 
45  inline Matrix33 rotationX(double theta){
46  Matrix33 R;
47  double c = cos(theta);
48  double s = sin(theta);
49  R << 1.0, 0.0, 0.0,
50  0.0, c, -s,
51  0.0, s, c ;
52  return R;
53  }
54 
55  inline Matrix33 rotationY(double theta){
56  Matrix33 R;
57  double c = cos(theta);
58  double s = sin(theta);
59  R << c, 0.0, s,
60  0.0, 1.0, 0.0,
61  -s, 0.0, c ;
62  return R;
63  }
64 
65  inline Matrix33 rotationZ(double theta){
66  Matrix33 R;
67  double c = cos(theta);
68  double s = sin(theta);
69  R << c, -s, 0.0,
70  s, c, 0.0,
71  0.0, 0.0, 1.0;
72  return R;
73  }
74 
77 
79 
80  inline Matrix33 hat(const Vector3& c) {
81  Matrix33 m;
82  m << 0.0, -c(2), c(1),
83  c(2), 0.0, -c(0),
84  -c(1), c(0), 0.0;
85  return m;
86  }
87 
88  template<class M> inline void setMatrix33(const Matrix33& m33, M& m, size_t row = 0, size_t col = 0){
89  m(row, col) = m33(0, 0); m(row, col+1) = m33(0, 1); m(row, col+2) = m33(0, 2);
90  ++row;
91  m(row, col) = m33(1, 0); m(row, col+1) = m33(1, 1); m(row, col+2) = m33(1, 2);
92  ++row;
93  m(row, col) = m33(2, 0); m(row, col+1) = m33(2, 1); m(row, col+2) = m33(2, 2);
94  }
95 
96  template<class M> inline void setTransMatrix33(const Matrix33& m33, M& m, size_t row = 0, size_t col = 0){
97  m(row, col) = m33(0, 0); m(row, col+1) = m33(1, 0); m(row, col+2) = m33(2, 0);
98  ++row;
99  m(row, col) = m33(0, 1); m(row, col+1) = m33(1, 1); m(row, col+2) = m33(2, 1);
100  ++row;
101  m(row, col) = m33(0, 2); m(row, col+1) = m33(1, 2); m(row, col+2) = m33(2, 2);
102  }
103 
104  template<class Array> inline void setMatrix33ToRowMajorArray
105  (const Matrix33& m33, Array& a, size_t top = 0) {
106  a[top++] = m33(0, 0);
107  a[top++] = m33(0, 1);
108  a[top++] = m33(0, 2);
109  a[top++] = m33(1, 0);
110  a[top++] = m33(1, 1);
111  a[top++] = m33(1, 2);
112  a[top++] = m33(2, 0);
113  a[top++] = m33(2, 1);
114  a[top ] = m33(2, 2);
115  }
116 
117  template<class Array> inline void getMatrix33FromRowMajorArray
118  (Matrix33& m33, const Array& a, size_t top = 0) {
119  m33(0, 0) = a[top++];
120  m33(0, 1) = a[top++];
121  m33(0, 2) = a[top++];
122  m33(1, 0) = a[top++];
123  m33(1, 1) = a[top++];
124  m33(1, 2) = a[top++];
125  m33(2, 0) = a[top++];
126  m33(2, 1) = a[top++];
127  m33(2, 2) = a[top ];
128  }
129 
130  template<class V> inline void setVector3(const Vector3& v3, V& v, size_t top = 0){
131  v[top++] = v3(0); v[top++] = v3(1); v[top] = v3(2);
132  }
133 
134  template<class V> inline void setVector3(const Vector3& v3, const V& v, size_t top = 0){
135  v[top++] = v3(0); v[top++] = v3(1); v[top] = v3(2);
136  }
137 
138  template<class V> inline void getVector3(Vector3& v3, const V& v, size_t top = 0){
139  v3(0) = v[top++]; v3(1) = v[top++]; v3(2) = v[top];
140  }
141 
142  template<class M> inline void setVector3(const Vector3& v3, M& m, size_t row, size_t col){
143  m(row++, col) = v3(0); m(row++, col) = v3(1); m(row, col) = v3(2);
144  }
145 
146  template<class M> inline void getVector3(Vector3& v3, const M& m, size_t row, size_t col){
147  v3(0) = m(row++, col);
148  v3(1) = m(row++, col);
149  v3(2) = m(row, col);
150  }
151 
152 };
153 
154 #endif
int c
Definition: autoplay.py:16
Matrix33 rotationX(double theta)
Definition: Eigen3d.h:45
Matrix33 rotationZ(double theta)
Definition: Eigen3d.h:65
HRP_UTIL_EXPORT bool isOrthogonalMatrix(Matrix33 &m)
Definition: Eigen3d.cpp:152
HRP_UTIL_EXPORT void calcRotFromRpy(Matrix33 &out_R, double r, double p, double y)
Definition: Eigen3d.cpp:138
void setVector3(const Vector3 &v3, V &v, size_t top=0)
Definition: Eigen3d.h:130
void getMatrix33FromRowMajorArray(Matrix33 &m33, const Array &a, size_t top=0)
Definition: Eigen3d.h:118
Vector3Ref getVector3Ref(const double *data)
Definition: Eigen3d.h:20
Eigen::Vector3d Vector3Ref
Definition: Eigen3d.h:19
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
Matrix33 rotFromRpy(const Vector3 &rpy)
Definition: Eigen3d.h:33
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
void setMatrix33ToRowMajorArray(const Matrix33 &m33, Array &a, size_t top=0)
Definition: Eigen3d.h:105
Matrix33 rodrigues(const Vector3 &axis, double q)
Definition: Eigen3d.h:27
#define HRP_UTIL_EXPORT
png_bytepp row
Definition: png.h:1759
void setMatrix33(const Matrix33 &m33, M &m, size_t row=0, size_t col=0)
Definition: Eigen3d.h:88
HRP_UTIL_EXPORT Vector3 rpyFromRot(const Matrix33 &m)
Definition: Eigen3d.cpp:99
void setTransMatrix33(const Matrix33 &m33, M &m, size_t row=0, size_t col=0)
Definition: Eigen3d.h:96
Matrix33 hat(const Vector3 &c)
Definition: Eigen3d.h:80
HRP_UTIL_EXPORT void calcRodrigues(Matrix33 &out_R, const Vector3 &axis, double q)
Definition: Eigen3d.cpp:22
HRP_UTIL_EXPORT Vector3 omegaFromRot(const Matrix33 &r)
Definition: Eigen3d.cpp:63
void getVector3(Vector3 &v3, const V &v, size_t top=0)
Definition: Eigen3d.h:138
JSAMPIMAGE data
Definition: jpeglib.h:945
* y
Definition: IceUtils.h:97
Matrix33 rotationY(double theta)
Definition: Eigen3d.h:55


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:03