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 #ifndef VRML_MATRI4X4_H 00020 #define VRML_MATRI4X4_H 00021 00022 #include <stdexcept> 00023 #include <iostream> 00024 00025 #include "vector4.h" 00026 00027 namespace vrml { 00028 00032 class Matrix4x4 00033 { 00034 public: 00035 Matrix4x4(); 00036 ~Matrix4x4(); 00037 00038 Matrix4x4(const Matrix4x4& other); 00039 explicit Matrix4x4(const double* data); 00040 00045 void makeDiag(double d = 1.0); 00046 00050 double* operator[](unsigned int m) throw (std::runtime_error) 00051 { 00052 if (m > 4) 00053 throw std::runtime_error("index exceeds matrix dimensions"); 00054 return &_data[m*4]; 00055 } 00056 00060 const double* operator[](unsigned int m) const throw (std::runtime_error) 00061 { 00062 if (m > 4) 00063 throw std::runtime_error("index exceeds matrix dimensions"); 00064 return &_data[m*4]; 00065 } 00066 Matrix4x4& operator=(const Matrix4x4& other); 00068 Matrix4x4 operator* (const Matrix4x4& other) const; 00070 Vector4 operator* (const Vector4& vec) const; 00071 00075 static Matrix4x4 translate(double dx, double dy, double dz); 00076 00083 static Matrix4x4 rotate(double phi, double u, double v, double w); 00084 00088 static Matrix4x4 scale(double sx, double sy, double sz); 00092 static Matrix4x4 identity(); 00093 00094 protected: 00096 void copy(const Matrix4x4& other); 00097 double _data[16]; 00098 }; 00099 00100 std::ostream &operator<<(std::ostream &stream, const Matrix4x4& mat); 00101 00102 } 00103 00104 #endif