00001
00002 /***************************************************************************
00003  *  hom_transform.h - Homogenous affine transformation
00004  *
00005  *  Created: Wed Sep 26 14:47:35 2007
00006  *  Copyright  2007-2008  Daniel Beck
00007  *
00008  ****************************************************************************/
00009
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00022  */
00023
00024 #include "hom_transform.h"
00025 #include "hom_coord.h"
00026 #include "matrix.h"
00027
00028 #include <core/exceptions/software.h>
00029
00030 #include <cmath>
00031
00032 namespace fawkes {
00033 
00034 /** @class HomTransform geometry/hom_transform.h
00035  * This class describes a homogeneous transformation.
00036  * @author Daniel Beck
00037  */
00038 
00039 /** Constructor. */
00040 HomTransform::HomTransform()
00041 {
00042   m_matrix = new Matrix(4, 4);
00043   m_matrix->id();
00044 }
00045 
00046 /** Copy constructor.
00047  * @param t a HomTransform
00048  */
00049 HomTransform::HomTransform(const HomTransform& t)
00050 {
00051   m_matrix = new Matrix(*(t.m_matrix));
00052 }
00053 
00054 /** Constructor from a Matrix.
00055  * @param m a Matrix
00056  */
00057 HomTransform::HomTransform(const Matrix& m)
00058 {
00059   if ((m.num_rows() != 4) || (m.num_cols() != 4))
00060   {
00061     throw fawkes::IllegalArgumentException("The matrix to create a HomTransform has to be 4x4.");
00062   }
00063
00064   m_matrix = new Matrix(m);
00065 }
00066 
00067 /** Destructor */
00068 HomTransform::~HomTransform()
00069 {
00070   delete m_matrix;
00071 }
00072 
00073 /** Reset transformation. */
00074 HomTransform&
00075 HomTransform::reset()
00076 {
00077   m_matrix->id();
00078   return *this;
00079 }
00080 
00081 /** Invert the transformation.
00082  * @return reference to the inverted transformation
00083  */
00084 HomTransform&
00085 HomTransform::invert()
00086 {
00087   float ct[3] = { (*m_matrix)(0, 3), (*m_matrix)(1, 3), (*m_matrix)(2, 3) };
00088   Matrix rot  = m_matrix->get_submatrix(0, 0, 3, 3);
00089
00090   m_matrix->overlay(0, 0, rot.transpose());
00091   (*m_matrix)(0, 3) = -ct[0] * (*m_matrix)(0, 0) - ct[1] * (*m_matrix)(0, 1) - ct[2] * (*m_matrix)(0, 2);
00092   (*m_matrix)(1, 3) = -ct[0] * (*m_matrix)(1, 0) - ct[1] * (*m_matrix)(1, 1) - ct[2] * (*m_matrix)(1, 2);
00093   (*m_matrix)(2, 3) = -ct[0] * (*m_matrix)(2, 0) - ct[1] * (*m_matrix)(2, 1) - ct[2] * (*m_matrix)(2, 2);
00094
00095   return *this;
00096 }
00097 
00098 /** Obtain inverse transform.
00099  * @return the inverse transform
00100  */
00101 HomTransform
00102 HomTransform::get_inverse()
00103 {
00104   HomTransform t(*this);
00105   t.invert();
00106
00107   return t;
00108 }
00109 
00110 /** Add rotation around the x-axis.
00111  * @param rad rotation angle in rad
00112  */
00113 void
00114 HomTransform::rotate_x(float rad)
00115 {
00116   float cos = cosf(rad);
00117   float sin = sinf(rad);
00118   float s1[3] = { (*m_matrix)(0,1), (*m_matrix)(1,1), (*m_matrix)(2,1) };
00119   float s2[3] = { (*m_matrix)(0,2), (*m_matrix)(1,2), (*m_matrix)(2,2) };
00120
00121   (*m_matrix)(0,1) = s1[0] * cos + s2[0] * sin;
00122   (*m_matrix)(1,1) = s1[1] * cos + s2[1] * sin;
00123   (*m_matrix)(2,1) = s1[2] * cos + s2[2] * sin;
00124   (*m_matrix)(0,2) = -s1[0] * sin + s2[0] * cos;
00125   (*m_matrix)(1,2) = -s1[1] * sin + s2[1] * cos;
00126   (*m_matrix)(2,2) = -s1[2] * sin + s2[2] * cos;
00127 }
00128 
00129 /** Add rotation around the y-axis.
00130  * @param rad rotation angle in rad
00131  */
00132 void
00133 HomTransform::rotate_y(float rad)
00134 {
00135   float cos = cosf(rad);
00136   float sin = sinf(rad);
00137   float s1[3] = { (*m_matrix)(0,0), (*m_matrix)(1,0), (*m_matrix)(2,0) };
00138   float s2[3] = { (*m_matrix)(0,2), (*m_matrix)(1,2), (*m_matrix)(2,2) };
00139
00140   (*m_matrix)(0,0) = s1[0] * cos - s2[0] * sin;
00141   (*m_matrix)(1,0) = s1[1] * cos - s2[1] * sin;
00142   (*m_matrix)(2,0) = s1[2] * cos - s2[2] * sin;
00143
00144   (*m_matrix)(0,2) = s1[0] * sin + s2[0] * cos;
00145   (*m_matrix)(1,2) = s1[1] * sin + s2[1] * cos;
00146   (*m_matrix)(2,2) = s1[2] * sin + s2[2] * cos;
00147 }
00148 
00149 /** Add rotation around the z-axis.
00150  * @param rad rotation angle in rad
00151  */
00152 void
00153 HomTransform::rotate_z(float rad)
00154 {
00155   float cos = cosf(rad);
00156   float sin = sinf(rad);
00157   float s1[3] = { (*m_matrix)(0,0), (*m_matrix)(1,0), (*m_matrix)(2,0) };
00158   float s2[3] = { (*m_matrix)(0,1), (*m_matrix)(1,1), (*m_matrix)(2,1) };
00159
00160   (*m_matrix)(0,0) = s1[0] * cos + s2[0] * sin;
00161   (*m_matrix)(1,0) = s1[1] * cos + s2[1] * sin;
00162   (*m_matrix)(2,0) = s1[2] * cos + s2[2] * sin;
00163
00164   (*m_matrix)(0,1) = -s1[0] * sin + s2[0] * cos;
00165   (*m_matrix)(1,1) = -s1[1] * sin + s2[1] * cos;
00166   (*m_matrix)(2,1) = -s1[2] * sin + s2[2] * cos;
00167 }
00168 
00169 /** Add translation to the transformation.
00170  * @param dx offset along x-axis
00171  * @param dy offset along y-axis
00172  * @param dz offset along z-axis
00173  */
00174 void
00175 HomTransform::trans(float dx, float dy, float dz)
00176 {
00177   (*m_matrix)(0, 3) += (*m_matrix)(0, 0) * dx + (*m_matrix)(0, 1) * dy + (*m_matrix)(0, 2) * dz;
00178   (*m_matrix)(1, 3) += (*m_matrix)(1, 0) * dx + (*m_matrix)(1, 1) * dy + (*m_matrix)(1, 2) * dz;
00179   (*m_matrix)(2, 3) += (*m_matrix)(2, 0) * dx + (*m_matrix)(2, 1) * dy + (*m_matrix)(2, 2) * dz;
00180 }
00181
00182 
00183 /** Modified Denavit-Hartenberg transformation.
00184  * DH-transformation as used by Aldebaran
00185  * @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"
00186  *
00187  * @param alpha the angle from the Z_i-1 axis to the Z_i axis about the X_i-1 axis
00188  * @param a     the offset distance between the Z_i-1 and Z_i axes along the X_i-1 axis
00189  * @param theta the angle between the X_i-1 and X_i axes about the Z_i axis
00190  * @param d     the distance from the origin of frame X_i-1 to the X_i axis along the Z_i axis
00191  */
00192 void
00193 HomTransform::mDH(const float alpha, const float a, const float theta, const float d)
00194 {
00195   if (alpha)  rotate_x(alpha);
00196   if (a || d) trans(a, 0, d);
00197   if (theta)  rotate_z(theta);
00198 }
00199
00200 
00201 /** Set the translation.
00202  * @param x the translation along the x-axis
00203  * @param y the translation along the y-axis
00204  * @param z the translation along the z-axis
00205  */
00206 void
00207 HomTransform::set_trans(float x, float y, float z)
00208 {
00209   Matrix& matrix_ref = *m_matrix;
00210   matrix_ref(0, 3) = x;
00211   matrix_ref(1, 3) = y;
00212   matrix_ref(2, 3) = z;
00213 }
00214 
00215 /** Assignment operator.
00216  * @param t the other transformation
00217  * @return reference to the lhs transformation
00218  */
00219 HomTransform&
00220 HomTransform::operator=(const HomTransform& t)
00221 {
00222   (*m_matrix) = *(t.m_matrix);
00223
00224   return *this;
00225 }
00226 
00227 /** Multiplication-assignment operator.
00228  * @param t the rhs transformation
00229  * @return reference to the result of the multiplication
00230  */
00231 HomTransform&
00232 HomTransform::operator*=(const HomTransform& t)
00233 {
00234   (*m_matrix) *= (*t.m_matrix);
00235
00236   return *this;
00237 }
00238 
00239 /** Comparison operator.
00240  * @param t the other transformation
00241  * @return true, if both transormations are equal
00242  */
00243 bool
00244 HomTransform::operator==(const HomTransform& t) const
00245 {
00246   return ((*m_matrix) == *(t.m_matrix));
00247 }
00248 
00249 /** Prints the matrix.
00250  * @param name Heading of the output
00251  * @param col_sep a string used to separate columns (defaults to '\\t')
00252  * @param row_sep a string used to separate rows (defaults to '\\n')
00253  */
00254 void
00255 HomTransform::print_info(const char *name, const char *col_sep, const char *row_sep) const
00256 {
00257   m_matrix->print_info(name ? name : "HomTransform", col_sep, row_sep);
00258 }
00259
00260 
00261 /** Returns a copy of the matrix.
00262  * @return the matrix of the transformation
00263  */
00264 const Matrix&
00265 HomTransform::get_matrix() const
00266 {
00267   return *m_matrix;
00268 }
00269
00270 } // end namespace fawkes
00271