matrix4x4.cpp
Go to the documentation of this file.
00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
00003 //
00004 // This file is part of HOG-Man.
00005 // 
00006 // HOG-Man is free software: you can redistribute it and/or modify
00007 // it under the terms of the GNU General Public License as published by
00008 // the Free Software Foundation, either version 3 of the License, or
00009 // (at your option) any later version.
00010 // 
00011 // HOG-Man is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU General Public License for more details.
00015 // 
00016 // You should have received a copy of the GNU General Public License
00017 // along with HOG-Man.  If not, see <http://www.gnu.org/licenses/>.
00018 
00019 #include "matrix4x4.h"
00020 #include <cmath>
00021 #include <iomanip>
00022 #include <cstdlib>
00023 #include <cstring>
00024 #include "stuff/os_specific.h"
00025 using namespace std;
00026 
00027 namespace vrml {
00028 
00029 Matrix4x4::Matrix4x4()
00030 {
00031   memset(_data, 0, 16 * sizeof(double));
00032 }
00033 
00034 Matrix4x4::~Matrix4x4()
00035 {
00036 }
00037 
00038 Matrix4x4::Matrix4x4(const Matrix4x4& other)
00039 {
00040   copy(other);
00041 }
00042 
00043 void Matrix4x4::copy(const Matrix4x4& other)
00044 {
00045   memcpy(_data, other._data, 16 * sizeof(double));
00046 }
00047 
00048 void Matrix4x4::makeDiag(double d)
00049 {
00050   memset(_data, 0, 16 * sizeof(double));
00051   for (unsigned int i = 0; i < 4; ++i)
00052     (*this)[i][i] = d;
00053 }
00054 
00055 Matrix4x4& Matrix4x4::operator=(const Matrix4x4& other)
00056 {
00057   if (this != &other){
00058     copy(other);
00059   }
00060   return *this;
00061 }
00062 
00063 Matrix4x4 Matrix4x4::operator* (const Matrix4x4& other) const
00064 {
00065   Matrix4x4 prod;
00066   for (uint row = 0; row < 4; row++)
00067     for (uint col = 0; col < 4; col++){
00068       prod[row][col] = 0.;
00069       for (uint i=0; i < 4; i++)
00070         prod[row][col] += (*this)[row][i]*other[i][col];
00071     }
00072 
00073   return prod;
00074 }
00075 
00076 Matrix4x4 Matrix4x4::translate(double dx, double dy, double dz)
00077 {
00078   Matrix4x4 trans;
00079   trans.makeDiag(1.0);
00080   trans[0][3] = dx;
00081   trans[1][3] = dy;
00082   trans[2][3] = dz;
00083   return trans;
00084 }
00085 
00086 Matrix4x4 Matrix4x4::rotate(double phi, double u, double v, double w)
00087 {
00088   Matrix4x4 matrix;
00089 
00090   double rcos = cos(phi);
00091   double rsin = sin(phi);
00092   matrix[0][0] =      rcos + u*u*(1-rcos);
00093   matrix[1][0] =  w * rsin + v*u*(1-rcos);
00094   matrix[2][0] = -v * rsin + w*u*(1-rcos);
00095   matrix[0][1] = -w * rsin + u*v*(1-rcos);
00096   matrix[1][1] =      rcos + v*v*(1-rcos);
00097   matrix[2][1] =  u * rsin + w*v*(1-rcos);
00098   matrix[0][2] =  v * rsin + u*w*(1-rcos);
00099   matrix[1][2] = -u * rsin + v*w*(1-rcos);
00100   matrix[2][2] =      rcos + w*w*(1-rcos);
00101   matrix[3][3] = 1.0;
00102 
00103   return matrix;
00104 }
00105 
00106 std::ostream &operator<<(std::ostream &stream, const Matrix4x4& mat)
00107 {
00108   stream << "Matrix4x4:" << std::endl;
00109   for(unsigned int i = 0; i < 4; i++){
00110     stream << "\t";
00111     for(unsigned int j = 0; j < 4; j++)
00112       stream << std::setw(7) << mat[i][j] << " ";
00113     stream << std::endl;
00114   }
00115   return stream;
00116 }
00117 
00118 Matrix4x4::Matrix4x4(const double* data)
00119 {
00120   memcpy(_data, data, 16*sizeof(double));
00121 }
00122 
00123 Vector4 Matrix4x4::operator* (const Vector4& vec) const
00124 {
00125   Vector4 prod;
00126   for (uint row = 0; row < 4; row++){
00127     prod[row] = 0;
00128     for (uint col = 0; col < 4; col++)
00129       prod[row] += (*this)[row][col] * vec[col];
00130   }
00131   return prod;
00132 }
00133 
00134 Matrix4x4 Matrix4x4::identity()
00135 {
00136   Matrix4x4 ident;
00137   ident.makeDiag(1.0);
00138   return ident;
00139 }
00140 
00141 Matrix4x4 Matrix4x4::scale(double sx, double sy, double sz)
00142 {
00143   Matrix4x4 scale;
00144   scale[0][0] = sx;
00145   scale[1][1] = sy;
00146   scale[2][2] = sz;
00147   scale[3][3] = 1;
00148   return scale;
00149 }
00150 
00151 }


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Mon Oct 6 2014 00:06:58