Fawkes API Fawkes Development Version
|
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