hom_transform.cpp

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  * @return reference to this
00075  */
00076 HomTransform&
00077 HomTransform::reset()
00078 {
00079   m_matrix->id();
00080   return *this;
00081 }
00082 
00083 /** Invert the transformation.
00084  * @return reference to the inverted transformation
00085  */
00086 HomTransform&
00087 HomTransform::invert()
00088 {
00089   float ct[3] = { (*m_matrix)(0, 3), (*m_matrix)(1, 3), (*m_matrix)(2, 3) };
00090   Matrix rot  = m_matrix->get_submatrix(0, 0, 3, 3);
00091 
00092   m_matrix->overlay(0, 0, rot.transpose());
00093   (*m_matrix)(0, 3) = -ct[0] * (*m_matrix)(0, 0) - ct[1] * (*m_matrix)(0, 1) - ct[2] * (*m_matrix)(0, 2);
00094   (*m_matrix)(1, 3) = -ct[0] * (*m_matrix)(1, 0) - ct[1] * (*m_matrix)(1, 1) - ct[2] * (*m_matrix)(1, 2);
00095   (*m_matrix)(2, 3) = -ct[0] * (*m_matrix)(2, 0) - ct[1] * (*m_matrix)(2, 1) - ct[2] * (*m_matrix)(2, 2);
00096 
00097   return *this;
00098 }
00099 
00100 /** Obtain inverse transform.
00101  * @return the inverse transform
00102  */
00103 HomTransform
00104 HomTransform::get_inverse()
00105 {
00106   HomTransform t(*this);
00107   t.invert();
00108 
00109   return t;
00110 }
00111 
00112 /** Add rotation around the x-axis.
00113  * @param rad rotation angle in rad
00114  */
00115 void
00116 HomTransform::rotate_x(float rad)
00117 {
00118   float cos = cosf(rad);
00119   float sin = sinf(rad);
00120   float s1[3] = { (*m_matrix)(0,1), (*m_matrix)(1,1), (*m_matrix)(2,1) };
00121   float s2[3] = { (*m_matrix)(0,2), (*m_matrix)(1,2), (*m_matrix)(2,2) };
00122 
00123   (*m_matrix)(0,1) = s1[0] * cos + s2[0] * sin;
00124   (*m_matrix)(1,1) = s1[1] * cos + s2[1] * sin;
00125   (*m_matrix)(2,1) = s1[2] * cos + s2[2] * sin;
00126   (*m_matrix)(0,2) = -s1[0] * sin + s2[0] * cos;
00127   (*m_matrix)(1,2) = -s1[1] * sin + s2[1] * cos;
00128   (*m_matrix)(2,2) = -s1[2] * sin + s2[2] * cos;
00129 }
00130 
00131 /** Add rotation around the y-axis.
00132  * @param rad rotation angle in rad
00133  */
00134 void
00135 HomTransform::rotate_y(float rad)
00136 {
00137   float cos = cosf(rad);
00138   float sin = sinf(rad);
00139   float s1[3] = { (*m_matrix)(0,0), (*m_matrix)(1,0), (*m_matrix)(2,0) };
00140   float s2[3] = { (*m_matrix)(0,2), (*m_matrix)(1,2), (*m_matrix)(2,2) };
00141 
00142   (*m_matrix)(0,0) = s1[0] * cos - s2[0] * sin;
00143   (*m_matrix)(1,0) = s1[1] * cos - s2[1] * sin;
00144   (*m_matrix)(2,0) = s1[2] * cos - s2[2] * sin;
00145 
00146   (*m_matrix)(0,2) = s1[0] * sin + s2[0] * cos;
00147   (*m_matrix)(1,2) = s1[1] * sin + s2[1] * cos;
00148   (*m_matrix)(2,2) = s1[2] * sin + s2[2] * cos;
00149 }
00150 
00151 /** Add rotation around the z-axis.
00152  * @param rad rotation angle in rad
00153  */
00154 void
00155 HomTransform::rotate_z(float rad)
00156 {
00157   float cos = cosf(rad);
00158   float sin = sinf(rad);
00159   float s1[3] = { (*m_matrix)(0,0), (*m_matrix)(1,0), (*m_matrix)(2,0) };
00160   float s2[3] = { (*m_matrix)(0,1), (*m_matrix)(1,1), (*m_matrix)(2,1) };
00161 
00162   (*m_matrix)(0,0) = s1[0] * cos + s2[0] * sin;
00163   (*m_matrix)(1,0) = s1[1] * cos + s2[1] * sin;
00164   (*m_matrix)(2,0) = s1[2] * cos + s2[2] * sin;
00165 
00166   (*m_matrix)(0,1) = -s1[0] * sin + s2[0] * cos;
00167   (*m_matrix)(1,1) = -s1[1] * sin + s2[1] * cos;
00168   (*m_matrix)(2,1) = -s1[2] * sin + s2[2] * cos;
00169 }
00170 
00171 /** Add translation to the transformation.
00172  * @param dx offset along x-axis
00173  * @param dy offset along y-axis
00174  * @param dz offset along z-axis
00175  */
00176 void
00177 HomTransform::trans(float dx, float dy, float dz)
00178 {
00179   (*m_matrix)(0, 3) += (*m_matrix)(0, 0) * dx + (*m_matrix)(0, 1) * dy + (*m_matrix)(0, 2) * dz;
00180   (*m_matrix)(1, 3) += (*m_matrix)(1, 0) * dx + (*m_matrix)(1, 1) * dy + (*m_matrix)(1, 2) * dz;
00181   (*m_matrix)(2, 3) += (*m_matrix)(2, 0) * dx + (*m_matrix)(2, 1) * dy + (*m_matrix)(2, 2) * dz;
00182 }
00183 
00184 
00185 /** Modified Denavit-Hartenberg transformation.
00186  * DH-transformation as used by Aldebaran
00187  * @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"
00188  *
00189  * @param alpha the angle from the Z_i-1 axis to the Z_i axis about the X_i-1 axis
00190  * @param a     the offset distance between the Z_i-1 and Z_i axes along the X_i-1 axis
00191  * @param theta the angle between the X_i-1 and X_i axes about the Z_i axis
00192  * @param d     the distance from the origin of frame X_i-1 to the X_i axis along the Z_i axis
00193  */
00194 void
00195 HomTransform::mDH(const float alpha, const float a, const float theta, const float d)
00196 {
00197   if (alpha)  rotate_x(alpha);
00198   if (a || d) trans(a, 0, d);
00199   if (theta)  rotate_z(theta);
00200 }
00201 
00202 
00203 /** Set the translation.
00204  * @param x the translation along the x-axis
00205  * @param y the translation along the y-axis
00206  * @param z the translation along the z-axis
00207  */
00208 void
00209 HomTransform::set_trans(float x, float y, float z)
00210 {
00211   Matrix& matrix_ref = *m_matrix;
00212   matrix_ref(0, 3) = x;
00213   matrix_ref(1, 3) = y;
00214   matrix_ref(2, 3) = z;
00215 }
00216 
00217 /** Assignment operator.
00218  * @param t the other transformation
00219  * @return reference to the lhs transformation
00220  */
00221 HomTransform&
00222 HomTransform::operator=(const HomTransform& t)
00223 {
00224   (*m_matrix) = *(t.m_matrix);
00225 
00226   return *this;
00227 }
00228 
00229 /** Multiplication-assignment operator.
00230  * @param t the rhs transformation
00231  * @return reference to the result of the multiplication
00232  */
00233 HomTransform&
00234 HomTransform::operator*=(const HomTransform& t)
00235 {
00236   (*m_matrix) *= (*t.m_matrix);
00237 
00238   return *this;
00239 }
00240 
00241 /** Comparison operator.
00242  * @param t the other transformation
00243  * @return true, if both transormations are equal
00244  */
00245 bool
00246 HomTransform::operator==(const HomTransform& t) const
00247 {
00248   return ((*m_matrix) == *(t.m_matrix));
00249 }
00250 
00251 /** Prints the matrix.
00252  * @param name Heading of the output
00253  * @param col_sep a string used to separate columns (defaults to '\\t')
00254  * @param row_sep a string used to separate rows (defaults to '\\n')
00255  */
00256 void
00257 HomTransform::print_info(const char *name, const char *col_sep, const char *row_sep) const
00258 {
00259   m_matrix->print_info(name ? name : "HomTransform", col_sep, row_sep);
00260 }
00261 
00262 
00263 /** Returns a copy of the matrix.
00264  * @return the matrix of the transformation
00265  */
00266 const Matrix&
00267 HomTransform::get_matrix() const
00268 {
00269   return *m_matrix;
00270 }
00271 
00272 } // end namespace fawkes
00273 

Generated on Tue Feb 22 13:31:26 2011 for Fawkes API by  doxygen 1.4.7