00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
00035
00036
00037
00038
00039
00040 HomTransform::HomTransform()
00041 {
00042 m_matrix = new Matrix(4, 4);
00043 m_matrix->id();
00044 }
00045
00046
00047
00048
00049 HomTransform::HomTransform(const HomTransform& t)
00050 {
00051 m_matrix = new Matrix(*(t.m_matrix));
00052 }
00053
00054
00055
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
00068 HomTransform::~HomTransform()
00069 {
00070 delete m_matrix;
00071 }
00072
00073
00074
00075
00076 HomTransform&
00077 HomTransform::reset()
00078 {
00079 m_matrix->id();
00080 return *this;
00081 }
00082
00083
00084
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
00101
00102
00103 HomTransform
00104 HomTransform::get_inverse()
00105 {
00106 HomTransform t(*this);
00107 t.invert();
00108
00109 return t;
00110 }
00111
00112
00113
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
00132
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
00152
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
00172
00173
00174
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
00186
00187
00188
00189
00190
00191
00192
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
00204
00205
00206
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
00218
00219
00220
00221 HomTransform&
00222 HomTransform::operator=(const HomTransform& t)
00223 {
00224 (*m_matrix) = *(t.m_matrix);
00225
00226 return *this;
00227 }
00228
00229
00230
00231
00232
00233 HomTransform&
00234 HomTransform::operator*=(const HomTransform& t)
00235 {
00236 (*m_matrix) *= (*t.m_matrix);
00237
00238 return *this;
00239 }
00240
00241
00242
00243
00244
00245 bool
00246 HomTransform::operator==(const HomTransform& t) const
00247 {
00248 return ((*m_matrix) == *(t.m_matrix));
00249 }
00250
00251
00252
00253
00254
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
00264
00265
00266 const Matrix&
00267 HomTransform::get_matrix() const
00268 {
00269 return *m_matrix;
00270 }
00271
00272 }
00273