homogen.cpp
Go to the documentation of this file.
1 /*
2 ROBOOP -- A robotics object oriented package in C++
3 Copyright (C) 1996-2004 Richard Gourdeau
4 
5 This library is free software; you can redistribute it and/or modify
6 it under the terms of the GNU Lesser General Public License as
7 published by the Free Software Foundation; either version 2.1 of the
8 License, or (at your option) any later version.
9 
10 This library is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU Lesser General Public License for more details.
14 
15 You should have received a copy of the GNU Lesser General Public
16 License along with this library; if not, write to the Free Software
17 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 
19 
20 Report problems and direct all questions to:
21 
22 Richard Gourdeau
23 Professeur Agrege
24 Departement de genie electrique
25 Ecole Polytechnique de Montreal
26 C.P. 6079, Succ. Centre-Ville
27 Montreal, Quebec, H3C 3A7
28 
29 email: richard.gourdeau@polymtl.ca
30 
31 -------------------------------------------------------------------------------
32 Revision_history:
33 
34 2004/07/01: Etienne Lachance
35  -Added protection on trans function.
36  -Added doxygen documentation.
37 
38 2004/07/01: Ethan Tira-Thompson
39  -Added support for newmat's use_namespace #define, using ROBOOP namespace
40 
41 2004/08/10: Etienne Lachance
42  -Make sure input vector is 3x1 in trans function.
43 
44 2006/01/21: Etienne Lachance
45  -Include utils.h instead of robot.h.
46 
47 2006/11/15: Richard Gourdeau
48  -atan2() in irotk()
49 -------------------------------------------------------------------------------
50 */
51 
57 static const char rcsid[] = "$Id: homogen.cpp,v 1.15 2006/11/15 18:35:17 gourdeau Exp $";
59 
60 #include "utils.h"
61 
62 #ifdef use_namespace
63 namespace ROBOOP {
64  using namespace NEWMAT;
65 #endif
66 
67 
70 {
71  Matrix translation(4,4);
72 
73  translation << fourbyfourident; // identity matrix
74 
75  if (a.Nrows() == 3)
76  {
77  translation(1,4) = a(1);
78  translation(2,4) = a(2);
79  translation(3,4) = a(3);
80  }
81  else
82  cerr << "trans: wrong size in input vector." << endl;
83 
84  translation.Release(); return translation;
85 }
86 
89 {
90  Matrix rot(4,4);
91  Real c, s;
92 
93  rot << fourbyfourident; // identity matrix
94 
95  c = cos(alpha);
96  s = sin(alpha);
97 
98  rot(2,2) = c;
99  rot(3,3) = c;
100  rot(2,3) = -s;
101  rot(3,2) = s;
102 
103  rot.Release(); return rot;
104 }
105 
106 
107 ReturnMatrix roty(const Real beta)
109 {
110  Matrix rot(4,4);
111  Real c, s;
112 
113  rot << fourbyfourident; // identity matrix
114 
115  c = cos(beta);
116  s = sin(beta);
117 
118  rot(1,1) = c;
119  rot(3,3) = c;
120  rot(1,3) = s;
121  rot(3,1) = -s;
122 
123  rot.Release(); return rot;
124 }
125 
126 
127 ReturnMatrix rotz(const Real gamma)
129 {
130  Matrix rot(4,4);
131  Real c, s;
132 
133  rot << fourbyfourident; // identity matrix
134 
135  c = cos(gamma);
136  s = sin(gamma);
137 
138  rot(1,1) = c;
139  rot(2,2) = c;
140  rot(1,2) = -s;
141  rot(2,1) = s;
142 
143  rot.Release(); return rot;
144 }
145 
146 // compound rotations
147 
148 
149 ReturnMatrix rotk(const Real theta, const ColumnVector & k)
151 {
152  Matrix rot(4,4);
153  Real c, s, vers, kx, ky, kz;
154 
155  rot << fourbyfourident; // identity matrix
156 
157  vers = SumSquare(k.SubMatrix(1,3,1,1));
158  if (vers != 0.0) { // compute the rotation if the vector norm is not 0.0
159  vers = sqrt(1/vers);
160  kx = k(1)*vers;
161  ky = k(2)*vers;
162  kz = k(3)*vers;
163  s = sin(theta);
164  c = cos(theta);
165  vers = 1-c;
166 
167  rot(1,1) = kx*kx*vers+c;
168  rot(1,2) = kx*ky*vers-kz*s;
169  rot(1,3) = kx*kz*vers+ky*s;
170  rot(2,1) = kx*ky*vers+kz*s;
171  rot(2,2) = ky*ky*vers+c;
172  rot(2,3) = ky*kz*vers-kx*s;
173  rot(3,1) = kx*kz*vers-ky*s;
174  rot(3,2) = ky*kz*vers+kx*s;
175  rot(3,3) = kz*kz*vers+c;
176  }
177 
178  rot.Release(); return rot;
179 }
180 
181 
184 {
185  Matrix rot(4,4);
186  Real ca, sa, cb, sb, cc, sc;
187 
188  rot << fourbyfourident; // identity matrix
189 
190  ca = cos(a(1));
191  sa = sin(a(1));
192  cb = cos(a(2));
193  sb = sin(a(2));
194  cc = cos(a(3));
195  sc = sin(a(3));
196 
197  rot(1,1) = cb*cc;
198  rot(1,2) = sa*sb*cc-ca*sc;
199  rot(1,3) = ca*sb*cc+sa*sc;
200  rot(2,1) = cb*sc;
201  rot(2,2) = sa*sb*sc+ca*cc;
202  rot(2,3) = ca*sb*sc-sa*cc;
203  rot(3,1) = -sb;
204  rot(3,2) = sa*cb;
205  rot(3,3) = ca*cb;
206 
207  rot.Release(); return rot;
208 }
209 
210 
213 {
214  Matrix rot(4,4);
215  Real c1, s1, ca, sa, c2, s2;
216 
217  rot << fourbyfourident; // identity matrix
218 
219  c1 = cos(a(1));
220  s1 = sin(a(1));
221  ca = cos(a(2));
222  sa = sin(a(2));
223  c2 = cos(a(3));
224  s2 = sin(a(3));
225 
226  rot(1,1) = c1*c2-s1*ca*s2;
227  rot(1,2) = -c1*s2-s1*ca*c2;
228  rot(1,3) = sa*s1;
229  rot(2,1) = s1*c2+c1*ca*s2;
230  rot(2,2) = -s1*s2+c1*ca*c2;
231  rot(2,3) = -sa*c1;
232  rot(3,1) = sa*s2;
233  rot(3,2) = sa*c2;
234  rot(3,3) = ca;
235 
236  rot.Release(); return rot;
237 }
238 
239 
240 ReturnMatrix rotd(const Real theta, const ColumnVector & k1,
241  const ColumnVector & k2)
243 {
244  Matrix rot;
245 
246  rot = trans(k1)*rotk(theta,k2-k1)*trans(-k1);
247 
248  rot.Release(); return rot;
249 }
250 
251 // inverse problem for compound rotations
252 
255 {
256  ColumnVector k(4);
257  Real a, b, c;
258 
259  a = (R(3,2)-R(2,3));
260  b = (R(1,3)-R(3,1));
261  c = (R(2,1)-R(1,2));
262  k(4) = atan2(sqrt(a*a + b*b + c*c),(R(1,1) + R(2,2) + R(3,3)-1));
263  k(1) = (R(3,2)-R(2,3))/(2*sin(k(4)));
264  k(2) = (R(1,3)-R(3,1))/(2*sin(k(4)));
265  k(3) = (R(2,1)-R(1,2))/(2*sin(k(4)));
266 
267  k.Release(); return k;
268 }
269 
270 
273 {
274  ColumnVector k(3);
275 
276  if (R(3,1)==1) {
277  k(1) = atan2(-R(1,2),-R(1,3));
278  k(2) = -M_PI/2;
279  k(3) = 0.0;
280  } else if (R(3,1)==-1) {
281  k(1) = atan2(R(1,2),R(1,3));
282  k(2) = M_PI/2;
283  k(3) = 0.0;
284  } else {
285  k(1) = atan2(R(3,2), R(3,3));
286  k(2) = atan2(-R(3,1), sqrt(R(1,1)*R(1,1) + R(2,1)*R(2,1)));
287  k(3) = atan2(R(2,1), R(1,1));
288  }
289 
290  k.Release(); return k;
291 }
292 
293 
296 {
297  ColumnVector a(3);
298 
299  if ((R(3,3)==1) || (R(3,3)==-1)) {
300  a(1) = 0.0;
301  a(2) = ((R(3,3) == 1) ? 0.0 : M_PI);
302  a(3) = atan2(R(2,1),R(1,1));
303  } else {
304  a(1) = atan2(R(1,3), -R(2,3));
305  a(2) = atan2(sqrt(R(1,3)*R(1,3) + R(2,3)*R(2,3)), R(3,3));
306  a(3) = atan2(R(3,1), R(3,2));
307  }
308 
309  a.Release(); return a;
310 }
311 
312 #ifdef use_namespace
313 }
314 #endif
void Release()
Definition: newmat.h:514
ReturnMatrix rotd(const Real theta, const ColumnVector &k1, const ColumnVector &k2)
Rotation around an arbitrary line.
Definition: homogen.cpp:240
ReturnMatrix trans(const ColumnVector &a)
Translation.
Definition: homogen.cpp:68
ReturnMatrix rpy(const ColumnVector &a)
Roll Pitch Yaw rotation.
Definition: homogen.cpp:182
double Real
Definition: include.h:307
Real SumSquare(const BaseMatrix &B)
Definition: newmat.h:2095
int Nrows() const
Definition: newmat.h:494
Utility header file.
ReturnMatrix rotx(const Real alpha)
Rotation around x axis.
Definition: homogen.cpp:87
ReturnMatrix irotk(const Matrix &R)
Obtain axis from a rotation matrix.
Definition: homogen.cpp:253
ReturnMatrix ieulzxz(const Matrix &R)
Obtain Roll, Pitch and Yaw from a rotation matrix.
Definition: homogen.cpp:294
ReturnMatrix rotz(const Real gamma)
Rotation around z axis.
Definition: homogen.cpp:127
The usual rectangular matrix.
Definition: newmat.h:625
FloatVector FloatVector * a
ReturnMatrix roty(const Real beta)
Rotation around x axis.
Definition: homogen.cpp:107
ReturnMatrix irpy(const Matrix &R)
Obtain Roll, Pitch and Yaw from a rotation matrix.
Definition: homogen.cpp:271
GetSubMatrix SubMatrix(int fr, int lr, int fc, int lc) const
Definition: newmat.h:2146
FloatVector FloatVector FloatVector * alpha
ReturnMatrix eulzxz(const ColumnVector &a)
Euler ZXZ rotation.
Definition: homogen.cpp:211
Column vector.
Definition: newmat.h:1008
Real fourbyfourident[]
Used to initialize a matrix.
Definition: robot.cpp:133
static const char rcsid[]
RCS/CVS version.
Definition: homogen.cpp:58
ReturnMatrix rotk(const Real theta, const ColumnVector &k)
Rotation around arbitrary axis.
Definition: homogen.cpp:149


kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:16