Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
hom_transform.cpp
1 
2 /***************************************************************************
3  * hom_transform.h - Homogenous affine transformation
4  *
5  * Created: Wed Sep 26 14:47:35 2007
6  * Copyright 2007-2008 Daniel Beck
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 #include "hom_transform.h"
25 #include "hom_coord.h"
26 #include "matrix.h"
27 
28 #include <core/exceptions/software.h>
29 
30 #include <cmath>
31 
32 namespace fawkes {
33 
34 /** @class HomTransform geometry/hom_transform.h
35  * This class describes a homogeneous transformation.
36  * @author Daniel Beck
37  */
38 
39 /** Constructor. */
41 {
42  m_matrix = new Matrix(4, 4);
43  m_matrix->id();
44 }
45 
46 /** Copy constructor.
47  * @param t a HomTransform
48  */
50 {
51  m_matrix = new Matrix(*(t.m_matrix));
52 }
53 
54 /** Constructor from a Matrix.
55  * @param m a Matrix
56  */
58 {
59  if ((m.num_rows() != 4) || (m.num_cols() != 4))
60  {
61  throw fawkes::IllegalArgumentException("The matrix to create a HomTransform has to be 4x4.");
62  }
63 
64  m_matrix = new Matrix(m);
65 }
66 
67 /** Destructor. */
69 {
70  delete m_matrix;
71 }
72 
73 /** Reset transformation.
74  * @return reference to this
75  */
78 {
79  m_matrix->id();
80  return *this;
81 }
82 
83 /** Invert the transformation.
84  * @return reference to the inverted transformation
85  */
88 {
89  float ct[3] = { (*m_matrix)(0, 3), (*m_matrix)(1, 3), (*m_matrix)(2, 3) };
90  Matrix rot = m_matrix->get_submatrix(0, 0, 3, 3);
91 
92  m_matrix->overlay(0, 0, rot.transpose());
93  (*m_matrix)(0, 3) = -ct[0] * (*m_matrix)(0, 0) - ct[1] * (*m_matrix)(0, 1) - ct[2] * (*m_matrix)(0, 2);
94  (*m_matrix)(1, 3) = -ct[0] * (*m_matrix)(1, 0) - ct[1] * (*m_matrix)(1, 1) - ct[2] * (*m_matrix)(1, 2);
95  (*m_matrix)(2, 3) = -ct[0] * (*m_matrix)(2, 0) - ct[1] * (*m_matrix)(2, 1) - ct[2] * (*m_matrix)(2, 2);
96 
97  return *this;
98 }
99 
100 /** Obtain inverse transform.
101  * @return the inverse transform
102  */
105 {
106  HomTransform t(*this);
107  t.invert();
108 
109  return t;
110 }
111 
112 /** Add rotation around the x-axis.
113  * @param rad rotation angle in rad
114  */
115 void
117 {
118  float cos = cosf(rad);
119  float sin = sinf(rad);
120  float s1[3] = { (*m_matrix)(0,1), (*m_matrix)(1,1), (*m_matrix)(2,1) };
121  float s2[3] = { (*m_matrix)(0,2), (*m_matrix)(1,2), (*m_matrix)(2,2) };
122 
123  (*m_matrix)(0,1) = s1[0] * cos + s2[0] * sin;
124  (*m_matrix)(1,1) = s1[1] * cos + s2[1] * sin;
125  (*m_matrix)(2,1) = s1[2] * cos + s2[2] * sin;
126  (*m_matrix)(0,2) = -s1[0] * sin + s2[0] * cos;
127  (*m_matrix)(1,2) = -s1[1] * sin + s2[1] * cos;
128  (*m_matrix)(2,2) = -s1[2] * sin + s2[2] * cos;
129 }
130 
131 /** Add rotation around the y-axis.
132  * @param rad rotation angle in rad
133  */
134 void
136 {
137  float cos = cosf(rad);
138  float sin = sinf(rad);
139  float s1[3] = { (*m_matrix)(0,0), (*m_matrix)(1,0), (*m_matrix)(2,0) };
140  float s2[3] = { (*m_matrix)(0,2), (*m_matrix)(1,2), (*m_matrix)(2,2) };
141 
142  (*m_matrix)(0,0) = s1[0] * cos - s2[0] * sin;
143  (*m_matrix)(1,0) = s1[1] * cos - s2[1] * sin;
144  (*m_matrix)(2,0) = s1[2] * cos - s2[2] * sin;
145 
146  (*m_matrix)(0,2) = s1[0] * sin + s2[0] * cos;
147  (*m_matrix)(1,2) = s1[1] * sin + s2[1] * cos;
148  (*m_matrix)(2,2) = s1[2] * sin + s2[2] * cos;
149 }
150 
151 /** Add rotation around the z-axis.
152  * @param rad rotation angle in rad
153  */
154 void
156 {
157  float cos = cosf(rad);
158  float sin = sinf(rad);
159  float s1[3] = { (*m_matrix)(0,0), (*m_matrix)(1,0), (*m_matrix)(2,0) };
160  float s2[3] = { (*m_matrix)(0,1), (*m_matrix)(1,1), (*m_matrix)(2,1) };
161 
162  (*m_matrix)(0,0) = s1[0] * cos + s2[0] * sin;
163  (*m_matrix)(1,0) = s1[1] * cos + s2[1] * sin;
164  (*m_matrix)(2,0) = s1[2] * cos + s2[2] * sin;
165 
166  (*m_matrix)(0,1) = -s1[0] * sin + s2[0] * cos;
167  (*m_matrix)(1,1) = -s1[1] * sin + s2[1] * cos;
168  (*m_matrix)(2,1) = -s1[2] * sin + s2[2] * cos;
169 }
170 
171 /** Add translation to the transformation.
172  * @param dx offset along x-axis
173  * @param dy offset along y-axis
174  * @param dz offset along z-axis
175  */
176 void
177 HomTransform::trans(float dx, float dy, float dz)
178 {
179  (*m_matrix)(0, 3) += (*m_matrix)(0, 0) * dx + (*m_matrix)(0, 1) * dy + (*m_matrix)(0, 2) * dz;
180  (*m_matrix)(1, 3) += (*m_matrix)(1, 0) * dx + (*m_matrix)(1, 1) * dy + (*m_matrix)(1, 2) * dz;
181  (*m_matrix)(2, 3) += (*m_matrix)(2, 0) * dx + (*m_matrix)(2, 1) * dy + (*m_matrix)(2, 2) * dz;
182 }
183 
184 
185 /** Modified Denavit-Hartenberg transformation.
186  * DH-transformation as used by Aldebaran
187  * @see http://robocup.aldebaran-robotics.com/index.php?option=com_content&task=view&id=30#id2514205 "3.2.2.1.3.2. Forward kinematics model parameters"
188  *
189  * @param alpha the angle from the Z_i-1 axis to the Z_i axis about the X_i-1 axis
190  * @param a the offset distance between the Z_i-1 and Z_i axes along the X_i-1 axis
191  * @param theta the angle between the X_i-1 and X_i axes about the Z_i axis
192  * @param d the distance from the origin of frame X_i-1 to the X_i axis along the Z_i axis
193  */
194 void
195 HomTransform::mDH(const float alpha, const float a, const float theta, const float d)
196 {
197  if (alpha) rotate_x(alpha);
198  if (a || d) trans(a, 0, d);
199  if (theta) rotate_z(theta);
200 }
201 
202 
203 /** Set the translation.
204  * @param x the translation along the x-axis
205  * @param y the translation along the y-axis
206  * @param z the translation along the z-axis
207  */
208 void
209 HomTransform::set_trans(float x, float y, float z)
210 {
211  Matrix& matrix_ref = *m_matrix;
212  matrix_ref(0, 3) = x;
213  matrix_ref(1, 3) = y;
214  matrix_ref(2, 3) = z;
215 }
216 
217 /** Assignment operator.
218  * @param t the other transformation
219  * @return reference to the lhs transformation
220  */
223 {
224  (*m_matrix) = *(t.m_matrix);
225 
226  return *this;
227 }
228 
229 /** Multiplication-assignment operator.
230  * @param t the rhs transformation
231  * @return reference to the result of the multiplication
232  */
235 {
236  (*m_matrix) *= (*t.m_matrix);
237 
238  return *this;
239 }
240 
241 /** Comparison operator.
242  * @param t the other transformation
243  * @return true, if both transormations are equal
244  */
245 bool
247 {
248  return ((*m_matrix) == *(t.m_matrix));
249 }
250 
251 /** Prints the matrix.
252  * @param name Heading of the output
253  * @param col_sep a string used to separate columns (defaults to '\\t')
254  * @param row_sep a string used to separate rows (defaults to '\\n')
255  */
256 void
257 HomTransform::print_info(const char *name, const char *col_sep, const char *row_sep) const
258 {
259  m_matrix->print_info(name ? name : "HomTransform", col_sep, row_sep);
260 }
261 
262 
263 /** Returns a copy of the matrix.
264  * @return the matrix of the transformation
265  */
266 const Matrix&
268 {
269  return *m_matrix;
270 }
271 
272 } // end namespace fawkes
273