fEulerPara.cpp
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  * The University of Tokyo
8  */
9 /*
10  * fEulerPara.cpp
11  * Create: Katz Yamane, 01.07.09
12  */
13 
14 #include "fEulerPara.h"
15 
16 #define tiny 0.05
17 #define eps 1e-8
18 
19 void fEulerPara::interpolate(const fEulerPara& ep1, const fEulerPara& _ep2, double t)
20 {
21  fEulerPara ep2;
22  ep2.set(_ep2);
23  double cth = ep1 * ep2;
24  if(cth < 0)
25  {
26  ep2 *= -1.0;
27  cth *= -1.0;
28  }
29  if(cth > 1.0) cth = 1.0;
30  double th = acos(cth);
31  double sth = sin(th);
32  double th2 = th * t;
33  fEulerPara ep_temp1, ep_temp2;
34  if(fabs(sth) < eps)
35  {
36  ep_temp1 = (1-t)*ep1;
37  ep_temp2 = t*ep2;
38  add(ep_temp1, ep_temp2);
39  }
40  else
41  {
42  ep_temp1 = sin(th-th2) * ep1;
43  ep_temp2 = sin(th2) * ep2;
44  add(ep_temp1, ep_temp2);
45  (*this) /= sth;
46  }
47  unit();
48 }
49 
50 void fEulerPara::set(const fEulerPara& _ep)
51 {
52  m_scalar = _ep.m_scalar;
53  m_vec.set(_ep.m_vec);
54 }
55 
56 void fEulerPara::set(const fMat33& _mat)
57 {
58  static fMat33 mat;
59  mat.set(_mat);
60  double tr, s;
61  tr = mat(0,0) + mat(1,1) + mat(2,2);
62  if(tr >= 0)
63  {
64  s = sqrt(tr + 1);
65  m_scalar = 0.5 * s;
66  s = 0.5 / s;
67  m_vec(0) = (mat(2,1) - mat(1,2)) * s;
68  m_vec(1) = (mat(0,2) - mat(2,0)) * s;
69  m_vec(2) = (mat(1,0) - mat(0,1)) * s;
70  }
71  else
72  {
73  int i = 0;
74  if(mat(1,1) > mat(0,0)) i = 1;
75  if(mat(2,2) > mat(i,i)) i = 2;
76  switch(i)
77  {
78  case 0:
79  s = sqrt((mat(0,0) - (mat(1,1) + mat(2,2))) + 1);
80  m_vec(0) = 0.5 * s;
81  s = 0.5 / s;
82  m_vec(1) = (mat(0,1) + mat(1,0)) * s;
83  m_vec(2) = (mat(2,0) + mat(0,2)) * s;
84  m_scalar = (mat(2,1) - mat(1,2)) * s;
85  break;
86  case 1:
87  s = sqrt((mat(1,1) - (mat(2,2) + mat(0,0))) + 1);
88  m_vec(1) = 0.5 * s;
89  s = 0.5 / s;
90  m_vec(2) = (mat(1,2) + mat(2,1)) * s;
91  m_vec(0) = (mat(0,1) + mat(1,0)) * s;
92  m_scalar = (mat(0,2) - mat(2,0)) * s;
93  break;
94  case 2:
95  s = sqrt((mat(2,2) - (mat(0,0) + mat(1,1))) + 1);
96  m_vec(2) = 0.5 * s;
97  s = 0.5 / s;
98  m_vec(0) = (mat(2,0) + mat(0,2)) * s;
99  m_vec(1) = (mat(1,2) + mat(2,1)) * s;
100  m_scalar = (mat(1,0) - mat(0,1)) * s;
101  break;
102  }
103  }
104 }
105 
106 void fEulerPara::angvel2epdot(const fEulerPara& _ep, const fVec3& _omega)
107 {
108  fEulerPara ep(_ep);
109  static fVec3 vel;
110  vel.set(_omega);
111  double e0 = ep(3), e1 = ep(0), e2 = ep(1), e3 = ep(2);
112  double x = vel(0), y = vel(1), z = vel(2);
113  m_scalar = - 0.5 * (e1*x + e2*y + e3*z);
114  m_vec(0) = 0.5 * (e0*x - e3*y + e2*z);
115  m_vec(1) = 0.5 * (e3*x + e0*y - e1*z);
116  m_vec(2) = 0.5 * (- e2*x + e1*y + e0*z);
117 }
118 
119 fEulerPara mat2ep(const fMat33& _mat)
120 {
121  fEulerPara ret;
122  static fMat33 mat;
123  mat.set(_mat);
124 #if 1 // based on Baraff's code
125  double tr, s;
126  tr = mat(0,0) + mat(1,1) + mat(2,2);
127  if(tr >= 0)
128  {
129  s = sqrt(tr + 1);
130  ret(3) = 0.5 * s;
131  s = 0.5 / s;
132  ret(0) = (mat(2,1) - mat(1,2)) * s;
133  ret(1) = (mat(0,2) - mat(2,0)) * s;
134  ret(2) = (mat(1,0) - mat(0,1)) * s;
135  }
136  else
137  {
138  int i = 0;
139  if(mat(1,1) > mat(0,0)) i = 1;
140  if(mat(2,2) > mat(i,i)) i = 2;
141  switch(i)
142  {
143  case 0:
144  s = sqrt((mat(0,0) - (mat(1,1) + mat(2,2))) + 1);
145  ret(0) = 0.5 * s;
146  s = 0.5 / s;
147  ret(1) = (mat(0,1) + mat(1,0)) * s;
148  ret(2) = (mat(2,0) + mat(0,2)) * s;
149  ret(3) = (mat(2,1) - mat(1,2)) * s;
150  break;
151  case 1:
152  s = sqrt((mat(1,1) - (mat(2,2) + mat(0,0))) + 1);
153  ret(1) = 0.5 * s;
154  s = 0.5 / s;
155  ret(2) = (mat(1,2) + mat(2,1)) * s;
156  ret(0) = (mat(0,1) + mat(1,0)) * s;
157  ret(3) = (mat(0,2) - mat(2,0)) * s;
158  break;
159  case 2:
160  s = sqrt((mat(2,2) - (mat(0,0) + mat(1,1))) + 1);
161  ret(2) = 0.5 * s;
162  s = 0.5 / s;
163  ret(0) = (mat(2,0) + mat(0,2)) * s;
164  ret(1) = (mat(1,2) + mat(2,1)) * s;
165  ret(3) = (mat(1,0) - mat(0,1)) * s;
166  break;
167  }
168  }
169 #else
170  double e0, e1, e2, e3, temp, ee, e0e1, e0e2, e0e3;
171  ee = (mat(0,0) + mat(1,1) + mat(2,2) + 1) / 4;
172  if(ee < 0) ee = 0;
173  e0 = sqrt(ee);
174  ret.Ang() = e0;
175  if(e0 < tiny)
176  {
177  temp = ee - ((mat(1,1) + mat(2,2)) / 2);
178  if(temp < 0) temp = 0;
179  e1 = sqrt(temp);
180  e0e1 = mat(2,1) - mat(1,2);
181  if(e0e1 < 0) e1 *= -1.0;
182  ret(0) = e1;
183  if(e1 < tiny)
184  {
185  temp = ee - ((mat(2,2) + mat(0,0)) / 2);
186  if(temp < 0) temp = 0;
187  e2 = sqrt(temp);
188  e0e2 = mat(0,2) - mat(2,0);
189  if(e0e2 < 0) e2 *= -1.0;
190  ret(1) = e2;
191  if(e2 < tiny)
192  {
193  temp = ee - ((mat(0,0) + mat(1,1)) / 2);
194  if(temp < 0) temp = 0;
195  e3 = sqrt(temp);
196  e0e3 = mat(1,0) - mat(0,1);
197  if(e0e3 < 0) e3 *= -1.0;
198  ret(2) = e3;
199  }
200  else
201  {
202  temp = 4 * e2;
203  ret(2) = (mat(1,2) + mat(2,1)) / temp;
204  }
205  }
206  else
207  {
208  temp = 4 * e1;
209  ret(1) = (mat(0,1) + mat(1,0)) / temp;
210  ret(2) = (mat(0,2) + mat(2,0)) / temp;
211  }
212  }
213  else
214  {
215  temp = 4 * e0;
216  ret(0) = (mat(2,1) - mat(1,2)) / temp;
217  ret(1) = (mat(0,2) - mat(2,0)) / temp;
218  ret(2) = (mat(1,0) - mat(0,1)) / temp;
219  }
220 #endif
221  ret.unit();
222  return ret;
223 }
224 
225 fMat33 ep2mat(const fEulerPara& _epara)
226 {
227  fMat33 ret;
228  fEulerPara epara(_epara);
229  double ex = epara(0), ey = epara(1), ez = epara(2), e = epara(3);
230  double ee = e * e, exx = ex * ex, eyy = ey * ey, ezz = ez * ez;
231  double exy = ex * ey, eyz = ey * ez, ezx = ez * ex;
232  double eex = e * ex, eey = e * ey, eez = e * ez;
233  ret(0,0) = exx - eyy - ezz + ee;
234  ret(1,1) = - exx + eyy - ezz + ee;
235  ret(2,2) = - exx - eyy + ezz + ee;
236  ret(0,1) = 2.0 * (exy - eez);
237  ret(1,0) = 2.0 * (exy + eez);
238  ret(0,2) = 2.0 * (ezx + eey);
239  ret(2,0) = 2.0 * (ezx - eey);
240  ret(1,2) = 2.0 * (eyz - eex);
241  ret(2,1) = 2.0 * (eyz + eex);
242  return ret;
243 }
244 
245 fEulerPara fEulerPara::operator= (const fEulerPara& ep)
246 {
247  m_vec = ep.m_vec;
248  m_scalar = ep.m_scalar;
249  return (*this);
250 }
251 
252 fEulerPara fEulerPara::operator= (const fMat33& mat)
253 {
254  fMat33 tmp(mat);
255 // fEulerPara ep = mat2ep(tmp);
256 // (*this) = ep;
257  (*this) = mat2ep(tmp);
258  return *this;
259 }
260 
261 fEulerPara operator * (double d, const fEulerPara& ep)
262 {
263  fEulerPara ret;
264  ret.m_vec = d * ep.m_vec;
265  ret.m_scalar = d * ep.m_scalar;
266  return ret;
267 }
268 
269 fEulerPara angvel2epdot(const fEulerPara& _epara, const fVec3& vel)
270 {
271  fEulerPara ret, epara(_epara);
272  fMat33 mat(epara(3), -epara(2), epara(1), epara(2), epara(3), -epara(0), -epara(1), epara(0), epara(3));
273  ret(3) = - epara.Vec() * vel * 0.5;
274  ret.Axis() = mat * vel * 0.5;
275  return ret;
276 }
277 
278 fVec3 epdot2angvel(const fEulerPara& _epara, const fEulerPara& _edot)
279 {
280  fVec3 ret;
281  fEulerPara epara(_epara), edot(_edot);
282  fMat33 mat(epara(3), epara(2), -epara(1), -epara(2), epara(3), epara(0), epara(1), -epara(0), epara(3));
283  ret = (- edot(3)*epara.Vec() + mat*edot.Vec()) * 2;
284  return ret;
285 }
286 
287 fVec3 epddot2angacc(const fEulerPara& _e, const fEulerPara& _de, const fEulerPara& _dde)
288 {
289  fVec3 ret;
290  fEulerPara e(_e), de(_de), dde(_dde);
291  fMat33 mat1(e(3), e(2), -e(1), -e(2), e(3), e(0), e(1), -e(0), e(3));
292  fMat33 mat2(de(3), de(2), -de(1), -de(2), de(3), de(0), de(1), -de(0), de(3));
293  ret = 2 * (-de(3)*de.Vec() + mat2*de.Vec() - dde(3)*e.Vec() + mat1*dde.Vec());
294  return ret;
295 }
296 
297 void fEulerPara::unit()
298 {
299  double len = sqrt((*this) * (*this));
300  if(len > eps) (*this) /= len;
301 }
302 
303 fEulerPara unit(const fEulerPara& ep)
304 {
305  fEulerPara ret = ep;
306  ret.unit();
307  return ret;
308 }
309 
310 fEulerPara operator - (const fEulerPara& _ep)
311 {
312  fEulerPara ret, ep(_ep);
313  ret.Ang() = -ep.Ang();
314  ret.Axis() = -ep.Axis();
315  return ret;
316 }
317 
318 double fEulerPara::rotation(const fEulerPara& ep0, const fVec3& s)
319 {
320  double y = ep0.m_vec * s;
321  double x = ep0.m_scalar;
322  return atan2(y, x);
323 }
324 
fVec4::add
void add(const fVec4 &vec1, const fVec4 &vec2)
Definition: fMatrix4.cpp:156
i
png_uint_32 i
Definition: png.h:2732
fEulerPara::unit
void unit()
Convert to unit vector.
Definition: fEulerPara.cpp:297
fEulerPara::rotation
double rotation(const fEulerPara &ep0, const fVec3 &s)
returns the rotation angle that makes the orientation closest to ep0 by rotating around axis s
Definition: fEulerPara.cpp:318
fMat33::set
void set(const fMat33 &mat)
Copies a matrix.
Definition: fMatrix3.cpp:623
fEulerPara::Ang
friend double & Ang(fEulerPara &ep)
Access the scalar part (cos(theta/2))
Definition: fEulerPara.h:47
fVec3
3-element vector class.
Definition: fMatrix3.h:206
mat2ep
fEulerPara mat2ep(const fMat33 &_mat)
Definition: fEulerPara.cpp:119
fEulerPara::mat2ep
friend fEulerPara mat2ep(const fMat33 &)
Convert an orientation matrix to Euler parameters.
Definition: fEulerPara.cpp:119
fVec4::Vec
fVec3 & Vec()
Definition: fMatrix4.h:118
fEulerPara::angvel2epdot
void angvel2epdot(const fEulerPara &_ep, const fVec3 &_omega)
Convert angular velocity to Euler parameter veclotiy.
Definition: fEulerPara.cpp:106
fEulerPara::operator=
fEulerPara operator=(const fEulerPara &)
Assignment operator.
Definition: fEulerPara.cpp:245
fMat33
3x3 matrix class.
Definition: fMatrix3.h:29
angvel2epdot
fEulerPara angvel2epdot(const fEulerPara &_epara, const fVec3 &vel)
Definition: fEulerPara.cpp:269
unit
fEulerPara unit(const fEulerPara &ep)
Definition: fEulerPara.cpp:303
fVec3::set
void set(double *v)
Set element values from array or three values.
Definition: fMatrix3.h:314
operator-
fEulerPara operator-(const fEulerPara &_ep)
Definition: fEulerPara.cpp:310
eps
#define eps
Definition: fEulerPara.cpp:17
fEulerPara::interpolate
void interpolate(const fEulerPara &ep1, const fEulerPara &ep2, double t)
SLERP interpolation: interpolates ep1 and ep2 into t:(1-t)
Definition: fEulerPara.cpp:19
epddot2angacc
fVec3 epddot2angacc(const fEulerPara &_e, const fEulerPara &_de, const fEulerPara &_dde)
Definition: fEulerPara.cpp:287
fEulerPara::Axis
friend fVec3 & Axis(fEulerPara &ep)
Access the vector part (a*sin(theta/2))
Definition: fEulerPara.h:59
operator*
fEulerPara operator*(double d, const fEulerPara &ep)
Definition: fEulerPara.cpp:261
fVec4::m_scalar
double m_scalar
Definition: fMatrix4.h:161
fEulerPara.h
Euler parameter representation of orientation.
ep2mat
fMat33 ep2mat(const fEulerPara &_epara)
Definition: fEulerPara.cpp:225
epdot2angvel
fVec3 epdot2angvel(const fEulerPara &_epara, const fEulerPara &_edot)
Definition: fEulerPara.cpp:278
fVec4::m_vec
fVec3 m_vec
Definition: fMatrix4.h:160
fEulerPara
Euler parameter class.
Definition: fEulerPara.h:28
tiny
#define tiny
Definition: fEulerPara.cpp:16
fEulerPara::set
void set(const fVec3 &v, double s)
Set the elements.
Definition: fEulerPara.h:75

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:02